智能网联汽车定位算法计算优化

摘 要：智能网联汽车要实现自动驾驶，需要连续、可靠的定位信息。多传感器融合定位技术是智能网联汽车的必备解决方案。在多传感器融合系统中，惯性测量单元是必不可少的传感器。但是惯性导航的机械编排计算量大，实时定位效率较低。实际上，惯性导航算法在速度更新时，无须进行旋转效应和划桨效应补偿。从实测结果可知，省略两项高阶补偿项，不仅不会影响其定位效果，也能降低定位算法的计算量，从而满足智能网联汽车自动驾驶的低时延要求。

关键词：汽车定位技术；惯性导航定位；机械编排算法；高阶补偿

中图分类号：TP301.6 文献标识码：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 引 言

智能网联汽车是指搭载先进的传感器、控制器等装置，实现替代人来操作的新一代汽车。在智能网联汽车的应用中，导航定位系统是其核心部分。智能网联汽车导航定位系统的目的是获取汽车的实时位置、速度、姿态信息，从而将这些信息传输给下游模块，如路径规划模块、自动控制模块等[1]。

目前，智能网联汽车所使用的汽车导航定位技术主要有卫星导航定位技术、视觉定位技术、激光雷达定位技术等。卫星导航定位技术是当前最常用的车辆定位技术[2]，但是它容易受环境干扰，如城市高楼地段、隧道场景等，其定位精度下降甚至无法定位。视觉定位技术成本低，但极易受光照条件、摄像头角度等外界因素影响。激光雷达定位精度高，但工作时受天气影响大，且定位误差会随着汽车运动而累积[3]。因此，单一的传感器无法为智能网联汽车提供连续、可靠的定位信息，采用多传感器融合技术是实现智能网联汽车在复杂场景下获取可靠定位的必然手段[4-5]。

惯性导航系统（Inertial Navigation System， INS）是一种航位推算系统，由惯性测量单元（Inertial Measurement Unit， IMU）和导航处理器组成。惯性导航系统有许多优点，如更新频率高、自主导航不受外界环境影响、姿态信息丰富等。因此INS系统是多传感器融合中必备的定位技术[6]。但是INS在航位推算算法中涉及高阶计算，计算量大，耗时长。而智能网联汽车要求定位模块具有实时性，所以本文将采用实车测试的数据，对多传感器融合的INS定位算法进行简化。

1 惯性导航定位原理

惯性导航系统由惯性测量单元IMU和导航处理器组成。IMU包含一组三个相互正交配置的加速度计和与加速度计对准安装的陀螺仪。INS导航处理器通过对陀螺仪测量的角速度信息进行积分得到载体的姿态信息。IMU的加速度计输出的是比力信息，比力是除了引力以外其他外力引起的加速度。在捷联惯性导航系统中，IMU的加速度计是与导航载体固联的，因此结合载体姿态信息即可将加速度测量的比力转换到导航坐标系中，然后采用引力模型即可得到载体加速度，将加速度进行积分即可得到速度信息[6]。

Niu[7-9]等学者详细描述了惯性导航系统的航位推算算法原理。首先是进行初始对准。惯性导航系统通过集成惯性测量单元来计算导航结果，导航方程的每次迭代都使用上一历元的导航解作为起点，因此，在使用INS提供导航解决方案之前，必须进行初始化，获取初始的位置、速度和姿态信息。初始位置和速度可以从全球导航卫星系统（Global Navigation Satellite System， GNSS）模块获取，初始姿态的获取也叫INS的初始对准。在智能网联汽车中，我们可利用外部辅助信息进行初始对准。在此我们使用GNSS估计速度来确定载体的初始姿态信息[10]。

INS初始化后，不再需要外部信息，即可持续自主地进行航位推算。INS航位推算流程如图1所示。

INS航位推算分为以下步骤：

1）位置更新。结合上一历元的位置数据和新采集的数据即可实时推算出当前最新的位置信息。INS航位推算位置更新离散化方程如下：

其中：，分别表示tk时刻和tk-1时刻智能网联汽车在导航坐标系（n系）下的三维位置信息；，分别表示tk时刻和tk-1时刻智能网联汽车在导航坐标系（n系）下的三维速度信息；M，N，h，φ分别表示子午圈曲率半径、卯酉圈曲率半径、地理高程、智能网联汽车当前纬度；∆t表示tk时刻和tk-1时刻的时间差。

