文章目录
- 前言
- 一、Function 'quiver' not supported for code generation.
- 二、在仿真环境中添加障碍物
- 三、simulink中function函数初始化
- 五、在MATLAB中，实现在绘图时只删除上一次绘制的图形而不是整个图形界面
- 六、matlab simulink中，程序不断发出机器人位置信息，创建一个模块收集机器人从开始到结束的所有位姿
- 总结
前言
近期实验室购置了阿木实验室KKSwarm集群仿真平台，其整体开放环境为MATLAB，近期基于该平台做了一些集群编队的工作，期间遇到了一些小问题，记录下遇到的问题和解决方法。
一、Function ‘quiver’ not supported for code generation.
解决方法：添加coder.extrinsic('quiver')，其他的同理~
二、在仿真环境中添加障碍物
解决方法：添加coder.extrinsic('quiver')，其他的同理~
三、simulink中function函数初始化
matlab中搭建的simulink，每次有参数传入后都会初始化函数的定义，但只想在参数第一次传入时进行初始化，后面调用时不再初始化。
persistent k
persistent pose_x
persistent pose_y
persistent pose_th
persistent h_circle
persistent h_quiver
persistent h1_arrows
persistent h2_arrows
if isempty(k)
k = 0;
end
if isempty(pose_x)
pose_x = zeros(6,5000); % 初始化x坐标的空矩阵
end
if isempty(pose_y)
pose_y = zeros(6,5000); % 初始化y坐标的空矩阵
end
if isempty(pose_th)
pose_th = zeros(6,5000); % 初始化th坐标的空矩阵
end
if isempty(h_circle)
h_circle = zeros(6,1); % 初始化th坐标的空矩阵
end
if isempty(h_quiver)
h_quiver = zeros(6,1); % 初始化th坐标的空矩阵
end
if isempty(h1_arrows)
h1_arrows = zeros(6,6); % 初始化th坐标的空矩阵
end
if isempty(h2_arrows)
h2_arrows = zeros(6,6); % 初始化th坐标的空矩阵
end
五、在MATLAB中，实现在绘图时只删除上一次绘制的图形而不是整个图形界面
目前已知各个机器人的位置，仿真时把个机器人之间通过线段连接起来，并且每当有新的位置信息传入时，只删除之前的连线。但保留机器人的轨迹。
%% ====Animation====
area = compute_area(pose_x(1,k),pose_y(1,k),0.5);
%hold off;
ArrowLength=0.1;
for j=1:N
h_quiver(j) = quiver(pose_x(j,k),pose_y(j,k),ArrowLength*cos(pose_th(j,k)),ArrowLength*sin(pose_th(j,k)),'*k');hold on;
if j==1
state=2;
else
state=1;
end
h_circle(j) = draw_circle (pose_x(j,k),pose_y(j,k),0.06,state);hold on;
end
for i=1:N
for j=1:N
if A(i,j)==1
[lineHandle, arrowHandle] = draw_arrow([pose_x(j,k),pose_y(j,k)],[pose_x(i,k),pose_y(i,k)], 0.05);hold on;
h1_arrows(i, j) = lineHandle;
h2_arrows(i, j) = arrowHandle;
end
end
end
if size(ob_temp)~=[0 0]
plot(ob_temp(:,1),ob_temp(:,2),'Xk','LineWidth',2);hold on;
end
axis(area);
grid on;
drawnow;
for j=1:N
delete(h_quiver(j));
end
for j=1:N
delete(h_circle(j));
end
for i=1:N
for j=1:N
if A(i,j)==1
delete(h1_arrows(i, j)); % 删除线条
delete(h2_arrows(i, j)); % 删除箭头头部
end
end
end
color='mgbkrc';
for i=1:N
plot(pose_x(i,5:k),pose_y(i,5:k),color(1,i),'LineWidth',2);
hold on
end
需要注意的是，想要单独删除某个绘制的图形时，需要创建句柄表示绘制出的图形，通过delete进行删除。但如果调用.m文件进行画图时，确保调用的.m文件有对应的返回值，否则句柄无法收到消息，进而无法通过delete删除。
举例：
//单独删除箭头draw_arrow图形，包含两部分箭头和线段，要添加两个返回值a,b
function [a,b] = draw_arrow(startpoint,endpoint,headsize)
% draw the communication direction between two agents
% accepts two [x y] coords and one double headsize
v1 = headsize*(startpoint-endpoint)/sqrt((startpoint(1)-endpoint(1))^2+(startpoint(2)-endpoint(2))^2);
theta = 22.5*pi/180;
theta1 = -1*22.5*pi/180;
rotMatrix = [cos(theta) -sin(theta) ; sin(theta) cos(theta)];
rotMatrix1 = [cos(theta1) -sin(theta1) ; sin(theta1) cos(theta1)];
v2 = v1*rotMatrix;
v3 = v1*rotMatrix1;
x1 = endpoint+2*v1;
x2 = x1 + v2;
x3 = x1 + v3;
b = fill([x1(1) x2(1) x3(1)],[x1(2) x2(2) x3(2)],[0 0 0]);% this fills the arrowhead (black)
a = plot([startpoint(1) endpoint(1)],[startpoint(2) endpoint(2)],'linewidth',0.5,'color',[0 0 0]);
//同样在调用时储存每次画出的图形
for i=1:N
for j=1:N
if A(i,j)==1
[lineHandle, arrowHandle] = draw_arrow([pose_x(j,k),pose_y(j,k)],[pose_x(i,k),pose_y(i,k)], 0.05);hold on;
h1_arrows(i, j) = lineHandle;
h2_arrows(i, j) = arrowHandle;
end
end
end
//对应的删除操作
for i=1:N
for j=1:N
if A(i,j)==1
delete(h1_arrows(i, j)); % 删除线条
delete(h2_arrows(i, j)); % 删除箭头头部
end
end
end
六、matlab simulink中，程序不断发出机器人位置信息，创建一个模块收集机器人从开始到结束的所有位姿
wp1传入[10x3]列的矩阵，前6行依次代表6个机器人的位置坐标，后4行代表障碍物的位置坐标，每当有新的位置传进来wp1就会更新，记录下各机器人从开始位置到目标位置的所有坐标。
开始时采用的办法：
function saveData(pose_x, pose_y, pose_th)
% 保存数据到 .mat 文件
save('robot_pose_data.mat', 'pose_x', 'pose_y', 'pose_th');
end
但运行时save一直报错，无法保存机器人位置数据。
最终采用的方法：
通过To Workspace分别来收集机器人当前时刻位姿和当前时刻的目标位姿：
代码：
function [xp1,xp2,xp3,xp11,xp22,xp33] = visualizeRobots(wp1)
N = 6;
persistent poseX
persistent poseY
persistent poseth
persistent poseXX
persistent poseYY
persistent posethth
%% Adjacent matrix
A=[0 0 0 0 0 0; % a(ij)
1 0 1 1 0 0;
1 1 0 0 0 1;
1 1 0 0 1 0;
1 0 0 1 0 1;
1 0 1 0 1 0];
% delta_x = [0 -0.2 0.2 -0.4 0.0 0.4]; % 6个机器人相对间隔误差每一列代表一个机器人
% delta_y = [0 0.4 0.4 0.0 -0.3 0.0]; % 领航者与自己无误差
% 计算当前机器人的目标位置
pos_targetsX = [wp1(1,1) wp1(1,1)-0.2 wp1(1,1)+0.2 wp1(1,1)-0.4 wp1(1,1) wp1(1,1)+0.4];
pos_targetsY = [wp1(1,2) wp1(1,2)+0.4 wp1(1,2)+0.4 wp1(1,2) wp1(1,2)-0.3 wp1(1,2)];
pos_targetsth = [wp1(1,3) wp1(1,3) wp1(1,3) wp1(1,3) wp1(1,3) wp1(1,3)];
% if isempty(posel)
% posel = zeros(6,1); % 初始化x坐标的空矩阵
% end
coder.extrinsic('quiver','draw_circle','draw_arrow','gcs','get_param','assign') ;
% 存放机器人的位姿
pose_x = wp1(1:6, 1);
pose_y = wp1(1:6, 2);
pose_th = wp1(1:6, 3);
% 存放障碍物的位置
ob_temp = [wp1(7:10,1),wp1(7:10,2)];
% 将当前数据存入历史数据
%historyData{end+1} = struct('pose_x', pose_x, 'pose_y', pose_y, 'pose_th', pose_th, 'obstacles', ob_temp);
%% ====Animation====
area = compute_area(pose_x(1),pose_y(1),0.5);
hold off;
ArrowLength=0.1;
for j=1:N
quiver(pose_x(j),pose_y(j),ArrowLength*cos(pose_th(j)),ArrowLength*sin(pose_th(j)),'*k');hold on;
if j==1
state=2;
else
state=1;
end
draw_circle (pose_x(j),pose_y(j),0.06,state);hold on;
end
for i=1:N
for j=1:N
if A(i,j)==1
draw_arrow([pose_x(j),pose_y(j)],[pose_x(i),pose_y(i)], 0.05);hold on;
end
end
end
if size(ob_temp)~=[0 0]
plot(ob_temp(:,1),ob_temp(:,2),'Xk','LineWidth',2);hold on;
end
axis(area);
grid on;
drawnow;
poseX = pose_x;
poseY = pose_y;
poseth = pose_th;
poseXX = pos_targetsX;
poseYY = pos_targetsY;
posethth = pos_targetsth;
xp1 = poseX';
xp2 = poseY';
xp3 = poseth';
xp11 = poseXX;
xp22 = poseYY;
xp33 = posethth;
end
保存的数据：
总结
最终借助MATLAB强大的数据分析功能绘制出，预期的图形：
因为才开始学习使用Matlab还用许多不清楚的地方，故记录下遇到的问题供以后温故知新！
