一种采用基于粒子群优化的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)以移动机器人的东向和北向的速度和加速度作为状态变量,则滤波器的系统方程如式所示：

其中，

式中，V_E,k，V_N,k，Acc_E,k，Acc_N,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，V_E,k+1，V_N,k+1，Acc_E,k+1，Acc_N,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，T为相邻两帧图片的采样时间,也即数据的采样周期，为系统随机噪声，Q为系统噪声协方差矩阵；且系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；

(2)滤波器的观测方程如下式所示：

式中，v_3×1为观测方程随机噪声，v_3×1～N(0,R)，R为观测噪声协方差矩阵；

步骤4：根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Q_i,R_i进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值；

粒子群算法中，每个粒子即d维解空间的一个候选解，根据自身和群体经验调整轨迹向最优点靠拢；粒子x通过迭代更新自己的速度和位置：

式中：G为迭代次数，和分别为粒子x在第G代的飞行位置和速度；为粒子x迄今为止搜索到的最好位置；为整个粒子群迄今为止搜索到的最好位置；w为惯性权重，c₁、c₂为学习因子，r₁和r₂为0～1之间的随机数；

通过不断学习更新，粒子向个体最优位置和全局最优位置加速运动，最终输出全局最优解；将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。

进一步的，所述步骤1具体包括：

(1)设车载摄像头的投影中心距离地面的垂直距离为Z_R，摄像头成像平面上横轴与纵轴的归一化焦距分别为f为摄像头焦距，dx和dy分别为成像平面的横轴与纵轴上单位像素的尺寸大小，主光轴点坐标(c₁,c₂),针孔摄像头模型的归一化映射关系如下：

式中，(X_R，Y_R，Z_R)是真实的地面上的某一特定点在相机坐标系中的坐标，此特定点投影在图像坐标系上就是SURF特征点；

(2)考虑镜头畸变系数为[k₁,k₂,k₃,k₄,k₅]，包括径向畸变系数和切向畸变系数，则相应的图像坐标系上的点可以表示为：

其中切向畸变向量则相应的像素点的最终坐标为：

假设镜头畸变系数[k₁,k₂,k₃,k₄,k₅]可忽略，均为零，则有：

将上式对时间微分得：

式中，V_x，V_y，V_z分别为坐标点X_R、Y_R、Z_R关于时间的导数，表示摄像头在相机坐标系x、y、z三个轴方向上的瞬时速度；v_x、v_y分别为坐标点x_c、y_c关于时间的导数，表示SURF特征点在图像坐标系x、y两个轴方向上的瞬时速度，将前式带入本式得：

若考虑地面水平则Z_R为常值，关于时间的导数为0，即V_Z＝0，则有：

根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，得到SURF特征点在图像上的横向位移变化Δx和纵向位移变化Δy,则有：

式中，T为相邻两帧图片的采样时间，也即数据的采样周期。

进一步的，所述步骤3系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；Q为系统噪声协方差矩阵，记为

Q＝diag(q₁₁，q₂₂，q₃₃，q₄₄)

R为观测噪声协方差矩阵，记为R＝(r₁₁，r₂₂，r₃₃)

将Q_i,R_i作为粒子，令系统噪声矩阵对角元素相等，即

q₁₁＝q₂₂＝q₃₃＝q₄₄，r₁₁＝r₂₂＝r₃₃。

进一步的，所述步骤4中的根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Q_i,R_i进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值，具体为：

将移动机器人东向和北向的速度和加速度的估计惯性导航系统测量得到的东向和北向的速度和加速度y_k的均方差作为目标函数，目标函数值最小时视为达到最优估计：

式中，Q_i,R_i为卡尔曼滤波器的噪声输入参数，f(Q_i,R_i)为Q_i,R_i为参数时测量值与估计值的均方差，为估计值，M为估计长度，y_k为实测数据。

进一步的，在所述步骤4的粒子群算法中，粒子数取100，预定迭代次数为200，根据经验先设

c₁＝c₂＝1.9

使用粒子群算法对滤波参数Q_i,R_i进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。

本发明的优点在于：

(1)通过视觉获取室内移动机器人的东向和北向速度，仅需要车载摄像头，不需要借助其他外部设备，独立性很强。

(2)采用CKF将视觉导航信息与惯性导航系统的导航信息进行数据融合,可有效的对系统误差进行补偿，得到的导航信息比视觉、惯性任何单一导航方法得到的信息精度更高。

(3)通过粒子群算法来对滤波参数进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。改善了因为不准确的噪声统计增加导航模型的不确定性而引起的CKF状态估计效果差，导航精度低的问题。从而实现在视觉信息发生失锁后对惯性导航系统的导航速度误差进行有效的补偿，进而提高了惯性视觉组合导航的导航精度。

