CN103925925A - 一种用于多点定位系统的实时高精度位置解算方法 - Google Patents

Abstract

本发明提供一种用于多点定位系统的实时高精度位置解算方法，所述解算方法包括以下步骤：（1）多点定位系统的中心处理单元将接收到的远端单元信息存放到缓存，并判断时间标签一致性，筛选出有效信息；（2）利用Chan算法对有效信息进行处理，计算出目标定位初值；（3）将目标定位初值代入Taylor级数展开法，进行递归运算，获取高精度的定位信息；（4）利用自适应Kalman滤波算法对定位信息进行滤波处理，然后将处理后的定位信息上报至显控系统或指挥系统。本发明较传统方法在实时性和解算精度上有明显改善，且通过多种算法联合进行目标定位，降低了对中心处理单元的硬件和对远端单元数量的要求，减少了系统的硬件成本。

Description

一种用于多点定位系统的实时高精度位置解算方法

技术领域

本发明涉及一种多点定位系统领域，尤其是涉及一种用于多点定位系统的实时高精度位置解算方法。

背景技术

多点定位系统是继ADS-B技术之后，国际民航组织大力推广的又一新型航管监视技术。多点定位系统兼容了ADS-B的全部功能，还能够对仅装备了普通A/C/S模式应答机的飞机实时高精度可靠监视，但是这种实时高精度可靠监视要基于中心处理单元实时高精度的位置解算。由于技术水平局限性，在同步时钟精度不够高的情况下，如何进一步提高目标解算精度，以确保国际民航所要求机场场面目标监控精度，这一直是多点定位系统问世以来所难以克服及未解决的难题。

传统的多点定位解算方法主要采用的是Chan算法、Taylor级数展开法、Friedlander算法、Fang算法等为代表的视距定位计算方法。Chan算法计算量小，无需迭代运算，但在非视距环境下定位精度较差；Taylor级数展开法则需要一个迭代初值进行迭代运算，才能在短时间内解算得到精确目标位置；Friedlander算法对远端单元的数量有较大依赖型，当数量减少时，定位精度明显降低；Fang算法等为代表的视距算法不符合多点定位系统实际工程超视距的需要。

总之，上述任何一种位置解算算法都或多或少存在缺陷，不能在成本控制、实时性和定位精度三者间取得最佳平衡。因此，必须寻找一种实时高精度目标位置解算方法，以解决机场场面定位实时高精度的需求。

发明内容

本发明的目的在于：针对现有技术存在的问题，提供一种能解决机场场面定位实时高精度需求的用于多点定位系统的实时高精度位置解算方法。

本发明的发明目的通过以下技术方案来实现：

一种用于多点定位系统的实时高精度位置解算方法，其特征在于，多点定位系统的远端单元由一个主单元和多个副单元组成，所述解算方法包括以下步骤：

（1）多点定位系统的中心处理单元将接收到的远端单元信息存放到缓存，并判断时间标签一致性，筛选出有效信息；

（2）利用Chan算法对有效信息进行处理，计算出目标定位初值；

（3）将目标定位初值代入Taylor级数展开法，进行递归运算，获取高精度的定位信息；

（4）利用自适应Kalman滤波算法对定位信息进行滤波处理，然后将处理后的定位信息上报至显控系统或指挥系统。

优选的，步骤（1）中所述的判断时间标签一致性包括步骤：每隔_△t₁时间，以主单元接收到信息的时间和内容为基准，与其前后_△t₂时间长度内其他副单元接收信息内容进行比对，若有相同信息内容的目标，则作为有效信息用于后续运算，若无相同信息内容的目标，则作为无效信息进行抛弃，每条信息经过_△t₃时间后将进行抛弃。

优选的，所述的_△t₁、_△t₂、_△t₃三个时间根据远端单元实际分布情况进行确定。

优选的，所述的_△t₁=1s、_△t₂=300ms、_△t₃=3s。

优选的，所述的步骤（2）包括以下步骤：

（21）设副单元的空间位置为（x_i,y_i,z_i），主单元为(x₀,y₀,z₀)，（x,y,z）为目标的位置坐标，目标与第i个副单元的距离为r_i，△r_i代表目标到主单元与第i个副单元之间的距离差；则有： $r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n,$ Δr_i＝c×TDOA_i＝r_i-r₀,i＝1,2,...n，上式中c为光速3×10⁸m／s，TDOA_i为目标信息传输到主单元与第i个副单元之间的时间差；

（22）建立观测方程并线性化：

∵

其中f_i＝(Δr_i)²-K_i+K₀,i＝1,2,...n

$K_0=x_0^2+y_0^2+z_0^2,K_i=x_i^2+y_i^2+z_i^2$

（23）运用Chan算法进行初始位置解算：

∵ $(x_i-x_0)x+(y_i-y_0)y+(z_i-z_0)z-{\mathrm{\Δr}}_i{\·}_{}{r}_0=\frac{1}{2}[{\left(\mathrm{\Δ}{r}_i\right)}^2-K_i+K_0]$

