imu+wheel融合

news2024/9/20 14:56:05

Imu+Wheel融合

文章目录

Imu+Wheel融合
1 轮速计
- 1.1 航迹递推
- - 1.1.1 基于欧拉法
  - 1.1.2 基于二阶Runge-Kutta积分
  - 1.1.3 群空间闭式积分
- 1.2 雅可比计算
2 IMU观测更新
3 数据处理

1 轮速计

1.1 航迹递推

常见的轮速计积分的方式有三种：欧拉积分、二阶Runge-Kutta积分、群空间闭式积分。在我们代码中，分别实现了前面两种递归公式！

在这里插入图片描述

1.1.1 基于欧拉法

误差主要来源于平移积分过程中认为角度恒定。直线运动时不受影响，以及 Δt 很小时误差影响也较小。

$\begin{bmatrix}\theta_{t+1}\\x_{t+1}\\y_{t+1}\end{bmatrix}=\begin{bmatrix}\theta_t\\x_t\\y_t\end{bmatrix}+\begin{bmatrix}\omega\Delta t\\\cos\left(\theta_t\right)*v_x\Delta t\\\sin\left(\theta_t\right)*v_x\Delta t\end{bmatrix}$

代码实现：下面旋转矩阵展开就是上面的形式。手推即可验证，有时间补充

// 方法1：利用旋转矩阵进行递推更新
const Eigen::Matrix3d delta_R = Eigen::AngleAxisd(u_now(1), Eigen::Vector3d::UnitZ()).toRotationMatrix();
const Eigen::Vector3d delta_p = Eigen::Vector3d(u_now(0), 0., 0.);

R_G_O = old_R_G_O * delta_R;       // R_Go(k+1) = R_Go(k)*delta_R
p_G_O = old_p_G_O + old_R_G_O * delta_p;
x_n = x_p + u_now(1); // theta

1.1.2 基于二阶Runge-Kutta积分

平移积分过程中认为角度为运动前后的均值，误差比欧拉积分小一点

$\begin{bmatrix}\theta_{t+1}\\x_{t+1}\\y_{t+1}\end{bmatrix}=\begin{bmatrix}\theta_t\\x_t\\y_t\end{bmatrix}+\begin{bmatrix}\omega\Delta t\\\cos{(\theta_t+\frac{\omega\Delta t}2)}v_x\Delta t\\\sin{(\theta_t+\frac{\omega\Delta t}2)}v_x\Delta t\end{bmatrix}$

代码实现：

// 方法2
x_now(0) = x_pre(0) + u_now(0) * ::cos( x_pre(2) + 0.5 * u_now(1) ); // x
x_now(1) = x_pre(1) + u_now(0) * ::sin( x_pre(2) + 0.5 * u_now(1) ); // y
x_now(2) = x_pre(2) + u_now(1); // theta

1.1.3 群空间闭式积分

参考

u就是Δp
$\begin{bmatrix}\theta_{t+1}\\\mathbf{p}_{t+1}\end{bmatrix}=\begin{bmatrix}\theta_{t}\\\mathbf{p}_{t}\end{bmatrix}+\begin{bmatrix}\alpha\\\mathbf{R}(\theta_{t})\mathbf{A}(\alpha)\mathbf{u}\end{bmatrix}$

其中 $\mathbf{A}(\alpha)$
$=\begin{bmatrix}\frac{\sin(\alpha)}{\alpha}&-\frac{1-\cos(\alpha)}{\alpha}\\\frac{1-\cos(\alpha)}{\alpha}&\frac{\sin(\alpha)}{\alpha}\end{bmatrix}$

代码实现：

// 方法3
const Eigen::Matrix3d A_old_yaw = xxx;		

const Eigen::Matrix3d delta_R = Eigen::AngleAxisd(u_now(1), Eigen::Vector3d::UnitZ()).toRotationMatrix();

const Eigen::Vector3d delta_p = Eigen::Vector3d(u_now(0), 0., 0.);

R_G_O = old_R_G_O * delta_R;       // R_Go(k+1) = R_Go(k)*delta_R
p_G_O = old_p_G_O + old_R_G_O * A_old_yaw * delta_p;
x_n = x_p + u_now(1); // theta

1.2 雅可比计算

如果只有 x y yaw三个状态量，雅可比计算如下：

$F_{3\times3}=\begin{bmatrix}1&0&-\Delta s_ksin(\theta_k+\frac12\Delta\theta_k)\\0&1&\Delta s_kcos(\theta_k+\frac12\Delta\theta_k)\\0&0&1\end{bmatrix}$

实际系统是三维的，所以共6个自由度，即由6个变量组成：

// 计算状态雅可比矩阵
F = Matrix6::Identity();
F(3, 2) = -u_now(0) * std::sin(x_p);
F(4, 2) = u_now(0) * std::cos(x_p);

分别对 r p yaw x y z进行求导
$\begin{equation} F = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & -u_{\text{now}}(0) \sin(x_p) & 1 & 0 & 0 \\ 0 & 0 & u_{\text{now}}(0) \cos(x_p) & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation}$

// 更新状态向量
x_now(0) = x_pre(0);  // 保持滚转角 r 不变
x_now(1) = x_pre(1);  // 保持俯仰角 p 不变
x_now(2) = x_pre(2) + u_now(1);  // 更新偏航角 y
x_now(3) = x_pre(3) + u_now(0) * std::cos(x_pre(2));  // 更新位置 x
x_now(4) = x_pre(4) + u_now(0) * std::sin(x_pre(2));  // 更新位置 y
x_now(5) = x_pre(5);  // 保持位置 z 不变

2 IMU观测更新

只使用imu测出的角度增量作为观测值，而且观测方程刚好是线性的！不需要再次计算雅可比矩阵

2D: x y yaw

$\begin{aligned}h(X_k)&=HX_k\\&=(0,0,1)\begin{bmatrix}x_k\\y_k\\\theta_k\end{bmatrix}\\&=\theta_k\end{aligned}$

3D：r p yaw x y z

H << 0, 0, 1, 0, 0 , 0;

标准EKF过程

double old_angle = x_p;  // 获取角度值
double new_angle = x_n; 

DataType h = new_angle - old_angle;  // 预测delta_theta

// 观测误差----测试把误差反过来会怎么样(会跑飞！)---常规EKF公式都是 残差r = 观测z-预测CX
double error = z - h;

// 卡尔曼增益
double S = H * P_now * H.transpose() + R;
Matrix6x1 K = P_now * H.transpose() * (1 / S);

// 状态更新
const Eigen::Matrix3d delta_R = Eigen::AngleAxisd(K[2] * error, Eigen::Vector3d::UnitZ()).toRotationMatrix();
R_G_O = R_G_O * delta_R;       // R_Go(k+1) = R_Go(k)*delta_R
p_G_O[0] += K[3] * error;
p_G_O[1] += K[4] * error;
x_n += K[2] * error;