CN105517645A - 作业机械的姿势运算装置、液压挖掘机的姿势运算装置及作业机械 - Google Patents

Abstract

作业机械的姿势运算装置具备：计测装置，其设于作业机械，用于检测角速度及加速度；姿势角运算部，其根据由所述计测装置检测出的角速度及加速度来求解所述作业机械的姿势角；第一互补滤波器，其被设定第一截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第一姿势角；第二互补滤波器，其被设定与所述第一截止频率不同的第二截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第二姿势角；切换部，其根据所述计测装置的状态来切换并输出所述第一姿势角或所述第二姿势角。

Description

作业机械的姿势运算装置、液压挖掘机的姿势运算装置及作业机械

技术领域

本发明涉及作业机械的姿势运算装置、液压挖掘机的姿势运算装置及作业机械。

背景技术

近年来，在液压挖掘机或推土机等作业机械中，已知有检测作业机械的位置来进行作业管理或者根据作业机械的位置信息和表示施工现场的地形的施工信息来进行施工管理的技术(例如专利文献1)。

在先技术文献

专利文献

专利文献1：日本特开2005-61024号公报

发明内容

发明要解决的课题

在求解作业机械所具备的作业机的位置及姿势的情况下，需要准确地求解与作业机械的倾斜度相关的信息。例如，将IMU(InertialMeasurementUnit：惯性计测装置)搭载于作业机械，根据IMU的检测值能获得侧倾角及俯仰角这样的姿势角来作为与作业机械的倾斜度相关的信息。在根据IMU的检测值来求解作业机械的姿势角的情况下，姿势角的精度会对例如要求解的作业机等的位置及姿势的精度造成影响。

本发明的目的在于在求解作业机械的姿势角时抑制姿势角的精度降低。

用于解决课题的方案

本发明涉及的作业机械的姿势运算装置具备：计测装置，其设于作业机械，用于检测角速度及加速度；姿势角运算部，其根据由所述计测装置检测出的角速度及加速度来求解所述作业机械的姿势角；第一互补滤波器，其被设定第一截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第一姿势角；第二互补滤波器，其被设定与所述第一截止频率不同的第二截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第二姿势角；切换部，其根据所述作业机械的状态，来切换并输出所述第一姿势角或所述第二姿势角。

优选为，所述第一截止频率高于所述第二截止频率。

优选为，所述切换部在判定为所述作业机械处于准静止状态的情况下输出所述第一姿势角，在判定为所述作业机械处于动态状态的情况下输出所述第二姿势角。

优选为，所述切换部在将所述第一姿势角切换为所述第二姿势角之后，使用从切换时的所述第一姿势角减去所述第二姿势角得到的值作为修正值，来对所述第二姿势角进行修正。

优选为，所述切换部在将所述第二姿势角切换为所述第一姿势角之后，使用将切换时的所述第二姿势角的误差乘以大于0且小于1的系数得到的值作为修正值，来对所述第一姿势角进行修正。

本发明涉及的液压挖掘机的姿势运算装置具备：计测装置，其设于液压挖掘机的上部回转体，用于检测角速度及加速度；姿势角运算部，其根据所述计测装置检测出的所述角速度及所述加速度来求解所述液压挖掘机的姿势角；第一互补滤波器，其被设定第一截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第一姿势角；第二互补滤波器，其被设定比所述第一截止频率低的第二截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第二姿势角；切换部，其在判定为所述液压挖掘机处于准静止状态的情况下输出所述第一姿势角，在判定为所述液压挖掘机处于动态状态的情况下输出所述第二姿势角。

本发明涉及的作业机械具备：前述的作业机械的姿势运算装置；使用从所述作业机械的姿势运算装置输出的所述第一姿势角或所述第二姿势角来求解安装于所述作业机械的作业机的位置的控制部。

本发明能够在求解作业机械的姿势角时抑制姿势角的精度降低。

附图说明

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

图2是表示本实施方式涉及的作业机械的控制系统的图。

图3是表示IMU的一例的框图。

图4是表示传感器控制装置的框图。

图5是表示互补滤波器的特性的图。

图6是表示误差的频率特性的图。

图7是表示第一互补滤波器的增益及第二互补滤波器的增益与频率的关系的图。

图8是表示本实施方式的变形例中的第一姿势角和第二姿势角的切换所使用的表的一例的图。

图9是表示传感器控制装置的切换部所输出的液压挖掘机的姿势角、第一姿势角和第二姿势角的时间变化的一例的图。

图10是表示求解姿势角的处理的一例的流程图。

图11是表示根据IMU的状态来切换姿势角的结果的一例的图。

具体实施方式

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

<作业机械的整体结构>

图1是本实施方式涉及的作业机械的立体图。图2是表示本实施方式涉及的作业机械的控制系统的图。作为作业机械的液压挖掘机100具有车辆主体1和作业机2。车辆主体1具有作为回转体的上部回转体3和作为行驶体的行驶装置5。上部回转体3在发动机室3EG的内部收容图2所示的作为动力产生装置的发动机36及液压泵37等装置。发动机室3EG配置在上部回转体3的后端侧。说明本实施方式时所定义的方向见后述。

在本实施方式中，液压挖掘机100中的作为动力产生装置的发动机36使用例如柴油发动机等内燃机，但动力产生装置并不局限于此。液压挖掘机100的动力产生装置可以是例如将内燃机、发电电动机和蓄电装置组合而成的所谓混合动力方式的装置。

上部回转体3具有驾驶室4。驾驶室4设置在上部回转体3的前端侧。即，驾驶室4设置在与发动机室3EG所配置的一侧相反的一侧。在驾驶室4内配置有图2所示的第一显示装置28及操作装置30。对这些构件在后叙述。在上部回转体3的上方安装有扶手19及天线20、21。针对天线20、21的详细情况在后叙述。

行驶装置5具有履带5a、5b。行驶装置5通过设置在左右的液压马达5c中的一方或两方进行驱动，履带5a、5b旋转，由此使液压挖掘机100向前后行驶或回转。作业机2安装在上部回转体3的驾驶室4的侧方侧。

液压挖掘机100也可以具备如下的行驶装置，即，代替履带5a、5b而具有轮胎，并将图2所示的发动机36的驱动力经由传动机构向轮胎传递而能够行驶的行驶装置。作为这样的形态的液压挖掘机100，已知有例如车轮式液压挖掘机。另外，液压挖掘机100也可以是具有如下结构的例如反铲式装载机：具备上述那样的具有轮胎的行驶装置，并且在车辆主体安装作业机，而不具备图1所示那样的上部回转体3及其回转机构。即，反铲式装载机是在车辆主体安装作业机且具备构成车辆主体的一部分的行驶装置的装置。

就上部回转体3而言，配置作业机2及驾驶室4的一侧为前方，配置发动机室3EG的一侧为后方。朝向前方时，左侧为上部回转体3的左方，朝向前方时，右侧为上部回转体3的右方。另外，就液压挖掘机100或车辆主体1而言，以上部回转体3为基准，行驶装置5侧为下方，以行驶装置5为基准，上部回转体3侧为上方。在液压挖掘机100设于水平面的情况下，下方为铅垂方向、即重力的作用方向侧，上方为与铅垂方向相反的一侧。

