KR102479701B1

KR102479701B1 - 작업 기계

Info

Publication number: KR102479701B1
Application number: KR1020217017803A
Authority: KR
Inventors: 신지 이시하라; 시호 이즈미; 야스히코 가나리
Original assignee: 히다찌 겐끼 가부시키가이샤
Priority date: 2018-12-27
Filing date: 2019-11-21
Publication date: 2022-12-22
Also published as: JP7182458B2; KR20210088691A; JP2020105802A; EP3904606B1; US20220056674A1; CN113167052A; WO2020137275A1; EP3904606A1; CN113167052B; EP3904606A4

Abstract

상부 선회체(2)를 하부 주행체(3)에 대하여 미리 정한 방향을 향한 제1 교정 자세에 있어서의 프런트 작업기(1)의 관성 계측 장치(14 내지 16) 및 차체 관성 계측 장치(13)의 검출 결과에 기초하여, 상부 선회체(2)의 선회축에 수직으로 전방을 향하여 설정된 제1축 주위에 있어서의 차체 관성 계측 장치(13)의 설치 오차를 산출하고, 상부 선회체(2)를 하부 주행체(3)에 대하여 제1 교정 자세로부터 90도 선회시킨 제2 교정 자세에 있어서의 차체 관성 계측 장치(13)의 검출 결과와, 차체 관성 계측 장치(13)의 제1축 주위에 있어서의 설치 오차에 기초하여, 상부 선회체(2)의 선회축 및 제1축에 수직으로 측방을 향하여 설정된 제2축 주위에 있어서의 차체 관성 계측 장치(13)의 설치 오차를 산출한다. 이에 의해, 자세 정보를 검출하기 위한 센서 설치 오차를 고정밀도로 산출할 수 있고, 센서의 교정 정밀도를 향상시킬 수 있다.

Description

작업 기계

본 발명은, 작업기를 갖는 작업 기계에 관한 것이다.

근년, 정보화 시공에 대한 대응에 수반하여, 작업 기계에 있어서 붐, 암, 버킷 등의 피구동 부재를 갖는 작업기의 자세나 버킷 등의 작업구의 위치를 오퍼레이터에 대하여 표시하는 머신 가이던스의 기능이나, 버킷 등의 작업구가 시공 목표면을 따라 움직이도록 제어하는 머신 컨트롤의 기능 등을 갖는 것이 실용화되고 있다. 이러한 기능의 대표적인 것으로서는, 유압 셔블의 버킷 선단 위치와 버킷 각도를 모니터에 표시하거나, 버킷 선단이 시공 목표면에 일정 이상 근접하지 않도록 동작에 제한을 가하거나 하는 경우가 있다.

이러한 기능을 실현하는 데는, 작업기의 자세 연산이 필요하고, 이 자세 연산의 정밀도가 높을수록 질이 높은 시공을 실현할 수 있다. 작업기의 자세를 연산하기 위해서는, 예를 들어 포텐시오미터나 관성 계측 장치(IMU) 등의 센서를 사용하여 붐, 암, 버킷의 각각의 회전 각도를 검출할 필요가 있다.

또한, 고정밀도의 자세 연산에는, 센서의 설치 위치나 각도 등을 정확하게 파악할 필요하다. 그러나, 실제의 운용에 있어서는, 센서를 작업 기계에 설치할 때에 설치 오차가 발생하기 때문에, 작업 기계의 작업기의 자세를 정확하게 연산하기 위해서는, 그러한 오차를 보정하기 위한 어떠한 교정 수단을 구비할 필요가 있다.

이러한 센서의 교정에 관한 기술로서, 예를 들어 특허문헌 1에는, 하부 주행체와, 미리 설정된 선회축 주위에 선회 가능하게 되도록 상기 하부 주행체 상에 마련된 상부 선회체를 구비한 건설기계이며, 미리 설정된 X 검출축 주위의 각속도를 검출하는 X축 자이로 센서와, 상기 X 검출축과 직교하는 Y 검출축 주위의 각속도를 검출하는 Y축 자이로 센서와, 상기 X 검출축 및 상기 Y 검출축과 직교하는 Z 검출축 주위의 각속도를 검출하는 Z축 자이로 센서와, 상기 각 자이로 센서에 의한 검출 결과에 기초하여 상기 선회축 주위의 상기 상부 선회체의 각속도를 산출하는 각속도 산출 수단을 구비하고, 상기 각속도 산출 수단은, 상기 X 검출축, 상기 Y 검출축 및 상기 Z 검출축에 의해 정의되는 검출용 3차원 좌표계를, 상기 선회축에 일치하는 Z 기준축과 이것에 직교하는 평면 상에서 서로 직교하는 X 기준축 및 Y 기준축에 의해 정의되는 기준 3차원 좌표계로 좌표 변환함으로써 상기 검출용 3차원 좌표계와 상기 기준 3차원 좌표계의 어긋남을 보정하여 상기 Z 기준축 주위의 각속도를 산출하기 위해서, (A) 상기 Z 검출축이 상기 Z 기준축에 대하여 상기 X 기준축 주위에 제1 각도 회전하고, 또한, 상기 Y 기준축 주위에 제2 각도 회전하고 있는 경우에, 상기 각 자이로 센서에 의한 검출 결과, 상기 제1 각도 및 상기 제2 각도를 사용하여 상기 기준 3차원 좌표계에 있어서의 3축 주위의 각속도를 각각 나타내는 3개의 관계식 중 상기 X 기준축 및 상기 Y 기준축에 관한 2개의 관계식에 의해 표시되는 각속도가 각각 0인 것을 이용하여, 이들 2개의 관계식에 기초하여 상기 제1 각도 및 상기 제2 각도를 산출하고, (B) 상기 3개의 관계식 중 상기 Z 기준축에 관한 관계식과, 산출된 상기 제1 각도 및 상기 제2 각도에 기초하여 상기 Z 기준축 주위의 각속도를 산출하는 것이 개시되어 있다.

일본 특허 공개 제2015-1385호 공보

그러나, 일반적으로 알려져 있는 바와 같이, 자이로 센서의 출력은 어떠한 바이어스(자이로 바이어스)를 갖기 때문에, 실제로 각속도가 발생하고 있지 않은 축 방향에 대해서도 자이로 센서로부터 어떠한 값이 출력되어 버린다. 또한, 자이로 바이어스는 센서 소자의 온도 등에 의해 크게 변화해 버리기 때문에, 교정 동작을 행하는 환경에 의해 교정의 정밀도가 현저하게 다른 것을 생각할 수 있다. 따라서, 자이로 바이어스의 영향에 대하여 고려하고 있지 않은 상기 종래 기술에 있어서, 자이로 센서로부터의 출력값은, 자이로 바이어스의 영향에 의해 참된 설치각으로부터 오차를 가진 값이 된다.

또한, 상기 종래 기술에 있어서는, 교정의 과정에 있어서 역정접 함수(tan＾(-1))를 이용하고, 또한, 역정접 함수의 변수의 분모 부분에 선회각 속도 ωz, 분자 부분에 각속도 ωy가 포함되어 있고, 각속도 ωy의 노이즈 등의 영향을 작게 하기 위해서는, 분모의 선회각 속도 ωz를 크게 할 필요가 있다. 그러나, 각속도 ωy의 노이즈 등의 영향이 작아지는 것과 같은 선회각 속도 ωz가 얻어질수록 선회 속도를 크게 하는 것은 곤란하고, 교정의 정밀도를 향상시킬 수 없다.

본 발명은 상기에 감안하여 이루어진 것이고, 자세 정보를 검출하기 위한 센서의 설치 오차를 고정밀도로 산출할 수 있고, 센서의 교정 정밀도를 향상시킬 수 있는 작업 기계를 제공하는 것을 목적으로 한다.

