基于改进A*算法的POL-Robot路径规划*

褚金奎，李 晋，李金山，张志超

（大连理工大学机械工程学院，辽宁 大连 116023）

0 引言

随着科学技术的发展，智能移动机器人不仅在工业、医疗等行业得到广泛应用，在反恐、排雷、救援和空间等对人有害与危险场合也得到很好的应用[1]。作为智能移动机器人控制系统的重要组成部分，导航技术一直备受关注。偏振光导航是一种新兴的仿生导航方法，其仿生灵感来自沙蚁，相比现有卫星、惯性导航方法等，具有无累计误差、精度高、抗干扰性强等优点，多种偏振光导航方法已被广泛应用到机器人领域。

LAMBRINOS等[2]模仿沙蚁的偏振导航机理制作导航器件，验证了移动机器人仿生偏振导航的可行性，并将全景相机与偏振测量单元组合，提高导航性能；褚金奎等[3]制作了六通道偏振光传感器，设计一种偏振光/GPS/码盘的组合导航系统，实现机器人的二维导航；DUPEYROUX等[4]制作了基于紫外光的点源式单通道偏振光传感器，结合光流传感器提出了一种路径整合策略并装配于六足机器人，在不同的天气、地形下进行了测试补偿；武进等[5]设计了一种偏振光/MIMU/GPS组合导航控制系统，并成功应用到四悬翼无人机中，提高其抗磁干扰能力。褚金奎等[6]设计了一种偏振光/双目视觉仿生组合导航系统，将偏振光传感器和双目相机的数据融合，实现地图构建，可满足室外机器人自主导航需求，但欠缺路径规划功能。本文将基于该系统，融入智能规划算法，增加路径规划功能，实现基于偏振光的移动机器人智能导航应用。

路径规划是机器人完成智能导航任务的前提，是指在工作环境下，基于某种评价指标寻找一条从起点到和目标点的无碰撞路径。现有的路径规划算法主要包括：基于搜索的A*算法[7]、Dijkstra[8]算法等；基于采样Informed RRT*算法[9]等；基于智能方法的遗传算法[10]、蚁群算法[11]等。A*算法被广泛应用于自主移动机器人路径规划，但该算法规划的路径仍存在冗余点、路径曲率非连续等问题，不利于机器人的控制。王殿君[12]以相邻两点为直线的思想，隔项简化，只保留起点、拐点和终点，有效简化了路径点，且能计算出拐点处机器人的旋转方向和角度，但不具备动态避障能力；段书用等[13]提出了安全距离矩阵和与之配合的启发函数来改进传统A*算法，提高了路径规划的安全性，但同时增加了路径长度；有研究提出了通过拓展搜索邻域增加机器人的移动方向，并减小了路径长度，但搜索的效率会变低[14-15]。

综上所述，本文将基于A*算法搜索过程简便的优点，完善上述改进A*算法的不足，从启发函数方面改进A*算法，并提出一种梯度插补方法平滑路径，最后将其应用到自主研发的基于偏振光的移动机器人POL-Robot中，实现POL-Robot的智能导航。仿真与室外实验表明，本文提出的改进方案减少了路径冗余点，且路径平滑，为移动机器人智能导航提供了新思路。

1 偏振光传感器定向原理

太阳发出的自然光在传播过程中遇到大气中的各种粒子等会发生散射现象，在天空中形成稳定的偏振光分布模式，利用瑞利散射理论可建立全天空在晴朗无云条件下的理想大气偏振模式。在某一时刻天空中任意一点偏振光的最大偏振方向即E矢量总是垂直于由太阳S、观测点O和被观测点P组成的平面。倘若处于O点的观察者可以敏感出天空某点的E矢量方向与自身方向的几何关系，则可确定出自身的方向。

本文使用东北天坐标系为导航坐标系（n），偏振光传感器固联于载体坐标系为右前上坐标系（b），图1中hs为太阳高度角，fs为太阳方位角。由偏振光传感器可测得入射光偏振E矢量与传感器体轴的夹角α，经太阳视位置计算得到绝对航向角。