∴GX_Chan＝F

其中 $X_{\mathrm{Chan}}=\left[\begin{array}{c}x\\ y\\ z\\ r_0\end{array}\right],F=\frac{1}{2}\left[\begin{array}{c}{\left({\mathrm{\Δr}}_i\right)}^2-K_1+K_0\\ \·\\ \·\\ \·\\ {\left({\mathrm{\Δr}}_n\right)}^2-K_n+K_0\end{array}\right]$

$G=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -{\mathrm{\Δr}}_1\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ x_n-x_0& y_n-y_0& z_n-z_0& -{\mathrm{\Δr}}_n\end{array}\right]$

由于到达时间差存在误差，设n_i为TDOA^τ _i,1的噪声，φ为误差向量：Δr_i＝Δr_i ⁰+cn_ii＝1,…,n

$n_i\↔\mathrm{TDOA}{\mathrm{\τ}}_{i,1}$

φ＝F-GX_Chan ⁰,其中G⁰X_Chan ⁰＝F⁰

$F^0=\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\Δr}}_1}^0\right)}^2-K_1+K_0\\ \·\\ \·\\ \·\\ {\left({{\mathrm{\Δr}}_n}^0\right)}^2-K_3+K_0\end{array}\right],G^0=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -\mathrm{\Δ}{r_1}^0\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ x_n-x_0& y_n-y_0& z_n-z_0& -{{\mathrm{\Δr}}_n}^0\end{array}\right]$

$\mathrm{\φ}=\frac{1}{2}\left[\begin{array}{c}{({{\mathrm{\Δr}}_1}^0+{\mathrm{cn}}_1)}^2-K_1+K_0\\ \·\\ \·\\ \·\\ {({{\mathrm{\Δr}}_n}^0+{\mathrm{cn}}_n)}^2-K_3+K_0\end{array}\right]-\{\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\Δr}}_1}^0\right)}^2-K_1+K_0\\ \·\\ \·\\ \·\\ {\left({{\mathrm{\Δr}}_n}^0\right)}^2-K_3+K_0\end{array}\right]+\left[\begin{array}{c}{\mathrm{cn}}_1\\ \·\\ \·\\ \·\\ {\mathrm{cn}}_n\end{array}\right]r_0\}$

计算结果：

B＝diag{r₁,…,r_n},n＝(n₁,…,n_n)^T

ψ＝E[φφ^T]＝c²BQB

（24）引入几何精度因素X′chan进行初值估计：

${r_i}^{\′}=X_{\mathrm{Chan}}\left(4\right)+{\mathrm{\Δr}}_i\⇒B=\mathrm{diag}({r_1}^{\′}...{r_n}^{\′})$

ψ＝c²BQB

X′_Chan＝(G^Tψ^-1G)^-1G·ψ^-1F

利用X′chan估计各分量的关联性，利用r₀进一步精确估算位置，建立方程H=G_αZ：

$H=\left[\begin{array}{c}{\left(X_{\mathrm{Chan}}^{\′}\right[1]-x_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\′}\right[2]-y_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\′}\right[3]-z_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\′}\right[4\left]\right)}^2\end{array}\right],G_a=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right],Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]$

可获得位置估算：

$Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]={\left({G_a}^T{{\mathrm{\ψ}}_{\mathrm{\ϵ}}}^{-1}{G}_a\right)}^{-1}{G}_a\·{{\mathrm{\ψ}}_{\mathrm{\ϵ}}}^{-1}\·H$

定位结果为：

$Z_p=\±\sqrt{Z}+\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]$ ±取决于X′_chan

Z_p即为Chan算法获得的目标定位初值。

优选的，所述的步骤（3）包括以下步骤：运用Taylor级数展开法对Chan算法获得的目标定位初值进行递归运算，每一次递归中通过求解测量误差最小二乘解来逐步逼近真实位置，直到达到所要求的精度门限；设上一步由Chan算法获得的目标定位初值Z_p为（x_e,y_e,z_e），建立多点定位系统远端第i个副单元的观测方程：

$r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n$ $=r_0+{\mathrm{\Δr}}_i-w_i=r_0+c\×{\mathrm{TDOA}}_i-w_i$ w_i为剩余数据

进行Taylor级数展开：

定义:x＝x_e+δ_x,y＝y_e+δ_y,z＝z_e+δ_z

$a_{i1}=\frac{{\∂r}_i}{{\∂}_x}{|}_{(x_e,y_e,z_e)},a_{i2}=\frac{{\∂r}_i}{{\∂}_y}{|}_{(x_e,y_e,z_e)},a_{i3}=\frac{{\∂r}_i}{{\∂}_z}{|}_{(x_e,y_e,z_e)}$

忽略二次和高次项的Taylor级数展开：

$r_i{|}_{x_e,y_e,z_e}=\sqrt{{(x_e-x_i)}^2+{(y_e-y_i)}^2+{(z_e-z_i)}^2}$

