双轮差速模型

移动距离

delta_dist = (encoder_right + encoder_left)/linesNum * circumference / 2;

旋转角度

Delta_th = (float)(encoder_right - encoder_left) / wheel_track;

将移动距离转换到车体坐标系

delta_x = cos(delta_th) * delta_dist;
delta_y = -sin(delta_th) * delta_dist;

从车体坐标系转到世界坐标系

pos_x += (cos(th) * delta_x - sin(th) * delta_y);
pos_y += (sin(th) * delta_x + cos(th) * delta_y);

安装ros串口

sudo apt-get install ros-melodic-serial

delta_dist = (encoder_right + encoder_left)/linesNum * circumference / 2;
Delta_th = (float)(encoder_right - encoder_left) / wheel_track;
//                    delta_speed = delta_dist / dt;
                    delta_x = cos(delta_th) * delta_dist;
                    delta_y = -sin(delta_th) * delta_dist;
                    th += delta_th;
                    pos_x += (cos(th) * delta_x - sin(th) * delta_y);
                    pos_y += (sin(th) * delta_x + cos(th) * delta_y);

Circumference 为轮周长

linesNum为一圈的脉冲数=变比*脉冲数

Wheel_track

假设 轮子的周长为circumference

轮距为whell_dist

那么小车原地旋转一周的走过的路径为pi * whell_dist，小车的姿态变化为360度。

小车转的圈数为cylinder_num = pi * whell_dist / circumference

所产生的脉冲数为 total_num = cylinder_num * linesNum * 2

所以对应小车姿态变化一度为total_num / 360

姿态转化为odom

geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
odom.pose.pose.position.x = pos_x;
odom.pose.pose.position.y = pos_y;
odom.pose.pose.position.z = 0;
odom.pose.pose.orientation = odom_quat;