CN111765906B

CN111765906B - 误差校准方法及装置

Info

Publication number: CN111765906B
Application number: CN202010745632.6A
Authority: CN
Inventors: 孙国岐; 郭承志; 贾全
Original assignee: Sany Robot Technology Co Ltd
Current assignee: Sany Robot Technology Co Ltd
Priority date: 2020-07-29
Filing date: 2020-07-29
Publication date: 2022-06-14
Anticipated expiration: 2040-07-29
Also published as: CN111765906A

Abstract

本发明实施例提供的误差校准方法及装置，涉及惯性导航模块领域，所述方法包括采集预定时间内车辆处于第一预定位置时的第一加速度；将车辆的车体旋转预定角度使车辆处于第二预定位置，采集预定时间内车辆处于第二预定位置时的第二加速度；根据第一加速度及第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角；根据横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角；根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准。避免了旋转车体法对于车体重量的局限性，避免了间接校准法带来的二次误差，提升了惯性导航模块的安装精度，进一步使惯性导航模块的定位更加准确。

Description

误差校准方法及装置

技术领域

本发明涉及惯性导航模块领域，具体而言，涉及一种误差校准方法及装置。

背景技术

装备制造业是国家重点支持的行业，亟需优化其产业结构，智能制造是产业结构转变过程中的新兴制造模式。其中，自动导引车(Automated Guided Vehicle，AGV)是实现智能制造的重要部分。

AGV之所以能够自动运行，其中定位模块起到了非常重要的作用。定位模块的定位方式很多采用惯性导航模块(Inertial measurement unit，IMU)与其它传感器的融合来实现，除了IMU的内部参数外，在安装过程中无法保证IMU坐标系与车体坐标系完全一致，如果不对IMU坐标系加以校准，将导致定位模块有非常大的误差，因此安装IMU的误差校准是必不可少的。

现有技术中，对于安装IMU的误差校准技术有旋转车体法、间接校准法等，其主要存在以下不足：

旋转车体法只适用于比较轻便的物体，例如无人机、玩具车等设备，而AGV车体比较重，因此旋转车体法并不适用于AGV。

间接校准法通常先把IMU校准到车体另外一个传感器的坐标系下，然后再计算该传感器坐标系与车体坐标系的误差，会引入二次误差，因此效果不太理想。

发明内容

本发明的目的包括，例如，提供了一种误差校准方法及装置，其能够减小将惯性导航模块安装在AGV车体上的误差。

本发明的实施例可以这样实现：

第一方面，本发明实施例提供了一种误差校准方法，应用于惯性导航模块，所述惯性导航模块安装在车辆，包括：

采集预定时间内所述车辆处于第一预定位置时的第一加速度；

将所述车辆的车体旋转预定角度使所述车辆处于第二预定位置，采集预定时间内所述车辆处于所述第二预定位置时的第二加速度；

根据所述第一加速度及所述第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角；所述标准坐标系为车辆坐标系；所述实际坐标系为惯性导航模块安装的坐标系；

根据所述横滚误差角和所述俯仰误差角计算所述标准坐标系与所述实际坐标系之间的航向误差角；

根据所述横滚误差角、所述俯仰误差角及所述航向误差角对所述惯性导航模块的安装误差进行校准。

在可选的实施方式中，根据所述第一加速度及所述第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角的步骤，包括：

根据所述预定角度获取所述第一加速度与所述第二加速度之间的转化关系；

根据所述转化关系、预定的转换矩阵及预定的所述标准坐标系下的标准加速度计算所述横滚误差角及所述俯仰误差角；所述标准加速度包括第一标准加速度和第二标准加速度；所述第一标准加速度为所述车辆处于第一预定位置时的标准加速度；所述第二标准加速度为所述车辆处于第二预定位置时的标准加速度。

在可选的实施方式中，所述标准坐标系与实际坐标系由绕X轴的横滚角φ、绕Y轴的俯仰角θ和绕Z轴的航向角ψ三个欧拉角构成的方向余弦矩阵表示；

所述预定的转换矩阵为：

其中，T₂为所述实际坐标系与所述标准坐标系之间绕Y轴的转换矩阵；T₁为所述实际坐标系与所述标准坐标系之间绕Z轴的转换矩阵；T₃为所述实际坐标系与所述标准坐标系之间绕X轴的转换矩阵。在可选的实施方式中，根据所述转化关系、预定的转换矩阵及预定的所述标准坐标系下的标准加速度计算所述横滚误差角及所述俯仰误差角的步骤，包括：

