//通过配置的轮子最大加速度进行降速 //适当比例的降速,算完后记得把最新速度数据放在ringbuffer里 //wheelOne 一号轮速度数组首地址 //wheelTwo 二号轮速度数组首地址 //wheelThree 三号轮速度数组首地址 static public void DynamicalAjusting(float[] wheelOne, float[] wheelTwo, float[] wheelThree) { float time = 0.0f; int n = PointsInfo.pnts.Count(); float tempAcc = 0.0f; //每次加速度降低至上次的百分值 float percent = 0.9f; //先正向削减速度 for (int i = 2; i < n + 1; i++) { //粗略计算每两示教点之间的运动的时间 time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2; //轮1 //只处理速度同向的情况 if (wheelOne[i - 1] * wheelOne[i - 2] > 0) { tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc > MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelOne[i - 1] = wheelOne[i - 1] > 0 ? wheelOne[i - 2] + tempAcc * percent * time : wheelOne[i - 2] - tempAcc * percent * time; } //轮2 //只处理速度同向的情况 if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0) { tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc > MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelTwo[i - 1] = wheelTwo[i - 1] > 0 ? wheelTwo[i - 2] + tempAcc * percent * time : wheelTwo[i - 2] - tempAcc * percent * time; } //轮3 //只处理速度同向的情况 if (wheelThree[i - 1] * wheelThree[i - 2] > 0) { tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc > MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelThree[i - 1] = wheelThree[i - 1] > 0 ? wheelThree[i - 2] + tempAcc * percent * time : wheelThree[i - 2] - tempAcc * percent * time; } } //反向削减速度 for (int i = n; i > 1; i--) { //粗略计算每两示教点之间的运动的时间 time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2; //轮1 //只处理速度同向的情况 if (wheelOne[i - 1] * wheelOne[i - 2] > 0) { tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc < -MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelOne[i - 2] = wheelOne[i - 2] > 0 ? wheelOne[i - 1] - tempAcc * percent * time : wheelOne[i - 1] + tempAcc * percent * time; } //轮2 //只处理速度同向的情况 if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0) { tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc < -MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelTwo[i - 2] = wheelTwo[i - 2] > 0 ? wheelTwo[i - 1] - tempAcc * percent * time : wheelTwo[i - 1] + tempAcc * percent * time; } //轮3 //只处理速度同向的情况 if (wheelThree[i - 1] * wheelThree[i - 2] > 0) { tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time; } else { tempAcc = 0.0f; } if (tempAcc < -MotionCardParameter.GetAccMax()) { //每次削减0.05的加速度 wheelThree[i - 2] = wheelThree[i - 2] > 0 ? wheelThree[i - 1] - tempAcc * percent * time : wheelThree[i - 1] + tempAcc * percent * time; } } for (int i = 0; i < n - 1; i++) { Calculate.TriWheelVel2_t tempVel2; tempVel2 = Calculate.TriWheelVel2ResultantVel(wheelOne[i], wheelTwo[i], wheelThree[i], PointsInfo.pnts[i].posAngle); //存入新计算的速度 PointsInfo.pnts[i].velMax = tempVel2.speed; } }
private void SpeedReplanning() { //将txt文档中的数据存入ringBuffer SpeedPlanning.MoveTxtToRingBuffer(); int num = PointsInfo.pnts.Count(); float[] angleErr = new float[num]; //数据读取 StreamReader speedReplanFile = File.OpenText(SpeedReplanInfo); List <string> tempString = new List <string>(); while (!speedReplanFile.EndOfStream) { tempString.Add(speedReplanFile.ReadLine()); } speedReplanFile.Close(); if (num < 3) { return; } //计算每一段的平均角度偏差 //将一段里面的打滑量放在后端点数组 for (int i = 1; i < num; i++) { float angleErrSum = 0; int count = 0; foreach (string str in tempString) { string[] sArray = str.Split(','); if (float.Parse(sArray[0]) > PointsInfo.pnts[i - 1].length && float.Parse(sArray[0]) < PointsInfo.pnts[i].length) { angleErrSum += float.Parse(sArray[1]); count++; } if (float.Parse(sArray[0]) > PointsInfo.pnts[i].length) { break; } } if (count != 0) { angleErr[i] = angleErrSum / count; } else { angleErr[i] = 0; } } //对第一点和第二点进行忽略 for (int i = 1; i < num - 1; i++) { if (angleErr[i] > 0.1f) { if (angleErr[i] > 40) { angleErr[i] = 40; } PointsInfo.pnts[i - 1].velMax *= (1 - angleErr[i] / 100); } } float tempVell = 0.0f; //通过v2^2 - v1^2 = 2*a*s对速度再次规划 for (int i = 0; i < num - 1; i++) { if (PointsInfo.pnts[i + 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i + 1].length - PointsInfo.pnts[i].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i + 1].velMax) { PointsInfo.pnts[i + 1].velMax = tempVell; } } } for (int i = num - 1; i > 0; i--) { if (PointsInfo.pnts[i - 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i].length - PointsInfo.pnts[i - 1].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i - 1].velMax) { PointsInfo.pnts[i - 1].velMax = tempVell; } } } for (int i = 0; i < num; i++) { Console.WriteLine(PointsInfo.pnts[i].velMax); } SpeedPlanning.MoveListToTxt(); //将速度小于最小速度的做处理 for (int i = 0; i < num; i++) { if (PointsInfo.pnts[i].velMax < 70) { PointsInfo.pnts[i].velMax = 70; } } }
static public void SpeedPlan() { int num = PointsInfo.pnts.Count(); //将曲率半径左移一个单位,便于机器提前反应 for (int i = 0; i < num - 1; i++) { PointsInfo.pnts[i].curvatureR = PointsInfo.pnts[i + 1].curvatureR; } //电机最大速度能够满足的最小曲率半径 float curvatureMaxVell = MotionCardParameter.GetVelMax() * MotionCardParameter.GetVelMax() / (2 * MotionCardParameter.GetAccMax()); //将过大的曲率半径全设为能最大速度能满足的最小曲率半径 for (int i = 0; i < num; i++) { if (PointsInfo.pnts[i].curvatureR > curvatureMaxVell) { PointsInfo.pnts[i].curvatureR = curvatureMaxVell; } } //curvature[0] = curvature[1]; //curvature[n - 1] = curvature[n - 2]; //通过曲率半径计算该段能满足的最大速度 for (int i = 0; i < num; i++) { PointsInfo.pnts[i].velMax = (float)Math.Sqrt((2 * MotionCardParameter.GetAccMax()) * PointsInfo.pnts[i].curvatureR); } float tempVell = 0.0f; //通过v2^2 - v1^2 = 2*a*s对速度再次规划 for (int i = 0; i < num - 1; i++) { if (PointsInfo.pnts[i + 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i + 1].length - PointsInfo.pnts[i].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i + 1].velMax) { PointsInfo.pnts[i + 1].velMax = tempVell; } } } for (int i = num - 1; i > 0; i--) { if (PointsInfo.pnts[i - 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i].length - PointsInfo.pnts[i - 1].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i - 1].velMax) { PointsInfo.pnts[i - 1].velMax = tempVell; } } } float[] wheelOne = new float[PointsInfo.pnts.Count()]; float[] wheelTwo = new float[PointsInfo.pnts.Count()]; float[] wheelThree = new float[PointsInfo.pnts.Count()]; //计算此时三个轮的速度 CalThreeWheelVel(wheelOne, wheelTwo, wheelThree); //动态的对速度进行平衡 while (true) { int ipoint = 0; for (ipoint = 3; ipoint < num; ipoint++) { float time = 0.0f; float lll; float vvv; lll = (PointsInfo.pnts[ipoint - 1].length - PointsInfo.pnts[ipoint - 2].length); vvv = (PointsInfo.pnts[ipoint - 1].velMax + PointsInfo.pnts[ipoint - 2].velMax) / 2; time = lll / vvv; float a1, a2, a3; //如果判断某一个轮子加速度大于最大加速度时,进行调节 a1 = (wheelOne[ipoint - 1] - wheelOne[ipoint - 2]) / time; a2 = (wheelTwo[ipoint - 1] - wheelTwo[ipoint - 2]) / time; a3 = (wheelThree[ipoint - 1] - wheelThree[ipoint - 2]) / time; if (((a1 > MotionCardParameter.GetAccMax()) && (wheelOne[ipoint - 1] * wheelOne[ipoint - 2] > 0)) || ((a2 > MotionCardParameter.GetAccMax()) && (wheelTwo[ipoint - 1] * wheelTwo[ipoint - 2] > 0)) || ((a3 > MotionCardParameter.GetAccMax()) && (wheelThree[ipoint - 1] * wheelThree[ipoint - 2] > 0))) { //平衡法规划速度 DynamicalAjusting(wheelOne, wheelTwo, wheelThree); break; } } if (ipoint == num) { for (int i = 1; i < num - 1; i++) { Calculate.TriWheelVel1_t tempTrueVell; tempTrueVell.v1 = wheelOne[i]; tempTrueVell.v2 = wheelTwo[i]; tempTrueVell.v3 = wheelThree[i]; float vellCar1, vellCar2, vellCar3, vellCar; float angErr = PointsInfo.pnts[i + 1].posAngle - PointsInfo.pnts[i].posAngle; angErr = angErr > 180 ? angErr - 360 : angErr; angErr = angErr < -180 ? 360 + angErr : angErr; //粗略计算每两示教点之间的运动的时间 float time = (PointsInfo.pnts[i + 1].length - PointsInfo.pnts[i].length) / (PointsInfo.pnts[i + 1].velMax + PointsInfo.pnts[i].velMax) * 2; vellCar1 = DecreseVellByOneWheel(PointsInfo.pnts[i].velMax, PointsInfo.pnts[i].direction, angErr / time, PointsInfo.pnts[i].posAngle, 1, tempTrueVell.v1); vellCar2 = DecreseVellByOneWheel(PointsInfo.pnts[i].velMax, PointsInfo.pnts[i].direction, angErr / time, PointsInfo.pnts[i].posAngle, 2, tempTrueVell.v2); vellCar3 = DecreseVellByOneWheel(PointsInfo.pnts[i].velMax, PointsInfo.pnts[i].direction, angErr / time, PointsInfo.pnts[i].posAngle, 3, tempTrueVell.v3); if (Math.Abs(vellCar1) >= Math.Abs(vellCar2) && Math.Abs(vellCar1) >= Math.Abs(vellCar3)) { vellCar = vellCar1; //将计算的最新合速度放入缓存池中 PointsInfo.pnts[i].velMax = vellCar; } else if (Math.Abs(vellCar2) >= Math.Abs(vellCar1) && Math.Abs(vellCar2) >= Math.Abs(vellCar3)) { vellCar = vellCar2; //将计算的最新合速度放入缓存池中 PointsInfo.pnts[i].velMax = vellCar; } else if (Math.Abs(vellCar3) >= Math.Abs(vellCar2) && Math.Abs(vellCar3) >= Math.Abs(vellCar1)) { vellCar = vellCar3; //将计算的最新合速度放入缓存池中 PointsInfo.pnts[i].velMax = vellCar; } } } if (ipoint == num) { PointsInfo.pnts[0].velMax = 100; PointsInfo.pnts[num - 1].velMax = 100; //通过v2^2 - v1^2 = 2*a*s对速度再次规划 for (int i = 0; i < num - 1; i++) { if (PointsInfo.pnts[i + 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i + 1].length - PointsInfo.pnts[i].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i + 1].velMax) { PointsInfo.pnts[i + 1].velMax = tempVell; } } } for (int i = num - 1; i > 0; i--) { if (PointsInfo.pnts[i - 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i].length - PointsInfo.pnts[i - 1].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i - 1].velMax) { PointsInfo.pnts[i - 1].velMax = tempVell; } } } //将速度小于最小速度的做处理 for (int i = 0; i < num; i++) { if (PointsInfo.pnts[i].velMax < 70) { PointsInfo.pnts[i].velMax = 70; } } break; } } }