光電有源定位法精度分析及提升方法研究

黃小帥 王增發(fā)

（中國科學院長春光學精密機械與物理研究所 航空成像與測量技術(shù)研究一部 吉林省長春市 130033）

目前空中平臺對目標的定位方法主要分為兩種體制，一種是電子/雷達定位法，另一種是光電定位的方法。前者利用 的平臺位置信息是通過目標和測試平臺的電磁波相位、頻率等物理量解算的距離和利用方向參數(shù)[1]方位信息進行定位[2]，這些物理量本身分辨力和精度很高，能夠達到mm 甚至μm 數(shù)量級，所以解算出來的目標位置信息精度較高；光電定位法主要是依據(jù)測試平臺位置信息、姿態(tài)信息以及相對目標的距離信息等物理量進行解算，得到目標的經(jīng)度、緯度和高度的位置信息，這些物理量本身精度和分辨力相對低一些，角度誤差和測距誤差對10km 外的目標折算到距離，誤差能夠達到m 數(shù)量級。與電子定位方法的誤差相比光電定位的誤差是由測量方法導致的系統(tǒng)誤差，這是兩種測量體制帶來的固有誤差，只能不斷優(yōu)化定位策略來降低誤差影響大的物理量對結(jié)果精度的影響，并通過增加一定測量次數(shù)來降低隨機誤差對定位結(jié)果的影響。而這個優(yōu)化的增加測量次數(shù)的過程勢必會犧牲算法效率和實時性，所以光電定位的“快和準”是無法同時滿足的矛盾體。

1 光電定位體系

在光電定位的分類中，按照直接測距法和間接測距法把光電定位分為主動法定位和被動法定位[3]。前者平臺主動向目標輻射激光源，利用激光雷達等設(shè)備對目標進行直接測距[4]，再利用這個測距值結(jié)合其他測量參數(shù)解算目標位置信息；后者無法直接獲得平臺相對目標的距離信息，而是通過多航跡點和圖像匹配等方法[5]間接獲得目標的距離信息，再通過與主動法定位類似的算法解算目標的位置信息。因為主動法和被動法本質(zhì)區(qū)別就是在探測平臺上是否對目標主動輻射能量來直接測得平臺和目標的相對距離，所以也可稱為有源法定位和無源法定位。

除了獲取目標距離信息的方法有所不同，兩種定位法的光電定位的核心都為坐標轉(zhuǎn)換。目標定位的結(jié)果就是確定目標在一個統(tǒng)一坐標系中的空間位置，想要得到目標的經(jīng)度、緯度和高度，相當于用已知的條件和測量到的參數(shù)列矩陣方程進行求解，最終得到目標的位置信息。這些參數(shù)是通過不同平臺的測量數(shù)據(jù)，每個平臺參考坐標系不同，需要將它們最終統(tǒng)一到同一個坐標系下，所以定位的核心為坐標轉(zhuǎn)換。下面以激光測距定位法作為定位核心算法，推導這個過程。

2 光電有源定位法推導和精度分析

2.1 坐標系的引入

（1）WGS-84 大地坐標系[6]：以參考橢球的中心O 為原點，橢球旋轉(zhuǎn)軸ON 位北極，并且位大地坐標系的Z 軸。

（2）地球坐標系[7]：地球坐標系又稱為空間直角坐標系，原點在地球中心，Z 軸與地球自轉(zhuǎn)軸重合，X、Y 軸互相垂直并固定于赤道面上。

（3）地理坐標系[8]：以定位站為圓心，X軸沿原點緯線切線向東，Y 軸沿原點經(jīng)線切線向北，Z 軸垂直于原點所在平面指向天頂。

圖1：光電定位流程圖

圖2：各項參數(shù)的真值

圖3：地球相關(guān)參數(shù)的理論真值

（4）載機坐標系[9]：原點在無人機質(zhì)心位置，X軸位無人機航向，Y 軸指向右翼。

（5）光電平臺坐標系：與載機坐標系為同一坐標系，當剛性安裝到飛機上時，會因為安裝誤差產(chǎn)生微小的旋轉(zhuǎn)和偏移。

為了計算方便，以上坐標系均采用右手坐標系。

2.2 定位方程的推導

