CN107614803B - 作业机械的校正装置、作业机械以及作业机械的校正方法 - Google Patents

Abstract

在修正因输出具有供工作装置安装并进行回转的回转体的作业机械的姿势的姿势检测装置相对于作业机械的偏移而引起的误差时，使用所述作业机械为第一姿势时的所述作业机械的一部分的位置即第一位置与所述作业机械为第二姿势时的所述一部分的位置即第二位置来修正所述误差。

Description

作业机械的校正装置、作业机械以及作业机械的校正方法

技术领域

本发明涉及作业机械的校正装置、作业机械以及作业机械的校正方法。

背景技术

在具备回转体的作业机械中，作为用于检测作业姿势并确定的装置，已知有具备陀螺仪传感器的装置(例如专利文献1)。

在先技术文献

专利文献

专利文献1：日本特开2015-001385号公报

发明内容

发明要解决的课题

用于检测作业机械的姿势的装置(以下适当称为姿势检测装置)安装于作业机械，对加速度进行检测或者对角速度进行检测。若姿势检测装置相对于应安装于作业机械的基准的位置而偏移地被安装，则检测值中包含误差。使用包含误差的检测值求出的作业机械的姿势也包含误差，因此，需要对姿势检测装置的检测值所包含的误差进行修正。在成为姿势检测装置的基准的轴相对于作业机械的前后方向上的轴产生了横摆方向的偏移的情况下，有时姿势检测装置所检测的工作装置的姿势角倾斜。

本发明的目的在于，对姿势检测装置的检测值所包含的误差进行修正，该误差是由于姿势检测装置被设置为横摆角相对于作业机械的前后方向偏移而产生倾斜所引起的。

用于解决课题的方案

本发明是一种作业机械的校正装置，其中，所述作业机械具有供工作装置安装并进行回转的回转体，所述作业机械的校正装置在修正因输出所述作业机械的姿势的姿势检测装置相对于所述作业机械的偏移而引起的误差时，使用所述作业机械为第一姿势时的所述作业机械的一部分的位置即第一位置与所述作业机械为第二姿势时的所述一部分的位置即第二位置来修正所述误差。

优选的是，所述一部分的位置是所述工作装置的一部分的位置，所述第一位置是所述作业机械设置于倾斜面且所述回转体朝向第一方向时的位置，所述第二位置是所述作业机械设置于倾斜面且所述回转体朝向第二方向时的位置。

优选的是，所述第一位置以及所述第二位置是所述姿势检测装置输出的俯仰角为0度时的位置。

优选的是，所述一部分的位置是所述作业机械具有的所述工作装置的一部分的位置。

优选的是，所述第一位置以及所述第二位置是以所述作业机械以外的位置为基准而得到的所述一部分的位置，通过使用从所述姿势检测装置输出的与所述作业机械的姿势相关的信息而求出。

优选的是，所述作业机械的校正装置一边修正用于对与所述作业机械的姿势相关的信息进行修正的参数，一边反复进行所述第一位置以及所述第二位置的重新计算，使用所述第一位置与所述第二位置之差成为阈值以下时的所述参数来修正所述误差。

优选的是，与所述作业机械的姿势相关的信息为所述姿势检测装置输出的俯仰角以及侧倾角。

本发明是具有前述的作业机械的校正装置的作业机械。

本发明是一种作业机械的校正方法，其中，所述作业机械具有供工作装置安装并进行回转的回转体，在所述作业机械的校正方法中，在修正因输出所述作业机械的姿势的姿势检测装置相对于所述作业机械的偏移而引起的误差时，获取所述作业机械为第一姿势时的所述作业机械的一部分的位置即第一位置，获取所述作业机械为第二姿势时的所述一部分的位置即第二位置，使用所述第一位置以及所述第二位置来修正所述误差。

本发明能够修正检测装置的检测值所包含的误差，该误差是因姿势检测装置被设置为横摆角相对于作业机械的前后方向偏移而产生倾斜所引起的。

附图说明

图1是实施方式1的作业机械的立体图。

图2是用于说明车身坐标系的图。

图3是示出包含实施方式1的作业机械的校正装置在内的作业机械的校正系统的一例的图。

图4是示出在液压挖掘机放置于倾斜面的情况下，IMU具有安装误差的情况和不具有安装误差的情况的图。

图5是用于说明IMU不具有安装误差的情况下的铲尖的位置的图。

图6是用于说明IMU具有安装误差的情况下的铲尖的位置的图。

图7是示出实施方式1的作业机械的校正方法的处理例的流程图。

图8是示出为了修正IMU的计测误差而将液压挖掘机设置于倾斜面的状态的侧视图。

图9是示出设置于倾斜面的液压挖掘机的第一姿势的图。

图10是示出设置于倾斜面的液压挖掘机的第二姿势的图。

图11是示出第一姿势下的工作装置的位置与第二姿势下的工作装置的位置之差的侧视图。

图12是示出第一姿势下的工作装置的位置与第二姿势下的工作装置的位置之差的主视图。

图13是示出用于得到第一姿势以及第二姿势的变形例的图。

图14是示出在实施方式2中以第一姿势计测第一位置的例子的图。

图15是示出在实施方式2中以第二姿势计测第二位置的例子的图。

具体实施方式

参照附图对用于实施本发明的方式(实施方式)详细进行说明。

实施方式1.

<作业机械的整体结构>