作业机2具有动臂6、斗杆7、铲斗8、动臂油缸10、斗杆油缸11和铲斗油缸12。动臂6的基端部经由动臂销13能够摆动地安装于上部回转体3的前部。斗杆7的基端部经由斗杆销14能够摆动地安装于动臂6的前端部。在斗杆7的前端部经由铲斗销15安装有铲斗8。铲斗8以铲斗销15为中心进行摆动。铲斗8在与铲斗销15相反的一侧安装有多个刃8B。刃尖8T为刃8B的前端。

铲斗8可以不具有多个刃8B。即，可以是不具有图1所示那样的刃8B、而将刃尖通过钢板形成为直线形状这样的铲斗。作业机2可以具备例如具有一个刃的倾转铲斗(tiltbucket)。倾转铲斗是如下述的铲斗：具备铲斗转斗油缸，通过铲斗向左右倾转倾斜，从而液压挖掘机即使处于倾斜地面也能够将斜面、平地成形、平整为任意的形状，且还能利用底板进行滚压作业。此外，作业机2也可以代替铲斗8而具备法面铲斗或者具有凿岩用的钎头的凿岩用的配件等。

图1所示的动臂油缸10、斗杆油缸11和铲斗油缸12是分别由工作油的压力(以下，适当称作液压)来驱动的液压缸。动臂油缸10驱动动臂6而使其进行升降。斗杆油缸11驱动斗杆7来使其绕着斗杆销14进行转动。铲斗油缸12驱动铲斗8而使其绕着铲斗销15进行转动。

在动臂油缸10、斗杆油缸11及铲斗油缸12等液压缸与图2所示的液压泵37之间设有图2所示的液压控制阀38。液压控制阀38包括用于驱动液压马达5c的行驶用控制阀、用于控制动臂油缸10、斗杆油缸11及铲斗油缸12以及使上部回转体3回转的回转马达的作业机用控制阀。图2所示的作业机控制装置25通过控制液压控制阀38而能控制向动臂油缸10、斗杆油缸11、铲斗油缸12、回转马达或液压马达5c供给的工作油的流量。其结果是，能控制动臂油缸10、斗杆油缸11及铲斗油缸12等的动作。

在上部回转体3的上部安装有天线20、21。天线20、21用于检测液压挖掘机100的当前位置。天线20、21如图2所示与用于检测液压挖掘机100的当前位置的位置检测装置23电连接。位置检测装置23利用RTK-GNSS(RealTimeKinematic-GlobalNavigationSatelliteSystems，GNSS称作全球卫星导航系统)来检测液压挖掘机100的当前位置。在以下的说明中，将天线20、21适当称作GNSS天线20、21。

与GNSS天线20、21接收到的GNSS电波对应的信号被输入到位置检测装置23。位置检测装置23检测GNSS天线20、21的设置位置。

GNSS天线20、21优选如图1所示那样设置在上部回转体3之上，且设置在液压挖掘机100的沿左右方向分开的两端位置。GNSS天线20、21可以安装在上部回转体3之上，且如图1所示那样安装在扶手19上。另外，GNSS天线20、21也可以设置于在上部回转体3的后端设置的配重3CW的上部或者设置于驾驶室4的后方且发动机室3EG之上。总之，GNSS天线20、21最好设置在彼此尽可能分离的位置，这样能提高液压挖掘机100的当前位置的检测精度。另外，GNSS天线20、21优选设置在尽量不妨碍操作者的视野的位置。

(液压挖掘机的控制系统)

使用图2来说明液压挖掘机100的控制系统。在液压挖掘机100中，作为控制系统而包括作为作业机械的姿势运算装置的传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28、检测角速度及加速度的作为惯性计测装置的IMU29、第二显示装置39。上述装置设于上部回转体3。例如，IMU29设置在驾驶室4的下部。第一显示装置28及第二显示装置39设置在驾驶室4的内部的驾驶席附近。

传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28及位置检测装置23与设置在液压挖掘机100内的车内信号线41电连接。传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28及位置检测装置23能够经由车内信号线41彼此进行通信。传感器控制装置24、IMU29和第二显示装置39与不同于车内信号线41的车内信号线42电连接。传感器控制装置24、IMU29和第二显示装置39能够经由车内信号线42彼此进行通信。IMU29可以不与车内信号线42电连接，而与车内信号线41电连接。

传感器控制装置24与检测图1所示的动臂油缸10、斗杆油缸11及铲斗油缸12的行程的传感器、检测动臂6的角度及斗杆7的角度的传感器以及检测上部回转体3的回转角度的传感器等各种传感器类35电连接。检测动臂6的角度、斗杆7的角度及回转角度的传感器不一定是必需的。传感器控制装置24在对各种的传感器类35所检测出的信号实施滤波处理或A/D(Analog/Digital)转换等各种信号处理之后，将处理后的信号向车内信号线41输出。

作为检测动臂油缸10、斗杆油缸11及铲斗油缸12的行程的传感器，可以使用检测各液压缸的伸缩量的接近传感器或回转式编码器等。另外，可以在动臂6的基端部及斗杆7的基端部设置未图示的编码器，利用该编码器能将检测各液压缸的伸缩量的接近传感器或回转式编码器的检测值重置。

传感器控制装置24从车内信号线42获取IMU29检测出的信号、具体而言是加速度及角速度。传感器控制装置24使用从IMU29获取的加速度及角速度来求解作业机械的姿势角，对求出的姿势角实施本实施方式涉及的滤波处理来除去噪声后向车内信号线41输出。姿势角是与液压挖掘机100的倾斜度相关的信息。姿势角的精度会影响第二显示装置39对刃尖8T的位置信息的显示精度及液压挖掘机100的作业机控制的精度。通过执行本实施方式涉及的滤波处理，由此来确保姿势角的精度。针对本实施方式涉及的滤波处理在后叙述。

作业机控制装置25根据来自操作装置30的输入来控制图1所示的作业机2的动作、上部回转体3的动作及行驶装置5的动作。操作装置30具有作为操作部的作业机操作构件31L、31R及行驶操作构件33L、33R、作业机操作检测部32L、32R及行驶操作检测部34L、34R。在本实施方式中，作业机操作构件31L、31R及行驶操作构件33L、33R是先导压力方式的杆，但并不局限于此。作业机操作构件31L、31R及行驶操作构件33L、33R可以是例如使用电位计而能够检测各操作构件(杆)的操作量的电气方式的杆。

作业机操作构件31L、31R是设置在驾驶席的左右、供液压挖掘机100的操作者对作业机2进行操作的构件，例如是操纵杆那样的具备抓握部分和杆件的操作杆。这种结构的作业机操作构件31L、31R能够抓住抓握部而向前后左右倾倒。例如，通过对设于左方的作业机操作构件31L进行操作，而能够使斗杆7及上部回转体3动作，通过对设于右方的作业机操作构件31R进行操作，而能够使铲斗8及动臂6动作。

作业机操作检测部32L、32R根据对作业机操作构件31L、31R的输入、即操作内容来产生先导压力，并将产生的工作油的先导压力向液压控制阀38所具备的作业用控制阀供给。作业用控制阀根据该先导压力的大小进行动作，而从液压泵37向图1所示的动臂油缸10、斗杆油缸11及铲斗油缸12等供给工作油。在作业机操作构件31L、31R为电气方式的杆的情况下，作业机操作检测部32L、32R使用例如电位计等来检测对作业机操作构件31L、31R的输入、即操作内容，并将输入转换成电信号(检测信号)而向作业机控制装置25发送。作业机控制装置25根据该检测信号来控制作业用控制阀。

