CN111551973A - 一种露天矿无人驾驶惯导系统的故障检测和矫正方法 - Google Patents

Abstract

本发明公开了一种露天矿无人驾驶惯导系统的故障检测和矫正方法，基于GNSS+RTK+IMU融合惯导系统，在硬件上无任何添加改动，充分利用惯导的自身特性，通过对定位数据和融合数据的二次处理，与矿卡惯导安装自身物理属性，实现惯导的故障检测和矫正，同时提高了惯导的稳定性和定位准确性。

Description

一种露天矿无人驾驶惯导系统的故障检测和矫正方法

技术领域

本发明属于矿用运输车辆无人驾驶技术领域，尤其涉及一种露天矿无人驾驶惯导系统的故障检测和矫正方法。

背景技术

当前矿区面临高温、寒冷、多风、危险度高等恶劣作业环境，摧残着矿区作业司机，也给矿区安全生产带来严峻挑战。矿区积极推进创建一个高效、绿色、安全的智慧矿区作业系统。

随着传感器、5G、CV2X、定位等技术不断成熟，无人驾驶矿卡已成为智慧矿上必不可少的一部分，其中定位导航是整个无人驾驶技术的基础核心。

采用以GNSS+RTK+IMU为核心定位无人矿卡，在长期运行过程中，会由于一些异常因素如RTK数据丢失、IMU累计误差、车辆颠簸、GNSS定位异常等等情况下，出现惯导定位精度降低，在严重情况下定位精度下降到米级以上，无法满足矿区无人驾驶应用技术指标，严重威胁到矿区无人运输的安全性。因此必须实时监测惯导系统状态，并在惯导系统故障后进行矫正，保证无人驾驶系统长时间安全、可靠运行。

发明内容

为了解决上述已有技术存在的不足，本发明提供一种露天矿无人驾驶惯导系统的故障检测和矫正方法，提高露天矿无人驾驶安全性和稳定性，本发明的具体技术方案如下：

一种露天矿无人驾驶惯导系统的故障检测和矫正方法，其特征在于，所述故障检测和矫正方法基于惯导检测和矫正系统实现，所述惯导检测和矫正系统包括惯导系统和惯导故障检测和矫正模块，其中，

所述惯导系统以GNSS、RTK和IMU为基础核心，包括在矿卡车顶的前轮连线中心处安装惯导模块、在所述惯导模块前方以惯导模块为中点对称安装的GPS主天线和GPS从天线，所述惯导模块载体坐标系与矿卡自身载体坐标系一致；

所述惯导故障检测和矫正处理模块，实时监测所述惯导系统输出的GNSS及RTK获取的GPS主天线定位数据和GNSS、RTK及IMU获取的车辆质心定位数据；

所述故障检测和矫正方法包括以下步骤：

S1：以惯导模块为坐标原点，以东、北、天三个方向为坐标轴，建立惯导模块载体坐标系；测量GPS主天线在惯导模块载体坐标系下向量Z1、车辆质心在惯导模块载体坐标系下向量Z2，将Z1，Z2分别配置到惯导模块，使得惯导系统输出的车辆质心定位数据中位置信息为矿卡质心坐标，计算得出GPS主天线到矿卡质心向量Z＝Z1-Z2，将Z保存到本地配置文件，供惯导系统故障检测和矫正使用；

S3：配置惯导模块，定时输出GPS主天线定位数据和车辆质心定位数据，保存配置，将惯导模块接入无人驾驶系统；

S4：启动惯导故障检测和矫正处理模块，以阻塞的方式接收惯导系统输出的GPS主天线定位数据和车辆质心定位数据，利用数据中自带时间戳信息，确认两组数据时间同步；

S5：提取GPS主天线定位数据和车辆质心定位数据在导航坐标系下纬度、经度、高度信息，分别记为矩阵φ_n1和φ_n2；

S6：提取GPS主天线定位数据和车辆质心定位数据中定位状态、RTK龄期、纬度方差、经度方差状态信息，分别记为H₁和H₂；