$r_i\≈r_i{|}_{x_e,y_e,z_e}+a_{i1}{\mathrm{\δ}}_x+a_{i2}{\mathrm{\δ}}_y+a_{i3}{\mathrm{\δ}}_z\≈r_0+c\×{\mathrm{TDOA}}_i-w_i$ $\⇒a_{i1}{\mathrm{\δ}}_x+a_{i2}{\mathrm{\δ}}_y+a_{i3}{\mathrm{\δ}}_z=r_0+c\×{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}-w_i$ $\⇒\mathrm{A\δ}=F-W$

其中δ＝[δ_x,δ_y,δ_z]^T; $F=r_0+c\×{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}$

则δ的加权最小二乘估计为：

δ＝(A^TR^-1A)^-1·A^TR^-1·F

其中： $A=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]F=\left[\begin{array}{c}{r}_0+c\×{\mathrm{TDOA}}_1-r_1{|}_{x_e,y_e,z_e}\\ r_0+c\×{\mathrm{TDOA}}_2-r_2{|}_{x_e,y_e,z_e}\\ \·\\ \·\\ \·\\ r_0+c\×{\mathrm{TDOA}}_n-r_n{|}_{x_e,y_e,z_e}\end{array}\right]$

R＝E[W^TW],W＝[w₁,w₂,…,w_n]^T

进行下一次递归时，令x′=x+δ_x，y′=y+δ_y，z′=z+δ_z，重复上述过程，直到δ_x，δ_y，δ_z足够小，满足设定门限：

|δ_x|+|δ_y|+|δ_z|＜η  η为设定精度门限值，

则此时的（x′,y′,z′）即为目标的解算位置。

优选的，步骤（4）中所述的自适应Kalman滤波算法为

