CN110530361A - 一种基于农业机械双天线gnss自动导航系统的转向角度估计器 - Google Patents

Abstract

本发明公开一种基于农业机械双天线GNSS自动导航系统的转向角度估计器，(1)通过航向校正器用将双天线GNSS输出的航向信息进行校正得到车体航向信息并通过第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测，基于车辆的轴距L、车速V和车体航向角速率根据车辆动力学模型对车辆转向轮角θ_m(j)进行推算；(2)陀螺仪信号处理单元对陀螺仪输出的角速度信号进行预处理，得到的陀螺仪角速率信号ω_g(j)与车体航向角速率相减得到车轮转向角速率ω_s(j)，第二三阶状态观测器用以对转向角加速度进行观测；(3)通过二阶状态观测器对θ_m(j)和

Description

一种基于农业机械双天线GNSS自动导航系统的转向角度估 计器

技术领域

本发明属于农业机械自动导航领域，具体涉及一种基于单陀螺仪与状态观测理论的农业机械双天线GNSS自动导航系统转向角度估计器。

背景技术

随着农业机械自动化水平的提高，农业机械自动导航技术应用越来越广泛，尤其是双天线GNSS自动导航系统在输出定位信息同时能够输出航向与俯仰两个车体姿态信息，避免了增加姿态传感器的成本，越来越受到青睐。

前轮转向角度是农业机械自动导航技术必需的基本信息。针对农业机械前轮转向角度的获取问题，现有技术中有多种技术方案：如授权公告号为【CN101326094B】的发明专利公开一种基于转向组件角度及角速率传感器的车轮陀螺仪，用来估算一系统的至少两个部分之间相对坐标，该方案中采用两个陀螺仪，一个安装在车身上，一个安装在车轮上，采用卡尔曼滤波的方式对两个陀螺仪信息进行融合；而采用两个陀螺仪安装非常麻烦，对很多车辆来说是安装不上的，只具有理论基础，实用性较差；或者是采用安装霍尔式轴角传感器的方式，但是，由于农业机械转向机构的差异性，轴角传感器安装过程中往往存在安装困难，易于损坏等问题，限制了农业农机自动导航技术的发展。

发明内容

本发明利用状态观测理论提出一种农业机械双天线GNSS自动导航系统转向角度估计器，采用二阶状态观测器对通过陀螺仪和双天线GNSS所获得的信息进行融合，有效提高了角度估计的精度和鲁棒性。

本发明是采用以下的技术方案实现的：一种基于农业机械双天线GNSS自动导航系统的转向角度估计器，包括航向矫正器、第一三阶状态观测器、量测角度计算器、陀螺仪信号处理单元、第二三阶状态观测器和二阶状态观测器；

所述航向校正器用以将双天线GNSS输出的航向信息校正为农业机械车头方向在北-东- 地坐标系下的车体航向信息j表示时刻；第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测；量测角度计算器基于车辆的轴距L、车速V和车体航向角速率根据车辆动力学模型对车辆转向轮角θ_m(j)进行推算；

所述陀螺仪信号处理单元用以对陀螺仪输出的角速度信号进行预处理，以输出滤波后的陀螺仪角速率信号ω_g(j)；得到的陀螺仪角速率信号ω_g(j)与车体航向角速率相减得到另一组车轮转向角速率ω_s(j)，并输入第二三阶状态观测器；第二三阶状态观测器用以对转向角加速度进行观测；

所述二阶状态观测器接收量测角度计算器得到的车辆转向轮角θ_m(j)与转向角加速度信息，以对车轮转向角θ(j)进行观测。

进一步的，所述车辆转向轮角θ_m(j)通过以下方式获得：

进一步的，所述二阶状态观测器利用和θ_m(j)对轮角转向角度θ(j)进行估计，并表示为：

其中，k_2θ，k_2ω为二阶状态观测器系数， 为j-1和j时刻转向角速度估计值， 为j-1和j时刻转向角度估计值，T_s为采样时间间隔。

进一步的，所述陀螺仪信号处理单元包括AD转换器和惯性滤波器，分别对应的对陀螺仪输出的角速率信号进行AD采样和惯性滤波，所述惯性滤波器的表达形式如下：

ω_g(j)＝k₁ω_g(j)+k₂ω_g(j-1)+k₃ω_g(j-2) (5)

式中，k₁，k₂，k₃满足条件：k₁+k₂+k₃＝1。

进一步的，当航向信息方向指示为车体左侧至车体右侧时，航向校正器则通过以下方式校正：

当航向信息方向指示为车体右侧至车体左侧时，航向校正器则通过以下方式校正：

其中，为双天线GNSS输出的航向信息。

与现有技术相比，本发明的优点和积极效果在于：

本发明所述方案在双天线导航系统基础上只需使用一个陀螺仪，安装方便，成本低；采用三阶状态观测器实现对车体航向信息和陀螺仪角速率信号的微分运算，利用积分代替微分，避免了微分运算对误差的放大作用；并采用二阶状态观测器对通过陀螺仪和双天线GNSS所获得的信息进行融合，有效提高了角度估计的精度和鲁棒性，具有较高的实际应用和推广价值。

