基于optitrack的四旋翼无人飞行器室内控制平台软件设计——小论文

基于optitrack的四旋翼无人飞行器室内控制平台软件设计

学院：机械工程专业：机械工程

姓名：高伟晋学号：41340116

指导教师：缪存孝讲师

摘要

课题的背景与意义

随着无人机的小型化和轻量化发展，越来越多的无人机应用推广到了室内。相应地，对室内地面站软件的要求也越来越高。在靠MOCAP等室内定位系统获取到的飞行器位姿的基础上进行飞行器的控制，是目前室内飞行器地面站需要实现的基本功能。进行室内轨迹的规划与跟踪，执行特定的室内动作，都需要室内地面站的支持。本文即关注飞行器的室内飞行控制地面站的开发，尝试实现飞行器的室内定点悬停和简单的航迹跟踪功能。

在室外靠GPS定位导航的无人机地面站已经比较成熟，而在应用于室内的无人机地面站尚处在探索阶段。首先，室内的定位问题也没有通用的解决方案，目前用到的方法有IMU，光流，动作捕捉，视觉等。各种方法各有优劣，动作捕捉由于定位精度高（相对于IMU），处理速度块（相对于视觉）等优点，广泛应用于室内的定位应用。其次，由于四旋翼飞行器结构简单、成本低廉、飞行性能卓越以及飞行控制方式独特等原因，已经成为室内SLAM，室内短距通信等研究的重要平台。最后，四旋翼飞行器的编队飞行是无人机研究的又一热点与难点，而室内控制平台的开发是研究飞行器的室内编队飞行的前提。无人机编队飞行，协同执行任务提高了效率与可靠性，在应用领域意义重大，可以用来开发与验证算法，促进其它相关领域的发展。

相关原理介绍

无人机实时定位解决方案是利用高精度的OptiTrack三维运动捕捉系统。OptiTrack利用了双目定位的原理。双目定位原理是如果两台已知参数和相对位置的摄像头同时看到一个点，那么从几何学上来说，这个点的位置是唯一确定的。OptiTrack系统包括至少6台以上的摄像头，相当于多对双目；又由于其能自身发射红外光，镜头只接收红外光，不受外界可见光影响，所以捕捉位置精度高，在0.1mm以下。OptiTrack只对点进行定位，相对于传统图像视觉，处理信息量少，因此延时很短，在4ms以下。地面站和室内定位系统OptiTrack之间通过UDP 协议进行通信，利用制作商提供的SDK在MATLAB中实现飞行器位置和姿态数据的获取。获取的过程是首先进行网络通信的连接，然后获取信息的描述文件，最后利用信息描述文件进行消息的解析，得到飞行器的位置和姿态信息。只要运行地面站和运行室内定位系统的电脑在同一局域网中，地面站就能获取到室内定位系统发出的数据。有时为了保持实时性，两

者可以运行在一台电脑上，使用本地回环地址进行通信。

航点设置是本课题无人机地面控制站的一项重要功能，也是实现自主飞行的必要模块。通过手动输入航点的方式控制飞行器飞到指定的地点。其中手动输入航点分为两种方式，一是键盘输入，二是直接从坐标系中选取。航点的控制最终要转换为四旋翼飞行器姿态设定值。一个典型的四旋翼控制过程分为三个环节，分别是电机的控制，姿态的控制和位置的控制。在本平台中电机和姿态的控制在飞行控制器中完成，在地面站上进行的是姿态规划算法，即根据航点的偏差解算姿态的设定值。姿态控制算法用来控制飞行器姿态的三个自由度，以给定姿态与姿态检测算法得出的姿态偏差作为输入，以四旋翼无人机的四个电机转速作为输出，从而达到控制飞行器姿态的目的。姿态的规划算法的具体原理是根据位置偏差计算出期望的速度，根据期望速度和实际速度计算出期望的加速度，根据期望的加速度计算期望的姿态角。

地面管理软件和四旋翼无人飞行器之间的数据通信使用的是MA VLink协议。MA VLink 是无人飞行器与地面站之间通讯，以及无人飞行器之间通讯最常用的协议，该协议在2009年被瑞士苏黎世联邦理工学院的发布，经过多年在其他开源飞控上的测试，已经证明了其实用性和稳定性。本平台使用无线数传来进行MA VLink通信。硬件上，无线数传属于串口，最高波特率可到921600，支持全双工。在通信过程中，为了避免数据的丢失，在程序实现上利用两个线程分别进行读写操作。在写线程中，把将要发送的数据打包成MA VLink协议格式的字符串，通过串口发送。在读线程中，程序时刻判断串口接受缓存区是否有完整的一帧消息，如果有则根据信息的ID进行数据的解析。由于MATLAB在处理多线程方面的短板，该部分使用c++语言实现，写成Simulink的S函数格式，编译成MEX文件在Simulink中调用。

