摘 要：智能網(wǎng)聯(lián)汽車要實現(xiàn)自動駕駛，需要連續(xù)、可靠的定位信息。多傳感器融合定位技術(shù)是智能網(wǎng)聯(lián)汽車的必備解決方案。在多傳感器融合系統(tǒng)中，慣性測量單元是必不可少的傳感器。但是慣性導(dǎo)航的機械編排計算量大，實時定位效率較低。實際上，慣性導(dǎo)航算法在速度更新時，無須進行旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償。從實測結(jié)果可知，省略兩項高階補償項，不僅不會影響其定位效果，也能降低定位算法的計算量，從而滿足智能網(wǎng)聯(lián)汽車自動駕駛的低時延要求。

關(guān)鍵詞：汽車定位技術(shù)；慣性導(dǎo)航定位；機械編排算法；高階補償

中圖分類號：TP301.6 文獻標(biāo)識碼：A 文章編號：2096-4706（2024）16-0079-05

Optimization of Intelligent Connected Automobile Positioning Algorithm Calculation

Abstract： Intelligent connected automobiles require continuous and reliable positioning information to achieve autonomous driving. Multi-sensor fusion positioning technology is an essential solution for intelligent connected automobiles. In multi-sensor fusion systems， inertial measurement units are essential sensors. However， the mechanical arrangement of inertial navigation requires a large amount of computation， and the real-time positioning efficiency is relatively low. Actually， the inertial navigation algorithm does not require compensation for rotation and paddle effects during speed updates. From the actual test results， it can be seen that omitting two high-order compensation terms not only does not affect its positioning effect， but also reduces the computational complexity of the positioning algorithm， thereby meeting the low latency requirements of intelligent connected automobile autonomous driving.

Keywords： automotive positioning technology; inertial navigation positioning; mechanical arrangement algorithm; high-order compensation

0 引 言

智能網(wǎng)聯(lián)汽車是指搭載先進的傳感器、控制器等裝置，實現(xiàn)替代人來操作的新一代汽車。在智能網(wǎng)聯(lián)汽車的應(yīng)用中，導(dǎo)航定位系統(tǒng)是其核心部分。智能網(wǎng)聯(lián)汽車導(dǎo)航定位系統(tǒng)的目的是獲取汽車的實時位置、速度、姿態(tài)信息，從而將這些信息傳輸給下游模塊，如路徑規(guī)劃模塊、自動控制模塊等[1]。

目前，智能網(wǎng)聯(lián)汽車所使用的汽車導(dǎo)航定位技術(shù)主要有衛(wèi)星導(dǎo)航定位技術(shù)、視覺定位技術(shù)、激光雷達定位技術(shù)等。衛(wèi)星導(dǎo)航定位技術(shù)是當(dāng)前最常用的車輛定位技術(shù)[2]，但是它容易受環(huán)境干擾，如城市高樓地段、隧道場景等，其定位精度下降甚至無法定位。視覺定位技術(shù)成本低，但極易受光照條件、攝像頭角度等外界因素影響。激光雷達定位精度高，但工作時受天氣影響大，且定位誤差會隨著汽車運動而累積[3]。因此，單一的傳感器無法為智能網(wǎng)聯(lián)汽車提供連續(xù)、可靠的定位信息，采用多傳感器融合技術(shù)是實現(xiàn)智能網(wǎng)聯(lián)汽車在復(fù)雜場景下獲取可靠定位的必然手段[4-5]。

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System， INS）是一種航位推算系統(tǒng)，由慣性測量單元（Inertial Measurement Unit， IMU）和導(dǎo)航處理器組成。慣性導(dǎo)航系統(tǒng)有許多優(yōu)點，如更新頻率高、自主導(dǎo)航不受外界環(huán)境影響、姿態(tài)信息豐富等。因此INS系統(tǒng)是多傳感器融合中必備的定位技術(shù)[6]。但是INS在航位推算算法中涉及高階計算，計算量大，耗時長。而智能網(wǎng)聯(lián)汽車要求定位模塊具有實時性，所以本文將采用實車測試的數(shù)據(jù)，對多傳感器融合的INS定位算法進行簡化。