在進行定位方程推導之前，需要首先推導坐標轉(zhuǎn)換的矩陣變換方程作為先驗知識。以二維坐標系為例，三維右手坐標系是二維坐標系的推廣。平面直角坐標系xoy 經(jīng)過逆時針旋轉(zhuǎn)θ 得到x′oy′坐標系，通過平面上任意一點P 推導坐標轉(zhuǎn)換的方程。

用極坐標法能夠很好的反映坐標旋轉(zhuǎn)關(guān)系，P 點在原坐標系上表示為P(x0,y0)。

其中R 為P 點到原點的歐氏距離。在x′oy′坐標系上表示為P(x'0,y'0)。

則將（1）式代入（2）式，并轉(zhuǎn)換為矩陣相乘的形式可得：

從二維坐標系可推廣到三維坐標系。為了計算方便，在坐標轉(zhuǎn)換過程需要將矩陣增加一維將線性空間的笛卡爾坐標系轉(zhuǎn)換到仿射空間的齊次坐標系，這樣能夠方便用矩陣的乘法表示坐標的旋轉(zhuǎn)、平移和縮放。通過這些先驗知識對光電定位的模型進行推導。

光電定位流程如圖 1 所示。

當光電平臺發(fā)現(xiàn)目標時，轉(zhuǎn)動平臺保持目標在圖像中心，打開激光雷達進行激光測距，得到此時平臺相對于零位的俯仰角α（水平為0，上正下負）和方位角λ 以及測距值R。則目標在光電平臺坐標系下的坐標值為：

齊次坐標法通過仿射空間從平臺坐標系轉(zhuǎn)換到大地坐標系如下：

其中js、ws是飛機經(jīng)緯值。其中Xoffset、Yoffset、Zoffset見公式（7）：

其中RNx見公式（8）：

其中a 是地球長半軸，b 是地球短半軸，e 是地球第一偏心率。

其中，Δφba、Δθba、Δψba分別為減振器的橫滾角、俯仰角和偏航角。當光電平臺通過減振器與載機剛性連接時，Δφba、Δθba、Δψba表示固定的安裝誤差，能夠比較準確的測量出來，可以通過坐標轉(zhuǎn)換修訂；當光電平臺與載機為柔性連接時，可在光電平臺內(nèi)安裝子慣性測量單元（Inertial Measurement Unit，以下簡稱IMU），則認為為單位矩陣E，具體的論述會在第3 章展開，這里采用子IMU 的方法進行計算。

最后，地球坐標系轉(zhuǎn)換為大地坐標系，求出目標位于大地坐標系中的坐標值 (B,L,H) 。

其中，B 是大地緯度，RN是地球卯酉圈曲率半徑。

若由地球坐標系轉(zhuǎn)換為大地坐標系，則近似的數(shù)學關(guān)系為：

當然未來得到更加精確的位置信息，則可以通過迭代法取得精確的結(jié)果。

直到：

其中ε1、ε2，根據(jù)要求的精度設(shè)定，經(jīng)過多次仿真發(fā)現(xiàn)，10-6的精度一般需要4~5 次就能實現(xiàn)。迭代初值為：

圖4：飛機位置誤差分布直方圖

圖5：定位誤差分布仿真圖

最終經(jīng)過上述過程解算得到目標的經(jīng)度、緯度和高度值的位置信息。

2.3 定位精度分析

光電定位中所有參數(shù)都是測量值，在測量過程中無法避免的會產(chǎn)生誤差，影響最終定位精度。首先根據(jù)上一節(jié)定位方程推導結(jié)果分析一下定位所需要的參數(shù)及其誤差來源。

（1）光電平臺測得的方位角和俯仰角。這兩個角度值是由平臺角度傳感器測得，根據(jù)當前角度測量技術(shù)，采用16 位角度編碼器測角，分辨力達19.77″，測角精度σ1=σ2=0.05°實驗室測得為正態(tài)分布；

（2）光電平臺測得目標到平臺的直線距離。由激光測距機測得，實驗室測得誤差呈正態(tài)分布，精度σ3=5m@25Km；