地面站软件总体设计

本课题选择的软件开发环境是MATLAB，相比于其他面向对象的语言，MA TLAB属于解释性语言，在执行过程中逐渐编译，所以方便排查错误。本课题的大部分程序代码是在MATLAB中完成的，包括程序的调试，界面的设计，Simulink模块的设计，与室内定位系统和飞行控制器的通信等。MATLAB目前在各个操作系统平台都有对于版本，除了在串口操作的地方有少量的改动外，程序可以很方便地移植到其他平台，这增加了软件的灵活性。本课题选择的飞行控制器是pixhawk，pixhawk是面向科研的飞行控制器，控制能力和处理速度都比大部分面向普通用户的飞行控制器强，它是瑞士苏黎世联邦理工学院开发的软件和硬件都开源的项目，具有丰富的外设接口和成熟的固件，非常适合进行二次开发。硬件上能连接的外设有无线数传，遥控，电调，GPS等，使用的固件支持MA VLink通信协议，支持外部位置估计。本课题选择的室内定位系统是OptiTrack，OptiTrack是世界上最大的运动捕捉系统提供商NaturalPoint开发的高性能三维光学跟踪系统，能够提供智能机器人（如无人机、地面机器人）的实时位置和姿态信息。相比于利用深度图和彩色图进行融合进行定位的Kinect，其处理速度快，精度高，应用领域更加广泛。

本课题开发的地面站软件要实现的功能有：获取室内定位系统发出的无人飞行器当前位

置和姿态；实现地面管理软件与飞行控制器之间的通信；进行航点的规划和飞行器的控制。为了借助Simulink强大的建模和仿真功能，本课题的地面软件核心是运行在Simulink上的，界面程序只是负责和Simulink模块交流，设置通信参数，获取飞行状态，控制飞行过程等。该Simulink模型可以与实际的硬件通信，能通过局域网从室内定位系统获取到飞行器的当前位置和姿态；能与串口设备进行通信，利用MA VLink协议与飞行控制器通信，发送控制命令，接收状态消息；能把当前点和期望点的误差通过物理模型转化为飞行器姿态设置点。

按照上述的功能需求，设计地面站程序的总体流程图。在地面站开始运行后，首先要设置与室内定位系统和飞行控制器通信的参数，确保通信连接成功后，进行坐标的标定和第一个航点的设定，然后判断飞行器是否到达设定航点，如果到达，开始设置下一个航点，如此循环，完成航迹的控制。为了便于操作，将这些功能与交互界面结合起来，在交互界面上设置通信参数，控制通信的开始结束，进行航点的设置，显示飞行过程中三维姿态和三维位置。软件测试结果及结论

为了验证软件的功能，排查各种不合理使用下的程序出错问题，判断提供的软件使用说明等文件是否完整等，在软件设计完成后需要进行测试。测试方案是进行航点跟踪的实验。具体步骤是：1.设置与飞行控制器和室内定位系统的通信参数，等待连接成功。2.开始运行控制系统，观察界面是否显示飞行器的三维位置和三维姿态信息。3.进行坐标的标定，将飞行器起飞点设为原点。4.设置目标点，观察飞行器状况，在飞行器到达设定点后重复此步骤。在进行实验的过程中，记录飞行器的期望位置和实际位置。从飞行的实验结果来看，飞行器在高度方向上的控制效果较好，能稳定在0.05m以内，而在水平方向上控制效果较差，稳定在0.1m 的范围内。控制精度可以通过调节控制器的PID参数来提高，这不在本课题的讨论范围内。从测试过程来看，设计的地面控制软件运行稳定，对于不合理操作会给出提示，软件使用说明完整清晰，室内定点悬停和航点跟踪功能全部实现。

通过对本课题的深入总结，得到如下结论：（1）飞行器期望位置和实际位置之间的误差已经在可以接受范围之内，可以实现室内定点悬停和航点跟踪功能。（2）OptiTrack及其他比如Vicon等光学三维位置定位的方法延时小，精度高，适合无人机的室内定位应用。（3）如果能用此地面站同时控制多架飞行器，可以实现诸如飞行编队，协调作业等功能。

关键字：无人机，地面站，飞行控制平台，mavlink协议，OptiTrack