[发明专利]一种基于MEMS的IMU和GPS紧组合导航方法

权利要求书

1.一种基于MEMS的IMU和GPS紧组合导航方法，其特征在于，包括以下步骤：

步骤1：采用最小二乘法对GPS原始数据进行解算，输出GPS导航估计值，包括位置估计值和速度估计值；

步骤2：将GPS位置、速度估计值提供给IMU作为初始位置和初始速度；IMU采用卡尔曼滤波方法进行初始对准；并对IMU测量数据进行解算，得到IMU导航信息；其中，所述IMU导航信息包括姿态、位置和速度信息；

步骤3：利用IMU导航信息计算多普勒频率提供给GPS进行捕获跟踪辅助，提高GPS捕获跟踪性能；

步骤4：建立动态误差模型，采用扩展卡尔曼滤波器将GPS和IMU的导航信息数据融合，反馈误差对同时刻的IMU的导航信息进行校正，输出经过融合并校正的最佳位置、速度、姿态信息。

2.根据权利要求1所述的基于MEMS的IMU和GPS紧组合导航方法，其特征在于，步骤1包括以下步骤：

步骤1.1：对GPS原始数据应用最小二乘法进行导航解算，分别将伪距残差、伪距率作为观测量，计算GPS位置估计值和速度估计值；

步骤1.2：最小二乘法利用公式求解位置和钟差值：

(1)

其中，，是第j颗卫星的伪距残差，；

， (2)

式中，由光速与卫星接收机间的传输时间相乘求得；是卫星接收机坐标间的真实距离；，分别是在ECEF坐标系下真实值与估计值的位置修正误差、时钟修正误差；

利用公式求解速度和钟漂值：

(3)

其中，，是第j颗卫星的伪距率残差，

， (4)

式中，是第j颗卫星与接收机间的单位视线矢量，是伪距速率，是多普勒频率，分别是在ECEF坐标下速度分量、钟漂；

步骤1.3：将ECEF坐标系下的GPS位置信息转换到大地坐标系下，即纬度、经度、高程；将ECEF坐标系下的GPS速度信息转换到地理坐标系下，即东向、北向、天向。

3.根据权利要求1所述的基于MEMS的IMU和GPS紧组合导航方法，其特征在于，步骤2包括以下步骤：

步骤2.1：获取IMU数据的初始信息，即将GPS位置（纬度、经度、高程）、速度（东向、北向、天向）估计值提供给IMU作为初始位置和初始速度；

步骤2.2：IMU建立初始对准误差模型，离散化后采用卡尔曼滤波方法进行初始对准；

惯性导航系统姿态和速度误差方程：

(5)

(6)

静基座时，、很小不计，误差方程展开如下：

(7)

(8)

取状态变量，噪声矩阵，建立对准误差方程：

(9)

式中，

，，

，

量测方程：

(10)

式中，是观测量，是噪声矩阵；

(11)

步骤2.3：利用四阶龙哥库塔算法对IMU测量数据进行解算，解算四元数微分方程、速度微分方程和位置微分方程，得到IMU导航信息，包括姿态、位置和速度信息，并更新相应的姿态、位置、速度。

4.根据权利要求1所述的基于MEMS的IMU和GPS紧组合导航方法，其特征在于，步骤3包括以下步骤：

步骤3.1：利用IMU导航信息计算多普勒频率，计算公式为：

(12)

其中，为载波波长，为卫星星历提供的卫星速度，为IMU提供的载体速度，为第j颗卫星到用户连线方向的单位矢量, 是载体运动产生的多普勒频率，是卫星运动产生的多普勒频率；

步骤3.2：将步骤3.1计算所得多普勒频率加入到GPS跟踪环路当中，由此对捕获跟踪过程进行IMU辅助，以提高GPS捕获跟踪性能。

5.根据权利要求1所述的基于MEMS的IMU和GPS紧组合导航方法，其特征在于，步骤4包括以下步骤：

步骤4.1：建立动态误差模型；

系统状态方程为：

(13)

式中，为状态变量，为系统噪声，为系统矩阵，为噪声矩阵，，；

系统的量测方程：

(14)

式中，为伪距、伪距率残差量测量，为观测矩阵，为量测噪声；

(1)其中，，，

，

，

，

，

(2)其中，当观测到n颗卫星时，

伪距量测方程为： (15)

式中，，

，

，

伪距率量测方程为： (16)

式中，，

，，

，，

其中，是卫星和接收机间的单位视线矢量；是的导数；、分别是经纬度；、分别是卯酉圈曲率半径、子午圈曲率半径；

步骤4.2：状态方程离散化后，根据状态方程和观测方程，并给定初始状态估计和协方差，及过程噪声协方差Q、量测噪声协方差R，采用扩展卡尔曼滤波器将GPS和IMU的导航信息数据融合；

步骤4.3：将卡尔曼滤波结果的反馈误差对同时刻的IMU的导航信息进行校正，输出经过融合并校正的最佳位置、速度、姿态信息。