Matlab 多机器人编队数据分析

---

前言

近期实验室购置了阿木实验室KKSwarm集群仿真平台，其整体开放环境为MATLAB，近期基于该平台做了一些集群编队的工作，期间遇到了一些小问题，记录下遇到的问题和解决方法。

---

一、Function ‘quiver’ not supported for code generation.

解决方法：添加coder.extrinsic('quiver')，其他的同理~

二、在仿真环境中添加障碍物

解决方法：添加coder.extrinsic('quiver')，其他的同理~

三、simulink中function函数初始化

matlab中搭建的simulink，每次有参数传入后都会初始化函数的定义，但只想在参数第一次传入时进行初始化，后面调用时不再初始化。

 persistent kpersistent pose_xpersistent pose_ypersistent pose_thpersistent h_circlepersistent h_quiverpersistent h1_arrowspersistent h2_arrowsif isempty(k)k = 0;endif isempty(pose_x)pose_x = zeros(6,5000);  % 初始化x坐标的空矩阵endif isempty(pose_y)pose_y = zeros(6,5000);  % 初始化y坐标的空矩阵endif isempty(pose_th)pose_th = zeros(6,5000);  % 初始化th坐标的空矩阵endif isempty(h_circle)h_circle = zeros(6,1);  % 初始化th坐标的空矩阵endif isempty(h_quiver)h_quiver = zeros(6,1);  % 初始化th坐标的空矩阵endif isempty(h1_arrows)h1_arrows = zeros(6,6);  % 初始化th坐标的空矩阵endif 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:Nh_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==1state=2;elsestate=1;endh_circle(j) = draw_circle (pose_x(j,k),pose_y(j,k),0.06,state);hold on;endfor i=1:Nfor j=1:Nif 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;endendendif size(ob_temp)~=[0 0]plot(ob_temp(:,1),ob_temp(:,2),'Xk','LineWidth',2);hold on;endaxis(area);grid on;drawnow; for j=1:Ndelete(h_quiver(j));endfor j=1:Ndelete(h_circle(j));endfor i=1:Nfor j=1:Nif A(i,j)==1delete(h1_arrows(i, j)); % 删除线条delete(h2_arrows(i, j)); % 删除箭头头部endendendcolor='mgbkrc';for i=1:Nplot(pose_x(i,5:k),pose_y(i,5:k),color(1,i),'LineWidth',2);hold onend

需要注意的是，想要单独删除某个绘制的图形时，需要创建句柄表示绘制出的图形，通过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:Nfor j=1:Nif 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;endendend

//对应的删除操作for i=1:Nfor j=1:Nif A(i,j)==1delete(h1_arrows(i, j)); % 删除线条delete(h2_arrows(i, j)); % 删除箭头头部endendend

六、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 poseXpersistent poseYpersistent posethpersistent poseXXpersistent poseYYpersistent posethth%% Adjacent matrixA=[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坐标的空矩阵
%     endcoder.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:Nquiver(pose_x(j),pose_y(j),ArrowLength*cos(pose_th(j)),ArrowLength*sin(pose_th(j)),'*k');hold on;if j==1state=2;elsestate=1;enddraw_circle (pose_x(j),pose_y(j),0.06,state);hold on;endfor i=1:Nfor j=1:Nif A(i,j)==1draw_arrow([pose_x(j),pose_y(j)],[pose_x(i),pose_y(i)], 0.05);hold on;endendendif size(ob_temp)~=[0 0]plot(ob_temp(:,1),ob_temp(:,2),'Xk','LineWidth',2);hold on;endaxis(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还用许多不清楚的地方，故记录下遇到的问题供以后温故知新！