[发明专利]机器人路径跟踪方法

权利要求书

1.一种机器人路径跟踪方法，其特征在于：包括以下步骤：

S1.获取机器人的全局路径，根据这段行走的距离判断为直线模式或圆弧模式；

S2.直线模式采用pid控制算法来进行线速度与角速度的控制；

S3.在圆弧模式下行走，利用贝塞尔曲线对转弯点处进行插值；

S4.计算机器人当前位置到局部目标点之间拟合弧线的相对曲率；

S5.在直线行走的过程中，不停地判断机器人有没有接近当前的转弯点位置，如果到达当前的转弯点的前2米，那么减速；等到达转弯点位置后，停止运动，开始旋转，使机器人朝向新的局部目标点集，期间不停地判断机器人有没有到达转弯点的位姿，如果满足条件，那么开始加速前进，不停地判断机器人有没有到达最终的目标点，同样如果到达目标点前m米，减速，最后安全到达终点位置；

S6.在圆弧行走过程中，不停地判断机器人有没有到达最终的目标点位置，如果到达目标点前m米，那么减速，直到安全到达点位置后停止，在拐弧线模式下，机器人在拐弯处会根据当前路径的弯曲程度d，计算出一组线速度和角速度驱动机器人过弯。

2.根据权利要求1所述机器人路径跟踪方法，其特征在于：步骤S1具体过程如下：

S11.根据机器人当前位置(x,y)向前搜索5米的距离，取出这段距离中所有的点来进行判断；

S12.将这段距离中的点位(x_i,y_i)与(x_i+1,y_i+1)进行计算，保存记录两个点之间的斜率

S13.遍历这个两点之间的斜率，当前后n个k值保持相同的斜率范围内是，则这段行走的距离模式可判断为直线模式，当连续k＞α后，则这段行走的距离为圆弧模式，α为斜率阈值。

3.根据权利要求1所述机器人路径跟踪方法，其特征在于：步骤S2具体过程如下：

S21.在机器人当前位置以初始速度V向下一个点位前进，实时获取机器人当前位置，判断机器人在计算两点(x₁,y₁)与(x₂,y₂)之间的距离D及偏角β，并将直线的点拟合成直线，具体公式如下：

(x₁,y₁)与(x₂,y₂)两个位姿之间的距离；

β为(x₁,y₁)与(x₂,y₂)两个位姿的斜率；

拟合直线：Ax+By+C＝0，其中(x₁,y₁)为当前位置坐标，(x₂,y₂)为目标点的位置坐标，；

S22.机器人行走中，求取机器人位置(x₀,y₀)距离直线Ax+By+C＝0的距离：

S23.当距离小于阈值a时，机器人按照固定的速度行驶，当阈值大于a时，角速度ω根据距离D来进行调整，

k为比系数，纠正转角ω来靠近直线，按照速度V前进，知道终点位置(x_e,y_e)。

4.根据权利要求1所述机器人路径跟踪方法，其特征在于：步骤S3具体过程如下：

S31.塞尔曲线公式P＝(1-t)²P0+2t(1-t)P1+t²P2，设置转弯距离λ，根据转弯点的下标和λ，计算控制点c_i1，c_i2的下标；

S32.输入全局路径、转弯点和两个控制点c_i1，c_i2；

S33.采用分段读取的方法获取所有的插值点和原路径点，不包含被插值的点，具体如下：总共段数为L＝2×i+1，迭代次数为L次，当L为偶数，则装入直线段；当L为奇数，则装入弧线段，即插值点。

5.根据权利要求1所述机器人路径跟踪方法，其特征在于：步骤S4具体过程如下：

S41.利用纯追踪模型跟踪预设的全局路径，根据当前速度计算前向距离，设置最大前向距离lookaheadmax＝v×t+ζ和最小前向距离lookaheadmin＝v×t-ζ，式中t为模拟时间、ζ为增益系数；

S42.根据前向距离和当前机器人的位置，在局部目标点集中找到当前时刻需要跟踪的局部目标点，计算机器人当前位置到局部目标点之间拟合弧线的相对曲率即当前点与选择的前向点之间距离生成的直线的最远距离：

参考点(x₀,y₀),前向点(x₁,y₁)，在点的集合set{(x₀,y₀),......(x₁,y₁)}中，取最大值为圆弧的相对曲率：

两点之间的直线表达式：y-y₁＝[(y₀-y₁)/(x₀-x₁)]*(x-x₁)

其中(x_i,y_i)为集合set中的点，max的取值的下标i即为转弯点；

S43.计算当前时刻机器人的角速度ω＝t*v*γ，t为仿真时间，v为速度阈值，γ为增益系数。