CN114384932B - 一种基于距离测量的无人机导航对接方法 - Google Patents

Abstract

本发明涉及一种基于距离测量的无人机导航对接方法，包括：获取无人机与MP之间的距离信息，以及MP位移量；在悬停阶段分别计算得到MP速度、无人机与MP之间的相对位置；根据当前无人机与MP之间的距离信息、MP位移量、MP速度、无人机与MP之间的相对位置，在接近阶段确定出相对位置估计值，并输出相应控制指令给无人机；在接近阶段，实时判断当前无人机与MP之间的距离信息是否小于设定距离阈值，若判断为是，则进入降落阶段，基于视觉降落法则进行无人机的降落控制；否则继续在接近阶段更新相对位置估计值并相应控制。与现有技术相比，本发明能够在无GPS的环境下，准确可靠地实现无人机自主对接，具有部署成本低、应用范围广、节约计算资源等优势。

Description

一种基于距离测量的无人机导航对接方法

技术领域

本发明涉及无人机对接技术领域，尤其是涉及一种基于距离测量的无人机导航对接方法。

背景技术

近年来，无人机(UAV，Unmanned Aerial Vehicle，无人驾驶飞机)的自主对接一直是一个热门的研究领域，一般来说，它包括两个阶段：接近阶段和着陆阶段。首先，无人机需要对目标进行相对定位，其中，目标可以是一个固定的着陆点，比如有人或无人的地面车辆，或海上的船只。其次，当相对位置被确定后，则需要根据导航规则进行导航，当目标进入无人机的视野时，无人机将降落在设定的目标区域。传统上，由于机载视觉系统可以提供丰富而准确的位置信息，在导航定位中得到了广泛的应用，目前使用主流的可视化方法，可以相对成熟地解决着陆问题，因而当前大部分对接工作是在接近阶段进行的。

在很多情况下，当设置有GPS时，只需在无人机和目标之间传输GPS数据就可以实现接近能力，例如，在多通道环境下，位置和姿态估计的准确性可以通过RTK-GNSS技术来完成。最近的研究表明，通过整合GPS和视觉数据，无人机以50公里/小时的速度降落在车辆上，或者固定翼无人机可以与空中目标成功对接。

现有研究中，Padhy等人开发了一个在有GPS限制的走廊环境中无人机自主导航和避免碰撞的模型；Goforth等人提出了一种无人机的定位方法，通过将朝下的单眼RGB相机拍摄的图像与飞行位置的现有卫星图像对齐，取代了有噪声或不可靠的GPS信号的空中GPS系统；Nguyen等人则提出了一个集成的相对位置估计和控制方案，以同时实现两个目标：只使用距离和里程表测量值进行相对定位，并通过设计适当的有界控制律将无人机导航到所需位置，通过大量的室外飞行试验，表明无人机可以从50m以外接近移动的无人地面车辆(UGV)，然后准确地降落在1.5m*2m的大平台上。

虽然上述方法在很多情况下可以完成无人机自主降落的任务，但是，上述自主对接方案中，UAV需要依赖GPS位置信息或车辆里程表位置信息，很大程度上取决于是否有GPS的覆盖，而且精度也取决于信号的强度。事实上，GPS在很多地方都无法使用，如室内、峡谷、森林、洞穴或深井，因此，在无GPS以及无车辆里程表的环境下，采用上述方法均难以保证无人机导航对接的准确度。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于距离测量的无人机导航对接方法，以能够在无GPS的环境下，准确地实现无人机自主对接。

本发明的目的可以通过以下技术方案来实现：一种基于距离测量的无人机导航对接方法，包括以下步骤：

S1、获取无人机与MP(Mobile Platform，移动平台)之间的距离信息，以及MP的位移量；

S2、根据步骤S1获取的距离信息以及位移量，在悬停阶段分别计算得到MP的速度、无人机与MP之间的相对位置；

S3、根据当前无人机与MP之间的距离信息，以及当前MP的位移量，结合步骤S2得到的MP的速度、无人机与MP之间的相对位置，在接近阶段确定出相对位置估计值，并输出相应控制指令给无人机；

