2017, Vol. 50文章信息
- 陈小桥, 叶晓涵, 胡婷, 夏彤
- CHEN Xiaoqiao, YE Xiaohan, HU Ting, XIA Tong
- 基于双目视觉的无人机定位与控制系统
- Binocular vision-based unmanned aircraft localization and control system
- 武汉大学学报(工学版), 2017, 50(4): 624-629
- Engineering Journal of Wuhan University, 2017, 50(4): 624-629
- http://dx.doi.org/10.14188/j.1671-8844.2017-04-022
-
文章历史
- 收稿日期: 2017-01-15
无人驾驶飞行器通常简称无人机.它是指一种机上无驾驶员操作，仅由无线电遥控操纵或自身程序控制，利用空气动力承载飞行的飞行器.与载人飞机相比，它结构简单、成本低廉、适应性好，安全性能高.因此，广泛应用在军事、民用和科学研究等多个领域中.无人机根据结构的不同，可分为固定翼型无人机和旋翼型无人机.
作为一种常见的旋翼式无人机，四旋翼无人机由4个结构高度对称的旋翼提供升力，可实现偏航、俯仰及翻滚等动作，因其动力学模型简单，与固定翼飞行器相比可实现垂直起飞与降落，且在飞行过程中可保持悬停等正受到国内外越来越多的关注，成为近年来无人机领域中研究的热点[1].
然而，目前在四旋翼飞行器的控制方面仍存在某些问题：
1) 结构上，由于四旋翼主打轻型，所以负重能力有限，续航时间不足，飞行过程易受到外界气流干扰，尤其在室内飞行时，更易受墙壁回流气流的影响；
2) 算法上，四旋翼只有4个输入控制量，只有6个自由度，因此为欠驱动系统，需要额外的姿态反馈和定位导航来维持其稳定性[2-3]，需要设计更高级的模型和复杂的算法；
3) 定位上，目前广泛使用的是GPS定位，但GPS室内信号差，定位精度低，不适用于飞行器的室内定位.
本系统针对以上问题，研究了一套飞行器室内定位与控制方案，利用双目视觉解决了在室内气流影响大、没有GPS信号的情况下，飞行器难以稳定控制的问题.且本系统设计了良好的人机交互界面，实现了飞行器空间位置的三维重构和鲁棒性控制.
1 系统方案设计本系统由四旋翼飞行器、两个红外感光摄像头、RGBD摄像头、凌动处理器平台等构成.系统结构图如图 1所示.
|
| 图 1 系统结构图 Figure 1 System composition diagram |
Wii Remote核心为一个CMOS红外摄像头，成像视野近似为四棱锥形，其水平视角和竖直视角都为有限值，本系统采用的CMOS摄像头水平视角为40°，竖直视角为25°，水平方向和竖直方向上的成像原理相同.定位原理如图 2所示.
|
| 图 2 CMOS摄像头定位原理 Figure 2 CMOS camera positioning principle |
十字叉为目标在水平面上的投影，目标投影与相机的连线为OB′，左视野边界为OB，视野中线为OA，构建如图所示三角形OAB和三角形OAB′，已知相机的像素值为1 024×768，即水平方向上40°视角上的物体会映射到1 024个像素点上.Wii Remote是一个红外感知设备，视角与普通摄像头相比略小，因此在误差可接受范围内.这种映射关系可近似为线性的，即由像素值X1，可以推导出θX，过程如下式:
(1)
(2)
(3)
同样的过程可以得到在竖直方向上对应的偏角θY.
1.1.2 双目定位本系统采用了两个Wii Remote对无人机红外标识点进行定位.
现在比较流行的双目定位算法多是基于模式识别、特征检测的图像处理算法，而Wii Remote CMOS感光摄像头，只对940 nm波长的红外光成像，由于室内光的主要成分是可见光，即使有红外光，强度也比红外光源弱而且分散，因此不会对飞行器上的红外标识点定位造成干扰，避免了传统双目视觉中要先进行的目标检测，极大地简化了运算，提高了效率和实时性.
根据两个单摄像头所获取θX1、θY1、θX2、θY2的定位信息，和摄像头的布设信息，可以计算出目标的三维坐标.布设情况如图 3所示，计算过程如下:
(4)
(5)
(6)
(7)
(8)
(9)
(10)
(11)
|
| 图 3 双目摄像头布设坐标系 Figure 3 Binocular camera coordinate system |
在式(4)~(7)中，已代入视角度数等常数参数.由此可以得出目标位置在设定坐标系下的三维坐标，即完成了室内的无人机定位.
1.2 飞行器控制算法 1.2.1 飞行器姿态解算运载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一，求解姿态矩阵的方法主要有欧拉法、方向余弦法、四元数法.采用四元数法解算运载体姿态，虽然物理意义难以理解，但计算量小，实时性好，不存在“万向死锁”情况，因此本系统采用的是四元数法[4-5].
工程上一般运用范数为1的特征四元数，特征四元数的标量部分表示转角的一般余弦值，其矢量部分表示瞬时转轴n的方向.如下式表示矢量R相对参考坐标系旋转一个转角θ：
(12)
旋转轴n的方向由四元数的虚部确定，表示旋转轴n与参考坐标系轴间的方向余弦值.
四元数的求解用一阶龙格库塔法，如下式所示：
(13)
用四元数表示的姿态矩阵如下式所示：
(14)
将四元数带回姿态矩阵中，求出姿态角如下：
(15)
迭代求取过程中Δθ采用加速度和角加速度互补滤波的方法计算[6-8].
由此可以获取飞行器在飞行过程中的3个方向的姿态.
1.2.2 飞行器定位控制飞行器定位控制使用了多级串级PID控制理论，通过1.1节中计算得出的飞行器三维坐标定位信息，来计算期望的三维空间飞行速度，再通过期望飞行速度，计算得期望飞行姿态.将期望飞行姿态与飞行器控制器进行通信，飞行器以俯仰角、偏航角、翻滚角为输入量，对四轴的转速进行控制[9-13].
流程框图如图 4所示.
|
| 图 4 串级PID流程框图 Figure 4 Cascade PID process |
在许多不同的学科中，手势(gesture)有着其独特的含义，可能这些含义之间有某些异同.在人机交互领域，手势通常被作为传达一些简单的指令.在本系统中，使用手势产生简单的指令，来对无人机进行飞行控制.
Kinect传感器可以传回在摄像头前的人的骨骼帧信息，一共有20个关节信息，其中重点使用到的为ELBOW、SHOULDER、WRIST等关节的坐标信息.通过计算各个关节之间相对位置，来判断当前的手势状态.
经过数据处理之后，可以得到具体的手势状态判断[14-15]，如图 5所示.
|
| 图 5 手势状态 Figure 5 Gesture state |
固定时间间隔之间的手势状态变换，可以得到手势信息，由手势信息产生对应的简单控制指令，状态转换时对应的指令如表 1所示.
| 当前状态 | 上一步状态 | 指令 |
| LeftState | SureState | 向左飞行 |
| RightState | SureState | 向右飞行 |
| UpState | SureState | 向上飞行 |
| DonwState | SureState | 向下飞行 |
除了以上状态转化外，系统还支持手部关节实时跟踪，根据每帧数据中ELBOW、SHOULDER、WRIST关节的相对位置，实现连续的状态转换，以达到对飞行器轨迹的连续控制.
1.4 系统搭建和图形界面本系统在基于凌动处理器的嵌入式平台上编写了控制程序软件.程序通过蓝牙与Wii Remote传感器连接通信，并通过蓝牙返回的数据对无人机红外标识点的三维坐标进行解算，以得到无人机在室内的具体位置.程序通过NRF无线模块与无人机进行通信，向无人机下达飞行指令，使用PID计算控制无人机的飞行姿态.程序通过USB与Kinect传感器进行通信，对Kinect传回的人体信息进行数据处理，得到手势状态.
控制程序软件使用Qt搭建图形界面，可以直观地实时显示飞行器在飞行过程中的空间位置模拟图，实现目标运动轨迹的三维重构，并可以通过按钮进行飞行器控制，如图 6所示.
|
| 图 6 三维重构图 Figure 6 Three-dimensional reconstruction map |
飞行器的定位测试分为两种：一种是飞行器静止时的坐标测试，建准后的坐标系反映了飞行器在有效范围内的坐标捕获和坐标演算情况，用于测试坐标系校准和定位有效范围的判断，有测试者手拿着红外光源在房间内来回走动，观察返回的坐标情况；另一种是飞行器在飞行过程中的位置捕获、定位、数据滤波的测试.在飞机飞行时，存储下位置和速度，利用MATLAB工具绘图进行分析.
2.1.2 测试结果对于建准后的坐标系，表 2给出了部分测试点的测试结果.除了列出的几个典型值外，还测试出L=2 m，H=1 m时，有效定位范围为2 m×3 m×2 m的空间，L=3 m，H=1 m时，有效定位范围为3 m×3 m×2 m的空间, 且随着H和L的增大，有效定位范围也会随之增大，但精度会有一定程度的下降.
| 测试点坐标(x, y, z) | (0, 0, 0) | (100, 0, 0) |
| 测试次数 | 200 | 200 |
| 误差1%内次数 | 135 | 167 |
| 误差1%~10%内次数 | 54 | 33 |
| 误差10%以上次数 | 11 | 0 |
另一种是飞行器在飞行过程中的位置捕获、定位、数据滤波的测试.由静止时的测量可以确定，飞行器的定位精度在cm级，能够满足飞行器实时控制的要求.飞行器飞行过程中的坐标及滤波后的结果测试如图 7和图 8所示，滤波后的位置和速度均没有毛刺，符合控制要求.
|
| 图 7 滤波后速度 Figure 7 Velocity after filter |
|
| 图 8 位置测试 Figure 8 Test of position |
Kinect人体手势姿态识别的测试结果在室内进行，为了测试手势以及其他肢体动作对飞行器控制的效果，随机选取无肢体残疾的10名志愿者，对每个人的各种动作做了3次有效的测试.
2.2.2 测试结果每种手势可以得到150个测试结果，如表 3所示.
| 操作手势 | 测试次数 | 正确次数 | 正确率/% |
| 上升 | 10×3 | 29 | 97 |
| 下降 | 10×3 | 28 | 93 |
| 向右飞 | 10×3 | 28 | 93 |
| 向左飞 | 10×3 | 27 | 90 |
| 画圆 | 10×3 | 25 | 83 |
本文设计实现了一个基于Intel凌动处理器的室内飞行器定位与控制系统，实现了在室内没有GPS信号的情况下对飞行器的精确定位，以及基于手势姿态识别的对四旋翼的智能控制.
本系统的优势如下：
1) 精确的室内定位
基于双目视觉和红外成像的定位方法实现了室内cm级定位，可根据需定位的范围对红外摄像头进行组网，实现更大范围的定位追踪，具有极强的扩展性.红外定位不仅可应用于飞行器的控制，更适用于大型商场等地的室内定位与消息推送，具有宽广的应用前景.与其他信源加标签的方式(zigbee，射频等)相比，数据处理方法更简单，更能满足实时控制要求.
2) 稳定的自主飞行
在一定范围内识别飞行器，对飞行器进行定位追踪，自动控制飞行器的飞行，可实现定点起降、定点悬停、空中飞舞，手抓急停等高难度动作，为无人化操作提供可能.
3) 友好的界面显示和交互体验
本系统利用Qt语言和PS工具设计了交互友好的界面，且实现了目标运动轨迹的三维重构，极大地增强了可视性.
| [1] |
郭芳. 复杂环境下四旋翼无人机定位研究[D]. 天津: 天津大学, 2012.
Guo Fang. Localization for quadrotor unmanned aerial vehicle in complex environment[D]. Tianjin: Tianjin University, 2012. http://www.wanfangdata.com.cn/details/detail.do?_type=degree&id=D322729 |
| [2] |
姜成平. 一种四旋翼无人机控制系统的设计与实现研究[D]. 哈尔滨: 哈尔滨工业大学, 2014.
Jiang Chenping. Research on design and realization of control system for a quadrotor unmanned aerial vehicle[D]. Harbin: Harbin Institute of Technology, 2014 http://cdmd.cnki.com.cn/Article/CDMD-10213-1014081879.htm |
| [3] |
王伟, 吴旻, 王昱. 四旋翼无人机的室内自主飞行控制[J].
计算机仿真, 2015(1): 64–68, 136.
Wang Wei, Wu Hao, Wang Yu. Indoor autonomous flight control of quadrotor aircraft[J]. Computer Simulation, 2015(1): 64–68, 136. |
| [4] |
马敏, 吴海超. 基于四元数自补偿四旋翼飞行器姿态解算[J].
制造业自动化, 2013(23): 18–21.
Ma Ming, Wu Haichao. Quaternion-based self-compensating attitude estimation for a quad-rotor aircraft[J]. Manufacturing Automation, 2013(23): 18–21. DOI:10.3969/j.issn.1009-0134.2013.23.006 |
| [5] |
陈孟元, 谢义建, 陈跃东. 基于四元数改进型互补滤波的MEMS姿态解算[J].
电子测量与仪器学报, 2015(9): 1391–1397.
Chen Mengyuan, Xie Yijian, Chen Yuedong. Attitude estimation of MEMS based on improved quaternion complementary filter[J]. Journal of Electronic Measurement and Instrumentation, 2015(9): 1391–1397. |
| [6] |
梁延德, 程敏, 何福本, 李航. 基于互补滤波器的四旋翼飞行器姿态解算[J].
传感器与微系统, 2011, 11(11): 56–58, 61.
Liang Yande, Chen Ming, He Fuben, Li Hang. Attitude estimation of a quad-rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies, 2011, 11: 56–58, 61. DOI:10.3969/j.issn.1000-9787.2011.11.017 |
| [7] |
李杰, 齐晓慧, 韩帅涛, 刘星海. 小型四旋翼无人机飞行控制系统设计与实现[J].
中国测试, 2014(2): 90–93.
Li Jie, Qi Xiaohui, Han Shuaitao, Liu Xinghai. Design and implementation of flight control system for small quad-rotor unmanned aircraft[J]. China Measurement and Test, 2014(2): 90–93. DOI:10.11857/j.issn.1674-5124.2014.02.023 |
| [8] |
万晓凤, 康利平, 余运俊, 林伟财. 互补滤波算法在四旋翼飞行器姿态解算中的应用[J].
测控技术, 2015(2): 8–11.
Wan Xiaofeng, Kang Liping, Yu Yunjun, Lin Weicai. Application of complementary filter algorithm in attitude estimation of a quad-rotor aircraft[J]. Measurement & Control Technology, 2015(2): 8–11. |
| [9] | Zhen Ziyang, Pu Huangzhong, Chen Qi, Wang Xinhua. Nonlinear intelligent flight control for quadrotor unmanned helicopter[J]. Transactions of Nanjing University of Aeronautics and Astronautics, 2015(1): 29–34. |
| [10] | Ni Peipei, Qiang Yu, Wang Guosheng, Su Kuifeng, Feng Guo. Design of quadrotor's autonomous flight control system based on beagle bone black[C]// Proceedings of the 2015 Chinese Intelligent Automation Conference-Intelligent Technology and Systems, 2015: 591-599 http://link.springer.com/10.1007/978-3-662-46466-3_59 |
| [11] | Chen Yanmin, He Yongling, Zhou MinFeng. Decentralized PID neural network control for a quadrotor helicopter subjected to wind disturbance[J]. Journal of Central South University, 2015(1): 168–179. |
| [12] |
唐贤伦, 仇国庆, 李银国, 曹长修. 基于MATLAB的PID算法在串级控制系统中的应用[J].
重庆大学学报(自然科学版), 2005(9): 65–67.
Tang Xianlun, Chou Guoqing, Li Yinguo, Cao Changxiu. Application of PID algorithm based on MATLAB in cascade control system[J]. Journal of Chongqing University (Natural Science Edition), 2005(9): 65–67. |
| [13] |
冯庆端, 裴海龙. 串级PID控制在无人机姿态控制的应用[J].
微计算机信息, 2009(22): 9–10, 45.
Feng Qinduan, Pei Hailong. The application of cascade PID control in UAV attitude control[J]. Microcomputer Information, 2009(22): 9–10, 45. DOI:10.3969/j.issn.1008-0570.2009.22.004 |
| [14] |
李红波, 丁林建, 吴渝, 等. 基于Kinect骨骼数据的静态三维手势识别[J].
计算机应用与软件, 2015(9): 161–165.
Li Hongbo, Ding Linjian, Wu Yu. Static three-dimensional gesture recognition based on Kinect skeleton data[J]. Computer Applications and Software, 2015(9): 161–165. |
| [15] |
赵飞飞, 刘喆颉, 吕玉祥, 等. 基于Kinect骨骼信息的手势识别[J].
中国科技论文, 2015(20): 2412–2415.
Zhao Feifei, Liu Zhexie, Lü Yuxiang, et al. Gesture recognition based on Kinect skeleton information[J]. China Science Paper, 2015(20): 2412–2415. DOI:10.3969/j.issn.2095-2783.2015.20.015 |