0 引　言

随着无人平台作业自主性、智能化等能力的提升，对无人平台的作业能力提出更高要求，多任务需求使得单一无人平台难以独立完成任务，必须协同其他多个无人平台，以无人集群形式协同完成任务。对于无人集群来说，导航信息是其航行和任务顺利完成的基本前提和保障条件。卫星导航由于精度高、误差不随时间积累、能全天候提供PVT信息、价格低、重量轻、体积小等优势，成为无人集群主要的导航手段之一^{[1 − 2]}。对于无人车集群来说，其作业场景包括密林等卫星导航受限或拒止的情况^{[3 − 4]}，为此基于相对测量的协同定位技术被提出。其中基于无线电测距的协同定位方法由于计算量小等优势，成为无人集群协同定位的重要方式之一。

国内外学者，针对基于无线电测距的协同定位技术展开了广泛研究^{[5 − 8]}。针对无线电测距存在的非视距传输问题，徐淑萍等^[9]提出2种基于Chan-Taylor 的协同定位方法来抑制NLOS误差，有效避免NLOS误差带来的影响，而后通过动静态定位实验验证了算法性能；闫宝芳等^[10]通过计算接收信噪比，通过信噪比大小来辨别接收信号为视距信号还是非视距信号，信噪比阈值的范围对该算法的性能有较大影响；王端荣等^[11]利用协同定位结果残差来判断是否受到NLOS污染，而后将污染严重的测量数据丢弃，改善了协同定位性能，但是在一定程度上也消去了一部分有效的表征定位目标的信息。针对无线电测距存在的协同定位不连续的问题，Gao等^[12]和杨秀梓等^[13]采用惯导等推算导航传感器与无线电协同定位组合，从而提升协同定位系统精度。

上述算法研究中，多采用KF滤波来融合无线电测距信息与其他导航传感器信息。信息融合时，需要将无线电测量模型近似线性处理。在平台间距离足够远，误差比较小的情况下，这样处理对定位误差的影响不大。然而当距离较短，测距误差较大的情况，非线性处理将导致协同定位误差变大。针对这个问题，Zeng等^[14]提出基于EKF的无线电测距信息融合方法，提高了协同定位性能。为了进一步改善无人集群中基于无线电协同定位中的非线性，提出一种面向无人集群应用的基于UKF的无线电协同定位方法。

1 基于无线点测距的协同定位模型

基于无线电测距的协同定位通过测量主从节点之间的无线信号传输时间，即到达时间来测量主从节点之间的距离。常用的测距方法主要有单向单程测距法、双向双程测距法。单向单程测距法首先要实现全局时间同步，在时间同步的基础上开展测距。在测距中，主节点向从节点发送测距信号，当从节点收到测距信号后，通过解调解码得到主节点发送信号的时刻，而后根据从节点本地时间计算得到主从节点之间的距离。双向双程测距法无需先进行全局时钟同步。在测距中，主节点首先给从节点发送测距信号，从节点收到测距信号后给主节点发送测距信号的响应信号，待主节点收到测距响应信号后再给从节点发送测距信号，通过测距信号传输时间的计算得到主从节点之间的距离。由于无人车集群相对来说机动性不强，且对成本具有一定的要求，这里采用双向双程测距方法。

基于无线电测距的定位的原理如图1所示。图1为一个二维空间协同定位模型，为了得到从节点的位置，需要从节点与3个主节点之间进行测距。

假设从节点第k时刻的坐标为$ \left( {{x_k},{y_k},{z_k}} \right) $，第i个主节点第k时刻的坐标为$ \left( {{x_{i,k}},{y_{i,k}},{z_{i,k}}} \right) $，通过数据链测得的从节点与第i个主节点第k时刻的距离为$ {r_{i,k}} $，从而有

$ {\left( {{x_k} - {x_{i,k}}} \right)^2} + {\left( {{y_k} - {y_{i,k}}} \right)^2} + {\left( {{z_k} - {z_{i,k}}} \right)^2} = r_{i,k}^2。$

(1)

由于采用双向双程测距方法，可以认为在测距过程中完成主从节点的时钟同步，因此式(1)中可以不考虑从节点的本地时钟偏差。从而，N个主从节点的测距方程为：