图1是实施方式1的作业机械的立体图。图2是用于说明车身坐标系的图。在本实施方式中，作业机械为液压挖掘机100。液压挖掘机100具有车身1以及工作装置2。车身1具有回转体3、驾驶室4以及行驶体5。回转体3以回转中心轴Zr为中心能够回转地安装于行驶体5。回转体3收容有液压泵以及发动机等装置。

回转体3在安装有工作装置2的状态下回转。在回转体3的上部安装有扶手9。在扶手9安装有天线21、22。天线21、22是RTK-GNSS(Real Time Kinematic-Global NavigationSatellite Systems，GNSS称为全球导航卫星系统)用的天线。天线21、22沿着车身坐标系(Xm、Ym、Zm)的Ym轴隔开一定距离而配置。天线21、22接收GNSS电波，并输出与接收到的GNSS电波相应的信号。天线21、22也可以是GPS(Global Positioning System)用的天线。

驾驶室4载置于回转体3的前部。行驶体5具有履带5a、5b。通过履带5a、5b旋转而使液压挖掘机100行驶。

工作装置2安装于车身1的前部，具有动臂6、斗杆7、铲斗8、动臂缸10、斗杆缸11以及铲斗缸12。动臂6的基端部经由动臂销13以能够转动的方式安装于车身1的前部。即，动臂销13相当于动臂6的相对于回转体3转动的转动中心。斗杆7的基端部经由斗杆销14以能够转动的方式安装于动臂6的前端部。即，斗杆销14相当于斗杆7的相对于动臂6转动的转动中心。铲斗8经由铲斗销15以能够转动的方式安装于斗杆7的前端部。即，铲斗销15相当于铲斗8的相对于斗杆7转动的转动中心。

图1所示的动臂缸10、斗杆缸11以及铲斗缸12分别是由液压驱动的液压缸。动臂缸10的基端部经由动臂缸基部销10a以能够转动的方式安装于回转体3。动臂缸10的前端部经由动臂缸顶部销10b以能够转动的方式安装于动臂6。动臂缸10通过在液压的作用下进行伸缩而驱动动臂6。

斗杆缸11的基端部经由斗杆缸基部销11a以能够转动的方式安装于动臂6。斗杆缸11的前端部经由斗杆缸顶部销11b以能够转动的方式安装于斗杆7。斗杆缸11通过在液压的作用下进行伸缩而驱动斗杆7。

铲斗缸12的基端部经由铲斗缸基部销12a以能够转动的方式安装于斗杆7。铲斗缸12的前端部经由铲斗缸顶部销12b以能够转动的方式安装于第一连杆构件47的一端以及第二连杆构件48的一端。第一连杆构件47的另一端经由第一连杆销47a以能够转动的方式安装于斗杆7的前端部。第二连杆构件48的另一端经由第二连杆销48a以能够转动的方式安装于铲斗8。铲斗缸12通过在液压的作用下进行伸缩而驱动铲斗8。

铲斗8具有多个铲8B。多个铲8B沿着铲斗8的宽度方向排列为一列。铲8B的前端为铲尖8BT。铲斗8是作业工具的一例。作业工具不局限于铲斗8。作业工具例如也可以是具有单个铲的倾转铲斗，也可以是法面铲斗或者具备凿岩用的尖端的凿岩用配件，也可以是除此以外的工具。

在回转体3安装有位置检测装置23、作为姿势检测装置的一例的IMU(InertialMeasurement Unit：惯性计测装置)24、作业机械的校正装置30、以及控制液压挖掘机100的控制装置25。位置检测装置23被输入来自天线21、22的信号。位置检测装置23使用从天线21、22获取到的信号，对全球坐标系(Xg、Yg、Zg)中的天线21、22的当前位置以及回转体3的方位进行检测并输出。回转体3的方位表示回转体3在全球坐标系中的朝向。回转体3的朝向例如能够以回转体3的绕全球坐标系的Zg轴的前后方向的朝向表示。在本实施方式中，由方位角θd表示回转体3的方位。方位角θd是回转体3的前后方向上的基准轴的绕全球坐标系的Zg轴的旋转角。在本实施方式中，位置检测装置23根据两个天线21、22的相对位置来计算方位角θd。

接着，对坐标系进行说明。上述的车身坐标系(Xm、Ym、Zm)是以固定于车身1的原点为基准的坐标系，在本实施方式中是以固定于回转体3的原点为基准的坐标系。在实施方式中，车身坐标系(Xm、Ym、Zm)的原点例如是回转体3的摆动圆的中心。摆动圆的中心存在于回转体3的回转中心轴Zr上。车身坐标系(Xm、Ym、Zm)的Zm轴是成为回转体3的回转中心轴Zr的轴，Xm轴是沿着回转体3的前后方向延伸且与Zm轴正交的轴，Ym轴是与Zm轴以及Xm轴正交的、沿回转体3的宽度方向延伸的轴。Xm轴是回转体3的前后方向上的基准轴。前述的全球坐标系(Xg、Yg、Zg)是由GNSS计测的坐标系，是以固定于地球的原点为基准的坐标系。如图1所示，IMU24具有自身的坐标系(Xi、Yi、Zi)。

在本实施方式中，IMU24设置于驾驶室4的下方。IMU24对作用于液压挖掘机100的加速度进行检测。IMU24能够检测车身1的宽度方向的倾斜角、在本实施方式中为回转体3的宽度方向的倾斜角。在本实施方式中，车身1的宽度方向是与动臂销13的轴向平行的方向。车身1的宽度方向的倾斜角是图2所示的绕车身坐标系(Xm、Ym、Zm)的Xm轴的角度θr。以下，将角度θr适当称作侧倾角θr。

