单车心形跟随仿真

本教程将会引导读者完成单车心形跟随仿真的演示。在使用此功能之前，确保已经完成了仿真系统搭建

本教程作为单车航点跟随的进阶版本(因为算法原理是一样的)。将会引导读者如何自定义航路点，让小车按照给定的航路点进行运动。

打开matlabR20021b，在命令行中输入 rosinit，启动ROS Master节点

rosinit

此时将会弹出ROS master 的窗口，不要关闭。最小化即可。如图1所示：

图1 rosinit

提示

在某些电脑环境中，即使按照仿真系统搭建步骤完成搭建，在运行 rosinit 时，可能会出现 Cannot connect to ROS master at http://localhost:11311. Check the specified address or hostname 字样。

解决的办法是，将 rosinit 命令改为， rosinit("ip",端口)。如下图所示：

解决办法

打开 kk-robot-swarm/src/matlab/pid_heart/b1_PP_s_heart.slx 文件，如图2所示。

图2 b1_PP_s_heart

点击菜单栏上的 ROS -- Deplay to Localhost。如图3所示

图3 b1_PP_s_heart ROS

接着点击菜单栏上的 仿真 -- 运行。启动仿真

图4 b1_PP_s_heart ROS

等待编译完成，系统将会运行仿真。如图4所示：

图5 仿真结果

小车将会先走一个正方形的航点，然后开始走心形航点。最后会在心形的凹槽部分画圆。

自定义航点

在这个Demo中。航点是由 plot_a_heart 模块生成的，如下图所示。双击该模块进入：

plot_a_heart

代码如下：

function z = plot_a_heart()
%% waypoints 应为 列数 等于 2 的数组。
x0=1;
y0=1;

dt=1;
t=0:dt:4*2*pi;

N=length(t);
x=zeros(1,N);
y=zeros(1,N);

for ii = 1:N
    x(ii) =x0+  1.8/32 *( 16*sin(t(ii)/4)^3);
    y(ii) =y0+  1.8/32 *(  13*cos(t(ii)/4)-5*cos(t(ii)/2) -2*cos(3*t(ii)/4)-cos(t(ii))  );
  
end

heart=[x,x,x;y,y,y]';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 四边形
A=0.3;
C=1.7;
B=(A+C)/2;

wp1=[A,A; B,A;  C,A; C,B;C,C; B,C; A,C;A,B];   
WAY_P=[wp1;wp1;heart];
z=WAY_P;

% plot(x,y)

提示

waypoints 应为列数等于 2 的数组

将想要自定义路径的代码替换即可。

8字形

%期望8字轨迹
sampleTime=0.01;
st=sampleTime;
tNum=1:1:20*pi;
T=60;
Ax=0.8;Ay=0.8;f=2*pi/T;%Yawd0=atan2(f*Ay,2*f*Ax);
Xd0=1;Yd0=1;%初始位置

Xd=Ax*sin(2*f*(tNum))+Xd0;
Yd=Ay*sin(f*(tNum))+Yd0;    
 
wp=[Xd,Yd];
plot(Xd ,Yd,'*', 'Linewidth',5)

六边形

theta = linspace(0,2*pi,7);% linspace(x1,x2,N)用于产生x1,x2之间的N点行矢量。其中x1、x2、N分别为起始值、中止值、元素个数
x= 1+cos(theta);
y= 1+sin(theta);
wp=[x',y'];
plot(x,y,'*', 'Linewidth',5);