$ \left\{ {\begin{array}{*{20}{c}} {{{\left( {{x_k} - {x_{1,k}}} \right)}^2} + {{\left( {{y_k} - {y_{1,k}}} \right)}^2} + {{\left( {{z_k} - {z_{1,k}}} \right)}^2} = r_{1,k}^2}，\\ {{{\left( {{x_k} - {x_{2,k}}} \right)}^2} + {{\left( {{y_k} - {y_{2,k}}} \right)}^2} + {{\left( {{z_k} - {z_{2,k}}} \right)}^2} = r_{2,k}^2}，\\ \vdots \\ {{{\left( {{x_k} - {x_{{N},k}}} \right)}^2} + {{\left( {{y_k} - {y_{{N},k}}} \right)}^2} + {{\left( {{z_k} - {z_{{N},k}}} \right)}^2} = r_{{N},k}^2}。\end{array}} \right. $

(2)

通过求解式（2）可以得到从节点的位置信息。一般来说，既可以采用最小二乘方法得到从节点的定位信息，也能利用KF滤波方法得到从节点的位置。KF滤波相比最小二乘更有效地利用了历史信息。

2 基于KF的协同定位方法

在基于KF滤波的协同定位方法中，从节点的运动模型采用匀速模型，其状态方程采用误差状态方程为：

$ \partial {{\boldsymbol{X}}_{k/k - 1}} = {\boldsymbol{\varPhi}} \cdot \partial {{\boldsymbol{X}}_{k - 1}} + {{\boldsymbol{n}}_k} ，$

(3)

式中：$ {{\boldsymbol{n}}_k} $为状态噪声；$ \partial {{\boldsymbol{X}}_k} $为第k时刻的误差状态量，其表示为：

$ \partial {{\boldsymbol{X}}_k} = \left[ {\begin{array}{*{20}{c}} {\partial {x_k}} \\ {\partial {y_k}} \\ {\partial {z_k}} \\ {\partial {{\dot x}_k}} \\ {\partial {{\dot y}_k}} \\ {\partial {{\dot z}_k}} \end{array}} \right]。$

(4)

式中：$ {\dot x_{}} $为$ x $对微分。$ {\boldsymbol{ \varPhi}} $为状态一步转移矩阵，其表示为

$ {\boldsymbol{\varPhi}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&T&0&0 \\ 0&1&0&0&T&0 \\ 0&0&1&0&0&T \\ 0&0&0&1&0&0 \\ 0&0&0&0&1&0 \\ 0&0&0&0&0&1 \end{array}} \right]。$

(5)

式中：T为KF滤波周期。

用$ \rho _{i,k}^{} $为从节点与第i个主节点之间的伪距，将其在$ (x,y,z) $处泰勒展开，忽略二阶以上的高次项，可得

$ \begin{split} \rho _{i,k}^{} \approx & {r_{i,k}} + \frac{{\partial \rho _{i,k}^{}}}{{\partial x}}{\text{δ}} x + \frac{{\partial \rho _{i,k}^{}}}{{\partial y}}{\text{δ}} y + \frac{{\partial \rho _{i,k}^{}}}{{\partial z}}{\text{δ}} z = \\ &{r_{i,k}} + \frac{{{x_k} - {x_{i,k}}}}{{\rho _{i,k}^{}}}{\text{δ}} x + \frac{{{y_k} - {y_{i,k}}}}{{\rho _{i,k}^{}}}{\text{δ}} y + \frac{{{z_k} - {z_{i,k}}}}{{\rho _{i,k}^{}}}{\text{δ}} z \end{split}，$

(6)

令

