基于SLAM的移动机器人自主返航控制系统设计

0 引言

地面无人作战平台是一种应用于军事领域的特种移动机器人，可代替士兵在危险的战场环境中，执行侦察、排爆和火力打击等军事任务，能够提高作战效能，减少人员伤亡，是作战、反恐的有力助手。

目前，投入使用的地面无人作战平台，通常采用遥控操作的方式进行控制，其智能水平较低，而且严重依赖通信链路。操作中若通信中断或发生故障，移动机器人将处于失控状态，影响军事任务。如果移动机器人能够自主返航至出发点，这将极大地提高机器人的生存能力。

移动机器人自主返航是指在控制信号丢失后，由机器人自身的传感器感知本体和外界信息，建立环境模型，机器人通过这些环境数据，控制自身无碰撞地返航。

精确定位是实现移动机器人自主返航的前提。传统的航迹推测法具有自包含的优点，能够依据运动学模型推测机器人的航迹。该方法在短距离、短时间内能获得较好定位效果，但是随着时间、距离的增加其定位会严重发散。文献[3]采用了单目视觉系统，在航迹推测法记录的路径点中加入了图像信息来提高定位精度，返航时通过当前图像与参考图像之间的图像匹配对机器人位姿进行校正，达到减少甚至消除里程计所造成的误差。文献[4]提出了一种基于全景视觉的机器人自主返航方法，使用SIFT特征作为视觉路标建立一个具有多重特征的稳定位置表示，将当前图像中的路标向量与目标位置的路标向量一一配对，最后计算出一个平均返航向量来完成返航任务。

本文根据无人平台的应用需求，设计了一套基于同步定位与地图创建（SLAM)方法的移动机器人自主返航控制系统。该系统利用激光雷达感知建图实现自身定位，通过建立当前局部地图与已获得的全局地图间的对应关系，减小因航迹推测误差造成的机器人定位的不确定性。当通信信号中断后，机器人自主启动返航模式，系统通过路径规划追踪之前记录的路径点并完成实时避障，精准返航至恢复信号处或起始位置。

1 基于SLAM自主返航控制系统设计

1.1 系统工作原理

移动机器人遥控阶段：操作人员遥控操作机器人从起始位置出发，到达目标点执行任务。系统采集电机码盘数据和激光雷达感知的环境数据，完成同步定位，并创建增量式地图，并每隔一定距离记录路径的绝对坐标点P0、P1、P2…Pn。

移动机器人自主返航阶段：遥控信号消失后，控制系统自主开启返航模式。系统通过里程计与地图信息匹配，确定机器人当前位姿，通过路径规划控制机器人追踪之前记录的路径点Pn、Pn-1…P2、P、P。，舍弃某些路径规划无法到达的路径点，最终返航至恢复信号或起始位置处。

1.2 系统硬件设计

自主返航控制系统的硬件主要由NVIDIA公司的TX1核心处理板、Hokuyo公司的UTM_30LX单线激光雷达、运动控制板（MCU)及Elmo Whistle袖珍型数字伺服驱动器等组成。其中，控制单元由TX1和MCU组成，TX1与MCU之间通过UART串口进行通信，激光雷达作为环境感知传感器。控制系统结构图如图1所示。

遥控阶段，MCU上的无线接收模块接收遥控控制信号，通过CAN总线将信号传给电机驱动器，电机驱动器控制左右电机按照遥控指令旋转。电机码盘采集电机脉冲信息，通过电机驱动器传输至MCU，MCU将电机转速信息通过UART串口传输至TX1，TX1将电机左右转速转化为实际速度并计算里程，同时接收激光雷达感知的环境信息共同完成同步定位与地图创建。

当遥控信号消失后，系统自主开启返航模式。控制单元读取传感器信息，计算机器人当前位姿，通过路径规划算出一条最优可行路径，同时计算出角速度跟线速度并发送给电机驱动器，控制机器人驶向目标坐标点。

1.3系统软件设计

系统整体软件流程如图2所示。系统启动后，接收电机码盘和激光雷达的信息，判断当前遥控信号是否正常。如果正常，则开启遥控工作模式，通过里程计信息和激光雷达感知的环境信息完成同步定位，并创建增量式地图，同时每隔一定距离记录一次路径坐标点；反之，系统自主开启返航模式，按照路径规划已规划好的速度，追踪之前记录的路径坐标点，通过当前建立的局部地图与之前建立的全局地图进行匹配，原路返回至起始坐标点。在遥控阶段与自主返航阶段，地图都持续更新。

2 SLAM方法

2.1 移动机器人定位方法

移动机器人定位方法分为相对定位、绝对定位和组合定位。相对定位法又称航迹推测法，主要依靠里程计、陀螺仪及其他惯性传感器，在给定初始位姿的前提下，通过积分计算机器人行走里程，最终确定机器人相对初始点的距离和方向。绝对定位主要通过导航信标、主动和被动标识、地图匹配和全球定位系统（GPS）进行定位。组合定位是一种基于航迹推测与绝对定位信息矫正相结合的方法。