2）姿态更新。惯性导航航位推算中的姿态更新是利用IMU实时输出的角速度测量值来更新载体姿态信息的过程。IMU的陀螺仪采集到原始的角速度信息后，导航处理器对原始数据进行误差补偿，才可进行后续处理。陀螺仪误差包括零偏误差、比例因子误差、交叉耦合误差等。陀螺仪测量数据在系统误差被补偿后，即可根据姿态更新离散化方程进行旋转、整合、积分，输出载体最新的三维姿态。下式为INS航位推算姿态更新离散化方程：

其中：，分别表示tk时刻和tk-1时刻的姿态四元数；I表示单位矩阵。

∆Φx，∆Φy，∆Φz分别表示智能网联汽车IMU三轴陀螺仪测得的角速度增量。

3）速度更新。IMU的加速度计采集到原始的比力数据后，也需要对原始比力数据进行零偏误差、比例因子误差、交叉耦合误差的补偿。接着，经过预处理的比力数据需要进行投影转换。该转换是将比力从IMU载体坐标系（b系）转换到导航坐标系（n系），这里需要使用姿态更新输出的载体最新姿态信息。然后，使用引力模型，将比力转换为加速度。最后，根据速度更新离散化方程更新载体的三维速度，这里需要使用位置更新输出的载体最新位置信息。INS航位推算速度更新方程如下：

其中，，分别表示tk时刻和tk-1时刻智能网联汽车在导航坐标系（n系）下的三维速度信息；表示tk时刻智能网联汽车的姿态余弦矩阵；∆v（tk）表示智能网联汽车IMU三轴加速度计测得的比力； 表示由哥氏加速度、向心加速度、重力加速度共同产生的速度增量补偿项。在速度更新时，需要进行旋转效应补偿及划桨效应补偿[11]。

旋转效应是由载体的线运动方向在空间旋转而引起的，其离散化计算方程如下：

其中，∆θk表示tk时刻智能网联汽车中IMU测得的角速度增量；∆vk表示tk时刻智能网联汽车中IMU测得的加速度增量。

划桨效应是当载体沿纵轴作线振动的同时又沿横轴作同频同相的角振动时产生的，其离散化计算方程如下：

其中，∆θk，∆θk-1分别表示tk时刻和tk-1时刻智能网联汽车中IMU测得的角速度增量；∆vk，∆vk-1分别表示tk时刻和tk-1时刻智能网联汽车中IMU测得的加速度增量。

从上述旋转效应和划桨效应方程中可以看出，旋转效应和划桨效应产生的必要条件是角运动和线运动的组合运动。由于智能网联汽车的运动皆属于低动态场景，不存在剧烈的角运动和线运动组合。因此，在INS速度更新时，其旋转效应和划桨效应补偿量为零。那么，在智能网联汽车的INS航位推算时，旋转效应和划桨效应补偿对导航误差的影响极其微弱。从上述方程中也可以看出，旋转效应和划桨效应补偿皆属于高阶项。由此，从理论分析可知，在INS速度更新时省略旋转效应和划桨效应这两项高阶补偿项，可以在不影响定位精度的前提下，提升INS的解算速度。

2 实验验证

2.1 试验准备

为了验证上述结论，我们采集了真实的智能网联汽车车载测试数据进行分析。车载测试数据来源于一辆智能网联汽车，是由比亚迪秦改装而成的智能网联试验平台。车辆上搭载了一个车载级IMU传感器和GNSS接收机模块。

IMU来源于德国博世集团（Bosch），型号为SMI130。Bosch SMI130是一款汽车微机电系统（Micro-Electro-Mechanical System，MEMS）传感器。SIM130包含不同测量范围的16位数字陀螺仪和12位数字加速度计，它结合了来自消费类电子产品和汽车电子产品的技术，具有多编程选项、低信号噪声、易于集成、低功耗、较小占位空间等优势，因此在汽车应用中通用性强。IMU关键参数如表1所示。

GNSS接收机模块是u-blox NEO-M8N。NEO-M8N可同时接收GPS、GLONASS（格洛纳斯，苏联/俄罗斯研制）、北斗、Galileo（伽利略，欧盟研制）卫星信号，具有低成本、低功耗、高灵敏度、性能佳等优势。GNSS模块NEO-M8N的关键参数如表2所示。

车载验证测试于2019年5月20日在北京回龙观进行，测试场景为室外较开阔路段兼室内地下停车场场景。测试时长为半小时，路程长15 千米。

2.2 试验结果分析

