一种霍尔角度传感器安装角度偏置的估计方法与流程

本发明涉及一种霍尔角度传感器安装角度偏置的估计方法，属于多信息融合技术领域。

背景技术：

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

精准农业的实现离不开自动化农机设备，而自动化农机设备的自动化实现需要获取农机相关的运动参数以对其自动驾驶路径进行规划；霍尔传感器就是一种可以用于获取自动化农机设备的运动参数的设备，比如用于获取农机前轮转向角的霍尔角度传感器，用于实时获取农机前轮转向角以确定农机的自动驾驶路径。通常会将霍尔角度传感器安装在农机前轮转向节处，用于实时测量农机的前轮转动角度，为规划自动化农机设备的自动驾驶路径提供相应的角度参数。

根据霍尔角度传感器的工作原理可知，霍尔角度传感器测量的是绝对角度，测量范围是0到360度，而霍尔角度传感器在安装时会有一个随机且固定的安装角度偏置，称为安装偏置角，即农机前轮摆正时，霍尔角度传感器测量的角度不是0而是一个随机且固定的偏置角度。

在农机自动驾驶时，需要根据农机前轮相对于摆正状态时左转或者右转的角度，那么就需要确定安装偏置角的角度值，以便对农机行使角度进行校准。传统的校准方法是先手动直行开动农机，采集此时霍尔角度传感器的测量数据，取平均值作为安装偏置角的角度值，称为粗校准过程；然后令农机自动驾驶一段距离，采集霍尔角度传感器的测量数据再取平均值，在粗校准基础上进一步校准安装偏置角的角度值，称为精校准过程。

上述校准方法的缺点是费时费力，而且需要专业的技术服务人员进行操作；其次，若霍尔传感器损坏需要更换或者因为农闲将霍尔角度传感器拆下后，再次安装时依然会存在一个安装偏置角，且该角度值为随机的，那么就需要重复上述校准操作，不但费时费力影响农业生产也增加了售后服务成本。

技术实现要素：

为了解决现有技术在校准霍尔角度传感器安装偏置角时费时费力的问题，本发明提供了一种霍尔角度传感器安装角度偏置的估计方法，用以实时估计出霍尔角度传感器当前的安装角度偏置，以便实时获取到农机前轮转向的实际值，此方法不但避免了需要专业人员多次校准带来的费时费力的麻烦，而且实时校准还避免了农机因为使用磨损而导致的使用误差，所述方法包括：

获取霍尔角度传感器实时测量得到的车辆前轮角度θh；

根据车辆运动学模型预测车辆的前轮转动角度θv；

根据实时测量得到的车辆前轮角度θh和预测的车辆的前轮转动角度θv进行kalman组合滤波估计，得到霍尔角度传感器实时的安装角度偏置θbias，以便根据θbias对车辆前轮角度θh进行补偿修正，得到补偿修正后的车辆前轮角度θd；

其中所述霍尔角度传感器安装于车辆前轮转向节处。

可选的，所述根据实时测量得到的车辆前轮角度θh和预测的车辆的前轮转动角度θv进行kalman组合滤波估计，得到霍尔角度传感器实时的安装角度偏置θbias，包括：

2.1选取前轮角度偏差δθ和霍尔角度传感器安装角度偏置θbias作为kalman滤波估计的状态量x，计算所述状态量x的状态转移矩阵φ；其中，所述前轮角度偏差δθ为补偿修正后的车辆前轮角度真值θd与根据车辆运动学模型预测的车辆的前轮转动角度θv的差值；

其中，状态量x为：

根据前轮角度偏差δθ和霍尔角度传感器安装角度偏置θbias之间的关系得到状态量x的状态转移矩阵φ为：

其中，τ为根据霍尔角度传感器精度值确定的相关时间；

2.2根据所述状态量x的状态转移矩阵φ一步预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1；

2.3更新n时刻的状态估计量和n时刻的状态协方差pn；

2.4根据更新后的n时刻的状态估计量计算得到霍尔角度传感器安装角度偏置θbias。

可选的，所述2.2根据所述状态量x的状态转移矩阵φ一步预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1，小包括：

根据下述公式预测得到预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1：

其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；为n-1时刻状态量x的状态估计值；

pn-1为n-1时刻状态协方差矩阵，为系统噪声矩阵；δt为滤波估计更新周期。

可选的，所述2.3更新n时刻的状态估计量和n时刻的状态协方差pn，包括：

根据下述公式更新n时刻的状态估计量

