算法模块
| 文件名 | 功能简要 |
|---|---|
| Eul_w_filter.cpp / Eul_w_filter.h | 用于欧拉角速度滤波（姿态估计的角速度平滑） |
| foot_placement.cpp / foot_placement.h | 足端位置规划（多用于步态规划、平衡控制） |
| gait_scheduler.cpp / gait_scheduler.h | 步态调度器（步态状态机、步态切换、步态周期控制） |
| joystick_interpreter.cpp / joystick_interpreter.h | 摇杆解释器（人机交互或远程控制的速度指令解析） |
| mpc.cpp / mpc.h | 模型预测控制（用于轨迹规划、步态控制或平衡控制） |
| pino_kin_dyn.cpp / pino_kin_dyn.h | Pinocchio动力学/运动学接口（封装好的运动学求解器） |
| priority_tasks.cpp / priority_tasks.h | 任务优先级模块（多任务控制，如Stack-of-Tasks等） |
| StateEst.cpp / StateEst.h | 状态估计模块（卡尔曼滤波或其他方法估计机器人状态） |
| wbc_priority.cpp / wbc_priority.h | 具有优先级的全身控制（Whole-Body Control）模块 |
jocabi矩阵的作用
四元数的形式
-
四元数通常表示为：q=w+xi+yj+zk
w：实部（标量部分），表示旋转的角度信息。
x,y,z：虚部（向量部分），表示旋转轴的方向。
避免万向节锁：欧拉角在特定角度（如俯仰角±90°）时丢失自由度，而四元数无此问题。 -
四元数的规范化：
$ | | q | | = \sqrt { w ^ { 2 } + x ^ { 2 } + y ^ { 2 } + z ^ { 2 } } = 1$ -
四元数到旋转矩阵：$ R = \begin{bmatrix} 1 - 2 y ^ { 2 } - 2 z ^ { 2 } 2 x y - 2 w z 2 x z 2 x z 2 x z \ 2 x y + 2 w z 1 - 2 x ^ { 2 } - 2 z ^ { 2 } 2 y z - 2 w x \ 2 x z - 2 w y 2 y z + 2 w x 1 - 2 x ^ { 2 } - 2 y ^ { 2 } \end{bmatrix} $
旋转矩阵的使用
-
绕x轴旋转：
M << 1, 0, 0,
0, cos(theta), -sin(theta),
0, sin(theta), cos(theta); -
绕y轴旋转：
M << cos(theta), 0, sin(theta),
0, 1, 0,
-sin(theta), 0, cos(theta); -
绕z轴旋转：
M << cos(theta), -sin(theta), 0,
sin(theta), cos(theta), 0,
0, 0, 1;
贝塞尔曲线的使用
- $ B ( s ) = \sum _ { i = 0 } ^ { n } C ( n , i ) ( 1 - s ) ^ { n - i } s ^ { i } P _ { i }， 其中 s∈[0,1] $ 。
- 上面的贝塞尔曲线本身就是连续没有拐点的，同时导数也是连续的。
- 在一维（Bezier_1D）里，P 是一个向量，保存了 控制点的值。
卡尔曼滤波
卡尔曼滤波是一种递归状态估计算法，通过融合系统模型预测和传感器观测数据，在噪声干扰下得到最优状态估计。
- 预测-更新循环：交替进行模型预测和测量校正。
- 最小方差准则：通过协方差矩阵量化不确定性，追求估计误差最小。
状态方程：$ x _ { k } = A _ { k } x _ { k - 1 } + B _ { k } u _ { k } + w _ { k }$
观测方程： $z _ { k } = H _ { k } x _ { k } + v _ { k } $
以下是卡尔曼滤波（Kalman Filter）的详细过程解析，结合数学推导和实际意义说明：
- 卡尔曼滤波五大公式及解释
假设线性系统模型如下：
- 状态方程：( \mathbf{x}_k = \mathbf{A}k \mathbf{x}{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k )
- 观测方程：( \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf )
其中： - ( \mathbf{x}_k )：系统状态（如位置、速度）。
- ( \mathbf{u}_k )：控制输入。
- ( \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k) )：过程噪声（建模误差）。
- ( \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k) )：观测噪声。
- ( \mathbf{A}_k, \mathbf{B}_k, \mathbf{H}_k )：状态转移、控制输入、观测矩阵。
步骤1：状态预测（时间更新）
-
先验状态估计：
[ \hat{\mathbf{x}}_k^- = \mathbf{A}k \hat{\mathbf{x}}{k-1} + \mathbf{B}_k \mathbf{u}_k
]- 基于上一时刻最优估计 ( \hat{\mathbf{x}}_{k-1} ) 和系统模型预测当前状态。
-
先验协方差预测：
[
\mathbf{P}_k^- = \mathbf{A}k \mathbf{P}{k-1} \mathbf{A}_k^T + \mathbf{Q}_k
]- ( \mathbf{P}_k^- ) 表示预测状态的不确定性（含过程噪声 ( \mathbf{Q}_k )）。
步骤2：卡尔曼增益计算
[
\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}
]
- 物理意义：
- 分子 ( \mathbf{P}_k^- \mathbf{H}_k^T )：预测状态与观测的协方差。
- 分母 ( \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k )：预测观测的总不确定性（含传感器噪声 ( \mathbf{R}_k )）。
- 作用：权衡模型预测与观测的信任度。若传感器噪声大（( \mathbf{R}_k ) 大），增益 ( \mathbf{K}_k ) 减小，更依赖模型预测。
步骤3：状态更新（测量校正）
[
\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-)
]
- 残差项 ( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^- )：观测值与预测值的差异（即“新息”）。
- 更新逻辑：通过卡尔曼增益将残差加权修正预测值。
步骤4：协方差更新
[
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-
]
- 意义：修正后的状态不确定性降低（( \mathbf{P}_k \leq \mathbf{P}_k^- )）。
- 扩展卡尔曼滤波（EKF）
针对非线性系统（如机器人SLAM）： - 线性化：在当前估计点对非线性模型 ( f(\mathbf{x}), h(\mathbf{x}) ) 进行一阶泰勒展开。
- 状态方程雅可比矩阵：( \mathbf{F}k = \left. \frac{\partial f}{\partial \mathbf{x}} \right|{\hat{\mathbf{x}}_{k-1}} )。
- 观测方程雅可比矩阵：( \mathbf{J}k = \left. \frac{\partial h}{\partial \mathbf{x}} \right|{\hat{\mathbf{x}}_k^-} )。
- 公式调整：
- 预测：( \hat{\mathbf{x}}k^- = f(\hat{\mathbf{x}}{k-1}) )。
- 协方差：( \mathbf{P}_k^- = \mathbf{F}k \mathbf{P}{k-1} \mathbf{F}_k^T + \mathbf{Q}_k )。
- 增益：( \mathbf{K}_k = \mathbf{P}_k^- \mathbf{J}_k^T (\mathbf{J}_k \mathbf{P}_k^- \mathbf{J}_k^T + \mathbf{R}_k)^{-1} )。
MPC的控制过程
- 线性化动力学（Ac, Bc）
- 当前机器人站在世界系某个角度（base_rot），所以要将惯性矩阵和其他力项转换到世界系。
- 离散变换，把连续时间Ac, Bc乘上dt得到离散时间矩阵A, B。
- 累积状态转移矩阵（Aqp, Aqp1）
- 累积输入矩阵（Bqp1, Bqp11, Bqp）
- 构建QP问题