CN105339759B - 作业机械的控制系统以及作业机械的控制方法 - Google Patents

Abstract

作业机械的控制系统对作业机械进行控制，该作业机械具备行驶装置、具有作业用具的工作装置以及供所述工作装置安装且被安装于所述行驶装置而相对于所述行驶装置回转的回转体。作业机械的控制系统包括：位置检测装置，其检测作为所述作业机械的一部分的位置的第一位置，并作为所述第一位置的信息而输出；状态检测装置，其检测表示所述作业机械的动作的动作信息并将该动作信息输出；以及处理装置，其使用所述第一位置的信息以及所述动作信息，求出与所述一部分的位置相当的第二位置，并使用所述第二位置的信息，求出所述工作装置的至少一部分的位置。

Description

作业机械的控制系统以及作业机械的控制方法

技术领域

本发明涉及在具备工作装置的作业机械中使用的作业机械的控制系统以及作业机械。

背景技术

目前已知有如下的技术：利用GPS(Global Positioning System：全球定位系统)等对作业机械的三维位置进行测位，并使用得到的作业机械的位置信息来管理作业机械、管理基于作业机械的施工状态、控制作业机械等(例如专利文献1)。

在先技术文献

专利文献

专利文献1：日本特开2007-147588号公报

发明内容

发明要解决的技术问题

作为具备对作业机械的位置进行测位的设备的作业机械，具有如下的作业机械：利用该设备检测到的作业机械的位置信息，使设置在作业机械的驾驶室内的显示装置显示作业的引导画面、控制工作装置的动作等。基于这种作业机械的施工被称为信息化施工。

有时进行基于信息化施工的建设工程。为了进行信息化施工，利用搭载有GPS天线等的液压挖掘机来进行法面成形这样的施工，期待工期缩短或消除熟练操作员不足的问题。

由于GPS的测位卫星的位置、电离层、对流层或者GPS天线周边的地形的影响，测位结果有时产生偏差。在信息化施工中，基于测位结果求出铲斗的铲尖位置，进行工作装置的控制以及引导画面的显示，但由于测位结果的偏差的影响，有可能产生施工面起伏、引导画面所显示的铲斗的铲尖摇晃等。作为结果，有可能导致完成后的施工面不光滑、施工中的引导画面的视觉确认性下降等。

本发明的目的在于，在基于对作业机械的位置进行了测位的结果进行信息化施工的作业机械中，降低测位结果的偏差对信息化施工造成的影响。

用于解决技术问题的手段

本发明提供一种作业机械的控制系统，其是对作业机械进行控制的系统，该作业机械具备：行驶装置；具有作业用具的工作装置；以及供所述工作装置安装且被安装于所述行驶装置而进行回转的回转体，所述作业机械的控制系统包括：位置检测装置，其检测作为所述作业机械的一部分的位置的第一位置，并作为所述第一位置的信息而输出；状态检测装置，其检测表示所述作业机械的动作的动作信息并将该动作信息输出；以及处理装置，其使用所述第一位置的信息以及所述动作信息，求出与所述一部分的位置相当的第二位置，并使用所述第二位置的信息，求出所述工作装置的至少一部分的位置。

优选为，所述处理装置使用根据所述第一位置以及所述动作信息而得到的信息、即作为所述回转体的旋转中心轴与和所述行驶装置接地的面对应的面的交点的特定点的位置，求出所述第二位置。

优选为，所述处理装置对所述特定点的位置实施平滑化处理，并使用所述平滑化处理后的所述特定点的位置，求出所述第二位置的信息。

优选为，所述处理装置使用所述动作信息对所述第一位置实施平滑化处理，求出所述第二位置的信息。

优选为，当由所述位置检测装置进行的所述作业机械的位置的检测正常、且所述作业机械的行驶停止、且所述回转体不回转时，所述处理装置使用所述第二位置的信息，求出所述工作装置的至少一部分的位置。

优选为，当所述作业机械的行驶停止、且所述回转体正在回转时，所述处理装置将求出所述第二位置的信息的处理中断。

优选为，当所述回转体的回转停止时，所述处理装置使用在将求出所述第二位置的处理中断前得到的所述第二位置的信息，求出所述工作装置的至少一部分的位置。

优选为，当将求出所述第二位置的处理中断时，所述处理装置在所述作业机械开始了行驶的情况下，停止求出所述第二位置的信息的处理。

优选为，在所述位置检测装置对所述作业机械的位置的检测为正常、且所述作业机械的行驶停止的情况下，所述处理装置停止求出所述第二位置的处理。

优选为，所述处理装置具有：位置推定部，其使用所述动作信息来推定所述作业机械的位置，并对通过推定得到的所述作业机械的推定位置进行修正而作为第二位置输出；以及误差运算部，其使用所述第一位置的信息以及所述动作信息中的至少一方，求出所述推定位置所包含的误差并向所述位置推定部推出，所述位置推定部使用所述误差运算部输出的所述误差来修正所述推定位置。

优选为，所述处理装置使用由所述位置检测装置进行的所述作业机械的位置的检测的状态以及所述作业机械的动作状态，来选择向所述误差运算部输入的信息。

本发明是具备上述的作业机械的控制系统的作业机械。

本发明是一种作业机械的控制方法，所述作业机械具备：行驶装置；具有作业用具的工作装置；以及供所述工作装置安装且被安装于所述行驶装置而进行回转的回转体，在对所述作业机械进行控制时，使用所述作业机械所具备的位置检测装置检测到的作为所述作业机械的一部分的位置的第一位置以及所述作业机械所具备的状态检测装置检测到的所述作业机械的动作信息，求出与所述一部分的位置相当的所述作业机械的第二位置，并使用所述第二位置，求出所述工作装置的至少一部分的位置。

优选为，使用根据所述第一位置以及所述动作信息而得到的信息、即作为所述回转体的旋转中心轴与和所述行驶装置接地的面对应的面的交点的特定点的位置，求出所述第二位置。

优选为，对所述特定点的位置实施平滑化处理，并使用所述平滑化处理后的所述特定点的位置，求出所述第二位置。

优选为，使用所述动作信息对所述第一位置实施平滑化处理，求出所述第二位置。

优选为，在求出所述第二位置的情况下，使用所述动作信息来推定所述作业机械的位置，由此求出推定位置，使用所述第一位置以及所述动作信息中的至少一方来计算所述推定位置所包含的误差，并使用所述误差运算部输出的所述误差来修正所述推定位置。

本发明在基于对作业机械的位置进行了测位的结果而进行信息化施工的作业机械中，能够降低测位结果的偏差对信息化施工造成的影响。

附图说明

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

图2是表示控制系统以及液压系统的结构的框图。

图3是液压挖掘机的侧视图。

图4是液压挖掘机的后视图。

图5是实施方式1所涉及的控制系统的控制框图。

图6是表示液压挖掘机的姿势的俯视图。

图7是表示实施方式1所涉及的装置控制器所具有的位置信息运算部的图。

图8是表示实施方式1所涉及的控制系统的处理的一例的流程图。

图9是用于说明平滑化处理的状态的转变的图。

图10是装置控制器使平滑化处理的状态转变的处理的流程图，尤其是表示与平滑化处理的中断相关的处理。

图11是装置控制器使平滑化处理的状态转变的处理的流程图，尤其是表示与平滑化处理的复位相关的处理。

图12是实施方式2所涉及的控制系统的控制框图。

图13是表示实施方式2所涉及的装置控制器所具有的位置信息运算部的图。

图14是表示实施方式2所涉及的控制系统的处理的一例的流程图。

图15是实施方式3所涉及的控制系统的控制框图。

图16是表示实施方式3所涉及的装置控制器所具有的位置/姿势信息运算部的图。

图17是实施方式3所涉及的装置控制器所具有的位置/姿势信息运算部的控制框图。

图18是表示记述有在选择供误差运算部使用的观测方程式时使用的信息的表的一例的图。

图19是表示实施方式3所涉及的控制系统的处理的一例的流程图。

具体实施方式

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

实施方式1.

<作业机械的整体结构〉

图1是实施方式1所涉及的作业机械的立体图。图2是表示控制系统200以及液压系统300的结构的框图。作为作业机械的液压挖掘机100具有作为主体部的车辆主体1和工作装置2。车辆主体1具有作为回转体的上部回转体3和作为行驶体的行驶装置5。上部回转体3在机械室3EG的内部收容有作为动力产生装置的发动机以及液压泵等装置。

在本实施方式中，液压挖掘机100的作为动力产生装置的发动机使用例如柴油发动机等内燃机，但动力产生装置不限定于内燃机。液压挖掘机100的动力产生装置也可以是例如组合内燃机、发电电动机以及蓄电装置而得到的所谓的混合动力方式的装置。另外，液压挖掘机100的动力产生装置也可以不具有内燃机而是组合蓄电装置和发电电动机得到的装置。

上部回转体3具有驾驶室4。驾驶室4设置在上部回转体3的另一端侧。即，驾驶室4设置在与配置有机械室3EG的一侧相反的一侧。在驾驶室4内配置有图2所示的显示部29以及操作装置25。在上部回转体3的上方安装有扶手9。

在行驶装置5的上方搭载有上部回转体3。行驶装置5具有履带5a、5b。行驶装置5被设置在左右的液压马达5c的一方或两方驱动。通过行驶装置5的履带5a、5b旋转而使液压挖掘机100行驶。工作装置2安装在上部回转体3的驾驶室4的侧方侧。

液压挖掘机100也可以具备如下的行驶装置：该行驶装置代替履带5a、5b而具备轮胎，并能够将发动机的驱动力经由传动装置传递给轮胎进行行驶。作为这种方式的液压挖掘机100，例如具有轮式液压挖掘机。

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

工作装置2具有动臂6、斗杆7、作为作业用具的铲斗8、动臂缸10、斗杆缸11以及铲斗缸12。动臂6的基端部经由动臂销13而以能够转动的方式安装于车辆主体1的前部。斗杆7的基端部经由斗杆销14而以能够转动的方式安装于动臂6的前端部。在斗杆7的前端部经由铲斗销15而安装有铲斗8。铲斗8以铲斗销15为中心进行转动。铲斗8在与铲斗销15相反的一侧安装有多个斗齿8B。铲尖8T为斗齿8B的前端。

铲斗8也可以不具有多个斗齿8B。即，也可以不具有图1所示那样的斗齿8B而是铲尖通过钢板形成为直线形状那样的铲斗。工作装置2例如也可以具备具有单个斗齿的倾转铲斗。倾转铲斗是指如下的铲斗：具备铲斗倾转缸，通过使铲斗向左右倾斜转动，即便液压挖掘机100位于倾斜地，也能够将斜面、平地成形为自由的形状或者平整地面，并且还能够利用底板进行碾压作业。此外，工作装置2也可以代替铲斗8而具备凿岩用的附件来作为作业用具，其中，该凿岩用的附件具备法面铲斗或凿岩用的钎头。

图1所示的动臂缸10、斗杆缸11以及铲斗缸12分别是指被工作油的压力驱动的液压缸。以下，将工作油的压力称为适当液压。动臂缸10驱动动臂6使其进行升降。斗杆缸11驱动斗杆7使其绕斗杆销14转动。铲斗缸12驱动铲斗8使其绕铲斗销15转动。

在动臂缸10、斗杆缸11以及铲斗缸12等液压缸与图2所示的液压泵36、37之间设置有图2所示的方向控制阀64。方向控制阀64对从液压泵36、37向动臂缸10、斗杆缸11以及铲斗缸12等供给的工作油的流量进行控制，并且切换工作油流动的方向。方向控制阀64包括：用于驱动液压马达5c的行驶用方向控制阀；以及用于控制动臂缸10、斗杆缸11和铲斗缸12以及使上部回转体3回转的回转马达38的工作装置用方向控制阀。

当从操作装置25供给的被调整为规定的先导压力的工作油使方向控制阀64的阀柱动作时，从方向控制阀64流出的工作油的流量被调整，从而控制从液压泵36、37向动臂缸10、斗杆缸11、铲斗缸12、回转马达38或液压马达5c供给的工作油的流量。其结果是，能够对动臂缸10、斗杆缸11以及铲斗缸12等的动作进行控制。

另外，通过图2所示的装置控制器39对图2所示的控制阀27进行控制，从而控制从操作装置25向方向控制阀64供给的工作油的先导压，因此，能够控制从方向控制阀64向动臂缸10、斗杆缸11、铲斗缸12或者回转马达38供给的工作油的流量。其结果是，装置控制器39能够对动臂缸10、斗杆缸11、铲斗缸12以及上部回转体3的动作进行控制。

