CN114063647B - 一种基于距离测量的多无人机互定位方法 - Google Patents

Abstract

本发明公开了一种基于距离测量的多无人机互定位方法，先利用集群中的通信链路对机间距离进行测量，再根据多智能体协同理论建立基于通信距离测量的多无人机互定位模型；其次，通过半定规划松弛算法和正交普氏分析算法得到目标无人机的初步解，再对信标无人机进行权重更新，然后利用极大似然估计法求解目标无人机的最终解，从而实现了多无人机互定位。

Description

一种基于距离测量的多无人机互定位方法

技术领域

本发明属于无人机技术领域，更为具体地讲，涉及一种基于距离测量的多无人机互定位方法。

背景技术

在现代战争中，无人机快速精确的导航和定位是提高武器系统命中精度及杀伤力的关键因素之一。在开阔无干扰的定位环境中，GPS能全天候地提供精确的位置、速度、时间信息，是当前最好的解决方案。然而当无人机集群处于卫星拒止区域中时，定位信号易受电磁干扰以及障碍物中金属结构的屏蔽，从而无人机接收到的信号质量大大衰减，经常会出现无法定位的问题。对于这种情况，无人机需要通过互定位技术确定彼此的位置，使无人机具有误差有限的定位能力，导航能力得到恢复。

互定位不是利用环境中已存在的地标进行工作，而是将无人机本身作为移动坐标，无人机以彼此为地标，进行信息共享，从而确定自身的位置。对于上述过程有不同的测量类型，如基于信号到达的方位测量，基于无人机之间的距离测量，基于信号衰减程度的场强测量。目前，在现有技术中一种基于方位测量的协同定位方法，是从信号到达角中提取信息，通过简单的几何结构，给出了一种新的基于最小二乘准则的位置估计。然而在卫星拒止区域中基于方位测量的方法无人机需要携带高功耗的传感器，有能源效率低下的风险。在许多场景中，基于无人机之间的距离测量由于其低功耗、低成本以及定位精度较高的特点，相比于其它测量类型应用更加广泛。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于距离测量的多无人机互定位方法，利用集群中的通信链路对机间距离进行测量，结合惯性导航系统和少数已定位无人机来定位进入卫星拒止区域无人机的全球坐标，使无人机集群整体的定位精度得到提高。

为实现上述发明目的，本发明一种基于距离测量的多无人机互定位方法，其特征在于，包括以下步骤：

(1)、建立多无人机互定位模型；

(1.1)、搭建多架信标无人机定位一架目标无人机的应用场景，其中，多架信标无人机用集合M表示，

m表示集合M信标无人机的总个数；目标无人机用i表示；

设置各测量时刻，记为t_k，k＝1,2,…,n，n为总的时刻测量数；

(1.2)、在测量时刻t_k测得集合M中各信标无人机的全球坐标，记为p_M[k]，其中，第j个信标无人机的全球坐标表示为

表示集合M中第j个信标无人机的全球坐标值；

在测量时刻t_k测得目标无人机i自身的惯性导航坐标，记为p_i[k]＝[x_i,y_i,z_i]^T，[x_i,y_i,z_i]表示目标无人机自身的惯性导航坐标值；

在测量时刻t_k测得集合M中各信标无人机与目标无人机i的距离，其中，第j架信标无人机与目标无人机i的距离记为d_j[k]；

(1.3)、设置旋转矩阵R和平移矩阵T，R＝{r_oh}＝[r₁₁,r₁₂,r₁₃；r₂₁,r₂₂,r₂₃；r₃₁,r₃₂,r₃₃]，T＝{t_q}＝[t₁,t₂,t₃]，其中，r_oh表示R中第o行第h列的元素值，o,h∈[1,3]；t_q表示T中第q列的元素值，q∈[1,3]；

将旋转矩阵R和平移矩阵T作为集合M中各信标无人机的全球坐标系和目标无人机i自身惯性导航坐标系之间的转换关系，从而将目标无人机i的全球坐标表示为R·p_i[k]+T；

(1.4)、将集合M中信标无人机j与目标无人机i的距离d_j[k]通过全球坐标表示为：

d_j[k]＝||p_M→j[k]-(R·p_i[k]+T)|| (1)

省略时间下标[k]，整理并展开全球坐标下的距离等式(1)，得：

将展开式(2)中16项未知数{r_oh},{t_q},

分别定义为独立变量Θ＝[θ₁,…,θ₁₆]^T，其中，θ₁,…,θ₉对应{r_oh}，θ₁₀,…,θ₁₂对应{t_q}，θ₁₃,…,θ₁₆对应

