基于分层搜索的多无人车应急冲突解脱与恢复方法及系统

技术领域

本发明涉及机器人技术领域，具体涉及基于分层搜索的多无人车应急冲突解脱与恢复方法及系统。

背景技术

分层集中式的多无人车路径规划研究广泛应用于工厂物流，军队协同编队、飞机调度等现实场景中，但该类框架的运行需要依赖一个准确、恒定的先验地图，即保证规划阶段和执行阶段的外部环境需保持不变且地图能准确反应障碍的分布。实际应用情景中，该假设受到多因素的挑战，一方面，因传感器限制(视野受遮挡、感知不确定性或定位失准等)，初始地图中的障碍物信息(分布位置、大小和形状等)本身带有误差；另一方面，在任务执行中进入环境的第三方物体或因动力系统损坏而处于急停等特殊状态的执行车辆，会成为地图中新增障碍，导致初始地图的障碍分布不再有效。

突发故障下的安全运动规划方法的研究目前仍处于萌芽阶段，而对于多无人车故障安全运动规划方面研究则近乎空白。因此，本发明汇总单无人车应对障碍的规避方法并讨论其应用于多车的可能性。现存的面向突发环境变化的无人车应急响应方法主要采用规划局部轨迹绕行和全局轨迹重规划。

一：局部轨迹绕行方法

常用于车辆局部绕行规划的算法有人工势场、弹性带算法和模型预测控制算法。其中，人工势场法通过综合周围障碍物的距离和自身车体运动方向等信息，完善对障碍物斥力势场的建模过程，通过斥力将无人车推离障碍物。但该方法未虑车辆运动学模型，输出轨迹不便于跟踪。基于弹性带算法的紧急避障路径规划方法，利用弹性带理论(ElasticBand Theory)进行规划，在规划时提出将路径的局部曲率变化最小作为约束条件,从而提高车辆紧急避障时的操纵稳定性。但基于弹性带的路径规划方法聚焦于横向偏移量的躲避，在较大速度情况下车辆改变容易发生甩尾或者侧翻危险。模型预测(MPC)方法被更广泛的应用，该方法根据车辆的尺寸确定禁区圆半径，若模型预测过程中某一时刻车辆质心落入禁区内，避障惩罚函数值将急剧增大，从而确保所生成的故障安全路径成功避开障碍物。上述的规避算法的规划目的都为反应式的局部绕行，仅适用于简单的障碍物，对于环境变化较大，原轨迹已经不可用的情况难以适用。

二：全局轨迹规划策略

全局轨迹规划策略可在更大范围内绕开障碍物，保证解的完整性。LingguangWang等人利用变分法，从一个未来姿态出发，将部分初始轨迹引入约束，沿着初始轨迹在预定时间内规划规避轨迹，保证了轨迹的连续性和匹配性。但是，此类方法对于规划器的要求很严苛，需要在无人车到达未来姿态前规划出躲避结果，鲁棒性较弱。Christian Pek等人提出了一种借助过渡轨迹的缝合策略,以减轻规划器的负担：在重新规划规避轨道的同时，通过模型预测控制(MPC)快速生成一条连接轨迹，该轨迹可将车辆引向规避轨道，即一条完整轨迹是初始轨道、连接轨道和部分规避轨道的结合。但该方法在连接轨迹处无法保证无碰性。因此，LiBai等人改善了上述方法，通过并行计算，将拼接轨迹两端沿着原始和规避轨迹的各个点尝试，在碰撞约束前提下，得到成本最小的拼接轨迹。但扩展至多车系统时该方法仍造成难以解决的计算消耗，并不适用于集中式框架的多无人车规划系统。

因此，目前急需一种紧急风险规避方法以提升集中式多无人车框架的实用性和鲁棒性。

发明内容

有鉴于此，本发明提供了一种基于分层搜索的多无人车应急冲突解脱与恢复方法及系统，能够针对现有集中式多无人车路径规划框架，本发明加入突发障碍规避方法，使其具备先验地图变动情况下主动适应变化并安全完成任务的功能，大大提高了多无人车系统的稳定性和鲁棒性。

为达到上述目的，本发明的技术方案包括如下步骤：

各无人车沿着既定轨迹Trajectory

在t

在t

若轨迹重规划时间小于缓冲时间长度T，则执行安全行车轨迹Trajectory

进一步地，在t

规避轨迹规划过程构建双层框架结构：