行驶操作构件33L、33R是设置在驾驶席的前方、供操作者在使液压挖掘机100的行驶装置5行驶或回转时进行操作的构件。行驶操作构件33L、33R是例如具备抓握部和杆件的操作杆(以下，适当称作行驶杆)。这样的行驶操作构件33L、33R能够由操作者抓住抓握部而向前后倾倒。就行驶操作构件33L、33R而言，若两个操作杆同时向前倾倒，则液压挖掘机100前进，若同时向后倾倒则液压挖掘机100后退。

行驶操作构件33L、33R可以是操作者用脚踩踏而能够操作的未图示的踏板，例如是跷跷板式的踏板。通过对踏板的前侧或后侧中的任一侧进行踩踏，由此能够与前述的操作杆同样地产生先导压力，控制行驶用控制阀，而使液压马达5c进行驱动，从而使液压挖掘机100前进或后退。若对两个踏板同时踩踏前侧，则液压挖掘机100前进，若对两个踏板同时踩踏后侧则液压挖掘机100后退。若踩踏一方的踏板的前侧或后侧，则仅履带5a、5b的一侧旋转，能够使液压挖掘机100的行驶装置5回转。

这样，操作者在想要使液压挖掘机100行驶的情况下，只要执行用手使操作杆向前后倾倒或者用脚对踏板的前侧或后侧进行踩踏中的任一方的动作，就能够驱动行驶装置5的液压马达5c。如图2所示，行驶操作构件33L、33R及行驶操作检测部34L、34R存在两组。通过对左侧的行驶操作构件33L进行操作，由此能够使左侧的液压马达5c驱动而使左侧的履带5b进行动作。通过操作右侧的行驶操作构件33R，由此能够使右侧的液压马达5c驱动而使右侧的履带5a进行动作。

行驶操作检测部34L、34R根据对行驶操作构件33L、33R的输入、即操作内容而产生先导压力，并将产生的先导压力向液压控制阀38所具备的行驶用控制阀供给。行驶用控制阀根据该先导压力的大小进行动作，而向行驶用的液压马达5c供给工作油。在行驶操作构件33L、33R为电气方式的杆的情况下，行驶操作检测部34L、34R使用例如电位计等来检测对行驶操作构件33L、33R的输入、即操作内容，并将输入转换成电信号(检测信号)而向作业机控制装置25发送。作业机控制装置25根据该检测信号来控制行驶用控制阀。

发动机控制装置26通过调整向发动机36供给的燃料的量来控制发动机36的输出。发动机36对与输出轴机械连结的液压泵37进行驱动。与发动机36连结的液压泵37向液压挖掘机100所具备的动臂油缸10、斗杆油缸11及铲斗油缸12等液压设备供给工作油。在发动机控制装置26上电连接有旋转速度检测传感器36R及燃料调整刻度盘26D。发动机控制装置26根据旋转速度检测传感器36R所检测出的发动机36的曲轴的旋转速度及燃料调整刻度盘26D的设定等，来控制向发动机36供给的燃料的量。这样，发动机控制装置26控制发动机36的输出。

泵控制装置27控制液压挖掘机100所具备的液压泵37。液压泵37是例如通过改变斜盘的偏转角来改变工作油的喷出量等的斜盘式的液压泵。泵控制装置27例如经由车内信号线41而从作业机控制装置25获取液压控制阀38的液压传感器38C所检测出的先导压力。泵控制装置27根据获取的先导压力来控制液压泵37的斜盘的偏转角，由此控制从液压泵37喷出的工作油的流量。从液压泵37喷出的工作油经由液压控制阀38所具备的作业用控制阀或行驶用控制阀向动臂油缸10、斗杆油缸11、铲斗油缸12、液压马达5c及用于使上部回转体3回转的回转马达中的至少一方供给，从而驱动上述构件中的至少一方。

第一显示装置28是显示图像的装置。第一显示装置28包括显示部28M和控制部28C。第一显示装置28如图1所示设置在液压挖掘机100的驾驶室4内，且设置在驾驶席附近。在本实施方式中，第一显示装置28将例如液压挖掘机100的运转信息显示于显示部28M。运转信息例如是液压挖掘机100的累积工作时间、燃料的剩余量或发动机36的冷却水温度等。在液压挖掘机100具备周边监视用或后方监视器用的相机等的情况下，第一显示装置28可以显示该相机拍摄到的图像。

在本实施方式中，第一显示装置28除了将各种图像显示于显示部28M以外，还作为输入装置发挥功能。因此，第一显示装置28在显示部28M的下方具备输入装置28I。在本实施方式中，输入装置28I的多个按钮式的开关相对于显示部28M的横向平行地排列。通过对输入装置28I进行操作，由此能够切换显示部28M所显示的图像或者执行与液压挖掘机100的动作相关的各种设定。需要说明的是，也可以通过将输入装置28I组入显示部28M而成的触控面板来构成第一显示装置28。另外，输入装置28I也可以为与第一显示装置28独立的个体而设于驾驶席附近的控制台。

第二显示装置39是显示图像的装置。第二显示装置39包括显示部39M和控制部39C。第二显示装置39如图1所示设置在液压挖掘机100的驾驶室4内的驾驶席附近。在本实施方式中，第二显示装置39将例如液压挖掘机100所具备的铲斗8的刃尖8T的相对于施工现场的地形的位置信息作为图像而显示于显示部28M。此时，第二显示装置39可以将与刃尖8T要挖掘的施工现场的地形相关的信息和刃尖8T的位置信息一起显示。

在本实施方式中，第二显示装置39的显示部39M例如是液晶显示装置，但并不局限于此。控制部39C对显示部39M的动作进行控制并求解刃尖8T的位置信息。另外，控制部39C将表示刃尖8T的位置与施工现场的地形的相对位置关系的指引图像显示于显示部39M。因此，控制部39C中存储有关于施工现场的地形的全球坐标位置信息。

在本实施方式中，第二显示装置39在显示部39M的下方具备输入装置39I。在本实施方式中，输入装置39I的多个按钮式的开关相对于显示部39M的横向平行地排列。通过对输入装置39I进行操作，由此能够切换显示部39M所显示的指引图像或者改变指引的内容。在本实施方式中，第二显示装置39的功能可以通过第一显示装置28来实现，也可以通过将输入装置39I组入显示部39M而成的触控面板来构成第二显示装置39。另外，也可以使用在一个框体内收纳有第一显示装置28和第二显示装置39而成的显示装置。

IMU29检测液压挖掘机100的角速度及加速度。伴随着液压挖掘机100的动作而产生在行驶时产生的加速度、在回转时产生的角加速度及重力加速度等各种各样的加速度，IMU29对包括至少重力加速度在内的加速度进行检测，不区分各加速度的种类地输出检测出的加速度。关于IMU29的详细情况在后叙述，为了以更高的精度检测加速度，该IMU29优选设置在例如液压挖掘机100的上部回转体3的回转中心轴上，但也可以如前所述那样设置在驾驶室4的下部。