相对定位方法具有自包含的特点，能够依据运动学模型自我推测机器人的航迹，但是这种方法存在因时间、距离增加而增加的累积航迹误差。绝对定位方法中，信标或标识牌的建设和维护成本较高，GPS只能用于室外。因此，本文采用航迹推测法与地图匹配相结合的组合定位方法。

2.2 同步定位原理

定义机器人在t时刻的状态为s_t激光雷达测量的路标为（θ₁，θ₂，…θ₃，}，u_t为从机器人里程计获取的运动信息，z_t为机器人与路标的距离与夹角信息，n_t为数据关联，则SLAM问题的数学定义可由式（1)表示：

机器人状态的三个分量分别为坐标x、坐标y以及朝向角θ，每一个路标都有x坐标和y坐标。系统属于（2n+3）维高阶非线性问题，因此采用扩展卡尔曼滤波来表述该问题。

系统读取里程计的数据，根据运动控制量u_t预测机器人当前位置，以此作为扩展卡尔曼滤波的先验估计；以激光雷达感知的各路标的距离和夹角信息作为测量信息，并对先验信息进行校正。机器人定位的准确性依赖于环境地图，借助准确的定位信息，机器人能够对环境地图持续准确地更新。因此，在这一过程中，机器人的定位与地图创建同步进行。

对EKF-Based SLAM问题总结如下:

1）预测

系统通过读取里程计的里程信息，更新位置s和预测协方差矩阵P。扩展卡尔曼滤波的预测方程由公式（2）表示。

2.3 地图构建方法及更新原理

常用的地图构建方法有栅格法、特征表示法和拓扑表示法等。特征表示法提取特征困难，并且环境中相似特征很难分辨；拓扑表示法通常使用节点表示某个特定位置，用线段或圆弧表示这些位置的关系，其路径规划效率很高，适用于大规模环境下的应用，但难点在于如何构造与维护拓扑地图。

移动机器人完成地图创建与自主返航任务在室内条件下进行，针对室内环境，选择栅格地图表示法。栅格地图便于表示障碍物，不需要占用大量存储空间。栅格地图将整幅地图分解为许多相同的栅格，对每个栅格来说，只能有3种情况：被占用（有障碍）、自由区域（无障碍）和未知区域。采用二维笛卡尔矩形栅格来表示激光雷达探测到的环境，通过二维数组记录环境地图中对应栅格是否有障碍物信息。

通过式（7)将激光雷达测量的障碍物距离信息映射到机器人的世界坐标系中：

其中，（x’_n，y’_n）表示障碍物在世界坐标系的位置，（x_r，y_r)和θ_r表示机器人参考中心在世界坐标系的位置和航向角，ρ_n和Φ_n则分别表示激光雷达探测到的距离和激光束相对丁主扫描方向的夹角，如图3所示。

将障碍物位置信息映射到环境地图中相应的栅格单元：

其中，（x_g,n,y_g,n)为障碍栅格在世界坐标系的位置，w为栅格宽度。这样就可以把整个环境用栅格表示出来。将激光雷达感知的环境信息整合到世界坐标系中后，需要对相邻时刻的障碍物信息进行数据关联。其基本分析算法如下：

1）利用激光雷达感知的环境信息，建立T周期与T+1周期的障碍物链表。

2）对T+1周期的障碍物链表与T周期的障碍物链表采用最近邻法进行配对，匹配标准是两个障碍物节点估计坐标（x_g,n,y_g,n)之间的距离小于一个阈值，匹配成功的一对障碍物可看做同一个障碍物。若匹配失败，则将该障碍物加入到临时存储区。

3）建立T+2周期的障碍物链表，将临时存储区中的障碍与T+2周期的障碍物进行配对，若配对成功，则将该障碍作为新障碍加入到地图中，否则将该障碍从临时存储区中剔除。

地图更新的整体流程如图4所示。

3 路径跟踪控制方法

3.1 路径跟踪整体流程

路径跟踪的目的是使机器人可以稳定、高效地跟踪几何路径，到达指定地点型。本文根据路径跟踪规划出一条无碰撞的最优路径，并计算出合适的线速度和角速度，移动机器人按照顺序追踪之前记录的路径点，到达目标点附近后，通过当前局部地图与之前建立的全局地图进行匹配，最终返回至起始点。

自主返航阶段路径跟踪控制流程如图5所示，当遥控信号中断后，移动机器人自主开启返航模式。此时，机器人根据传感器信息计算出当前位姿，并追踪路径记录的最后一个路径坐标点，通过路径规划计算出该阶段的线速度和角速度。移动机器人不断接收传感器信息，计算当前位姿，判断目标点是否到达；在与目标点的距离小于一定阀值后，通过当前感知的局部地图信息与全局地图进行匹配，调整移动机器人的位姿使其更加准确地到达目标点。

为避免之前记录的路径点受新障碍物的影响而无法到达，以及提高返航效率，程序中设定：如果在一定时间内未到达目标点，则放弃该点，转为追踪下一目标点；以此类推，直到遥控信号恢复或返回到初始位置。

3.2 全局路径规划

