一种基于光流法的惯性视觉组合导航方法技术

一种基于光流法的惯性视觉组合导航方法，本发明专利技术涉及惯性视觉组合导航方。本发明专利技术的目的是为了解决现有惯性导航长时间工作会有累积误差，导航信息不准确；现有卫星导航在室内或有建筑物遮挡时会发生屏蔽中断或动态误差过大；以及现有视觉导航算法较为复杂，且易受相机姿态、光照变化、图像噪声等影响的问题。过程为：一、定义坐标系；二、在无人机上搭载IMU、相机和高度计三种传感器，得到二维光流数据；三、得到无人机在世界坐标系下的位置；四、根据IMU测量信息进行惯性导航，解算出无人机在世界坐标系下的位置和姿态；五、得到融合后的无人机在世界坐标系下的位置及姿态信息。本发明专利技术用于无人机自主导航技术领域。

【技术实现步骤摘要】
一种基于光流法的惯性视觉组合导航方法
本专利技术涉及无人机自主导航
，特别是涉及在无GPS时的小型无人机复杂环境中的自主导航。
技术介绍
目前，微型无人机常用的导航方式有惯性导航，卫星导航，视觉导航等。给定初始条件后，惯性导航可实现完全自主的导航，不依赖外部信号，也不受外部环境干扰，但是长时间工作会有累积误差，导航信息不准确。卫星导航中最常见的是GPS导航，具有全球性和高精度等特点，但是GPS导航受人为因素影响较大，且在室内或有建筑物遮挡时会发生屏蔽中断或动态误差过大。视觉导航是一种自主导航方式，通过相机在运动过程中采集的图像变化信息，来确定机体相对于场景的位置。相比较而言，视觉导航可以在复杂的环境中实现导航定位和感知避障，无需过多的外部信号，还能再现环境的形状、颜色等信息，为飞行器控制和路径规划提供了依据。但视觉导航算法较为复杂，且易受相机姿态、光照变化、图像噪声等影响。综上，现有导航方式在一定程度上有缺陷：1)现有惯性导航长时间工作会有累积误差，导航信息不准确；2)现有卫星导航受人为因素影响较大，且在室内或有建筑物遮挡时会发生屏蔽中断或动态误差过大；3)现有视觉导航算法较为本文档来自技高网...

【技术保护点】
1.一种基于光流法的惯性视觉组合导航方法，其特征在于：所述方法具体过程为：步骤一、定义世界坐标系OwXwYwZw、无人机机体坐标系ObXbYbZb、相机坐标系OcXcYcZc、物理成像坐标系O1xy和像素图像平面坐标系Ouv；步骤二、在无人机上搭载IMU、相机和高度计三种传感器，通过相机采集图像，将采集到的图像进行金字塔LK解算，得到二维光流数据；IMU由陀螺仪和加速度计组成；步骤三、将步骤二中得到的二维光流数据信息转换为三维导航信息即无人机在世界坐标系下的位置；步骤四、根据IMU测量信息进行惯性导航，解算出无人机在世界坐标系下的位置和姿态；步骤五、将步骤四解算出的无人机位置、姿态信息和步骤三...

