室内无人机定点悬控系统

孟令康，秦海超，鲍丹阳

(哈尔滨工业大学(深圳) 机电工程与自动化学院，广东 深圳 518000)

0 引言

近年来无人机应用市场日趋火热，无人机开始被应用在多个领域之中[1-4]，比如航拍、植保、运输、安防等[5]。无人机的自主飞行依赖导航系统提供的定位信息进行控制。当前，为了在保障安全可靠的前提下便于高校及研究所科研人员对无人机控制的关键技术展开研究，迫切需要一套成熟的无人机室内定位技术解决方案。基于此，本文做了相关研究并开发了基于图像处理的室内无人机定位系统，可对无人机控制相关的科研进程提供有效可靠的实验条件，也将加速无人机技术应用到各个行业中。

当前较为成熟的定位方案包括以下几种：

1) 全球卫星导航系统

如美国的GPS(global positioning system)和中国的BDS(beiDou navigation satellite System))，但此类方案在民用范围内精度受限制[6]，一般必须与INS(inertial navigation system)[7]的惯性数据融合后才能实际使用，且全球卫星导航系统在室内的应用效果会因建筑环境对卫星信号的影响而大打折扣[8]，导致更加难以形成可靠的室内定位方案。

2) 光流法定位方案

其原理为采集安装在机器人底部的微型相机垂直向下拍摄所得的连续图像帧序列，通过光流法计算相邻两帧图像之间基于像素的位移[9]。与诸如超声波、激光、气压计等高度传感器获得的相对高度数据融合后得到基于参考坐标系尺度下机器人的相对位移，进而完成定位的任务。但其有效性对地面纹理要求高且不具有通用性，而光流算法本身及其数据融合算法的有效性对定位的精度影响也很大，最重要的是方案本身具有不可避免的长期漂移性[10]。

3) 并发建图与定位技术

近年来发展迅速，一般分为基于视觉和基于激光雷达两种方案[11]。基于视觉的方案算法复杂度高，即便普通的定位实时性也需要相当程度的硬件支持[12]。而基于激光雷达方案的复杂度相对于视觉方案低，但硬件成本更高[13]。

4) 无线测距定位方案

采用多基站测距后解算目标位置[14]。由于无线通讯的特点，其测距受遮挡干扰大，对机体自身姿态变化敏感，且动态性能差。在无人机系统中的应用不具备通用性。

本文采取基于图像处理的室内定位系统，设计四旋翼无人机实验平台，将图像识别的结果结合目标无人机自身的高度值解算出无人机的水平位置信息，同时基于上述定位系统设计了基于四旋翼无人机的定点悬停控制方案，通过进行室内定点悬停实验对该系统方案的有效性进行验证。

1 基于图像处理的室内定位系统

1.1 定位系统介绍

针对室内无人机，本文提出一种基于图像处理的定位系统，通过单相机实时采集定位区域的画面，对无人机机体的图像特征进行识别后进行定位，实现无人机的自主飞行。

1.2 相机模型

相机将三维世界中的坐标点(单位为cm)映射到二维图像平面(单位为像素)的过程能够用一个几何模型进行描述。这个模型有很多种，其中最简单的称为针孔模型(常用且有效)。如图1所示，针孔模型描述了一束光线通过针孔(即光心)之后，在针孔背面投影成像的关系。又由于相机镜头上透镜的存在，使光线投影到成像平面的过程中产生畸变。故本文使用针孔和畸变两个模型来描述整个投影过程，把外部的三维点投影到相机内部成像平面，构成相机内参数。

设Oxyz为相机坐标系，习惯上z轴指向相机前方，x向右，y向下，O为摄像机的光心，也是针孔模型中的针孔。参考坐标系下的空间点P经过小孔O投影之后，落在物理成像平面Ox'y'上，成像点为P'。

图1 针孔模型

设P的坐标为[X,Y,Z]T，P'为[X',Y',Z']T，并且设物理成像平面到小孔的距离为f，即焦距。根据三角形相似关系，则有：

(1)

其中负号表示成像为倒立。为简化模型，如图2所示，本文对图像进行预处理，将成像平面对称至相机前方，和三维空间点一同置于摄像机坐标系的同一侧，则有：

(2)

图2 成像等效转换图示

在物理成像平面上固定像素坐标系O-x-y，原点O位于图像左上角，u轴向右与x轴平行，v轴向下与y轴平行。相对于成像平面，像素坐标系在u轴上缩放了α倍，在v轴上缩放了β倍，同时原点平移了[cx,cy]T，则P'点坐标与像素坐标[u,v]T有如下关系：

(3)

将式(3)代入式(2)，并将α合并为fx，β合并为fy，得:

(4)

其中：f的单位为m；α、β的单位为像素每米；fx，fy的单位为像素。式(4)化为矩阵形式如下：

(5)

其中：K为相机的内参数矩阵，Pc为三维空间点在相机坐标系下的坐标。

1.3 位置解算模型

P点在相机坐标系下坐标Pc和参考坐标系下坐标Pr的关系由旋转矩阵R和平移向量t描述如下：

(6)

由公式(4)得：

(7)

由公式(6)得：

(8)

由式(7)、式(8)得，已知相机内参数及其位姿，则可通过相机识别结果并结合无人机高度即可解算出水平位置信息，用于无人机位置控制。定位系统算法流程如图3所示。

图3 定位系统算法流程图

2 室内定点悬停控制系统设计