IMU29对图1所示的坐标系(X、Y、Z)中的X轴方向、Y轴方向及Z轴方向的加速度以及绕X轴、Y轴及Z轴的角速度(旋转角速度)进行检测。在图1所示的例子中，X轴是与液压挖掘机100的前后方向平行的轴，Y轴是与液压挖掘机100的宽度方向平行的轴，Z轴是与X轴及Y轴这两方正交的轴。坐标系(X、Y、Z)例如可以设为车身坐标系。

图3是表示IMU29的一例的框图。IMU29包括回转仪29V、加速度传感器29A、AD转换部29AD和物理量转换部29PT。回转仪29V检测液压挖掘机100的角速度。加速度传感器29A检测液压挖掘机的加速度。由回转仪29V检测出的角速度及由加速度传感器29A检测出的加速度均是模拟量。AD转换部29AD将这些模拟量转换成数字量。物理量转换部29PT将AD转换部29AD的输出转换成物理量。具体而言，物理量转换部29PT将与回转仪29V的检测值对应的AD转换部29AD的输出转换成角速度ω，将与加速度传感器29A的检测值对应的AD转换部29AD的输出转换成加速度Ac。物理量转换部29PT将角速度ω及加速度Ac向车内信号线42输出。

液压挖掘机100的倾斜度可以用俯仰角、侧倾角及横摆角来表示。俯仰角是液压挖掘机100绕Y轴倾斜时的角度，侧倾角是液压挖掘机100绕X轴倾斜时的角度，横摆角是液压挖掘机100绕Z轴倾斜时的角度。在本实施方式中，将俯仰角及侧倾角称作液压挖掘机100的姿势角。在本实施方式中，传感器控制装置24经由车内信号线42来获取IMU29检测出的液压挖掘机100的角速度及加速度。传感器控制装置24根据获取的液压挖掘机100的角速度及加速度来求解姿势角。以下，适当使用符号θ来表示姿势角。

图4是表示传感器控制装置24的框图。传感器控制装置24包括作为姿势角运算部的第一姿势角运算部51、同样作为姿势角运算部的第二姿势角运算部52、第一互补滤波器53、第二互补滤波器54及切换部55。第一姿势角运算部51及第二姿势角运算部52根据液压挖掘机100的角速度ω及加速度Ac来求解液压挖掘机100的姿势角θ。在本实施方式中，第一姿势角运算部51根据IMU29检测出的液压挖掘机100的加速度Ac来求解姿势角θ。更具体而言，第一姿势角运算部51根据重力加速度的方向来求解姿势角θ。第二姿势角运算部52根据IMU29检测出的液压挖掘机100的角速度ω来求解姿势角θ。更具体而言，第二姿势角运算部52对角速度ω进行积分来求解姿势角θ。

第一互补滤波器53设定第一截止频率，减少由第一姿势角运算部51及第二姿势角运算部52求出的姿势角θ中含有的杂音而输出第一姿势角θ1。第二互补滤波器54设定与第一截止频率不同的第二截止频率，减少由第一姿势角运算部51及第二姿势角运算部52求出的姿势角θ中含有的杂音而输出第二姿势角θ2。第一互补滤波器53和第二互补滤波器54仅是截止频率((cutofffrequency))不同。

第一互补滤波器53具有滤波部53F和加法运算部53AD。滤波部53F具有第一LPF(LowPassFilter)a和第一HPF(HighPassFilter)a。加法运算部53AD将第一LPFa的输出与第一HPFa的输出相加而输出。加法运算部53AD的输出是第一互补滤波器53的输出。将第一互补滤波器53的输出适当称作第一姿势角θ1。

第二互补滤波器54具有滤波部54F和加法运算部54AD。滤波部54F具有第二LPF(LowPassFilter)b和第二HPF(HighPassFilter)b。加法运算部54AD将第二LPFb的输出和第二HPFb的输出相加而输出。加法运算部54AD的输出是第二互补滤波器54的输出。将第二互补滤波器54的输出称作第二姿势角θ2。

切换部55具有处理部55C和切换器55S。切换部55根据液压挖掘机100的动作状态来切换并输出第一姿势角θ1或第二姿势角θ2。接着，对该输出进行说明。切换部55的处理部55C根据液压挖掘机100的状态、例如液压挖掘机100是动作还是静止，来判定输出第一姿势角θ1或第二姿势角θ2中的哪一方。处理部55C的判定结果经由判定结果输出线55a向切换器55S输出，切换器55S根据处理部55C的判定结果而将第一姿势角θ1或第二姿势角θ2中的任一方作为由传感器控制装置24求出的液压挖掘机100的姿势角θo经由姿势角输出线55b向车内信号线41输出。

图5是表示互补滤波器的特性的图。图5的纵轴表示增益GN，横轴表示频率f。图5的曲线(LPF和HPF)表示互补滤波器的频率特性。互补滤波器具备LPF(LowPassFilter)和HPF(HighPassFilter)，根据图5可知，是LPF的增益GN和HPF的增益GN之和成为1的滤波器。例如，向互补滤波器输入姿势角θ时，LPF的输出LPF(θ)和HPF的输出HPF(θ)之和成为1。即，成为LPF(θ)+HPF(θ)＝θ。将LPF的增益GN和HPF的增益GN均成为0.5时的频率称作截止频率fc。传感器控制装置24所具有的第一互补滤波器53及第二互补滤波器54如前所述那样仅是截止频率fc不同。

图4所示的第一姿势角运算部51根据重力加速度的方向而求出的姿势角θ通过真实的姿势角θtr与误差θan之和来求出。误差θan由于例如冲击加速度等那样的重力加速度以外的加速度而产生。误差θan是高频成分为主体的杂音。图4所示的第二姿势角运算部52对角速度ω进行积分而求出的姿势角θ通过真实的姿势角θtr与误差θwn之和来求出。误差θwn由于积分所累积的偏差而产生。误差θwn是低频成分为主体的杂音。

这样，第一姿势角运算部51根据重力加速度的方向而求出的姿势角θ含有高频成分为主体的误差θan，因此向第一互补滤波器53的第一LPFa及第二互补滤波器54的第二LPFb输入。第二姿势角运算部52对角速度ω进行积分而求出的姿势角θ含有低频成分为主体的误差θwn，因此向第一互补滤波器53的第一HPFa及第二互补滤波器54的第二HPFb输入。

第一LPFa的输出成为LPFa(θtr+θan)，第一HPFa的输出成为HPFa(θtr+θwn)。第二LPFb的输出成为LPFb(θtr+θan)，第二HPFb的输出成为HPFb(θtr+θwn)。若LPFa(θtr+θan)、HPFa(θtr+θwn)、LPFb(θtr+θan)及HPFb(θtr+θwn)均具有线性，则式(1)至式(4)成立。

LPFa(θtr+θan)＝LPFa(θtr)+LPFa(θan)··(1)

HPFa(θtr+θwn)＝HPFa(θtr)+HPFa(θwn)··(2)

LPFb(θtr+θan)＝LPFb(θtr)+LPFb(θan)··(3)

HPFb(θtr+θwn)＝HPFb(θtr)+HPFb(θwn)··(4)

