一种避停优先的移动机器人运动规划与避障决策方法

1.本发明涉及移动机器人导航技术领域，具体涉及一种避停优先的移动机器人运动规划与避障决策方法。


背景技术：

2.随着社会的不断进步，移动机器人导航技术得到了快速的发展，人工智能、传感器、应用电子等领域的兴起也为移动机器人导航技术的发展注入了鲜活的活力，这使得移动机器人导航技术渗透到各行各业，应用到了越来越多的任务场景。一般来说，现有的导航避障策略可以分为两类，包括基于传统控制方法的导航避障策略和基于深度学习的导航避障策略。
3.传统控制方法包括：基于pid控制的导航策略：通过对移动机器人速度、航向等参数进行不断调整，使移动机器人保持在规划好的路径上；模糊控制算法：将传感器获取的障碍物信息转化为模糊变量，然后使用模糊控制算法来调整移动机器人的速度和航向等参数，以实现避障和导航。但是传统控制方法仍存在一些问题。
4.（1）传统控制方法需要准确的系统数学模型，才能对系统进行有效的建模和控制。但是，在实际应用中，移动机器人所面临的复杂环境和不确定性因素很多，难以准确地建立数学模型，导致传统控制方法的可靠性和实用性受到限制。
5.（2）对复杂环境的适应能力较差：传统控制方法的适用范围较窄，难以适应复杂多变的环境，尤其是在遇到未知障碍物或者不确定性较大的情况下，传统控制方法的效果会受到很大的影响。
6.基于深度学习的导航避障策略：随着深度学习技术的快速发展，该技术也逐渐应用于移动机器人导航避障策略中。例如，可以使用卷积神经网络（cnn）对移动机器人周围环境进行图像识别和分类，从而实现对障碍物的自动识别和避障行为的智能调节。但是基于深度学习的导航避障策略也存在一些问题：（1）数据量和计算资源需求较高：深度学习需要大量的数据进行训练和优化，而且需要强大的计算资源支持，这对于一些资源受限的应用场景来说不太适合。
7.（2）模型解释性较差：深度学习模型通常是一个黑盒子，很难直接理解其决策过程和内部结构，这给可靠性和安全性带来了一定的挑战。
8.（3）可迁移性较差：基于深度学习的导航避障策略很大程度上依赖于训练数据的特点和场景，因此很难直接迁移到其他场景或任务中。
9.（4）对实时性要求较高：基于深度学习的模型需要较长的推理时间，这对于实时性要求较高的应用场景来说可能会产生问题。
10.本发明旨在改善传统控制方法导航时存在死区和迂回现象、导致移动机器人运动路径不够优化和平滑，以及基于深度学习的导航策略数据量要求高，模型解释性差，可迁移
性差，实时要求高等问题。通过引入状态自动机模型，本发明提供了一种易于理解与解释，灵活性强，实时性好，可靠性高，数据需求量少的导航避障策略。


技术实现要素：

