基于最小二乘算法雷達(dá)定位誤差配準(zhǔn)方法研究*

裴 楊 王 碩

（92941部隊(duì) 葫蘆島 125000）

1 引言

基于三坐標(biāo)雷達(dá)（three-dimension radar）的無(wú)人機(jī)目標(biāo)定位與跟蹤，是典型的單站測(cè)角測(cè)距問(wèn)題，其目標(biāo)指示精度，與無(wú)人機(jī)的姿態(tài)角密切相關(guān)，而無(wú)人平臺(tái)的位姿估計(jì)一直是行業(yè)內(nèi)研究的重點(diǎn)和難點(diǎn)。通常情況下，機(jī)載雷達(dá)在進(jìn)行目標(biāo)定位與跟蹤時(shí)，會(huì)因?yàn)椴淮_定因素，尤其是實(shí)時(shí)系統(tǒng)的數(shù)據(jù)采集，在數(shù)據(jù)輸入和輸出過(guò)程中，都會(huì)引入噪聲。而對(duì)誤差的估計(jì)并非絕對(duì)，只能通過(guò)計(jì)算，將誤差以較大概率落在某一范圍內(nèi)。最小二乘濾波是一種經(jīng)典的濾波方法，尤其是應(yīng)對(duì)載荷測(cè)量精度有限的情況。

常用的誤差配準(zhǔn)算法有實(shí)時(shí)質(zhì)量控制算法、最小二乘算法、廣義最小二乘算法、最大似然算法等。胡雷［1］提出了采用最小二乘算法對(duì)單平臺(tái)級(jí)、多平臺(tái)級(jí)的多傳感器進(jìn)行空間配準(zhǔn)的辦法，證明了最小二乘在誤差配準(zhǔn)時(shí)較其他算法有精確和快速的優(yōu)勢(shì)。李家強(qiáng)［2］在傳統(tǒng)廣義最小二乘算法的基礎(chǔ)上進(jìn)行了改進(jìn)，以收斂步數(shù)作為量測(cè)精度的加權(quán)因子，提高了目標(biāo)定位與跟蹤的準(zhǔn)確度。文獻(xiàn)［3～8］對(duì)卡爾曼濾波和最小二乘算法進(jìn)行了比較，并運(yùn)用于微距視覺(jué)圓目標(biāo)定位、無(wú)源協(xié)同定位等領(lǐng)域，證明了最小二乘算法具有更好的魯棒性和準(zhǔn)確性。文獻(xiàn)［9～12］分析了傳感器的掛載平臺(tái)指向誤差組成和分析方法，明確了傳感器的指向誤差合成，奠定了本文研究的基礎(chǔ)。

2 基于濾波算法的誤差配準(zhǔn)方法研究

2.1 傳感器量測(cè)誤差分析

傳感器的目標(biāo)探測(cè)原理如圖1 所示，P 表示目標(biāo)實(shí)際位置，xyz-O坐標(biāo)系表示無(wú)人機(jī)航跡坐標(biāo)系系。nv-o坐標(biāo)系表示激光成像雷達(dá)生成的圖像坐標(biāo)系，p點(diǎn)表示目標(biāo)在圖像中的位置，通過(guò)目 標(biāo) 在系下的球坐標(biāo)用NVP(dVP，βVP，εVP) 表示。因此，目標(biāo)P 相對(duì)于無(wú)人機(jī)的直角坐標(biāo)NVP(xVP，yVP，zVP)可以表示為

圖1 三坐標(biāo)雷達(dá)探測(cè)原理

式中，βVP、εVP分別表示目標(biāo)與無(wú)人機(jī)之間的方向角和高低角測(cè)量值，目標(biāo)在無(wú)人機(jī)航向右側(cè)為正，（-π/2 ＜β＜π/2 ，-π/2 ＜ε＜π/2）。此時(shí)，無(wú)人機(jī)計(jì)算得到的目標(biāo)位置坐標(biāo)是在沒(méi)有姿態(tài)誤差情況下得到的。當(dāng)無(wú)人機(jī)存在姿態(tài)誤差時(shí)，角度的真實(shí)值為

