[发明专利]一种融合光流位置和速度信息的组合导航算法

摘要

本发明提供一种融合光流位置和速度信息的组合导航算法，利用光流传感器输出的位置和速度信息以及MEMS IMU、磁力计、气压高度计、激光测距传感器的数据，采用扩展卡尔曼滤波器完成数据融合，解算出载体的位置、速度和姿态信息。本算法采用站心坐标系作为导航坐标系，实时解算出载体相对初始位置点的位置坐标。本算法可以实现载体在GNSS拒止条件下的导航与定位，为载体提供准确的航向、姿态和速度信息，并有效减缓捷联纯惯性定位误差的漂移速率。

主权项

1.一种融合光流位置和速度信息的组合导航算法，其特征在于：包括导航系统，所述导航系统包括光流传感器、IMU、磁力计、气压高度计和激光测距传感器，还包括以下步骤：S1：由导航计算机读取安装于载体上的光流传感器、IMU、磁力计、气压高度计和激光测距传感器的数据信息，其中，从光流传感器读取的是像素坐标系下u轴方向和v轴方向的两帧有效图像之间的像素位移，从IMU上读取的是角速度和加速度数据，从磁力计上读取的是地磁强度数据，从气压高度计上读取的是气压高度数据，从激光测距传感器上读取的是激光测距高度数据；S2：采用变权值法对步骤S1中获取的数据信息中的气压高度与激光测距高度进行融合，计算获得一个融合高度，并将融合高度作为扩展卡尔曼滤波数据融合的高度量测；S3：导航算法根据光流传感器数据更新标志，判断光流传感器数据是否更新，如果光流传感器数据没有更新，进入步骤S4；如果光流传感器数据有更新，则进入步骤S5；S4：在光流传感器数据无更新情况下，导航算法进行捷联纯惯性导航递推，解算出载体的位置、速度和姿态信息；S5：在光流传感器数据有更新情况下，对光流传感器输出的两维像素位移进行角运动补偿，得到线运动对应的像素位移，并利用相机分辨率和相机与被拍摄平面之间的物理距离对线运动像素位移进行物理尺度转换，将线运动像素位移转化为米为单位的相机坐标系下右向和前向位移；并根据两帧有效光流传感器数据输出之间的间隔时间计算出相机坐标系下右向和前向速度；S6：根据步骤S5的结果，通过扩展卡尔曼滤波对光流传感器数据计算得到的载体系位置和速度、IMU数据、磁力计数据、融合高度进行数据融合，解算出载体的位置、速度和姿态信息。