基于卡尔曼滤波算法的汽车运动参数测试方法研究

文章编号:10056734(2001)03001905

基于卡尔曼滤波算法的汽车运动参数测试方法研究

倪江生

(东南大学,江苏南京210096)

摘要:提出了一种采用组合惯性测量元件进行汽车运动参数测试的方法,并利用卡尔曼滤波算法提高了测试系统的精度。仿真结果表明:卡尔曼滤波算法对汽车姿态、速度等的解算具有较好的效果。

关键词:惯性系统;运动参数;卡尔曼滤波

中图分类号:666.123;467.11　　　　　　　　　文献标识码:A

M easurem en t M ethod for Autom obile M otion Param eters

Based on Kal man F ilter

N I J iang sheng

(Sou theast U n iversity,N an jing210096,Ch ina)

Abstract:A new m easu rem en t m ethod u sing m u lti dem en si on in tegrated inertial sen so r fo r

au tom ob ile m o ti on p aram eters is p ropo sed in th is pap er,and the m easu rem en t p recisi on is i m2 p roved based on the Kal m an filter.T he si m u lati on resu lts show that the Kal m an filter algo2 rithm is effective in calcu lating the attitude and velocity of au tom ob ile.

Key words:inertial system;m o ti on param eter;Kal m an filter

1　引　言

汽车的整车运动性能,如操纵稳定性、制动性等,是道路行驶的重要技术性能,关系到汽车的行驶安全。随着高速公路及高速车辆的迅速发展,道路交通安全日益重要,相应地对汽车性能测试的要求也越来越高。

汽车运动性能的道路试验离不开运动参数的测试,如汽车的运动速度、加速度、运动轨迹、转向角速度、姿态角等的测试。目前,这些参数都依靠各个单独的仪器进行测试,主要利用五轮仪、非接触式车速仪以及方位陀螺仪、垂直陀螺仪等仪器。这样构成的测试系统信息量少、测试参数不够完整,而且使用不方便。因此,研究一种体积小、功能强、使用方便的便携式路试系统具有重要的实际意义和经济价值。

近年来,各种微小型廉价惯性元件发展迅速[1-2],面向汽车用的廉价产品更日益受到关注。这些惯性元件一方面用于车辆的辅助导航,另一方面用于汽车的电子控制,如转向控制、AB S等。本文针对惯性元件在汽车上的应用,提出一种车辆道路试验的测试方法,采用廉价的集成多微惯性传感器测量车体的运动信息,并基于扩展卡尔曼滤波算法计算汽车的运动参数。仿真结果表明:这种方法是完全可行的。

2　测试系统的构成及基本原理

本系统的核心测量元件采用六维压电式惯性传感器,即三向加速度计及三向速率陀螺。从理论上讲,空间六维信息可以确定一个刚体在空间的位置及其运动参数。测试系统的构成如图1所示。

基金项目:江苏省汽车工程重点实验室开放基金项目(K99043)

收稿日期:20010602

作者简介:倪江生(1962-),男,东南大学仪器科学与工程系副教授,从事精密仪器及测试技术专业。

图1　测试系统框图如图1所示,加速度计及陀螺仪分别用于测量车体沿车体坐标系X ,Y ,Z 三个方向的加速度和旋转速率,这种压电式惯性元件的优点是体积小,使用方便,很适合汽车的道路试验。系统还采用了汽车测试中常用的非接触式光电测速传感器,虽然六维惯性元件已经可以测量所需信息,但由于低价元件性能不高,解算的积累误差增长较快,完全依赖惯性测量信息有时不能满足要求,因此采用非接触式速度传感器有利于提高系统测试精度,增加数据处理的灵活性。除上述传感器以外,汽车性能测试还需踏板力计及扭力方向盘等。传感器的信号经必要的

接口及调理电路进行外置式AD 转换器,然后连接便携式计算机的并行口,计算机对测试数据进行处理并保存。

测试系统的关键是通过惯性元件的信号解算出汽车运动的各种参数,将捷联式惯性导航的原理应用于短时惯性测量系统,从而得出满足汽车性能测试要求的数据。3　扩展卡尔曼滤波器的设计3.1　系统状态方程

测试系统的状态向量设为:

X (k )=

X GX (k )X A X (k )

X GY (k )X A Y (k )X GZ (k )X A Z (k )

,　　

X GX

(k )=[Η(k )　Η

(k )　Η¨(k )　Η…

(k )]′,X A X

(k )=[x (k )　x

(k )　x ¨

(k )　x …

(k )]′,

X GY (k )=[Χ(k )　Χ

(k )　Χ¨

(k )　Χ…

(k )]′,X A Y (k )=[y (k )　y (k )　y ¨

(k )　y …

(k )]′,X GZ (k )=[Ω(k )　Ω

(k )　Ω¨

(k )　Ω…

(k )]′,

X A Z (k )=[z (k )　z

(k )　z ¨(k )　z …

(k )]′

,(1)

其中,上式中,Η,Χ,Ω分别为车体的俯仰、倾斜、航向角。状态方程如下:

X (k +1)=FX (k )+W (k ),

(2)

其中,状态转移矩阵可表示为

F =

F GX

000000

F A X

00000F GY

000000F A Y

000000F GZ

00

F A Z

,　F GX =F ZX =F GY =F A Y =F GZ =F A Z =

1

T s

12T 2s 16T 3s 01T s

12T 2s 001T s

1

,(3)

式中,T s 为采样间隔。方程(2)中,W (k )为过程噪声,其协方差矩阵为[3]:

Q =

Q G

000000

Q A

000000Q G

000000Q A

000000Q G

00

Q A

,Q G =

T 7s

252

Ρ

21

T 6s

72

Ρ21

T 5s

30

Ρ21

T 4s

24

Ρ21

T 6s

72

Ρ

21

T 5s

20

Ρ

21

T 4s

8

Ρ

21

T 3s

6

Ρ

21

T 5

s

30

Ρ21T 4s

8

Ρ21T 3s

3

Ρ21T 2s

2

Ρ21

T 4s

Ρ21

T 3

s

Ρ21

T 2

s

Ρ21

T s Ρ21

,Q A =

T 7s

252

Ρ

22

T 6s

72

Ρ22

T 5s

30

Ρ22

T 4s

24

Ρ22T 6s

72

Ρ

22

T 5s

20

Ρ

22

T 4s

8

Ρ

22

T 3s

6

Ρ22

T 5s

30

Ρ22T 4s

8

Ρ22T 3s

3

Ρ22

T 2s

2

Ρ22

T 4

s

Ρ22

T 3

s

Ρ22

T 2

s

Ρ22T s Ρ21

。(4)0

2　　　　　　　　　　　　　　　　　　　　中国惯性技术学报　　　　　　　　　　　　　　　　　　　2001年

3.2　系统量测方程

根据文献[4],车体相对地球的加速度与惯性传感器输出之间的关系如下式所示:

V

ep =f -(2Ξie +Ξep )×V ep +g ,(5)

其中,V ・

ep 为车体平台相对地球的加速度,f 为加速度计测得的比力向量,Ξie ,Ξep 分别为地球相对惯性系、车体平台相对地球的转动角速度向量,g 为重力加速度向量。

从方程式(9)可以看出:加速度计的输出信号中除了要测量的车体运动外,还包含了另外两项,一项是重力加速度,另一项是由地球自转和车体绕地球转动而产生的加速度。对于汽车性能测试而言,其特点是车速低,测试时间短,地球运动引起的哥氏加速度和离心加速度很小,而且其积累误差也较小。另一方面,对于民用测试系统来说,一般选用低价的惯性元件,其灵敏度和精度不可能太高,因此对微弱的哥氏、离心加速度难以检测。根据文献[5]的分析,将方程(9)进行简化,如下式所示:

V

ep =f +g 。

(6)　　车体的姿态速率与速率陀螺输出之间的关系如下:

Ξp b =Ξib -(Ξie +Ξep ),

(7)

其中,Ξp b 为车体相对车体平台的姿态速率向量,Ξib 为车体相对惯性系的姿态速率向量,陀螺仪可测出其大小。对于低精度陀螺仪表,地球自转等角速度难于敏感,同时考虑到汽车测试的要求和特点,将方程(7)简化为

Ξp b =Ξib

(8)

根据上式,可得速率陀螺的量测方程为

Ξx Ξy Ξz

=-s Χc Ηc Χ

s Η01

c

Χc Η

s Χ0

Ω

Η

Χ

。

(9)

根据方程(6),可得加速度计的量测方程为:

f x

f y

f

z

=T b p (x

¨y

¨z

¨

+00g

),

(10)

其中,g 为重力加速度,T b p 为捷联矩阵,

T b

p =

c Χc Ω-s Χs Ηs Ωc Χs Ω+s Χs Ηc Ω-

s Χc

Η

　-c Ηs Ω　c Ηc Ω

　s Ηs Χc Ω+c Χs Ηs Ωs Χs Ω-c Χs Ηc Ω

c Χc Η

。

(11)

　　除上述量测变量以外,光电速度计的输出为:

V 2

=x 2

+y 2

,

(12)

上式中,为方便量测方程求偏导数,取速度信号的平方作为输出。另外,取垂直方向的速度作为量测变量,如下式所示:

V z =z

,

(13)

因此,系统量测向量为

Y =[Ξx 　Ξy 　Ξz 　f x 　f y 　f z 　V 2

　V z ],

(14)量测方程为

Y (k +1)=h [X (k +1)]+V (k +1)。

(15)

3.3　扩展卡尔曼滤波算法

1

2第3期　　　　　　　　　　　倪江生:基于卡尔曼滤波算法的汽车运动参数测试方法研究　　　　　　　　　　

①,②,③为x ,y ,z 速率陀螺;④,⑤,⑥为x ,y ,z 加速度计输出

图2

　惯性测量单元输出信号的仿真

图3　设定轨迹和测试仿真轨迹上述系统的扩展卡尔曼滤波算法如下:X ^(k +1)=F (k +1 k )X (k )+K g (k +1)[Y (k +1)-h [X ^

(k +1 k )]],　　　　　　　