S7：将H₁和H₂中状态量与惯导系统正常状态H比较，如果H₁∈H或H₂∈H，则当前惯导系统状态正常，否则惯导系统状态异常，发送至无人驾驶系统决策机构；

S8：根据惯导模块在导航坐标系下的纬度，计算出导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b和矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ；

S9：将导航坐标系位置矩阵φ_n1和φ_n2分别乘以导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b，得到矿卡自身载体坐标系下位置矩阵，记为φ_b1和φ_b2，由φ_b1和φ_b2得出在矿卡自身载体坐标系下车辆质心到GPS主天线定位数据的向量φ＝φ_b1-φ_b2；

S10：设定P为定位误差允许范围矩阵，若Z-P<φ<Z+P成立，则当前惯导系统获取的车辆质心定位正常，将车辆质心定位数据在导航坐标系下纬度、经度、高度信息发送至无人驾驶系统决策机构；

S11：若Z-P<φ<Z+P不成立，则当前惯导系统获取的车辆质心定位异常，利用φ_b1和Z对车辆质心定位矫正，计算φ_b1-Z得到矿卡自身载体坐标系质心向量，乘以矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ得到导航坐标系下车辆质心向量，并发送给无人驾驶系统决策机构，实现无人驾驶惯导系统的检测和矫正。

本发明的有益效果在于：

1.本发明提出采用监测定位数据的定位状态、RTK龄期、纬度方差、经度方差等状态信息与监测定位数据偏差同步进行的故障检测方法，有效提高惯导系统定位异常检测效率；

2.本发明通过对GPS主天线定位数据和车辆质心定位数据二次处理，结合矿卡惯导安装物理属性，提出惯导定位异常数据的矫正方法，有效增强了惯导系统定位稳定性和精确性。

3.本发明提高了惯导的稳定性和定位准确性，从而提高矿区无人驾驶的可靠性和安全性，间接提高的矿区作业的效率和精度，为智慧矿区创造的经济收益。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：

图1是本发明的方法流程图；

图2是本发明的惯导系统安装示意图。

附图标号说明：

1-矿卡质心；2-惯导模块；3-GPS主天线；4-GPS从天线。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

本发明提供一种解决露天矿无人驾驶惯导系统故障检测和矫正方法，将惯导系统安装到无人矿卡上，惯导故障检测和矫正模块启动后，以阻塞的方式接收惯导系统数据，如果接收超时，惯导系统异常，上报无人驾驶决策机构。

将接收数据以本地追加方式存储，判断接收数据类型，提取GPS主天线定位数据和车辆质心定位数据，丢弃无用数据。

解析GPS主天线定位数据和车辆质心定位数据，提取时间戳信息，判断时间是否同步，如果时间不同步，返回继续接收。如果时间同步继续解析GPS主天线定位数据和车辆质心定位数据的定位状态、RTK龄期、纬度方差、经度方差四个要素。将GPS主天线定位数据和车辆质心定位数据的状态四要素分别判断是否为惯导系统正常状态子集，如果GPS主天线定位数据和车辆质心定位数据状态四要素都不符合子集条件，表示当前惯导系统异常并上报无人驾驶决策机构。否则惯导系统状态正常。

实时动态监测GPS主天线定位数据和车辆质心定位数据中位置信息，利用位置信息的纬度信息，得出导航坐标系和载体坐标系间转换矩阵，将GPS主天线定位数据和车辆质心定位数据中位置信息由导航坐标系转为载体坐标系，计算载体坐标系下GPS主天线位置与车辆质心位置在东北天三个方向的差值，得到惯导实时动态GPS主天线和车辆质心计算距离。计算GPS主天线与车辆质心动态计算距离与安装实际物理距离差值，得到GPS主天线和车辆质心的动态偏差。

