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

权利要求书

1.一种姿态传感器的校准方法，所述姿态传感器安装于行驶装置上，其特征在于，所述姿态传感器的校准方法包括：

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

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

控制所述行驶装置在到达终点后掉头，并按终点到起点的方向沿预设路线反向行驶，在反向行驶过程中，对行驶装置的反向轨迹点中与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点判断是否满足第二条件，对满足第二条件的反向轨迹点判断为反向条件轨迹点，采集反向条件轨迹点的位置和航向角，其中第二条件为与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点与预设路线的对应点的距离偏差小于第三阈值，且与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点与预设路线的对应点的航向角度偏差小于第四阈值；

采集反向条件轨迹点的姿态数据，将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组，其中第三条件为反向条件轨迹点与对应的正向条件轨迹点之间的距离小于第五阈值且该两者之间的航向角之差的绝对值减去180度的差小于第六阈值；

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

2.如权利要求1所述的姿态传感器的校准方法，其特征在于，根据至少一个计算轨迹点组的姿态数据来计算和校准所述姿态传感器包括：

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

3.如权利要求1所述的姿态传感器的校准方法，其特征在于，所述姿态数据包括横滚角和/或俯仰角。

4.如权利要求1所述的姿态传感器的校准方法，其特征在于，所述预设路线为直线。

5.如权利要求4所述的姿态传感器的校准方法，其特征在于，所述预设路线的方程为：(x-x1)/cos(a)＝(y-y1)/sin(a)＝L，其中设定所述起点的坐标为(x1,y1)，a为所述预设路线的设定航向角度。

6.如权利要求1所述的姿态传感器的校准方法，其特征在于，所述行驶装置为农业机械，所述第一阈值和/或所述第三阈值为2.5cm，所述第二阈值和/或所述第四阈值为0.2°。

7.如权利要求1所述的姿态传感器的校准方法，其特征在于，所述行驶装置上设有显示装置，在正向行驶过程中，所述显示装置实时显示采集的正向轨迹点的位置和航向角和采集的正向条件轨迹点的姿态数据，在反向行驶的过程中，所述显示装置实时显示采集的反向条件轨迹点的位置和航向角。

8.一种行驶装置，包括检测所述行驶装置的姿态的姿态传感器，其特征在于，所述行驶装置应用如权利要求1至7任一所述的姿态传感器的校准方法对所述姿态传感器进行校准。

9.如权利要求8所述的行驶装置，其特征在于，包括定位装置、存储装置、和与所述定位装置、所述存储装置和所述姿态传感器信号连接的控制装置，所述定位装置用于对所述行驶装置进行定位，所述存储装置用于存储所述姿态传感器和所述定位装置的测量数据，所述控制装置被配置为用于计算和处理所述定位装置、所述姿态传感器的测量数据和用于控制所述行驶装置行走。

10.如权利要求8或9任一所述的行驶装置，其特征在于，所述行驶装置包括农业机械。