CN114326810A - 一种无人机在复杂动态环境下的避障方法 - Google Patents

Abstract

本发明涉及移动机器人自主导航技术领域，更具体地，涉及一种无人机在复杂动态环境下的避障方法。相比于已有的建模为匀速直线运动物体而言，本发明引入了高机动性目标的特性，引入了多种模型并使用IMM算法进行结合从而进行障碍物轨迹预测以提高鲁棒性。另一方面，由于感知会有误差，障碍物建模时考虑了不确定性。建模成椭球时，椭球质点信息会经过卡尔曼滤波器进行更好地估计，且多个子模型会并行预测估计；椭球大小充分考虑安全性，进行关于时间推移的膨胀，从而能更安全地避开动态障碍物。

Description

一种无人机在复杂动态环境下的避障方法

技术领域

本发明涉及移动机器人自主导航技术领域，更具体地，涉及一种无人机在复杂动态环境下的避障方法。

背景技术

无人机在复杂动态的环境中实现合理规划与避障是无人机自主导航的关键问题。在真实的飞行环境中，无人机面对的往往是非常复杂的场景，障碍物也会有静态和动态之分，这对无人机的轨迹规划算法的稳定性和准确性的要求非常高。对于静态障碍物，不同障碍物的形状会差别巨大，需要进行合理规避。对于动态障碍物，运动轨迹时不确定的，机动类型不同会导致难以跟踪，规划算法需要有合理的障碍物预测机制。同时，受无人机机体限制，传感器和计算资源的配置是受限的，所以轨迹规划算法需要良好的实时性。因此，如何根据无人机的局部感知信息，规划出一条鲁棒性高、实时性好、运动可行、能量消耗小、能提前规避各类障碍物的运动轨迹，是无人机在复杂动态环境下实现自主避障导航的关键性问题。

目前，对于复杂动态场景下进行动态障碍物避障的方法，有很多学者提出了不同的方法。最经典的方法是采用速度障碍法(Velocity Obstacle，VO)来进行避障，它的思路是只要在未来有可能发生碰撞的速度，全部排除在外。由此引申的方法有互惠式VO方法保证互相避障时不抖动、ORCA方法实现分布式避障等。但这类算法的缺点在于，没有考虑到实际机器人的运动学特性，而且障碍物并不能与机器人进行交互，所以互惠对于避障是不起作用的。另外还有的方法使用了人工势场法用于动态障碍物环境下进行避障。这类方法的原理是将障碍物当作斥力点，局部目标点当作引力点，模拟斥力和引力来进行避障。这种方法的缺点是运动轨迹较容易陷入局部极小值。另外，有学者基于对动态障碍物进行预测的方法进行轨迹规划，将障碍物假设为匀速直线运动模型，在此基础上加入高斯模型来引入障碍物的不确定性，从而尽可能远离障碍物。这种方法的优点是考虑到的障碍物的运动趋势，且将此方法融入多项式轨迹优化算法中，避障的效果较好，缺点是匀速直线运动不能够预测所有的动态障碍物趋势，如果出现高机动性的障碍物，会出现较大的误差。

发明内容

本发明为克服上述现有技术中的缺陷，提供一种无人机在复杂动态环境下的避障方法，能够对动态障碍物进行合理预测，从而能更安全地避开动态障碍物。

为解决上述技术问题，本发明采用的技术方案是：一种无人机在复杂动态环境下的避障方法，包括以下步骤：

S1.引入二阶常速模型和三阶常速模型，每个模型中，障碍物状态信息分别先经过模型进行状态估计，然后经过卡尔曼滤波器进行模型滤波预测；

S2.使用交互式多模型滤波算法，将步骤S1中的几个模型并联起来，综合每个模型的预测，最终得到与多个模型交互作用的结果；

S3.将障碍物进行建模，利用感知模块和模型交互得到的位置以及长宽高，将障碍物建模为一个椭球，椭球的中心由上述模型预测得到，椭球的大小由感知模块计算得出；

S4.考虑不确定性因素，对障碍物椭球的大小充分考虑安全性，用膨胀的方法，让椭球未来的预测大小随着时间的推移而膨胀；

