一种用于组合导航动基座初始对准的载体前进/倒车判定方法

技术领域

本发明属于多信息融合领域，应用在精准农业、自动驾驶等领域，涉及一种用于组合导航动基座初始对准的载体前进/倒车判定方法。

背景技术

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

精准农业技术的重要组成部分就是具有信息获取及处理系统的智能化的农业机械。这些智能化的农业机械通常采用了全球导航卫星系统(GNSS)、自动监测和自动控制技术，装备有各种传感器和由微处理器组成的智能控制器。并且，智能化的农业机械常采用惯性/卫星组合导航的方式为载体提供全面的位置、速度和姿态信息。

智能化的农业机械，一般在车体顶部安装单天线GNSS定位模块和IMU，并通过GNSS和IMU进行组合导航为载体提供位置、速度、姿态信息。组合导航第一步是初始对准，即给定IMU初始的位置、速度、姿态信息，其位置、速度信息可以由GNSS给定，针对姿态信息中的航向，常规做法是利用磁力计估算初始航向或者双天线GNSS量测得到或者载体初始指北形成水平指北方位系统或者在高动态机动条件下采用最优滤波估计。针对精准农业领域中低成本单天线和低精度6轴IMU的系统，既没有磁力计信息可以利用，也没有双天线的量测值，更不可能获得高动态条件。如果要求用户每次启动设备车体朝向正北方向进行初始对准更是不现实，唯一只能利用GNSS速度信息估算初始航向。而GNSS速度估算航向更确切的说是估算航迹的方向而不是车辆车头的方向，因为倒车的情况下利用GNSS速度估算的航向和车辆车头的航向是正好反向，二者相差180度。如果不能正确判断载体的前进/倒车状态而直接把速度估算的航向用于组合导航的初始对准，那么组合导航整个系统一定是发散的。

发明内容

[技术问题]

本发明解决的技术问题：初始对准时，利用GNSS速度信息估算初始航向，但是此时载体必需要处于前进运动状态，否则初始航向会相差180度。

[技术方案]

本发明提供一种用于组合导航动基座初始对准的载体前进或倒车状态的判定方法，所述组合导航是指利用单天线GNSS和6轴IMU进行组合导航，所述方法包括如下步骤：

(1)利用载体前向(轴)加速度判定载体的疑似运动状态(前进加速或倒车减速)，

①判断载体是否处于运动状态未知，所述运动状态未知是指当载体静止或接近匀速运动，此时前向轴加速度小于阈值；

②如果载体处于运动状态未知，则判断一段时间内载体的前向轴加速度的变化趋势，当该时间段内载体的前向轴加速度持续大于设定阈值时，对该时间段内的前向轴加速度利用最小二乘法进行直线拟合；

③如果拟合得到的直线的斜率大于设定阈值，则把载体的状态定义为疑似载体前进状态；

(2)在判定疑似载体前进状态的基础上，利用GNSS速度进一步判定载体是前进状态还是其他状态；

如果，从当前k时刻持续到未来k+i时刻，GNSS的速度都大于设定阈值，则可以判定载体处于前进状态，否则，载体运动状态置为运动状态未知。

在本发明的一种实施方式中，步骤(1)①中阈值设为1.8m/s

在本发明的一种实施方式中，步骤(1)②判断一段时间内载体的前向轴加速度的变化趋势的方法是：

Acc

M

Acc

其中：

Acc

Acc

Acc

在本发明的一种实施方式中，步骤(1)②中对该时间段内的前向轴加速度利用最小二乘法进行直线拟合的方法是：

y＝a*x+b

其中：

x轴数据：k到k-m时刻前向轴加速度对应的时间增量；

y轴数据：k时刻到k-m时刻的前向轴加速度；

m：参与直线拟合的数据个数，m的取值可以综合考虑直线拟合的误差以及载体的加速时间。

在本发明的一种实施方式中，步骤(1)③中，直线的斜率的阈值可以设为4.0。

在本发明的一种实施方式中，步骤(2)中，利用GNSS速度进一步判定载体是前进状态还是其他状态的方法是：

V

M

V

其中：

V

V

V

[有益效果]

前进加速和倒车减速，对于前向轴加速度来说其实是同一个状态(均为正)，直线拟合的斜率也很相似，本发明是先利用载体前向轴加速度判断出前进加速或后退减速这两种疑似前进状态，然后再利用GNSS速度从这两个状态里判断出真实的状态。

具体地，当载体前向轴加速度连续多个历元即连续一段时间都大于一定的阈值时，对过去一段时间内的前向轴加速度进行直线拟合。如果拟合直线的斜率大于设定阈值，说明这段时间内载体处于连续前进加速或者持续倒车减速状态，把当前状态定义为载体疑似前进状态。紧接着统计未来一段时间内的GNSS速度，如果GNSS速度持续大于设定阈值，那么可判定载体处于前进状态。因为如果载体是倒车减速状态，那么GNSS速度会持续减小，不可能持续大于设定阈值。判定前进状态后可以利用GNSS的速度估算载体的航向进行组合导航初始对准。

附图说明

图1本发明方法的流程图。

图2载体持续前进加速时前向(轴)加速度(a)、加速度变化趋势(b)及GNSS速度变化(c)。

图3载体先加速后退后刹车时前向(轴)加速度(a)、加速度变化趋势(b)及GNSS速度变化(c)。

具体实施方式

实施例1