$\left\{\begin{array}{c}X\left(k\right|k-1)=\mathrm{AX}(k-1|k-1)+\mathrm{BU}\left(k\right)+[W\left(k\right)+V\left(k\right)]\\ P\left(k\right|k-1)=\mathrm{AP}(k-1|k-1)A^{\′}+Q_{w+v}\\ X\left(k\right|k)=X\left(k\right|k-1)+\mathrm{Kg}\left(k\right)[Z\left(k\right)-\mathrm{HX}\left(k\right|k-1)]\\ \mathrm{Kg}\left(k\right)=P\left(k\right|k-1)H^{\′}/[\mathrm{HP}\left(k\right|k-1)H^{\′}+R]\\ P\left(k\right|k)=(I-\mathrm{Kg}\left(k\right)H)P\left(k\right|k-1)\end{array}\right.$

其中，A和B为系统参数，H为测量系统的参数，X（k|k-1）表示利用上一状态预测的当前状态，X（k-1|k-1）表示上一状态最优结果，U（k）为系统控制量，W（k）为系统过程噪声，P为X的协方差矩阵，A′为A的转置矩阵，Q为W的协方差矩阵，Z(k)为当前状态测量值，Kg为卡尔曼增益，R为U（k）协方差，I为单位矩阵，V（k）为未知的机动加速度，Q_w+v为[W(k)+V（k）]的协方差。

优选的，所述的步骤（4）还包括步骤：在对加速度假设的噪声进行统计估值时，和式中每项乘以不同的加权系数，按指数加权法，选取加权系数β_i使之满

${\mathrm{\&beta;}}_k={\mathrm{\&beta;}}_{i}b,0<b<1,\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_i=1$

β_i＝d_kbⁱ

d_k＝(1-b)/(1-b^k-1),i＝0,1,…,k

上式中：b成为遗忘因子，用（1-d_k-1）/(k-1)作为权系数，便得到指数加权渐消记忆的时变噪声统计估值器。

与现有技术相比，本发明具有以下优点：

1、本方法通过接收远端单元经纬度信息和时间标签信息作为输入，进行定位运算，解算出目标的实时位置信息，相比较之前采用传统多点定位算法所得的解算结果，在实时性和解算精度上都有明显改善；

2、采用Chan算法和Taylor级数展开法相结合的方式进行目标位置解算，在满足定位精度的同时，显著减少运算时间；

3、采用改进的自适应卡尔曼滤波算法，使其对非机动目标和机动目标都能有效滤波。

4、采用Chan算法、Taylor级数展开法、自适应Kalman滤波算法联合进行目标定位，在实时性和定位精度要求上实现最佳平衡，使其能够满足国际民航组织所要求机场场面目标监控精度；

5、本方法不需要增加额外的硬件设备，能有效降低成本；

6、本方法在系统后端完成，应用灵活度高，便于移植；

7、本方法运算量较小，普通计算机即可完成全部运算，同时对于远端单元数量没有较多要求，能有效降低系统成本，方便系统搭建调试；

8、本方法可在隐身飞机、手机定位跟踪等众多军民领域推广应用。

附图说明

图1为联合定位解算方法流程框图；

图2为远端单元空间分布示意图；

图3为改进卡尔曼滤波预测位置与实际位置；

图4为经典卡尔曼滤波测量位置与实际位置；

图5为经工程验证本发明所获得的目标位置信息能够达到国际民航所要求机场场面目标监控精度的实验结果图。

具体实施方式

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

实施例

如图1所示，本发明主要采用Chan算法、Taylor级数展开法、自适应Kalman滤波算法联合进行目标定位，多点定位系统的远端单元由一个主单元和多个副单元组成，其主要步骤为：第一，多点定位系统的中心处理单元将接收到的远端单元信息存放到缓存，并判断时间标签一致性，筛选出有效信息；第二，利用Chan算法具有计算量比较小、求解速度比较快的特点，先来对有效信息进行处理，计算出目标定位初值；第三，将目标定位初值代入Taylor级数展开法，进行递归运算，获取高精度的定位信息；第四，利用自适应Kalman滤波算法对定位信息进行滤波处理，然后将处理后的定位信息上报至显控系统或指挥系统。

本发明方法具体步骤如下：

首先判断时间标签一致性，多点定位系统的远端单元由一个主单元和多个副单元组成。中心处理单元将接收到的远端单元信息存放到缓存。每隔_△t₁时间，以主单元接收到信息的时间和内容为基准，与其前后_△t₂时间长度内其他副单元接收信息内容进行比对。若有相同信息内容的目标，则作为有效信息用于后续运算，若无相同信息内容的目标，则作为无效信息进行抛弃。每条信息经过_△t₃时间后将进行抛弃。

其中_△t₁、_△t₂、_△t₃三个时间根据远端单元实际分布情况进行确定，目前本方法中取_△t₁=1s、_△t₂=300ms、_△t₃=3s。

接下来利用上一步获得的有效信息进行目标位置解算，如图2所示，设各远端单元的副单元的空间位置为（x_i,y_i,z_i），主单元为(x₀,y₀,z₀)，（x,y,z）为目标的位置坐标，目标与第i个副单元的距离为r_i，△r_i代表目标到主单元与第i个副单元之间的距离差。

则有： $r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n,$ Δr_i＝c×TDOA_i＝r_i-r₀,i＝1,2,...n，上式中c为光速3×10⁸m／s，TDOA_i为目标信息传输到主单元与第i个副单元之间的时间差。

建立观测方程并线性化：

∵

其中f_i＝(Δr_i)²-K_i+K₀,i＝1,2,...n

$K_0=x_0^2+y_0^2+z_0^2,K_i=x_i^2+y_i^2+z_i^2$

1.运用Chan算法进行初始位置解算：

∵ $(x_i-x_0)x+(y_i-y_0)y+(z_i-z_0)z-{\mathrm{\&Delta;r}}_i{\&CenterDot;}_{}{r}_0=\frac{1}{2}[{\left(\mathrm{\&Delta;}{r}_i\right)}^2-K_i+K_0]$

∴GX_Chan＝F

其中: $X_{\mathrm{Chan}}=\left[\begin{array}{c}x\\ y\\ z\\ r_0\end{array}\right],F=\frac{1}{2}\left[\begin{array}{c}{\left({\mathrm{\&Delta;r}}_i\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({\mathrm{\&Delta;r}}_n\right)}^2-K_n+K_0\end{array}\right]$

$G=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -{\mathrm{\&Delta;r}}_1\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_n-x_0& y_n-y_0& z_n-z_0& -{\mathrm{\&Delta;r}}_n\end{array}\right]$

由于到达时间差存在误差，设n_i为TDOA^τ _i,1的噪声，φ为误差向量：

Δr_i＝Δr_i ⁰+cn_ii＝1,…,n

$n_i\&LeftRightArrow;\mathrm{TDOA}{\mathrm{\&tau;}}_{i,1}$

φ＝F-GX_Chan ⁰,其中G⁰X_Chan ⁰＝F⁰

$F^0=\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\&Delta;r}}_1}^0\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({{\mathrm{\&Delta;r}}_n}^0\right)}^2-K_3+K_0\end{array}\right],G^0=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -\mathrm{\&Delta;}{r_1}^0\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_n-x_0& y_n-y_0& z_n-z_0& -{{\mathrm{\&Delta;r}}_n}^0\end{array}\right]$

$\mathrm{\&phi;}=\frac{1}{2}\left[\begin{array}{c}{({{\mathrm{\&Delta;r}}_1}^0+{\mathrm{cn}}_1)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {({{\mathrm{\&Delta;r}}_n}^0+{\mathrm{cn}}_n)}^2-K_3+K_0\end{array}\right]-\{\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\&Delta;r}}_1}^0\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({{\mathrm{\&Delta;r}}_n}^0\right)}^2-K_3+K_0\end{array}\right]+\left[\begin{array}{c}{\mathrm{cn}}_1\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\mathrm{cn}}_n\end{array}\right]r_0\}$

计算结果：

B＝diag{r₁,…,r_n},n＝(n₁,…,n_n)^T

