机器人室内运动轨迹修正控制算法

彭益堂，朱敦尧，唐 健，毛大智

(武汉大学卫星导航定位技术研究中心，湖北 武汉 430079)

移动机器人兼具环境信息感知、动态决策与智能规划、行为精确控制与执行、自主导航与定位等复杂功能于一体[1]，是当前研究的热点，在军事、工业、农业、服务等行业具有巨大的应用潜力。目前，多数移动机器人在室内复杂环境中运动时，仅依赖里程计和声呐等传感器来确定自身位置，机器人的实际运动轨迹和预设轨迹会出现较大偏差，严重影响了机器人的深层次应用开发。本文针对室内机器人运动轨迹不精确问题提出了一种解决方案。

对机器人运动轨迹偏差的限制可从硬件和软件两方面考虑。从硬件角度看，可以采用高精度传感器和高可靠性的运动控制系统来提升机器人导航的精确性；从软件角度看，可以改进机器人导航定位算法，得到精确的位姿参数反馈给运动控制系统。本文主要从软件算法的角度修正机器人运动轨迹偏差：首先，将LiDAR传感器信息转化为正确的运动控制命令，实时地反馈到运动控制端以修正机器人的运动轨迹偏差；其次，通过大量试验优化算法中的轨迹修正模型参数，进一步提升机器人室内运动轨迹的精确度。

1 室内测图运动轨迹控制算法

1.1 机器人室内测图与定位模型

本文在机器人控制平台上安装LiDAR传感器设备，利用开源的Hector SLAM算法包进行调试，搭建SLAM功能模块，实现移动机器人室内未知环境下的同时定位与地图创建，以便对机器人坐标定位信息进行采集。

1.1.1 运动模型

为准确描述移动机器人的运动，需要将实际的运动轨迹参数化，利用数据来表示机器人在环境中的位置和姿态信息，并进行调控，如图1所示。

图1 机器人运动模型[2]

设k时刻机器人运动状态为Xk=(xk,yk,θk)，则基于控制命令的运动模型可表示为

(1)

式中，V为速度；W为角速度；U为噪声项，表示轮子运动中的机械误差或环境噪声；同时有机器人速度

(2)

式中，轮子半径为r；轮距为2l；轮子速度为φ。

1.1.2 SLAM模块

SLAM功能模块能在未知环境中获取机器人的实时位姿信息并创建环境地图，具体处理过程如图2所示。首先，传感器对环境信息进行扫描，并对观测量进行信息处理，对采集到的点云和图像数据进行加工；其次，需要进行特征匹配，在滤波器算法中进行优化，进而呈现出移动机器人在局部环境中的运动轨迹和创建的地图信息；最后，通过SLAM功能模块输出的机器人运动轨迹和地图信息，可以通过轨迹修正算法来调整机器人运动控制状态。

具体环境中，将LiDAR安置在机器人面板上对走廊地图进行扫描时，可以看到SLAM地图创建和真实环境的对比情况，如图3、图4所示。

图2 SLAM框架

图3 SLAM真实测试环境

图4 SLAM地图创建结果

激光测距仪扫描环境信息的过程中，观测量是环境路标和机器人之间的距离和方位角，设路标点为i，传感器位置为s点，这时系统的观测模型为

(3)

由于传感器的位置与机器人的位置之间可相互转换，借此可得到观测值Z和机器人位置之间的函数关系，即传感器观测模型。在观测过程中，观测噪声服从高斯分布，也可以把上述观测量转换到极坐标系中。

1.2 PID轨迹修正控制算法

PID控制(proportion,integral,differential)是比例-积分-微分控制器的简称，分为P(比例单元)、I(积分单元)和D(微分单元)3部分，主要适用于线性反馈控制系统。在机器人轨迹控制实际应用中，控制器将传感器收集到的地图和定位数据与预期参考值进行比较，然后把差值传送到输入控制端，用于求解所期望的输出数据，新的输入能够保持系统参数向标准值的方向转变。

PID回路包括3部分：传感器测量信息、控制器决策信息、输出设备单元。在回路中利用需求结果和测量结果的差值来计算系统的纠正值，再根据纠正值，做出相应决策，以消除误差。在控制回路中，消除纠正值的含义是指清除当前的误差、平均过去的误差或改变未来的误差。

如图5所示，安置在机器人面板上的激光测距仪信息就是PID控制结构[3]中的反馈信息源，利用信息源做出决策信息是运动控制的关键部分，本文在轨迹修正决策中采取了PI闭环控制算法，利用激光雷达对机器人的定位输出对机器人的运动状态进行实时的反馈式调控。

图5 反馈控制结构

在机器人的轨迹修正过程中主要采用PI控制算法，具体内容分为以下两部分：

1.2.1 方位角修正原理

(1) 转向设置：当机器人偏移预设直线轨迹时，机器人的偏移方向通过激光雷达SLAM输出坐标信息可判断。

(2) 转动角度：转动幅度的大小不是任意值，根据机器人的硬件特性和多次试验测试模拟出最佳的转向参数，对应不同的坐标阈值分别设定不同的转向角度，才能保证修正轨迹的精确性。设机器人横轴上偏离的距离为d，单位为m；运动速度为v,单位为m/s；调整的角度为θ,单位为度。经过试验测试总结，有如下调整关系

