[发明专利]一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法

说明书

技术领域

本发明涉及惯性/视觉组合导航领域，尤其涉及一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法。

背景技术

近年来,伴随着计算机技术、电子技术、通讯技术、先进控制以及人工智能的迅猛发展,有关移动机器人技术的研究以及应用取得了巨大的进步。智能移动机器人作为集环境感知、动态决策以及实时行为控制与执行等众多特性为一体的复杂综合系统,目前已经在军事、民用和科学研究以及工业生产等领域获得越来越普遍的应用,用以代替人类来执行一些需要在恶劣条件或是危险条件下开展的工作。定位与导航作为室内移动机器人完成任务的首要前提,逐渐成为该领域的研究热点。然而,在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中,室内机器人获取导航信息的准确性、实时性及鲁棒性会受到很大的影响。如何实现在室内环境下获取有效的导航信息以满足高精度导航智能移动机器人的要求且尽量避免使其受到外界环境的影响,具有十分重要的科学理论意义以及工程应用价值。将航行载体从起始点引导到目的地的过程称为导航。导航有多种技术途径,如无线电导航、天文导航、卫星导航、红外导航、惯性导航、视觉导航等。其中，惯性导航利用加速度计与陀螺仪计算航程，推知当前位置和下一步目的地，自主性强、不易受干扰，是目前的主要导航方法。但惯性导航系统(Inertial Navigation System，INS)由于其固有的导航误差积累，导航精度随时间增长而降低，并且设备成本较高，因而不能完全满足实际应用的需要。此外，视觉导航采用成像设备拍摄图像，运用机器视觉等相关技术识别路径，实现自动导航。视觉导航因其应用范围广，在理论上具有最佳引导柔性，近年来发展十分迅速。但视觉导航方法主要存在图像匹配的精度较差、目标图像中的像点定位有误差、测量系统的标定误差以及成像系统的空间分辨率有限的缺点，其应用也受到限制。

因此，视觉导航与惯性导航作为两种自主导航方式，具有很好的互补性。两者的组合可以解决无GPS环境下的导航问题，对于室内导航、机器人路径规划、无人机自主着陆，自动行驶车的导航、地下交通导航定位、矿井作业安全等应用需求具有重要意义。一般采用卡尔曼滤波器将视觉导航信息与惯性导航系统的导航信息进行数据融合。应用卡尔曼滤波器进行数据融合需要解决两个问题：系统噪声协方差矩阵和测量噪声协方差矩阵的选取。前者影响KF算法的滤波性能及参数估计精度，增大了系统的不确定性；后者取值不当会影响滤波器的修正速度，使滤波过程不稳定甚至发散。

粒子群优化算法是模拟鸟群捕食行为过程中通过集体协作，寻求群体最优解的全局优化算法。每个粒子(d维解空间的一个候选解)根据自身和群体经验调整飞行轨迹向最优点靠拢。通过不断学习更新，粒子向个体最优位置和全局最优位置加速运动，最终输出全局最优解。粒子群算法模拟生物进化过程，可以随机搜索得到全局最优参数。

发明内容

本发明为了解决移动机器人运动在弱光或是无光环境时,视觉导航系统不能提供长时间高精度导航从而导致惯性视觉组合导航系统出现严重的精度下降的缺点，提出一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法。

本发明的技术方案具体如下：一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法，包括以下步骤：

步骤1：在视觉信号有效时,利用移动机器人搭载的摄像头采集动态图像,通过图像特征提取和最近邻匹配法来确定摄像头的速度；采用SURF算法分别提取视频中的相邻两个图像帧中的SURF特征点，并记录特征点在图像坐标系中的位置坐标，同时根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，来确定摄像头在水平面上的速度V_x、V_y；

步骤2：根据步骤1获得的摄像头在水平面上的速度V_x、V_y，结合惯性导航系统测量得到的航向角计算得到移动机器人的东向速度和北向速度公式如下：

步骤3：利用CKF,将移动机器人的速度和惯性导航系统测量得到的航向角变化量ω_z进行数据融合；

CKF根据系统各时刻的量测值、状态值及选定的滤波参数值，通过时间更新与量测更新得到移动机器人的东向和北向的速度以及加速度的估计记东向速度和加速度的估计分别为北向速度和加速度的估计分别为

(1)以移动机器人的东向和北向的速度和加速度作为状态变量,则滤波器的系统方程如式所示：

其中，