IMU24根据检测到的角速度，能够检测相对于重力所作用的方向的车身1的前后方向的倾斜角、在本实施方式中是回转体3的前后方向的倾斜角。车身1的前后方向是图2所示的车身坐标系(Xm、Ym、Zm)的Xm轴所延伸的方向。车身1的前后方向的倾斜角是图2所示的绕车身坐标系(Xm、Ym、Zm)的Ym轴的角度θp。以下，将角度θp适当称作俯仰角θp。

IMU24能够利用一个装置求出液压挖掘机100的加速度、角速度、侧倾角θr、俯仰角θp以及横摆角θy这样的在液压挖掘机100的控制中所需的信息。控制装置25使用工作装置2的位置、例如铲斗8的铲尖8BT的在全球坐标系中的位置来控制工作装置2。在求出全球坐标系中的工作装置2的位置时，使用侧倾角θr、俯仰角θp以及方位角θd。在本实施方式中，作业机械的校正装置30求出工作装置2的位置，但工作装置2的位置也可以由控制装置25求出，还可以由控制装置25以外的设备求出。

<作业机械的校正装置30以及作业机械的校正系统40>

图3是示出包括实施方式1的作业机械的校正装置30在内的作业机械的校正系统40的一例的图。作业机械的校正系统40包括作业机械的校正装置30、位置检测装置23、IMU24以及输入输出装置26。在本实施方式中，位置检测装置23并非是必须的。以下，将作业机械的校正装置30适当称作校正装置30，将作业机械的校正系统40适当称作校正系统40。

校正装置30具有处理部31、存储部32以及输入输出部33。处理部31具有修正部31A以及位置计算部31B。处理部31例如是CPU(Central Processing Unit)等的处理器以及存储器。处理部31执行实施方式的作业机械的校正方法。修正部31A主要通过执行本实施方式的作业机械的校正方法来修正IMU24的检测值所包含的误差，该误差是由于IMU24被设置为横摆角相对于液压挖掘机1的前后方向偏移而产生倾斜所引起的。位置计算部31B主要使用修正后的IMU24的检测值来求出工作装置2的位置。

存储部32例如使用RAM(Random Access Memory)、ROM(Random Access Memory)、闪速存储器、EPROM(Erasable Programmable Random Access Memory)、EEPROM(Electrically Erasable Programmable Random Access Memory)等的非易失性或易失性的半导体存储器、磁盘、软盘以及光磁盘中的至少一种。

存储部32存储用于使处理部31执行实施方式的作业机械的校正方法的计算机程序、以及处理部31在执行实施方式的作业机械的校正方法时使用的信息。处理部31从存储部32读入前述的计算机程序并执行，由此实现实施方式的作业机械的校正方法。输入输出部是用于将校正装置30与设备类连接的接口电路。在输入输出部33连接IMU24、位置检测装置23以及输入输出装置26。

输入输出装置26具有显示部26D和输入部26I。输入输出装置26的显示部26D例如显示校正装置30的计算结果以及向校正装置30输入的信息。显示部26D是液晶显示器或有机EL(Electro Luminescence)显示器等，但不局限于此。输入部26I是向校正装置30输入信息的按钮式的输入键，但不局限于这种结构。

由于无法在成为车身坐标系的基准位置的回转体3的回转中心配置IMU24，因此，IMU24的坐标系(Xi、Yi、Zi)与车身坐标系(Xm、Ym、Zm)不同。对于IMU24而言，若IMU24的坐标系(Xi、Yi、Zi)的Xi轴与车身坐标系(Xm、Ym、Zm)的Xm轴平行，则能够保证根据IMU24检测到的角速度以及加速度求出的侧倾角θr以及俯仰角θp的精度。在本实施方式中，Xi轴是成为IMU24的基准的轴。当IMU24的Xi轴相对于车身坐标系的Xm轴具有横摆角的偏移，即具有角度偏移时，安装于液压挖掘机100的一部分即回转体3的IMU24相对于回转体3具有角度偏移。以下，将该角度偏移适当称作安装误差。表示IMU24相对于液压挖掘机100的偏移。在IMU24具有安装误差的情况下，IMU24检测且校正装置30识别的液压挖掘机100的俯仰角θp以及侧倾角θr包含误差。即，IMU24的检测值包含因IMU24的安装误差而引起的误差。以下，将该误差适当称作计测误差。

图4是示出在将液压挖掘机100放置于倾斜面PD的情况下，IMU24具有安装误差的情况和不具有安装误差的情况的图。图5是用于说明IMU24不具有安装误差的情况下的铲尖8BT的位置的图。图6是用于说明IMU24具有安装误差的情况下的铲尖8BT的位置的图。

图4示出将图1所示的液压挖掘机100放置于相对于水平面的倾斜角为

的倾斜面PD上时的IMU24的状态。图4中的A示出IMU24的坐标系的Xi轴与车身坐标系的Xm轴平行的情况。即，示出IMU24不具有安装误差的情况。图4中的B示出IMU24的坐标系的Xi轴与车身坐标系的Xm轴不平行的情况。该例示出IMU24具有安装误差的情况。具体而言，IMU24绕图1所示的IMU24的坐标系的Xi轴旋转地安装于车身1，在本实施方式中为回转体3，结果为IMU24的坐标系的Xi轴相对于车身坐标系的Xm轴偏移了角度Δθy的状态。即，为IMU24被安装为相对于液压挖掘机100的前后方向产生了横摆方向的偏移的状态。