根据前述的互补滤波器的特性，LPFa(θ)+HPFa(θ)＝θ及LPFb(θ)+HPFb(θ)＝θ成立。在第一互补滤波器53中，滤波部53F的输出、即第一LPFa的输出和第一HPFa的输出被加法运算部53AD相加。加法运算部53AD的输出、即第一姿势角θ1成为θtr+LPFa(θan)+HPFa(θwn)。在第二互补滤波器54中，滤波部54F的输出、即第二LPFb的输出和第二HPFb的输出被加法运算部54AD相加。加法运算部54AD的输出、即第二姿势角θ2成为θtr+LPFb(θan)+HPFb(θwn)。

误差θan是高频成分为主体，因此通过第一LPFa及第二LPFb而被减小。因此，LPFa(θan)及LPFb(θan)的值变小。误差θwn是低频成分为主体，因此通过第一HPFa及第二HPFb而被减小。因此，LPFa(θan)及HPFa(θwn)以及LPFb(θan)及HPFb(θwn)的值变小，作为加法运算部53AD的输出的第一姿势角θ1及作为加法运算部54AD的输出的第二姿势角θ2成为接近真实的姿势角θtr的值。

图6是表示误差θan及误差θwn的频率特性的图。图6的纵轴表示误差θan及误差θwn的频谱，横轴表示频率f。假设在使用性能高的IMU29的情况下，IMU29所检测的角速度ω及加速度Ac的精度也高，因此，图4所示的传感器控制装置24具有的第一姿势角运算部51所求出的姿势角θ的误差θan及第二姿势角运算部52所求出的姿势角θ的误差θwn变小。在IMU29的性能低的情况下，IMU29所检测的角速度ω及加速度Ac的精度变低，因此，图4所示的传感器控制装置24具有的第一姿势角运算部51所求出的姿势角θ的误差θan及第二姿势角运算部52所求出的姿势角θ的误差θwn变大。其结果是，如图6所示，误差θwn和误差θan分别即使超过互补滤波器的截止频率fc也还是存在，且在包含截止频率fc在内的规定的频率f的范围内重合。误差θwn在比截止频率fc大的频率下也还是存在，误差θan在比截止频率fc小的频率也还是存在。

因而，在IMU29的性能低的情况下，凭借一个互补滤波器，无法充分地除去作为杂音的误差θwn及误差θan，可能会导致姿势角θ的精度降低。这样的话，可能会影响第二显示装置39对刃尖8T的位置信息的显示精度及液压挖掘机100的作业机控制的精度。高性能的IMU29的价格也高，因此会导致液压挖掘机100的制造成本的上升。即，为了将性能低的IMU29应用于液压挖掘机100，需要考虑图6所示的特性。因此，传感器控制装置24使用截止频率fc不同的第一互补滤波器53和第二互补滤波器54，以使得即使在使用性能比较低的IMU29的情况下也能够抑制姿势角θ的精度降低。

图7是表示第一互补滤波器53的增益GN及第二互补滤波器54的增益GN与频率f的关系的图。图7的纵轴表示增益GN，横轴表示频率f。频率fch为第一互补滤波器53的第一截止频率，频率fc1为第二互补滤波器54的第二截止频率。在本实施方式中，第一截止频率fch比第二截止频率fc1高。即，第二截止频率fc1比第一截止频率fch低。

第一互补滤波器53的第一截止频率fch设定为能够充分地减少角速度ω的积分误差、即误差θwn的频率。第二互补滤波器54的第二截止频率fc1设定为能够充分地减少重力加速度以外的加速度所引起的误差θan的频率。

第一互补滤波器53虽能通过第一HPFa有效地减少角速度ω的积分所引起的误差θwn，但难以有效地减少重力加速度以外的加速度所引起的误差θan。因此，第一互补滤波器53在液压挖掘机100处于静止的状态或接近静止的状态、即被视作静止的状态(适当称作准静止状态)的情况下，能够精度良好地求解姿势角θ，但在液压挖掘机100处于不是准静止状态的动态状态的情况下，姿势角θ的精度会降低。在本实施方式中，动态状态是指液压挖掘机100被视作进行动作的状态。

第二互补滤波器54虽能通过第二LPFa有效地减少重力加速度以外的加速度所引起的误差θan，但难以有效地减少角速度ω的积分所引起的误差θwn。因此，第二互补滤波器54在液压挖掘机100处于动态状态的情况下，能够精度良好地求解姿势角θ，但在液压挖掘机100处于准静止状态的情况下，与第一互补滤波器53所算出的姿势角θ相比，姿势角θ的精度会降低。即，第二互补滤波器54虽然短时间的动态特性优异，但在准静止状态下，与动态状态同样地，存在角速度ω的积分所引起的误差θwn。

图4所示的传感器控制装置24所具备的切换部55根据液压挖掘机100的动作状态是准静止状态还是动态状态，来切换并输出第一姿势角θ1或第二姿势角θ2。例如，在液压挖掘机100处于准静止状态的情况下，切换部55将第一互补滤波器53输出的第一姿势角θ1作为液压挖掘机100的姿势角θo向车内信号线41输出。在液压挖掘机100处于动态状态的情况下，切换部55将第二互补滤波器54输出的第二姿势角θ2作为液压挖掘机100的姿势角θo向车内信号线41输出。

这样，传感器控制装置24在液压挖掘机100处于准静止状态的情况下，将第一互补滤波器53的第一姿势角θ1设为液压挖掘机100的姿势角θo，因此，能够在准静止状态下抑制姿势角θo的精度降低。在液压挖掘机100处于动态状态的情况下，传感器控制装置24将第二互补滤波器54的第二姿势角θ2设为液压挖掘机100的姿势角θo，因此在动态状态下也能够抑制姿势角θo的精度降低。其结果是，传感器控制装置24在液压挖掘机100处于准静止状态及动态状态的任一状态下都能抑制液压挖掘机100的姿势角θo的精度降低。

在液压挖掘机100进行动作时，使用第二互补滤波器54输出的第二姿势角θ2来求解例如图1所示的铲斗8的刃尖8T的位置。另外，在液压挖掘机100静止时，使用第一互补滤波器53输出的第一姿势角θ1来求解铲斗8的刃尖8T的位置。因此，能够抑制图2所示的第二显示装置39求解以铲斗8的刃尖8T的位置为代表的作业机2的位置或液压挖掘机100的车辆主体1的位置等时的精度降低。

切换部55的处理部55C使用例如如下的条件A和条件B来判定准静止状态和动态状态，并根据其判定结果来控制切换器55S。

条件A：在进行切换的判定的时刻之前的规定期间内，第一姿势角θ1的标准偏差比预先设定的阈值小。

条件B：重力加速度以外的加速度的大小比预先设定的阈值小。

第一姿势角θ1根据IMU29检测出的角速度ω或加速度Ac而求出，包含重力加速度在内的加速度由IMU29检测出。即，处理部55C根据液压挖掘机100所具备的IMU29的状态来判定准静止状态和动态状态。

对上述的条件B进行说明。IMU29如前所述那样检测包括至少重力加速度在内的加速度，不区分检测出的各加速度的种类而将检测出的加速度输出。重力加速度是已知的。因此，处理部55C根据IMU29输出的加速度来运算X轴方向或Y轴方向的加速度。处理部55C从求出的X轴方向的加速度中减去重力加速度的与X轴方向相当的重力加速度，则能够求解重力加速度以外的加速度的大小。处理部55C对重力加速度以外的加速度的大小和预先设定的阈值进行比较。需要说明的是，处理部55C也可以从求出的Y轴方向的加速度中减去重力加速度的与Y轴方向相当的重力加速度而求解重力加速度以外的加速度的大小，并与预先设定的阈值进行比较，判定条件B是否成立。

