//返回三个轮子轮速
//speed 单位mm/s
//direction 速度的方向 单位 度
//rotationVell 旋转速度 单位 度/s
//posAngle 机器人的姿态 单位 度
static public TriWheelVel1_t CaculateThreeWheelVel(float speed, float direction, float rotationVell, float angleZ)
{
TriWheelVel1_t vell;
float Vx, Vy;
float theta;
float robotR = 0.0f;
const float ALPHA = 60.0f;
robotR = MotionCardParameter.GetRobotR();
rotationVell = rotationVell / CHANGE_TO_ANGLE;
Vx = speed * (float)Math.Cos(direction * CHANGE_TO_RADIAN);
Vy = speed * (float)Math.Sin(direction * CHANGE_TO_RADIAN);
theta = angleZ;
vell.v1 = (float)(-(float)Math.Cos((ALPHA + theta) * CHANGE_TO_RADIAN) * Vx - (float)Math.Sin((theta + ALPHA) * CHANGE_TO_RADIAN) * Vy + rotationVell * robotR);
vell.v2 = (float)((float)Math.Cos(theta * CHANGE_TO_RADIAN) * Vx + (float)Math.Sin(theta * CHANGE_TO_RADIAN) * Vy + rotationVell * robotR);
vell.v3 = (float)(-(float)Math.Cos((ALPHA - theta) * CHANGE_TO_RADIAN) * Vx + (float)Math.Sin((ALPHA - theta) * CHANGE_TO_RADIAN) * Vy + rotationVell * robotR);
return(vell);
}
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;
}
}
}
//通过配置的轮子最大加速度进行降速
//适当比例的降速，算完后记得把最新速度数据放在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;
}
}
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;
}
}
}