在IMU24不具有安装误差的情况下，俯仰角θp为0度，侧倾角θr为

在IMU24具有安装误差的情况下，俯仰角θp成为与0度不同的值，侧倾角θr成为与

不同的值。

在图5以及图6中，工作装置2所具有的铲斗8的铲尖8BT的实际的高度为Hr，图3所示的校正装置30所识别的铲斗8的铲尖8BT的高度为Hb。在不具有IMU24的安装误差的液压挖掘机100放置于倾斜面PD的情况下，当图1所示的回转体3回转时，如图5所示，校正装置30所识别的铲尖8BT的高度Hb与实际的高度Hr没有变化。实际的高度Hr是从基准面PH到铲尖8BT的位置为止的高度。在具有IMU24的安装误差的液压挖掘机100放置于倾斜面PD的情况下，当图1所示的回转体3回转时，如图6所示，校正装置30所识别的回转体3的铲尖8BT的高度Hb与实际的高度Hr不同。

作为一例，考虑IMU24的坐标系的Xi轴相对于车身坐标系的Xm轴偏移的角度、即IMU24的安装误差为±1度左右的液压挖掘机100。在该液压挖掘机100放置于倾斜面PD的情况下，使回转体3转动时，校正装置30所识别的铲尖8BT的高度Hb包含误差。当倾斜面PD的倾斜度变大时，工作装置2成为最大范围的状态之际，校正装置30所识别的铲尖8BT的高度Hb有时包含无法保证用于进行使工作装置2沿着设计面动作的控制的精度这一程度的误差。

俯仰角θp对铲斗8的铲尖8BT的位置具有影响，侧倾角θr影响到铲斗8的铲尖的平行度，因此，针对IMU24的俯仰角θp以及侧倾角θr而进行设置时的倾斜度修正。当液压挖掘机1为大型时，IMU24的安装误差对倾斜面上的俯仰角θp的误差造成的影响较大，结果可知，对倾斜面上的铲尖8BT的位置精度造成影响。因此，在本实施方式中，对IMU24的计测误差进行修正。

校正装置30所识别的铲尖8BT的高度Hb所包含的误差在放置液压挖掘机100的斜面的从下方朝向上方的方向与车身坐标系的Xm轴为正交的状态时成为最大。校正装置30以及本实施方式的校正方法在IMU24具有安装误差的情况下，通过修正IMU24的检测值来修正计测误差。接着，对校正装置30执行本实施方式的校正方法来修正IMU24的计测误差的处理进行说明。

<安装误差的修正>

图7是示出实施方式1的作业机械的校正方法的处理例的流程图。图8是示出为了修正IMU24的计测误差而将液压挖掘机100设置于倾斜面PD的状态的侧视图。图9是示出设置于倾斜面PD的液压挖掘机100的第一姿势FF的图。图10是示出设置于倾斜面PD的液压挖掘机100的第二姿势FS的图。图11是示出第一姿势FF下的工作装置2的位置与第二姿势FS下的工作装置2的位置之差的侧视图。图12是示出第一姿势FF下的工作装置2的位置与第二姿势FS下的工作装置2的位置之差的主视图。

在本实施方式中，在修正IMU24的计测误差的情况下，如图8所示，具有修正对象的IMU24的液压挖掘机100设置于倾斜角为

的倾斜面PD。在该状态下，校正装置30的修正部31A获取液压挖掘机100为图9以及图11所示的第一姿势FF时的液压挖掘机100的一部分Pm的位置即第一位置Pf(步骤S101)。接着，校正装置30的修正部31A获取液压挖掘机100为图10以及图11所示的第二姿势FS时的液压挖掘机100的一部分Pm的位置即第二位置Ps(步骤S102)。

第一位置Pf为液压挖掘机100设置于倾斜面PD且回转体3朝向第一方向时的位置，第二位置Ps为液压挖掘机100设置于倾斜面PD且回转体3朝向第二方向时的位置。即，第一位置Pf与第二位置Ps是回转体3的朝向不同时的不同的两个位置。

在本实施方式中，液压挖掘机100的一部分Pm是回转体3以及安装于回转体3的工作装置2的一部分，是回转体3的回转中心上以外的位置即可。在该例中，一部分Pm是工作装置2的一部分，更具体而言是图1所示的斗杆缸顶部销11b的部分，但不局限于该部分。

第一姿势FF以及第二姿势FS是IMU24输出的俯仰角θp为0度时的姿势。即，第一位置Pf以及第二位置Ps是IMU24输出的俯仰角θp为0度时的位置。将倾斜面PD的从下方朝向上方的方向设为倾斜方向DD。倾斜方向DD与水平面所成的角度为倾斜角

与倾斜方向DD正交的方向与水平面平行。IMU24输出的俯仰角θp为0度的情况是指，IMU24的坐标系中的Yi轴与倾斜方向DD平行的情况。

第二姿势FS是工作装置2的姿势与第一姿势FF不同的姿势。在本实施方式中，第二姿势FS是回转体3从IMU24输出的俯仰角θp为0度的第一姿势FF的状态回转，而IMU24输出的俯仰角θp再次为0度时的姿势。在该情况下，回转体3进行180度回转。

