一种基于共形预测与安全时间区间的不平坦地形车辆动态避障方法及系统

本发明涉及移动机器人、无人驾驶车辆、越野无人平台以及非结构化环境路径规划，尤其涉及一种基于共形预测与安全时间区间的不平坦地形车辆动态避障方法及系统。本发明适用于矿区巡检、野外运输、灾害救援、农业作业、林地通行、无人地面平台自主导航等需要在不平坦地形中同时处理静态地形风险、动态障碍物风险和滚动执行稳定性的场景。

背景技术：

1、在非结构化不平坦地形中，车辆的可通行性不仅取决于二维平面上的障碍物位置，还受到地形高度、坡度、侧倾、粗糙度、静态安全间隙以及车辆运动学约束的共同影响。对于低速越野车辆、无人地面平台或移动机器人而言，单纯以二维距离最短为目标生成路径，容易导致车辆进入坡度较大、地表粗糙、静态安全间隙不足或者姿态风险较高的区域，从而造成能耗增加、跟踪困难甚至通行失败。

2、现有静态路径规划方法，例如hybrid a、kino a、采样式规划器、图搜索规划器以及已有不平坦地形规划器，能够在静态地图上生成从起点到目标点的全局路径。该类方法可为车辆提供长期几何方向，但其通常不直接处理动态障碍物未来时空占用。若车辆仅沿静态全局路径行驶，在行人、其他车辆、施工设备或动态机器人穿越路径时，静态路径可能在局部区域产生时空冲突。

3、现有动态避障方法常采用两类策略。第一类策略直接根据动态障碍物点预测结果进行避障，该方法依赖预测器输出的单一点序列，难以表达预测误差和置信范围。第二类策略采用固定圆形或各向同性半径对动态障碍物进行膨胀，该方法虽然易于实现，但无法充分利用预测误差在运动方向与横向方向上的差异。在狭窄地形通道中，过大的圆形膨胀会删除大量实际上仍可通过的时空区域，导致不必要的停车、绕行或无解；过小的膨胀又可能不足以反映预测不确定性。

4、共形预测能够在一定校准条件下把预测误差转换为预测区域，但共形预测区域的几何形状、层级结构和下游规划表示同样重要。若将全部未来预测区域均视作硬障碍，规划器容易过度保守；若仅将预测区域作为软代价，又难以排除近时刻的直接碰撞风险。因此，需要一种能够同时支持硬区间删除和软风险评价的时空表示。

5、sipp类安全时间区间规划能够通过对空间单元的占用时间取补集得到安全时间区间，使规划器在空间状态之外考虑到达时间。然而，传统sipp通常面向平面栅格环境，对不平坦地形属性、ackermann车辆运动原语、动态障碍物预测不确定性、地形能耗代理和滚动轨迹发布稳定性的考虑不足。此外，纯局部动态规划如果缺少静态全局引导，容易出现局部绕行、反复等待或轨迹抖动；如果每个周期都直接进行完整全局时空搜索，又会导致计算量过大。其中，sipp表示安全时间区间路径规划；ackermann运动原语表示基于阿克曼转向运动学模型生成的车辆运动原语。

6、因此，亟需一种能够将静态地形引导路径、运动方向对齐的分层椭圆共形预测、动态硬占据/动态软风险张量、安全时间区间、地形时间—能耗—风险代价以及执行感知轨迹衔接机制统一起来的车辆动态避障方法，使车辆能够在不平坦地形动态障碍物环境中安全优先、能耗合理且稳定地通行。

技术实现思路

1、本发明的目的在于提供一种基于共形预测与安全时间区间的不平坦地形车辆动态避障方法及系统，用以解决以下技术问题：动态障碍物预测误差难以转化为适合局部时空搜索的规划约束；圆形或固定半径膨胀无法利用预测误差的方向性；静态全局路径与局部动态避障之间缺少明确的尺度关系；不平坦地形中的高度、法向量、粗糙度、静态净空和动态风险难以统一进入局部搜索；滚动重规划过程中候选轨迹频繁切换导致车辆执行不稳定。

2、本发明通过静态地形引导路径提供长期方向，通过轨迹级分层椭圆共形预测把动态障碍物预测结果转换为动态硬占据张量和动态软风险张量，通过安全时间区间表达动态可通行时间窗，并在局部规划目标附近执行地形多代价安全时间区间搜索。候选轨迹生成后，本发明并不无条件替换当前轨迹，而是通过轨迹衔接、安全检查和收益判据确定是否发布，从而提高滚动规划稳定性。

3、具体的技术方案：

4、一种基于共形预测与安全时间区间的不平坦地形车辆动态避障方法，车辆系统首先获取不平坦地形地图、车辆状态、目标状态和动态障碍物历史状态；然后从静态地形层生成静态地形引导路径；随后对动态障碍物预测中心进行轨迹级分层椭圆共形校准，实时更新动态硬占据张量和动态软风险张量；之后基于动态硬占据张量构建安全时间区间，在净空检查后的局部目标下执行地形多代价安全时间区间局部搜索；最后根据候选轨迹的安全性、连续性和收益确定轨迹发布结果。

5、具体步骤包括：

6、步骤一，输入信息获取；获取不平坦地形地图、车辆状态、目标状态和动态障碍物历史状态；

