差速机器人的纯轨迹跟踪仿真（Matlab）

最新推荐文章于 2025-04-27 10:55:28 发布

だが，断る

最新推荐文章于 2025-04-27 10:55:28 发布

阅读量1w

点赞数 16

分类专栏： MATLAB

本文链接：https://blog.csdn.net/weixin_49578216/article/details/116516755

版权

差速机器人的纯轨迹跟踪仿真（Matlab）

刚入门，有的地方不对，烦请大家指正。

1 差速机器人运动模型

1.1 运动学分析建模

运动特性为两轮差速驱动，其底部后方两个同构驱动轮的转动为其提供动力，前方的随动轮起支撑作用并不推动其运动，如图1两轮差速驱动示意图所示。

在这里插入图片描述
定义其左右驱动轮的中心分别为ωl和ωr，且车体坐标系中这两点在惯性坐标系下移动的线速度为vl和vr，理想情况下即为左右轮转动时做圆周运动的线速度。该值可以通过电机驱动接口输出的角转速Φl、Φr和驱动轮半径r求得，即：
在这里插入图片描述
令两驱动轮中心连线的中点为机器的基点C，C点在大地坐标系XOY下的坐标为（x,y），机器的瞬时线速度为vc，瞬时角速度为ωc，姿态角θ即为vc与X轴夹角。此时，机器位姿信息可用适量P = [x,y,θ]T表示。机器人瞬时线速度为vc可以表示为：
在这里插入图片描述
令左右轮间距为l，且机器瞬时旋转中心为Oc，转动半径即为C到Oc的距离R。机器在做同轴圆周运动时，左右轮及基点所处位置在该圆周运动中的角速度相同ωl=ωr=ωc，到旋转中心的半径不同，有：

则机器的瞬时角速度ωc可以表示为：
在这里插入图片描述

1.2 差速机器人的运动状态方程

在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：
在这里插入图片描述

2 控制器设计：（pure pursuit）纯轨迹算法

Pure pursuit方法的依据是使下图所示的单车模型以适当的轮转向δ运动，并恰好使机器人轮轴中心经过当前的路点。这样一来，我们就可以根据当前的路点以及单车几何模型计算去当前的期望的轮转向角δ。
我们有以下几何关系：
在这里插入图片描述

3程序源码（Matlab）

3.1 分为几个步骤

1、经纬度轨迹数据的导入与筛除
2、数据转化到笛卡尔坐标系和轨迹的平移及变换
3、轨迹跟踪算法
4、处理跟踪轨迹数据

3.2 代码

%筛除错误数据%
new_GPCHC = [];
a = 1;
for i = 1:3431

    if abs(m.GPCHC(i,13)-noproave_lon)<0.04
       new_GPCHC(a,1) = m.GPCHC(i,13);
       new_GPCHC(a,2) = m.GPCHC(i,14);
    a = a+1;
    end
    
end
position = new_GPCHC(:,1:2);
%筛除错误数据结束%

%MATLAB程序实现经纬度转换成笛卡尔坐标%：
M_PI=3.14159265358979323846;
L = 6381372 * M_PI * 2;     %地球周长  
W = L;                      % 平面展开后，x轴等于周长  
H = L / 2;                  % y轴约等于周长一半  
mill = 2.3;                 % 米勒投影中的一个常数，范围大约在正负2.3之间  

n=size(position,1);
new_position=[];
for i =1:n
    lon=position(i,1);
    lat=position(i,2);
    x = lon * M_PI / 180; % 将经度从度数转换为弧度  
    y = lat * M_PI / 180; %将纬度从度数转换为弧度  
    y1 = 1.25 * log(tan(0.25 * M_PI + 0.4 * y)); % 米勒投影的转换  
    % 弧度转为实际距离  
    dikaerX = (W / 2) +