基于全局SLAM的四足机器人路径规划研究

2021-12-31刘宇飞邢伯阳王志瑞

无人系统技术 2021年6期

刘宇飞，江磊，邢伯阳，王志瑞

（中国北方车辆研究所兵器地面平台无人中心，北京 100072）

1 引言

为了实现四足机器人在未知崎岖地形行走，尽管可以通过本体传感器获得机器人的行走状态，但如果没有感知系统，机器人将无法在崎岖地形上自主行走。基于环境感知的四足机器人路径规划可以实现足式机器人的自主行走。

波士顿动力公司开发的小狗机器人是经典的四足机器人，此机器人已经在环境建模和运动规划中进行了研究[1-2]。研究人员使用视觉传感器来构建地形环境，并基于视觉传感器生成三维地图以完成路径规划[3]。波士顿动力公司开发的大型四足机器人配备了GPS、IMU、激光和双目相机传感器[4]。实现了视觉测量、环境地图构建和定位，可以通过感知信息判断机器人的合适落足点[5]。

Belter[6]研究了基于视觉传感器的机器人环境感知和建模。视觉和惯性导航传感器的组合可以实现姿态和运动参数的估计。研究内容包括模拟环境中的地形识别与感知、落足点选择和路径规划[7]。Fankhauser[8]提出了一种基于外部视觉感知的四足机器人运动规划方法。根据获得的地形高程图，规划摆动腿落足点未知，并且基于优化方法完成四足机器人攀爬障碍物和楼梯。控制框架包含地图建模、状态估计、运动控制和规划，并将所提出的方法在四足机器人上进行了验证。

基于EKF 的SLAM 方法是解决定位问题的最早方法[9]，但由于EKF 的计算复杂度较高，因此会降低定位效率。早期的稀疏EIF 算法使用稀疏结构来减少计算量，但结构的稀疏性将导致结果的发散，因此学者开始研究SLAM 问题的稀疏结构[10-11]。基于姿态滤波和稀疏特征点提出了稀疏滤波方法[12]。为了提高滤波算法的精度，提出了一种滑动窗口滤波的方法来提高精度，包含了前一时刻的姿态信息。

足式机器人的导航规划可以分为全局地图的路径规划和局部地图的落足点规划两部分。全局路径规划可以归结为优化问题，其中优化的目标函数是路径。优化问题的约束条件是速度约束以及起点和终点的位置约束，学者们通过将约束问题转化为非约束问题来解决此优化问题[13]。路径规划问题转化为一种图形方法，包括几何方法和采样方法。在成本函数方面，采用了人为设计的成本函数[13]，但是人为设计的函数很难覆盖潜在的问题空间，因此最终提出了一种基于示教的成本函数方法。

本文的主要贡献是开发一种旨在实现自主行走的感知和导航算法，提出了全局SLAM 定位算法来实现地图的重建和定位，提出了用于全局和局部地图的路径规划以实现自主导航和避障任务。

2 基于激光雷达的地形构建

激光雷达可以得到地形环境下的点云信息，通过对点云信息的分析和处理，建立三维地形图。为了更准确地描述地形信息，建立了基于地形建模的4 个坐标系系统，如图1 所示。坐标系系统分别为大地坐标系OW、本体坐标系OB、传感器坐标系OV和腿部坐标系OL[14]。腿部坐标系为传感坐标系为为了建立基于机体质心的地形信息，将激光雷达信息转换到机体坐标系下：

图1 地形建模框架下的坐标系系统Fig.1 Coordinate system for terrain modeling framework

式中，TBW为机体坐标系和大地坐标系之间的转换矩阵，它通过IMU 数据和机器人本体数据得到；TVW为传感坐标系和大地坐标系之间的转换矩阵。

通过激光雷达得到的点云信息生成地形高程图。在地形坐标系上，高度测量被近似成一个高斯概率分布，其中高度平均值为p，标准差为σ2p。如果通过激光传感器可以得到障碍物在传感器坐标系下的位置VrVP，即可以得到在大地坐标系下的高度测量值：

由于在高度测量过程中，物体在两个坐标系统下的横纵坐标不变，因此矩阵P=[0 0 1]。高度测量的标准差如下所示：

式中，ΣV为激光传感器模型的协方差矩阵；ΣP,q为传感器坐标系旋转的协方差矩阵；JV为传感器测量的雅克比矩阵；Jq为传感器坐标旋转的雅克比矩阵。

针对地形高程图的云图更新，采用扩展卡尔曼滤波的方式对更新数据进行数据融合。扩展卡尔曼滤波方程中的状态变量等同于地图信息中的高度信息。扩展卡尔曼滤波更新方程如下所示：

在状态更新方程中，状态变量即是高度信息，Hk为单位矩阵，协方差矩阵Pk等同于高度估计的标准差。因此，将地形高程图估计值通过扩展卡尔曼滤波的方式融合到高度测量值中。

最终可以得到实时更新的地形建模高度信息和高度的标准差，从而得到实时的地形信息。

