ABSTRACT

In this paper, we discussed the method of using laser intensity direction and ranging (LIDAR) to solve the difference between the direction of the head of the unmanned aerial vehicle (UAV) and the direction of the target center, and the distance between the UAV and the target. The EKF fusion gyroscope, accelerometer and magnetometer data are used to analyze the attitude of the UAV. The attitude control of the UAV is carried out using a bicyclic PD controller with angular rate inner ring and angular outer ring. The feedback angle calculated by the lidar information is fed back to the steering control PID controller, and the output of the controller is connected to the yaw angle controller for the head locking target. The target distance is fed back to the pitch control PID controller, and the output of the controller is connected to the pitch angle controller to maintain the distance between the UAV and the surround target. The combination of two controllers controls the UAV to fly around the target. The experiment platform of quad-rotor is designed. The feasibility of using the lidar around the target flight is verified by the lidar object experiment.

Keywords:Bridge Inspection, Lidar, Circling Flight

无人机环绕目标飞行控制技术研究

屈利伟，莫宏伟^*

哈尔滨工程大学，黑龙江 哈尔滨

收稿日期：2017年10月16日；录用日期：2017年10月26日；发布日期：2017年11月2日

摘 要

论述了利用激光雷达测量信息解算无人机机头方向与环绕目标中心方向角度差即转差角和无人机与环绕目标间距离的方法。通过EKF融合陀螺仪、加速度计和磁力计数据用于无人机的姿态解算，采用具有角速率内环和角度外环的双环PD控制器进行无人机的姿态控制。将由激光雷达信息解算出的转差角反馈到转向控制PID控制器，控制器的输出量接入偏航角控制器用于机头锁定目标。将目标距离反馈到定距控制PID控制器，控制器的输出量接入俯仰角控制器用于维持无人机与环绕目标间的距离。两控制器相结合控制无人机环绕目标飞行。搭建了四旋翼无人机实验平台，通过柱状物体环绕实验验证了利用激光雷达解算数据控制四旋翼环绕目标飞行的可行性。

关键词 :桥梁检测，激光雷达，环绕飞行

Copyright © 2017 by authors and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY).

http://creativecommons.org/licenses/by/4.0/

1. 引言

桥梁作为道路的重要组成部分，是交通工程中的关键性枢纽，在交通运输中起着非常重要的作用。在桥梁建成投入使用后，随着时间的积累，桥梁的路面和桥梁支座出现老化或破损现象，一些桥梁常年带病运营，潜伏着巨大的安全隐患。过早的改造这些桥梁会造成很大的经济负担，因此有必要采用有效的桥梁检测手段监控桥梁的运行状态 [1] 。常用的检测手段有人工望远镜、桥检车、登高车和无人机等 [2] 。无人机桥梁检测系统具有使用成本低，安全风险小，定点悬停观测等优点，然而对无人机操控者的操控技术有较高要求。桥梁支座是桥梁易坏部位，是桥梁检测的重点检查对象，人为控制无人机环绕桥墩检查桥梁支座健康状态对无人机操控者操控技术要求较高，采用自动环绕桥墩飞行技术可大幅降低无人机操控难度，同时提高桥梁检测效率。

环绕飞行的实现需要借助于目标跟踪技术的使用，环绕目标即为被跟踪目标，无人机作为跟踪者。目标跟踪有两个大的问题需要解决，一个是目标是什么，另一个是目标在哪里 [3] 。目标识别是指将一个特殊的目标从其他目标中识别出来的过程，要识别出特殊目标就要确定该目标有别于其他目标的特征，可以将目标的大小、形状、重量等参数作为目标特征。文 [4] 提出在红外图像中采用顶帽变换和均值滤波提高目标的可见性，采用两层阈值法更精确地计算目标尺寸和利用后处理方法提取小目标。文 [5] 通过对运动物体的角点分析和形状特征提取，实现对运动目标的检测并采用并行卡尔曼滤波器对视频中的动态目标进行跟踪。确定目标在哪里有两种方式，一种是可时刻确定目标和跟随者的位置，不断调节跟随者的位置实现对目标的跟踪。如通过GPS定位，确定无人机的位置，通过规划无人机运动的航点，实现对目标的环绕控制。另一种是可时刻确定目标与跟随者之间的距离及目标处于跟踪者的方位，通过维持跟随者和目标之间的距离实现对目标的跟踪。跟随者和目标之间相对距离的测量有多种方式，如基于声波传输的超声波测距，基于光传输的激光测距仪、激光雷达，基于三角测距原理的双目视觉技术等 [6] 。其中激光雷达和双目视觉可以确定目的方位，适合用于环绕飞行控制。

