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

四轮驱小车的话

转向角度计算方法: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数据

cpp 复制代码
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;
    else
        cmdFrame.data[0] = 0xff;
    cmdFrame.data[1] = int(abs(leftSpeed) * 1000) & 0xff;         //速度值从m/s变为mm/s
    cmdFrame.data[2] = (int(abs(leftSpeed) * 1000) >> 8) & 0xff;

    if (rightSpeed < 0)
        cmdFrame.data[3] = 0x00;
    else
        cmdFrame.data[3] = 0xff;
    cmdFrame.data[4] = int(abs(rightSpeed) * 1000) & 0xff;        //速度值从m/s变为mm/s
    cmdFrame.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);
}
相关推荐
漫路在线13 分钟前
JS逆向-某易云音乐下载器
开发语言·javascript·爬虫·python
先做个垃圾出来………41 分钟前
哈夫曼树(Huffman Tree)
数据结构·算法
BillKu2 小时前
Vue3 Element Plus 对话框加载实现
javascript·vue.js·elementui
phoenix@Capricornus2 小时前
反向传播算法——矩阵形式递推公式——ReLU传递函数
算法·机器学习·矩阵
初遇你时动了情3 小时前
html js 原生实现web组件、web公共组件、template模版插槽
前端·javascript·html
Inverse1623 小时前
C语言_动态内存管理
c语言·数据结构·算法
数据与人工智能律师3 小时前
虚拟主播肖像权保护,数字时代的法律博弈
大数据·网络·人工智能·算法·区块链
前端小崔3 小时前
从零开始学习three.js(18):一文详解three.js中的着色器Shader
前端·javascript·学习·3d·webgl·数据可视化·着色器
wuqingshun3141593 小时前
蓝桥杯 16. 外卖店优先级
c++·算法·职场和发展·蓝桥杯·深度优先
运维@小兵3 小时前
vue配置子路由,实现点击左侧菜单,内容区域显示不同的内容
前端·javascript·vue.js