CN112945275B - 一种判断载体前进后退及组合导航航向初始对准的方法 - Google Patents

Abstract

本发明涉及一种载体组合导航航向初始对准的方法，具体说是判断载体前进后退及组合导航航向初始对准的方法。它先利用载体中的GNSS RTK定位的位置和速度信息判断载体处于静止或运动状态，再计算载体前向速度，接着判断载体前进后退，最后组合导航初始对准载体航向角。采用该方法进行组合导航航向初始对准，成本较低，自动化程度较高。

Description

一种判断载体前进后退及组合导航航向初始对准的方法

技术领域

本发明涉及一种载体组合导航航向初始对准的方法，具体说是判断载体前进后退及组合导航航向初始对准的方法。

背景技术

随着农业自动化的发展、国家惠农政策的普及以及农民老龄化的比例上升，精准农业逐渐成为国际研究热点。精准农业的普及可以解放双手提高工作效率、提供土地利用率、及时播种收割、节约种子等投入，达到减少劳动力、减少投入、增加产量的目标。

精准农业领域中一般采用GNSS和IMU进行组合导航为载体提供位置、速度、姿态信息。组合导航第一步是初始对准即给定IMU初始的位置、速度、姿态信息。其位置、速度信息可以由GNSS给定，姿态信息中的航向现有技术的做法是利用磁力计估算初始航向或者双天线GNSS量测或者载体初始指北形成水平指北方位系统。然而，利用磁力计估算的方式需要安装9轴的IMU，利用双天线GNSS量测的方式需要安装两个GNSS，成本较高。利用载体初始指北形成水平指北方位系统的方式需要在载体每次启动时由人工操作，自动化程度较低。

发明内容

本发明要解决的技术问题是提供一种判断载体前进后退及组合导航航向初始对准的方法，采用该方法进行组合导航航向初始对准，成本较低，自动化程度较高。

为解决上述问题，提供以下技术方案：

本发明的判断载体前进后退及组合导航航向初始对准的方法的特点是包括如下步骤：

第一步，利用载体中的GNSS RTK定位的位置和速度信息判断载体处于静止或运动状态。

第二步，计算载体前向速度

当载体处于静止状态时，标定加速度计的零偏以及复位载体前向速度，

V_front＝0

其中，V_front为载体前向速度，当载体静止时复位置0，acc_bias为载体前向加速度计的零偏，acc_k为k时刻的加速度测量值，n为载体静止时加速度计量测计数值。

当载体处于运动状态时，先补偿修正加速度计零偏

acc^comp _k＝acc_k-acc_bias

其中，acc^comp _k为补偿修正之后的加速度量测值。

然后，积分得到载体前向速度

其离散形式为

V_{front_k}＝V_{front_k-1}+acc^comp _k*dt

其中，V_{front_k}为k时刻的载体前向速度，dt为量测更新周期。

第三步，载体前进后退判断：

其中，V_state为载体运动状态：1代表前进，-1代表后退，0代表不满足判断条件。V_limit为满足组合导航初始对准的速度下限门限值。

第四步，组合导航初始对准载体航向角：

利用GNSS速度来初始化组合导航的初始航向即：

其中，

为载体车头航向角；V_E为GNSS测量得到的载体东向速度；V_N为GNSS测量得到的载体北向速度；π为1弧度即180度；V_state为载体运动状态，1代表前进，-1代表后退，0代表无法判断前进或后退，不满足组合导航初始对准条件即不进行初始化。

其中，第一步中判断载体静止或运动的条件为

V_{GNSS_K}＜V_threshold

P_k/k+1＜P_threshold

其中，V_{GNSS_K}为GNSS在K时刻的速度；V_threshold为静止判断速度阈值，这里取0.1m/s；P_k/k+1为K到K+1时刻GNSS位置的变化量，

P_threshold为静止判断位置阈值。

载体静止或运动状态判断：

采用直方统计的方法设定载体静止状态判断阈值，即当K时刻满足上述判断条件时，直方统计值累加即H＝H+1；当K时刻不满足上述判断条件，则直方统计值累减即H＝H-1，最小到H＝0；如果H>N，即连续N个时刻点都满足静止条件，此时判定载体处于静止状态，否则载体处于运动状态，其中N为预设阀值。

采取以上方案，具有以下优点：

由于本发明的判断载体前进后退及组合导航航向初始对准的方法先利用载体中的GNSS RTK定位的位置和速度信息判断载体处于静止或运动状态，再计算载体前向速度，接着判断载体前进后退，最后组合导航初始对准载体航向角。判断载体处于静止或运动状态利用单天线GNSS和6轴IMU即可，从而无需安装双天线GNSS和9轴IMU，组合导航的成本较低。而且，采用该方法无需人工初始指北，大大提高了农机作业的自动化程度。

附图说明

图1是本发明的判断载体前进后退及组合导航航向初始对准的方法中载体的运动轨迹示意图。

具体实施方式

以下结合附图对本发明做进一步详细说明。