S4、在接近阶段，实时判断当前无人机与MP之间的距离信息是否小于设定距离阈值，若判断为是，则执行步骤S5，否则返回执行步骤S3；

S5、进入降落阶段，基于视觉降落法则，输出相应控制指令给无人机。

进一步地，所述无人机与MP之间的距离信息具体是通过UWB传感器采集获取，所述UWB传感器分别安装在无人机和MP上；

所述MP的位移量具体是通过视觉里程计采集获取，所述视觉里程计安装在无人机上。

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

S21、根据步骤S1获取的无人机与MP之间的距离信息，确定出MP的速度，所述MP的速度包括速度数值以及速度方向角；

S22、根据MP的速度数值以及MP的位移量，确定出无人机与MP之间的相对位置。

进一步地，所述MP的速度数值具体为：

其中，|v|为MP的速度数值，d₀、d₁、d₂分别为相邻三个单位周期时刻获取的无人机与MP之间的距离信息，T为设定的单位周期。

进一步地，所述MP的速度方向角具体为MP的速度与二维平面坐标系y轴之间的夹角α：

α＝π-∠OO′p_M(1)-∠p_M(0)p_M(1)O′

其中，O为无人机的悬停位置，O′为无人机的局部移动位置，p_M(0)为MP在初始时刻的位置，p_M(1)为MP在第一个单位周期时刻的位置。

进一步地，所述步骤S22中无人机与MP之间的相对位置具体为：

p_U(k+1)＝p_U(k)+φ(k)＝p_U(k)+Tu_U(k)

p_M(k+1)＝p_M(k)+Tv

其中，q(k)为k时刻无人机与MP之间的相对位置，p_U(k)为k时刻无人机的位置，p_M(k)为k时刻MP的位置，φ(k)为k时刻MP的位移量，u_U(k)为k时刻的无人机控制信息，v为MP的速度。

进一步地，所述步骤S3中具体是通过自适应估计器，以在接近阶段确定出相对位置估计值；

所述自适应估计器输出相对位置估计值给飞机控制器，由飞机控制器输出对应的控制指令给无人机。

进一步地，所述相对位置估计值具体为：

或/>

其中，为相对位置q(k)的估计值，P(-1)＝P_-1，/>P(k)是n×n正定矩阵，K(k)为引入的记号函数，λ∈(0,1)为遗忘因子，ζ(k)为估计系统参数，/>为辅助参数。

进一步地，所述飞机控制器具体为饱和有界反比例速度控制器。

进一步地，所述飞机控制器根据自适应估计器输出的相对位置估计值，输出对应的控制指令具体为：

其中，u_U(k)为k时刻的无人机控制信息，U_max为无人机允许的最大速度，G＞0为比例增益，为(k-1)时刻的相对位置估计值。

与现有技术相比，本发明针对无GPS环境，提出一种无人机与MP之间的导航对接方法，通过获取无人机与MP之间的距离信息、以及MP的位移量，分别在在悬停阶段确定出MP的速度和相对位置；在接近阶段基于遗忘最小二乘法的估计控制法、以进行相对位置估计、进而相应控制无人机；在降落阶段才用基于视觉的方法进行降落控制。由此能够在无GPS的环境下，准确地实现无人机自主对接。

本发明基于UWB传感器和视觉里程计分别采集获取无人机与MP之间的距离信息、MP的位移量，解决了现有方法依赖GPS位置信息、车辆里程计、视觉搜索的问题，同时还能有效节约部署维护成本和计算量。

本发明在接近阶段设计自适应估计器，以通过遗忘最小二乘迭代的方法进行相对位置估计的不断更新，从而满足真实环境下无人机导航对接方法需求；此外还设计饱和有界反比例速度控制器，以对无人机进行导航，自适应估计器与饱和有界反比例速度控制器相结合，能够实时保证无人机导航对接的准确性。

附图说明

图1为本发明的方法流程示意图；

图2为实施例的应用过程示意图；

图3为无人机与MP的相对位置示意图；

图4为悬停阶段的速度计算示意图；

