技术领域
[0001] 本
发明涉及导航
定位领域，特别是涉及一种GPS信息缺失情况下的无人车导航方法及系统。
背景技术
[0002] 随着
机器人技术的发展，一定程度上能将人们从繁琐的劳动中解放出来，目前的机器人在很多领域都已取得了不错的应用前景，自从六十年代初诞生以来，机器人在减轻人们的劳动强度、提高社会生产效率及改变生产生活的模式等方面发挥出巨大的作用。经过多年的快速累积式发展，机器人已经在工业自动化各个领域中得到应用，成为关键的
机电一体化设备。同时，伴随着社会的快速发展和人们生活
水平的不断快速提高，一些非制造领域的机器人也相继被研发了出来，例如农业应用机器人、服务型机器人、水下探测机器人、医疗业机器人、娱乐机器人、军用化机器人等各种各样用途的甚至一些特种机器人也纷纷面世，而且正在以飞快的速度向生活生产实用化方向踏步式挺进。可以说，
移动机器人成为经济社会发展中一个绝对不可忽视的重要分支。而目前所广泛应用的移动机器人，基本均为地面移动机器人(UGV，Unmanned Ground Vehicle)，UGV在工程探险、军事侦测、抢险救援等方面有很强的优越性，能够帮助人类在各种恶劣条件下完成任务，但是在一些复杂的工况下，有效的开展UGV的相关工作仍然需要解决诸多问题。
[0003] 对于UGV而言，实现自身准确的导航定位是执行一切任务的必备前提。目前而言，在综合考虑
硬件成本、系统可靠性、导航
精度等因素的
基础上，利用卫星
导航系统(例如GPS(Global Positioning System))实现UGV的导航是首选。然而，GPS卫星
信号受环境影响较大，在复杂建筑群(例如城市环境下)、峡谷、森林等地会因信号衰减或缺失而无法准确导航定位。
发明内容
[0004] 本发明的目的是提供一种GPS信息缺失情况下的无人车导航方法及系统，在GPS信息缺失的情况下实现对无人车的导航定位。
[0005] 为实现上述目的，本发明提供了如下方案：
[0006] 一种GPS信息缺失情况下的无人车导航方法，应用于一种无人车导航定位装置；该导航定位装置包括无人车和至少一架具有自主导航能
力的无人机；所述无人车与所述无人机无线通讯连接；
[0007] 该无人车导航方法包括：
[0008] 获取无人车自身定位系统的定位信息、无人机的广播信息以及接收到的可见星信息；
[0009] 根据所述广播信息和所述可见星信息计算几何精度因子；
[0010] 构建卡尔曼滤波模型，并根据所述几何精度因子的大小确定所述卡尔曼滤波模型量测噪声的协方差矩阵；
[0011] 将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型，得到更新后的无人车定位信息。
[0012] 可选的，所述根据所述广播信息和所述可见星信息计算几何精度因子，具体包括：
[0013] 根据所述无人车的定位信息和所述广播信息计算无人车至各无人机的
位置矢量；
[0014] 根据所述无人车的定位信息和所述可见星信息计算无人车至各可见星的位置矢量；
[0015] 将所述无人车至各无人机的位置矢量和所述无人车至各可见星的位置矢量组合构建成矢量矩阵M；
[0016] 根据公式DOP＝(MTM)-1计算中间矩阵DOP；
[0017] 根据公式 计算几何精度因子GDOP，其中trace()表示求矩阵的迹。
[0018] 可选的，所述构建卡尔曼滤波模型，并根据所述几何精度因子的大小确定所述卡尔曼滤波模型量测噪声的协方差矩阵，具体包括：
[0019] 以无人车的定位信息作为状态变量构建卡尔曼滤波模型的状态转移模型；
[0020] 以无人车至各无人机的相对距离以及无人车至各可见星的伪距作为观测量构建卡尔曼滤波模型的量测模型；
[0021] 以各所述观测量的量测噪声的方差为对
角线元素构建中间协方差矩阵R；
[0022] 根据所述几何精度因子的大小，利用所述中间协方差矩阵R构建量测噪声的协方差矩阵
[0023] 可选的，所述将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型，得到更新后的无人车定位信息，具体包括：
[0024] 将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型得到当前状态的估计结果矩阵；
[0025] 将当前状态的估计结果矩阵中的各个元素作为无人车的坐标数据，得到无人车定位信息。
[0026] 一种GPS信息缺失情况下的无人车导航系统，应用于一种无人车导航定位装置；该导航定位装置包括无人车和至少一架具有自主导航能力的无人机；所述无人车与所述无人机无线通讯连接；
[0027] 该无人车导航系统包括：
[0028] 信息获取模
块，用于获取无人车自身定位系统的定位信息、无人机的广播信息以及接收到的可见星信息；
[0029] 几何精度因子计算模块，用于根据所述广播信息和所述可见星信息计算几何精度因子；
[0030] 卡尔曼滤波模型构建模块，用于构建卡尔曼滤波模型，并根据所述几何精度因子的大小确定所述卡尔曼滤波模型量测噪声的协方差矩阵；
[0031] 定位信息更新模块，用于将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型，得到更新后的无人车定位信息。
[0032] 可选的，所述几何精度因子计算模块包括：
[0033] 车至机矢量计算单元，用于根据所述无人车的定位信息和所述广播信息计算无人车至各无人机的位置矢量；
[0034] 车至星矢量计算单元，用于根据所述无人车的定位信息和所述可见星信息计算无人车至各可见星的位置矢量；
[0035] 矢量矩阵构建单元，用于将所述无人车至各无人机的位置矢量和所述无人车至各可见星的位置矢量组合构建成矢量矩阵M；
[0036] 中间矩阵计算单元，用于根据公式DOP＝(MTM)-1计算中间矩阵DOP；
[0037] 几何精度因子计算单元，用于根据公式 计算几何精度因子GDOP，其中trace()表示求矩阵的迹。
[0038] 可选的，所述卡尔曼滤波模型构建模块包括：
[0039] 状态转移模型构建单元，用于以无人车的定位信息作为状态变量构建卡尔曼滤波模型的状态转移模型；
[0040] 量测模型构建单元，用于以无人车至各无人机的相对距离以及无人车至各可见星的伪距作为观测量构建卡尔曼滤波模型的量测模型；
[0041] 中间协方差矩阵构建单元，用于以各所述观测量的量测噪声的方差为对角线元素构建中间协方差矩阵R；
[0042] 量测噪声的协方差矩阵构建单元，用于根据所述几何精度因子的大小，利用所述中间协方差矩阵R构建量测噪声的协方差矩阵
[0043]
[0044] 可选的，所述定位信息更新模块包括：
[0045] 滤波单元，用于将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型得到当前状态的估计结果矩阵；
[0046] 定位信息确定单元，用于将当前状态的估计结果矩阵中的各个元素作为无人车的坐标数据，得到无人车定位信息。
[0047] 根据本发明提供的具体
实施例，本发明公开了以下技术效果：本发明的GPS信息缺失情况下的无人车导航方法及系统，本发明首先利用无人机的广播信息结合GPS可见星的信息实现无人车定位，能够在可见星信息缺失的情况下依然可以通过无人机的广播信息进行定位。另外，通过计算几何精度因子，并在几何精度因子的基础上确定所述卡尔曼滤波模型量测噪声的协方差矩阵，进而利用卡尔曼滤波对无人车的定位信息进行更新，实现了利用几何精度因子对可见星信息和无人机广播信息
置信度的运算，根据置信度调整可见星信息和广播信息在卡尔曼滤波定位中比重，降低在GPS可见星信息缺失情况下对导航定位的干扰，提高了导航定位精度。
附图说明
[0048] 为了更清楚地说明本发明实施例或
现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0049] 图1为本发明实施例1的GPS信息缺失情况下的无人车导航方法的方法
流程图；
[0050] 图2为本发明实施例2的GPS信息缺失情况下的无人车导航系统的系统结构图。
具体实施方式
[0051] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0052] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0053] 一种GPS信息缺失情况下的无人车导航方法及系统，均应用于一种无人车导航定位装置。该导航定位装置满足以下条件：1、具备至少一架UAV(Unmanned Aerial Vehicle，无人机)，UAV数量需视实际使用情况而定，且UAV具有足够导航精度的自主导航能力(即：UAV具有自主导航定位能力，例如UAV上搭载有GPS导航设备，且该GPS设备可以正常稳定运行，导航设备包括但不限于GPS导航设备；UAV的导航精度可以满足具体的使用要求，使用要求取决于具体情况与任务，一般由相关技术人员确定)。2、UAV具备通过无线通信向UGV发送信息的能力，且利用该无线通信，UGV可实现UAV与UGV之间的相对距离的测算。3、UGV配备有GPS导航设备，除此之外同时具有一定的自主导航定位能力，可在短时间内维持一定的导航精度(例如，装配有MEMS
陀螺仪、MEMS
加速度计、磁罗盘、码盘等)。
[0054] 其中，UAV的数量选择需要考虑如下因素，包括，UGV的运行范围，通信系统信号的
覆盖范围，UGV的导航精度要求，GPS信号的遮挡情况，系统的硬件成本等。UGV的自主导航能力指的是不依赖GPS信息，而采用其他方式进行导航的导航能力。
[0055] 在每个UAV与UGV的通信周期内，UAV都将对外广播自身导航定位的结果，而每个UGV在通信周期内，将接收所有能接收到的UAV的广播信息，同时利用相关
算法完成对相对距离的估算；在得到上述信息的基础上，UGV同时搜索并接收所有能接收到的GPS卫星(GPS可见星)所发射的
卫星信号。
[0056] 本发明的技术方案的基本原理如下：
[0057] 在获取到GPS卫星信号和UAV的
广播信号后，对GPS卫星信号和UAV的广播信号的置信度进行自主评价，在上述信息的基础上，UGV利用自身融合算法实现导航信息的自适应融合，其中置信度自主评价与自适应融合算法指的是：根据现有导航信息对GPS卫星信号和UAV的广播信号所构成的GDOP(Geometric Dilution Precision，几何精度因子)值进行测算，根据测算得到的GDOP值的大小动态调整GPS信息与UAV广播信息的可信度，由于GDOP的值大小反比于GPS可见星信息与UAV广播信息的精度，可以根据GDOP的值大小动态调整GPS可见星信息与UAV广播信息在导航信息融合算法中所占的比重，从而实现：在GPS可见星信息与UAV广播信息置信度较高的情况下，将GPS可见星信息与UAV广播信息以“较相信”的状态引入导航信息融合算法，达到提升导航定位精度的目的；而当GPS可见星信息与UAV广播信息置信度不足的情况下，降低GPS可见星信息与UAV广播信息在导航融合算法中的比重，将GPS可见星信息与UAV广播信息以“低置信”的状态引入导航信息融合算法，防止低置信度的GPS可见星信息与UAV广播信息对UGV自身导航定位精度产生影响。
[0058] 实施例1：
[0059] 该实施例提供一种GPS信息缺失情况下的无人车导航方法。
[0060] 图1为本发明实施例1的GPS信息缺失情况下的无人车导航方法的方法流程图。
[0061] 参见图1，该无人车导航方法包括：
[0062] 步骤101：获取无人车自身定位系统的定位信息、无人机的广播信息以及接收到的可见星信息。
[0063] 在系统开始启动工作后，全部UGV和UAV开始运行，UGV利用除GPS以外的自身导航设备进行自主导航，UAV利用自身全部导航设备进行导航，并且全部UAV对外广播当前自身位置坐标。UGV接收所有能接收到的UAV的广播信息并进行相对距离测算，同时利用自身的GPS设备搜索并接收当前时刻所有GPS可见星的信息并进行导航解算。
[0064] 假定系统中存在m架无人机(分别命名为无人机-1，无人机-2，…，无人机-m)，可见星的数量为n个(分别命名为卫星-1，卫星-2，…，卫星-n)。本发明的方案通常用于可见星数量低于4个情况下。因为一般当可见星数量为4个及以上时，能够实现较为精确的GPS导航。假定下文所有的坐标均已统一至同一
坐标系(例如WGS-84坐标系)
[0065] 无人机-1在k时刻的真实位置坐标为 无人机-2在k时刻的真实位置坐标为 其余无人机真实位置坐标依此类推。UGV在k时刻的真实位置坐标为[0066] 无人机-1在k时刻由自身导航系统提供的位置坐标为 无人机-2在k
时刻由自身导航系统提供的位置坐标为 其余无人机依此类推。UGV在k时刻由自身导航系统提供的位置坐标增量为 在k-1时刻由导
航系统提供的位置坐标为 在k时刻由导航系统提供的位置坐标为
且满足下式：
[0067]
[0068] 无人车利用通信系统解算得到的距离无人机-1的相对距离为：d1，解算得到的距离无人机-2的相对距离为：d2，其余无人机依此类推。通过查阅GNSS星历，可得k时刻卫星-1的准确位置为 其对应的解算得到的伪距观测量为 卫星-2的准确位置为 其对应的解算得到的伪距观测量为 其余卫星依此类
推。
[0069] 步骤102：根据所述广播信息和所述可见星信息计算几何精度因子。
[0070] GDOP指在导航变化量最大的方向上，导航坐标的变化量与距离变化量之比，即导航坐标梯度。而通俗的讲，GDOP表征用户和可见卫星在空间几何分布的好坏，即用于导航的现有参考信息的几何分布好坏(置信度)。
[0071] GDOP的计算过程具体如下：
[0072] 1、根据所述无人车的定位信息和所述广播信息计算无人车至各无人机的位置矢量。
[0073]
[0074] 分别为无人车至各无人机的位置矢量。
[0075] 2、根据所述无人车的定位信息和所述可见星信息计算无人车至各可见星的位置矢量。
[0076]
[0077]
[0078] 分别为无人车至各可见星的矢量。
[0079] 3、将所述无人车至各无人机的位置矢量和所述无人车至各可见星的位置矢量组合构建成可见星矢量矩阵M。
[0080]
[0081] 4、根据公式DOP＝(MTM)-1计算中间矩阵DOP。
[0082] 5、根据公式 计算几何精度因子GDOP，其中()T代表求取矩阵的转置，()-1代表求取矩阵的逆trace()表示求矩阵的迹。迹为矩阵对角线元素的和。
[0083] 步骤103：构建卡尔曼滤波模型，并根据所述几何精度因子的大小确定所述卡尔曼滤波模型量测噪声的协方差矩阵。
[0084] 该步骤103具体包括：
[0085] 1、以无人车的定位信息作为状态变量构建卡尔曼滤波模型的状态转移模型。
[0086] 状态变量为
[0087] Δt代表GPS的时钟误差，一般认为是常值。
[0088] 对应的状态转移模型为：
[0089] Xk＝Xk-1+GΔuk+wk
[0090] 其中，wk代表k时刻的系统过程噪声，且为均值为0、方差为Q的高斯白噪声，为控制输入矩阵。
[0091] 2、以无人车至各无人机的相对距离以及无人车至各可见星的伪距作为观测量构建卡尔曼滤波模型的量测模型。
[0092] 观测量如下：
[0093]
[0094] Δt代表GNSS的时钟误差，c代表光速，vG1,…vGn,v1,…,vm均为量测噪声，且为均值为0、方差依序为RG1,RG2,…,RGn,R1,R2,…,Rm的高斯白噪声，vG1,…vGn是由于电离层误差，
对流层误差等导致，v1,…,vm由于无人机自身导航误差，通信系统误差等导致。
[0095] 基于上述观测量得到量测模型
[0096]
[0097] 基于上述内容，卡尔曼滤波模型可简化为：
[0098] Xk＝ΦXk-1+GΔuk+wk
[0099] Zk＝h(Xk)+vk
[0100] 其中Φ代表一步状态转移矩阵，h()代表量测函数，vk代表k时刻的量测噪声。
[0101] 3、以各所述观测量的量测噪声的方差为对角线元素构建中间协方差矩阵R。
[0102] 即该中间协方差矩阵即为量测噪声vk的协方差矩阵R。
[0103]
[0104] 4、根据所述几何精度因子的大小，利用所述中间协方差矩阵R构建量测噪声的协方差矩阵。
[0105] 量测噪声的协方差矩阵Rk是随着GDOP的变化而动态变化的。对于GDOP的计算而言，小于3可以视为
星座分布较好，即信息的置信度较高；大于8则可以视为星座分布较差，即信息的置信度较低。由此可得量测噪声的协方差矩阵Rk。
[0106]
[0107] 步骤104：将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型，得到更新后的无人车定位信息。
[0108] 该步骤104具体包括：
[0109] 将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型得到当前状态的估计结果矩阵；将当前状态的估计结果矩阵中的各个元素作为无人车的坐标数据，得到无人车定位信息。
[0110] 具体如下：
[0111] 针对该卡尔曼滤波模型，采用扩展Kalman滤波(Extended Kalman Filter，EKF)算法进行滤波处理，具体计算流程如下：
[0112]
[0113] Pk,k-1＝ΦPk-1ΦT+Q
[0114] Kk＝Pk,k-1HT[HPk,k-1HT+Rk]-1
[0115]
[0116] Pk＝[I-KkH]Pk,k-1
[0117] 其中， 为在第k步(第k步对应第k时刻，第k-1步对应第k-1时刻)的量测信息， 代表k-1步滤波结果， 代表状态一步预测，Pk,k-1代表一步预测误差方差，Kk代表滤波增益，Pk代表第k步滤波误差方差，Pk-1代表k-1步的滤波误差方差；I代表对应维度的单位矩阵，其中H代表h函数在 处的雅可比矩阵，而其中的 (第k步滤波估计结果)和Pk(第k步滤波估计结果误差协方差矩阵)即为滤波算法的最终输出，而 中包含着对k时刻的UGV位置的估计结果，利用 中的估计结果，对UGV的导航结果进行更新，即进行如下操作：
[0118]
[0119]
[0120]
[0121] 分别代表 中的第一、二、三个元素。
[0122] 实施例2：
[0123] 该实施例2提供一种GPS信息缺失情况下的无人车导航系统。
[0124] 图2为本发明实施例2的GPS信息缺失情况下的无人车导航系统的系统结构图。
[0125] 该无人车导航系统包括：
[0126] 信息获取模块201，用于获取无人车自身定位系统的定位信息、无人机的广播信息以及接收到的可见星信息；
[0127] 几何精度因子计算模块202，用于根据所述广播信息和所述可见星信息计算几何精度因子；
[0128] 卡尔曼滤波模型构建模块203，用于构建卡尔曼滤波模型，并根据所述几何精度因子的大小确定所述卡尔曼滤波模型量测噪声的协方差矩阵；
[0129] 定位信息更新模块204，用于将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型，得到更新后的无人车定位信息。
[0130] 可选的，所述几何精度因子计算模块202包括：
[0131] 车至机矢量计算单元，用于根据所述无人车的定位信息和所述广播信息计算无人车至各无人机的位置矢量；
[0132] 车至星矢量计算单元，用于根据所述无人车的定位信息和所述可见星信息计算无人车至各可见星的位置矢量；
[0133] 矢量矩阵构建单元，用于将所述无人车至各无人机的位置矢量和所述无人车至各可见星的位置矢量组合构建成矢量矩阵M；
[0134] 中间矩阵计算单元，用于根据公式DOP＝(MTM)-1计算中间矩阵DOP；
[0135] 几何精度因子计算单元，用于根据公式 计算几何精度因子GDOP，其中trace()表示求矩阵的迹。
[0136] 可选的，所述卡尔曼滤波模型构建模块203包括：
[0137] 状态转移模型构建单元，用于以无人车的定位信息作为状态变量构建卡尔曼滤波模型的状态转移模型；
[0138] 量测模型构建单元，用于以无人车至各无人机的相对距离以及无人车至各可见星的伪距作为观测量构建卡尔曼滤波模型的量测模型；
[0139] 中间协方差矩阵构建单元，用于以各所述观测量的量测噪声的方差为对角线元素构建中间协方差矩阵R；
[0140] 量测噪声的协方差矩阵构建单元，用于根据所述几何精度因子的大小，利用所述中间协方差矩阵R构建量测噪声的协方差矩阵
[0141]
[0142] 可选的，所述定位信息更新模块204包括：
[0143] 滤波单元，用于将所述定位信息、所述广播信息、所述可见星信息输入构建好的卡尔曼滤波模型得到当前状态的估计结果矩阵；
[0144] 定位信息确定单元，用于将当前状态的估计结果矩阵中的各个元素作为无人车的坐标数据，得到无人车定位信息。
[0145] 根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0146] 1.本发明有效的解决了传统的依赖于GPS信息的UGV导航算法无法在GPS缺失/失效情况下准确导航定位的问题，有效的扩展了UGV的应用范围，同时提升了UGV的导航精度。
[0147] 2.本发明可以对GPS信息与UAV广播信息的置信度进行在线自主评价，在GPS信息与UAV广播信息置信度较高的情况下，将GPS信息与UAV广播信息以“较相信”的状态引入导航信息融合算法，达到提升导航定位精度的目的；而当GPS信息与UAV广播信息置信度不足的情况下，降低GPS信息与UAV广播信息在导航融合算法中的比重，将GPS信息与UAV广播信息以“低置信”的状态引入导航信息融合算法，防止低置信度的GPS信息与UAV广播信息对UGV自身导航定位精度产生不良影响，提高了定位精度。
[0148] 本
说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0149] 本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。
