迭代最小二乘衛星定位算法

王奉帥,劉聰鋒

(1.中國電子科技集團公司第五十四研究所，河北 石家莊050081；2.西安電子科技大學，陜西 西安710071)

0 引言

全球定位系統具有全天候、全球性、實時連續性和高精度等特點，因此受到了廣泛的應用，例如飛機導航、導彈制導、地球變形監測、汽車防盜跟蹤和手機定位等等。

傳統的衛星導航定位解算采用的是最小二乘算法，對觀測矩陣進行求逆來獲取未知向量，其中求逆過程包含了大量復雜的乘除運算,增加了系統功耗。坐標旋轉數字計算方法 (Coordinate Rotation Digital Computer,CORDIC)的應用越來越廣泛，該算法能夠減少硬件對時間和空間資源的需求。MR Mosavi等人結合偽距和載波相位，利用傳統最小二乘確定接收機坐標，定位精度有所提升[1]。Yuheng He等人處理定位解算時，每次旋轉選擇合適的CORDIC角度，可以簡化最小二乘運算[2]。N.Rahemi等人根據偽距方差對觀測矩陣加權，再用最小二乘求解，可以提高定位精度[3]。李春華等人提出了一種基于互差中值理論的加權最小二乘算法,該算法的定位精度較最小二乘算法和直接解算方法的定位精度有較大提高,并能有效地減少問題衛星的影響[4]。徐飛采用了CORDIC算法對旋轉矩陣中的三角函數計算,實現了硬件加速[5]。

本文給出了基于CORDIC算法實現近似正交三角(Orthogonal-Triangular，QR)分解[6-9]的迭代最小二乘算法(以下簡稱迭代最小二乘算法)，利用該算法進行定位解算，沒有損失定位精度，而且運行穩定可靠。

1 GPS定位原理

最小二乘算法是導航設備最常用到的定位解算算法，該算法根據4顆或更多衛星的偽距解算出接收機位置[10]。衛星觀測偽距及星地幾何距離如圖1所示。

圖1 衛星觀測偽距及星地幾何距離

接收機接收到衛星k的偽距ρ(k)，具有下述表達式：

ρ(k)=r(k)+c(dt-dt(k))+T(k)+I(k)+ε(k)，
k=1,2,...,n。

(1)

r(k)表示衛星k到接收機的幾何距離：

(2)

式(1)中包含了4個未知數x，y，z和dt，其他參數均能通過先驗信息獲得。顯然該方程是非線性的，對r(k)在(x0,y0,z0)處展開泰勒級數，舍棄二階及更高階項可以得到：

(3)

將式(1)在(x0,y0,z0,dt0)處展開泰勒級數,并舍棄二階及更高階項可以得到

Ax=b，

(4)

其中，

其中，

未知向量x是Ax=b的最小二乘解

minx‖Ax-b‖2。

(5)

可以得出x=(ATA)-1b，為了求出未知向量，需要對矩陣ATA求逆，這其中包含了復雜的運算，QR分解將矩陣ATA分解為Q和R矩陣，其中Q為正交矩陣，R為上三角矩陣。

2 基于CORDIC的近似QR分解算法

如果將式(4)左乘矩陣AT,可以得到

ΑTΑx=ΑTb;

(6)

若矩陣ATA能夠進行QR分解，即

ΑTΑ=QR;

(7)

將式(7)兩端左乘矩陣QT可得

QTΑTΑ=R;

(8)

因此式(6)兩端左乘矩陣QT可以表示為

Rx=QTΑTb。

(9)

利用Givens變換可以實現矩陣A的QR分解，而Givens變換又可以利用CORDIC近似旋轉算法實現。Volder 提出的CORDIC方法以其運算及硬件結構簡單和多能性而倍受關注，并已廣泛應用于信號處理領域[11-15]。典型的Givens變換矩陣為:

G為n×n的矩陣，令x=[x1…xi…xj…xn]T，向量x左乘矩陣G，表示將x的xi，xj旋轉角度φ，轉換完的向量為y=[y1…yi…yj…yn]T,可以得到

(10)

其他元素保持不變,選擇合適的角度φ可以使yj=0。同樣選擇合適的G，可以將矩陣A轉換成上三角矩陣。為了方便計算機計算，可以令 tanφ=2-k，k為整數。利用CORDIC算法近似實現矩陣的QR分解就是通過迭代不同的旋轉角度，每次迭代旋轉的角度為2的整數冪，最終可將yj轉換為0。CORDIC 算法的收斂性滿足收斂定理,其中第k次迭代為[16]：

(11)

(12)

變換過程如圖2所示。

圖2 基于CORDIC近似旋轉算法的Givens變換示意

通過一系列變換，式(6)最終可以變成：

(13)

最終可以求解出未知向量

(14)

3 實驗結果與分析

將接收機天線放置在基準點上收取GPS信號,接收到的可見衛星為7顆，分別將接收到的衛星偽距和電文存儲下來，利用Matlab軟件處理存儲的數據。