其中，zn为n时刻的观测量即修正后的车辆前轮角度实际测量值和根据车辆运动学模型预测的前轮转动角度之间差值；hn为状态观测矩阵；kn为n时刻的滤波增益：

hn＝[10]

kn＝pn|n-1hn^t(hnpn|n-1hn^t+rn)^-1

其中，rn为n时刻观测噪声协方差矩阵，由gnss信息统计得到；

根据下述公式更新n时刻的状态协方差pn：

pn＝(i-knhn)pn|n-1(i-knhn)t+knrnkn^t

其中，i为单位矩阵。

可选的，所述2.4根据更新后的n时刻的状态估计量计算得到霍尔角度传感器安装角度偏置θbias为根据下式计算得到n时刻的霍尔角度传感器安装角度偏置θbias(n)：

其中，表示状态量的第二维即当前周期滤波估计的安装角度偏置。

可选的，所述根据车辆运动学模型预测车辆的前轮转动角度θv，包括：

根据下述公式预测车辆的前轮转动角度θv：

其中，θv为车辆前轮预测角度；ω为车辆旋转角速度；l为车辆轴距；v为车辆速度。

可选的，所述得到霍尔角度传感器实时的安装角度偏置θbias，以便根据θbias对车辆前轮角度θh进行补偿修正，得到补偿修正后的车辆前轮角度θd；其中，补偿修正后的车辆前轮角度θd＝θh-θbias。

本申请的第二个目的在于提供一种基于kalman滤波的农机前轮角度修正方法，所述霍尔角度传感器安装于农机前轮转向节处，所述方法采用上述基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法得到霍尔角度传感器安装角度偏置θbias，进而根据θd＝θh-θbias得到修正后的农机前轮角度θd，θh为霍尔角度传感器实时测量得到的车辆前轮角度。

本申请的第三个目的在于提供一种农机自动驾驶路线规划方法，所述方法采用霍尔角度传感器实时测量得到的车辆前轮角度，所述霍尔角度传感器安装于农机前轮转向节处，其特征在于，所述方法采用上述基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法得到霍尔角度传感器安装角度偏置θbias，或者采用上述基于kalman滤波的农机前轮角度修正方法得到修正后的农机前轮角度θd，进而根据霍尔角度传感器安装角度偏置θbias或者修正后的农机前轮角度θd进行农机自动驾驶路线规划。

本申请的第四个目的在于提供一种自动驾驶农机，所述农机的前轮转向节处安装有霍尔角度传感器，并且所述农机采用上述基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法得到霍尔角度传感器安装角度偏置θbias，或者采用上述基于kalman滤波的农机前轮角度修正方法得到修正后的农机前轮角度θd，进而根据霍尔角度传感器安装角度偏置θbias或者修正后的农机前轮角度θd确定自动驾驶路线。

可选的，所述农机包括拖拉机、插秧机、平地机、打药机、收割机。

本发明有益效果是：

通过提供一种基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法，霍尔角度传感器安装于车辆前轮转向节处，根据实时测量得到的车辆前轮角度θh和根据车辆运动学模型预测的车辆的前轮转动角度θv进行kalman组合滤波估计，得到霍尔角度传感器实时的安装角度偏置θbias，以便根据θbias对车辆前轮角度θh进行补偿修正，得到补偿修正后的车辆前轮角度θd。此方法不但避免了需要专业人员多次校准带来的费时费力的麻烦，而且实时校准还避免了农机因为使用磨损而导致的使用误差，同时，本申请方法的估计精度收敛后在±0.1度范围内，误差非常小；校准时间在5s范围内即可收敛完成，相对于传统先进行粗校准再进行精校准的方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人员。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一个实施例中霍尔角度传感器安装角度偏置的估计方法的流程图。

图2是一种车辆运动学模型示意图。

图3是本发明一个实施例中kalman滤波估计方法的流程图。

图4是本发明一个实施例中霍尔角度传感器安装角度偏置估计结果仿真图。

图5是本发明一个实施例中kalman滤波估计修正后的车辆前轮角度仿真图。

图6是本发明一个实施例中霍尔角度传感器在农机上的安装示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

实施例一：

本实施例提供一种基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法，参见图1，所述方法包括：

获取霍尔角度传感器实时测量得到的车辆前轮角度θh；

根据车辆运动学模型预测车辆的前轮转动角度θv；

根据实时测量得到的车辆前轮角度θh和预测的车辆的前轮转动角度θv进行kalman组合滤波估计，得到霍尔角度传感器实时的安装角度偏置θbias，以便根据θbias对车辆前轮角度θh进行补偿修正，得到补偿修正后的车辆前轮角度θd。