其中，(ψVP，θVP，φVP) 表示無(wú)人機(jī)的初始姿態(tài)角誤差，無(wú)人機(jī)對(duì)地面物體進(jìn)行測(cè)量的數(shù)據(jù)主要受到有效載荷的量測(cè)誤差的影響。

2.2 載荷測(cè)量誤差

除環(huán)境因素引起的隨機(jī)誤差外，傳感器平臺(tái)的對(duì)準(zhǔn)誤差是載荷角度誤差的主要組成部分。在此所提的載荷對(duì)準(zhǔn)誤差并不是指吊艙平臺(tái)的安裝誤差，而是由于無(wú)人機(jī)慣性導(dǎo)航設(shè)備測(cè)量的姿態(tài)角誤差對(duì)傳感器平臺(tái)帶來(lái)的測(cè)量誤差。

1）載荷自身的系統(tǒng)誤差

有效載荷在安裝時(shí)都需要進(jìn)行角度的校準(zhǔn)，但由于飛行過(guò)程中的氣流、機(jī)械震動(dòng)等因素，在飛行至目標(biāo)區(qū)域時(shí)，吊艙和機(jī)體之間的軸線對(duì)準(zhǔn)會(huì)發(fā)生偏移，因此，載荷對(duì)目標(biāo)進(jìn)行測(cè)量的方位角β和高低角ε可由式和表示為

其中，(βk，εk)分別為方位角和高低角的載荷測(cè)量值，(Δβ，Δε)為對(duì)應(yīng)的測(cè)量誤差，ΔαC為云臺(tái)反饋的對(duì)準(zhǔn)誤差，于是(βk+Δβ+ΔαC，εk+Δε+ΔαC)就構(gòu)成了方位角、高低角的真實(shí)值，(δ1，δ2)為隨機(jī)誤差。

2）載荷安裝對(duì)準(zhǔn)誤差

載荷安裝過(guò)程中，必須要與無(wú)人機(jī)基座保持完整耦合，并保證絕對(duì)平行，但實(shí)際上是無(wú)法達(dá)到零誤差的安裝，在理想情況下，載荷角度測(cè)量的基準(zhǔn)坐標(biāo)軸系OX1Y1Z1應(yīng)與機(jī)動(dòng)平臺(tái)的坐標(biāo)軸系OX0Y0Z0完全重合，但由于姿態(tài)角誤差，在測(cè)量時(shí)會(huì)出現(xiàn)如圖2所示的傾角。

圖2 三坐標(biāo)雷達(dá)安裝對(duì)準(zhǔn)誤差

載荷安裝對(duì)準(zhǔn)誤差是由于內(nèi)部傳感器的不穩(wěn)定造成的，也就是慣性單元對(duì)無(wú)人機(jī)的姿態(tài)角測(cè)量誤差。

2.3 基于卡爾曼濾波的載荷角度誤差修正

無(wú)人機(jī)對(duì)陸目標(biāo)進(jìn)行定位與跟蹤模型中，采用的是“東-北-天”坐標(biāo)系，下面將采用標(biāo)準(zhǔn)卡爾曼濾波（Kalman Filter，KF）對(duì)目標(biāo)定位與跟蹤的誤差進(jìn)行分析和修正。

1）狀態(tài)方程

從無(wú)人機(jī)對(duì)地面物體的測(cè)量數(shù)據(jù)轉(zhuǎn)換到系中的坐標(biāo)為

當(dāng)無(wú)人機(jī)的姿態(tài)角為(ψ，θ，φ)時(shí)，從系向CV系的轉(zhuǎn)換矩陣為

