一种车载组合导航设备姿态自适应估计方法

1.本发明属于多传感器信息融合领域，具体来说涉及一种车载组合导航设备姿态自适应估计方法。


背景技术：

2.车载组合导航设备(车载组合导航系统)在正常工作之前，需要良好的初始状态对准。由于车载组合导航设备在车体上的安装方式不确定，导航设备载体坐标系和车身坐标系之间的相对关系需要一种自适应方法进行估计。
3.目前国内外主要商用车辆的导航方式为gnss与数字地图相结合的方式。这种方式的其中一缺点是车辆为平面上的导航、没有姿态角的输出；这对于只是定位需求而言足够，但是对于车辆进行外部环境检测时不能给出其姿态参考。
4.国内外已经有不少组合导航方面的研究，并在车载组合导航设备中得到实际应用。其中，惯性导航系统(ins)通常以来gnss给出的位置信息进行初始化，ins也可利用gnss的位置和速度在行进过程中完成导航姿态对准，或者利用gnss双天线定向技术完成导航设备姿态组对准。但上述的这些姿态估计方法存在精度低、过程复杂等缺点。另外，还有一些技术方案中提到利用惯性传感器和全球卫星导航系统输出的位置差值，采用卡尔曼滤波技术估计出导航设备相对于车体的姿态角，或者利用地磁传感器和惯性传感器估计出导航设备相对于车体的姿态角；但是这些方案面对复杂环境，还存在收敛速度较慢、估计精度不理想及环境适应性不强等问题。


技术实现要素：

