[发明专利]一种基于站心坐标系的小型无人机无缝自主式导航方法

摘要

本发明提供一种基于站心坐标系的小型无人机无缝自主式导航方法，本方法以站心坐标系为导航坐标系，利用多传感器冗余辅助基于低成本的MEMS IMU的捷联惯导，利用扩展卡尔曼滤波完成数据融合，实现无人机的位置、速度和姿态解算。本导航方法采用传感器数据健康状态管理系统，对参与导航数据融合的多传感器数据进行自适应切换，并将状态方程维数和量程方程维数进行固化，有效简化了数据融合算法实现，确保无人机在有无GNSS环境下的无缝自主式导航。

主权项

1.一种基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：包括导航系统，所述导航系统包括一套主IMU、主磁力计和主气压高度计，一套备用IMU、备用磁力计和备用气压高度计，以及激光测距传感器，GNSS和视觉导航传感器；还包括以下步骤：S1：由导航计算机读取安装于无人机上的主IMU、主磁力计、主气压高度计、备用IMU、备用磁力计、备用气压高度计、激光测距传感器、GNSS以及视觉导航传感器的数据信息，并将读取的数据信息提供给传感器数据健康管理系统；S2：传感器数据健康状态管理系统对步骤S1获得的所有数据信息进行筛选，筛选出有效传感器数据用于导航数据融合解算，并对气压高度和激光高度进行预处理，计算出两者融合高度，该融合高度将作为导航数据融合的高度量测；传感器数据健康状态管理系统还将判断出GNSS与视觉导航传感器是否处于数据有效工作状态，当传感器数据健康状态管理系统判断出GNSS定位有效时，进入步骤S3，当传感器数据健康状态管理系统判断出GNSS无定位，但视觉导航传感器数据有效时，进入步骤S4；S3：当GNSS定位有效时，如果当前导航周期GNSS数据有更新，将GNSS输出的经纬度转换到以无人机Home点为原点的站心坐标系下，再结合GNSS输出的地理坐标系的速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期GNSS数据没有更新则直接进行捷联纯惯性导航递推；S4：当GNSS无定位，但视觉导航传感器数据有效时，如果当前导航周期视觉导航传感器数据有更新，则采用视觉导航传感器输出的载体坐标系下的位移和速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期视觉导航传感器数据没有更新则直接进行捷联纯惯性导航递推。