Matlab实现姿态角变化可视化实时显示

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

在使用AppDesigner 做上位机的过程中，想着利用Unity3D做一个可视化的界面来显示姿态角，奈何C#基础薄弱，而且Matlab于Unity之间的联动异常复杂，只好放弃这种做法，偶然在b站上面看到，Matlab自身也可以做一个可视化的界面，尽管可能建模没那么好看（以至于只能使用长方体代替），但是能够实现功能总是好的。下面我将介绍一下Matlab如何实现这个姿态角变化的可视化。

---

一、Patch函数

首先要介绍一下这个Patch函数，这个函数用来绘制一个或多个填充多边形区域，多个咱们暂且不说，咱们只说我们用到的，下面是官网上的解释。

可以看到，当我们指定了面和顶点之后，就可以绘制一个正方形，那么我们推广一下，这里只是四个顶点，一个长方体有8个顶点，而且长方体是三维物体，左边点肯定是3维坐标点，最终的矩阵应该是3行8列；f是8个顶点，那么6个面每个面都需要4个点来产生，于是f应该是4行6列的矩阵；比如我们可以设为如下的方式（举个例子可能看起来不是很好）；

v = [0 0 0;0 3 0;3 3 0;3 0 0;0 0 3;0 3 3;3 3 3;3 0 3];
f = [1 2 3 4;5 6 7 8;1 4 8 5;1 2 6 5;2 3 7 6;3 4 8 7];
  aaa=patch('Faces',f,'Vertices',v');

如果不选颜色，画出的图可能是黑色的。

二、计算旋转矩阵

1、rotx、roty、rotz函数使用

以rotx函数为例：实际上就是绕x轴旋转一定的角度，我们也叫横滚角（roll），rotx的参数就是这个横滚角，计算后我们得到的是绕x轴的旋转矩阵。类似的，我们也可以得到绕y轴和z轴的旋转矩阵如下所示：

Rx = rotx(roll);
Ry = roty(pitch);
Rz = rotz(yaw);

2.任意角的旋转矩阵

显然，我们只需要分别绕x、y、z旋转即可，Rotation_Mat是旋转矩阵，代码如下（示例）：

  Rotation_Mat = Rz*Ry*Rx*v'；

三、绘图

1、绘制旋转之后的图像

 delete(aaa);
  aaa=patch('Faces',f,'Vertices',Rotation_Mat');

重新绘制旋转之后的图像之前要把原来的给删除，不然会出现重影的现象。之后要在循环中一直收姿态角的数据传给旋转矩阵，然后不停的使用patch函数进行绘图。
—**

2、上位机有关代码

下面这部分是上位机中有关绘制3D图的相关代码，其中是在AppDesigner中写的。

A = fread(s,3,'int16')/32768*180;
roll = A(1);
pitch = A(2);
yaw = A(3);              
Rx = rotx(roll);
Ry = roty(pitch);
Rz = rotz(yaw);
Rotation_Mat = Rz*Ry*Rx*Cubic_Point';
     aaa=patch(app.Obiect_3D,'Faces',Cubic_Plane,'Vertices',Rotation_Mat','FaceVertexCData',Cubic_Corlor,'FaceColor','flat');
view(app.Obiect_3D,3); 
axis(app.Obiect_3D,[-3,3,-3,3,-3,3]);%坐标系范围
   
set(app.Obiect_3D, 'XGrid', 'on');
set(app.Obiect_3D, 'YGrid', 'on');
set(app.Obiect_3D, 'ZGrid', 'on');
xlabel(app.Obiect_3D,'X-axis');
ylabel(app.Obiect_3D,'Y-axis');
zlabel(app.Obiect_3D,'Z-axis');
drawnow;
delete(aaa);

总结

这里只是一个方法，代码不一定所有都环境下都适用，如有错误，请各位大佬指正。