1 概述

以下为两轮差速机器人计算的相关公式.

  • v: 机器人的线速度

  • w: 机器人的角速度

  • L: 两轮之间的距离

  • x: 机器人车体朝向在全局坐标系 x 轴的位置

  • y: 机器人车体朝向在全局坐标系 x 轴的位置

  • θ: 机器人车体朝向与全局坐标系 x 轴正方向的夹角

  • 坐标系遵守右手法则: X轴指向前方, Y轴指向左方, Z轴指向上方.

2 运动学正解

1
2
3
4
5
6
7
8
9
v = ( v(R) + v(L) ) / 2

w = ( v(R) + v(L) ) / L

x(t+1) = x(t) + v * cos(θ) * dt

x(t+1) = x(t) + v * cos(θ) * dt

θ(t+1) = θ(t) + w * dt

3 运动学逆解

1
2
3
v(R) = v + (L / 2) * w;

v(R) = v - (L / 2) * w;

4 编码电机的计算

  • 轮子的半径为 rd, 直径为 dl, 圆周率为 PI.

  • 编码脉冲计数为 Tick, s 为 1 个 Ticks 轮子走过的距离, S 为经过 dt 时间后轮子走过的举例.

4.1 轮子的速度计算

左右轮子的计算公式为.

1
s = 2 * rd * PI

轮子的速度为.

1
2
3
4
5
S = s * Ticks

v(R/L) = S / dt

w(R/L) = v(R/L) / rd

5 PID

由于两轮差速机器人需要控制 v(R) == v(L), 这里描述一下基础版的 PID 算法.

PID的精髓是: 比例调节(P), 积分调节(I), 微分调节(D).

快准稳!!!

1
u = kp * error + ki ∗ ∫error + kd * ( derror / dt)

在实际的计算当中, 我们当作这个过程是离散的.

5.1 比例控制算法

假设目标值为 T = 100, 初始数值为 0, 此时 kp = 0.5.

第一时刻: u = 0.5 * 100 = 50, 此时 T = 0 + 50 = 50.
第二时刻: u = 0.5 * 50 = 25, 此时 T = 50 + 25 = 75.
…

但是光靠比例控制算法我们可能无法到达目标值, 实际的情况下轮子存在摩擦力, 还可能有其他阻力, 这些阻力可能抵消掉我们通过 kp 靠近的数值, 我们称之为稳态误差.

5.1 积分控制算法

积分控制算法用来帮我们抵消掉存在的阻力.

假设目标值为 T = 100, 初始数值为 0, 此时 kp = 0.5, ki = 0.2.

第一时刻: u = 0.5 * 100 + 0.2 * 100 = 50 + 20 = 70, 此时 T = 0 + 70 = 70.
第二时刻: u = 0.5 * 30 + 0.2 * (100 + 30) = 15 + 26 = 41, 此时 T = 70 + 41 = 111.

这个积分项会将前面若干次的误差进行累计，所以可以很好的消除稳态误差.

5.2 微分控制算法

当车辆离目标非常接近的时候, 我们要考虑到平稳过渡过去, 为了使这个过程足够平缓, 我们加入了微分项.

1
u= kd * (error(t) - error(t-1))

假设目标值为 T = 100, 初始数值为 0, 此时 kp = 0.5, ki = 0.2, kd = 0.1.

第一时刻: u = 0.5 * 100 + 0.2 * 100 + 0.1 * 100 = 50 + 20 + 10 = 80, 此时 T = 0 + 80 = 80.
第一时刻: u = 0.5 * 20 + 0.2 * (100 + 20) + 0.1 * (20 - 100) = 10 + 24 - 8 = 26, 此时 T = 80 + 26 = 106.