CN114526735B - 一种无人飞行器集群仅测距初始相对位姿确定方法 - Google Patents

Abstract

本发明提供一种无人飞行器集群仅测距初始相对位姿确定方法，包括步骤1，基于飞行器的飞行任务信息，将测量信息投影在当地水平坐标系下以建立等效测量模型，得到二维加速度和距离信息；步骤2，基于二维加速度积分的位置信息和二维距离信息建立水平坐标系下飞行器间相对位置及航向角的线性求解模型；步骤3，通过至少5次测距以求解所述线性求解模型，当测距次数等于5时，通过5次测距通信求解算法求解线性求解模型；步骤4，当测距次数大于5时，通过递推最小二乘算法求解线性求解模型，实时输出递推结果，实现飞行器仅测距初始相对位姿求解。本发明仅依靠机载惯性单元、气压高度计与数据链TOA测距组合就能实现集群无人飞行器相对导航任务。

Description

一种无人飞行器集群仅测距初始相对位姿确定方法

技术领域

本发明属于集群无人飞行器自主相对导航技术领域，具体涉及一种无人飞行器集群仅测距初始相对位姿确定方法。

背景技术

集群无人飞行器在区域协同侦察，森林火灾探测及危险环境中执行搜救等诸多任务中有广泛应用前景。相比单个高集成度无人飞行器，集群无人飞行器具有雷达截面小，成本低，数量多，容错率高等特点，能够通过群体效应实现单个大型飞行器难以完成的任务。而实现集群成员间的高精度相对定位是保障集群任务与安全的关键前提。

传统的集群飞行器常使用独立于集群外的公共参考坐标的外部定位系统实现成员间相对定位，如运动捕捉系统（MCS）,全球导航卫星系统，但MCS系统需要预先布置于环境中，无法应用于未知环境。GNSS导航信号在复杂环境中存在衰减、多路径效应、无信号、甚至信号欺骗等问题。因此，GNSS拒止环境下无人飞行器集群的自主相对导航已经成为了行业内的热点问题。

目前集群无人飞行器的自主相对导航主要分为：基于视觉传感器的相对导航方式及基于机载无线电测距的相对导航方式。基于视觉传感器的相对导航方式如基于无人飞行器的仅测角机载红外传感系统，利用红外传感相机获得相邻飞行器的方向角。由于红外传感器视野范围有限，需要在不同方向布置机载传感器阵列以实现目标飞行器始终处于测量视野范围内。也有基于无人飞行器的仅测角视觉相机相对定位系统，类似于红外传感系统，同样存在视场(FOV)有限的问题，需要测量飞行器对目标飞行器进行追踪机动，以保证目标处于观测视野内，且当目标飞行器距离较远时，视觉相机存在特征识别点模糊等问题，一定程度上限制了无人飞行器间的相对飞行距离。

机载无线电设备能够实现全方位通信测距，且相邻设备间的通信几乎不受空间环境的影响，近年来受到了领域内研究人员的广泛关注。此类机载无线电设备包括：超宽带（UWB），蓝牙传感器等。集群成员利用机载蓝牙传感器向相邻成员交换彼此的地理北及速度信息，实现集群间的相对定位。此外，也有利用蓝牙传感器通信测距，并使用光流法测速辅助的相对定位方法，因光流法测速只适用于低速，光线良好的环境，因此对无人飞行器的飞行速度及环境均有一定限制。由于蓝牙传感器传输带宽较小，传输距离较近，因此对无人飞行器间远距离飞行环境下的测距通信带来一定限制。由于超宽带测距通信系统具有传输距离远，通信带宽大，信号传输稳定等优点，因此在集群无人飞行器的相对导航研究中得到了广泛应用。通过在环境中布置多个超宽带固定锚点可以实现对无人飞行器的测距定位，但这种方式无法应用于未知的飞行环境。因此，也可以利用单架悬停的无人飞行器作为UWB通信测距锚点，进而实现集群成员间的相对定位。这种相对定位算法的初始化需要集群成员中的单架无人飞行器首先做悬停机动，同时集群中的其他成员向该悬停无人飞行器发送自身的运动信息及地理北信息才能启动。利用UWB测距与视觉即时定位与地图构建（V-SLAM）组合的相对定位方法无需设备充当静止锚点，仅通过最少6次通信测距，同时交换由视觉SLAM装置测量得到的每台设备相对于各自位置原点的位移，进而求解得到两台设备的4自由度相对位姿。V-SLAM设备同样只适用于低速环境，且对机载计算资源有较高要求。

