基于地标的无人机仓库环境导航方法研究

汉鑫，刘云清，董岩，颜飞，张华鑫

（长春理工大学 电子信息工程学院，长春 130022）

微小型无人机的导航技术包括位置估计和位姿控制问题，是无人机完成任务与应用的关键技术。近年来四旋翼飞行器的室内导航技术得到了广泛的研究和关注，文献［1-3］研究了单目相机的定位与导航问题，单目视觉技术的难点在于无法得到绝对尺度信息，通常需要声呐传感器或事先的测量标定来实现尺度估计。文献［4］利用了双目相机与卡尔曼滤波算法实现自主导航，双目视觉技术利用两套图像传感器在提取特征点信息的同时对目标进行尺度信息的估算，具备实时处理未知特征点的优点，由于计算时延较大，其使用的嵌入式图像处理硬件有较高规格。文献［5］利用运动捕捉系统对飞行器进行实时位姿检测，运动捕捉系统依托环绕场景布置的运动捕捉相机对被测物体上反光特征点进行位置解算，适用于开阔的室内环境。文献［6-7］设计了结合光流及地标的室内导航方法，光流算法利用地面纹理特征估计航速，地面纹理及环境光照是影响估计精度的重要因素。文献［8］提出一种结合激光测距仪与单目视觉的室内导航方法，激光雷达地图构建精细，需要多种算法共同协作。针对存在磁场干扰的环境，文献［9］利用磁向计与GPS传感器组合导航，通过计算实际与预定路线的航向角偏差量来实现室外环境导航。文献［10］通过激光雷达传感器与电磁场检测传感器设计了电力巡线场景中的避障与导航方法。文献［11］设计了跟踪地面路径的导航系统以完成在工厂内部的巡视任务。

综上所述，针对四旋翼无人机在仓库环境中结合地标与进行视觉伺服进行导航设计的研究较少，在室内导航的研究中大多没有考虑到环境中磁场干扰对导航精度的影响。本文针对以上问题及无人机的成本等因素，提出一种通过地标表示仓库立体环境，使用视觉伺服控制技术实现四旋翼飞行器在仓库货架环境的导航方法，并利用卡尔曼滤波算法解决了在仓库环境中磁场对无人机航向角控制的干扰问题。

1 基于地标的单目相机定位

为了实现飞行器在仓库环境中对自身的精确定位，本文设计了两种地标作为飞行器定位的指引。在货架侧方粘贴色块组合标志，用于表示货物的存储位置，提供给飞行器相对货架的位置信息与横向距离信息，在地面辅设了用于飞行器循迹与校准方向的直线标志，地面直线标志铺设在货架过道的正中央，标志颜色与地面颜色有较大反差即可。本文设计的导航方法如图1所示。

图1 组合地标导航示意图

地标图像的特征信息需要被转换成飞行器的控制量，首先提取两种地标各自的图像特征，然后结合应用场景案建立仓库导航环境的相机坐标系、机体坐标系、世界坐标系，最后通过相机的外参进行地标特征信息的坐标变换得到可供飞行控制器直接使用的导航信息。

1.1 地标的图像特征提取

本文利用单目相机模块对地标进行特征解算。飞行器侧面安置的图像模块解析色块地标，在飞行器下方安置的图像模块用于解析地面的直线地标。其中色块识别基于色调过滤算法［12］，对可精确识别的七种颜色进行编码，使视觉模块可按顺序输出色块地标的颜色编码S和组合色块整体的角度信息φI，如图2所示。对相机进行标定，根据已知的色块大小与像素坐标间的关系得到图像的绝对尺度信息。

图2 色块标志的编码处理

色块组合地标的图像处理结果为识别的颜色组合值、角度值、色块中心点的图像平面坐标、色块宽度所占像素数及高度像素数。对直线地标的图像处理结果为图像平面中矢量的线段端点坐标。接下来会利用两个模块得到的信息进行组合地标的位置解算。

1.2 地标特征的位置解算

在得到相机坐标系下的位置特征信息后，还需要建立相关坐标系，将飞控订阅的姿态数据用作图像模块的外参，通过坐标系之间的关系将图像的特征信息转换成世界坐标系下的位置偏差量以作为飞行器控制量。飞行器各个坐标系间的相对关系图如图3所示。

