l_f 领航跟随法的实现

领航跟随法的实现，可以用于多机器人的编队控制。 )

%%%%%%%%%%%%%%%%%%%%%%Leader-follower%%%%%%%%%%%%%%%%
%%%%%%%%%%%%两个机器人保持固定角度与固定距离，一段，


clc;
close all;
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%给定初始条件%%%%%%%%%%%%%%%%
%%%%%主机器人1%%%%%%%
X1（1）=100;Y1（1）=0;C1（1）=0;V1=50;W1=0;
%%%%%从机器人2%%%%%%%
X2（1）=0;Y2（1）=200;C2（1）=0;

%%%%%其他初始条件%%%%%%
k1=0.1;k2=0.1;d=13;k=0;T=0.1;
l121=100;F021=pi*0.75;%%%%最终状态


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L12（1）=sqrt（（X1（1）-X2（1））^2+（Y1（1）-Y2（1））^2）;
V2（1）=0;


for k=1:200;
  L12（k）=sqrt（（X1（k）-X2（k））^2+（Y1（k）-Y2（k））^2）;

  FF2（k）=pi-atan（abs（（Y1（k）-Y2（k））/（X2（k）-X1（k））））;
  R12（k）=C1（k）+FF2（k）-C2（k）;
  F12（k）=C1（k）-C2（k）;
  S12（k）=k1*（l121-L12（k））;
  B12（k）=k2*（F021-FF2（k））;
  
 
  
  V2（k）=S12（k）*cos（R12（k））-L12（k）*sin（R12（k））*（W1+B12（k

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1878  2013-05-08 15:31  l_f.m

----------- ---------  ---------- -----  ----

                 1878                    1