（3）飛機姿態(tài)角，包括飛機橫滾角、俯仰角和偏航角，需要分情況討論。上文提到，當光電平臺與飛機剛性連接時，飛機姿態(tài)角由飛機主IMU 測得；而當光電平臺內(nèi)部有一個與主IMU 捷聯(lián)的子IMU 時，平臺內(nèi)子IMU 可以直接解算飛機的位置信息和姿態(tài)信息。通過自動校靶[10]，能夠通過飛機主IMU 對平臺子IMU 陀螺漂移進行補償，保證平臺子IMU 姿態(tài)信息測量精度的穩(wěn)定性。實驗室測得為正態(tài)分布，其中俯仰角和橫滾角的精度σ4=σ5=0.05°，航向角精度σ6=0.1°。

（4）飛機的位置信息由飛機GPS/北斗信號獲得。通過三星定位原理在飛機上安裝定位信號接收機，進行飛機實時定位。其中位置結(jié)果通過經(jīng)度緯度和高度體現(xiàn)，實驗室測得為正態(tài)分布，經(jīng)緯度的精度σ7=σ8=0.0001°，高度的精度σ9=10m。

這些測量參數(shù)來源不同，相互獨立，正是通過定位方程進行傳遞，影響最終目標的定位精度。而最終定位精度的計算方法主要有兩種方法：直接法和間接法。直接法可以需要將搭建相關(guān)平臺，對地面上精準位置的目標進行多次定位，而要想通過定位測量值與真值比較，根據(jù)誤差理論，需要進行200~1000 次等精度測量試驗才能得到定位誤差的概率分布函數(shù)，實現(xiàn)難度大；間接法通過多元定位方程，通過飛行誤差傳遞的過程，得到每一項參數(shù)的誤差對定位精度的影響；既可以通過全微分法分析偏差和標準差結(jié)果，也可以通過計算機模擬的方法進行分析，通過分析每個環(huán)節(jié)的誤差得到最終的定位誤差，本文采用間接法對定位精度進行分析。假設(shè)定位誤差為?Y，xi為每一項參數(shù)的真值，?xi未每一項參數(shù)的誤差，f 為定位方程映射規(guī)則，則

根據(jù)公式（5）和主、子IMU 理論，定位方程中至少需要連乘6 個3×3 的矩陣，最終得到的定位方程式為每一項是多個三角函數(shù)的相乘的多項式，用全微分法估算定位誤差計算龐雜而且分析難度很大，所以采用蒙特卡洛方法（Monte Carlo method）。作為統(tǒng)計模擬方法，采用計算機生成大量隨機數(shù)據(jù)的方法對結(jié)果進行估計，得到誤差的概率分布函數(shù)。

分別根據(jù)誤差正態(tài)分布?xi~N(0，σi)得到每個參數(shù)對應的10000 組值作為參數(shù)的誤差。首先假設(shè)σi=0，即如果沒有誤差，得到的定位值為真值。各參數(shù)真值如圖 2 所示。要計算定位經(jīng)度還需要一些理論真值作為先驗知識，這些理論真值如所示。將這些參數(shù)帶入定位公式計算得到目標的經(jīng)度為125.38781°，緯度為42.08505°，高度為57.82988m。地球相關(guān)參數(shù)的理論真值如圖3所示。

10000組飛機位置誤差分布直方圖如圖4 所示，飛機姿態(tài)角和平臺測量角以及定位等參數(shù)誤差分布與之類似。

將誤差加入，經(jīng)過10000 次的蒙特卡洛仿真試驗，得到最終目標定位結(jié)果分布如圖 5 所示。

為了更直觀，將目標定位的經(jīng)度、緯度和高度10000 組結(jié)果分布進行統(tǒng)計，如圖6 所示。

可以看出定位結(jié)果分布也呈正態(tài)分布，進而得到經(jīng)度誤差為σB=1.94408×10-4°，緯度誤差為σL=4.43348×10-4°，高度誤差為σH=33.83841m。根據(jù)公式（12）折算為空間歐氏距離誤差為σr=63.37698m，相對誤差為歐式距離公式為：

圖6：目標定位結(jié)果分布直方圖

其中Xi，Yi，Zi 為定位測量值，X0，Y0，Z0 為定位真值。根據(jù)誤差理論知，定位誤差呈正態(tài)非同分布，歐式距離分布為瑞利分布如圖 7 所示。

2.4 小結(jié)