본원은 상기 과제를 해결하는 수단을 복수 포함하고 있지만, 그 일례를 들면, 하부 주행체와, 상기 하부 주행체에 대하여 선회 가능하게 마련된 상부 선회체로 이루어지는 차체와, 상기 차체에 설치되고, 회동 가능하게 연결된 복수의 프런트 부재로 이루어지는 다관절형의 작업기와, 상기 작업기의 적어도 하나의 프런트 부재에 마련되어, 그 프런트 부재의 자세 정보를 검출하는 하나 이상의 제1 자세 검출 장치와, 상기 상부 선회체에 마련되어, 상기 상부 선회체의 자세 정보를 검출하는 제2 자세 검출 장치와, 상기 제2 자세 검출 장치의 설치 오차를 산출하는 제어 장치를 구비한 작업 기계에 있어서, 상기 제어 장치는, 상기 상부 선회체를 상기 하부 주행체에 대하여 미리 정한 방향을 향한 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치 및 상기 제2 자세 검출 장치의 검출 결과에 기초하여, 상기 상부 선회체의 선회축에 수직으로 전방을 향하여 설정된 제1 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하고, 상기 상부 선회체를 상기 하부 주행체에 대하여 상기 제1 교정 자세로부터 90도 선회시킨 제2 교정 자세에 있어서의 상기 제2 자세 검출 장치의 검출 결과와, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차에 기초하여, 상기 상부 선회체의 선회축 및 상기 제1 축에 수직으로 측방을 향하여 설정된 제2 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하는 것으로 한다.

본 발명에 따르면, 자세 정보를 검출하기 위한 센서의 설치 오차를 고정밀도로 산출할 수 있고, 센서의 교정 정밀도를 향상시킬 수 있다.

도 1은, 제1 실시 형태에 따른 작업 기계의 일례인 유압 셔블의 외관을 모식적으로 도시하는 도면이다.
도 2는, 유압 셔블에 탑재되는 컨트롤러의 처리 기능의 일부를 모식적으로 도시하는 도면이다.
도 3은, 컨트롤러의 자세 연산 장치의 처리 기능을 개략적으로 도시하는 기능 블록도이다.
도 4는, 자세 검출 장치 교정부의 처리 기능을 개략적으로 도시하는 기능 블록도이다.
도 5는, 유압 셔블의 상면도이다.
도 6은, 유압 셔블의 측면도이다.
도 7은, 유압 셔블의 배면도이다.
도 8은, 유압 셔블의 측면도이다.
도 9는, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 롤 각 및 피치각의 계측 결과의 변화 일례를 도시하는 도면이다.
도 10은, 자세 연산 장치의 처리 내용을 도시하는 흐름도이다.
도 11은, 자세 연산 장치의 처리 내용을 도시하는 흐름도이다.
도 12는, 자세 연산 장치의 처리 내용을 도시하는 흐름도이다.
도 13은, 제2 실시 형태에 있어서의 자세 연산 장치의 자세 검출 장치 교정부의 처리 기능을 개략적으로 도시하는 기능 블록도이다.
도 14는, 제2 실시 형태에 있어서의 자세 연산 장치의 처리 내용을 도시하는 흐름도이다.
도 15는, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 참된 롤 각과 관성 계측 장치에 의한 검출 결과로서의 롤 각의 계측 결과의 변화 일례를 도시하는 도면이다.
도 16은, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 참된 롤 각과 관성 계측 장치에 의한 검출 결과로서의 롤 각의 계측 결과의 변화 일례를 도시하는 도면이다.

이하, 본 발명의 실시 형태를 도면을 참조하면서 설명한다. 또한, 본 실시 형태에서는, 작업 기계의 일례로서, 프런트 작업기의 선단에 작업구로서 버킷을 구비하는 유압 셔블을 예시하여 설명하지만, 브레이커나 마그네트 등의 버킷 이외의 어태치먼트를 구비하는 유압 셔블에 본 발명을 적용하는 것도 가능하다.

<제1 실시 형태>

본 발명의 제1 실시 형태를 도 1 내지 도 12를 참조하면서 설명한다.

도 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)(단, 한쪽의 주행 모터만 도시)에 의해 각각 구동된다.

붐(4), 암(5) 및 버킷(6)은, 프런트 작업기(1)를 포함하는 평면 상에서 동작하고, 이하에서는 이 평면을 동작 평면이라고 칭하는 경우가 있다. 즉 동작 평면이란, 붐(4), 암(5) 및 버킷(6)의 회동축에 직교하는 평면이고, 붐(4), 암(5) 및 버킷(6)의 폭 방향의 중심에 설정할 수 있다.

오퍼레이터가 탑승하는 운전실(9)에는, 유압 액추에이터(2a 내지 6a)를 조작하기 위한 조작 신호를 출력하는 조작 레버(조작 장치)(9a, 9b)가 마련되어 있다. 도시는 하지 않지만 조작 레버(9a, 9b)는 각각 전후 좌우로 경도 가능하고, 조작 신호인 레버의 경도량, 즉 레버 조작량을 전기적으로 검지하는 도시하지 않은 검출 장치를 포함하고, 검출 장치가 검출한 레버 조작량을 제어 장치인 컨트롤러(20)(후의 도 2 참조)에 전기 배선을 통해 출력한다. 즉, 조작 레버(9a, 9b)의 전후 방향 또는 좌우 방향으로, 유압 액추에이터(2a 내지 6a)의 조작이 각각 할당되어 있다. 또한, 상세히 기술을 생략하지만, 운전실(9)에는 주행 모터(3a)를 조작하기 위한 조작 신호를 출력하는 주행 조작 레버도 마련되어 있다.

붐 실린더(4a), 암 실린더(5a), 버킷 실린더(6a), 선회 모터(2a) 및 좌우의 주행 모터(3a)의 동작 제어는, 도시하지 않은 엔진이나 전동 모터 등의 원동기에 의해 구동되는 유압 펌프 장치(7)로부터 각 유압 액추에이터(2a 내지 6a)에 공급되는 작동유의 방향 및 유량을 컨트롤 밸브(8)로 제어함으로써 행한다. 컨트롤 밸브(8)는, 도시하지 않은 파일럿 펌프로부터 전자 비례 밸브를 통해 출력되는 구동 신호(파일럿압)에 의해 행하여진다. 조작 레버(9a, 9b)로부터의 조작 신호에 기초하여 컨트롤러(20)로 전자 비례 밸브를 제어함으로써, 각 유압 액추에이터(2a 내지 6a)의 동작이 제어된다.

또한, 조작 레버(9a, 9b)는 유압 파일럿 방식이어도 되고, 각각 오퍼레이터에 의해 조작되는 조작 레버(9a, 9b)의 조작 방향 및 조작량에 따른 파일럿압을 컨트롤 밸브(8)에 구동 신호로서 공급하고, 각 유압 액추에이터(2a 내지 6a)를 구동하도록 구성해도 된다.

상부 선회체(2)의 상부에는, GNSS(Global Navigation Satellite System)용의 수신 안테나(18)가 배치되어 있고, 수신 안테나(18)에서 수신한 신호에 기초하여 도시하지 않은 위치 연산 장치로 유압 셔블(100)의 지구 좌표계에 있어서의 위치의 연산을 행한다.

상부 선회체(2), 붐(4), 암(5) 및 버킷(6)에는, 각각, 자세 센서로서 관성 계측 장치(IMU: Inertial Measurement Unit)(13 내지 16)가 배치되어 있다. 이후, 이들의 관성 계측 장치를 구별할 필요가 있는 경우에는, 각각, 차체 관성 계측 장치(13), 붐 관성 계측 장치(14), 암 관성 계측 장치(15) 및 버킷 관성 계측 장치(16)라고 칭한다.

관성 계측 장치(13 내지 16)는, 각속도 및 가속도를 계측하는 것이다. 관성 계측 장치(13 내지 16)가 배치된 상부 선회체(2)나 각 피구동 부재(4 내지 6)가 정지하고 있는 경우를 생각하면, 각 관성 계측 장치(13 내지 16)에 설정된 IMU 좌표계에 있어서의 중력 가속도의 방향(즉, 연직 하향 방향)과, 각 관성 계측 장치(13 내지 16)의 설치 상태(즉, 각 관성 계측 장치(13 내지 16)와 상부 선회체(2)나 각 피구동 부재(4 내지 6)의 상대적인 위치 관계)에 기초하여, 각 피구동 부재(4 내지 6)의 방향(대지 각도: 수평 방향에 대한 각도) 및 상부 선회체(2)의 전후 방향 대지 각도(피치각)나 좌우 방향의 대지 각도(롤 각)를 검출할 수 있다. 여기서, 관성 계측 장치(14 내지 16)는, 복수의 피구동 부재의 각각의 자세에 관한 정보(이후, 자세 정보라고 칭한다)를 검출하는 자세 정보 검출 장치를 구성하고 있다.

도 2는, 유압 셔블에 탑재되는 컨트롤러의 처리 기능의 일부를 모식적으로 도시하는 도면이다.