在上部回转体3的上部安装有天线21、22。天线21、22用于检测液压挖掘机100的当前位置。天线21、22与图2所示的全局坐标运算装置23电连接。全局坐标运算装置23是检测液压挖掘机100的位置的位置检测装置。全局坐标运算装置23利用RTK-GNSS(Real TimeKinematic-Global Navigation Satellite Systems，GNSS是指全球导航卫星系统)来检测液压挖掘机100的当前位置，更具体而言检测液压挖掘机100的一部分的当前位置。在以下的说明中，将天线21、22适当称为GNSS天线21、22。在本实施方式中，全局坐标运算装置23检测GNSS天线21、22的至少一个的位置来作为液压挖掘机100的一部分的当前位置。与GNSS天线21、22接收到的GNSS电波对应的信号输入到全局坐标运算装置23。全局坐标运算装置23求出全局坐标系中的GNSS天线21、22的设置位置。作为全球导航卫星系统的一例，举出GPS(Global Positioning System)，但全球导航卫星系统不限定于此。

在RTK-GNSS中，由于测位卫星的配置、电离层、对流层或GNSS天线周边的地形的影响，测位的状态会发生变化。在该测位的状态中，例如具有Fix(精度±1cm至2cm左右)、Float(精度±10cm至几m左右)、单独测位(精度±几m左右)、非测位(不能进行测位计算)等。以下将测位的状态为Fix的情况称为正常，将为Fix以外的状态的情况称为异常。

如图1所示，GNSS天线21、22优选设置于上部回转体3之上且为液压挖掘机100的在左右方向、即宽度方向上分离的两端位置处。在本实施方式中，GNSS天线21、22安装于分别安装在上部回转体3的宽度方向两侧的扶手9。GNSS天线21、22安装于上部回转体3的位置不限定于扶手9，但将GNSS天线21、22设置在尽可能远离的位置处能够提高液压挖掘机100的当前位置的检测精度，因此是优选的。另外，GNSS天线21、22优选设置在尽量不妨碍操作员的视野的位置处。例如，GNSS天线21、22也可以配置于在机械室3EG的后方配置的配重之上。

如图2所示，液压挖掘机100的液压系统300具备发动机35以及液压泵36、37。液压泵36、37被发动机35驱动而排出工作油。从液压泵36、37排出的工作油供给至动臂缸10、斗杆缸11以及铲斗缸12。另外，液压挖掘机100具备回转马达38。回转马达38为液压马达，其被从液压泵36、37排出的工作油驱动。回转马达38使上部回转体3回转。在图2中，图示出两个液压泵36、37，但液压泵也可以为一个。回转马达38不限定于液压马达，也可以是电动马达。

作业机械的控制系统即控制系统200包括：全局坐标运算装置23；作为用于检测角速度及加速度的状态检测装置的IMU(Inertial Measurement Unit：惯性计测装置)24；操作装置25；作为处理装置的装置控制器39；作为处理装置的显示控制器28；以及显示部29。操作装置25是用于操作图1所示的工作装置2、上部回转体3以及行驶装置5中的至少一个的装置。操作装置25为了驱动工作装置2等而是受理操作员进行的操作，并输出与操作量相应的先导液压。

操作装置25具有在操作员的左侧设置的左操作杆25L以及在操作员的右侧配置的右操作杆25R。左操作杆25L以及右操作杆25R的前后左右的动作对应于两个轴的动作。例如，右操作杆25R的前后方向上的操作对应于动臂6的操作。例如，右操作杆25R的左右方向上的操作对应于铲斗8的操作。例如，左操作杆25L的前后方向上的操作对应于斗杆7的操作。例如，左操作杆25L的左右方向上的操作对应于上部回转体3的回转。

在本实施方式中，操作装置25使用先导液压方式。基于动臂操作、铲斗操作、斗杆操作、回转操作以及行驶操作，而从液压泵36向操作装置25供给经由未图示的减压阀被减压为规定的先导压力的工作油。

根据右操作杆25R的前后方向上的操作，能够向先导油路450供给先导液压，从而受理操作员对动臂6进行的操作。右操作杆25R所具备的阀装置根据右操作杆25R的操作量而打开，从而向先导油路450供给工作油。另外，压力传感器66检测此时的先导油路450内的工作油的压力作为先导压。压力传感器66将检测到的先导压作为动臂操作信号MB而向装置控制器39发送。

在操作装置25与动臂缸10之间的先导油路450中，设置有压力传感器68、控制阀(以下适当称为介入阀)27C以及梭阀51。根据右操作杆25R的左右方向上的操作，能够向先导油路450供给先导液压，从而受理操作员对铲斗8进行的操作。右操作杆25R所具备的阀装置根据右操作杆25R的操作量而打开，从而向先导油路450供给工作油。另外，压力传感器66检测此时的先导油路450内的工作油的压力作为先导压。压力传感器66将检测到的先导压作为铲斗操作信号MT向装置控制器39发送。

根据左操作杆25L的前后方向上的操作，能够向先导油路450供给先导液压，从而受理操作员对斗杆7进行的操作。左操作杆25L所具备的阀装置根据左操作杆25L的操作量而打开，从而向先导油路450供给工作油。另外，压力传感器66检测此时的先导油路450内的工作油的压力作为先导压。压力传感器66将检测到的先导压作为斗杆操作信号MA向装置控制器39发送。

根据左操作杆25L的左右方向上的操作，能够向先导油路450供给先导液压，从而受理操作员对上部回转体3进行的回转操作。左操作杆25L所具备的阀装置根据左操作杆25L的操作量而打开，从而向先导油路450供给工作油。另外，压力传感器66检测此时的先导油路450内的工作油的压力作为先导压。压力传感器66将检测到的先导压作为回转操作信号MR向装置控制器39发送。

通过对右操作杆25R进行操作，操作装置25将与右操作杆25R的操作量对应的大小的先导液压供给至方向控制阀64。通过对左操作杆25L进行操作，操作装置25将与左操作杆25L的操作量对应的大小的先导液压供给至控制阀27。通过该先导液压，方向控制阀64的阀柱移动。

在先导油路450中设置有控制阀27。右操作杆25R以及左操作杆25L的操作量由设置于先导油路450的压力传感器66进行检测。压力传感器66检测到的先导液压输入至装置控制器39。装置控制器39将与输入的先导液压对应的先导油路450的控制信号N输出至控制阀27，从而对先导油路450进行开闭。右操作杆25R或左操作杆25L的操作方向与操作对象(铲斗8、斗杆7、动臂6、上部回转体3)之间的关系不限定于上述关系，也可以是不同的关系。

操作装置25具有行驶用杆25FL、25FR。在本实施方式中，操作装置25使用先导液压方式，因此，从液压泵36向方向控制阀64供给被减压后的工作油，并基于先导油路450内的工作油的压力来驱动方向控制阀64的阀柱。这样，从液压泵36、37向液压挖掘机100的行驶装置5所具备的液压马达5c、5c供给工作油，从而液压挖掘机100能够行驶。先导油路450内的工作油的压力即先导压由压力传感器27PC进行检测。

在液压挖掘机100的操作员使行驶装置5动作的情况下，操作员对行驶用杆25FL、25FR进行操作。操作员对行驶用杆25FL、25FR的操作量被压力传感器27PC检测到，并作为操作信号MD向装置控制器39输出。

左操作杆25L以及右操作杆25R的操作量例如也可以由电位计以及霍尔IC等进行检测，装置控制器39基于这些检测值来控制方向控制阀64以及控制阀27，由此控制工作装置2。这样，左操作杆25L以及右操作杆25R也可以是电动方式。

控制系统200具有第一行程传感器16、第二行程传感器17以及第三行程传感器18。例如，第一行程传感器16设置于动臂缸10，第二行程传感器17设置于斗杆缸11，第三行程传感器18设置于铲斗缸12。第一行程传感器16检测与动臂缸10的伸长对应的位移量，并输出至装置控制器39。第二行程传感器17检测与斗杆缸11的伸长对应的位移量，并输出至装置控制器39。第三行程传感器18检测与铲斗缸12的伸长对应的位移量，并输出至装置控制器39。

装置控制器39具有CPU(Central Processing Unit)等作为处理器的处理部39P与RAM(Random Access Memory)以及ROM(Read Only Memory)等作为存储装置的存储部39M。在装置控制器39中输入有全局坐标运算装置23的检测值、IMU24的检测值、压力传感器27PC、66、68的检测值、第一行程传感器16的检测值、第二行程传感器17的检测值以及第三行程传感器18的检测值。装置控制器39根据全局坐标运算装置23的检测值以及IMU24的检测值，求出与液压挖掘机100的位置相关联的位置信息IPL，并输出至显示控制器28。装置控制器39基于图2所示的压力传感器66的检测值来对控制阀27以及介入阀27C进行控制。

图2所示的方向控制阀64例如是比例控制阀，利用从操作装置25供给的工作油来控制。方向控制阀64配置在动臂缸10、斗杆缸11、铲斗缸12以及回转马达38等液压促动器与液压泵36、37之间。方向控制阀64控制从液压泵36、37向动臂缸10、斗杆缸11、铲斗缸12以及回转马达38供给的工作油的流量。

全局坐标运算装置23从图2所示的修正数据接收装置26接收修正数据C1。修正数据接收装置26与全局坐标运算装置23连接。修正数据C1是由在液压挖掘机100的外部设置的GNSS接收机生成的、可在RTK-GNSS中利用的信息，是从具有与修正数据接收装置26通用规格的通信功能的装置发送来的信息。另外，也可以将修正数据接收装置26设为电话线路用的调制解调器，利用修正数据发送服务而从外部得到修正数据C1。修正数据接收装置26将修正数据C1输出至全局坐标运算装置23。GNSS天线21、GNSS天线22从多个测位卫星接收信号，并输出至全局坐标运算装置23。

全局坐标运算装置23基于从GNSS天线21、GNSS天线22输入的测位卫星的信号和从修正数据接收装置26接收到的修正数据C1，对作为GNSS天线21的位置的基准位置数据P1和作为GNSS天线22的位置的基准位置数据P2进行测位。全局坐标运算装置23根据作为GNSS天线21的位置的基准位置数据P1与作为GNSS天线22的位置的基准位置数据P2之间的相对位置来计算出回转体方位数据Q。也可以通过GNSS天线21、22以及全局坐标运算装置23来构成GPS罗盘，从而得到回转体方位数据Q。

设置有GNSS天线21、22的部分是液压挖掘机100的一部分。因此，基准位置数据P1、P2是表示液压挖掘机100的一部分、具体而言设置有GNSS天线21、22的部分的位置的信息。以下，将设置有GNSS天线21、22的部分的位置适当称为第一位置。基准位置数据P1、P2是第一位置的信息。

在本实施方式中，回转体方位数据Q是根据GNSS天线21、22获取到的基准位置数据P、即基准位置数据P1以及基准位置数据P2中的至少一方而确定的方位相对于全局坐标的基准方位(例如北)而形成的角度，即方位角。方位角也是液压挖掘机100的偏转角。回转体方位数据Q表示上部回转体3即工作装置2所朝向的方位。

全局坐标运算装置23具有CPU等作为处理器的处理部与RAM和ROM等作为存储装置的存储部。全局坐标运算装置23将测出的两个基准位置数据P1、P2即基准位置数据P以及回转体方位数据Q输出至装置控制器39。

显示控制器28具有CPU等作为处理器的处理部28P与RAM和ROM等作为存储装置的存储部28M。显示控制器28在显示部29除了显示例如后述的引导画面等图像之外，还使用从装置控制器39得到的液压挖掘机100的位置信息IPL生成表示铲斗8的铲尖8T的三维位置即铲尖位置的铲斗铲尖位置数据S。显示部29例如是液晶显示装置等，但不限定于此。显示部29能够使用例如将输入部与显示部一体化而成的触摸面板。在本实施方式中，与显示部29邻接地设置有开关29S。开关29S是用于使后述的挖掘控制执行或使执行中的挖掘控制停止的输入装置。在显示部29使用触摸面板的情况下，开关29S也可以组入到触摸面板的输入部中。

显示控制器28能够将工作装置2所挖掘的对象的目标施工面的图像和使用铲斗铲尖位置数据S生成的铲斗8的图像作为引导画面而显示在显示部29中。显示控制器28使液压挖掘机100的操作员通过引导画面来识别目标施工面与铲斗8的位置关系，从而能够减轻进行信息化施工时的操作员的负担。

IMU24是对表示液压挖掘机100的动作的动作信息MI进行检测的状态检测装置。液压挖掘机100的动作包括上部回转体3的动作以及行驶装置5的动作中的至少一方。在本实施方式中，动作信息MI也可以包括表示液压挖掘机100的姿势的信息。表示液压挖掘机100的姿势的信息例示出液压挖掘机100的侧倾角、俯仰角以及方位角。

在本实施方式中，IMU24对液压挖掘机100的角速度以及加速度进行检测。伴随着液压挖掘机100的动作，在液压挖掘机100中产生行驶时产生的加速度、回转时产生的角加速度以及重力加速度这样的各种加速度，但IMU24对至少包含重力加速度在内的加速度进行检测，并且不区分各加速度的种类而输出检测到的加速度。重力加速度是与重力对应的加速度。在图1所示的车身坐标系(x，y，z)中，IMU24对x轴方向、y轴方向以及z轴方向的加速度a和绕x轴、y轴以及z轴的角速度(旋转角速度)ω进行检测。这些成为动作信息MI。车身坐标系是指以液压挖掘机100为基准的由(x，y，z)表示的三维坐标系。

在IMU24检测的动作信息MI中，包括上部回转体3以成为上部回转体3的旋转中心轴的z轴为中心而回转时的角速度ω。回转时的角速度ω也可以通过对从表示GNSS天线21、22的位置的信息中获取到的上部回转体3的回转角度以时间进行微分而求出。通过对回转时的角速度ω以时间进行积分，能够求出回转角度。

IMU24安装于上部回转体3。为了使IMU24以更高的精度检测加速度等，期望将IMU24设置在例如液压挖掘机100的上部回转体3的回转中心轴上，但IMU24也可以设置在驾驶室4的下部。

图3是液压挖掘机100的侧视图。图4是液压挖掘机100的后视图。车辆主体1的相对于左右方向、即宽度方向的倾斜角θ4为液压挖掘机100的侧倾角，车辆主体1的相对于前后方向的倾斜角θ5为液压挖掘机100的俯仰角，绕z轴的上部回转体3的角度为液压挖掘机100的方位角。侧倾角通过对由IMU24检测到的绕x轴的角速度以时间进行积分而求出，俯仰角通过对由IMU24检测到的绕y轴的角速度以时间进行积分而求出，方位角通过对由IMU24检测到的绕z轴的角速度以时间进行积分而求出。绕z轴的角速度是液压挖掘机100的回转时的角速度ω。即，通过对回转时的角速度ω以时间进行积分而得到液压挖掘机100、更具体而言上部回转体3的方位角。

IMU24以规定的周期对液压挖掘机100的加速度以及角速度进行更新。IMU24的更新周期优选比全局坐标运算装置23中的更新周期短。IMU24检测到的加速度以及角速度作为动作信息MI而输出至装置控制器39。装置控制器39对从IMU24获取到的动作信息MI实施滤波处理以及积分这样的处理，而求出作为侧倾角的倾斜角θ4、作为俯仰角的倾斜角θ5以及方位角。装置控制器39将求出的倾斜角θ4、倾斜角θ5以及方位角作为与液压挖掘机100的位置相关联的位置信息IPL而输出至显示控制器28。

显示控制器28从全局坐标运算装置23获取基准位置数据P以及回转体方位数据Q。回转体方位数据Q是表示液压挖掘机100的方位的信息，在本实施方式中，是表示上部回转体3的方位的信息。具体而言，回转体方位数据Q是上部回转体3的方位角。在本实施方式中，显示控制器28生成铲斗铲尖位置数据S来作为工作装置位置数据。铲斗铲尖位置数据S也可以通过装置控制器39来生成。然后，显示控制器28使用铲斗铲尖位置数据S和目标施工信息T，生成表示挖掘对象的目标形状的目标挖掘地形数据U。目标施工信息T存储在显示控制器28的存储部28M(目标施工信息储存部28C)中。目标施工信息T是成为液压挖掘机100所具备的工作装置2的挖掘对象的挖掘后的完成目标的信息，包括根据设计数据得到的目标施工面的信息。目标挖掘地形数据U是：在将车身坐标系中的通过铲尖8T的当前时刻下的铲尖位置的垂线与目标施工面的交点作为挖掘对象位置时，表示挖掘对象位置的前后的单个或多个拐点的位置的信息和其前后的线的角度信息。

装置控制器39根据第一行程传感器16检测到的动臂缸长，计算出车身坐标系中的动臂6相对于与水平面正交的方向(z轴方向)的倾斜角θ1(参照图3)。装置控制器39根据第二行程传感器17检测到的斗杆缸长，计算出斗杆7相对于动臂6的倾斜角θ2(参照图3)。装置控制器39根据第三行程传感器18检测到的铲斗缸长，计算出铲斗8相对于斗杆7的倾斜角θ3。IMU24将回转时的角速度ω输出至装置控制器39。

如上所述，装置控制器39从IMU24获取上部回转体3以图1所示的z轴为中心而回转时的上部回转体3的回转时的角速度ω。另外，装置控制器39从压力传感器66获取动臂操作信号MB、铲斗操作信号MT、斗杆操作信号MA以及回转操作信号MR。

装置控制器39从显示控制器28获取目标挖掘地形数据U。装置控制器39根据自身求出的工作装置2的角度(θ1，θ2，θ3)，求出铲斗8的铲尖8T的位置(以下适当称为铲尖位置)。装置控制器39的存储部39M存储工作装置2的数据(以下适当称为工作装置数据)。工作装置数据包括动臂6的长度L1、斗杆7的长度L2以及铲斗8的长度L3这样的设计尺寸。如图3所示，动臂6的长度L1相当于从动臂销13到斗杆销14的长度。斗杆7的长度L2相当于从斗杆销14到铲斗销15的长度。铲斗8的长度L3相当于从铲斗销15到铲斗8的铲尖8T的长度。铲尖8T为图1所示的斗齿8B的前端。另外，工作装置数据包括相对于车身坐标系的位置PL的动臂销13的位置信息。装置控制器39能够使用长度L1、L2、L3、倾斜角θ1、θ2、θ3以及位置PL来求出相对于位置PL的铲尖位置。

装置控制器39基于目标挖掘地形数据U与铲斗8的铲尖8T之间的距离以及铲斗8的铲尖8T的速度，来调整从操作装置25输入的动臂操作信号MB、铲斗操作信号MT以及斗杆操作信号MA，以使得铲斗8的铲尖8T沿着目标挖掘地形数据U移动。装置控制器39以使铲斗8的铲尖8T沿着目标挖掘地形数据U移动的方式生成用于控制工作装置2的控制信号N，并输出至图2所示的控制阀27。通过这样的处理，根据相对于目标挖掘地形数据U的距离来限制工作装置2向目标挖掘地形数据U接近的速度。

根据来自装置控制器39的控制信号N，在动臂缸10、斗杆缸11以及铲斗缸12分别设置了两个的控制阀27进行开闭。基于左操作杆25L或右操作杆25R的操作和控制阀27的开闭指令，方向控制阀64的阀柱进行动作，从而向动臂缸10、斗杆缸11以及铲斗缸12供给工作油。

全局坐标运算装置23对全局坐标系中的GNSS天线21、22的基准位置数据P1、P2进行检测。在本实施方式中，全局坐标系是指例如GNSS中的坐标系。在图3中，全局坐标系是由(Xg，Yg，Zg)表示的三维坐标系。现场坐标系是以成为设置于液压挖掘机100的作业区域GA的基准的例如基准桩60的位置PG为基准的、由(X，Y，Z)表示的三维坐标系。如图3所示，位置PG例如位于设置于作业区域GA的基准桩60的前端60T。全局坐标系(Xg，Yg，Zg)和现场坐标系(X，Y，Z)相互能够转换。

图2所示的显示控制器28基于全局坐标运算装置23的检测结果，求出全局坐标系中的车身坐标系的位置。在本实施方式中，例如，车身坐标系的位置PL是作为回转体的旋转中心轴的z轴与和行驶装置5接地的面相当的面的交点。在本实施方式中，位置PL的坐标在车身坐标系中为(0，0，0)。行驶装置5接地的面是履带5a、5b接触的作业区域GA的表面GD。与行驶装置5接地的面相当的面可以是作业区域GA的表面GD，也可以是由履带5a、5b接地的部分规定的平面CP。由履带5a、5b接地的部分规定的平面CP在车身坐标系(x，y，z)中根据液压挖掘机100的设计尺寸唯一地确定。

位置PL不限定于z轴与平面CP的交点。在本实施方式中，后述的伪不动点的位置可以与位置PL一致，也可以与位置PL不一致。车身坐标系的位置PL也可以在其他的位置，例如可以将动臂销13的轴向长度的中心点作为位置PL。位置PL也可以位于z轴上且为用于供上部回转体3回转的摆动圆上。如上所述，装置控制器39求出相对于位置PL的铲尖位置、即车身坐标系中的铲尖位置，因此，若得到全局坐标系中的位置PL的坐标，则能够将车身坐标系中的铲尖位置的坐标转换成全局坐标系中的铲尖位置的坐标。

为了抑制铲斗8侵占目标挖掘地形，装置控制器39以使工作装置2向挖掘对象接近的方向上的速度成为限制速度以下的方式进行控制。将该控制适当称为挖掘控制。挖掘控制是如下的控制：基于从显示控制器28获取到的目标挖掘地形数据U以及铲斗铲尖位置数据S，来运算工作装置2与挖掘对象的相对位置，并使工作装置2向挖掘对象接近的方向上的速度成为限制速度以下。通过执行这样的控制，能够将挖掘对象施工成目标形状(目标施工信息T表示的形状)。接着，对控制系统200更详细地进行说明。

〈控制系统200〉

图5是实施方式1所涉及的控制系统200的控制框图。在本实施方式中，控制系统200的装置控制器39与显示控制器28能够经由信号线而相互交换信息。另外，装置控制器39能够经由信号线从全局坐标运算装置23获取信息。在控制系统200内传递信息的信号线例示出CAN(Controller Area Network)这样的车内信号线。在本实施方式中，控制系统200中的装置控制器39和显示控制器28是分别独立的装置，但两者也可以由一个装置来实现。

显示控制器28具有铲尖位置计算部28A、目标挖掘地形数据生成部28B以及目标施工信息储存部28C。铲尖位置计算部28A以及目标挖掘地形数据生成部28B通过处理部28P执行存储于存储部28M的计算机程序来实现。目标施工信息储存部28C通过存储部28M的存储区域的一部分来实现。

铲尖位置计算部28A基于从装置控制器39获取的位置信息IPL，生成表示通过成为上部回转体3的回转中心轴的z轴的、液压挖掘机100的回转中心的位置的回转中心位置数据XR。铲尖位置计算部28A从装置控制器39获取的位置信息IPL包括基于基准位置数据P1、P2的基准位置数据P1c、P2c以及液压挖掘机100的姿势角。姿势角是侧倾角θ4、俯仰角θ5以及方位角θdc。

铲尖位置计算部28A基于回转中心位置数据XR、工作装置2的倾斜角θ1、θ2、θ3、动臂6的长度L1、斗杆7的长度L2以及铲斗8的长度L3，生成表示铲斗8的铲尖8T的当前位置的铲斗铲尖位置数据S，并输出至目标挖掘地形数据生成部28B。铲斗铲尖位置数据S是表示工作装置2的位置的信息。在本实施方式中，工作装置2的位置不限定于铲尖位置、即铲斗8的铲尖8T的三维位置，只要是工作装置2的特定部分的位置即可。例如，工作装置2的位置可以是铲斗8的尾部的位置，也可以是法面铲斗的底的部分的位置，还可以是安装工作装置2的附件的部分的位置。

目标挖掘地形数据生成部28B获取储存于目标施工信息储存部28C的目标施工信息T和来自铲尖位置计算部28A的铲斗铲尖位置数据S。目标挖掘地形数据生成部28B将车身坐标系中的通过铲尖8T的当前时刻下的铲尖位置的垂线与目标施工面的交点设定为挖掘对象位置。目标挖掘地形数据生成部28B基于目标施工信息T与铲斗铲尖位置数据S，生成目标挖掘地形数据U，并输出至后述的装置控制器39的处理部39P所具有的工作装置控制部39C。

装置控制器39的处理部39P具有姿势角运算部39A、位置信息运算部39B以及工作装置控制部39C。姿势角运算部39A、位置信息运算部39B以及工作装置控制部39C通过处理部39P执行存储于存储部39M的计算机程序来实现。在本实施方式中，工作装置控制部39C也可以是与装置控制器39独立的控制装置。

在姿势角运算部39A中输入作为IMU24的检测值的加速度a(ax，ay，az)和角速度ω(ωx，ωy，ωz)即动作信息MI以及作为全局坐标运算装置23的检测值的回转体方位数据Q(方位角θda)。另外，在处理部39P的姿势角运算部39A以及位置信息运算部39B中输入压力传感器66、27PC的检测值STr、STd。

全局坐标运算装置23生成表示电波的接收状态或与装置控制器39的通信状态的信息、即状态信息SR，并输出至装置控制器39的处理部39P以及显示控制器28的处理部28P。状态信息SR具有在全局坐标运算装置23无法接收电波的情况下、在电波的接收状态下降的情况下或者在全局坐标运算装置23与装置控制器39的通信产生不良的情况下等表示各个情况下的接收状态或通信状态的信息。表示接收状态或通信状态的信息表示全局坐标运算装置23进行的测位的状态。对于测位的状态而言，如上所述举出测位的精度好的状态(Fix)、不能够测位的状态(非测位)、能够测位但信息较少的状态以及测位的精度差的状态(Float，单独测位)等。这样，全局坐标运算装置23是判定基于RTK-GNSS的测位是否产生了不良的测位状态判断装置。

在本实施方式中，测位状态判断装置、即全局坐标运算装置23将测位的精度好的状态(Fix)判定为测位的状态正常。另外，全局坐标运算装置23将不能够测位的状态(非测位)、能够测位但信息较少的状态以及测位的精度差的状态(Float，单独测位)判定为测位的状态异常。即，全局坐标运算装置23将测位的状态为Fix的情况判定为正常，将Fix以外的状态的情况判定为异常。

显示控制器28在获取到状态信息SR时将与测位的状态相应的信息显示在图2所示的显示部29中。在状态信息SR表示不能够测位的状态的情况下，显示控制器28使图2所示的显示部29显示基于RTK-GNSS的测位产生了异常。

姿势角运算部39A根据IMU24的检测值求出作为液压挖掘机100的侧倾角的倾斜角θ4以及作为液压挖掘机100的俯仰角的倾斜角θ5，并输出至位置信息运算部39B以及显示控制器28的铲尖位置计算部28A。姿势角运算部39A能够对IMU24检测到的绕z轴的角速度ω进行积分而求出方位角θdi。侧倾角θ4、俯仰角θ5以及方位角θdi是姿势角。

姿势角运算部39A根据作为位置检测装置的全局坐标运算装置23的状态，切换自身求出的方位角θdi或者从全局坐标运算装置23获取到的方位角θda，并作为方位角θdc输出至显示控制器28的铲尖位置计算部28A或者位置信息运算部39B。即，在基于RTK-GNSS的测位为正常的情况下，使用从全局坐标运算装置23获取到的方位角θda来求出铲斗铲尖位置数据S，在基于RTK-GNSS的测位为异常的情况下，使用通过对IMU24检测到的绕z轴的角速度ω进行积分而求出的方位角θdi来求出铲斗铲尖位置数据S。另外，从姿势角运算部39A送至显示控制器28的倾斜角θ4、倾斜角θ5以及方位角θdc是与液压挖掘机100的位置相关联的位置信息IPL。以下，将倾斜角θ4适当称为侧倾角θ4，将倾斜角θ5适当称为俯仰角θ5。

在本实施方式中，如上所述，位置信息IPL是与作为作业机械的液压挖掘机100的位置相关联的信息。位置信息IPL除了包括液压挖掘机100的位置本身的信息之外，还包括求出液压挖掘机100的位置所需的信息。液压挖掘机100的位置本身的信息例示出基准位置数据P1、P2以及铲斗铲尖位置数据S，求出液压挖掘机100的位置所需的信息例示出倾斜角θ4、倾斜角θ5以及方位角(θda、θdi或θdc)。

位置信息运算部39B使用从全局坐标运算装置23获取到的基准位置数据P1、P2和从IMU24获取到的动作信息MI，求出与基准位置数据P1、P2表示的位置相当的位置。基准位置数据P1、P2是第一位置的信息。以下，将位置信息运算部39B根据基准位置数据P1、P2以及动作信息MI求出的位置适当称为第二位置。第二位置的信息是基准位置数据P1i、P2i。基准位置数据P1i、P2i由位置信息运算部39B生成。以下，将全局坐标运算装置23输出的基准位置数据P1、P2适当称为第一基准位置数据P1、P2，将由位置信息运算部39B生成的基准位置数据P1i、P2i适当称为第二基准位置数据P1i、P2i。

第二基准位置数据P1i、P2i通过由姿势角运算部39A根据作为IMU24的检测值的加速度a(ax，ay，az)和角速度ω(ωx，ωy，ωz)求出的侧倾角θ4和俯仰角θ5以及从姿势角运算部39A输出的方位角θdc求出。该方位角θdc是姿势角运算部39A从全局坐标运算装置23获取到的方位角θda或将方位角θda与通过回转时的角速度ω的积分而获得的角度相加后得到的方位角。在该情况下，全局坐标运算装置23根据第一基准位置数据P1、P2求出方位角θda，并输出至姿势角运算部39A。在回转时的角速度ω为0的情况下，从姿势角运算部39A输出的方位角θdc与从全局坐标运算装置23获取到的方位角θda相等。这样，位置信息运算部39B使用第一基准位置数据P1、P2以及动作信息MI来生成第二基准位置数据P1i、P2i。在本实施方式中，姿势角运算部39A也可以从全局坐标运算装置23获取第一基准位置数据P1、P2，并使用第一基准位置数据P1、P2来求出方位角θda。

由于测位卫星的配置、电离层、对流层或者GNSS天线周边的地形的影响，全局坐标运算装置23的测位结果有时产生偏差。当测位结果产生偏差时，在挖掘控制中，可能产生施工面起伏而无法按照设计面进行施工。另外，当测位结果产生偏差时，可能产生引导画面显示的铲斗8的铲尖摇晃而操作员的视觉确认性下降。在图2所示的操作装置25为先导压方式的情况下，还有可能发生在左操作杆25L或右操作杆25R产生油击而使操作员感觉到不适感。

考虑通过对从全局坐标运算装置23输出的第一基准位置数据P1、P2实施低通滤波处理或移动平均这样的平滑化处理，来降低由于全局坐标运算装置23的测位结果的偏差而造成的影响。在液压挖掘机100中，通过挖掘时的姿势角的变动，GNSS天线21、22的位置也变动。因此，当对第一基准位置数据P1、P2直接进行平滑化处理时，在平滑化处理后的GNSS天线21、22的位置的计算中，产生由于进行平滑化处理所引起的时间延迟。其结果是，可能导致平滑化处理后的GNSS天线21、22的位置与实际的GNSS天线21、22的位置不同。

在液压挖掘机100为了进行作业而运转的时间中，在液压挖掘机100中存在绝对位置不变化的不动点，若知道GNSS天线21、22的位置与不动点的相对关系，则控制系统200的装置控制器39能够根据GNSS天线21、22的位置来计算出不动点的绝对位置。装置控制器39对不动点的绝对位置应用平滑化处理，从而得到偏差降低了的不动点。若装置控制器39根据应用了平滑化处理后的不动点的绝对位置计算出GNSS天线21、22的位置，则能够不受平滑化处理的时间延迟的影响而降低基于RTK-GNSS的测位的偏差。

实际上，只要液压挖掘机100的发动机35运转，就会由于工作装置2的动作等而产生振动，因此将被看作不动点的近似的位置选定为伪不动点。控制系统200的装置控制器39通过对选定的伪不动点与上述的不动点同样地进行处理，从而能够使用伪不动点来对基于RTK-GNSS的测位进行备份。能够将伪不动点看作不动点的情况为，液压挖掘机100不移动的情况、即图1所示的履带5a、5b不移动的情况。

在本实施方式中，控制系统200、更具体而言控制系统200的装置控制器39对上述的伪不动点实施平滑化处理，并使用实施平滑化处理后的伪不动点来求出GNSS天线21、22的位置、即第二位置。如后所述，伪不动点在液压挖掘机100为了进行作业而运转的时间中，能够看作绝对位置不随时间的经过而变化，因此能够忽略平滑化处理所造成的延迟的影响。其结果是，装置控制器39能够使平滑化处理后的GNSS天线21、22的位置与实际的GNSS天线21、22的位置一致。这样，通过装置控制器39对伪不动点实施平滑化处理，能够降低由于全局坐标运算装置23的测位结果的偏差而造成的影响。其结果是，装置控制器39能够抑制使用了挖掘控制的施工时的精度下降以及引导画面的视觉确认性的下降。

位置信息运算部39B获取对图2所示的左操作杆25L的先导压进行检测的压力传感器66的检测值STr(对应于上述的回转操作信号MR)以及对行驶用杆25FL和行驶用杆25FR的先导压进行检测的压力传感器27PC的检测值STd(对应于上述的操作信号MD)。位置信息运算部39B基于获取到的检测值STr、Std，进行以是否执行平滑化处理为代表的各种判定。

接着，对伪不动点进行说明。在本实施方式中，伪不动点为图3以及图4所示的液压挖掘机100的位置PL。需要说明的是，虽然如上所述那样将位置PL决定为车身坐标系的原点，但也可以将车身坐标系的原点决定在其他的位置。因此，在以下的说明中，伪不动点有时也称为特定点。在作业中的液压挖掘机100中存在发生旋转(以下有时也称为回转)那样的移动的情况下，由于其旋转的支点不动，因此，若伪不动点位于该支点，则由控制系统200求出的位置、例如包括工作装置2的特定部分的位置或者铲斗8的铲尖8T的位置在内的工作装置2的位置的误差最小。即便在无法将伪不动点设为旋转的支点的情况下，若将伪不动点尽量设定在支点的附近，则也能够减小由控制系统200求出的位置(工作装置2的位置)的误差。

上部回转体3回转时的支点为回转中心轴、即z轴，因此将伪不动点设在z轴上。考虑虽然侧倾角θ4的方向以及俯仰角θ5的方向上的旋转的支点不是固定点，但一定位于液压挖掘机100接地的面上。在本实施方式中，如上所述，位置PL是作为回转体的旋转中心轴的z轴与和行驶装置5接地的面相当的面的交点。在本实施方式中，通过将伪不动点设在液压挖掘机100接地的面上，从而即便在作业中的液压挖掘机100中存在发生旋转那样的移动的情况下，也认为伪不动点不动。因此，在产生了基于RTK-GNSS的测位的偏差时，能够降低由控制系统200求出的位置、更具体而言GNSS天线21、22的绝对位置的偏差。

液压挖掘机100能够进行各种作业，作为如上述那样即便液压挖掘机100发生旋转也认为伪不动点不动的情况，例如有法面施工等的作业。在该情况下，在行驶装置5停止的状态下，有时仅通过工作装置2或上部回转体3的操作来进行挖掘或平整。在使用能够进行信息化施工的液压挖掘机100来进行这种法面施工等施工的情况下，控制系统200使用伪不动点以及基于RTK-GNSS的测位结果来求出液压挖掘机100的第二位置、具体而言GNSS天线21、22的位置。这样一来，控制系统200能够抑制挖掘控制的精度下降以及引导画面的视觉确认性的下降。

〈伪不动点的求解方法〉

说明液压挖掘机100的控制系统200、具体而言装置控制器39根据GNSS天线21、22的绝对位置求出伪不动点的方法以及根据伪不动点重新求出GNSS天线21、22的绝对位置的方法。

式(1)是将车身坐标系中的位置PL的位置向量与GNSS天线21、22的位置向量的差分转换成图3所示的现场坐标系(X，Y，Z)中的位置PL的位置向量与GNSS天线21、22的位置向量的差分的式子。式(2)是根据现场坐标系中的GNSS天线21、22的位置向量的测定值Ral来计算现场坐标系中的位置PL的位置向量Rfl的式子，是式(1)的变形。式(3)是表示根据现场坐标系中的位置PL的位置向量Rfl求解现场坐标系中的GNSS天线21、22的位置向量的测定值Ral的式子将其作为求解现场坐标系中的GNSS天线21、22的位置向量的计算值Ralc的式子。

Rfl-Ral＝Clb(Rfb-Rab)…(1)

Rfl＝Clb(Rfb-Rab)+Ral…(2)

Ralc＝Clb(Rab-Rfb)+Rfl…(3)

在此，

Rfb：车身坐标系中的位置PL的位置向量的校正值；

Rab：车身坐标系中的GNSS天线21、22的位置向量的校正值；

Rfl：现场坐标系中的位置PL的位置向量的计算值；

Ral：现场坐标系中的GNSS天线21、22的位置向量的测定值；

Ralc：现场坐标系中的GNSS天线21、22的位置向量的计算值；

Clb：从车身坐标系向现场坐标系的坐标旋转矩阵；

校正值是指通过对液压挖掘机100的各位置以及尺寸进行计测而得到的位置PL以及GNSS天线21、22的位置的值，其存储于装置控制器39的存储部39M以及显示控制器28的存储部28M中的至少一方。校正值可以基于液压挖掘机100的设计尺寸而得到，但设计尺寸有可能根据每个液压挖掘机100而产生偏差。因此，校正值优选基于计测(校准)而得到。

坐标旋转矩阵Clb使用侧倾角θ4、俯仰角θ5以及偏转角、即方位角θd如式(4)那样来表现。侧倾角θ4、俯仰角θ5以及方位角θd通过姿势角运算部39A对由IMU24检测到的绕x轴的角速度ωx、绕y轴的角速度ωy以及绕z轴的角速度ωz以时间进行积分而求出。式(4)中的sx为sinθ4，sy为sinθ5，sz为sinθd，cx为cosθ4，cy为cosθ5，cz为cosθd。

[数式1]

控制系统200通过使用式(2)，能够求出作为伪不动点的特定点(本实施方式中为位置PL)的位置。另外，控制系统200通过使用式(3)，能够使用作为伪不动点的特定点的位置来求出GNSS天线21、22的绝对位置、即现场坐标系中的位置或者全局坐标系中的位置。控制系统200通过使用式(2)以及式(3)，能够求出GNSS天线21、22的绝对位置。

〈平滑化处理〉

在本实施方式中，装置控制器39对作为伪不动点的特定点的位置实施平滑化处理。在本实施方式中，在平滑化处理中例如使用式(5)所示的低通滤波器。

Rft＝{(M-1)×Rftpr+Rfl}/M…(5)

式(5)中的Rft是本次的控制周期中的低通滤波器的输出，Rftpr是上次的控制周期中的低通滤波器(以下适当称为滤波器)的输出。这些均为特定点的位置向量。M是平均化常数。在本实施方式中，平均化常数M的初始值为1，每一次控制周期结束M都增加1，直至M的值达到设定值Mmax为止。

在本实施方式中，装置控制器39在开始平滑化处理时，使上次的控制周期中的滤波器的输出Rftpr暂时存储于存储部39M。存储部39M存储上次的控制周期中的滤波器的输出Rftpr，直至下一个控制周期的滤波处置被执行或者执行中的平滑化处理被复位。

装置控制器39使用式(2)，求出表示特定点的位置的位置向量Rfl，并将得到的位置向量Rfl赋给式(5)。装置控制器39通过式(5)，按照控制的每一个周期对特定点的位置向量Rfl实施平滑化处理，具体而言实施低通滤波处理。装置控制器39在低通滤波处理之后，将本次的控制周期中的低通滤波器的输出Rft作为平滑化处理后的特定点的位置向量而输出。以下将平滑化处理后的特定点的位置向量适当称为位置向量Rft。位置向量Rft是第二基准位置数据P1i、P2i。第二基准位置数据P1i、P2i是通过平滑化处理而得到的信息。这样，装置控制器39通过实现式(5)所示的低通滤波器的功能，从而对特定点的位置实施平滑化处理，并使用平滑化处理后的特定点的位置来求出第二位置。

装置控制器39在初次的平滑化处理或者对平滑化处理进行了复位后，将上次的控制周期中的滤波器的输出Rftpr作为式(5)的Rfl，将平均化常数M设为1。初次的平滑化处理是指，在装置控制器39开始平滑化处理时不具有上次的控制周期中的滤波器的输出Rftpr的情况。

装置控制器39在由于某种原因而中断了平滑化处理的情况下，不输出本次的控制周期中的滤波器的输出Rft，而保持上次的控制周期中的滤波器的输出Rftpr以及上次的控制周期中的平均化常数M。在该情况下，装置控制器39使存储部39M除了上次的控制周期中的滤波器的输出Rftpr之外还暂时存储上次的控制周期中的平均化常数M。装置控制器39在再次开始中断了的平滑化处理的情况下，将当前的控制周期中的特定点的位置向量Rfl与存储在存储部39M中的中断前的滤波器的输出Rftpr及平均化常数M赋给式(5)。通过该处理，装置控制器39使特定点的位置向量Rfl变得平滑。

为了避免全局坐标运算装置23的测位结果的异常值，装置控制器39在开始了平滑化处理的情况下(除了开始初次的平滑化处理的情况)或者从平滑化处理的中断恢复的情况下，执行判定处理。当执行判定处理时，装置控制器39使用式(6)来求出差分ΔRfl。在式(6)中，Rfl是本次的控制周期中的特定点的位置向量，Rftpr是在平滑化处理的开始前或者从平滑化处理的中断恢复即再次开始的时刻存储在存储部39M中的滤波器的输出。

ΔRfl＝|Rfl-Rftpr|…(6)

在判定处理中，若差分ΔRfl小于预先确定的阈值，则装置控制器39将本次的控制周期中的特定点的位置向量Rfl判定为正常，并使用本次的控制周期中的特定点的位置向量Rfl来执行平滑化处理。若差分ΔRfl为预先确定的阈值以上，则装置控制器39将本次的控制周期中的特定点的位置向量Rfl判定为异常。这样，在判定为异常的情况下，装置控制器39代替本次的控制周期中的特定点的位置向量Rfl而使用存储在存储部39M中的滤波器的输出Rftpr，求出式(5)的滤波器的输出Rft。通过这样的处理，在全局坐标运算装置23的测位结果产生了异常值的情况下，装置控制器39能够抑制由于异常值引起的铲斗8的铲尖位置的变动。在差分ΔRfl成为预先确定的阈值以上的状态持续了预先确定的设定值Nt秒的情况下，装置控制器39执行超时处理。具体而言，装置控制器39对平滑化处理进行复位。

全局坐标运算装置23的测位结果产生异常值的情况包括产生如下情况中的任一种的情况或者同时产生了多种的情况，这些情况为，全局坐标运算装置23所输出的第一基准位置数据P1、P2的坐标值显示异常值的情况、在全局坐标运算装置23与装置控制器39之间产生通信错误的情况以及基于RTK-GNSS的测位产生了不良的情况。由于GNSS天线21、22无法接收来自测位卫星的电波或者难以接收电波，因此基于RTK-GNSS的测位产生不良。

在本实施方式中，低通滤波器不限定于式(5)所示的内容。平滑化处理不限定于低通滤波处理，例如也可以为对特定点的位置进行移动平均的处理。

在本实施方式中，装置控制器39在基于RTK-GNSS的测位为正常的情况下执行平滑化处理。当装置控制器39对特定点实施平滑化处理时，装置控制器39所具有的姿势角运算部39A求出侧倾角θ4、俯仰角θ5以及方位角θdc，并输出至位置信息运算部39B以及显示控制器28的铲尖位置计算部28A。全局坐标运算装置23根据接收到的基准位置数据P1、P2而得到方位角θdc、即回转体方位数据Q，因此能够求出关于车身坐标系求出的工作装置2的位置来作为现场坐标系中的工作装置2的位置。

图6是表示液压挖掘机的姿势的俯视图。由姿势角运算部39A求出的方位角θdc表示作为上部回转体3的前后轴的x轴相对于现场坐标系(X，Y，Z)的Y轴的斜度。通过方位角θdc决定液压挖掘机100的方位D1。

位置信息运算部39B根据由姿势角运算部39A求出的侧倾角θ4、俯仰角θ5以及方位角θdc来求出坐标旋转矩阵Clb。在该情况下，位置信息运算部39B将由姿势角运算部39A求出的方位角θdc赋给式(4)的θd，求出坐标旋转矩阵Clb。另外，位置信息运算部39B根据在基于RTK-GNSS的测位为正常的状态下从全局坐标运算装置23获取到的基准位置数据P1、P2来求出现场坐标系中的GNSS天线21、22的位置向量的测定值Ral。然后，位置信息运算部39B将求出的坐标旋转矩阵Clb和位置向量的测定值Ral赋给式(2)，求出现场坐标系中的位置PL的位置向量Rfl。位置向量Rfl为计算值。

在得到位置向量Rfl之后，位置信息运算部39B通过将位置向量Rfl赋给式(5)，从而对位置向量Rfl实施平滑化处理。位置信息运算部39B将平滑化处理后的位置向量Rfl、即低通滤波器的输出Rft赋给式(3)的Rfl，而求出现场坐标系中的GNSS天线21、22的位置向量、即第二基准位置数据P1i、P2i。现场坐标系中的GNSS天线21、22的位置向量是式(3)所示的计算值Ralc。位置信息运算部39B将第二基准位置数据P1i、P2i作为基准位置数据P1c、P2c而输出至显示控制器28的铲尖位置计算部28A。

接着，对装置控制器39的处理部39P所具有的工作装置控制部39C进行说明。工作装置控制部39C基于从显示控制器28获取到的目标挖掘地形数据U，生成用于控制工作装置2向目标挖掘地形数据U接近的速度的控制信号N。工作装置控制部39C将生成的控制信号N给予控制阀27，通过使控制阀27开闭，来控制工作装置2向目标挖掘地形数据U接近的速度。

图7是表示实施方式1所涉及的装置控制器39所具有的位置信息运算部39B的图。位置信息运算部39B包括判定部40A、特定点运算部40B、平滑化处理部40C以及位置计算部40D。判定部40A判定装置控制器39执行平滑化处理还是使平滑化处理停止、是否中断执行中的平滑化、是否再次开始中断中的平滑化处理以及是否对平滑化处理进行复位。这些判定基于压力传感器66的检测值STr以及压力传感器27PC的检测值STd而进行。

特定点运算部40B使用式(2)求出特定点的位置向量Rfl。平滑化处理部40C使用式(5)，对由特定点运算部40B求出的特定点的位置向量Rfl实施平滑化处理。位置计算部40D将平滑化处理后的位置向量Rft赋给式(3)的Rfl而求出第二基准位置数据P1i、P2i，并将第二基准位置数据P1i、P2i作为基准位置数据P1c、P2c而输出至显示控制器28。接着，对本实施方式所涉及的控制系统200执行平滑化处理而求出铲斗8的铲尖位置的处理的一例进行说明。

〈控制系统200的处理的一例〉

图8是表示实施方式1所涉及的控制系统200的处理的一例的流程图。在步骤S101中，控制系统200的装置控制器39所具有的位置信息运算部39B的判定部40A判定装置控制器39执行平滑化处理所需的执行条件是否成立。执行条件在基于RTK-GNSS的测位正常、且液压挖掘机100未行驶、且上部回转体3未回转的情况下成立。

在开始条件成立的情况下(步骤S101，是)，装置控制器39在步骤S102中求出特定点。具体而言，装置控制器39所具有的位置信息运算部39B的特定点运算部40B求出特定点、具体而言特定点的位置向量Rfl。在步骤S103中，装置控制器39对由特定点运算部40B求出的特定点的位置向量Rfl实施平滑化处理。在步骤S104中，装置控制器39所具有的位置信息运算部39B的位置计算部40D使用平滑化处理后的位置向量Rfl即位置向量Rft，求出第二基准位置数据P1i、P2i。然后，位置计算部40D将求出的第二基准位置数据P1i、P2i作为基准位置数据P1c，P2c而输出至显示控制器28。

在步骤S105中，显示控制器28的处理部28P使用从装置控制器39获取到的液压挖掘机100的位置信息IPL，求出铲斗8的铲尖8T的三维位置即铲尖位置。具体而言，处理部28P生成表示铲尖位置的铲斗铲尖位置数据S。位置信息IPL是基准位置数据P1c、P2c、侧倾角θ4、俯仰角θ5以及方位角θdc。接着，返回到步骤S101进行说明。在步骤S101中，在开始条件不成立的情况下(步骤S101，否)，装置控制器39结束处理。即，判定部40A在基于RTK-GNSS的测位正常、液压挖掘机100未行驶以及上部回转体3未回转这些情况中的任一种不成立的情况下，判定为执行条件不成立。接着，对平滑化处理的状态的转变进行说明。

〈平滑化处理的状态的转变〉

图9是用于说明平滑化处理的状态的转变的图。在本实施方式中，平滑化处理在状态1(接通，执行平滑化处理)、状态2(断开，停止平滑化处理)、状态3(中断，中断执行中的平滑化处理)以及状态4(复位，平滑化处理的复位)这四个状态之间转变。

在基于RTK-GNSS的测位正常、且液压挖掘机100的行驶停止(未行驶)、且上部回转体3未回转的情况下，装置控制器39使平滑化处理的状态转变至状态1。即，状态1是上述的执行条件成立的情况下的平滑化处理的状态。在状态1中，装置控制器39使用作为第二位置的信息的第二基准位置数据P1i、P2i求出铲尖位置。

在执行条件不成立的情况下，更具体而言在液压挖掘机行驶的情况下，装置控制器39使平滑化处理从状态1转变至状态2(I)。即，装置控制器39在液压挖掘机100行驶时停止求出第二位置的处理，即停止平滑化处理。在状态2中，在基于RTK-GNSS的测位正常、且液压挖掘机100未行驶、且上部回转体3未回转的情况下，装置控制器39使平滑化处理从状态2转变至状态1(I)。

在状态1中，在执行条件不成立的情况下，更具体而言在基于RTK-GNSS的测位成为了异常的情况以及上部回转体3回转的情况中的至少一方成立时，装置控制器39使平滑化处理从状态1转变至状态3(II)。在状态3中，装置控制器39将求出作为第二位置的信息的第二基准位置数据P1i、P2i的处理、即平滑化处理中断。在状态3中，在基于RTK-GNSS的测位正常、且液压挖掘机100未行驶、且上部回转体3停止即未回转的情况下，装置控制器39使平滑化处理从状态3转变至状态1(II)。在该情况下，装置控制器39再次开始中断了的平滑化处理。在再次开始中断了的平滑化处理的情况下，装置控制器39使用在中断平滑化处理前得到的第二基准位置数据P1i、P2i来求出铲尖位置。

在执行避免全局坐标运算装置23的测位结果的异常值的处理或者超时处理的情况下，装置控制器39使平滑化处理从状态1转变至状态4(III)。在状态4中，在基于RTK-GNSS的测位正常、且液压挖掘机100未行驶、且上部回转体3未回转、且平滑化处理的复位完成的情况下，装置控制器39使平滑化处理从状态4转变至状态1(III)。

在平滑化处理的中断中液压挖掘机100开始行驶的情况下，装置控制器39使平滑化处理从状态3转变至状态2(IV)。即，装置控制器39将求出第二位置的信息的处理即平滑化处理停止。在状态4中，在液压挖掘机100行驶、或上部回转体3回转、或基于RTK-GNSS的测位异常的情况下，装置控制器39使平滑化处理从状态4转变至状态2(V)。

〈液压挖掘机100的状态以及基于RTK-GNSS的测位的状态的判定〉

当位置信息运算部39B使平滑化处理的状态转变时，图7所示的位置信息运算部39B的判定部40A判定液压挖掘机100的状态以及基于RTK-GNSS的测位的状态。在压力传感器27PC检测到行驶用杆25FL以及行驶用杆25FR中的至少一方的先导压的情况下，判定部40A判定为液压挖掘机100正在行驶。在作为用于对上部回转体3进行回转操作的操作杆的左操作杆25L被向左右任一个方向操作、压力传感器66检测到先导压的情况下，判定部40A判定为上部回转体3正在回转。在全局坐标运算装置23所生成的状态信息SR表示基于RTK-GNSS的测位的状态异常的情况下，判定部40A判定为测位的状态异常。

〈使平滑化处理的状态转变的处理〉

图10是装置控制器39使平滑化处理的状态转变的处理的流程图，尤其是示出与平滑化处理的中断相关的处理。在步骤S201中，在装置控制器39执行平滑化处理的情况下，装置控制器39所具有的位置信息运算部39B的判定部40A判定中断平滑化处理的条件是否成立。中断平滑化处理的条件是基于RTK-GNSS的测位成为了异常的情况以及上部回转体3正在回转的情况中的至少一方成立的情况。在判定部40A判定为中断平滑化处理的条件成立的情况下(步骤S201，是)，在步骤S202中，装置控制器39的位置信息运算部39B将平滑化处理中断(II)。

在步骤S203中，判定部40A判定液压挖掘机100是否正在行驶。在判定部40A判定为液压挖掘机100正在行驶的情况下(步骤S203，是)，在步骤S204中，位置信息运算部39B使中断了的平滑化处理停止(IV)。接着，返回到步骤S201进行说明。在判定部40A判定为中断平滑化处理的条件不成立的情况下(步骤S201，否)，装置控制器39结束处理。

接着，返回步骤S203进行说明。在判定部40A判定为液压挖掘机100未行驶的情况下(步骤S203，否)，在步骤S205中，判定部40A判定执行条件是否成立。在判定部40A判定为执行条件成立了的情况下(步骤S205，是)，在步骤S206中，位置信息运算部39B使用中断了平滑化处理时的信息，执行平滑化处理(II)。中断了平滑化处理时的信息是存储在存储部39M中的中断前的滤波器的输出Rftpr以及平均化常数M。在判定部40A判定为执行条件不成立的情况下(步骤S205，否)，位置信息运算部39B返回步骤S202，执行步骤S202以后的处理。

图11是装置控制器39使平滑化处理的状态转变的处理的流程图，尤其是示出与平滑化处理的复位相关的处理。在步骤S301中，在装置控制器39正在执行平滑化处理的情况下，判定部40A判定对平滑化处理进行复位的条件是否成立。对平滑化处理进行复位的条件是产生了异常值的状态持续预先确定的时间(设定值Nt秒)时执行超时处理来作为避免全局坐标运算装置23的测位结果的异常值的处理的情况。在判定部40A判定为对平滑化处理进行复位的条件成立了的情况下(步骤S301，是)，在步骤S302中，装置控制器39的位置信息运算部39B对平滑化处理进行复位(III)。

在步骤S303中，判定部40A判定平滑化处理的恢复条件是否成立。平滑化处理的恢复条件是，在状态4下基于RTK-GNSS的测位正常、且液压挖掘机100未行驶、且上部回转体3未回转、且平滑化处理的复位完成的情况。在判定部40A判定为平滑化处理的恢复条件成立了的情况下(步骤S303，是)，在步骤S304中，位置信息运算部39B执行平滑化处理(III)。

接着，返回步骤S301进行说明。在判定部40A判定为对平滑化处理进行复位的条件不成立的情况下(步骤S301，否)，位置信息运算部39B在步骤S305中继续进行执行中的平滑化处理。接着，返回步骤S303进行说明。在判定部40A判定为平滑化处理的恢复条件不成立的情况下(步骤S303，否)，在步骤S306中使平滑化处理停止(V)。

本实施方式使用来自全局坐标运算装置23的第一位置的信息、即第一基准位置数据P1、P2以及来自IMU24的动作信息MI，求出与液压挖掘机100一部分的位置相当的第二位置，并使用得到的第二位置的信息，求出工作装置2的至少一部分的位置。在本实施方式中，使用第一基准位置数据P1、P2以及从IMU24得到的特定点的信息、即作为上部回转体3的旋转中心轴的z轴与和行驶装置5接地的面对应的面的交点的信息，求出第二位置。特定点在液压挖掘机100的作业中能够被看作绝对位置不随着时间的经过而变化，因此，装置控制器39即便对特定点的位置实施平滑化处理，并使用平滑化处理后的特定点的位置来求出第二位置，也能够忽略由于平滑化处理而造成的延迟的影响。其结果是，本实施方式能够使第二位置与液压挖掘机100的一部分的位置一致，因此，在基于对作业机械的位置进行了测位的结果而进行信息化施工的作业机械中，能够降低测位结果的偏差对信息化施工造成的影响。作为一例，能够抑制使用了挖掘控制的施工时的精度下降以及引导画面的视觉确认性的下降。

以上，对实施方式1进行了说明，但实施方式1的结构也能够适当应用在以下的实施方式中。

实施方式2.

图12是实施方式2所涉及的控制系统200a的控制框图。图13是表示实施方式2所涉及的装置控制器39a具有的位置信息运算部39Ba的图。控制系统200a与实施方式1的控制系统200相同，但作为IMU24的检测值的动作信息MI输入至装置控制器39a的处理部39Pa所具有的位置信息运算部39Ba这一点以及位置信息运算部39Ba的结构不同。与实施方式1相同，装置控制器39a通过CPU等处理器以及RAM以及ROM等存储装置来实现。装置控制器39a的处理部39Pa的功能通过处理部39Pa从存储部39M读入用于实现其功能的计算机程序并执行来实现。

位置信息运算部39Ba具有判定部40A、速度计算部40E以及平滑化处理部40Ba。判定部40A与实施方式1所涉及的装置控制器39的判定部40A相同，故省略说明。速度计算部40E根据从IMU24获取到的动作信息MI即角速度ω以及IMU24与GNSS天线21、22的相对位置关系，求出在GNSS天线21、22产生的速度v。即，产生某一角速度ω是在车辆主体1产生移动从而与IMU24设置在同一车辆主体1上的GNSS天线21、22移动的情况。IMU24与GNSS天线21、22的相对位置关系(例如设计尺寸)是已知的。因此，根据角速度ω以及相对位置关系求出GNSS天线21、22的移动(移动距离)，因此，作为结果，求出GNSS天线21、22在规定时间内移动了的距离、即速度v。时间dt是控制的一个周期。

〈平滑化处理〉

在本实施方式中，装置控制器39a、更具体而言平滑化处理部40Ba使用速度v，对第一位置、更具体而言第一位置的信息即基准位置数据P1、P2实施平滑化处理。在本实施方式中，在平滑化处理中使用式(7)所示的低通滤波器。

P_i＝{P+(M-1)×(P_i-1+vdt)}/M…(7)

式(7)中的P是本次的控制周期中的第一位置的信息即第一基准位置数据P1、P2。P_i-1是上次的控制周期中的低通滤波器的输出、即上次的控制周期中实施了平滑化处理的第一位置的信息即第一基准位置数据P1、P2。第一基准位置数据P1、P2由全局坐标运算装置23输出。式(7)中的P_i是本次的控制周期中的低通滤波器的输出，是第二位置的信息即第二基准位置数据P1i、P2i。式(7)中的v是由速度计算部40E根据IMU24检测到的角速度ω以及IMU24与GNSS天线21、22的相对位置关系求出的GNSS天线21、22的速度。式(7)中的dt是由装置控制器39a进行的控制的一个周期。vdt是液压挖掘机100在由装置控制器39a控制的一个周期内移动的距离。M是平均化常数。平均化常数M与实施方式1相同。装置控制器39a的平滑化处理部40Ba通过实现式(7)所示的低通滤波器的功能，从而使用动作信息MI来对第一位置实施平滑化处理来求出第二位置。

在本实施方式中，装置控制器39a的位置信息运算部39Ba所具有的速度计算部40E按照控制的每一个周期求出速度v，平滑化处理部40Ba使用速度v，按照控制的每一个周期对第一基准位置数据P1、P2实施平滑化处理。速度v根据IMU24的检测值即角速度ω以及IMU24与GNSS天线21、22的相对位置关系而求出。装置控制器39a的位置信息运算部39Ba使用IMU24的检测值，对从全局坐标运算装置23输出的第一基准位置数据P1、P2实施平滑化处理。这样，位置信息运算部39Ba使用IMU24的检测值来实施平滑化处理。因此，位置信息运算部39Ba能够通过IMU24的检测值来反映挖掘时的液压挖掘机100的姿势变化对GNSS天线21、22的位置变化造成的影响，从而求出第二基准位置数据P1i、P2i。其结果是，装置控制器39a能够降低由于全局坐标运算装置23的测位结果的偏差造成的影响，因此能够抑制使用了挖掘控制的施工时的精度下降以及引导画面的视觉确认性的下降。

〈控制系统200a的处理的一例〉

图14是表示实施方式2所涉及的控制系统200a的处理的一例的流程图。在步骤S401中，控制系统200a的装置控制器39a所具有的位置信息运算部39Ba的判定部40A判定装置控制器39a执行平滑化处理所需的执行条件是否成立。执行条件如在实施方式1中说明的那样。

在开始条件成立了的情况下(步骤S401，是)，装置控制器39a在步骤S402中从IMU24获取角速度ω，并从全局坐标运算装置23获取第一基准位置数据P1、P2。在步骤S403中，装置控制器39a的位置信息运算部39Ba所具有的平滑化处理部40Ba使用速度v，对第一基准位置数据P1、P2实施平滑化处理。速度v由速度计算部40E根据角速度ω以及IMU24与GNSS天线21、22的相对位置关系而求出。IMU24与GNSS天线21、22的相对位置关系优选作为校正值而基于计测(校准)来得到。

在步骤S404中，装置控制器39a所具有的平滑化处理部40Ba将滤波器的输出、即第二基准位置数据P1i、P2i作为基准位置数据P1c、P2c而输出至显示控制器28。在步骤S405中，显示控制器28的处理部28P使用从装置控制器39获取到的液压挖掘机100的位置信息IPL，求出铲斗8的铲尖8T的三维位置即铲尖位置。接着，返回步骤S401进行说明。在步骤S401中，在开始条件不成立的情况下(步骤S401，否)，装置控制器39a结束处理。在本实施方式中，关于使平滑化处理的状态转变的处理，与实施方式1相同。

本实施方式中，使用第一基准位置数据P1、P2以及来自IMU24的动作信息MI求出第二位置，并使用得到的第二位置的信息，求出工作装置2的至少一部分的位置。在实施方式1中，使用第一基准位置数据P1、P2以及从IMU24得到的特定点的信息求出第二位置，但在本实施方式中，根据由IMU24检测到的动作信息MI、更具体而言角速度等求出速度，并使用得到的速度，对第一位置的信息即第一基准位置数据P1、P2实施平滑化处理，求出第二位置。本实施方式通过IMU24的检测值、具体而言角速度来反映挖掘时的液压挖掘机100的姿势变化对GNSS天线21、22的位置变化造成的影响，从而能够求出第二位置。其结果是，本实施方式能够降低全局坐标运算装置23的测位结果的偏差对信息化施工造成的影响。

以上，对实施方式2进行了说明，但实施方式2的结构也能够适当应用在以下的实施方式中。

实施方式3.

图15是实施方式3所涉及的控制系统200b的控制框图。图16是表示实施方式3所涉及的装置控制器39b具有的位置/姿势信息运算部39Bbd的图。实施方式3在位置姿势运算方法中使用卡尔曼滤波器。控制系统200b与实施方式1的控制系统200相同，但具有位置/姿势信息运算部39Bb这一点以及将作为IMU24的检测值的动作信息MI输入至装置控制器39b的处理部39Pb所具有的位置/姿势信息运算部39Bb这一点不同。与实施方式1相同，装置控制器39b通过CPU等处理器以及RAM以及ROM等存储装置来实现。装置控制器39b的处理部39Pb的功能通过处理部39Pb从存储部39M读入用于实现其功能的计算机程序并执行来实现。

位置/姿势信息运算部39Bb具有位置推定部40F、误差运算部40Bb、选择部40Ab以及特定点运算部40B。位置推定部40F使用由IMU24检测到的动作信息MI来推定液压挖掘机100的位置、速度、方位角以及姿势角这样的位置姿势推定值。液压挖掘机100的位置是GNSS天线21、22的位置。在本实施方式中，位置推定部40F在推定液压挖掘机100的位置、速度、方位角以及姿势角这样的位置姿势值来求出位置姿势推定值时，使用惯性导航。位置推定部40F将通过推定得到的液压挖掘机100的位置作为第二位置而输出、具体而言第二基准位置数据P1i、P2i而输出。另外，位置推定部40F使用误差运算部40Bb输出的误差来修正第二位置。

误差运算部40Bb将第一基准位置数据P1、P2、液压挖掘机100的速度V、方位角θda、特定点(在本实施方式中为位置PL)以及静止时的角速度ω＝0中的至少一个用作观测值，求出由位置推定部40F推定出的液压挖掘机100的位置、速度、方位角以及姿势角或者它们所具有的误差，并输出至位置推定部40F。即，误差运算部40Bb将用于修正位置姿势推定值的信息发送至位置推定部40F。位置推定部40F使用用于修正位置姿势推定值的信息，来修正之前求出的位置姿势推定值的误差。然后，位置推定部40F根据修正后的位置姿势推定值来计算出第二位置数据。误差运算部40Bb使用的观测值中的第一基准位置数据P1、P2、液压挖掘机100的速度V、方位角θda从全局坐标运算装置23得到。误差运算部40Bb将从全局坐标运算装置23得到的全局坐标系的第一基准位置数据P1、P2以及速度V转换到现场坐标系。特定点、在本实施方式中为位置PL以及特定点的位置向量Rfl由特定点运算部40B求出。在本实施方式中，误差运算部40Bb包括卡尔曼滤波器。

选择部40Ab根据液压挖掘机100的状态来选择误差运算部40Bb所使用的观测值。液压挖掘机100的状态包括静定状态、非静定状态、上部回转体3正在回转的状态以及液压挖掘机100正在行驶的状态。

图17是实施方式3所涉及的装置控制器39b所具有的位置/姿势信息运算部39Bb的控制框图。位置推定部40F对由IMU24测定出的角速度进行积分来计算车身的姿势角的推定值以及方位角的推定。位置推定部40F对由IMU24测定出的加速度进行积分，来计算出液压挖掘机100的推定速度以及推定位置。

选择部40Ab包括举动检测部42a、判定器42b以及选择器42c。在举动检测部42a中输入车身信息IFb以及作为IMU24的检测值的角速度ω和加速度a。在本实施方式中，车身信息IFb是对图2所示的左操作杆25L和右操作杆25R的先导压进行检测的压力传感器66的检测值STr、以及对行驶用杆25FL和行驶用杆25FR的先导压进行检测的压力传感器27PC的检测值STd。举动检测部42a使用角速度ω、加速度a以及车身信息IFb来检测液压挖掘机100的状态，将与检测结果对应的信号输出至判定器42b。

在判定器42b中输入来自举动检测部42a的信号、车身信息IFb以及全局坐标运算装置23输出的状态信息SR。判定器42b基于输入的信息使选择器42c动作，来选择向误差运算部40Bb输入的观测值。在选择器42c中输入观测值、即全局坐标运算装置23接收到的第一基准位置数据P1、P2、液压挖掘机100的速度Vc、方位角θda、特定点运算部40B求出的特定点的位置向量Rfl以及液压挖掘机100不回转时的角速度ω＝0。全局坐标运算装置23使用来自测位卫星的电波(信号)，求出第一基准位置数据P1、P2，与此同时求出液压挖掘机100的速度Vc。方位角θda由全局坐标运算装置23根据第一基准位置数据P1、P2来求出。

误差运算部40Bb被从选择部40Ab的选择器42c输入与液压挖掘机100的状态相应的观测值。误差运算部40Bb包括卡尔曼滤波器。误差运算部40Bb获取观测向量，并利用状态方程式来修正事先预测出的状态向量，并求出事后的推定值。通过重复进行该处理，来求出更准确的推定值。式(8)是卡尔曼滤波器的计算式。X_k|k(X为粗体字)是通过事后推定而得到的状态向量，X_k|k-1(X为粗体字)是通过事先推定而得到的状态向量，K(K为粗体字)是卡尔曼增益，z_k(z为粗体字)是观测向量，H_k(H为粗体字)是观测矩阵。误差运算部40Bb使用式(8)求出通过事后推定而得到的状态向量。

【数式2】

X_k|k＝X_k|k-1+K(z_k-H_kX_k|k-1)…(8)

卡尔曼增益K(K为粗体字)由式(9)求出。P_k|k-1(P为粗体字)是推定误差的协方差，R_k(R为粗体字)是观测误差的协方差。通过推定误差的协方差P_k|k-1以及观测误差的协方差R_k的设定，确定状态向量X_k|k(X为粗体字)以及观测向量z_k(z为粗体字)的权重。

【数式3】

K＝P_k|k-1H_k ^T(H_kP_k|k-1H_k ^T+R_k)^-1…(9)

对状态向量进行说明。当以通常字体表示预测值，以斜体表示修正值时，由式(10)至式(14)定义错误状态向量。在此，

δΨⁿ _nb(Ψ为粗体字)：导航坐标系中的液压挖掘机100的角度误差向量[rad]；

δωb(ωb为粗体字)：IMU24的角速度偏置误差向量[rad/s]；

δP^l _lb(P为粗体字)：现场坐标系中的以现场坐标系为基准的车身坐标原点的位置误差向量[m]；

δVⁿ _eb(V为粗体字)：现场坐标系中的以ECEF(Earth Centered Eartth Fixed：地球中心地球固定)坐标系为基准的车身坐标原点的速度误差向量[m/s]；

δAb(Ab为粗体字)：IMU24的加速度偏置误差向量[m/s²]；

C_b ⁿ(C为粗体字)：从车身坐标系向导航坐标系的坐标旋转矩阵；

Ψⁿ _nb(Ψ为粗体字)：导航坐标系中的液压挖掘机100的角度向量[rad]；

ωb(ωb为粗体字)：IMU24的角速度向量[rad/s]；

Ab(Ab为粗体字)：IMU24的加速度向量[m/s²]；

P^l _lb(P为粗体字)：现场坐标系中的以现场坐标系为基准的车身坐标原点的位置向量[m]；

Vⁿ _eb(V为粗体字)：现场坐标系中的以ECEF坐标系为基准的车身坐标原点的速度向量[m/s]；

I：单位矩阵。

【数式4】

δC_b ⁿ＝C_b ⁿC_b ^nT＝I+[δΨⁿ _nb ^∧]…(10)

【数式5】

δωb＝ωb-ωb…(11)

【数式6】

【数式7】

δVⁿ _cb＝Vⁿ _eb-Vⁿ _eb…(13)

【数式8】

δAb＝Ab-Ab…(14)

对状态方程式进行说明。式(11)至式(19)是基于错误状态模式的状态方程式。噪声项省略。在此，

ωⁿ _ie(ω为粗体字)：导航坐标系中的地球自转速度向量[rad/s]；

Aⁿ _ib(A为粗体字)：导航坐标系中的以惯性坐标系为基准的车身坐标原点的加速度向量[m/s²]。

【数式9】

(δΨⁿ _ab)/(dt)＝[ωⁿ _ie ^∧](δΨⁿ _nb)-C_b ⁿ(δωb)…(16)

【数式10】

(δωb)/(dt)＝0…(16)

【数式11】

(δP^l _lb)/(dt)＝(δV^l _lb)＝(δVⁿ _eb)…(17)

【数式12】

(δVⁿ _eb)/(dt)＝-[Aⁿ _lb ^∧](δΨⁿ _nb)-2[ωⁿ _ie ^∧](δVⁿ _eb)-C_b ⁿ(δAb)…(18)

【数式13】

(δAb)/(dt)＝0…(19)

式(20)至式(24)表示关于用斜体显示的观测值的观测方程式。噪声项省略。式(20)是GNSS天线21、22的位置的观测方程式，式(21)是GNSS天线21、22的速度的观测方程式。式(22)是特定点的速度的观测方程式。用于静止时以及回转时。式(23)是静止时的液压挖掘机100的加速度的观测方程式。式(23)是液压挖掘机100不回转时的基于GPS罗盘的方位角的观测方程式。在此，

P^l _la(斜体)：现场坐标系中的以现场坐标系为基准的GNSS天线21、22的位置[m]；

Vⁿ _ea(斜体)：导航坐标系中的以ECEF坐标系为基准的GNSS天线21、22的速度[m/s]；

Vⁿ _eq(斜体)：导航坐标系中的以ECEF坐标系为基准的特定点的速度[m/s]；

Ψ_z(斜体)：基于GPS罗盘的液压挖掘机100的方位角测定值[rad]；

δC_b ^nT：姿势角误差的旋转矩阵(δC_b ^nT＝I-[δΨⁿ _nb ^∧])；

δΨ_z：液压挖掘机100的方位角的误差(δΨⁿ _nb的Z成分)[rad]；

R^b _ba：车身坐标系中的以车身坐标系为基准的GNSS天线21、22的位置[m]；

R^b _bq：车身坐标系中的以车身坐标系为基准的特定点的位置[m]；

ω^b _nb：车身坐标系中的以导航坐标系为基准的车身坐标系的角速度向量[rad/s]；

Ψ_z：通过惯性导航运算得到的液压挖掘机100的方位角[rad]。

【数式14】

【数式15】

Vⁿ _ea＝(Vⁿ _eb-δV^a _eb)+(δC_b ^nT)C_b ⁿ(ω^b _nb+δωb)×R^b _ba…(21)

【数式16】

Vⁿ _eq(＝0)＝(Vⁿ _eb-δVⁿ _eb)+(δC_b ^nT)C_b ⁿ(ω^b _nb+δωb)×R^b _bq…(22)

【数式17】

ω^b _nb(＝0)＝ω^b _nb+δωb…(23)

【数式18】

Ψ_z＝Ψ_z-δΨ_z…(24)

针对上述的式(10)中的[δΨⁿ _nb ^∧](Ψ为粗体字)、式(15)以及式(18)中的[ωⁿ _ie ^∧](ω为粗体字)以及式(18)中的[Aⁿ _ib](A为粗体字)进行说明。δΨⁿ _nb(Ψ为粗体字)、ωⁿ _ie(ω为粗体字)以及Aⁿ _ib(A为粗体字)为(α，β，γ)的三维元坐标系中的向量或者侧倾方向、俯仰方向、偏转方向的向量。侧倾方向是绕α轴的方向，俯仰方向是绕β轴的方向，偏转方向是绕γ轴的方向。在该情况下，[δΨⁿ _nb ^∧](Ψ为粗体字)由式(25)表示，[ωⁿ _ie ^∧](ω为粗体字)由式(26)表示，[Aⁿ _ib](A为粗体字)由式(27)表示。δΨ_α、δΨ_β、δΨ_γ依次为绕α轴、绕β轴、绕γ轴的液压挖掘机100的角度误差。ωi_α、ωi_β、ωi_γ依次为绕α轴、绕β轴、绕γ轴的地球自转速度。Ai_α、Ai_β、Ai_γ依次为绕α轴、绕β轴、绕γ轴的液压挖掘机100的车身坐标原点的加速度。

【数式19】

【数式20】

【数式21】

误差运算部40Bb在事先推定中通过解算式(15)至式(19)所示的状态方程式，而能够求出式(10)至式(14)所示的状态向量的事先推定值、即状态向量X_k|k-1。在本实施方式中，状态向量是导航坐标系中的液压挖掘机100的角度向量Ψⁿ _nb(Ψ为粗体字)、IMU24的角速度向量ωb(ωb为粗体字)、IMU24的加速度向量Ab(Ab为粗体字)、现场坐标系中的以现场坐标系为基准的车身坐标原点的位置向量P^l _lb(P为粗体字)以及现场坐标系中的以ECEF坐标系为基准的车身坐标原点的速度向量δV^l _eb(V为粗体字)。在误差运算部40Bb通过事先推定而求出状态向量X_k|k-1的情况下，位置推定部40F获取所求出的姿势角(侧倾角θ4、俯仰角θ5、方位角θdc)、第二基准位置数据P1i、P2i以及速度V，并用作位置、速度以及姿势的预测值。

观测矩阵通过观测方程式的雅可比式而得到。误差运算部40Bb使用式(20)至式(24)求出观测向量z_k(z为粗体字)，并且根据式(9)求出卡尔曼增益K(K为粗体字)。并且，误差运算部40Bb通过将通过事先推定而得到的状态向量X_k|k-1、观测向量z_k(z为粗体字)以及卡尔曼增益K(K为粗体字)赋给式(8)并进行解算，能够求出作为事后的推定值的状态向量X_k|k。

角速度偏置误差FBa的向量是IMU24的角速度偏置误差向量δωb(ωb为粗体字)。车身角度误差FBb的向量是导航坐标系中的液压挖掘机100的角度误差向量δΨⁿ _nb(Ψ为粗体字)。车身速度误差FBc的向量是现场坐标系中的以ECEF坐标系为基准的车身坐标原点的速度误差向量δV^l _eb(V为粗体字)。车身位置误差FBd的向量是现场坐标系中的以现场坐标系为基准的车身坐标原点的位置误差向量δP^l _lb(P为粗体字)。加速度偏置误差FBe的向量是IMU24的加速度偏置误差向量δAb(Ab为粗体字)。

如上所述，通过事后推定而得到的状态向量X_k|k对应于角速度偏置误差FBa、车身角度误差FBb、车身速度误差FBc、车身位置误差FBd以及加速度偏置误差FBe。误差运算部40Bb将通过事后推定求出的状态向量X_k|k赋给位置推定部40F。位置推定部40F使用从误差运算部40Bb获取到的状态向量X_k|k来修正位置姿势推定值。更具体而言，位置推定部40F使用状态向量X_k|k来修正位置姿势推定值所包含的误差(修正位置姿势推定值)。位置推定部40F使用状态向量X_k|k修正了位置姿势推定值(推定位置)后的位置为液压挖掘机100的第二位置。这样，位置推定部40F通过使用动作信息推定液压挖掘机的位置，并使用状态向量X_k|k修正所得到的推定位置，从而求出液压挖掘机的第二位置。

图18是表示记述有在选择误差运算部40Bb所使用的观测方程式时使用的信息的表44的一例的图。表44存储在图15所示的装置控制器39b的存储部39M中。在本实施方式中，误差运算部40Bb根据液压挖掘机100的状态来选择在推定状态向量X_k|k时使用的观测值。因此，被误差运算部40Bb使用的观测方程式根据被误差运算部40Bb使用的观测值的不同而不同。误差运算部40Bb在推定状态向量X_k|k时，从式(20)至式(24)中选择与根据液压挖掘机100的状态而选择的观测值对应的观测方程式。

如图18所示，液压挖掘机100的状态由表示基于RTK-GNSS的测位状态的状态A、状态B以及状态C与表示液压挖掘机100的动作状态、即车身状态的车身静定1、车身静定2以及车身行驶的组合来确定。基于RTK-GNSS的测位状态是由全局坐标运算装置23进行的液压挖掘机100的位置的检测的状态。在本实施方式中，测位状态为三个且车身状态为三个，因此液压挖掘机100的状态合计为九个。下面示出测位状态以及动作状态的详细内容。在本实施方式中，不限定测位状态以及动作状态的数量以及内容。

状态A：测位状态为Fix；

状态B：测位状态为Fix且非测位以外的状态；

状态C：测位状态为非测位；

车身静定1：液压挖掘机100停止且上部回转体3也停止；

车身静定2：液压挖掘机100停止且上部回转体3正在回转；

车身行驶：液压挖掘机正在行驶。

选择部40Ab的判定器42b根据来自举动检测部42a的信号以及车身信息Ifb来判定动作状态，并根据全局坐标运算装置23输出的状态信息SR来判定测位状态。判定器42b基于在存储于存储部39M的表44中判定出的动作条件以及测位条件，根据被误差运算部40Bb使用的观测方程式，确定向误差运算部40Bb输入的观测值。然后，以使确定出的观测值输入至误差运算部40Bb的方式使选择器42c动作。

判定器42b在使用式(20)的观测方程式的情况下，将与全局坐标运算装置23接收到的GNSS天线21、22的位置对应的第一基准位置数据P1、P2作为向误差运算部40Bb输入的观测值。判定器42b在使用式(21)的观测方程式的情况下，将与全局坐标运算装置23接收到的GNSS天线21、22的位置对应的第一基准位置数据P1、P2以及全局坐标运算装置23求出的GNSS天线21、22的速度Va转换成现场坐标系，作为向误差运算部40Bb输入的观测值。判定器42b在使用式(22)的观测方程式的情况下，将图16所示的特定点运算部40B求出的特定点的位置向量Rfl作为向误差运算部40Bb输入的观测值。判定器42b在使用式(23)的观测方程式的情况下，将静止时的角速度、即液压挖掘机100静止时的角速度ω＝0作为向误差运算部40Bb输入的观测值。判定器42b在使用式(24)的观测方程式的情况下，将全局坐标运算装置23求出的、基于GPS罗盘的液压挖掘机100的方位角θda作为向误差运算部40Bb输入的观测值。

误差运算部40Bb使用从选择器42c输入的观测值，并使用与输入的观测值对应的观测方程式来求出观测向量z_k(z为粗体字)。这样，误差运算部40Bb根据液压挖掘机100的状态、即测位状态以及车身状态来变更在求出观测向量z_k(z为粗体字)时使用的观测方程式，因此能够根据液压挖掘机100的状态而不使用不需要的观测方程式。其结果是，误差运算部40Bb能够减轻运算的负荷。

在液压挖掘机100停止且上部回转体3不回转的情况下，误差运算部40Bb使用液压挖掘机100的特定点的速度为0且角速度ω＝0这样的观测值，能够求出观测向量z_k(z为粗体字)。其结果是，能够降低由于全局坐标运算装置23的测位结果的偏差而造成的影响。

图19是表示实施方式3所涉及的控制系统200b的处理的一例的流程图。在步骤S501中，装置控制器39b的位置/姿势信息运算部39Bb推定下一个时刻、在本实施方式中为下一个控制周期中的的液压挖掘机100的状态向量，并且获取观测值。

在步骤S502中，位置/姿势信息运算部39Bb的误差运算部40Bb根据测位状态以及车身状态，来选择在求出观测向量z_k(z为粗体字)时使用的观测方程式。在步骤S503中，误差运算部40Bb求出作为事后推定值的状态向量X_k|k，将与该状态向量X_k|k对应的角速度偏置误差FBa、车身角度误差FBb、车身速度误差FBc、车身位置误差FBd以及加速度偏置误差FBe赋给位置推定部40F。位置推定部40F使用从误差运算部40Bb获取到的角速度偏置误差FBa、车身角度误差FBb、车身速度误差FBc、车身位置误差FBd以及加速度偏置误差FBe，来修正IMU24检测到的角速度ω和加速度a、位置推定部40F根据角速度ω求出的角度以及位置推定部40F根据加速度a求出的速度和位置。

在步骤S504中，位置/姿势信息运算部39Bb将通过上述修正而得到的第二基准位置数据P1i、P2i作为基准位置数据P1c、P2c，输出至显示控制器28的铲尖位置计算部28A。在步骤S505中，显示控制器28的处理部28P使用从装置控制器39获取到的液压挖掘机100的位置信息IPL，求出铲斗8的铲尖8T的三维位置即铲尖位置。

本实施方式使用作为第一位置的信息的第一基准位置数据P1、P2以及来自IMU24的动作信息MI求出第二位置，并使用得到的第二位置的信息求出工作装置2的至少一部分的位置。在实施方式1中，使用第一基准位置数据P1、P2以及从IMU24得到的特定点的信息求出第二位置，而在本实施方式中，通过将特定点(伪不动点)静止这一条件追加到观测方程式中，能够与实施方式1同样地求出第二位置。

在本实施方式中，液压挖掘机100的位置通过惯性导航来推定，液压挖掘机100的位置姿势误差所包含的误差以及IMU24的误差等由卡尔曼滤波器求出。本实施方式通过惯性导航来推定下一个时刻的液压挖掘机100的位置，使用第一位置的信息以及动作信息MI并利用卡尔曼滤波器求出的误差，来修正推定出的液压挖掘机100的位置。在实施方式1以及实施方式2中，对由全局坐标运算装置23得到的位置的信息进行平滑化处理，但在本实施方式中，使用由卡尔曼滤波器求出的误差来修正通过惯性导航预先推定出的位置，或者使用由卡尔曼滤波器求出的状态向量来修正通过惯性导航预先推定出的状态向量。因此，本实施方式能够排除平滑化处理的延迟的影响，因此，在基于对作业机械的位置进行了测位后的结果而进行信息化施工的作业机械中，能够更可靠地降低测位结果的偏差对信息化施工造成的影响。

以上，对实施方式1至实施方式3进行了说明，但实施方式1至实施方式3并不被上述的内容所限定。另外，上述的构成要素中包括本领域技术人员能够容易想到的、实质上相同的、所谓的等同范围内的要素。此外，上述的构成要素能够适当进行组合。

此外，在不脱离实施方式1至实施方式3的宗旨的范围内，能够进行构成要素的各种省略、置换以及变更中的至少一种。例如，装置控制器39执行的各处理也可以由装置控制器39、显示控制器28、泵控制器或者除此以外的控制器执行。作业机械不限定于液压挖掘机100，也可以是轮式装载机或者推土机这样的其他作业机械。图5所示的姿势角运算部39A以及位置/姿势信息运算部39B包含在装置控制器39中，但也可以任意一方或双方包含在显示控制器28中，还可以包含在显示控制器28以外的控制器中。

附图标记说明

1 车辆主体

2 工作装置

3 上部回转体

5 行驶装置

8 铲斗

8B 斗齿

8T 铲尖

21、22 天线(GNSS天线)

23 全局坐标运算装置

25 操作装置

28 显示控制器

28A 铲尖位置计算部

28B 目标挖掘地形数据生成部

28C 目标施工信息储存部

28M 存储部

28P 处理部

39、39a、39b 装置控制器

39A 姿势角运算部

39B、39Ba 位置信息运算部

39Bb 位置/姿势信息运算部

39C 工作装置控制部

39M 存储部

39P、39Pa、39Pb 处理部

40A 判定部

40Ab 选择部

40B 特定点运算部

40Ba、40C 平滑化处理部

40D 位置计算部

40E 速度计算部

40F 位置推定部

40Bb 误差运算部

41a、41g、41p 车身坐标系转换部

41b、41c、41h、41j 加减运算器

41d、41f 更新部

41i 现场坐标系转换部

41k 速度修正部

41m 积分器

41n 位置修正部

42a 举动检测部

42b 判定器

42c 选择器

44 表

60 基准桩

100 液压挖掘机

200、200a、200b 控制系统

FBa 角速度偏置误差

FBb 车身角度误差

FBc 车身速度误差

FBd 车身位置误差

FBe 加速度偏置误差

K 卡尔曼增益

M 平均化常数

MI 动作信息

P、P1c、P2c 基准位置数据

P1、P2 第一基准位置数据

P1i、P2i 第二基准位置数据

SR 状态信息

a 加速度

g 重力加速度

ω 角速度

θ4 侧倾角

θ5 俯仰角

θda、θdc、θdi 方位角

Claims (4)

1.一种作业机械的控制系统，其是对作业机械进行控制的系统，该作业机械具备：行驶装置；具有作业用具的工作装置；以及供所述工作装置安装且被安装于所述行驶装置而进行回转的回转体，

所述作业机械的控制系统包括：

位置检测装置，其检测作为所述作业机械的一部分的位置的第一位置并作为所述第一位置的信息而输出；

状态检测装置，其检测表示所述作业机械的动作的动作信息并将该动作信息输出；以及

处理装置，其对根据所述第一位置以及所述动作信息而得到的信息、即作为所述回转体的旋转中心轴与和所述行驶装置接地的面对应的面的交点的特定点的位置实施平滑化处理，并使用所述平滑化处理后的所述特定点的位置，求出与所述一部分的位置相当、且降低了所述第一位置的测位结果的偏差的影响的第二位置，并使用所述第二位置的信息，求出所述工作装置的至少一部分的位置。

2.根据权利要求1所述的作业机械的控制系统，其中，

当由所述位置检测装置进行的所述作业机械的位置的检测正常、且所述作业机械的行驶停止、且所述回转体不回转时，所述处理装置使用所述第二位置的信息，求出所述工作装置的至少一部分的位置。

3.根据权利要求1或2所述的作业机械的控制系统，其中，

基于所述处理装置求出的所述工作装置的一部分的位置，对所述工作装置的动作进行控制，或者在设于所述作业机械的显示装置显示所述工作装置的一部分的位置。

4.一种作业机械的控制方法，所述作业机械具备：行驶装置；具有作业用具的工作装置；以及供所述工作装置安装且被安装于所述行驶装置而进行回转的回转体，

在对所述作业机械进行控制时，

使用所述作业机械所具备的位置检测装置检测到的作为所述作业机械的一部分的位置的第一位置以及所述作业机械所具备的状态检测装置检测到的所述作业机械的动作信息，求出作为所述回转体的旋转中心轴与和所述行驶装置接地的面对应的面的交点的特定点的位置，

对所述特定点的位置实施平滑化处理，并使用所述平滑化处理后的所述特定点的位置求出与所述一部分的位置相当、且降低了所述第一位置的测位结果的偏差的影响的所述作业机械的第二位置，

使用所述第二位置，求出所述工作装置的至少一部分的位置。