为了对比速度更新中高阶补偿项对定位精度的影响，我们将上述实车测试数据采集到实验室后，采用多传感器融合算法平台进行定位解算和误差分析。GNSS和惯性导航系统的融合采用松组合的模式，使用GNSS接收机输出的位置和速度信息作为GNSS/INS组合导航系统的测量输入，即可使用卡尔曼滤波器来估计IMU误差。随后卡尔曼滤波器估计的IMU误差可以对INS输出的导航信息进行校正，校正后的INS导航结果即可输出使用。在GNSS/INS组合导航系统中，惯性导航航位推算算法采用前文所述步骤进行。下面将对省略速度更新的高阶补偿项和加入高阶补偿的测试结果进行对比分析。

图2展示了INS速度更新时加入和省略高阶修正项的轨迹图，从图中可以看出，两者的轨迹非常接近。

图3展示了省略和加入高阶补偿项的位置差距、速度差距和姿态差距，表3列出了其统计值，其中位置、速度和姿态误差的单位分别是米、米每秒、度。

从图3和表3中可以看出，在速度更新时，省略旋转效应补偿及划桨效应补偿：

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

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

3）三维姿态包括横滚角、俯仰角和航向角，其中横滚角差距最大值为0.14度，均方根值为0.02度；俯仰角差距最大值为0.10度，均方根值为0.01度；航向角差距最大值为0.17度，均方根值为0.03度。

从以上结果可知，在智能网联汽车的惯性导航算法解算中，省略旋转效应补偿及划桨效应补偿，其位置差距为厘米级，姿态差距小于0.2度，因此，智能网联汽车惯性导航算法在速度更新时，可以省略旋转效应和划桨效应补偿。

此外，针对该组测试数据，我们通过多次运行算法，对比测试了高阶补偿项对算法运行效率的影响。试验分别对省略高阶补偿项和加入高阶补偿项的定位算法各运行1 000次，统计其运行时间的平均值，得到以下结果：

1）在GNSS和IMU组合导航定位解算时，加入高阶补偿项，离线解算完该组测试数据（IMU采样率为10 Hz，GNSS采样率为1 Hz，测试时长30分钟，测试距离15千米）的时间平均值为17.057 9秒。

2）针对相同的测试数据，在省略高阶补偿项时，其离线解算时间平均值为15.827 7秒。

从以上结果可知，每进行一次IMU航位推算，加入高阶补偿的运行时间比省略高阶补偿项的运行时间多0.068毫秒。

我们知道，智能网联汽车要实现完全无人驾驶，需要满足低时延的要求[12]。从以上结果可知，在智能网联汽车的定位模块中，惯性导航系统在进行速度更新时，无须进行高阶的旋转效应补偿及划桨效应补偿。省略这两项高阶补偿，既不会影响智能网联汽车的定位精度，也可以降低智能网联汽车定位算法的计算量，从而提升智能网联汽车的计算效率，进一步助力实现无人驾驶低时延的目标。

3 结 论

多传感器融合定位算法是当今智能网联汽车最为重要的定位系统，而IMU是定位系统中必不可少的传感器。本文通过实车测试，验证了INS航位推算过程中，无须进行速度更新的旋转效应和划桨效应补偿。省略两项高阶补偿项，不仅对定位、测速和定姿结果影响微弱，同时，也能在一定程度上降低定位算法的计算量和缩短定位解算时间，从而满足智能网联汽车低时延的目标。

参考文献：

[1] 蒋孙权.无人驾驶汽车关键技术研究 [J].时代汽车，2020（3）：25-27.

[2] 钟振，王祥.基于RTK的GNSS/INS实时组合导航系统设计 [J].现代信息科技，2021，5（12）：72-74+79.

[3] 尹皓，李海滨，王利利.基于多传感器融合的移动机器人定位研究 [JTdqJpnZizy+1FQfNsngpfRWnFqNXqf2jm98t/twnPMQ=].机床与液压，2021，49（9）：6-10.

[4] 张文玥，娄小平，陈福笛.移动机器人多传感器融合定位仿真研究 [J].计算机仿真，2023，40（3）：436-441.

[5] 梁高丽，邓仕军，雷浩.一种基于多传感信息融合的定位算法研究 [J].现代信息科技，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] 龚琳琳，张全，牛小骥.惯性导航算法的载体高动态特性研究 [J].导航定位学报，2015，3（2）：71-75.

[12] 毛广辉.自动驾驶芯片需要满足高吞吐与低时延 [J].电子产品世界，2020，28（12）：10+14.