一种基于单天线的GNSS-INS车辆定姿方法

摘要

本发明公开了一种基于单天线的GNSS‑INS车辆定姿方法，包括以下步骤：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；根据车辆位置、速度和航向角速率信息进行车辆航向角解算。采用单天线GNSS与低成本IMU/MEMS传感器组组合，基于农用车辆的运动学模型进行定姿、定向，具有短期精度高、长期稳定性高的优点，避免了使用陀螺仪时的发散现象，同时降低GNSS测姿的噪声水平，测姿精度可提高数倍。

说明书

技术领域

本发明涉及农业车辆自动化领域，尤其涉及一种基于单天线的GNSS-INS车辆定姿方法。

背景技术

随着以生物技术、GNSS技术、信息技术为先导的现在科学技术发展及其在农业上的广泛应用，精准农业应用而生，而农机自动驾驶系统作为其核心载体，其在耕地、播种、施肥、灌溉、植保和收获等各种农艺作业中，将有着十分重要的意义。

为提高农业车辆自动驾驶系统的精度，需要测量农业车辆位置坐标、航向角以及姿态角等导航精度，且测量值应尽量准确。特别是当装备了GNSS接收机的农用车辆在田间行走时，由于轮胎在土壤里受力不均，车体发生颠簸导致GNSS接收天线的位置倾斜，其结果是GNSS天线与车体质心不重合，因此需要精确已知车体的姿态角和航向角。

目前常采用多天线GNSS测姿的方法，但是该方法具有成本高、天线接收延迟、实时性差、安装难度大等缺点；而陆地导航系统的中常用惯性导航INS进行定姿，也由于其成本高等原因不适用于农用车辆的定姿定向。

发明内容

鉴于目前农业车辆自动化领域存在的上述不足，本发明提供一种基于单天线的GNSS-INS车辆定姿方法，能够通过单天线实现对车辆定姿定向，提高了定姿精度。

为达到上述目的，本发明的实施例采用如下技术方案：

一种基于单天线的GNSS-INS车辆定姿方法，所述基于单天线的GNSS-INS车辆定姿方法包括以下步骤：

将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；

通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；

通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；

根据车辆位置、速度和航向角速率信息进行车辆航向角解算。

依照本发明的一个方面，所述将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上时，需保证IMU测量单元的三轴坐标系与机车三轴坐标系一致。

依照本发明的一个方面，所述通过加速度计和陀螺仪融合的方法进行车辆姿态角解算包括：

当车辆处于静止或者匀速运动的情况下时，其加速度值为零，根据加速度计的原理，可准确得到车辆的横滚角和俯仰角：

θ＝sin^-1(a_x)

式中，a_x、a_y为载体坐标系的x轴和y轴的加速度；θ为俯仰角；φ为横滚角；

当车辆运动或者抖动比较大时，引入陀螺仪，采用加速度计和陀螺仪融合的方法进行，具体如下：

首先构建横滚角和俯仰角组合滤波器，具体滤波器如下：

其中待估参数如下：

x(t)＝[θφω_xω_yω_z]^T；

观测向量如下：

z(t)＝[^bf_x^bf_y^bω_ibx^bω_iby^bω_ibz]^T，其中^bf_x,y和^bω_ibx,y,z为加速计和陀螺仪输出值；

状态转移矩阵如下：

观测设计矩阵如下：

f(x(t))＝[sinθcosφω_xω_yω_z]^T

根据上述参数计算得到车辆的横滚角和俯仰角。

依照本发明的一个方面，所述通过加速度计和陀螺仪融合的方法进行车辆姿态角解算包括：

当车辆处于静止或者匀速运动的情况下时，其加速度值为零，根据加速度计的原理，可准确得到车辆的横滚角和俯仰角：

θ＝sin^-1(a_x)

式中，a_x、a_y为载体坐标系的x轴和y轴的加速度；θ为俯仰角；φ为横滚角；

当车辆运动或者抖动比较大时，引入陀螺仪，采用加速度计和陀螺仪融合的方法进行，具体如下：