激光測距結(jié)合測角有源定位法作為本文核心算法，具有算法效率高，實時性好，工程可實現(xiàn)性強，成本適中，環(huán)境適應性強，在現(xiàn)代各環(huán)節(jié)高精度傳感器測量的條件下，能夠保證定位范圍的情況下，實現(xiàn)很高的定位精度。通過對該算法的定位精度分析可知，光電定位是各個環(huán)節(jié)綜合影響的結(jié)果，需要結(jié)合實際的使用場景，分析各個測量環(huán)節(jié)對結(jié)果的影響。

進一步提升定位精度主要考慮兩方面：最簡單而直接的方法是，在一定的成本約束下，通過采用更高性能傳感器提高各個環(huán)節(jié)參數(shù)的測量精度，這個方法不在本文研究的范圍內(nèi)；第二種方法就是從系統(tǒng)和算法的角度出發(fā)，分析誤差的根本來源，通過結(jié)合一定策略和算法削弱這些誤差對精度的影響同時提升定位的能力，這也是下一章的重點。

3 定位能力提升措施分析

除了之前提到的測量隨機誤差對定位結(jié)果影響，由于測量策略造成的系統(tǒng)誤差與隨機誤差同時作用的情況下帶來的誤差對目標定位有很大影響，需要對核心算法進行改進。

3.1 角度誤差帶來的拓寬效應

根據(jù)誤差理論不確定度的合成，有

分析角度誤差包括姿態(tài)角測量誤差、測距誤差以及飛機的位置誤差，通過控制變量法，只取一類誤差為變量，其他誤差為0 的情況下分析定位精度，發(fā)現(xiàn)各個誤差的合成符合公式（21），而且角度誤差帶來的定位距離誤差是定位誤差最主要來源。

假設(shè)飛行高度為7000m，角度誤差為1mrad，則對垂直向下的目標進行定位是，角度誤差帶來的偏移量約為±7m，同樣飛行高度，當斜距為10000m 時，同樣的角度誤差帶來的偏移量約為±10m，而且隨著傾斜角度α 和飛機距離目標的距離R 的增大，這種角度誤差帶來的拓寬效應愈發(fā)明顯。而根據(jù)實際使用場景，飛機飛行高度一定，飛機需要與目標保持安全距離，這個安全距離是遠大于飛機飛行高度的，所以飛機多時候是無法對目標垂直定位的，這種拓寬效應也是無法消除的。而減弱這種拓寬效應主要有兩種措施：

圖7：目標定位測量值與真值距離分布直方圖

圖8：載機振動帶來的視軸中心角度偏移

（1）以飛行高度極限值和安全距離極限值為定位閾值對目標定位，低于飛行距離和超過安全距離對目標定位都會加劇角度誤差拓寬效應的影響。

（2）使用多點交匯定位（至少三點），結(jié)合最小二乘濾波器對目標進行定位。

將飛機的導航位置信息根據(jù)公式（12）轉(zhuǎn)換到地球空間直角坐標系下，得到飛機的空間直角坐標(Xpi, Ypi, Zpi),(i=1,2,3….n,n ≥3)，當n=3 時，則

Ri,(i=1,2,3….n,n ≥3)為激光測距值，通過公式（22），可以得到目標在地球空間直角坐標系下的坐標值(x,y,z)，再根據(jù)公式（14）或者公式（15）~（18）得到目標的經(jīng)度、緯度和高度坐標值。

圖9：偏離視軸中心定位

圖10：卡爾曼濾波定位目標狀態(tài)

當n>3，由于測量次數(shù)n 大于未知量參數(shù)t（t=3），采用最小二乘濾波器對數(shù)據(jù)估計，由于公式中含有平方項，所以模型為非線性的最小二乘濾波器，采取的方法為：首先根據(jù)公式（21）用方程組得到目標定位的初值；然后對(Xpi-x)2+(Ypi-y)2+(Zpi-z)2=Ri2(i=4,5….,n,n>3)，取泰勒級數(shù)展開的一階近似，將非線性模型轉(zhuǎn)換為線性最小二乘濾波器，再經(jīng)過多次迭代，得到定位精確值。

