基于路径像素分布统计导引的无人机自主循迹

冯宇，崔峰,2，高东,2

（1.国家空间科学中心，北京100000；2.中国科学院大学，北京100000）

0 引言

随着计算机视觉技术的快速发展，基于低成本视觉传感器的视觉导航方法在无人机导航和路径规划领域显示出极大的优势，被广泛研究和应用，尤其应用于全球定位系统（global positioning system，GPS）信号弱，甚至信号被阻断的场景中。文章研究在无路径先验信息的情况下，无人机基于视觉传感器沿指定路径进行循迹飞行的路径规划算法设计。循迹飞行的前提是识别图像路径。文章［1］提出根据高斯分布模型的自适应阈值分割方法，使用了基于形态学变换的二值图优化算法得到车道线的边缘图，改进概率霍夫变换，有效地检测出车道路。文章［2］主要针对自动驾驶过程中路径类型预判较慢且准确度不高的问题，提出了一种图像边沿特征提取算法。文章［3］基于改进的自适应遗传分割算法对图像进行阈值分割，使用霍夫变换的图像处理方法，并通过逆透视变换矫正路径的畸变，有效地提高了路径识别的精确度。

传统的路径规划算法，如A*算法［4］、人工势场法［5］等，需要提前载入环境信息。智能算法如遗传算法［6］、粒子群算法［7］等实时采集环境信息，根据载体在环境中的状态进行路径规划。这些算法用于在复杂环境中寻找最优路径来避开障碍，与循迹飞行的应用场景和目的略有不同。

循迹飞行的难点之一是拐角处的路径规划，检测出路径上的拐角是基于视觉信息进行导引的难点之一。关键拐角检测法之一是基于模板的角点检测，如Harris角点检测算法［8］、KLT角点检测算法［9］等。以上算法可以检测出所有符合条件的拐角，但具体拐角的结构分布还需要进一步分析。文章［10］建立了基于机载DSP对采集的图像进行连通域分割提取目标，进行结构矩匹配路径模板，进而解算位置偏差和运动方向指令的系统。

目前，基于视觉的无人机自主循迹路径规划算法的不足是，对于不同形状的拐角没有统一的直接选取目标位置的方法。受A*算法中环境建模的思想启发，对提取出路径的图像进行栅格化处理，并统计各栅格中路径像素和，从统计结果中分析出路径在图像中的分布信息，并确定局部路径规划中的目标位置坐标。根据无人机当前位置、姿态和目标点坐标信息，确定无人机的机动位置和转向指令。设置包含不同形状的路径环境，使用无人机进行算法测试。飞行结果表明，设计的路径规划算法对不同形状路径有适用性，能够满足无人机自主循迹飞行的需求。

1 基于视觉导引的无人机飞行控制

在基于视觉信息导引飞行的问题中，惯导器件等传感器获取无人机的位姿信息，视觉信息用于解算无人机与指定路径的相对位置信息，并根据该信息设计无人机的飞行路径。

图像中路径识别过程去除噪声和背景干扰，明确路径区域对应的坐标；提取路径分布信息是将图像中路径几何信息转换为数字信息，为路径规划提供输入；路径规划是确定飞行目标航迹点。

路径规划的结果是得到路径的点序列，将路径点序列坐标信息转换到导航坐标系下。定义导航坐标系为北东地（NED），原点定义为起飞前无人机重心位置。无人机本体系定义如图1所示，以重心为原点，oxb轴正方向沿机体纵轴指向机头，ozb轴正方向指向机体正下方。ozb、oxb和oyb轴构成右手法则［11-12］。

图1 无人机本体坐标系

多旋翼底层飞行控制分为四个层次，分别为位置控制、姿态控制、控制分配和电机控制。X型四旋翼的动力学方程为：

其中，P为无人机位置向量；V为无人机速度矢量；e为导航坐标系D轴正方向的单位向量；R是由无人机本体系到导航系的旋转矩阵；u是无人机旋翼产生的升力；Θ表示无人机的姿态角；W是机体姿态角变化率与机体角速度之间的转换矩阵；ω表示无人机三轴角速度；J表示无人机的转动惯量；Gτ表示陀螺力矩；τ表示三轴力矩。各向量分量如下式定义：

在滚转角和俯仰角较小的飞行姿态下，忽略高阶项，对动力学方程进行线性化，得到解耦的动力学方程。

水平位置动力学方程：

其中，φ，θ和ψ分别表示无人机的滚转角、俯仰角和偏航角。

高度动力学方程：

姿态动力学方程:

多旋翼飞行器是欠驱动系统［13］，有4个输入（u∈ℝ,τ∈ℝ3），6个输出（P∈ℝ3,Θ∈ℝ3），因此多旋翼只能跟4个指令（Pd∈ℝ3,ψd∈ℝ），剩余的指令（φd∈ℝ,θd∈ℝ）由期望指令Pd和ψd确定。因此，无人机机动的关键在于给出期望位置和期望偏航角，即从路径点坐标信息转换为控制指令。

图2 无人机级联控制

2 自主循迹算法设计

2.1 路径信息提取

无人机循迹飞行的前提是能够辨认出目标路径，视觉传感器采集的信息包含目标路径及背景干扰信息，在路径规划前需要从采集的原始图像中提取不含干扰的路径信息。

视觉传感器输出RGB格式的图像，基于像素统计的算法，关键在于将路径作为图像前景，使其和背景显式区分开，最优选择是将背景像素置为0，即转化为二值图像。

一般视觉传感器的主要噪声是椒盐噪声，采用效率较高的中值滤波来去除椒盐噪声［14］。对于一副灰度图像g(x,y)，令Sxy为中心在点(x,y)处，大小为m×n的矩形子图像窗口的中值滤波器，使用一个像素邻域中灰度的中值代替该像素的值，即：

采集的图像经过灰度化和中值滤波处理后，只含有较少的噪声。随后使用灰度阈值进行二值处理［15］，在图像中只显示主路径区域。

测试环境中主路径为红色，背景为绿色，如图3所示。对应的灰度图，背景的灰度大于路径的灰度，二值化结果为路径为黑色区域，背景为白色区域。为达到统计目标，对二值图进行补图处理。

图3 路径整体图和初步处理

在路径两侧加入干扰条纹保证视觉传感器的光流计算起作用，在二值后统计像素时是一大干扰，严重时会丢失有用路径信息，如图4所示。

图4 包含干扰的路径图及普通二值处理

去除干扰项的一种方法为直接提取主路径，根据实时光照条件下采集的图像像素点三通道矩阵的元素数值确定图像中主路径区域的RGB三通道阈值，设置同时满足3个阈值条件的像素点为前景，其余像素点为背景。对一幅RGB图像I(x,y)=(R,G,B)，(x,y)为像素点在数字图像坐标系中的坐标，R,G,B为对应三通道的值。主路径的像素点的三通道值满足如下条件：

创建同大小的单通道全零矩阵g(x,y)，在该矩阵中，原图像处于上述阈值的像素点对应的坐标(xp,yp)处的值为1，即g(xp,yp)=1，则新的g(x,y)矩阵即为待统计的图像矩阵。

第二种方法是基于图像灰度分布直方图［16］，但噪声像素点数量和主路经区域像素点数量有时难以区分，因此选用上文的去干扰的方法，如图5所示。

图5 去除干扰后的灰度图

2.2 路径分布统计和路径规划

经过2.1节的预处理，获取了实时局部路径信息，本节论述目标路径点选取方法。

在无法获取全局路径信息的前提下，提出一种在飞行过程中利用当前时刻采集的图像，进行局部路径规划，最终拼接为全局路径的方法。

在提取出路径区域后，将图像栅格化，统计路径像素在不同栅格中的分布情况，确定图像中的目标栅格，转换到实际运动场景中，作为目标路径点。

图像gh×w(x,y)有h行w列像素，对应像素最大横坐标值为w，最大纵坐标值为h。将图像均匀分为m×n个栅格区域，m,n均为大于1的整奇数，则每一栅格的高h0、宽w0为：

其中，h0,w0均为大于0的整数。由于gh×w的元素为0或1，用矩阵SZm×n的元素表示各个栅格中的像素的和。

按上述处理，若栅格(p,q)被背景覆盖，则对应SZm×n[(p,q)]=0，若栅格(r,s)被路径覆盖，则SZm×n(r,s)≈h0×w0，其余情形的值介于二者之间。

为了保持无人机跟踪的准确度和效率，使单个本体轴持续指向运动方向，因此在初始对准路径中间线后，大部分时间采集的图像两侧区域是背景。计算线状结构分布时，可以只关注图像中间区域，当图像两侧出现路径时，不影响对当前弯道的处理。具体处理方法为只对120×(51:110)区域的像素进行栅格化，两侧图像直接设置为背景。

将整幅图g120×160均匀划分为3×3个栅格，分块统计各块像素和，如图6所示。

图6 不同拐角处理和像素统计结果

