CN114413892A - 一种新型果园机器人组合导航方法 - Google Patents

Abstract

本发明公开了一种新型果园机器人组合导航方法，能够在果园的复杂环境中提供更高的导航精度。相较于传统组合导航算法，新算法提出新的速度误差定义，此定义考虑了速度向量大小和方向的差异，有利于提高组合导航滤波精度。利用新的速度误差定义改进的滤波算法，提出了基于状态变换卡尔曼滤波的果园机器人惯性/卫星组合导航算法。此算法将系统矩阵计算中的比力项用重力项替代，克服了传统基于EKF的组合导航算法面临的由于比力量化噪声较大带来的系统矩阵计算不精确，导致方差不一致的问题。此外，该算法还具有更好的动态适应性，在卫星信号失锁情况下，具有更好的定位精度。

Description

一种新型果园机器人组合导航方法

技术领域

本发明属于导航系统领域，具体涉及一种新型果园机器人组合导航方法。

背景技术

随着计算机技术和传感器技术的迅速发展，智能机器人已被广泛应用于工业、航天/服务业以及农业等多个领域。农业生产是我国重要的研究领域。提高农业智能化、自动化水平是重中之重。农业机器人是农业发展的一个重要方向，人工作业成本由此能够得到有效的改善，使农业生产更加规模化，效率化。在这之中，果园机械化是农业生产机械化的一个重要组成部分。我国是全球最大的水果生产国，水果消费占食物消费的比重也在上升，果品产业在我国农村经济中有举足轻重的地位。但是，目前果园管理作业主要还是靠人工来执行，其中，育苗栽种、施肥打药、采摘等工作都在果园行间完成，占总人力需求的50％以上。

果园机器人的定位导航技术是果园机器人实现自主作业功能的重要基础，也是果园机器人研发的一个重要组成部分。稳定而精确的定位是果园机器人定位导航的基础，但这也是目前果园机器人项目需要攻克的难点。首先，在果园里，果树呈较为规律的排列，结构紧密，果树的树冠一般都非常宽大，这些高大的果树会干扰GPS接收机和卫星之间的通讯信号，减小接收的收星数和信噪比，导致GPS定位的结果会产生较大的误差。其次，果园环境是半结构环境，果园内大多数都是土壤地面，并且路面凹凸不平，可能还会有杂草，环境相较室内环境恶劣许多，这种情况对果园机器人前进路线规划以及位置姿态实时估计都会造成很大干扰。最后，当捷联惯性组合导航系统平台处于不利的环境下时，例如在准静态的环境下，即处于静态的载体仍然受到振动、风动等外部力的影响，一贯的捷联惯性组合导航滤波方式很容易出现方差估计不一致的问题，这就会导致方差的理论值与实际估计值不符，进而导致导航精度的下降甚至是滤波的发散。因此，在果园环境中实现稳定而精确的导航是果园机器人定位导航面临的首要问题。

发明内容

为解决上述问题，本发明公开了一种新型果园机器人组合导航方法，该方法可克服当导航平台处于不利的环境下时，一贯的捷联惯性组合导航滤波出现方差估计不一致的问题；并且，在多处卫星信号失锁的情况下，相比于一般的组合导航算法定位精度更高。

为达到上述目的，本发明的技术方案如下：

一种新型果园机器人组合导航方法，包括以下步骤：

(1)从MEMS惯性器件读取果园机器人位置，速度信息，然后SINS进行解算，得到机器人的位置、速度、姿态的信息；

(2)从机器人上的GPS接收机读取机器人的GPS位置、速度信息；

(3)将从SINS和GPS接收机获得的位置，速度信息传入到基于状态变换的卡尔曼滤波器中；

(4)将滤波结果返回给MEMS惯性器件进行误差补偿、校正；

(5)SINS输出机器人的位置、速度这些导航信息。