图1 偏振光定向原理

偏振E矢量在世界坐标系中表示为En=[ex ey ez]T，在载体坐标系中表示为Eb=[cosαsinα0]T，通过方向余弦矩阵Cbn可以实现E在b系和n系的转换：

将天顶点作为观测点，传感器体轴可表示为OPn=[ 001]T，即θ=0、φ=0，则方向余弦可简化为式（2），实际实验时，偏振定向传感器若没有正对天顶点，会产生误差，Wei等[16]提出一种补偿算法，有效提高偏振光传感器的输出精度。

太阳视线在导航坐标系下投影坐标为：

由瑞利散射理论可知En=OSn×OPn，联立上式可求得航向角为：

2 基于偏振光的智能导航系统实现

智能导航系统框架如图2所示，本文基于ROS操作系统中构建智能导航系统框架，PC层中，双目相机、偏振光传感器、SLAM、路径规划等均作为节点分布于ROS操作系统中，节点之间的通信消息始终由机器人中的PC处理和传输，其中使用偏振光/双目视觉组合导航方法，将偏振光传感器/双目相机的数据融合，完成SLAM建图，得到二维地图中各点的位置和姿态；路径规划节点处理改进A*路径规划算法与路径优化，使用动态窗口法，用于实时规划路径；以I/O、UART等方式完成Pixhawk处理器与PC的交互通信。因此，通过融合获取的地图及各类传感器信息，可得到导航所需的移动指令，控制移动机器人完成智能导航任务。

图2 基于偏振光的智能导航系统框架

2.1 改进A＊算法

标准A*算法在遍历搜索基础上加入启发函数引导搜索方向，在搜索空间中对每一个可能节点进行代价值评估，每次从队列中选取f(n)值最小的节点作为下一个待遍历的节点，循环遍历直至找到目标节点。其代价值的计算如下所示：

式中：f(n)为当前点n相对于起始节点和目标节点的总代价值；g(n)为从当前点n到起始节点的真实代价值；h(n)为从当前点n到的目标节点的估计代价值。

A*算法中搜索邻域表示机器人的转向范围，常用邻域搜索方式的有4邻域、8邻域、24邻域，搜索邻域数越多时，因需要扩展更多节点，搜索效率会越低，但增加了搜索方向，其最终得到的路径长度会减小。因此，本文中采用8邻域搜索方式。

启发函数h(n)直接影响A*算法的运行效率，设定当前节点n到的目标节点的真实代价值为H(n)，当h(n)＞H(n)时，扩展节点较少，运行效率较高，但通常不会得到最短路径；当h(n)＜H(n)时，A*算法可规划出较短路径，但其扩展的节点更多，运行效率更低；当h(n)=H(n)时为理想情况，能得到最优路径且不扩展多余节点，运行效率高。二维地图中常用的计算启发函数的方法有曼哈顿距离如式（6）所示，欧几里得距离如式（7）所示。在实际应用中，这两种启发函数都不是最佳选择，曼哈顿距离为两个坐标点之间横轴与纵轴的绝对值之和，但忽略了对角线距离；欧几里得距离为两点间直线距离，但其忽略了障碍物情况。因此本文结合曼哈顿距离和欧几里德距离，提出一种新的启发函数，其计算如公式（8）所示：

式中：n.x、n.y为二维平面下机器人当前位置坐标；goal.x、goal.y为终点坐标。

在12×16的栅格地图下，对应用不同启发函数的A*算法进行验真，实验结果如图3和表1所示。由实验结果可得，本文改进的启发函数较传统启发函数，生成为路径长度更短，路径较平滑，总的运行时间也更短。

图3 不同启发函数生成的路径图对比

表1 实验数据对比

2.2 路径优化处理

改进A*算法在路径回溯过程中，连接各个栅格的中点即可生成路径，但会造成路径中转折点过多、路径不平滑等问题，如图4虚线所示，导致机器人出现多次不必要的启停，影响机器人的寿命及移动效率，需进行路径优化处理。

图4 不同启发函数生成的路径图对比

