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

说明书

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

技术领域

本发明涉及组合导航算法技术领域，特别是涉及一种融合光流位置和速度信息的组合导航算法。

背景技术

基于低精度的MEMS IMU的惯性导航无法为载体提供可用导航数据，常采用GNSS数据来对抑制惯导误差的发散，但GNSS在室内、城市侠道、桥梁及隧道等环境下无法提供定位信息，在这些GNSS拒止环境下，需要寻找伪GNSS来对惯导进行辅助，所谓伪GNSS，即在GNSS拒止环境下，其在功能上部分或全部替代GNSS实现速度或位置的测量，从而与惯导及其它传感器数据进行融合，实现GNSS拒止环境下的导航与定位功能。

发明内容

本发明所要解决的技术问题是：为了克服GNSS拒止环境下导航定位问题，本发明提供一种融合光流位置和速度信息的组合导航算法，本算法融合了光流传感器、MEMS IMU、磁力计、气压高度计、激光测距传感器的数据信息，采用扩展卡尔曼滤波完成数据融合，在站心坐标系下解算出载体的位置、速度和姿态信息。

本发明解决其技术问题所要采用的技术方案是：

本发明所涉及的组合导航系统包括安装于载体上的光流传感器、MEMS IMU(简称IMU)、磁力计、气压高度计和激光测距传感器，光流传感器相机坐标系、IMU坐标系、磁力计坐标系与右前上载体坐标系重合，光流传感器与激光测距传感器安装于载体底部，激光测距传感器测量方向与载体上轴方向相反，其中，

IMU包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)；磁力计采用三轴正交磁力计，用于地磁测量；气压高度计用于测量气压高度；光流传感器用于测量像素坐标系下两帧有效图像之间的像素位移；激光测距传感器用于测量传感器和反射点之间的单维距离。

本发明涉及到的坐标系包括载体坐标系、站心坐标系、东北天地理坐标系、导航坐标系、地磁东北天坐标系、相机坐标系、像素坐标系，其中，载体坐标系指以装载导航系统的载体的“右前上”坐标系，安装于载体上的具有坐标的传感器(IMU、磁力计、光流传感器)其安装与载体坐标系重合；站心坐标系为载体导航起始点为原点的东北天地理坐标系，组合导航的位置估计以站心坐标系表示；东北天地理坐标系的原点为载体质心，组合导航的速度估计表示在本坐标系中，且认为等同于表示于站心坐标系中；在本算法中导航坐标系采用站心坐标系；地磁东北天坐标系以磁北为北向，东北天地理坐标系天向轴旋转一个地磁偏角即与地磁东北天坐标系重合，相机坐标系即相机的“右前上”坐标系，像素坐标系原点为图像左上角，单位为像素(pixel),u轴与相机坐标系的右向轴方向相同，v轴与相机坐标系的前向轴方向相同。

一种融合光流位置和速度信息的组合导航算法，包括以下步骤：

S1：由导航计算机读取安装于载体上的光流传感器、IMU、磁力计、气压高度计和激光测距传感器的数据信息，其中，从光流传感器读取的是像素坐标系下u轴方向和v轴方向的两帧有效图像之间的像素位移，从IMU上读取的是角速度和加速度(比力)数据，从磁力计上读取的是地磁强度数据，从气压高度计上读取的是气压高度数据，从激光测距传感器上读取的是激光测距高度数据；

S2：采用变权值法对步骤S1中获取的数据信息中的气压高度与激光测距高度进行融合，计算获得一个融合高度，并将融合高度作为扩展卡尔曼滤波(EKF)数据融合的高度量测；

S3：导航算法根据光流传感器数据更新标志，判断光流传感器数据是否更新，如果光流传感器数据没有更新，进入步骤S4；如果光流传感器数据有更新，则进入步骤S5；