2. 无人机姿态控制

2.1. 坐标系及坐标变换矩阵

为正确解算无人机姿态信息，本文使用的坐标系如下：

载体坐标系(b系)：坐标系原点位于无人机重心，为右手笛卡尔坐标系，z轴指向天，其x轴正向指向机头方向，y轴正向与x轴垂直指向机体左侧。

水平坐标系(h系)：坐标系原点位于无人机重心，为右手笛卡尔坐标系，z轴指向天，x轴和y轴正交位于水平面内。

地理坐标系(n系)：坐标系原点位于无人机重心，为右手笛卡尔坐标系，z轴指向天，其x轴指向东向，y轴指向北向。

采用欧拉角变换的方法将地理坐标系变换为载体坐标系，变换过程为首先绕 $z_n$ 轴逆时针转动 $\gamma$ 将n系变为h系，然后绕 $x_h$ 轴逆时针旋转 $\alpha$ ，将h系变为I系，最后绕 $y_I$ 轴逆时针旋转 $\theta$ 将I系变为b系 [7] [8] 。其中 $\alpha$ 为横滚角， $\theta$ 为俯仰角， $\gamma$ 为偏航角，地理坐标系到载体坐标系变换示意图如图1所示。

载体坐标系到水平坐标系姿态变换矩阵为：

$C_b^h={\left(C_h^b\right)}^{\text{T}}={\left(C_I^b{C}_h^I\right)}^{\text{T}}=\left[\begin{array}{ccc}\cos \theta & \sin \alpha \sin \theta & \cos \alpha \sin \theta \\ 0& \cos \alpha & -\sin \alpha \\ -\sin \theta & \cos \theta \sin \alpha & \cos \alpha \cos \theta \end{array}\right]$ (1)

载体坐标系到地理坐标系的姿态变换矩阵为：

$C_b^n={\left(C_n^b\right)}^{\text{T}}={\left(C_I^b{C}_h^I{C}_n^h\right)}^{\text{T}}=\left[\begin{array}{ccc}\mathrm{c}\theta \mathrm{c}\gamma & \mathrm{s}\alpha \mathrm{s}\theta \mathrm{c}\gamma -\mathrm{c}\alpha \mathrm{s}\gamma & \mathrm{s}\alpha \mathrm{s}\gamma +\mathrm{c}\alpha \mathrm{s}\theta \mathrm{c}\gamma \\ \mathrm{c}\theta \mathrm{s}\gamma & \mathrm{c}\alpha \mathrm{c}\gamma +\mathrm{s}\alpha \mathrm{s}\theta \mathrm{s}\gamma & \mathrm{c}\alpha \mathrm{s}\theta \mathrm{s}\gamma -\mathrm{s}\alpha \mathrm{c}\gamma \\ -\mathrm{s}\theta & \mathrm{s}\alpha \mathrm{c}\theta & \mathrm{c}\alpha \mathrm{c}\theta \end{array}\right]$ (2)

上式中s代表sin，c代表cos。

图1. 坐标系变换示意图

2.2. 无人机姿态解算

四旋翼无人机依靠四个桨的转动产生载体坐标系中z轴方向的升力，通过改变无人机的姿态即可改变升力的方向，从而控制四旋翼运动，因此四旋翼的姿态是其运动控制的关键因素。常用于无人机姿态测量的传感器是陀螺仪和加速度计，通过陀螺仪测量的角速率进行二次积分获得姿态角或在假设无人机运动加速度为零的情况下通过重力加速度的分解来计算姿态角。由于传感器测量误差，积分会有误差累积，无人机的加速运动会对姿态角测量产生影响等因素，采用EKF滤波融合二者数据可以获得更准确的姿态角。

多旋翼无人机姿态解算系统为非线性系统，其离散时间状态空间模型为：

