移动机器人自主返航控制系统设计与实验

刘开创，施家栋，王建中，李 彬，曹赫

( 北京理工大学 爆炸科学与技术国家重点实验室，北京 100081 )



移动机器人自主返航控制系统设计与实验

( 北京理工大学 爆炸科学与技术国家重点实验室，北京 100081 )

针对遥控移动机器人在通信信号中断后，无法对其进行控制的问题，设计了移动机器人自主返航控制系统，实现信号中断后移动机器人按原路径自主返航直至恢复控制信号或返回初始位置；提出了基于控制意图融合里程计和光纤陀螺仪的定位算法，对移动机器人运动学模型的结构参数进行了校正，大幅度提高了移动机器人定位精度；基于嵌入式Linux平台设计了返航控制软件系统，在信号正常的情况下，启动遥控模式，遥控机器人移动并记录路径坐标点，信号中断则开启返航模式，采用线性控制率和PID控制率实现对移动机器人路径跟踪控制；基于实际应用环境开展自主返航实验验证，实验结果表明该控制系统能够在通信信号中断后以较高的精度控制移动机器人实现自主返航；该系统能够解决移动机器人在应用中的实际问题，且代码可移植性高、通用性强。

移动机器人;自主返航; 融合定位; 路径跟踪

0 引言

移动机器人具有移动功能，广泛地应用于工业、农业、服务和国防等行业。目前无人驾驶汽车和无人战车已经基本能够实现城市化道路和野外环境中A点到B点的自主机动，并开始朝着更高智能的方向发展。然而，小型移动机器人由于其体积和重量的限制，无法携带足够的高精度传感器和高性能的处理器，尤其是在无GPS信号的复杂非结构化未知动态环境中，难以实现长时间、高精度的自主导航。因此，目前小型移动机器人基本上都采用遥控方式进行控制，其智能水平低下，应用有限。由于需要反馈视频、音频、数据等信息，一旦通信信号中断，移动机器人将处于失控状态。如果在完成任务后或通信信号中断时，移动机器人能够自主返回指定地点，将极大提高机器人的生存能力和操作效率。

文献[1]介绍了移动机器人基于GPS和航迹推算(DR)组合定位的方法，根据航迹推算短时间定位精度高、长时间误差发散，使用差分GPS无遮挡定位精度高、信号受遮挡定位精度差的特点，提出了使用改进比例无迹卡尔曼滤波组合定位技术，通过实验验证了该组合定位技术能够提高移动机器人的定位精度。文献[2]是以GPS定位为核心的导航系统，同时融合了方位传感器和光电码盘的数据信息，提高了移动机器人的定位精度，初步完成移动机器人导航任务。文献[3]针对移动机器人室内未知环境下导航问题，使用基于贝叶斯规则的栅格地图表达环境信息，并改进建模精度；使用粒子滤波实现机器人的自定位；通过仿真和实验证明方法的可行性。

本文根据移动机器人的应用需求，设计了一套基于嵌入式Linux平台的自主返航控制系统，实现了遥控过程中的定位和路径记录；通信信号中断后自主启动返航模式，利用机器人的定位和路径跟踪返回初始位置。

1 自主返航控制系统设计

1.1 系统工作原理

操作人员通过遥控方式控制移动机器人从起始位置出发，到达目标区域并执行任务，在此过程中移动机器人控制系统处于遥控模式，实时地采集编码器和陀螺仪数据，基于运动学模型进行定位，并每隔一定距离记录路径的相对坐标点P0、P1、P2…Pn。

当移动机器人行驶或执行任务过程中，通信信号中断，控制系统通过判断有无通信数据自主启动返航模式，移动机器人同样通过编码器和陀螺仪的数据进行实时定位，并跟踪记录的路径点坐标Pn、Pn-1、Pn-2…P0，沿原路径返回直至恢复通信信号或回到初始位置。

1.2 系统硬件设计

移动机器人采用遥控和自主相结合的控制方式，其控制系统结构如图1所示。

图1 控制系统结构框图

自主返航控制系统主要由控制单元、陀螺仪、电机驱动器等组成，如图2所示。控制单元是整个硬件系统的核心，将接收的控制指令转换为驱动器对应指令形式，发送给两侧电机驱动器，驱动电机转动；遥控信号正常时，控制单元接收里程计和光纤陀螺仪数据，同时根据定位算法，并记录路径坐标点；控制信号丢失，开启自主返航模式，控制单元接收传感器数据得出机器人当前位姿，并根据路径跟踪控制算法计算出其所需速度并发送，使机器人趋向于目的坐标点。

