CN113874862A

CN113874862A - 工程机械

Info

Publication number: CN113874862A
Application number: CN202080037240.2A
Authority: CN
Inventors: 石原新士; 泉枝穗; 小竹伸一
Original assignee: Hitachi Construction Machinery Co Ltd
Current assignee: Hitachi Construction Machinery Co Ltd
Priority date: 2019-06-06
Filing date: 2020-05-28
Publication date: 2021-12-31
Also published as: EP3981923A4; KR102606049B1; JP7245119B2; US20220222392A1; KR20210151964A; JP2020200597A; EP3981923A1; EP3981923B1; WO2020246369A1

Abstract

不论前作业机的抬升下降等工程机械的姿势变化如何，均能够利用GNSS计算出准确的方位角。为此，控制器(20)在位置姿势运算部(20a)中，在由车身IMU(13)获取到的上部旋转体(2)的姿势角和由动臂IMU(14)获取到的前作业机(1)的姿势角中的至少一方为阈值以上时，判断成由GNSS接收机(17c)计算出的上部旋转体(2)的方位角的质量低，并基于方位角的质量和由GNSS接收机(17c)计算出的上部旋转体(2)的方位角执行偏置除去运算，基于由GNSS接收机(17c)计算出的上部旋转体(2)的方位角和除去了陀螺偏置的上部旋转体(2)的角速度计算出上部旋转体(2)的修正方位角，使用该修正方位角对前作业机(1)的三维位置及姿势进行运算。

Description

工程机械

技术领域

本发明涉及上部旋转体能够相对于下部行驶体旋转的液压挖掘机等工程机械。

背景技术

伴随着面向信息化施工的对策，开发出了具有将多关节型的前作业机的位置和姿势向操作员显示的机械指导、和以使前作业机的位置及姿势沿着目标施工面移动的方式进行控制的机械控制的功能的液压挖掘机。

在这样的液压挖掘机中，为了应对三维信息化施工(3D信息化施工)，需要提供施工现场中的液压挖掘机自身的位置(坐标)和方位(前作业机的朝向)。为了提供这些信息，以往，开发出了在上部旋转体安装有两根GNSS(Global Navigation Satellite System，全球导航卫星系统)天线而进行GNSS测位的液压挖掘机。

在使用两根GNSS天线进行GNSS测位的液压挖掘机中，GNSS天线无法捕捉到所需数量的卫星信号的情况下，GNSS测位不再被正常地实施。在这样的状态下，由于无法提供位置和方位信息，所以不得不停止机械指导和机械控制的动作，作业效率有可能降低。

在专利文献1中，示出了如下技术：除了基于两根GNSS天线实施GNSS测位以外，还接收利用与施工现场所准备的基准局之间的无线通信发送的修正数据，针对位置信息实施RTK(Real Time Kinematic，实时动态)测位，并且具备陀螺仪等偏航角计测机构，由此计算出液压挖掘机车身(上部旋转体)的方位角，在RTK测位异常时能够使用该方位角计测位置信息。

现有技术文献

专利文献

专利文献1：日本特开2004-125580号公报

发明内容

在利用两根GNSS天线计算车身(上部旋转体)的方位角的情况下，通常进行的是将两根GNSS天线中的一根天线用作主天线、将另一根用作副天线并在该主、副天线之间进行RTK测位。由此不论能否与施工现场所设置的基准局进行通信均能够针对车身的方位角实施RTK测位。

另外，由于液压挖掘机的前作业机由金属部件构成，所以可以想到GNSS的卫星信号会被前作业机反射或屏蔽。

另外，在液压挖掘机的挖掘动作中，由于前作业机会反复进行上下动作，所以会成为反复发生卫星信号反射或中断那样的GNSS使用环境。在这样的使用环境中，由于卫星配置显著地失衡，所以车身方位角的运算精度会降低。

并且，由于在液压挖掘机中频繁地进行前作业机的抬升下降作业，所以尽管没有旋转上部旋转体，但基于GNSS计算出的车身的方位角仍会有所偏差。若将该计算结果用于机械指导，则看上去像是上部旋转体在摇摇晃晃地动作。

此外，对于实施RTK测位，只要能够补充最少四个以上的卫星即可。因此，如上述那样仅由于作业机抬升下降而使得基于来自基准局的修正数据进行的RTK测位本身无法再实施的情况少，GNSS接收机持续进行正常的测位(RTK-Fix状态)。

因此，在RTK测位变得异常时实施对策的专利文献1的技术中，无法解决上述课题。也就是说，若限定于方位角的计算，则GNSS测位变得异常的场景与专利文献1不同，在专利文献1的技术中，无法防止因前作业机的抬升下降等工程机械的姿势变化导致的车身(上部旋转体)方位角的运算精度的降低。

此外，只要将两根GNSS天线沿前后方向呈直线状排列配置，则屏蔽区域不会左右对称(参照后述的图15B及图15C)，因此上述现象多少会被缓和，但产生屏蔽区域的现象本身不会被消除。因此，无法根本上解决上述课题。

另外，只要使GNSS天线比前作业机的最高点高，则基于前作业机产生的屏蔽的影响会消失。但是，若GNSS天线变长则从构造上说GNSS天线会振动，反而有测位精度降低的隐忧。

本发明是为了解决上述课题而做出的，提供一种不论前作业机的抬升下降等前作业机的姿势变化如何，均能够利用GNSS计算出准确的车身方位角的工程机械。

本发明为了解决上述课题，提供一种工程机械，具备：下部行驶体；上部旋转体，其能够相对于下部行驶体旋转；多关节型的前作业机，其相对于上部旋转体能够沿垂直方向旋转地安装；GNSS系统，其具有安装于上述上部旋转体的两根GNSS天线、和基于上述GNSS天线接收到的卫星信号计算出上述GNSS天线的三维坐标及上述上部旋转体的方位角的GNSS接收机；角速度获取装置，其安装于上述上部旋转体，获取上述上部旋转体的角速度；车身姿势角获取装置，其安装于上述上部旋转体，获取上述上部旋转体的姿势角；前姿势角获取装置，其获取上述前作业机的姿势角；以及控制器，上述控制器基于由上述GNSS接收机计算出的上述GNSS天线的三维坐标及上述上部旋转体的方位角、由上述车身姿势角获取装置获取到的上述上部旋转体的姿势角、由上述角速度获取装置获取到的上述上部旋转体的角速度以及由上述前姿势角获取装置获取到的上述前作业机的姿势角，对上述前作业机的位置及姿势进行运算，并基于上述前作业机的位置及姿势控制上述前作业机的动作，在上述工程机械中，上述控制器基于由上述车身姿势角获取装置获取到的上述上部旋转体的姿势角和由上述前姿势角获取装置获取到的上述前作业机的姿势角中的至少一方对由上述GNSS接收机计算出的上述上部旋转体的方位角的质量进行判断，并基于上述方位角的质量的判断结果和由上述GNSS接收机计算出的上述上部旋转体的方位角，执行从由上述角速度获取装置获取到的上述上部旋转体的角速度除去陀螺偏置(Gyro bias)的偏置除去运算，并且，基于除去了上述陀螺偏置的上述上部旋转体的角速度判断上述上部旋转体有无旋转动作，基于由上述GNSS接收机计算出的上述上部旋转体的方位角、除去了上述陀螺偏置的上述上部旋转体的角速度、和上述上部旋转体的旋转动作的判断结果计算出上述上部旋转体的修正方位角，使用上述修正方位角对上述前作业机的位置及姿势进行运算。

发明效果

根据本发明，不论前作业机的抬升下降等工程机械的姿势变化如何，均能够利用GNSS计算出准确的上部旋转体的方位角，高精度且稳固(鲁棒性)地获取上部旋转体的方位角。

附图说明

图1是表示作为本发明的实施方式的工程机械的一个例子的液压挖掘机的外观及内部构造的一部分的图。

图2是表示搭载于本发明的第1实施方式中的液压挖掘机的控制器的处理功能的一部分的框图。

图3是表示作为第1实施方式的特征部分的控制器的位置姿势运算部的处理功能的框图。

图4是表示修正方位角运算部的处理功能的框图。

图5A是表示方位角质量判断部的质量判断处理的一个例子的图。

图5B是表示方位角质量判断部的质量判断处理的另一例子的图。

图6是表示修正方位角运算部的运算步骤的流程图。

图7是表示本发明的第2实施方式中的修正方位角运算部的处理功能的框图。

图8是表示本发明的第3实施方式中的液压挖掘机的外观及其周围状况的图。

图9是表示第3实施方式中的控制器的处理功能的框图。

图10是表示第3实施方式中的修正方位角运算部的处理功能的框图。

图11是表示本发明的第4实施方式中的控制器的处理功能的框图。

图12是表示第4实施方式中的修正方位角运算部的处理功能的框图。

图13是液压挖掘机的俯视图。

图14A是表示液压挖掘机的前作业机下降的状态下的卫星信号的接收状况的图。

图14B是表示液压挖掘机的前作业机下降的状态下的卫星信号的接收状况的图。

图14C是表示液压挖掘机的前作业机下降的状态下的卫星信号的接收状况的图。

图15A是表示液压挖掘机的前作业机抬升的状态下的卫星信号的接收状况的图。

图15B是表示液压挖掘机的前作业机抬升的状态下的卫星信号的接收状况的图。

图15C是表示液压挖掘机的前作业机抬升的状态下的卫星信号的接收状况的图。

图15D是表示液压挖掘机的前作业机抬升的状态下的卫星信号的接收状况的图。

图16是表示液压挖掘机被配置于倾斜面的状态下的卫星信号的接收状况的图。

图17是表示液压挖掘机的背面存在陡峭的倾斜面的情况下的卫星信号的接收状况的图。

具体实施方式

以下，遵照附图说明本发明的实施方式。

<第1实施方式>

图1是表示作为本发明的工程机械的一个例子的液压挖掘机的外观及内部构造的一部分的图。

在图1中，液压挖掘机100具备：将沿垂直方向分别转动的作为多个被驱动部件的动臂4、斗杆5、铲斗(作业件)6连结而构成的多关节型的前作业机1；和构成车身的上部旋转体2及下部行驶体3，上部旋转体2相对于下部行驶体3能够旋转地搭载。另外，前作业机1的动臂4的基端能够沿垂直方向转动地支承于上部旋转体2的前部，斗杆5的一端能够沿垂直方向转动地支承于动臂4的前端，在斗杆5的另一端能够沿垂直方向转动地支承有铲斗6。动臂4、斗杆5、铲斗6、上部旋转体2及下部行驶体3通过作为液压执行机构的动臂缸4a、斗杆缸5a、铲斗缸6a、旋转马达2a及左右的行驶马达3a、3b而分别被驱动。

动臂4、斗杆5及铲斗6在包含前作业机1的平面上动作，以下存在将该平面称为动作平面的情况。也就是说，动作平面是指与动臂4、斗杆5及铲斗6的转动轴正交的平面，能够设定于动臂4、斗杆5及铲斗6的宽度方向的中心位置。

在操作员所搭乘的操作室9内(驾驶室)，设置有生成用于对液压执行机构2a、4a～6a进行操作的操作信号的操作杆装置9a、9b和监视器11等。

操作杆装置9a、9b是所谓电气杆方式，分别包含能够向前后左右倾倒的操作杆、和以电气方式检测这些操作杆的倾倒量即杆操作量的未图示的检测装置，检测装置经由电线将检测出的杆操作量作为电气操作信号输出到作为控制装置的控制器20。另外，对操作杆装置9a、9b的操作杆的前后方向或左右方向，分别分配有液压执行机构2a、4a～6a的操作指令。

关于旋转马达2a、动臂缸4a、斗杆缸5a、铲斗缸6a的动作控制，通过对控制阀8内的各个滑阀进行控制来控制从由发动机或电动马达等原动机40驱动的液压泵装置7向液压执行机构2a、4a～6a供给的工作油的方向及流量而进行。控制阀8内的滑阀通过从未图示的先导泵经由电磁比例阀10a、10b···(参照图2)输出的驱动信号(先导压)而被操作。控制器20基于从操作杆装置9a、9b输出的操作信号生成控制信号，将该控制信号输出到电磁比例阀10a、10b···，由此从电磁比例阀10a、10b···将驱动信号(先导压)输出到控制阀8内的各个滑阀，从而控制各液压执行机构2a～6a的动作。

此外，操作杆装置9a、9b也可以为液压先导方式。在该情况下，操作杆装置9a、9b分别生成与操作杆的操作方向及操作量相应的先导压，该先导压作为驱动信号直接被供给到控制阀8，从而控制各液压执行机构2a～6a的动作。

在上部旋转体2上配置有惯性计测装置(IMU：Inertial Measurement Unit)13。

惯性计测装置13是计测上部旋转体2的角速度及加速度的装置，在本发明中，惯性计测装置13构成获取上部旋转体的角速度的角速度获取装置。

通过惯性计测装置13计测的上部旋转体2的角速度包含上部旋转体2的前后方向上的角速度、左右方向上的角速度和旋转方向上的角速度。

另外，通过将计测出的上部旋转体2的前后方向上的角速度、左右方向上的角速度、旋转方向上的角速度分别以时间进行积分，而能够分别计测上部旋转体2的前后方向上的倾斜度(俯仰角)、左右方向上的倾斜度(横摆角)和旋转方向上的倾斜度(偏航角)。即，在本发明中，惯性计测装置13除了构成角速度获取装置以外，还构成获取上部旋转体2的姿势角的车身姿势角获取装置。

此外，也可以对姿势角的获取利用使用了加速度的方法。具体而言，若考虑配置有惯性计测装置13的上部旋转体2静止的情况，则通过比较对惯性计测装置13设定的车身坐标系中的铅垂方向(即重力加速度作用的方向)、和惯性计测装置13实际检测出的加速度，而能够分别检测上部旋转体2向前后方向的倾斜度(俯仰角)、向左右方向的倾斜度(横摆角)。

在图1中，Σm示出了车身坐标系。车身坐标系Σm是对上部旋转体2设定的三维正交坐标系。作为其一个例子，车身坐标系Σm是将上部旋转体2的旋转中心的轴线S(参照图2)与上部旋转体2的基座框架(旋转框架)上表面的交点设为原点、且将旋转中心的轴线S设为z轴(参照图2)、将与z轴正交的前后方向上的轴线设为x轴、将与z轴和x轴正交的左右方向上的轴线设为y轴的正交坐标系。

上述的上部旋转体2的前后方向上的角速度是车身坐标系Σm的绕y轴的角速度，左右方向上的角速度是绕其x轴的角速度，旋转方向上的角速度是绕其z轴的角速度。另外，上部旋转体2的前后方向上的倾斜度是车身坐标系Σm的绕y轴的倾斜度(俯仰角)，左右方向上的倾斜度是绕其x轴的倾斜度(横摆角)，旋转方向上的倾斜度是绕其z轴的倾斜度(偏航角)。

此外，在本实施方式中，设想的是利用在惯性计测装置中安装有这些倾斜度(角度)的运算功能的传感器，但在惯性计测装置13中没有内置角度计算功能的情况下，只要在控制器20中准备角度运算功能，当然就能够实现本发明。

在上部旋转体2上安装有两根GNSS天线17a、17b。由GNSS天线17a、17b各自接收到的卫星信号在GNSS接收机17c(参照图2及图3等)中，执行GNSS天线17a、17b的三维坐标的计算和上部旋转体2的方位角的计算等各种测位运算。在GNSS接收机17c中，GNSS天线17a、17b的三维坐标被运算为全局坐标系的坐标值。另外，在本实施方式中，将两根GNSS天线17a、17b中的一根用作主天线，将另一根用作副天线，GNSS接收机17c在该主、副天线间实施RTK(Real Time Kinematic)测位。因此，即使不以无线通信与设置于施工现场的基准局连接，或者无论能否与基准局进行无线通信，对于上部旋转体2的方位角均能够实施RTK测位。另一方面，对于三维坐标的位置信息，通过以无线通信与设置于施工现场的基准局连接而实施利用了从基准局发送的修正信息、或经由网络发布的修正信息的RTK测位，获取位置信息。

以下，为了简化说明，存在将GNSS天线17a、17b和GNSS接收机17c统称为GNSS系统17的情况。

在作为前作业机1的各结构要素的动臂4、斗杆5、铲斗6上，为了计测各自的姿势(姿势角)而在适当的位置设置有惯性计测装置14、15、16。惯性计测装置14～16在本发明中构成获取前作业机1的姿势角的前姿势角获取装置。

以下，为了分别区分惯性计测装置13、14、15、16，根据设置场所适当称为车身IMU13、动臂IMU14、斗杆IMU15、铲斗IMU16。在本发明中，车身IMU13构成安装于上部旋转体2的至少一个车身倾斜传感器，动臂IMU14构成安装于前作业机1的至少一个前倾斜传感器。

图2是表示搭载于本发明的第1实施方式中的液压挖掘机100的控制器20的处理功能的一部分的框图。

在图2中，控制器20具有用于控制液压挖掘机100的动作的各种功能，作为其一部分，具有位置姿势运算部20a、施工目标面运算部20b、监视器显示控制部20c及液压系统控制部20d的各功能。位置姿势运算部20a、施工目标面运算部20b、监视器显示控制部20c及液压系统控制部20d由CPU构成。

另外，控制器20具有作为数据库的存储装置20e、存储运算中途的值的存储装置(例如RAM)20f以及存储程序和设定值等的未图示的存储装置(例如ROM)。

位置姿势运算部20a输入来自惯性计测装置13～16的检测结果(上部旋转体2的角速度及姿势角(俯仰角)和前作业机1的姿势角)、以及来自GNSS接收机17c的运算结果(GNSS天线17a、17b的三维坐标及上部旋转体2的方位角)，并基于这些输入值，进行运算例如对上部旋转体2设定的车身坐标系Σm中的前作业机1的三维位置和姿势的位置姿势运算处理。

施工目标面运算部20b基于由施工管理者预先存储于存储装置20e的三维施工图纸等施工信息21、以及由位置姿势运算部20a运算出的前作业机1的三维位置和姿势，对定义施工对象的目标形状的施工目标面进行运算。

监视器显示控制部20c对设在操作室9内的监视器11的显示进行控制，基于由施工目标面运算部20b运算出的施工目标面、以及由位置姿势运算部20a运算出的前作业机1的位置和姿势，对针对操作员的操作支援的指示内容进行运算，并显示于操作室9内的监视器11。即，监视器显示控制部20c承担例如将具有动臂4、斗杆5、铲斗6等被驱动部件的前作业机1的姿势、铲斗6的前端位置和角度显示于监视器11来支援操作员的操作的作为机械指导系统的功能的一部分。

液压系统控制部20d对由液压泵装置7、控制阀8、电磁比例阀10a、10b···、液压执行机构2a～6a等构成的液压挖掘机100的液压系统进行控制，基于从操作杆装置9a、9b输出的操作信号(电气信号)对前作业机1的被驱动部件的目标动作速度进行运算，并以实现该目标动作速度的方式控制液压挖掘机100的液压系统。另外，液压系统控制部20d基于由施工目标面运算部20b运算出的施工目标面、以及由位置姿势运算部20a运算出的前作业机1的位置和姿势，对前作业机1的被驱动部件的目标动作速度进行运算，并以实现该目标动作速度的方式对从操作杆装置9a、9b输出的相对应的操作信号进行修正，控制液压挖掘机100的液压系统。由此液压系统控制部20d承担例如以铲斗6等作业件的前端不会比一定距离更接近目标施工面的方式对动作施加限制、或以作业件(例如铲斗6的齿尖)沿着目标施工面动作的方式进行控制的作为机械控制系统的功能的一部分。

像这样控制器20基于由GNSS接收机17c计算出的GNSS天线17a、17b的三维坐标及上部旋转体2的方位角、由车身IMU13(车身姿势角获取装置)获取到的上部旋转体2的姿势角、由车身IMU13(角速度获取装置)获取到的上部旋转体2的角速度、以及由动臂IMU14、斗杆IMU15及铲斗IMU16(前姿势角获取装置)获取到的前作业机1的姿势角对前作业机1的位置及姿势进行运算，并基于前作业机1的位置及姿势对前作业机1的动作进行控制。

另外，控制器20作为本发明的特征，在位置姿势运算部20a中进行以下的运算处理。

首先控制器20在位置姿势运算部20a中，基于由车身IMU13(车身姿势角获取装置)获取到的上部旋转体2的姿势角和由动臂IMU14(前姿势角获取装置)获取到的前作业机1的姿势角中的至少一方，对由GNSS接收机17c计算出的上部旋转体2的方位角的质量进行判断，并基于方位角质量的判断结果、和由GNSS接收机17c计算出的上部旋转体2的方位角，执行从由车身IMU13(角速度获取装置)获取到的上部旋转体2的角速度除去陀螺偏置的偏置除去运算，且基于除去了陀螺偏置的上部旋转体2的角速度对上部旋转体2有无旋转动作进行判断，基于由GNSS接收机17c计算出的上部旋转体2的方位角、除去了陀螺偏置的上部旋转体2的角速度和上部旋转体2的旋转动作的判断结果计算出上部旋转体2的修正方位角，并使用该修正方位角对前作业机1的位置及姿势进行运算。

另外，控制器20基于由车身IMU13(车身倾斜传感器)检测出的上部旋转体2的姿势角和由动臂IMU14(前倾斜传感器)检测出的前作业机1的姿势角中的至少一方，对由GNSS接收机17c计算出的上部旋转体2的方位角的质量进行判断，在判断成上部旋转体2的方位角的质量低下时执行偏置除去运算。此外，在本说明书中“上部旋转体2的方位角的质量低下”表示在位置姿势运算部20a(后述的三维位置姿势运算部20a-2)使用由GNSS接收机17c计算出的上部旋转体2的方位角对前作业机1的三维位置及姿势进行运算时，上部旋转体2的方位角的质量差到该运算精度在液压系统控制部20d中无法确保前作业机1的恰当的动作控制的程度。另外，在以下的说明中，将“上部旋转体2的方位角的质量低下”也称为“上部旋转体2的方位角的质量低”、“上部旋转体2的方位角的质量不良”等，将并非如此的情况也称为“上部旋转体2的方位角的质量高”、“上部旋转体2的方位角的质量良好”等。

另外，控制器20具备对在位置姿势运算部20a中判断成上部旋转体2的方位角的质量高时的方位角进行存储的存储装置20f(方位角存储装置)，在判断成上部旋转体2的方位角的质量低、且判断成上部旋转体2无旋转动作时，将刚在判断成上部旋转体2的方位角的质量低之前存储于存储装置20f(方位角存储装置)的方位角用作修正方位角，对前作业机1的位置及姿势进行运算。

而且，控制器20在位置姿势运算部20a中判断成上部旋转体2的方位角的质量低、且上部旋转体2有旋转动作后，在保持上部旋转体2的方位角的质量低的状态而经过了规定时间时，将上部旋转体2的方位角的精度有可能低这一情况通知给用户。

以下遵照图3具体地说明上述的位置姿势运算部20a的处理内容的详情。图3是表示作为本实施方式的特征部分的控制器20的位置姿势运算部20a的处理功能的框图。

在图3中，位置姿势运算部20a具有修正方位角运算部20a-1和三维位置姿势运算部20a-2。

在修正方位角运算部20a-1中，输入由车身IMU13、动臂IMU14、GNSS系统17计测出或运算出的值(信号)。更具体而言，输入由GNSS接收机17c计算出的上部旋转体2的方位角、由车身IMU13计测出的上部旋转体2的角速度和姿势角、由动臂IMU14计算出的动臂4的姿势角。

修正方位角运算部20a-1对由GNSS接收机17c计算出的上部旋转体2的方位角实施后述的修正运算，计算出修正方位角。另外，在无法以足够的精度运算修正方位角的情况下，对用于将该状态通知给操作员的低精度警告的标志进行计算。并且，在低精度警告标志成为有效的情况下，经由监视器显示控制部20c，在监视器11上显现出消息等而向操作员进行通知。

在三维位置姿势运算部20a-2中，输入由IMU13～16检测出的姿势角、由GNSS接收机17c计算出的GNSS天线17a、17b的三维坐标、由修正方位角运算部20a-1计算出的修正方位角。

三维位置姿势运算部20a-2基于由GNSS系统17获取到的GNSS天线17a、17b的三维坐标，对例如车身坐标系Σm中的液压挖掘机100的位置和姿势进行运算。

三维位置姿势运算部20a-2基于车身坐标系Σm中的液压挖掘机100的位置和姿势、由IMU13～16检测出的姿势角以及由修正方位角运算部20a-1计算出的修正方位角，对例如车身坐标系Σm中的前作业机1的三维位置及姿势进行运算，如上述那样，将该运算值向施工目标面运算部20b、监视器显示控制部20c及液压系统控制部20d发送。

三维位置姿势运算部20a-2也可以对代替车身坐标系Σm而设定于液压挖掘机100周围的现场坐标系中的前作业机1的三维位置及姿势进行计算，也可以将以车身坐标系Σm计算出的前作业机1的三维位置及姿势转换成现场坐标系后在控制中使用。

本实施方式中的GNSS系统17如上述那样，在两根GNSS天线17a、17b之间实施RTK测位，由此获取上部旋转体2的方位角。因此，液压挖掘机100的姿势变化会影响方位角的质量(后述)。

另一方面，对于三维坐标的位置信息，实施利用了通过以无线通信与设置于施工现场的基准局连接而从基准局发送的修正信息、或经由网络发布的修正信息的RTK测位。因此，液压挖掘机100的姿势变化给三维位置信息造成的影响与方位角相比非常小。另外，被三维姿势运算部20a-2利用的三维坐标只要以两根GNSS天线17a、17b中的、在后述的图15B及图15C中DOP(Dilution Of Precision，精度衰减因子)的恶化率低的GNSS天线17a或17b为起点即可。也就是说，在如图15B及图15C那样天顶方向的一部分被遮盖的状态下，只要将卫星的配置不偏向特定场所的一侧的GNSS天线用于位置运算即可。因此，在因液压挖掘机100的姿势变化而GNSS系统17获取的方位角的质量低下的情况下，通过GNSS系统17的RTK测位获取到的三维坐标的位置信息的精度的降低也会少，能够在三维位置姿势运算部20a-2中利用该三维的位置信息。

基于这样的想法，本发明在三维姿势运算部20a-2中，对液压挖掘机100的上部旋转体2的方位角使用由修正方位角运算部20a-1计算出的修正方位角，并且使用测位精度的降低少的GNSS天线17a、17b的三维坐标对前作业机1的三维位置及姿势进行运算。三维姿势运算部20a-2中的除此以外的运算由于与以往相比没有特别不同的方面，所以省略详细的说明。

以下，对于成为本发明的特征的修正方位角运算部20a-1，使用图4详细地进行说明。图4是表示修正方位角运算部20a-1的处理功能的框图。

在图4中，修正方位角运算部20a-1具有方位角质量判断部MH01、偏置除去执行部MH02、旋转动作判断部MH03、方位角统合运算部MH04这四个运算功能。

<方位角质量判断部MH01>

方位角质量判断部MH01利用液压挖掘机100的姿势信息，对GNSS系统17计算出的上部旋转体2的方位角的质量进行判断。在本实施方式中，作为液压挖掘机100的姿势信息，使用由车身IMU13和动臂IMU14获取到的姿势角信息。

在此，首先说明对GNSS系统17计算出的上部旋转体2的方位角的质量进行判断的必要性。

由于液压挖掘机100的前作业机1由金属部件构成，所以可以想到GNSS的卫星信号会被前作业机1反射或屏蔽。

另外，在液压挖掘机100的挖掘动作中，由于前作业机1反复进行上下动作，所以会成为反复发生卫星信号反射或中断那样的环境。

使用图13～图16说明在这样的使用环境中使用GNSS天线17a、17b进行GNSS测位的情况下的课题。

图13是液压挖掘机100的俯视图。图14A～图14C是表示前作业机1下降的状态下的卫星信号的接收状况的图。图15A～图15D是表示前作业机1抬升的状态下的卫星信号的接收状况的图。图16是表示液压挖掘机100被配置于倾斜面的状态下的卫星信号的接收状况的图。

如图13所示，GNSS天线17a、17b被配置于前作业机1的中心线1a左右。

如图14A所示，在动臂4处于下降姿势、前作业机1下降的状态下，由于与GNSS天线17a、17b相比前作业机1处于较低的位置，所以屏蔽物相对于GNSS天线17a、17b的天顶方向的卫星信号可接收区域200的图像成为图14B及图14C那样。图14B与在图13中配置在左侧的GNSS天线17a的俯视图相对应，图14C与在图13中配置在右侧的GNSS天线17b的俯视图相对应。如图14B及图14C所示，由于在各个GNSS天线17a、17b的上空没有任何屏蔽物，所以为难以发生卫星信号屏蔽或反射的状况。

另一方面，如图15A所示，在动臂4处于抬升姿势、前作业机1抬升的状态下，与GNSS天线17a、17b相比前作业机1处于较高的位置。在这样的状况下，GNSS天线17a、17b的天顶方向的卫星信号可接收区域200被前作业机1局部遮挡，GNSS天线17a、17b无法再接收到存在前作业机1的方向的卫星信号。而且，由于GNSS天线17a、17b相对于前作业机1配置在左右，所以要对被屏蔽的区域如图15B及图15C所示那样成为对称关系的情况加以注意。图15B与在图13中配置在左侧的GNSS天线17a的俯视图相对应，图15C与在图13中配置在右侧的GNSS天线17b的俯视图相对应。

另外，在图15B及15C中涂黑的部位示出了在某一时刻前作业机1屏蔽卫星信号的区域，涂灰色的部位示意地示出了由于前作业机1的连续的姿势变化而卫星信号被屏蔽的区域。

另外，如上述那样，在使用两根GNSS天线17a、17b计算上部旋转体2的方位角的情况下，实施将GNSS天线17a、17b中的一方设为主天线、将另一方设为副天线的RTK测位。此时，由于仅两根GNSS天线17a、17b能够共通地利用的卫星被用于测位，所以成为如图15D所示那样在组合了各个GNSS天线17a、17b中的屏蔽区域得到的区域中卫星信号被屏蔽的状况。也就是说，有仅由于使前作业机1抬升而天顶方向的一半的卫星信号无法再用于测位的隐忧。在这样的状况下能够利用卫星信号的卫星配置显著失衡，因此方位角的运算精度降低。

另外，由于在液压挖掘机100中频繁地进行前作业机1的抬升下降，所以尽管没有旋转上部旋转体2，通过GNSS测位计算出的方位角也会有所偏差。若将该计算结果用于机械指导，则看上去像是上部旋转体2在摇摇晃晃地动作。

而且，如图16所示那样，在液压挖掘机100被配置于倾斜地面的情况等液压挖掘机100的车身(上部旋转体2)在前后方向具有倾斜度的情况下，即使是动臂下降姿势，也有可能发生天顶方向被屏蔽的状况。在这样的状况下由于能够被利用卫星信号的卫星配置失衡，所以方位角的运算精度降低。

返回到图4，说明方位角质量判断部MH01中的方位角质量的判断条件。

如图14A所示那样，在动臂4处于下降姿势时，由于在GNSS天线17a、17b的上空部不存在障碍物，所以判断成GNSS系统17运算出的上部旋转体2的方位角的质量良好。另一方面，如图15A所示那样，在动臂4处于抬升姿势时，由于特定方向的卫星信号被屏蔽，所以判断成GNSS系统17运算出的上部旋转体2的方位角的质量差。方位角质量判断部MH01基于这样的考虑方法对方位角质量进行判断。即，将由动臂IMU14获取到的动臂角作为前作业机1的姿势角而输入，在动臂角(姿势角)为规定值以上时，判断成上部旋转体2的方位角的质量良好，若比规定值小则判断成方位角的质量差。在此，规定值例如以水平面为基准为30deg。

另外，如图16所示，在液压挖掘机100被配置于倾斜地面的情况下，与图14A的状况相比，也判断成GNSS系统17运算出的方位角的质量差。因此，在该情况下也是，方位角质量判断部MH01将由车身IMU13获取到的车身倾斜角(作为上部旋转体2的前后方向(绕y轴)的倾斜度的俯仰角)作为车身姿势角而输入，在车身姿势角(俯仰角)为规定值以上时，判断成上部旋转体2的方位角的质量良好，若比规定值小则判断成方位角的质量差。在此，规定值例如以水平面为基准为15deg。

上述动臂角和车身倾斜角的规定值作为用于判断上部旋转体2的方位角的质量的判定值，被存储于控制器20的存储装置(例如ROM)。

图5A是表示方位角质量判断部MH01中的质量判断处理的其他例子的图。

方位角质量判断部MH01也可以具有图5A所示那样的方位角质量运算部MH01-A。方位角质量运算部MH01-A构成为基于上述的判断条件，将动臂角度和车身倾斜角度(车身姿势角、即作为上部旋转体2的前后方向(绕y轴)的倾斜度的俯仰角)作为输入、将方位角质量作为输出的三维表。在该三维表中，方位角质量以0到1的数值表示，设定为方位角质量越低则方位角质量的数值越大的方式。即，数值为0时表示方位角质量最佳，为1时表示方位角质量最差。

方位角质量运算部MH01-A的三维表如以下那样构成。

首先，三维表被设定为例如即使动臂角度为最小值(动臂下降)但若车身倾斜角(俯仰角)大则方位角质量也会成为1。另外，三维表被设定为即使车身倾斜角度(俯仰角)为最小值(在车身保持水平时)但若动臂角度大(若动臂抬升)则方位角质量也会成为1。而且，三维表被设定为例如在车身倾斜角度(俯仰角)为10度且动臂角度为10度时计算出的方位角质量的值(B)比在车身倾斜角度(俯仰角)为0度且动臂角度为10度时计算出的方位角质量的值(A)大(A＜B)。

图5B是表示方位角质量判断部MH01中的质量判断处理的另一其他例子的图。

方位角质量判断部MH01也可以具有图5B所示那样的方位角质量运算部MH01-B。如图5B所示，方位角质量运算部MH01-B具有在动臂角度为规定位置以上时使方位角质量成为1的二维表T1、和在车身倾斜角度(俯仰角)为规定值以上时方位角质量成为1的二维表T2，通过将以该两个二维表T1、T2计算出的方位角质量的值相加并以1使相加后的值饱和，而计算方位角质量。

方位角质量判断部MH01使用在方位角质量运算部MH01-A或BMH01-B中计算出的方位角质量，若该方位角质量为规定值、例如0.8以上则判断成方位角质量良好，若比规定值小则判断成方位角质量差。

<偏置除去执行部MH02>

偏置除去执行部MH02实施车身IMU13检测出的角速度(旋转方向(绕z轴)的角速度)的偏置、所谓陀螺偏置的除去。

关于陀螺偏置的除去，通常为适用卡尔曼滤波(KF：Kalman Filter)或观测器的方法。在本发明的实施方式中，仅说明利用了卡尔曼滤波的陀螺偏置的除去方法。关于卡尔曼滤波的详情，例如请参照“足立、丸田：卡尔曼滤波的基础、东京电机大学出版局、2012”。

在此，若将上部旋转体2的方位角设为θ、将实际的上部旋转体的角速度设为ω、将车身IMU13检测出的角速度设为ω^mes、将陀螺偏置设为ω^b、将控制器20的运算周期设为Δt，则下述的式(1)、(2)成立。

【数1】

θ_k＝θ_k-1+Δtω_k-1...式(1)

【数2】

此外，下标k、k-1是各运算中的表示时刻的符号。关于式(1)、(2)，将状态向量x_k如式(3)那样定义。

【数3】

而且，若将GNSS接收机17c计算出的方位角θ设为观测值y_k，则能够导出下述式(4)、(5)。

【数4】

【数5】

y_k＝[1 0]x_k...式(5)

由于对式(4)、(5)适用KF，所以为简化了式(4)、(5)的各矩阵的表现。

【数6】

【数7】

y_k＝H_kx_k+v_k...式(a2)

在此，新追加的W_k-1、v_k分别为过程噪声、观测噪声。过程噪声、观测噪声均是服从平均值为0的正太分布的噪声，w_k的方差是Q_k，v_k的方差是R_k。该Q_k、R_k均被作为设计值处理。

若将状态向量x的推断值设为x^(^标注在x的上方)、将状态向量x的协方差矩阵设为P，则能够通过下式给出卡尔曼滤波的预测式(a3)～(a6)及更新式(a7)～(a9)。

【数8】

【数9】

【数10】

【数11】

【数12】

【数13】

【数14】

P_k|k＝P_k|k-₁-K_kH_kP_k|k-1...式(a9)

通过以上的运算，能够逐次推断由方位角θ_k和陀螺偏置ω^b构成的状态向量x^(^标注在x的上方)。

但是，在由方位角质量判断部MH01计算的方位角质量低的情况下，由于作为观测值y_k的方位角相对于真实的方位角具有偏差，所以陀螺偏置ω^b的推断值产生误差的可能性高。为了解决该现象，期望以仅在方位角质量高的情况下陀螺偏置的推断有效的方式构建逻辑。

例如，在使用卡尔曼滤波的情况下，在方位角质量低的情况下以非常大的值设定观测噪声的方差R。若将观测噪声的方差R设为大的值，则式(a5)的Pxy取大的值。因此，式(a7)的逆矩阵接近0，由此卡尔曼增益也接近0。

在卡尔曼增益K成为0时，基于卡尔曼滤波生成的更新式(a8)、(a9)能够如下那样简化。

【数15】

【数16】

P_k|k＝P_k|k-1...式(a11)

式(a10)、(a11)正是预测式(a3)、(a4)被直接输出的式子。而且，由于式(a3)从式(3)、(4)变成

【数17】

，所以陀螺偏置的推断值的更新停止。

偏置除去执行部MH02将从车身IMU13计算出的角速度ωmes减去了在上述运算中计算出的陀螺偏置ω^b的推断值(上述式(a12)的值)得到的值作为除去了陀螺偏置的角速度ω而输出。

<旋转动作判断部MH03>

旋转动作执行判断部MH03使用除去了由偏置除去执行部MH02计算出的陀螺偏置的角速度ω，判断上部旋转体2有无旋转动作。具体而言在角速度ω的绝对值为规定值(例如0.5deg/s)以上时判断成上部旋转体2在旋转。该规定值当然需要根据车身IMU13的性能进行设计。

此外，角速度ω的取绝对值的理由是因为上部旋转体2能够自如地进行绕左(+的角速度)、绕右(-的角速度)的旋转。

<方位角统合运算部MH04>

方位角运算统合运算部MH04使用方位角质量判断部MH01、偏置除去运算执行部MH02、旋转动作执行判断部MH03各自的运算结果、和从GNSS系统17输出的上部旋转体2的方位角，进行修正方位角的计算。以下示出具体的处理内容。

首先，在由方位角质量判断部MH01运算出的方位角质量良好的情况下，不论旋转动作执行判断部MH03中判断成有无旋转动作，均将来自GNSS接收机17c的计算值即上部旋转体2的方位角θ直接作为修正方位角θ_m输出。也就是说，成为θ_m＝θ。

但是，在GNSS接收机17c的采样周期比车身IMU13的采样周期慢的情况下，也可以通过由偏置除去运算执行部MH02计算出的角速度ω对各采样周期期间的方位角进行插补。例如，只要遵照控制器20的运算周期Δt，对GNSS系统17输出的方位角θ逐次加上“Δt×ω”即可。

以下说明在由方位角质量判断部MH01运算出的方位角质量不良的情况下的方位角统合运算部MH04的运算内容。

首先，在由旋转动作执行判断部MH03判断成无旋转动作的情况下，方位角统合运算部MH04将刚在判断成方位角质量不良之前的方位角θ₀作为修正方位角θ_m而输出。

这是利用了只要没有实施旋转动作则方位角不会发生变化这一事实。此外，控制器20将来自GNSS接收机17c的计算值即上部旋转体2的方位角θ逐次存储于存储装置20f(参照图2)，并在判断成方位角质量不良时，将刚在此之前存储于存储装置20f的方位角θ读为方位角θ₀，由此能够将刚在方位角质量被判断成不良之前的方位角θ₀作为修正方位角θ_m而输出。

接着，在由旋转动作执行判断部MH03判断成有旋转动作的情况下、且在从判断成方位角质量不良起的时间t小于规定值Tmax时，方位角统合运算部MH04以刚在方位角质量被判断成不良之前的方位角θ₀为基准值，将对该方位角θ₀加上了陀螺偏置除去执行部MH02的输出ω的累计值得到的值作为修正方位角θ_m而输出。也就是说修正方位角θ_m被以下式给出。

【数18】

θ_m＝θ_o+∑Δtω...式(6)

而且，在由旋转动作执行判断部MH03判断成有旋转动作的情况下、且在方位角质量被判断成不良起的时间t为规定值Tmax以上时，方位角统合运算部MH04使低精度警告标志有效。该状态表示无法用由偏置除去执行部MH02计算出的角速度的积分值计算出足够精度的方位角。因此，期望修正方位角θ_m以式(7)的值停止。此外，整数N是到规定时间为止的抽样数，为“Tmax÷Δt”。

【数19】

遵照图6的流程图对此前说明过的修正方位角运算部20a-1的运算步骤进行说明。

在步骤FC01中，按控制器20的每个运算周期从各种传感器(IMU13～16、GNSS系统17)进行信息获取。

在步骤FC02中，进行从IMU13～16获取到的各姿势角是否为有可能使方位角质量降低的值的确认。例如，在动臂角为规定值(例如以水平面为基准为30deg等)以上的情况下，进行肯定判定而转移到步骤FC03。当转移到步骤FC03后，将方位角质量良好这一情况存储于存储装置20f(参照图2)。另一方面，在进行了否定判定的情况下，转移到步骤FC04，并在存储装置20f中存储为方位角质量不良。

此外，由于步骤FC02～FC04是方位角质量判断部MH01的运算内容，所以也可以不经由判断部而如图5那样设为表参照。

在步骤FC05中，参照在步骤FC03、FC04中存储于存储装置20f的方位角质量，在该值良好(是)的情况下进行向步骤FC06转移的判断，在不良(否)的情况下进行向步骤FC07转移的判断。

在转移到步骤FC06的情况下，由于从GNSS系统17获取到的方位角的可靠性高，所以使观测噪声的方差为小的值。另一方面，在转移到步骤FC07的情况下，由于从GNSS系统17获取到的方位角的可靠性低，所以使观测噪声的方差R为大的值。

在步骤FC08中，进行利用了在步骤FC06、FC07中确定出的方差R的卡尔曼滤波(KF)的运算，推断陀螺偏置。

当步骤FC08的处理完成后，转移到步骤FC21。在步骤FC21中，通过从车身IMU13获取到的角速度ωmes减去在步骤FC08中推断出的陀螺偏置ω^b而实施陀螺偏置除去操作。

以上的步骤FC05～FC08的一系列运算相当于陀螺偏置除去执行部MH02。

在步骤FC09中，对由陀螺偏置除去执行部MH02计算出的角速度ω和规定值进行比较。若角速度ω的绝对值小于规定值(是)，则转移到步骤FC10，判断成无旋转动作。另一方面，若角速度ω的绝对值为规定值以上(否)，则转移到步骤FC11，判断成有旋转动作。

以上的步骤FC09～FC11的一系列运算相当于旋转动作执行判断部MH03。

在步骤FC12中，再次参照存储于存储装置20f的方位角质量，在方位角质量良好(是)的情况下向步骤FC13转移，在不良(否)的情况下向步骤FC15转移。

在转移到步骤FC13的情况下，由于方位角质量良好，所以关于修正方位角θ_m，将GNSS系统17输出的方位角θ作为修正方位角θ_m而输出。然后，转移到步骤FC14，使与低精度警告相关的标志无效，结束运算。

在转移到步骤FC15的情况下，参照在步骤FC10、FC11中确定出的旋转动作的有无，在无旋转动作的情况下(是的情况下)转移到步骤FC16，另一方面在有旋转动作的情况下(否的情况下)转移到步骤FC17。

在转移到步骤FC16的情况下，即使方位角质量不良，由于没有实施旋转动作，所以也会将刚在方位角质量成为不良之前的方位角θ₀作为修正方位角θ_m而输出。然后，转移到步骤FC14，使与低精度警告相关的标志无效，结束运算。

在转移到步骤FC17的情况下，确认从方位角质量成为不良起的经过时间t是否小于规定时间Tmax。在经过时间t小于规定时间(是)的情况下转移到步骤FC18。另一方面，在经过时间为规定值以上(否)的情况下转移到步骤FC19。

在转移到步骤FC18的情况下，修正方位角θ_m通过式(6)进行计算。然后，转移到步骤FC14，使与低精度警告相关的标志无效，结束运算。

在转移到步骤FC19的情况下，修正方位角θ_m通过式(7)进行计算。然后，转移到步骤FC20，使与低精度警告相关的标志有效，结束运算。

如以上那样在本实施方式中，控制器20在位置姿势运算部20a中，在由车身IMU13(车身姿势角获取装置)获取到的上部旋转体2的姿势角和由动臂IMU14(前姿势角获取装置)获取到的前作业机1的姿势角中的至少一方为规定值以上时，判断成由GNSS接收机17c计算出的上部旋转体2的方位角的质量低，基于方位角的质量和由GNSS接收机17c计算出的上部旋转体2的方位角，执行从由车身IMU13(角速度获取装置)获取到的上部旋转体2的角速度除去陀螺偏置的偏置除去运算，并且，基于除去了陀螺偏置的上部旋转体2的角速度判断上部旋转体2有无旋转动作，基于由GNSS接收机17c计算出的上部旋转体2的方位角、除去了陀螺偏置的上部旋转体2的角速度和上部旋转体2的旋转动作的判断结果计算出上部旋转体2的修正方位角，因此，不论前作业机1的抬升下降等液压挖掘机100的姿势变化如何，均能够利用GNSS计算出准确的上部旋转体2的方位角，并高精度且稳固地获取上部旋转体2的方位角。因此能够减少使机械指导或机械控制的动作停止的频度，从而提高作业效率。

<第2实施方式>

第1实施方式是仅使用追加安装于液压挖掘机100的传感器的结构。

本发明的第2实施方式通过利用原本安装于液压挖掘机100的传感器而实现运算内容的容易化或稳固化。

首先，若概略地说明本实施方式的特征则如以下那样。

在本实施方式中，控制器20在进行由GNSS接收机17c计算出的上部旋转体2的方位角的质量的判断时，作为由动臂IMU14(前姿势角获取装置)获取到的前作业机1的姿势角的备用而输入指示动臂4的抬升动作(前作业机1的动作)的操作杆装置9a(第1操作杆装置)的操作信号，并基于该操作杆装置9a的操作信号，在操作杆装置9a的前作业机1的抬升方向操作持续了规定时间以上时，判断成上部旋转体2的方位角的质量低。规定时间是例如在前作业机1处于动臂抬升开始时的标准姿势时将操作杆装置9a向动臂抬升方向进行满操作、直至动臂4的角度达到上述的规定值的经过时间，作为用于判断上部旋转体2的方位角的质量的判定值，被存储于控制器20的存储装置(例如ROM)。

另外，控制器20在进行上部旋转体2有无旋转动作的判断时，作为除去了陀螺偏置的上部旋转体2的角速度的备用而输入操作杆装置9b(第2操作杆装置)的操作信号，并基于该操作杆装置9b的操作信号，在操作杆装置9b的操作信号的大小为规定值以上时，判断成上部旋转体2有旋转动作。规定值是在产生了操作信号时旋转马达2a开始旋转的操作信号的值，作为上部旋转体2有无旋转动作的判定值，被存储于控制器20的存储装置(例如ROM)。

以下基于图7说明上述的控制器20的处理内容的详情。图7是表示本实施方式中的修正方位角运算部20a-1的处理功能的框图。

本实施方式中的修正方位角运算部20a-1向图4所示的方位角质量判断部MH01及旋转动作执行判断部MH03作为追加信号进一步输入操作杆装置9a、9b的操作信号(电气信号)。

方位角质量判断部MH01除了使用了从上述的车身IMU13及动臂IMU14输出的姿势角信息的判断以外，甚至还通过利用从操作杆装置9a输出的动臂抬升的操作信号来判断方位角质量。例如，当基于从操作杆装置9a输出的动臂抬升的操作信号进行的动臂抬升的操作持续了规定时间后，动臂缸4a逐渐伸长，最终达到图15A的姿势。也就是说，即使不获取动臂IMU14的姿势角，通过利用动臂抬升的操作信号及其经过时间，也能够推测方位角质量的判断所需的前作业机1的姿势。

像这样，只要利用操作杆装置9a的操作信号，即使在万一发生了因故障等而无法利用动臂IMU14的信号的状况的情况下，也能够继续进行方位角质量的判断，从而能够实现提高作为系统的稳固性。

另外，本实施方式中的旋转动作执行判断部MH03除了使用了从偏置除去执行部MH02输出的角速度ω的判断之外，还通过利用从操作杆装置9b输出的旋转操作信号来判断有无旋转动作。

在与旋转操作相关的操作信号的值小于规定值(例如按照先导压换算值小于0.5MPa)的情况下，由于液压油不会流入旋转马达2a，所以上部旋转体2不会旋转，但若操作信号的值成为旋转马达2a旋转的操作信号的下限值即规定值以上(例如按照先导压换算值为1.0MPa以上)，则液压油会流入旋转马达2a而上部旋转体2旋转。

因此，只要对与旋转动作相关的操作信号进行确认，就能够在车身IMU13检测出角速度的变化之前判断有无旋转动作。像这样使用了旋转操作信号的旋转动作的判定在陀螺偏置除去执行部MH02中的陀螺偏置的除去性能不充分、无法充分地降低图6的流程图中的步骤FC09的用于判断旋转动作的角速度的阈值的情况下，尤其有效。

此外，在第1及第2实施方式中，向方位角质量判断运算部MH01仅输入了动臂IMU14的姿势角，但本发明并不限定于此。在想要更准确地判断前作业机1的姿势的情况下，只要以输入动臂IMU14、斗杆IMU15、铲斗IMU16全部的姿势角的方式进行变更即可。

另外，在第2实施方式中，说明了操作杆装置9a、9b为电气杆方式的情况，但在操作杆装置9a、9b为液压先导方式的情况下，只要设置对与各个操作杆的操作方向及操作量相应的先导压进行检测的压力传感器、且输入该压力传感器的信号即可。

像这样根据本实施方式，由于将操作杆装置9a、9b的操作信号用作前作业机1的姿势角或上部旋转体2的角速度的备用，所以能够进一步提高作为系统的稳固性，从而能够提高作业效率。

<第3实施方式>

第1及第2实施方式是对前作业机1使用由IMU代表的倾斜角检测器的实施方式，但本发明即使不对前作业机1使用倾斜角检测器也能够实施。

首先，若概略地说明本实施方式的特征则如以下那样。

在本实施方式中，液压挖掘机100作为车身IMU13(车身姿势角获取装置)及动臂IMU14(前姿势角获取装置)中的至少一方的姿势角获取装置而具备图像识别装置35(参照图8)，图像识别装置35基于从至少一台摄像头30或32或33(参照图8)获取到的图像信息识别上部旋转体2的姿势角和前作业机1的姿势角中的至少一方，控制器20基于由图像识别装置35识别出的上部旋转体2的姿势角和前作业机1的姿势角中的至少一方对由GNSS接收机17c计算出的上部旋转体2的方位角的质量进行判断，在判断成上部旋转体2的方位角的质量低时执行偏置除去运算。

以下使用图8～图10说明第3实施方式的详情。

图8是表示本实施方式中的液压挖掘机100的外观及其周围状况的图。此外，为了简化说明，表示与图1相同的结构要素的附图标记省略了图示。

与本实施方式相关的液压挖掘机100具备设置在操作室9的顶部前侧的立体摄像头30、和设置在操作室9的顶部后侧的无线通信机31a。无线通信机31a与作业现场内所具备的其他无线通信机31b进行无线通信。无线通信机31b将作业现场内所具备的管理用摄像头32拍摄到的图像信息向作业现场内发布。此外，管理用摄像头32并不限定于固定于作业现场内的摄像头。例如，也可以将摄像头33和无线通信机31c搭载于能够在作业现场内自由飞行的无人机(drone)34。

图9是表示本实施方式中的控制器20的处理功能的框图。此外，为了简化说明，图3所示的施工目标面运算部20b及液压系统控制部20d省略了图示。

本实施方式中的控制器20的处理功能除了变更为输入设置于图8所示的液压挖掘机的立体摄像头30及无线通信机31b的信息的方面以外，均与图3所示的第1实施方式中的控制器20的处理功能相同。控制器20作为其功能的变更部分，追加了基于获取到的图像信息计算出前作业机的姿势信息的图像识别部20g。将立体摄像头30、管理用摄像头32、图像识别部20g整合得到的结构相当于上述的图像识别装置35。

通过图像识别装置35的图像识别部20g计算的姿势信息较大程度地依存于立体摄像头30和管理用摄像头32的性能(析像度等)。在本发明中，只要具有能够判断前作业机1是抬升姿势(图15A)还是下降姿势(图14A)的性能则足够。

在本实施方式中，在修正方位角运算部20a-1中利用由上述的图像识别部20g运算出的姿势信息。

图10是表示本实施方式中的修正方位角运算部20a-1的处理功能的框图。

在图10中，在修正方位角运算部20a-1中，对图4所示的方位角质量判断部MH01及旋转动作执行判断部MH03，作为追加信号进一步输入来自图像识别部20g的信号。

方位角质量判定部MH01除了由车身IMU13获取到的车身倾斜角的信息以外，还使用由图像识别部20g运算出的前作业机1的抬升姿势、下降姿势信息来判断方位角质量。也就是说，在由图像识别部20g运算出的姿势信息中，若前作业机1为抬升姿势，则判断成方位角质量差。

此外，在利用设定于无人机34的管理用摄像头33的情况下，不仅能够获取液压挖掘机100自身的图像信息还能够获取液压挖掘机100周边的图像信息。在这样的情况下，方位角质量运算部MH01也可以不仅考虑液压挖掘机100自身的姿势信息还考虑周边障碍物的信息来计算方位角质量。

另外，在图像识别部20g中，也可以不仅运算前作业机1的姿势角还运算上部旋转体2的姿势角，并在方位角质量运算部MH0中利用该上部旋转体2的姿势角。但是，由于该情况下的姿势角为基于图像处理得到的姿势信息，所以方位角质量的判断精度比车身IMU13差，因此期望限定于作为备用的利用。

对于旋转动作执行判断部MH03，也可以利用由图像识别部20g获取到的上部旋转体2的姿势信息。但是，在该情况下也是，基于通过图像处理得到的姿势信息进行的有无旋转动作的判断精度比车身IMU13差，因此期望限定于作为备用的利用。

此外，在图8的液压挖掘机100中设为使用了立体摄像头30和管理用摄像头32双方的结构，但当然也能够通过任一方执行本发明。

像这样根据本实施方式，即使不使用动臂IMU14或车身IMU13，也能够不论前作业机1的抬升下降等液压挖掘机100的姿势变化如何，均利用GNSS计算出准确的上部旋转体2的方位角，从而高精度且稳固地获取上部旋转体2的方位角。

另外，在将由图像识别装置35获取到的姿势信息用作动臂IMU14或车身IMU13的备用的情况下，能够进一步提高作为系统的稳固性，从而提高作业效率。

<第4实施方式>

例如，在山谷部的作业现场，存在如图17所示那样在液压挖掘机100的背后具有陡峭的倾斜面的情况。在这样的状况下，由于无法接收液压挖掘机100的背面的卫星信号，所以方位角的质量当然也会降低。

本发明的第4实施方式是鉴于这样的状况而做出的。

首先，若概略地说明本实施方式的特征则如以下那样。

在本实施方式中，控制器20具备存储了液压挖掘机100(工程机械)周边的地形信息的地形信息存储装置20g(参照图11)，并基于由GNSS接收机17c计算出的GNSS天线17a、17b的三维坐标，读入存储于地形信息存储装置20g的液压挖掘机100(工程机械)周边的地形信息，除了基于由车身IMU13(车身姿势角获取装置)获取到的上部旋转体2的姿势角和由动臂IMU14(前姿势角获取装置)获取到的前作业机1的姿势角进行的上部旋转体2的方位角的质量判断以外，或者代替该判断，在液压挖掘机100(工程机械)的周边地形有可能屏蔽GNSS天线17a、17b的卫星信号可接收区域200时，判断成由GNSS接收机17c计算出的上部旋转体2的方位角的质量低，执行偏置除去运算。

以下基于图11说明控制器20的处理内容的详情。图11是表示本实施方式中的控制器20的处理功能的框图。

在本实施方式中，控制器20还具备周边地形参照部20h。

周边地形参照部20h输入由GNSS系统17获取到的GNSS天线17a、17b的三维坐标信息，参照液压挖掘机周边的地形信息，将该参照的地形信息输出到修正方位角运算部20a-1。

图12是表示本实施方式中的修正方位角运算部20a-1的处理功能的框图。

如图12所示那样，本实施方式中的修正方位角运算部20a-1的特征在于，能够不仅根据液压挖掘机100的姿势信息(前作业机1的抬升下降等液压挖掘机100的姿势变化)，还根据周边的地形信息确定方位角质量。也就是说，在本车位置的周边具有屏蔽卫星信号那样的地形(图17的陡坡等)的情况下，不论液压挖掘机100的姿势如何，均判断成方位角质量低下。

根据本实施方式，不论前作业机1的抬升下降等液压挖掘机100的姿势变化如何，且/或不论液压挖掘机100的周边地形如何，均能够高精度且稳固地获取上部旋转体2的方位角，提高作业效率。

此外，虽然以上说明了第1～第4实施方式，但也可以设为将这些实施方式中的某两个以上的实施方式组合得到的结构，该结构当然也与本发明的范围相符。

附图标记说明

1 前作业机

2 上部旋转体

3 下部行驶体

4 动臂

5 斗杆

6 铲斗

9 操作室

9a、9b 操作杆装置

11 监视器

13 惯性计测装置(车身IMU)(角速度获取装置)(车身姿势角获取装置)(车身倾斜传感器)

14 惯性计测装置(动臂IMU)(前姿势角获取装置)(前倾斜传感器)

15 惯性计测装置(斗杆IMU)(前姿势角获取装置)

16 惯性计测装置(铲斗IMU)(前姿势角获取装置)

17 GNSS系统

17a、17b GNSS天线

17c GNSS接收机

20 控制器

20a 位置姿势运算部

20a-1 修正方位角运算部

20a-2 三维位置姿势运算部

20b 施工目标面运算部

20c 监视器显示控制部

20d 液压系统控制部

20e 存储装置

20f 存储装置

20g 图像识别部

20h 周边地形参照部

30 立体摄像头

31a 无线通信机

31b 无线通信机

31c 无线通信机

32 管理用摄像头

33 摄像头

34无人机(drone)

35 图像识别装置

100 液压挖掘机

200 卫星信号可接收区域

Σm 车身坐标系

MH01 方位角质量判断部

MH02 偏置除去执行部

MH03 旋转动作判断部

MH04 方位角统合运算部。

Claims

1.一种工程机械，具备：

下部行驶体；

上部旋转体，其能够相对于下部行驶体旋转；

多关节型的前作业机，其相对于上部旋转体能够沿垂直方向旋转地安装；

GNSS系统，其具有安装于所述上部旋转体的两根GNSS天线、和基于所述GNSS天线接收到的卫星信号计算出所述GNSS天线的三维坐标及所述上部旋转体的方位角的GNSS接收机；

角速度获取装置，其安装于所述上部旋转体，获取所述上部旋转体的角速度；

车身姿势角获取装置，其安装于所述上部旋转体，获取所述上部旋转体的姿势角；

前姿势角获取装置，其获取所述前作业机的姿势角；以及

控制器，

所述控制器基于由所述GNSS接收机计算出的所述GNSS天线的三维坐标及所述上部旋转体的方位角、由所述车身姿势角获取装置获取到的所述上部旋转体的姿势角、由所述角速度获取装置获取到的所述上部旋转体的角速度以及由所述前姿势角获取装置获取到的所述前作业机的姿势角，对所述前作业机的位置及姿势进行运算，并基于所述前作业机的位置及姿势控制所述前作业机的动作，所述工程机械的特征在于，

所述控制器基于由所述车身姿势角获取装置获取到的所述上部旋转体的姿势角和由所述前姿势角获取装置获取到的所述前作业机的姿势角中的至少一方，对由所述GNSS接收机计算出的所述上部旋转体的方位角的质量进行判断，

基于所述方位角的质量的判断结果和由所述GNSS接收机计算出的所述上部旋转体的方位角，执行从由所述角速度获取装置获取到的所述上部旋转体的角速度除去陀螺偏置的偏置除去运算，并且基于除去了所述陀螺偏置的所述上部旋转体的角速度判断所述上部旋转体有无旋转动作，

基于由所述GNSS接收机计算出的所述上部旋转体的方位角、除去了所述陀螺偏置的所述上部旋转体的角速度和所述上部旋转体的旋转动作的判断结果计算出所述上部旋转体的修正方位角，使用所述修正方位角对所述前作业机的位置及姿势进行运算。

2.如权利要求1所述的工程机械，其特征在于，

所述车身姿势角获取装置是安装于所述上部旋转体的至少一个车身倾斜传感器，

所述前姿势角获取装置是安装于所述前作业机的至少一个前倾斜传感器，

所述控制器基于由所述车身倾斜传感器检测出的所述上部旋转体的姿势角和由所述前倾斜传感器检测出的所述前作业机的姿势角中的至少一方，对由所述GNSS接收机计算出的所述上部旋转体的方位角的质量进行判断，在判断成所述上部旋转体的方位角的质量低下时执行所述偏置除去运算。

3.如权利要求1所述的工程机械，其特征在于，

作为所述车身姿势角获取装置及所述前姿势角获取装置中的至少一方的姿势角获取装置而具备图像识别装置，

所述图像识别装置基于从至少一台摄像头获取到的图像信息识别所述上部旋转体的姿势角和所述前作业机的姿势角中的至少一方，

所述控制器基于由所述图像识别装置识别出的所述上部旋转体的姿势角和所述前作业机的姿势角中的至少一方，对由所述GNSS接收机计算出的所述上部旋转体的方位角的质量进行判断，在判断成所述上部旋转体的方位角的质量低下时执行所述偏置除去运算。

4.如权利要求1所述的工程机械，其特征在于，

所述控制器具备存储有所述工程机械的周边的地形信息的地形信息存储装置，

基于由所述GNSS接收机计算出的所述GNSS天线的三维坐标，读入存储于所述地形信息存储装置的所述工程机械的周边的地形信息，

除了基于由所述车身姿势角获取装置获取到的所述上部旋转体的姿势角和由所述前姿势角获取装置获取到的所述前作业机的姿势角进行的所述上部旋转体的方位角的质量判断以外，或代替该判断，在所述工程机械的周边地形有可能屏蔽所述GNSS天线的卫星信号可接收区域时，判断成由所述GNSS接收机计算出的所述上部旋转体的方位角的质量低，执行所述偏置除去运算。

5.如权利要求1所述的工程机械，其特征在于，

所述控制器具备对判断成所述上部旋转体的方位角的质量高时的方位角进行存储的方位角存储装置，

并在判断成所述上部旋转体的方位角的质量低、且判断成所述上部旋转体无旋转动作时，将刚在判断成所述上部旋转体的方位角的质量低之前存储于所述方位角存储装置的方位角用作修正方位角，对所述前作业机的位置及姿势进行运算。

6.如权利要求1所述的工程机械，其特征在于，

所述控制器在判断成所述上部旋转体的方位角的质量低、且判断出所述上部旋转体有旋转动作之后，在保持所述上部旋转体的方位角的质量低的状态而经过了规定时间时，将所述上部旋转体的方位角的精度有可能低这一情况通知给用户。

7.如权利要求1所述的工程机械，其特征在于，

还具备指示所述前作业机的动作的第1操作杆装置，

所述控制器在进行由所述GNSS接收机计算出的所述上部旋转体的方位角的质量的判断时，作为由所述前姿势角获取装置获取到的所述前作业机的姿势角的备用而输入所述第1操作杆装置的操作信号，并基于该第1操作杆装置的操作信号，在所述第1操作杆装置的所述前作业机的抬升方向操作持续了规定时间以上时，判断成所述上部旋转体的方位角的质量低。

8.如权利要求1所述的工程机械，其特征在于，

还具备指示所述上部旋转体的动作的第2操作杆装置，

所述控制器在进行所述上部旋转体有无旋转动作的判断时，作为除去了所述陀螺偏置的所述上部旋转体的角速度的备用而输入所述第2操作杆装置的操作信号，并基于该第2操作杆装置的操作信号，在所述第2操作杆装置的操作信号的大小为规定值以上时，判断成所述上部旋转体有旋转动作。