图5为悬停阶段的速度方向计算示意图；

图6为实施例中EKF估计的速度结果(含传感器测量噪声)；

图7为实施例中误差比较示意图(含传感器测量噪声)；

图8为实施例中EKF估计的速度结果(含传感器测量噪声和MP速度噪声)；

图9为实施例中误差比较(含传感器测量噪声和MP速度噪声)；

图10为实施例中空间运动轨迹的位置示意图(含传感器测量噪声)；

图11为实施例中相对位置估计分析图(含传感器测量噪声)；

图12为实施例中无人机和MP空间位置示意图(含传感器测量噪声)；

图13为实施例中UWB测量数据(含传感器测量噪声)；

图14为实施例中空间运动轨迹的位置示意图(含传感器测量噪声和MP速度噪声)；

图15为实施例中相对位置估计分析图(含传感器测量噪声和MP速度噪声)；

图16为实施例中无人机和MP空间位置示意图(含传感器测量噪声和MP速度噪声)；

图17为实施例中UWB测量数据(含传感器测量噪声和MP速度噪声)。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种基于距离测量的无人机导航对接方法，包括以下步骤：

S1、获取无人机与MP之间的距离信息，以及MP的位移量，其中，无人机与MP之间的距离信息具体是通过UWB传感器采集获取，UWB传感器分别安装在无人机和MP上；

MP的位移量具体是通过视觉里程计采集获取，视觉里程计安装在无人机上；

S2、根据步骤S1获取的距离信息以及位移量，在悬停阶段分别计算得到MP的速度、无人机与MP之间的相对位置；

S3、根据当前无人机与MP之间的距离信息，以及当前MP的位移量，结合步骤S2得到的MP的速度、无人机与MP之间的相对位置，在接近阶段确定出相对位置估计值，并输出相应控制指令给无人机；

S4、在接近阶段，实时判断当前无人机与MP之间的距离信息是否小于设定距离阈值，若判断为是，则执行步骤S5，否则返回执行步骤S3；

S5、进入降落阶段，基于视觉降落法则，输出相应控制指令给无人机。

本实施例应用上述技术方案，如图2所示，包含三个阶段：悬停、接近和着陆。主要从三方面展开应用：在悬停阶段，基于几何工具和扩展卡尔曼滤波(EKF)进行速度和相对位置计算；在接近阶段，基于遗忘最小二乘法进行相对位置的估计控制，并建立了收敛性；在降落阶段，选用基于视觉的方法进行降落。

本技术方案中，由于有一个低级别的控制器在跟踪控制中考虑到了无人机的动态，在任务计划层面上，假设有一个单一的积分器模型：

p_U(k+1)＝p_U(k)+Tu_U(k) (1)

其中，是无人机在时间瞬间的位置，k≥0，T代表取样期，即设定的单位周期，u_U(k)是UAV的控制输入。同样地，考虑MP的动态模型为：

p_M(k+1)＝p_M(k)+Tv (2)

其中，是时间瞬间k≥0的MP位置，/>是MP的速度、为一个常数，位移量为φ(k)＝Tu_U(k)，也就是说，MP被认为以一个时间不变的速度运动。一般来说，假设MP在平地上运动，所以MP的三维参数的高度为0，不发生变化。

为了让MP进入无人机的相机视野内，两者的位置需要满足以下条件：

其中，是一个常数，它取决于无人机的高度和相机最大视觉距离，即为设定的距离阈值。

如图3所示，定义无人机与MP的相对位置为的相对位置定义如下：

在这个设置中，一个UWB传感器被安装在MP上，而另一个UWB传感器被安装在无人机上，应该注意的是无人机节点在同一时间只能完成一个距离测量。

在悬停阶段和接近阶段，主要包括以下内容：

a)基于UWB的MP的速度计算

在悬停阶段，通过悬停无人机测量移动平台的运动，然后设计基于凯莱-门格行列式的速度估计器。如图4所示，其中，O是无人机的悬停位置，p_M(k)是MP在时间k的位置，d_i是UWB测距传感器测得的无人机和MP之间各个时刻的距离信息，θ_k是MP和无人机在时刻k和时刻k-1的线的包含角，s是MP在每个采样时间间隔的移动距离。明显地，MP的速度v不能只通过一个距离测量来确定，需要通过三次距离测量得到。