图2 控制单元和光纤陀螺仪

电机驱动器控制电机转动的同时，连接电机光电编码器，电机工作时，光电编码器感知电机转速信息，通过电机驱动器发送到控制单元上，电机驱动器同时接收控制单元发送出来的控制指令，控制电机转动；光纤陀螺仪感知移动机器人转弯角速度，并且实时发送到控制单元。

控制单元选用以PowerPC架构为主芯片的嵌入式Linux控制板，该控制板具有运算速度块、存储空间大、接口丰富、实时性好的优点，使用Linux C对控制单元实现移动机器人自主返航控制系统软件开发。使用里程计计算航向角偏差较大，所以引入光纤陀螺仪来计算移动机器人的航向角。本文中光纤陀螺仪选用Fizoptika VG095M光纤陀螺，该陀螺仪输出模拟信号，所以使用数据采集板将模拟信号转换成数字信号，并通过RS232接口和控制单元相连接。由于体积限制，本文中选用Elmo Whistle袖珍型数字伺服驱动器作为直流电机的驱动器，驱动器和控制单元之间通过CAN总线连接。

1.3 系统软件设计

移动机器人控制系统在通信信号正常的情况下，需要接收遥控终端发送过来的控制信号，控制左右两侧电机，驱动移动机器人运动，同时，控制单元采集陀螺仪和编码器信号，基于控制意图实现融合定位，并每隔一定距离记录路径的相对坐标点。处理移动机器人当前的位姿信息并记录路径坐标点。当系统判断在一定时间范围内无通信数据，将自动切换到自主返航模式，系统将实时计算当前的位姿信息，跟踪记录的路径，沿原路径返回直至恢复通信或回到初始位置。

系统软件流程如图3所示，系统启动后，对接口进行初始化设置，接收传里程计和光纤陀螺仪的信息，随后判断远程遥控信号是否正常，如果正常，则开始路径记录模式，将传感器的数据代入运动学模型，计算出机器人当前的位姿， 记录移动机器人所走过的路径坐标点，反之，开启路径跟踪模式，控制移动机器人返航。之后，程序需要挂起一段时间，以匹配控制信号，防止程序执行出错。

图3 系统软件流程图

2 基于控制意图的融合定位方法

2.1 移动机器人定位方法

移动机器人常用的定位方法有相对定位、绝对定位和组合定位。相对定位一般采用航迹推算，通过测量机器人相对于初始位置的距离和角度来确定机器人的当前位姿，该方法常用本体传感器，如里程计、惯性传感器。绝对定位，主要采用导航信标、主动或被动标识、地图匹配或全球定位系统进行定位，该方法通常采用外部传感器，如激光雷达、GPS。组合定位，基于航迹推测与绝对信息较正相结合的方法[4-5]。

本文采用相对定位方法，使用里程计和光纤陀螺仪作为定位传感器。里程计具有价格低廉、采样速率高、在短距离内的定位精度高的优势，但是依靠里程计进行航迹推算定位，长距离运行后累积误差较大，特别是转向角度。因此，引入单轴光纤陀螺仪对转向角度误差进行校正。光纤陀螺仪，具有结构紧凑、精确度高、抗干扰能力强、启动时间短、寿命长等优势。

移动机器人定位原理如图4所示，可以看出在移动机器人在工作时，融合里程计和光纤陀螺仪得到的移动机器人速度，代入运动学模型，计算出移动机器人相对于初始位置的坐标。

图4 移动机器人定位原理框图

2.2 差分运动学模型

移动机器人运动学模型来描述机器人平台的物理学运动学规律，是对移动机器人运动轨迹计算和运动控制的主要依据。本文采用的是航迹推算的定位方法，计算移动机器人相对于初始位置的位姿。移动机器人采用驱动方式为差分驱动，如图5所示。

图5 差分移动机器人运动示意图

在δt的时间内，移动机器人由AB运动到A′B′，在AB处移动机器人位姿为(x0,y0,θ0)，则A′B′处移动机器人的位姿(x,y,θ)，结果为：

(1)

在式(1)中，v、ω为移动机器人直线速度和角速度，如下所示：

(2)