处理部55C经由图4所示的加速度发送线L1或第一姿势角发送线L2来获取自IMU29获取的加速度Ac及作为第一互补滤波器53的输出的第一姿势角θ1，并判定条件A及条件B是否同时成立。在条件A和条件B这两方成立的情况下，可以视作是准静止状态。在本实施方式中，准静止状态是指液压挖掘机100不进行行驶、上部回转体3的回转及作业机3的动作中的任一方而完全静止的状态或者液压挖掘机100不进行行驶及上部回转体3的回转而仅是作业机3动作的状态。这种情况下，处理部55C以使切换器55S与第一互补滤波器53的加法运算部53AD连接的方式使切换器55S动作。切换器55S将第一互补滤波器53输出的第一姿势角θ1作为液压挖掘机100的姿势角θo向车内信号线41输出。

在条件A和条件B不成立的情况下，即，在条件A及条件B的至少一方不成立的情况下，可以视作是动态状态、即液压挖掘机100进行动作的状态。这种情况下，处理部55C以使切换器55S与第二互补滤波器54的加法运算部54AD连接的方式使切换器55S动作。切换器55S将第二互补滤波器54输出的第二姿势角θ2作为液压挖掘机100的姿势角θo向车内信号线41输出。若切换部55使用条件A及条件B来切换第一姿势角θ1和第二姿势角θ2，则仅凭IMU29的检测值就能够实现上述的切换。

在本实施方式中，条件A的规定期间设定为例如1秒，但并不局限于此。与条件A的标准偏差进行比较的阈值不受限定，可以设定为例如0.1度。条件B在重力加速度以外的加速度小于预先设定的阈值的情况下成立，在检测出预先设定的阈值以上的重力加速度以外的加速度的情况下不成立。条件B的阈值不受限定，可以在例如重力加速度的0.1倍以上的范围内适当设定。

(判定是准静止状态还是动态状态的变形例)

在本实施方式中，图4所示的切换部55的处理部55C根据图2所示的IMU29的检测值来切换第一姿势角θ1或第二姿势角θ2，而作为液压挖掘机100的姿势角θo来输出。第一姿势角θ1或第二姿势角θ2的选择并不局限于此，处理部55C可以使用例如与液压挖掘机100的动作相关的信息(以下，适当称作动作信息)来切换第一姿势角θ1或第二姿势角θ2。

在本实施方式中，动作信息是与液压挖掘机100产生了何种动作相关的信息。例如，动作信息包括图1所示的上部回转体3是否进行回转的信息、行驶装置5是否进行动作的信息或作业机2是否进行动作的信息等。动作信息使用例如下述的检测值等：从对上部回转体3的回转进行检测的传感器输出的检测值；在用于使上部回转体3回转的回转马达设置解析器等回转角度传感器而从这样的角度检测器或旋转传感器输出的检测值；或者从对由图2所示的作业机操作检测部32L、32R所生成的先导压力进行检测的液压传感器输出的检测值。即，动作信息可以是例如上部回转体3或作业机2等实际是否进行动作的信息，也可以是对用于使上部回转体3或作业机2等进行动作的操作构件进行操作的信息。

图8是表示本实施方式的变形例中的第一姿势角θ1和第二姿势角θ2的切换所使用的表TB的一例的图。在本变形例中，切换部55的处理部55C根据基于IMU29的检测值所作出的是准静止状态还是动态状态的判定以及上部回转体3是否在回转中的判定，来切换第一姿势角θ1或第二姿势角θ2。表TB中记载了相对于上部回转体3的状态和基于IMU29的检测值的条件A及条件B而言应作为液压挖掘机100的姿势角θo输出的姿势角。上部回转体3的状态用ON或OFF来表示，在为ON时，上部回转体3处于回转中，为OFF时，上部回转体3处于停止中。条件A及条件B用A&B或NOT(A&B)来表示，A&B表示准静止状态，NOT(A&B)表示动态状态。

在基于IMU29的检测值的判定结果为处于准静止状态且根据动作信息可知上部回转体3处于回转中(ON)的情况下，切换部55将第二姿势角θ2作为液压挖掘机100的姿势角θo输出。由于上部回转体3实际上进行动作，因此使用第二姿势角θ2来作为液压挖掘机100的姿势角θo，由此能够确保姿势角θo的精度。

在基于IMU29的检测值的判定结果为处于准静止状态且根据动作信息可知上部回转体3处于停止中(OFF)的情况下，切换部55将第一姿势角θ1作为液压挖掘机100的姿势角θo输出。由于处于准静止状态且上部回转体3实际上停止，因此使用第一姿势角θ1作为液压挖掘机100的姿势角θo，由此能够减少角速度ω的积分所引起的误差。

在基于IMU29的检测值的判定结果为处于动态状态且根据动作信息可知上部回转体3处于回转中(ON)的情况下，切换部55将第二姿势角θ2作为液压挖掘机100的姿势角θo输出。由于处于动态状态且上部回转体3实际上进行动作，因此使用第二姿势角θ2作为液压挖掘机100的姿势角θo，由此能够确保姿势角θo的精度。

在基于IMU29的检测值的判定结果为处于动态状态且根据动作信息可知上部回转体3处于停止中(OFF)的情况下，切换部55可以将第一姿势角θ1或第二姿势角θ2中的任一方作为液压挖掘机100的姿势角θo输出，但在本变形例中输出第二姿势角θ2。

在本变形例中，切换部55根据基于IMU29的检测值所作出的是准静止状态还是动态状态的判定以及上部回转体3是否处于回转中的判定，来切换第一姿势角θ1或第二姿势角θ2。通过这样，切换部55能够更加精度良好地判定液压挖掘机100的状态，而选择适当的姿势角。在本变形例中，不局限于上述的处理，切换部55也可以不使用基于IMU29的检测值的判定结果而是根据上部回转体3是否处于回转中的判定，来切换第一姿势角θ1或第二姿势角θ2。例如，可以在上部回转体3处于回转中时将第二姿势角θ2作为液压挖掘机100的姿势角θo，在上部回转体3处于停止中时将第一姿势角θ1作为液压挖掘机100的姿势角θo。

图9是表示传感器控制装置24的切换部55所输出的液压挖掘机100的姿势角θo、第一姿势角θ1和第二姿势角θ2的时间变化的一例的图。图9所示的粗实线示出后述的缓和项dif的时间变化的一例。图9的纵轴表示姿势角θ，横轴表示时间t。图9的Sst所示的区间表示准静止状态，将第一姿势角θ1作为液压挖掘机100的姿势角θo输出。图9的Sdm所示的区间表示动态状态，将第二姿势角θ2作为液压挖掘机100的姿势角θo输出。在图9所示的例子中，时间t1至时间t2以及时间t3以后为准静止状态Sst，时间t2至时间t3为动态状态Sdm。

液压挖掘机100的姿势角θo在时间t2从第一姿势角θ1切换为第二姿势角θ2，在时间t3从第二姿势角θ2切换为第一姿势角θ1。就第二姿势角θ2而言，因对角速度ω进行积分而引起的误差θwn积蓄，因此，在时间t2处，第一姿势角θ1和第二姿势角θ2成为不同的值。同样，在时间t3处，第二姿势角θ2和第一姿势角θ1成为不同的值。