ψ＝E[φφ^T]＝c²BQB

引入几何精度因素X′chan进行初值估计：

${r_i}^{\&prime;}=X_{\mathrm{Chan}}\left(4\right)+{\mathrm{\&Delta;r}}_i\&DoubleRightArrow;B=\mathrm{diag}({r_1}^{\&prime;}...{r_n}^{\&prime;})$

ψ＝c²BQB

X′_Chan＝(G^Tψ^-1G)^-1G·ψ^-1F

利用X′chan估计各分量的关联性，利用r₀进一步精确估算位置，建立方程H=G_αZ：

$H=\left[\begin{array}{c}{\left(X_{\mathrm{Chan}}^{\&prime;}\right[1]-x_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[2]-y_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[3]-z_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[4\left]\right)}^2\end{array}\right],G_a=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right],Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]$

可获得位置估算：

$Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]={\left({G_a}^T{{\mathrm{\&psi;}}_{\mathrm{\&epsiv;}}}^{-1}{G}_a\right)}^{-1}{G}_a\&CenterDot;{{\mathrm{\&psi;}}_{\mathrm{\&epsiv;}}}^{-1}\&CenterDot;H$

定位结果为：

$Z_p=\&PlusMinus;\sqrt{Z}+\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]$ ±取决于X′_chan

Z_p即为Chan算法获得的初始位置。

运用Taylor级数展开法对Chan算法获得的初始位置进行递归运算，每一次递归中通过求解测量误差最小二乘解来逐步逼近真实位置，直到达到所要求的精度门限。

设上一步由Chan算法获得的目标位置初值Z_p为（x_e,y_e,z_e），建立多点定位系统远端第i个副单元的观测方程：

$r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n$ $=r_0+{\mathrm{\&Delta;r}}_i-w_i=r_0+c\&times;{\mathrm{TDOA}}_i-w_i$ w_i为剩余数据

进行Taylor级数展开：

定义:x＝x_e+δ_x,y＝y_e+δ_y,z＝z_e+δ_z

$a_{i1}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_x}{|}_{(x_e,y_e,z_e)},a_{i2}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_y}{|}_{(x_e,y_e,z_e)},a_{i3}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_z}{|}_{(x_e,y_e,z_e)}$

忽略二次和高次项的Taylor级数展开：

$r_i{|}_{x_e,y_e,z_e}=\sqrt{{(x_e-x_i)}^2+{(y_e-y_i)}^2+{(z_e-z_i)}^2}$

$r_i\&ap;r_i{|}_{x_e,y_e,z_e}+a_{i1}{\mathrm{\&delta;}}_x+a_{i2}{\mathrm{\&delta;}}_y+a_{i3}{\mathrm{\&delta;}}_z\&ap;r_0+c\&times;{\mathrm{TDOA}}_i-w_i$ $\&DoubleRightArrow;a_{i1}{\mathrm{\&delta;}}_x+a_{i2}{\mathrm{\&delta;}}_y+a_{i3}{\mathrm{\&delta;}}_z=r_0+c\&times;{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}-w_i$ $\&DoubleRightArrow;\mathrm{A\&delta;}=F-W$

其中δ＝[δ_x,δ_y,δ_z]^T; $F=r_0+c\&times;{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}$

则δ的加权最小二乘估计为：

δ＝(A^TR^-1A)^-1·A^TR^-1·F

其中： $A=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]F=\left[\begin{array}{c}{r}_0+c\&times;{\mathrm{TDOA}}_1-r_1{|}_{x_e,y_e,z_e}\\ r_0+c\&times;{\mathrm{TDOA}}_2-r_2{|}_{x_e,y_e,z_e}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ r_0+c\&times;{\mathrm{TDOA}}_n-r_n{|}_{x_e,y_e,z_e}\end{array}\right]$

R＝E[W^TW],W＝[w₁,w₂,…,w_n]^T

进行下一次递归时，令x′=x+δ_x，y′=y+δ_y，z′=z+δ_z，重复上述过程，直到δ_x，δ_y，δ_z足够小，满足设定门限：

|δ_x|+|δ_y|+|δ_z|＜η  η为设定精度门限值。

则此时的（x′,y′,z′）即为目标的解算位置。

运用自适应Kalman滤波算法对定位信息进行滤波后上报显控系统或指挥系统。

先介绍经典卡尔曼滤波：

假设A和B为系统参数，H为测量系统的参数，X（k|k-1）表示利用上一状态预测的当前状态，X（k-1|k-1）表示上一状态最优结果，U（k）为系统控制量，W（k）为系统过程噪声，P为X的协方差矩阵，A′为A的转置矩阵，Q为W的协方差矩阵，Z(k)为当前状态测量值，Kg为卡尔曼增益，R为U（k）协方差，I为单位矩阵。则它可由下面5个公式表达：