2.1 控制系统简介

四旋翼无人机的电机对称地安装在飞行器的支架端，旋翼均处于同一高度平面，且结构和半径都相同，其结构形式如图4所示。

图4 四旋翼结构示意图

由于四旋翼无人机的特殊结构，使其成为一种六自由度的垂直升降机，通过调节4个电机转速来调整无人机姿态，进而改变4个电机对机体作用力的大小和方向，实现对飞行器位置的控制。

四旋翼无人机室内定点悬停控制系统，将参考坐标系下无人机的姿态和位置作为系统的控制量，4个电机的转速作为控制系统的输出量，可实现垂直起飞及定点悬停等功能。

2.2 姿态解算

姿态解算方案如图5所示，飞行控制器安装有惯性测量单元，可对机体三轴线加速度与三轴角速度进行测量，利用惯性测量单元的输出结合机体自身重力采用牛顿梯度下降算法进行姿态解算，获得对水平轴对应姿态角的估计。

另安装有地磁传感器，对无人机所在位置的磁场进行测量，结合陀螺仪对竖直轴对应角速度测量值的积分进行互补滤波，最终获得偏航角估计。

图5 姿态解算方案示意图

2.3 高度控制

在位置控制中，水平两个方向是基于姿态的控制方式，其结构相同，而竖直方向即高度控制则基于4个电机合力的控制方式，然而当机体具有一定的水平倾角时，升力的改变会导致机体水平加速度的改变。

这种关系造成了高度控制与水平控制的耦合关系，故在基于模型控制时，需在对水平姿态角度的控制量中加上升力的影响。但考虑在四旋翼无人机的室内悬停控制情况下，除垂直起飞过程外，高度控制量无大的干扰或波动且不存在大角度控制量，故本文将忽略其耦合关系，对高度和水平位置分别进行控制。

如图6所示，通过三环PID控制提高高度环的稳定性和抗干扰性，其中对加速度环中的微分项引入了二阶低通滤波，竖直高度、竖直速度及竖直加速度反馈量通过传感器组数据融合后获得，其控制输出为四电机各自的基础转速控制量。

图6 高度控制环

目前可实现对高度进行测量的传感器包括有GPS、超声波、激光、气压计等，考虑到室内空间的特点，如图7所示，结合对传感器自身测量精度、稳定性以及输出频率等要求，选择激光测距的方案对机体高度进行测量。竖直加速度由加速度计竖直轴测量值经“体-地”坐标系转换后获得，竖直速度采用捷联惯导方案融合竖直高度与数值解速度后获得，其中参与融合的竖直高度单独由测量频率、稳定性，平滑度更高的气压传感器获得。

图7 高度数据融合方案示意图

2.4 水平位置控制

定位系统向无人机输出水平位置，无人机对位置数据微分后经低通滤波获得水平方向的速度估计值，对水平位置环路实现如图8所示串级PID控制，其中水平两个方向控制环路结构相同，其控制输出为参考坐标系下期望姿态角。

图8 水平位置控制环

2.5 姿态控制

为简化控制结构，将偏航角的期望值置0，仅通过水平姿态角且不影响控制效果，以姿态解算获得的姿态角和陀螺仪对角速度的测量值作为反馈量，设计如图9所示串级PID控制，其中各姿态角控制环路结构相同，其控制输出为四电机各自的补偿控制量。

图9 姿态控制环

3 实验验证

本文采用等轴距为33cm的四旋翼无人机作为载体，飞行控制计算机和外部设备均安装在机体中央位置，在其顶部中央固定有特定颜色的轻量球体，各机臂中央安装有30 A无刷电调，机臂外端安装有1200kV无刷电机，并采用5200mAh容量的3S锂电池，组成无人机平台的动力配置。无人机通过双蓝牙与上位机进行通讯，分别完成与图像定位系统的数据通讯任务和实时飞行数据的采集任务。无人机平台在实现自主飞行的同时仍保留遥控器配置作为应急方案切换。

上位机配置处理器为Intel(R) Core(TM) i5-4210M，主频为2.60GHz，安装RAM 4.00GB，操作系统为Windows 10(1803)教育版。

控制系统结构图如图10所示，四旋翼无人机和上位机之间通过蓝牙进行无线通讯，无人机将高度数据发送给上位机，上位机结合相机获得的图像数据解算出水平位置信息，并发送给无人机进行位置控制。

图10 室内定点悬停控制系统结构图

通过实验给出位置跟踪曲线(图11-图13)和姿态跟踪曲线(图14-图16)，其中，无人机位置期望均为起飞时坐标即初始位置，初始高度为激光传感器固定高度，航向角为基于地理坐标系下磁场方向所建立，其具体数值见表1，相关控制参数见表2。

表1 系统参数

表2 控制参数

图11 x轴位置跟踪曲线

图12 y轴位置跟踪曲线

图13 高度跟踪曲线

图14 横滚角跟踪曲线

图15 俯仰角跟踪曲线

图16 航向角跟踪曲线

4 结语

本文通过设计基于图像处理的室内定位方案，为室内无人机系统提供实时的定位数据，同时设计了四旋翼无人机实验平台，结合基于图像处理的室内定位方案进行了室内定点悬停实验，达到了预期的效果，证实了本文提出的室内定位方案的可行性，且该目标定位方案对机体自身负荷小，精度高，实时性好，可移植性强，对其他摄像头和无人机平台都具有较好的通用性。