对此，本文提出一种基于g(n)的梯度插补方法，从目标点到起始点过程中，g(n)函数值逐渐减小，通过g(n)函数对影响方向的栅格节点进行加权梯度计算，得到下一个路径点最终的前进方向，然后以较小的步长分割路径，得到更多的路径点，重复此过程即可插值得到平滑的路径。具体实现如图4实线所示，Q为起始点，M为目标点即当前点a，a的父节点为c，dx、dy为离目标点a的偏离量，b、d所在栅格为空白可通过区域，原路径为ac，影响路径方向的节点有a、b、c、d，考虑路径可通过当前节点周围非障碍物的其他空白栅格，通过g(n)函数对a、b、c、d节点进行加权梯度计算，得到新的路径点a1，重复此过程，即可获得平滑路径。

基于g(n)的梯度计算公式为：

式中：θ为路径回溯过程中的路径点，第一个路径点为目标点a；η为步长，设置η为0.5；g(n)为关于坐标x、y的目标函数，即g(θ)=J(x,y)；∇θJ()θ为参数θ处的梯度。

因此，θ的更新公式为：

其中，初始偏移量dx、dy计算公式为：

取grad(θ)=∇θ+1J(θ)，分别计算栅格节点a、b、c、d的梯度值，加权后得到偏移量dx和dy的更新值。

代入公式，获得下一个路径点坐标值：

综上所述，路径优化处理具体流程如图5所示。

图5 路径优化处理流程

3 室外智能导航实验

3.1 实验平台

自主搭建移动机器人POL-Robot平台如图6所示。本平台采用模块化设计，独立的传动系统和上位机装置系统，便于规划与管理。基于Pixhawk搭建控制系统，实际工作时，Pixhawk以串口形式接受PC生成的控制指令，融合偏振定向传感器、双目相机、IMU等信息控制机器人移动，同时将机器人的实时位姿和速度信息发送给PC端。

图6 POL-Robot平台

3.2 路径规划实验与结果

为验证改进A*算法的有效性，基于POL-Robot平台在某地进行实验，如图7所示。使用偏振光/双目视觉组合导航方法，将偏振光传感器/双目相机的数据融合，完成SLAM建图，得到全局静态地图，即一张12×16的栅格地图，其中，每个栅格的长宽为50 cm。在此基础上，分别在两种不同的地图环境下验证改进效果。

图7 实际环境

分别以标准A*算法和本文改进A*算法在两种不同的地图环境下验证，如图8所示。表2所示为两种算法规划路径的对比。

表2 实验数据对比

图8 标准A*算法与改进A*算法在不同地图下生成的路径对比图

实验结果证明，两种算法均能准确到达目标点，本文改进A*算法能够有效完成POL-Robot机器人的路径规划，相比标准A*算法，改进A*算法生成的路径中转折点个数平均减少53.8%，路径更加平滑，路径更加平滑，可减少机器人的启停及加减速次数，且路径长度更短，平均减小6.1%，极大增加了基于偏振光的移动机器人在复杂环境下智能导航的效率与稳定性。

4 结束语

偏振光导航及其组合导航已被广泛应用到机器人领域，具有良好的自主性和抗干扰性。基于偏振光的组合导航系统，可满足室外复杂干扰环境下机器人导航时的精度和可靠性要求，但欠缺路径规划功能。为实现偏振光移动机器人的智能导航应用，本文将基于该系统，融入智能规划算法，实现路径规划功能。针对传统路径规划算法存在搜索效率低、生成路径曲率非连续等问题，本文提出一种改进A*算法和轨迹优化方法，首先，结合曼哈顿距离和欧几里德距离改进其启发函数，然后，利用梯度插补方式平滑路径。仿真及实验表明，本文改进A*算法能够有效完成POL-Robot机器人的路径规划，相比标准A*算法，改进A*算法生成的路径中转折点个数平均减少53.8%，路径更加平滑，可减少机器人的启停及加减速次数，且路径长度更短，平均减小6.1%，极大增加了基于偏振光的移动机器人在复杂环境下智能导航的效率与稳定性。