张老五 2013-06-14 02:40 采纳率: 0%
浏览 4643

有关于移动机器人的视觉导航的MATLAB程序

有关于移动机器人的视觉导航的MATLAB程序,要求是导航不是领航,并能生成二维曲线图,进行动画仿真
我这儿有一个领航的程序,求高手改成导航的程序:(着急用不胜感激)
clear;
clf;
clc;
maxT=100; %模拟时间
t=0:maxT; %虚拟时间 单位:秒
UL=0.2; %领航机器人速度 单位:m/s
Um=0.5; %跟随机器人 最大输出速度 单位:m/s
XL=UL.*t; %初始化 xl
YL=0.*XL; %初始化 yl
YF=0.*t; XF=0.*t; %初始化 xf and yf
XF(1)=-4; YF(1)=2; %设定 xf and yf Xo (x,y)
dp=0.5; %设定 保持距离 单位:m
et=0.*t; %初始化保持距离 单位:m
ect=0.*t; %初始化速度 单位:m
dt=0.*t; %初始化机器人间距 单位:m
Rotatc=0.*t;
dt(1)=sqrt((abs(XF(1)-XL(1)))^2+(abs(YF(1)))^2);
et(1)=dt(1)-dp;
ect(1)=0;
U=0.*t; %初始化输出速度 单位:m/s
R=0.*t; %初始化角速度 单位:rad/s
R(1)=pi/2; %跟随机器人 初始朝向角
Rm=0.1; %跟随机器人最大角速度
agtanX=0.*t;

agtanY=0.*t;
Angletan=0.*t;
Rotat=0.*t;
%agtanX(1)=5*cos(R(1))+XF(1);
%agtanY(1)=5*sin(R(1))+YF(1);

%subplot(2,2,1),plot([XF(1),agtanX(1)],[YF(1),agtanY(1)],'-g');

kp=0.4;
ki=0.6;
krp=0.2;
kri=0.7;
Ut=0;
Pl=4;

for n=2:maxT

dt(n)=sqrt((XF(n-1)-XL(n))^2+(YF(n-1)-YL(n))^2); %机器人间距
et(n)=dt(n)-dp; %保持距离
ect(n)=(et(n)-et(n-1)); %分离速度
U(n)=kp*ect(n)+ki*et(n); %跟随机器人输出速度

if (U(n)>Um)                    %为了不使机器人过速
    U(n)=Um; 
end;

Angletan(n)=atan2((YL(n)-YF(n-1)),(XL(n)-XF(n-1)));     %跟随机器人期望达到的朝向
Rotat(n)=Angletan(n)-R(n-1);                %跟随机器人当前角度和期望朝向间的夹角    

if (abs(Rotat(n))>pi)                       %转化为(-pi,pi)
    Rotat(n)=(2*pi-abs(Rotat(n)))*(-abs(Rotat(n))/Rotat(n));
end

Rotatc(n)=kri*(Rotat(n)-Rotat(n-1))+krp*Rotat(n-1);     %角速度

if (abs(Rotatc(n))>=Rm)                     %限制角速度
    Rotatc(n)=(Rotatc(n)/abs(Rotatc(n)))*Rm;       
end  

R(n)=R(n-1)+Rotatc(n);                      %跟随机器人朝向
%Ut=U(n)/(abs(Rotatc(n)))*(sqrt(2-(cos(Rotat(n)))));%跟随机器人移动的距离
XF(n)=XF(n-1)+U(n)*cos(R(n-1)+Rotatc(n)/2);               %移动
YF(n)=YF(n-1)+U(n)*sin(R(n-1)+Rotatc(n)/2);

pause(0.01);shg;                            %绘制跟随轨迹
subplot(2,2,1),plot(XL(n),YL(n),'dr');hold on;

agtanX(n)=Pl*cos(R(n))+XF(n);                %绘制指向标记
agtanY(n)=Pl*sin(R(n))+YF(n);      
subplot(2,2,1),plot(XF(n),YF(n),'ob','Markersize',12);axis([-10,30,-20,20]);hold on;    
subplot(2,2,1),plot([XF(n),agtanX(n)],[YF(n),agtanY(n)],'-g');

end

%main pic
%subplot(2,2,1),plot(XL,YL,'dr');
%hold on;
agtanX(1)=Pl*cos(R(1))+XF(1); %绘制指向标记
agtanY(1)=Pl*sin(R(1))+YF(1);

subplot(2,2,1),plot(XF(1),YF(1),'ob','Markersize',12);%axis([0,30,-20,20]);
subplot(2,2,1),plot([XF(1),agtanX(1)],[YF(1),agtanY(1)],'-g');
title('\fontsize{12}\bf 路线图')
xlabel('\fontsize{12}\bfx\rightarrow');
ylabel('\fontsize{12}\bfy\rightarrow');
%grid on;
%output U speed
subplot(2,2,2),plot(t,U,'-b','LineWidth',2);%axis([0,60,-2,2]);
title('\fontsize{12}\bf 输出速度')
xlabel('\fontsize{12}\bft\rightarrow');
ylabel('\fontsize{12}\bfy\rightarrow');
%distance
subplot(2,2,3),plot(t,dt,'-b','LineWidth',2);%axis([0,30,0,20]);
title('\fontsize{12}\bf 间距')
xlabel('\fontsize{12}\bft\rightarrow');
ylabel('\fontsize{12}\bfy\rightarrow');
%distance Rotatc
subplot(2,2,4),plot(t,Rotatc,'-b','LineWidth',2);
title('\fontsize{12}\bf 角速度')
xlabel('\fontsize{12}\bft\rightarrow');
ylabel('\fontsize{12}\bfy\rightarrow');grid on;
shg;

  • 写回答

2条回答

报告相同问题?

悬赏问题

  • ¥15 求差集那个函数有问题,有无佬可以解决
  • ¥15 【提问】基于Invest的水源涵养
  • ¥20 微信网友居然可以通过vx号找到我绑的手机号
  • ¥15 寻一个支付宝扫码远程授权登录的软件助手app
  • ¥15 解riccati方程组
  • ¥15 display:none;样式在嵌套结构中的已设置了display样式的元素上不起作用?
  • ¥15 使用rabbitMQ 消息队列作为url源进行多线程爬取时,总有几个url没有处理的问题。
  • ¥15 Ubuntu在安装序列比对软件STAR时出现报错如何解决
  • ¥50 树莓派安卓APK系统签名
  • ¥65 汇编语言除法溢出问题