本实施例所要判断的“前进”状态，是指载体即农机的车头向前移动的状态，区别于倒车状态；“疑似前进”状态包括：载体即农机处于加速前进或减速倒车的状态。

本实施例的“前向轴加速度”中的“前”是指在车体所处的笛卡尔坐标系内，本领域常用的“前右下或前左上”坐标，与车辆实际是加速前进还是减速倒车没有关系。本实施例的“前向轴加速度”中的“加速度”表示速度的变化趋势。加速前进和减速后退时，都会出现“前向轴加速度”为正的情况。

(一)载体疑似前进状态判定：

(1)判断载体是否处于运动状态未知：

当载体静止或接近匀速运动时，其前向轴加速度很小，一定小于设定阈值，此时可以直接判定载体处于运动状态未知。其他状态则判定为“非”运动状态未知。

(2)判断载体前向轴加速度是否持续大于设定阈值

载体坐标系采用前-右-下笛卡尔坐标，当载体前向轴加速度持续大于设定阈值时，说明载体可能处于持续前进加速或持续后退减速状态，可开启直线拟合。判断前向轴加速度的变化趋势的方法如下：

Acc

M

Acc

其中：

Acc

Acc

Acc

由于农机载体动态较低，加减速持续时间很短，这里取n＝3，即连续4个历元的前向轴加速度满足上述条件。本发明里IMU更新频率为50Hz即更新周期为20ms，这里连续4个历元的持续时间为80ms。

(3)前向轴加速度直线拟合

当载体前向轴加速度满足上述条件时(即，连续4个历元的前向轴加速度都大于载体前向轴加速度阈值)，对k时刻到k-m时刻的前向轴加速度采用最小二乘法进行直线拟合，m:参与直线拟合的数据个数。拟合直线方程的形式为：

y＝a*x+b

其中：

x轴数据：x轴数据为k到k-m时刻前向轴加速度对应的时间增量，即0*dt、1*dt……(m-1)*dt。其中dt为IMU更新周期即0.02s。

y轴数据：k时刻到k-m时刻的前向轴加速度；

m:参与直线拟合的数据个数，综合考虑直线拟合的误差以及载体的加速时间，这里m取15即对过去300ms的数据进行直线拟合。

(4)判断载体是否处于“疑似载体前进”状态

如果直线斜率a大于设定阈值，则说明载体处于前进持续加速或者倒退持续减速状态，此时把载体定义为“疑似载体前进”状态，否则，回到运动状态未知。

a>K

其中：

K

(二)载体真实运动状态判定：

当载体运动状态满足“疑似载体前进”时，利用GNSS的速度判断载体的真实运动状态。

如果GNSS的速度满足下述条件，即从当前k时刻持续到未来k+i时刻，GNSS的速度都大于设定阈值，则可以判定载体处于前进状态。否则，IMU数据更新，回到步骤(一)(1)，载体运动状态置为“载体运动状态未知”。

V

M

V

其中：

V

V

V

本发明中GNSS更新频率是10Hz即周期为0.1s，综合考虑这里i取40，即GNSS连续4秒时间的速度都满足阈值判定条件。

由于载体的动态性很小(动态性是指载体的速度/加速度/加加速度都比较小，这里可以认为速度比较小)，如果载体处于持续倒车减速状态，那么4秒的时间内GNSS速度肯定减小到0，不会满足上述速度阈值判断条件，即不会把持续倒车减速判定为持续前进加速。针对不同载体和不同动态，上述持续时间可动态调节(如果是汽车载体，速度比较大，倒车减速到停车时间比较长，这个时间可依据经验加长)。

(三)初始对准

判定载体处于前进状态后，利用GNSS的东向和北向速度初始对准组合导航的初始航向。

实施例2

如图2所示是载体先持续加速前进、后匀速运动的状态数据(类似于汽车起步)。从图2(a)可以看出在155到182历元时，载体前向轴加速度始终为正且存在从小到大或从大到小的变化，在历元161和180时间点前向轴加速度达到最大值且超过上述设定阈值1.8m/s/s。图2(b)是实时最小二乘拟合的直线的斜率即反应前向轴加速度变化的趋势，从图中可以看出和图2(a)变化趋势吻合，且在历元162和180处达到最大值，并且超过上述设定阈值4.0。此处2点都可以判定载体为“载体疑似前进”运动状态。此时观察图2(c)，GNSS的速度持续增加然后稳定，且大于上述设定阈值0.6m/s。此时可以判定载体处于前进运动状态，满足组合导航初始对准条件。

如图3所示是载体先持续加速后退然后停车(持续减速后退)的状态数据。从图3(a)可以看出在80到120历元处，前向轴加速度始终为负数且存在先反向增大后减小的变化。在历元107到历元120是刹车过程即持续后退减速状态。从图3(b)可以看出此时拟合直线的斜率很大，满足上述阈值判定条件，但是前向轴加速度不满足上述阈值判定条件(值得注意的是，此次实验前向轴加速度不满足条件，但是如果存在“急刹”等条件时，前向轴加速度是可以满足上述条件的)。再看图3(c)，当载体刹车即持续后退减速时，GNSS的速度很快减小到接近0，此时是一定不满足上述速度判定条件的，所以不会把“持续后退减速”状态判定为“持续前进加速”状态，不会存在误判。

虽然本发明已以较佳实施例公开如上，但其并非用以限定本发明，任何熟悉此技术的人，在不脱离本发明的精神和范围内，都可做各种的改动与修饰，因此本发明的保护范围应该以权利要求书所界定的为准。