[发明专利]姿态传感器的校准方法和行驶装置

说明书

本发明公开一种姿态传感器的校准方法和行驶装置，校准方法包括：控制行驶装置按照预设路线从起点往终点方向正向行驶，计算正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差；采集正向条件轨迹点的姿态数据与沿预设路线与起点和/或与终点的距离；在反向行驶过程中，采集反向条件轨迹点的位置和航向角；将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组，其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值；根据至少一个计算轨迹点组计算校准偏差值，根据校准偏差值校准姿态传感器。

技术领域

本发明涉及一种辅助驾驶领域，特别涉及一种姿态传感器的校准方法和行驶装置。

背景技术

具有辅助驾驶功能的行驶装置一般采用姿态传感器进行姿态检测，例如一些包括农业机械在内的作业机械，其姿态传感器一般安装在驾驶室内，通过姿态传感器来测试车辆的俯仰角和横滚角，即车体姿态。姿态传感器安装位置一般在行驶装置后轴上方的某一平面处，但由于姿态传感器存在安装误差，导致测量的俯仰角和横滚角与行驶装置的实际运动姿态存在一个固定的偏差角。如果不对该偏差角进行校准，姿态传感器在辅助行驶装置驾驶进行坡度补偿时，会导致定位不准确，影响作业精度，即需要得到姿态传感器的校准偏差值，在姿态传感器对行驶装置进行姿态测量时利用该校准偏差值进行校准。

发明内容

本发明的目的在于提供一种姿态传感器的校准方法，利用该校准方法能够得到有效精确地对姿态传感器进行校准。

本发明第一方面公开一种姿态传感器的校准方法，所述姿态传感器安装于行驶装置上，所述姿态传感器的校准方法包括：

控制所述行驶装置按照预设路线从起点往终点方向正向行驶，在正向行驶过程中，采集行驶装置的多个正向轨迹点的位置和航向角，计算采集的正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差；

采集正向轨迹点中满足第一条件的正向条件轨迹点的姿态数据，采集正向条件轨迹点沿预设路线与起点和/或与终点的距离，其中第一条件为距离偏差小于第一阈值且航向角度偏差小于第二阈值；

控制所述行驶装置在到达终点后掉头，并按终点到起点的方向沿预设路线反向行驶，在反向行驶过程中，对行驶装置的反向轨迹点中与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点判断是否满足第二条件，对满足第二条件的反向轨迹点判断为反向条件轨迹点，采集反向条件轨迹点的位置和航向角，其中第二条件为与预设路线的对应点的距离偏差小于第三阈值且与预设路线的对应点的航向角度偏差小于第四阈值；采集反向条件轨迹点的姿态数据，将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组，其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值；

根据至少一个计算轨迹点组的姿态数据来计算所述姿态传感器的校准偏差值，根据所述校准偏差值校准所述姿态传感器。

在一些实施例中，根据至少一个计算轨迹点组的姿态数据来计算和校准所述姿态传感器包括：

取一个计算轨迹点组，计算取得的计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值，将求得的第一平均值作为所述姿态传感器的校准偏差值，或取多个计算轨迹点组，对取得的每个计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值，将求得的多个第一平均值再平均计算得到第二平均值，将第二平均值作为所述姿态传感器的校准偏差值。

在一些实施例中，所述姿态数据包括横滚角和/或俯仰角。

在一些实施例中，所述预设路线为直线。

在一些实施例中，所述预设路线的方程为：(x-x1)/cos(a)＝(y-y1)/sin(a)＝L，其中设定所述起点的坐标为(x1,y1)，a为所述预设路线的设定航向角度。