附图说明

图1为本发明实施例所述硬件安装结构原理图；

图2为本发明实施例所述估计器的原理框图；

图3为本发明实施例三阶状态观测器原理框图；

图4为本发明实施例二阶状态观测器原理框图；

图5为本发明实施例测试试验1情况下的测量角度与估计角度的对比曲线图；

图6为图5中测量角度与估计角度的差值曲线示意图；

图7为本发明实施例所述方案与霍尔传感器所得结果的追踪误差示意图。

具体实施方式

为了能够更清楚的理解本发明的上述目的和优点，下面结合附图对本发明的具体实施方式做详细地描述：

为同时获取车辆的航向角和横滚角信息，农业机械导航系统的双天线通常垂直于车辆前进方向安装。本发明所述的转向角度估计器硬件安装如图1所示，陀螺仪1安装于前轮轴11 上，根据具体需要可安装在左前轮轴或右前轮轴，为北-东-地坐标系下车体前进方向航向角，θ为待估计的车轮转向角，参考坐标系为车头方向X，车体右侧方向Y，指向地为Z。

实施例，一种农业机械双天线GNSS自动导航系统转向角度估计器，如图2所示，陀螺仪输出的角速率信号经陀螺仪信号处理单元进行AD采样及惯性滤波后，输出滤波后的陀螺仪角速率信号ω_g(j)；AD转换器对陀螺仪输出的角速率模拟信号进行采样完成模拟信号到数字信号的转换；惯性滤波器目的在于抑制陀螺仪信号中的随机干扰；

航向校正器将双天线输出的航向信息校正为农业机械车头方向在北-东-地坐标系下的航向信息第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测；量测角度计算器利用车辆的轴距L，车速V，车体航向角速率根据车辆动力学模型对车辆转向轮角θ_m(j)进行推算，并作为二阶状态观测器的量测信息；

陀螺仪角速率信号ω_g(j)与车体航向角速率相减得到另一组车轮转向角速率ω_s(j)；第二三阶状态观测器对转向角加速度进行观测；最后通过二阶状态观测器根据利用动力学模型计算得到的车辆转向轮角θ_m(j)与转向角加速度信息对车轮转向角θ(j)进行观测，该系统设计鲁棒性高，能够获得精确的转向角。

本实施例中，双天线GNSS板卡输出的航线信息是基于北-东-地参考坐标系的，当航向信息方向指示为车体左侧至车体右侧时，航向校正器实现的数学公式为：

当航向信息指示为车体右侧至车体左侧时，航向校正器实现的数学公式为：

另外，本实施例中，所述惯性滤波器的实现公式为：

ω_g(j)＝k₁ω_g(j)+k₂ω_g(j-1)+k₃ω_g(j-2) (5)

式中k₁，k₂，k₃需满足条件：k₁+k₂+k₃＝1

图3为三阶状态观测器的原理示意图，为便于描述，定义j时刻三阶状态观测器的输入量为I_3θ(j)，其一阶导数二阶导数三阶状态观测器实现的数学公式为：

图2中，第一三阶状态观测器的输入第二三阶状态观测器的输入I_3θ(j)＝ω_s(j)，积分器为数字积分器，实现数字积分，定义积分器输入为I_i(j)，输出为I_o(j)采样时间间隔为 T_s，公式描述为：

I_o(j)＝I_o(j-1)+T_sI_i(j) (7)

通过量测角度计算器利用车辆的轴距L，车速V，车体航向角速率根据车辆动力学模型对车辆转向轮角θ_m(j)进行推算，即：

二阶状态观测器利用和θ_m(j)对轮角转向角度进行估计，图4为二阶状态观测器的原理示意图，并表示为：

其中，k_2θ，k_2ω为二阶状态观测器系数， 为j-1和j时刻转向角速度估计值， 为j-1和j时刻转向角度估计值，T_s为采样时间间隔。

本实施例采用状态观测器理论获取安装于前轮转轴的陀螺仪与安装于车体的双天线 GNSS间相对转向角度，在农业车辆北-东-地坐标系下的前轮转向角速率ω_g和车体航向角已知的前提下，定义车体航向角速率并采用两种方式获得车辆转向角：1)对ω_g与做差得到前轮相对于车身的转向角速率ω_s，对ω_s积分：2)根据车辆动力学模型，利用车体航向角速率车辆速度V，车辆轴距L，车辆前轮转向角度表示为：考虑到对于双天线系统而言，采用公式(1)所获取的前轮转向角度存在积分累计误差；采用公式(2)获取的转向角度由于运算过程中的差分运算以及反正切运算，对信号干扰存在放大作用，精度低，本实施例创造性的设计二阶状态观测器对两种数据进行融合，以有效提高角度估计的精度与鲁棒性。

