原文地址：https://husarion.com/tutorials/ros-tutorials/3-simple-kinematics-for-mobile-robot

代码地址：https://github.com/husarion/hFramework/blob/master/src/rosbot/ROSbot.cpp#L136

核心部分代码：

enc_FR = wheelFR->getDistance();
enc_RR = wheelRR->getDistance();
enc_RL = wheelRL->getDistance();
enc_FL = wheelFL->getDistance();
//四个*的弧度
wheel_FL_ang_pos = 2 * 3.14 * enc_FL / enc_res;
wheel_FR_ang_pos = 2 * 3.14 * enc_FR / enc_res;
wheel_RL_ang_pos = 2 * 3.14 * enc_RL / enc_res;
wheel_RR_ang_pos = 2 * 3.14 * enc_RR / enc_res;

//左轮和右轮编码器值
enc_L = (enc_FL + enc_RL) / (2 * tyre_deflection);
enc_R = (enc_FR + enc_RR) / (2 * tyre_deflection);

//左轮和右轮角速度
wheel_L_ang_vel = ((2 * 3.14 * enc_L / enc_res) - wheel_L_ang_pos) / delay_s;
wheel_R_ang_vel = ((2 * 3.14 * enc_R / enc_res) - wheel_R_ang_pos) / delay_s;
//左轮和右轮弧度
wheel_L_ang_pos = 2 * 3.14 * enc_L / enc_res;
wheel_R_ang_pos = 2 * 3.14 * enc_R / enc_res;

robot_angular_vel = (((wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / (robot_width * diameter_mod)) - robot_angular_pos) / delay_s;
robot_angular_pos = (wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / (robot_width * diameter_mod);
robot_x_vel = (wheel_L_ang_vel * wheel_radius + robot_angular_vel * robot_width / 2) * cos(robot_angular_pos);
robot_y_vel = (wheel_L_ang_vel * wheel_radius + robot_angular_vel * robot_width / 2) * sin(robot_angular_pos);
robot_x_pos = robot_x_pos + robot_x_vel * delay_s;
robot_y_pos = robot_y_pos + robot_y_vel * delay_s;
sys.delay(loop_delay);

未完待续