對存儲數據分別用求逆最小二乘算法和基于CORDIC近似QR分解的迭代最小二乘算法進行定位解算。在利用CORDIC近似QR分解的最小二乘算法時,迭代次數大于7得到的定位精度與求逆最小二乘算法基本一致。圖3和圖4給出了CORDIC近似旋轉最小二乘算法在迭代次數為6和7時的定位精度與求逆最小二乘算法的定位精度對比，其中迭代初始點都為坐標原點。可以看出在迭代次數為7時，基于CORDIC近似旋轉最小二乘算法定位精度很接近。迭代次數為6時，基于CORDIC近似旋轉最小二乘算法定位精度雖然也很高，但沒有特別穩定。

圖3 垂直定位誤差絕對值

圖4 水平定位誤差絕對值

在單次定位解算中，最小二乘求逆算法和迭代最小二乘算法的運算量是不一樣的，如果矩陣求逆采用常用的初等變換方法實現，迭代最小二乘算法采用本文給出的方法，可以得到每種算法的運算量，具體如表1所示。根據計算機原理可知，計算機操作的運算復雜度排序為：移位運算<加法運算<乘法運算<除法運算。

表1 不同算法運算量對比

運算初等變換求逆迭代QR分解(itg=6)迭代QR分解(itg=7)移位運算(次數)加運算(次數)乘運算(次數)除運算(次數)0969616727864849064

4 結束語

利用迭代最小二乘算法進行定位解算時，迭代次數越多，達到的精度越高，同時運算量也會相應增加。但在不影響定位精度的前提下，迭代最小二乘算法的運算量不管是在數量還是復雜度上都要優于傳統最小二乘算法，從而有效地提高了數據處理器的運算效率。另外，利用CORDIC算法對觀測矩陣進行QR分解代替求逆運算，能夠減少大量運算，這對載波相位差分以及定向解算也具有很大的參考意義。

[1] Mosavi M R,Azarshahi S,Emamgholipour I,et al.Least Squares Techniques for GPS Receivers Positioning Filter using Pseudo-Range and Carrier Phase Measurements[J].Iranian Journal of Electrical & Electronic Engineering,2014,10(1):18-26.

[2] He Yuheng,Martin R,Bilgic A M.Approximate Iterative Least Squares Algorithms for GPS Positioning[C]∥ IEEE International Symposium on Signal Processing and Information Technology，2011:231-236.

[3] Rahemi N,Mosavi M R,Abedi A A,et al.Accurate Solution of Navigation Equations in GPS Receivers for Very High Velocities Using Pseudorange Measurements[J].Advances in Aerospace Engineering,2014(2):1-8.

[4] 李春華,蔡成林,梁愈高,等.一種高精度的北斗偽距單點定位加權算法[J].測繪科學,2015,40(9):33-38.

[5] 徐飛,肖鐵軍,華純,等.基于FPGA的視頻圖像旋轉硬件加速器的設計與實現[J].傳感器與微系統,2010,29(10):100-102.

[6] Meher P K,Sang Y P.CORDIC Designs for Fixed Angle of Rotation[J].IEEE Transactions on Very Large Scale Integration Systems,2013,21(2):217-228.

[8] 張朝柱,韓吉南,燕慧智.高速高精度固定角度旋轉CORDIC算法的設計與實現[J].電子學報,2016,44(2):485-490.

[9] Ramadoss R,Kermani M M,Azarderakhsh R.Reliable Hardware Architectures of the CORDIC Algorithm With a Fixed Angle of Rotations[J].IEEE Transactions on Circuits and Systems II:Express Briefs,2017,64(8):972-976.

[10] He Y,Bilgic A.Iterative Least Squares Method for Global Positioning System[J].Advances in Radio Science,2011,9(1):203-208.

[11] Biswas D,Maharatna K.A CORDIC-Based Low-Power Statistical Feature Computation Engine for WSN Applications[J].Circuits Systems & Signal Processing,2015,34(12):4011-4028.

[12] Torres V,Valls J,Canet M J.Optimised CORDIC-based Atan2 Computation for FPGA Implementations[J].Electronics Letters,2017,53(19):1296-1298.

[13] Aggarwal S,Meher P K,Khare K.Concept,Design,and Implementation of Reconfigurable CORDIC[J].IEEE Transactions on Very Large Scale Integration Systems,2016,24(4):1588-1592.

[14] Chen Jiyang,Lei Yuanwu,Peng Yuanxi,et al.Configurable Floating-Point FFT Accelerator on FPGA Based Multiple-Rotation CORDIC[J].Chinese Journal of Electronics,2016,25(6):1063-1070.

[15] Heidarpour M,Ahmadi A,Rashidzadeh R.A CORDIC Based Digital Hardware For Adaptive Exponential Integrate and Fire Neuron[J].IEEE Transactions on Circuits & Systems.Part I:Regular Papers,2016,63(11):1986-1996.

[16] Nawandar N K,Garg B,Sharma G K.RICO:A Low Power Repetitive Iteration CORDIC for DSP Applications in Portable Devices[J].Journal of Systems Architecture,2016,70:82-92.

[17] 史方顯,曾立,陳昱,等.改進型高速高精度CORDIC算法及其在DDFS中的應用[J].電子學報,2017,45(2):446-451.