由于倾角(横滚角或者俯仰角)和倾角角速度存在导数关系，因此可将系统倾斜真实角度可用来做一个状态向量，采用加速度计估计出陀螺仪常值偏差b，以此偏差作为状态向量得到相应的状态方程和观测方程：

式中，ω_gyro为包含固定偏差的陀螺仪输出角速度，为加速度计经处理后得到的角度值，w_g为陀螺仪测量噪声，w_a为加速度计测量噪声，b为陀螺仪漂移误差，w_g与w_a相互独立，并假设二者为满足正态分布的白色噪声。同时设定卡尔曼滤波器的系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，其形式如下：

R＝[r_acce]

式中，q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差，r_acce为加速度计测量噪声；

根据上述参数计算得到车辆的横滚角和俯仰角。

依照本发明的一个方面，所述根据车辆位置、速度和航向角速率信息进行车辆航向角解算包括：

航向角可由东、北向速度计算得到，即：

ψ_p＝arctan(v_E/v_N)

式中，ψ_p为GNSS航向角，v_E、v_N为东北向速度；

当车辆静止或者速度非常小时，引入z轴陀螺仪和GNSS进行组合来进行航向角的高精度解算，系统方程和观测方程如下：

式中，ψ_GNSS为GNSS输出的航向角，b_r为陀螺仪漂移误差，w_hd为航向过程噪声，T_b为一阶马尔科夫相关时间。

线性化之后如下：

当车辆运动时，H_yaw＝[10]，其它情况下H_yaw＝[00]。

依照本发明的一个方面，所述MEMS传感器为六轴MEMS传感器。

本发明实施的优点：本发明所述的基于单天线的GNSS-INS车辆定姿方法，包括以下步骤：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；根据车辆位置、速度和航向角速率信息进行车辆航向角解算。采用单天线GNSS与低成本IMU/MEMS传感器组组合，基于农用车辆的运动学模型进行定姿、定向，该方法吸收了IMU陀螺仪短期精度高和GNSS单天线测姿长期稳定性高的优点，避免了使用陀螺仪时的发散现象，同时降低GNSS测姿的噪声水平，测姿精度可提高数倍。

下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

具体实施方式

实施例一

一种基于单天线的GNSS-INS车辆定姿方法，所述基于单天线的GNSS-INS车辆定姿方法包括以下步骤：

步骤S1：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；

所述步骤S1将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上的具体实施方式可为：将GNSS天线固定安装于机车中心质心位置，以精确测量出载体机车的位置和速度，传感器MEMS-IMU测量单元固定安装于载体机车上时需保证IMU的三轴坐标系与机车三轴坐标系一致；同时IMU测量单元需随机车车轮转向而实时转向，即将IMU单元固装于车轴以便IMU能精确敏感机车的航向角速率，使IMU敏感航向信息与GNSS测量的航向信息一致。

步骤S2：通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；

所述步骤S2通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息，采用了3+3六轴集成MEMS-IMU传感器，能精确获得车辆的航向角速率信息。

步骤S3：通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；

所述步骤S3通过加速度计和陀螺仪融合的方法进行车辆姿态角解算具体可包括：

因MEMS-IMU的陀螺测量精度无法敏感地球自转角速率，故MEMS系统无法进行全自主对准；当车辆处于静止或者匀速运动的情况下时，其加速度值为零，根据加速度计的原理，可准确得到车辆的横滚角和俯仰角：

θ＝sin^-1(a_x)

式中，a_x、a_y为载体坐标系的x轴和y轴的加速度；θ为俯仰角；φ为横滚角；

当车辆运动或者抖动比较大时，由于其它方向的加速度的影响，采用上式不能准确计算出姿态角，为此引入陀螺仪，采用加速度计和陀螺仪融合的方法进行解算，具体如下：

首先构建横滚角和俯仰角组合滤波器，具体滤波器如下：

其中待估参数如下：

x(t)＝[θφω_xω_yω_z]^T；

观测向量如下：

z(t)＝[^bf_x^bf_y^bω_ibx^bω_iby^bω_ibz]^T，其中^bf_x,y和^bω_ibx,y,z为加速计和陀螺仪输出值；