因此，现有技术中，集群无人飞行器自主相对导航，需要利用多视觉传感器、具有锚点的无线电测距设备或无线电测距与光流法测速辅助的方法完成相对导航，以上相对导航方法增加了飞行器的额外负重，对飞行器的机动有较大限制，且对机载在线计算有较高要求。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种无人飞行器集群仅测距初始相对位姿确定方法，仅使用具有全向性通信功能的测距天线，结合气压高度计及机载惯性单元即可快速递推求解无人飞行器间的初始相对位姿，实现集群无人飞行器间的自主相对导航。

本发明的一种基人飞行器集群仅测距初始相对位姿确定方法，包括步骤如下：

步骤1，基于飞行器的飞行任务信息，将测量信息投影在二维平面上以建立等效测量模型，具体为：将飞行器的三维加速度和角速度、测距信息的投影至当地水平坐标系，得到二维加速度和距离，对二维加速度进行二次积分得到位置信息；

步骤2，基于步骤1得到的位置信息和距离建立水平坐标系下飞行器间相对位置及航向角的线性求解模型；

步骤3，通过至少5次测距以求解所述线性求解模型，当测距次数等于5时，通过5次测距通信求解算法对所述线性求解模型进行求解，得到飞行器仅测距初始相对位姿；

步骤4，当测距次数大于5时，通过递推最小二乘算法对所述线性求解模型进行求解，实时输出递推求解结果，实现飞行器仅测距初始相对位姿的求解。

作为优选，步骤1中建立飞行器三维加速度和角速度、测距信息的水平坐标系投影等效测量模型如下：

步骤1.1，将三维加速度和角速度投影到二维空间，得到加速度的二维投影等效模型：

其中，i表示标号为i的飞行器，S _i 为飞行器本体系下的加速度，a _i 为当地水平坐标系下的等效加速度，θ _i 为飞行器i的俯仰角、φ _i 为滚转角；

步骤1.2，对二维加速度进行二次积分，得到位置信息；

步骤1.3，将测距信息投影到二维空间，得到距离信息的二维等效模型：

其中，D _i 为两架飞行器间的空间距离，△h _i 为两架飞行器间的高度差，d _i 为飞行器在二维平面下的相对距离。

作为优选，步骤2中构造的线性求解模型的方法为：以两个飞行器为一组，分别在各自的初始水平坐标系（里程系）下得到各自位置信息，转换位置信息至同一里程系下，并利用二维测距信息构建相对距离关系等式，得到飞行器间的相对位置及相对航向角的线性求解模型。

作为优选，线性求解模型具体为：

，其中，M为关于两个飞行器各自位置信息的关系矩阵，未知量x为待求解的仅测距初始相对位姿，B为关于两个飞行器的位置信息及测距信息的位置矩阵；未知量x包含飞行器间相对位置（u,v）及相对航向角△ψ，表示如下：

，L ₁ ,L ₂ ,L ₃ 为关于飞行器相对位置和相对航向角的算子。

作为优选，线性求解模型中的M及B矩阵表示如下：

其中，（A，B）、（x，y）分别表示飞行器在各自二维里程系下的位置，下标1,2，…n表示对应的测距次数；

其中，

，d_i为无人飞行器在二维平面下的相对距离，下标i表示第i次测距。

作为优选，未知量x中的L ₁ ,L ₂ ,L ₃ 算子表示如下：

其中，u,v为相对位置，△ψ为相对航向角。

作为优选，步骤3中测距通信求解算法具体为：

，其中，

为当测距次数n为5时，矩阵H=[M _5×7 –B _5×1 ]的极大线性无关组，λ ₁ ，λ ₂ 为待求解系数，利用关于飞行器相对位置和相对航向角的算子L ₁ ,L ₂ ,L ₃ 方程式以及

各元素线性组合导出的已知量构造线性方程组求解得出；得到λ ₁ ，λ ₂ 后带入

即可求解未知量x。

作为优选，步骤4中递推最小二乘算法为通过系统当前时刻运行产生的数据实时估计待求解未知量的方法，具体递推形式如下：

，其中，K _n 为新息增益,

为待求解量，P _n 为x的协方差矩阵，m _n ,b _n 分别为矩阵M _n×7 及列向量B _n×1 的行分块矩阵，具体表示如下：

。

作为优选，步骤4中系统初值生成方法为：当测距次数为6~10时，调用步骤3中5次测距算法，将前5次测距通信数据用于初值生成，其余测距通信数据用于递推求解，当测距次数大于10时，采用如下式的方法将前10次测距通信数据用于初值生成：