S5.对预测得到的当前和未来的障碍物状态指定代价函数，作为其中一项优化项加入轨迹优化的目标函数中；最终优化得到一条无碰撞的安全飞行轨迹。

在本发明中，首先提出使用交互式多模型的方法引入多个子模型进行滤波预测和加权融合，得到鲁棒性更高的障碍物预测轨迹。然后利用感知和模型交互得到的障碍物信息，将障碍物建模为一个椭球，并使用椭球方程作为软约束的优化项以优化路径。最后将障碍物未来的椭球大小表现为逐渐膨胀的形式，通过计算考虑膨胀速率，对障碍物未来的不确定性进行估计。相比直接使用感知和轨迹优化的方法，该方法能够更好地了解动态障碍物的信息，并通过鲁棒性更强的策略，从而更好地避开动态障碍物，在复杂动态环境下安全地实现自主导航。

进一步的，所述的步骤S1中二阶常速模型和三阶常速模型表示如下：

二阶常速模型：

三阶常速模型：

式中，p、

分别为障碍物的位置、速度和加速度的分量；w(t)是均值为零、方差为σ²的白噪声；上述是对一个坐标轴的阐述，对于笛卡尔坐标轴的每一个方向都按此模型计算。

进一步的，所述的步骤S2中采用马尔可夫切换稀疏的交互式多模型(Interacting Multiple Model，IMM)滤波算法。

进一步的，所述的马尔可夫切换稀疏的交互式多模型首先将当前时刻状态与前一时刻的状态输入每个模型，进行交互和滤波预测，从而得到当前时刻的估计值；然后进行滤波预测，将每个模型进行卡尔曼滤波处理；最后更新每一个子模型的概率，使用最大似然函数法，通过计算各个模型与当前机动模式匹配程度的大小，得到各个模型的输出概率和输出结果，最终进行加权融合，得到交互输出的最终结果。

进一步的，所述的感知模块用于利用各种传感器的信息，计算出障碍物的位置信息和大小信息。

进一步的，所述的步骤S3中，将动态障碍物建模为椭球，椭球的方程可表示如下：

(Q-p^o)^TΘ(Q-p^o)＝1

式中，Q是无人机的位置，p^o是障碍物中心的位置，Θ是椭球的形状参数，是一个对角矩阵，具体如下：

式中，r_f为膨胀半径，

是障碍物椭球在三个坐标轴上的长度。

进一步的，在所述的步骤S4中，使用膨胀的方法，对障碍物椭球未来的大小进行等速率膨胀；获取膨胀速率的方法包括以下步骤：

取其中一个障碍物的任意一轴进行考虑，以X轴为例，椭球在t₀时刻在X 轴上的位置为

半轴长为r_x，速度为

加速度为

假定障碍物有最大速度v_max和最大加速度a_max，考虑障碍物的匀加速运动特性，在t_i时刻椭球中心在该轴预期可以到达的位置如下：

其中，函数b(·)指的是限幅函数，即b(v_max,v)＝min(v_max,max(-v_max,v))；

在加速度不确定的情况下，椭球中心可以到达的范围如下：

其中，

指的是t₀到t_i时间段内能往负半轴移动的最远位移，

指的是 t₀到t_i时间段内能往正半轴移动的最远位移；

因此，膨胀后的半轴长要取

和到达范围内的点中的最大距离再加上原椭球的半轴长，结果计算如下：

因此，椭球未来的大小可以表示如下：

指的就是椭球在该轴大小的膨胀速率，具体为：

进一步的，在所述的步骤S5中，避障函数的优化项表示为：

式中，N_c指从时刻t₀到未来指定一段时间内的所有离散时刻数量，N_d指所有障碍物数量，

指在时刻i下障碍物j的位置，

指在时刻i下障碍物j的椭球形状参数；

在所述的步骤S5中，对于每个时刻i，无人机在该时刻会有一个控制点Q_i，在这个时刻，每个障碍物j同样预测得到障碍物的位置和形状，位置计算得到p_ij，形状计算得到

计算障碍物与无人机之间的大致距离，通过椭球的方程和梯度的方法来进行优化；整个约束项J_d将作为一个软约束项，加入到局部路径的轨迹规划函数中，参与轨迹优化问题的求解。