$\left\{\begin{array}{c}X\left(k\right|k-1)=\mathrm{AX}(k-1|k-1)+\mathrm{BU}\left(k\right)+W\left(k\right)\\ P\left(k\right|k-1)=\mathrm{AP}(k-1|k-1)A^{\&prime;}+Q\\ X\left(k\right|k)=X\left(k\right|k-1)+\mathrm{Kg}\left(k\right)[Z\left(k\right)-\mathrm{HX}\left(k\right|k-1)]\\ \mathrm{Kg}\left(k\right)=P\left(k\right|k-1)H^{\&prime;}/[\mathrm{HP}\left(k\right|k-1)H^{\&prime;}+R]\\ P\left(k\right|k)=(I-\mathrm{Kg}\left(k\right)H)P\left(k\right|k-1)\end{array}\right.$

对于非机动目标来说，其运动数学模型可以精确描述的，而对于可以精确描述数学模型的运动目标，经典Kalman滤波可以很好地完成对目标的估计和预测。但多点定位系统工程应用环境中会有部分机动目标，因此需要我们对卡尔曼滤波进行适当改进。本算法中采用一种自适应卡尔曼滤波算法，将机动加速度看成虚拟误差，认为目标仍做匀速运动。则经典的卡尔曼滤波公式可改写为：

$\left\{\begin{array}{c}X\left(k\right|k-1)=\mathrm{AX}(k-1|k-1)+\mathrm{BU}\left(k\right)+[W\left(k\right)+V\left(k\right)]\\ P\left(k\right|k-1)=\mathrm{AP}(k-1|k-1)A^{\&prime;}+Q_{w+v}\\ X\left(k\right|k)=X\left(k\right|k-1)+\mathrm{Kg}\left(k\right)[Z\left(k\right)-\mathrm{HX}\left(k\right|k-1)]\\ \mathrm{Kg}\left(k\right)=P\left(k\right|k-1)H^{\&prime;}/[\mathrm{HP}\left(k\right|k-1)H^{\&prime;}+R]\\ P\left(k\right|k)=(I-\mathrm{Kg}\left(k\right)H)P\left(k\right|k-1)\end{array}\right.$

上式中：V（k）为未知的机动加速度，Q_w+v为[W(k)+V（k）]的协方差。

同时对于机动目标应强调新近机动加速度数据的作用，对于陈旧的数据应渐渐遗忘，这里采用渐消记忆指数加权方法来实现。即在对加速度假设的噪声进行统计估值时，和式中每项乘以不同的加权系数，按指数加权法，选取加权系数β_i使之

${\mathrm{\&beta;}}_k={\mathrm{\&beta;}}_{i}b,0<b<1,\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_i=1$

满βⁱ＝d_kbⁱ

d_k＝(1-b)/(1-b^k-1),i＝0,1，…,k

上式中：b成为遗忘因子，用（1-d_k-1）/(k-1)作为权系数，便得到指数加权渐消记忆的时变噪声统计估值器。

基于上述改进方法，我们进行了仿真实验。图3和图4分别为改进卡尔曼滤波算法和经典卡尔曼滤波算法对机动目标的预测，可见改进卡尔曼滤波算法能够很好的收敛，而经典卡尔曼滤波算法会出现发散现象。

经工程验证，通过本发明方法所获得的目标位置信息能够达到国际民航所要求机场场面目标监控精度，实验结果见图5。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，应当指出的是，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种用于多点定位系统的实时高精度位置解算方法，其特征在于，多点定位系统的远端单元由一个主单元和多个副单元组成，所述解算方法包括以下步骤：

（1）多点定位系统的中心处理单元将接收到的远端单元信息存放到缓存，并判断时间标签一致性，筛选出有效信息；

（2）利用Chan算法对有效信息进行处理，计算出目标定位初值；

（3）将目标定位初值代入Taylor级数展开法，进行递归运算，获取高精度的定位信息；

（4）利用自适应Kalman滤波算法对定位信息进行滤波处理，然后将处理后的定位信息上报至显控系统或指挥系统。

2.根据权利要求1所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，步骤（1）中所述的判断时间标签一致性包括步骤：每隔_△t₁时间，以主单元接收到信息的时间和内容为基准，与其前后_△t₂时间长度内其他副单元接收信息内容进行比对，若有相同信息内容的目标，则作为有效信息用于后续运算，若无相同信息内容的目标，则作为无效信息进行抛弃，每条信息经过_△t₃时间后将进行抛弃。

3.根据权利要求2所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，所述的_△t₁、_△t₂、_△t₃三个时间根据远端单元实际分布情况进行确定。

4.根据权利要求3所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，所述的_△t₁=1s、_△t₂=300ms、_△t₃=3s。

5.根据权利要求1所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，所述的步骤（2）包括以下步骤：

（21）设副单元的空间位置为（x_i,y_i,z_i），主单元为(x₀,y₀,z₀)，（x,y,z）为目标的位置坐标，目标与第i个副单元的距离为r_i，△r_i代表目标到主单元与第i个副单元之间的距离差；则有： $r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n,$ Δr_i＝c×TDOA_i＝r_i-r₀,i＝1,2,...n，上式中c为光速3×10⁸m／s，TDOA_i为目标信息传输到主单元与第i个副单元之间的时间差；

（22）建立观测方程并线性化：

∵

其中f_i＝(Δr_i)²-K_i+K₀,i＝1,2,...n

$K_0=x_0^2+y_0^2+z_0^2,K_i=x_i^2+y_i^2+z_i^2$

（23）运用Chan算法进行初始位置解算：

∵ $(x_i-x_0)x+(y_i-y_0)y+(z_i-z_0)z-{\mathrm{\&Delta;r}}_i{\&CenterDot;}_{}{r}_0=\frac{1}{2}[{\left(\mathrm{\&Delta;}{r}_i\right)}^2-K_i+K_0]$

∴GX_Chan＝F

其中: $X_{\mathrm{Chan}}=\left[\begin{array}{c}x\\ y\\ z\\ r_0\end{array}\right],F=\frac{1}{2}\left[\begin{array}{c}{\left({\mathrm{\&Delta;r}}_i\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({\mathrm{\&Delta;r}}_n\right)}^2-K_n+K_0\end{array}\right]$

$G=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -{\mathrm{\&Delta;r}}_1\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_n-x_0& y_n-y_0& z_n-z_0& -{\mathrm{\&Delta;r}}_n\end{array}\right]$

由于到达时间差存在误差，设n_i为TDOA^τ _i,1的噪声，φ为误差向量：

Δr_i＝Δr_i ⁰+cn_ii＝1,…,n

$n_i\&LeftRightArrow;\mathrm{TDOA}{\mathrm{\&tau;}}_{i,1}$

φ＝F-GX_Chan ⁰,其中G⁰X_Chan ⁰＝F⁰

$F^0=\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\&Delta;r}}_1}^0\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({{\mathrm{\&Delta;r}}_n}^0\right)}^2-K_3+K_0\end{array}\right],G^0=\left[\begin{array}{cccc}{x}_1-x_0& y_1-y_0& z_1-z_0& -\mathrm{\&Delta;}{r_1}^0\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_n-x_0& y_n-y_0& z_n-z_0& -{{\mathrm{\&Delta;r}}_n}^0\end{array}\right]$

