[发明专利]一种GNSS与V-SLAM融合定位方法及系统

权利要求书

1.一种GNSS与V-SLAM融合定位方法，其特征在于，包括：

获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据；

基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度；

基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度；

采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果；

所述采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置，具体包括：

判断卫星数目是否小于设定数目；

若是，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置；

若否，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置；

所述松组合模型的建立方法为：

根据所述GNSS定位结果和所述视觉导航结果建立松组合模型；

所述紧组合模型的建立方法为：

根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型；

所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程；

所述紧组合模型状态方程为：

X_TC,k+1＝Φ_TC,k+1,kX_TC,k+w_k；

所述紧组合模型观测方程为：

δz_TC,k+1＝H_k+1X_TC,k+1+v_k+1；

其中，X_TC,k+1为t_k+1时刻的紧组合状态向量，X_TC,k为t_k时刻的紧组合状态向量，Φ_TC,k+1,k为从t_k时刻到t_k+1时刻的紧组合状态转移矩阵，δz_TC,k+1为t_k+1时刻的紧组合观测向量，w_k为过程噪声向量，H_k+1为t_k+1时刻的松组合设计矩阵，v_k+1为观测噪声向量，任意时刻的紧组合观测向量

其中，P¹表示第1颗卫星的伪距观测值，表示根据当前估计位置计算的第1颗卫星的伪距，P²表示第2颗卫星的伪距观测值，表示根据当前估计位置计算的第2颗卫星的伪距，Pⁿ表示第n颗卫星的伪距观测值，表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，Δr_E表示导航坐标系下东方向前后帧的相对位置变化，Δr_N表示导航坐标系下北方向前后帧的相对位置变化，Δr_U表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，表示估计的东方向的速度误差，表示估计的北方向的速度误差，表示估计高程方向的速度误差，表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δV_E导航坐标系下的东方向的速度误差，δV_N导航坐标系下的北方向的速度误差，δV_U为导航坐标系下的高程方向的速度误差，V_E表示东方向上的GNSS速度和视觉导航速度的组合结果，V_N表示北方向上的GNSS速度和视觉导航速度的组合结果，V_U表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e¹表示接收机到第1颗卫星的单位余弦矩阵，e²表示接收机到第2颗卫星的单位余弦矩阵，eⁿ表示接收机到第n颗卫星的单位余弦矩阵，ε¹为第1颗卫星的观测噪声，ε²为第2颗卫星的观测噪声，εⁿ为第n颗卫星的观测噪声，ε^V为视觉导航结果的噪声。