3 SLAM 全局定位模块构建

3.1 自主定位模块化设计

自主定位模块将融合GPS、IMU、里程计等导航定位信息与全局地图信息、激光雷达信息，完成四足机器人自主行走中准确位姿信息的自主获取，保证机器人能够进行准确的人员与轨迹跟踪。

在本系统的自主定位中，考虑足式机器人大环境自足行走时，需要考虑环境过大带来的地图全局一致性无法满足的问题，以及地图过大带来的占用内存过大的问题。本系统采用拓扑度量定位框架下的地图动态加载方法，将全局地图划分成一系列局部地图，并以带尺度的拓扑地图表示。在定位中，系统采用当前定位结果动态加载邻近地图的方式进行信息融合，并采用记忆机制来克服运动中定位融合匹配失败问题。

3.2 拓扑度量定位框架设计

在已有全局地图的前提下，系统将全局地图划分成一系列局部地图，并保证有一定的重叠。地图的表示形式为拓扑地图，具体为无向图，图中的节点为局部地图，边为局部地图之间的相对变换。在定位过程中，主要分为两层定位步骤，其中第一层负责选取一个节点作为参考点，即根据前一次定位结果，融合GPS、IMU、里程计信息，推算距离当前位姿最近的一个节点作为参考点，用于后续定位的子地图，并将机器人表示在该节点的坐标系下；随着机器人的不断移动，参考点不断变化，即子地图将不断切换。第二层将融合地图信息，通过选取参考点一定大小邻域的若干子地图，并与实时获得的激光数据进行迭代最近点法（ICP）匹配，根据匹配结果，得到当前机器人在参考点下的位姿，完成系统自主定位。

对于Codom，该项约束由多传感器插值里程获得。即从上一个获取激光的时刻开始的IMU 数据，在下一个激光数据获得前，均通过四足机器人的运动模型积分得到相对于上一个时刻的位姿。当下一次激光获取时，以IMU 积分的数据为初值，通过和上一个时刻的激光数据进行迭代最近点法（ICP）匹配，得到相邻两帧的增量式相对变换。作为当前时刻相对于上一个时刻的位姿，IMU 数据的积分清零，清除IMU 积分的累积误差。通过累积一系列相对变换可以最终得到机器人的里程轨迹。有了该轨迹，可以约束变量位姿之间的相对位姿变换需要尽可能和里程一致。以长度为3 的窗口为例，定义里程轨迹的位姿为U，优化代价函数Codom如下：

根据代价函数，取当前时刻和子地图之间的位姿变换，即为当前在子地图下的定位。生成的全局定位地图如图2 所示，机器人可以根据全局地图来规划自己的运动轨迹。

图2 全局地图生成Fig.2 Global map generation

4 全局和局部地图的路径规划

4.1 基于Dijkstra 算法的全局路径导航

路径规划模块将完成四足机器人在全局导航定位地图下的全局路径规划任务，其将跟踪指定的人为设定轨迹点或行人轨迹，根据局部环境信息、障碍物信息等规划安全无碰撞的局部行走路径点信息。系统将采用Dijkstra 算法进行局部路径点规划，再采用TEB 平滑算法进行全局路径的平滑优化处理。

系统采用Dijkstra 算法在局部地图构建输出的实时局部栅格地图中进行路径规划，Dijkstra算法的起点为机器人当前所在栅格，目标点为下一个要到达的目标节点所在栅格。启发式算法（如A*算法）相比于Dijkstra 的搜索空间缩小很多，即在计算量上大大降低。Dijkstra 算法的具体步骤如下。

记栅格地图为G={V,E}，V为栅格，即顶点，E为两点之间的边：

（1）初始时令S={V0},T=V-S={其余顶点}；

（2）从T中选取一个与S中顶点有关联边且权值最小的顶点W，加入到S中；

（3）对其余T中顶点的距离值进行修改：若加进W作中间顶点，从V0到Vi的距离值缩短，则修改此距离值；

（4）重复上述步骤2、3，直到S中包含目标顶点。

经过Dijkstra 搜索，从当前栅格到目标栅格的路径可以得到。

由于栅格地图的分辨率较低，生成的局部路径需要进一步处理。先将栅格路径进行离散采样，再放入TEB 算法进行优化处理。TEB 算法是一个相对较新的全局导航算法，可以输出移动机器人的路径点以及速度，它采用了g2o 优化：将局部路径节点当作节点，即待解量；将节点之间的约束作为条件。经过优化后，局部路径相对会更加平滑，并且可以调节距离障碍物之间的距离。

4.2 基于势场理论的局部地图避障

在局部地图中通过相邻的网格信息来判断障碍物的位置和高度信息，当检测到障碍物高度和尺寸超过了腿部摆动阈值，需要通过避障实现局部地图通过。通过障碍检测的方式去感知物体在全局坐标系下的位置，并依据势场理论完成机器人的自主避障[15]。

设定地图的栅格尺寸为m×n，单个栅格尺寸为a×a，设置栅格信息格式为如式(10)所示：