方法（2）可以消除角度誤差拓寬帶來的影響，但是多次引入了飛機位置誤差和測距誤差。一方面配合最小二乘法濾波器對數(shù)據(jù)的篩選估計，可以進一步消除隨機誤差的影響，另一方面，如果飛機位置精度和測距精度高，對結(jié)果影響不大，否則，精度還有一定下降。這個方法最大的缺陷是單機多點定位時，定位實時性差，需要規(guī)劃圍繞目標較長的路徑（路徑太短，誤差大），因此只能定位固定的目標；而采取多機定位，在任務階段至少需要三架飛機，且測距和飛機定位狀態(tài)基本一致，并且需要三架飛機的時統(tǒng)相匹配。所以方法（2）在同等條件下定位精度雖然可以更高，但是時間成本或者任務成本也必然更高。

為了既能提高定位精度又能減小成本，結(jié)合核心算法做出改進。首先利用方法（1）得到一定精度的目標定位信息作為初值；然后在飛機航跡點上任意一點進行激光測距，得到(Xp-x)2+(Yp-y)2+(Zpz)2=R2。然后對此公式用泰勒一級展開截斷，按照線性最小二乘法對定位結(jié)果進行迭代，由于初值結(jié)果精度較高，迭代次數(shù)很少算法效率會很高，而且也極大削弱了角度誤差的影響。也可通過雙機交匯定位進一步提升定位的效率和實時性，與方法（2）相比，保證了定位精度和實時性的同時降低了成本。

3.2 載機振動的影響

光電平臺是根據(jù)圖像進行采集和定位。首先需要平臺觀察到目標，載機的振動會影響平臺的視軸穩(wěn)定性，反映在圖像上會使得目標質(zhì)心與圖像中心存在像素點的偏移，這個偏移量反映在平臺上可以認為是角度的偏移，進而影響定位精度，如圖 8 所示。

當對目標定位期間為飛機振動較大的時刻，則可能為定位結(jié)果引入無法預料的誤差。所以光電平臺需要良好的抗振環(huán)境，減振器的使用就是光電平臺必不可少的一部分。而減振器的安裝會引入安裝誤差，這個安裝誤差屬于系統(tǒng)誤差，可以在地面調(diào)試的時候測量并進行補償，如公式（11）所示，將安裝誤差代入公式進行定位計算，則可以消除振動帶來的影響。

當飛機振動環(huán)境惡劣，剛性連接減振器對飛機振動傳遞大，且長時間工作會對減振器產(chǎn)生不可逆的損傷，所以平臺減振器與載機為柔性連接。而飛機的振動來源復雜，無法進行量化，所以對于平臺定位來說是不可預計且無法忽略的重要誤差來源。采取的措施為在光電平臺內(nèi)部安裝子IMU 來消除振動帶來的影響。而且子IMU能夠取代主IMU 的功能，即獲得光電平臺的姿態(tài)信息和位置信息，則公式（11）的結(jié)果為1。這一方法直接的優(yōu)點就是減少一級誤差傳遞，提高了定位精度。

由于在平臺內(nèi)放置了IMU，為了盡量不占用成像系統(tǒng)空間，采用體積小的IMU。在進行標校后的子IMU 姿態(tài)測量和位置信息獲取與飛機主IMU 精度相當；在一定時間后，隨著子IMU 內(nèi)部陀螺的漂移，導致測量精度嚴重下降，則需要將飛機內(nèi)高穩(wěn)定性激光材料的IMU 解算數(shù)據(jù)結(jié)合高精度定位定姿系統(tǒng)，通過平臺自動的動態(tài)校靶功能抑制補償陀螺漂移帶來的姿態(tài)誤差，保證定位精度。安裝子IMU 的方法也保證在每次拆卸維修光電平臺后安裝到飛機上不用重新測量安裝偏差進行補償，只需要按照一定流程進行校靶和對齊就能進行飛行和定位，縮短任務準備階段的時間，所以在第2章的推導使用了子IMU 的方法。當然子IMU 壓縮了成像系統(tǒng)空間，可導致成像作用距離減小，而光電平臺主要的任務是獲取圖像，因此，需要在子IMU 的精度和體積上進行一定取舍。

3.3 非視軸中心定位

當對多個目標定位時，兩個目標位置關(guān)系如所示，其中對目標2 在視軸中心進行定位，則目標1 偏離視軸中心。反映在圖像上就是目標2 在圖像正中心，而目標1 在距離圖像中心會有幾個像素點的偏移。