移动机器人路径规划是指在有障碍物的工作环境中，如何寻找一条从给定起点到给定终点的运动路径，使机器人安全无碰撞地绕过所有障碍物，并尽量满足某些优化指标，比如时间最短、耗能最低等。

在路径跟踪过程中，从当前位置追踪目标坐标点时，首先根据全局路径规划一条无障碍的最优路径，本文采用基于概率的路径搜索方法。在栅格地图中，以当前机器中心所在的栅格为当前栅格u，与当前栅格直接相邻的栅格集合为V，v为V中任一节点，目标点所在的栅格为目标栅格，如图6所示。

采用三个参数map_n（u，v)、σ（i）、δ（i）来对路径的优劣程度进行评价。其中map_n（u，v)为栅格u与栅格V的关联程度，当u和v之间无通路时map_n（u，v)为零，否则map_n（u，v)为大于0的常数。

σ（i）为临接栅格v相应于目标栅格的目标趋向性评价因子，临接栅格与目标栅格之间的距离越近，则σ（i）的值越大，反之越小。邻接栅格σ（i）取值都是经验值，在图6中，黑色斜线处为障碍物，当前栅格右上方的栅格因为距离目标点距离最近，具有最高的重要性值10，左下方的栅格因为距离目标点最远，具有最小的重要性值。

δ（i）为当前栅格与某个临接栅格之间的中心距，在距离搜索过程中，应优先选择距离较短的临接栅格。与当前栅格中心距越小的邻接栅格，其δ（i）的值越大。选择最优临接栅格的概率公式如式（9）所示：

其中，j_v为临接栅格的任一节点。

基于概率的路径搜索步骤如下：

1）计算当前栅格周围所有邻接栅格的P(u,v)值，选择P(u,v)值最大的一个作为子目标点ul；

2）对u与ul两栅格之间的关联程度进行修正，使得下一条路径从u选择通过ul的概率会增大，从而形成一种正反馈机制，最终得到最优路径。修正公式如（10）所示（其中B为一固定正常数）：

map_n(u,ul)=map_n(u,ul)+B (10)

3）以栅格ul作为新的起始栅格，重复（1）、（2）步骤，直到得到一条从当前位置到目标位置的无碰撞最优路径。

3.3 局部路径规划

在全局路径规划的基础上，移动机器人为了能够更加实时、准确地躲避障碍物，需要进行局部路径规划。本文采用的局部路径规划方法为动态窗口法。该方法主要是在速度空间（v，ω）中采样多组速度，并模拟机器人在这些速度下和一定时间内的轨迹。得到多组轨迹后，对这些轨迹进行评价，选择最优轨迹所对应的速度来驱动机器人运动。

本文使用的移动机器人平台为两轮差速驱动，无法纵向移动，只能前进和旋转，在机器人的相邻时刻（一般码盘采样周期ms)，运动距离短，因此可将相邻时刻的运动轨迹看作直线，设（v，ω）为机器人坐标系下的线速度和角速度，则通过式（11)的运动学模型可推算机器人在一定时间内的轨迹。

其中，x_t+1，y_t+1，θ_t+1;x_t，y_t，θ_t分别为t+1、t时刻机器人在世界坐标系的位置坐标。

在速度（v，ω）的二维空间内，存在无穷多组速度，但是根据机器人本身的限制和环境限制，可以将采样速度控制在一定范围内。

1）机器人受自身最大速度和最小速度的限制。

其中，v_c，ω_c为机器人当前线速度和角速度，α和α为最大线加速度和角加速度。

3）基于移动机器人安全的考虑，为了在碰到障碍物前停下来，故在最大减速度条件下，速度需要有一个范围。

其中，dist(v，ω)为速度（v，ω）对应轨迹上离障碍物最近的距离，这个判断需要提前模拟出轨迹，然后判断这对速度是否可用。

在采样好的速度组中，有若干条轨迹可行，因此采用评价函数的方式为每条轨迹进行评价，采用的评价函数为：

其中，heading(v，ω)为机器人在当前设定的采样速度下，达到模拟轨迹末端时，机器人朝向和目标之间的夹角，夹角越小，该项权值越大。dist(v，ω)表示机器人在当前轨迹上与最近障碍物的距离，如果在这条轨迹上无障碍物，则将其设为一个常数。velocity(v，ω)用来评价当前轨迹的速度大小。

对三个评价因子分别进行归一化处理，以达到平滑的目的。总的来说，三者构成的评价函数的物理意义是：在局部路径规划的过程中，使得机器人避开障碍，朝着目标以较快速度行驶。

经过全局路径规划和局部路径规划后，机器人按照合适的路线以一定的速度到达要追踪的路径坐标点附近，在机器人中心与目标点的距离小于一定阈值后，首先将当前位置感知局部地图中的障碍物链表与以目标点为中心的局部地图中障碍物链表按照上述2.3节中的方法进行配对，通过配对得出对应障碍物之间的相对位置关系，代入式（7）

4.3 基于SLAM的自主返航实验

为了验证本文设计的自主返航系统的有效性，需要对移动机器人进行室内自主返航实验。图7为返航实验时建立的栅格地图。