根据以下公式计算所述横滚误差角及所述俯仰误差角：

其中，A1_b为车辆处于所述第一预定位置的所述第一标准加速度；A2_b为车辆处于所述第二预定位置的所述第二标准加速度；

为偏差转换矩阵，

是

的转置矩阵，

为所述实际坐标系与所述标准坐标系之间绕Y轴的偏差转换矩阵；

为所述实际坐标系与所述标准坐标系之间绕Z轴的偏差转换矩阵；

为所述实际坐标系与所述标准坐标系之间绕X轴的偏差转换矩阵

其中，θ^error为所述俯仰误差角；φ^error为所述横滚误差角；ψ^error为所述航向误差角；

A1_v为所述第一加速度；A2_v为所述第二加速度；

为当预定角度为π时所述第一加速度与所述第二加速度之间的转化关系。

在可选的实施方式中，根据所述横滚误差角、所述俯仰误差角计算所述标准坐标系与所述实际坐标系之间的航向误差角的步骤，包括：

控制所述车辆匀速行走一段直线距离，获取所述车辆在直线距离段的第三加速度；

根据所述第三加速度、所述横滚误差角、所述俯仰误差角及所述直线距离计算所述标准坐标系与所述实际坐标系之间的航向误差角。

第二方面，本发明实施例提供了一种误差校准装置，应用于惯性导航模块，所述惯性导航模块安装在车辆，所述装置包括：

采集模块，用于采集预定时间内所述车辆处于第一预定位置时的第一加速度；

以及还用于将所述车辆的车体旋转预定角度使所述车辆处于第二预定位置，采集预定时间内所述车辆处于所述第二预定位置时的第二加速度；

处理模块，用于根据所述第一加速度及所述第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角；所述标准坐标系为车辆坐标系；所述实际坐标系为惯性导航模块安装的坐标系；

以及还用于根据所述横滚误差角和所述俯仰误差角计算所述标准坐标系与所述实际坐标系之间的航向误差角；

以及还用于根据所述横滚误差角、所述俯仰误差角及所述航向误差角对所述惯性导航模块的安装误差进行校准。

在可选的实施方式中，所述处理模块根据所述第一加速度及所述第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角的方式，包括：

所述预定的转换矩阵为：

其中，T₂为所述实际坐标系与所述标准坐标系之间绕Y轴的转换矩阵；T₁为所述实际坐标系与所述标准坐标系之间绕Z轴的转换矩阵；T₃为所述实际坐标系与所述标准坐标系之间绕X轴的转换矩阵。

在可选的实施方式中，所述处理模块根据所述转化关系、预定的转换矩阵及预定的所述标准坐标系下的标准加速度计算所述横滚误差角及所述俯仰误差角的方式，包括：

根据以下公式计算所述横滚误差角及所述俯仰误差角：

为偏差转换矩阵，

是

的转置矩阵，

为所述实际坐标系与所述标准坐标系之间绕X轴的偏差转换矩阵；

A1_v为所述第一加速度；A2_v为所述第二加速度；

在可选的实施方式中，所述处理模块根据所述横滚误差角、所述俯仰误差角计算所述标准坐标系与所述实际坐标系之间的航向误差角的方式，包括：

本发明实施例的有益效果包括，例如：采集车辆处于不同位置时的加速度，通过不同位置的加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角，再根据已知的横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角，最后根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准；避免了旋转车体法对于车体重量的局限性，避免了间接校准法带来的二次误差，提升了惯性导航模块的安装精度，进一步使惯性导航模块的定位更加准确。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明实施例提供的一种车体坐标系的示意图。

图2为本发明实施例提供的一种惯性导航模块坐标系示意图。

图3为本发明实施例提供的航向角的示意图。

图4为本发明实施例提供的一种误差校准方法的流程示意图。

图5为本发明实施例提供的另一种误差校准方法的流程示意图。

图6为本发明实施例提供的第一预定位置的示意图。

图7为本发明实施例提供的第二预定位置的示意图。

图8为本发明实施例提供的航向误差角的示意图。

图9为本发明实施例提供的一种误差校准装置的功能模块示意图。

图标：100-误差校准装置；110-采集模块；120-处理模块。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

需要说明的是，在不冲突的情况下，本发明的实施例中的特征可以相互结合。

旋转车体法：由于AGV车体比较重，只能绕Z轴做航向的旋转，在不借助外部设备情况下无法做到绕X轴做横滚和绕Y轴做俯仰方向旋转。因此这种旋转校准法只适用于比较轻便的物体，例如无人机、玩具车等设备。

