[发明专利]一种定位导航方法、装置及存储介质

权利要求书

1.一种定位导航方法，其特征在于，包括如下步骤：

根据GPS卫星信号对处于监测范围内的目标载体进行初步定位；

计算所述目标载体在地理坐标系下的运动信息，将所述运动信息转换为世界坐标系下的运动信息，根据经转换得到的运动信息得到所述目标载体的当前位置坐标和所述目标载体的速度，根据所述当前位置坐标和所述速度得到运动轨迹；

获取预设在所述监测范围内信标的信标位置信息，所述信标为用于对所述目标载体进行定位的标识物，根据所述信标位置信息和所述目标载体的当前位置坐标得到所述目标载体与所述信标之间的相对位置数据；

根据所述相对位置数据调整所述目标载体的运动轨迹。

2.根据权利要求1所述的定位导航方法，其特征在于，所述计算所述目标载体在地理坐标系下的运动信息的步骤包括：

通过预设在所述目标载体上的陀螺仪获取角速度数据，根据所述角速度数据建立姿态矩阵，根据四元数与所述姿态矩阵的对应关系计算所述姿态矩阵得到姿态信息，所述姿态矩阵为：

其中，

T₁₁＝cosγcosψ+sinγsinθsinψ，

T₁₂＝-cosγsinψ+sinγsinθcosψ，

T₁₃＝-sinγcosθ，

T₂₁＝cosθsinψ，

T₂₂＝cosθcosψ，

T₂₃＝sinθ，

T₃₁＝sinγcosψ-cosγsinθsinψ，

T₃₂＝-sinγsinψ-cosγsinθcosψ，

T₃₃＝cosγcosθ，

θ为俯仰角，θ＝arcsin(T₃₂)，γ为横滚角，ψ为航向角，

通过预设在所述目标载体上的加速度计获取加速度数据，根据所述加速度数据建立位置矩阵，计算所述位置矩阵得到位置信息，所述位置矩阵为：

其中，

C₁₁＝-sinλ，

C₁₂＝cosλ，

C₁₃＝0，

C₂₁＝sinL cosλ，

C₂₂＝-sinL sinλ，

C₂₃＝cosL，

C₃₁＝cosL cosλ，

C₃₂＝cosL sinλ，

C₃₃＝sinL，

λ为经度，L为纬度，(λ，L)为地理坐标系下目标载体位置点；

所述将所述运动信息转换为世界坐标系下的运动信息的步骤包括：

根据地球坐标系与地理坐标系的换算关系对所述运动信息的位置信息进行换算，所述换算关系为L＝sin^-1(C₃₃)，

3.根据权利要求1所述的定位导航方法，其特征在于，所述获取预设在所述监测范围内信标的信标位置信息的步骤包括：

获取预设在所述目标载体上摄像设备所拍摄的所述监测范围的图像，从所述图像查找信标的特征信息，根据所述特征信息与多个预存特征信息逐一匹配，得到信标位置信息，其中，所述预存特征信息为与所述特征信息对应的信标位置信息。

4.根据权利要求3所述的定位导航方法，其特征在于，所述根据所述信标位置信息和所述目标载体的当前位置坐标得到所述目标载体与所述信标之间的相对位置信息的步骤包括：

所述信标位置信息包括地理坐标系下的信标坐标，根据所述信标坐标计算信标与地面点D的距离H，所述地面点D为信标投影于地面的点，根据所述目标载体的当前位置坐标计算摄影设备至所述地面点D的距离L，根据勾股定理、所述距离H和所述距离L得到所述目标载体与所述信标之间的相对位置数据。