$\mathrm{\&phi;}=\frac{1}{2}\left[\begin{array}{c}{({{\mathrm{\&Delta;r}}_1}^0+{\mathrm{cn}}_1)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {({{\mathrm{\&Delta;r}}_n}^0+{\mathrm{cn}}_n)}^2-K_3+K_0\end{array}\right]-\{\frac{1}{2}\left[\begin{array}{c}{\left({{\mathrm{\&Delta;r}}_1}^0\right)}^2-K_1+K_0\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left({{\mathrm{\&Delta;r}}_n}^0\right)}^2-K_3+K_0\end{array}\right]+\left[\begin{array}{c}{\mathrm{cn}}_1\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\mathrm{cn}}_n\end{array}\right]r_0\}$

计算结果：

B＝diag{r₁,…,r_n},n＝(n₁,…,n_n)^T

ψ＝E[φφ^T]＝c²BQB

（24）引入几何精度因素X′chan进行初值估计：

${r_i}^{\&prime;}=X_{\mathrm{Chan}}\left(4\right)+{\mathrm{\&Delta;r}}_i\&DoubleRightArrow;B=\mathrm{diag}({r_1}^{\&prime;}...{r_n}^{\&prime;})$

ψ＝c²BQB

X′_Chan＝(G^Tψ^-1G)^-1G·ψ^-1F

利用X′chan估计各分量的关联性，利用r₀进一步精确估算位置，建立方程H=G_αZ：

$H=\left[\begin{array}{c}{\left(X_{\mathrm{Chan}}^{\&prime;}\right[1]-x_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[2]-y_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[3]-z_0)}^2\\ {\left(X_{\mathrm{Chan}}^{\&prime;}\right[4\left]\right)}^2\end{array}\right],G_a=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right],Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]$

可获得位置估算：

$Z=\left[\begin{array}{c}{(x-x_0)}^2\\ {(y-y_0)}^2\\ {(z-z_0)}^2\end{array}\right]={\left({G_a}^T{{\mathrm{\&psi;}}_{\mathrm{\&epsiv;}}}^{-1}{G}_a\right)}^{-1}{G}_a\&CenterDot;{{\mathrm{\&psi;}}_{\mathrm{\&epsiv;}}}^{-1}\&CenterDot;H$

定位结果为：

$Z_p=\&PlusMinus;\sqrt{Z}+\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]$ ±取决于X′_chan

Z_p即为Chan算法获得的目标定位初值。

6.根据权利要求1所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，所述的步骤（3）包括以下步骤：运用Taylor级数展开法对Chan算法获得的目标定位初值进行递归运算，每一次递归中通过求解测量误差最小二乘解来逐步逼近真实位置，直到达到所要求的精度门限；设上一步由Chan算法获得的目标定位初值Z_p为（x_e,y_e,z_e），建立多点定位系统远端第i个副单元的观测方程：