【技术特征摘要】
1.一种基于光流法的惯性视觉组合导航方法，其特征在于：所述方法具体过程为：步骤一、定义世界坐标系OwXwYwZw、无人机机体坐标系ObXbYbZb、相机坐标系OcXcYcZc、物理成像坐标系O1xy和像素图像平面坐标系Ouv；步骤二、在无人机上搭载IMU、相机和高度计三种传感器，通过相机采集图像，将采集到的图像进行金字塔LK解算，得到二维光流数据；IMU由陀螺仪和加速度计组成；步骤三、将步骤二中得到的二维光流数据信息转换为三维导航信息即无人机在世界坐标系下的位置；步骤四、根据IMU测量信息进行惯性导航，解算出无人机在世界坐标系下的位置和姿态；步骤五、将步骤四解算出的无人机位置、姿态信息和步骤三得到的无人机在世界坐标系下的位置信息进行扩展卡尔曼滤波融合，得到融合后的无人机在世界坐标系下的位置及姿态信息。2.根据权利要求1所述一种基于光流法的惯性视觉组合导航方法，其特征在于：所述步骤一中定义世界坐标系OwXwYwZw、无人机机体坐标系ObXbYbZb、相机坐标系OcXcYcZc、物理成像坐标系O1xy和像素图像平面坐标系Ouv；具体为：a.世界坐标系OwXwYwZw：使用北-东-地坐标系作为世界坐标系，世界坐标系原点Ow为无人机初始位置在地面的投影，坐标轴OwXw指向地球北，OwYw指向地球东，OwZw垂直于地球表面并指向下；世界坐标系是固定坐标系；b.无人机机体坐标系ObXbYbZb：机体坐标系原点Ob取在无人机的质心上，ObXb平行于无人机桨盘平面并指向前，ObYb平行于无人机桨盘平面并指向右，ObZb垂直于无人机桨盘平面并指向下；机体坐标系是动坐标系；c.相机坐标系OcXcYcZc：假设相机固连于无人机质心，相机镜头垂直于无人机机身向下安装，则相机坐标系的原点Oc取在无人机的质心上，OcXc轴与无人机纵轴ObXb重合，指向无人机头部为正；OcZc轴与相机的光轴重合，指向目标为正；OcYc轴垂直于OcXcZc平面，方向按右手直角坐标系确定；相机坐标系是动坐标系；d.物理成像坐标系O1xy：相机内在成像过程中所形成的像平面坐标系，以米为单位，用(x,y)表示；e.像素图像平面坐标系Ouv：相机采集的数字图像在计算机内以M×N数组的形式存在，数组中的每一个元素称为像素，M代表像素的行数，N代表像素的列数，每个像素的值表示该点在图像中的亮度，在图像上定义了直角坐标系Ouv作为像素图像平面坐标系。3.根据权利要求1或2所述一种基于光流法的惯性视觉组合导航方法，其特征在于：所述步骤二中在无人机上搭载IMU、相机和高度计三种传感器，通过相机采集图像，将采集到的图像进行金字塔LK解算，得到二维光流数据；具体过程为：步骤2-1、将相机采集到的彩色图像的前后两帧转换为灰度图像；步骤2-2、在前一帧中使用Shi-Tomasi角点检测法找到特征点，并将前一帧特征点坐标精确到亚像素精度；步骤2-3、采用金字塔思想的LK算法检测出前一帧识别出来的特征点在后一帧中的位置并确定特征点在后一帧中坐标；步骤2-4、最后根据特征点在前后两帧的坐标信息画出当前帧即后一帧所对应的光流示意图，得到二维光流数据；步骤2-5、重复步骤2-1至步骤2-3，直至处理完采集到的图像的所有帧。4.根据权利要求3所述一种基于光流法的惯性视觉组合导航方法，其特征在于：所述步骤三中将步骤二中得到的二维光流数据信息转换为三维导航信息即无人机在世界坐标系下的位置；具体过程为：步骤3-1、首先确定相机参数和初始位姿参数，然后读取高度计、陀螺仪和光流数据；步骤3-2、根据3-1中读取的数据信息计算初始无人机在世界坐标系下的欧拉角及姿态旋转矩阵；步骤3-3、根据相机的内参矩阵，将像素图像平面坐标系上的特征点坐标和光流数据转换到物理成像坐标系平面上：计算特征点坐标转换公式为：其中，K是相机的内参矩阵，x,y表示特征点在物理成像坐标系平面上的坐标，u,v表示特征点在像素图像平面坐标系上的坐标；f为相机的焦距，fx为x方向上缩放比例，fy为y方向上缩放比例，cx为x方向原点平移距离，cy为y方向原点平移距离；计算图像上的特征点在相机坐标系下的平移速度即光流信息，公式为：Tx＝(ωxy-ωyx)xZc/f2+xTz/f-(vx-ωzy)Zc/f-ZcωxTy＝(ωxy-ωyx)yZc/f2+yTz/f-(vy+ωzx)Zc/f+Zcωy其中，Tz为图像上的特征点在相机坐标系下Zc轴方向上的平移速度，为滚转角，θ为俯仰角；为相机坐标系质心坐标值；Tx为图像上的特征点在相机坐标系下Xc轴方向的平移速度，ωx为Xc轴方向角速度，ωy为Yc轴方向角速度，vx为Xc轴方向的速度，ωz为Zc轴方向角速度；Ty为图像上的特征点在相机坐标系下Yc轴方向的平移速度，vy为Yc方向的速度；步骤3-4、通过步骤一中各坐标系的定义得到相机坐标系与无人机机体坐标系之间转换关系，根据两个坐标系之间的转换矩阵计算无人机在世界坐标系下的平移速度：其中，表示相机坐标系到世界坐标系之间的转换矩阵，Tw为无人机在世界坐标系下的平移速度，Tc为相机坐标系下的平移速度；步骤3-5、对无人机在世界坐标系下的平移速度进行一步积分，得到无人机在世界坐标系下的位置信息。5.根据权利要求4所述一种基于光流法的惯性视觉组合导航方法，其特征在于：所述步骤五中将步骤四解算出的无人机在世界坐标系下的位置、姿态信息和步骤三得...

【专利技术属性】
技术研发人员：白成超，郭继峰，张文苑，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江,23