间接校准法：间接校准法通常先把IMU校准到车体另外一个传感器的坐标系下，如激光雷达、相机等，然后再计算该传感器坐标系与车体坐标系的误差；这种方法是假设传感器坐标系相对车体为已知的，但实际也需要人为的校准，因此间接校准法会引入二次误差，且计算过程复杂、繁琐，因此效果不太理想。

在本发明实施例中，车体坐标系的定义符合右手定则，具体为：X轴为车体前向方向，Y轴为车体左侧方向，Z轴为垂直车体平面朝上方向，如图1所示。

请参考图1，为本发明实施例提供的一种车体坐标系的示意图。

请参考图2，为本发明实施例提供的一种惯性导航模块坐标系示意图。

在图2中，XYZ坐标系为车体坐标系，x'y'z'坐标系为IMU坐标系，定义z'轴与XZ平面之间构成的夹角为横滚误差角φ^error，x'轴与XY平面之间构成的夹角为俯仰误差角θ^error，x'轴在XY平面的投影与X轴之间夹角为航向误差角ψ^error。

需要说明的是，AGV在空间的转动可以由绕X轴的横滚角φ、绕Y轴的俯仰角θ、绕Z轴的航向角ψ三个欧拉角来表示，车体坐标系与IMU坐标系之间的坐标转换用这三个欧拉角构成的方向余弦矩阵来表示。

在本实施例中，定义AGV在空间的旋转顺序为Z-X-Y，则从实际坐标系到标准坐标系之间的转化关系可以分为三次的转换：

第一次，绕Y轴旋转的俯仰角θ，定义从旋转前到旋转后的转换矩阵为：

同理，第二次，绕Z轴旋转的航向角ψ，如图3所示，定义从旋转前到旋转后的转换矩阵为：

同理，第三次旋转为绕X轴旋转的横滚角φ，定义从旋转前到旋转后的转换矩阵为：

因此，从实际坐标系转换到标准坐标系的方向余弦矩阵表示为：

请参考图4，为本发明实施例提供的一种误差校准方法的流程示意图。

步骤101，采集预定时间内车辆处于第一预定位置时的第一加速度。

步骤102，将车辆的车体旋转预定角度使车辆处于第二预定位置，采集预定时间内车辆处于第二预定位置时的第二加速度。

步骤103，根据第一加速度及第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角。

步骤104，根据横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角。

步骤105，根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准。

在本实施例中，首先采集预定时间内车辆处于第一预定位置时的第一加速度，将车辆的车体旋转预定角度使车辆处于第二预定位置，采集预定时间内车辆处于第二预定位置时的第二加速度，其中，第一预定位置与第二预定位置相差180度；然后根据第一加速度及第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角，其中，标准坐标系为车辆坐标系，实际坐标系为惯性导航模块安装的坐标系；根据横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角；最后，根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准。避免了旋转车体法对于车体重量的局限性，避免了间接校准法带来的二次误差，提升了惯性导航模块的安装精度，进一步使惯性导航模块的定位更加准确。

请参考图5，为本发明实施例提供的另一种误差校准方法的流程示意图。

需要说明的是，本实施例所提供的误差校准方法，其基本原理及产生的技术效果和上述实施例相同，为简要描述，本实施例部分未提及之处，可参考上述的实施例中相应内容。

惯性导航模块安装在AGV的车体，安装时尽可能保证惯性导航模块的安装方向与车体的方向保持一致，且靠近车体正中心。具体示意图如图1所示。

采集第一加速度时，先选择一个平面，使车辆的车体在平面上保持静止，然后通过在车体边缘划线的方式记录下车体的初始方向轮廓，并通过加速度计采集预定时间内的一组加速度数据，并求取该加速度数据的平均值，并将该平均值作为第一加速度。

请参考图6，为本发明实施例提供的第一预定位置的示意图。

需要说明的是，第一预定位置为车体边缘划线部分所包含的区域；预定时间可以是但不限于1分钟、两分钟，具体值在此不做限制。

需要说明的是，在本实施例中，预定角度为180°；第二预定位置为车体处于第一预定位置时，将车体绕车体正中心旋转180°得到的区域。

请参考图7，为本发明实施例提供的第二预定位置的示意图。

将车体从第一预定位置移动到第二预定位置可以车体绕车体正中心旋转180°得到，也可以根据车体边缘划线得到的初始方向轮廓调转车头的方向。