在本实施方式中，第一位置Pf以及第二位置Ps是回转体3的朝向相差180度时的不同的两个位置，但不局限于这种位置关系。例如，第一位置Pf以及第二位置Ps也可以是回转体3的朝向相差180度以外的大小时的不同的两个位置。在该情况下，需要根据不同的两个朝向之间的大小来修正第一位置Pf以及第二位置Ps。通过将回转体3的朝向相差180度时不同的两个位置设为第一位置Pf以及第二位置Ps，不需要第一位置Pf以及第二位置Ps的修正，故是优选的。

第一位置Pf以及第二位置Ps由图9以及图10所示的外部计测装置TS进行计测。在本实施方式中，外部计测装置TS例如是被称为全站仪的计测装置，但不局限于此。在本实施方式中，第一位置Pf以及第二位置Ps是全球坐标系(Xg、Yg、Zg)中的位置，但不局限于此。也可以从图3所示的输入输出装置26向校正装置30输入第一位置Pf以及第二位置Ps。另外，也可以通过外部计测装置TS与校正装置30的输入输出部33连接，从而校正装置30从外部计测装置TS直接获取第一位置Pf以及第二位置Ps。

在IMU24具有安装误差的情况下，第一位置Pf与第二位置Ps不同。例如，如图11以及图12所示，第一位置Pf的距基准面PH的高度Hf与第二位置Ps的距基准面PH的高度Hs不同。其结果是，产生高度Hf与高度Hs之差Δh。因IMU24的安装误差而引起的一部分Pm的高度的误差D成为Δh/2。

在本实施方式中，校正装置30的修正部31A使用第一位置Pf和第二位置Ps来修正IMU24的计测误差(步骤S103)。例如，校正装置30使用从第一位置Pf与第二位置Ps之差Δh得到的误差D来修正IMU24的计测误差。第二姿势FS时的实际的俯仰角θpt2、误差D、以及从车身坐标系的原点到工作装置2的一部分Pm为止的距离L之间的关系使用倾斜角

以及角度Δθy并通过式(1)来求出。

距离L是从车身坐标系的原点到一部分Pm为止的距离，是车身坐标系的Xm方向上的距离。距离L根据工作装置2的姿势以及尺寸而求出。倾斜角

是在第一位置Pf以及第二位置Ps的计测时设置液压挖掘机100的倾斜面PD的倾斜角。倾斜角

是在液压挖掘机100从第一姿势FF向第二姿势FS变化之际回转体3回转了时、IMU24检测到并输出的侧倾角θr的峰值。Δθy是横摆角误差。横摆角误差Δθy是IMU24的坐标系的Xi轴相对于车身坐标系的Xm轴偏移了时的Xi轴与Xm轴所成的角度。横摆角误差Δθy是IMU24绕Zi轴旋转地安装于液压挖掘机100、在本实施方式中为回转体3而产生的误差。

若对式(1)进行变形而求解横摆角误差Δθy，则得到式(2)。

校正装置30的修正部31A对式(2)赋予误差D、距离L以及从IMU24的检测值得到的倾斜角

来求出横摆角误差Δθy。校正装置30的修正部31A使得到的横摆角误差Δθy存储于图3所示的存储部32。校正装置30的处理部31、更具体而言是图3所示的位置计算部31B从存储部32读出横摆角误差Δθy，使用该横摆角误差Δθy来修正IMU24检测到并输出的加速度以及角度。

式(3)表示IMU24检测到并输出的加速度的修正值Gxn、Gyn、Gzn。位置计算部31B在对从IMU24获取到的加速度进行修正的情况下，以横摆角误差Δθy来修正式(3)。

[数学式1]

位置计算部31B使用横摆角误差Δθy，对从IMU24获取到的角度进行修正、在本实施方式中对俯仰角θp以及侧倾角θr进行修正。式(4)表示修正后的侧倾角θrn。式(5)表示修正后的俯仰角θpn。位置计算部31B将从存储部32读出的横摆角误差Δθy、从IMU24输出的侧倾角θr以及俯仰角θp赋予式(4)以及式(5)，从而求出修正后的侧倾角θrn以及修正后的俯仰角θpn。使用修正后的侧倾角θrn以及修正后的俯仰角θpn及方位角θd来求出工作装置2的位置。

[数学式2]

[数学式3]

作为工作装置2的位置，以求出铲斗8的铲尖8BT的位置(以下称作铲尖位置)的例子进行说明。当将铲尖位置设为PB时，车身坐标系(Xm、Ym、Zm)中的铲尖位置PB根据工作装置2的尺寸以及姿势而求出。所得到的铲尖位置PB例如通过式(1)，从车身坐标系(Xm、Ym、Zm)转换为全球坐标系(Xg、Yg、Zg)的值。

PBg＝R·PBm+T···(6)

式(6)中的PBg是全球坐标系(Xg、Yg、Zg)中的铲尖位置PB，PBm是车身坐标系中的铲尖位置PB，R是式(7)所示的旋转矩阵，T是式(8)所示的平移矢量。

[数学式4]

[数学式5]

根据式(7)可知，旋转矩阵R中包含侧倾角θr、俯仰角θp以及方位角θd。侧倾角θr以及俯仰角θp是IMU24检测到并输出的值。方位角θd是位置检测装置23根据天线21、22的相对位置计算并输出的值。平移矢量T根据由位置检测装置23检测到的全球坐标系(Xg、Yg、Zg)中的天线21、22的位置与车身坐标系(Xm、Ym、Zm)之间的位置关系而求出。

<变形例1>

当使用误差D以及从车身坐标系的原点到工作装置2的一部分Pm为止的距离L时，利用式(9)求出第二姿势FS时的实际的俯仰角θpt2。