5.本发明针对车载组合导航设备安装方式不确定而无法初始化的问题，提供了一种车载组合导航设备姿态自适应估计方法。
6.为实现上述目的，本发明提供技术方案如下：
7.一种车载组合导航设备姿态自适应估计方法，应用于车载组合导航系统，包括惯性传感器imu和gnss模块，所述方法包括：
8.s1，传感器数据采集和校正；
9.s2，计算导航设备姿态角的俯仰角和横滚角；
10.s3，设置导航系统状态并初始化；
11.s4，组合导航算法初始化；
12.s5，对当前导航系统状态进行惯导推算；
13.s6，车辆状态识别；
14.s7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵；
15.s8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正；
16.s9，评定导航设备安装姿态的估计结果。
17.优选地，所述s1包括以下步骤：
18.s1.1，按照一定采样频率分别采集惯性传感器imu的惯性传感器信息、can总线的车速度信息及gnss模块的gnss输出信息，并标记采样时间戳，惯性传感器imu包括三轴加速度计和三轴角速度计，惯性传感器信息包括三轴加速度数据和三轴角速度数据；
19.s1.2，采用二阶巴特沃斯低通滤波器对三轴加速度数据进行滤波，采用滑动窗口对三轴角速度数据进行滤波。
20.优选地，所述s2包括以下步骤：
21.在平直路面且车体静止状态t0下，计算导航设备姿态角的俯仰角θ
p
(t0)和横滚角θr(t0)；俯仰角计算公式为：
22.横滚角计算公式为：
23.其中，x、y、z分别为三轴加速度x轴、y轴、z轴的加速度值。
24.优选地，所述s3包括以下步骤：
25.s3.1，设置导航设备相对车体的姿态角s3.1，设置导航设备相对车体的姿态角为导航设备相对车体的俯仰角，导航设备相对车体的俯仰角，为导航设备相对车体的横滚角，为导航设备相对车体的横滚角，为导航设备相对车体的航向角，初值为0；
26.s3.2，设置导航系统的姿态角s3.2，设置导航系统的姿态角为导航系统的俯仰角，初值为步骤s2所得的俯仰角θ
p
(t0)，为导航系统的横滚角，初值为步骤s2所得的俯仰角θr(t0)，为导航系统的航向角，其中，θ
gnss
为在车速大于vh时，基于gnss模块获取的车体航向角；
27.s3.3，由gnss模块获取的全局位置和三维速度信息分别初始化导航系统的速度和位置。
28.优选地，所述s4包括以下步骤：
29.设置导航坐标系到载体坐标系的导航变换矩阵，并基于导航系统姿态角的初值初始化导航变换矩阵；
30.设置车体坐标系到载体坐标系的车体变换矩阵，并基于导航设备相对车体姿态角的初值初始化车体变换矩阵；
31.基于导航变换矩阵获取对应的四元素初值；
32.设定卡尔曼滤波器的状态向量，包括姿态误差、速度误差、位置误差、角速度计动态零漂、加速度计动态零漂；
33.设定卡尔曼滤波器的状态转移矩阵。
34.优选地，基于导航变换矩阵、车体变换矩阵及四元素初值，采用捷联惯导更新方程进行惯导解算，更新导航系统状态。
35.优选地，所述车辆状态识别包括：
36.车身速度小于v
stop
、加速度值方差小于阈值且持续时间大于t
stop
时，则为停止状态；
37.角速度计z轴角速度值投影到车体坐标系后的绝对值小于wb
straight
且车身速度大于v
straight
时，车辆处于高速直行状态；
38.角速度计z轴角速度值投影到车体坐标系后的绝对值大于wb
turn
时，车辆处于转弯状态；
39.否则为其他状态；
40.其中，v
stop
为预设的车辆停止时的速度阈值，t
stop
为预设的车辆停止时的持续时间阈值，wb
straight
为预设的车辆直行时的角速度阈值，v
straight
为预设的车辆直行时的速度阈值，wb
turn
为预设的车辆转弯时的角速度阈值。
41.优选地，所述s7包括：
42.当车辆处于静止状态，卡尔曼滤波器的观测矩阵h设置为观测三维速度误差，噪声矩阵r设置为常数，观测向量z为车体在导航坐标系下的三维速度；
43.当车辆处于转弯状态或高速直行状态时，卡尔曼滤波器的观测矩阵h设置为观测三维速度和位置误差，噪声矩阵r由角速度计和卫星信噪比按预设的自适应计算公式进行自适应调整，观测向量z为车身在导航坐标系下的三维速度误差和位置误差。
44.优选地，所述自适应计算公式为r＝n1
×
p
900/cn0-0.7
n2
×qabs(wbz)*10
，其中，n1、n2、q和p为常数，cno为gnss信噪比，wbz为车体转向角速度值。
45.优选地，所述步骤s9包括以下步骤：
46.s9.1，基寸获取导航设备相对车体的当前航向角；
47.s9.2，当检测到导航设备相对车体的当前航向角收敛到方差小于一定阈值时认为估计完毕，否则返回步骤5。
48.与现有技术相比，本发明的有益效果为：
49.本发明利用低成本惯性传感器和全球导航卫星系统并辅助车身速度信息估计导航设备相对于车体的姿态角，内容涉及惯性导航、卡尔曼滤波和车体模型约束等，对导航定位设备安装方式自适应性强且有更好的精度；同时解决了以往人工标定效率低且过程繁琐问题。
附图说明
50.图1为本发明流程图。
具体实施方式
51.下面将结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
52.本发明是基于瑞芯微rv1126平台和恩智浦32位单片机s32k144进行开发设计的。考虑单片机实时性相对较好，整个系统中传感器的数据获取和解析均在s32k144上完成，采
用freertos实时操作系统进行任务调度，很好的保证了传感器之间的时间对齐。系统的算法部分涉及到大量的矩阵计算，因此在算法实现过程中移植了eigen库，算法部分运行在基于linux操作系统的rv1126上。
53.参照图1所示，一种车载组合导航设备姿态自适应估计方法，应用于车载组合导航系统，包括惯性传感器imu和gnss模块，惯性传感器imu为惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器，该惯性传感器imu包括三轴加速度计和三轴角速度计；该车载组合导航设备姿态自适应估计方法包括9个步骤。
54.步骤1，传感器数据采集和校正，具体为：
55.步骤1.1，以100hz、10hz、2hz采样频率分别采集惯性传感器imu的惯性传感器信息、can总线的车速度信息及gnss模块的gnss输出信息，并设置系统时间戳；惯性传感器信息包括三轴加速度数据和三轴角速度数据；
56.步骤1.2，采用二阶巴特沃斯低通滤波器对三轴加速度数据进行滤波，采用滑动窗口对三轴角速度数据进行滤波。
57.本发明步骤1.2中，考虑车体发动机震动对三轴加速度计影响较大，因此采用二阶巴特沃斯低通滤波器对加速度计测量值进行滤波处理，低通滤波器使用matlab工具箱中的filter designer进行参数设计，设置低通滤波器的采样频率fs为imu采样频率fs＝100hz，将低通滤波器的截止频率fc设置为小于车体震动频率，这里车体振动频率为30hz，则fc＝20hz，然后自动生成滤波器差分方程的参数表。通过滑动窗口滤波，使角速度计输出值更平滑，滑动窗口大小根据采样频率进行选择，本实施例中窗口大小设置为50。最后将加速度计和角速度计分别减去各自的零偏，其中加速度计零偏采用六面法实验得到，角速度计零偏由静置后的均值计算得到，这里的“静止后的均值”为imu静止时，一段时间内角速度计输出值的平均值。
58.步骤2，计算导航设备姿态角的俯仰角和横滚角，具体为：
59.在平直路面且车体静止状态t0下，通过式(1)和式(2)分别计算导航设备姿态角的俯仰角θ
p
(t0)和横滚角θr(t0)；
[0060][0061][0062]
其中，x、y、z分别为三轴加速度x轴、y轴、z轴的加速度值。
[0063]
姿态角是指机体坐标系与地面惯性坐标系之间的夹角，可用横滚角、俯仰角、偏航角三个角表示。
[0064]
步骤3，设置导航系统状态并初始化；
[0065]
s3.1，设置导航设备相对车体的姿态角s3.1，设置导航设备相对车体的姿态角为导航设备相对车体的俯仰角，导航设备相对车体的俯仰角，为导航设备相对车体的横滚角，为导航设备相对车体的横滚角，为导航设备相对车体的航向角，初值为0；
[0066]
步骤3.2，将步骤2所得的导航设备姿态角的俯仰角θ
p
(t0)和横滚角θr(t0)作为导
航系统姿态角θ
nb
的俯仰角和横滚角的初始值；由can总线信息解算车速度，在车速大于预设的速度阈值vh时，基于gnss模块获取车体系x轴(这里设置车辆正前方为车体系的x轴，车辆正右边为y轴，车辆正下方为z轴)在导航坐标系下的航向角，也即车体航向角θ
gnss
，顺时针为正；通过式(4)计算得到导航系统姿态角的航向角
[0067][0068]
则载体系相对导航坐标系的姿态角θ
nb
，也即导航系统姿态角信息可表示为：
[0069][0070]
步骤3.3，由gnss模块获取的全局位置和三维速度信息分别初始化导航系统的速度和位置。
[0071]
步骤4，组合导航算法初始化，具体包括：
[0072]
设置导航坐标系到载体坐标系的导航变换矩阵并步骤3.2所得的基于导航系统姿态角的初值[θ
p
(t0) θr(t0) θ
gnss
]初始化导航变换矩阵；
[0073]
设置车体坐标系到载体坐标系的车体变换矩阵基于步骤3所得的导航设备相对车体的姿态角θ
vb
的初值[θ
p
(t0) θr(t0) 0]初始化车体变换矩阵
[0074]
基于导航变换矩阵获取对应的四元素初值q0；
[0075]
设定卡尔曼滤波器的状态向量x
p
，表示为：
[0076]
x
p
＝[δθ
n δv
n δp
n δb
w δba]
t
ꢀꢀ
(6)
[0077]
其中δθn为姿态误差，δvn为速度误差，δpn为位置误差，δbw为角速度计动态零漂，δba加速度计动态零漂；
[0078]
设定卡尔曼滤波器的状态转移矩阵f，状态转移矩阵f由惯性导航算法的姿态误差方程、速度误差方程、位置误差方程和imu动态零偏模型确定；
[0079]
姿态误差方程表示为：
[0080][0081]
式中，为角度数计测量误差，为导航系计算误差，&表示微分。
[0082]
速度误差方程表示为：
[0083][0084]
式中，为加速度计测量误差，δgn分别为地球自转角速度计算误差、导航系旋转计算误差和重力误差。
[0085]
位置误差方程表示为：
[0086]
[0087][0088]
δh
&
＝δvuꢀꢀ
(11)
[0089]
其中，δl、δλ、δh分别表示纬度误差、经度误差和高度误差，rm为子午曲率半径，rn为横向曲率半径，l纬度，h为海拔高度。
[0090]
imu动态零偏模型的状态转移矩阵表示为：
[0091][0092]
其中，f
aa
、f
gg
表示加速度计和角速度计动态零偏状态转移矩阵，α、β为常数，i为三维单位矩阵。本实施中，α＝150，β＝1。
[0093]
这里，姿态误差方程、速度误差方程、位置误差方程和imu动态零偏模型为本领域的公知常识，本领域技术人员可根据实际情况自行设置。
[0094]
步骤5，对当前导航系统状态进行惯导推算，具体为：
[0095]
由姿态更新方程、速度更新方程和位置更新方程进行惯导推算，更新系统的姿态、速度和位置。其中，姿态更新方程可获得载体在导航坐标系下更新后的航向gnss模块可获取车辆在导航系下的航向θ
gntss
，则更新后导航设备相对车体姿态角的航向角计算公式为：
[0096][0097]
这里，姿态更新方程、速度更新方程和位置更新方程为本领域的公知常识，在此不做赘述。
[0098]
步骤6，车辆状态识别，由车体速度和imu测量值将车辆运行过程中的状态划分为停止状态、高速直行状态、转弯状态及其他状态，各状态的判定状态具体为：
[0099]
车身速度小于v
stop
、加速度值方差小于阈值且持续时间大于t
stop
时，则为停止状态；v
stop
为预设的车辆停止时的速度阈值，t
stop
为预设的车辆停止时的持续时间阈值，本实施例中，t
stop
＝0.8s；
[0100]
角速度计z轴角速度值投影到车体坐标系后的绝对值小于wb
straight
且车身速度大于v
straight
时，车辆处于高速直行状态，wb
straight
为预设的车辆直行时的角速度阈值，v
straight
为预设的车辆直行时的速度阈值，本实施例中，
[0101]
角速度计z轴角速度值投影到车体坐标系后的绝对值大于wb
turn
时，车辆处于转弯状态，wb
turn
为预设的车辆转弯时的角速度阈值；本实施例中，
[0102]
否则为其他状态。
[0103]
这里需要注意的是，其他状态时不进行卡尔曼观测融合，只挑出特征明显的状态也即静止状态、转弯状态或高速直行状态来融合。
[0104]
步骤7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵。
[0105]
本发明中，基于不同的车辆状态先选择不同的观测矩阵h，基于观测方程v＝z-h*x
p
，从而获得不同的卡尔曼观测新息(新的信息)。
[0106]
该基于车辆状态选择卡尔曼滤波器的误差观测矩阵为：
[0107]
(1)当车辆处于静止状态，卡尔曼滤波器的观测矩阵h设置为三维速度误差观测，即观测矩阵h对应三维速度元素设置为1，噪声矩阵r为0.0625，由gnss模块得到的观测向量z为车体在导航坐标系下的三维速度，在静止时，gnss模块观测的三维速度就是测量误差。
[0108]
(2)当车辆处于转弯状态或高速直行状态时，卡尔曼滤波器的观测矩阵h设置为三维速度误差和位置误差观测，三维速度观测矩阵设置为1，位置观测矩阵tpr由下式计算得到：
[0109]
tpr＝diag([rm h，(rn h)cosl，-1])
ꢀꢀꢀꢀ
(14)
[0110]
式中，rm为子午圈主曲率半径，rn为卯酉圈主曲率半径，h、l分别为gnss模块输出的海拔高度和纬度。
[0111]
噪声矩阵r由角速度计和卫星信噪比按预设的自适应计算公式进行自适应调整：
[0112]
r＝n1
×
p
900/cn0-0.7
n2
×qabs(wbz)*10
ꢀꢀ
(15)式中，n1、n2、p和q为常数，q、p为信噪比、车体转向角速度值分别对此时观测噪声贡献程度的放大系数，cn0为gnss信噪比，wbz为车体转向角速度值；本实施例中，n1＝0.2，n2＝0.1，p＝3.0，q＝5.0；
[0113]
gnss模块得到的观测向量z为车身在导航坐标系下的三维速度误差和位置误差，其中，三维速度误差设置为：
[0114]verror
＝v
nav-v
gnss
ꢀꢀ
(16)
[0115]vnav
是惯导解算的三维速度，v
gnss
是gnss测量的三维速度。
[0116]
三维位置误差设置为：
[0117]
p
error
＝p
nav-p
gnss
ꢀꢀ
(17)
[0118]
p
nav
是惯导解算的三维位置，p
gnss
是gnss测量的三维位置。
[0119]
步骤8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正。
[0120]
本发明步骤8中，基于步骤4中的卡尔曼滤波器的状态转移矩阵和步骤7中卡尔曼滤波器的误差观测矩阵，进行卡尔曼滤波计算，获得惯性导航系统误差的最优估计，将步骤5中通过惯性导航算法更新的导航系统姿态、速度和位置减去卡尔曼滤波器输出的误差，得到修正后的导航系统姿态、速度和位置。
[0121]
步骤9，评定导航设备安装姿态的估计结果，具体为：
[0122]
s9.1，基寸获取导航设备相对车体的当前航向角
[0123]
s9.2，当检测到导航设备相对车体的当前航向角收敛到方差小于一定阈
值的时认为估计完毕，否则返回步骤5。
[0124]
本发明步骤9.1中，由步骤8中修正后的姿态获得在基于式(13)获得当前时刻导航设备相对车体的航向角；本发明步骤9.2中，不断迭代步骤5到步骤9过程，在多种信息观测约束下，导航设备相对车体姿态角θ
vb
的航向角快速收敛到真值，最后通过方差检验当的方差小于0.001阈值并持续50个计算周期，认为收敛完成。
[0125]
此时该收敛完成的航向角为自适应估计得到的车载组合导航设备姿态角的航向角，步骤2计算得到的导航设备姿态角的俯仰角θ
p
(t0)和横滚角θr(t0)为自适应估计得到的车载组合导航设备姿态角的俯仰角和横滚角。