使车辆的车体在第二预定位置的平面上保持静止，然后通过加速度计采集预定时间内的一组加速度数据，并求取该加速度数据的平均值，并将该平均值作为第二加速度。

需要说明的是，步骤103包括两个子步骤，本步骤中未提及之处将在其子步骤中进行详细的阐述。

子步骤103-1，根据预定角度获取第一加速度与第二加速度之间的转化关系。

在本实施例中，定义车体在第一预定位置时车体的第一加速度为A1_v，定义车体在第二预定位置时车体的第二加速度为A2_v。由于第一预定位置与第二预定位置相差180°，因此第一加速度与第二加速度之间的转换关系为：

由上述转换关系可以得到：

其中，

为当预定角度为π时第一加速度与第二加速度之间的转化关系，az_v为第一加速度与第二加速度之和在Z轴的分量。

子步骤103-2，根据转化关系、预定的转换矩阵及预定的标准坐标系下的标准加速度计算横滚误差角及俯仰误差角。

从实际坐标系转换到标准坐标系的方向余弦矩阵即为预定的转换矩阵。

标准加速度包括第一标准加速度A1_b和第二标准加速度A2_b，第一标准加速度为车辆处于第一预定位置时的标准加速度，第二标准加速度为车辆处于第二预定位置时的标准加速度，

本实施例中，根据以下公式计算所述横滚误差角及所述俯仰误差角：

其中，

为偏差转换矩阵，

是

的转置矩阵，

其中，θ^error为所述俯仰误差角；φ^error为所述横滚误差角；ψ^error为所述航向误差角。

由

可以得到

进一步得到以下等式：

由上述等式得到：

需要说明的是，步骤104包括两个子步骤，本步骤中未提及之处将在其子步骤中进行详细的阐述。

子步骤104-1，控制车辆匀速行走一段直线距离，获取车辆在直线距离段的第三加速度。

控制车辆匀速行走一段直线距离，通过加速度计实时获取车辆在行走直线时的加速度A_b，将该加速度A_b投影到车体坐标系所在平面x'y'z'，此时，惯导模块坐标系投射的平面与车体坐标系XYZ之间相差的角即为航向误差角ψ^error。

加速度A_b在车体坐标系所在平面x'y'z'的投影即为第三加速度A_h；

因此

请参考图8，为本发明实施例提供的航向误差角的示意图。

子步骤104-2，根据第三加速度、横滚误差角、俯仰误差角及直线距离计算标准坐标系与实际坐标系之间的航向误差角。

通过第三加速度Ah分别在X轴、Y轴的积分式，得到：

其中，

为A_h在X轴的积分，

为A_h在Y轴的积分。

由上述积分式得到：

需要说明的是，在本实施例中通过多次控制车辆匀速行走一段直线距离，来求取多个航向误差角ψ^error，去掉超出预定阈值范围的ψ^error，在一种优选的实施例中预定阈值范围为±5°，最后取剩下的航向误差角ψ^error的平均值，将该平均值作为最终的航向误差角ψ^error。

根据横滚误差角、俯仰误差角及航向误差角校准惯性导航模块。

综上所述，本发明实施例提供的误差校准方法，所述方法包括采集预定时间内车辆处于第一预定位置时的第一加速度；将车辆的车体旋转预定角度使车辆处于第二预定位置，采集预定时间内车辆处于第二预定位置时的第二加速度；根据第一加速度及第二加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角；根据横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角；根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准。采集车辆处于不同位置时的加速度，通过不同位置的加速度计算标准坐标系与实际坐标系之间的横滚误差角及俯仰误差角，再根据已知的横滚误差角和俯仰误差角计算标准坐标系与实际坐标系之间的航向误差角，最后根据横滚误差角、俯仰误差角及航向误差角对惯性导航模块的安装误差进行校准；避免了旋转车体法对于车体重量的局限性，避免了间接校准法带来的二次误差，提升了惯性导航模块的安装精度，进一步使惯性导航模块的定位更加准确。

请参照图9，为本发明实施例所提供的一种误差校准装置的功能模块示意图。需要说明的是，本发明实施例所提供的一种误差校准装置100，其基本原理及产生的技术效果与前述方法实施例相同，为简要描述，本实施例中未提及部分，可参考前述方法实施例中的相应内容。误差校准装置100用于执行图4及图5所述的误差校准方法，其包括采集模块110及处理模块120。

可以理解的，在一种实施例中，通过采集模块110执行步骤101及步骤102。

可以理解的，在一种实施例中，通过处理模块120执行步骤103、步骤104及步骤105。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。