K g (k +1)=P g (k +1 k )H ′g (k +1)[H g (k +1)P g (k +1 k )H ′g (k +1)+R (k +1)]-1

,

P g (k +1 k )=F P g (k )F ′+Q (k ),P g (k +1)=(I m x -K g (k +1))H g (k +1)P g (k +1 k )。

(16)

上式中,

H g (k +1)=

5Y (k +1)

5X (k +1)

X (k +1)=X ^

(k +1 k )

　。(17)

4　测试系统的仿真

惯性测试系统最大的问题是随着时间的增长其积累误差快速增大,且误差的发散没有边界。在短时

惯性测量的情况下,且采用低精度测量元件,系统的积累误差如

何是一个决定性的问题。另外,通过惯性传感器的测量信息直接求解运动参数与综合采用惯性传感器和速度传感器并基于卡尔曼滤波算法解算运动参数两种方法相比较在测试误差方面有何不同,下面通过计算机仿真进行进一步的分析。

仿真采用的行驶路径应能充分反映汽车的直线加减速运动和转向运动。设运动轨迹如图3中实线所示,汽车从A 点开始沿Y 向加速度运动,加速度为1m

s 2

,前进30m 后绕半径为15m

的圆周作匀速运动,环绕1

14

圈后沿-X 方向作减速运动,减速

度为1m s 2

最终到达B 点停止。

设系统采用低精度的惯性元件,三向加速度计和三向速率陀

螺都具有较大的零位偏置和随机

误差。设三向加速度计的零位偏

置为10m g (g 为重力加速度单位),随机干扰为10m g ,三向速率陀螺的零位偏置为0.1(°) s 。随机干

扰为0.2(°) s 。另外设光电测速仪的随机误差为1km h 。各随机误差都为均方差。仿真时将惯性元件

的理论输出值加上零位偏置,再加上由程序产生的均匀分布白噪声,然后分别用直接求解法和卡尔曼滤波进行解算,比较两者的结果。直接求解时采用四阶龙格库塔法解方程(9)。

仿真结果如图2、3、4所示。图2表示的是带有零位偏置和随机噪声的量测信号。图3表示了设定轨迹和测试仿真轨迹,其中实线为设定轨迹,短实线为惯性测量信号直接求解结果,长实线为卡尔曼滤波的计算轨迹。图4为测试误差的仿真曲线,即测试轨迹与设定轨迹之差,实线为直接求解法的误差曲线,虚线为卡尔曼滤波的误差曲线。

2

2　　　　　　　　　　　　　　　　　　　　中国惯性技术学报　　　　　　　　　　　　　　　　　　　2001年

从图3可以看到:由于惯性元件的零位偏置较大,单独依靠六维惯性传感器的解算轨迹具有很大误差(图3短虚线)。采用卡尔曼滤波算法,解算轨迹与原设定轨迹比较接近(图3长实线)。

从图4可以看到:在惯性传感器直接解算的情况下,误差随时间快速增长(图4实线),而采用卡尔曼滤波后,误差的增长要缓慢得多(图4虚线),在Z 方向上,速度及位置误差均为零(图4.6,4.9)。不管是姿态角误差(图4①～③)还是速度误差(图4④～⑥)、位置误差(图4⑦～⑨),通过卡尔曼滤波后都有显著减小

。

①,②,③为航向、俯仰、倾斜角误差(rad );④,⑤,⑥为x ,y ,z 方向的速度误差(m s );⑦,⑧,⑨为x ,y ,z 方向的位置误差(m )。

图4　测试误差的仿真曲线

5　讨　论

从仿真结果看:即使是短时惯性测量系统,如果惯性测量元件的零位偏置误差较大,则依靠惯性传感器直接解算所测得的姿态、速度、位置等也会有较大的误差,而且误差增长速度非常快。这种情况下,惯性测量系统只适用于时间非常短的汽车测试项目,如制动试验等(只需要几秒钟),而不太适合时间较长的道路试验项目。

为了提高廉价惯性测量系统的精度,满足汽车测试的要求,可以采用卡尔曼滤波算法。仿真表明:对于汽车姿态、速度等运动参数,使用卡尔曼滤波后精度显著提高。参考文献:

[1]H iro sh i K O .Op tical fiber gyro scope fo r automobile [J ].Op tronics .1994,(3):6168(in Japanese ).

[2]N obo ru W .Gyro scope fo r automobile app licati on [J ].M aterials Integrati on ,1999.12(9):6570(in Japanese ).[3]B illur B .Inertial navigati on system fo r mobile robo ts [J ].IEEE T ransacti ons on Robo tics and A utom ati on ,1995,11

(3):328

342.

[4]陈哲.捷联惯导系统原理[M ].北京:宇航出版社,1986.

[5]倪江生,翟羽健.惯性测量系统简化模型的研究[J ].东南大学学报,1999.29(5):61

.

3

2第3期　　　　　　　　　　　倪江生:基于卡尔曼滤波算法的汽车运动参数测试方法研究