MP的单位周期位移量s＝T||v||，定义x＝s²，根据凯莱-门格行列式，可以得到描述x、d₀、d₁和d₂之间几何关系的代数方程，如下所示：

其中，det为行列式计算符号，那么这个方程有三个解：

x₁＝0

由于x必须是正数，那么因此可以得到一个唯一的MP速度的解：

b)基于UWB测量数据的MP速度方向的计算

为了确定MP的运动方向，无人机不能保持静止状态，必须进行一些局部运动。在此，本技术方案提出一种可能的机动策略，在时间0、T和2T进行距离测量，得到d₀、d₁和d₂。

如图5所示，无人机在0时刻以给定的MP恒定速度||开始线性运动，从O点移动到O′，在T时测量d₂，然后返回初始位置，在2T时进行计划的测量d₃。并且无人机从O点到O′点保持高度不变，而MP的高度是已知的。由此，MP的速度角可以转换为二维平面计算。

|v|已经由(6)计算出来了，然后可以计算出||p_M(0)-p_M(1)||＝||p_M(1)-p_M(2)||＝||v||T。现在检查O、p_M(0)、p_M(1)和p_M(2)之间的距离，可以发现，只有p_M(1)和O的连线是未知的，所以，通过解决相应四点的凯莱-门格行列式的方程：

D(O,p_M(0),p_M(1),p_M(2))＝0 (7)

其中，D(O,p_M(0),p_M(1),p_M(2)是O、p_M(0)、p_M(1)和p_M(2)的凯莱-门格行列式行列式，然后可以得到为了找到速度v的方向，需要解决角度α的值，如图5所示。在一个三角形中，从几何关系上可以得出MP速度的与y轴的夹角α：

α＝π-∠OO′p_M(1)-∠p_M(0)p_M(1)O′ (8)

其中，∠OO′p_M(1)和∠p_M(0)p_M(1)O′可以通过余弦定律在ΔOO′p_M(1)和Δp_M(0)P_M(1)O′中计算的得到。

虽然获得了α的值，但相对于MP运动的线性轨迹而言，v的方向仍有翻转的模糊性，由此通过设计一个三角形来消除这种模糊性。最终，MP速度的大小和方向可以通过(6)和(8)得到。

c)相对定位的自适应估计器迭代计算

根据图2，在接近阶段设计一个饱和有界反比例速度控制器u_U(k)为：

其中，U_max为无人机允许的最大速度，G＞0为比例增益，表示相对位置q(k)的估计。从公式(9)中可以看出，控制器可以通过比例增益和相对位置的估计值来计算。

由图3可知，能够验证q(k)与测量值d(k)、T、u_U(k)和c之间的关系如下：

其中，通过公式(10)和(11)可以得到以下方程：

定义估计系统ζ(k)和辅助参数为：

而根据公式(12)，可以得出：

对于公式(15)，为了增强新数据提供的信息量，并逐渐削弱旧数据以防止数据饱和，最小成本函数J(k)为：

其中，λ∈(0,1)为遗忘因子。使用构建的估计系统(15)，基于时变参数的遗忘因子最小二乘法，可以表示为：

或

其中，为相对位置q(k)的估计值，P(-1)＝P_-1，/>P(k)是n×n正定矩阵，K(k)为引入的记号函数。

d)收敛性证明

假设1.当k≠j时，UAV和MP的相对位置变化率与ω(j)不相关。同时，变化率应该是平方约束的，即||ω(k)||＜σ＜∞，σ为某个常数值。

定理1.对于控制器(9)的系统(1)和(2)，在假设1下，如果存在α＞0和β＞0这样的满足如下的持续激励条件，

对于任意的k＞0，其中N≥3，0＜α≤β＜∞，则是有界的如下：

证明：

定义和q(k)的差值为：

根据公式(16)～(19)，可得

定义两个简易参数为：