使无人机oxb轴指向运动方向，则已经飞过的路径在栅格网中的区域是图像中心与下边缘中点连线经过的栅格，在后续选取时排除该栅格。小姿态角时，无人机在图像中的投影位于SZm×n((m+1)/2,(n+1)/2)处。选取SZm×n的其他元素中最大元素下标(mi,ni)对应的栅格作为目标栅格。当路径完整覆盖多个栅格，(mi,ni)对应多个元素下标，则取几个坐标的中心坐标作为(mi,ni)。将其转换到图片gh×w(x,y)中，则目标点在图像矩阵的下标(Y,X)计算为：

转换到导航坐标系的OXwYw平面内的坐标(Pwx,Pwy)为：

其中，Rwb为从oxbyb平面到OXwYw平面的旋转变换矩阵；Twb为本体系原点到导航系原点位移在OXwYw平面内投影向量；Rbp为从数字图像坐标系到本体坐标系oxbyb平面的旋转变换矩阵；k为像素与实际场景之间的尺度因子。实时处理得到一系列路径点序列Pxyz={(Pwxi,Pwyi,Z),i>1}，Z为飞行高度在导航系的对应坐标。

直线段飞行只需给出前进指令，控制的关键点在拐角处。无人机由进入拐角时的姿态机动为指向数字图像坐标系的点P(X,Y)，偏航角的变化Δψ为：

在没有大角度俯仰和滚转运动的情况下，本体系ozb轴与导航系OZw轴近似平行，因此本体系的偏航角的变化可视为在导航系中的偏航角变化。

由于上述目标路径点的计算是基于实时采集图像结果，因此局部路径飞行过程中，不能以式（11）的计算结果作为飞控输入。飞控指令分为两个阶段，第一阶段为改变偏航角：

式中，Pd,X,Pd,Y,Pd,Z,ψd为飞控的期望水平位置、期望高度和期望偏航角；Pt,X,Pt,Y,Pt,Z,ψt为无人机实时位置和偏航角。第二阶段进行位置调整，调整偏航角后，规划的目标路径点坐标与无人机实时位置差值在无人机本体系中表示为：

在导航系中的期望状态为：

式中，Rwb为第一阶段改变偏航角后，本体系到导航系的旋转矩阵。

由于硬件系统执行指令的同时在采集图像，新图像的处理结果生成新的指令，如果一次指令对应的机动，如偏航角，变化比较大，则可能造成指令叠加，因此采用步进指令调整无人机位姿，例如每次改变0.03弧度。

3 飞行试验验证

使用的无人机平台为parrot mambo，底部搭载120×160分辨率、60FPS的相机、超声波传感器和惯性测量单元。

无人机上并未搭载GPS接收装置。如图7所示，为无人机的外部结构。

图7 Parrot mambo外部结构

主路径设置为如图8所示。

图8 主路径形状

导航坐标系为NED。基于提出的算法，驱动无人机沿路径飞行。无人机飞行轨迹如图9所示。

图9 无人机飞行轨迹

图10 导航系X轴运动状态和指令

图11 导航系Y轴运动和指令

图12 无人机的偏航角状态和指令

指令只在本体系oxb和偏航角上有偏移量，但最终控制量转换到参考坐标系中，作为期望位置和期望偏航角。oxb轴正方向始终指向前进方向。在仿真和飞行测试中，提出的算法满足了循迹飞行的需要，但还面临在实际飞行过程中，硬件步进式地执行位置偏置和偏航角指令，带来的效率较低的问题。

结果分析

（1）在起飞阶段设置起飞至距离地面1.1 m高，循迹飞行过程保持定高，算法在不同形状的路段都能选取对应的目标路径序列点。

（2）在直道飞行过程中，按算法中的控制指令设置，偏航角期望值和估计值保持一致。

（3）在本体轴oxb上的指令经过旋转矩阵转换到导航系中，在二维平面上运动。无人机状态滞后于指令的状态。

（4）在弧度拐角处，由于给出线性的分布，无人机飞行轨迹呈现较为尖锐的拐角，在锐角拐角处，能够较为平滑地偏转和运动。

（5）在实际机动过程中，设置偏航角期望值和位置偏置命令频率较高，幅度较小，增大了机动需要消耗的时间。

4 结语

为了达到快速精准循迹飞行的目的，本文研究了搭载视觉传感器的无人机实时采集的环境图像，对图像灰度化处理，去除背景中的干扰，提取出路径区域，将预处理图像栅格化，分别统计不同栅格的像素和。为了保证跟踪效率，避免视野外围的路径对当前位置规划的影响，只对图像中间部分的像素进行统计，根据统计结果确定路径序列点。路径点选取在直线、弧线段和不同拐角处都适用。为了保证跟踪效率，在飞行规程中，结合路径点的相对位置关系和无人机实时姿态，设置对应的位移和偏航角指令，无人机本体