本发明还提供一种电子设备，包括：存储器、处理器以及存储在存储器上并可在处理器上运行计算机程序，所述的处理器执行所述计算机程序，实现以上所述的无人机在复杂动态环境下的避障方法。

本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述的计算机程序被处理器执行时，实现以上所述的无人机在复杂动态环境下的避障方法。

与现有技术相比，有益效果是：本发明提供的一种无人机在复杂动态环境下的避障方法，相比于已有的建模为匀速直线运动物体而言，本发明引入了高机动性目标的特性，引入了多种模型并使用IMM算法进行结合从而进行障碍物轨迹预测以提高鲁棒性。另一方面，由于感知会有误差，障碍物建模时考虑了不确定性。建模成椭球时，椭球质点信息会经过卡尔曼滤波器进行更好地估计，且多个子模型会并行预测估计；椭球大小充分考虑安全性，进行关于时间推移的膨胀，从而能更安全地避开动态障碍物。

附图说明

图1是本发明的方法流程示意图。

图2是本发明的处理过程示意图。

图3是本发明在仿真平台中实施该技术方案的演示示意图。

图4是本发明在实际场景中实施该技术方案的演示示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。下面结合具体实施方式对本发明作在其中一个实施例中说明。其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，全文中出现的“和/或”的含义为，包括三个并列的方案，以“A和/ 或B”为例，包括A方案，或B方案，或A和B同时满足的方案。

实施例1：

如图1所示，一种无人机在复杂动态环境下的避障方法，包括以下步骤：

S1.引入二阶常速模型和三阶常速模型，每个模型中，障碍物状态信息分别先经过模型进行状态估计，然后经过卡尔曼滤波器进行模型滤波预测。

考虑随机干扰的情况下。对于一般情况下的机动目标，即大部分情况下目标做匀速或匀加速直线运动时，可采用如下的二阶常速(Constant Velocity， CV)模型和三阶常加速(Constant Acceleration，CA)模型。

二阶常速模型：

三阶常速模型：

式中，p、

分别为障碍物的位置、速度和加速度的分量；w(t)是均值为零、方差为σ²的白噪声；上述是对一个坐标轴的阐述，对于笛卡尔坐标轴的每一个方向都按此模型计算。

为了增加模型的鲁棒性，在经过每个子模型之后，叠加卡尔曼滤波器，卡尔曼滤波器可以使得在线性系统中建立准确的模型。如果对机动性更强的障碍物，可以采用更好的子模型，以增加鲁棒性。

S2.使用交互式多模型滤波算法，将步骤S1中的几个模型并联起来，综合每个模型的预测，最终得到与多个模型交互作用的结果。

采用具有马尔可夫切换稀疏的交互式多模型(Interacting Multiple Model，IMM)滤波算法，以适应障碍物运动的多变性和不确定性。IMM算法首先将当前时刻状态与前一时刻的状态输入每个模型，进行交互和滤波预测，从而得到当前时刻的估计值。然后进行滤波预测，将每个模型进行卡尔曼滤波处理。最后更新每一个子模型的概率，使用最大似然函数法，通过计算各个模型与当前机动模式匹配程度的大小，得到各个模型的输出概率和输出结果，最终进行加权融合，得到交互输出的最终结果。

利用模型预测，可以对障碍物未来的位置进行估计，其中即考虑了不确定性因素。

S3.将障碍物进行建模，利用感知模块和模型交互得到的位置以及长宽高，将障碍物建模为一个椭球，椭球的中心由上述模型预测得到，椭球的大小由感知模块计算得出。

将动态障碍物建模为椭球，椭球的方程可表示如下：

(Q-p^o)^TΘ(Q-p^o)＝1 (3)

式中，Q是无人机的位置，p^o是障碍物中心的位置，Θ是椭球的形状参数，是一个对角矩阵，具体如下：

式中，r_f为膨胀半径，

是障碍物椭球在三个坐标轴上的长度。

S4.考虑不确定性因素，对障碍物椭球的大小充分考虑安全性，用膨胀的方法，让椭球未来的预测大小随着时间的推移而膨胀。

考虑对障碍物未来的大小进行不确定性估计，将安全性纳入到大小预测的考虑中。这里使用膨胀的方法，对障碍物椭球未来的大小进行等速率膨胀，以方便避障算法规划并躲避椭球。该方法的关键是获得障碍物椭球的膨胀速率。

