车轮安装惯性测量单元的安装参数现场标定方法

1.本发明属于惯性导航技术领域，具体涉及一种车轮安装惯性测量单元的安装参数现场标定方法，用以提高基于车轮安装惯性测量单元的车辆定位精度和效果。


背景技术：

2.自动驾驶技术发挥越来越重要的作用，车辆自主定位是自动驾驶车辆的一个核心功能。惯性导航具有输出信息不间断、不受外界干扰等独特优势，被广泛用于车辆自主定位。
3.惯性导航面临的主要问题是定位误差随时间发散严重，针对该问题，文献[1]([1]collin j.mems imu carouseling for ground vehicles[j].ieee transactions on vehicular technology,2015,64(6):2242-2251)中，collin提出将微机电系统(micro-electro-mechanical system，mems)惯性测量单元(initial measurement unit，imu)安装在车轮的车辆定位方案，通过三轴加速度计感知重力加速度获得车轮转动的圈数，实现“里程计”测量效果，利用车轮旋转调制作用抑制陀螺的零漂，并由陀螺感知车辆的姿态，最终达到定位的目的，在近1km的定位实验中最大误差不超过8m。
[0004]
常规的里程计往往要在车辆出厂前安装好。常规的旋转调制捷联惯导系统需要复杂精密的机电控制系统，可有效抑制惯性器件的系统误差。一个imu仅仅通过安装在车轮上便可等效于里程计和单轴旋转捷联惯导系统的组合，大大提升了惯性导航定位精度。
[0005]
文献[1]中，利用特制夹具，将mems imu安装于车轮轴心，但imu外壳中心不等同于加速度计敏感中心，安装时难以保证加速度计敏感中心恰好在车轮转轴上，存在偏心距，由此会因车轮旋转产生向心加速度和切向加速度并作用于加速度计，影响对车轮转速的准确估计。imu相对于车轮的轴对准误差在安装时也是难以避免的，它将等效为imu的系统误差影响车辆方位估计精度。尤其在车辆运动速度较快时，安装误差将使得基于车轮安装imu的车辆定位模型误差突显，进一步降低车辆定位精度。利用特制夹具可以一定程度减少安装误差，但增加了安装难度和使用成本。


技术实现要素：