，其中P _n-1 为x的协方差矩阵初值，

为待求解量的初值。

本发明的有益效果：

本发明通过将集群无人飞行器三维运动解耦为二维水平运动，在二维平面下，以待求量相对位置和航向角的非线性形式构造新的待求状态量，将待求解量相对位置和航向角的非线性求解问题转化成了新状态量的线性模型最小二乘求解问题；利用新状态量元素分量间的约束关系，建立待求解量的最少5次测距通信求解算法；引入递推最小二乘算法，对待求解量进行递推求解，实现无人飞行器相对位置及航向角的实时递推求解。无需GNSS导航信号等集群外部定位系统，不需要使用机载视觉测量设备，仅使用具有全向性通信功能的测距天线，结合气压高度计及机载惯性单元即可快速递推求解无人飞行器间的初始相对位姿，实现集群无人飞行器间的自主相对导航。与现有技术相比，本发明有效降低了集群飞行器初始相对位姿确定的硬件代价和计算负载，极大减少了初始位姿确定的计算时间，提升了拒止环境下的导航系统快速响应能力。

附图说明

图 1 是本发明方法中无人飞行器测距示意图；

图 2 是本发明一个实施例中两飞行器的仿真飞行轨迹示意图；

图 3 是本发明一个实施例中飞行器A对飞行器B的相对位置估计误差曲线图；

图 4 是本发明一个实施例中飞行器A对飞行器B的相对距离估计误差曲线图；

图 5 是本发明一个实施例中飞行器A对飞行器B的相对航向角估计误差曲线图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

本发明提出一种基于集群无人飞行器的仅测距初始相对位姿确定方法，针对目前通过无线电测距的测量方式实现无人飞行器集群相对导航方法中，需要多个无线电测距锚点、利用相机光流法测速辅助或无线电测距与视觉SLAM装置组合求解集群成员间的相对位置的过程中成员间相对距离有限，只适用于低速光照充足环境及机载计算资源要求较高等问题；本发明的方法通过建立三维加速度及角速度、测距信息的二维等效模型，构造二维平面下待求解量相对位置及航向角的线性最小二乘求解模型，并引入递推最小二乘算法实现无人飞行器间相对位置及航向角的实时递推求解,无需GNSS导航信号等集群外部定位系统，不需要使用机载视觉测量设备，仅使用具有全向性通信功能的测距天线，结合气压高度计及机载惯性单元即可快速递推求解无人飞行器间的初始相对位姿，实现集群无人飞行器间的自主相对导航。

本发明以集群无人飞行器飞行任务为背景，以二维平面下等效后的飞行器的加速度和角速度进行飞行轨迹生成，以机载数据链TOA模式测距的方式进行相对距离测量，通过递推最小二乘算法实时求解出无人飞行器间的相对位置及姿态，从而实现集群无人飞行器间的自主相对导航。

具体说明如下：

步骤1，建立飞行器三维加速度和角速度、测距信息的水平坐标系投影等效模型如下：

步骤1.1，将三维加速度和角速度投影到二维空间，得到加速度的二维投影等效模型：

（1）

其中， i表示标号为i的飞行器，S _i 为飞行器本体系下的加速度，a _i 为当地水平坐标系下的等效加速度，θ _i 为飞行器i的俯仰角、φ _i 为滚转角。

步骤1.2，对二维加速度进行二次积分，得到位置信息。

步骤1.3，将测距信息投影到二维空间，得到距离信息的二维等效模型：

（2）

其中，D _i 为两架飞行器间的空间距离，△h _i 为两架飞行器间的高度差，d _i 为飞行器在二维平面下的相对距离。

步骤2，基于步骤1得到的位置信息和距离建立水平坐标系下飞行器间相对位置及航向角的线性求解模型。以两个飞行器为一组，分别在各自的初始水平坐标系（二维里程系）下得到各自位置信息，转换位置信息至同一里程系下，并利用二维测距信息构建相对距离的关系等式，得到飞行器间的相对位置及相对航向角的线性求解模型。

建立水平坐标系下飞行器间待求解量相对位置和航向角的线性求解模型如下：

（3）

式中，M为关于两个飞行器各自位置信息的关系矩阵，未知量x为待求解的仅测距初始相对位姿，B为关于两个飞行器的位置信息及测距信息的位置矩阵。

未知量x包含飞行器间相对位置（u,v）及相对航向角△ψ，表示如下：

（4）