障碍物在栅格地图中的高度信息可以表示为h[p,q]，代表了指定区域地形范围内的平均高度。

式中，k为指定栅格中的点云数量。

通过变量h[p,q]去评价所在地形区域的平坦程度和地形的几何信息。根据采集得到的点云数据，通过各个区域的点云平均高度差进行比较，变量δWh(i)为每个栅格间的高度差：

栅格信息包含了采集区域的水平和高度值，首先对采集区域的地形综合评估，对高度差进行检测，得到前方路面的高度最低区域，此区域即为机器人的安全通过区域。本文把机器人本体和四条腿近似等价成一个虚拟机体模型，提出了基于人工势场理论的四足机器人自主避障策略，模拟引力场的原理来表达目标位置、障碍物位置以及机器人运动方向的关系。定义障碍物对机器人产生排斥力Fo，目标对机器人产生吸引力Fd，系统的势场Us可以表示为：

根据Khatib 提出的模型，吸引力产生的势函数为：

障碍物体和机器人之间的排斥力产生的势函数为：

根据势函数可得，机器人和障碍物产生的排斥力为：

为了保证障碍物和目标的安全距离，在势场函数中考虑障碍物和目标的距离，修改后的势函数为：

结合吸引力和排斥力产生的势函数，可以得到机器人避障的评价函数：

取机器人在地图中的位置为Gn,n，如图3 所示。对于备选的5 个备选栅格单元，其邻域单元分别为通过势场法计算机器人的行走方向，建立虚拟机体模型包络本体和各腿的运动范围，根据机器人和障碍物之间的安全距离得到四足机器人最优避障运动方向。

图3 虚拟机体模型最优运动方向Fig.3 Optimal locomotion direction for virtual trunk body model

基于本体传感器采集得到的数据，建立一个以机器人本体质心为圆心，质心到腿部足端位置为半径的虚拟机体模型；以包络的虚拟机体模型作为避障运动的采样区域，根据地形高程图得到的数据，计算机器人质心和障碍物之间的垂直距离dOV和质心沿最优方向的距离dOT，基于人工势场理论建立避障评价函数，得到四足机器人的避障最优方向和最短距离，通过计算圆切点M和障碍边界点N的连线，得到四足机器人避障偏航角度φ：

四足机器人根据感知得到的避障偏航角度实现最优运动方向的局部地图避障。

5 试验验证

根据提出的全局和局部地图导航定位和路径规划方法，将感知系统安装在四足机器人上，并进行了野外环境自主试验，四足机器人和感知系统如图4 所示。

图4 四足机器人和感知系统Fig.4 Quadruped robot and perception system

试验结果证明，四足机器人满足上述路径跟踪和速度要求，感知规划决策频率为 5 Hz，障碍避障决策频率和定位迭代频率分别为 10 Hz和 50 Hz。四足机器人的野外环境性能试验如图5 所示。

图5 四足机器人野外环境性能试验Fig.5 Field experimental performance test of quadruped robot

如图5 所示，感知生成的路径点为20 个，第一个目标点为47.25 m，机器人跟踪的最近路径点为0.3 m，基于Dijkstra 算法生成的最优绿色轨迹保证了四足机器人可以根据全局SLAM 定位中的最优路径行走，四足机器人在行走过程中的路径变化如图6 所示。

图6 四足机器人在行走过程中的路径跟踪Fig.6 Path tracking diversification of quadruped robot during walking

基于拓扑度量相结合的定位方法减少了整体采集特征点的数量，采用了点特征和鲁棒线特征。为了显示定位方法的效果，作者计算了特定轨迹上不同方法的成功定位次数，并找到了相应的正确特征匹配的数量。将所提出的方法与 P3P 定位方法进行了对比分析，从统计结果可以看出，提出的定位方法可以利用点特征和更鲁棒的线特征进行定位，总体定位成功的次数比其他方法多，绝对轨迹误差更低。对比了 P3P 方法与本论文方法的正确定位特征匹配的数量，结果如图7 所示，箭头所指红色代表定位成功地点，蓝色代表定位失败地点。可以看出，本文的定位成功次数明显更多，且找到的正确特征匹配的数量也更多，从而表明本论文的定位方法的鲁棒性更强。

图7 两种方法的成功定位次数Fig.7 Number of successful localization between two methods

为了验证四足机器人可以根据雷达信息自主避开局部地图中的障碍物，将基于雷达的偏航角与基于运动学的偏航角进行了对比，如图8 所示。图中的红色实线代表通过雷达信息和评价函数得到的机器人最优可行转向角度，蓝色星线代表了四足机器人本体传感计算得到的偏航角度，通过对比验证了机器人本体可以实时跟踪雷达反馈回的偏航角度。四足机器人运动过程中由于足端运动离散性会有一定的滞后，导致跟踪效果无法实时。由试验结果得知，四足机器人最初与障碍物的距离很近为不可通过地形，通过避障评价函数计算得出机器人的偏航角度，当避开障碍物后偏航角度逐渐减小并继续沿目标点行走。