从P^-1(k)的定义，可以得出以下结论

根据上述公式，利用持续激励条件，可以得出

进而可以得到如下

同理，可以得到

所以有

以及

通过使用公式2x^Ty≤x^Tx+y^Ty，可以得到

通过对f(λ)求导，令f′(λ)＝0，得到结合上述方程可得出

因此，定理1的结论可以通过使用(26)和(29)得到。

以上验证了本技术方案在接近阶段采用自适应估计器进行相对位置估计的收敛性。

本实施例中，在悬停阶段：设定移动平台(MP)的速度为5m/s，从初始坐标(25，5)处开始运动，无人机的初始坐标在(0，0)处悬停进行测距，采样周期为T＝0.1s。采集50个测距样点，用UWB传感器测量UAV和MP之间的距离d(k)，其测量误差噪声为(-0.1，0.1)范围内的随机噪声。速度的估计值和误差对比分析如图6和图7所示，其中观察值由公式计算，由一系列距离测量得到，EKF-v值是由EKF计算得到。

根据图6的数据可知，EKF更新迭代估计的速度大小与实际速度接近，而图7所示则表明本技术方案提出的方法误差小、精度好，能够很好地完成无人机在悬停阶段的MP速度的计算。

然后，考虑在保持匀速直线运动的MP速度中加入±0.1m/s的随机噪声误差。实验结果如图8和图9所示。悬停阶段的仿真实验表明，同时存在传感器噪声和移动平台速度噪声，EKF估计的速度值与移动平台的真实值相差0.5米以内，可以满足实际工程应用需求。

接近阶段：设计MP初始坐标为(25，5，0.2)，其中，0.2是MP甲板与地面的高度。已知前一阶段无人机的初始悬停位置设定为(0，0，4)，4是无人机离地面的高度。从悬停阶段得到MP的估计速度和方向，设定MP的实际速度为5m/s，无人机的最大速度为8m/s进行模拟，采样周期为T＝0.1s，遗忘因子为λ＝0.8，无人机控制系数为G＝0.85。假设无人机处于4m的高度，机载相机的广角投射到地面上，作为半径为5m的圆心(当测量距离d(k)≤5m时，认为MP在无人机的视野范围内)。在测距中，加入了(-0.1，0.1)随机测量噪声(本实施例选择的UWB传感器的精度在±10cm以内)。仿真实验的结果如图10、11、12、13所示。

从图10可以看出，使用本技术方案提出的控制法，无人机逐渐接近MP，直到MP出现在无人机视觉传感器可测范围内，并且无人机与MP之间的距离在预设值内(其中仿真设定的高度差为3.8m)，此外还可以使用视觉着陆。从图11中的数据可以看出，相对位置、相对位置估计和相对位置误差将趋于一致。

车辆和无人机的空间坐标如图12所示。可以看到，无人机总是以二维坐标接近MP，最后接近MP的顶部，这样无人机就可以进行着陆了。图13显示了UAV保持高度与MP二维坐标距离接近零，即MP进入UAV视野范围内。

为了进一步探索算法的抗干扰性能，本实施例在MP的速度中加入一个±0.1m/s的随机噪声。实验结果如图14、15、16、17所示。仿真实验表明，当UWB测量和MP速度都存在噪声时，使用本技术方案提出的方法仍能准确可靠地完成导航对接任务。

综上可知，本技术方案为无GPS环境下无人机与MP的导航对接提供了一种新的控制方法，能够满足探险、矿井、野外和室内等无GPS环境下低部署成本、低计算量和适应性强的需求，在适应场景上领先于现有依赖GPS信息、车辆里程计以及视觉搜索的无人机导航对接方法；

本技术方案基于UWB距离测量以进行移动平台速度的计算，通过测量采集距离信息，利用几何工具，并通过扩展卡尔曼滤波进行对噪声的处理，实现了对移动平台速度的准确估计，具有鲁棒性强、准确性高和耗时短的优点；