1 慣性導(dǎo)航定位原理

慣性導(dǎo)航系統(tǒng)由慣性測量單元IMU和導(dǎo)航處理器組成。IMU包含一組三個相互正交配置的加速度計和與加速度計對準(zhǔn)安裝的陀螺儀。INS導(dǎo)航處理器通過對陀螺儀測量的角速度信息進行積分得到載體的姿態(tài)信息。IMU的加速度計輸出的是比力信息，比力是除了引力以外其他外力引起的加速度。在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，IMU的加速度計是與導(dǎo)航載體固聯(lián)的，因此結(jié)合載體姿態(tài)信息即可將加速度測量的比力轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，然后采用引力模型即可得到載體加速度，將加速度進行積分即可得到速度信息[6]。

Niu[7-9]等學(xué)者詳細(xì)描述了慣性導(dǎo)航系統(tǒng)的航位推算算法原理。首先是進行初始對準(zhǔn)。慣性導(dǎo)航系統(tǒng)通過集成慣性測量單元來計算導(dǎo)航結(jié)果，導(dǎo)航方程的每次迭代都使用上一歷元的導(dǎo)航解作為起點，因此，在使用INS提供導(dǎo)航解決方案之前，必須進行初始化，獲取初始的位置、速度和姿態(tài)信息。初始位置和速度可以從全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System， GNSS）模塊獲取，初始姿態(tài)的獲取也叫INS的初始對準(zhǔn)。在智能網(wǎng)聯(lián)汽車中，我們可利用外部輔助信息進行初始對準(zhǔn)。在此我們使用GNSS估計速度來確定載體的初始姿態(tài)信息[10]。

INS初始化后，不再需要外部信息，即可持續(xù)自主地進行航位推算。INS航位推算流程如圖1所示。

INS航位推算分為以下步驟：

1）位置更新。結(jié)合上一歷元的位置數(shù)據(jù)和新采集的數(shù)據(jù)即可實時推算出當(dāng)前最新的位置信息。INS航位推算位置更新離散化方程如下：

其中：，分別表示tk時刻和tk-1時刻智能網(wǎng)聯(lián)汽車在導(dǎo)航坐標(biāo)系（n系）下的三維位置信息；，分別表示tk時刻和tk-1時刻智能網(wǎng)聯(lián)汽車在導(dǎo)航坐標(biāo)系（n系）下的三維速度信息；M，N，h，φ分別表示子午圈曲率半徑、卯酉圈曲率半徑、地理高程、智能網(wǎng)聯(lián)汽車當(dāng)前緯度；?t表示tk時刻和tk-1時刻的時間差。

2）姿態(tài)更新。慣性導(dǎo)航航位推算中的姿態(tài)更新是利用IMU實時輸出的角速度測量值來更新載體姿態(tài)信息的過程。IMU的陀螺儀采集到原始的角速度信息后，導(dǎo)航處理器對原始數(shù)據(jù)進行誤差補償，才可進行后續(xù)處理。陀螺儀誤差包括零偏誤差、比例因子誤差、交叉耦合誤差等。陀螺儀測量數(shù)據(jù)在系統(tǒng)誤差被補償后，即可根據(jù)姿態(tài)更新離散化方程進行旋轉(zhuǎn)、整合、積分，輸出載體最新的三維姿態(tài)。下式為INS航位推算姿態(tài)更新離散化方程：

其中：，分別表示tk時刻和tk-1時刻的姿態(tài)四元數(shù)；I表示單位矩陣。

?Φx，?Φy，?Φz分別表示智能網(wǎng)聯(lián)汽車IMU三軸陀螺儀測得的角速度增量。

