一种面向空地异构多机器人的安全轨迹协同规划方法

技术领域

本发明涉及智能机器人技术领域，特别是一种面向空地异构多机器人的安全轨迹协同规划方法。

背景技术

由于自动控制和人工智能的进步，无人机和无人车已成功应用于各种军事和民用领域。然而，由于其不完整的感知、决策和控制能力，无人机或无人车仍然面临许多挑战。因此，有必要将无人机和无人车结合起来进行协同感知、决策和控制，以利用它们的异构和互补特性。这种组合，即空地异构系统，已应用于包括定位、农业、监视、交通和建筑等许多领域。

空地系统的轨迹规划需要无人机和无人车利用环境信息实时动态规划协同任务的轨迹。现有的轨迹规划方法包括基于演化等数值优化的规划方法。这些方法为了考虑轨迹的平滑度和车辆模型通常会带来很大的计算负担。近年来，采用滚动时域优化的模型预测控制(MPC)在轨迹规划领域中显示出巨大的潜力，因为它可以更好地处理动态环境和轨迹平滑。为了实现避障轨迹的快速规划，通常将势函数作为软约束引入到MPC中，但势场函数的不可微分特性导致了高成本的计算负担，并经常出现次优路径和局部最小值点问题，且软约束不提供避障保证。

与单一的无人机或无人车系统不同，空地异构系统允许无人机和无人车共同规划轨迹，需要应对传感信息不一致的问题。目前对于该问题的研究，当无人车无法自主做出决定时，通常指定无人机作为无人车的完全替代决策者。然而，无人机也有传感和能量限制，因此难以保证无人机作为决策者是完全可靠的。因此，空地系统面临的挑战之一是如何动态解决决策冲突以实现协同轨迹规划，同时完成预定义任务。行为控制是应对这一挑战的解决方案之一，在这种情况下，基于零空间的行为控制方法通过融合单体的基本任务来解决轨迹规划冲突，以进行空地系统的协同轨迹规划，并确保高优先级任务完全执行和低优先级任务部分执行。然而，传统的零空间投影行为控制方法采用有限状态机或模糊任务监督器，通常会导致任务优先级的频繁切换，从而降低控制性能和系统稳定性。对此基于模型预测控制的任务监督器通过考虑预测信息，给出了更智能的任务优先级切换，但整数规划的性质使得在任务数量很大时难以实时实现。

因此需要考虑由无人机作为辅助角色而无人车作为执行器所组成的异构空地系统，无人车将无人机的辅助决策与自身决策相结合，仅依靠轻型计算资源进行实时的协同轨迹规划。

发明内容

本发明的目的在于提供一种面向空地异构多机器人的安全轨迹协同规划方法，将模型预测控制方法与基于零空间投影的行为控制方法相结合，采用事件触发机制，有效地融合无人机和无人车之间的决策信息。首先，无人机使用非线性模型预测控制生成无人车的引导轨迹，其中考虑多个障碍物的耦合效应以避免局部极小值问题；其次，采用基于零空间的行为控制框架将引导轨迹合并到无人车自身规划的任务中；最后开发了一个事件触发任务监督器来决定所有任务的优先级。该方法可以有效平衡消耗的计算资源，提高空地异构系统对于复杂环境的自主轨迹规划能力。

为实现上述目的，本发明的技术方案是：一种面向空地异构多机器人的安全轨迹协同规划方法，包括引导轨迹规划器构建、协同任务设计和复合任务动态切换，所述步骤S1所述的基于非线性模型预测控制的引导轨迹规划器的构建，其特征在于，具体包括以下步骤：

步骤S1、获取无人机传感探测信息，利用非线性模型预测控制方法，构建在线局部引导轨迹规划器，分析无人机视角下障碍物分布对无人车的耦合信息，产生无人机针对无人车高空视角的干预轨迹。

步骤S2、针对无人机与无人车的功能特性，基于行为控制方法进行空地异构系统协同、干预等基本任务的设计。

步骤S3、应用事件触发与零空间投影机制，设计任务融合规则，有效地合成多视角下的决策信息，同时为无人机和无人车输出协同规划的轨迹结果。

进一步地，所述步骤S1的具体包括以下步骤：

步骤S11、无人车时域模型空间重构：

所述非线性无人车运动学模型的表达式为,将包含横、纵坐标、高度、航向角的状态向量定义为ξ

其中

其中，

其中，ν

进一步地，得到重构的空间模型公式为：

步骤S12：构建基于非线性模型预测控制的轨迹生成器：

基于非线性模型预测控制的轨迹生成器是用来规划从当前位置开始到未来一段空间预测范围内的优化轨迹，定义N