도 2에 있어서, 컨트롤러(제어 장치)(20)는, 유압 셔블(100)의 소정의 위치에 탑재되는 것이며, 유압 셔블(100)의 동작을 제어하기 위한 다양한 기능을 갖고 있고, 그 일부로서 자세 연산 장치(21), 모니터 표시 제어 장치(22), 유압 시스템 제어 장치(23) 및 시공 목표면 연산 장치(24)의 각 기능부를 갖고 있다.

자세 연산 장치(21)는, 관성 계측 장치(13 내지 16)로부터의 검출 결과에 기초하여, 프런트 작업기(1)를 포함하는 유압 셔블(100)의 자세 정보를 연산하는 자세 연산 처리(후술)를 행한다. 또한, 상기 자세 연산 장치(21)에 있어서 유압 셔블(100)의 자세를 연산할 때에는, 차체(상부 선회체(2) 및 하부 주행체(3)), 붐(4), 암(5) 및 버킷(6)의 각각의 각도가 필요하고, 상기 자세 연산 장치(21)는, 각 관성 계측 장치(13 내지 16)로부터 검출 신호로서 입력되는 각 관성 계측 장치(13 내지 16) 자신의 자세 정보(대지 각도: 수평 방향에 대한 각도)를 유압 셔블(100)의 자세로 변환하고 있다.

시공 목표면 연산 장치(24)는, 도시하지 않은 기억 장치 등에 시공 관리자에 의해 미리 기억되어 있는 3차원 시공 도면 등의 시공 정보(17)에 기초하여, 시공 대상의 목표 형상을 정의하는 시공 목표면을 연산한다.

모니터 표시 제어 장치(22)는, 운전실(9)에 마련된 도시하지 않은 모니터의 표시를 제어하는 것이고, 시공 목표면 연산 장치(24)로 연산된 시공 목표면과, 자세 연산 장치(21)로 연산된 유압 셔블(100)의 자세에 기초하여, 오퍼레이터에 대한 조작 지원의 지시 내용을 연산하고, 운전실(9)의 모니터에 표시한다. 즉, 모니터 표시 제어 장치(22)는, 예를 들어 붐(4), 암(5), 버킷(6) 등의 피구동 부재를 갖는 프런트 작업기(1)의 자세나, 버킷(6)의 선단 위치와 각도, 버킷(6)과 시공 목표면의 상대 위치 등을 모니터에 표시하여 오퍼레이터의 조작을 지원하는 머신 가이던스 시스템으로서의 기능의 일부를 담당하고 있다.

유압 시스템 제어 장치(23)는, 유압 펌프 장치(7)나 컨트롤 밸브(8), 각 유압 액추에이터(2a 내지 6a) 등으로 이루어지는 유압 셔블(100)의 유압 시스템을 제어하는 것이고, 시공 목표면 연산 장치(24)로 연산된 시공 목표면과, 자세 연산 장치(21)로 연산된 프런트 작업기(1)의 자세에 기초하여, 프런트 작업기(1)의 동작을 연산하고, 그 동작을 실현하도록 유압 셔블(100)의 유압 시스템을 제어한다. 즉, 유압 시스템 제어 장치(23)는, 예를 들어 버킷(6) 등의 작업구의 선단이 시공 목표면에 일정 이상 근접하지 않도록 동작에 제한을 가하거나, 작업구(예를 들어, 버킷(6)의 클로 끝)가 시공 목표면을 따라 움직이도록 제어하거나 하는 머신 컨트롤 시스템으로서의 기능의 일부를 담당하고 있다.

도 3은, 컨트롤러의 자세 연산 장치의 처리 기능을 개략적으로 도시하는 기능 블록도이고, 도 4는, 자세 연산 장치의 자세 검출 장치 교정부의 처리 기능을 개략적으로 도시하는 기능 블록도이다. 또한, 도 5 내지 도 8은, 각 관성 계측 장치의 유압 셔블에 있어서의 설치 위치나, 유압 셔블 및 각 관성 계측 장치의 미리 설정된 좌표계의 관계 등을 도시하는 도면이고, 도 5는 유압 셔블의 상면도, 도 6 및 도 8은 측면도, 도 7은 배면도이다.

먼저, 본 실시 형태에 있어서의 교정 처리의 기본 원리에 대하여 설명한다.

도 5 내지 도 8에 도시한 바와 같이, 본 실시 형태에서는, 유압 셔블(100)이 배치되는 공간의 좌표계로서, 상부 선회체(2)의 수평 방향 전방 방향에 x축을, x축에 수직인 수평 방향 좌측 방향에 y축을, 수직 상향 방향에 z축을 각각 설정한다. 또한, x축의 방향을 향하여 시계 방향을 정방향으로 하는 롤 각, x축의 방향을 향하여 상측 방향을 정방향으로 하는 피치각을 정의한다. 또한, 이후의 설명에 있어서는, 각 변수의 첨자로서 기재하는 bd는 차체에 관한 것인 것, bm은 붐(4)에 관한 것인 것, am은 암(5)에 관한 것인 것, bk는 버킷(6)에 관한 것인 것을 각각 나타내는 것으로 한다.

도 6에 도시한 바와 같이, 본 실시 형태에서 정의한 좌표계에 있어서는, 예를 들어 프런트 작업기(1)의 피구동 부재(붐(4), 암(5), 버킷(작업구)(6))의 각 링크 길이를 각각 Lbm, Lam, Lbk, 상부 선회체(2) 및 프런트 작업기(1)의 피구동 부재(4 내지 6)의 자세각(대지각: 수평 방향에 대한 각도)을 각각 θbd, θbm, θam, θbk로 하면, 붐(4)의 차체측의 단부의 회동 중심으로부터 버킷(6)의 클로 끝까지의 수평 거리 X는 하기의 (식 1)로부터 구할 수 있다.

도 5에 도시한 바와 같이, 차체 관성 계측 장치(13)는 가속도의 검출에 있어서의 센서축으로서, 상부 선회체(2)의 선회축을 따라 상방 방향에 z축, z축에 수직이 되는 전방에 x축, z축 및 x축에 수직 좌측 방향에 y축을 설정한다. 마찬가지로, 관성 계측 장치(14 내지 16)는 가속도의 검출에 있어서의 센서축으로서, 각 피구동 부재(4 내지 6)의 링크 길이 방향 전방에 x축, x축의 방향을 향하여 프런트 작업기(1)의 동작 평면에 수직인 방향 좌측 방향에 y축, x축 및 y축에 수직인 상방 방향에 z축을 설정한다. 또한, 차체 관성 계측 장치(13)의 검출값을 (x0, y0, z0), 붐 관성 계측 장치(14)의 검출값을 (x1, y1, z1), 암 관성 계측 장치(15)의 검출값을 (x2, y2, z2), 버킷 관성 계측 장치(16)의 검출값을 (x3, y3, z3)이라고 나타내는 것으로 한다. 여기서, 예를 들어 차체 관성 계측 장치(13)는, 피치 방향의 설치 오차 θt0, 롤 방향의 설치 오차 φt0을 가지고 상부 선회체(2)에 설치되어 있는 것으로 한다.

또한, 도 7에 도시한 바와 같이, 유압 셔블(100)이 각도(경사도) φg의 경사면 상에 설치되어 있는 상황을 생각한다. 도 5에 있어서는, 유압 셔블(100)은 정지 상태에 있다고 가정하면, 차체 관성 계측 장치(13)로 검출되는 가속도는 중력 가속도 G만이다. 이 때문에, 차체 관성 계측 장치(13) 자신의 롤 각(대지각)을 φ0으로 하면, 센서축의 y축 및 z축의 방향에 있어서의 가속도는 하기의 (식 2) 및 (식 3)에서 각각 얻을 수 있다.

즉, 차체 관성 계측 장치(13)는, 상기의 (식 2) 및 (식 3)에서 산출되는 가속도 y0, z0을 이용하여, 차체 관성 계측 장치(13) 자신의 롤 각을 하기의 (식 4)에 의해 산출할 수 있다.

여기서, 상기의 (식 4)에서 얻어진 차체 관성 계측 장치(13) 자신의 롤 각 φ0은, 하기의 (식 5)에 나타내는 바와 같이, 설치 오차 φt0과 경사면 각도 φg의 차가 되고 있다.

