IMU姿态惯性推导
本文是我上学期间写得,之前已经在另一个博客发布过,如今转至此发布。
- 最近从事行人惯性导航的研究,本人也是一个小白,其中看了很多文献,有很多个人思考很费时间的地方,撰写此随笔的目的不仅是给自己做一个笔记,也是给各位有需要的仁兄一点个人理解。
- 本文只关于使用IMU传感器为主的行人导航算法。
- 本文为一篇行人惯性导航的入门,主要针对其中重要的涉及的知识点之间的解释、串联,包括航向更新、速度更新、位置更新、坐标变换原理即代码,不深究其推导。要是有什么讲得不对的地方,请给予指正,谢谢。
引言
以IMU为主,其他传感器(多为磁力计、高度计、气压计、压力传感器、肌肉电传感器等)为辅的行人导航研究中,主要有两种方案(这里忽略SLAM、激光雷达之类的,)。一个是基于航迹推移的方案(SHS,step-and-heading systems),另一个是基于惯性积分的方案(INS,inertial navigation systems)。个人研究的技术方向属于后者(INS),所以后面将会着重介绍后者的技术细节。
SHS
也就是常见的航迹推移算法(PDR,Pedestrian Dead Reckoning),根据计算出来的步长和航向,通过三角函数关系递推下一步的位置。所以该方案的核心就是求步长和航向,同时也是该算法的两个主要误差来源。求步长有诸多步长估计模型,比如Weinberg模型[1]
l = k × ( a m a x − a m i n ) 1 / 4 l=k \times (a_{max}-a_{min})^{1/4} l=k×(amax−amin)1/4
或者Scarlet模型[2]
l = k × ∑ i = 1 N a i N − a m i n a m a x − a m i n l=k \times \cfrac{\cfrac{\sum_{i=1}^Na_i}{N}-a_{min}}{a_{max}-a_{min}} l=k×amax−aminN∑i=1Nai−amin
等等,详细细节可自行了解。但是无论使用哪一个步长模型,这里面的参数(比如k)都跟个体参数(步频、步长、腿长等)的不同而不同,即需要对人体进行运动学建模,这也是这种模型最大的一个弊端(个人觉得)[3]。
关于航向修正放在后面(INS)一起总结,因为无论是SHS,还是INS,航向漂移都是两者的痛点。
INS
基于惯性递推的技术方案,给出常见的递推公式[4]便一目了然。
p
k
n
=
p
n
−
1
n
+
v
k
−
1
n
Δ
t
v
k
n
=
v
k
−
1
n
+
{
R
(
q
k
−
1
)
f
b
+
g
n
}
Δ
t
q
b
,
k
n
=
Ω
(
ω
k
Δ
t
)
q
b
,
k
−
1
n
p_k^n=p_{n-1}^n+v_{k-1}^n\Delta t \\ v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta t \\ q_{b,k}^n = \Omega(\omega_k \Delta t)q_{b,k-1}^n
pkn=pn−1n+vk−1nΔtvkn=vk−1n+{R(qk−1)fb+gn}Δtqb,kn=Ω(ωkΔt)qb,k−1n
其中
n
n
n、
b
b
b分别表示导航坐标系和载体坐标系,
p
p
p、
v
v
v、
q
q
q分别表示位置、速度、姿态四元数(对应欧拉角,俯仰角pitch、滚转角raw、航向角yaw)。这里是一个简化后的模型,忽略了地球自转、经纬度等因素的影响,因为这些因素对低精度的惯性器件影响可以忽略不计,具体细节可以参考文献[7],这里不再展开。
INS特点:
- 基于惯性递推的方案相比于航迹推移算法,其本身的姿态推导不需要为人体运动学建模,适用性更好;
- 基于惯性递推的方案误差随时间的立方根次增长,这是其最大的弊端;
- 基于惯性递推的方案一般结合零速修正技术,使用卡尔曼滤波器将两者进行融合,限制误差的增长。
关于INS方法的技术点
- 零速修正
- 使用零速修正技术,重点是使用什么检测器检测零速条件!
- 当前有很多零速检测器,比SHOE、ARED、MBGTD等,其中SHOE公认精度是最好的[6]。
- 航向修正
- 无论是SHS还是INS,在航向修正方面所遇到的问题都基本相同,及决方案大致一样,要么借助磁力计,要么是HDR/iHDR,或者其他的方案。
干货
下面所讲的内容是个人所经历的,也是本文的重点。如果看惯性导航参考书[5]的话,你一般都会看到复杂的推导,容易搞蒙,尤其是涉及到坐标系的转换关系,除非死磕硬骨头才能拿下。其实有一本不错的讲义[8],本人的大多数困惑都能从上边找到答案。
- 姿态更新
- 首先要明白IMU测的是相对于惯性坐标系
i
i
i 的加速度和角速度,不是相对于导航系
n
n
n ,这之间存在转换。这也是为什么我早期一直很困惑的原因。具体的推导比较复杂,本文只给出关键结果。姿态的更新一般用四元数、旋转矩阵或旋转向量表示。本文以旋转矩阵进行说明。不同表达形式,都有各自的优缺点。旋转矩阵存在万向锁问题,四元数存在交换不可逆误差等。一般在使用的时候结合使用。
C b ( k ) n ( k ) = C n ( k − 1 ) n ( k ) C i ( k − 1 ) n ( k − 1 ) C b ( k − 1 ) i ( k − 1 ) C b ( k ) b ( k − 1 ) = C n ( k − 1 ) n ( k ) C b ( k − 1 ) n ( k − 1 ) C b ( k ) b ( k − 1 ) C_{b(k)}^{n(k)}=C_{n(k-1)}^{n(k)}C_{i(k-1)}^{n(k-1)}C_{b(k-1)}^{i(k-1)}C_{b(k)}^{b(k-1)}=C_{n(k-1)}^{n(k)}C_{b(k-1)}^{n(k-1)}C_{b(k)}^{b(k-1)} Cb(k)n(k)=Cn(k−1)n(k)Ci(k−1)n(k−1)Cb(k−1)i(k−1)Cb(k)b(k−1)=Cn(k−1)n(k)Cb(k−1)n(k−1)Cb(k)b(k−1)
其中, C b ( k ) b ( k − 1 ) C_{b(k)}^{b(k-1)} Cb(k)b(k−1) 表示以i
系作为参考基准,b
系从 t k − 1 t_{k-1} tk−1 时刻到 t k t_{k} tk 时刻的旋转变化, C b ( k ) b ( k − 1 ) C_{b(k)}^{b(k-1)} Cb(k)b(k−1) 由角速度 w i b b w_{ib}^b wibb 确定; C n ( k ) n ( k − 1 ) C_{n(k)}^{n(k-1)} Cn(k)n(k−1) 表示以i
系作为参考基准,n
系从 t k − 1 t_{k-1} tk−1 时刻到 t k t_{k} tk 时刻的旋转变化, C n ( k − 1 ) n ( k ) C_{n(k-1)}^{n(k)} Cn(k−1)n(k) 由计算角速度 − w i n n -w_{in}^n −winn 确定; C n ( k − 1 ) n ( k − 1 ) C_{n(k-1)}^{n(k-1)} Cn(k−1)n(k−1) 和 C b ( k ) n ( k ) C_{b(k)}^{n(k)} Cb(k)n(k) 分别表示 t k − 1 t_{k-1} tk−1 和 t k t_{k} tk 时刻的捷联姿态矩阵。通常在导航更新周期 [ t k − 1 , t k ] [t_{k-1},t_k] [tk−1,tk] 内,可认为速度和位置引起的 w i n n w_{in}^n winn 计算角速度很小,即可视为常值[8];通常在更新周期 [ t k − 1 , t k ] [t_{k-1},t_k] [tk−1,tk] 内,导航系的变化很缓慢,所以 C n ( k − 1 ) n ( k ) ≈ I 3 × 3 C_{n(k-1)}^{n(k)} \approx I_{3 \times 3} Cn(k−1)n(k)≈I3×3[5]。从这个角度应该能更好的理解坐标系之间的变化关系。
- 首先要明白IMU测的是相对于惯性坐标系
i
i
i 的加速度和角速度,不是相对于导航系
n
n
n ,这之间存在转换。这也是为什么我早期一直很困惑的原因。具体的推导比较复杂,本文只给出关键结果。姿态的更新一般用四元数、旋转矩阵或旋转向量表示。本文以旋转矩阵进行说明。不同表达形式,都有各自的优缺点。旋转矩阵存在万向锁问题,四元数存在交换不可逆误差等。一般在使用的时候结合使用。
用四元数表示为:
Q
(
t
k
)
=
Q
(
t
k
−
1
)
⨂
q
(
ω
k
Δ
t
)
q
(
ω
k
Δ
t
)
=
c
o
s
ϕ
2
+
ϕ
→
ϕ
s
i
n
ϕ
2
Q(t_k) = Q(t_{k-1})\bigotimes q(\omega_k \Delta t) \\ q(\omega_k \Delta t) = cos\cfrac{\phi}{2} + \cfrac{\overrightarrow{\phi}}{\phi}sin\cfrac{\phi}{2}
Q(tk)=Q(tk−1)⨂q(ωkΔt)q(ωkΔt)=cos2ϕ+ϕϕsin2ϕ
其中
Q
Q
Q 为四元数,表示载体的姿态,
ϕ
\phi
ϕ 是旋转矢量
ϕ
→
\overrightarrow{\phi}
ϕ 的模长,四元数、旋转矢量、旋转矩阵可以互相转换。这里所写的形式和之前的递推公式没有什么区别,只是一个是四元数的左乘,一个是四元数的右乘。为什么四元数需要结合旋转向量使用,就是弥补各自的缺点(建议看书,这里面存在一个理论推导)。
- 速度更新
- 速度的更新也存在一个坐标系的转换;
- 其实只要理解的旋转矩阵的更新,后面的推导都是在旋转矩阵更新的基础上的得到;
- 关于速度更新,更注意的一点是剔除重力因素的影响。
- 上面的递推公式 v k n = v k − 1 n + { R ( q k − 1 ) f b + g n } Δ T v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta T vkn=vk−1n+{R(qk−1)fb+gn}ΔT,其中 R ( q k − 1 ) f b R(q_{k-1})f^b R(qk−1)fb 对所测到的加速度 f b f^b fb(一般称为比力) 进行坐标变换, R ( q k − 1 ) R(q_{k-1}) R(qk−1) 表示对四元数转换为旋转矩阵。
- 位置更新
- 这就不用说了,一眼明了。
相关代码(Python)
p k n = p n − 1 n + v k − 1 n Δ T v k n = v k − 1 n + { R ( q k − 1 ) f b + g n } Δ T q b , k n = Ω ( ω k Δ t ) q b , k − 1 n p_k^n=p_{n-1}^n+v_{k-1}^n\Delta T \\ v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta T \\ q_{b,k}^n = \Omega(\omega_k \Delta t)q_{b,k-1}^n pkn=pn−1n+vk−1nΔTvkn=vk−1n+{R(qk−1)fb+gn}ΔTqb,kn=Ω(ωkΔt)qb,k−1n
def nav_eq(self, xin, imu, qin, dt):
# imu[0:3] acc 加速度
# imu[3:6] gyro 角速度
# imu[6:9] attitude 航向
# update Quaternions
x_out = np.copy (xin) # initialize the output
# 这里是求 大omega,即 w*t 的反对称矩阵
omega = np.array ([[0, -imu[3], -imu[4], -imu[5]],
[imu[3], 0, imu[5], -imu[4]],
[imu[4], -imu[5], 0, imu[3]],
[imu[5], imu[4], -imu[3], 0]])
# 求角速度的二范数,即模长
# qout输出旋转矩阵 $\Omega$
# 定时增量法,四元数微分方程的毕卡求解法,四元数的更新为了避免其自身的不可交换误差,会先计算旋转矢量,利用旋转矢量更新四元数,具体参考文献[8]
norm_w = LA.norm (imu[3:6])
if (norm_w * dt != 0):
q_out = (np.cos (dt * norm_w / 2) * np.identity (4) + (1 / (norm_w)) * np.sin (dt * norm_w / 2) * omega).dot (qin)
else:
q_out = qin
attitude = quat2euler (q_out, 'sxyz') # update euler angles
x_out[6:9] = attitude
Rot_out = quat2mat (q_out) # 将更新后的四元数转换成旋转矩阵
acc_n = Rot_out.dot (imu[0:3]) # 对所测的加速度进行坐标变换
acc_n = acc_n + np.array ([0, 0, self.config["g"]]) # 剔除重力影响
x_out[3:6] += dt * acc_n # 速度更新
x_out[0:3] += dt * x_out[3:6] + 0.5 * np.power (dt, 2) * acc_n # 位置更新,注意这里跟给定的参考文献有所区别,两者差个加速度项
return x_out, q_out, Rot_out
在此感谢大佬的开源pyshoe。如果是小白,强烈推荐这个开源,因为其还有对标论文,可以便看边上手。同时还需要感谢大佬实验室OpenSHOE。这两个开源项目给我的研究带来了巨大的飞跃,在此重重感谢。
总结
关于IMU的行人导航技术,还有很多研究的热点,比如初始校准、高程问题、航向问题、多运动模型的问题等等。以上只是个人的理解,如果有什么不对的地方,请大佬尽管指出,虚心请教。欢迎各位同学留言,互相学习、探讨。
参考文献
[1] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J].
[2] Scarlett J. Enhancing the performance of pedometers using a single accelerometer[J].
[3] 潘献飞, 穆华, 胡小平. 单兵自主导航技术发展综述[J]. 导航定位与授时, 2018, 5(1):11.
[4] Wagstaff B , Peretroukhin V , Kelly J . Robust Data-Driven Zero-Velocity Detection for Foot-Mounted Inertial Navigation[J]. IEEE Sensors Journal, 2019, PP(99):1-1.
[5] 秦永元. 惯性导航.第2版[M]. 科学出版社, 2014.
[6] Wahlstrm J , Skog I . Fifteen Years of Progress at Zero Velocity: A Review[J]. 2020.
[7] Muhammad I , Kuk C , Seung-Ho B , et al. Drift Reduction in Pedestrian Navigation System by Exploiting Motion Constraints and Magnetic Field[J]. Sensors (Basel, Switzerland), 2016, 16(9).
[8] 捷联惯导算法与组合导航原理讲义. 严恭敏,翁浚 编著.