发明内容
(一)解决的技术问题
针对现有技术的不足,本发明提供了结合无人机高精度定位和视觉跟踪技术的配网巡检方法,具备实现无人机自主导航飞行提高巡检效率的优点,进而解决上述背景中提出的问题。
(二)技术方案
为实现上述实现无人机自主导航飞行提高巡检效率的优点,本发明采用的具体技术方案如下:
结合无人机高精度定位和视觉跟踪技术的配网巡检方法,该方法包括以下步骤:
S1、基于多传感器数据融合实现无人机的高精度定位;
S2、基于改进区域建议网络和透视模型实现无人机载前端目标跟踪与智能识别。
进一步的,所述基于多传感器数据融合实现无人机的高精度定位,包括以下步骤:
S11、利用载波相位差分定位提高GPS观测值精度,并通过椭球坐标转换的七参数法将所述实时位置信息转换为当地的坐标系统,获得精确的定位信息;
S12、将所述定位信息融入到基于视觉信息的即时定位与地图构建算法中;
S13、利用扩展卡尔曼滤波器实现即时定位与地图构建算法与无人机自主导航系统的融合,并将滤波器输出的位姿信息反馈到无人机飞行控制终端。
进一步的,所述载波相位差分定位由基准站与流动站组成,所述利用载波相位差分定位提高GPS采集到无人机实时位置信息精度,包括以下步骤:
S111、所述基准站通过GPS接收机接收实时位置信息;
S112、通过计算求出GPS实时相位差分修正值;
S113、将所述修正值通过无线电传递至所述流动站;
S114、所述流动站同时接收卫星信号,并完成对卫星信号的解算求出定位信息。
进一步的,所述基于视觉信息的即时定位与地图构建算法采用特征提取与检测-即时定位与地图构建算法作为核心,所述特征提取与检测-即时定位与地图构建算法的实现包括以下步骤:
S121、通过相机提取无人机所拍摄图像的特征并进行匹配,跟踪每一帧相机,对无人机进行位姿估计与优化;
S122、通过插入新的关键帧及移除老的关键帧实现局部地图的构建;
S123、检测无人机拍摄新帧是否存在闭环,并进行位姿图优化。
进一步的,利用扩展卡尔曼滤波器实现即时定位与地图构建算法与无人机自主导航系统的融合,包括以下步骤:
S131、基于扩展卡尔曼滤波器原理设计简单模型,并进行测试;
S132、建立无人机的系统动力学模型和扩展卡尔曼滤波器的软件架构;
S133、根据简单模型的测试结果对扩展卡尔曼滤波器的参数进行调整,保证测试系统稳定运行;
S134、建立基于扩展卡尔曼滤波器的机器人操作系统程序架构。
进一步的,所述扩展卡尔曼滤波器由卡尔曼滤波器经过线性化得出,其计算过程如下:
Xk=fk-1(Xk-1,uk-1)+Wk-1;
Zk=Hk(Xk,uk)+Vk;
其中,k表示时刻,fk-1(Xk-1,uk-1)是系统k时刻的系统状态向量,uk-1是系统k-1时刻的系统状态方程,uk-1是k-1时刻的控制输入,Wk-1为过程激励噪声,Zk是k时刻的观测真值,Hk是系统k时刻的观测量矩阵,Vk是测量时引入的噪声。
进一步的,所述改进区域建议网络为区域建议网络通过在深度学习区域卷积神经网络(R-CNN)目标检测算法的基础上根据交并比注意力机制进行改进实现,所述透视模型包括输电塔及输电线。
进一步的,所述基于改进区域建议网络和透视模型实现无人机载前端目标跟踪与智能识别,包括以下步骤:
S21、利用改进区域建议网络准确定位输电塔,并通过核相关滤波器实现对杆塔的连续视觉跟踪;
S22、采用全卷积网络从复杂场景中分离出输电线,并计算得到消失点将其作为飞行航向的重要依据。
进一步的,所述交并比注意力机制使用建议框和真实框的交并比分数评价所述区域建议网络中锚点框的关注程度,其公式如下:
score(i,j,c)=max(IOU(proposali,j,c,gtk)),
k=1,2,…N;
其中,A,B分别代表两个图像区域的集合,N为每幅图像的真实框数量,proposal、gt分别为建议框和真实框,score(i,j,c)是特征图在(i,j,c)处相应建议框和全部真实框之间最大的交并比分数。
进一步的,所述透视模型采用所述区域卷积神经网络和所述核相关滤波器相结合的视觉策略跟踪算法。
(三)有益效果
与现有技术相比,本发明提供了结合无人机高精度定位和视觉跟踪技术的配网巡检方法,具备以下有益效果:本发明首先引入位置信息融合即时定位与地图构建算法,提高无人机的定位精度与导航能力。其次利用扩展卡尔曼滤波器算法将多传感器数据融合,将滤波器输出的位姿信息反馈到无人机飞行控制终端,以此调整飞行控制方式与飞行机制,做到准确避障,实现自主飞行;然后利用改进的区域建议网络准确定位输电塔,并通过核相关滤波器实现对杆塔的连续视觉跟踪,避免无人机自主巡检过程中面临的障碍物集中、视线较差的环境下难以做到合理避障的问题。
具体实施方式
为进一步说明各实施例,本发明提供有附图,这些附图为本发明揭露内容的一部分,其主要用以说明实施例,并可配合说明书的相关描述来解释实施例的运作原理,配合参考这些内容,本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点,图中的组件并未按比例绘制,而类似的组件符号通常用来表示类似的组件。
根据本发明的实施例,提供了结合无人机高精度定位和视觉跟踪技术的配网巡检方法。
现结合附图和具体实施方式对本发明进一步说明,如图1-14所示,根据本发明实施例的结合无人机高精度定位和视觉跟踪技术的配网巡检方法,该方法包括以下步骤:
S1、基于多传感器数据融合实现无人机的高精度定位;
S2、基于改进区域建议网络和透视模型实现无人机载前端目标跟踪与智能识别。
在一个实施例中,所述基于多传感器数据融合实现无人机的高精度定位,包括以下步骤:
S11、利用载波相位差分定位(RTK)提高GPS观测值精度,并通过椭球坐标转换的七参数法将所述实时位置信息转换为当地的坐标系统,获得精确的定位信息;
S12、将所述定位信息融入到基于视觉信息的即时定位与地图构建算法中;
S13、利用扩展卡尔曼滤波器实现即时定位与地图构建算法与无人机自主导航系统的融合,并将滤波器输出的位姿信息反馈到无人机飞行控制终端。
在一个实施例中,所述载波相位差分定位由基准站与流动站组成,所述利用载波相位差分定位提高GPS采集到无人机实时位置信息精度,包括以下步骤:
S111、所述基准站通过GPS接收机接收实时位置信息;
S112、通过计算求出GPS实时相位差分修正值;
S113、将所述修正值通过无线电传递至所述流动站;
S114、所述流动站同时接收卫星信号,并完成对卫星信号的解算求出定位信息。
其中,全球定位系统(GPS):作为实时卫星定位系统,可快速为用户提供经纬度和高度实时位置信息,提供精度较高的导航服务。导航系统主要分为三部分:太空中的导航卫星,地面的卫星控制站,以及用户接收机。卫星控制站负责收集数据,发送指令到太空中卫星。轨道上多颗导航卫星向地面发射无线电信号,用户接收机接收信号并对信号中数据进行解调,计算获得导航与位置信息。描述接收机位置信息需要以地球坐标系为参考系,地球参考系分为直角坐标系与大地坐标系。直角坐标系原点与地球质心重合,z轴指向北极,x轴指向地球0经度方向(即赤道面与格林威治子午圈角点),y轴指向东经90度。本发明采用的计算方法基于直角坐标系。
假设待求用户接收机坐标为,其与轨道上四颗卫星进行通信,可以确定以下四个方程。
[(x1-x)2+(y1-y)2+(z1-z)2]1/2+c(νt1-vt0)=d1;
[(x2-x)2+(y2-y)2+(z2-z)2]1/2+c(vt2-vt0)=d2;
[(x3-x)2+(y3-y)2+(z3-z)2]1/2+c(vt3-vt0)=d3;
[(x4-x)2+(y4-y)2+(z4-z)2]1/2+c(vt4-vt0)=d4;
(xi,yi,zi)(i=1,2,3,4)代表四颗卫星的坐标,di表示卫星与接收机之间的距离,vti(i=1,2,3,4)为卫星的钟差,vt0为用户接收机的钟差,联立四个方程求解出用户接收机坐标。
椭球坐标转换的七参数法:在RTK系统中,GPS使用WGS-84坐标系,我国实际测绘场景中使用的是BJ-54坐标系,需要将WGS-84坐标转换为当地的坐标系统,RTK系统的计算步骤如下:
本发明选择椭球坐标转换的七参数法进行坐标转换,WGS-84坐标系以地球质心为原点,BJ-54以椭球中心为原点,对坐标系进行平移,设各个方向上的平移量分别为Δx,Δy,Δz。除了原点不同,两个坐标系的坐标轴方向也不一样,对坐标轴进行旋转,设各个坐标轴旋转量分别为ωx,ωy,ωz,转换公式如下所示:
上式中m为尺度因子,转换坐标系后,进行载波差分信号的计算。定义基准站b与卫星s的载波相位信号差值
计算公式如下所示:
是系统初始模糊度,
是t时刻到初始时刻t
0的变化值。
基站收到信号,计算波长与相位差值的乘积,得到理想距离。该距离不考虑信号传输过程中的干扰,称为“伪距”
基准站相对于伪距的修正值
如下公式所示:
为实际距离,δM
b为多路径效应,V
b为接收机噪声。用该修正值对用户接收机进行修正:
假设基准站与用户接收机之间的距离小于30米,则可认为
设ΔδL=c(δt
u-δt
b)+(δM
u-δM
b)+(V
u-V
b),则:
上式中,(xu,yu,zu)为固定值坐标,Es(t0)为常数,ΔδL在相邻历元之间的变化量小于厘米级允许的误差,因此也可以作为常数计算。根据以上公式,在基准站与用户接收机同时与同样的四颗卫星通信时,可以经过上述计算得到移动站修正后的位置信息。
在一个实施例中,所述基于视觉信息的即时定位与地图构建算法采用特征提取与检测-即时定位与地图构建算法作为核心,所述特征提取与检测-即时定位与地图构建算法的实现包括以下步骤:
S121、通过相机提取无人机所拍摄图像的特征并进行匹配,跟踪每一帧相机,对无人机进行位姿估计与优化;
S122、通过插入新的关键帧及移除老的关键帧实现局部地图的构建;
S123、检测无人机拍摄新帧是否存在闭环,并进行位姿图优化。
其中,即时定位与地图构建技术:即时定位与地图构建技术是指机器人在未知的环境中,利用其搭载的传感器建立地图,进行定位与导航。目前用于SLAM(即时定位与地图构建技术)的传感器主要有视觉传感器、激光雷达、惯性测量单元IMU和超声波传感器等。随着图像处理技术的发展,基于视觉信息的SLAM成为目前无人车、无人机自动导航的主流。本发明是基于视觉的SLAM算法。
视觉SLAM常用的算法有ORB(Oriented Fast and Brief)-SLAM,ORB-SLAM是基于稀疏特征点的视觉SLAM系统,其核心是提取图像中的ORB作为系统主要特征,ORB-SLAM系统框架主要由跟踪、局部图像构造和闭环检测三部分构成。其在PTAM算法框架上,改进了框架中的组件,主要有四点:
a.ORB-SLAM主要基于ORB图像特征进行匹配和定位,ORB特征具有更好的尺度不变性与旋转不变性,因此匹配的效率也更高。
b.加入了闭环检测机制,能消除系统中的累计误差。
c.需要具有一定公共点和平移量的前后两帧初始化系统。
d.相比PTAM严格筛选新加入的关键帧的机制,ORB-SLAM选择前期加入满足一定条件尽可能多的点,后期进行判断,删除多余的地图点与关键帧。
跟踪是SLAM系统中的主进程,通过相机提取图像特征并进行匹配,跟踪每一帧相机,对无人机进行位姿估计与优化。通过新的关键帧(云点)插入与移除老的关键帧(云点)构建局部地图。闭环检测则是检测无人机拍摄新帧是否存在闭环,即是否经过同一个地方,再进行位姿图优化,这三部分并行完成任务。
①跟踪:特征提取是跟踪中的重要部分。目前比较经典的图像特征主要有Harris、SIFT、SURF、FAST和ORB。与SURF、SIFT等图像特征相比较,ORB特征运算量较少,可通过CPU计算获得等同于GPU的特征效果。并且ORB特征具有尺度不变性与旋转不变性。因此在SLAM需要实时信息来计算的情况下,常用ORB作为图像特征。ORB角点提取流程主要如下:
构建层数为N的特征金字塔,根据金字塔层数的增加获取图像特征数量减少。
对每层图像特征分配采用如下策略。设金字塔底层图像像素宽度为W,高度为H,尺度因子为s,第n层金字塔的面积S为:
S=W×H×(s2)n-1;
N层金字塔的总面积为:
S=W×H×(s2)0+W×H×(s2)1+…+W×H×(s2)n-1;
设提取特征点数为N,则单位面积提取的特征点数为
每一层分配的特征点数为
本发明中ORB-SLAM设定提取的特征金字塔高度为8,尺度因子为1.2。提取ORB特征的流程主要如下:
提取FAST角点,设置阈值T,以待确定的像素点p(像素值Ip)为圆心,绘制半径为3像素的圆。如果圆周上n个点的像素值大于Ip+T或者小于Ip-T,则确定该点为角点。
计算rBRIEF描述子,以角点为中心,选取31×31的图像邻域,每个像素使用其领域中5×5的平均灰度值代替,因此对应获得n=(31-5+1)×(31-5+1)个对比的像素块。像素块两两之间进行比较,获得长度为m=C(N,2)的二进制编码。根据均方差最大原理,筛选获得256对,即为rBRIEF描述子。提取特征后,需要进行算法初始化,对前后两帧图像进行特征匹配。当前若匹配特征点数较少,则认为跟踪丢失,此时算法会进行重定位,寻找新的相邻图像帧进行初始化。初始化成功后,以前一帧图像为原点建立坐标系。在运动过程中,将从当前图像帧提取的图像特征与原点匹配,获得后一帧图像的旋转角度与位移量。通过多视角几何算法,可以解算出当前无人机的位姿。同时还以三角测量获得的三维坐标设为地图的初始点,并使用全集束调整对位姿和地图点进行优化。
②局部地图构建:在获取初始位姿与特征后,在处理新一帧的图像时,要将其特征投影到地图中进行匹配。全局地图包含了SLAM运行过程中所产生的所有特征点,若与全局地图做匹配则计算量会非常庞大,所以需要局部地图(其仅包含了距离当前拍摄图像较近的地图点)。局部地图构建主要是在跟踪过程中插入新的关键帧,并对局部地图进行局部BA优化。判断当前帧是否为关键帧,若是关键帧,则插入局部地图。关键帧筛选主要有一下几个原则:
a.关键帧之间要间隔20帧以上;
b.当前帧与其他关键帧的匹配点数要达到50以上;
c.当前帧与关键帧的共视点达到90%以上;
插入关键帧到地图中还需要剔除部分地图点。地图点是通过相邻的关键帧多视角几何计算得到的,保留地图点不被剔除有如下的原则:
a.有三个以上关键帧可以检测到该点;
b.跟踪时,有四分之一的可视帧可以匹配到该点;
得到保留地图点的集合后,计算保留地图点的空间位置,将新产生的地图点添加到地图中,使用局部集束优化Local BA调整局部地图点和位姿,对相邻的关键帧中重复的地图点进行融合,然后调整当前处理的关键帧,检测去除冗余的关键帧。
③闭环检测:闭环检测是SLAM系统中重要的一部分。在SLAM运算过程中,通过上一刻观测值对当前时刻无人机位姿进行估计,在这个递推过程中,运动姿态估计误差会不断积累,因此需要闭环检测来消除。闭环检测是检测在运动过程中是否有经过同一个地方(相机拍摄过同一个地方),将检测到的闭环传给后端,可以调整无人机的路径规划。
在闭环检测中,需要计算相邻两帧图像之间的相似度,其主要通过词袋模型来实现。基于前端相机获取到图像ORB特征,词袋模型使用聚类方法与K叉树算法构建特征字典。在图像中,可以提取大量的特征点,在计算相似度过程中,则需要特征点构成的特征向量,因此需要特征字典。要构成维度为k的字典,则可以使用K均值(K-means)算法。流程如下:
a.随机选取k个中心点;
b.对每个特征点,计算其与中心点的距离,与其距离最小的则将其划入该中心点所属的类;
c.重新计算中心点;
d.如果中心点位置变化的误差小于一定范围内,则认为算法收敛,退出;或者设定迭代次数,完成迭代退出。
为了保证字典的搜索效率,使用K叉树构建字典。首先基于K-means算法得到k类中心,将其作为其第一层节点。对各类中的特征点再进行K-means聚类,中心点作为下一层节点。设定深度为d,则通过d轮迭代构建一颗分支数目为k的树。
构建特征字典后,计算图像A、B之间的相似度。相似度计算通过TF-IDF模型来完成,公式如下所示
TFi表示图像特征i在图像中出现的频率,对该值取对数,得到IDFi。
如果该值越低,则表明这个特征对图像的区分度越高,能够较好的区分出不同的图像。ωi的权重μi=TFi×IDFi,将多个特征组合,构成词包向量:
计算前后两帧图像之间的词包向量的距离,即可得到相似度。计算距离的方法有欧氏距离、汉明距离等。在本文中所使用的方法是L1范式距离,如下所示:
vAi表示只在图像A中出现的单词的向量,vBi表示只在图像B中出现的单词的向量,|vAi-vBi|表示在A、B中都含有的向量。当s大于设定阈值时,则认为前后两帧图像相似度较大,可能是闭环。
如果在环境变化程度不大的情况下,仅使用L1范式计算距离判断闭环会出现较大误差。因此需要设置先验相似度:
s(vt,vt-Δt)表示前后关键帧图像的相似性,若当前帧与某关键帧的相似度,大于当前帧与上一关键帧的三倍,则认为可能存在闭环。
在寻找闭环的过程中,如果跟踪丢失,可以使用重定位的方法恢复当前的姿态估计。
在一个实施例中,利用扩展卡尔曼滤波器实现即时定位与地图构建算法与无人机自主导航系统的融合,包括以下步骤:
S131、基于扩展卡尔曼滤波器原理设计简单模型,并进行测试;
S132、建立无人机的系统动力学模型和扩展卡尔曼滤波器的软件架构;
S133、根据简单模型的测试结果对扩展卡尔曼滤波器的参数进行调整,保证测试系统稳定运行;
S134、建立基于扩展卡尔曼滤波器的机器人操作系统程序架构。
在一个实施例中,所述扩展卡尔曼滤波器由卡尔曼滤波器经过线性化得出,其计算过程如下:
Xk=fk-1(Xk-1,uk-1)+Wk-1;
Zk=Hk(Xk,uk)+Vk;
其中,k表示时刻,fk-1(Xk-1,uk-1)是系统k时刻的系统状态向量,uk-1是系统k-1时刻的系统状态方程,uk-1是k-1时刻的控制输入,Wk-1为过程激励噪声,Zk是k时刻的观测真值,Hk是系统k时刻的观测量矩阵,Vk是测量时引入的噪声。
其中,基于多传感器的SLAM定位方案中,比较常用的是IMU与视觉传感器融合的方案。IMU能获取到无人机飞行过程中的姿态,以此来矫正视觉SLAM在运动过程中累积的定位偏差。目前常用扩展卡尔曼滤波方法来进行数据融合。
本发明主要构建与无人机对应的EKF运动学模型,以及其传感器获取数据应用于EKF的算法。
①基于RTK的传感器融合原理:
a.卡尔曼滤波
卡尔曼滤波是常用来做传感器数据融合的一种方法,其主要通过上一时刻的传感器获得的观测量,来进行下一时刻状态的最优估计。
在一个理想的环境下,把物体运动当作一个线性系统,可以由以下公式表示:
其中,xk是k时刻系统的状态向量,包括无人机该时刻的坐标信息与位姿信息,ωk是系统中的过程噪声,Hk是状态转换矩阵,将测量状态映射到系统中,vk是测量的噪声。
卡尔曼滤波通过不断的进行测量当前状态的信息来进行下一刻的预测。预测的流程如下:根据k时刻状态估计k+1时刻的状态;预测k+1时刻均方误差;预测k+1时刻测量向量。
更新状态的流程如下:对状态向量进行更新;更新当前时刻系统的测量残差;更新卡尔曼增益。
b.扩展卡尔曼滤波
卡尔曼滤波基于线性模型使用线性方程计算,而实际中物体的运动系统通常都是非线性的。这就需要扩展卡尔曼滤波算法。
假设当前运动系统的状态方程如以下公式表示。
Xk=fk-1(Xk-1,uk-1)+Wk-1;
Zk=Hk(Xk,uk)+Vk;
Xk是系统t时刻的系统状态向量,fk-1(Xk-1,uk-1)是系统t-1时刻的系统状态方程,Hk是系统k时刻的观测量矩阵,Vk是测量时引入的噪声。
EKF为了将非线性函数线性化,在(Xk,uk)处进行一阶泰勒展开
上述公式中Of(X-Xk)和Of(X-Xk)是大于等于二阶的泰勒展开级数,在计算中可以忽略。
根据上面扩展卡尔曼滤波器的介绍,可知其是建立在运动模型上计算的,并且由无人机的运动学模型决定滤波器的性能。本发明采用的无人机模型状态数量主要参考ROS的开发包,选用维度为15的状态向量。滤波器主要有四个输入模块,分别为IMU,RTK、气压计(测量高度)以及超声模块。RTK模块提供修正后的水平方向上位置信息,气压计提供高度信息,IMU提供姿态角信息。
其中X,Y,Z表示位置坐标信息,θ,Ψ,Φ分别表示姿态角中的俯仰角、偏航角、翻滚角。根据扩展卡尔曼滤波算法,在无人机SLAM中。将无人机定位坐标信息,其余传感器的数据构成状态向量的初始值和滤波器协方差矩阵以及噪声协方差。
设当前系统状态方程如公式
及
所示。当t时刻SLAM更新时,对应X,Y,Z与位姿信息都会更新。矩阵H表示为:
Hk=(I6×6 O6×9);
滤波器预测流程与标准的EKF一致:
下一刻的状态估计为:
对系统函数求导(计算雅克比矩阵):
下一时刻的均方误差预测结果为
最后的预测结果为
输出结果维度为6,即预测得到对下一时刻的位姿信息。
对测量向量进行更新:
对测量残差进行更新:
对测量残差协方差进行更新:
对卡尔曼增益进行更新:
对状态估计值进行更新:
对EKF方差进行更新:
②导航系统
a.系统流程
在实际情况下,如果应用SLAM于整个系统,计算量十分庞大。因此将相机作为视觉里程计,仅使用SLAM处理图像数据。处理后数据与传感器观测数据使用EKF算法进行融合,最终得到下一时刻的预测值(位姿信息),输出到无人机控制系统中。
控制系统根据当前位置与预测值的距离,以及姿态的差值判断是否在设定的阈值范围内,来决定飞行是否需要调整路线与飞行姿态。
b.系统动力学模型
本发明利用扩展卡尔曼滤波器算法将传统的无人机自主导航系统与SLAM相融合。本发明首先基于扩展卡尔曼滤波器原理设计简单模型,并进行测试;然后建立无人机的系统动力学模型和EKF的软件架构;之后根据简单模型的测试结果对EKF参数进行调整,保证测试系统稳定运行;最后建立基于扩展卡尔曼滤波器的机器人操作系统程序架构。
本发明利用robot_localization设计导航算法软件x、y、z是ENU三维位置坐标;
θ、ψ是roll、pitch、yaw 3个姿态角;滤波器包括惯性测量单元、常规GPS、高度计和视觉里程计(VO)4个输入模块;IMU由飞行控制器的陀螺仪、加速度计和地磁计组成,提供
对应信息;绝对位置传感器GPS采集水平方向位置信息,V
0采集水平位置、高度和姿态角数据,当V
0数据更新时,EKF融合的位置信息都作为其位置信息初值。
下式是非线性模型转移公式:
VP=APdt;
VE=VE;
VP=VP;
P(x,y,z)
T是位置向量,即
是姿态角向量,VP是位置速度;AP是加速度向量,VE是角速度向量,dt是计算间隔时间。
c.改进的扩展卡尔曼滤波器软件框架
本发明采用即来即算的扩展卡尔曼滤波器思路。标准卡尔曼滤波器框架中的H矩阵是测量方程h的雅克比矩阵,将其直接设置为单位阵。更新EKF阶段采用“部分计算”方法,融合多个频率相差较大的传感器数据,降低计算量,提高其适应性。具体方法为:在更新EKF调整H矩阵时,只计算传感器输出的状态变量相关量,若在某一时刻更新m(非0)个状态向量,设置m×12维H矩阵,m秩的矩阵。下列公式详细说明EKF算法。
设系统方程:
Xk=f(Xk-1)+Wk-1;
Zk=HkXk+Vk;
X
k表示控制系统时刻k的状态向量,Z
k表示控制系统时刻k的测量向量,f(X
k-1)是控制系统非线性方程,如式(44)~(48)所示,各状态向量用H矩阵代替,W
k-1表示控制系统噪声序列,V
k表示测量噪声。若ROS系统检测当前SLAM更新数据,位置坐标X、Y、Z,和姿态角
θ、ψ被更新。H矩阵为:
Hk=(I6×6 O6×9);
滤波器的预测阶段与标准EKF一致,状态估计一步预测:
均方误差一步预测:
预测测量向量时,只更新位置数据,所以采用6×15维的Hk矩阵获得6×1向量测量预测:
矩阵的维度用下标(6×15)表示。在更新阶段,所有参与计算的相关向量都是通过上述降维的Hk矩阵进行的降维处理。更新测量向量,即在Zk中输入VO最新输出的位置数据:
测量残差更新:
测量残差协方差更新:
卡尔曼增益更新:
状态估计值更新:
EKF方差更新:
在改进的扩展卡尔曼滤波器方法的更新阶段,卡尔曼增益等变量都降维到6维矩阵,可有效降低系统运算量。20Hz是扩展卡尔曼滤波器的普通运行频率,当只有IMU没有位姿数据时,按照20Hz频率更新,当GPS或VO等更新时扩展卡尔曼滤波器临时增加一帧计算。
d.扩展卡尔曼滤波器参数整定
扩展卡尔曼滤波器将动力学模型数据、滤波器协方差阵初值P0、过程噪声协方差矩阵Q和状态向量初值X0数据按照一定规则进行数据融合。
扩展卡尔曼滤波器迭代计算的第一帧是X0和P0初值,当X0和P0初值设置为零时,主要影响算法的收敛情况,不影响系统输出,本发明主要对过程噪声协方差的矩阵进行调整。Wk-1是衡量状态变量相对于系统模型的偏差程度的系统噪声序列,在扩展卡尔曼滤波器中,假设高斯白噪声是噪声序列,期望设置为O,Q矩阵代表协方差。扩展卡尔曼滤波器的均方误差矩阵直接受其影响,卡尔曼增益阵受其间接影响。状态转移矩阵和实际飞行过程的差别受过程噪声协方差阵衡量,估计结果和各传感器间的关系也受其衡量。
e.导航系统ROS程序架构
为解决机载硬件计算限制,将SLAM系统简化为视觉里程计,为提高计算速度,光束的平均差仅对帧间位姿优化进行计算,并且对其迭代次数进行限制。基于无人机机载ROS导航系统,ROS模块包括飞行控制器MAVROS主程序、EKF主程序、输入转换器和数据记录器4个部分。
在一个实施例中,所述改进区域建议网络为区域建议网络通过在深度学习区域卷积神经网络目标检测算法的基础上根据交并比注意力机制进行改进实现,所述透视模型包括输电塔及输电线。
在一个实施例中,所述基于改进区域建议网络和透视模型实现无人机载前端目标跟踪与智能识别,包括以下步骤:
S21、利用改进区域建议网络准确定位输电塔,并通过核相关滤波器实现对杆塔的连续视觉跟踪;
S22、采用全卷积网络从复杂场景中分离出输电线,并计算得到消失点将其作为飞行航向的重要依据。
其中,透视导航模型的巡检原理:根据投影理论空间平行线在无穷远处相交,相交点通常被称之为消失点VP,使用VP可以确定空间直线的方向,因此VP识别技术被广泛应用于道路检测和机器人导航。基于VP的深度学习道路分割方法也得到了显著发展,被成功应用于自动驾驶系统。无人机巡检方案首先创新性地引入透视关系导航模型,它由输电塔定位和输电线路提取两部分构成。为实现对输电塔的稳定实时定位,将采用快速R-CNN模型和核相关滤波器KCF相结合的视觉策略跟踪算法。此外为提取复杂背景环境下的输电线,本发明将深度学习算法分段运行并基于边缘检测算法识别线路走廊中呈现出的VP,由VP确定无人机的精确飞行航向,以增强此方法在电力巡检任务中的实用性。
传统自主导航模型中无人机必须在输电线路上方飞行,通过向下的摄像机来保持输电线的平行性。由于这种导航模式仅依赖于所检测到的输电线,有时会因不可靠的直线提取算法而影响巡检系统的性能。为更好地实现基于输电塔的无人机连续飞行方案,本发明创新性地设计了一个透视导航模型,其核心环节是对输电塔的实时精准定位和VP的稳定扫描计算。该导航模型与传统模型相比的最大特点是,它构建于三维空间中并考虑了透视关系。导航模型的输入为透视图像,输出为无人机在三维运动空间的速度控制量。为能从数学角度抽象描述算法原理,首先定义三个右手法则坐标系:全惯性坐标系C
g=o
gx
gy
gz
g;固定坐标系C
b=o
bx
by
bz
b和相机坐标系C
c=o
cx
cy
cz
c。为简化问题假设坐标原点O
b和O
c重合,然后令
表示相机帧相对于惯性系的旋转矩阵,相对于惯性系的固定坐标系用
表示,从而固定坐标系相对于相机坐标系的姿态旋转矩阵计算公式如下:
关于相机坐标系Cc分别定义了对应于VP和无人机航向的两个单位三维矢量β1、β2,这两个矢量的推导如下公式所示:
上式中的
是VPν的齐次坐标,式(57)中的x
b是固定坐标系中与X轴平行的单位空间矢量,κ为固有矩阵。为进一步说明β
1和β
2的关系,引入了旋转轴1=(1x,1y,1z)和旋转角γ,它们的定义来源于1=β
1×β
2和γ=arcsin||l||。根据Rodriguez公式构造两个向量之间关系的旋转矩阵
计算公式如下:
上式中E为单位矩阵,为1^与1相关联的3×3反对称矩阵,通过对旋转矩阵
的分解,无人机可以得到通过VP定义的方向旋转量。空间矢量之间的相对变化可以反映在图像平面上,通过如下公式表示:
其中λ为标量,
为投影点P
A的齐次坐标,若P
A与VP重合则无人机将沿VP定义的空间方向飞行。另外利用P
A与被测目标边界之间的平面位置关系,便可以预测无人机与目标之间的空间相对位置。比如若P
A落在字符B所指的探测塔边界框内,则表示无人机最终将飞向塔台并与塔相撞。为避免这种碰撞情况无人机应调整航向,使其投影P
A落在检测到的边界框之外。通过进一步的分析,透视导航模型可以挖掘出更多的信息。理想情况下,当投影P
A与输电线的VP一致时,无人机将沿此输电线飞行。另外,当云台摄像机与地面平行且与塔中心高度相同时,视界线会出现在影像平面的中间,塔心P
B的投影正好位于地平线上。由于输电线路位于塔中心偏上方处,因此可以在图像的右上角清晰地看到它们且其背景是蓝天。从以上结构原理分析,可以得出透视导航方案设计的重要依据:a.只要对输电塔进行可靠的定位,无人机便能实现在相邻杆塔之间的输电线路一侧自主飞行。b.当无人机沿VP定义的方向飞行时,飞行路线与输电线平行。c.受益于输电塔实时可靠定位和对VP的精确估计,在飞行路线透视导航方案中输电线路的提取变得更加稳健、巡检结果更加清晰。
在一个实施例中,所述交并比注意力机制使用建议框和真实框的交并比分数评价所述区域建议网络中锚点框的关注程度,其公式如下:
score(i,j,c)=max(IOU(proposali,j,c,gtk)),
k=1,2,…N;
其中,A,B分别代表两个图像区域的集合,N为每幅图像的真实框数量,proposal、gt分别为建议框和真实框,score(i,j,c)是特征图在(i,j,c)处相应建议框和全部真实框之间最大的交并比分数。
其中,基于改进区域建议网络的输电塔检测的原理:在透视导航方案中输电塔作为电力走廊唯一的固定标识物,必须对它进行准确定位。但受嵌入式处理器计算能力、算法速度和精度的限制,传统检测算法难以满足输电塔检测的实时性和鲁棒性要求。为此本文提出优化方案,在深度学习R-CNN目标检测算法基础上根据交并比IOU注意力机制改进了区域建议网络RPN,它与检测网络共享卷积特征,从而能实现几乎无成本的区域建议。滑动窗口中的可共享特征映射到一个低维向量,用于进一步的目标分类和边框回归。RPN的实质是一种注意力机制,它利用分类置信度帮助后续网络确定需重点关注的检测区域。传统RPN存在锚点框的类内差异不明显、分类置信度会忽略边框回归结果等问题,会影响检测模型的定位匹配和精确度。为此本发明利用IOU注意力机制对RPN进行了改进,使用建议框和真实框的IOU分数来评价对各锚点框的关注程度。
score(i,j,c)=max(IOU(proposali,j,c,gtk)),;
k=1,2,…N
上式中A,B分别代表两个图像区域的集合,N代表每幅图像的真实框数量proposal、gt分别指建议框和真实框,score(i,j,c)是特征图在(i,j,c)处相应建议框和全部真实框之间最大的IOU分数。根据IOU的计算公式可知,它可以定量评价预测区域和真实区域间的相似性。IOU注意力机制能简单描述RPN的定位准确度,在一定层度上缓解分类置信度和定位不匹配的问题。此外IOU是非极大值抑制置信度,其值会相对均匀的分散在0、1之间,这能保留更多的建议框,提高算法的精度。改进前的RPN架构,其分类任务采用Softmax激活函数。改进后的RPN架构,包括锚点框分类、偏移回归和建议框IOU回归三个学习任务,使用Sigmoid为激活函数来优化分类网络参数量,其分类任务的二分类交叉熵损失函数为:
Lcls(p,y)=-log(p)-(1-y)log(1-p);
上式中y、p分别代表真实值和预测值,IOU回归任务的Huber损失函数为:
Lcls(p,y)=-log(p)-(1-y)log(1-p);
上式中y、
分别代表真实值和预测值,在此训练网络中δ=3,改进后的RPN的损失函数在原有基础上修改为:
上式中i是锚点索引值,α=0.1,p*、t*、iou*表示真实值,p、t、iou表示预测值,Ncls为训练样本的锚点框数,Nreg、Niou是锚点框数,δ=0.1为Huber损失的参数,th=0.5表示训练模块IOU的阈值。
另外为弥补速度慢的缺点,本方案采取输电塔实时跟踪和鲁棒检测相结合的方式,其主要算法是核相关滤波KCF。KCF通过图像的矩阵循环获得训练样本,并利用傅立叶变换避免复杂的矩阵求逆运算,KCF的优良特性使得网络对输电塔的跟踪精度高且计算成本低。跟踪结果与目标有明显偏差时,根据最近的检测结果重新初始化跟踪器。同时为解决不同运行频率检测和跟踪之间的匹配问题,跟踪结果被记录在具有先进先出属性的寄存队列中。寄存队列用时间标记当前输入帧,当下一帧到达后依据时间标记和相应跟踪结果将当前帧推入队列。根据时间标记,检测模型可以在队列中找到匹配的跟踪目标。
在一个实施例中,所述透视模型采用所述区域卷积神经网络和所述核相关滤波器相结合的视觉策略跟踪算法。
其中,基于全卷积网络的平行输电线VP导航原理:在无人机自主导航过程中,平行输电线的VP是提供精确飞行航向的重要视觉信号。传统的VP检测方法虽然能满足基本导航需要,但其过多地依赖于先验知识会降低系统的智能性,缺失实际应用前景。受卷积神经网络语义分割方法的启发,本发明在性能优异的全卷积网络FCNs基础上,使用一个深度学习通道进行端到端训练。传统的VP检测方法载入FCNs模型后,可减少因先验知识过滤器所造成的视觉资源消耗,并缓解从复杂背景中进行窄线提取工作的难度。当前有众多FCNs结合残差的成熟网络结构,但考虑到精确像素级分割巨大的计算成本和繁重的人工标记工作,本方案在区域层面执行分割任务。同时为减少人工标注输电线路的工作量,本发明采用一种简单有效的人工标注方法。输电线路被分成几个平行的线路组,每个平行线路组对应一个相位电压。同一组中的输电线被红色多边形覆盖,这些多边形即为训练和评估FCNs的真实标签。训练模型采用交叉熵方法定义损失函数,见式(66),其中N0是输出神经元的数量,ak是真实标签,bk代表预测结果。
FCN由卷积网络和反褶积网络组成,在前向卷积部分采用VGG-16层网络进行多维特征描述,其中完全连接的层被转换为卷积以生成热图。为能恢复出原始图像的输入尺寸,热图被缩小到输入图像的1/322,并通过反向反褶积层进行双线性上采样。同时将上采样的粗略中间结果与融合层的得分进行相加融合校正,以实现对输电线路的精确分割。
VP检测方法是在FCNs生成的深灰色传输线区域内用线段检测器LSD识别输电线,其结果C显示的白色区域。无需参数调整的线性时间LSD算法作为一种有效的预处理器,能够适应一定程度的环境变化。因此本发明在Hough变换之前使用LSD,这种排列方式可提高Hough线检测的质量,可用于计算VP的浅灰色直线段Hough检测结果。
基于透视导航模型的总体导航方案,将一个长距离的检查任务分解成几个相邻输电塔之间的短子任务,短子任务又进一步被分为初始化和检查两个阶段。在初始化阶段,首先通过巡检无人机确定下一个输电塔的位置。然后云台调节俯仰角和偏航角,使相机光轴指向检测塔的中心。由于平移/倾斜/缩放(pan/tilt/zoom,PTZ)控制器主动跟踪塔台,在飞行航向调整过程中VP位置几乎没有变化,所以实际应用中只需计算一次VP便可将FCNs成功应用到导航系统。特别地当VP不存在时,导航系统会将无人机调整至经验飞行航向。从经验航向投影到塔中心的水平距离是边界框宽度的2倍,无人机可以按照经验方向在输电线走廊中沿z型轨迹飞行。在正常巡检阶段无人机沿预期航向飞行,当探测跟踪视觉策略启用时,云台持续调整偏航和俯仰角以跟踪下一个输电塔,直到摄像机光轴和预期航向之间的相对偏航角至90°,飞行过程中一旦输电线跟踪失败,无人机会立即旋停,之后跟踪器将使用最新的检测结果再次初始化导航系统。
本发明通过实地与实验验证对各个模块进行数据对比与分析,具体验证如下:
一:在室外场地验证导航系统性能,主要方式如下:设定飞行轨迹,在无人机上的处理后端上运行扩展卡尔曼滤波器,将融合数据通过蓝牙发送至无人机控制芯片,在无人机飞行到距离预计位置在设定阈值0.5米范围以内时,输入下一刻位置。无人机通过PID算法来对飞行位置进行调整。本次测试以绕行输电塔为例,从地面起飞,围绕输电塔一周进行拍摄,设定轨迹如图15(a)所示。对比使用RTK进行导航飞行的轨迹与使用融合数据的EKF导航飞行轨迹(如图15(b)所示)。
表1
从表1EKF与RTK航迹坐标与设定航迹坐标测量误差中可以看出,表示使用融合数据的EKF得到航迹的坐标与设定轨迹的最大误差值和平均误差值均小于仅使用RTK导航的误差,可见融合视觉SLAM的数据通过EKF输出,能提高导航系统的飞行精度与稳定性。但在图15(b)中可以看到,在起飞降落过程中,在水平面方向上的定位会出现较大偏差。这是因为在实际过程中,无人机起飞与降落受空气阻力等影响较大的原因导致。由于无人机在飞行中易受到风扰,在z轴方向上,无人机误差存在持续的抖动,因此平均误差相较于水平方向上更大。对无人机两种导航定位方式的航行坐标进行采样,得到各个坐标轴上波形图,如图16所示。其中在x轴、y轴图像上出现较大振荡,与图15(b)无人机在起飞降落阶段航迹呈现的误差保持一致。
表2
从图17的斜面轨迹运行的距离误差图像和表2斜面轨迹测量误差(与RTK对比)中斜面轨迹测量误差看,除Y轴最大误差、Z轴最大误差和Z轴均方根差以外,融合视觉里程计算法的扩展卡尔曼滤波器的其他误差均小于扩展卡尔曼滤波器,其中距离的均方根差减小5.18%,在此测试情况下当导航系统中加入视觉里程计时其导航性能更好。
二:通过和传统目标检测算法的比较,证明Faster R-CNN最适合于输电塔的可靠性定位。
实验共收集1280张的输电塔无人机透视图像,其中1000张作为训练样本、其余为测试样本。为找出输电塔可靠定位的最佳方法,本发明对R-CNN、SSD和YOLO v2这三种先进的目标检测框架,从速度、平均精度AP和召回率(Recall)三个方面进行了曲线比较,结果如表3速度和平均精度所示。在速度方面,SSD300优于其它方法,同时达到了较高的AP在AP方面,Faster R-CNN(VGG16)较其它方法稍有优势。
表3
R-CNN、SSD和YOLO v2三种检测方法的错误比率,如图18所示。从总体而言,浅灰色实线表示的Faster R-CNN(ZF)在较大召回范围内保持100%精度,即没有错误检测,而SSD和YOLO都会遇到不同程度的错误检测,甚至在召回率较低的情况下也会出错。当召回率接近1时所有曲线都会断开,其原因是所有算法都存在某些未被检测到的边界框。在电力自主巡检任务中可以接受一定程度的漏检FN,却不能容忍误检FP,否则会给无人机带来极大的安全隐患。
三:通过实验说明本发明的VP检测方法在鲁棒性和准确性方面,较其它方法都有显著提高。
此本实验中训练FCNs共收集了600张图片,其中400张用于训练、其余作为测试集。训练集的数据来自农村、山区和郊区,图像背景包括树木、土地、蓝天和高山,此外同一目标的图像还包括光照、视角和距离的变化。典型目标的测试结果如图19所示,前四幅图像从PTZ相机获得,分辨率为1920×1080,其余4张来自VP检测阶段的处理结果,分辨率为1280×720,并进行了JPEG压缩。图19(b)显示输电线被共同工作的无人机部分遮挡,明亮光线下图像对比度降低难以区分出输电线,而此时FCNs仍然有效并表现出优异的检测性能。在图19(c)中,输电线路几乎淹没在树木里,边缘清晰的远处树木和纹理特征鲜明的附近塔杆不会对输电线提取造成干扰。图19(d)表明,无人机的摄影距离和角度发生变化时,仍然可以准确提取输电线。实验还在黄昏和清晨对整个系统进行了测试,并展示了4幅在VP检测阶段的典型图像,对应于图19(e)到(h)。在实际电力巡检任务中,输电线路大多以蓝天为背景,传统VP检测方法往往因恶劣天气而失效。如图19(e)和(f)所示,基于FCNs的VP检测算法,能在多云天气条件下正常工作。如图19(g)和(h)所示,在黄昏和黎明时刻不同照明条件下,FCNs均可成功提取出可用于VP检测的直线段。
在自主驾驶领域大多数VP检测方法都具有良好性能,这是因为路面边缘清晰更容易与背景区分。相比之下,输电线具有较少的像素且边缘模糊,提取过程容易受到阻碍难以进行。本发明比较了基于边缘的方法及其与FCNs相结合的改进方法,选择分辨率为1280×720的图像进行测试集,其累积像素误差曲线如图20所示。此结果表明,FCNs算法可减小累积像素误差。另外,因光照改变传统方法的误差曲线末端呈陡峭式上升,而引入FCNs算法后误差曲线较平滑、巡检系统抗环境干扰的能力变强。
四:在实际任务中对巡检系统进行测试,以证明本发明方法的可行性。
从初始阶段开始同步记录无人机航路点,及航路点相对于惯性坐标系的方位,其中航路点是视觉定位系统与GPS融合的结果。以50为间隔对航路点进行采样,用以清晰地描述航线轨迹,如图21所示,相邻航路点之间的平均飞行持续时间约为4秒。PTZ相机的光轴由深灰色箭头表示,在位置A调整偏航和俯仰角,将检测到的输电塔定位到图像中心,然后无人机旋转到由VP确定出的预期航向。为避免起落架遮挡相机,从航路点B开始无人机继续沿预期方向飞行,同时开始旋转。相邻塔端之间的检查在C位置,相对偏航角达到90o。然后下一次检查从航路点C开始,到航路点D结束。
在不同的初始位置进行了多次测试,图22中共绘制了9条轨迹,每个轨迹都与图21所示的轨迹相似,所有的实际轨迹都符合预期结果。图22(b)显示了轨迹在高度方面的变化,及输电线方向与轨道1和2之间存在的偏差。由于输电线路固有的悬垂特性,这可能导致VP导航方向和理想航向之间的差异不可避免。另外,所检测到的输电线数目受拍摄距离、拍摄角度、光照和天气等因素干扰,它对计算VP也有很大影响。
综上所述,借助于本发明的上述技术方案,本发明首先引入位置信息融合即时定位与地图构建算法,提高无人机的定位精度与导航能力。其次利用扩展卡尔曼滤波器算法将多传感器数据融合,将滤波器输出的位姿信息反馈到无人机飞行控制终端,以此调整飞行控制方式与飞行机制,做到准确避障,实现自主飞行;然后利用改进的区域建议网络准确定位输电塔,并通过核相关滤波器实现对杆塔的连续视觉跟踪,避免无人机自主巡检过程中面临的障碍物集中、视线较差的环境下难以做到合理避障的问题。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。