关于二轮差速小车轮速计算和里程计计算

简介: 关于二轮差速小车轮速计算和里程计计算

里程计是衡量我们从初始位姿到终点位姿的一个标准,通俗的说,我们要实现机器人的定位与导航,就需要知道机器人行进了多少距离,是往哪个方向行进的 里程计的计算是指以机器人上电时刻为世界坐标系的起点(机器人的航向角是世界坐标系X正方向)开始累积计算任意时刻机器人在世界坐标系下的位姿。


通常计算里程计方法是速度积分推算:通过左右电机的编码器测得机器人的左右轮的速度VL和VR,在一个短的时刻△t内,认为机器人是匀速运动,并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量,然后将增量进行累加处理,关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计。


1 逆解

通过设定的v和角度,计算向左右轮发的速度 ROBOT_RADIUS指的是机器人半径,也就是机器人两轮间距的一半

// 计算左右轮期望速度
    if(RobotV == 0)
    {
        leftdata.d = -YawRate * ROBOT_RADIUS;
        rightdata.d = YawRate * ROBOT_RADIUS;
    } 
    else if(YawRate == 0)
    {
        leftdata.d = RobotV;
        rightdata.d = RobotV;
    }
    else
    {
        leftdata.d  = YawRate * (r - ROBOT_RADIUS);
        rightdata.d = YawRate * (r + ROBOT_RADIUS);
    } `


2 里程计正解

根据左右轮速和角度来计算里程计

``  //===========================速度计算和Angle获取

 // x方向速度,以及角速度

vx       = (rightVelNow.d + leftVelNow.d) / 2.0 / 1000.0;        //m/s
vth      = (rightVelNow.d - leftVelNow.d) / ROBOT_LENGTH ;       //rad/s
th       = angleNow.d*0.01745;//实时角度信息(rad)
double dt = (curr_time - last_time_).toSec(); //间隔时间
double delta_x = (vx_ * cos(th_)) * dt;         //th_弧度
double delta_y = (vx_ * sin(th_)) * dt;
double delta_th = vth_ * dt;`
//打印时间间隔调试信息,不用的时候可以关闭
//ROS_INFO("dt:%f\n",dt);                       //s
     //里程计累加
        x_ += delta_x;
        y_ += delta_y;
        //实时角度信息,如果这里不使用IMU,也可以通过这种方式计算得出
        th_ += delta_th;



//里程计解算积分后角度`


相关文章
配电网三相不平衡潮流计算【隐式Zbus高斯法】【可设定变压器数量、位置、绕组方式】
配电网三相不平衡潮流计算【隐式Zbus高斯法】【可设定变压器数量、位置、绕组方式】
|
6月前
|
数据安全/隐私保护
matlab批量计算地震加速度反应谱、速度谱、位移谱,伪速度谱、伪加速度谱;反应谱对比图
地震波格式转换、时程转换、峰值调整、规范反应谱、计算反应谱、计算持时、生成人工波、时频域转换、数据滤波、基线校正、Arias截波、傅里叶变换、耐震时程曲线、脉冲波合成与提取、三联反应谱、地震动参数、延性反应谱、地震波缩尺、功率谱密度
|
6月前
|
数据安全/隐私保护 Perl
批量计算地震波PGA/PGV/PGD、PSA/PSV/PSD、特征周期、卓越频率、Arias强度、特征强度、能量密度、Housner强度等30+参数
地震波格式转换、时程转换、峰值调整、规范反应谱、计算反应谱、计算持时、生成人工波、时频域转换、数据滤波、基线校正、Arias截波、傅里叶变换、耐震时程曲线、脉冲波合成与提取、三联反应谱、地震动参数、延性反应谱、地震波缩尺、功率谱密度
|
机器学习/深度学习 传感器 算法
穿墙雷达中基于快速墙体时延估计的点目标后投影算法附matlab代码
穿墙雷达中基于快速墙体时延估计的点目标后投影算法附matlab代码
|
机器学习/深度学习 传感器 安全
【刚度计算】基于石川公式法齿轮时变啮合刚度计算附matlab代码
【刚度计算】基于石川公式法齿轮时变啮合刚度计算附matlab代码
|
传感器 数据采集 算法
【使用 DSP 滤波器加速速度和位移】使用信号处理算法过滤加速度数据并将其转换为速度和位移研究(Matlab代码实现)
【使用 DSP 滤波器加速速度和位移】使用信号处理算法过滤加速度数据并将其转换为速度和位移研究(Matlab代码实现)
162 0
|
机器学习/深度学习 传感器 算法
毫米波系统中混合预编码的交替最小化附matlab代码
毫米波系统中混合预编码的交替最小化附matlab代码
|
传感器 算法
【改进的多同步挤压变换】基于改进多同步挤压的高分辨率时频分析工具,用于分析非平稳信号(Matlab代码实现)
【改进的多同步挤压变换】基于改进多同步挤压的高分辨率时频分析工具,用于分析非平稳信号(Matlab代码实现)
155 0
|
Python
【能量算子】评估 EEG 中的瞬时能量:非负、频率加权能量算子(Python&Matlab代码实现)
【能量算子】评估 EEG 中的瞬时能量:非负、频率加权能量算子(Python&Matlab代码实现)
197 0
|
存储 算法 数据中心
基于蓄电池进行调峰和频率调节研究【超线性增益的联合优化】(Matlab代码实
基于蓄电池进行调峰和频率调节研究【超线性增益的联合优化】(Matlab代码实