取其中一个障碍物的任意一轴进行考虑，以X轴为例，椭球在t₀时刻在X 轴上的位置为

半轴长为r_x，速度为

加速度为

假定障碍物有最大速度v_max和最大加速度a_max，考虑障碍物的匀加速运动特性，在t_i时刻椭球中心在该轴预期可以到达的位置如下：

其中，函数b(·)指的是限幅函数，即b(v_max,v)＝min(v_max,max(-v_max,v))；

在加速度不确定的情况下，椭球中心可以到达的范围如下：

其中，

指的是t₀到t_i时间段内能往负半轴移动的最远位移，

指的是 t₀到t_i时间段内能往正半轴移动的最远位移；

因此，膨胀后的半轴长要取

和到达范围内的点中的最大距离再加上原椭球的半轴长，结果计算如下：

因此，椭球未来的大小可以表示如下：

指的就是椭球在该轴大小的膨胀速率，具体为：

其他轴以及其他障碍物同理。

通过上述膨胀方法，在t₀到t_i时刻，不管障碍物椭球以任意机动性运动，都会在t_i时刻的膨胀椭球内。这个膨胀椭球就是无人机的不安全范围，用该椭球来进行轨迹优化，可以实现在规划的未来一段时间内避开障碍物，从而保证了安全性。

S5.对预测得到的当前和未来的障碍物状态指定代价函数，作为其中一项优化项加入轨迹优化的目标函数中；最终优化得到一条无碰撞的安全飞行轨迹。

利用上述椭球方程，可以得到障碍物的相关特性，从而进行规划。如，在公式(3)中，当左边等式等于1时，表示恰好在椭球表面，小于等于1时，表示障碍物与无人机发生了碰撞，这种情况是不能出现的，大于1时表示没有发生碰撞。并且可以利用左边等式的值，得知离椭球大致有多远的距离，值越大距离越远，形成了可以优化的梯度。

避障函数的优化项表示为：

式中，N_c指从时刻t₀到未来指定一段时间内的所有离散时刻数量，N_d指所有障碍物数量，

指在时刻i下障碍物j的位置，

指在时刻i下障碍物j的椭球形状参数；

在所述的步骤S5中，对于每个时刻i，无人机在该时刻会有一个控制点Q_i，在这个时刻，每个障碍物j同样预测得到障碍物的位置和形状，位置计算得到p_ij，形状计算得到

计算障碍物与无人机之间的大致距离，通过椭球的方程和梯度的方法来进行优化；整个约束项J_d将作为一个软约束项，加入到局部路径的轨迹规划函数中，参与轨迹优化问题的求解。

经过上述轨迹优化函数和安全性的设计，最终可以优化得到一条无碰撞的安全飞行轨迹。

实施例2

本实施例提供一种电子设备，包括：存储器、处理器以及存储在存储器上并可在处理器上运行计算机程序，所述的处理器执行所述计算机程序，实现实施例1述的无人机在复杂动态环境下的避障方法。

实施例3

本实施例提供一种计算机可读存储介质，其上存储有计算机程序，所述的计算机程序被处理器执行时，实现实施例1所述的无人机在复杂动态环境下的避障方法。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种无人机在复杂动态环境下的避障方法，其特征在于，包括以下步骤：

S1.引入二阶常速模型和三阶常速模型，每个模型中，障碍物状态信息分别先经过模型进行状态估计，然后经过卡尔曼滤波器进行模型滤波预测；

S2.使用交互式多模型滤波算法，将步骤S1中的几个模型并联起来，综合每个模型的预测，最终得到与多个模型交互作用的结果；

S3.将障碍物进行建模，利用感知模块和模型交互得到的位置以及长宽高，将障碍物建模为一个椭球，椭球的中心由上述模型预测得到，椭球的大小由感知模块计算得出；

S4.考虑不确定性因素，对障碍物椭球的大小充分考虑安全性，用膨胀的方法，让椭球未来的预测大小随着时间的推移而膨胀；

S5.对预测得到的当前和未来的障碍物状态指定代价函数，作为其中一项优化项加入轨迹优化的目标函数中；最终优化得到一条无碰撞的安全飞行轨迹。

