[发明专利]一种避停优先的移动机器人运动规划与避障决策方法

权利要求书

1.一种避停优先的移动机器人运动规划与避障决策方法，包括：

A.建立状态自动机模型：

将移动机器人的行驶状态和转移条件抽象为状态自动机模型；移动机器人的行驶状态包括正常行驶状态、避障过渡状态、避障状态、停车状态；正常行驶状态：当最优路径的覆盖区域无障碍物时，移动机器人按最优路径行驶；避障过渡状态：降低移动机器人的移动速度，实时检测最优路径的稳定性；避障状态：移动机器人选择新的最优路径避开障碍物；停车状态：移动机器人将速度降为零，等待稳定的最优路径或操作员下达指令；其中，当最优路径的路径上连续点之间曲率变化小于设定值时，最优路径为稳定状态，当最优路径的路径上连续点之间曲率变化大于或者等于设定值时，最优路径为不稳定状态；

转移条件包括：

a.从正常行驶状态转移到避障过渡状态：当移动机器人检测到当前最优路径上出现障碍物时，移动机器人切换到避障过渡状态；b.从避障过渡状态转移到避障状态：当移动机器人检测到新的最优路径稳定时，移动机器人进入避障状态，切换到新的最优路径，避开障碍物；c.从避障过渡状态转移到停车状态：当移动机器人检测到最优路径不稳定时，移动机器人切换到停车状态，等待得到稳定的最优路径；或者当移动机器人遇到无法避开的障碍物时，切换到停车状态等待操作员指令；d.从停车状态转移到正常行驶状态：当操作员下达开始任务的指令时，移动机器人切换至正常行驶状态；e.从停车状态转移到避障状态：最优路径稳定时；f.从避障状态转移到正常行驶状态：移动机器人按新的最优路径避开障碍物后，切换回正常行驶状态；

B.离线生成备选路径簇：

基于移动机器人当前行驶状态和运动约束，用采样的离散点做前向模拟生成能够使移动机器人到达激光雷达范围边界的不同备选路径的集合，在导航过程中，将存在阻碍移动机器人的障碍物的路径从备选路径集合中删除，得到备选路径簇；

用采样的离散点做前向模拟生成备选路径的集合时，具体包括：

在一个完整的备选路径簇中，将第一次采样按照角度划分为为9组，将第二次、第三次、第四次和第五次的采样依次划分为7、5、3和1组，总计生成条备选路径；进行角度划分时，将移动机器人的运动学约束的左最大转角和右最大转角分别设定正和负，按照每次采样的组数均匀划分；

C.构建高层导航：

在可行驶区域地图和概率栅格地图所构建的距离地图上，结合与移动机器人最近的障碍物的距离以及移动机器人到目标点的欧氏距离对所有的栅格构建代价值，以所述代价值作为启发式函数的规划方向，规划出接近道路中心、远离障碍物的具有全局路径点的全局路径，将全局路径发送至中层导航；

D.构建中层导航：

中层导航订阅当前高层导航下发的基于全局地图的全局路径点，用于对全局路径进行二次规划，包括地图接收处理、混合规划；

地图接收处理：根据移动机器人在全局地图中的位置切换子地图；

混合规划模块：使用混合规划算法在子地图上进行路径规划，并且把规划出的局部路径点发布给底层导航；

E.构建底层导航：

读取离线生成的备选路径簇和中层导航发布的局部路径点，局部路径点即为目标点的序列；移动机器人根据定位信息对激光雷达获取的经过地形分析后的局部点云，按照点云高度进行处理，地形分析部分得到的地面点云，为可行驶区域；地面点云H1以上的点云被当作障碍物并被归为不可行驶区域、在相应的可行使区域中剔除不可行驶区域，高于地面但是低于H1的点云被当做斜坡；按照最优路径选择算法在备选路径簇中选择一条最优路径，超出可行驶区域的备选路径直接剔除，最优路径具有移动机器人的速度和方向信息，移动机器人按得到的速度和方向信息进行驱动。

2.根据权利要求1所述的避停优先的移动机器人运动规划与避障决策方法，其特征在于：每条备选路径由五次样条曲线生成。

3.根据权利要求1所述的避停优先的移动机器人运动规划与避障决策方法，其特征在于：构建底层导航，按照最优路径选择算法在备选路径中选择一条最优的路径时，

（1）基于射线投射的可行路径判断：在二维平面上，一条射线用起点和方向向量表示，其中是射线与x轴的夹角，则射线上与起点距离为t的点的坐标为，其中，；对于给定的障碍物，障碍物的边界由n个点组成，第i个点的坐标为，，则所述障碍物的边界表示为n条线段，第i条线段为：

；

；

其中表示线段上距离起点的比例；所述障碍物用所有线段的并集表示；

交点求取任务：从目标点出发沿着方向发射一条射线，则射线与障碍物的第i条线段的交点表示为：

；

为障碍物边界第i个点的横坐标，为障碍物边界第i个点的纵坐标；

求射线与障碍物所有线段的交点，然后选取距离目标点最近的交点作为有效交点；

根据备选路径改变射线方向后，重复上述交点求取任务，判断每条射线与障碍物是否存在有效交点，如是，则该备选路径不在可行路径上；如否，则该备选路径在可行路径上，由此能够在备选路径簇上筛选出可行路径集；

（2）对可行路径集中每条可行路径进行评分，选取最优路径：在移动机器人的坐标系下定义移动机器人当前位置与目标点的连线方向与移动机器人移动方向的夹角为goalDir，移动机器人当前位置与可行路径终点的连线方向与移动机器人移动方向的夹角为MPDir；则可行路径的评分为：

；

其中，角度差异angDiff=|goalDir-MPDir|；

遍历可行路径集中的每一条可行路径，得分最高的可行路径即为最优路径。