由圖9 可以看出，在相同飛行高度上，角度偏移量與光電平臺的俯仰角度有關(guān)，當俯仰角越小時，角度偏移量越小，反映在像素偏移量上越小，測量精度也越低。所以對于非視軸中心目標的定位也在保證安全距離情況下，盡可能大的俯仰角，這與減弱角度誤差拓寬效應的策略相同。當然，也可以將目標放置到視軸中心進行定位來確保最優(yōu)的定位精度。

3.4 運動目標的定位

目前所使用的定位方法只針對固定目標有效，而對運動目標，無法對目標的運動趨勢進行預測。提到狀態(tài)預測自然而然想到的是卡爾曼濾波器[11]，而對于定位系統(tǒng)來說，系統(tǒng)狀態(tài)方程和觀測方程為非線性方程[12]。所以采用擴展卡爾曼濾波（Extended Kalman Filter, EKF），通過非線性模型在狀態(tài)值附近泰勒級數(shù)展開并取一階截斷進行線性化，并假設(shè)線性化后狀態(tài)仍然服從高斯分布，在對其使用卡爾曼濾波獲得狀態(tài)估計。

考慮一種簡單的情況：假設(shè)目標與飛機同方向，速度大小不同做勻速直線運動，如圖 10 圖 9 飛機對目標2 進行定位和跟蹤。假設(shè)k 時刻目標相對載機的運動狀態(tài)向量為其中[xk,yk,zk]和分別對應目標相對載機的距離地球空間直角坐標系的三軸分量及其速度分量，而載機每個時刻的位置和速度是已知的。

卡爾曼濾波主要需要兩個方程對目標定位的結(jié)果進行預測，而假設(shè)每個參數(shù)和變量都遵循高斯分布，則有公式：

忽略下標j 得到：

本例通過載機與目標相對的位移與速度關(guān)系，可得到目標的運動狀態(tài)方程：

其中，

根據(jù)光電平臺的測角量反映目標的映射光學得到測量方程：

其中，h 為飛機高度，xpk 為飛機k 時刻x 軸坐標。

對方程（27）求導可得到雅可比矩陣，通過有源定位法得到初值，將初值代入EKF 的公式進行迭代，得到 在需要任意時刻的值，就能預測目標的位置和速度信息。

通過本例的簡單推導，就能對一般情況下運動目標定位進行推廣。結(jié)合核心算法和第1 節(jié)的提示，將核心算法得到的定位值作為初值，目標在地球直角坐標系下的狀態(tài)為狀態(tài)方程，以核心算法的方程作為觀測方程，在航向上多個航跡點上使用擴展卡爾曼濾波進行迭代解算，能夠?qū)崿F(xiàn)對目標位置、速度等參數(shù)的測量和預測。而為了減小計算量，提高運算效率并保證預測精度，可以根據(jù)需求和實際使用環(huán)境規(guī)劃航行軌跡，例如使載機圍繞目標勻速直線運動或勻速圓周運動等。本方法可以與第1 節(jié)最小二乘法同步使用，保證定位精度的同時實現(xiàn)對運動目標的定位。

3.5 小結(jié)

本章結(jié)合實際，對系統(tǒng)進行分析，提出了在核心算法基礎(chǔ)上，平臺內(nèi)加裝子IMU，合理規(guī)劃航線，并在航線的航跡點上結(jié)合最小二乘法和擴展卡爾曼濾波法，在保證定位效率的同時，提高了定位精度和定位能力。

4 結(jié)論

在光電載荷定位需求上出發(fā)，推導了可以廣泛應用的測角、激光測距有源法的光電定位方法作為核心算法，并分析了定位的誤差來源。以仿射空間的齊次坐標轉(zhuǎn)換為核心的光電定位算法有效的通過采用蒙特卡洛方法對定位精度進行仿真，以現(xiàn)階段能夠達到的各項參數(shù)的精度指標，最終目標定位能夠?qū)崿F(xiàn)0.25%@25Km 的相對誤差。并對定位實際情況分析，提出了結(jié)合最小二乘濾波器的激光測距有源法的光電定位方法，有效減少角度誤差帶來的影響；通過子IMU 的安裝消除載機震動帶來的隨機誤差；設(shè)置一定的定位策略減小非視軸中心目標定位的誤差；并結(jié)合拓展卡爾曼濾波的算法法實現(xiàn)對運動目標的定位，提升了定位能力。