差速移动机器人轨迹跟踪控制方法及实现-NJUST

摘要： 本文针对差速移动机器人轨迹跟踪问题,提出一种使用PID环节对航向角和距离进行控制算法。首先对实际GPS传感器的经纬度进行转换，其次建立差速移动机器人运动学模型，最后利用控制算法对离散的数据点进行跟踪并与PurePursuit算法进行横向对比。
关键词：经纬度与平面坐标转换转换、差速移动机器人建模、PID轨迹跟踪、PurePursuit算法

一、引言

随着智能机器人领域不断发展，差速移动机器人在服务、物流、探测等领域具有极广的应用前景，而对具有非完整系统的差速移动机器人的运动控制是关注的热点。轨迹跟踪问题作为其控制的重要研究问题，受到研究者的广泛关注。轨迹跟踪是使差速移动机器人的实际轨迹快速且稳定地跟踪上期望轨迹，以满足移动机器人的付能要求。而在实际应用中，由于差速移动机器人自身的结构和系统以及外部的不确定因素，要达到理想的跟踪效果，就显得困难重重。
文献[1-2]提出了一种基Backstepping的控制方法，计算出合适的目标速度，并在此基础上加上最小均方算法作为观测器，实现了最小化轨迹跟踪误差。文献[3-4]运用终端滑动模态技术，提出基于滑模控制的轨迹跟踪控制策略，采用基于反算法的有限时间控制方法，提高了轨迹跟踪的收敛付能。由于滑模变结构具有不连续付的本质，引起控制系统的抖振将是不可避免的。文献[5-7]则针对抖振和收敛速度，提出了新型滑模变控制策略，在解决了系统中未知参数带来的影响基础上，抖振现象也被有效地抑制了，且较好地消除了跟踪误差。文献[8]提出了基于优化方法的模糊反演控制器在轨迹跟踪问题中的应用，利用动力学模型设计出力矩的控制律，达到了良好的跟踪效果。

二·、基础准备

1.去除GPS数据中的坏点

GPS传感器数据不一定是完全正确的，存在一些坏点的情况，因此需要对GPS原始的经纬度数据进行数据校验，校验方法采用3σ准则，适合数据点较多时找出粗大误差点并将其剔除。使用方法：
首先计算经纬度数据标准差。
其次每一个经纬度数据与均值的差的绝对值与3倍标准差比较，若大于3倍标准差则剔除。
最后循环直至所有数据都比较过。

2.经纬度与平面坐标系转换

常用的地球经纬度与平面坐标的转换方法有米勒投影、墨卡托投影、横轴墨卡托投影、高斯-克吕格投影、Lambert等角正割圆锥投影等。这里需求是地球经纬度坐标转平面笛卡尔坐标，所以选用米勒投影，转换公式：
$$x=(W/2)+(W/(2\times M_{P}I))\times Longitude\times (M_{P}I/180)$$
$$y=(H/2)-(H/(2\times mill))\times 1.25\times log(tan(0.25\times M_{P}I+0.4\times Lattitude\times (M_{P}I/180)))$$
其中$M_{P}I=$圆周率，$L=6381372\times M_{P}I\times 2$为地球周长，$W$，$H$是经纬度展开后$X$轴，$Y$轴长度，$mill=2.3$为米勒投影常数。

3.差速移动机器人数学建模

参考图1建立的两轮差速移动机器人模型，$V_L$ 、$V_R$分别为机器人左轮与右轮瞬时下线速度，$V、$W分别为机器人质心瞬时线速度与角速度，D为左右轮之间距离为$0.4m$。

图1：两轮差速移动机器人模型
现在考虑在极短时间$∆t$内，机器人位姿变化。首先假定在该时间内，认为机器人朝x轴方向匀速运动，匀速转动。
位姿变化量：

于是

令∆t→0 得到：

因此若已知机器人某时刻位姿与角速度、线速度，则可以通过上式计算下一时刻机器人位姿。其中x ̇、y ̇、θ ̇分别为机器人位置与航向角瞬时导数。
对上式进行离散化得该系统的状态方程，T为采样时间。

这里定义$p^k=[x^k,y^k,{\theta }^k]{]}^T$为机器人k时刻位姿，$(x^k,y^k)$为机器人位置坐标，${\theta }^k$为机器人航向角。
运动学模型：
图2是差速移动机器人相邻两时刻位姿，${\theta }_1$是相邻两时刻机器人绕圆弧运动的角度，${\theta }_3$是相邻时刻机器人航向角的变化量，$d$是右路比左轮多走的距离，R是机器人圆弧运动的半径。

