移动机器人的一点简单运动学
原文地址：https://husarion.com/tutorials/ros-tutorials/3-simple-kinematics-for-mobile-robot
有助于理解公式的文章：https://blog.csdn.net/x_r_su/article/details/53375905
https://blog.csdn.net/heyijia0327/article/details/44983551
https://blog.csdn.net/u012836279/article/details/79839395
代码地址：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);
未完待续