在切换部55将从传感器控制装置24输出的姿势角θo从第一姿势角θ1切换为第二姿势角θ2时或者从第二姿势角θ2切换为第一姿势角θ1时，若直接进行切换，则在切换时液压挖掘机100的姿势角θo可能会变得不连续。另外，如前所述，就第二姿势角θ2而言，因对角速度ω进行积分所引起的误差θwn积蓄，因此，在将第二姿势角θ2用作液压挖掘机100的姿势角θo的情况下，需要减少积分所引起的误差θwn。

为了减少在姿势角θo的切换时产生的姿势角θo的不连续及积分所引起的误差θwn，在本实施方式中，切换部55的处理部55C使用式(5)至式(10)来求解姿势角θo并将其输出。

θo＝θ1+dif··(5)

θo＝θ2+dif···(6)

dif＝Ftr×dif_prev···(7)

dif＝dif_prev···(8)

dif＝dif_prev+θ1-θ2···(9)

dif＝dif_prev+θ2-θ1···(10)

式(5)在准静止状态下求解姿势角θo时使用，式(6)在动态状态下求解姿势角θo时使用。式(5)及式(6)的dif为缓和项。式(7)的缓和项dif在准静止状态下使用，式(8)的缓和项dif在动态状态下使用。式(7)的Ftr为缓和系数。缓和系数Ftr大于0且小于1(0＜Ftr＜1)。式(9)的缓和项dif在从准静止状态向动态状态转变时使用。式(10)的缓和项dif在从动态状态向准静止状态转变时使用。式(8)至式(10)的dif_prev是刚刚之前的IMU29的状态(准静止状态Sst或动态状态Sdm)下的缓和项dif。dif_prev的初始值为0。

如图9所示，第一姿势角θ1在准静止状态Sst下保持高精度，但在动态状态Sdm下产生大的误差。第二姿势角θ2在准静止状态Sst及动态状态Sdm的任一种状态下都产生因积分的累积而引起的误差。由于dif_prev的初始值为0，因此，在时间t1至时间t2的准静止状态Sst下，缓和项dif＝0。其结果是，根据式(5)，准静止状态Sst下的液压挖掘机100的姿势角θo成为第一姿势角θ1。

在从准静止状态Sst向动态状态Sdm切换时，即，在时间t＝t2时，处理部55C使用式(9)来求解缓和项dif。如前所述，由于时间t＝t2时的缓和项dif为0，因此缓和项dif成为第一姿势角θ1与第二姿势角θ2之差即θ1-θ2的值。这种情况下的缓和项dif如图9所示那样为负的值。在时间t2时，基于式(5)得出的姿势角θo为θ1，向式(6)的缓和项dif输入的值为θ1-θ2的值，因此基于式(6)得出的姿势角θo也为θ1。因此，在从准静止状态Sst向动态状态Sdm切换时，液压挖掘机100的姿势角θo连续地变化。

在时间t2至时间t3的动态状态Sdm下，缓和项dif的值保持在切换时即时间t2得到的θ1-θ2的值不变。动态状态Sdm下的液压挖掘机100的姿势角θo根据式(6)将动态状态Sdm下的第二姿势角θ2与在时间t2得到并被保持的缓和项dif＝θ1-θ2的值相加来求出。此时使用的缓和项dif根据式(8)来说成为dif_prev，因此在动态状态Sdm下使用的缓和项dif使用在时间t2得到并被保持的缓和项dif＝θ1-θ2的值。这样，切换部55的处理部55C在将第一姿势角θ1切换为第二姿势角θ2之后，将从切换时的第一姿势角θ1减去第二姿势角θ2所得的值、即切换时的缓和项dif用作修正值来对求出的第二姿势角θ2进行修正，从而获取姿势角θo。通过这样，能够减少向动态状态Sdm切换以前产生的第二姿势角θ2的积分的蓄积所引起的误差θwn对液压挖掘机100的姿势角θo造成的影响。

在从动态状态Sdm再次向准静止状态Sst切换时，即在时间t3，处理部55C使用式(10)来求解缓和项dif。式(10)的dif_prev是已经得到且被保持的缓和项dif。即，式(10)的dif_prev是时间t2下的缓和项dif、即时间t2下的θ1-θ2的值。根据式(10)，时间t3下的缓和项dif成为将在时间t2下得到且被保持的θ1-θ2的值与在时间t3下得到的θ2-θ1的值相加所得的值。通过使用式(10)，由此在从动态状态Sdm向准静止状态Sst切换时，液压挖掘机100的姿势角θo连续地变化。

在时间t3以后的准静止状态Sst下，处理部55C使用式(5)来求解液压挖掘机100的姿势角θo。此时的缓和项dif由式(7)来决定。式(7)中的dif_prev是从动态状态Sdm向准静止状态Sst再次切换的时刻、即时间t3下的缓和项dif。在时间t3以后的准静止状态Sst下，利用缓和系数Ftr的效果，缓和项dif的值逐渐变小，收敛为0。即，在时间t3以后的准静止状态Sst下，液压挖掘机100的姿势角θo收敛为第一姿势角θ1。这样，切换部55的处理部55C在将第二姿势角θ2切换为第一姿势角θ1之后，将切换时的第二姿势角的误差即切换时的缓和项dif乘以大于0且小于1的作为系数的缓和系数Ftr所得的值用作修正值，来对第一姿势角θ1进行修正。通过这样，由此，在从动态状态Sdm切换为准静止状态Sst之后，液压挖掘机100的姿势角θo连续地变化。

(求解姿势角θo的处理的一例)

图10是表示求解姿势角θo的处理的一例的流程图。在求解液压挖掘机100的姿势角θo时，图2及图4所示的传感器控制装置24在步骤S101中经由车内通信线42来获取基于IMU29得到的角速度ω及加速度Ac的检测值。在步骤S102中，图2所示的第一姿势角运算部51根据IMU29检测出的加速度Ac来求解姿势角θ。在步骤S103中，图2所示的第二姿势角运算部52根据IMU29检测出的角速度ω来求解姿势角θ。步骤S102和步骤S103的顺序没有关系。

在步骤S104中，图4所示的第一互补滤波器53的第一LPFa对根据加速度Ac得到的姿势角θ实施滤波处理。在步骤S105中，图4所示的第二互补滤波器54的第二LPFb对根据加速度Ac得到的姿势角θ实施滤波处理。在步骤S106中，图4所示的第一互补滤波器53的第一HPFa对根据角速度ω得到的姿势角θ实施滤波处理。在步骤S107中，图4所示的第二互补滤波器54的第二HPFb对根据角速度ω得到的姿势角θ实施滤波处理。步骤S104、步骤S105、步骤S106和步骤S107的顺序没有关系。

接着，进入步骤S108，第一互补滤波器53求解第一姿势角θ1。具体而言，加法运算部53AD通过将第一LPFa的输出与第一HPFa的输出相加来求出第一姿势角θ1。在步骤S109中，第二互补滤波器54求解第二姿势角θ2。具体而言，加法运算部54AD通过将第二LPFb的输出与第二HPFb的输出相加来求出第二姿势角θ2。步骤S108和步骤S109的顺序没有关系。