本技术方案基于UWB测量数据进行移动平台速度方向的计算，通过无人机机载视觉里程计计算的位移信息和UWB测量的距离信息，利用几何工具，计算出移动平台方向，并推导出相对位置，以满足现实中无人机与移动平台进行对接的先验条件，即相对位置的确定；

本技术方案还提出一种相对定位的自适应估计器迭代算法，设计一个饱和有界反比例速度控制器进行对无人机的导航，通过遗忘最小二乘迭代的方法，对历史数据进行相对位置估计的不断更新，从而满足真实环境下无人机导航对接方法需求。

Claims (5)

1.一种基于距离测量的无人机导航对接方法，其特征在于，包括以下步骤：

S1、获取无人机与MP之间的距离信息，以及MP的位移量；

S2、根据步骤S1获取的距离信息以及位移量，在悬停阶段分别计算得到MP的速度、无人机与MP之间的相对位置；

S3、根据当前无人机与MP之间的距离信息，以及当前MP的位移量，结合步骤S2得到的MP的速度、无人机与MP之间的相对位置，在接近阶段确定出相对位置估计值，并输出相应控制指令给无人机；

S4、在接近阶段，实时判断当前无人机与MP之间的距离信息是否小于设定距离阈值，若判断为是，则执行步骤S5，否则返回执行步骤S3；

S5、进入降落阶段，基于视觉降落法则，输出相应控制指令给无人机；

所述步骤S2具体包括以下步骤：

S21、根据步骤S1获取的无人机与MP之间的距离信息，确定出MP的速度，所述MP的速度包括速度数值以及速度方向角；

S22、根据MP的速度数值以及MP的位移量，确定出无人机与MP之间的相对位置；

所述MP的速度数值具体为：

其中，|v|为MP的速度数值，d₀、d₁、d₂分别为相邻三个单位周期时刻获取的无人机与MP之间的距离信息，T为设定的单位周期；

所述步骤S22中无人机与MP之间的相对位置具体为：

p_U(k+1)＝p_U(k)+φ(k)＝p_U(k)+Tu_U(k)

p_M(k+1)＝p_M(k)+Tv

其中，q(k)为k时刻无人机与MP之间的相对位置，p_U(k)为k时刻无人机的位置，p_M(k)为k时刻MP的位置，φ(k)为k时刻MP的位移量，u_U(k)为k时刻的无人机控制信息，v为MP的速度；

所述步骤S3中具体是通过自适应估计器，以在接近阶段确定出相对位置估计值；

所述自适应估计器输出相对位置估计值给飞机控制器，由飞机控制器输出对应的控制指令给无人机；

所述相对位置估计值具体为：

或/>

其中，为相对位置q(k)的估计值，P(-1)＝P_-1，/>P(k)是n×n正定矩阵，K(k)为引入的记号函数，λ∈(0,1)为遗忘因子，ζ(k)为估计系统参数，/>为辅助参数。

2.根据权利要求1所述的一种基于距离测量的无人机导航对接方法，其特征在于，所述无人机与MP之间的距离信息具体是通过UWB传感器采集获取，所述UWB传感器分别安装在无人机和MP上；

所述MP的位移量具体是通过视觉里程计采集获取，所述视觉里程计安装在无人机上。

3.根据权利要求1所述的一种基于距离测量的无人机导航对接方法，其特征在于，所述MP的速度方向角具体为MP的速度与二维平面坐标系y轴之间的夹角α：

α＝π-∠OO′p_M(1)-∠p_M(0)p_M(1)O′

其中，O为无人机的悬停位置，O′为无人机的局部移动位置，p_M(0)为MP在初始时刻的位置，p_M(1)为MP在第一个单位周期时刻的位置。

4.根据权利要求1所述的一种基于距离测量的无人机导航对接方法，其特征在于，所述飞机控制器具体为饱和有界反比例速度控制器。

5.根据权利要求4所述的一种基于距离测量的无人机导航对接方法，其特征在于，所述飞机控制器根据自适应估计器输出的相对位置估计值，输出对应的控制指令具体为：

其中，u_U(k)为k时刻的无人机控制信息，U_max为无人机允许的最大速度，G＞0为比例增益，为(k-1)时刻的相对位置估计值。