其中，l(·)和l

步骤S13、构建优化问题的目标函数，具体即，构建优化目标的避障惩罚函数：

轨迹生成器对于障碍物的躲避是通过考虑目标函数中作为软约束的惩罚项实现的，该惩罚函数考虑了多个障碍物之间对于无人车的耦合影响，并且仅在障碍物接近无人车的安全距离时才起作用，以避免不必要的惩罚效果。避障惩罚函数被设计为：

其中，D(k，n)表示在第k个采样步上无人车和第n个障碍物之间的欧式距离,D

所提出的惩罚函数放大了局部极小值点的避障成本，使得多障碍物的耦合点可以被视为一个虚拟障碍物，在跟踪和避障成本之间的权衡中，当面对局部极小点时，会产生向外的排斥力，以防止所规划的轨迹接近它。

所述的优化目标的避障惩罚函数为：

/>

进一步地，总结预测范围N

其中，

其中，l

其中，

进一步地，所述步骤S2的具体包括以下步骤：

非线性模型预测控制构建的引导轨迹生成器从辅助视角提取的环境信息来优化引导状态集合

所述的基于行为控制方法进行空地异构系统协同、干预等基本任务的设计。

通过融合无人车的主观视角的信息，将无人机的轨迹引导作为基本任务之一，同时考虑自主轨迹跟踪和躲避障碍物等其它基本任务，再通过基于事件触发机制的零空间投影融合形成复合任务。

定义任务变量为ε，对应的任务函数可以表示为：

ε＝f(p)∈R

其中，p为系统状态向量，上标M∈{2,3}取决于考虑的机器人对象是无人机还是无人车，其相应的偏导数为：

其中，v是机器人的速度向量，J(p)表示任务的雅可比矩阵。因此单个任务的速度输出指令为：

其中，Λ是正定的常数增益矩阵，ε

进一步地，所述步骤S3的具体包括以下步骤：

通过零空间投影的方法，形成最终复合任务的输出指令，是通过任务监管器将基本任务进行优先级排序相结合而实现的，其中低优先级任务被投影到高优先级任务的零空间中，以消除任务冲突，多项任务的合成输出

其中上标η∈{a，g}既可以表示无人机也可以表示无人车，

进一步地，所述的一种基于事件触发机制通过任务监督器来动态调整任务优先级并平衡计算资源消耗。

定义以下两个优先级切换指标：

通过跟踪误差指标r

定义评估检测到的障碍物的实时影响r

考虑无人机感知精度或引导轨迹的计算负担，轨迹跟踪任务根据跟踪误差指标r

本发明还提供了一种面向空地异构多机器人的安全轨迹协同规划系统，其包括处理器、存储器以及存储于存储器上的计算机程序，当处理器运行所述程序指令时，能够实现如上文所述的方法步骤。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序指令，所述指令由处理器加载并执行时，能够实现如上文所述的方法。

相较于现有技术，本发明具有以下有益效果：

无人机使用非线性模型预测控制为无人车生成引导轨迹，其中避障的惩罚函数被设计为软约束。与以往将避障也视为软约束的方案不同，所述的惩罚函数考虑了障碍物分布密度来反映无人车周围多个障碍物的非线性耦合效应。实现能够在不增加在线计算负担的情况下，有效地避免局部极小值问题。所述的基于零空间投影的协同框架，解决无人机和无人车视角感知信息不一致导致的决策冲突问题，与使用以避障作为硬约束的非线性模型预测控制方法相比，所述的协同框架生成轨迹方案的速度要快得多，同时避免在决策融合过程中不同任务的权重参数的调整，还可以避免无人机辅助轨迹可能违反约束的问题。所述的基于事件触发机制的任务管理器与传统任务监管器相比，大大降低了任务切换频率，提高了规划轨迹的平滑度。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例的方法原理框图。

图2为本发明实施例的空地异构多机器人轨迹规划实验场景图。

图3为本发明实施例的空地异构多机器人协同规划方法与传统方法的轨迹图。

图4为本发明实施例的事件触发监管器和传统方法的任务优先级切换情况图。

具体实施方式

下面结合附图，对本发明的技术方案进行具体说明。

本实施例提供了一种面向空地异构多机器人的安全轨迹协同规划方法，本实施例选用一架无人机、一辆无人车和若干障碍物进行举例说明，具体包括以下步骤：

获取无人机传感探测信息，利用非线性模型预测控制方法，构建在线局部引导轨迹规划器，分析无人机视角下障碍物分布对无人车的耦合信息，产生无人机针对无人车高空视角的干预轨迹。