在每个测量时刻t_k可以得到m个等式(2)，因此在最终时刻t_n共可以得到nm个等式，整合所有等式得到多无人机互定位模型为：

subject to.C_(Θ)＝0 (3)

其中，系数矩阵

A_[kj]为t_k时刻信标无人机j与目标无人机i距离等式中独立变量的行向量系数；b是nm×1的列向量，其第(k-1)m+j行为/>

C_(Θ)表示独立变量之间的正交约束条件集；

(2)、通过半定规划松弛算法将公式(3)转为凸优化问题并求解多无人机互定位模型；

(2.1)、定义X＝[Θ,-1]^T[Θ,-1]，P＝[A b]^T[A b]；＜P,X>为矩阵P和矩阵X的内积；X_17,17是变量矩阵X的第17行17列的元素；rank(X)为矩阵X的秩；

(2.2)、通过X、P可以将公式(3)转为凸优化问题，表示为：

其中，Q_e为17×17的对称矩阵，q_e为标量由公式(3)中C_(Θ)＝0变换得到；

(2.3)、通过半定规划松弛算法求解公式(4)，得到旋转矩阵R的估计值

以及准确的平移矩阵T：

(2.4)、通过正交普氏分析算法优化估计值

从而得到旋转矩阵R；

subject to. ΩΩ^T＝I,detΩ＝1 (5)

其中，Ω为正交矩阵，||·||_F表示求矩阵的F范数；

(3)、更新信标无人机的权重；

(3.1)、根据步骤(2)求得的平移矩阵T和旋转矩阵R，得到信标无人机j与目标无人机i的距离误差比Δ_j％：

(3.2)、更新信标无人机j的权重；

其中，w_j为防止单架信标无人机权重过小设置的基础权重；

(4)、通过极大似然估计优化R和T；

(4.1)、设距离测量时存在均值为0、标准差为σ²的高斯噪声ξ干扰，则将距离测量值表示为：

(4.2)、在高斯噪声干扰下，构建旋转矩阵R和平移矩阵T的极大似然函数为：

(4.3)、求解等式(8)中R和T的极大似然解，等同于求解

的最小值；

将步骤(3)中更新后的权重

代入/>

得：

(4.4)、将步骤(2)得到的旋转矩阵R和平移矩阵T作为极大似然估计迭代求解的初始值，然后利用梯度下降算法迭代优化R和T：

subject to.RR^T＝1,det(R)＝1 (10)

(4.5)、设置阈值Λ，当公式(10)求解的最小值小于等于时阈值Λ，停止迭代，并输出优化后的R和T；

(5)、将输出的R和T代入至步骤(1.3)中的目标无人机i的全球坐标表达式，从而得到目标无人机i的全球坐标，实现了多无人机互定位。

本发明的发明目的是这样实现的：

本发明一种基于距离测量的多无人机互定位方法，先利用集群中的通信链路对机间距离进行测量，再根据多智能体协同理论建立基于通信距离测量的多无人机互定位模型；其次，通过半定规划松弛算法和正交普氏分析算法得到目标无人机的初步解，再对信标无人机进行权重更新，然后利用极大似然估计法求解目标无人机的最终解，从而实现了多无人机互定位。

同时，本发明一种基于距离测量的多无人机互定位方法还具有以下有益效果：

(1)、适用范围广且适应能力强：场景中无人机集群不需要以任何指定的模式移动，在三维空间中所有无人机的飞行轨迹都是随机，然而利用极大似然估计可以有效降低测量噪声的干扰，使算法对噪声有较强的鲁棒性。

(2)、成本消耗和需求信息少：该多无人机互定位模型最少只需要一个配备GPS的信标无人机便可对整个集群进行定位，在定位过程中也仅需要测量信标无人机和目标无人机之间的距离就实现定位。

(3)、定位精度高且定位速度快：本发明利用集群中的通信链路对机间距离进行测量，结合惯性导航系统和少数已定位无人机来定位进入卫星拒止区域无人机的全球坐标，针对距离测量中的系统偏差，本发明又进行了无人机的权重分配，使无人机集群整体的定位精度得到提高。

附图说明

图1是本发明一种基于距离测量的多无人机互定位方法流程图；

图2是卫星拒止区域中无人机互定位场景图。

图3是两架信标无人机定位一架目标无人机实际轨迹和拟合轨迹对比图。

图4是本发明提出的算法对环境噪声的鲁棒性曲线。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

实施例

图1是本发明一种基于距离测量的多无人机互定位方法流程图。