차체 관성 계측 장치(13)에 관한 상기의 (식 5)의 연산은, 프런트 작업기(1)에 구비된 붐 관성 계측 장치(14), 암 관성 계측 장치(15) 및 버킷 관성 계측 장치(16) 각각에 대해서도 마찬가지로 행할 수 있다. 즉, 각 관성 계측 장치(14 내지 16)의 롤 각을 φi, 설치 오차를 φti(i=1 내지 3)로 하면, 붐(4), 암(5) 및 버킷(6) 각각에 대하여 하기의 (식 6) 내지 (식 8)을 얻을 수 있다.

이어서, 롤 각에 있어서의 설치 각 오차의 연산에 대하여 설명한다.

일반적으로, 관성 계측 장치 등의 센서 설치 오차는 평균값이 0(제로)이 되는 정규 분포를 따른다. 따라서, 이 원리를 따르면, 각 관성 계측 장치(13 내지 16)의 설치 오차의 총합은 0(제로)이 된다. 즉, 상기의 (식 5) 내지 (식 8)의 총합을 취하면 0(제로)이 되는 점에서, 경사면 각도 φg를 하기의 (식 9)에서 얻을 수 있다.

상기의 (식 9)에 의해 얻어진 경사면 각도 φg를 상기의 (식 5)에 대입함으로써, 하기의 (식 10)과 같이 설치 오차 φt0을 산출할 수 있다.

이어서, 경사면 각도의 연산에 대하여 설명한다.

상기의 (식 10)에 의해, 롤 방향의 설치 오차 φt0이 이미 산출될 수 있다. 따라서, 차체 관성 계측 장치(13)의 검출 결과(출력값)를 사용함으로써, 롤 각에 대하여 연산한 상기의 (식 4) 및 (식 5)를 사용하여, 하기의 (식 11)로부터 경사면의 경사 각도 φg를 산출할 수 있다.

이어서, 피치각에 있어서의 설치 오차의 연산에 대하여 설명한다.

롤 각의 연산 시와 마찬가지로, 차체가 정지 상태에 있는 경우, 차체 관성 계측 장치(13) 자신의 피치각을 θ0으로 하면, 센서축의 x축 및 z축의 방향에 있어서의 가속도는 하기의 (식 12) 및 (식 13)에서 각각 얻을 수 있다.

즉, 차체 관성 계측 장치(13)는, 상기의 (식 12) 및 (식 13)에서 산출되는 가속도 x0, z0을 이용하여, 차체 관성 계측 장치(13) 자신의 피치각을 하기의 (식 14)에 의해 산출할 수 있다.

여기서, 상기의 (식 14)에서 얻어진 차체 관성 계측 장치(13) 자신의 피치각 θ0은, 하기의 (식 15)에 나타내는 바와 같이, 설치 오차 φt0과 참된 피치각 θ0r의 차가 되고 있다.

도 9는, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 롤 각 및 피치각의 계측 결과의 변화 일례를 도시하는 도면이고, 횡축에 선회각을, 종축에 롤 각 및 피치각을 각각 나타내고 있다.

도 9에 도시한 바와 같이, 하부 주행체(3)에 대하여 상부 선회체(2)를 선회 동작시키면, 차체 관성 계측 장치(13)의 검출값은 설치 오차 θt0에 구애되지 않고, 차체의 배치 위치의 경사각 φg의 2배의 값(2φg)을 진폭으로 하는 정현 곡선을 나타낸다. 또한, 차체 관성 계측 장치(13)에 의해 검출되는 롤 각과 피치각은, 요 방향(선회축 주위)으로 90도 선회한 경우와 일치하고, 선회각에 대한 정현 곡선과 코사인 곡선의 관계를 갖는다. 따라서, 도 9에 도시한 바와 같이, 예를 들어 선회각 0도로 참된 롤 각 A1의 값(즉, 상기의 (식 4)에서 산출한 φ0, 설치 오차 φt0을 적용한 값)으로부터 선회각 90도에 있어서의 참된 피치각 B1을 추정할 수 있다. 그리고, 이와 같이 하여 구한 선회각 90도의 참된 피치각 B1(추정값)로부터, 상기의 (식 14)로 계산한 피치각 B2의 값을 뺌으로써 설치 오차 θt0을 산출할 수 있다. 또한, 상기의 (식 14)에서 선회각 90도의 피치각을 계산하기 위해서는, 실제로 상부 선회체(2)를 초기 자세(제1 자세: 하부 주행체(3)의 전방과 상부 선회체(2)의 전방을 일치시킨 자세이고, 선회각 0도의 상태)에 대하여 90도 선회시킬 필요가 있다.

본 실시 형태에 있어서는, 상술한 기본 원리에 기초하여 차체 관성 계측 장치(13)의 롤 각의 설치 오차 φt0 및 피치각의 설치 오차 θt0을 산출함으로써, 차체 관성 계측 장치(13)의 검출값의 교정을 행한다. 이러한 차체 관성 계측 장치(13)의 교정에 관한 컨트롤러(20)의 처리 기능에 대해서, 이하에 상세하게 설명한다.

도 3에 있어서, 자세 연산 장치(21)는, 관성 계측 장치(13 내지 16)로부터의 검출 결과에 기초하여, 프런트 작업기(1)의 자세를 연산하는 자세 연산 처리를 행하는 것이고, 자세 검출 장치 교정부(211), 자세각 변환부(212), 셔블 자세 연산부(213) 및 교정 결과 기억부(214)의 각 기능부를 갖고 있다.

자세 검출 장치 교정부(211)는 후술하는 연산 방법을 따라, 각 부위에 대한 각 관성 계측 장치(13 내지 16)의 설치 각도의 오차(설치 각도 오차 θt)를 연산한다. 연산된 설치 각도 오차 θt는, 교정 결과 기억부(214)에 보내져서 보존된다. 또한, 자세 검출 장치 교정부(211)는, 교정 작업의 단축화 및 교정 정밀도의 향상을 도모하기 위해서, 교정 결과 기억부(214)에 보존되어 있는 전회의 교정 결과를 초기 교정값으로 하여 사용해도 된다.

교정 결과 기억부(214)는, ROM(Read Only Memory)이나 RAM(Random Access Memory) 등의 기억 장치이고, 자세 검출 장치 교정부(211)에 의한 교정 결과, 즉, 자세 검출 장치 교정부(211)에 의해 연산되는 설치 각도 오차 θt를 보존하고 있다.

자세각 변환부(212)는, 각 관성 계측 장치(13 내지 16)의 검출 결과(즉, 각 관성 계측 장치(13 내지 16) 자신의 각도: 검출 각도 θs)에 교정 결과 기억부(214)로부터 출력된 설치 각도 오차(각도 θt)를 가산함으로써 각 부위의 자세 각도를 산출한다.

셔블 자세 연산부(213)는, 자세각 변환부(212)에서 연산된 각 부위의 자세 각도를 따라, 유압 셔블(100)의 자세를 산출한다.

도 4에 있어서, 자세 연산 장치(21)의 자세 검출 장치 교정부(211)는, 설치 오차의 연산 처리에 관한 처리부인, 제1 자세 정보 기억부(211a), 제1축 오차 연산부(211b), 제2 자세 정보 기억부(211c), 제1축 각도 연산부(211d), 제2축 각도 연산부(211e) 및 제2축 오차 연산부(211f)와, 자세 검출 장치 교정부(211)의 전체의 처리 동작을 제어하는 도시하지 않은 제어 기능부를 구비하고 있다.

이하, 자세 검출 장치 교정부(211)를 포함하는 자세 연산 장치(21)의 처리를 도 10 내지 도 12를 참조하면서 설명한다. 도 10 내지 도 12는, 자세 연산 장치(21)의 처리 내용을 도시하는 흐름도이다.

도 10 내지 도 12에 도시하는 자세 연산 장치(21)에 있어서의 교정 작업의 처리는, 예를 들어 오퍼레이터가 운전실(9) 내에 탑재된 콘솔을 조작함으로써 개시된다. 또한, 가이던스의 지시 표시에 이용하는 모니터가 터치 패널식의 태블릿 PC라면, 이 모니터에 콘솔의 기능을 갖게 해도 된다.

도 10에 있어서, 교정 작업이 개시되면, 자세 연산 장치(21)의 자세 검출 장치 교정부(211)는, 먼저, 정지 판정 처리를 행한다(스텝 S100). 도 11에 도시한 바와 같이, 정지 판정 처리에서는, 먼저, 자세 검출기인 관성 계측 장치(13 내지 16)의 출력값을 취득하고(스텝 S101), 관성 계측 장치(13 내지 16)의 출력값으로부터 얻어지는 모든 각속도(여기서는, 절댓값)가 미리 정한 소정의 역치 미만인지의 여부를 판정하고(스텝 S102), 판정 결과가 NO의 경우에는, 스텝 S101, S102의 처리를 반복한다. 이때, 역치는, 자이로 바이어스를 고려해서 0(제로)이 아닌 정값으로 설정하는 것이 바람직하다.