2.根据权利要求1所述的无人机在复杂动态环境下的避障方法，其特征在于，所述的步骤S1中二阶常速模型和三阶常速模型表示如下：

二阶常速模型：

三阶常速模型：

式中，p、

分别为障碍物的位置、速度和加速度的分量；w(t)是均值为零、方差为σ²的白噪声；上述是对一个坐标轴的阐述，对于笛卡尔坐标轴的每一个方向都按此模型计算。

3.根据权利要求2所述的无人机在复杂动态环境下的避障方法，其特征在于，所述的步骤S2中采用马尔可夫切换稀疏的交互式多模型。

4.根据权利要求3所述的无人机在复杂动态环境下的避障方法，其特征在于，所述的马尔可夫切换稀疏的交互式多模型首先将当前时刻状态与前一时刻的状态输入每个模型，进行交互和滤波预测，从而得到当前时刻的估计值；然后进行滤波预测，将每个模型进行卡尔曼滤波处理；最后更新每一个子模型的概率，使用最大似然函数法，通过计算各个模型与当前机动模式匹配程度的大小，得到各个模型的输出概率和输出结果，最终进行加权融合，得到交互输出的最终结果。

5.根据权利要求1所述的无人机在复杂动态环境下的避障方法，其特征在于，所述的感知模块用于利用各种传感器的信息，计算出障碍物的位置信息和大小信息。

6.根据权利要求1所述的无人机在复杂动态环境下的避障方法，其特征在于，所述的步骤S3中，将动态障碍物建模为椭球，椭球的方程可表示如下：

(Q-p^o)^TΘ(Q-p^o)＝1

式中，Q是无人机的位置，p^o是障碍物中心的位置，Θ是椭球的形状参数，是一个对角矩阵，具体如下：

式中，r_f为膨胀半径，

是障碍物椭球在三个坐标轴上的长度。

7.根据权利要求6所述的无人机在复杂动态环境下的避障方法，其特征在于，在所述的步骤S4中，使用膨胀的方法，对障碍物椭球未来的大小进行等速率膨胀；获取膨胀速率的方法包括以下步骤：

取其中一个障碍物的任意一轴进行考虑，以X轴为例，椭球在t₀时刻在X轴上的位置为

半轴长为r_x，速度为

加速度为

假定障碍物有最大速度v_max和最大加速度a_max，考虑障碍物的匀加速运动特性，在t_i时刻椭球中心在该轴预期可以到达的位置如下：

其中，函数b(·)指的是限幅函数，即b(v_max,v)＝min(v_max,max(-v_max,v))；

在加速度不确定的情况下，椭球中心可以到达的范围如下：

其中，

指的是t₀到t_i时间段内能往负半轴移动的最远位移，

指的是t₀到t_i时间段内能往正半轴移动的最远位移；

因此，膨胀后的半轴长要取

和到达范围内的点中的最大距离再加上原椭球的半轴长，结果计算如下：

因此，椭球未来的大小可以表示如下：

指的就是椭球在该轴大小的膨胀速率，具体为：

8.根据权利要求7所述的无人机在复杂动态环境下的避障方法，其特征在于，在所述的步骤S5中，避障函数的优化项表示为：

式中，N_c指从时刻t₀到未来指定一段时间内的所有离散时刻数量，N_d指所有障碍物数量，

指在时刻i下障碍物j的位置，

指在时刻i下障碍物j的椭球形状参数；

在所述的步骤S5中，对于每个时刻i，无人机在该时刻会有一个控制点Q_i，在这个时刻，每个障碍物j同样预测得到障碍物的位置和形状，位置计算得到p_ij，形状计算得到

计算障碍物与无人机之间的大致距离，通过椭球的方程和梯度的方法来进行优化；整个约束项J_d将作为一个软约束项，加入到局部路径的轨迹规划函数中，参与轨迹优化问题的求解。

9.一种电子设备，包括：存储器、处理器以及存储在存储器上并可在处理器上运行计算机程序，其特征在于，所述的处理器执行所述计算机程序，实现权利要求1至8任一项所述的无人机在复杂动态环境下的避障方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述的计算机程序被处理器执行时，实现权利要求1至8任一项所述的无人机在复杂动态环境下的避障方法。

Images