在框架的下层以无人车时空状态作为节点，以停车距离、原轨迹偏离程度、与他车扰程度和终点距离为启发代价，以动能变化和停车时间为实际代价构建规划命题；使用混合A*算法求解规划命题，为受障碍物影响无人车搜索出一组达到安全停靠位置的运动状态节点序列，作为一条备选规避轨迹；设定混合A*算法的循环跳出条件为最长搜索时间，设定安全停靠位置为车速为0的节点位置，由此获得多条备选规避轨迹。

在框架的上层对每一个无人车生成的备选轨迹组做时空碰撞检测，删去相互影响的发生碰撞的轨迹，并选择执行代价最小的节点集作为紧急停靠轨迹Trajectory

进一步地，时空碰撞检测，具体为：依照时间顺序，依次计算各无人车同时刻状态间的距离信息，并依照车体形状和安全距离进行判断，得到相互影响的发生碰撞的轨迹。

进一步地，针对每个受新增障碍影响的车辆进行轨迹重规划，生成从未来位姿点s

本发明实施例还提供了基于分层搜索的多无人车应急冲突解脱与恢复系统，该系统设置于每辆无人车上，包括障碍物检测模块、规避轨迹规划模块、轨迹重规划模块和执行模块。

障碍物检测模块，用于在无人车沿着既定轨迹行驶Trajectory

规避轨迹规划模块，用于在t

轨迹重规划模块，用于针对每个受新增障碍影响的车辆进行轨迹重规划，生成从未来位姿点s

执行模块，用于获取轨迹重规划时间，判断轨迹重规划时间小于缓冲时间长度T，则执行轨迹Trajectory

进一步地，规避轨迹规划模块为双层框架结构：在框架的下层以无人车时空状态作为节点，以停车距离、原轨迹偏离程度、与他车扰程度和终点距离为启发代价，以动能变化和停车时间为实际代价构建规划命题；使用混合A*算法求解规划命题，为受障碍物影响无人车搜索出一组达到安全停靠位置的运动状态节点序列，作为一条备选规避轨迹；设定混合A*算法的循环跳出条件为最长搜索时间，设定安全停靠位置为车速为0的节点位置，由此获得多条备选规避轨迹；在框架的上层对每一个无人车生成的备选轨迹组做时空碰撞检测，删去相互影响的发生碰撞的轨迹，并选择执行代价最小的节点集作为紧急停靠轨迹Trajectory

进一步地，轨迹重规划模块，采用基于冲突的搜索CBS算法的分层规划框架，将未受影响的车辆轨迹加入时空约束以保证新规划的轨迹不影响正常车辆的运行。其次，依据受新增障碍影响车辆不同的起始未来状态s

有益效果：

1.本发明对多无人车控制跟踪过程中突发的环境改变，提出了一种基于分层搜索的多无人车应急冲突解脱与恢复方法，该方法针对现有集中式多无人车路径规划框架，本发明加入突发障碍规避方法，使其具备先验地图变动情况下主动适应变化并安全完成任务的功能，大大提高了多无人车系统的稳定性和鲁棒性。

2.本发明提供的一种基于分层搜索的多无人车应急冲突解脱与恢复方法，针对环境变化造成初始轨迹不可用的问题，在进行重规划轨迹时，将未受影响的车辆轨迹加入时空约束以保证新规划车辆不会影响正常车辆的运行。其次，依据受影响车辆不同的起始未来位姿，异步进行规划，提高了重规划效率，能够实现快速恢复。

3.本发明提供的基于分层搜索的多无人车应急冲突解脱与恢复方法，对产生碰撞的无人车进行的约束并将正常运行无人车加入时空约束以得到安全的无人车运行轨迹，极大提高了无人车在遇到突发情况下的系统运行的稳定性

附图说明

图1是本发明提供的基于分层搜索的多无人车应急冲突解脱与恢复方法流程图；

图2是本发明所使用的规避轨迹规划框架图；

图3是本发明规避轨迹起始点选择示意图；

图4是本发明备选规避迹生成示意图；

图5是本发明规避轨迹选择示意图；

图6是本发明使用的无人车运动模型坐标图；

图7是本发明所恢复的安全行车轨迹生成示意图；

图8是本发明综合测试效果图；

图9是本发明提供的基于分层搜索的多无人车应急冲突解脱与恢复组成框图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种基于分层搜索的多无人车应急冲突解脱与恢复方法，其流程如图1所示，具体包括如下步骤：

各无人车沿着既定轨迹行驶Trajectory

在t

规避轨迹规划过程构建双层框架结构，如图2所示。图3是本发明规避轨迹起始点选择示意图。