图2：差速移动机器人相邻两时刻位姿
由图和几何关系可得：
$$V=(V_L+V_R)/2$$
$${\theta }_1={\theta }_2={\theta }_3$$
换言之机器人航向角变化多少，它就绕其运动轨迹的圆心旋转了多少角度。由于相邻时刻时间近似很短，${\theta }_2$很小，于是得到如下近似公式：
$${\theta }_2\approx sin({\theta }_2)=d/D=((〖V_R-V{〗}_L)\times ∆t)/D$$
于是可以得到机器人质心的角速度
$$W={\theta }_1/∆t=〖V_R-V{〗}_L/D$$
已知机器人质心的角速度与线速度，可得机器人圆弧运动的半径，进一步可得机器人运动的曲率
$$\gamma =1/R=W/V=(2\times (〖V_R-V{〗}_L))/(l\times (V_R〖+V{〗}_L))$$

三、基于PID的差分移动机器人轨迹跟踪方法

1、控制律设计

依照上述差速移动机器人数学建模，选择通过机器人角速度与线速度控制机器人运动，换言之通过公式一反向推到就是通过左右轮速度控制机器人向目标点运动，控制律的设计如图3。
因为经过转换后GPS数据是离散点构成，这里采用分段执行的形式控制机器人走完全程。在每一个阶段，通过k时刻位姿计算与目标点航向角的偏差，这里要注意航向角取值范围，通过航向角PID控制器输出k时刻机器人角速度$W(k)$；同样计算k时刻与目标点位置偏差，通过距离PID控制器输出k时刻机器人线速度$V(k)$，这样便可以通过状态方程计算机器人$k+1$时刻的位姿$p(k+1)=[x(k+1),y(k+1),\theta (k+1){]}^T$。最后判断是否抵近该阶段目标点，如果抵近则将该时刻位姿作为下一阶段起始位姿，进入下一阶段的轨迹跟踪；否则，更新当前位姿，不断重复进行上述操作。

图3：PID控制律设计流程图

2、航向角PID控制器设计

航向角PID控制器的输入为k时刻姿态位置与目标点位置的角度偏差${\theta }_{e}rro{r}_k$，经过比例环节、积分环节与微分环节，输出k时刻机器人的角速度W(k)。
比例环节：$-k1\ast {\theta }_{e}rro{r}^k$
积分环节：$ki\ast$sum_θ_error${}^k$
微分环节：kd*(${\theta }_{e}rror$^k- ${\theta }_{e}rror$^(k-1))
即k时刻的角速度：
$W^k=-k1\ast$θ_erro$r^k+ki\ast$sum_θ_error${}^k+kd\ast ($θ_error$^k- ${\theta }_{e}rror$^(k-1))$

3、距离PID控制器设计

距离PID控制器直接采用比例环节，思路是当机器人k时刻位置距离目标点较远时，机器人移动线速度会变快，相反，当机器人距离目标点较远时，线速度会变慢，以方便判断机器人是否接近目标点并顺利进入下一阶段轨迹跟踪。

即k时刻的线速度：
$$V^k=k2\ast sqrt(((x^k-x^{(}k+1){)}^2+(y^k-y^{(}k+1){)}^2))$$

四、基于PurePursuit算法的差分移动机器人轨迹跟踪方法

1. PurePursuit算法基础

PurePursuit算法是一种路径跟踪算法。它计算角速度命令，该命令将机器人从其当前位置移到机器人前方的某个预视点。假定线速度是恒定的，因此可以在任意点更改机器人的线速度。然后，该算法会根据机器人的当前位置在路径上移动前瞻点，直到路径的最后一点，可以等效的认为机器人不断追逐它前面的一个点。
纯跟踪算法是一种用几何关系确定曲率的方法，该曲率将驱动机器人到达选定的路径点（目标点）。该目标点是已知轨迹上与当前机器人位置偏离的一个视距ld的点，假定车辆中心点可以按照一定的转弯半径r前进抵达该预瞄点，然后根据视距ld，转弯半径r，车辆坐标系下预瞄点的朝向角a之间的几何关系确定机器人转向角。

图4：PurePursuit算法几何关系图
由图4中几何关系，以下两个方程成立：
$$x^2+y^2=〖l_d{〗}^2$$
$$x+d=r$$
方程1，实际上是一个半径l的圆，其与轨迹的交点都是车辆下一步可能到达的目标点。接下来通过几何关系的转化并上述两方程联立，消去d和y，可以得到圆弧曲率γ 与视距l_d的关系表达式：
$$\gamma =2x/l_d^2$$

2.控制律设计

PurePursuit算法无论针对汽车模型还是两轮差速移动机器人模型，其控制律的核心就是确定视距$l_d$。视距l_d与机器人该态下的线速度有关，因此在此假设一个常量$Con$，和一个视距系数K，则可以得到视距的表达式：
$$l_d=Kv+Con$$
不难发现：PurePursuit算法控制核心是一个P控制器。