$ \left\{ \begin{gathered} {e_{i,x}} = \frac{{{x_k} - {x_{i,k}}}}{{\rho _{i,k}^{}}}，\\ {e_{i,y}} = \frac{{{y_k} - {y_{i,k}}}}{{\rho _{i,k}^{}}}，\\ {e_{i,z}} = \frac{{{z_k} - {z_{i,k}}}}{{\rho _{i,k}^{}}}。\\ \end{gathered} \right. $

(7)

则有：

$ \rho _{i,k}^{} = r_{i,k}^{} + {e_{i,x}}{\text{δ}} x + {e_{i,y}}{\text{δ}} y + {e_{i,z}}{\text{δ}} z。$

(8)

通过无线电相对测距获得的从节点与第i个主节点之间的几何距离$ \rho _{i,k}^{link} $可写为：

$ \rho _{i,k}^{link} = = r_{i,k}^{} + {e_{i,x}}{\text{δ}} x + {e_{i,y}}{\text{δ}} y + {e_{i,z}}{\text{δ}} z + n ，$

(9)

式中，$ n $为伪距测量噪声，进一步化简为

$ {\text{δ}} \rho _{i,k}^{} = \rho _{i,k}^{link} - r_{i,k}^{} = {e_{i,x}}{\text{δ}} x + {e_{i,y}}{\text{δ}} y + {e_{i,z}}{\text{δ}} z + n ，$

(10)

从而可得相对定位的量测模型为：

$ {\text{δ}} {{\boldsymbol{\rho }}_k} = \left[ {\begin{array}{*{20}{c}} {{\text{δ}} \rho _k^1} \\ {{\text{δ}} \rho _k^2} \\ \vdots \\ {{\text{δ}} \rho _k^n} \end{array}} \right] = {{\boldsymbol{H}}_k} \cdot {\text{δ}} {{\boldsymbol{X}}_k}。$

(11)

3 基于UKF的协同定位方法

基于UKF的协同定位方法的状态方程与基于KF的协同定位方法一致，主要区别在于量测方程。从式（6）可知，在基于KF的协同定位方法中，通过泰勒展开，将原本非线性的量测模型线性化了。在基于无线电的协同定位中，由于无人车集群之间的相互距离有时比较小，从而使得量测模型在线性化时引入较大的误差。基于UKF的协同定位方法保留了量测模型的非线性，能够有效缓解基于KF的协同定位方法中，线性化量测模型带来的定位误差。

基于UKF的协同定位方法的处理流程如下：

1）初始化$ \partial {{\mathbf{X}}_0} $和协方差$ {{{P}}_0} $。

2）计算2N+1个Sigma点以及它们的权值，

$\begin{aligned} &\left\{ {\begin{array}{*{20}{c}} {\partial {{\boldsymbol{X}}_{j,k - 1}} = \partial {{\boldsymbol{X}}_0}}，\\ {{{\boldsymbol{P}}_{k - 1}} = {{\boldsymbol{P}}_0}} 。\end{array}} \right.\\ &\left\{ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\partial {{\boldsymbol{X}}_{j,k - 1}} = \bar x + {{\left( {\sqrt {\left( {N + \lambda } \right){{\boldsymbol{P}}_{_{k - 1}}}} } \right)}_j}，\begin{array}{*{20}{c}} {}{\begin{array}{*{20}{c}} { j = 1,2, \cdots ,N}，&{} \end{array}} \end{array}}&{} \end{array}} \\ {\begin{array}{*{20}{c}} {\partial {{\boldsymbol{X}}_{j,k - 1}} = \bar x + {{\left( {\sqrt {\left( {N + \lambda } \right){{\boldsymbol{P}}_{k - 1}}} } \right)}_j}}，{j = N + 1,N + 2, \cdots ,2N} 。\end{array}} \end{array}} \right. \end{aligned}$

(12)

