14、协作定位算法及相关技术解析

协作定位算法及相关技术解析

1. 球面插值（SI）算法

在协作定位场景中，当距离误差主要由高斯噪声主导时，某些联合CT算法能实现较高的定位精度。然而，在集群飞行中，由于集群成员之间的几何关系，会产生非视觉测距误差。在存在非视觉测距误差和其他干扰的环境下，集群系统中各成员的测距误差不再服从高斯分布。为应对这种情况，提出了基于球面插值（SI）算法的协作定位算法，以在一定程度上提高非高斯观测误差情况下的协作定位解算精度。

SI算法具有高效性，它仅需相对简单的矩阵运算，计算强度低，且无需大量迭代或搜索。该算法通过在观测方程中引入距离方程误差来替代传统的观测误差，并以此为优化基础，将原本的非线性约束问题转化为线性约束问题。最后，使用对偶最小二乘法（LS）求解位置，有效提高了非高斯噪声环境下的定位精度。

基于相对距离差的协作导航模型，可根据相关方程得到描述集群中标签成员和锚点成员之间关系的方程。使用LS方法求解标签成员在相对坐标系中的位置坐标，具体公式如下：
$\hat{\mathbf{r}} {j \rightarrow i}=\arg \min \left[\mathbf{v} {i \rightarrow j}, \cdots\right]=\frac{1}{2}\left(\mathbf{R} {j \rightarrow}^{T} \mathbf{R} {j \rightarrow}\right)^{-1} \mathbf{R} {j \rightarrow}^{T}\left(\boldsymbol{\eta} {i \rightarrow j}+2 r_{i \rightarrow j} \Delta