进入步骤S110，图4所示的切换部55的处理部55C在液压挖掘机100处于准静止状态的情况下(步骤S110中为是)，使处理进入步骤S111。在步骤S111中，处理部55C以使传感器控制装置24输出第一姿势角θ1来作为液压挖掘机100的姿势角θo的方式控制切换器55S。在液压挖掘机100处于动态状态的情况下(步骤S110中为否)，在步骤S112中，处理部55C以使传感器控制装置24输出第二姿势角θ2来作为液压挖掘机100的姿势角θo的方式控制切换器55S。

(评价例)

图11是表示根据液压挖掘机100的动作状态而切换了所输出的姿势角θo的结果的一例的图。图11的纵轴表示姿势角θo的一个即俯仰角θp，横轴表示时间t。图11中的实线表示液压挖掘机100的俯仰角θp，单点划线表示第一姿势角θ1，双点划线表示第二姿势角θ2。图11所示的例子示出使图1所示的液压挖掘机100的动臂6从抬起的状态一下子朝向下方下降并静止时的、液压挖掘机100的俯仰角θp的时间变化。在降低动臂6并使其停止时，铲斗8不接地。

动臂6在时间t1停止，产生液压挖掘机100的绕Y轴的运动(俯仰)、即摆动。可知，在液压挖掘机100没有发生俯仰的情况下(时间t1以前)，第一姿势角θ1作为姿势角θo而被输出，在液压挖掘机100发生了俯仰的期间(时间t1以后的规定期间)，第二姿势角θ2作为姿势角θo而被输出。

在时间t2至t3的期间，液压挖掘机100发生了回摆。可知，在由于俯仰结束而判定为准静止状态之后(时间t2以后)，姿势角θo逐渐接近第一姿势角θ1，在时间t3，姿势角θo与第一姿势角θ1大致一致。在判定为准静止状态之后，由于积分所引起的误差的积蓄，第二姿势角θ2成为与第一姿势角θ1不同的值。在本实施方式中，在俯仰结束之后，将第一姿势角θ1设为液压挖掘机100的姿势角θo，因此能够抑制积分误差的影响。

在本实施方式中，使用设定第一截止频率的第一互补滤波器53和设定与第一截止频率不同的第二截止频率的第二互补滤波器54。第一互补滤波器53减少因对角速度ω进行积分而累积的误差(杂音)，第二互补滤波器54减少因重力加速度以外的加速度所引起的误差(杂音)。在本实施方式中，根据液压挖掘机100的动作状态，来切换第一互补滤波器53输出的姿势角和第二互补滤波器54输出的姿势角。其结果是，由于利用与液压挖掘机100的动作状态相应的适当的互补滤波器来求解液压挖掘机100的姿势角θo，因此，在动态状态及准静止状态中的任一种状态下均能抑制姿势角θo的精度降低。

精度高的IMU29价格高，廉价的IMU29的精度相对低。本实施方式即使在使用了精度低的IMU29的情况下，在动态状态及准静止状态中的任一种状态下均能抑制姿势角θo的精度降低。因此，能够抑制姿势角θo的精度降低，且能减少液压挖掘机100的制造成本。

以上，对本实施方式及其变形例进行了说明，但本实施方式及其变形例不受上述的内容限定。另外，上述的结构要素包括本领域技术人员能容易地想到的结构要素、实质上相同的结构要素以及所谓的等同范围内的结构要素。而且，上述的结构要素可以适当组合。而且，可以在不脱离本实施方式及其变形例的主旨的范围内进行结构要素的各种省略、置换及变更中的至少一方。例如，作业机2虽然具有动臂6、斗杆7和铲斗8，但装配于作业机2的配件不局限于此，不限定于铲斗8。作业机械不限定于液压挖掘机100。例如，可以将本实施方式涉及的姿势运算装置应用于推土机、电动平路机或自卸车等，来执行作业机控制或行驶控制等。

在本实施方式中，使用了第一互补滤波器53及第二互补滤波器54，但还可以追加设定与第一截止频率及第二截止频率不同的第三截止频率的第三互补滤波器，或者追加设定与第一截止频率、第二截止频率及第三截止频率不同的第四截止频率的第四互补滤波器。即，截止频率不同的互补滤波器的个数不限定于两个。

符号说明

1车辆主体

2作业机

3上部回转体

4驾驶室

5行驶装置

6动臂

7斗杆

8铲斗

8T刃尖

20、21天线

23位置检测装置

24传感器控制装置

25作业机控制装置

26发动机控制装置

27泵控制装置

28第一显示装置

28M显示部

29IMU

29A加速度传感器

29V回转仪

30操作装置

35传感器类

36发动机

37液压泵

38液压控制阀

39第二显示装置

41、42车内信号线

51第一姿势角运算部

52第二姿势角运算部

53第一互补滤波器

53F滤波部

53AD加法运算部

54F滤波部

54AD加法运算部

54第二互补滤波器

55切换部

55C处理部

55S切换器

100液压挖掘机

TB表

Claims (7)

1.一种作业机械的姿势运算装置，其具备：

计测装置，其设于作业机械，用于检测角速度及加速度；

姿势角运算部，其根据由所述计测装置检测出的角速度及加速度来求解所述作业机械的姿势角；

第一互补滤波器，其被设定第一截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第一姿势角；

第二互补滤波器，其被设定与所述第一截止频率不同的第二截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第二姿势角；

切换部，其根据所述作业机械的状态，来切换并输出所述第一姿势角或所述第二姿势角。

2.根据权利要求1所述的作业机械的姿势运算装置，其中，

所述第一截止频率高于所述第二截止频率。

3.根据权利要求2所述的作业机械的姿势运算装置，其中，

所述切换部在判定为所述作业机械处于准静止状态的情况下输出所述第一姿势角，在判定为所述作业机械处于动态状态的情况下输出所述第二姿势角。

4.根据权利要求1～3中任一项所述的作业机械的姿势运算装置，其中，

所述切换部在将所述第一姿势角切换为所述第二姿势角之后，使用从切换时的所述第一姿势角减去所述第二姿势角得到的值作为修正值，来对所述第二姿势角进行修正。

5.根据权利要求4所述的作业机械的姿势运算装置，其中，

所述切换部在将所述第二姿势角切换为所述第一姿势角之后，使用将切换时的所述第二姿势角的误差乘以大于0且小于1的系数得到的值作为修正值，来对所述第一姿势角进行修正。

6.一种液压挖掘机的姿势运算装置，其具备：

计测装置，其设于液压挖掘机的上部回转体，用于检测角速度及加速度；

姿势角运算部，其根据所述计测装置检测出的所述角速度及所述加速度来求解所述液压挖掘机的姿势角；

第一互补滤波器，其被设定第一截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第一姿势角；

第二互补滤波器，其被设定比所述第一截止频率低的第二截止频率，减少由所述姿势角运算部求出的所述姿势角中含有的杂音而输出第二姿势角；

切换部，其在判定为所述液压挖掘机处于准静止状态的情况下输出所述第一姿势角，在判定为所述液压挖掘机处于动态状态的情况下输出所述第二姿势角。

7.一种作业机械，其具备：

权利要求1～5中任一项所述的作业机械的姿势运算装置；

使用从所述作业机械的姿势运算装置输出的所述第一姿势角或所述第二姿势角来求解安装于所述作业机械的作业机的位置的控制部。