如果动态偏差处于故障检测定位误差允许范围，说明惯导系统位置定位正常，将车辆质心定位数据发送无人驾驶决策机构。否则表示当前惯导系统定位异常，无法提供准确定位，对当前车辆质心定位数据矫正，利用GPS主天线定位和惯导安装物理尺寸，推算出当前车辆质心实际位置信息，将实际质心位置信息发送至无人驾驶决策机构。

具体的，一种露天矿无人驾驶惯导系统的故障检测和矫正方法，其特征在于，故障检测和矫正方法基于惯导检测和矫正系统实现，惯导检测和矫正系统包括惯导系统和惯导故障检测和矫正模块，其中，

惯导系统以GNSS、RTK和IMU为基础核心，包括在矿卡车顶的前轮连线中心处安装惯导模块、在惯导模块前方以惯导模块为中点对称安装的GPS主天线和GPS从天线，惯导模块载体坐标系与矿卡自身载体坐标系一致；为了避免安装误差和测量误差较大，惯导模块需安装在相对标准的水平面，如图2所示，确保安装可靠、牢固；

惯导故障检测和矫正处理模块，实时监测惯导系统输出的GNSS及RTK获取的GPS主天线定位数据和GNSS、RTK及IMU获取的车辆质心定位数据；

故障检测和矫正方法包括以下步骤：

S1：以惯导模块为坐标原点，以东、北、天三个方向为坐标轴，建立惯导模块载体坐标系；测量GPS主天线在惯导模块载体坐标系下向量Z1、车辆质心在惯导模块载体坐标系下向量Z2，将Z1，Z2分别配置到惯导模块，使得惯导系统输出的车辆质心定位数据中位置信息为矿卡质心坐标，计算得出GPS主天线到矿卡质心向量Z＝Z1-Z2，将Z保存到本地配置文件，供惯导系统故障检测和矫正使用；

S3：配置惯导模块，定时输出GPS主天线定位数据和车辆质心定位数据，保存配置，将惯导模块接入无人驾驶系统；

S4：启动惯导故障检测和矫正处理模块，以阻塞的方式接收惯导系统输出的GPS主天线定位数据和车辆质心定位数据，利用数据中自带时间戳信息，确认两组数据时间同步；具体的，如果阻塞超时，表示当前惯导模块异常，上报无人驾驶决策机构；否则将接收数据采用尾部追加方式本地存储，提取GPS主天线定位数据和车辆质心定位数据，舍弃无用数据；解析提取GPS主天线定位数据和车辆质心定位数据中时间戳信息，确认两组数据时间同步，如果不同步，继续接收惯导数据；如果同步进行后续分析处理操作；

S5：提取GPS主天线定位数据和车辆质心定位数据在导航坐标系下纬度、经度、高度信息，分别记为矩阵φ_n1和φ_n2；

S6：提取GPS主天线定位数据和车辆质心定位数据中定位状态、RTK龄期、纬度方差、经度方差状态信息，分别记为H₁和H₂；

S7：将H₁和H₂中状态量与惯导系统正常状态H比较，其中，H是一个集合，包含定位状态离散量、RTK龄期、纬度方差、经度方差范围，如果H₁∈H或H₂∈H，则当前惯导系统状态正常，否则惯导系统状态异常，发送至无人驾驶系统决策机构；

S8：根据惯导模块在导航坐标系下的纬度，计算出导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b和矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ；

S9：将导航坐标系位置矩阵φ_n1和φ_n2分别乘以导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b，得到矿卡自身载体坐标系下位置矩阵，记为φ_b1和φ_b2，φ_b1＝φ_n1*C_n ^b，φ_b2＝φ_n2*C_n ^b，由φ_b1和φ_b2得出在矿卡自身载体坐标系下车辆质心到GPS主天线定位数据的向量φ＝φ_b1-φ_b2；

S10：设定P为定位误差允许范围矩阵，若Z-P<φ<Z+P成立，则当前惯导系统获取的车辆质心定位正常，将车辆质心定位数据在导航坐标系下纬度、经度、高度信息发送至无人驾驶系统决策机构；