3．基于两轮差速模型的PurePursuit算法步骤

1、给定机器人初始位姿。
2、找到机器人路径上最接近机器人的路径点A。
3、通过上述控制律计算当前视距$l_d$，从路径点A的位置沿着轨迹方向搜索，找到满足欧氏距离大于等于视距$l_d$的目标点G。
4、由上述曲率计算公式计算曲率$\gamma$，再由上述公式5,可以得到机器人角速度与线速度。
5、由公式3计算转向角，根据运动学公式8更新机器人位姿，重复步骤2直到走到轨迹终点。

五、仿真实验比较

仿真平台：Matlab2020b
原始经纬度经过数据坏点去除过程与经纬度坐标变换过程得到被跟踪轨迹图5：

图5

图6
经过图5进行放大分析发现，在被跟踪轨迹起始处与终点处各存在一些密集的点列，如图6所示。这里可以采取常规方法比如卡尔曼滤波的方式对其进行去除，但是本次仿真认为这是移动机器人在起始与终点处存在正常的现象，不予去除，同时也可以检验算法在此处的实用性。
接下来分别使用PID与PurePursuit算法对上述被跟踪轨迹图进行轨迹跟踪仿真测试，并计算其系统指标：均方误差与最大误差、平均跟踪速度、机器人运行完总的时间，机器人走过全部的距离。
轮子半径$0.125$ W最大$120rpm$ $D=0.4m$单位都是$m$。

1、基于PID的差分移动机器人轨迹跟踪仿真

(1)不同参数下的仿真对比以确定最优参数

对于PID控制，参数确定尤为重要，这里需要调整参数有（航向角PID控制器：）$K{p}_1$,$Ki{,}_1$,Kd_1$;（距离PID控制器：）$Kp_2$。通过参数试调发现，对比均方误差、最大误差、平均跟踪速度、机器人运行总的时间，机器人走过全部的距离发现：$Kp_1$变化对其影响较大，而其余参数分别稳定在：$Ki_1=0.0935$，$Kd_1=0.1$，$Kp_2=1$。接下来通过对$Kp_1$的变化的仿真，分别对比归一化的均方误差、最大误差、平均跟踪速度、机器人运行总的时间以及机器人走过全部的距离的值，以确定最优的$Kp_1$参数，仿真对比结果如图7所示：

图7：不同$K{p}_1$下，系统指标变化
不难发现：通过对图上归一化数据值进行对比来确定最优Kp_1参数是比较困难，不妨假定每个系统指标的权重以确定最优参数，利用权重值公式如下：
$$O_K{p}_1=0.3\ast Maximu{m}_{e}rror+0.4\ast Mea{n}_{s}quar{e}_{e}rror+0.1\ast time+0.1\ast Journey+0.1\ast (1-V_{e}qual)$$
其中计算不同$K{p}_1$下的$O_K{p}_1$的值，找到其值最小所对应的$kp-1$参数即为最优参数，此时$k{p}_1$最优为$9$。此时系统指标值如表所示
表1：在最优参数下，系统指标
最优参数：$K{p}_1=9，K{i}_1=0.0935，K{d}_1=0.1，K{p}_2=1$

系统指标	系统值
最大误差（Maximum_error）	9.999606651839190e-04m
均方误差（Mean_square_error）	1.671075839879241e-05m
平均跟踪速度（V_equal）	0.041352911250914m/s
机器人运行总的时间（time）	1.571330000000000e+04s
机器人走过全部的距离（Journey）	6.497907003589852e+02m