또한, 스텝 S102에서의 판정 결과가 "예"인 경우, 즉, 관성 계측 장치(13 내지 16)의 출력값으로부터 얻어지는 각속도의 모두가 미리 정한 역치 미만인 경우에는, 유압 셔블(100)이 작업을 행하고 있지 않은 정지 상태라고 판정하고(스텝 S103), 판정 결과가 정지 상태로 될 때까지 정지 판정 처리를 반복한다(스텝 S110). 스텝 S110에서의 판정 결과가 "예"인 경우에는, 교정 개시 시의 초기 자세(제1 자세)에서 계측한 각 관성 계측 장치(13 내지 16)의 검출값을 제1 자세 정보 기억부(211a)에 기억한다(스텝 S120).

계속해서, 제1축 오차 연산부(211b)는, 제1 자세 정보 기억부(211a)에 기억된, 제1 자세에 있어서의 각 관성 계측 장치(13 내지 16)의 정보로부터, 상술한 기본 원리에 기초하여, 차체 관성 계측 장치(13)의 롤 각 방향(제1축)의 설치 오차 φt0을 산출한다(스텝 S130).

계속해서, 자세 연산 장치(21)의 자세 검출 장치 교정부(211)는, 자세 변경 지시 처리를 행한다(스텝 S140). 자세 변경 처리는, 피치각(제2축)의 교정을 행하기 위한 정보 취득을 위해서, 조작 레버(9a, 9b)의 조작에 의한 상부 선회체(2)의 선회를 오퍼레이터에 지시하는 처리이다.

자세 변경 처리에 있어서의 오퍼레이터로의 지시는 선회각에 따라서 표시 내용을 축차 변경함으로써 행하도록 해도 된다. 또한, 자세 변경 지시 처리에 있어서의 오퍼레이터로의 지시 방법으로서는, 예를 들어, 모니터 표시 제어 장치(22)를 통해 운전실(9) 내의 모니터에 지시 내용을 표시하는 경우나, 음성으로 지시 내용을 통지하는 경우 등이 생각된다.

도 12에 도시한 바와 같이, 자세 변경 지시 처리에서는, 자세 연산 장치(21)의 자세 검출 장치 교정부(211)은, 먼저, 자세 검출기인 각 관성 계측 장치(13 내지 16)의 출력값을 취득하고(스텝 S141), 하부 주행체(3)에 대한 상부 선회체(2)의 초기 자세(제1 자세)로부터의 선회각을 연산한다(스텝 S142). 선회각의 연산은, 스텝 S141에서 취득한 관성 계측 장치(13 내지 16)의 출력값(각속도)을 각각 적분함으로써 얻어진다.

계속해서, 선회각이 소정값(역치)의 범위 내인지의 여부를 판정한다(스텝 S143). 예를 들어, 초기 자세로부터 선회각 10도마다 관성 계측 장치(13 내지 16)의 출력값을 취득하는 경우에는, 소정값의 범위에는 10도를 포함하는 범위를 지정한다. 10도의 정수배의 각도를 각각 포함하는 범위를 소정값의 범위로 하여 설정함으로써, 10도마다의 출력값을 취득할 수 있다.

스텝 S143에서의 판정 결과가 "예"인 경우에는, 선회 동작의 정지를 오퍼레이터에 지시한다(스텝 S144). 또한, 스텝 S143에서의 판정 결과가 "아니오"인 경우에는, 선회 동작의 계속을 오퍼레이터에 지시한다(스텝 S145). 이와 같이, 오퍼레이터로의 지시를 개시한 후는, 소정의 각도에 도달할 때까지 작업 지시를 계속하는 것이 바람직하다. 또한, 그때에는, 진척 상황을 오퍼레이터에 통지하는 것이 바람직하다.

스텝 S140의 자세 변경 지시 처리가 종료하면, 유압 셔블(100)이 정지 중인지의 여부, 즉 관성 계측 장치(13 내지 16)의 출력값으로부터 얻어지는 모든 각속도(여기서는, 절댓값)가 미리 정한 소정의 역치 미만인지의 여부를 판정한다(스텝 S150). 스텝 S150의 판정은, 선회 동작에 의한 원심 가속도에 의해 상기의 (식 14)가 성립하지 않게 되는 것을 회피하기 위한 처리이고, 각속도가 완전히 0(제로)이 아니어도 되기 때문에, 스텝 S110과는 다른 판정 조건을 설정해도 된다.

스텝 S160에서의 판정 결과가 "예"인 경우에는, 계측한 각 관성 계측 장치(13 내지 16)의 검출값을 제2 자세 정보 기억부(211c)에 기억하고(스텝 S160), 필요 동작이 완료되었는지의 여부를 판정한다(스텝 S170). 또한, 스텝 S150에서의 판정 결과가 "아니오"인 경우에는, 계속해서, 필요 동작이 완료되었는지의 여부를 판정한다(스텝 S170).

스텝 S170에서의 판정 결과가 "아니오"인 경우, 즉, 이후의 연산에 필요한 정보의 취득이 완료되지 않았다고 판정한 경우에는, 스텝 S140 내지 S160의 처리를 반복한다. 필요한 정보란, 구체적으로는, 예를 들어 선회각 0도의 기본 자세로부터 선회각 90도 이상의 자세까지의 선회각 10도마다의 각 관성 계측 장치(13 내지 16)의 출력값이다. 스텝 S140 내지 S160의 처리를 반복함으로써, 자세 정보를 거의 선회각 10도의 간격으로 취득할 수 있다.

또한, 스텝 S143에서의 판정에 사용하는 소정 범위에는 미세한 값(좁은 범위)을 설정할수록 이상적인 플롯 파형을 얻는 것이 가능해지지만, 선회각을 소정 범위에 들어가게 하기 위해서는 오퍼레이터의 기량이 필요해진다. 따라서, 이 소정 범위는, 예를 들어 ±1도 정도의 범위로 설정함으로써 오퍼레이터의 부담을 저감시키는 것이 바람직하다. 가령 선회각이 1도 정도 달랐다고 해도, 피치각의 설치 오차의 산출 결과로의 영향은 극히 작다.

스텝 S170에서의 판정 결과가 "예"인 경우에는, 제1축 각도 연산부(211d)는, 제1축 오차 연산부(211b)에서 산출한 설치 오차를 사용하여, 상부 선회체(2)의 롤 각(제1축 주위의 각도)을 산출한다(스텝 S180). 이 연산은, 제2 자세 정보 기억부(211c)에 기억된 모든 데이터에 대하여 실행된다. 예를 들어, 제2 자세 정보 기억부(211c)에 선회각 0도 내지 90도에 있어서의 10도마다의 자세 정보(관성 계측 장치(13 내지 16)의 검출값)가 기억되어 있는 경우에는, 그것들의 정보를 따라서 선회각과 롤 각(검출값)의 관계를 묘화하고, 제1축 오차 연산부(211b)에서 산출한 설치 오차의 분만큼 시프트 연산을 행함으로써, 참된 롤 각을 묘화할 수 있다.

계속해서, 제2축 각도 연산부(211e)는, 제1축 각도 연산부(211d)에서 연산한 롤 각에 기초하여 피치각의 연산을 행한다(스텝 S190). 여기서, 피치각의 연산은, 도 9에서 설명한 바와 같이, 참된 롤 각 A1의 위상을 90도 어긋나게 함으로써 참된 피치각 B1을 산출하는 조작에 상당한다.

계속해서, 제2축 오차 연산부(211f)는, 제2 자세 정보 기억부(211c)에 기억된 가속도에 관한 정보(롤 각의 검출값)에 기초하여 각 선회각에 있어서의 피치각(예를 들어, 도 9의 피치각 B2(검출값)에 상당)을 산출하고, 제2축 각도 연산부(211e)의 연산 결과인 피치각과 비교함으로써, 각 관성 계측 장치(13 내지 16)의 피치각의 설치 오차(제2축 오차)를 산출한다(스텝 S200).

