一种基于六自由度IMU和动力学的车身姿态和侧向速度估计方法

本论文由同济大学智能车研究所夏新博士，熊璐教授，刘伟硕士和余卓平教授根据其车辆状态估计方面研究成果撰写完成，已收录于2018 IEEE IV。本文献提出了一种基于六自由度IMU和动力学的车身姿态和侧向速度估计方法，文中用IMU结合动力学和运动学分别为估计方法设计了三个估计器，展现了作者对IMU、估计算法的研究深度和应用广度，值得分享。

摘要

本论文利用六自由度惯性测量单元 (Inertial Measurment Unit) 结合车辆动力学提出了一种车身姿态和侧向速度的估计方法。该估计方法仅依赖于IMU和车辆动力学，无需依赖其他外部信息源如GNSS(Global NavigationSatellite System)、视觉等。本文使用常用的卡尔曼滤波算法设计了三个估计器：基于IMU信息设计了姿态角估计器用于估计俯仰角和侧倾角；基于IMU设计了侧向速度估计器；基于车辆动力学模型设计了车辆侧向速度估计器。在小侧向激励的工况下，基于车辆动力学的侧向速度估计器的侧向速度估计结果相比于基于IMU的估计结果更加可靠，该侧向速度可用于修正两个基于运动学模型估计器的累积误差。在大侧向激励工况下，由于车辆动力学模型的模型精度较低，车辆动力学模型的侧向速度估计结果精度也较低，此时基于IMU的姿态角估计器和侧向车速估计器进行开环运算。最后进行了蛇行和双移线工况的实车实验，验证了算法的有效性。

1前言

自动驾驶技术的发展衍生了大量前沿问题。其中，准确的车身姿态和车辆侧向速度估计方法成为了研究热点。从感知层面来看，如车载视觉模块的图像处理算法通常需要已知车身姿态信息，又如车身姿态和速度信息可辅助高精度定位；从车辆动力学控制角度看，侧向车速是车辆转向控制的基础。

能够直接测量车身姿态和侧向速度的设备价格十分昂贵，如OxTS公司的RT3000或者是Kistler公司的S-Motion，无法用在量产车上。更加实际可行的实现方法是融合智能车上多源传感器信息对其进行估计。然而，传统车载级MEMS（Micro-Electro-Mechanical System）IMU的角速度和加速度传感器测量值中通常伴随有不稳定零偏，长时间对其直接积分来获取车身姿态和车辆侧向车速将面临较大的累积误差。尽管通过GNSS或者摄像头模块能够对MEMS IMU提供有效的辅助信息，但是GNSS和摄像头在使用时面临信号遮挡和多路径、光线变换等问题，通过GNSS和摄像头辅助IMU方式有待进一步研究。

此外，可使用车辆标配传感器如方向盘转角和轮速传感器的输出，用于基于车辆动力学模型的侧向车速估计器。使用该估计结果可计算车辆由于平动产生的侧向加速度，去除该部分加速度后，加速度测量值中的余下部分即可用于车辆姿态估计。但在极限工况下，由于模型失准，基于动力学模型的侧向车速估计器的估计精度会下降。

本文利用6自由度IMU（三轴陀螺仪和三轴加速度计）和车辆动力学对车身姿态和侧向速度同时进行估计。设计了基于运动学模型的车辆侧向速度估计器。在小侧向激励工况下，基于动力学模型的估计器能够给出精确的侧向速度。鉴于基于动力学模型的侧向车速估计器和基于运动学模型的估计器能同时输出侧向速度，两个结果的偏差能够作为反馈，补偿基于运动学模型的侧向速度估计器在小侧向激励工况下的累积误差。此外，由基于动力学模型的估计器输出的侧向速度，能够抵除侧向加速度中由运动产生的加速度，从而利用重力产生的加速度分量计算姿态角。在极限工况下，切断反馈信息，利用开环积分直接计算姿态和侧向速度。MEMS在剧烈工况下进行短时的积分不会产生大的累积误差，文末的蛇行和双移线实验也验证了这一点。

2姿态及侧向速度估计

2.1.过程模型

a.传感器模型

通过Allan方差分析IMU中的误差源，陀螺仪和加速度计的输出中包含真值，常值零偏和随机游走项误差以及宽带噪声项误差。通过一阶马尔科夫模型来描述随机游走误差。τ是时间常数，Wb是宽带噪声。陀螺仪测量模型如下式给出：

加速度模型建立如下：

下标s表示传感器的测量值，上标·表示变量的导数，Φ，θ和φ分别是侧倾，俯仰和横摆角，a下标x,y,z分别是纵向，侧向，垂向加速度。

b.欧拉角和速度动力学

选择欧拉角来表征车辆的姿态。旋转顺序是Z-Y-X。绕每根轴旋转就得到了横摆角、俯仰角和侧倾角。欧拉角动力学由下式给出：

车身的速度动力学由下式给出，v下标x,y,z分别是纵向，侧向和垂向速度。g是重力加速度。

c.轮胎模型

估计算法的精度取决于动力学模型的准确性，特别是轮胎模型。考虑到侧向力存在非线性情况，本文中采用了刷子轮胎模型。假设不存在纵向力，刷子轮胎模型的公式如下：