图3 飞行器各坐标系转换关系图

对色块坐标进行位置解算，在相机坐标系c1下的色块中心点坐标转换到机体坐标系坐标：

式中，ax、ay、az为相机坐标系c1原点指向机体坐标系原点的向量a沿机体坐标系各个轴的分解，向量a由图像模块的安装位置决定，Rot表示关于x、y、z轴旋转的基本齐次变换矩阵，Trans表示关于x、y、z轴平移的基本齐次变换矩阵。

对地面直线坐标进行位置解算，相机坐标系c2下的直线端点坐标、转换为机体坐标系下的LB1、LB2坐标：

式中，向量b为相机坐标系c2原点指向机体坐标系原点的向量，由图像模块的安装位置决定。

利用飞行器不同姿态下图像深度值与飞行器实际高度值之间的几何关系，Zc2表达式为：

式中，hI为飞行控制器通过气压计获取的飞行器当前高度信息。θ、φ分别为飞行器相对世界坐标系的俯仰角与横滚角。

根据仓库环境特点将飞行器所在的世界坐标系w的原点ow设置为飞行器质心在仓库过道的投影点，其zw轴穿过投影点并与地面垂直且正方向朝下，xw轴正方向与飞行器前进方向相同且平行于地面的直线标志，yw轴与前两轴正交且满足右手定则。现将直线端点在机体坐标系下的坐标LB1、LB2转换到机体水平坐标系w1，其xw1、yw1轴为机体坐标系中xB、yB轴在过oB的水平面的投影且正方向相同，坐标系原点ow1位于飞行器质心：

转换后的线段端点坐标Lw1(xw1,yw1,zw1)、Lw2(xw2,yw2,zw2)所构成的矢量平行于世界坐标系的xwowyw平面，利用其坐标值对航向角的观测值Ψg进行表示：

其中，Ψc为磁向计测得的地面直线地标正方向所指的真实角度值。

由于仓库内货架等金属物品带来的磁场干扰非常严重，所以本文将航向角的观测值Ψg与飞行控制器中IMU模块输出的航向角ΨIMU进行了卡尔曼滤波融合，以得到经过滤波后的航向角Ψf。随后再将融合后的航向角Ψf用于色块中心点坐标的世界坐标值转换中，得到坐标：

在获得色块中心点在世界坐标系的坐标Pw(xw,yw,zw)后，可解算飞行器相对色块标志的准确位置，进一步得出位置控制量，即飞行器投影点距地面直线的距离△L及飞行器距色块的距离△X、△Y：

式中wL为已知的直线距货架的距离。

2 基于卡尔曼滤波算法的航向角修正算法

位置解算的准确度依赖于飞行器姿态角的准确性，而在仓库环境下的货架、货物等工业制品存在较大的磁场干扰，会导致飞行器的航向角的测定出现偏差，使位置解算产生误差。本设计利用卡尔曼滤波算法对飞行器的航向角进行修正，将根据地面直线地标解算出的航向角观测信息与通过飞控数据预测的航向角进行卡尔曼滤波处理。

首先根据预估航向角与陀螺仪角速度ωgyro存在的微分关系对航向角进行状态空间描述，并根据采样周期dt对状态方程与输出方程进行离散化处理。航向角Ψ作为状态向量，Ψg作为观测向量表示如下：

式中，εm、εg分别为陀螺仪的噪声与航向角观测的噪声。

k+1时刻的飞行器航向角可由k时刻的最优航向角与k时刻的角速度进行积分来预估，且不存在控制量，即当前时刻航向角的估计为：

k+1时刻的预估航向角Ψ(k+1|k)对应的噪声协方差为P(k+1| k)：

式中，Q为过程噪声的协方差，P(|k k)为上一时刻最优状态估计的协方差。

根据式（10）、（11）得到当前时刻的卡尔曼增益Kg为：

式中，R为测量噪声的协方差。

通过上一节得到的当前时刻航向角观测值Ψg与预估值Ψ(k+1|k)来计算当前时航向角的最优估计Ψ(k+1|k+1)：

更新当前时刻最优估计航向角的协方差P(k+1|k+1)：