11.为解决上述技术问题，本发明提供一种避停优先的移动机器人运动规划与避障决策方法。
12.为解决上述技术问题，本发明采用如下技术方案：一种避停优先的移动机器人运动规划与避障决策方法，包括：a.建立状态自动机模型：将移动机器人的行驶状态和转移条件抽象为状态自动机模型；移动机器人的行驶状态包括正常行驶状态、避障过渡状态、避障状态、停车状态；正常行驶状态：当最优路径的覆盖区域无障碍物时，移动机器人按最优路径行驶；避障过渡状态：降低移动机器人的移动速度，实时检测最优路径的稳定性；避障状态：移动机器人选择新的最优路径避开障碍物；停车状态：移动机器人将速度降为零，等待稳定的最优路径或操作员下达指令；其中，当最优路径的路径上连续点之间曲率变化小于设定值时，最优路径为稳定状态，当最优路径的路径上连续点之间曲率变化大于或者等于设定值时，最优路径为不稳定状态；转移条件包括：a.从正常行驶状态转移到避障过渡状态：当移动机器人检测到当前最优路径上出现障碍物时，移动机器人切换到避障过渡状态；b.从避障过渡状态转移到避障状态：当移动机器人检测到新的最优路径稳定时，移动机器人进入避障状态，切换到新的最优路径，避开障碍物；c.从避障过渡状态转移到停车状态：当移动机器人检测到最优路径不稳定时，移动机器人切换到停车状态，等待得到稳定的最优路径；或者当移动机器人遇到无法避开的障碍物时，切换到停车状态等待操作员指令；d.从停车状态转移到正常行驶状态：当操作员下达开始任务的指令时，移动机器人切换至正常行驶状态；e.从停车状态转移到避障状态：最优路径稳定时；f.从避障状态转移到正常行驶状态：移动机器人按新的最优路径避开障碍物后，切换回正常行驶状态；b.离线生成备选路径簇：基于移动机器人当前行驶状态和运动约束，用采样的离散点做前向模拟生成能够使移动机器人到达激光雷达范围边界的不同备选路径的集合，在导航过程中，将存在阻碍移动机器人的障碍物的路径从备选路径集合中删除，得到备选路径簇；用采样的离散点做前向模拟生成备选路径的集合时，具体包括：在一个完整的备选路径簇中，将第一次采样按照角度划分为为9组，将第二次、第三次、第四次和第五次的采样依次划分为7、5、3和1组，总计生成条备选路径；进行角度划分时，将移动机器人的运动学约束的左最大转角和右最大转角分别设定正和负，按照每次采样的组数均匀划分；c.构建高层导航：在可行驶区域地图和概率栅格地图所构建的距离地图上，结合与移动机器人最近的障碍物的距离以及移动机器人到目标点的欧氏距离对所有的栅格构建代价值，以所述代价值作为启发式函数的规划方向，规划出接近道路中心、远离障碍物的具有全局路径点的
全局路径，将全局路径发送至中层导航；d.构建中层导航：中层导航订阅当前高层导航下发的基于全局地图的全局路径点，用于对全局路径进行二次规划，包括地图接收处理、混合规划；地图接收处理：根据移动机器人在全局地图中的位置切换子地图；混合规划模块：使用混合规划算法在子地图上进行路径规划，并且把规划出的局部路径点发布给底层导航；e.构建底层导航：读取离线生成的备选路径簇和中层导航发布的局部路径点，局部路径点即为目标点的序列；移动机器人根据定位信息对激光雷达获取的经过地形分析后的局部点云，按照点云高度进行处理，地形分析部分得到的地面点云，为可行驶区域；地面点云h1以上的点云被当作障碍物并被归为不可行驶区域、在相应的可行使区域中剔除不可行驶区域，高于地面但是低于h1的点云被当做斜坡；按照最优路径选择算法在备选路径簇中选择一条最优路径，超出可行驶区域的备选路径直接剔除，最优路径具有移动机器人的速度和方向信息，移动机器人按得到的速度和方向信息进行驱动。
13.进一步地，每条备选路径由五次样条曲线生成。
14.进一步地，构建底层导航，按照最优路径选择算法在备选路径中选择一条最优的路径时，（1）基于射线投射的可行路径判断：在二维平面上，一条射线用起点和方向向量表示，其中是射线与x轴的夹角，则射线上与起点距离为t的点的坐标为，其中，；对于给定的障碍物，障碍物的边界由n个点组成，第i个点的坐标为，，则所述障碍物的边界表示为n条线段，第i条线段为：；；其中表示线段上距离起点的比例；所述障碍物用所有线段的并集表示；交点求取任务：从目标点出发沿着方向发射一条射线，则射线与障碍物的第i条线段的交点表示为：；为障碍物边界第i个点的横坐标，为障碍物边界第i个点的纵坐标；求射线与障碍物所有线段的交点，然后选取距离目标点最近的交点作为有效交点；根据备选路径改变射线方向后，重复上述交点求取任务，判断每条射线与障碍物
是否存在有效交点，如是，则该备选路径不在可行路径上；如否，则该备选路径在可行路径上，由此能够在备选路径簇上筛选出可行路径集；（2）对可行路径集中每条可行路径进行评分，选取最优路径：在移动机器人的坐标系下定义移动机器人当前位置与目标点的连线方向与移动机器人移动方向的夹角为goaldir，移动机器人当前位置与可行路径终点的连线方向与移动机器人移动方向的夹角为mpdir；则可行路径的评分为：；其中，角度差异angdiff=|goaldir-mpdir|；遍历可行路径集中的每一条可行路径，得分最高的可行路径即为最优路径。
15.与现有技术相比，本发明的有益技术效果是：本发明可以根据不同的状态和转移条件来定义避障规则，使得策略的设计和实现更加直观和可理解；状态自动机模型可以根据不同的应用场景进行设计和调整，可以灵活地添加、删除、修改状态和转移条件，可以适应不同的环境和任务需求；可以在实时性要求较高的场景中实现快速的决策和控制，响应速度快；在避障决策过程中能够保证优先选择安全、可行的路径，从而保证行驶的安全性和稳定性；相对于基于深度学习的导航避障策略，本发明对于数据的要求较低，可以减少对大量数据的依赖。
附图说明
16.图1为本发明导航系统示意图；图2为本发明备选路径簇的示意图；图3为本发明导航系统的运行流程图；图4为本发明状态自动机模型的示意图。
具体实施方式
17.下面结合附图对本发明的一种优选实施方式作详细的说明。
18.本发明根据不同的状态和转移条件来定义避障规则，使得策略的设计和实现更加直观和可理解。如图1所示，导航系统由三层架构构成，分别是底层导航、中层导航、高层导航，三层的导航分别完成宏观系统上的三种大功能，分别为运动控制与避障、符合阿克曼（ackerman）模型的局部规划、全局范围内基于距离地图的路径规划。
19.本发明具体采用如下技术方案：a.建立状态自动机模型：根据移动机器人导航场景的特点和任务需求，设计适合的状态自动机模型。在移动机器人导航过程中，首先需要将移动机器人的行驶状态和事件抽象为有限状态自动机模型，状态自动机模型包括初始状态、中间状态和结束状态，以及状态之间的转移规则，用于描述移动机器人在不同环境下的行驶状态和行为决策。在设计状态自动机模型时，需要考虑移动机器人导航的特殊性质，如动态障碍物、环境变化等因素，同时需要考虑状态转移的逻辑和事件触发的条件。
20.其中，根据移动机器人导航场景的特点和任务需求建立状态自动机模型时，状态包括正常行驶状态、避障过渡状态、避障状态、停车状态。正常行驶状态：当底层导航选择出
的最优路径的覆盖区域无障碍物时，移动机器人按最优路径行驶。避障过渡状态：降低移动机器人的移动速度，实时检测选择出的最优路径的稳定性。避障状态：移动机器人选择新的最优路径避开障碍物。停车状态：移动机器人将速度降为零，等待稳定的最优路径或操作员下达指令。
21.转移条件包括：a.从正常行驶状态转移到避障过渡状态：当移动机器人检测到当前最优路径上出现障碍物时，移动机器人需要切换到避障过渡状态。b.从避障过渡状态转移到避障状态：当移动机器人检测到新的最优路径稳定时，移动机器人进入避障状态，切换到新的最优路径，避开障碍物。c.从避障过渡状态转移到停车状态：当移动机器人检测到最优路径的波动较大不稳定时，移动机器人切换到停车状态，等待得到稳定的最优路径或移动机器人遇到无法避开的障碍物时，需要切换到停车状态等待操作员指令。d.从停车状态转移到正常行驶状态：当操作员下达开始任务的指令时，移动机器人切换回正常行驶状态。e.从停车状态转移到避障状态：最优路径稳定时。f.从避障状态到正常行驶状态：移动机器人按新的最优路径避开障碍物后，切换回正常行驶状态。上述即为本实施例中状态自动机模型的状态和转移条件，具体如图4所示。
22.b.离线生成备选路径簇：离线的备选路径簇是基于移动机器人当前状态、用采样的离散点做前向模拟生成的能够到达感知传感器（激光雷达）范围边界的不同备选路径的集合，可以预测一段范围内可能出现的情况。这些备选路径是基于车辆运动约束生成的，每条备选路径由五次样条曲线生成，一组备选路径簇可以看作是从起始状态到感知传感器范围边界的可行路径。在导航过程中，感知传感器（激光雷达）检测到阻塞某些路径的障碍物，便将该部分路径删除，仅保留可通行的路径子集，从而得到可行路径集。备选路径簇是脱机生成的，在一个完整的备选路径簇中，将第一次采样分组groupid设置为9组，为了降低传感器边界路径的稠密性，对第二次、第三次、第四次、第五次采样后的备选路径依次设置为7、5、3、1组，总计生成条备选路径，并命名相应的pathid。根据车辆的运动学约束的最大转角，将左最大转角和右最大转角分别设定正负，按照采样次数均分为9、7、5、3段，作为每一段备选路径的角度约束。每一段备选路径的长度约束设置为1米。
23.使用matlab模拟备选路径生成的具体方法如下：定义移动机器人的最大转弯角度为angle，第一次采样的角度偏移步长deltaangle1=angle/4，因此可以生成条路径。以第一条路径为例，此时的偏移量shift1即为-angle，每条路径半径dis为1，数组wayptsstart=[0,0,0;dis,shift1,0]用于存放第一次采样备选路径的起点与终点位置。采用的计算机代码具体如下：pathstartr=0:0.01:dis；pathstartshift=spline(wayptsstart(:,1),wayptsstart(:,2),pathstartr)；对0到1区间进行100次数据均匀插值，将101个角度放入行向量pathstartshift中存储。采用的计算机代码具体如下：pathstartx=pathstartr .cos(pathstartshiftpi /180)；pathstarty=pathstartr .sin(pathstartshiftpi /180)；pathstartz=zeros(size(pathstartx))。
[0024]
通过pathstartshift将pathstartr这条0到1区间的直线变成曲线，即获得了一条经第一次采样的备选路径。
[0025]
由于采用的五次样条模拟路径，第一段则由shift1=-angle:deltaangle1:angle计算，在后续模拟样条中设置第二次采样的角度偏移步长deltaangle2=angle/3，第三次采样的角度偏移步长deltaangle3=angle/2，第四次采样的角度偏移步长deltaangle4=angle，使得采样的备选路径数依次为9、7、5、3、1条。后续几次采样原理同第一次。采用的计算机代码具体如下：for shift2 =
ꢀ‑
anglescale+shift1:deltaangle2scale:anglescale+shift1for shift3 =
ꢀ‑
anglescale^2+shift2:deltaangle3scale^2:anglescale^2+shift2for shift4=-anglescale^3+shift3:deltaangle4scale^3:anglescale^3+shift3for shift5=shift4waypts=[pathstartr’, pathstartshift' . pathstartz’；2dis, shift2, 0；3dis, shift3, 0；4dis, shift4, 0；5dis
ꢀ‑ꢀ
0.001. shift5, 0；5dis, shift5, 0]；pathr=0 : 0.01 : waypts(end, 1)；pathshift = spline(waypts(:, 1), waypts(:, 2), pathr)；pathx=pathr .cos (pathshiftpi/180)；pathy=pathr .sin (pathshiftpi/180)；pathz=zeros (size (pathx))；plot3 (pathx, pathy, path2)；endendendend。
[0026]
相比于第一次采样，后续几次采样角度进行了一定比例scale的缩放，此时的pathr相比于第一次的pathstartr，区间从0-1变成了0-5的范围，从的行向量变成的行向量，相同的是pathr的值仍是以0.01递增。最终获得如附图2的备选路径簇。
[0027]
备选路径簇是基于车辆运动约束生成的，每条备选路径生成五次样条曲线，使移动机器人的避障路径更加稳定平滑。
[0028]
c.构建高层导航：高层导航的作用是在全局栅格地图上进行路径规划，并且将路径规划得到的全局路径点发送至中层导航，中层导航解构后发送给底层导航。高层导航的地图信息来源为使用rtk位姿拼接成的点云地图栅格化后的栅格地图。如果在特殊场景中使用的栅格地图无
法满足要求，需要在点云地图的基础上抽取一定数量的点，使用单位圆覆盖算法生成拓扑地图。在可行驶区域地图和概率栅格地图所构建的距离地图上，通过与最近障碍物的距离和到目标点的欧氏距离对所有栅格构建代价值，以此代价值作为启发式函数的规划方向，规划出接近道路中心、远离障碍物的高质量的全局路径。规划得到的全局路径以nav_msgs::path消息的形式发送给中层导航，nav_msgs::path消息中包含有一系列的全局路径点。
[0029]
d.构建中层导航：中层导航在整个导航系统中担任的角色主要是负责对高层导航规划出的全局路径进行二次规划，主要包含了两个模块：地图接收处理模块、混合规划模块。
[0030]
地图接收处理模块：根据移动机器人在当前全局地图中的位置切换子地图（局部地图）。
[0031]
混合规划模块：在子地图上使用混合算法进行路径规划，并且把规划出的局部路径点发布给底层导航。
[0032]
中层导航会订阅高层导航下发的基于全局地图的全局路径点，一般来说，全局路径点由于在全局范围内进行规划，考虑到资源消耗以及规划频率的平衡，所以两个全局路径点之间的距离比较远，如果只使用底层导航承接来自于高层导航的全局路径点，在两个全局路径点之间的较长距离中，无法形成有效牵引，导致底层导航陷入到局部最优的境地，可能会卡死在某一位置。这也是加入中层导航的原因之一。而且中层导航涉及到在全局范围内的子地图切换，会根据当前全局路径点的位置和子地图的包含关系，自动切换子地图。
[0033]
为了解决广泛存在于阿克曼车体、栅格地图之间栅格大小与车体大小相矛盾的问题，中层导航使用混合算法处理地图。因为除了局部范围内的点云，底层导航对于外界信息一无所知，引入中层导航必不可少。
[0034]
使用基于距离地图的算法，解决了在全局可行驶区域地图上现有的算法以欧氏距离作为启发式函数参考的代价值规划出的路径距离障碍物较近，当面临需要避障的情况时，可供移动机器人避障的自由空间较小，影响移动机器人行驶的平顺性和安全性的问题。
[0035]
e.构建底层导航：底层导航是整个导航系统与底盘进行交互的关键模块，是导航系统将算法实际应用到底盘上的转化器。首先读取离线生成的备选路径簇，为了降低读取的代价将这些运动元信息以点的形式存储在相应的txt文件中，在底层导航开启之前进行读取，读取后在内存中以容器的方式进行存储。
[0036]
移动机器人在运行过程中会根据rtk实时定位或者在全局先验地图中经由定位算法获得的定位信息，根据velodyne激光雷达提供的经过地形分析后的局部点云，按照点云高度进行处理：高于地面点云15cm的被当作障碍物，移动机器人无法通行，会被归为不可行驶方位并在相应的可行使区域中剔除此部分。而高于地面但是低于15cm的点会被视为斜坡部分。根据障碍物的遮挡信息、地形分析信息以及目标点方位信息进行全方位的权衡，按照最优路径选择算法在备选路径中选择一条最优路径，最优路径本身携带有相应的速度（角速度、线速度）和方向信息，车体在得到相应的信息之后进行驱动。在最优路径的选择过程
中，对于目标点的方位分析十分明了，即底层导航会优先选择最接近当前目标点的路径。其中，目标点指的是移动机器人在当前位置将要前往的下一个目标位置，最优路径的选择原理与方法如下：（1）基于射线投射（raycasting）的可行路径判断：raycasting（射线投射）是一种基于几何计算的算法，用于检测场景中的碰撞、可见性等问题。射线投射的基本思想是从一个点发射一条射线，判断该射线与场景中的物体是否相交，从而得到碰撞点或者可见性。在二维平面上，一条射线可以用起点和方向向量表示，其中是射线与x轴的夹角，则射线上与起点距离为t的点的坐标为，其中，；对于给定的障碍物，可以将其表示为一个多边形或一组线段，障碍物的边界由n个点组成，每个点的坐标为，，则所述障碍物的边界表示为n条线段，第i条线段为：；；其中表示线段上距离起点的比例；所述障碍物用所有线段的并集表示。
[0037]
交点求取任务：从目标点出发沿着方向发射一条射线，检测该射线是否与障碍物相交，射线的参数方程为：。
[0038]
则射线与障碍物的第i条线段的交点表示为：；求出射线与障碍物所有线段的交点，然后选取距离目标点最近的交点作为有效交点；根据备选路径改变射线方向后，重复上述交点求取任务，判断每条射线与障碍物是否存在有效交点，如是，则该备选路径不在可行路径上；如否，则该备选路径在可行路径上，由此能够在备选路径簇上筛选出可行路径集。
[0039]
（2）对可行路径集的每条可行路径进行评分，选取最优的路径：在移动机器人的坐标系下定义，移动机器人当前位置与目标点的连线方向与移动机器人移动方向的夹角为goaldir，移动机器人当前位置与可行路径终点的连线方向与移动机器人移动方向的夹角为mpdir；则可行路径的评分为：；其中，角度差异angdiff=|goaldir-mpdir|，角度差异越小，说明该可行路径的终点与目标点越接近，因此该可行路径的得分就应该越高。这里使用的是一个平方根函数来对角度差异进行归一化，确保可行路径评分在[0,1]范围内。遍历可行路径集中的每一条可
行路径，评分最高的可行路径即为最优路径。
[0040]
本发明导航系统的运行流程参考图3。
[0041]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
[0042]
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

技术特征：
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|；遍历可行路径集中的每一条可行路径，得分最高的可行路径即为最优路径。

技术总结
本发明涉及移动机器人导航技术领域，公开了一种避停优先的移动机器人运动规划与避障决策方法，包括建立状态自动机模型、离线生成备选路径簇、构建高层导航、构建中层导航、构建底层导航。本发明可以根据不同的状态和转移条件来定义避障规则，使得策略的设计和实现更加直观和可理解；可以在实时性要求较高的场景中实现快速的决策和控制，响应速度快；在避障决策过程中能够保证优先选择安全、可行的路径，从而保证行驶的安全性和稳定性；相对于基于深度学习的导航避障策略，本发明对于数据的要求较低，可以减少对大量数据的依赖。可以减少对大量数据的依赖。可以减少对大量数据的依赖。


技术研发人员：陈宗海 王纪凯 张梦杰 李剑宇 徐萌
受保护的技术使用者：中国科学技术大学
技术研发日：2023.07.06
技术公布日：2023/8/5