在框架的下层以停车距离、原轨迹偏离程度、与他车扰程度和终点距离为启发代价，以动能变化和停车时间为实际代价构建规划命题。

所构建的规划命题中成本函数为g

/>

为了让无人车更快找到满足安全停靠要求的终止点，本发明设计了代价地图来评估当前点的时空干扰程度。

本发明计算地图上每一个网格点代价计算如公式所示，其中函数nearestdis代表网格点(x，y)到其他不受影响车辆轨迹的最小距离，w和h分别代表地图的长度和宽度。最后可以得到最终的代价地图，其中总代价函数为h

h

V(s

最终本发明设计h

然后利用混合A*算法求解规划命题，为受障碍物影响无人车搜索出一组达到安全停靠位置的运动状态序列，作为一条备选规避轨迹；设定混合A*算法的循环跳出条件为最长搜索时间(例如可设定为1ms)，由此获得多条备选规避轨迹，每条备选规避轨迹的生成均符合无人车运动模型。图4是本发明备选规避迹生成示意图。

在框架的上层对每一个无人车生成的备选轨迹组做时空碰撞检测，删去相互影响的发生碰撞的轨迹，并选择执行代价最小的节点集作为紧急停靠轨迹Trajectory

本发明所使用的无人车运动模型具体为：

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，无人车运动学模型能有效反映车辆位置、速度、加速度等与时间的关系。并且在无人车轨迹规划过程中应用运动学模型，可以使规划出的轨迹更切合实际，满足行驶过程中的运动学几何约束。本发明所采用的无人车模型，无人车在应用场景内采用前轮转向后轮驱动的后驱车的运动模式，因此采用阿克曼转向几何作为如图所示的运动学模型，该模型可以完整描述无人车的转向运动和加减速运动等运动过程。其中，表示全局坐标系下车辆后轴中心的位置，θ表示车辆的偏航角，v和ω表示车辆的行驶线速度和转动角速度，δ表示车辆前轮的转向角，L

当转向角固定在δ时，无人车移动轨迹可以近似表示为一个移动的圆弧，圆弧半径表示为

ω和θ间的运动学关系可表示为：

取机器人姿态信息的三元组x＝(x,y,θ)

其中控制输入受条件v∈[-v

在t

图7是本发明所恢复的安全行车轨迹生成示意图。

轨迹重规划采用基于冲突的搜索CBS算法的分层规划框架，将未受影响的车辆轨迹加入时空约束以保证新规划的轨迹不影响正常车辆的运行。其次，依据受新增障碍影响车辆不同的起始未来状态s

若所述轨迹重规划时间小于所述缓冲时间长度T，则执行安全行车轨迹Trajectory

图8是本发明综合测试效果图。

本发明另外一个实施例还提供了基于分层搜索的多无人车应急冲突解脱与恢复系统，其框架结构如图9所示，该系统设置于每辆无人车上，包括障碍物检测模块、规避轨迹规划模块、轨迹重规划模块和执行模块；；

障碍物检测模块，用于在无人车沿着既定轨迹行驶Trajectory

规避轨迹规划模块，用于在t

本发明实施例中，规避轨迹规划模块为双层框架结构：在框架的下层以无人车时空状态作为节点，以停车距离、原轨迹偏离程度、与他车扰程度和终点距离为启发代价，以动能变化和停车时间为实际代价构建规划命题；使用混合A*算法求解规划命题，为受障碍物影响无人车搜索出一组达到安全停靠位置的运动状态节点序列，作为一条备选规避轨迹；设定混合A*算法的循环跳出条件为最长搜索时间，由此获得多条备选规避轨迹。

在框架的上层对每一个无人车生成的备选轨迹组做时空碰撞检测，删去相互影响的发生碰撞的轨迹，并选择执行代价最小的节点集作为紧急停靠轨迹Trajectory

轨迹重规划模块，用于针对每个受新增障碍影响的车辆进行轨迹重规划，轨迹重规划模块生成从未来位姿点s

本发明实施例中，轨迹重规划模块，采用基于冲突的搜索CBS算法的分层规划框架，将未受影响的车辆轨迹加入时空约束以保证新规划的轨迹不影响正常车辆的运行。其次，依据受新增障碍影响车辆不同的起始未来状态sf，异步进行规划；最后，在满足运动学约束的情况下获得新规划轨迹，即安全行车轨迹Trajectory

所述执行模块，在时间段T内仍使无人车执行既定轨迹Trajectory

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。