室内移动机器人路径规划算法

高 翔，郭卫东

（北京航空航天大学 机械工程及自动化学院，北京 100191）

0 引言

路径规划是指在具有障碍物的环境中，按照一定的评价标准，寻找一条从起始位姿到目标位姿的最优路径［1］。根据对环境信息的掌握程度，路径规划技术包括环境已知的全局路径规划和环境未知的局部路径规划［2］。本文采用最优化方法对路径规划问题进行建模，采用带有惩罚函数的改进粒子群算法对路径规划问题的最优化模型进行求解，并在MATLAB中通过仿真试验验证算法的有效性。

1 路径规划建模

1.1 机器人与障碍物的数学表示

将机器人Ar视为二维环境下的自由移动刚体，其最小外接圆定义为机器人圆Cr，半径为Rr，圆心为Or（xr，yr）。以Or为原点，机器人前进方向为X轴正方向，建立机器人坐标系XrYrOr，如图1所示。

其中：g（s）和h（s）均为由机器人声纳安装位置决定的函数；N为非负整数集。

对ΦR的点，将其变换到世界坐标系XgYgOg中得到点集Φg。本算法中，用点集Φg替代障碍物集Obtacle作为障碍物的数学表示。

路径示意图如图3所示。曲线PsPg表示机器人的真实路径。机器人的起始位姿为Ps（xs，ys，θs），目标位姿为Pg（xg，yg，θg），其中θs与θg为在起点处与终点处世界坐标系与机器人坐标系间的夹角。取Ps和Pg间的（s－2）个中间位姿Pj（xj，yj，θj），则P＝｛Ps，P1，…，Pg｝即作为一条候选路径的数学表示。

1.2 路径规划的评价函数

对于路径规划，考虑3个评价指标：路径长度、路径平滑度以及路径安全度。

第1条指标是路径的长度。路径越短则路径越优［3］，其评价公式为：

第2条指标是路径平滑度。机器人的转动与机器人的运动时间有直接的关系，如果路径中存在角度较大的转动，机器人必须减慢速度甚至停止，这样走完路径所耗费的时间就会增加很多，因此将路径平滑度作为另外一个优化指标。路径平滑度定义如下：

其中：Ij（0≤Ij≤π）为向量Pj－1Pj与PjPj＋1间的夹角；k1和k2为调节两部分影响强度的因子。

第3条指标为路径安全度。路径上各位姿与障碍物的距离越远，则该路径越安全。如前所述，本文中用点集Φg来表示障碍物。对于路径中的每个位姿Pj（xj，yj，θj），将位姿与障碍物间的距离定义为机器人中心与所有影响障碍物点垂直于SG连线方向的距离的最小值（如图4所示），对位姿影响的障碍物点定义为沿SG连线方向上与位姿中心的距离小于某个固定值Sf的那些障碍物点。则路径安全度的评价公式如下：

其中：k为位姿Pj的影响障碍物点的个数；dsj为位姿Pj中心与影响障碍物点沿垂直于SG连线方向的距离。

设声纳所能测量的最大距离为Lm，将以机器人坐标系原点为圆心、Lm为半径的圆定义为机器人感知圆Cs，将路径中位于感知圆Cs内的各位姿与障碍物的距离表示的和作为路径安全度的评价函数：

其中：q为位于感知圆Cs内部的位姿Pj个数。

图1 机器人及其坐标系

图2 声纳与障碍物交点示意图

图3 路径示意图

综上所述，路径规划的最终评价函数为：

其中：ω1，ω2，ω3为调节3个评价指标影响大小的参数。

1.3 路径规划的约束条件

在路径规划过程中，还需要满足各种约束：运动学约束、始末位姿约束和避障约束。

（1）运动学约束：机器人在有限时间内只能移动有限距离和角度，因此其速度应该在有限区间内，即：

其中：Xd，Yd，θd均为与机器人硬件相关的参数。

（2）始末位姿约束：机器人的初始位姿和目标位姿均已知，故有以下等式约束：

（3）避障约束：路径规划时，应保证每个障碍物点都位于机器人位姿外接圆之外，即：

综上所述，路径规划问题建模如下：

2 基于粒子群算法的求解

2.1 基本粒子群优化算法

粒子群算法（PSO）的数学表示如下［4］：设搜索空间为r维，粒子总数为u，第l个粒子的位置向量为xl（t）＝（xl1，xl2，…，xlr），瞬时速 度 向 量 为vl（t）＝（vl1，vl2，…，vlr），第l个粒子搜索到的最优位置为pl＝（pl1，pl2，…，plr），整个粒子群搜索到的最优位置为g＝（g1，g2，...，gr）。则每个粒子的速度和位置更新公式为：

其中：c1，c2分别为自身认识因子和社会认识因子，用来调整pl和g的影响强度，c1，c2为正数；r1l，r2l为区间［0，1］内的随机数；ω为速度惯性因子，表示前一时刻速度的影响强度。

2.2 约束处理机制

基本的粒子群算法用于求解无约束最优化问题，因此，对于上述约束优化问题，需要相应的约束处理机制。本文采用文献［5］提出的方法，将罚函数法与PSO算法相结合，在迭代过程中用惩罚方法构造增广目标函数，将约束问题转化为无约束优化问题。加入惩罚项的增广目标函数如下：

其中：f（x）为初始目标函数；γ（t）＝为惩罚函数的因子，t为粒子群算法的迭代次数；P（x）为惩罚项。

3 仿真结果与分析

在MATLAB中使用含有一个多边形障碍物的模拟环境来检验算法的有效性。机器人的起始位姿和目标位姿如图5所示，规划的结果如图6所示。

仿真结果验证了本文中所提算法的有效性。采用这种算法的移动机器人具有良好的避障能力。

4 结论及展望

本文提出了一种考虑路径平滑度和路径安全度的路径规划算法并仿真验证了该算法的有效性。在本文中，采用障碍物点来作为环境的数学表示，这是一种较为简单的方法，在复杂环境中，这可能导致路径规划算法的失效，在将来的工作中，将对障碍物的数学表示作进一步的研究。

图4 路径安全度评价

图5 带有障碍物的仿真环境

图6 规划结果示意图

［1］Roland S，Iiiah R N，Davide Scaramuzza.Introduction to Autonomous Mobile Robot［M］.USA：The MIT Press，2012.

［2］蔡自兴，贺汉根.未知环境中移动机器人导航控制理论与方法［M］.北京：科学出版社，2009.

［3］张纯刚，席裕庚.全局环境未知时基于滚动窗口的机器人路径规划［J］.中国科学，2001，31（1）：51-58.

［4］James Kennedy，Russell Eberhart.Particle swarm optimization［C］//Proceedings of IEEE International Conference on Neural Networks.［s.l.］：IEEE，1995：1942-1948.

［5］张国英，吴艺娟.基于多级惩罚函数的粒子群约束优化算法［J］.北京石油化工学院学报，2008（12）：30-32.