针对无人机与无人车的功能特性，基于行为控制方法进行空地异构系统协同、干预等基本任务的设计。

应用事件触发与零空间投影机制，设计任务融合规则，有效地合成多视角下的决策信息，同时为无人机和无人车输出协同规划的轨迹结果。

在本实施例中，该空地系统执行任务的主要目标是让无人车尽可能跟踪预先计划的轨迹，而无人机生成引导轨迹，保证无人车在沿预先规划的轨迹移动时避开障碍物和局部极小点，并且无人机需要与无人车保持所需的编队结构。

在本实施例中，考虑一组典型的异构空地系统，其中无人车在到达目标位置过程中避开可能遭遇的多个障碍物，并且无人机可以获得监视范围内障碍物的信息。定义空地系统在全局坐标系W_xyz中的包含横、纵坐标、高度、航向角的状态向量为ξ

本空地系统采用虚拟结构作为三维空间下的编队策略，并跟踪相对于无人车的预先规划的参数轨迹

其中，γ为无人机与无人车的相对距离，在XY平面上的投影为连接着无人车与目标点的直线γ′。γ′与X轴的夹角为β，

步骤S11、无人车时域模型空间重构：

所述非线性无人车运动学模型的表达式为：

其中，v

进一步地，将包含横、纵坐标、高度、航向角的状态向量定义为ξ

其中

其中，

其中，v

步骤S12、构建基于非线性模型预测控制的轨迹生成器：

基于非线性模型预测控制的轨迹生成器是用来规划从当前位置开始到未来一段空间预测范围内的优化轨迹，定义N

其中，l(·)和l

步骤S13、构建优化问题的目标函数

进一步地，构建优化目标的避障惩罚函数。

考虑多个障碍物之间对于无人车的耦合影响，并且仅在障碍物接近无人车的安全距离时才起作用，以避免不必要的惩罚效果。避障惩罚函数被设计为：

其中，D(k，n)表示在第k个采样步上无人车和第n个障碍物之间的欧式距离,D

所述的优化目标的避障惩罚函数为：

进一步地，总结预测范围N

其中，

其中，l

其中，

进一步地，所述步骤S2的具体包括以下步骤：

为了融合无人车的主观视角的信息，将无人机的轨迹引导作为基本任务之一，同时考虑自主轨迹跟踪和躲避障碍物等其它基本任务，再通过基于事件触发机制的零空间投影融合形成复合任务。

步骤S21、基本任务的设计：

进一步地，我们为空地系统定义了两个基本任务函数，即轨迹跟踪任务

轨迹跟踪任务函数定义为：

其中，p

基于闭环逆运动学的无人车对应的轨迹跟踪任务输出为：

无人机以保持编队几何形状的轨迹跟踪任务函数的输出信号为：

其中，

其中，

考虑在无人机的轨迹生成器中，避障是通过考虑目标函数中的软约束来实现的。由于软约束的固有性质和异构空地系统的传感器信息不一致，生成的轨迹仍可能违反安全约束。在这里，引入了严格的避障作为一项基本任务来补偿这种不一致。

当无人车的安全范围D

其中，

避障任务的雅可比矩阵

避障任务的输出由下式得到：

其中

步骤S22、多任务冲突消解

进一步地，通过零空间投影的方法，形成最终复合任务的输出指令，是通过任务监管器将基本任务进行优先级排序相结合而实现的，其中低优先级任务被投影到高优先级任务的零空间中，以消除任务冲突。多项任务的合成输出

上标η∈{a，g}既可以表示无人机也可以表示无人车，

进一步地，定义以下两个优先级切换指标：

通过跟踪误差指标r

定义评估检测到的障碍物的实时影响r

考虑无人机感知精度或引导轨迹的计算负担，轨迹跟踪任务根据跟踪误差指标re在自主跟踪任务和引导轨迹跟踪任务之间切换。定义以下触发规则：

其中，

多个障碍物通常会导致不断违反避障约束，为了解决这个问题，定义如下触发规则：

条件T

在本实施例中，将所述的提出的协同轨迹规划方法与将避障作为硬约束的非线性模型预测控制方法进行比较，以及将部署在空地系统中的事件触发任务监督器与仅部署在没有无人机辅助的无人车平台上的传统基于有限状态机的任务监督器进行比较，并对结果进行统计分析。

所述协同轨迹规划方法的参数设置如表1所示。

表1协同轨迹规划方法的参数设置

/>

考虑存在多个障碍物形成局部最小值点的场景如图2所示，轨迹规划结果如图3所示。在t

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。