机器人之里程计

1.两轮小车的位置描述

p(x, y, thelt)

2.两轮小车的运动模型

1).小车的线速度v = (vl +vr) /2
2).小车的运动可以微分成绕着某个点做圆周运动

2.1如何推导出p

// dl为小车的相对位移，dx和dy可以通过两轮编码器获得
dl = (dx +dy)/2
x = x_last + v_xdt
x = x_last + dx
x = x_last + dlcos(thelt)

3.小车速度转换成左右轮的速度

v_r = v + d/2