[发明专利]一种多源测距下联合目标定位与传感器配准方法

权利要求书

1.一种多源测距下联合目标定位与传感器配准方法，其特征在于，包括以下步骤：

步骤1，初始化测距传感器系统偏差估计值β⁰为零，表示初始化时第n个测距传感器的系统偏差估计值；

步骤2，在l-1次迭代得到的系统偏差估计值β^l-1下，l为当前进行的迭代次数，计算各测距传感器量测时刻k的目标初始位置X⁰(k)，表示第n个测距传感器量测时刻k的目标初始位置；

步骤3，求取本次迭代的系统偏差估计β^l：利用步骤2得到的目标初始位置计算得到其本次迭代的系统偏差估计值；

步骤4，判断系统偏差估计值是否收敛，即|β^l-β^l-1|≤ε，ε为一较小的正实数，如果收敛，各个测距传感器的系统偏差估计为β＝[β₁ β₂ … β_n]^T，执行步骤5，β_n表示第n个测距传感器的系统偏差估计值；如果没有收敛，返回步骤2；

步骤5，求取目标位置：利用收敛的系统偏差估计β，计算得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

2.根据权利要求1所述的方法，其特征在于，步骤2包括如下步骤：

步骤2-1，测距传感器的量测模型如下：

z(k)＝h(x(k))+β+w(k)(1)

其中，k＝1,2,…,N，N表示各测距传感器时间采样总数，且量测时间同步，第k时刻测距传感器的量测为z(k)，第k时刻第n个测距传感器的量测信息为径向距离r_n(k)，z(k)＝[z₁(k) z₂(k) … z_n(k)]^T＝[r₁(k) r₂(k) … r_n(k)]^T，h(·)是已知的非线性量测方程，x(k)为第k时刻测距传感器获取的目标真实的位置状态，β为各测距传感器的距离系统偏差，β＝[β₁ β₂,…,β_n]，β_n表示第n个测距传感器的距离系统偏差，w(k)为第k时刻测距传感器零均值的高斯噪声，其方差矩阵为n表示第n个测距传感器，各测距传感器的量测噪声均独立；

步骤2-2，通过各测距传感器的量测信息Z求解测距传感器的测距系统偏差，对量测进行误差消除，Z＝[z(k)；k＝1,…,N]，从而得到对目标位置的正确估计：通过最大化似然函数p(Z|X,β)来联合估计距离系统偏差β和目标位置X＝{x(k)；k＝1,…,N}：

{X^,β^}=argmaxX,βp(z(1),z(2)...,z(N)|X,β)=argmaxβ{Πk=1Nmax p(z(k)|x(k),β)}---(2)]]>

根据各测距传感器之间的量测噪声相互独立的假设，省去时间标号k，得到：

p(z1,z2,...,zn|x,β)=Πi=1np(zi|x(k),βi)=K1exp{-12Σi=1n(zi-z‾)TΣzi-1(zi-z‾)}---(3)]]>

式中，K₁表示简化后一常量，z_n表示第n个测距传感器的量测，x(k)表示k时刻目标位置，在假设已知第i个测距传感器的距离系统偏差β_i的条件下，把第i个测距传感器的量测z_i投影到目标状态空间：

xi=hi-1(zi-βi),i=1,2,...,n---(4);]]>

x_i表示在第i个测距传感器的量测下目标的状态空间位置；

步骤2-3，量测z_i的量测方程是与为测距传感器i在笛卡尔坐标下的位置，x₁(k)与x₂(k)为目标在笛卡尔坐标下的位置，由式(4)得知x_i是高斯随机变量通过非线性系统得到的随机变量，通过一阶泰勒线性化近似展开得到其概率密度函数近似表达式，其协方差的逆为：

Σxi-1=HiTΣzi-1Hi---(5)]]>

其中，

H_i为h_i(·)在x处的雅可比矩阵，此时似然函数即公式(3)在目标状态空间下近似为：

p(z1,z2,...,zn|x,β)≈K2exp{-12Σi=1n(x-xi)TΣxi-1(x-xi)}=K2exp{-12(xT[Σi=1nΣxi-1]x-2xT[Σi=1nΣxi-1xi]x+[Σi=1nxiTΣxi-1xi])}---(7)]]>

K₂表示化简过程中的常量；

使用矩阵运算公式：

y^TAy-2y^TB+B^TA^-1B＝(y-A^-1B)A(y-A^-1B)(8)

其中，B为矩阵，A为对称矩阵，式(7)重写为：

p(z1,z2,...,zn|x,β)≈K2exp{-12(x-x^)T[Σi=1nΣxi-1](x-x^)-12([Σi=1nxiTΣxi-1xi]-[Σi=1nΣxi-1xi]T×[Σi=1nΣxi-1]-1[Σi=1nΣxi-1xi])}---(9)]]>

其中，

步骤2-4，当式(9)中的第一项为零时，即时似然函数存在最大值，式(10)是目标在k时刻状态的最大似然估计，也是此时刻下不同测距传感器量测的融合结果，式(9)中包含了距离系统偏差β，式(9)中的第二项重写为：

[Σi=1nxiTΣxi-1xi]-[Σi=1nΣxi-1xi]T[Σi=1nΣxi-1]-1[Σi=1nΣxi-1xi]=XT(k)Σ-1(k)X(k)---(11)]]>

其中，表示k时刻第n个传感器对目标位置的估计，且：

Σ-1(k)=block-diagΣx1-1(k)Σx2-1(k)...Σxn-1(k)-[{Σxi-1(k)[Σl=1nΣxl-1(k)]-1Σxj-1(k)}ij]---(12)]]>

block-diag表示为括号内各方块矩阵组成的块对角矩阵，{·}_ij表示n×n矩阵中的子矩阵，其中i,j＝1,2,…,n；

步骤2-5，将代入式(9)得到：

p(z(k)|x^(k),β)=K exp{-12XT(k)Σ-1(k)X(k)}---(13)]]>

其中，K是归一化常量，X⁰(k)表示测距传感器在量测时刻k的目标初始位置。