基于卡尔曼滤波算法的轨迹预测

邱 润，黎敬涛，李孝疆，杨改娣，宋开雨

（昆明理工大学，云南 昆明 650504）

0 引言

自从20 世纪70 年代美国国家航空航天局（NASA）运用卡尔曼滤波解决阿波罗登月计划的轨道预测问题后，卡尔曼滤波突破了当时工程领域维纳滤波的局限，其原理是通过观测值与理论实际数据融合得到当前的最佳预估值[1]。观测数据中包含噪声，卡尔曼滤波就是对包含噪声的数据进行处理，卡尔曼最佳估计也可以看作是一个滤波过程[2]。卡尔曼滤波有两种建模方法：一种是基于控制理论通过运动模型根据前一时刻的状态来估计得到当前时刻的最佳估计值与协方差矩阵，以此类推进行下一时刻的最佳估计，这个建模求解的重点在于卡尔曼增益；另一种是利用一些概率性质进行建模的概率模型。本文将卡尔曼滤波运用到行车的轨迹预测中，通过运动系统方程、测量模型以及它们的误差来求得卡尔曼增益，随后用卡尔曼滤波后的值反过来修正误差，从而在行车过程中获得最佳的轨迹预测。

1 卡尔曼滤波理论

1.1 卡尔曼滤波的预测过程

（1）通过状态估计公式得到此时刻的先验估计值，即：

式中：和ut-1分别为上一时刻的先验估计值和控制量，A为状态转移矩阵，B为控制矩阵。

（2）通过当前时刻的先验误差来表示系统包含的不确定性，即：

式中：Pt-1为上一时刻的误差，Q为过程噪声的方差。

在车辆的运行过程中，受传感器测量精度问题的影响会存在方差为R的测量噪声Vt。而建立的运动系统也不是百分百符合真实运动，即存在方差为Q的过程噪声w。这两个噪声都服从期望为零的标准正态分布，即：

根据观测矩阵H、先验误差以及测量噪声的方差R，计算卡尔曼增益系数为：

（3）通过先验估计值、真实值Zt与卡尔曼增益Kt，计算最佳估计值为：

随着车辆运动的进行，系统会不断积累误差，需要通过协方差矩阵来不断更新误差[3]，即：

式中：Pt为更新后的误差，I为单位矩阵。

1.2 卡尔曼滤波的最新研究

卡尔曼滤波拥有广泛的应用领域，例如故障诊断[4]、系统监测[5-6]、巡航制导等[7]。文献[8]在精密工程中运用扩展卡尔曼滤波进行故障诊断，对工程结构进行探伤，成功降低了测量噪声的影响，准确锁定损伤位置并掌握损伤程度。文献[9]将卡尔曼滤波运用到建筑工程领域，通过建立时序预测模型对大坝的渗流、位移等检测数据做出预测，从而掌握数据的变化趋势，实现对大坝危险情况的预测。袁等人[10]运用低成本深度传感器收集行人运动的数据，通过卡尔曼滤波获取状态的预估值，从而获得行人的道路轨迹预测，弥补了传统轨迹预测算法无法挖掘行走意图的缺陷。

2 构建小车运动模型

2.1 简化阿克曼运动模型

在全局坐标系XOY下，连接车辆重心（A，B两点），将车辆的正前方视为横轴指向（OB）、车辆的左侧面视为纵轴指向（OA），建立参考坐标系xoy，如图1 所示。严格来说，需要根据阿克曼转向理论建立车辆的运动模型[11]。但是在合理的假设条件下，可以将车辆的运动看作是质点运动，将阿克曼转向几何模型简化成自行车运动模型。将车辆前后四轮简化为前后各一个轮，前后轮间用车轴进行连接[12]。

图1 阿克曼转向理论简化模型

选取参考点C，ψ为车辆方向的横摆角，δ为车辆转向角度，β为车辆滑移角（车辆的航向与中心点速度Vc的夹角），L为小车的轴距。假设C 到前轮的距离为Lf，C 到后轮的距离为Lr，则Lf+Lr=L。对于自行车运动模型来说，小车每时每刻的速度方向都与过车身质点的旋转半径相垂直，即A 点的旋转半径RA（OA）垂直于Va，B 点的旋转半径RB（OB）垂直于Vb，C 点旋转半径R（OC）垂直于Vc。在相同的时间间隔内，小车在X方向和Y方向上位移的增量可以通过速度V来表示。

根据横摆角计算起始两点在全局坐标上的分速度，可以构建微分方程：

将点C 选在车辆后方，此时Lr=0，则Lf=L，这样就可以达到任意时刻车辆的滑移角β为0。根据角速度公式可以得到：