(2）仿真分析

通过PID控制差速移动机器人实现对已知轨迹跟踪的整体效果如图8所示，初始航向角选取pi/2。经过放大，如图9所示，黑点为被跟踪轨迹点，红点为跟踪轨迹点，从图中可以看出轨迹效果很好。

图8

图9
图9的跟踪轨迹显示的红点是每个阶段跟踪的最后一个点，这里为画图方便省略了中间一系列的轨迹点。当然可以将中间轨迹点绘制出来，其效果图可以从图10与图11中看出，无论是直线段还是拐点处，PID控制都能实较好的跟踪。

图10

图11
当然还要考虑一下遗留的起始与结尾处密集点的轨迹跟踪，这里从图12不难发现，虽然实现了轨迹的跟踪但是实现的效果，但是经过分析放大之后的图13，发现跟踪轨迹的效果不是很好，原因是起始与结尾处被跟踪点距离太近，造成跟踪线速度与角速度存在较大偏差。

图12

图13

2.基于PurePursuit算法的差分移动机器人轨迹跟仿真

基于PurePursuit算法差分移动机器人的程序设计并未按照上述算法理论进行，而是使用matlab自带的差分模型与PurePursuit控制器，组成了基于PurePursuit算法的差分移动机器人轨迹跟踪系统。其只作为与PID控制方法的比较，采用matlab自带模型可以更加直观具体阐明PID算法的优缺点。
经过相应程序仿真不难发现，基于PurePursuit算法控制的差分移动机器人仿真时间特别长，这样便不利于比较不同参数下仿真对比结果，因此这里只选取一组比较典型的参数进行仿真并直接与PID控制方法进行比较。

图14

图15
针对相同数据处理后产生的被跟踪轨迹，图14是基于PurePursuit算法的差分移动机器人轨迹跟踪的结果，可以直观看出，其完成了轨迹跟踪的预期效果。经过放大，如图15所示，显示的是起始处与结尾处，其较好的规避了密集点所带来的影响，达到了较为顺滑的跟踪效果。

图16

图17
通过观察图16直线段与图17拐点处跟踪效果，基于PurePursuit算法控制的差分移动机器人较好的实现了轨迹的跟踪，并且其跟踪方法也明显从图16、17中看出不同。该方法不同于PID控制跟踪那样跟踪每一个点，而是针对轨迹趋势的跟踪，这一方面避免了面密集点或者杂乱点的影响，另一方面这也比较符合实际生产应用。
如表2所示，在差分模型与PurePursuit模型参数选定下进行仿真的系统指标。
表2：在选定参属下，系统指标
参数：$ld=0.3m$、目标半径$=0.1m$、理想速度$=0.6m$、采样时间$0.1s$
系统指标 系统值

系统指标	系统值
最大误差（Maximum_error）	0.234413427072807m
均方误差（Mean_square_error）	4.861275615251850e-04m
平均跟踪速度（V_equal）	0.599999999999905m/s
机器人运行总的时间（time）	1.071600000000000e+03s
机器人走过全部的距离（Journey）	6.429599999998987e+02m

六、总结

本文首先对GPS原始数据点中坏点的去除与经纬度坐标的转换得到了需要被跟踪的轨迹线段，但是通过仿真发现，最好还是要对处理好的数据进行滤波处理，比如卡尔曼滤波。其次通过对差分移动机器人运动原理与控制过程的分析成功建立了机器人运动学模型，并成功用在了之后控制算法中。
最后通过仿真发现：基于PID控制的差分移动机器人轨迹跟踪可以实现轨迹的跟踪，但是本文所用的方法:分阶段跟踪类似于点与点之间的路径规划，这是此方法的一点不足之处。此外与基于PurePursuit算法通过仿真跟踪轨迹图对比发现：PurePursuit算法的跟踪轨迹更加顺滑，更符合实际差分移动机器人的运动方式。
通过对表1与表2的系统指标分析发现：
最大误差与均方误差：基于PID控制的差分移动机器人在对轨迹进行跟踪时具有良好的轨迹拟合性。
平均速度与与运行总时间：在相同采样时间下，基于PurePursuit算法控制的机器人在实际轨迹跟踪时速度较快，但是仿真时间偏长。

参考文献

[1] Kanayama Y， Kimura Y， Miyazaki F， et al． A stable tracking control method for an autonomous mobile robot[C],1990.Proceedings.IEEE Xplore,1991:384-389.
[2]Anushree R,Prasad B K S,Design and development of novel control strategy for trajectory tracking of mobile robot:Featured with tracking error minimization[C].IEEE,2016:1-6．
[3]李世华,田玉平.移动小车的轨迹跟踪控制[J].控制与决策，2000,15(5):626-628．
[4]张扬名,刘国荣,杨小亮.基于滑模变结构的移动机器人轨迹跟踪控制[J].计算机工程,2013,39(5):160-164.
[5]席雷平,陈自力,齐晓慧.具有抖振抑制特付的机械臂快速滑模变结构控制[J].电机与控制学报,2012, 16(7):97-102.
[6]Jia P,Song C,Zhang X.Control and simulation of adaptive global trajectory tracking for nonholonomic mobile robots with parameter uncertainties[C],Sensor Network and Automation.
IEEE,2012:314-317.
[7]王宗义,李艳东,朱玲.非完整移动机器人的双自适应神经滑模控制[J].机械工程学报,2010,46(23):16-22.
[8] Swadi S M, Tawfik M A, Abdulwahab E N， et al． Fuzzy-Backstepping Controller Based on Optimization Method for Trajectory Tracking of Wheeled Mobile Robot[C].International Conference on Computer Modelling and Simulation,2016:147-152.
[9]CSDN一些相关文章与实现，同时感谢各位博友
链接: 仿真程序实现(Matlab)link.
链接: 仿真图片link.
此外仿真所用数据如果需要请私聊我