采用里程计进行航迹推算，由于两侧轮径不相等、轴距不精确和平均轮径不精确，这些因素会造成定位过程中的误差。文献[6]提出了UMBmark实验，通过该实验能够校正移动机器人系统参数，文献[7]在UMBmark实验的基础上，提出了校正和补偿系统误差的方法，引入了系统误差来源的3个方面：平均轮半径不确定性Es、有效轴距不确定性Eb、左右轮直径不相等Ed，经过实验校正之后，移动机器人的结构参数如下：

(3)

其中：rL、rR是校正后左右轮半径，ra是左右两侧轮实际平均半径，rn为公称半径，ba为校正后的轴距，bn为公称轴距。

2.3 基于里程计和光纤陀螺仪混合定位算法

里程计和光纤陀螺仪的数据代入运动学模型之前，需要对两者的数据进行数据融合。根据转弯半径的大小，选择里程计或陀螺仪的数据代入运动学模型进行航向角的计算，如图6所示。

根据当前的控制指令求出移动机器人期望转弯半径，计算公式如下：

(4)

式中，vE表示期望移动机器人的直线速度， ωE表示期望移动机器人的角速度，RE表示期望转弯半径。

图6 里程计与陀螺仪数据融合流程图

根据控制指令计算出移动机器人转弯半径，当该转弯半径大于设定的半径阈值Rth，使用里程计的数据计算得到角度值，反之，使用光纤陀螺仪采集的角速率进行转向角度计算。

3 路径跟踪控制方法

路径跟踪控制的目的是使机器人可以稳定、高效的跟踪几何路径，达到指定地点[8]。本文根据控制率计算出合适的直线速度、角速度，使移动机器人按照记录的路径坐标点稳定、高效到达目标位置，并且能够稳定在该位置点。

路径跟踪控制流程如图7所示，控制信号的中断后，移动机器人启动自主返航模式，控制单元接收传感器的数据，代入运动学模型，计算出机器人当前位姿。在路径记录过程中，记录的最后一个路径坐标点为路径跟踪的第一个点。机器人同时不断接收传感器数据，计算机器人当前位置，判断目标点是否到达，并继续追踪下一个点，直至恢复通信或返回到初始位置。

图7 路径跟踪控制流程图

路径跟踪控制的关键是控制率的设计，本文要求移动机器人在返航过程中能够稳定、高效。对于移动机器人的直线速度，|Δθ|较小时，直线速度快，|Δθ|较大时，直线速度慢甚至为零，同时要求远离目标点时速度要快，接近目标点时速度要慢。航向角是控制移动机器人趋向目标点的关键，要求能够准确可靠的转过计算出来的角度，所以对机器人的航向角控制采用PID控制算法。

移动机器人路径跟踪如图8所示，通过对机器人的直线速度和角速度控制，从当前点M行进到目标点Maim。

图8 移动机器人路径跟踪示意图

移动机器人直线速度和角速度采用的路径跟踪控制的控制率为：

(5)

式中，vaim、ωaim分别表示期望达到的机器人直线速度和角速度，vmax表示返航过程中机器人的最大速度，|Δθ|为目标角与航向角之差的绝对值，λ表示航向角偏差绝对值的放大系数， η是一个变量，和Maim与M之间的距离差Δs为线性关系，η=k×Δs-a，且k<1，a为常数[9]。kP、kI、kD为角速度PID控制的比例系数、积分时间常数、微分时间常数，Ts为采样时间， Δθ(k)表示k时刻航向角与目标角的偏差，Δθ(k-1)表示 k-1时刻航向角与目标角的偏差。

4 实验验证

4.1 实验平台简介

本文的实验平台为履带式移动机器人，采用差速驱动的运动方式，能够通过远程终端对其进行远程遥控，电机为MAXON有刷直流电机，公称轴间距为Dn=330 mm，履带轮的公称半径为rn=82.5 mm。

基于图2所示的控制系统，使用嵌入式Linux C进行编程，实现整个自主返航控制系统的定位和控制算法。

4.2 结构参数校正与定位实验

在进行返航实验之前，需要对实验平台的结构参数进行校正。在此实验平台上，参照文献[7]进行参数校正实验，可以求出：

代入式(3)，可以求出校正后的左右轮半径和轴距：

在进行定位实验之前，还需要设定合适转弯半径阈值，经过多次实验，设置转弯半径的阈值Rth=1.5 m。