为便于验证本发明方案的效果，进行了实物实验。实物实验平台按照图1搭建，其中双天线GNSS输出航向信息为车体左侧天线指向右侧天线，同时在车轮转向轴安装霍尔式轴角传感器作为评估本发明估计精度的基准。惯性滤波器的参数取值为k₁＝0.6，k₂＝0.2，k₃＝0.2，第一三阶状态观测器和第二三阶状态观测器的参数取值均为k_3θ＝60，k_3ω＝128，k_3α＝10000；二阶状态观测器参数取值为k_2θ＝100，k_2ω＝150；双天线板卡采用司南K728，输出航向信息为车体左侧至车体右侧，采用公式(3)进行车身航向校正；测试农业车辆为某厂家1004拖拉机，车辆转向轮轴同时安装有霍尔式轮角传感器，作为转向轮角的参考，传感器型号为德国 NOVOTECHNIK公司生产的RF4000-120。

测试试验1：由驾驶员驾驶车辆，车辆前进速度为6km/h，行进路线为直线和S型，角度车辆值和估计值如图5所示，两者之差如图6所示；测试过程中，0-40s为直线驾驶，40-60s为S型路线驾驶，图中实线为本发明所得前轮转向角度估计值，虚线为霍尔传感器测量的前轮转向角度值，根据图5，直线行进过程中两者之差小于1°，S型路线行进过程中，由于估计器对于频率信号的滤波效果，两者之差小于4.3°；

测试试验2：需要说明的是由于霍尔式轮角传感器本身存在的测量误差问题，测试过程1 所得两者之差仅可作为评估两种方式所获取转向角度的比较依据；直线追踪精度是农业机械自动导航系统的主要技术指标，为进一步验证本发明在农业机械自动导航系统中的适用性，分别由霍尔传感器的角度值和本发明的估计器所得角度估计值作为农业机械导航系统轮角值的输入完成农业机械直线导航的路径追踪，导航控制的其他硬件条件及算法均相同，两者所得直线追踪误差如图7所示，0-50s采用霍尔轮角传感器获取轮角值完成直线追踪，50-100s 采用本发明的轮角估计器获取轮角值完成直线追踪，根据图7，采用本发明的角度估计器所得前轮转向角度值完成直线路径追踪，追踪误差小于5cm，与采用霍尔传感器测量角度实现直线路径追踪精度相当，精度高，鲁棒性好，可实现双天线农业机械自动导航系统轮角测量的完全互换。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (5)

1.基于农业机械双天线GNSS自动导航系统的转向角度估计器，其特征在于，包括航向矫正器、第一三阶状态观测器、量测角度计算器、陀螺仪信号处理单元、第二三阶状态观测器和二阶状态观测器；

所述航向校正器用以将双天线GNSS输出的航向信息校正为农业机械车头方向在北-东-地坐标系下的车体航向信息j表示时刻；第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测；量测角度计算器基于车辆的轴距L、车速V和车体航向角速率根据车辆动力学模型对车辆转向轮角θ_m(j)进行推算；

所述陀螺仪信号处理单元用以对陀螺仪输出的角速度信号进行预处理，以输出滤波后的陀螺仪角速率信号ω_g(j)；得到的陀螺仪角速率信号ω_g(j)与车体航向角速率相减得到另一组车轮转向角速率ω_s(j)，并输入第二三阶状态观测器；第二三阶状态观测器用以对转向角加速度进行观测；

所述二阶状态观测器接收量测角度计算器得到的车辆转向轮角θ_m(j)与转向角加速度信息，以对车轮转向角θ(j)进行观测。

2.根据权利要求1所述的基于农业机械双天线GNSS自动导航系统的转向角度估计器，其特征在于：所述车辆转向轮角θ_m(j)通过以下方式获得：

3.根据权利要求1所述的基于农业机械双天线GNSS自动导航系统的转向角度估计器，其特征在于：所述二阶状态观测器利用和θ_m(j)对轮角转向角度进行估计，并表示为：

其中，k_2θ，k_2ω为二阶状态观测器系数，为j-1和j时刻转向角速度估计值，为j-1和j时刻转向角度估计值，T_s为采样时间间隔。

4.根据权利要求1所述的基于农业机械双天线GNSS自动导航系统的转向角度估计器，其特征在于：所述陀螺仪信号处理单元包括AD转换器和惯性滤波器，分别对应的对陀螺仪输出的角速率信号进行AD采样和惯性滤波，所述惯性滤波器的表达形式如下：

ω_g(j)＝k₁ω_g(j)+k₂ω_g(j-1)+k₃ω_g(j-2) (5)

式中，k₁，k₂，k₃满足条件：k₁+k₂+k₃＝1。

5.根据权利要求1所述的基于农业机械双天线GNSS自动导航系统的转向角度估计器，其特征在于：

当航向信息方向指示为车体左侧至车体右侧时，航向校正器则通过以下方式校正：

当航向信息方向指示为车体右侧至车体左侧时，航向校正器则通过以下方式校正：

其中，为双天线GNSS输出的航向信息。