S11：若Z-P<φ<Z+P不成立，则当前惯导系统获取的车辆质心定位异常，利用φ_b1和Z对车辆质心定位矫正，φ_b2＝φ_b1-Z，得到矿卡自身载体坐标系质心向量φ_b2，再乘以矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ得到导航坐标系下车辆质心向量φ_n2，即φ_n2＝φ_b2*C_b ⁿ，并发送给无人驾驶系统决策机构，实现无人驾驶惯导系统的检测和矫正。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本发明中，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，而不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种露天矿无人驾驶惯导系统的故障检测和矫正方法，其特征在于，所述故障检测和矫正方法基于惯导检测和矫正系统实现，所述惯导检测和矫正系统包括惯导系统和惯导故障检测和矫正模块，其中，

所述惯导系统以GNSS、RTK和IMU为基础核心，包括在矿卡车顶的前轮连线中心处安装惯导模块、在所述惯导模块前方以惯导模块为中点对称安装的GPS主天线和GPS从天线，所述惯导模块载体坐标系与矿卡自身载体坐标系一致；

所述惯导故障检测和矫正处理模块，实时监测所述惯导系统输出的GNSS及RTK获取的GPS主天线定位数据和GNSS、RTK及IMU获取的车辆质心定位数据；

所述故障检测和矫正方法包括以下步骤：

S1：以惯导模块为坐标原点，以东、北、天三个方向为坐标轴，建立惯导模块载体坐标系；测量GPS主天线在惯导模块载体坐标系下向量Z1、车辆质心在惯导模块载体坐标系下向量Z2，将Z1，Z2分别配置到惯导模块，使得惯导系统输出的车辆质心定位数据中位置信息为矿卡质心坐标，计算得出GPS主天线到矿卡质心向量Z＝Z1-Z2，将Z保存到本地配置文件，供惯导系统故障检测和矫正使用；

S3：配置惯导模块，定时输出GPS主天线定位数据和车辆质心定位数据，保存配置，将惯导模块接入无人驾驶系统；

S4：启动惯导故障检测和矫正处理模块，以阻塞的方式接收惯导系统输出的GPS主天线定位数据和车辆质心定位数据，利用数据中自带时间戳信息，确认两组数据时间同步；

S5：提取GPS主天线定位数据和车辆质心定位数据在导航坐标系下纬度、经度、高度信息，分别记为矩阵φ_n1和φ_n2；

S6：提取GPS主天线定位数据和车辆质心定位数据中定位状态、RTK龄期、纬度方差、经度方差状态信息，分别记为H₁和H₂；

S7：将H₁和H₂中状态量与惯导系统正常状态H比较，如果H₁∈H或H₂∈H，则当前惯导系统状态正常，否则惯导系统状态异常，发送至无人驾驶系统决策机构；

S8：根据惯导模块在导航坐标系下的纬度，计算出导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b和矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ；

S9：将导航坐标系位置矩阵φ_n1和φ_n2分别乘以导航坐标系到矿卡自身载体坐标系转换的姿态矩阵C_n ^b，得到矿卡自身载体坐标系下位置矩阵，记为φ_b1和φ_b2，由φ_b1和φ_b2得出在矿卡自身载体坐标系下车辆质心到GPS主天线定位数据的向量φ＝φ_b1-φ_b2；

S10：设定P为定位误差允许范围矩阵，若Z-P<φ<Z+P成立，则当前惯导系统获取的车辆质心定位正常，将车辆质心定位数据在导航坐标系下纬度、经度、高度信息发送至无人驾驶系统决策机构；

S11：若Z-P<φ<Z+P不成立，则当前惯导系统获取的车辆质心定位异常，利用φ_b1和Z对车辆质心定位矫正，计算φ_b1-Z得到矿卡自身载体坐标系质心向量，乘以矿卡自身载体坐标系到导航坐标系转换的姿态矩阵C_b ⁿ得到导航坐标系下车辆质心向量，并发送给无人驾驶系统决策机构，实现无人驾驶惯导系统的检测和矫正。

Images