在进行kalman组合滤波估计时，包括：

2.1选取前轮角度偏差δθ和霍尔角度传感器安装角度偏置θbias作为kalman滤波估计的状态量x，计算所述状态量x的状态转移矩阵φ；其中，所述前轮角度偏差δθ为补偿修正后的车辆前轮角度真值θd与根据车辆运动学模型预测的车辆的前轮转动角度θv的差值；

其中，状态量x为：

根据前轮角度偏差δθ和霍尔角度传感器安装角度偏置θbias之间的关系得到状态量x的状态转移矩阵φ为：

其中，τ为根据霍尔角度传感器精度值确定的相关时间；

2.2根据所述状态量x的状态转移矩阵φ一步预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1；

具体的，根据下述公式预测得到预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1：

其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；为n-1时刻状态量x的状态估计值；

pn-1为n-1时刻状态协方差矩阵，为系统噪声矩阵；δt为滤波估计更新周期。

2.3更新n时刻的状态估计量和n时刻的状态协方差pn；

根据下述公式更新n时刻的状态估计量

其中，zn为n时刻的观测量即修正后的车辆前轮角度实际测量值和根据车辆运动学模型预测的前轮转动角度之间差值；hn为状态观测矩阵；kn为n时刻的滤波增益：

hn＝[10]

kn＝pn|n-1hn^t(hnpn|n-1hn^t+rn)^-1

其中，rn为n时刻观测噪声协方差矩阵，由gnss信息统计得到；

根据下述公式更新n时刻的状态协方差pn：

pn＝(i-knhn)pn|n-1(i-knhn)^t+knrnkn^t

其中，i为单位矩阵。

2.4根据更新后的n时刻的状态估计量计算得到霍尔角度传感器安装角度偏置θbias；

具体的，根据下式计算得到n时刻的霍尔角度传感器安装角度偏置θbias(n)：

其中，表示状态量的第二维即当前周期滤波估计的安装角度偏置。

实施例二

本实施例提供一种基于kalman滤波的农机前轮角度修正方法，应用于农机设备中，将霍尔角度传感器安装在农机前轮转向节处，所述方法包括：

s1根据车辆运动学模型预测农机前轮转动角度；

具体的，请参考图2所示的车辆运动学模型，其中，θv为车辆前轮预测角度；ω为车辆旋转角速度，可由gnss双天线差分获得；l为车辆轴距；v为车辆速度，v也可由gnss量测获得。

s2将s1根据车辆运动学模型预测的前轮转动角度和霍尔角度传感器实时测量的车轮角度进行kalman组合滤波估计，实时估计霍尔角度传感器的安装角度偏置。

估计流程图如图3所示，估计过程为不断迭代的过程，包括：

s2.1霍尔角位移角度更新；

即获取霍尔角度传感器实时测量的车轮角度θh。

s2.2角度补偿修正；

即根据kalman滤波估计得到的安装角度偏置θbias，实时补偿修正农机的前轮转角，即霍尔角度传感器实时测量的前轮角度θh，得到经补偿修正后的农机前轮角度θd：

θd＝θh-θbias

其中：

θd：经补偿修正后的农机前轮角度真值；

θh：霍尔角度传感器实时测量的前轮角度；

θbias：kalman滤波估计的安装角度偏置。

由图3可知，在第一次进行角度补偿修正时，由于霍尔角度传感器的安装角度偏置短时间内变化非常小接近为零，因此可假设此时根据kalman滤波估计得到的安装角度偏置θbias为零，也即第一次进行角度补偿修正时，经补偿修正后的农机前轮角度θd就等于霍尔角度传感器实时测量的前轮角度θh。

s2.3判断农机是否处于静止状态；

因为当车辆静止时其不满足运动学模型，此时根据车辆运动学模型预测的前轮转角误差非常大，如果此时继续进行滤波估计会带来非常大的误差甚至是滤波发散。

考虑到霍尔角度传感器的安装角度偏置短时间内是不变的(也即，短时间内既没有重新安装该霍尔角度传感器，也不存在因为农机长时间使用导致的使用误差)，此时对零偏进行锁定不进行估计修正。

当判断农机处于运动状态时则进行下一步状态转移矩阵计算。

s2.4状态转移矩阵计算；

具体的，选取前轮角度偏差和霍尔霍尔角度传感器安装角度偏置作为滤波估计的状态量，即