在本实施例中，如图1所示，本发明一种基于距离测量的多无人机互定位方法，包括以下步骤：

S1、建立多无人机互定位模型；

S1.1、在本实施例中，假定卫星拒止环境下有M＝2架能与GPS正常通信的无人机，可以在全球坐标系中知道自己的位置，叫做信标无人机；一架受环境干扰失去GPS通信的无人机，未知自己在全球坐标系的位置，但可以通过整合陀螺仪和加速度计获得在自身惯性导航坐标系的位置，叫做目标无人机，从而搭建出多架信标无人机定位一架目标无人机的应用场景，如图2所示，其中，多架信标无人机用集合M表示，

目标无人机用i表示；

设置各测量时刻，记为t_k，k＝1,2,…,n，总的时刻测量数n为16次；

S1.2、在测量时刻t_k测得集合M中各信标无人机的全球坐标，记为p_M[k]，其中，第j个信标无人机的全球坐标表示为

表示集合M中第j个信标无人机的全球坐标值；

在测量时刻t_k测得目标无人机i自身的惯性导航坐标，记为p_i[k]＝[x_i,y_i,z_i]^T，[x_i,y_i,z_i]表示目标无人机自身的惯性导航坐标值；

在测量时刻t_k测得集合M中各信标无人机与目标无人机i的距离，其中，第j架信标无人机与目标无人机i的距离记为d_j[k]；

S1.3、设置旋转矩阵R和平移矩阵T，R＝{r_oh}＝[r₁₁,r₁₂,r₁₃；r₂₁,r₂₂,r₂₃；r₃₁,r₃₂,r₃₃]，T＝{t_q}＝[t₁,t₂,t₃]，其中，r_oh表示R中第o行第h列的元素值，o,h∈[1,3]；t_q表示T中第q列的元素值，q∈[1,3]；

将旋转矩阵R和平移矩阵T作为集合M中各信标无人机的全球坐标系和目标无人机i自身惯性导航坐标系之间的转换关系，从而将目标无人机i的全球坐标表示为R·p_i[k]+T；

S1.4、将集合M中信标无人机j与目标无人机i的距离d_j[k]通过全球坐标表示为：

d_j[k]＝||p_M→j[k]-(R·p_i[k]+T)|| (1)

省略时间下标[k]，整理并展开全球坐标下的距离等式(1)，得：

将展开式(2)中16项未知数{r_oh},{t_q},

分别定义为独立变量Θ＝[θ₁,…,θ₁₆]^T，其中，θ₁,…,θ₉对应{r_oh}，θ₁₀,…,θ₁₂对应{t_q}，θ₁₃,…,θ₁₆对应

在每个测量时刻t_k可以得到m个等式(2)，因此在最终时刻t_n共可以得到nm个等式，整合所有等式得到多无人机互定位模型为：

subject to.C_(Θ)＝0 (3)

其中，系数矩阵

A_[kj]为t_k时刻信标无人机j与目标无人机i距离等式中独立变量的行向量系数；b是nm×1的列向量，其第(k-1)m+j行为/>

C_(Θ)表示独立变量之间的正交约束条件集，在本实施例中，/>

具体取值如下：

C₆＝θ₁θ₂+θ₄θ₅+θ₇θ₈＝0

C₇＝θ₁θ₁₀+θ₄θ₁₁+θ₇θ₁₂-θ₁₃＝0

C₈＝θ₂θ₁₀+θ₅θ₁₁+θ₈θ₁₂-θ₁₄＝0

S2、通过半定规划松弛算法将公式(3)转为凸优化问题并求解多无人机互定位模型；

S2.1、公式(3)为一个非凸优化问题，我们通过半定规划松弛将其转为凸优化问题进行求解；首先我们定义X＝[Θ,-1]^T[Θ,-1]，P＝[A b]^T[A b]；<P,X>为矩阵P和矩阵X的内积；X_17,17是变量矩阵X的第17行17列的元素；rank(X)为矩阵X的秩；

S2.2、通过X、P可以将公式(3)转为凸优化问题，表示为：

其中，Q_e为17×17的对称矩阵，q_e为标量；公式(4)<Q_e,X>＝q_e中对应X的系数由公式(3)正交约束条件集C_(Θ)＝0中Θ的系数等效变换得到；

S2.3、通过半定规划松弛算法求解公式(4)，得到旋转矩阵R的估计值

以及准确的平移矩阵T：

S2.4、半定规划松弛可能会破坏旋转矩阵R的正交性，得到的解

不满足条件，我们可以通过正交普氏分析算法优化估计值/>

从而得到旋转矩阵R；