提出了一种新的速度误差定义，相比传统的速度误差定义，新的定义通过速度误差状态的严格变换避免系统矩阵中含有比力项，进而克服方差估计不一致的问题。从物理意义上可以解释为，两个向量之间不仅有大小上的差异，也存在方向上的差异。传统速度误差简单的定义为真实导航坐标系和计算的导航坐标系下两速度向量之间的大小差异。而新的速度误差定义中同时兼顾了真实导航坐标系和计算的导航坐标系下的速度向量的大小和方向差异即考虑了速度向量的方向差异的附加项，这可以提高不利环境下的初始对准和组合导航的滤波精度。

由新的速度误差定义推出的速度误差微分方程中不再含有比力项，而是由重力向量替代。由于惯性测量单元在一般情况下的输出都是比力增量而不是比力本身，并且比力增量的值会由于器件等原因会掺杂着量化噪声，所以传统基于扩展卡尔曼滤波的组合导航系统的系统矩阵的计算会被动态环境下的比力严重影响，进而导致方差不一致性。但是，新的系统矩阵中不再有比力项，而是被重力项所替代。新的基于状态变换的卡尔曼滤波方式可以避免由于比力量化噪声较大带来的系统矩阵计算不精确的问题，因为对一般载体的当地局部导航来说，重力项的变化很小，几乎为常值。

本发明的原理为：

当地导航系下定义的速度微分方程为：

其中，f^b为比力向量，gⁿ为果园当地重力矢量，

为地球相对惯性坐标系的角速率在导航系的投影，∧代表把向量转化成斜对称矩阵，

为姿态阵。

在此基础下，对速度误差进行新的定义：

其中，～代表变量的估计值，φⁿ和

分别为姿态误差向量和速度误差向量，

为新定义的速度误差向量。新的速度误差定义相比于传统的速度误差定义，兼顾了真实导航坐标系和计算的导航坐标系下的速度向量的大小以及方向差异，这有利于提高在果园路面凹凸不平环境下的组合导航滤波精度。

新的速度误差的微分方程为：

其中，ε^b和

分别为陀螺仪和加速度计的零偏；w_g和w_a分别为陀螺仪和加速度计的误差的白噪声向量。

为地球旋转矢量。

δrⁿ＝[δL,δλ,δh]^T (8)

其中，L，λ，h分别为果园机器人所在的经度，纬度，高度。R_N和R_E分别是子午线曲率半径和卯酉线曲率半径。v_E和v_N分别是果园机器人的东向速度和北向速度。

位置误差微分方程为：

其中，

由此，基于状态变换卡尔曼滤波的果园机器人组合导航系统在东北地坐标系下的惯导状态误差方程为：

F为系统矩阵，x为惯导状态误差向量，G为噪声转移矩阵，w为过程噪声向量，其对应的协方差矩阵为Q。其中，

w＝[w_gx,w_gy,w_gz,w_ax,w_ay,w_az] (14)

F₂₂＝[0_9×6] (19)

基于状态变换卡尔曼滤波的惯性/卫星果园机器人组合导航系统的观测方程为：

δz＝Hx+υ (23)

其中，

δz_v和δz_r分别为惯导与卫星之间速度误差和位置误差。υ_v,3×1和υ_v,3×1为对应的白噪声。滤波之后的速度更新为：

而姿态和位置的估计与传统的EKF相同。

本发明的有益效果为：

(1)本算法考虑了真实导航坐标系和计算的导航坐标系下的速度向量的大小和方向差异，这可以提高在果园路面颠簸环境下的初始对准和组合导航的滤波精度。

(2)本算法提出了一种新的速度误差定义，通过速度误差状态的严格变换，可以避免系统矩阵中含有比力项，进而克服方差估计不一致的问题。

(3)在多段卫星信号缺失的情况下，本算法比传统算法有更好的动态适应性，有更高的定位精度。

附图说明

图1为新型果园机器人惯性/卫星组合导航系统框图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图所示，本发明所述的一种新型果园机器人惯性/卫星组合导航方法，包括以下步骤：