계속해서, 자세 검출 장치 교정부(211)는, 제2축 오차 연산부(211f)에서의 연산 결과(설치 오차)를 교정 결과 기억부(214)에 보내서 기억시켜(스텝 S210), 모니터 표시 제어 장치(22)를 통해 운전실(9) 내의 모니터에 교정 처리가 종료한 것을 나타내는 정보를 표시하여, 오퍼레이터에 교정 처리의 종료를 보고하고(스텝 S220), 처리를 종료한다.

이상과 같이 구성한 본 실시 형태에 있어서의 효과를 설명한다.

작업기의 자세를 연산하기 위해서는, 관성 계측 장치(IMU) 등의 센서를 사용하여 붐, 암, 버킷의 각각의 회전 각도를 검출할 필요가 있다. 또한, 고정밀도의 자세 연산에는, 센서의 설치 위치나 각도 등을 정확하게 파악할 필요가 있다. 통상적으로는 센서의 설치 작업에는 전용의 지그 등을 이용하기 때문에, 설치 오차는 몇 번 정도의 작은 각도가 된다. 그러나, 유압 셔블에 있어서는 프런트 작업기의 각 부재의 링크 길이가 몇 미터 오더가 되기 때문에, 설치 오차가 몇 차례여도 클로 끝 위치의 연산 결과에 끼치는 영향은 매우 커진다. 예를 들어, 20t 클래스의 유압 셔블을 생각하면, 프런트 길이가 최장이 되는 자세(소위 맥스 리치)에서는 10m 정도가 된다. 이 자세에서는, 차체의 센서 오차가 0.5도였다고 해도, 작업구(버킷)의 클로 끝 위치의 높이의 오차는, 10m×sin(0.5×π÷180)≒0.09m로 계산되고, 클로 끝 위치의 높이에 9cm나 되는 오차가 발생해 버리게 된다.

종래 기술에는, 자이로 센서를 작업 기계에 설치할 때에 발생하는 설치 오차를 보정하는 경우가 있다. 그러나, 일반적으로 알려져 있는 바와 같이, 자이로 센서의 출력은 어떠한 바이어스(자이로 바이어스)를 갖기 때문에, 실제로 각속도가 발생하고 있지 않은 축 방향에 대해서도 자이로 센서로부터 어떠한 값이 출력되어 버린다. 또한, 자이로 바이어스는 센서 소자의 온도 등에 의해 크게 변화해 버리기 때문에, 교정 동작을 행하는 환경에 의해 교정의 정밀도가 현저하게 다른 것을 생각할 수 있다. 따라서, 자이로 바이어스의 영향에 대하여 고려하고 있지 않은 종래 기술에 있어서는, 자이로 센서로부터의 출력값이 자이로 바이어스의 영향에 의해 참된 설치각으로부터 오차를 가진 값으로 된다.

이에 비해, 본 실시 형태에 있어서는, 하부 주행체(3)와, 하부 주행체(3)에 대하여 선회 가능하게 마련된 상부 선회체(2)로 이루어지는 차체와, 차체에 설치되어, 회동 가능하게 연결된 복수의 프런트 부재(예를 들어, 붐(4), 암(5), 버킷(6))로 이루어지는 다관절형의 작업기(예를 들어, 프런트 작업기(1))와, 작업기의 적어도 하나의 프런트 부재에 마련되어, 그 프런트 부재의 자세 정보를 검출하는 하나 이상의 제1 자세 검출 장치(예를 들어, 관성 계측 장치(14 내지 16))와, 상부 선회체(2)에 마련되어, 상부 선회체(2)의 자세 정보를 검출하는 제2 자세 검출 장치(예를 들어, 관성 계측 장치(13))와, 제2 자세 검출 장치의 설치 오차를 산출하는 제어 장치(컨트롤러(20))를 구비한 작업 기계에 있어서, 제어 장치는, 상부 선회체(2)를 하부 주행체(3)에 대하여 미리 정한 방향을 향한 제1 교정 자세(초기 자세)에 있어서의 제1 자세 검출 장치 및 제2 자세 검출 장치의 검출 결과에 기초하여, 상부 선회체(2)의 선회축에 수직으로 전방을 향하여 설정된 제1 축 주위에 있어서의 제2 자세 검출 장치의 설치 오차를 산출하고, 상부 선회체(2)를 하부 주행체(3)에 대하여 제1 교정 자세로부터 90도 선회시킨 제2 교정 자세에 있어서의 제2 자세 검출 장치의 검출 결과와, 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차에 기초하여, 상부 선회체(2)의 선회축 및 제1축에 수직으로 측방을 향하여 설정된 제2 축 주위에 있어서의 제2 자세 검출 장치의 설치 오차를 산출하도록 구성했으므로, 자이로 바이어스를 고려할 필요 없이, 자세 정보를 검출하기 위한 센서 설치 오차를 고정밀도로 산출할 수 있고, 센서의 교정 정밀도를 향상시킬 수 있다.

또한, 차체에 설치되는 센서는, 상부 선회체의 하우징 내나 운전실 내에 설치되는 것이 많기 때문에, 설치 위치의 외부에서의 확인이 용이하지 않다. 또한, 프런트 작업기의 각 부재의 회동 중심은 각 링크의 핀 위치로서 유일하게 정해지는 것에 비해, 상부 선회체의 피치 방향(전후의 기울기 방향)이나 롤 방향(좌우의 기울기 방향)으로의 회전 중심은 유일하게 정해지지 않는다. 이 때문에, 프런트 작업기의 붐이나 암, 버킷 등에 설치되는 센서와는 다르게, 토탈 스테이션 등의 외부 계측 장치를 이용한 교정이 용이하지 않다.

이에 비해, 본 실시 형태에 있어서는, 상부 선회체(2)에 설치되는 차체 관성 계측 장치(13)를 토탈 스테이션과 같은 외부 계측 장치를 사용하여 측정할 필요가 없기 때문에, 관성 계측 장치의 교정을 보다 용이하게 행할 수 있다.

<제2 실시 형태>

본 발명의 제2 실시 형태를 도 13 및 도 14를 참조하면서 설명한다.

본 실시 형태는, 프런트 작업기를 외부 계측 장치에 의해 계측한 결과에 기초하여, 관성 계측 장치의 롤 각의 설치 오차를 연산하는 것이다.

도 13은, 본 실시 형태에 있어서의 자세 연산 장치의 자세 검출 장치 교정부의 처리 기능을 개략적으로 도시하는 기능 블록도이다. 또한, 도 14는, 본 실시 형태에 있어서의 자세 연산 장치(21)의 처리 내용을 도시하는 흐름도이다. 도면 중, 제1 실시 형태와 마찬가지의 부재에는 동일한 부호를 붙이고, 설명을 생략한다.

도 8에 도시한 바와 같이, 유압 셔블(100)의 프런트 작업기(1)에 있어서의 붐 풋 핀 위치 P1(xa, ya, za) 및 붐 선단 핀 위치 P2(xb, yb, zb)의 삼차원 좌표를 토탈 스테이션 등의 외부 계측 장치로 계측하는 경우를 생각한다. 이때, 위치 P1, P2의 각 좌표를 사용하여 외부 계측 장치에서 취득한 프런트 작업기(1)의 롤 각 φ1mes를 하기의 (식 16)으로 나타낼 수 있다.

또한, 상기의 (식 16)은, 프런트 작업기(1)가 하부 주행체(3)에 대하여 평행 상태에 있는(즉, 선회하고 있지 않은 자세인) 것을 전제로 하고 있다.

외부 계측 장치에서 취득한 프런트 작업기(1)의 롤 각 φ1mes와, 예를 들어 관성 계측 장치(14)에서 취득한 롤 각 φ1을 비교함으로써, 붐 관성 계측 장치(14)의 설치 오차 φt1을 하기의 (식 17)에서 구할 수 있다.

여기서, 상기의 (식 6) 및 (식 17)을 사용하여 경사면 경사 φg를 산출함으로써, 상기의 식 (5)를 변형한 하기의 (식 18)로부터 차체 관성 계측 장치(13)의 설치 오차 φt0을 산출할 수 있다.