subject to.ΩΩ^T＝I,detΩ＝1 (5)

其中，Ω为正交矩阵，||·||_F表示求矩阵的F范数；

在本实施例中，上述最小化问题是正交普氏分析算法中一类特殊情况，我们可以通过SVD奇异值分解进行求解。令

为矩阵的SVD分解，可得旋转矩阵估计解：

其中，U和V^*均是一个9×9的矩阵，J是一个9×9的对角矩阵。

S3、更新信标无人机的权重；

S3.1、当信标无人机集数量n≥2时，每架信标无人机测得的数据对于目标无人机i定位误差的影响是不一样的，因此我们根据步骤S2求得的平移矩阵T和旋转矩阵R，得到信标无人机j与目标无人机i的距离误差比Δ_j％：

S3.2、在本场景中存在两架信标无人机，每架信标无人机测得的数据对于定位误差影响是不一样的。考虑到各架信标无人机在测量时存在距离测量远近、飞行轨迹不同等因素，我们利用每架信标无人机各自的距离误差比分配权重，误差小的占据更多的权重。

其中，w_j为防止单架信标无人机权重过小设置的基础权重，我们设置基础权重为：

S4、通过极大似然估计优化R和T；

在有噪声情况下，旋转矩阵R和平移矩阵T的最优解通常是极大似然估计的，通过梯度下降算法我们可以得到R和T的极大似然估计解，提高目标无人机i的定位精度。前面半定规划松弛和正交普氏分析为求解R和T得极大似然估计给出了一个良好的初始解，接下来我们将使用梯度下降算法对矩阵R和T进行优化。下面我们对具体的求解过程进行描述，具体如下：

S4.1、设距离测量时存在均值为0、标准差为σ²的高斯噪声ξ干扰，则将距离测量值表示为：

S4.2、我们假设坐标值的测量没有噪声的干扰，且不同时刻测量的噪声是独立的，可得：(d_j[k]-||R·P_i[k]+T-P_M→j[k]||)～N(0,σ²)，因此在高斯噪声干扰下，我们可以构建旋转矩阵R和平移矩阵T的极大似然函数为：

S4.3、求解等式(8)中R和T的极大似然解，等同于求解

的最小值；

将步骤S3中更新后的权重

代入/>

得：

S4.4、将步骤S2得到的旋转矩阵R和平移矩阵T作为极大似然估计迭代求解的初始值，然后利用梯度下降算法迭代优化R和T：

subject to.RR^T＝1,det(R)＝1 (10)

S4.5、设置阈值Λ，当公式(10)求解的最小值小于等于时阈值Λ，停止迭代，并输出优化后的R和T；

S5、将输出的R和T代入至步骤S1.3中的目标无人机i的全球坐标表达式，从而得到目标无人机i的全球坐标，实现了多无人机互定位。

图3是本发明中两架信标无人机定位一架目标无人机实际飞行轨迹和拟合飞行轨迹对比图。图中实线为目标无人机的实际飞行轨迹曲线，虚线为目标无人机的拟合飞行轨迹曲线。经过基于距离测量的多无人机互定位算法流程我们得到目标无人机的全球坐标，所求得的定位结果比较精确。随着测量次数的增加，拟合飞行轨迹更加贴合真实飞行轨迹，定位误差逐渐减少。

图4是本发明中的多无人机互定位算法在噪声环境下的定位结果。定义卫星拒止环境下的信噪比为SNR_dB＝20log₁₀(A_signal/A_noise)。在SNR_dB＝30时的定位误差曲线如图4所示。处于上方的曲线为未进行过噪声处理的算法，处于下方的曲线为本发明中所提出的基于距离测量的多无人机互定位算法。通过对比发现，本发明的算法减少了目标无人机的定位误差，算法对噪声有很好的鲁棒性。

综述，通过分析目标无人机的拟合飞行轨迹和对环境噪声的鲁棒性，目标无人机的定位结果能够满足定位精度的要求，从而验证了本发明提出的一种基于距离测量的多无人机互定位方法实现了无人机在卫星拒止区域精确、快速、稳定的定位。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (1)

1.一种基于距离测量的多无人机互定位方法，其特征在于，包括以下步骤：

(1)、建立多无人机互定位模型；

(1.1)、搭建多架信标无人机定位一架目标无人机的应用场景，其中，多架信标无人机用集合M表示，

m表示集合M信标无人机的总个数；目标无人机用i表示；

设置各测量时刻，记为t_k，k＝1,2,…,n，n为总的时刻测量数；