1.从MEMS惯性器件读取果园机器人位置，速度信息，然后SINS进行解算，得到机器人的位置，速度，姿态的信息。

2.从机器人上的GPS接收机读取机器人的GPS位置，速度信息。

3.将从SINS和GPS接收机获得的位置，速度信息传入到基于状态变换的卡尔曼滤波器中。

4.将滤波结果返回给MEMS惯性器件进行误差补偿，校正。

5.SINS输出机器人的位置，速度等导航信息。

本发明考虑了真实导航坐标系和计算的导航坐标系下的速度向量的大小和方向差异，可以提高在果园路面颠簸环境下的初始对准和组合导航的滤波精度。

相较于传统组合导航算法，本发明提出了一种新的速度误差定义，此定义考虑了速度向量大小和方向的差异，有利于提高组合导航滤波精度。利用新的速度误差定义改进的滤波算法，提出了基于状态变换卡尔曼滤波的果园机器人惯性/卫星组合导航算法。此算法将系统矩阵计算中的比力项用重力项替代，克服了传统基于EKF的组合导航算法面临的由于比力量化噪声较大带来的系统矩阵计算不精确，导致方差不一致的问题。此外，该算法还具有更好的动态适应性，在卫星信号失锁情况下，具有更好的定位精度。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (2)

1.一种新型果园机器人组合导航方法，其特征在于：包括以下步骤：

(1)从MEMS惯性器件读取果园机器人位置，速度信息，然后SINS进行解算，得到机器人的位置、速度、姿态的信息；

(2)从机器人上的GPS接收机读取机器人的GPS位置、速度信息；

(3)将从SINS和GPS接收机获得的位置，速度信息传入到基于状态变换的卡尔曼滤波器中；

(4)将滤波结果返回给MEMS惯性器件进行误差补偿、校正；

(5)SINS输出机器人的位置、速度这些导航信息。

2.根据权利要求1所述的一种新型果园机器人组合导航方法，其特征在于：提出了一种新的速度误差定义，具体原理为：

当地导航系下定义的速度微分方程为：

其中，f^b为比力向量，gⁿ为果园当地重力矢量，

为地球相对惯性坐标系的角速率在导航系的投影，∧代表把向量转化成斜对称矩阵，

为姿态阵；

在此基础下，对速度误差进行新的定义：

其中，～代表变量的估计值，φⁿ和

分别为姿态误差向量和速度误差向量，

为新定义的速度误差向量；

新的速度误差的微分方程为：

其中，ε^b和

分别为陀螺仪和加速度计的零偏；w_g和w_a分别为陀螺仪和加速度计的误差的白噪声向量；

为地球旋转矢量；

δrⁿ＝[δL,δλ,δh]^T (8)

其中，L，λ，h分别为果园机器人所在的经度，纬度，高度；R_N和R_E分别是子午线曲率半径和卯酉线曲率半径；v_E和v_N分别是果园机器人的东向速度和北向速度；

位置误差微分方程为：

其中，

由此，基于状态变换卡尔曼滤波的果园机器人组合导航系统在东北地坐标系下的惯导状态误差方程为：

F为系统矩阵，x为惯导状态误差向量，G为噪声转移矩阵，w为过程噪声向量，其对应的协方差矩阵为Q；其中，

w＝[w_gx,w_gy,w_gz,w_ax,w_ay,w_az] (14)

F₂₂＝[0_9×6] (19)

基于状态变换卡尔曼滤波的惯性/卫星果园机器人组合导航系统的观测方程为：

δz＝Hx+υ (23)

其中，

δz_r＝δrⁿ+υ_v,3×1

δz_v和δz_r分别为惯导与卫星之间速度误差和位置误差；υ_v,3×1和υ_v,3×1为对应的白噪声；滤波之后的速度更新为：

而姿态和位置的估计与传统的EKF相同。

Images