이와 같이, 외부 계측 장치를 사용하여 붐 관성 계측 장치(14)의 교정을 행하고, 그 교정 결과를 사용함으로써, 간접적으로 차체 관성 계측 장치(13)의 설치 오차를 산출하여 교정을 실시하는 것이 가능하다. 또한, 본 실시 형태에 있어서는, 외부 계측 장치에 의한 붐 관성 계측 장치(14)의 교정 결과를 사용하여 차체 관성 계측 장치(13)의 설치 오차 φt0을 산출하는 경우를 예시하여 설명했지만, 외부 계측 장치에 의한 암 관성 계측 장치(15)나 버킷 관성 계측 장치(16)의 교정 결과를 사용한 경우에도, 마찬가지로 상기의 (식 17) 및 (식 18)을 사용함으로써 차체 관성 계측 장치(13)의 설치 오차 φt0을 산출할 수 있다.

도 13에 있어서, 자세 검출 장치 교정부(211A)는, 설치 오차의 연산 처리에 관한 처리부인, 제1 자세 정보 기억부(211a), 제1축 오차 연산부(211b), 제2 자세 정보 기억부(211c), 제1축 각도 연산부(211d), 제2축 각도 연산부(211e) 및 제2축 오차 연산부(211f)와, 자세 검출 장치 교정부(211)의 전체의 처리 동작을 제어하는 도시하지 않은 제어 기능부를 구비하고 있다.

이하, 자세 검출 장치 교정부(211A)를 포함하는 자세 연산 장치(21)의 처리를 도 14를 참조하면서 설명한다.

도 14에 도시하는 교정 작업의 처리는, 예를 들어 오퍼레이터가 운전실(9) 내에 탑재된 콘솔을 조작함으로써 개시된다. 도 14에 있어서, 교정 작업이 개시되면, 자세 검출 장치 교정부(211)는, 먼저, 교정 결과 기억부(214A)를 참조하여(스텝 S80), 필요 데이터가 있는지의 여부를 판정한다(스텝 S90). 여기서, 필요 데이터란, 본 실시 형태의 교정 처리의 기본 원리에서 설명한, 외부 계측 장치에 의한 프런트 작업기(1)의 교정 결과이고, 토탈 스테이션 등의 외부 계측 장치에 의해 미리 측정되어서 교정 결과 기억부(214A)에 기억되어 있다.

스텝 S90에서의 판정 결과가 "예"인 경우에는, 제1 실시 형태의 도 10에 있어서의 스텝 S130의 처리를 본 실시 형태의 기본 원리에 기초하여 관성 계측 장치(13 내지 16)의 롤 각의 설치 오차를 산출하는 스텝 S130A의 처리로 치환하여 스텝 S100 내지 S220의 처리를 행한다.

즉, 스텝 S130에서는, 제1축 오차 연산부(211b)는, 제1 자세 정보 기억부(211a)에 기억된 제1 자세에 있어서의 각 관성 계측 장치(13)의 정보 및 교정 결과 기억부(214A)에 기억된 관성 계측 장치(14 내지 16)의 외부 계측 장치에 의한 교정 결과로부터, 상술한 기본 원리에 기초하여, 차체 관성 계측 장치(13)의 롤 각 방향(제1축)의 설치 오차 φt0을 산출한다(스텝 S130A).

또한, 스텝 S90에서의 판정 결과가 "아니오"인 경우에는, 모니터 표시 제어 장치(22)를 통해 운전실(9) 내의 모니터에 필요 데이터가 부족한 것을 나타내는 정보를 표시하여 오퍼레이터에 보고하고(스텝 S91), 처리를 종료한다.

그 밖의 구성은 제1 실시 형태와 마찬가지이다.

이상과 같이 구성한 본 실시 형태에 있어서도 제1 실시 형태와 마찬가지의 효과를 얻을 수 있다.

<제3 실시 형태>

본 발명의 제3 실시 형태를 도 15 및 도 16을 참조하면서 설명한다.

본 실시 형태는, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 때의 관성 계측 장치의 계측 결과에 기초하여, 관성 계측 장치의 롤 각의 설치 오차를 연산하는 것이다.

도 15 및 도 16은, 하부 주행체에 대하여 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 참된 롤 각과 관성 계측 장치에 의한 검출 결과로서의 롤 각의 계측 결과의 변화 일례를 도시하는 도면이고, 횡축에 선회각을, 종축에 롤 각을 각각 나타내고 있다. 도면 중, 제1 실시 형태와 마찬가지의 부재에는 동일한 부호를 붙이고, 설명을 생략한다.

예를 들어, 차체 관성 계측 장치(13)가 롤 각의 설치 오차 φt0 및 피치각의 설치 오차 θt0을 가지고 있었다고 해도, 유압 셔블(100)의 상부 선회체(2)를 선회 동작시킨 때에 상기의 (식 4)에 의해 얻어지는 검출값의 최댓값과 최솟값의 차는 경사 각도 φg의 2배가 된다.

도 15에서는, 초기 자세(선회각 0도)의 상부 선회체(2)의 전후 방향이 경사 각도 φg의 경사면의 상하 방향에 대하여 직교하도록 유압 셔블(100)을 배치한 경우(도 7 참조)에, 상부 선회체를 선회 동작시킨 경우의 차체 관성 계측 장치의 참된 롤 각과 관성 계측 장치에 의한 검출 결과로서의 롤 각의 계측 결과의 변화를 나타내고 있다. 또한, 유압 셔블(100)의 상부 선회체(2)는 좌우로 자유롭게 회전할 수 있으므로, 선회각은 정부로 값을 갖지만, 설명의 간단화를 위하여 정방향의 선회만을 나타낸다.

먼저, 선회각이 0도일 때, 참된 롤 각 A1은 경사면 각도 φg이지만, 상기의 (식 4)에서 산출되는 롤 각 A2는 설치 오차 φt0만큼 작은 값, 즉 (φg-φt0)의 값을 취한다. 이 관계는, 선회각이 바뀌어도 계속하여 유지된다.

또한, 선회각이 180도가 되면, 상부 선회체(2)의 방향이 반전하기 때문에, 참된 롤 각 A1은 (-φg)의 값을 취한다. 한편으로, 상기의 (식 4)에서 산출되는 롤 각 A2는 (-φg-φt0)의 값을 취한다. 따라서, 이들의 관계로부터, 선회각이 0도인 경우의 상기의 (식 4)의 롤 각의 값과 선회각이 180도인 경우의 상기의 (식 4)의 롤 각의 값을 감산하면, 하기의 (식 19)의 관계가 얻어진다.

즉, 상기의 (식 19)의 양변을 2로 나눔으로써, 경사각 φg를 산출할 수 있다.

또한, 도 15에 있어서는, 초기 자세(선회각 0도)의 상부 선회체(2)의 전후 방향이 경사 각도 φg의 경사면의 상하 방향에 대하여 직교하도록 유압 셔블(100)을 배치한 경우를 생각했지만, 초기 자세가 반드시 이러한 상태가 된다고는 할 수 없다.

그래서, 도 16에 도시한 바와 같이, 초기 자세(선회각 0도)의 상부 선회체(2)의 전후 방향이 경사 각도 φg의 경사면의 상하 방향에 대하여 직교하고 있지 않은 경우를 생각한다.

도 16에 있어서는, 선회각 0도인 경우와 180도인 경우의 롤 각 A2의 차는 명백하게 2φg 미만이지만, 선회각 120도인 경우와 300도인 경우의 롤 각 A2의 차는 2φg로 되는 것을 알 수 있다. 즉, 어떤 초기 자세여도, 선회각에 대하여 상기의 (식 4)에서 얻어지는 값의 최댓값과 최솟값에 주목하면, 상기의 (식 19)의 관계식이 성립하는 것을 알 수 있다.

도 15 및 도 16에 있어서의 (식 4)의 롤 각 A2는, 상부 선회체(2)를 선회 동작시킨 때의 차체 관성 계측 장치(13)의 출력값(가속도)을 따라서 상기의 (식 4)의 연산을 행함으로써 플롯할 수 있다. 단, 플롯 파형에 검출값의 최댓값과 최솟값이 포함되기 위해서는, 선회각 180도 이상, 360도 미만의 선회 동작이 필요하게 된다.

상기의 (식 19)에 의해 얻어진 경사각 φg와, 상기 (식 4)에서 얻어지는 검출값 φ0을 사용함으로써, 상기의 (식 10)으로부터 차체 관성 계측 장치(13)의 롤 각의 설치 오차 φt0을 산출할 수 있다.

그 밖의 구성은 제1 및 제2 실시 형태와 마찬가지이다.

이상과 같이 구성한 제3 실시 형태에 있어서도 제1 및 제2 실시 형태와 마찬가지의 효과를 얻을 수 있다.

다음으로 상기의 각 실시 형태의 특징에 대하여 설명한다.

