一种基于能耗-时间最优的低空无人机与无人车在线对接方法

本发明本发明属于低空智能交通与空地协同机器人，具体涉及一种面向无人机与地面移动平台协同回收/补能/交接任务的能耗-时间联合优化的在线对接点求解与轨迹规划方法，适用于gps受限、地图不完备、环境存在未知障碍物的任务场景。

背景技术：

1、低空空地协同系统（uav-ugv）在巡检、配送、应急与移动补能等任务中得到了广泛应用，其关键目标是利用uav的机动与视角优势以及ugv的载荷与低单位能耗优势，实现任务执行后的在线会合、对接回收与协同返航。在典型的协同回收场景中，uav完成前端任务后需要快速与ugv会合并安全降落对接，而ugv则需在存在障碍物的地面环境中满足车辆运动学约束并抵达对接点。该问题天然具有“异构约束强耦合”的特征：uav平面上近似全向运动但能耗显著高于ugv，ugv则受最小转弯半径等非完整约束影响且必须避障。因此，现有技术围绕定位与末端对接、能耗-时间最优会合点选择与协同规划和避障与在线重规划。

2、在定位与末端对接方向，现有方法大体可分为两类：一类依赖gnss/rtk等外部基础设施实现全局导航与回收，但在遮挡、峡谷效应或室内环境下易出现覆盖与精度不足问题；另一类侧重近距离末端精对接，常采用视觉标志或视觉伺服实现精准降落，例如利用视觉完成位姿解算与末端引导。为降低对强基础设施的依赖，进一步出现了uwb测距/定位与视觉末端引导的融合式方案，使用uwb提供中远距离引导、视觉完成末端对接，从而提升gnss-denied环境下的可用性。然而，上述研究多数把关注点放在如何对准并降落，缺乏对协同车辆的运动学可达性与避障代价显式纳入对接点决策，导致在复杂地面环境中可能出现对接点几何上合理，但ugv代价巨大或不可达的工程问题。

3、在协同会合与路径规划方向，现有方法通常以距离最短或时间最短为主要优化目标，并在地图已知或弱变化条件下生成离线或滚动路径。常见的启发式会合点选择包括几何中心类策略以及基于几何最短连通结构的策略（如几何中心点和费马点思想），其优势在于形式简洁、计算开销低。但在异构系统中，uav单位位移能耗远高于ugv，且ugv必须遵守非完整约束；因此单纯以几何距离或时间为主导的会合点策略往往无法反映系统级最优：uav可能为了“缩短时间”而承担不必要的长距离飞行与高能耗，而ugv本可承担更多路程并在对接后搭载uav返航，从系统角度反而更优。此外，许多工作默认存在全局地图或静态障碍，而真实场景中障碍物常由ugv局部感知逐步发现并呈现不完备性，使离线会合点与离线路径在遭遇新障碍后失效。

4、因此，如何在未知或动态环境中，基于ugv的局部感知信息打破离线规划的局限，实现顾及运动学约束与系统能耗的在线会合点重选与轨迹协同重规划，成为提升系统鲁棒性与实用性的关键难点。

技术实现思路

1、发明目的：针对现有空地协同对接方法未兼顾异构能耗特性、非完整运动学约束与在线避障需求的不足，本发明提供一种基于能耗-时间最优的在线对接方法，实现对接点与协同轨迹的联合优化与动态更新。

2、技术方案：本发明提供基于能耗-时间最优的低空无人机与无人车在线对接方法，包括以下步骤：

3、步骤s1，获取无人机与无人车的实时状态信息、基地位置信息及系统约束参数，构建能耗-时间联合代价函数，并施加空地同步到达约束；

4、步骤s2，以对接点坐标为决策变量，在满足无人车dubins运动学约束与安全避障约束的前提下，对所述能耗-时间联合代价函数进行求解，得到初始最优对接点及无人机与无人车的初始协同轨迹；

5、步骤s3，无人车在执行所述初始协同轨迹的过程中，通过车载传感器实时感知前方障碍物，当检测到障碍物且满足触发条件时，对所述障碍物进行膨胀建模，生成无人车基于dubins曲线的最短绕障路径，并将绕障代价纳入所述能耗-时间联合代价函数；

6、步骤s4，基于已生成的绕障路径和当前无人车位姿，在线重新求解所述能耗-时间联合代价函数，将所述最优对接点更新为重新求解得到的新最优对接点，并同步生成更新后的无人机与无人车协同轨迹，且维持所述空地同步到达约束；

7、步骤s5，重复执行步骤s3至步骤s4，直至无人机与无人车在更新后的对接点完成会合、降落对接与搭载返航。