3）速度更新。IMU的加速度計采集到原始的比力數(shù)據(jù)后，也需要對原始比力數(shù)據(jù)進行零偏誤差、比例因子誤差、交叉耦合誤差的補償。接著，經(jīng)過預(yù)處理的比力數(shù)據(jù)需要進行投影轉(zhuǎn)換。該轉(zhuǎn)換是將比力從IMU載體坐標(biāo)系（b系）轉(zhuǎn)換到導(dǎo)航坐標(biāo)系（n系），這里需要使用姿態(tài)更新輸出的載體最新姿態(tài)信息。然后，使用引力模型，將比力轉(zhuǎn)換為加速度。最后，根據(jù)速度更新離散化方程更新載體的三維速度，這里需要使用位置更新輸出的載體最新位置信息。INS航位推算速度更新方程如下：

其中，，分別表示tk時刻和tk-1時刻智能網(wǎng)聯(lián)汽車在導(dǎo)航坐標(biāo)系（n系）下的三維速度信息；表示tk時刻智能網(wǎng)聯(lián)汽車的姿態(tài)余弦矩陣；?v（tk）表示智能網(wǎng)聯(lián)汽車IMU三軸加速度計測得的比力； 表示由哥氏加速度、向心加速度、重力加速度共同產(chǎn)生的速度增量補償項。在速度更新時，需要進行旋轉(zhuǎn)效應(yīng)補償及劃槳效應(yīng)補償[11]。

旋轉(zhuǎn)效應(yīng)是由載體的線運動方向在空間旋轉(zhuǎn)而引起的，其離散化計算方程如下：

其中，?θk表示tk時刻智能網(wǎng)聯(lián)汽車中IMU測得的角速度增量；?vk表示tk時刻智能網(wǎng)聯(lián)汽車中IMU測得的加速度增量。

劃槳效應(yīng)是當(dāng)載體沿縱軸作線振動的同時又沿橫軸作同頻同相的角振動時產(chǎn)生的，其離散化計算方程如下：

其中，?θk，?θk-1分別表示tk時刻和tk-1時刻智能網(wǎng)聯(lián)汽車中IMU測得的角速度增量；?vk，?vk-1分別表示tk時刻和tk-1時刻智能網(wǎng)聯(lián)汽車中IMU測得的加速度增量。

從上述旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)方程中可以看出，旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)產(chǎn)生的必要條件是角運動和線運動的組合運動。由于智能網(wǎng)聯(lián)汽車的運動皆屬于低動態(tài)場景，不存在劇烈的角運動和線運動組合。因此，在INS速度更新時，其旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償量為零。那么，在智能網(wǎng)聯(lián)汽車的INS航位推算時，旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償對導(dǎo)航誤差的影響極其微弱。從上述方程中也可以看出，旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償皆屬于高階項。由此，從理論分析可知，在INS速度更新時省略旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)這兩項高階補償項，可以在不影響定位精度的前提下，提升INS的解算速度。

2 實驗驗證

2.1 試驗準(zhǔn)備

為了驗證上述結(jié)論，我們采集了真實的智能網(wǎng)聯(lián)汽車車載測試數(shù)據(jù)進行分析。車載測試數(shù)據(jù)來源于一輛智能網(wǎng)聯(lián)汽車，是由比亞迪秦改裝而成的智能網(wǎng)聯(lián)試驗平臺。車輛上搭載了一個車載級IMU傳感器和GNSS接收機模塊。

IMU來源于德國博世集團（Bosch），型號為SMI130。Bosch SMI130是一款汽車微機電系統(tǒng)（Micro-Electro-Mechanical System，MEMS）傳感器。SIM130包含不同測量范圍的16位數(shù)字陀螺儀和12位數(shù)字加速度計，它結(jié)合了來自消費類電子產(chǎn)品和汽車電子產(chǎn)品的技術(shù)，具有多編程選項、低信號噪聲、易于集成、低功耗、較小占位空間等優(yōu)勢，因此在汽車應(yīng)用中通用性強。IMU關(guān)鍵參數(shù)如表1所示。