(1) 상기의 실시 형태에서는, 하부 주행체(3)와, 상기 하부 주행체(3)에 대하여 선회 가능하게 마련된 상부 선회체(2)로 이루어지는 차체와, 상기 차체에 설치되어, 회동 가능하게 연결된 복수의 프런트 부재(예를 들어, 붐(4), 암(5), 버킷(6))를 포함하는 다관절형의 작업기(예를 들어, 프런트 작업기(1))와, 상기 작업기의 적어도 하나의 프런트 부재에 마련되어, 그 프런트 부재의 자세 정보를 검출하는 하나 이상의 제1 자세 검출 장치(예를 들어, 관성 계측 장치(14 내지 16))와, 상기 상부 선회체에 마련되어, 상기 상부 선회체의 자세 정보를 검출하는 제2 자세 검출 장치(예를 들어, 관성 계측 장치(13))와, 상기 제2 자세 검출 장치의 설치 오차를 산출하는 제어 장치(예를 들어, 컨트롤러(20))를 구비한 작업 기계(예를 들어, 유압 셔블(100))에 있어서, 상기 제어 장치는, 상기 상부 선회체를 상기 하부 주행체에 대하여 미리 정한 방향을 향한 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치 및 상기 제2 자세 검출 장치의 검출 결과에 기초하여, 상기 상부 선회체의 선회축에 수직으로 전방을 향하여 설정된 제1 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하고, 상기 상부 선회체를 상기 하부 주행체에 대하여 상기 제1 교정 자세로부터 90도 선회시킨 제2 교정 자세에 있어서의 상기 제2 자세 검출 장치의 검출 결과와, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차에 기초하여, 상기 상부 선회체의 선회축 및 상기 제1 축에 수직으로 측방을 향하여 설정된 제2 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하는 것으로 하였다.

이렇게 구성함으로써, 자세 정보를 검출하기 위한 센서 설치 오차를 고정밀도로 산출할 수 있고, 센서의 교정 정밀도를 향상시킬 수 있다.

(2) 또한, 상기의 실시 형태에서는, (1)의 작업 기계(예를 들어, 유압 셔블(100))에 있어서, 상기 제어 장치(예를 들어, 컨트롤러(20))는, 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치(예를 들어, 관성 계측 장치(14 내지 16))의 검출 결과와 상기 제2 자세 검출 장치(예를 들어, 관성 계측 장치(13))의 검출 결과의 평균값에 기초하여, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것으로 하였다.

(3) 또한, 상기의 실시 형태에서는, (1)의 작업 기계(예를 들어, 유압 셔블(100))에 있어서, 상기 제어 장치(예를 들어, 컨트롤러(20))는, 상기 작업 기계의 외부에서 상기 제1 자세 검출 장치(예를 들어, 관성 계측 장치(14 내지 16))의 상기 제1 축 주위의 설치 오차를 계측 가능한 외부 계측 장치(예를 들어, 토탈 스테이션)에 의해 얻어진 상기 제1 자세 검출 장치의 설치 오차와, 상기 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치의 검출 결과와, 상기 제2 자세 검출 장치(예를 들어, 관성 계측 장치(13))의 검출 결과에 기초하여, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것으로 하였다.

(4) 또한, 상기의 실시 형태에서는, (1)의 작업 기계(예를 들어, 유압 셔블(100))에 있어서, 상기 제어 장치(예를 들어, 컨트롤러(20))는, 상기 상부 선회체(2)를 상기 하부 주행체(3)에 대하여 상기 제1 교정 자세로부터 180도 이상 선회시킨 경우의 상기 제2 자세 검출 장치(예를 들어, 관성 계측 장치(13))의 검출 결과의 최댓값과 최솟값으로부터 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것으로 하였다.

<부기>

또한, 본 발명은 상기의 실시 형태에 한정되는 것은 아니고, 그 요지를 일탈하지 않는 범위 내의 다양한 변형예나 조합이 포함된다. 또한, 본 발명은 상기의 실시 형태에서 설명한 모든 구성을 구비하는 것에 한정되지는 않고, 그 구성의 일부를 삭제한 것도 포함된다. 또한, 상기의 각 구성, 기능 등은, 그것들의 일부 또는 전부를, 예를 들어 집적 회로로 설계하는 등에 의해 실현해도 된다. 또한, 상기의 각 구성, 기능 등은, 프로세서가 각각의 기능을 실현하는 프로그램을 해석하고, 실행함으로써 소프트웨어로 실현해도 된다.

1: 프런트 작업기(작업기), 2: 상부 선회체, 2a: 선회 모터, 3: 하부 주행체, 3a: 주행 모터, 4: 붐, 4a: 붐 실린더, 5: 암, 5a: 암 실린더, 6: 버킷, 6a: 버킷 실린더, 7: 유압 펌프 장치, 8: 컨트롤 밸브, 9: 운전실, 9a, 9b: 조작 레버(조작 장치), 13: 차체 관성 계측 장치, 14: 붐 관성 계측 장치, 15: 관성 계측 장치, 16: 버킷 관성 계측 장치, 17: 시공 정보, 18: 수신 안테나, 20: 컨트롤러, 21: 자세 연산 장치, 22: 모니터 표시 제어 장치, 23: 유압 시스템 제어 장치, 24: 시공 목표면 연산 장치, 100: 유압 셔블, 211, 211A: 자세 검출 장치 교정부, 211a: 제1 자세 정보 기억부, 211b: 제1축 오차 연산부, 211c: 제2 자세 정보 기억부, 211d: 제1축 각도 연산부, 211e: 제2축 각도 연산부, 211f: 제2축 오차 연산부, 212: 자세각 변환부, 213: 셔블 자세 연산부, 214, 214A: 교정 결과 기억부

Claims

하부 주행체와, 상기 하부 주행체에 대하여 선회 가능하게 마련된 상부 선회체로 이루어지는 차체와,
상기 차체에 설치되고, 회동 가능하게 연결된 복수의 프런트 부재로 이루어지는 다관절형의 작업기와,
상기 작업기의 적어도 하나의 프런트 부재에 마련되어, 그 프런트 부재의 자세 정보를 검출하는 하나 이상의 제1 자세 검출 장치와,
상기 상부 선회체에 마련되어, 상기 상부 선회체의 자세 정보를 검출하는 제2 자세 검출 장치와,
상기 제2 자세 검출 장치의 설치 오차를 산출하는 제어 장치를 구비한 작업 기계에 있어서,
상기 제어 장치는,
상기 상부 선회체를 상기 하부 주행체에 대하여 미리 정한 방향을 향한 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치 및 상기 제2 자세 검출 장치의 검출 결과에 기초하여, 상기 상부 선회체의 선회축에 수직으로 전방을 향하여 설정된 제1 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하고,
상기 상부 선회체를 상기 하부 주행체에 대하여 상기 제1 교정 자세로부터 90도 선회시킨 제2 교정 자세에 있어서의 상기 제2 자세 검출 장치의 검출 결과와, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차에 기초하여, 상기 상부 선회체의 선회축 및 상기 제1 축에 수직으로 측방을 향하여 설정된 제2 축 주위에 있어서의 상기 제2 자세 검출 장치의 설치 오차를 산출하는 것을 특징으로 하는 작업 기계.
제1항에 있어서,
상기 제어 장치는, 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치의 검출 결과와 상기 제2 자세 검출 장치의 검출 결과의 평균값에 기초하여, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것을 특징으로 하는 작업 기계.
제1항에 있어서,
상기 제어 장치는, 상기 작업 기계의 외부에서 상기 제1 자세 검출 장치의 상기 제1 축 주위의 설치 오차를 계측 가능한 외부 계측 장치에 의해 얻어진 상기 제1 자세 검출 장치의 설치 오차와, 상기 제1 교정 자세에 있어서의 상기 제1 자세 검출 장치의 검출 결과와, 상기 제2 자세 검출 장치의 검출 결과에 기초하여, 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것을 특징으로 하는 작업 기계.
제1항에 있어서,
상기 제어 장치는, 상기 상부 선회체를 상기 하부 주행체에 대하여 상기 제1 교정 자세로부터 180도 이상 선회시킨 경우의 상기 제2 자세 검출 장치의 검출 결과의 최댓값과 최솟값으로부터 상기 제2 자세 검출 장치의 상기 제1 축 주위에 있어서의 설치 오차를 산출하는 것을 특징으로 하는 작업 기계.