7、步骤二，获得不平坦地形地图和静态地形层；所述不平坦地形地图至少提供静态占据、静态净空、高度、地形法向量、粗糙度和可通行性信息；

8、步骤三、静态地形引导路径生成；基于所述不平坦地形地图生成从车辆起始区域到目标区域的静态地形引导路径；

9、步骤四、动态障碍物预测与轨迹级椭圆共形校准；根据所述动态障碍物历史状态生成动态障碍物在未来预测时域内的离散预测中心，将预测残差在动态障碍物运动方向和与所述运动方向正交的法向方向上分解，利用校准轨迹在整个预测时域内的归一化残差最大值构造轨迹级非符合性分数，并针对多个误覆盖率获得共形分位数，以生成与动态障碍物运动方向对齐的多个椭圆共形预测层；

10、步骤五、转化为动态硬占据张量与动态软风险张量；从所述多个椭圆共形预测层中选取操作性置信层，经动态障碍物等效尺寸和车辆等效轮廓膨胀后栅格化为动态硬占据张量；将一个或多个附加置信层按照层权重和栅格到椭圆边界的距离栅格化为动态软风险张量；其中，所述动态硬占据张量用于删除安全时间区间，所述动态软风险张量仅用于代价评价；

11、步骤六、安全时间区间构建；结合静态可通行性和动态硬占据张量，在有限规划时间网格内为各空间栅格构建最大连续硬空闲时间段，得到安全时间区间集合；

12、步骤七、局部规划根节点和局部目标选择；以车辆当前状态或当前已发布轨迹上的有效接管状态作为局部规划根节点，根据该局部规划根节点在所述静态地形引导路径上的投影位置和静态净空选择局部规划目标；

13、步骤八、确定地形多代价安全时间区间搜索节点和运动原语；在所述安全时间区间集合上，以车辆连续位姿、空间栅格、航向离散层、到达时间和安全时间区间编号构成搜索节点，采用ackermann运动原语进行扩展；

14、步骤九、延迟出发等待和连续沿边检查；通过在父节点安全时间区间内延迟出发时刻实现等待；对每一候选运动原语进行连续沿边采样，检查地图边界、静态可通行性、静态硬净空和动态硬占据；

15、步骤十、计算标量时间—能耗—风险边代价；对满足硬约束的候选运动原语计算由行驶时间、地形相关能耗代理量、静态净空风险、地形姿态风险和动态软风险构成的标量加权边代价；

16、步骤十一、启发式、标量标签松弛和最佳进展恢复；以标量累计代价与启发式代价的加权和进行搜索；当到达局部规划目标时生成候选局部轨迹，当未到达局部规划目标时从已扩展安全节点中生成最佳进展轨迹前缀；

17、步骤十二、执行感知轨迹衔接与发布；对所述候选局部轨迹或最佳进展轨迹前缀进行轨迹衔接、静态安全和动态安全检查，并在其相对当前已发布轨迹延长可执行时域或改善目标进展时发布；否则继续执行当前仍满足安全检查的已发布轨迹。

18、一种基于共形预测与安全时间区间的不平坦地形车辆动态避障系统，用于实现所述的一种基于共形预测与安全时间区间的不平坦地形车辆动态避障方法；所述系统部署于无人地面车辆、越野机器人、矿区运输平台、农业机器人或其他移动机器人上，包括以下模块：

19、静态地形层与引导模块，用于接收点云、高程地图或栅格地图，构建静态占据、静态净空、高度、地形法向量、粗糙度和可通行性信息，并生成静态地形引导路径；

20、动态障碍物预测模块，用于维护动态障碍物历史状态缓存，并输出动态障碍物在未来预测时域内的离散预测中心；

21、轨迹级椭圆共形校准模块，用于将预测残差分解至运动方向坐标系，计算轨迹级非符合性分数和共形分位数，并生成多个椭圆共形预测层；

22、硬占据和软风险张量构建模块，用于将操作性置信层栅格化为动态硬占据张量，将附加置信层栅格化为动态软风险张量；

23、安全时间区间构建模块，用于根据静态可通行性和动态硬占据张量构建各空间栅格的安全时间区间集合。

24、与现有技术相比，本发明具有以下有益效果：

25、第一，本发明采用轨迹级、运动方向对齐的椭圆共形预测层，将动态障碍物预测误差在纵向和横向上分别标定，减少各向同性冗余膨胀，使狭窄地形通道中可用的时空搜索空间得以保留。

26、第二，本发明将共形预测层划分为动态硬占据张量和动态软风险张量。动态硬占据用于删除会导致直接冲突的安全时间区间，动态软风险仅进入标量边代价，使规划器能够在等待、绕行、行驶时间、地形能耗代理和动态风险之间进行权衡。

27、第三，本发明将不平坦地形中的高度、地形法向量、粗糙度、静态净空和动态软风险统一进入地形多代价安全时间区间局部搜索，能够生成更适合车辆在不平坦地形上执行的动态避障轨迹。

28、第四，本发明使用静态地形引导路径提供局部目标和有界引导偏置，而不把静态引导路径作为硬轨迹约束，因此允许车辆在动态障碍物阻挡时等待、减速、偏离或绕行。

29、第五，本发明在滚动规划中引入执行感知接管状态、安全检查窗口和收益判据，使候选轨迹即使生成也不必立即替换当前轨迹，从而降低频繁重规划导致的轨迹抖动，提高下游控制器跟踪稳定性。