GNSS接收機模塊是u-blox NEO-M8N。NEO-M8N可同時接收GPS、GLONASS（格洛納斯，蘇聯(lián)/俄羅斯研制）、北斗、Galileo（伽利略，歐盟研制）衛(wèi)星信號，具有低成本、低功耗、高靈敏度、性能佳等優(yōu)勢。GNSS模塊NEO-M8N的關(guān)鍵參數(shù)如表2所示。

車載驗證測試于2019年5月20日在北京回龍觀進行，測試場景為室外較開闊路段兼室內(nèi)地下停車場場景。測試時長為半小時，路程長15 千米。

2.2 試驗結(jié)果分析

為了對比速度更新中高階補償項對定位精度的影響，我們將上述實車測試數(shù)據(jù)采集到實驗室后，采用多傳感器融合算法平臺進行定位解算和誤差分析。GNSS和慣性導(dǎo)航系統(tǒng)的融合采用松組合的模式，使用GNSS接收機輸出的位置和速度信息作為GNSS/INS組合導(dǎo)航系統(tǒng)的測量輸入，即可使用卡爾曼濾波器來估計IMU誤差。隨后卡爾曼濾波器估計的IMU誤差可以對INS輸出的導(dǎo)航信息進行校正，校正后的INS導(dǎo)航結(jié)果即可輸出使用。在GNSS/INS組合導(dǎo)航系統(tǒng)中，慣性導(dǎo)航航位推算算法采用前文所述步驟進行。下面將對省略速度更新的高階補償項和加入高階補償?shù)臏y試結(jié)果進行對比分析。

圖2展示了INS速度更新時加入和省略高階修正項的軌跡圖，從圖中可以看出，兩者的軌跡非常接近。

圖3展示了省略和加入高階補償項的位置差距、速度差距和姿態(tài)差距，表3列出了其統(tǒng)計值，其中位置、速度和姿態(tài)誤差的單位分別是米、米每秒、度。

從圖3和表3中可以看出，在速度更新時，省略旋轉(zhuǎn)效應(yīng)補償及劃槳效應(yīng)補償：

1）北向位置差距最大值為4厘米，均方根值為0.7厘米；東向位置差距最大值為3.9厘米，均方根值為0.6厘米。

2）北向速度差距最大值為7.05厘米每秒，均方根值為0.66厘米每秒；東向速度差距最大值為7.42厘米每秒，均方根值為0.65厘米每秒。

3）三維姿態(tài)包括橫滾角、俯仰角和航向角，其中橫滾角差距最大值為0.14度，均方根值為0.02度；俯仰角差距最大值為0.10度，均方根值為0.01度；航向角差距最大值為0.17度，均方根值為0.03度。

從以上結(jié)果可知，在智能網(wǎng)聯(lián)汽車的慣性導(dǎo)航算法解算中，省略旋轉(zhuǎn)效應(yīng)補償及劃槳效應(yīng)補償，其位置差距為厘米級，姿態(tài)差距小于0.2度，因此，智能網(wǎng)聯(lián)汽車慣性導(dǎo)航算法在速度更新時，可以省略旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償。

此外，針對該組測試數(shù)據(jù)，我們通過多次運行算法，對比測試了高階補償項對算法運行效率的影響。試驗分別對省略高階補償項和加入高階補償項的定位算法各運行1 000次，統(tǒng)計其運行時間的平均值，得到以下結(jié)果：

1）在GNSS和IMU組合導(dǎo)航定位解算時，加入高階補償項，離線解算完該組測試數(shù)據(jù)（IMU采樣率為10 Hz，GNSS采樣率為1 Hz，測試時長30分鐘，測試距離15千米）的時間平均值為17.057 9秒。

2）針對相同的測試數(shù)據(jù)，在省略高階補償項時，其離線解算時間平均值為15.827 7秒。