状态转移矩阵如下：

观测设计矩阵如下：

f(x(t))＝[sinθcosφω_xω_yω_z]^T

根据上述参数计算得到车辆的横滚角和俯仰角，从而解算出了车辆的姿态角。

步骤S4：根据车辆位置、速度和航向角速率信息进行车辆航向角解算。

所述步骤S4根据车辆位置、速度和航向角速率信息进行车辆航向角解算的具体实施方式可为：

单天线GNSS测姿主要利用GNSS速度，其航向角可由东、北向速度计算得到，即：

ψ_p＝arctan(v_E/v_N)

式中，ψ_p为GNSS航向角，v_E、v_N为东北向速度；

在实际应用中，GNSS航向角可直接从GNSS接收机输出的NMEA语句中提取，但是当车辆静止或者速度非常小时，数学上造成数值不稳定性，而且速度测量误差可能会淹没真实速度值。

为提高其航向精度，引入z轴陀螺仪和GNSS进行组合，陀螺仪沿载体坐标系z轴向上安装。系统方程和观测方程如下：

式中，ψ_GNSS为GNSS输出的航向角，b_r为陀螺仪漂移误差，w_hd为航向过程噪声，T_b为一阶马尔科夫相关时间。

线性化之后如下：

当车辆运动时，H_yaw＝[10]，其它情况下H_yaw＝[00]。

通过上述的车辆姿态角解算及航向角解算，最终实现了基于单天线的GNSS-INS车辆的定姿。

本发明所述的基于单天线的GNSS-INS车辆定姿方法，包括以下步骤：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；根据车辆位置、速度和航向角速率信息进行车辆航向角解算。采用单天线GNSS与低成本IMU/MEMS传感器组组合，基于农用车辆的运动学模型进行定姿、定向，该方法吸收了IMU陀螺仪短期精度高和GNSS单天线测姿长期稳定性高的优点，避免了使用陀螺仪时的发散现象，同时降低GNSS测姿的噪声水平，测姿精度可提高数倍。本实施例所述的测姿方法结合IMU传感器，经测试其航向角误差小于0.2°，俯仰角和横滚角误差小于0.08°。在实际应用中，采用了3+3六轴集成IMU传感器，体积小，重量轻，性价比高，模块化设计便于集成到农业机械辅助驾驶控制系统之中。

在实际应用中，还可使用倾角传感器代替IMU传感器实现该定姿方案。

实施例二

一种基于单天线的GNSS-INS车辆定姿方法，所述基于单天线的GNSS-INS车辆定姿方法包括以下步骤：

步骤S1：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；

所述步骤S1将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上的具体实施方式可为：将GNSS天线固定安装于机车中心质心位置，以精确测量出载体机车的位置和速度，传感器MEMS-IMU测量单元固定安装于载体机车上时需保证IMU的三轴坐标系与机车三轴坐标系一致；同时IMU测量单元需随机车车轮转向而实时转向，即将IMU单元固装于车轴以便IMU能精确敏感机车的航向角速率，使IMU敏感航向信息与GNSS测量的航向信息一致。

步骤S2：通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；

所述步骤S2通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息，采用了3+3六轴集成MEMS-IMU传感器，能精确获得车辆的航向角速率信息。

步骤S3：通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；

所述步骤S3通过加速度计和陀螺仪融合的方法进行车辆姿态角解算具体可包括：

因MEMS-IMU的陀螺测量精度无法敏感地球自转角速率，故MEMS系统无法进行全自主对准；当车辆处于静止或者匀速运动的情况下时，其加速度值为零，根据加速度计的原理，可准确得到车辆的横滚角和俯仰角：

θ＝sin^-1(a_x)

式中，a_x、a_y为载体坐标系的x轴和y轴的加速度；θ为俯仰角；φ为横滚角；

当车辆运动或者抖动比较大时，由于其它方向的加速度的影响，采用上式不能准确计算出姿态角，为此引入陀螺仪，采用加速度计和陀螺仪融合的方法进行解算，具体如下：