[0006]
本发明的目的是针对现有技术存在的问题，提供一种车轮安装惯性测量单元的安装参数现场标定方法，抑制车轮imu安装误差对车辆定位精度的影响，提高基于车轮安装imu的车辆定位精度。
[0007]
为实现上述目的，本发明采用的技术方案是：
[0008]
一种车轮安装惯性测量单元的安装参数现场标定方法，包括以下步骤：
[0009]
s1，粗标定阶段，车辆在水平路面开始处于静止状态，之后沿直线行驶一小段距离，通过三轴姿态确定(triaxial attitude determination，triad)算法获得惯性测量单元相对于车轮的轴对准误差的粗略值；
[0010]
s2，精标定阶段，车辆在水平路面开始处于静止状态，之后在水平路面自由行驶，
通过车辆运动学约束和惯性测量单元的输出建立卡尔曼滤波器，并根据所述卡尔曼滤波器计算惯性测量单元相对于车轮的偏心距和轴对准误差精确值，同时估计出车辆位姿。
[0011]
具体地，步骤s1包括：
[0012]
s11，采集车辆停止状态下三轴加速度计(简称加速度计)的输出值，并对1秒内加速度计的输出值取平均值；
[0013]
s12，采集车辆沿直线行驶状态下三轴陀螺(简称陀螺)的输出值，并对1秒内陀螺的输出值取平均值；
[0014]
s13，根据步骤s11和步骤s12得到的平均值，采用triad算法计算得到惯性测量单元相对于车轮的轴对准误差矩阵粗略值。
[0015]
具体地，步骤s13中，计算所述轴对准误差矩阵粗略值的公式为：
[0016][0017]
其中，w2＝[0 g 0]
t
；
[0018]
为车轮坐标系w系的估计结果系到惯性测量单元坐标系b系的坐标变换矩阵；为1秒内陀螺的输出值平均值，为1秒内陀螺的输出值平均值；g为当地重力加速度值。
[0019]
具体地，步骤s2包括：
[0020]
s21，建立卡尔曼滤波器，设定卡尔曼滤波器的初值，并将粗标定阶段的结果作为精标定阶段中轴对准误差矩阵的初值；
[0021]
s22，按照时间顺序读取一帧车辆行驶过程中惯性测量单元的输出数据，并代入卡尔曼滤波器；
[0022]
s23，通过卡尔曼滤波器估计状态变量和均方误差阵；
[0023]
s24，状态反馈，更新轴对准误差值；
[0024]
s25，车身姿态计算与定位；
[0025]
s26，重复循环步骤s22至s25，直至步骤s22中数据全部读取完毕。
[0026]
具体地，步骤s21中，建立卡尔曼滤波器包括建立扩展卡尔曼滤波器的状态方程和观测方程；
[0027]
状态方程：
[0028]
观测方程：z＝h(x)+v
[0029]
其中，状态变量α为车轮转角，为车轮角速度，为车轮角加速度；惯性测量单元相对于车轮的轴对准误差用欧拉角表示，即横滚角γs、俯仰角θs和方位角ψs；r为惯性测量单元与车轮的偏心距；设定状态变量的初始方差阵为p(t0)；为高斯白噪声，其方差为q；fb为三轴加速度计输出；v为观测噪声，其方差阵为r；tk＝kt，t为采样周期，k为正整数。
[0030][0031][0032]
其中，
[0033][0034][0035][0036][0037][0038][0039]gn
＝[0 0
ꢀ‑
g]
t
[0040]
v系为车身坐标系，其坐标原点位于车轮与地面的接触点，n系为导航坐标系，其相对于地球凝固且与初始时刻v系重合，为a系到b系的坐标变换矩阵，w
ab
为b系相对于a系的角速度，为角速度w
ab
在c系下的投影，的三个元素分别记为的三个元素分别记为a
ab
为b系相对于a系的加速度，为加速度a
ab
在c系下的投影，r为车轮半径。
[0041]
具体地，步骤s23中，通过卡尔曼滤波器估计状态变量和均方误差阵的方法为：
[0042]
由式x(t
k/k-1
)＝φx(t
k-1
)预测状态变量x，由式p(t
k/k-1
)＝φp(t
k-1
)φ
t
+γqγ
t
预测均方误差阵；
[0043]
由式更新h(x)，由式更新雅可比矩阵hj；
[0044]
由下式估计状态变量x和均方误差阵p：
[0045]
k＝p(t
k/k-1
)h
jt
[hjp(t
k/k-1
)h
jt
+r]-1
[0046]
x(tk)＝x(t
k/k-1
)+k[z-hjx(t
k/k-1
)]
[0047]
p(tk)＝[i-khj]p(t
k/k-1
)。
[0048]
具体地，步骤s24中，由式更新并将更新后的作为下一轮循环的即将γs、θs和ψs置0。
[0049]
具体地，步骤s25中，车身姿态由更新；车辆位置由更新；车辆位置由更新。
[0050]
与现有技术相比，本发明的有益效果是：本发明利用现场标定方法，实现了对车轮imu安装参数的标定，消除车轮imu安装误差对车辆定位精度的影响，提高基于车轮安装imu的车辆定位精度。此外，在车轮安装imu时无需特制夹具，无需安装在车轮轴心，降低了安装难度和使用成本。
附图说明
[0051]
图1为本发明一种车轮安装惯性测量单元的安装参数现场标定方法的流程示意图。
[0052]
图2为本发明实施例中imu安装在车轮上的安装示意图。
[0053]
图3为本发明实施例中坐标系及部分变量定义示意图。
[0054]
图4为本发明实施例中车轮imu安装参数精标定结果示意图。
[0055]
图5为本发明实施例中车辆定位结果示意图。
具体实施方式
[0056]
下面将结合本发明中的附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动条件下所获得的所有其它实施例，都属于本发明保护的范围。
[0057]
本实施例提供了一种车轮安装惯性测量单元的安装参数现场标定方法，包括以下步骤：
[0058]
首先，利用魔力胶带将mems imu固定在车辆右轮，整个现场标定过程，均令小车在水平路面行驶。计算机通过蓝牙采集imu数据，并运行imu安装参数现场标定程序，imu型号为lpms-b2，采样率为50hz。
[0059]
如图2和图3所示，记imu坐标系为b系，其原点位于imu中心(准确地说，是加速度计敏感中心)；记车轮坐标系为w系，其x轴垂直于车轮转轴，由imu中心指向车轮中心，z轴沿车轮转轴方向，指向车身右侧，其原点位于车轮中心；记车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装imu的车轮与地面接触点；导航坐标系n系相对于地球凝固，n系与初始时刻v系重合；记车轮半径为r，imu中心距车轮转轴距离(即偏心距)为r。
[0060]
如图1所示，安装参数现场标定程序分为粗标定和精标定两个阶段，具体步骤如下所示：
[0061]
步骤s1：进行粗标定，在水平路面，车辆开始处于静止状态，采集静止状态下的加速度计输出，之后令车辆在水平路面沿直线运动一小段距离，采集直线运动状态下的陀螺输出，由triad算法确定轴对准误差矩阵粗值。
[0062]
步骤s2：进行精标定，令车辆由静止开始，在水平路面自由行驶，采集imu输出，代入扩展卡尔曼滤波器，估计出轴对准误差矩阵和偏心距，同时能估计车辆位置。
[0063]
具体地，步骤s1包括：
[0064]
步骤s11：车辆在水平路面，保持静止状态，采集加速度计输出，为减少噪声影响，对1秒内采集的加速度计输出取平均，结果记为
[0065]
步骤s12：车辆在水平路面，沿直线行驶一小段距离，采集陀螺输出，为减少噪声影响，对1秒内采集的陀螺输出取平均，结果记为
[0066]
步骤s13：根据步骤s11和步骤s12得到的平均值，采用triad算法计算得到惯性测量单元相对于车轮的轴对准误差矩阵粗略值；
[0067]
如图3所示，定义中间坐标系w
′
系由w系沿z轴旋转得到，w
′
系的y轴沿竖直方向向上；在水平路面，开始时刻静止，当地重力加速度大小为g，满足：
[0068][0069]
记为a系到b系的坐标变换矩阵。记w
ab
为b系相对于a系的角速度，为角速度w
ab
在c系下的投影，的三个元素分别记为记α为车轮转角(如图3所示)，车轮相对于车身的角速度w
vw
满足：
[0070][0071][0072]
记陀螺的输出为其中，i为惯性坐标系。mems陀螺精度相对较低，可忽略地球自转与imu线运动对陀螺输出的影响，陀螺的输出近似为粗标定阶段，车辆在水平路面直线行驶，满足w
vw
＝w
nw
，再由陀螺的平均输出满足：
[0073][0074]
采用triad算法得到计算式为：
[0075][0076]
其中，w2＝[0 g 0]
t
；g为当地重力加速度值。
[0077]
具体地，步骤s2包括：
[0078]
s21，建立卡尔曼滤波器，设定卡尔曼滤波器的初值，设定状态变量x初值，设定状态变量的初始方差阵为p(t0)，设定状态噪声的方差为q，设定观测噪声v的方差阵为r；并将粗标定阶段的结果作为精标定阶段中轴对准误差矩阵的初值，即令的初值，即令作为精标定阶段的初值；
[0079]
建立卡尔曼滤波器包括建立扩展卡尔曼滤波器的状态方程和观测方程；
[0080]
状态方程：
[0081]
观测方程：z＝h(x)+v
[0082]
其中，状态变量α为车轮转角，为车轮角速度，为车轮角加速度；轴对准误差矩阵转换为欧拉角，即横滚角γs，俯仰角θs，方位角ψs；r
为惯性测量单元与车轮的偏心距；fb为三轴加速度计输出；tk＝kt，t为采样周期，k为正整数；
[0083][0084][0085]
经过粗标定后γs和θs为小角，ψs为大角，采用大方位角小水平角姿态变换矩阵模型，满足：
[0086][0087]
车辆在水平路面行驶，满足：
[0088][0089]
满足由于车辆在水平路面行驶，车辆理论上不存在俯仰角速度，因此满足
[0090]
加速度计主要受到重力加速度、车轮相对于车身旋转产生的运动加速度以及车身相对于地球的加速度作用。
[0091]
记v
ab
为b系相对于a系的速度，为速度v
ab
在c系下的投影。imu安装在车辆的非转向轮，由车辆运动学特性得：
[0092]
记a
ab
为b系相对于a系的加速度，为加速度a
ab
在c系下的投影。车身相对于地球的加速度a
nv
由向心加速度a
nvc
和切向加速度a
nvt
组成，即a
nv
＝a
nvc
+a
nvt
，满足：
[0093][0094][0095]
由于车辆在水平路面行驶，车辆俯仰角速度为0，满足这里
[0096][0097]
imu由于车轮旋转产生相对于车轮中心的向心加速度和切向加速度，满足：
[0098][0099]
若imu中心在w系的y轴负半轴，r》0；若imu中心在w系的y轴正半轴，r《0。
[0100]
此外，加速度计还受到地球旋转或车身转向产生的科氏加速度，以及杆臂效应产生的加速度分量作用，由于这些加速度较小，可以忽略。加速度计的输出fb和运动加速度之间的关系近似满足：
[0101][0102]
式中，gv为重力加速度向量在v系的投影，gv＝[0 0
ꢀ‑
g]
t
。
[0103]
s22，按照时间顺序读取一帧车辆行驶过程中惯性测量单元的输出数据，并代入卡尔曼滤波器；
[0104]
s23，通过卡尔曼滤波器估计状态变量和均方误差阵；
[0105]
由式x(t
k/k-1
)＝φx(t
k-1
)预测状态变量x，由式p(t
k/k-1
)＝φp(t
k-1
)φ
t
+γqγ
t
预测均方误差阵；
[0106]
由式更新h(x)，由式更新雅可比矩阵hj；
[0107]
由下式估计状态变量x和均方误差阵p：
[0108]
k＝p(t
k/k-1
)h
jt
[hjp(t
k/k-1
)h
jt
+r]-1
[0109]
x(tk)＝x(t
k/k-1
)+k[z-hjx(t
k/k-1
)]
[0110]
p(tk)＝[i-khj]p(t
k/k-1
)。
[0111]
s24，状态反馈，更新轴对准误差值；
[0112]
由式更新并将更新后的作为下一轮循环的即即将γs、θs和ψs置0。
[0113]
s25，车身姿态计算与定位；
[0114]
车身姿态由更新；车轮转角和车轮半径相乘得到车辆里程，车辆位置由更新。
[0115]
s26，重复循环步骤s22至s25，直至步骤s22中数据全部读取完毕。
[0116]
如图4为精标定结果，图4(a)中偏心距最终估计值和直尺测量值一致，图4(b)中安装欧拉角由轴对准误差矩阵变换得到。如图5为本实施例中车辆的定位结果，远远大于一般的车载mems imu的所能达到的定位精度，从侧面反映出标定结果的有效性。
[0117]
本实施例中，根据实际应用场景，可以只选择粗标定或只选择精标定方法、或者同时采用粗标定和精标定方法来对车轮安装惯性测量单元的安装参数进行标定。本实施例中基于triad算法的粗标定计算式，也可以用其他类似原理的算法代替；本实施例中用于精标定的卡尔曼滤波器的状态方程和观测方程，可以有其他描述方式，比如欧拉角用四元数表示；扩展卡尔曼滤波器也可以由其他卡尔曼滤波器代替，比如无迹卡尔曼滤波器。
[0118]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