θpt2＝sin^-1(D/L)···(9)

校正装置30的修正部31A使用式(9)来求出实际的俯仰角θpt2。实际的俯仰角θpt2、横摆角误差Δθy、IMU24检测到并输出的侧倾角θr以及俯仰角θp之间的关系能够根据式(4)以及式(5)求出。

<变形例2>

图13是示出用于得到第一姿势FF以及第二姿势FS的变形例的图。在前述的例子中，如图8所示，将液压挖掘机100设置在倾斜面PD上。在变形例中，如图13所示，通过使液压挖掘机100的行驶体5的一部分搭到平台TB上，能够作出与将液压挖掘机100设置于倾斜面PD的情况同样的姿势。通过使用平台TB，即便在不存在倾斜面PD的情况下，通过准备平台TB，也能够使校正装置30对因IMU24的安装误差引起的计测误差进行修正。

本实施方式及其变形例能够修正IMU24的检测值所包含的误差，该误差是通过将IMU24设置为横摆角相对于液压挖掘机100的前后方向偏移而产生倾斜所引起的。IMU24的横摆方向的安装误差在安装有IMU24的液压挖掘机100的车身1为水平的状态下，对求出工作装置2的位置时的精度几乎不造成影响，但在液压挖掘机100放置于倾斜地时，求出工作装置2的位置时的精度降低。尤其是，在液压挖掘机100的车身1侧倾的姿势下，求出工作装置2的位置时的精度降低。

本实施方式及其变形例使用两个在包含至少一个使液压挖掘机100的车身1倾斜的姿势在内的两个姿势下计测到的、液压挖掘机100的一部分的局部位置，来修正因IMU24的安装误差引起的计测误差。这样，由于使用至少一个、在本实施方式中使用两个容易出现IMU24的横摆方向上的安装误差的影响的液压挖掘机100倾斜的姿势，因此，容易获得用于对因IMU24的横摆方向上的安装误差引起的计测误差进行修正的修正量。

在本实施方式及其变形例中，在液压挖掘机100的车身1侧倾的姿势、即IMU24输出的俯仰角θp为0度的第一姿势FF以及第二姿势FS下，分别计测第一位置Pf以及第二位置Ps。根据这样计测到的第一位置Pf以及第二位置Ps，求出用于对IMU24的横摆方向上的安装误差进行修正的修正量、即横摆角误差Δθy。这样，第一位置Pf与第二位置Ps是在工作装置2的位置的精度降低得较大的姿势下得到的，因此，两者的差分变大。其结果是，能够降低第一位置Pf以及第二位置Ps的计测误差的影响，因此，前述的修正量的精度降低得以抑制。

本实施方式及其变形例使用外部计测装置TS来计测液压挖掘机100的一部分Pm，因此，能够高精度地修正IMU24检测到并输出的俯仰角θp以及侧倾角θr。另外，本实施方式及其变形例由于利用外部计测装置TS而无需使用GPS等测位卫星的计测，因此，不会受到使用测位卫星的计测中的测位误差的影响。其结果是，本实施方式及其变形例能够高精度地修正IMU24检测到并输出的俯仰角θp以及侧倾角θr。

本实施方式及其变形例的结构在以下也能够适当应用。

实施方式2.

图14是示出在实施方式2中在第一姿势FF下对第一位置Pf进行计测的例子的图。图15是示出在实施方式2中在第二姿势FS下对第二位置Ps进行计测的例子的图。在本实施方式中，液压挖掘机100、位置检测装置23、IMU24、控制装置25、校正装置30以及校正系统40与实施方式1同样，因此省略说明。接着，使用图7所示的流程图，来说明实施方式2的作业机械的校正方法的处理例。

在本实施方式中，图14所示的第一位置Pf以及第二位置Ps是液压挖掘机100的一部分、在该例中是以液压挖掘机100以外的位置(以下适当称作计测位置)为基准而得到的工作装置2的一部分Pm的位置。在本实施方式中，计测位置是基准面PH的一部分PHbs。计测位置只要是在计测第一位置Pf时以及计测第二位置Ps时不动或相同的位置即可，不局限于基准面PH的一部分PHbs。以下，将基准面PH的一部分PHbs适当称作计测位置PHbs。

第一位置Pf以及第二位置Ps使用从IMU24输出的与液压挖掘机100的姿势相关的信息而求出。作为与液压挖掘机100的姿势相关的信息，例示出侧倾角θr、俯仰角θp以及方位角θd。

在本实施方式中，工作装置2的一部分Pm是铲斗8的铲尖8BT。第一位置Pf是在液压挖掘机100为第一姿势FF时，铲尖8BT与计测位置PHbs接触时的铲尖8BT的位置。第二位置Ps是在液压挖掘机100为第二姿势FS时，铲尖8BT与计测位置PHbs接触时的铲尖8BT的位置。这样，第一位置Pf以及第二位置Ps是在铲斗8的同一铲尖8BT与作为基准的点的同一部分接触的状态下得到的。图3所示的校正装置30的位置计算部31B使用从IMU24输出的与液压挖掘机100的姿势相关的信息即侧倾角θr以及俯仰角θp，求出铲尖8BT的位置。在本实施方式中，在求出铲尖8BT的位置时，除了从IMU24输出的侧倾角θr以及俯仰角θp之外，还使用工作装置2的位置方位角θd、工作装置2的姿势以及尺寸。