8、进一步地，所述空地同步到达约束为：无人机到达对接点的时刻不早于无人车到达对接点的时刻，即无人机晚于或与无人车同时到达，以避免无人机早到后悬停等待所造成的额外能耗。

9、进一步地，所述无人车dubins运动学约束包括无人车的最小转弯半径约束，无人车从起始位姿到目标对接点的可行路径由dubins曲线表征。

10、进一步地，所述步骤s3中对障碍物进行膨胀建模具体包括：根据无人车车体宽度l和障碍物最大半径，构造膨胀障碍圆，其半径。

11、进一步地，所述步骤s3中的触发条件为：无人车当前位置与障碍物之间的距离d(pg,pob)小于或等于所述膨胀障碍圆半径di与预设安全距离ds之和，即。

12、进一步地，所述步骤s3中生成无人车基于dubins曲线的最短绕障路径具体包括：在无人车当前位姿处，求解与膨胀障碍圆的可行切点及对应切线航向，并由dubins曲线生成绕障轨迹段，所述绕障轨迹段满足无人车最小转弯半径约束。

13、进一步地，所述步骤s4中在线重新求解所述能耗-时间联合代价函数时，当检测到候选最优对接点落入障碍物内部时，系统继续求解下一最优对接点作为替代，直至获得满足安全避障约束的可行对接点。

14、本发明另一方面提供一种基于能耗-时间最优的低空无人机与无人车在线对接系统，包括：

15、协同状态管理与代价建模模块，用于获取无人机与无人车的实时状态信息、基地位置信息及系统约束参数，构建能耗-时间联合代价函数，并施加空地同步到达约束；

16、对接点在线优化模块，用于以对接点坐标为决策变量，在满足无人车dubins运动学约束与安全避障约束的前提下，对所述能耗-时间联合代价函数进行求解，得到最优对接点及无人机与无人车的协同轨迹；

17、障碍触发绕障与目标级重规划模块，用于在无人车通过车载传感器实时感知到障碍物且满足触发条件时，对障碍物进行膨胀建模，生成无人车基于dubins曲线的最短绕障路径，将绕障代价纳入所述能耗-时间联合代价函数，并触发所述对接点在线优化模块重新求解更新后的最优对接点与协同轨迹。

18、进一步地，所述系统采用事件触发的循环结构：初始化时求解初始最优对接点；在无人车未进入对接圆区域前持续执行；当满足障碍触发条件时，完成障碍膨胀、求解新的最优对接点、更新无人机与无人车轨迹并继续执行；若未触发，则沿既定轨迹执行，直至完成对接与返航任务。

19、本发明的方法，用于解决无人机（uav）与地面移动平台（ugv）在地图不完备、障碍物在线出现、ugv存在最小转弯半径约束条件下的会合对接问题。本方法以系统级能耗–时间联合代价为优化目标，并引入同步到达约束，在ugv局部感知到障碍后触发目标级在线重规划，从而在保证可执行与安全避障的前提下，降低无人机悬停等待造成的额外能耗与总时延。

20、本发明的方法实现结构由三个按顺序连接的核心组件构成：协同状态管理与代价建模模块、对接点在线优化模块、障碍触发绕障与目标级重规划模块。整体工作流程如图1所示。首先，系统采集uav与ugv状态、基地信息及约束参数，构建能耗–时间联合代价与同步到达约束；随后对接点在线优化模块在可行域内求解最优对接点，并生成uav/ugv协同轨迹；最后，ugv在执行过程中利用激光雷达感知障碍物，当触发条件满足时对障碍进行膨胀建模并计算可执行绕障路径，将绕障代价纳入新的优化问题以更新对接点与协同轨迹，循环执行直至完成对接并进入搭载返航阶段。

21、为便于描述如图2所示，uav在时刻0位于空中初始位置，ugv位于地面初始位姿()，基地位置为；uav与ugv的巡航速度分别为与。系统目标是在满足ugv最小转弯半径约束（dubins运动学）及安全避障约束下，求解对接点并生成uav/ugv同步到达的协同轨迹，使得全系统的“能耗-时间”综合代价最小。ugv和uav的动力学模型如下：

22、                            （1）

23、                               （2）

24、其中与因其物理限制都是有上限的。

25、a） 能耗—时间联合代价下的初始最优对接点求解

