Unleashing Robotics: Mastering Quaternion Kinematics with Python-Chapter4-2（原创系列教程）ESKF与系统误差状态运动学（2）

第4章:ESKF与IMU驱动系统的误差状态运动学2（Error-State Kalman Filter and IMU-Driven System Error Kinematics）

Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter4(原创系列教程)
第4章:ESKF与IMU驱动系统的误差状态运动学2（Error-State Kalman Filter and IMU-Driven System Error Kinematics）
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 fanzexuan135@163.com

终于讲到ESKF了，由于本章内容太多，超过blog字数限制，我分开两部分讲解，这是第二部分，正文开始：

4.8 实践

通过上面的学习，在高级机器人应用中，你就可以使用这部分知识了，假设你有一个四足机器人,其状态由位置 $\mathbf{p}$,速度 $\mathbf{v}$,方向四元数 $\mathbf{q}$,加速度计偏差 ${\mathbf{a}}_b$ 和陀螺仪偏差 ${\mathbf{\omega }}_b$ 组成。机器人装备了一个IMU,提供加速度计测量值 ${\mathbf{a}}_m$ 和陀螺仪测量值 ${\mathbf{\omega }}_m$。此外,当一条腿着地时,我们可以使用腿部运动学计算足端位置,并将其用作位置和速度的测量值。

ESKF的预测步骤如下:

1. 使用IMU测量值和当前状态估计值来预测标称状态:

$$\begin{array}{rl}\mathbf{p}& \leftarrow \mathbf{p}+\mathbf{v}\Delta t+\frac{1}{2}(\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g})\Delta t^2\\ \mathbf{v}& \leftarrow \mathbf{v}+(\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g})\Delta t\\ \mathbf{q}& \leftarrow \mathbf{q}\otimes \mathbf{q}\{({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\Delta t\}\\ {\mathbf{a}}_b& \leftarrow {\mathbf{a}}_b\\ {\mathbf{\omega }}_b& \leftarrow {\mathbf{\omega }}_b\end{array}$$

2. 预测误差状态的协方差矩阵:

$$\mathbf{P}\leftarrow {\mathbf{F}}_x\mathbf{P}{\mathbf{F}}_x^{\top }+{\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^{\top }$$

其中 ${\mathbf{F}}_x$ 和 ${\mathbf{F}}_i$ 是误差状态动力学的雅可比矩阵,而 ${\mathbf{Q}}_i$ 是IMU噪声的协方差矩阵。

如果一条腿着地,我们进入ESKF的更新步骤:

3. 使用腿部运动学计算足端位置 $\mathbf{y}$,作为位置和速度的观测值。

4. 计算观测值的雅可比矩阵 $\mathbf{H}$:

$$\mathbf{H}={\mathbf{H}}_x{\mathbf{X}}_{\delta x}$$

其中 ${\mathbf{H}}_x$ 是观测值相对于真实状态的雅可比矩阵,而 ${\mathbf{X}}_{\delta x}$ 是真实状态相对于误差状态的雅可比矩阵。

5. 计算卡尔曼增益:

$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^{\top }(\mathbf{HP}{\mathbf{H}}^{\top }+\mathbf{V}{)}^{-1}$$

其中 $\mathbf{V}$ 是观测噪声的协方差矩阵。

6. 更新误差状态估计值:

$$\widehat{\delta \mathbf{x}}\leftarrow \mathbf{K}(\mathbf{y}-h({\hat{\mathbf{x}}}_t))$$

7. 将误差状态注入标称状态:

$$\mathbf{x}\leftarrow \mathbf{x}\oplus \widehat{\delta \mathbf{x}}$$

8. 更新误差状态的协方差矩阵:

$$\mathbf{P}\leftarrow (\mathbf{I}-\mathbf{KH})\mathbf{P}$$

9. 重置误差状态:

$$\widehat{\delta \mathbf{x}}\leftarrow 0$$

$$\mathbf{P}\leftarrow \mathbf{GP}{\mathbf{G}}^{\top }$$

其中 $\mathbf{G}$ 是重置操作的雅可比矩阵,主要影响方向部分:

$$\mathbf{G}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& \mathbf{I}-[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }& 0\\ 0& 0& {\mathbf{I}}_6\end{array}\right]$$

这个过程不断重复,使四足机器人能够估计其状态,即使在IMU存在偏差和噪声的情况下。腿部运动学提供的测量值帮助校正了IMU积分产生的漂移,从而实现了准确的状态估计。

这个示例展示了ESKF在四足机器人状态估计中的核心思想，代码如下。

import numpy as np

# 定义一些辅助函数
def skew(v):
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def quaternion_multiply(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([w1*w2 - x1*x2 - y1*y2 - z1*z2,
                     w1*x2 + x1*w2 + y1*z2 - z1*y2,
                     w1*y2 - x1*z2 + y1*w2 + z1*x2,
                     w1*z2 + x1*y2 - y1*x2 + z1*w2])

def quaternion_to_rotation(q):
    return np.array([[q[0]**2+q[1]**2-q[2]**2-q[3]**2, 2*(q[1]*q[2]-q[0]*q[3]), 2*(q[1]*q[3]+q[0]*q[2])],
                     [2*(q[1]*q[2]+q[0]*q[3]), q[0]**2-q[1]**2+q[2]**2-q[3]**2, 2*(q[2]*q[3]-q[0]*q[1])],
                     [2*(q[1]*q[3]-q[0]*q[2]), 2*(q[2]*q[3]+q[0]*q[1]), q[0]**2-q[1]**2-q[2]**2+q[3]**2]])

def quaternion_from_axis_angle(axis, angle):
    axis = axis / np.linalg.norm(axis)
    return np.concatenate(([np.cos(angle/2)], np.sin(angle/2) * axis))

# 设置初始状态和协方差
p = np.zeros(3)
v = np.zeros(3)
q = np.array([1, 0, 0, 0])
a_b = np.zeros(3)
w_b = np.zeros(3)
P = np.eye(15)

# 设置IMU测量值和噪声
a_m = np.array([0, 0, -9.81])  # 重力加速度
w_m = np.array([0, 0, 0])
a_n = np.random.normal(0, 0.1, 3)
w_n = np.random.normal(0, 0.01, 3)
a_w = np.random.normal(0, 0.001, 3)
w_w = np.random.normal(0, 0.0001, 3)

# 设置时间步长和重力向量
dt = 0.01
g = np.array([0, 0, -9.81])

# 预测步骤
R = quaternion_to_rotation(q)
p += v * dt + 0.5 * (R @ (a_m - a_b) + g) * dt**2
v += (R @ (a_m - a_b) + g) * dt
q = quaternion_multiply(q, quaternion_from_axis_angle(w_m - w_b, np.linalg.norm(w_m - w_b) * dt))
F_x = np.eye(15)
F_x[0:3, 3:6] = np.eye(3) * dt
F_x[3:6, 6:9] = -R @ skew(a_m - a_b) * dt
F_x[3:6, 9:12] = -R * dt
F_x[3:6, 12:15] = np.eye(3) * dt
F_x[6:9, 6:9] = R.T @ quaternion_to_rotation(quaternion_from_axis_angle(w_m - w_b, np.linalg.norm(w_m - w_b) * dt))
F_x[6:9, 12:15] = -np.eye(3) * dt
F_i = np.zeros((15, 12))
F_i[3:6, 0:3] = np.eye(3)
F_i[6:9, 3:6] = np.eye(3)
F_i[9:12, 6:9] = np.eye(3)
F_i[12:15, 9:12] = np.eye(3)
Q_i = np.zeros((12, 12))
Q_i[0:3, 0:3] = np.eye(3) * 0.1**2 * dt**2
Q_i[3:6, 3:6] = np.eye(3) * 0.01**2 * dt**2
Q_i[6:9, 6:9] = np.eye(3) * 0.001**2 * dt
Q_i[9:12, 9:12] = np.eye(3) * 0.0001**2 * dt
P = F_x @ P @ F_x.T + F_i @ Q_i @ F_i.T

print("Prediction Step:")
print("p:", p)
print("v:", v)
print("q:", q)
print("P:", P)

# 更新步骤 (假设我们得到了一个位置测量值)
y = np.array([0.1, 0.2, 0.05])  # 模拟一个位置测量值
H_x = np.zeros((3, 15))
H_x[0:3, 0:3] = np.eye(3)
X_dx = np.zeros((15, 15))
X_dx[0:6, 0:6] = np.eye(6)
X_dx[6:9, 6:9] = np.eye(3) - skew(0.5 * np.zeros(3))
X_dx[9:15, 9:15] = np.eye(6)
H = H_x @ X_dx
V = np.eye(3) * 0.01**2
K = P @ H.T @ np.linalg.inv(H @ P @ H.T + V)
delta_x = K @ (y - p)
p += delta_x[0:3]
v += delta_x[3:6]
q = quaternion_multiply(q, quaternion_from_axis_angle(delta_x[6:9], np.linalg.norm(delta_x[6:9])))
a_b += delta_x[9:12]
w_b += delta_x[12:15]
P = (np.eye(15) - K @ H) @ P
G = np.eye(15)
G[6:9, 6:9] = np.eye(3) - skew(0.5 * delta_x[6:9])
P = G @ P @ G.T

print("Update Step:")
print("p:", p)
print("v:", v) 
print("q:", q)
print("a_b:", a_b)
print("w_b:", w_b)
print("P:", P)

这个示例首先进行预测步骤,使用IMU测量值和当前状态估计来预测新的标称状态,并预测误差状态的协方差矩阵。然后模拟了一个位置测量值,并使用这个测量值来更新状态估计。在更新步骤中,计算卡尔曼增益,更新误差状态估计,将误差状态注入标称状态,更新误差状态的协方差矩阵,并重置误差状态。

[1] Bledt, Gerardo, et al. “Contact model fusion for event-based locomotion in unstructured terrains.” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.