0 引言
近年来，煤矿安全生产正面临着巨大挑战，机器人化替代已成为重要的解决办法。国家煤矿安全监察局于2019年发布了《煤矿机器人重点研发目录》[1]，其中包括应用于掘进、采煤、运输、安控和救援5类38种煤矿机器人，并对每种机器人提出了基本功能要求。智能矿山建设的核心架构是机器人化开采，煤矿机器人作为煤矿智能化的重要载体，是实现智能无人开采的重要途径[2-4]。煤矿机器人在煤矿日常生产作业、监测维护、矿难救援方面发挥着重要作用，但针对煤矿井下这一复杂环境，智能移动机器人的运动规划问题讨论较少，仍有诸多关键问题尚未解决。
煤矿井下智能移动机器人的导航问题就是要回答“Where am I[5]” “Where am I going[6]” “How should I go there[7]”,分别对应机器人定位[8]、规划和控制，其中运动规划是智能移动机器人研究的核心内容之一。运动规划是实现从一个状态到另一个状态整个过程的规划。移动机器人运动规划是最顶层概念，包括路径规划(不包含时间维度)和轨迹规划(包含时间维度)，其中，轨迹规划又可分为轨迹生成和轨迹优化。
目前，移动机器人路径规划方法较多，代表性的研究方法有人工势场法[9]、蚁群算法[10]、RRT*算法[11]、A*算法[12]等。其中，人工势场法用势能方程来描述物体所在的构造空间，能很好地实施底层的实时控制，但是缺乏地图全局信息，不适合应用于机器人全局路径规划。蚁群算法的应用较为成熟，其原理是根据蚁群相互协作寻找食物来求取最优路径，但容易陷入局部最优，且收敛速度慢。RRT*算法是基于树扩展的路径规划器，随着计算时间的增加可保证收敛到最优路径，但该算法需要庞大的搜索空间，效率不高。A* 算法具有搜索效率高、规划速度快等特点，避免了搜索过程中容易形成局部最优，在机器人路径规划中得到了广泛应用。
相比路径规划只考虑机器人的几何约束，轨迹规划则需要结合机器人的运动状态规划一系列合理的控制量，目前较为常用的方法有动态窗口法[13]、参数化最优控制法[14]和基于能量损失最小(Minimum Snap)的轨迹规划法[15]。其中，动态窗口法需要在速度空间中采样多组数据，模拟机器人在这些额定速度下一定时间范围内所生成的轨迹，并对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动，但如果窗口太大，可能会陷入局部最优解，若窗口太小，虽然可以在障碍物中任意游走，但效率太低。参数化最优控制法为给定起点、终点的状态(包括位姿、曲率、速度)等，找到一个合适的控制输入，产生相应的轨迹，且轨迹需要满足动态模型的约束，但符合要求的控制量存在多个，从多种可能中选取最优控制将给系统带来巨大的计算负担。基于Minimum Snap的轨迹规划法在目标函数中引入了光滑性约束与连续性约束，求解出了可用于移动机器人运动规划的7次多项式，包含时间分配、速度规划、加速度规划、加加速度规划，但所生成的轨迹只考虑了中间点的光滑性和连续性约束，并没有考虑轨迹的形状，即是否会发生碰撞。
针对以上问题，本文提出了一种煤矿井下移动机器人运动规划方法，仿真实验验证了该方法的正确性和有效性。
1 运动规划方法
煤矿井下移动机器人运动规划方法由路径规划、轨迹生成、轨迹优化3个部分构成，其中路径规划采用基于图搜索的A*算法实现。轨迹生成通过构建基于Minimum Snap的目标函数，并施加等式约束来实现。轨迹优化则是在轨迹生成的基础上施加不等式约束来实现，通过引入调和函数Bezier Curve，构建基于Bezier Curve的Minimum Snap的轨迹优化问题，使目标函数的求解变得简单，最终生成一条适用于煤矿井下移动机器人的能量损失最小、连续、光滑、无碰撞、可执行的运动轨迹。运动规划方法流程如图1所示。
图1 运动规划方法流程
Fig.1 Flow of motion planning method
2 基于图搜索A*算法的路径规划
路径规划是指以一定的搜索策略在地图中找到一条由起点到终点无碰撞的连续路径。煤矿井下移动机器人应遵循效率优先原则，因此，应搜索一条最短路径作为全局路径。A*算法是一种应用最广泛、最高效的最短路径搜索算法，其核心在于评价函数的设计，算法中距离估算值与实际值越接近，最终搜索速度越快。A*算法流程如图2所示，分为开始搜索、路径排序、继续搜索3个步骤。
图2 A*算法流程
Fig.2 A* algorithm flow
2.1 开始搜索
(1) 从起点开始，将起点加入到一个由节点组成的open list(开放列表)中，起始阶段的open list中只有一个起点，逐渐会加入更多的项。open list是一个待检查的节点列表，路径可能经过这些节点，也有可能不经过。
(2) 查看与起点相邻的8个节点，首先将机器人无法到达的节点和地图边缘所占用的节点去除，然后将其他节点加入到open list中，并将起点设置为这些节点的父节点。
(3) 将起点从open list中移除，加入到close list(封闭列表)中，为后续步骤做准备。
2.2 路径排序
(1) 计算组成路径的节点，定义第m个节点的评价函数为
f(m)=g(m)+c(m)+h(m)
(1)
式中：f(m)为从起始节点经由节点m到目标节点的评价函数；g(m)为起始节点到当前节点m的代价函数；c(m)为当前节点m到临近子节点mi(i取1～8)的代价函数；h(m)为临近子节点mi到目标节点估计路径的代价函数。
(2) 上述代价函数采用二维环境下两点之间的欧氏距离来计算，具体表达形式如下：
(2)
式中a、b分别为首末两点的坐标信息，a点坐标为(xa,ya),b点坐标为(xb,yb)。
(3) 遍历open list所加入的节点，选择使得评价函数f(m)最小的临近子节点mi为机器人下一步前进的子节点。
2.3 继续搜索
(1) 从open list中选取使得评价函数f(m)最小的临近子节点mi，并将其放到close list中。
(2) 检查所有与mi相邻的临近节点(排除不可行区域)，如果临近节点不在open list中，则加入open list，重新进行评价函数的计算；如果临近节点在open list中，则检查这条路径是否更优。
(3) 如果路径更优，则将该节点的父节点替换为当前所遍历的子节点，然后重新计算该节点的评价函数，如果不是最优，则不操作。
(4) 随着终点加入到open list中，表明全局路径已经找到，此时close list中所记录的节点即为A*算法搜索到的从起点到终点的全局最优路径的结果。
2.4 仿真实验
建立10 m×10 m的随机地图，使用上述基于图搜索的A*算法在随机地图上进行路径搜索。A*算法路径规划结果如图3所示，图中绿色点代表地图中出现的障碍物(障碍物出现的概率为20%)，起点坐标为(0.5,0.5)，终点坐标为(10,10)，彩色的连线为使用欧氏距离作为代价函数从起点开始搜索迭代至终点所得到的全局最优路径。从图3可看出，虽然A*算法找到了一条从起点到终点的全局最优路径，但是路径所经过的节点不够平滑，造成了大量的能量损失。
图3 A*算法路径规划结果
Fig.3 A* algorithm path planning results
3 基于Minimum Snap的轨迹生成
路径规划是在地图中搜索一条从起点到终点的路径，所得到的路径是由一系列离散的空间点组成，并且比较稀疏，不光滑，不含有任何时间信息，因此，无法直接转换为执行器运行控制量。为了减小能量损失，更好地控制机器人运动，需要将稀疏的路径点变成连续、光滑的曲线。同时，需要满足连续性、光滑性的约束条件，计算出机器人在不同时刻的合理控制量，包含时间(time)分配、速度(velocity)规划、加速度(acceleration)规划，并保证在同一时刻各控制量的连续性，这就需要进行轨迹规划。
3.1 基于Minimum Snap的目标函数构建
通常满足约束条件的轨迹有无数条，但实际问题中，往往需要一条特定的最优轨迹，所以，需要构建一个最优的函数，在所有可行的轨迹中找出那条最优的特定轨迹。基于Minimum Snap的轨迹规划是为了控制移动机器人加加速度的变化率最小，因此，针对首末状态下约束位置、速度、加速度、加加速度，需要构建7次8阶多项式的目标函数才能满足方程半正定求解要求。本文路径规划的结果为N-1个中间点，连同起点和终点，可将待生成的轨迹分割为N段，其中任意一段轨迹可以表示为
(3)
式中：pi为多项式系数(i=0，1，…，7)；t为当前轨迹的时间。
整个轨迹可以看作是由N段给定时间的分段轨迹组合而成，可以表示为
(4)
式中T为整段轨迹总时间，T=(T1-T0)+(T2-T1)+…+(TN-TN-1)。
相应任意一段轨迹的加加速度的变化率可以表示为
(5)
将式(5)代入式(4)中，则可以表示出整段轨迹的Minimum Snap最小化目标函数定义为加加速度变化率的平方在每段轨迹相应时间段内的积分，通式可以表示为
J(T)=
(6)
式中：pi，pl均表示轨迹多项式各阶的系数，i,l均为轨迹多项式的阶数，j表示不同时间段内各自的轨迹序号(j取1～N)。
将各段轨迹的Minimun Snap最小化目标函数表示为矩阵形式：
(7)
式中：pj为第j 段轨迹多项式系数矩阵：Qj为第j段轨迹多项式信息矩阵。
将式(7)代入式(6)，扩展成整个轨迹的Minimun Snap最小化目标函数，具体表达式为
J(T)=
(8)
由式(8)可知，整个轨迹的信息矩阵为对称阵，因此，目标函数的最小化求解问题建模成了一个数学上的二次规划(Quadratic Programming，QP)问题。本文对二次规划问题通过构建等式约束求解，分别包括连续性等式约束和光滑性等式约束。
3.2 连续性等式约束构建
轨迹的连续性约束就是在轨迹的中间连接点处，即第j段轨迹段尾与第j+1段轨迹段首处的位置、速度、加速度、加加速度分别相等，因此，构建连续性等式约束：
⟹
(9)
式中：k为导数阶数(k=0，1，2，3)，分别代表各段轨迹位置、速度、加速度、加加速度的表达式；pj,i，pj+1,l分别代表第j段轨迹段尾与第j+1段轨迹段首的多项式系数。
3.3 光滑性等式约束构建
轨迹的光滑性约束就是指轨迹在起点、终点及在每2段轨迹的路径连接点处，移动机器人所经过的位置相对固定，其速度、加速度、加加速度都能满足某一特定值，因此,构建光滑性等式约束：
⟹
⟹
(10)
3.4 轨迹多项式求解
将式(9)、式(10)代入式(8)联立求解，分别对x轴、y轴求解基于Minimum Snap的目标函数，计算满足约束条件的每段轨迹系数P，将系数代入每段状态方程中求解出整段轨迹的各个方向的状态量，在Matlab中可以使用quadprog() 函数求解。
3.5 仿真实验
基于图搜索的A*算法快速规划出一条可通行的全局路径作为基于Minimum Snap轨迹生成的初值。在此基础上，对连接各段轨迹的中间点进行等式约束，使其满足光滑性约束和连续性约束。同时，使整段轨迹的目标函数能量损失最小。基于Minimum Snap的轨迹生成结果如图4所示，绿色连线为轨迹生成后的结果。从图4可看出，绿色连线虽然在图3基础上进行了轨迹生成，使得轨迹平滑，但是部分节点出现了超调现象。
图4 基于Minimum Snap的轨迹生成结果
Fig.4 Results of trajectory generation based on Minimum Snap
4 基于Corridor的轨迹优化
基于Minimum Snap的轨迹生成只限制了轨迹必须要经过起点、终点以及中间路径点，对轨迹本身没有进行约束，且在目标函数求解过程中容易出现超调现象。所生成的轨迹只考虑了轨迹曲线的光滑性约束和连续性约束，并没有考虑到其是否会发生碰撞。因此，基于Minimum Snap轨迹生成，期望规划出的轨迹和路径大致重合，而且轨迹中的速度和加速度不超过最大限幅值。通过构建基于Corridor的不等式约束和时间分配调整来解决超调问题[16]。
4.1 基于Corridor的不等式约束构建
为了避免发生碰撞，需要限制轨迹的形状，期望规划出的轨迹必须在Corridor内。如果能把可通行区域当作约束加入到QP问题中，那么解得的轨迹自然就在Corridor内。然而，不管是等式约束还是不等式约束，都是针对一个特定的时刻，而实际希望的是所有时刻T轨迹都在Corridor内。因此，在路径上采样一些中间点，并对中间点都加上Corridor的不等式约束。尽管这种方法只能保证采样点在Corridor内,但实际过程中，如果Corridor的大小和采样步长设置合理，可使中间点位置倾向实现，但不一定严格满足，即可规划出更加高效合理的轨迹。同时，可以避免轨迹出现超调现象。
构建Corridor的数学模型，对于中间每一个采样点的位置施加不等式约束，即
(11)
式中：xmax，xmin分别为Corridor的x轴上下界；ymax,ymin分别为Corridor的y轴上下界。
4.2 时间分配调整
时间分配是轨迹规划中很关键的一个问题，它直接影响规划结果的好坏。有了时间分配，就可以规划得到轨迹参数，对于轨迹参数，求速度和加速度曲线的极值，判断是否超过最大限幅，如果所有极值都小于最大限幅，则得到可行轨迹。如果不满足，则需要调整该段的时间。显然，增大时间会使速度和加速度都变小。通常以一个固定的比例来调整时间，并进行迭代，每调整一次，重新生成轨迹并进行时间分配，如若不满足，再次加大时间，直至满足要求为止。
4.3 仿真实验
基于图搜索的A*算法快速规划出一条可通行的全局路径作为轨迹生成的初值，对各段轨迹连接的中间点同时施加等式约束和不等式约束，使得整段轨迹所经过的所有中间点都被限定在一定的范围内，且都倾向于实现，但不一定严格满足。基于Corridor的轨迹优化结果如图5所示，最终生成一条移动机器人由起点到终点的能量损失最小、连续、光滑、无碰撞、可执行的运动轨迹。可以看出，图5是在图4基于Minimum Snap轨迹生成的基础上，施加了基于Corridor的不等式约束(即将位置、速度、加速度限定在某一范围内)，得到了彩色圆圈所表示的最终优化后的轨迹。
图5 基于Corridor的轨迹优化结果
Fig.5 Results of trajectory optimization based on Corridor
4.4 运动轨迹结果分析
设定整体规划时间为12 s，将整段轨迹分为11段，每段轨迹的运行时间可根据轨迹的具体情况进行调整，并对速度和加速度的最大值和最小值进行限定。通过构建基于Minimum Snap的目标函数，同时引入连续性、光滑性的等式约束和对位置、速度、加速度范围限定的不等式约束，得到的轨迹优化结果包括时间分配，如图6所示，表明每段轨迹的时间是根据约束条件自由分配的，目的是使优化生成的轨迹更加符合实际情况。整段轨迹不同时刻的位置规划、速度规划、加速度规划如图7所示，速度和加速度在整段轨迹的任何时刻都是光滑且连续的，不会出现突加或者突减的情况。
图6 各段轨迹的时间分配
Fig.6 Time allocation of trajectory
(a) 位置规划结果
(b) 速度规划结果
(c) 加速度规划结果
图7 整段轨迹位置、速度 、加速度规划结果曲线
Fig.7 Curves of planning results of whole trajectory of position, speed and acceleration
5 基于Bezier Curve的轨迹优化
基于Bezier Curve的Minimum Snap轨迹优化问题包含构建基于Minimum Snap的目标函数，引入等式约束和不等式约束，整合整段轨迹所有待优化的控制量，引入一个调和函数Bezier Curve多项式，根据控制点插值生成优化后的机器人运动轨迹Bezier Curve。轨迹原始目标函数经Bezier Curve转换后起始于第一个控制点，终止于最后一个控制点，并且不通过中间的控制点，且所生成的轨迹将会被控制点所形成的凸包完全包围。同时，轨迹高阶导数的控制点与原有控制点之间存在某种线性关系，使得位置、速度、加速度、加加速度之间的求解变得简单，不需要迭代求解。
5.1 Bezier Curve多项式建立
Bezier Curve多项式参数方程的表达式由式(3)变换为
Bj(t)=
(12)
式中：
为凸包多边形的i+1个控制点；
为伯恩斯坦(Berstein)基函数，其表达式为
(13)
式中
为Bezier Curve多项式n次i阶的组合。
由式(12)可知，n次Bezier Curve多项式由控制点以及对应的权重函数
组成。归一化Bezier Curve的高阶控制点与低阶控制点之间存在线性关系，可以表示为
(14)
式中：
为u轴上的第j段轨迹l阶i次Bezier曲线的控制点；
为u轴上的第j段轨迹i次Bezier曲线的控制点。
5.2 约束条件构建
固定等式约束主要包括起点、终点的位置、速度、加速度、加加速度等，通常表示为
(15)
式中：
为第j段1～l阶的标量系数(为保证归一化后数值优化的稳定性)；
表示u轴上的第j段l阶为某一定值。
连续性等式约束主要用于两端Bezier Curve连接处，保证位置、速度、加速度、加加速度连续，通常表示为
(16)
安全性约束主要通过Bezier Curve使所有控制点在Corridor内，由于Bezier Curve的凸包特性，使得整段Bezier Curve都处于可通行区域。通常安全性约束表示为
(17)
式中
为最大、最小的极限位置。
动力学约束主要针对移动机器人的物理运动极限，通常表示为
(18)
式中：
为最大、最小速度；
为最大、最小加速度。
5.3 仿真实验
设定机器人运行的最大速度为0.4 m/s，最大加速度为0.4 m/s2，所生成的每段轨迹速度和加速度的曲线如图8所示。从图8可看出，轨迹原始目标函数经Bezier Curve转换后起始于第一个控制点，终止于最后一个控制点，并且不通过中间的控制点，使轨迹高阶导控制点的求解变得简单，且所生成的轨迹将会被控制点所形成的凸包完全包围。
(a) 速度曲线
(b) 加速度曲线
图8 每段轨迹的速度、加速度曲线
Fig.8 Curves of speed and acceleration of each trajectory
6 结论
(1) 提出了一种煤矿井下移动机器人运动规划方法，该方法的路径规划采用基于图搜索的A*算法，轨迹生成通过构建能量损失最小的目标函数实现，引入等式约束和不等式约束进行轨迹优化，最终生成一条适用于煤矿井下移动机器人的能量损失最小、连续、光滑、无碰撞、可执行的运动轨迹。
(2) 基于图搜索的A*算法通过开始搜索、路径排序、继续搜索3个步骤循环迭代，在地图上快速、高效地搜索出了一条最短路径作为全局路径规划的结果，为轨迹生成提供初值。
(3) 构建基于Minimum Snap轨迹生成的目标函数，通过引入连续性、光滑性的等式约束，使得A*算法搜索出的全局路径由一系列稀疏离散的点转换成连续、光滑的曲线。
(4) 通过调整时间分配和构建基于Corridor轨迹规划的不等式约束，来解决轨迹生成求解过程中出现的超调现象，并对整段轨迹本身进行约束，避免发生碰撞。同时，计算出了机器人在不同时刻的包含时间分配、位置规划、速度规划、加速度规划的状态量。
(5) 引入调和函数Bezier Curve，构建基于Bezier Curve的Minimum Snap的轨迹优化问题，根据具体物理含义的控制点插值生成优化后的机器人运动轨迹，使得轨迹高阶导控制点的求解变得简单，且所生成的轨迹将被控制点所形成的凸包完全包围。
[1] 国家煤矿安全监察局.煤矿机器人重点研发目录[EB/OL].http://www. chinacoal-safety.gov.cn/gk/tzgg/201901/t20190109_223745.shtml,2019.1.2.
State Administration of Coal Mine Safety.Key research and development catalogue of coal mine robot[EB/OL]. http://www. chinacoal-safety.gov.cn/gk/tzgg/201901/t20190109_223745.shtml,2019.1.2.
[2] 葛世荣. 煤矿机器人现状及发展方向[J]. 中国煤炭, 2019，45(7):18-27.
GE Shirong.Present situation and development direction of coal mine robot[J]. China Coal,2019,45(7):18-27.
[3] 王国法, 赵国瑞, 任怀伟. 智慧煤矿与智能化开采关键核心技术分析[J]. 煤炭学报, 2019, 44(1):41-48.
WANG Guofa,ZHAO Guorui,REN Huaiwei. Analysis on the key technologies of intelligent coal mine and intelligent mining[J].Journal of China Coal Society,2019, 44(1):41-48.
[4] 葛世荣, 胡而已, 裴文良. 煤矿机器人体系及关键技术[J]. 煤炭学报, 2020, 45(1):455-463.
GE Shirong, HU Eryi, PEI Wenliang. Classification system and key technology of coal mine robot[J]. Journal of China Coal Society, 2020, 45 (1) : 455-463.
[5] KHAN S, AHMMED M K. Where am I? autonomous navigation system of a mobile robot in an unknown environment[C]// 5th IEEE International Conference on Informatics, Electronics & Vision (ICIEV), 2016.
[6] DURRANT-WHYTE H . Where am I? a tutorial on mobile vehicle localization[J]. Industrial Robot,1994,21(2):11-16.
[7] JIANG Ping, FENG Zuren, CHENG Yongqiang. A mosaic of eyes[J]. Robotics & Automation Magazine,2011,18(3):104-113.
[8] 杨林，马宏伟，王岩，等．煤矿巡检机器人同步定位与地图构建方法研究[J].工矿自动化，2019，45(9)：18-24．
YANG Lin,MA Hongwei,WANG Yan,et al.Research on method of simultaneous localization and mapping of coal mine inspection robot[J].Industry and Mine Automation,2019,45(9):18-24.
[9] ROSTAMI S M H, SANGAIAH A K, WANG J, et al. Obstacle avoidance of mobile robots using modified artificial potential field algorithm[J]. Eurasip Journal on Wireless Communications & Networking,2019,2019(1):1-19.
[10] GARCIA M A P, MONTIEL O, CASTILLO O, et al. Path planning for autonomous mobile robot navigation with ant colony optimization and fuzzy cost function evaluation[J]. Applied Soft Computing,2009,9(3):1102-1110.
[11] ARSLAN O, BERNTORP K, TSIOTRAS P. Sampling-based algorithms for optimal motion planning using closed-loop prediction[C]//IEEE International Conference on Robotics and Automation (ICRA), 2017.
[12] TAN J, ZHAO L, WANG Y, et al. The 3D path planning based on A* algorithm and artificial potential field for the rotary-wing flying robot[C]// IEEE International Conference on Intelligent Human-machine Systems & Cybernetics, 2016.
[13] FOX D, BURGARD W, THRUN S. The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4(1):23-33.
[14] KELLY A, NAGY B. Reactive nonholonomic trajectory generation via parametric optimal control[J]. The International Journal of Robotics Research, 2003, 22(7/8):583-601.
[15] MELLINGER D, KUMAR V. Minimum snap trajectory generation and control for quadrotors[C]// IEEE International Conference on Robotics and Automation (ICRA),2011:2520-2525.
[16] CHEN J, LIU T, SHEN S. Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments[C]//IEEE International Conference on Robotics and Automation (ICRA), 2016:1476-1483.