附图说明

图1为本发明的流程框图。

图2为本发明中粒子群算法优化噪声矩阵的流程图。

具体实施方式

下面结合附图对本发明作更进一步的详细说明：

本发明是一种采用基于粒子群优化的CKF(Cubature Kalman filters，容积卡尔曼滤波)的惯性/视觉组合导航方法，流程如图1所示，包括以下几个步骤：

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

所述的摄像头速度获取过程包括：

(1)设车载摄像头的投影中心距离地面的垂直距离为Z_R，摄像头成像平面上横轴与纵轴的归一化焦距分别为f为摄像头焦距，dx和dy分别为成像平面的横轴与纵轴上单位像素的尺寸大小，主光轴点坐标(c₁,c₂),针孔摄像头模型的归一化映射关系如下：

式中，(X_R，Y_R，Z_R)是真实的地面上的某一特定点在相机坐标系中的坐标。此特定点投影在图像坐标系上就是SURF特征点。

(2)考虑镜头畸变系数为[k₁,k₂,k₃,k₄,k₅]，包括径向畸变系数和切向畸变系数，则相应的图像坐标系上的点可以表示为：

其中切向畸变向量则相应的像素点的最终坐标为：

假设镜头畸变系数[k₁,k₂,k₃,k₄,k₅]可忽略，均为零，则：

将上式对时间微分得：

式中，V_x，V_y，V_z分别为坐标点X_R、Y_R、Z_R关于时间的导数，表示摄像头在相机坐标系x、y、z三个轴方向上的即时速度；v_x、v_y分别为坐标点x_c、y_c关于时间的导数，表示SURF特征点在图像坐标系x、y两个轴方向上的即时速度，将前式带入本式得：

若考虑地面水平则Z_R为常值，关于时间的导数为0，即V_Z＝0，则有：

根据最近邻匹配法对两帧图像上的SURF特征点进行匹配，得到SURF特征点在图像上的横向位移变化Δx和纵向位移变化Δy,则有：

式中，T为相邻两帧图片的采样时间，也即数据的采样周期。

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

步骤3：利用CKF,将移动机器人的速度和惯性导航系统测量得到的航向角变化量ω_z进行数据融合。CKF根据系统各时刻的量测值和状态值及选定的滤波参数值，通过时间更新与量测更新得到移动机器人的东向和北向的速度以及加速度的估计记东向速度和加速度的估计分别为北向速度和加速度的估计分别为

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

其中，

式中，V_E,k，V_N,k，Acc_E,k，Acc_N,k分别代表k时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，V_E,k+1，V_N,k+1，Acc_E,k+1，Acc_N,k+1分别代表k+1时刻移动机器人的东向速度、北向速度、东向加速度和北向加速度，T为相邻两帧图片的采样时间,也即数据的采样周期，为系统随机噪声，Q为系统噪声协方差矩阵。且系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；

(2)滤波器的观测方程如下式所示：

式中，v_3×1为观测方程随机噪声。v_3×1～N(0,R)，R为观测噪声协方差矩阵。

步骤4：根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Q_i,R_i进行寻优，将得到的修正值作为CKF的输入参数，直至得到最优的状态估计值。

粒子群算法中，每个粒子(d维解空间的一个候选解)根据自身和群体经验调整轨迹向最优点靠拢。粒子x通过迭代更新自己的速度和位置：

式中：G为迭代次数，和分别为粒子x在第G代的飞行位置和速度；为粒子x迄今为止搜索到的最好位置；为整个粒子群迄今为止搜索到的最好位置；w为惯性权重，c₁、c₂为学习因子，r₁和r₂为0～1之间的随机数。

对本发明的有益效果说明如下：

系统方程中的状态量初始值为实际测量所得的实验数据，通常均取为0；Q为系统噪声协方差矩阵，记为

Q＝diag(q₁₁，q₂₂，q₃₃，q₄₄)

R为观测噪声协方差矩阵。记为R＝(r₁₁，r₂₂，r₃₃)

本发明中，将Q_i,R_i作为粒子，为使优化计算简化，令系统噪声矩阵对角元素相等，即

q₁₁＝q₂₂＝q₃₃＝q₄₄，r₁₁＝r₂₂＝r₃₃，

将移动机器人东向和北向的速度和加速度的估计惯性导航系统测量得到的东向和北向的速度和加速度y_k的均方差作为目标函数，目标函数值最小时视为达到最优估计。

式中，Q_i,R_i为卡尔曼滤波器的噪声输入参数，f(Q_i,R_i)为Q_i,R_i为参数时测量值与估计值的均方差，为估计值，M为估计长度，y_k为实测数据。

在粒子群算法中，粒子数取100，预定迭代次数为200，根据经验先设

c₁＝c₂＝1.9

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