一种复杂环境下无人机航迹规划方法

本发明涉及无人机自主航迹规划，具体为一种复杂环境下无人机航迹规划方法。

背景技术：

1、无人机在建筑物密集区域、厂区通道、施工现场及其他低空复杂环境中执行巡检、探测任务时，需要根据机载定位设备、惯性测量设备及环境感知设备持续获取自身运动状态和周围障碍物信息，并据此规划满足飞行动力约束的航迹。现有无人机航迹规划方法通常利用激光雷达、深度相机或者毫米波雷达构建局部环境地图，将障碍物区域标记为不可通行区域，在检测到原航迹与障碍物发生冲突时重新搜索局部航迹，再对新生成航迹进行平滑处理并发送至飞行控制器执行。

2、在实际飞行过程中，不同设备的数据采集频率、处理延迟及传输时刻存在差异，环境感知数据到达规划单元时，无人机已经沿原航迹继续运动。特别是在建筑物转角、狭窄通道及设备遮挡区域内，移动障碍物可能暂时离开传感器视场，现有处理方式容易将未取得障碍物回波的空间直接作为可通行空间；当障碍物再次进入视场时，局部地图又重新发生变化，造成航迹在相邻规划周期内反复失效和重新生成。

3、现有方法在环境信息更新后，通常根据最新局部地图判断整条原航迹是否存在碰撞风险，或者按照障碍物附近的固定空间范围进行局部重规划，难以明确区分原航迹中仍可继续执行的部分及已经失去通行依据的部分。同时，新航迹通常以无人机当前位置或者当前规划时刻作为起点，未充分考虑已经下发但尚未完成执行的控制指令、当前速度状态及允许机动能力，可能使所选航迹切换位置与无人机实际能够脱离原航迹的位置不一致，导致新旧航迹衔接处出现速度、加速度及航向不连续。

4、此外，规划单元生成新航迹后，若原航迹终止、新航迹生效及飞行控制器执行指向分别更新，在部分记录未完成写入的情况下，可能出现规划单元采用新航迹而飞行控制器仍执行原航迹的状态不一致。由此，需要解决复杂环境信息持续更新条件下，如何准确确定受环境变化影响的航迹区间，结合无人机实际执行状态确定可实施的航迹切换边界，并保证新旧航迹连续承接及执行状态一致的问题。

技术实现思路

1、针对现有技术的不足，本发明提供了一种复杂环境下无人机航迹规划方法，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明提供如下技术方案：一种复杂环境下无人机航迹规划方法，包括：

3、s1、获取无人机状态数据及环境感知数据，按采集时刻对应位姿转换环境感知数据，形成含占用证据体、可通行证据体、遮挡保持证据体的环境证据版本；

4、s2、按执行顺序将当前有效航迹划分为控制航迹段，生成扫掠空间体，将覆盖扫掠空间体的可通行证据体关联为支持证据集合，形成航迹支持关系版本；

5、s3、环境证据版本更新时确定变化证据体，沿航迹支持关系版本定位支持失效控制航迹段，合并连续控制航迹段形成候选失效航迹区间；

6、s4、依据已下发控制指令、飞行状态及飞行动力约束预测指令承接状态，沿当前有效航迹确定执行承诺前沿，形成切换状态管束；

7、s5、在候选失效航迹区间后确定支持有效的回接航迹段，形成回接状态管束；依据环境证据版本建立时空支持图，生成连接两状态管束的候选控制航迹；

8、s6、依次校核候选控制航迹的动力约束、环境证据反向支持及双端状态承接；校核通过后，在同一切换时刻终止当前航迹版本并生效候选航迹版本。

9、进一步的，s1包括：

10、将有效回波点及由环境感知设备测量误差、无人机位置误差、坐标转换误差确定的扩展范围形成占用证据体；

11、将感知设备发射位置至首个有效回波点的有效探测空间形成可通行证据体；

12、将前一环境证据版本中因探测射线被位于射线前段的占用证据体截断而未被重新观测的占用空间形成遮挡保持证据体。

13、进一步的，s2包括：

14、基于无人机机体基准空间、位置测量误差空间、航迹跟踪允许误差空间，按照控制航迹段各时刻的期望位置及期望姿态形成扫掠空间体；

15、仅在扫掠空间体于计划执行时间内被可通行证据体完整覆盖且不与占用证据体、遮挡保持证据体相交时，将可通行证据体写入支持证据集合。

16、进一步的，s3包括：

17、按照统一空间单元标识比较前一环境证据版本及更新后的环境证据版本，将证据类型转换空间单元、占用边界归属改变空间单元分别确定为变化证据体；