其中，L₁,L₂,L₃为关于飞行器相对位置和相对航向角的算子，表示如下：

（5）

M及B矩阵表示如下：

（6）

（7）

（8）

如图1至图2所示，分别为无人飞行器测距示意图及两飞行器的仿真飞行轨迹示意图。其中，{h ₁ }为飞行器B的里程坐标系，即飞行器B在t ₀ 时刻本体系在水平面上的投影为{h ₁ }， ^h1 p _i ^h1 =(A _i ,B _i )表示飞行器A在其里程坐标系{h ₁ }下测量的第i个点的位置，其中，A _i ,B _i 通过对飞行器A二维投影下的加速度进行时间积分得到。

{h ₂ }为飞行器A的里程坐标系，即飞行器A在t ₀ 时刻本体系在水平面上的投影为{h ₂ }， ^h2 p _i ^h2 =(x _i ,y _i )表示飞行器A在其里程坐标系{h ₂ }下测量的第i个点的位置，其中，x _i ,y _i 通过对飞行器B二维投影下的加速度进行时间积分得到。

则矩阵M _n×7 的构建方法如下：

两无人机测距时满足如下等式关系：

（9）

其中，p _i ^h1 即为 ^h1 p _i ^h1 表示飞行器B在其里程坐标系{h ₁ }下测量的第i个点的位置， ^h1 p _i ^h2 =(X _i ,Y _i )表示飞行器A在其里程坐标系{h ₂ }下测量的第i个点的位置，并经过坐标转换表示在{h ₁ }坐标系下。

其中，坐标转换为将{h ₂ }投影到{h ₁ }中，以统一坐标系进行相减运算：X _i ,Y _i 满足如下表达式：

（10）

将式(10)带入式(9)中，化简可得：

（11）

其中，b _i 如式(8)所示，且当有多次测距信息时，式(11)可以写成矩阵方程(3)关于未知量x的线性形式。

步骤3，以构造的线性求解模型为基础，通过至少5次测距以求解所述线性求解模型。当测距次数等于5时，通过5次测距通信求解算法对线性求解模型进行求解。

5次测距通信求解算法如下：

（12）

其中，为H=[M _5×7 –B _5×1 ]矩阵的极大线性无关组，λ ₁ ，λ ₂ 为待求解系数。

由式(5)

可得下式(13)

（13）

基于式(5)、式(13)建立的未知量x中分量间的关系，构造的如下线性方程组求解得出λ ₁ ，λ ₂ ：

（14）

其中，方程组中的元素α _ij 及β _i 为极大线性无关组

中各元素线性组合导出的已知量。求解该矩阵方程，即可解出λ ₁ ，λ ₂ ，将λ ₁ ，λ ₂ 带入式(12)

，即可求解未知量x。

步骤4，当测距次数大于5时，引入递推最小二乘算法，系统当前时刻运行产生的数据实时输出递推结果。构造式(3)给出的线性求解模型的递推形式如下：

（15）

其中，K _n 为新息增益，

为待求解量，P _n 为x的协方差矩阵，m _n ,b _n 分别为矩阵M _n×7 及列向量B _n×1 的行分块矩阵，具体表示如下：

（16）

系统初值生成方法为：当测距次数为6~10时，调用步骤3中5次测距算法，将前5次测距信息用于初值生成，其余测距数据用于递推求解，当测距次数大于10时，采用如下式的方法将前10次测距信息用于初值生成：

（17）

本发明方法的实例：结合图3到图5说明本发明的实例验证，设定如下计算条件和技术参数：

1）无人机A在水平惯性坐标系下的初始位置及速度为：

[0m,100m；4.3m/s,2.5m/s]；

2）无人机B在水平惯性坐标系下的初始位置及速度为：

[0m,0m；4m/s,0m/s]；

3）飞行器的机载惯性单元及机载数据链的随机误差模型均为零均值高斯噪声，对应的噪声幅值见表1如下：

基于本发明的相对导航方法与上述设置的计算条件和技术参数，采用Matlab软件进行仿真验证，仿真时间为60s。如图3到5所示分别是集群成员之间相对位置和航向角估计误差曲线，前20秒内生成初值，在此期间内没有求解相对位置，基于递推最小二乘算法，在21-60秒进行递推计算。由图中曲线可知，x、y轴的相对位置误差、距离误差及航向角估计误差均能收敛，且位置误差在初始相对距离的10%以内，航向角误差在初始相对角度的1%以内。

因此，采用本发明方法，仅依靠机载惯性单元、气压高度计与数据链TOA测距组合就能实现集群无人飞行器相对导航任务。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (9)

