[发明专利]基于农业机械双天线GNSS自动导航系统的转向角度估计器

权利要求书

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

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

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

所述二阶状态观测器接收量测角度计算器得到的车辆转向轮角θ_m(j)与第二三阶状态观测器得到的转向角加速度信息，以对车轮转向角θ(j)进行观测，其中，二阶状态观测器利用和θ_m(j)对车轮转向角进行估计，并表示为：

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

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

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

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

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

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

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

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

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