(1.2)、在测量时刻t_k测得集合M中各信标无人机的全球坐标，记为p_M[k]，其中，第j个信标无人机的全球坐标表示为

表示集合M中第j个信标无人机的全球坐标值；

在测量时刻t_k测得目标无人机i自身的惯性导航坐标，记为p_i[k]＝[x_i,y_i,z_i]^T，[x_i,y_i,z_i]表示目标无人机自身的惯性导航坐标值；

在测量时刻t_k测得集合M中各信标无人机与目标无人机i的距离，其中，第j架信标无人机与目标无人机i的距离记为d_j[k]；

(1.3)、设置旋转矩阵R和平移矩阵T，R＝{r_oh}＝[r₁₁,r₁₂,r₁₃；r₂₁,r₂₂,r₂₃；r₃₁,r₃₂,r₃₃]，T＝{t_q}＝[t₁,t₂,t₃]，其中，r_oh表示R中第o行第h列的元素值，o,h∈[1,3]；t_q表示T中第q列的元素值，q∈[1,3]；

将旋转矩阵R和平移矩阵T作为集合M中各信标无人机的全球坐标系和目标无人机i自身惯性导航坐标系之间的转换关系，从而将目标无人机i的全球坐标表示为R·p_i[k]+T；

(1.4)、将集合M中信标无人机j与目标无人机i的距离d_j[k]通过全球坐标表示为：

d_j[k]＝||p_M→j[k]-(R·p_i[k]+T)|| (1)

省略时间下标[k]，整理并展开全球坐标下的距离等式(1)，得：

将展开式(2)中16项未知数{r_oh},{t_q},

分别定义为独立变量Θ＝[θ₁,…,θ₁₆]^T，其中，θ₁,…,θ₉对应{r_oh}，θ₁₀,…,θ₁₂对应{t_q}，θ₁₃,…,θ₁₆对应

在每个测量时刻t_k可以得到m个等式(2)，因此在最终时刻t_n共可以得到nm个等式，整合所有等式得到多无人机互定位模型为：

subject to.C_(Θ)＝0 (3)

其中，系数矩阵

A_[kj]为t_k时刻信标无人机j与目标无人机i距离等式中独立变量的行向量系数；b是nm×1的列向量，其第(k-1)m+j行为

C_(Θ)表示独立变量之间的正交约束条件集；

(2)、通过半定规划松弛算法将公式(3)转为凸优化问题并求解多无人机互定位模型；

(2.1)、定义X＝[Θ,-1]^T[Θ,-1]，P＝[A b]^T[A b]；<P,X>为矩阵P和矩阵X的内积；X_17,17是变量矩阵X的第17行17列的元素；rank(X)为矩阵X的秩；

(2.2)、通过X、P可以将公式(3)转为凸优化问题，表示为：

其中，Q_e为17×17的对称矩阵，q_e为标量由公式(3)中C_(Θ)＝0变换得到；

(2.3)、通过半定规划松弛算法求解公式(4)，得到旋转矩阵R的估计值

以及准确的平移矩阵T：

(2.4)、通过正交普氏分析算法优化估计值

从而得到旋转矩阵R；

subject to.ΩΩ^T＝I,detΩ＝1 (5)

其中，Ω为正交矩阵，||·||_F表示求矩阵的F范数；

(3)、更新信标无人机的权重；

(3.1)、根据步骤(2)求得的平移矩阵T和旋转矩阵R，得到信标无人机j与目标无人机i的距离误差比Δ_j％：

(3.2)、更新信标无人机j的权重；

其中，w_j为防止单架信标无人机权重过小设置的基础权重；

(4)、通过极大似然估计优化R和T；

(4.1)、设距离测量时存在均值为0、标准差为σ²的高斯噪声ξ干扰，则将距离测量值表示为：

ξ～N(0,σ²)；

(4.2)、在高斯噪声干扰下，构建旋转矩阵R和平移矩阵T的极大似然函数为：

(4.3)、求解等式(8)中R和T的极大似然解，等同于求解

的最小值；

将步骤(3)中更新后后的权重

代入/>

得：

(4.4)、将步骤(2)得到的旋转矩阵R和平移矩阵T作为极大似然估计迭代求解的初始值，然后利用梯度下降算法迭代优化R和T：

subject to.RR^T＝1,det(R)＝1 (10)

(4.5)、设置阈值Λ，当公式(10)求解的最小值小于等于时阈值Λ，停止迭代，并输出优化后的R和T；

(5)、将输出的R和T代入至步骤(1.3)中的目标无人机i的全球坐标表达式，从而得到目标无人机i的全球坐标，实现了多无人机互定位。

Images