$ \left\{ {\begin{array}{*{20}{l}} {\omega _0^m = \displaystyle\frac{\lambda }{{N + \lambda }}}，\\ {\omega _0^c = \displaystyle\frac{\lambda }{{N + \lambda }} + \left( {1 - {\alpha ^2} + \beta } \right)} 。\end{array}} \right. $

(13)

$ \begin{array}{*{20}{c}} {\omega _j^m = \omega _j^c = \displaystyle\frac{1}{{2\left( {N + \lambda } \right)}}}，&{j = 1,2, \cdots ,2N} 。\end{array} $

(14)

式中，$ \lambda = {\alpha ^2}\left( {N + \kappa } \right) - N $，$ \alpha $通常取一个比较小的正值，它用于决定Sigma点的散步情况；$ \kappa $可以取0值；$ \beta $表示状态量的分布信息；$ {\left( {\sqrt {\left( {N + \lambda } \right){{\boldsymbol{P}}_{k - 1}}} } \right)_j} $表示矩阵平方根第j列；$ \omega _j^m $表示第j组状态量均值的权值，$ \omega _j^c $表示第j组状态量方差的权值。

3）进行时间更新

根据状态方程，得到2N+1个Sigma点传播后的状态$ \partial {{\boldsymbol{X}}_{j,k/k - 1}} $，进而得到状态预测值：

$ \partial {{\boldsymbol{\bar X}}_{k/(k - 1)}} = \sum\limits_{j = 0}^{2N} {\omega _j^m\partial {{\boldsymbol{X}}_{j,k/(k - 1})}}。$

(15)

相应的状态协方差计算如下：

$\begin{split} {{\boldsymbol{P}}_{k/(k - 1)}} =& \sum\limits_{j = 0}^{2N} {\omega _j^c\left[ {\partial {{\boldsymbol{X}}_{j,k/(k - 1)}} - \partial {{{\boldsymbol{\bar X}}}_{k/(k - 1)}}} \right]} \times\\ &{\left[ {\partial {{\boldsymbol{X}}_{j,k/(k - 1)}} - \partial {{{\boldsymbol{\bar X}}}_{k/(k - 1)}}} \right]^{\rm T}} + {\boldsymbol{Q}} 。\end{split}$

(16)

4）进行量测更新

将$ \partial {{\boldsymbol{X}}_{j,k/(k - 1)}} $对应元素代入式（2），得到2N+1个Sigma点对应的2N+1伪距预测值$ \rho _{i,j,k}^{link} $，将其写成M×(2N+1)的矩阵$ {{\boldsymbol{Z}}_{k/(k - 1)}} $，从而得到量测预测矩阵$ {{\boldsymbol{\bar Z}}_{k/(k - 1)}} $，其第i个元素为：

$ \bar \rho _{i,k}^{link} = \sum\limits_{j = 0}^{2N} {\omega _j^m\rho {{_{i,j,k}^{link}}_{j,k/(k - 1)}}}。$

(17)

自相关协方差计算为：

$ {\boldsymbol{P}}_{k/(k - 1)}^{zz} = \sum\limits_{j = 0}^{2N} {\omega _j^c \left[ {{{\boldsymbol{Z}} _{j,k/(k - 1)}} - {{{\boldsymbol{\bar Z}}}_{k/(k - 1)}}} \right]} {\left[ {{{\boldsymbol{Z}} _{j,k/(k - 1)}} - {{{\boldsymbol{\bar Z}}}_{k/(k - 1)}}} \right]^{\text T}}。$

(18)

互相关协方差计算为：

$ {\boldsymbol{P}}_{k/(k - 1)}^{xz} = \sum\limits_{j = 0}^{2N} {\omega _j^c \left[ {\partial {{\boldsymbol{X}} _{j,k/(k - 1)}} - \partial {{{\boldsymbol{\bar X}}} _{k/(k - 1)}}} \right]} {\left[ {{{\boldsymbol{Z}} _{j,k/k - 1}} - {{{\boldsymbol{\bar Z}}} _{k/(k - 1)}}} \right]^{\text T}}。$

(19)

卡尔曼滤波增益更新为：

$ {{\boldsymbol{K}}_k} = {\boldsymbol{P}}_{k/(k - 1)}^{zz}{\left( {{\boldsymbol{P}}_{k/(k - 1)}^{xz} + {\boldsymbol{R}}} \right)^{ - 1}}。$

(20)

式中：$ {\boldsymbol{R}} $为量测噪声矩阵，从而得到k时刻协同定位的最优估计为：

$ \partial {{\boldsymbol{\hat X}}_{k/(k - 1)}} = \partial {{\boldsymbol{\bar X}}_{k/(k - 1)}} + {{\boldsymbol{K}}_k}\left( {{{\boldsymbol{Z}}_k} - {{\boldsymbol{Z}}_{k/(k - 1)}}} \right)。$

(21)

最后对系统状态的协同方差进行更新：

$ {\boldsymbol{P}}_k^{} = {\boldsymbol{P}}_{k/(k - 1)}^{} - {{\boldsymbol{K}}_k}{\boldsymbol{P}}_{k/(k - 1)}^{zz}{\boldsymbol{K}}_k^{\mathrm{T}} 。$

(22)

4 实验验证及分析

验证实验系统主要包括5套无线电测距设备，每套无线电测距设备可同时测量其与其他4套无线电测距设备之间的距离，同时配备有GPS接收模块来确定自身位置。实验中，一套无线电测距设备作为从节点，其他4套作为主节点。通过断开从节点的GPS接收机天线，模拟从节点卫星导航拒止条件。其中，GPS接收机模块的定位精度为10 m，无线电测距精度为30 m。每个主节点每次完成距离测量后，将其通过GPS接收模块获得位置信息，通过无线电得到的与从节点间测距信息发送给从节点。通过上位机录取从节点接收到的所有主节点的导航信息，而后基于Matlab进行事后处理。

分别基于KF和UKF对从节点接收到的信息进行融合处理得到从节点基于主节点的协同定位结果，如图2和图3所示。由图2可知，采用UKF能使纬度位置误差收敛到一个相对较低均方误差。经过数据统计可得，基于KF的协同定位方法获得的从节点纬度均方误差为22.37 m，而采用基于UKF的协同定位方法，从节点的纬度均方误差为15.90 m，提升了28.92%。

对比图2和图3可知，经度均方误差与纬度均方误差的特征差不多，基于UKF的协同定位方法能够获得在0附近波动的均方误差，而基于KF的协同定位方法得到的经度均方误差具有一个非零的均值。经过数据统计可得，基于KF的协同定位方法获得的从节点经度均方误差为2.65 m，而采用基于UKF的协同定位方法，从节点的经度均方误差为1.21 m，提升了54.34%。

5 结　语

卫星导航受限或者拒止应用场景中，通常采用协同定位来获得无人集群高精度的导航信息。基于KF的无线电测距的协同定位方法需要将量测模型线性化，从而带来线性化误差。本文提出非线性的UFK模型来拟合基于无线电测距的协同定位的量测模型，从而避免了线性化模型误差。实验结果表明，采用UKF滤波模型能够有效降低协同定位误差，从而改善无人集群协同定位性能，可为后续工程应用提供借鉴和参考。