轮腿平衡步兵总结 - 控制算法篇
卡尔曼滤波
学习可以参考该视频
代码中执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为 EKF/UKF/ESKF/AUKF 等。
龙贝格观测器
Observer.c 中封装了龙贝格观测器，开始是想用来通过状态空间方程估计不可观测量(例如 pitch 速度)，但实际调试效果并不好，后面可以在此基础上重新探索一下。
PID 算法
该 PID 算法封装了多种 PID 优化算法，初始化例子如下
PID_Init(&balance_infantry->turn_pid, SPEED_W_MAX, 0, 0, 9, 0, 0, 0, 0, 0.001, 0.009, 1, DerivativeFilter | OutputFilter);
说明使用了输出滤波和不完全微分两种优化。
该 PID 算法需要特别注意 dt 对积分项和差分项的影响，当掉线模式下，需要使用 PID_Clear 函数消去 PID 的积分项。
IMU 滤波算法
递推最小二乘法
该算法并不算调试通过，可能存在一些 bug，若需要使用该算法，可以在该基础上修改使用。
信号发生器
在 SignalGenerator.c 中封装了锯齿波，正弦波，阶跃波三种信号，可用作调试 PID(例如调试云台时可使用锯齿波模拟反陀螺时的输入信号)。
系统辨识信号
基于信号发生器封装的，不同频率正弦波输入信号。
跟踪微分器
里程计与加速度计数据融合算法设计
由于平衡步兵轮子或多或少存在打滑现象，故里程计的数据并非完全可信，故需要依靠底盘 IMU 的加速度计来和里程计数据进行融合来得到较为准确的轮子里程计信息。
在卡尔曼滤波中的式子之中
$$
x_{k+1} = A x_k + B u_k
$$
其中
$$
x_k = [x,x_v]^T
$$
x 为位移，即平衡步兵状态变量的位移量。$x_v$为速度，即平衡步兵状态变量的速度量，$x_a$为加速度计观测量。
$$
A = \begin{bmatrix}
1 & dt \ 0 & 1
\end{bmatrix}
$$
$$
B = [0.5 * (dt)^2 , dt]^T
$$
$$
u_k = x_a
$$
而对于观测方程
$$
z_k = H x_k
$$
$$
H = \begin{bmatrix}
0 & 1
\end{bmatrix}
$$
平衡步兵控制算法
参考RoboMaster 平衡步兵机器人控制系统设计以及五连杆解算
速度获取算法
// 得到速度的方法1： 一阶低通滤波
rc_filter(&balance_infantry->state_vector[STATE_PITCH_V], (balance_infantry->state_vector[STATE_PITCH] - balance_infantry->pitch_last) / balance_infantry->delta_t, balance_infantry->delta_t, LP_PITCH_V_RC);
balance_infantry->pitch_last = balance_infantry->sensors_info.pitch;
该代码中，直接使用差分然后低通滤波获取其速度，速度滞后较为严重且不够准确，可以考虑使用观测器来获取速度(类似于卡尔曼滤波 CV 运动模型)。
斜坡变腿长算法
腿长控制思想
腿可以看作是一个模拟弹簧阻尼系统，其中 P 决定了系统的刚度，D 决定了系统的阻尼，同时利用前馈补偿系统重力，利用积分项来消除各种其它外来因素导致的误差。
起跳过程本质上是多过程的腿长控制过程，在起跳的准备阶段，需要低刚度高阻尼的 PID 来压缩腿长(即机器人下蹲)，同时储存能量；在起跳的启动阶段，需要高刚度低阻尼的 PID 来快速增加腿长长度形成足够的爆发力；在机器人即将离地时，需要高刚度低阻尼的 PID 来快速收回机器人的腿，以使机器人的轮子快速远离地面；在机器人落地时，则需要低刚度高阻尼的 PID 来进行落地时的“缓冲”作用。
关键参数调整
MAX_VMC_TORQUE 该参数决定了最大的摆动力矩，若该力矩过大，则可能因为位移期望与实际值过大导致腿部发散。若该值过小，则在上坡时扭矩可能会不够导致车身角度过大飞坡失败。
MAX_VMC_FORCE 该参数决定了最大力，PID 里还限制了比 MAX_VMC_FORCE 还小的最大输出力，故该参数只在起跳时起最大限幅作用。
细节处理
getMaxXLimit(balance_infantry);
balance_infantry->target_vector[STATE_X] = balance_infantry->state_vector[STATE_X] + LIMIT_MAX_MIN(balance_infantry->target_vector[STATE_X] - balance_infantry->state_vector[STATE_X],balance_infantry->limit_err_x_max, balance_infantry->limit_err_x_min);
void getMaxXLimit(BalanceInfantry *balance_infantry)
{
const static float x_limit = 1.3f;
float temp_x_v = LIMIT_MAX_MIN(balance_infantry->state_vector[STATE_X_V], x_limit, -x_limit);
balance_infantry->limit_err_x_max = temp_x_v + x_limit;
balance_infantry->limit_err_x_min = temp_x_v - x_limit;
}
该算法核心思想是将期望 x 值与实际 x 值的差值限制在一定范围以内，而且该限制还会将当前速度考虑在内，即当前速度越大，所允许的差值就越大，从而避免了期望 x 值与实际 x 值的差值过大导致的系统发散。
目前尚存在问题总结
转向控制的不稳定性，即在小陀螺状态下无法保持在原地旋转，这可能是因为控制时是将力矩相反地叠加到左右轮子扭矩中，这并不保证两个轮子的转速相同，故会因为电机特性，轮子特性，机器人本身机械特性导致两个轮子存在速度差而不能转速相同。
旋转与前后移动未完全解耦，这会导致若前后移动过程中开启小陀螺状态出现翻车现象。
转向过程中很明显会存在两腿长度交替变长变短过程，这可能是因为斜坡变腿长算法存在一定的问题，也可能是 roll 轴控制的 PID 参数 P 过大导致的。
赛场上并没有跑出平衡步兵的最大速度，在新的被动电容研发出来之后，需要优化一下机器人速度。
离地检测不够稳定，导致下落凤坡时腿部角度过大直接进入保护状态(虽然能站起来但下得不够丝滑)。
