基于传感器融合的户外道路下无人驾驶卡车姿态估计方法研究∗

王翔昌 吴训成 张伟伟

（上海工程技术大学 上海 201620）

1 引言

近年来，车辆主动安全系统已经迅速发展，例如，辅助停车［1］，车辆稳定性（包括偏航稳定性［2］，侧倾稳定性［3］和悬架［4］）控制系统和主动转向［5］。对于这些主动系统，获得自身的姿态是首要任务，包括车辆姿态角，速度和加速度。然而，传统传感器的传感精度很容易受到无人驾驶卡车质量和体积过大的影响。因此，无人驾驶卡车在车辆姿态估计［6］、环境特征检测［7］、障碍物检测［8］等方面还有很长的路要走。目前，惯性测量单元是获取车辆姿态的典型手段［9］。然而由于传感器的漂移问题，单纯依靠惯性测量单元无法准确估计车辆的姿态。微机电系统惯性测量单元包含三轴陀螺仪和三轴加速度是可选的解决方案。Wu［10］利用包含陀螺仪的低成本惯性测量单元来获取车身姿态信息，然而，陀螺仪易受到漂移问题的影响，使得测量结果出现误差。Ahmed［11］将陀螺仪和加速度计结合起来克服陀螺仪的漂移问题，但是该方案会随着时间累积噪声和偏差。对于道路环境的感知，有许多采用激光雷达进行道路障碍物检测的研究［12～15］。例如，Peng［16］采用激光雷达进行动态障碍物检测、跟踪与识别。Duan［17］采用多层激光雷达进行道路与障碍物信息提取。然而这些方法仅是独立使用激光雷达提取道路特征，并未与惯性测量单元进行融合，难以解决惯性测量单元出现的滑移现象。

本文针对无人驾驶卡车，考虑到车辆行驶在户外环境的不平整道路上，车辆在经过障碍物时车身姿态会在短时间内急剧变化。增加激光雷达来获取车身周围环境特征，包括障碍物的距离和尺寸信息，来帮助车辆获取更精确的姿态信息。不同于以往方法，本文设计了基于卡尔曼滤波的无加速度模型，来克服微机电惯性测量单元随着时间累积噪声和偏差的问题。融合激光雷达和微机电惯性测量单元来有效获取车身姿态信息，为后续的无人驾驶路径规划等应用提供基础姿态信息。

2 参考坐标系与车身姿态表示

2.1 参考坐标系

参考坐标系包括地面参考系和用于安装惯性测量单元和激光雷达的车身坐标系。参考坐标系的示意图如图1所示，地面坐标系和车身坐标系分别用PI和PV来表示。

图1 参考坐标系示意图（地面坐标系用PI来表示，车身坐标系用PV来表示）

地面坐标系选取东、北和上作为坐标轴朝向，具体可有式（1）来表示：

车身坐标系与车身纵向、横向和垂向对齐，作为安装惯性测量单元和激光雷达的坐标系。具体可由式（2）表示：

2.2 车身姿态表示

其中，c和s分别表示cos和sin，θ，ϕ和ψ分别表示俯仰角、横摆角和航向角。至此我们便可以利用转移矩阵进行俯仰角和横摆角的表示：

3 道路特征获取与姿态估计

3.1 道路障碍物检测

在本节，我们基于激光雷达的单线扫描模型，通过分析激光扫描线在凹、凸障碍物上的距离特征，建立多线扫描几何模型，获取道路特征。激光扫描线扫描凹、凸障碍物的几何特征如图2所示，雷达扫描线与障碍物分别相交于P1，P2，P3和P4。可以看到激光扫描线在不同的障碍物上呈现不同的特征，例如在图2（a）中，要比小，而在图2（b）中，要比大。

图2 凹、凸障碍物的几何模型

结合激光雷达的参数，激光扫描线在障碍物的分布可由式（7）计算得到：

其中，M为扫描线扫描一周的扫描点数，D为激光雷达与障碍物之间的距离，L为雷达相对于地面的安装高度。

由此我们可以得到激光雷达多线扫描几何模型由式（8）表示：

其中，t表示扫描点的个数，Num表示障碍物上的扫描点的个数。

3.2 扩展卡尔曼滤波器

扩展卡尔曼滤波器被设计用于非线性系统的信息融合［18］，本文用于对传感器融合系统进行时间和测量更新，以避免惯性测量单元随时间累积噪声和误差。其原理可由式（9）、（10）来表示：

其中，xt为状态向量，包含加速度、速度、俯仰角、横摆角，ut和yt分别表示非线性系统的输入和输出，vt和ωt分别表示过程和测量噪声。扩展卡尔曼滤波的整体流程可由下式表示。

1）时间更新

其中，xt|t-1为预测的状态估计，Pt|t-1为预测的协方差估计。

2）测量更新

其中，Zt为测量冗余，St为冗余协方差，Kt为滤波器增益，xtt为更新的状态估计，Ptt为更新的协方差估计。

3.3 无加速度模型设计与姿态估计

在车辆运动的短时间内，陀螺仪具有较高的测量精度，但是长时间会产生累积误差影响测量精度，而加速度计可以获得更精确的静态角度，因此加速度计获取的加速度可用于测量更新。然而加速度计的测量易受车辆运动的干扰，因此我们设计基于卡尔曼滤波器的无加速度模型，在消除车辆运动影响加速度计测量结果的同时，减少陀螺仪随时间累积的噪声和误差。

加速度计的测量结果、重力加速度以及外部加速度分别用At、gt和at来表示，包含横向、纵向以及垂向加速度。将加速度计测量结果减去外部加速度即可得到不受外部运动影响的加速度值，具体可由式（18）、（19）、（20）表示：

然后利用3.2节所推导的扩展卡尔曼滤波器进行外部加速度的估计与跟踪，完成无加速度模型的设计。

其中，at为外部加速度，αt为外部加速度的线性组成部分。

俯仰角和横摆角可通过更新的测量模型计算：

至此完成道路特征的获取以及基于卡尔曼滤波器的无加速度模型的建立并完成了姿态的估计与更新。

4 验结果与分析

本节中，我们进行实验验证和结果分析，将所提的方法与现有的方法进行对比比较，验证所提方法的优越性。

对比实验选取分布障碍物的测试道路，选取的对比方法一仅使用加速度计进行姿态估计，方法二使用加速度计和陀螺仪进行姿态估计。测试道路的轨迹图如图3所示，惯性测量单元和雷达的安装位置如图4所示。

图3 测试道路轨迹

图4 惯性测量单元和激光雷达的安装位置

激光雷达对道路特征的提取如图5所示，姿态估计结果如图6所示，我们可以看到，在有了道路障碍物的距离和尺寸信息，所提的加速度模型能够准确地移除车辆形式过障碍物时导致的外部姿态的变化，进行准确的自身姿态估计。而相比之下，方法一仅使用加速度计进行姿态估计容易受到传感器滑移问题的影响，方法二未采用扩展卡尔曼滤波器进行噪声的移除以及外部加速度的干扰，导致结果较真实值有很大的偏差。

图5 激光雷达道路特征提取结果

图6 姿态估计结果

5 结语

本文针对行驶在户外道路上的无人驾驶卡车，提出了一种用于精确姿态估计的方法，基于卡尔曼滤波的无加速度模型结合激光雷达提供的道路特征，能够有效地移除道路障碍物带来的外部加速度干扰，较现有的方法，极大地提高了姿态估计准确率，能为后续的路径规划等应用提供了精确的自身姿态信息。