imu+wheel融合

Imu+Wheel融合

1 轮速计

1.1 航迹递推

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

1.1.1 基于欧拉法

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

$$\left[\begin{array}{c}{\theta }_{t+1}\\ x_{t+1}\\ y_{t+1}\end{array}\right]=\left[\begin{array}{c}{\theta }_t\\ x_t\\ y_t\end{array}\right]+\left[\begin{array}{c}\omega \Delta t\\ \cos \left({\theta }_t\right)\ast v_x\Delta t\\ \sin \left({\theta }_t\right)\ast v_x\Delta t\end{array}\right]$$

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

// 方法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积分

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

$$\left[\begin{array}{c}{\theta }_{t+1}\\ x_{t+1}\\ y_{t+1}\end{array}\right]=\left[\begin{array}{c}{\theta }_t\\ x_t\\ y_t\end{array}\right]+\left[\begin{array}{c}\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{array}\right]$$

代码实现：

// 方法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
$$\left[\begin{array}{c}{\theta }_{t+1}\\ {\mathbf{p}}_{t+1}\end{array}\right]=\left[\begin{array}{c}{\theta }_t\\ {\mathbf{p}}_t\end{array}\right]+\left[\begin{array}{c}\alpha \\ \mathbf{R}({\theta }_t)\mathbf{A}(\alpha )\mathbf{u}\end{array}\right]$$

其中$\mathbf{A}(\alpha )$
$$=\left[\begin{array}{cc}\frac{\sin (\alpha )}{\alpha }& -\frac{1-\cos (\alpha )}{\alpha }\\ \frac{1-\cos (\alpha )}{\alpha }& \frac{\sin (\alpha )}{\alpha }\end{array}\right]$$

代码实现：

// 方法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\times 3}=\left[\begin{array}{ccc}1& 0& -\Delta s_{k}sin({\theta }_k+\frac{1}{2}\Delta {\theta }_k)\\ 0& 1& \Delta s_{k}cos({\theta }_k+\frac{1}{2}\Delta {\theta }_k)\\ 0& 0& 1\end{array}\right]$$

实际系统是三维的，所以共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{array}{c}F=\left[\begin{array}{cccccc}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{array}\right]\end{array}$$

// 更新状态向量
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{array}{rl}h(X_k)& =H{X}_k\\ & =(0,0,1)\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]\\ & ={\theta }_k\end{array}$$

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;

3 数据处理

对于imu和wheel的处理(有时间再补代码)

① 简单处理：找到wheel最近的imu数据

② 线性插值处理，更精确！