18、在下列任一情形下，将对应控制航迹段定位为支持失效控制航迹段：扫掠空间体未被更新后有效的支持证据集合完整覆盖；

19、扫掠空间体同占用证据体、遮挡保持证据体相交。

20、进一步的，s4包括：

21、按照已下发控制指令的执行顺序，以飞行控制器控制周期对位置、速度、航向角逐时段递推，得到全部已下发控制指令执行结束时的指令承接状态；

22、针对当前有效航迹的控制航迹段边界，计算可用时间及由速度调整时间、航向调整时间、控制响应延迟确定的最短机动时间，将候选失效航迹区间前首个可用时间不小于最短机动时间的边界确定为执行承诺前沿。

23、进一步的，s5包括：

24、从候选失效航迹区间终止控制航迹段的下一控制航迹段开始，按执行序号选取首个支持有效控制航迹段作为回接航迹段；

25、以可通行证据体在各时间层内的连通空间作为节点，在相邻时间层中，对节点中心距离不大于最大允许速度同时间层差的乘积加最大允许加速度同时间层差平方乘积一半，且过渡空间被可通行证据体完整覆盖的节点建立有向连接，形成时空支持图。

26、进一步的，按照时间层递增方向搜索连接切换状态管束及回接状态管束的节点序列；

27、节点过渡代价为节点中心距离、速度变化量同最大允许加速度之比的第一非负权重加权值、过渡空间至占用证据体边界最小距离倒数的第二非负权重加权值之和；

28、根据累计过渡代价最小的节点序列确定航迹连接点，以起止位置、起止速度、起止加速度作为边界条件生成分段五次多项式候选控制航迹。

29、进一步的，s6包括：

30、对候选控制航迹的位置函数分别求一阶导数、二阶导数、三阶导数，确定速度、加速度、加加速度并同对应飞行动力约束比较；

31、从候选控制航迹终点向起点逐段生成扫掠空间体，各扫掠空间体被有效可通行证据体完整覆盖且不与占用证据体、遮挡保持证据体相交时，环境证据反向支持校核通过。

32、进一步的，将候选控制航迹起始状态逐项同切换状态管束比较，将终止状态逐项同回接状态管束比较；

33、全部状态分量落入对应上下界后，在同一次不可拆分提交中写入当前航迹版本有效终止时刻、候选航迹版本有效起始时刻、飞行控制器当前有效航迹指向，三项写入全部成功时生效候选航迹版本，任一写入失败时撤销本次状态变化。

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

35、1、通过按环境感知数据的采集时刻对应无人机位姿完成坐标转换，形成占用证据体、可通行证据体、遮挡保持证据体，再将控制航迹段的扫掠空间体同可通行证据体建立支持关系，并在环境证据版本更新后沿支持关系定位支持失效控制航迹段，结合已下发控制指令、飞行状态及飞行动力约束确定执行承诺前沿，进一步生成连接切换状态管束及回接状态管束的候选控制航迹，使环境变化引起的航迹失效判断、实际可切换边界确定及候选航迹生成按照统一因果链连续执行，从而减少因感知信息更新时刻同航迹实际执行状态不一致而产生的无依据航迹替换，使无人机能够在保留可继续执行航迹部分的基础上绕开失效区间并接回支持有效的后续航迹。

36、2、通过为各控制航迹段分别生成包含机体外廓、位置测量误差及航迹跟踪允许误差的扫掠空间体，并将其同具有对应空间范围及有效时间范围的可通行证据体关联，在环境证据版本变化时按照完整覆盖关系、占用相交关系及遮挡保持关系确定支持有效状态，使候选失效航迹区间由变化证据体同具体控制航迹段之间的支持关系推导形成，而非仅依据障碍物瞬时位置或者固定距离范围确定，从而能够区分实际受到环境变化影响的航迹段及仍具有有效支持的航迹段，避免将因遮挡暂时未被观测的占用空间直接认定为可通行空间，并减少对未受影响航迹部分的重复处理。

37、3、通过对候选控制航迹依次执行动力约束校核、环境证据反向支持校核及双端状态承接校核，并在全部校核通过后于同一切换时刻终止当前航迹版本、启用候选航迹版本及更新飞行控制器的当前有效航迹指向，使候选控制航迹在生效前同时具有可执行的运动状态、完整的环境证据支持及明确的新旧航迹承接关系；任一版本记录写入失败时撤销本次状态变化并保持原航迹版本有效，从而避免当前航迹终止状态、候选航迹生效状态及飞行控制器执行指向不一致，保证航迹切换过程中的状态连续性、版本一致性及形成过程可追溯。