第一姿势FF是液压挖掘机100设置在基准面PH上的状态下的液压挖掘机100的姿势。第二姿势FS是液压挖掘机100设置在相对于基准面PH倾斜的倾斜面PD上的状态下的液压挖掘机100的姿势。校正装置30的修正部31A获取液压挖掘机100为第一姿势FF时的第一位置Pf(图7中的步骤S101)。接着，校正装置30的修正部31A获取液压挖掘机100为第二姿势FS时的第二位置Ps(图7中的步骤S102)。

在从IMU24输出的横摆角θy包含横摆角误差Δθy的情况下，第一位置Pf与第二位置Ps不一致。这是因为，当存在横摆角误差Δθy时，IMU24输出的俯仰角θp以及侧倾角θr也包含误差。为了修正俯仰角θp以及侧倾角θr，图3所示的校正装置30的修正部31A修正横摆角误差Δθy，并使用前述的式(4)以及式(5)来求出修正后的俯仰角θpn以及修正后的侧倾角θrn。位置计算部31B使用修正后的俯仰角θpn以及修正后的侧倾角θrn，重新计算第一位置Pf以及第二位置Ps。

修正部31A求出由位置计算部31B求出的第一位置Pf与第二位置Ps之差(以下适当称作位置差分)，并与阈值进行比较。修正部31A判定位置差分是否为阈值以下。在位置差分大于阈值的情况下，修正部31A以及位置计算部31B反复进行横摆角误差Δθy的修正以及第一位置Pf及第二位置Ps的重新计算，直至位置差分成为阈值以下。修正部31A将第一位置Pf与第二位置Ps之差成为阈值以下时的横摆角误差Δθy作为IMU24的横摆方向上的安装误差而存储于图3所示的存储部32。位置计算部31B从存储部32读出横摆角误差Δθy，使用式(4)以及式(5)来修正IMU24检测到并输出的加速度、俯仰角θp以及侧倾角θr。这样，校正装置30使用利用第一位置Pf以及第二位置Ps而得到的横摆角误差θy，来修正IMU24的计测误差(图7中的步骤S103)。

修正部31A一边使用用于修正与液压挖掘机100的姿势相关的信息的参数、在本实施方式中为横摆角误差Δ0y进行修正，一边反复进行第一位置Pf以及第二位置Ps的重新计算。然后，校正装置30的修正部31A使用第一位置Pf与第二位置Ps之差(以下适当称作位置差分)成为阈值以下时的横摆角误差Δθy，来修正因IMU24的安装误差引起的计测误差。

这样，校正装置30能够修正IMU24的检测值所包含的误差，该误差是因IMU24相对于液压挖掘机100的前后方向倾斜而产生了横摆方向的偏移所引起的。在本实施方式中，在修正部31A修正横摆角误差Δθy的情况下，修正部31A例如确定横摆角误差Δθy的初始值，使横摆角误差Δθy从初始值以规定的大小向横摆角误差Δθy从初始值增加的方向与从初始值减少的方向这两方变化。例如，能够设横摆角误差Δθy的初始值为0度，规定的大小为0.01度，但不局限于这些值。

与位置差分进行比较的阈值没有被限定，但例如使用距离的绝对值。在该情况下，阈值能够为例如GNSS的计测误差程度。另外，也可以使用通过重新计算得到的第一位置Pf与第二位置Ps之差成为修正前的第一位置Pf与第二位置Ps之差的规定的比例以下时的横摆角θy，来修正计测误差。在该情况下，阈值成为修正前的第一位置Pf与第二位置Ps之差的规定的比例。规定的比例例如能够为1％或5％，但不局限于这些值。

位置计算部31B在对从IMU24获取到的加速度进行修正的情况下，利用横摆角误差Δθy进行修正。位置计算部31B将位置差分成为阈值以下时的横摆角误差Δθy用作修正值，来对由IMU24检测到并输出的侧倾角θr以及俯仰角θp进行修正。

本实施方式使用在包含至少一个使液压挖掘机100的车身1倾斜的姿势在内的两个姿势下计测到的、液压挖掘机100的一部分的局部位置，来修正因IMU24的安装误差引起的计测误差。此时，液压挖掘机100的一部分的局部位置是以液压挖掘机100以外的计测位置PHbs为基准而计测的。这样，本实施方式至少使用一个容易产生IMU24的横摆方向上的安装误差的影响的液压挖掘机100倾斜的姿势，因此，容易获得用于对IMU24的横摆方向上的安装误差进行修正的修正量。本实施方式由于不需要外部计测装置TS，因此，即便在不存在外部计测装置TS的情况下，例如液压挖掘机100的作业现场，也能够修正因IMU24的安装误差而引起的计测误差。

在本实施方式中，使铲斗8的铲尖8BT与基准面PH的计测位置PHbs接触，但若知晓铲尖8BT与计测位置PHbs的位置关系，则也可以不使计测位置PHbs与铲尖8BT接触。例如，校正装置30也可以使用使铲尖8BT静止在计测位置PHbs的铅垂方向上的上方的规定位置时的IMU24的输出，来求出第一位置Pf以及第二位置Ps。这样，第一位置Pf以及第二位置Ps是以液压挖掘机100以外的位置为基准而得到的液压挖掘机100的一部分的位置即可。另外，液压挖掘机100的一部分的位置不局限于铲斗8的铲尖8BT，例如也可以是铲斗8的尾部或者图1所示的第二连杆构件48的一部分。

