ROS 中里程计的计算代码理解

---

double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

x'y'的坐标系代表的是将世界坐标系XOY移动到机器人中心，参考用。theta代表此时机器人相对于世界坐标系X轴旋转的角度(0-2pi),其中

 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

可以写成 deltaX = vx*cos(theta) * dt+ vy*cost(theta + pi/2), * dt 这个是最原始的意思， 即x方向的速度在X方向（世界坐标系）的位移，理论上是这个公式，化简下，

机器人坐标系统中，一般认为，vx是机器人前进的方向，利用右手定则即可确定vy

显然，机器人的速度vy的方向角度=机器人vx角度+pi/2， 如上图

cos(theta + pi/2)=cos(theta)*cos(pi/2) - sin(theta)*sin(pi/2) = -sin(theta),

就得到delta_x = (vx * cos(th) - vy * sin(th)) * dt;

同理  delta_y = vx*sin(theta) + vy * sin(theta + pi/2) = vx*sin(theta) * dt+ vy * cos(theta) *dt

这个dt内是认为机器人走直线，保持的上一时刻的theta