從以上結(jié)果可知，每進行一次IMU航位推算，加入高階補償?shù)倪\行時間比省略高階補償項的運行時間多0.068毫秒。

我們知道，智能網(wǎng)聯(lián)汽車要實現(xiàn)完全無人駕駛，需要滿足低時延的要求[12]。從以上結(jié)果可知，在智能網(wǎng)聯(lián)汽車的定位模塊中，慣性導(dǎo)航系統(tǒng)在進行速度更新時，無須進行高階的旋轉(zhuǎn)效應(yīng)補償及劃槳效應(yīng)補償。省略這兩項高階補償，既不會影響智能網(wǎng)聯(lián)汽車的定位精度，也可以降低智能網(wǎng)聯(lián)汽車定位算法的計算量，從而提升智能網(wǎng)聯(lián)汽車的計算效率，進一步助力實現(xiàn)無人駕駛低時延的目標(biāo)。

3 結(jié) 論

多傳感器融合定位算法是當(dāng)今智能網(wǎng)聯(lián)汽車最為重要的定位系統(tǒng)，而IMU是定位系統(tǒng)中必不可少的傳感器。本文通過實車測試，驗證了INS航位推算過程中，無須進行速度更新的旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補償。省略兩項高階補償項，不僅對定位、測速和定姿結(jié)果影響微弱，同時，也能在一定程度上降低定位算法的計算量和縮短定位解算時間，從而滿足智能網(wǎng)聯(lián)汽車低時延的目標(biāo)。

參考文獻：

[1] 蔣孫權(quán).無人駕駛汽車關(guān)鍵技術(shù)研究 [J].時代汽車，2020（3）：25-27.

[2] 鐘振，王祥.基于RTK的GNSS/INS實時組合導(dǎo)航系統(tǒng)設(shè)計 [J].現(xiàn)代信息科技，2021，5（12）：72-74+79.

[3] 尹皓，李海濱，王利利.基于多傳感器融合的移動機器人定位研究 [JTdqJpnZizy+1FQfNsngpfRWnFqNXqf2jm98t/twnPMQ=].機床與液壓，2021，49（9）：6-10.

[4] 張文玥，婁小平，陳福笛.移動機器人多傳感器融合定位仿真研究 [J].計算機仿真，2023，40（3）：436-441.

[5] 梁高麗，鄧仕軍，雷浩.一種基于多傳感信息融合的定位算法研究 [J].現(xiàn)代信息科技，2023，7（8）：33-35+39.

[6] GROVES，PAUL D. Principles of GNSS， Inertial， and Multisensor Integrated Navigation Systems [J].Industrial Robot，2013，67（3）：191-192.

[7] NIU X，ZHANG Q，GONG L，et al. Development and Evaluation of GNSS/INS Data Processing Software for Position and Orientation Systems [J].Survey Review，2015，47（341）：87-98.

[8] SHIN E H. Accuracy Improvement of Low Cost INS/GPS for Land Applications [J].Proceedings of the 2002 National Technical Meeting of The Institute of Navigation.San Diego：Institute of Navigation，2002：146-157.

[9] SHIN E H. Estimation Techniques for Low-Cost Inertial Navigation [D].Canada：University of Calgary，2005.

[10] GONG L L，ZHANG Q，LI Q L，et al. Study on Intelligent Setting of Initial Alignment for GNSS/INS Integration [C]//China Satellite Navigation Conference （CSNC） 2014 Proceedings： Volume III.Nanjing：Springer，2014：563-576.

[11] 龔琳琳，張全，牛小驥.慣性導(dǎo)航算法的載體高動態(tài)特性研究 [J].導(dǎo)航定位學(xué)報，2015，3（2）：71-75.

[12] 毛廣輝.自動駕駛芯片需要滿足高吞吐與低時延 [J].電子產(chǎn)品世界，2020，28（12）：10+14.