基于optitrack的四旋翼飞行器室内定点悬停simulink程序
背景
这是我的本科毕业设计。simulink工程的运行需要特定的硬件（室内定位系统optitrack）的支持，而且由于实验室规定关系，本文中关键部分的代码没有进行展示。
毕设最终成果视频：https://v.qq.com/x/page/v0519w1g4xc.html
本文的主要内容有：程序的实现原理，程序的具体实现方式，界面交互部分。
程序实现原理分三部分：和pixhawk飞控的串口通信模块，室内定位系统optitrack的通信，位置误差转换为飞行器姿态控制。
实现原理
串口通信
无线数传支持全双工，为了不丢失数据，需要在地面站软件中用两个线程分别来进行串口的读写操作。程序结构如下图：
该程序用c++语言写成，然后在matlab中用mex命令转化为mex64文件，在s函数中调用。最终实现的simulink模块如下图：
室内定位系统optitrack
从理论上说，对于三维空间中的一个点，只要这个点P能同时为两部摄像机所见，则根据同一时刻两部摄像机所拍摄的图像和两台摄像机之间的相对位置关系以及相机参数，可以唯一确定这一时刻点P在三维空间里的位置信息，这就是双目定位的原理。如图
OptiTrack系统在摄像头周围放置红外发光阵列，在目标物上固定高反射红外光的标志点小球，加装红外光滤波片的摄像机即可以很好的捕获空间中发射红外光的标志点，并将可见光部分的背景滤掉。
OptiTrack系统一般使用6台以上的摄像头，相当于15（c62）对双目，系统精度很高。然后高精度的点坐标根据预先创建的在四旋翼飞行器上的4个点所代表的刚体信息来解算飞行器的质心位置和姿态。
位置误差转化为姿态误差
四旋翼飞行器最常见的控制方法是分层控制。最低级别的是控制螺旋桨的转速。下一级别是飞行器的姿态控制，*别的是飞行器的航迹控制，控制框图如图所示：
在获取到室内定位系统传回的飞行器位置和姿态后，可以根据当前的期望航点计算需要的姿态控制量。这个计算可以在simulink模块中实现，然后给飞行器发送姿态命令。
simulink中实现的流程图为：
实现方式
串口通信
把cpp文件编译为mex文件在matlab的s函数中调用。关于cpp的内容和怎么编译成mex文件，在我的另一篇文章http://blog.csdn.net/iamqianrenzhan/article/details/73838733中有介绍。
optitrack通信
https://github.com/rshum19/Crazyflie-PosCtrl中可以下载到。
位置误差转化为飞行器姿态控制
https://github.com/rshum19/Crazyflie-PosCtrl中可以下载到。
界面交互部分
整个地面站软件分为两个部分，菜单部分有“文件”，“设置”，“显示”，“帮助”4个选项，界面部分把各功能模块分面板进行组织，分别为设置面板，控制面板，显示面板和状态面板。如下图：
设置面板用来在建立连接前设置连接参数，包括串口号，波特率，室内定位系统服务器IP地址，本机IP地址等。控制面板实现控制仿真程序的运行、航点的设置、命令的发送、坐标系的标定等功能。显示面板用来显示飞行器的实时姿态和位置，可以用图和表两种方式来表示，需要时可切换。