26、如图2所示，在无障碍或尚未触发避障重规划时，首先求解初始最优对接点。本发明将ugv的可达路径长度以dubins路径长度表征，将uav的飞行段用欧氏距离表征，并将对接后返航（或回到基地）的段长纳入统一代价。从系统级角度，引入：①协同系统能耗权重α∈(0,1)，用于刻画“uav段能耗 vsugv段能耗”的相对比重；②能耗/时间权重、，用于刻画任务对“能耗最优 vs 时间最优”的偏好（可设=1-）。据此构造能耗-时间联合代价函数，并以区域内坐标为决策变量求最小值，从而得到，具体公式如下：

27、           （3）

28、其中、是uav的能耗系数，是ugv的能耗系数。

29、同时，为保证对接过程的平滑与可实施性，本发明在求解中加入“空地同步到达”约束：uav到达对接点所需时间比ugv到达对接点所需时间晚，保证了uav的飞行时间最短。该约束使得两平台在对接点附近无需额外等待即可完成会合，具体约束如下式：

30、               （4）

31、b） ugv局部障碍膨胀建模与基于dubins的最短避障路径生成

32、在缺乏全局地图信息的场景中，ugv需依赖车载传感器在线感知障碍。如图3，当ugv在行进过程中检测到障碍物并逐步接近至触发条件（例如时，该障碍已被充分观测并可用于规划如下：

33、                        （5）

34、为反映ugv车体宽度l对安全避障的影响，本发明不将ugv视作点质点，而对障碍进行“膨胀”处理：若第i个障碍的最大半径为、中心为，则构造膨胀障碍圆，其半径

35、。膨胀圆如下：

36、                        (6)

37、随后，在ugv当前位姿处，依据ugv最小转弯半径约束，求解与膨胀圆的可行切点（左转/右转切线点）与对应切线航向，并由dubins曲线生成绕障轨迹段。该过程保证ugv绕障路径可跟踪、可执行，并能与后续对接点更新自然衔接。

38、c） 在线对接点更新与协同轨迹重规划（事件触发循环）

39、当障碍导致原对接点不再可行（例如对接点被障碍包围、ugv到达路径受阻、或安全距离约束被违反）时，本发明触发在线更新机制：在已生成的绕障dubins段长度基础上，重新构造包含绕障代价的优化问题，在线求解新的最优对接点，并同步生成新的uav/ugv协同轨迹。

40、具体地，ugv侧在执行绕障的同时，利用当前位姿与已观测障碍集合更新其到候选对接点的可达dubins长度；uav侧根据新对接点更新飞行参考。为保证会合的一致性，新优化同样维持“空地同步到达”时间约束，使得uav与ugv在更新后的对接点处完成会合、降落与搭载，代价函数（7）和约束条件（8）如下：

41、            （7）

42、              （8）

43、为使该在线过程可工程落地，本发明采用事件触发的循环结构：初始化时求解；在ugv未进入对接圆区域前持续执行；一旦满足障碍触发条件，则完成障碍膨胀、求解新对接点、更新uav/ugv轨迹并继续执行；若未触发，则沿既定轨迹执行。该循环直至完成对接与返航任务，其伪代码算法如下algorithm 1所示。

44、

45、与现有技术相比，本发明的有益效果在于：

46、1、总体算法技术路线的提出：本发明提出“能耗–时间联合最优+同步到达约束+目标级在线重规划”的空地协同在线对接技术路线，为无人机–地面车在地图不完备与动态障碍环境下实现可执行、可避障、可同步的对接回收提供统一框架，相比仅基于几何启发式或仅路径级重规划的方法，更适用于低空协同回收等对实时性与鲁棒性要求高的场景；

47、2、基于系统级联合代价的对接点在线最优化：本发明将uav高能耗特性与ugvdubins可达性统一映射到能耗–时间联合代价函数中，并通过权重化机制实现对任务偏好（节能/快速）的可调节优化；该技术特征直接带来“对接点选择更贴合异构平台代价结构”的效果，避免对接点几何上合理但ugv不可达或系统代价偏高的问题；

48、3、基于同步到达约束的等待抑制效果：本发明在优化中显式施加uav与ugv同步到达约束（或等效罚项），使协同轨迹在生成阶段即抑制uav悬停等待与ugv空等，从机制上减少等待造成的无效能耗与总时延，提高对接过程的连续性与稳定性；

49、4、基于障碍触发的目标级在线重规划效果：本发明采用“触发-膨胀-绕障dubins生成-对接点再优化”的闭环机制，在ugv局部感知到障碍后不仅更新路径，还在线更新对接点并同步更新uav/ugv轨迹，从而在满足车体尺寸与安全裕度约束的前提下保持可行性与代价优势，提升动态障碍场景下的对接成功率与鲁棒性。