其中，μ是峰值轮胎路面附着系数，α是轮胎侧偏角，Fy是侧向力，Fz是轮胎垂向力，d是轮胎接地印记长度，c1是侧向力侧偏刚度。

d.车辆动力学

基于单轨模型的车辆动力学方程由下式给出，其中γ是横摆角速度，Fyf和Fyr分别是前后轴侧向力，Nz是由车轮纵向力造成的横摆力矩，m是总质量，lf和lr分别是前后轴侧向力作用点到质心的距离，Iz是转动惯量，是状态变量。

为了计算轮胎力，侧偏角由下式获得：

2.2测量模型

如前文所述，本文建立了三个估计器，在小侧向激励工况下，可通过基于动力学模型的估计器估计出侧向车速，进一步使用该侧向车速估计出由于平动产生的侧向加速度，加速度传感器测量值剔除该平动加速度后，余下的部分即为由于姿态角导致的重力在加速度传感器测量值中的分量。由于本文关注的是姿态和侧向速度估计，因此假设在小纵向加速度情况下，纵向速度可直接从轮速传感器获取。然后，我们给出图1和图2来判断基于动力学模型估计器输出的侧向速度能否用于侧倾角反馈以及侧向速度的运动学估计，并判断轮速传感器输出的纵向速度能否用于俯仰角及纵向速度反馈。

图1 姿态估计反馈机制

图2 速度估计反馈机制

之后我们将用EKF(Extended Kalman Filter)和KF(Kalman Filter)分别估计姿态和侧向速度。从上面两个逻辑图可以看出，当车辆处于小激励情况下，新息可以用于时间更新过程，当车辆处于大激励运动时，反馈被切断。

动力学模型估计器一直处于运行状态，该估计器的测量值为横摆角速度。

2.3.估计方法

本文使用常用的EKF和KF滤波算法进行状态估计。欧拉角动力学是非线性的，我们使用扩展卡尔曼滤波（EKF）进行姿态估计。基于运动学的估计器中模型是线性的，因此选用卡尔曼滤波（KF）进行速度估计。对于估计侧向速度的动力学观测器，使用扩展卡尔曼滤波（EKF）处理轮胎模型的非线性特性。模块整体结构如图3表示，绿色表示传感器模块，两个黄色模块表示图1和图2的反馈机制。

图3 估计方法结构示意图

3实验验证

3.1.实验配置

硬件配置如图4所示，ADIS16445是一款包含三轴陀螺仪和三轴加速度计的IMU。XC2287是一款英飞凌生产的微控制单元，用于从ADIS16445读取数据，使用SPI通讯协议将信号从ADIS16445中读取出来，然后以500Kbps发送到CAN总线上。S-motion用于提供侧倾，俯仰角和侧向车速参考值。NI的CompactRIO用于数据采集。所有的硬件采集设备如图5所示。

图4 硬件配置图

图5 实验硬件示意图

3.2.实验结果

为了证明估计方法的有效性，在速度50~60Km/h的蛇行和双移线工况下进行了实验，实验结果如下所示。

a.蛇行实验

(a)侧向加速度

(b)纵向速度

(c)侧倾角

(d)俯仰角

(e)侧向速度

图6 蛇行工况实验结果

b.双移线实验

(a)侧向加速度

(b)纵向速度

(c)侧倾角

(d)俯仰角

(e)侧向速度

图7 双移线工况实验结果

两种工况下纵向速度保持在50~60Km/h，而最大侧向加速度达到了10m/s^2，表明车辆达到了极限工况状态，本文的估计方法依然可以较好地估计姿态和侧向速度。对于侧倾角，虽然开环积分时会存在一定的累积误差，但是由于侧倾角反馈机制，后面侧倾角的估计值接近于S-motion的测量值。由于俯仰角绝对值很小，即使轮速传感器存在很小的纵向速度误差，对结果也可能造成较大的俯仰角估计误差。在反馈逻辑中，我们把a0取为较小值，导致俯仰角的反馈持续时间小于侧倾角的反馈时间，这也解释了为什么俯仰角估计误差大于侧倾角估计误差。从图6(e)我们得知，蓝线跟踪红线的效果比绿线要好，表明使用基于车辆动力学模型估计出的侧向速度，能够对运动学模型的侧向速度和姿态估计提供有效的反馈。最大侧向速度估计误差小于0.3 m/s显示了较好的估计性能。从双移线工况也看出来相同的估计结果。

4结论

本文提出了智能车的姿态和侧向速度估计方法。这种方法融合了6自由度IMU和车辆动力学信息，不需要外界信息的辅助。考虑到IMU和车辆动力学的互补特性，我们建立了三个估计器：一个基于运动学模型的估计器用于估计姿态，一个基于运动学模型的估计器用于估计侧向速度，一个车辆动力学估计器用于估计侧向速度。在小激励情况下，车辆动力学估计器获得的侧向速度能够有效辅助另外两个估计器，即作为反馈修正运动学模型的姿态和侧向速度估计器的估计结果。在大侧向激励情况下，基于运动学模型的估计器独立运行。以上估计方法通过蛇行和双移线等极限工况证明了有效性。