局部全局坐标转换

模型

image-20211029181117606.png

公式:

image-20211029180943040.png

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

1
2
3
4
5
6
7
8
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;