差分轮算法－两个轮子计算速度的方法-阿克曼四轮小车计算方法

四轮驱小车的话：
转向角度计算方法：float turning_angle = z_angular / x_linear; // 转向角度，单位为弧度
速度的话直接用线速度
两轮驱动小车：
计算公式：
leftSpeed = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0; #左轮速度
rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;#右轮速度
其中x_linear和z_angular是由del_cmd话题发出来的twist数据

void OriginbotBase::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{DataFrame cmdFrame;float leftSpeed = 0.0, rightSpeed = 0.0;float x_linear = msg->linear.x; float z_angular = msg->angular.z;//差分轮运动学模型求解leftSpeed  = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;// RCLCPP_INFO(this->get_logger(), "leftSpeed = '%f' rightSpeed = '%f'", leftSpeed * 100, rightSpeed * 100);if (leftSpeed < 0)cmdFrame.data[0] = 0x00;elsecmdFrame.data[0] = 0xff;cmdFrame.data[1] = int(abs(leftSpeed) * 1000) & 0xff;         //速度值从m/s变为mm/scmdFrame.data[2] = (int(abs(leftSpeed) * 1000) >> 8) & 0xff;if (rightSpeed < 0)cmdFrame.data[3] = 0x00;elsecmdFrame.data[3] = 0xff;cmdFrame.data[4] = int(abs(rightSpeed) * 1000) & 0xff;        //速度值从m/s变为mm/scmdFrame.data[5] = (int(abs(rightSpeed) * 1000) >> 8) & 0xff;cmdFrame.check = (cmdFrame.data[0] + cmdFrame.data[1] + cmdFrame.data[2] + cmdFrame.data[3] + cmdFrame.data[4] + cmdFrame.data[5]) & 0xff;// 封装速度命令的数据帧cmdFrame.header = 0x55;cmdFrame.id     = 0x01;cmdFrame.length = 0x06;cmdFrame.tail   = 0xbb;try{serial_.write(&cmdFrame.header, sizeof(cmdFrame)); //向串口发数据}catch (serial::IOException &e){RCLCPP_ERROR(this->get_logger(), "Unable to send data through serial port"); //如果发送数据失败,打印错误信息}// 考虑平稳停车的计数值if((fabs(x_linear)>0.0001) || (fabs(z_angular)>0.0001))auto_stop_count_ = 0;// printf("Frame raw data: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \n", //         cmdFrame.header, cmdFrame.id, cmdFrame.length, cmdFrame.data[0], cmdFrame.data[1], cmdFrame.data[2], //         cmdFrame.data[3], cmdFrame.data[4], cmdFrame.data[5], cmdFrame.check, cmdFrame.tail);
}