式（12）、（13）、（14）为滤波过程的修正阶段，在飞行过程中不断对预测阶段与修正阶段做递归运算，即可得到航向角在当前时刻的最优估计值。随后将经过滤波处理后得到的最优航向角估计值Ψf与地面直线的航向角做差得到航向角偏差控制量△Ψ：

3 控制逻辑设计

飞行器在室外环境通常依靠GPS与IMU进行速度信息的融合估计，在室内环境下的常用方法则是使用光流法等方式进行速度估计，考虑到系统的复杂性及本设计中飞行控制器的控制方式，本文利用飞控自身的数据进行速度信息的获取，最后采用了基于小步长的位置控制方法作为系统的控制方式。

无人机在仓库环境下导航的流程图如图4所示，主要基于循线、对准色块、执行任务、再次循线、再次对准的流程。其中飞行器在循线的过程中在保证了自身距离货架的安全距离同时使模块采集的图像平面与货架平面接近平行以避免较大的图像畸变，而自主对准色块的环节在保证了下一阶段中位置控制的准确度的同时也使此方案具备一定的可拓展性，如进行仓库货物管理等任务。

图4 基于地标与单目视觉的导航与控制框架

最后对解算出的位置控制量进行小步长分解，使飞行器能更快地调整位姿，数据时戳的控制更加灵活。小步长控制量的实现不仅需要对API控制量进行分解，而且需要通过调整PID参数改变飞行器响应控制量的感度，使飞行器能更高频率地达到平衡状态以保证距货架的安全距离及图像处理的准确性。在每个控制周期内，方向控制器（heading）、高度控制器（altitude）、位置控制器（position）以不同频率不断执行，以减小控制量间的耦合，其控制频率间的关系：

4 实验结果和分析

4.1 实验环境

本设计采用“单片机控制端+开源飞行控制器”的硬件组成方式。其中飞行控制器为大疆创新公司的N3飞控。控制端的单片机为英飞凌公司的XMC4700。控制端单片机驱动各部分的底层通讯，融合图像模块与N3的姿态数据，将位置特征信息解算为飞行器的位置偏差量，依据控制逻辑解算控制量API，实现对四旋翼飞行器的导航控制。图5为导航试验平台，图6为正在模拟执行任务的飞行器。

图5 四旋翼飞行器导航实验平台

图6 实验中的四旋翼飞行器

本设计使用Pixy(CMUcam5)开源图像处理模块对地标进行图像处理。Pixy1代与2代模块都具有识别色块编码的功能，其中Pixy2代模块相比Pixy1代模块新增了对“直线”形状的快速检测算法。在飞行器侧面布置一个Pixy1代模块用于采集与分析色块组合图像信息，在飞行器下方布置一个Pixy2代模块用于处理地面直线标志信息。

4.2 实验结果分析

在模拟仓库货架过道的环境中进行飞行实验，图7为飞行过程中N3飞控输出的高度数据，其中飞行器首先在第一层货架执行了色块对准任务，在对准完成后保持了稳定的高度值，随后根据导航指令进行高度爬升，在到达预定高度后开始对准第二层货架上的色块，再下降高度，最后执行循线任务向第一层的下一个色块地标移动.在此过程中飞行器的高度控制稳定。图8为在相同时间段N3飞控输出的航向角数据，由图中数据可以看出在执行对第一层货架色块的对准任务时航向角控制稳定，在爬升高度时受飞行姿态影响，航向角控制出现轻微扰动，在下降回第一层货架后，飞行器在执行循线任务过程中受到磁场干扰，但在滤波补偿后恢复了对航向角的稳定控制。通过对系统模块的多项测试及实验结果分析，系统在3m～6m宽的模拟货架过道环境内达到了稳定的控制效果。

图7 系统输出的高度数据曲线

图8 系统输出的航向角数据曲线

5 结论

本文以四旋翼飞行器为研究对象，研究了适用于仓库货架环境的四旋翼飞行器室内导航方案。针对货架环境中存在磁场干扰的问题，提出了融合视觉和惯导信息对航向角进行卡尔曼滤波的算法，使依赖航向角进行解算的位置估计值与导航控制量更加准确。通过设计控制器保证了飞行器在仓库过道环境中的导航精确度。为说明导航方案的有效性进行了飞行实验，实验结果表明了稳定的控制效果。