差速机器人的纯轨迹跟踪仿真（Matlab） |
您所在的位置：网站首页 › 左右轮的位置 › 差速机器人的纯轨迹跟踪仿真（Matlab） |
差速机器人的纯轨迹跟踪仿真（Matlab）
|
差速机器人的纯轨迹跟踪仿真（Matlab）
刚入门，有的地方不对，烦请大家指正。 目录 差速机器人的纯轨迹跟踪仿真（Matlab）1 差速机器人运动模型1.1 运动学分析建模1.2 差速机器人的运动状态方程 2 控制器设计：（pure pursuit）纯轨迹算法3程序源码（Matlab）3.1 分为几个步骤3.2 代码3.3 效果 1 差速机器人运动模型 1.1 运动学分析建模运动特性为两轮差速驱动，其底部后方两个同构驱动轮的转动为其提供动力，前方的随动轮起支撑作用并不推动其运动，如图1两轮差速驱动示意图所示。
在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为： Pure pursuit方法的依据是使下图所示的单车模型以适当的轮转向δ运动，并恰好使机器人轮轴中心经过当前的路点。这样一来，我们就可以根据当前的路点以及单车几何模型计算去当前的期望的轮转向角δ。 我们有以下几何关系： 1、经纬度轨迹数据的导入与筛除 2、数据转化到笛卡尔坐标系和轨迹的平移及变换 3、轨迹跟踪算法 4、处理跟踪轨迹数据 3.2 代码 %筛除错误数据% new_GPCHC = []; a = 1; for i = 1:3431 if abs(m.GPCHC(i,13)-noproave_lon) time)&&(N~=qwe)) N = length(x_process); target_index = calc_target_current_distance(x,y,cx,cy); %计算控制量% ai = PIDcontrol(target_speed, v,Kp); di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Lf); [x,y,yaw,v] = updatestate(x,y,yaw,v, ai, di,dt,L) time = time + dt; x_process = [x_process;x]; y_process = [y_process;y]; plot(cx,cy,'b',x,y,'r*') title('跟踪过程') drawnow hold on end f2 = figure(2) plot(x_goal,y_goal,'b',x_process,y_process,'r') title('跟踪路径') legend('预定轨迹','实际跟踪路径'); %筛选出距离最近的点% newx = zeros(qwe+1,1); newy = zeros(qwe+1,1); locat = 0; for p = 1:(qwe+1) Dis = zeros(3429,1); for q = 1:3429 Dis(q,1) = (x_goal(q,1)-x_process(p,1))^2 + (y_goal(q,1)-y_process(p,1))^2; end [~, locat]= min(Dis); newx(p,1) = x_goal(locat,1); newy(p,1) = y_goal(locat,1); end %处理数据，计算均方差% sum = 0; for i = 1:(qwe+1) sum = sum + sqrt((newx(i,1)-x_process(i,1))^2+(newy(i,1)*10-y_process(i,1)*10)^2); end ave = sum/(qwe+1); fprintf('轨迹跟踪的均方差为%f\n',ave); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%函数定义%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [x, y, yaw, v] = updatestate(x, y, yaw, v, a, delta,dt,L) %更新当前差速机器人状态% x = x + v * cos(yaw) * dt; y = y + v * sin(yaw) * dt; yaw = yaw + v / L * tan(delta) * dt; v = v + a * dt; end function [a] = PIDcontrol(target_v, current_v, Kp) a = Kp * (target_v - current_v); end function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,index,k,Lfc,L,Lf) tx = cx(index); ty = cy(index); alpha = atan2((ty-y),(tx-x))-yaw; Lf = k * v + Lfc; delta = atan(2*L * sin(alpha)/Lf) ; end function [index] = calc_target_current_distance(x,y, cx,cy) N = length(cx); %计算目标横轴点的所有点数% Distance = zeros(N,1); %初始化距离矩阵% for i = 1:N Distance(i) = sqrt((cx(i)-x)^2 + (cy(i)-y)^2); %计算出每个目标点到当前点的距离% end [~, location]= min(Distance); index = location; index = index+1 ; end 3.3 效果
谢谢！ |
定义其左右驱动轮的中心分别为ω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可以表示为： 
【本文地址】
今日新闻 |
推荐新闻 |