$\{\begin{array}{l}{X}_k=f\left(X_{k-1}\right)+W_k\\ Y_k=H*X_k+V_k\end{array}$ (3)

式中 $X={\left[\alpha \theta \gamma \right]}^{\text{T}}$ 为无人机的姿态矩阵，Y为观测量，H为三阶单位矩阵，W和V分别为系统状态方程和观测方程的噪声向量。

$f\left(X_{k-1}\right)=\left[\begin{array}{c}{\alpha }_{k-1}+rx*T+\frac{\left(ry*\sin \alpha +rz*\cos \alpha \right)*T*\sin \theta }{\cos \theta }\\ {\theta }_{k-1}+ry*\cos \alpha *T-rz*\sin \alpha *T\\ {\gamma }_{k-1}+\frac{ry*\sin \alpha *T}{\cos \theta }+\frac{rz*\cos \alpha *T}{\cos \theta }\end{array}\right]$ (4)

上式中rx、ry和rz分别为载体坐标系下陀螺仪测量到的角速率，T为EKF解算周期。

扩展KALMAN滤波的迭代方程为 [9] [10] ：

$\{\begin{array}{l}{X}_{k/k-1}=f\left(X_{k-1}\right)\\ P_{k/k-1}=FI*P_{k/k-1}*F{I}^{\text{T}}+Q\\ K_k=P_{k/k-1}*H^{\text{T}}*{\left(H*P_{k/k-1}*H^{\text{T}}+R\right)}^{-1}\\ X_k=X_{k/k-1}+K_k*\left(Z_k-H*X_{k/k-1}\right)\\ P_k=P_{k/k-1}-K_k*H*P_{k/k-1}\end{array}$ (5)

Q是系统噪声误差阵，R是观测噪声误差阵，K为EKF滤波增益观测量Z = [accRollaccPitcompYaw]，accRoll和accPit为由加速度计数据解算的横滚角和俯仰角，compYaw为由磁力计数据解算的航向角， $f\left(X_{k-1}\right)$ 为系统状态方程，FI为系统状态方程系数泰勒级数的一阶近似。

$FI=\left[\begin{array}{ccc}1+\frac{\left(ry*\text{s}\alpha -rz*\text{s}\alpha \right)*\text{s}q*T}{c\theta }& \frac{rz*\text{s}\alpha *T}{\text{c}\theta *\text{c}\theta }& 0\\ -ry*\text{s}\alpha *T-rz*\text{c}\alpha *T& 1& 0\\ \frac{\left(ry*\text{c}\alpha -rz*\text{s}\alpha \right)*T}{\text{c}\theta }& \frac{\left(ry*\text{s}\alpha +rz*\text{c}\alpha \right)*\text{s}\theta *T}{\text{c}\theta *\text{c}\theta }& 1\end{array}\right]$

上式中s代表sin，c代表cos。

在忽略无人机的加速运动的情况下，无人机在地理坐标系下的加速度为 $a_n={\left[00g\right]}^{\text{T}}$ ，g为重力加速度，无人机在载体坐标系下的加速度为 $a_b={\left[a_x{a}_y{a}_z\right]}^{\text{T}}$ ，则 $a_b=C_n^b{a}_n\text{sin}\left(\alpha \right)$ 。

可以确定：

$\{\begin{array}{l}accPit=\arcsin \left(-\frac{a_x}{g}\right)\\ accRoll=\arctan \left(\frac{a_y}{a_z}\right)\end{array}$ (6)

无人机在地理坐标系下磁感应强度为 $m_n={\left[m_{nx}{m}_{ny}{m}_{nz}\right]}^{\text{T}}$ ，无人机在载体坐标系下磁感应强度为 $m_b={\left[m_x{m}_y{m}_z\right]}^{\text{T}}$ ， $m_n$ 和 $m_b$ 的关系为

$m_n=C_b^n{m}_b$ (7)

由上式可以得出

$compYaw=\arctan \left(\frac{m_y\cos \alpha -m_z\cos \alpha }{m_x\cos \theta +m_y\sin \theta \sin \alpha +m_z\sin \theta \cos \alpha }\right)$ (8)

2.3. 无人机姿态控制

采用具有角速率内环和姿态角外环的双环PD控制器，控制无人机的横滚角 $\alpha$ 和俯仰角 $\theta$ 。采用角速率PD控制器控制无人机的偏航角速率，控制频率为100 Hz，姿态角控制器框图如图2所示。

3. 无人机环绕目标飞行控制

3.1. 环绕目标识别

激光雷达固连在无人机上，可探测载体坐标系 $x_{b}o{y}_b$ 平面内的物体的距离和所处方位角。由于无人机在飞行过程中，机体有时是倾斜的，实际应用中，与无人机处于同一高度的水平面内的物体信息对于环绕目标飞行控制意义更大，因此有必要将载体坐标系的物体信息通过由机体姿态信息得到的坐标变换矩阵变换到水平面内，变换关系为 ${\left[x_h{y}_h{z}_h\right]}^{\text{T}}=C_b^h{\left[x_b{y}_{b}0\right]}^{\text{T}}$ 。

目标可以看作是由多个激光雷达探测点构成的集合，用 $OB{J}_i$ 表示第i个目标，包含n个激光雷达探测点的目标可表示为 $OB{J}_i=\left\{p_1,p_2,\cdots ,p_j,\cdots ,p_n\right\}$ ，其中 $p_j$ 为一个激光雷达探测点，包含距离 $r_j$ 和方位角 ${\gamma }_j$ 两个信息。由一个激光雷达探测点开始，根据目标扩充判定规则逐步添加临近的激光雷达探测点，最终获得独立的目标。如图3所示，有第n − 1和第n个相邻激光雷达探测点。

图2. 姿态角控制器框图

图3. 目标扩充判定规则示意图

同时考虑相邻探测点间夹角和探测点与激光雷达间距离两个因素将式(9)作为目标扩充判定规则 [11] ，若满足式(9)，则第n探测点相对于第n − 1探测点连续，目标集合可继续扩充，否则建立新的目标，其中FACD为连续体判定因数。

$\{\begin{array}{l}dDis=\left|r_n-r_{n-1}\right|\\ arcDis=\min \left(r_n,r_{n-1}\right)*\left|{\gamma }_n-{\gamma }_{n-1}\right|\\ dDis\le FACD*arcDis\end{array}$ (9)

其中r和 $\gamma$ 分别为激光雷达探测点的探测距离和方位角，下标表示探测点编号。

激光雷达进行距离探测时存在错误的距离探测信息，错误的距离探测信息可能会使一个目标被分割为两个目标，当相邻目标边界探测点间包含点数少于a个探测点(a为可选正数，例如a取3)，可用目标扩充判定规则对相邻目标边界进行判定，目标边界是指目标两端的激光雷达探测点，若满足目标扩充判定规则可以合并这两个目标。

为了在众多目标中确定环绕目标，环绕目标选取遵循以下三个限定条件：

1) 目标集合中所有探测点的平均距离avrD在[50, 250] cm范围内；

2) 目标集合中多数探测点位于机头方向正负50˚范围内；

3) 环绕目标集合包含的探测点数多余其他目标集合包含的探测点数。

3.2. 转差角和目标距离

转差角定义：将机头向着目标中心方向旋转，到达目标中心方向旋转过的角度，逆时针旋转的转差角为正，反之为负。

为了确定环绕目标的中心方向，若环绕目标左侧边界小于等于右侧边界，从右侧边界向内搜寻探测点作为新边界，将新边界与左侧边界比较，若新边界距离小于左侧边界距离或新边界与左侧边界夹角小于20˚，停止继续向内搜寻，将新边界作为环绕目标的新右侧边界。若环绕目标右侧边界小于左侧边界，从左侧边界向内搜寻探测点作为新边界，将新边界与右侧边界比较，若新边界距离小于右侧边界距离或新边界与右侧边界夹角小于20˚，停止继续向内搜寻，将新边界作为环绕目标的新左侧边界。

将棱柱形物体作为环绕目标，转差角 $\delta$ 如图4所示。

目标中心方向为：

${\gamma }_o=\left({\gamma }_l+{\gamma }_r\right)/2$ (10)

其中 ${\gamma }_o$ 为目标中心方向， ${\gamma }_l$ 为左侧边界方位角， ${\gamma }_r$ 为右侧边界方位角。

目标方向与机头方向的角度差即转差角为

${\phi }_d={\gamma }_o-{\gamma }_u$ (11)

其中 ${\phi }_d$ 为转差角， ${\gamma }_u$ 为机头在激光雷达探测区域对应方位角。

图4. 转差角示意图

无人机与环绕目标距离即目标距离为

$r_o=\left(r_1+\cdot \cdot \cdot +r_m+r_{m+1}+\cdot \cdot \cdot +r_{2m}\right)/\left(2*m\right)$ (12)

其中 $r_o$ 为目标距离， $r_1,r_2,\cdots ,r_m$ 为目标中心方向左侧与目标中心方向邻近的连续m个激光雷达探测点， $r_{m+1},r_{m+2},\cdots ,r_{2m}$ 为目标中心方向右侧与目标中心方向邻近的连续m个激光雷达探测点，m为可选正整数值。

3.3. 环绕控制

无人机环绕目标飞行，包括目标锁定控制和定距控制。目标锁定控制是指将转差角反馈到转向控制PD控制器，控制器输出量控制机头转向目标中心方向。定距控制是指在目标锁定的基础上将目标距离反馈到定距控制PD控制器，控制器的输出量用于控制无人机在设定的环绕距离上飞行。无人机环绕目标飞行控制框图如图5所示。

4. 实验与分析

4.1. 实验装置

实验使用的无人机为轴距550 mm的四旋翼无人机，控制系统主控制器采用支持硬件单精度浮点运算的Cortex-m4微控制器，工作频率为168 MHz。采用一体的MEMS三轴陀螺仪和三轴加速度计，数据输出频率为100 Hz，MEMS磁力计数据距输出频率为75 Hz。所用激光雷达可探测360˚范围内障碍物，有效探测距离为10~600 cm，探测频率为10 Hz，每秒探测2000个数据点。实验平台及实验环境如图6所示。

图5. 无人机环绕目标飞行控制框图 。(a) 目标锁定；(b)定距控制

图6. 实验装置及实验环境

4.2. 姿态角解算方法对比分析

采集传感器数据分别采用EKF滤波方法做多传感器数据融合和采用单一传感器计算姿态角对比如图7所示。图7(a)和图7(b)分别使用加速度计算姿态角(上图)，由于四旋翼无人机在飞行过程中机架高频振动和加速度计的噪声特性等因素导致加速度的测量有较大噪声，进而解算的姿态角也有较大噪声。采用加速度计数据和陀螺仪数据通过EKF融合计算姿态角(下图)，通过EKF数据融合后，横滚角和俯仰角的噪声明显减小。图7(c)中直接采用角速度积分计算偏航角(上图)，可以观察到随着时间增长偏航角积分误差累积越来越大。而采用磁力计数据计算的偏航角和角速度积分计算的偏航角通过EKF做数据融合(下图)，有效抑制了偏航角的误差累积。

4.3. 环绕飞行实验

实验中环绕目标为直径为80 cm的柱状物体，设定与目标距离为95 cm，环绕飞行一周的时间约为55 s，实验曲线如图8所示，整个环绕飞行中航向角即机头方向在[−180, 180]˚范围内变化，转差角在[−12, 8]˚范围内变化，达到了对环绕目标的锁定效果，在30~35 s内环绕速度过快，转差角出现了−12˚较大跟踪误差，实验中目标距离在设定距离正负10 cm范围内变化，完成了环绕目标飞行的控制。

图7. 单一传感器姿态解算和EKF融合对比。(a) 俯仰角；(b) 横滚角；(c) 偏航角

图8. 柱状物体环绕实验曲线

5. 结论

随着无人机产业的发展，无人机在工业领域的应用越来越广泛的应用，无人机作为一种新型的桥梁检测装备，受到了人们的关注。为了适应无人机对桥梁支座检测的需求，降低操控无人机的难度，提出了一种利用激光雷达解算的转差角和目标距离信息控制无人机环绕桥墩飞行的方法。通过柱状物体环绕实验验证了所提出环绕目标飞行控制方法的可行性。环绕飞行实验中无人机环绕速率不能过快且无人机与环绕目标间的距离存在小幅度的波动，激光雷达数据处理方法和环绕飞行的控制方法有待进一步改善。

文章引用

屈利伟,莫宏伟. 无人机环绕目标飞行控制技术研究
Research on Surround Flight Control of Unmanned Aerial Vehicle[J]. 人工智能与机器人研究, 2017, 06(04): 125-134. http://dx.doi.org/10.12677/airr.2017.64014

参考文献 (References)