定位实验中，控制移动机器人行走4×4 m的正方形，获得实验数据如图9所示，图中粗实线表示融合里程计和光纤陀螺仪的定位轨迹，细实线表示只使用里程计进行移动机器人定位的轨迹，实验结果，融合里程计和光纤陀螺仪的定位方法，可以明显提高移动机器人的定位精度。

图9 两种定位方法实验结果

4.3 自主返航实验

为了验证本文设计的自主返航系统的有效性，开展了自主返航实验。首先通过遥控方式控制移动机器人直行4.5米后，原地转弯90度后继续直行4.5米，然后中断遥控信号，移动机器人自主启动返航模式，自主返回起始位置，如图10所示。

图10 移动机器人自主返航实验结果

在实验之前，对PID参数进行标定，PID参数如下：

kI=10 000，kP=0.8，kD=0.05

进行了6组自主返航实验，实验数据如图11所示，并对6个返航终点坐标进行处理，得到平均中心点坐标为(-0.182 9, -0.228 4)。实验结果表明，本文设计的自主返航控制系统能够有效地实现较为精确的返航功能。

5 总结

针对移动机器人在控制信号丢失后会出现失控状态，设计了移动机器人自主返航控制系统。移动机器人定位采用融合光纤陀螺仪和里程计的方法，该方法能够实现移动机器人的精确定位。控制信号丢失后，采用线性控制和PID控制实现移动机器人对路径坐标点进行跟踪控制。通过实验验证移动机器人

自主返航控制系统能够实现短路程的精确返航。

[1] 林 武.基于GPS/DR的移动机器人组合定位技术研究[D]. 南京：南京理工大学, 2011.

[2] 孟祥荔.基于GPS定位的移动机器人导航系统的研究[D]. 天津：天津理工大学, 2007.

[3] 吴月华.室内未知环境下移动机器人导航关键问题研究[D]. 天津：天津大学, 2007.

[4] 于金霞,王 璐,蔡自兴. 未知环境中移动机器人自定位技术[M]. 北京:电子工业出版社, 2011.

[5] 徐 德,邹 伟. 室内移动式服务机器人的感知、定位与控制[M]. 北京：科学出版社，2008.

[6] Borenstein J, Feng L. UMBmark: A benchmark test for measuring odometry errors in mobile robots[A], SPIE Conference on Mobile Robots[C]. 1995,2591:113-124.

[7] Shi J D, Liu J, Wang J Z. Novel method to calibrate kinematic parameters for mobile robots[J]. Journal of Beijing Institute of Technology, 2015, 24(1), 91-96.

[8] 高 健. 小型履带式移动机器人遥自主导航控制技术研究[D]. 北京:北京理工大学, 2015.

[9] 聂 鑫,王耀南,余洪山. 基于行为控制的移动机器人导航方法[J].计算机测量与控制, 2011,19(11): 2699-2701.

Design and Experiment of Autonomous Returning System for Mobile Robots

Liu Kaichuang, Shi Jiadong, Wang Jianzhong, Li Bin, Cao Minghe

(State Key Laboratory of Explosion Science and Technology, Beijing Institute of Technology,Beijing 100081, China)

This paper introduces a control system of autonomous return mode for the mobile robot out of control when the control signal is interrupted. The system can enable the mobile robot to autonomous return follow the original path until the signal is re-connected or returned to the starting position after the interruption of the control signal. A fusion location algorithm based on control intention using odometer and fiber optic gyroscope is proposed. The structural parameters of the positioning algorithm are corrected, and more precise positioning is achieved. The return control system based on the embedded Linux platform is carried out. Remote control mode is activated when the control signal is normal. While mobile robot is moving and records the coordinate points. When the control signal is lost the autonomous return mode will be started, the mobile robot path-following control is achieved by linear control rate and PID control rate. Based on the actual application environment the autonomous return experimental verifications are launched, and the results show that the system is able to control the mobile robot to autonomous return with higher precision after interruption of control signal. This system can solve the practical problems in the application of the mobile robot, and the code is highly portable and general.

mobile robots; autonomous return; fusion location; path-following

2015-09-25；

2015-11-12。

北京理工大学基础研究基金(20130242015)。

刘开创(1989-)，河北邯郸人，硕士研究生，主要从事移动机器人导航、控制方向的研究。

1671-4598(2016)03-0071-05DOI：10.16526/j.cnki.11-4762/tp

TP

A