robots formation

clc clear all vr=0.25; wr=pi/180; r=vr/wr; %%初始化参数 x00=[];y00=[];theta00=[];v00=[];w00=[]; x00(1)=1;y00(1)=1;theta00(1)=0;v00(1)=vr;w00(1)=wr; %% %%初始化跟随机器人1的速度位姿 %跟随机器人1的理想 y01=[];x01=[];theta01=[]; v01=[];w01=[];beta01=[]; %跟随机器人1的当前 x1=[];y1=[];theta1=[]; v1=[];w1=[];beta1=[]; %跟随机器人1的初始位置 x1(1)=1;y1(1)=0;theta1(1)=0; v1(1)=1.0;w1(1)=0.6;beta1(1)=0; %初始化follower2的速度位姿 %跟随机器人2的理想 y02=[];x02=[];theta02=[]; v02=[];w02=[];beta02=[]; %跟随机器人2的当前 x2=[];y2=[];theta2=[]; v2=[];w2=[];beta2=[]; %跟随机器人2的初始位置 x2(1)=0;y2(1)=2;theta2(1)=0; v2(1)=1.0;w2(1)=0.6;beta2(1)=0; %初始化follower3的速度位姿 %跟随机器人3的理想 y03=[];x03=[];theta03=[]; v03=[];w03=[];beta03=[]; %跟随机器人3的当前 x3=[];y3=[];theta3=[]; v3=[];w3=[];beta3=[]; %跟随机器人2的初始位置 x3(1)=0;y3(1)=3;theta3(1)=0; v3(1)=1.0;w3(1)=0.6;beta3(1)=0; %% %初始化队形参数即增益系数和理想距离和角度 c11=10;c21=20;c31=30;%编队长度 ts=0.1; K11=1;K12=0.6;K13=0.5;%增益系数 t=0:0.1:1000% n=length(t); %% for i=1:2500 theta00(1)=pi/6; theta00(i+1)=pi/6+wr*0.1*i; %领航者初始化及现在 x00(i+1)=r*cos(wr*0.1*i); y00(i+1)=r*sin(wr*0.1*i); %跟随者1初始化 theta01(1)=1*pi/6; beta01(1)=pi/6; y01(1)=y00(1)+c11*sin(beta01(1)+theta01(1)); x01(1)=x00(1)+c11*cos(beta01(1)+theta01(1)); v01(1)=vr; w01(1)=wr; %跟随者1理想位置数据 beta01(i+1)=1*pi/6; theta01(i+1)=theta00(i+1); y01(i+1)=y00(i)+c11*sin(beta01(i)+theta01(i)); x01(i+1) =x00(i)+c11*cos(beta01(i)+theta01(i)); v01(i+1)=vr; w01(i+1)=wr; %跟随者2初始化 beta02(1)=pi/6; theta02(1)=pi/6; y02(1)= y00(1)+c21*sin(beta02(1)+theta02(1)); x02(1) =x00(1)+c21*cos(beta02(1)+theta02(1)); v02(1)=vr; w02(1)=wr; %跟随者2理想位置数据 beta02(i+1)=pi/6; theta02(i+1)=theta00(i+1); y02(i+1)= y00(i)+c21*sin(beta02(i)+theta02(i)); x02(i+1) =x00(i)+c21*cos(beta02(i)+theta02(i)); v02(i+1)=vr; w02(i+1)=wr; % %跟随者3初始化 beta03(1)=pi/6; theta03(1)=pi/6; y03(1)= y00(1)+c31*sin(beta03(1)+theta03(1)); x03(1) =x00(1)+c31*cos(beta03(1)+theta03(1)); v03(1)=vr; w03(1)=wr; %跟随者3理想位置数据 beta03(i+1)=pi/6; theta03(i+1)=theta00(i+1); y03(i+1)= y00(i)+c31*sin(beta03(i)+theta03(i)); x03(i+1) =x00(i)+c31*cos(beta03(i)+theta03(i)); v03(i+1)=vr; w03(i+1)=wr; %% %----------------------------计算follower1的相对位姿 e1(1)=cos(theta1(i))*(x01(i)-x1(i))+sin(theta1(i))*(y01(i)-y1(i)); e1(2)=-sin(theta1(i))*(x01(i)-x1(i))+cos(theta1(i))*(y01(i)-y1(i)); e1(3)=theta01(i) -theta1(i); xe1(i)=e1(1); ye1(i)=e1(2); thetae1(i)=e1(3); %% %-----------------------------------运动学控制率，输入控制follower1车的速度与角速度设计 v01(i)=vr; w01(i)=wr; v1(i)=v01(i)*cos(thetae1(i))+K11*xe1(i); w1(i)=w01(i)+K12*v01(i)*ye1(i)+K13*v01(i)*sin(thetae1(i)); %-------------------------------------- %计算follower1当前的位姿------------------------- %%%%%% theta1(i+1)=theta1(i)+ts*w1(i); x1(i+1)=x1(i)+ts*v1(i)*cos( theta1(i)+w1(i)*ts); y1(i+1)=y1(i)+ts*v1(i)*sin( theta1(i)+w1(i)*ts); %---------------------------------------------计算follower2的相对位姿--------------- e2(1) =cos(theta2(i))*(x02(i)-x2(i))+sin(theta2(i))*( y02(i)-y2(i)); e2(2)=-sin(theta2(i))*(x02(i)-x2(i))+cos(theta2(i))*(y02(i)-y2(i)); e2(3)= theta02(i) -theta2(i); xe2(i)=e2(1); ye2(i)=e2(2); thetae2(i)=e2(3); %-------------------------运动学控制率，输入控制follower2车的速度与角速度设计------ v02(i)=vr; w02(i)=wr; v2(i)=v02(i)*cos(thetae2(i))+K11*xe2(i); w2(i)=w02(i)+K12*v02(i)*ye2(i)+K13*v02(i)*sin(thetae2(i)); %------------------------------------------计算follower2当前的位姿------------------------------------------------------ theta2(i+1)=theta2(i)+ts*w2(i); x2(i+1)=x2(i)+ts*v2(i)*cos( theta2(i)+w2(i)*ts); y2(i+1)=y2(i)+ts*v2(i)*sin( theta2(i)+w2(i)*ts); %---------------------------------------------计算follower3的相对位姿--------------- e3(1) =cos(theta3(i))*(x03(i)-x3(i))+sin(theta3(i))*( y03(i)-y3(i)); e3(2)=-sin(theta3(i))*(x03(i)-x3(i))+cos(theta3(i))*(y03(i)-y3(i)); e3(3)= theta03(i) -theta3(i); xe3(i)=e3(1); ye3(i)=e3(2); thetae3(i)=e3(3); %-------------------------运动学控制率，输入控制follower3车的速度与角速度设计------ v03(i)=vr; w03(i)=wr; v3(i)=v03(i)*cos(thetae3(i))+K11*xe3(i); w3(i)=w03(i)+K12*v03(i)*ye3(i)+K13*v03(i)*sin(thetae3(i)); %------------------------------------------计算follower3当前的位姿------------------------------------------------------ theta3(i+1)=theta3(i)+ts*w3(i); x3(i+1)=x3(i)+ts*v3(i)*cos( theta3(i)+ts*w3(i)); y3(i+1)=y3(i)+ts*v3(i)*sin( theta3(i)+ts*w3(i)); end for i=2500:n vr=1; wr=pi/180; r=vr/wr; theta00(1)=pi/6; theta00(i+1)=pi/6+wr*0.1*i; %领航者初始化及现在 x00(i+1)=r*cos(wr*0.1*i); y00(i+1)=r*sin(wr*0.1*i); %跟随者1初始化 theta01(1)=1*pi/6; beta01(1)=pi/6; y01(1)=y00(1)+c11*sin(beta01(1)+theta01(1)); x01(1)=x00(1)+c11*cos(beta01(1)+theta01(1)); v01(1)=vr; w01(1)=wr; %跟随者1理想位置数据 beta01(i+1)=1*pi/6; theta01(i+1)=theta00(i+1); y01(i+1)=y00(i)+c11*sin(beta01(i)+theta01(i)); x01(i+1) =x00(i)+c11*cos(beta01(i)+theta01(i)); v01(i+1)=vr; w01(i+1)=wr; %跟随者2初始化 beta02(1)=pi/6; theta02(1)=pi/6; y02(1)= y00(1)+c21*sin(beta02(1)+theta02(1)); x02(1) =x00(1)+c21*cos(beta02(1)+theta02(1)); v02(1)=vr; w02(1)=wr; %跟随者2理想位置数据 beta02(i+1)=pi/6; theta02(i+1)=theta00(i+1); y02(i+1)= y00(i)+c21*sin(beta02(i)+theta02(i)); x02(i+1) =x00(i)+c21*cos(beta02(i)+theta02(i)); v02(i+1)=vr; w02(i+1)=wr; % %跟随者3初始化 beta03(1)=pi/6; theta03(1)=pi/6; y03(1)= y00(1)+c31*sin(beta03(1)+theta03(1)); x03(1) =x00(1)+c31*cos(beta03(1)+theta03(1)); v03(1)=vr; w03(1)=wr; %跟随者3理想位置数据 beta03(i+1)=pi/6; theta03(i+1)=theta00(i+1); y03(i+1)= y00(i)+c31*sin(beta03(i)+theta03(i)); x03(i+1) =x00(i)+c31*cos(beta03(i)+theta03(i)); v03(i+1)=vr; w03(i+1)=wr; %% %----------------------------计算follower1的相对位姿 e1(1)=cos(theta1(i))*(x01(i)-x1(i))+sin(theta1(i))*(y01(i)-y1(i)); e1(2)=-sin(theta1(i))*(x01(i)-x1(i))+cos(theta1(i))*(y01(i)-y1(i)); e1(3)=theta01(i) -theta1(i); xe1(i)=e1(1); ye1(i)=e1(2); thetae1(i)=e1(3); %% %-----------------------------------运动学控制率，输入控制follower1车的速度与角速度设计 v01(i)=vr; w01(i)=wr; v1(i)=v01(i)*cos(thetae1(i))+K11*xe1(i); w1(i)=w01(i)+K12*v01(i)*ye1(i)+K13*v01(i)*sin(thetae1(i)); %-------------------------------------- %计算follower1当前的位姿------------------------- %%%%%% theta1(i+1)=theta1(i)+ts*w1(i); x1(i+1)=x1(i)+ts*v1(i)*cos( theta1(i)+w1(i)*ts); y1(i+1)=y1(i)+ts*v1(i)*sin( theta1(i)+w1(i)*ts); %---------------------------------------------计算follower2的相对位姿--------------- e2(1) =cos(theta2(i))*(x02(i)-x2(i))+sin(theta2(i))*( y02(i)-y2(i)); e2(2)=-sin(theta2(i))*(x02(i)-x2(i))+cos(theta2(i))*(y02(i)-y2(i)); e2(3)= theta02(i) -theta2(i); xe2(i)=e2(1); ye2(i)=e2(2); thetae2(i)=e2(3); %-------------------------运动学控制率，输入控制follower2车的速度与角速度设计------ v02(i)=vr; w02(i)=wr; v2(i)=v02(i)*cos(thetae2(i))+K11*xe2(i);