以上，对实施方式1、其变形例以及实施方式2进行了说明，但并不通过上述的内容来限定本发明。另外，上述的构成要素包含本领域技术人员能够容易想到的构成要素、实质上相同的构成要素、所谓的同等范围内的构成要素。此外，能够对上述的构成要素适当进行组合。此外，能够在不脱离实施方式1、其变形例以及实施方式2的宗旨的范围内进行构成要素的各种省略、置换或者变更。

附图标记说明

1 车身

2 工作装置

3 回转体

4 驾驶室

5 行驶体

6 动臂

7 斗杆

8 铲斗

8B 铲

8BT 铲尖

10 动臂缸

11 斗杆缸

12 铲斗缸

13 动臂销

14 斗杆销

15 铲斗销

23 位置检测装置

25 控制装置

26 输入输出装置

30 校正装置

31 处理部

31A 修正部

31B 位置计算部

32 存储部

33 输入输出部

40 校正系统

100 液压挖掘机

D 误差

FF 第一姿势

FS 第二姿势

L 距离

PD 倾斜面

Pf 第一位置

PH 基准面

PHbs 计测位置

Pm 一部分

Ps 第二位置

TB 平台

TS 外部计测装置

θr 侧倾角

θp 俯仰角

θy 横摆角

倾斜角

Δθy 横摆角误差。

Claims (10)

1.一种作业机械的校正装置，其中，

所述作业机械具有供工作装置安装并进行回转的回转体，

所述作业机械的校正装置在修正因输出所述作业机械的姿势且安装于所述作业机械的姿势检测装置被设置为横摆角相对于所述作业机械的前后方向偏移而产生倾斜所引起的误差时，

将所述作业机械为规定的倾斜状态的情况设为第一姿势，将所述作业机械为与所述第一姿势不同的倾斜状态的情况设为第二姿势，使用所述作业机械为所述第一姿势时的所述作业机械的一部分的位置即第一位置与所述作业机械为所述第二姿势时的所述一部分的位置即第二位置来修正所述误差。

2.根据权利要求1所述的作业机械的校正装置，其中，

所述一部分的位置是所述工作装置的一部分的位置，

所述第一位置是所述作业机械设置于倾斜面且所述回转体朝向第一方向时的位置，

所述第二位置是所述作业机械设置于倾斜面且所述回转体朝向第二方向时的位置。

3.根据权利要求2所述的作业机械的校正装置，其中，

所述第一位置以及所述第二位置是所述姿势检测装置输出的俯仰角为0度时的位置。

4.根据权利要求1至3中任一项所述的作业机械的校正装置，其中，

所述一部分的位置是所述作业机械具有的所述工作装置的一部分的位置。

5.一种作业机械的校正装置，其中，

所述作业机械具有供工作装置安装并进行回转的回转体，

所述作业机械的校正装置在修正因输出所述作业机械的姿势且安装于所述作业机械的姿势检测装置被设置为横摆角相对于所述作业机械的前后方向偏移而产生倾斜所引起的误差时，

使用所述作业机械为第一姿势时的所述作业机械的一部分的位置即第一位置与所述作业机械为第二姿势时的所述一部分的位置即第二位置来修正所述误差，

所述第一位置以及所述第二位置是以所述作业机械以外的位置为基准而得到的所述一部分的位置，通过使用从所述姿势检测装置输出的与所述作业机械的姿势相关的信息而求出。

6.根据权利要求5所述的作业机械的校正装置，其中，

所述作业机械的校正装置一边修正用于对与所述作业机械的姿势相关的信息进行修正的参数，一边反复进行所述第一位置以及所述第二位置的重新计算，使用所述第一位置与所述第二位置之差成为阈值以下时的所述参数来修正所述误差。

7.根据权利要求6所述的作业机械的校正装置，其中，

与所述作业机械的姿势相关的信息为所述姿势检测装置输出的俯仰角以及侧倾角。

8.一种作业机械，其中，

所述作业机械具有权利要求1至7中任一项所述的作业机械的校正装置。

9.一种作业机械的校正方法，其中，

所述作业机械具有供工作装置安装并进行回转的回转体，

在所述作业机械的校正方法中，在修正因输出所述作业机械的姿势且安装于所述作业机械的姿势检测装置被设置为横摆角相对于所述作业机械的前后方向偏移而产生倾斜所引起的误差时，

将所述作业机械为规定的倾斜状态的情况设为第一姿势，获取所述作业机械为所述第一姿势时的所述作业机械的一部分的位置即第一位置，

将所述作业机械为与所述第一姿势不同的倾斜状态的情况设为第二姿势，获取所述作业机械为所述第二姿势时的所述一部分的位置即第二位置，

使用所述第一位置以及所述第二位置来修正所述误差。

10.一种作业机械的校正方法，其中，

所述作业机械具有供工作装置安装并进行回转的回转体，

在所述作业机械的校正方法中，在修正因输出所述作业机械的姿势且安装于所述作业机械的姿势检测装置被设置为横摆角相对于所述作业机械的前后方向偏移而产生倾斜所引起的误差时，

获取所述作业机械为第一姿势时的所述作业机械的一部分的位置即第一位置，

获取所述作业机械为第二姿势时的所述一部分的位置即第二位置，

使用所述第一位置以及所述第二位置来修正所述误差，

所述第一位置以及所述第二位置是以所述作业机械以外的位置为基准而得到的所述一部分的位置，通过使用从所述姿势检测装置输出的与所述作业机械的姿势相关的信息而求出。

Images