δθ：前轮角度偏差，即经补偿修正后的农机前轮角度真值θd与s1中根据车辆运动学模型预测的前轮转动角度θv的差值。

θbias：kalman滤波估计的安装角度偏置。

同样由于霍尔角度传感器的安装角度偏置短时间内变化非常小，其误差模型可近似为一阶马尔科夫过程，则因此第一次进行角度补偿修正时前轮角度偏差δθ实际上为霍尔角度传感器实时测量的前轮角度θh与s1中根据车辆运动学模型预测的前轮转动角度θv的差值。

因此根据前轮角度偏差δθ和霍尔角度传感器安装角度偏置之间的关系可以得到状态转移矩阵如下：

τ：相关时间，根据霍尔角度传感器的精度可选定3600秒，也可以选择其他值，比如7200秒、600秒等。

s2.5kalman一步预测：

在得到上述状态转移矩阵后，一步预测n-1时刻到n时刻的估计状态量和状态协方差矩阵pn|n-1。

其中：

n-1时刻状态量x的估计值；

pn-1：n-1时刻状态协方差矩阵；

φn|n-1：n-1时刻到n时刻状态转移矩阵；

系统噪声矩阵；

δt：滤波估计更新周期。

s2.6kalman滤波估计更新：

kalman滤波估计更新状态量和状态协方差矩阵。

为防止滤波估计出现异常偏差，在滤波估计时加入观测数据质量控制，即当前轮角度偏差δθ与估计的观测误差相差较大时，需要对估计的观测误差进行一定的修正，即若δθ＝θd-θv＝θh-θbias-θv大于10倍的观测误差则把估计的观测误差放大对应的倍数。

滤波估计时的滤波增益：

kn＝pn|n-1hn^t(hnpn|n-1hn^t+rn)^-1

其中：

kn：n时刻滤波增益；

hn：n时刻的观测矩阵，由观测量和状态量之间的关系得到hn＝[10]。

rn：n时刻观测噪声协方差矩阵，由gnss信息统计得到。

根据下述公式进行状态更新：

其中：

n时刻的状态估计量；

zn：n时刻的观测量即修正后的车辆前轮角度实际测量值和s1根据车辆运动学模型预测的前轮转角之间差值。

此处修正后的车辆前轮角度实际测量值可由安装在车辆前轮处的陀螺仪测量得到，与补偿修正后的车辆前轮角度真值θd存在误差。

根据下述公式进行状态协方差更新：

pn＝(i-knhn)pn|n-1(i-knhn)^t+knrnkn^t

其中：

pn：n时刻状态协方差矩阵；

i：单位矩阵。

s2.7反馈修正：

滤波估计后利用估计后的状态量对当前安装角度偏置和农机前轮角度进行反馈修正，得到修正后的。

具体的，在得到n时刻状态协方差矩阵后，即得到了前轮角度偏差δθ＝θd-θv和霍尔角度传感器安装角度偏置θbias，继而根据θd＝θh-θbias即可获得经补偿修正后的农机前轮角度θd。

为验证本申请所提出的基于kalman滤波的霍尔角度传感器安装角度偏置的估计方法实时估计出的安装角度偏置的准确性，本实施例进行了验证实验，如图6所示，为霍尔角度传感器在农机上的实际安装示意图，采用其他方式预先测量得到该霍尔角度传感器在农机上的安装角度偏置为133.5度。

如图4所示，为采用本实施例提供的基于kalman滤波估计的安装角度偏置的估计方法的仿真结果，根据图4可知，采用本实施例滤波估计的安装角度偏置稳定在133.5度左右，误差范围为±0.1度，且根据图4可知，本申请提供的基于kalman滤波估计的安装角度偏置的估计方法在5s内估计值已经基本收敛趋于稳定，完全满足农机自动驾驶的要求。在50s时已完全趋于稳定状态，相对于传统先进行粗校准再进行精校准的方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人员。

图5为手动开动农机采用本实施例提供的基于kalman滤波估计的安装角度偏置的估计方法估计出霍尔角度传感器安装角度偏置后，得到的修正后的车辆前轮角度仿真图。从图5农机前轮角度可以看出行驶路径有近似直线行驶、左极限方向盘打死调头、右极限方向盘打死调头、左右极限打死倒车以及最后时刻的静止停车，从图4可以看出本方法估计出的安装角度偏置一直很稳定，适应各种应用场景。

本发明实施例中的部分步骤，可以利用软件实现，相应的软件程序可以存储在可读取的存储介质中，如cpu或光盘或硬盘等。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。