$r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},i=\mathrm{0,1,2},...n$ $=r_0+{\mathrm{\&Delta;r}}_i-w_i=r_0+c\&times;{\mathrm{TDOA}}_i-w_i$ w_i为剩余数据

进行Taylor级数展开：

定义:x＝x_e+δ_x,y＝y_e+δ_y,z＝z_e+δ_z

$a_{i1}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_x}{|}_{(x_e,y_e,z_e)},a_{i2}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_y}{|}_{(x_e,y_e,z_e)},a_{i3}=\frac{{\&PartialD;r}_i}{{\&PartialD;}_z}{|}_{(x_e,y_e,z_e)}$

忽略二次和高次项的Taylor级数展开：

$r_i{|}_{x_e,y_e,z_e}=\sqrt{{(x_e-x_i)}^2+{(y_e-y_i)}^2+{(z_e-z_i)}^2}$

$r_i\&ap;r_i{|}_{x_e,y_e,z_e}+a_{i1}{\mathrm{\&delta;}}_x+a_{i2}{\mathrm{\&delta;}}_y+a_{i3}{\mathrm{\&delta;}}_z\&ap;r_0+c\&times;{\mathrm{TDOA}}_i-w_i$ $\&DoubleRightArrow;a_{i1}{\mathrm{\&delta;}}_x+a_{i2}{\mathrm{\&delta;}}_y+a_{i3}{\mathrm{\&delta;}}_z=r_0+c\&times;{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}-w_i$ $\&DoubleRightArrow;\mathrm{A\&delta;}=F-W$

其中δ＝[δ_x,δ_y,δ_z]^T; $F=r_0+c\&times;{\mathrm{TDOA}}_i-r_i{|}_{x_e,y_e,z_e}$

则δ的加权最小二乘估计为：

δ＝(A^TR^-1A)^-1·A^TR^-1·F

其中： $A=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]F=\left[\begin{array}{c}{r}_0+c\&times;{\mathrm{TDOA}}_1-r_1{|}_{x_e,y_e,z_e}\\ r_0+c\&times;{\mathrm{TDOA}}_2-r_2{|}_{x_e,y_e,z_e}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ r_0+c\&times;{\mathrm{TDOA}}_n-r_n{|}_{x_e,y_e,z_e}\end{array}\right]$

R＝E[W^TW],W＝[w₁,w₂,…,w_n]^T

进行下一次递归时，令x′=x+δ_x，y′=y+δ_y，z′=z+δ_z，重复上述过程，直到δ_x，δ_y，δ_z足够小，满足设定门限：

|δ_x|+|δ_y|+|δ_z|＜η  η为设定精度门限值，

则此时的（x′,y′,z′）即为目标的解算位置。

7.根据权利要求1所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，步骤（4）中所述的自适应Kalman滤波算法为

$\left\{\begin{array}{c}X\left(k\right|k-1)=\mathrm{AX}(k-1|k-1)+\mathrm{BU}\left(k\right)+[W\left(k\right)+V\left(k\right)]\\ P\left(k\right|k-1)=\mathrm{AP}(k-1|k-1)A^{\&prime;}+Q_{w+v}\\ X\left(k\right|k)=X\left(k\right|k-1)+\mathrm{Kg}\left(k\right)[Z\left(k\right)-\mathrm{HX}\left(k\right|k-1)]\\ \mathrm{Kg}\left(k\right)=P\left(k\right|k-1)H^{\&prime;}/[\mathrm{HP}\left(k\right|k-1)H^{\&prime;}+R]\\ P\left(k\right|k)=(I-\mathrm{Kg}\left(k\right)H)P\left(k\right|k-1)\end{array}\right.$

其中，A和B为系统参数，H为测量系统的参数，X（k|k-1）表示利用上一状态预测的当前状态，X（k-1|k-1）表示上一状态最优结果，U（k）为系统控制量，W（k）为系统过程噪声，P为X的协方差矩阵，A′为A的转置矩阵，Q为W的协方差矩阵，Z(k)为当前状态测量值，Kg为卡尔曼增益，R为U（k）协方差，I为单位矩阵，V（k）为未知的机动加速度，Q_w+v为[W(k)+V（k）]的协方差。

8.根据权利要求7所述的一种用于多点定位系统的实时高精度位置解算方法，其特征在于，所述的步骤（4）还包括步骤：在对加速度假设的噪声进行统计估值时，和式中每项乘以不同的加权系数，按指数加权法，选取加权系数β_i使之满

${\mathrm{\&beta;}}_k={\mathrm{\&beta;}}_{i}b,0<b<1,\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_i=1$

β_i＝d_kbⁱ

d_k＝(1-b)/(1-b^k-1),i＝0,1,…,k

上式中：b成为遗忘因子，用（1-d_k-1）/(k-1)作为权系数，便得到指数加权渐消记忆的时变噪声统计估值器。