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

摘要

本发明公开了一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法，主要步骤包括步骤一在视觉信号有效时,利用移动机器人搭载的摄像头采集动态视频,通过图像特征提取和最近邻匹配法来确定摄像头的速度；步骤二根据惯性导航系统测得的航向角并结合摄像头的速度计算出移动机器人的速度,使用CKF估计移动机器人的速度、加速度；步骤三利用CKF根据系统各时刻的量测值和状态值及选定的滤波参数值，通过时间更新与量测更新得到系统状态的估计值；步骤四根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Q和R进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。

主权项

一种采用基于粒子群优化的CKF的惯性/视觉组合导航方法，其特征在于，包括以下步骤：步骤1：在视觉信号有效时,利用移动机器人搭载的摄像头采集动态图像,通过图像特征提取和最近邻匹配法来确定摄像头的速度；采用SURF算法分别提取视频中的相邻两个图像帧中的SURF特征点，并记录特征点在图像坐标系中的位置坐标，同时根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，来确定摄像头在水平面上的速度Vx、Vy；步骤2：根据步骤1获得的摄像头在水平面上的速度Vx、Vy，结合惯性导航系统测量得到的航向角计算得到移动机器人的东向速度和北向速度公式如下：步骤3：利用CKF,将移动机器人的速度和惯性导航系统测量得到的航向角变化量ωz进行数据融合；CKF根据系统各时刻的量测值、状态值及选定的滤波参数值，通过时间更新与量测更新得到移动机器人的东向和北向的速度以及加速度的估计记东向速度和加速度的估计分别为北向速度和加速度的估计分别为(1)以移动机器人的东向和北向的速度和加速度作为状态变量,则滤波器的系统方程如式所示：w^k+1=10T0010T00100001w^k+ω4×1*]]>其中，w^k+1=VE,k+1VN,k+1AccE,k+1AccN,k+1,w^k=VE,kVN,kAccE,kAccN,k]]>式中，VE,k，VN,k，AccE,k，AccN,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，VE,k+1，VN,k+1，AccE,k+1，AccN,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，T为相邻两帧图片的采样时间,也即数据的采样周期，为系统随机噪声，Q为系统噪声协方差矩阵；且系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；(2)滤波器的观测方程如下式所示：VE~VN~ωZ=VE,kVN,kVN,kAccE,k-VE,kAccN,kVE,k2+VV,k2+v3×1]]>式中，v3×1为观测方程随机噪声，v3×1～N(0,R)，R为观测噪声协方差矩阵；步骤4：根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Qi,Ri进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值；粒子群算法中，每个粒子即d维解空间的一个候选解，根据自身和群体经验调整轨迹向最优点靠拢；粒子x通过迭代更新自己的速度和位置：vxd(G+1)=ωvxd(G)+c1r1(pbest(G)-Xxd(G))+c2r2(gbest(G)-Xxd(G))]]>Xxd(G+1)=Xxd(G)+vxd(G+1)]]>式中：G为迭代次数，和分别为粒子x在第G代的飞行位置和速度；为粒子x迄今为止搜索到的最好位置；为整个粒子群迄今为止搜索到的最好位置；w为惯性权重，c1、c2为学习因子，r1和r2为0～1之间的随机数；通过不断学习更新，粒子向个体最优位置和全局最优位置加速运动，最终输出全局最优解；将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。