設(shè)無(wú)人機(jī)的經(jīng)緯度和高度參數(shù)為(Lv，λv，Hv)，Hv表示無(wú)人機(jī)的海拔高度，因此，從CV系向CW系的旋轉(zhuǎn)矩陣和平移矩陣分別表示為

式中N表示無(wú)人機(jī)所在位置的地球曲率半徑，e為地球偏心率。因此，地面物體通過(guò)無(wú)人機(jī)的測(cè)量后，可以轉(zhuǎn)換到地理坐標(biāo)系中的位置為

其中M'是無(wú)人機(jī)的姿態(tài)測(cè)量誤差矩陣：

2）卡爾曼濾波

無(wú)人機(jī)對(duì)陸目標(biāo)指示是以直角坐標(biāo)進(jìn)行表示，提前假設(shè)無(wú)人機(jī)的姿態(tài)誤差對(duì)目標(biāo)定位精度的影響是一個(gè)線性系統(tǒng)，可以表示為

其中，xk∈Rn表示k時(shí)刻的狀態(tài)向量，f為n維向量函數(shù)，zk∈Rm為k時(shí)刻系統(tǒng)的觀測(cè)向量，h為m維向量函數(shù)，wk為均值為0、協(xié)方差為Qk的n維隨機(jī)過(guò)程噪聲，vk為均值為0、協(xié)方差為Rk的m維隨機(jī)量測(cè)噪聲，且wk、vk互不相關(guān)。

（1）預(yù)測(cè)狀態(tài)和修正狀態(tài)

其中K表示卡爾曼增益矩陣。

（2）時(shí)間更新

（3）狀態(tài)更新

2.4 基于最小二乘法的載荷角度誤差修正

基于三坐標(biāo)雷達(dá)的無(wú)人機(jī)對(duì)陸目標(biāo)指示需要對(duì)吊艙拍攝角度以及無(wú)人機(jī)姿態(tài)角度進(jìn)行測(cè)量，在這里假設(shè)無(wú)人機(jī)的姿態(tài)誤差對(duì)目標(biāo)定位精度的影響是一個(gè)非線性系統(tǒng)，在此將對(duì)這些參數(shù)的誤差進(jìn)行最小二乘濾波修正。

根據(jù)式（2）、（6）和式（7）可知，關(guān)于無(wú)人機(jī)對(duì)陸目標(biāo)定位與跟蹤問(wèn)題需要測(cè)量的確定性參數(shù)包括：與吊艙相關(guān)的距離、方向角、高低角的數(shù)據(jù)(d，β，ε)，與無(wú)人機(jī)相關(guān)的姿態(tài)數(shù)據(jù)(ψ，θ，φ)。其中，d表示無(wú)人機(jī)與目標(biāo)之間的真實(shí)距離，β和ε表示吊艙拍攝時(shí)的方向角和高低角，(ψ，θ，φ)表示無(wú)人機(jī)偏航角-俯仰角-橫滾角的真值。

與之對(duì)應(yīng)地，需要進(jìn)行誤差最優(yōu)估計(jì)的包括：無(wú)人機(jī)與目標(biāo)距離測(cè)量值Δd，無(wú)人機(jī)偏航角誤差Δψ，無(wú)人機(jī)俯仰角誤差Δθ，無(wú)人機(jī)橫滾角誤差Δφ，(Δψ，Δθ，Δφ)也就是載荷對(duì)準(zhǔn)誤差。根據(jù)坐標(biāo)轉(zhuǎn)換，無(wú)人機(jī)與目標(biāo)之間的距離要從測(cè)量值Δd向真實(shí)值d轉(zhuǎn)換，即從無(wú)人機(jī)航跡坐標(biāo)系向無(wú)人機(jī)載機(jī)坐標(biāo)系轉(zhuǎn)換，可以通過(guò)下式表示：

其中：

