全方位移动机器人路径规划算法设计

路径生成需要考虑的问题，一是由机器人自身结构和运动特性决定的轨迹约束；二是工作环境的布局带来的运动限制。通常情况下，机器人由一个起始位置（某一个工位）到达一个目标位置（另一个工位）的过程中，不可能以直线方式到达，这时需要重新设置路线绕路到达。在工作环境中也有许多固定的障碍物（如墙、柱子、设备等）需要避开。

路径规划要考虑的就是机器人能以最短的路线到达目标位置，又能顺利避开所有障碍物。为了以最短路线到达，尽量避免通过折线转向的情况，同时要考虑机器人本身的几何尺寸以避开所有障碍物，这需要根据实际应用的环境决定。

网格法是一种比较常用的方法，将整个厂房分为若干个区域和一些节点，每个节点有自己的坐标。网格的单位以一个车身的长度为基准，这样划分的目的是保证机器人从一个节点运动到另一个节点处时走过的距离都是车身的整数倍，便于在遇到障碍物时的通过性计算。障碍物也是必不可少的，它的位置坐标与节点相关，所处位置的节点将标明有障碍物。但是障碍物不能简单地看作一个点来处理，除了障碍物已经覆盖的节点将标明有障碍物外，在障碍物附近的节点与障碍物的最小距离如果没有超过机器人车宽的一半时，机器人也是无法通过的，这样的节点也应算作障碍物。当机器人从一节点前往下一个节点过程中会遇到障碍物时，这条路径就不会被考虑。如图7-1所示深色的节点标明有障碍物，加粗的线表示不被考虑的路线。有了这样的设定就可以开始设计路线了。

图7-1　网格划分规则

为了减小路径生成的时间，我们将机器人所在的当前节点坐标和目标节点的坐标包括该节点机器人的姿态记录在数据表Table1中，数据表Table2记录了每个节点可以前进到达下一个节点而不遇到障碍物的方向，最多为东、南、西、北四个方向，之前考虑的会遇到的障碍物的方向就不会记录在数据表中，有障碍物的节点所有方向的路线都不能前进。机器人经过的节点将从数据表Table2中移除，记录到已经过的节点的数据表Table3中。

当现场环境确定后，划分网格将每个节点的坐标和可继续前进的方向记录到Table2中，确定机器人的起始位置和目标位置，记录到Table1中。准备工作完成后，接下来是路径生成的过程。

首先，机器人从起始位置出发，查询数据表Table2中当前节点位置可选的下一个节点，优先选择与机器人当前运动方向相同的那一条路线，当没有路线可选时才选择返回上一个节点的路线。路线选择完成后，机器人当前点的坐标和机器人的前进方向信息从Table2中移除，记录到Table3中，选择好的新节点和机器人前进方向记录到Table1中，同时在Table2中将这次的路线清除，这样当此方向的路线最终无法到达目标而重新选择路线时，这条路线就不会被重复计算。当机器人到达某个节点处查询数据表Table2发现没有可以前进只有返回上一个节点的路线时，说明此路是无效的，回到上一个节点重新选择路线，并把Table3中这个节点的信息删除，依次类推到在起始点处没有可选路线时，机器人无法到达目标点。到达目标处时根据数据表Table3中的节点记录就能生成机器人运动过程中所走的路线。具体的路径搜索流程如图7-2所示。

当然，在寻找路径的过程中需要制定一些规则（图7-3～图7-7）：（1）当运动方向与目标姿态相反时，将选择转弯；（2）两个目标坐标值都没有找到时，如果机器人的运动方向与目标姿态平行，选择转弯，这样到达目标位置时的姿态才是正确的；（3）当一个坐标值已实现时，如果机器人的运动方向与目标姿态垂直，选择转向；（4）已经搜索过的路线不会重复再搜索一遍；（5）当搜索到重复点时，这条路线会被剔除，重新从重复点处开始继续寻找；（6）机器人要转向时，优先考虑当前位置与目标位置靠近的方向转弯，该方向没有路时考虑另一个方向，都没有路时继续往前行，在下一个点处转弯。

图7-2　路径搜索流程图

图7-3　寻找路径的规则（2）(www.xing528.com)

图7-4　寻找路径的规则（3）

图7-5　机器人避开障碍物

图7-6　到达目标位置姿态不对

图7-7　姿态不对后重新找路

由于机器人应用场合的特殊性，路径规划不仅要实现机器人从出发点准确地到达目标位置，而且要求机器人到达目标位置时的姿态达到预期设定的要求。因为路径规划只考虑东、南、西、北四个方向，所以把机器人的姿态归为垂直方向和水平方向两类。当时，我们认为机器人最终的姿态是水平方向的时，机器人的最终姿态是垂直方向的。这样，在我们的路径搜寻的算法中，就必须要加一些条件，保证机器人到达目标位置时的姿态是正确的。

当目标点的横、纵坐标都没到达时，机器人要沿目标姿态的垂直方向运动，这样才能在某一个坐标到达而转向去搜索另一个坐标位置，姿态能与目标姿态一致。如果沿平行方向运动就要90°转向前进。当有一个坐标已经搜索到，机器人要沿目标姿态平行方向运动，如果与姿态垂直，需要调整方向。

到达目标点后，根据实际要求的目标姿态角，进行曲线拟合。进入角就是路径规划时的姿态，出角就为目标姿态角。这样符合运动姿态的路线就生成了。