CN114394102A - 姿态传感器的校准方法和行驶装置 - Google Patents
姿态传感器的校准方法和行驶装置 Download PDFInfo
- Publication number
- CN114394102A CN114394102A CN202210079271.5A CN202210079271A CN114394102A CN 114394102 A CN114394102 A CN 114394102A CN 202210079271 A CN202210079271 A CN 202210079271A CN 114394102 A CN114394102 A CN 114394102A
- Authority
- CN
- China
- Prior art keywords
- condition
- point
- track
- reverse
- attitude sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2300/00—Indexing codes relating to the type of vehicle
- B60W2300/15—Agricultural vehicles
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/06—Direction of travel
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A40/00—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production
- Y02A40/10—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production in agriculture
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Manufacturing & Machinery (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种姿态传感器的校准方法和行驶装置,校准方法包括:控制行驶装置按照预设路线从起点往终点方向正向行驶,计算正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差;采集正向条件轨迹点的姿态数据与沿预设路线与起点和/或与终点的距离;在反向行驶过程中,采集反向条件轨迹点的位置和航向角;将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组,其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值;根据至少一个计算轨迹点组计算校准偏差值,根据校准偏差值校准姿态传感器。
Description
技术领域
本发明涉及一种辅助驾驶领域,特别涉及一种姿态传感器的校准方法和行驶装置。
背景技术
具有辅助驾驶功能的行驶装置一般采用姿态传感器进行姿态检测,例如一些包括农业机械在内的作业机械,其姿态传感器一般安装在驾驶室内,通过姿态传感器来测试车辆的俯仰角和横滚角,即车体姿态。姿态传感器安装位置一般在行驶装置后轴上方的某一平面处,但由于姿态传感器存在安装误差,导致测量的俯仰角和横滚角与行驶装置的实际运动姿态存在一个固定的偏差角。如果不对该偏差角进行校准,姿态传感器在辅助行驶装置驾驶进行坡度补偿时,会导致定位不准确,影响作业精度,即需要得到姿态传感器的校准偏差值,在姿态传感器对行驶装置进行姿态测量时利用该校准偏差值进行校准。
发明内容
本发明的目的在于提供一种姿态传感器的校准方法,利用该校准方法能够得到有效精确地对姿态传感器进行校准。
本发明第一方面公开一种姿态传感器的校准方法,所述姿态传感器安装于行驶装置上,所述姿态传感器的校准方法包括:
控制所述行驶装置按照预设路线从起点往终点方向正向行驶,在正向行驶过程中,采集行驶装置的多个正向轨迹点的位置和航向角,计算采集的正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差;
采集正向轨迹点中满足第一条件的正向条件轨迹点的姿态数据,采集正向条件轨迹点沿预设路线与起点和/或与终点的距离,其中第一条件为距离偏差小于第一阈值且航向角度偏差小于第二阈值;
控制所述行驶装置在到达终点后掉头,并按终点到起点的方向沿预设路线反向行驶,在反向行驶过程中,对行驶装置的反向轨迹点中与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点判断是否满足第二条件,对满足第二条件的反向轨迹点判断为反向条件轨迹点,采集反向条件轨迹点的位置和航向角,其中第二条件为与预设路线的对应点的距离偏差小于第三阈值且与预设路线的对应点的航向角度偏差小于第四阈值;采集反向条件轨迹点的姿态数据,将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组,其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值;
根据至少一个计算轨迹点组的姿态数据来计算所述姿态传感器的校准偏差值,根据所述校准偏差值校准所述姿态传感器。
在一些实施例中,根据至少一个计算轨迹点组的姿态数据来计算和校准所述姿态传感器包括:
取一个计算轨迹点组,计算取得的计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的第一平均值作为所述姿态传感器的校准偏差值,或取多个计算轨迹点组,对取得的每个计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的多个第一平均值再平均计算得到第二平均值,将第二平均值作为所述姿态传感器的校准偏差值。
在一些实施例中,所述姿态数据包括横滚角和/或俯仰角。
在一些实施例中,所述预设路线为直线。
在一些实施例中,所述预设路线的方程为:(x-x1)/cos(a)=(y-y1)/sin(a)=L,其中设定所述起点的坐标为(x1,y1),a为所述预设路线的设定航向角度。
在一些实施例中,所述行驶装置为农业机械,所述第一阈值和/或所述第三阈值为2.5cm,所述第二阈值和/或所述第四阈值为0.2°。
在一些实施例中,所述行驶装置上设有显示装置,在正向行驶过程中,所述显示装置实时显示采集的正向轨迹点的位置和航向角和采集的正向条件轨迹点的姿态数据,在反向行驶的过程中,所述显示装置实时显示采集的反向条件轨迹点的位置和航向角。
在一些实施例中,包括定位装置、存储装置、和与所述定位装置、所述存储装置和所述姿态传感器信号连接的控制装置,所述定位装置用于对所述行驶装置进行定位,所述存储装置用于存储所述姿态传感器和所述定位装置的测量数据,所述控制装置被配置为用于计算和处理所述定位装置、所述姿态传感器的测量数据和用于控制所述行驶装置行走。
在一些实施例中,所述行驶装置包括农业机械。
基于本发明提供的姿态传感器的校准方法,通过控制行驶装置按预设路线正向行驶和到达终点后反向行驶,在正向行驶过程中在采集的多个正向轨迹点中判断出正向条件轨迹点,并且在反向行驶过程中采集与正向条件轨迹点对应的反向条件轨迹点,并对满足第三条件的正向条件轨迹点和反向条件轨迹点形成计算轨迹点组来计算姿态传感器的校准偏差值,通过采取上述技术特征能够有效地保证用于计算校准偏差值的轨迹点的数据更加合适有效,从而能够更精确地计算出姿态传感器的校准偏差值,从而在姿态传感器检测行驶装置的姿态时能够利用该更准确的校准偏差值来更精确地校准对行驶装置的姿态检测数据。
通过以下参照附图对本发明的示例性实施例的详细描述,本发明的其它特征及其优点将会变得清楚。
附图说明
此处所说明的附图用来提供对本发明的进一步理解,构成本申请的一部分,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。在附图中:
图1为本发明实施例的姿态传感器的校准方法的流程图;
图2为本发明实施例的姿态传感器的校准方法的应用场景的示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的,决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
除非另外具体说明,否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时,应当明白,为了便于描述,附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论,但在适当情况下,所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中,任何具体值应被解释为仅仅是示例性的,而不是作为限制。因此,示例性实施例的其它示例可以具有不同的值。应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步讨论。
为了便于描述,在这里可以使用空间相对术语,如“在……之上”、“在……上方”、“在……上表面”、“上面的”等,用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是,空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如,如果附图中的器件被倒置,则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而,示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位),并且对这里所使用的空间相对描述作出相应解释。
本实施例的姿态传感器安装于行驶装置上,姿态传感器用于在行驶装置行驶过程中对行驶装置的姿态进行检测。姿态包括横滚角、俯仰角和/或航向角。例如,检测行驶装置的横滚角,横滚角为运载体横轴与水平面之间的夹角,行驶装置的横滚角也即行驶装置在向前方行驶时行驶装置的与行驶方向垂直的左右方向与水平面的夹角。例如,还可以检测行驶装置的俯仰角,俯仰角即运载体纵轴与水平面之间的夹角,行驶装置的俯仰角也即行驶装置的行驶方向与水平面之间的夹角。例如,还可以检测行驶装置的航向角,航向角即运载体绕竖直方向转动时相对于基准方向的夹角,行驶装置的航向角即行驶装置的车头朝向与基准方向的夹角,基准方向可以选择正北方向、正东方向、正南方向等多个方向作为基准方向。从而姿态传感器可以包括采集横滚角、俯仰角和/或航向角的传感器,例如姿态传感器可以包括陀螺仪、角度传感器等装置。
在将姿态传感器安装至行驶装置上时,由于安装误差,可能会导致姿态传感器与其在行驶装置上的基准安装平面之间存在夹角,从而可能导致姿态传感器测量得到的横滚角、俯仰角不能反映地面实际的坡度,从而不能反应行驶装置的实际的横滚角、俯仰角等姿态。因此,需要对姿态传感器进行校准,找到姿态传感器的校准偏差值,使姿态传感器的测量能够得到行驶装置的实际姿态。
姿态传感器的校准方法包括:
步骤a,控制行驶装置按照预设路线从起点往终点方向正向行驶,在正向行驶过程中,采集行驶装置的多个正向轨迹点的位置和航向角,计算采集的正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差。
起点为校准开始时行驶装置的初始位置,终点为在预设路线上选择的任一点,为提高校准精度,沿预设路线终点可选择距离起点较远处的一点,例如距离起点十多米处一点。正向指的是沿预设路线从起点到终点的行驶方向。在沿预设路线行驶的过程中,不可避免行驶装置会或多或少与预设路线有所偏离,在正向行驶过程中,可以根据一定规则选取几个正向轨迹点,例如可以选择按照从起点开始等距离间隔取点,或者不等距离间隔取点,或者随机取点等方式来选取多个正向轨迹点。在正向行驶的过程中,选取的正向轨迹点的航向角可通过姿态传感器来测量,位置可以通过定位装置来测量,其中定位装置可以为采用基于全球定位系统、全球导航卫星系统、北斗卫星导航定位系统、罗盘导航系统、伽利略定位系统等定位技术的定位装置。在采集到多个正向轨迹点的位置和航向角数据后,控制装置对姿态传感器的采集数据进行计算,来得到每个正向轨迹点与预设路线的对应点的距离偏差以及航向角度偏差。正向轨迹点与预设路线的对应点的距离偏差也即正向轨迹点与预设路线的横向偏差距离,也即正向轨迹点与预设路线的距离,则对应点指的是假设行驶装置完全按照预设路线行驶,正向轨迹点与预设路线不产生横向偏差时正向轨迹点应该位于预设路线上的对应的点。在实际计算时,可以通过过正向轨迹点做预设路线的法线,法线与预设路线的交点即为该对应点。航向角度偏差为正向轨迹点的航向角与对应点的航向角的角度差,对应点的航向角即行驶装置沿着预设路线不偏差行驶时行驶至对应点处的航向角,在计算时可以将过对应点的预设路线的切线的角度作为航向角。
步骤b,采集正向轨迹点中满足第一条件的正向条件轨迹点的姿态数据,采集正向条件轨迹点沿预设路线与起点和/或与终点的距离,其中第一条件为距离偏差小于第一阈值且航向角度偏差小于第二阈值。
正向条件轨迹点为采集的多个正向轨迹点中满足第一条件的正向轨迹点。当挑选出正向条件轨迹点后,通过采集正向条件轨迹点沿预设路线与起点的距离或者与终点的距离对正向条件轨迹点进行定位,正向条件轨迹点沿预设路线与起点或终点的距离也即正向条件轨迹点在预设路线上的对应点沿预设路线与起点或者终点的距离。当挑选出正向条件轨迹点后,采集正向条件轨迹点的姿态数据,例如横滚角、俯仰角数据。
步骤c,控制行驶装置在到达终点后掉头,并按终点到起点的方向沿预设路线反向行驶,在反向行驶过程中,采集行驶装置的反向轨迹点中与正向条件轨迹点对应且满足第二条件的反向条件轨迹点的位置和航向角,其中,对应指沿预设路线与起点和/或与终点的距离对应,第二条件为与预设路线的对应点的距离偏差小于第三阈值且与预设路线的对应点的航向角度偏差小于第四阈值。
在到达终点后,行驶装置掉头再以终点作为反向行驶的起始点开始沿预设路线往正向行驶的起点方向行驶。由于此时行驶装置行驶的方向与正向行驶的方向相反,即此时行驶装置的行驶方向为从终点往起点方向,故称此时行驶装置的行驶为反向行驶。在反向行驶过程中,采集反向条件轨迹点的位置和航向角。反向条件轨迹点为反向行驶过程中行驶装置的反向轨迹点中与正向条件轨迹点对应且满足第二条件的反向轨迹点,行驶装置的反向行驶轨迹由无数个反向轨迹点组成,其中一些反向轨迹点沿预设路线与起点或者终点的距离和正向条件轨迹点相同,则该反向轨迹点为与正向条件轨迹点对应的反向轨迹点,当对应的反向轨迹点满足第二条件时,该反向轨迹点即为反向条件轨迹点。反向轨迹点沿预设路线与起点或者终点的距离计算方式同上述正向条件轨迹点沿预设路线与起点的距离或者与终点的距离的计算方式。满足第二条件的反向轨迹点指的是与预设路线的对应点的距离偏差小于第三阈值且与预设路线的对应点的航向角度偏差小于第四阈值的反向轨迹点。本实施例中,在寻找反向条件轨迹点时,先找出与正向条件轨迹点对应的反向轨迹点,然后再对找出的与正向条件轨迹点对应的反向轨迹点判断是否满足第二条件,对于其中满足第二条件的反向轨迹点则判断为反向条件轨迹点。本实施例中第三阈值与第一阈值可以相同或者不同,第二阈值与第四阈值可以相同或者不同。
步骤d,采集反向条件轨迹点的姿态数据,将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组,其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值;
在找到反向条件轨迹点后,再判断反向条件轨迹点与其对应的正向条件轨迹点是否满足第三条件,即反向条件轨迹点与其对应的正向条件轨迹点两者之间的距离是否满足小于第五阈值,两者之间的航向角之差的绝对值减去180度的差小于第六阈值(即将正向条件轨迹点的航向角与反向条件轨迹点的航向角做差后取绝对值,再利用该绝对值减去180度得到差,再对该差判断是否小于第六阈值),对于满足第三条件的反向条件轨迹点与其对应的正向条件轨迹点列为一个计算轨迹点组。
步骤e,根据至少一个计算轨迹点组的姿态数据来计算姿态传感器的校准偏差值,根据校准偏差值校准姿态传感器。
在得到多个计算轨迹点组后,可以根据至少一个计算轨迹点组中的正向条件轨迹点的姿态数据和反向条件轨迹点的姿态数据来计算姿态传感器的校准偏差值。在计算得到校准偏差值后,可以利用校准偏差值来对姿态传感器进行校准。
本实施例的姿态传感器的校准方法,通过控制行驶装置按预设路线正向行驶和到达终点后反向行驶,在正向行驶过程中在采集的多个正向轨迹点中判断出正向条件轨迹点,并且在反向行驶过程中采集与正向条件轨迹点对应的反向条件轨迹点,并对满足第三条件的正向条件轨迹点和反向条件轨迹点形成计算轨迹点组来计算姿态传感器的校准偏差值,通过采取上述技术特征能够有效地保证用于计算校准偏差值的轨迹点的数据更加合适有效,从而能够更精确地计算出姿态传感器的校准偏差值,从而在姿态传感器检测行驶装置的姿态时能够利用该更准确的校准偏差值来更精确地校准对行驶装置的姿态检测数据。
在一些实施例中,根据至少一个计算轨迹点组的姿态数据来计算和校准姿态传感器包括:
取一个计算轨迹点组,计算取得的计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的第一平均值作为姿态传感器的校准偏差值,或取多个计算轨迹点组,对取得的每个计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的多个第一平均值再平均计算得到第二平均值,将第二平均值作为姿态传感器的校准偏差值。当姿态数据为横滚角时,如图2所示,(P1,P2)为一个计算轨迹点组,当行驶装置在倾斜的地面Y上行驶,地面Y与水平面夹角为Ag,行驶装置如B1箭头所示的正向方向正向行驶时,姿态传感器在正向条件轨迹点P1测得的与水平面X的夹角为A11,当行驶装置到达终点后掉头如B2箭头所示的反向进行反向行驶时,姿态传感器在与正向条件轨迹点P1对应的反向条件轨迹点P2处测得的行驶装置与水平面X的夹角为A12,则姿态传感器测量行驶装置的横滚角时的校准偏差值Af可由下式计算:Af=A11-Ag,Af=A12+Ag;根据上述两式可求得姿态传感器对应横滚角的校准偏差值Af:
Af=(A11+A12)/2。对应的当姿态数据为俯仰角时,计算原理相同。即根据一个计算轨迹点组,即可较为精确地求得姿态传感器的校准偏差值。当利用多个计算轨迹点组时,先通过各计算轨迹点组分别求得校准偏差值,再将多个校准偏差值取平均值作为姿态传感器的最终的校准偏差值,可更精确地计算得到姿态传感器的校准偏差值。
在一些实施例中,预设路线为直线。将预设路线设为直线,操作行驶装置正向行驶和反向行驶时能够更加准确,使校准过程也更加方便。当预设路线为直线时,所有正向条件轨迹点在预设路线上的对应点的航向角均相同,所有反向条件轨迹点在预设路线上的对应点的航向角也均相同。
在一些实施例中,预设路线的方程为:(x-x1)/cos(a)=(y-y1)/sin(a)=L,其中设定起点的坐标为(x1,y1),a为预设路线的设定航向角度,即a也为直线与二维坐标系的x轴的夹角角度,即本实施例中以x轴所在方向为航向角的基准方向。
在一些实施例中,行驶装置为农业机械,根据农业机械的驾驶精度要求,第一阈值和/或第三阈值为2.5cm,根据农业机械的驾驶精度要求,第二阈值和/或第四阈值为0.2°。
在一些实施例中,行驶装置上设有显示装置,在正向行驶过程中,显示装置实时显示采集的正向轨迹点的位置和航向角和采集的正向条件轨迹点的姿态数据,在反向行驶的过程中,显示装置实时显示采集的反向条件轨迹点的位置和航向角。显示装置包括与控制装置信号连接的显示屏。
在一些实施例中,行驶装置包括定位装置、存储装置、和与定位装置、存储装置和姿态传感器信号连接的控制装置,定位装置用于对行驶装置进行定位,存储装置用于存储姿态传感器和定位装置的测量数据,控制装置被配置为用于计算和处理定位装置、姿态传感器的测量数据和用于控制行驶装置行走。
在一些实施例中,行驶装置包括农业机械,姿态传感器用于对农业机械辅助驾驶。
在一些实施例中,在上面所描述的控制装置可以为用于执行本发明所描述功能的通用处理器、可编程逻辑控制器(Programmable Logic Controller,简称:PLC)、数字信号处理器(Digital Signal Processor,简称:DSP)、专用集成电路(Application SpecificIntegrated Circuit,简称:ASIC)、现场可编程门阵列(Field-Programmable Gate Array,简称:FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件或者其任意适当组合。
在一些实施例中,上面描述的存储装置可以用于存储程序或者数据的存储器,存储器可以是随机存取存储器(Random Access Memory,RAM),只读存储器(Read OnlyMemory,ROM),可编程只读存储器(Programmable Read-Only Memory,PROM),可擦除只读存储器(Erasable Programmable Read-Only Memory,EPROM)等。
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制;尽管参照较佳实施例对本发明进行了详细的说明,所属领域的普通技术人员应当理解:依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换;而不脱离本发明技术方案的精神,其均应涵盖在本发明请求保护的技术方案范围当中。
Claims (10)
1.一种姿态传感器的校准方法,所述姿态传感器安装于行驶装置上,其特征在于,所述姿态传感器的校准方法包括:
控制所述行驶装置按照预设路线从起点往终点方向正向行驶,在正向行驶过程中,采集行驶装置的多个正向轨迹点的位置和航向角,计算采集的正向轨迹点与预设路线的对应点的距离偏差和航向角度偏差;
采集正向轨迹点中满足第一条件的正向条件轨迹点的姿态数据,采集正向条件轨迹点沿预设路线与起点和/或与终点的距离,其中第一条件为距离偏差小于第一阈值且航向角度偏差小于第二阈值;
控制所述行驶装置在到达终点后掉头,并按终点到起点的方向沿预设路线反向行驶,在反向行驶过程中,对行驶装置的反向轨迹点中与正向条件轨迹点沿预设路线与起点和/或与终点的距离对应的反向轨迹点判断是否满足第二条件,对满足第二条件的反向轨迹点判断为反向条件轨迹点,采集反向条件轨迹点的位置和航向角,其中第二条件为与预设路线的对应点的距离偏差小于第三阈值且与预设路线的对应点的航向角度偏差小于第四阈值;
采集反向条件轨迹点的姿态数据,将满足第三条件的反向条件轨迹点与对应的正向条件轨迹点列为计算轨迹点组,其中第三条件为两者之间的距离小于第五阈值且两者之间的航向角之差的绝对值减去180度的差小于第六阈值;
根据至少一个计算轨迹点组的姿态数据来计算所述姿态传感器的校准偏差值,根据所述校准偏差值校准所述姿态传感器。
2.如权利要求1所述的姿态传感器的校准方法,其特征在于,根据至少一个计算轨迹点组的姿态数据来计算和校准所述姿态传感器包括:
取一个计算轨迹点组,计算取得的计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的第一平均值作为所述姿态传感器的校准偏差值,或取多个计算轨迹点组,对取得的每个计算轨迹点组的正向条件轨迹点的姿态数据与反向条件轨迹点的姿态数据求第一平均值,将求得的多个第一平均值再平均计算得到第二平均值,将第二平均值作为所述姿态传感器的校准偏差值。
3.如权利要求3所述的姿态传感器的校准方法,其特征在于,所述姿态数据包括横滚角和/或俯仰角。
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任一所述的行驶装置,其特征在于,所述行驶装置包括农业机械。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210079271.5A CN114394102B (zh) | 2022-01-24 | 2022-01-24 | 姿态传感器的校准方法和行驶装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210079271.5A CN114394102B (zh) | 2022-01-24 | 2022-01-24 | 姿态传感器的校准方法和行驶装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114394102A true CN114394102A (zh) | 2022-04-26 |
CN114394102B CN114394102B (zh) | 2023-05-05 |
Family
ID=81232655
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210079271.5A Active CN114394102B (zh) | 2022-01-24 | 2022-01-24 | 姿态传感器的校准方法和行驶装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114394102B (zh) |
Citations (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1993023721A1 (en) * | 1992-05-15 | 1993-11-25 | Zexel Corporation | Calibration method for a relative heading sensor |
JPH06273443A (ja) * | 1993-03-18 | 1994-09-30 | Nissan Motor Co Ltd | 検出ヨーレイト補正装置 |
DE102006061748A1 (de) * | 2005-12-31 | 2007-07-12 | Tsinghua University | Abweichungskorrektionssystem zur Positionierung eines beweglichen Gegenstandes und Bewegungsverfolgungsmethode |
US20100217517A1 (en) * | 2009-02-26 | 2010-08-26 | Aisin Aw Co., Ltd. | Navigation device, method, and program |
JP2012026992A (ja) * | 2010-07-28 | 2012-02-09 | Honda Motor Co Ltd | 車両ピッチ角の推定装置 |
CN103026176A (zh) * | 2010-07-22 | 2013-04-03 | 高通股份有限公司 | 用于校准车辆导航系统的动态参数的装置及方法 |
CN103759730A (zh) * | 2014-01-16 | 2014-04-30 | 南京师范大学 | 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 |
CN104290752A (zh) * | 2013-07-19 | 2015-01-21 | 现代摩比斯株式会社 | 偏航率传感器的偏差补正装置和方法及其车速控制系统 |
CN105116881A (zh) * | 2015-06-26 | 2015-12-02 | 武汉科技大学 | 一种基于在线学习机制的轮式机器人自适应导航方法 |
JP2016032960A (ja) * | 2014-07-31 | 2016-03-10 | 富士通株式会社 | 車両移動量算出装置,プログラム及び方法 |
CN106643800A (zh) * | 2016-12-27 | 2017-05-10 | 上海司南卫星导航技术股份有限公司 | 航向角误差校准方法及自动导航驾驶系统 |
CN108170140A (zh) * | 2017-12-26 | 2018-06-15 | 徐工集团工程机械股份有限公司 | 一种调试和检验摊铺机直线行走的方法 |
WO2018194768A1 (en) * | 2017-04-21 | 2018-10-25 | X Development Llc | Method and system for simultaneous localization and sensor calibration |
CN109470276A (zh) * | 2018-12-20 | 2019-03-15 | 中国人民解放军战略支援部队信息工程大学 | 基于零速修正的里程计标定方法与装置 |
DE102018007972A1 (de) * | 2018-10-09 | 2019-03-28 | Daimler Ag | Verfahren zur Kalibrierung eines Gierratensensors eines Fahrassistenzsystems |
JP2019113434A (ja) * | 2017-12-25 | 2019-07-11 | ITD Lab株式会社 | 校正装置、校正方法及び校正プログラム |
JP2019185437A (ja) * | 2018-04-11 | 2019-10-24 | 株式会社クボタ | 作業車 |
US20200124443A1 (en) * | 2018-10-19 | 2020-04-23 | Htc Corporation | Electronic device and pose-calibration method thereof |
CN111238471A (zh) * | 2020-01-17 | 2020-06-05 | 青岛农业大学 | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 |
CN111857104A (zh) * | 2020-08-03 | 2020-10-30 | 广州极飞科技有限公司 | 自驾仪校准方法、装置、电子设备及计算机可读存储介质 |
CN113341172A (zh) * | 2021-06-09 | 2021-09-03 | 徐州徐工矿业机械有限公司 | 一种挖掘机行驶速度和直线偏移量的自动检测系统及方法 |
CN113393656A (zh) * | 2021-06-10 | 2021-09-14 | 徐州徐工筑路机械有限公司 | 一种平地机无人驾驶系统 |
CN113655453A (zh) * | 2021-08-27 | 2021-11-16 | 阿波罗智能技术(北京)有限公司 | 用于传感器标定的数据处理方法、装置及自动驾驶车辆 |
-
2022
- 2022-01-24 CN CN202210079271.5A patent/CN114394102B/zh active Active
Patent Citations (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1993023721A1 (en) * | 1992-05-15 | 1993-11-25 | Zexel Corporation | Calibration method for a relative heading sensor |
JPH06273443A (ja) * | 1993-03-18 | 1994-09-30 | Nissan Motor Co Ltd | 検出ヨーレイト補正装置 |
DE102006061748A1 (de) * | 2005-12-31 | 2007-07-12 | Tsinghua University | Abweichungskorrektionssystem zur Positionierung eines beweglichen Gegenstandes und Bewegungsverfolgungsmethode |
US20100217517A1 (en) * | 2009-02-26 | 2010-08-26 | Aisin Aw Co., Ltd. | Navigation device, method, and program |
CN103026176A (zh) * | 2010-07-22 | 2013-04-03 | 高通股份有限公司 | 用于校准车辆导航系统的动态参数的装置及方法 |
JP2012026992A (ja) * | 2010-07-28 | 2012-02-09 | Honda Motor Co Ltd | 車両ピッチ角の推定装置 |
CN104290752A (zh) * | 2013-07-19 | 2015-01-21 | 现代摩比斯株式会社 | 偏航率传感器的偏差补正装置和方法及其车速控制系统 |
CN103759730A (zh) * | 2014-01-16 | 2014-04-30 | 南京师范大学 | 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 |
JP2016032960A (ja) * | 2014-07-31 | 2016-03-10 | 富士通株式会社 | 車両移動量算出装置,プログラム及び方法 |
CN105116881A (zh) * | 2015-06-26 | 2015-12-02 | 武汉科技大学 | 一种基于在线学习机制的轮式机器人自适应导航方法 |
CN106643800A (zh) * | 2016-12-27 | 2017-05-10 | 上海司南卫星导航技术股份有限公司 | 航向角误差校准方法及自动导航驾驶系统 |
WO2018194768A1 (en) * | 2017-04-21 | 2018-10-25 | X Development Llc | Method and system for simultaneous localization and sensor calibration |
JP2019113434A (ja) * | 2017-12-25 | 2019-07-11 | ITD Lab株式会社 | 校正装置、校正方法及び校正プログラム |
CN108170140A (zh) * | 2017-12-26 | 2018-06-15 | 徐工集团工程机械股份有限公司 | 一种调试和检验摊铺机直线行走的方法 |
JP2019185437A (ja) * | 2018-04-11 | 2019-10-24 | 株式会社クボタ | 作業車 |
DE102018007972A1 (de) * | 2018-10-09 | 2019-03-28 | Daimler Ag | Verfahren zur Kalibrierung eines Gierratensensors eines Fahrassistenzsystems |
US20200124443A1 (en) * | 2018-10-19 | 2020-04-23 | Htc Corporation | Electronic device and pose-calibration method thereof |
CN109470276A (zh) * | 2018-12-20 | 2019-03-15 | 中国人民解放军战略支援部队信息工程大学 | 基于零速修正的里程计标定方法与装置 |
CN111238471A (zh) * | 2020-01-17 | 2020-06-05 | 青岛农业大学 | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 |
CN111857104A (zh) * | 2020-08-03 | 2020-10-30 | 广州极飞科技有限公司 | 自驾仪校准方法、装置、电子设备及计算机可读存储介质 |
CN113341172A (zh) * | 2021-06-09 | 2021-09-03 | 徐州徐工矿业机械有限公司 | 一种挖掘机行驶速度和直线偏移量的自动检测系统及方法 |
CN113393656A (zh) * | 2021-06-10 | 2021-09-14 | 徐州徐工筑路机械有限公司 | 一种平地机无人驾驶系统 |
CN113655453A (zh) * | 2021-08-27 | 2021-11-16 | 阿波罗智能技术(北京)有限公司 | 用于传感器标定的数据处理方法、装置及自动驾驶车辆 |
Non-Patent Citations (7)
Title |
---|
孙中森;王曲;罗海勇;唐怀玉;: "基于在线地磁指纹的航迹校准算法" * |
张博渊;王世博;葛世荣;: "惯性导航初始对准偏差与安装偏差校准方法对采煤机定位精度影响" * |
张美娜;尹文庆;林相泽;吕晓兰;: "RTK-DGPS融合惯性传感器的车辆导航参数计算方法" * |
戚宪东;郭英;孙玉曦;姬现磊;: "基于四元数法的航向修正算法分析" * |
李维刚: "MEMS惯性测量单元全参数标定方法研究" * |
汪天生;李擎;: "改进的零速修正算法在行人自主导航中的应用" * |
黎永键;赵祚喜;: "基于Kalman滤波的田间导航车辆定位校正方法研究" * |
Also Published As
Publication number | Publication date |
---|---|
CN114394102B (zh) | 2023-05-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5574649A (en) | Position-locating method and apparatus including corrections for elevational changes | |
EP3171134B1 (en) | Sensor alignment calibration | |
CN106643800B (zh) | 航向角误差校准方法及自动导航驾驶系统 | |
CN100380098C (zh) | 车辆动态特性检测器、车载处理系统、检测信息校正器及车载处理器 | |
EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
US8209055B2 (en) | System for sensing state and position of robot | |
CN107521559B (zh) | 转向角标定方法、运动轨迹计算方法和设备和车载设备 | |
CN111309001B (zh) | 具有基于主方向的坐标校正的航位推算引导系统和方法 | |
US11486988B2 (en) | Method for calibrating the alignment of a moving object sensor | |
US11167743B2 (en) | Automatic pitch mounting compensation in an automatic steering system | |
CN111845740A (zh) | 动态偏航角速率偏差估计的方法和装置 | |
KR102431904B1 (ko) | 정밀지도를 이용한 라이다 센서 캘리브레이션 방법 | |
CN114167470A (zh) | 一种数据处理方法和装置 | |
US10024977B1 (en) | Low speed heading measurement | |
CN112977603B (zh) | 电机控制的方法和装置、农机以及计算机可读存储介质 | |
WO2011159185A1 (ru) | Способ и устройство определения направления начала движения | |
CN114394102A (zh) | 姿态传感器的校准方法和行驶装置 | |
JP2005250696A (ja) | 車両自律走行制御システム及び方法 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
JP3581392B2 (ja) | 積分型センシング装置 | |
JP2007155365A (ja) | 方位センサの補正係数演算装置及び演算プログラム | |
JP2017181195A (ja) | 測定装置、測定方法、及び、プログラム | |
JP4831441B2 (ja) | 方位センサの補正係数演算装置及び演算プログラム | |
JP3401997B2 (ja) | 車両走行方位算出装置 | |
JP2958020B2 (ja) | 移動車の走行制御装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |