two-wheels-robot-odometry-calculation

Posted on In

两轮差速里程计需要计算出：odom_x, odom_y和odom_phi。

预设

需要提前获知机器人的参数有：

  • 左右轮编码器：encoder_left和encoder_right
  • 左右轮的直径：R
  • 传动比：gear_ratio
  • 脉冲数：pulse
  • 两轮的间距：two_wheel_distance
  • 底盘上报数据的频率间隔固定：delta_second，单位秒

左右轮编码器

也就是左右轮的脉冲数，假设编码器的值不是累加的，即，机器停止后编码器值归零。

计算 odom_phi

计算出每个时间间隔内，左右轮编码的差值

1
2
auto diff_encoder_left; // 包含正负
auto diff_encoder_right;// 包含正负

计算单个脉冲行走的距离：左右轮直径 * M_PI / 传动比 / 脉冲数

1
auto pulse_length = R * M_PI / gear_ratio / pulse;

分别计算出左右轮的线速度：单个脉冲行走距离 * 轮子走过的脉冲数 / 时间差

1
2
auto left_wheel_velocity = pulse_length * diff_encoder_left / delta_second;
auto right_wheel_velocity = pulse_length * diff_encoder_right / delta_second;

计算角速度：(右轮线速度 - 左轮角速度) / 两轮间距

1
auto angle_velocity = (right_wheel_velocity - left_wheel_velocity) / two_wheel_distance

计算单位时间内转过的角度：角速度 * 时间差

1
auto delta_phi = angle_velocity * delta_second;

最后计算角度：delta_phi的积分，注意归一化

1
2
odom_phi += delta_phi;
最后将odom_phi归一化

计算 odom_x 和 odom_y

在上面计算的基础上，计算机器的平均线速度：(左轮线速度 + 右轮线速度) / 2

1
auto linear_velocity = (left_wheel_velocity + right_wheel_velocity) / 2

计算坐标x和y在单位时间内的变化

1
2
delta_x = linear_velocity * delta_second * cos(odom_phi_);
delta_y = linear_velocity * delta_second * sin(odom_phi_);

最后计算odom_x和odom_y：delta_x和delta_y分别做积分

1
2
odom_x_ += delta_x;
odom_y_ += delta_y;
参考1blog.csdn.net/just_do_it567/article/details/107562371
参考2blog.csdn.net/xingdou520/article/details/83691951