通过车辆转向角度δ可以计算出车辆的转弯半径R，根据速度进而可以得知横摆角ψ。根据横摆角度ψ，可以在单位时刻求出车辆在全局坐标系X方向的位移ΔX和在Y方向的位移ΔY，则小车的位移模型为：

在全局坐标系中，当车辆平行于X轴运动时，初始t=0，摆角ψ0=0。当小车转向δ0角度行驶到下一时刻（t=1）时，摆角更新为ψ1=ψ0+δ0=δ0；当t=2、转向角为δ1时，摆角更新为ψ2=ψ1+δ1=δ0+δ1。由此可知，车辆摆角与转向角的关系为ψt=ψt-1+δt-1。

2.2 卡尔曼滤波轨迹预测建模

将模型预测控制理论与状态参数估计相结合，以此来预测车辆的运动轨迹[13]。根据小车在建模过程中的运动系统是否符合线性系统特征，可以将其运动具体区分为两种状态。一种是无转向角时的直线运动，属于线性运动系统；另一种是存在转向角而不满足线性要求的转向运动，属于非线性运动系统。在具体应用时，必须先将非线性系统线性化。

直线运动模型的输出只取xt和yt的值，则根据观测方程得到：

将小车的运动看作是简单的自行车运动模型，当小车转向时，车辆的转向角度等于车辆前轮的偏角，如图2 所示。

图2 全局坐标系下的车辆运动

在全局坐标系中，小车通过单位时间由上一位置St-1行驶到当前位置St，空间状态方程方程为：

由于转向运动系统的输入方程包含三角函数，属于非线性系统方程，因此需要通过泰勒级数将其线性化。通过状态量X=[xy]T与控制量u=[vψ]T得到相应的雅克比矩阵A与B，即：

同直线运动一样，这里输出也只取xt和yt的值，根据观测方程得到观测矩阵H为单位矩阵。

3 卡尔曼滤波预测模型仿真分析

在Simulink 中进行实验，建模主要分为车辆的物理建模部分和卡尔曼滤波部分。自行车纵向运动物理模型如图3 所示，其中WhlAngF 为前轮转向角，xdotin 为横轴方向的速度，xdot 为车辆的横向速度，ydot 为车辆的纵向速度，psi 为偏航角，r为偏航率，Fz-F 与Fz-R 为前后轮的法向力。此外，Info 端口包含该模型的所有信息，可以通过连接一个总线选择器来选择实验所需的信息。

图3 自行车运动模型

通过设置初始的横向速度和车辆转向角，得到车辆在某仿真时刻的横向速度和纵向速度，进而可以得到某时刻的车辆状态。设定横轴方向速度为2 m/s、纵轴速度为0 m/s、仿真总时长为20 s、过程误差为0.001 且测量误差为0.1，则小车无转向角的直线运动轨迹仿真如图4（a）所示，小车转向角45°下做转向圆周运动的轨迹仿真如图4（b）所示。

图4 轨迹仿真结果

实验表明，卡尔曼滤波后的预测轨迹相比于观测轨迹更趋近于真实轨迹。

车辆在直线运动中真实轨迹值与测量轨迹值间的误差如图5（a）所示，真实轨迹值与卡尔曼滤波预测轨迹值间的误差如图5（b）所示。

根据图5，在直线运动t=6.9 s 时，测量误差约为2.2 m。经过卡尔曼滤波后，误差约为1.2 m，与测量误差相比减小了约1.0 m，证明卡尔曼滤波有效降低了误差。

图5 误差对比

小车直线运动位移仿真结果如图6（a）所示，转向运动位移仿真结果如图6（b）所示。

图6 运动位移仿真

由图6（a）可知，由于转向角为0 且过程误差较小，因此小车实际的位移呈线性增加，而且滤波后的估计值更趋近于真实值。由图6（b）可知，由于测量值与真实值间的误差较大，因此波动幅度也较大，通过卡尔曼滤波后的值相较于测量值更贴近于真实值。与此同时，由于小车从原点开始做圆周运动，因此从t=0 时刻开始，小车的位移先增加再减少，此后循环增加再减少。

4 结语

本文根据小车运动方程建立小车运动物理模型，通过Simulink 进行实验仿真，验证了卡尔曼滤波的轨迹预测的可行性。实验表明，卡尔曼滤波后的轨迹比观测出来的测量轨迹更加贴近于真实值，在测量误差的基础上降低了估计误差。但是运用自行车运动模型对车辆建模进行了简化，未来将基于详细具体的阿克曼转向原理进行建模分析。