本发明的判断载体前进后退及组合导航航向初始对准的方法包括如下步骤：

第一步，利用载体中的GNSS RTK定位的位置和速度信息判断载体处于静止或运动状态。

如图1所示，车辆运动状态：车辆在K时刻位置在p_k，在下一时刻可能前进运动到p'_k+1，也可能后退运动到p_k+1。

判断载体静止或运动的条件为

V_{GNSS_K}＜V_threshold

P_k/k+1＜P_threshold

其中，V_{GNSS_K}为GNSS在K时刻的速度；V_threshold为静止判断速度阈值，这里取0.1m/s；P_k/k+1为K到K+1时刻GNSS位置的变化量，

P_threshold为静止判断位置阈值，本实施例中取0.02m。

载体静止或运动状态判断：

采用直方统计的方法设定载体静止状态判断阈值，即当K时刻满足上述判断条件时，直方统计值累加即H＝H+1。

当K时刻不满足上述判断条件，则直方统计值累减即H＝H-1，最小到H＝0。

如果H>N，即连续N个时刻点都满足静止条件，此时判定载体处于静止状态，否则载体处于运动状态，其中N为预设阀值，本实施例中N设定为10。

第二步，计算载体前向速度

当载体处于静止状态时，标定加速度计的零偏以及复位载体前向速度，

V_front＝0

其中，V_front为载体前向速度，当载体静止时复位置0，acc_bias为载体前向加速度计的零偏，acc_k为k时刻的加速度测量值，n为载体静止时加速度计量测计数值。

当载体处于运动状态时，先补偿修正加速度计零偏

acc^comp _k＝acc_k-acc_bias

其中，acc^comp _k为补偿修正之后的加速度量测值。

然后，积分得到载体前向速度

其离散形式为

V_{front_k}＝V_{front_k-1}+acc^comp _k*dt

其中，V_{front_k}为k时刻的载体前向速度，dt为量测更新周期，本实施例中dt是50Hz即0.02秒。

第三步，载体前进后退判断：

其中，V_state为载体运动状态：1代表前进，-1代表后退，0代表不满足判断条件。V_limit为满足组合导航初始对准的速度下限门限值，本实施例中取0.6m/s。

第四步，组合导航初始对准载体航向角：

利用GNSS速度来初始化组合导航的初始航向即：

其中，

为载体车头航向角；V_E为GNSS测量得到的载体东向速度；V_N为GNSS测量得到的载体北向速度；π为1弧度即180度；V_state为载体运动状态，1代表前进，-1代表后退，0代表无法判断前进或后退，不满足组合导航初始对准条件即不进行初始化。

Claims (2)

1.一种判断载体前进后退及组合导航航向初始对准的方法，其特征在于包括如下步骤：

第一步，利用载体中的GNSS RTK定位的位置和速度信息判断载体处于静止或运动状态；

第二步，计算载体前向速度

当载体处于静止状态时，标定加速度计的零偏以及复位载体前向速度，

V_front＝0

其中，V_front为载体前向速度，当载体静止时复位置0，acc_bias为载体前向加速度计的零偏，acc_k为k时刻的加速度测量值，n为载体静止时加速度计量测计数值；

当载体处于运动状态时，先补偿修正加速度计零偏

acc^comp _k＝acc_k-acc_bias

其中，acc^comp _k为补偿修正之后的加速度量测值；

然后，积分得到载体前向速度

其离散形式为

V_{front_k}＝V_{front_k-1}+acc^comp _k*dt

其中，V_{front_k}为k时刻的载体前向速度，dt为量测更新周期；

第三步，载体前进后退判断：

其中，V_state为载体运动状态：1代表前进，-1代表后退，0代表不满足判断条件；V_limit为满足组合导航初始对准的速度下限门限值；

第四步，组合导航初始对准载体航向角：

利用GNSS速度来初始化组合导航的初始航向即：

其中，

为载体车头航向角；V_E为GNSS测量得到的载体东向速度；V_N为GNSS测量得到的载体北向速度；π为1弧度即180度；V_state为载体运动状态，1代表前进，-1代表后退，0代表无法判断前进或后退，不满足组合导航初始对准条件即不进行初始化。

2.如权利要求1所述的判断载体前进后退及组合导航航向初始对准的方法，其特征在于第一步中判断载体静止或运动的条件为

V_{GNSS_K}＜V_threshold

P_k/k+1＜P_threshold

其中，V_{GNSS_K}为GNSS在K时刻的速度；V_threshold为静止判断速度阈值，这里取0.1m/s；P_k/k+1为K到K+1时刻GNSS位置的变化量，

P_threshold为静止判断位置阈值；

载体静止或运动状态判断：

采用直方统计的方法设定载体静止状态判断阈值，即当K时刻满足上述判断条件时，直方统计值累加即H＝H+1；当K时刻不满足上述判断条件，则直方统计值累减即H＝H-1，最小到H＝0；如果H>N，即连续N个时刻点都满足静止条件，此时判定载体处于静止状态，否则载体处于运动状态，其中N为预设阀值。

Images