由于倾角(横滚角或者俯仰角)和倾角角速度存在导数关系，因此可将系统倾斜真实角度可用来做一个状态向量，采用加速度计估计出陀螺仪常值偏差b，以此偏差作为状态向量得到相应的状态方程和观测方程：

式中，ω_gyro为包含固定偏差的陀螺仪输出角速度，为加速度计经处理后得到的角度值，w_g为陀螺仪测量噪声，w_a为加速度计测量噪声，b为陀螺仪漂移误差，w_g与w_a相互独立，并假设二者为满足正态分布的白色噪声。同时设定卡尔曼滤波器的系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，其形式如下：

R＝[r_acce]

式中，q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差，r_acce为加速度计测量噪声；

根据上述参数计算得到车辆的横滚角和俯仰角，从而解算出了车辆的姿态角。

步骤S4：根据车辆位置、速度和航向角速率信息进行车辆航向角解算。

所述步骤S4根据车辆位置、速度和航向角速率信息进行车辆航向角解算的具体实施方式可为：

单天线GNSS测姿主要利用GNSS速度，其航向角可由东、北向速度计算得到，即：

ψ_p＝arctan(v_E/v_N)

式中，ψ_p为GNSS航向角，v_E、v_N为东北向速度；

在实际应用中，GNSS航向角可直接从GNSS接收机输出的NMEA语句中提取，但是当车辆静止或者速度非常小时，数学上造成数值不稳定性，而且速度测量误差可能会淹没真实速度值。

为提高其航向精度，引入z轴陀螺仪和GNSS进行组合，陀螺仪沿载体坐标系z轴向上安装。系统方程和观测方程如下：

式中，ψ_GNSS为GNSS输出的航向角，b_r为陀螺仪漂移误差，w_hd为航向过程噪声，T_b为一阶马尔科夫相关时间。

线性化之后如下：

当车辆运动时，H_yaw＝[10]，其它情况下H_yaw＝[00]。

通过上述的车辆姿态角解算及航向角解算，最终实现了基于单天线的GNSS-INS车辆的定姿。

包括以下步骤：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；根据车辆位置、速度和航向角速率信息进行车辆航向角解算。采用单天线GNSS与低成本IMU/MEMS传感器组组合，基于农用车辆的运动学模型进行定姿、定向，该方法吸收了IMU陀螺仪短期精度高和GNSS单天线测姿长期稳定性高的优点，避免了使用陀螺仪时的发散现象，同时降低GNSS测姿的噪声水平，测姿精度可提高数倍。本实施例所述的测姿方法结合IMU传感器，经测试其航向角误差小于0.2°，俯仰角和横滚角误差小于0.08°。在实际应用中，采用了3+3六轴集成IMU传感器，体积小，重量轻，性价比高，模块化设计便于集成到农业机械辅助驾驶控制系统之中。

在实际应用中，还可使用倾角传感器代替IMU传感器实现该定姿方案。

本发明实施的优点：本发明所述的基于单天线的GNSS-INS车辆定姿方法包括以下步骤：将GNSS天线固定安装于车辆中心质心位置，同时将MEMS传感器的IMU测量单元固定安装于车辆的转向车轴上；通过GNSS天线获取车辆位置和速度信息，通过IMU测量单元获取车辆的航向角速率信息；通过加速度计和陀螺仪融合的方法进行车辆姿态角解算；根据车辆位置、速度和航向角速率信息进行车辆航向角解算。采用单天线GNSS与低成本IMU/MEMS传感器组组合，基于农用车辆的运动学模型进行定姿、定向，该方法吸收了IMU陀螺仪短期精度高和GNSS单天线测姿长期稳定性高的优点，避免了使用陀螺仪时的发散现象，同时降低GNSS测姿的噪声水平，测姿精度可提高数倍。本发明所述的定姿方法结合IMU传感器，经测试其航向角误差小于0.2°，俯仰角和横滚角误差小于0.08°。采用了3+3六轴集成IMU传感器，体积小，重量轻，性价比高，模块化设计便于集成到农业机械辅助驾驶控制系统之中。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本领域技术的技术人员在本发明公开的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。