(4)

1.2.2 距离修正原理

(1) 方向设置：一般情况下机器人在行进过程中不能到达指定位置，即是积分式的修正控制模式。由机器人累计的距离增量和预设距离的差值可判断机器人修正后的运动方向。

(2) 修正距离：即对机器人SLAM功能模块输出的距离增量和预设距离的差值进行补偿。

2 轨迹修正算法测试

机器人典型的运动轨迹分为直线运动和曲线运动，曲线运动可以看作直线运动的叠加，因此在机器人运动学的研究领域中主要考察机器人直线运动和回环运动的轨迹准确性，本文针对这两种典型的运动轨迹设计了相应的试验，对轨迹修正算法的效果进行分析比较。

2.1 直线轨迹测试

(1) 直线行走测试:改变轨迹修正算法中的方位角调整参数，对比同等条件下，机器人直线运动10 m的修正效果，如图6和图7所示。

图6 轻微修正参数

图7 较佳修正参数

对比分析可以看到，修正参数经过调整后，对机器人运动轨迹的漂移有很好的限制作用，优化后的修正参数能够将机器人运动轨迹限制到厘米级别。

(2) 控制移动机器人在直线上来回行走5 m距离，比较修正前后的轨迹，结果如图8和图9所示。

图8 修正前后直线运动轨迹对比

图9 修正前后运动轨迹放大对比

图9是图8中机器人运动轨迹终点处放大部分。分析图9可知，进行修正后的机器人运动轨迹偏差能够有效收敛，未经修正的机器人运动轨迹一旦发生漂移就会不断累加。

2.2 回环路线测试

为了检测算法修正的稳定性，设计了典型的闭环检测[4]试验：控制移动机器人在室内沿墙行走，轨迹为长方形，本次试验设置在走廊环境，长方形轨迹的长4 m，宽为0.9 m，具体模型如图10所示。

图10 回环测试轨迹模型

试验结果分析可知，在正常的室内环境下，进行轨迹修正和不进行轨迹修正时，机器人按照预设路线的运动轨迹，可以看出轨迹修正算法的可行性。

如图11所示，试验表明修正后的Y轴平均误差在5 cm，未修正Y轴平均误差28 cm。显然，机器人运动轨迹偏差经过反馈控制修正后，可得到有效控制，仅依靠里程计定位的机器人运动轨迹误差在漂移后会不断累加，通过LiDAR传感器信息的约束能够收敛到厘米级别，也验证了轨迹修正算法的可行性和可靠性。

图11 机器人无人状态下室内回环测试

3 结 语

本文针对机器人运动轨迹偏差设计了一套轨迹修正算法，定性和定量地分析了机器人轨迹修正的数学模型和修正参数。通过大量的试验数据构建了机器人轨迹修正控制的经验模型，得到了可靠的轨迹修正控制参数。分析轨迹修正后的多个试验结果可知，对移动机器人室内测图的预设轨迹进行PID轨迹控制算法修正后，机器人室内运动过程中能够实时调整自己的运动状态，不需要附加人为的控制命令约束。另外，进行轨迹修正算法控制的移动机器人能够按照预设路线进行准确移动，具有很高的稳定性，在室内运动过程中能够达到预期的精度要求，经过大量试验得到的轨迹修正控制模型效果明显，验证了本套机器人运动轨迹偏差修正方案的可行性和可靠性。

[1] 李群明,熊蓉,褚健.室内自主移动机器人定位方法研究综述[J].机器人,2003,25(6):560-567,573.

[2] 王鸿鹏.复杂环境下轮式自主移动机器人定位与运动控制研究[D].天津：南开大学,2009.

[3] 董刚.智能小车运动控制系统研制[D].西安：西安科技大学,2009.

[4] 季秀才,郑志强,张辉.SLAM问题中机器人定位误差分析与控制[J].自动化学报,2008(3):323-330.

[5] CAO Zhengcai,ZHAO Yingtao,WU Qidi.Adaptive Trajectory Tracking Control for a Nonholonomic Mobile Robot[J].Chinese Journal of Mechanical Engineering，2011(4):546-552.

[6] NILSSON N J.A Mobile Automation an Application of Artificial Intelligence Techniques[C]∥International Joint Conference on Artificial Intelligence.Las Vegas:Morgan Kaufmann Publishers，1969：509-520.

[7] 徐曙.基于SLAM的移动机器人导航系统研究[D].武汉：华中科技大学,2014.

[8] 殷波.移动机器人同时定位与地图创建方法研究[D].青岛：中国海洋大学,2006.

[9] 徐则中.移动机器人的同时定位和地图构建[D].杭州：浙江大学,2004.

[10] 宋春林.基于激光测距仪的移动机器人SLAM研究与实现[D].哈尔滨：哈尔滨工业大学,2014.

[11] 周华.多传感器融合技术在移动机器人定位中的应用研究[D].武汉：武汉理工大学,2009.

[12] DISSANAYAKE G，NEWMAN P，CLARK S，et al.A Solution to the Simultaneous Localization and Map Building (SLAM) Problem [J].IEEE Transactions on Robotics and automation,2001，17(3)：229-241.

[13] 王璐，蔡自兴．未知环境中移动机器人并发建图与定位(CML)的研究进展[J]．机器人，2004，26(4)：380-384．