1.一种无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，包括

步骤如下：

步骤1，基于飞行器的飞行任务信息，将测量信息投影在二维平面上以建立等效测量模型，具体为：将飞行器的三维加速度和角速度、测距信息投影至当地水平坐标系，得到二维加速度和距离，对二维加速度进行二次积分得到位置信息；

步骤2，基于步骤1得到的位置信息和距离建立当地水平坐标系下飞行器间相对位置及航向角的线性求解模型，具体为

M_n×7x_7×1＝B_n×1

其中，M为关于两个飞行器各自位置信息的关系矩阵，未知量x为待求解的仅测距初始相对位姿，B为关于两个飞行器的位置信息及测距信息的位置矩阵，下标n为两个飞行器间的测距通信次数；

步骤3，通过至少5次测距通信以求解所述线性求解模型，当测距次数等于5时，通过建立对应的5次测距通信求解算法对所述线性求解模型进行求解，得到飞行器仅测距初始相对位姿；

步骤4，当测距次数大于5时，通过递推最小二乘算法对所述线性求解模型进行求解，实时输出递推求解结果，实现飞行器仅测距初始相对位姿求解。

2.根据权利要求1所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，步骤1中建立飞行器三维加速度和角速度、测距信息的水平坐标系投影等效测量模型如下：

步骤1.1，将三维加速度和角速度投影到二维空间，得到加速度的二维投影等效模型：

其中，i表示标号为i的飞行器，S_i为飞行器本体系下的加速度，a_i为当地水平坐标系下的等效加速度，θ_i为飞行器i的俯仰角、φ_i为滚转角；

步骤1.2，对二维加速度进行二次积分，得到位置信息；

步骤1.3，将测距信息投影到二维空间，得到距离信息的二维等效模型：

其中，D_i为两架飞行器间的空间距离，△h_i为两架飞行器间的高度差，d_i为飞行器在二维平面下的相对距离。

3.根据权利要求1或2中所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，步骤2中构造的线性求解模型的方法为：以两个飞行器为一组，分别在各自的初始水平坐标系即二维里程系下得到各自位置信息，转换位置信息至同一里程系下，并利用二维测距信息构建相对距离的关系等式，得到飞行器间的相对位置及相对航向角的线性求解模型。

4.根据权利要求3中所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，所述线性求解模型中未知量x包含飞行器间相对位置(u,v)及相对航向角△ψ，表示如下：x＝[u v cosΔψ sinΔψ L₁ L₂ L₃]^T，其中，L₁,L₂,L₃为关于飞行器相对位置和相对航向角的算子。

5.根据权利要求4中所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，所述线性求解模型中的M及B矩阵表示如下：

其中，(A，B)、(x，y)分别表示飞行器在各自二维里程系下的位置，下标1,2，…n表示对应的测距次数；

B_n×1＝[b₁ b₂...b_n]^T

其中，

d_i为无人飞行器在二维平面下的相对距离，下标i表示第i次测距。

6.根据权利要求5中所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，未知量x中的L₁,L₂,L₃算子表示如下：

其中，u,v为相对位置，△ψ为相对航向角。

7.根据权利要求6所述的无人飞行器集群仅测距初始相对位姿确定方法，其 特征在于，所述步骤3中测距通信求解算法具体为：

其中，

为当测距次数n为5时，矩阵H＝[M_5×7 –B_5×1]的极大线性无关组，λ₁，λ₂为待求解系数，利用关于飞行器相对位置和相对航向角的算子L₁,L₂,L₃方程式以及

各元素线性组合导出的已知量构造线性方程组求解得出；得到λ₁，λ₂后带入

即可求解未知量x。

8.根据权利要求1或7所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，步骤4中递推最小二乘算法为通过系统当前时刻运行产生的数据实时估计待求解未知量的方法，具体递推形式如下：

其中，K_n为新息增益,

为待求解量，P_n为x的协方差矩阵，m_n,b_n分别为矩阵M_n×7及列向量B_n×1的行分块矩阵，具体表示如下：

。

9.根据权利要求8所述的无人飞行器集群仅测距初始相对位姿确定方法，其特征在于，步骤4中系统初值生成方法为：当测距次数为6～10时，调用步骤3中所述5次测距通信求解算法，将前5次测距通信数据用于初值生成，其余测距数据用于递推求解，当测距次数大于10时，采用如下式的方法将前10次测距通信数据用于初值生成：

其中P_n-1为x的协方差矩阵初值，

为待求解量的初值。

Images