Balance Infantry

前言

Webots 仿真

简单平衡小车

参数确定

取两个轮子的质量均为1

小车主体质量为3

支撑板质量为1

主体质心距轮子0.04m，支撑为0.016

主体加支撑距轮子中心为L=0.034m

g=9.8

令$\xi = [\theta,\dot{\theta},x,\dot{x}]^T$；且令$\theta\to 0; \dot{\theta} \to 0$，相当于在0处进行泰勒展开

则状态转移方程 $ \dot{\xi} = A\xi + Bu $

先取

使用MATLAB计算K矩阵

1
2
3
4
5
6
7
8
9
10
Q = [10,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1];
R = 0.1;
A = [0,1,0,0; 432.353,0,0,0; 0,0,0,1; -4.9,0,0,0];
B = [0; -7.353; 0; 0.25];
C = [0,0,1,0];
D = 0;

sys = ss(A,B,C,D);
[K,~,~] = lqr(sys, Q,R);
disp(K);

得到 K 矩阵

则输出 u

代入仿真程序后，效果似乎不是很好，但也能初见雏形，手动调整一下参数，得到如下效果。

se.gif

速度跟随

哈工程方案

​ 直接修改target_pos来步进

湖大方案

​ 修改target_speed，并且乘上dt来修改target_dist(位移)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
static void WokingStateSet()
{
    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
    {
        ResetChassis();
        return;
    }
    else if (chassis_cmd_recv.chassis_mode == CHASSIS_STOP) // 未收到遥控器和云台指令底盘进入急停
    {
        for (uint8_t i = 0; i < JOINT_CNT; i++)
            HTMotorStop(joint[i]);
        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
            LKMotorStop(driven[i]);
        return; // 关闭所有电机,发送的指令为零
    }

    // 运动模式
    EnableAllMotor();
    // 设置目标速度/腿长/距离
    l_side.target_len += chassis_cmd_recv.delta_leglen;
    r_side.target_len += chassis_cmd_recv.delta_leglen;
    VAL_LIMIT(l_side.target_len, 0.13, 0.4); // 腿长限幅
    VAL_LIMIT(r_side.target_len, 0.13, 0.4);
    // 加速度限幅,防止键盘控制摔倒
    if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
        chassis.target_v = chassis_cmd_recv.vx;
    else
        chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
    // 模型距离参考输入
    chassis.target_dist += chassis.target_v * del_t;
    chassis.target_yaw = chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度
}

......
    
/**
 * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
 * @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
 *
 */
static void CalcLQR(LinkNPodParam *p)
{
    static float k[12][3] = {60.071162, -57.303242, -8.802552, -0.219882, -1.390464, -0.951558, 32.409644, -23.635877, -9.253521, 12.436266, -10.318639, -7.529540, 135.956804, -124.044032, 36.093582, 13.977819, -12.325081, 3.766791, 32.632159, -39.012888, 14.483707, 7.204778, -5.973109, 1.551763, 78.723013, -73.096567, 21.701734, 63.798027, -55.564993, 15.318437, -195.813486, 140.134182, 60.699132, -18.357761, 13.165559, 2.581731};
    float T[2] = {0}; // 0 T_wheel  1 T_hip
    float l = p->leg_len;
    float lsqr = l * l;
    // float dist_limit = abs(chassis.target_dist - chassis.dist) > MAX_DIST_TRACK ? sign(chassis.target_dist - chassis.dist) * MAX_DIST_TRACK : (chassis.target_dist - chassis.dist); // todo设置值
    // float vel_limit = abs(chassis.target_v - chassis.vel) > MAX_VEL_TRACK ? sign(chassis.target_v - chassis.vel) * MAX_VEL_TRACK : (chassis.target_v - chassis.vel);
    for (uint8_t i = 0; i < 2; ++i)
    {
        uint8_t j = i * 6;
        T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
               (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
               (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis.target_dist - chassis.dist) +
               (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis.target_v - chassis.vel) +
               (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis.pitch +
               (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis.pitch_w;
    }
    p->T_wheel = T[0];
    p->T_hip = T[1];
}
上交方案

​ 未知

ollie

腾讯Ollie轮腿机器人论文阅读及分享 - 知乎 (zhihu.com)

平衡中值修正

将 $\theta $ 修正为 $\theta - \alpha $

取观测空间 $\hat{\xi}$ 与 输出 y

则可以得到观测器方程

将之前得到的K矩阵添加一项

实际上不使用观测器，在状态空间中加上位移x，通过位移的积累，也可以使其平衡。这个时候通过修改期望位移x（而不是实际位置），或许就可以实现前进控制。

去掉位移x之后，就没法保持静止而是以一定速度向偏重的一方前进。添加观测器之后，理应是会收敛到实际值的，但是目前似乎一直是发散的，有点伤脑筋。

五连杆轮腿平衡车