(ψt，θt，φt)表示在t時(shí)刻無(wú)人機(jī)姿態(tài)角的測(cè)量值。一般說(shuō)來(lái)，姿態(tài)角誤差(Δψ，Δθ，Δφ)比較小，因此其正弦值約等于本身，余弦約等于1，且在同一時(shí)刻下的姿態(tài)角誤差相互垂直，所以有：

對(duì)式（16）求解，矩陣A3×3中的各元素為

式（16）可以簡(jiǎn)寫(xiě)為

從而，無(wú)人機(jī)與目標(biāo)之間的關(guān)系(β，ε)的測(cè)量值可以通過(guò)式（21）表示：

最終解得：

根據(jù)最小二乘濾波的原則，針對(duì)本問(wèn)題的核心在于求解誤差最小的方向角和高低角，通過(guò)k次測(cè)量，對(duì)方向角β和高低角ε建立極小化目標(biāo)函數(shù)：

問(wèn)題可以轉(zhuǎn)換為通過(guò)求極值的方法對(duì)無(wú)人機(jī)的姿態(tài)角誤差進(jìn)行最優(yōu)估計(jì)，對(duì)式（23）進(jìn)行目標(biāo)函數(shù)的偏導(dǎo)求解：

整理后結(jié)果可用式（25）表示：

其中：

令D= |B|，有：

因此，無(wú)人機(jī)的姿態(tài)角誤差為

3 仿真計(jì)算

設(shè)置初始條件，無(wú)人機(jī)所在位置坐標(biāo)為（28853，27163，5000），設(shè)置目標(biāo)運(yùn)動(dòng)路徑：初始坐標(biāo)為（32913，18703），經(jīng)過(guò)370s 的勻速直線運(yùn)動(dòng)以后到達(dá)（32913，20383）；以（0.75，0.75）m/s2的加速度運(yùn)動(dòng)30s，達(dá)到（32935，20406）；以初速度為（3.22，4）m/s，加速度為（0，0.04）m/s2運(yùn)動(dòng)200s；以（10，-15）m/s 進(jìn)行減速，10s 后，以（0，-15）m/s 的初速和（-0.075，-0.075）m/s2的加速度繼續(xù)運(yùn)動(dòng)50s，并分別采用卡爾曼濾波和最小二乘濾波對(duì)測(cè)量結(jié)果進(jìn)行修正。光電跟蹤傳感器的距離探測(cè)誤差為50m，角度探測(cè)誤差為0.2°。

仿真結(jié)果如下：

對(duì)目標(biāo)進(jìn)行跟蹤的結(jié)果圖3（a）所示，采用濾波算法對(duì)跟蹤結(jié)果進(jìn)行誤差修正結(jié)果如圖3（b）所示。

圖3 目標(biāo)定位與跟蹤

4 結(jié)語(yǔ)

通過(guò)對(duì)比不同條件下無(wú)人機(jī)目標(biāo)定位與跟蹤精度，可以得到如下結(jié)論：

1）采用濾波算法對(duì)目標(biāo)跟蹤數(shù)據(jù)進(jìn)行修正，能夠明顯提高目標(biāo)定位與跟蹤精度；

2）目標(biāo)定位與跟蹤精度在一定程度上仍然會(huì)受到距離和角度測(cè)量誤差的影響，尤其是測(cè)量角度誤差，距離越遠(yuǎn)，對(duì)目標(biāo)定位精度影響越大。

研究結(jié)果表明：對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤時(shí)，最小二乘濾波比卡爾曼濾波具有更好的收斂性，但是這兩種濾波算法對(duì)目標(biāo)在進(jìn)行加速運(yùn)動(dòng)時(shí)跟蹤效果都不理想，特別是當(dāng)目標(biāo)進(jìn)行大機(jī)動(dòng)時(shí)，位置偏移誤差較大。單純通過(guò)算法仍然無(wú)法提高目標(biāo)定位精度的數(shù)量級(jí)，需要采用多站點(diǎn)、多角度等雷達(dá)組網(wǎng)方法進(jìn)行目標(biāo)定位與跟蹤。