技术特征：
1.一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，包括以下步骤：s1，粗标定阶段，采用triad算法获得惯性测量单元相对于车轮的轴对准误差的粗略值；s2，精标定阶段，通过车辆运动学约束和惯性测量单元的输出建立卡尔曼滤波器，并根据所述卡尔曼滤波器计算惯性测量单元相对于车轮的偏心距和轴对准误差精确值，同时估计出车辆位姿。2.根据权利要求1所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s1包括：s11，采集车辆停止状态下三轴加速度计的输出值，并对1秒内三轴加速度计的输出值取平均值；s12，采集车辆沿直线行驶状态下三轴陀螺的输出值，并对1秒内三轴陀螺的输出值取平均值；s13，根据步骤s11和步骤s12得到的平均值，采用triad算法计算得到惯性测量单元相对于车轮的轴对准误差矩阵粗略值。3.根据权利要求2所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s13中，计算所述轴对准误差矩阵粗略值的公式为：其中，w2＝[0 g 0]
t
；为车轮坐标系w系的估计结果系到惯性测量单元坐标系b系的坐标变换矩阵；为1秒内三轴陀螺的输出值平均值，为1秒内三轴加速度计输出值平均值；g为当地重力加速度值。4.根据权利要求1所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s2包括：s21，建立卡尔曼滤波器，设定卡尔曼滤波器的初值，并将粗标定阶段的结果作为精标定阶段中轴对准误差矩阵的初值；s22，按照时间顺序读取一帧惯性测量单元的输出数据，并代入卡尔曼滤波器；s23，通过卡尔曼滤波器估计状态变量和均方误差阵；s24，状态反馈，更新轴对准误差值；s25，车身姿态计算与定位；s26，重复循环步骤s22至s25，直至步骤s22中数据全部读取完毕。5.根据权利要求4所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s21中，建立卡尔曼滤波器包括建立扩展卡尔曼滤波器的状态方程和观测方程；状态方程：观测方程：z＝h(x)+v其中，状态变量α为车轮转角，为车轮角速度，为车轮角加速度；惯性测量单元相对于车轮的轴对准误差用欧拉角表示，分别为横滚角γ
s
，俯仰角θ
s
，方位角ψ
s
；r为惯性测量单元与车轮的偏心距；设定状态变量的初始方差阵为p(t0)；
为高斯白噪声，其方差为q；f
b
为三轴加速度计输出；v为观测噪声，其方差阵为r；t
k
＝kt，t为采样周期，k为正整数；＝kt，t为采样周期，k为正整数；其中，其中，其中，其中，其中，其中，g
n
＝[0 0
ꢀ‑
g]
t
v系为车身坐标系，其坐标原点位于车轮与地面的接触点，n系为导航坐标系，其相对于地球凝固且与初始时刻v系重合，为a系到b系的坐标变换矩阵，w
ab
为b系相对于a系的角速度，为角速度w
ab
在c系下的投影，的三个元素分别记为和a
ab
为b系相对于a系的加速度，为加速度a
ab
在c系下的投影，r为车轮半径。6.根据权利要求4所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s23中，通过卡尔曼滤波器估计状态变量和均方误差阵的方法为：由式x(t
k/k-1
)＝φx(t
k-1
)预测状态变量x，由式p(t
k/k-1
)＝φp(t
k-1
)φ
t
+γqγ
t
预测均方误差阵p；由式更新h(x)，由式更新雅可比矩阵h
j
；由下式估计状态变量x和均方误差阵p：k＝p(t
k/k-1
)h
jt
[h
j
p(t
k/k-1)
h
jt
+r]-1
x(t
k
)＝x(t
k/k-1
)+k[z-h
j
x(t
k/k-1
)]p(t
k
)＝[i-kh
j
]p(t
k/k-1
)。7.根据权利要求4所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s24中，由式更新并将更新后的作为下一轮循环
的即将γ
s
、θ
s
和ψ
s
置0。8.根据权利要求4所述的一种车轮安装惯性测量单元的安装参数现场标定方法，其特征在于，步骤s25中，车身姿态由更新；车辆位置由更新；车辆位置由更新。

技术总结
本发明公开了一种车轮安装惯性测量单元的安装参数现场标定方法，包括粗标定和精标定两个阶段；粗标定阶段：在水平路面，车辆开始处于静止状态，采集静止状态下的加速度计输出，之后令车辆在水平路面沿直线运动一小段距离，采集直线运动状态下的陀螺输出，由TRIAD算法确定轴对准误差矩阵粗值。精标定阶段：令车辆由静止开始，在水平路面自由行驶，通过车辆运动学约束和惯性测量单元的输出建立卡尔曼滤波器，采集IMU输出，代入扩展卡尔曼滤波器，通过所述卡尔曼滤波器计算惯性测量单元相对于车轮的偏心距和轴对准误差精确值，同时能估计车辆位姿。本发明有助于减少车轮IMU安装难度和使用成本，提高基于车轮安装IMU的车辆定位精度。精度。精度。


技术研发人员：谭彩铭 王强 朱博 高翔
受保护的技术使用者：南京邮电大学
技术研发日：2021.11.09
技术公布日：2022/3/7