KR20160002721A

KR20160002721A - 작업 기계의 자세 연산 장치, 유압 셔블의 자세 연산 장치 및 작업 기계

Info

Publication number: KR20160002721A
Application number: KR1020157025601A
Authority: KR
Inventors: 야스오 가네미츠
Original assignee: 가부시키가이샤 고마쓰 세이사쿠쇼
Priority date: 2014-05-19
Filing date: 2014-05-19
Publication date: 2016-01-08
Also published as: US10519630B2; KR101764521B1; US9745726B2; DE112014000060T5; DE112014000060B4; CN105517645A; US20170321395A1; JP5844465B1; JPWO2015177843A1; CN105517645B; WO2015177843A1; US20160244949A1

Abstract

작업 기계의 자세 연산 장치는, 작업 기계에 구비되어, 각속도 및 가속도를 검출하는 계측 장치와, 상기 계측 장치에 의해 검출된 각속도 및 가속도로부터 상기 작업 기계의 자세각을 구하는 자세각 연산부와, 제 1 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 1 자세각을 출력하는 제 1 상보 필터와, 상기 제 1 차단 주파수와는 상이한 제 2 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 2 자세각을 출력하는 제 2 상보 필터와, 상기 계측 장치의 상태에 따라, 상기 제 1 자세각 또는 상기 제 2 자세각을 전환하여 출력하는 전환부를 포함한다.

Description

작업 기계의 자세 연산 장치, 유압 셔블의 자세 연산 장치 및 작업 기계{POSTURE CALCULATION DEVICE OF WORKING MACHINERY, POSTURE CALCULATION DEVICE OF EXCAVATOR, AND WORKING MACHINERY}

본 발명은 작업 기계의 자세 연산 장치, 유압 셔블의 자세 연산 장치 및 작업 기계에 관한 것이다.

최근, 유압 셔블 또는 불도저 등의 작업 기계에 있어서, 작업 기계의 위치를 검출하여 작업을 관리하거나, 작업 기계의 위치 정보와 시공 현장의 지형을 나타내는 시공 정보에 기초하여 시공을 관리하거나 하는 것이 있다 (예를 들어, 특허문헌 1).

일본 공개특허공보 2005-61024호

작업 기계가 구비하는 작업기의 위치 및 자세를 구하는 경우, 작업 기계의 기울기에 관한 정보를 정확하게 구할 필요가 있다. 예를 들어, IMU (Inertial Measurement Unit : 관성 계측 장치) 를 작업 기계에 탑재하고, IMU 의 검출값으로부터 롤각 및 피치각과 같은 자세각을 작업 기계의 기울기에 관한 정보로서 얻을 수 있다. IMU 의 검출값으로부터 작업 기계의 자세각을 구하는 경우, 자세각의 정밀도가, 예를 들어, 구해지는 작업기 등의 위치 및 자세의 정밀도에 영향을 준다.

본 발명은 작업 기계의 자세각을 구할 때에, 자세각의 정밀도 저하를 억제하는 것을 목적으로 한다.

본 발명은 작업 기계에 구비되어, 각속도 및 가속도를 검출하는 계측 장치와, 상기 계측 장치에 의해 검출된 각속도 및 가속도로부터 상기 작업 기계의 자세각을 구하는 자세각 연산부와, 제 1 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 1 자세각을 출력하는 제 1 상보 필터와, 상기 제 1 차단 주파수와는 상이한 제 2 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 2 자세각을 출력하는 제 2 상보 필터와, 상기 작업 기계의 상태에 따라, 상기 제 1 자세각 또는 상기 제 2 자세각을 전환하여 출력하는 전환부를 포함하는, 작업 기계의 자세 연산 장치이다.

상기 제 1 차단 주파수는, 상기 제 2 차단 주파수보다 높은 것이 바람직하다.

상기 전환부는, 상기 작업 기계가 준정지 상태라고 판정한 경우에는 상기 제 1 자세각을 출력하고, 상기 작업 기계가 동적 상태라고 판정한 경우에는 상기 제 2 자세각을 출력하는 것이 바람직하다.

상기 전환부는, 상기 제 1 자세각을 상기 제 2 자세각으로 전환한 후에는, 전환시에서의 상기 제 1 자세각으로부터 상기 제 2 자세각을 감산한 값을 보정값으로서 사용하여, 상기 제 2 자세각을 보정하는 것이 바람직하다.

상기 전환부는, 상기 제 2 자세각을 상기 제 1 자세각으로 전환한 후에는, 전환시에서의 상기 제 2 자세각의 오차에 0 보다 크고 1 보다 작은 계수를 곱한 값을 보정값으로서 사용하여, 상기 제 1 자세각을 보정하는 것이 바람직하다.

본 발명은, 유압 셔블의 상부 선회체에 구비되어, 각속도 및 가속도를 검출하는 계측 장치와, 상기 계측 장치가 검출한 상기 각속도 및 상기 가속도로부터 상기 유압 셔블의 자세각을 구하는 자세각 연산부와, 제 1 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 1 자세각을 출력하는 제 1 상보 필터와, 상기 제 1 차단 주파수보다 낮은 제 2 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 2 자세각을 출력하는 제 2 상보 필터와, 상기 유압 셔블이 준정지 상태라고 판정한 경우에는 상기 제 1 자세각을 출력하고, 상기 유압 셔블이 동적 상태라고 판정한 경우에는 상기 제 2 자세각을 출력하는 전환부를 포함하는, 유압 셔블의 자세 연산 장치이다.

본 발명은 전술한 작업 기계의 자세 연산 장치와, 상기 작업 기계의 자세 연산 장치로부터 출력된 상기 제 1 자세각 또는 상기 제 2 자세각을 사용하여, 상기 작업 기계에 장착된 작업기의 위치를 구하는 제어부를 구비한 작업 기계이다.

본 발명은 작업 기계의 자세각을 구할 때에, 자세각의 정밀도 저하를 억제할 수 있다.

도 1 은 본 실시형태에 관련된 작업 기계의 사시도이다.
도 2 는 본 실시형태에 관련된 작업 기계의 제어계를 나타내는 도면이다.
도 3 은 IMU 의 일례를 나타내는 블록도이다.
도 4 는 센서 제어 장치를 나타내는 블록도이다.
도 5 는 상보 필터의 특성을 나타내는 도면이다.
도 6 은 오차의 주파수 특성을 나타내는 도면이다.
도 7 은 제 1 상보 필터의 게인 및 제 2 상보 필터의 게인과 주파수의 관계를 나타내는 도면이다.
도 8 은 본 실시형태의 변형예에 있어서의 제 1 자세각과 제 2 자세각의 전환에 사용하는 테이블의 일례를 나타내는 도면이다.
도 9 는 센서 제어 장치의 전환부가 출력하는 유압 셔블의 자세각과, 제 1 자세각과, 제 2 자세각의 시간 변화의 일례를 나타내는 도면이다.
도 10 은 자세각을 구하는 처리의 일례를 나타내는 플로우 차트이다.
도 11 은 IMU 의 상태에 따라 자세각을 전환한 결과의 일례를 나타내는 도면이다.

본 발명을 실시하기 위한 형태 (실시형태) 에 대해, 도면을 참조하면서 상세하게 설명한다.

＜작업 기계의 전체 구성＞

도 1 은 본 실시형태에 관련된 작업 기계의 사시도이다. 도 2 는 본 실시형태에 관련된 작업 기계의 제어계를 나타내는 도면이다. 작업 기계로서의 유압 셔블 (100) 은 차량 본체 (1) 와 작업기 (2) 를 갖는다. 차량 본체 (1) 는, 선회체로서의 상부 선회체 (3) 와 주행체로서의 주행 장치 (5) 를 갖는다. 상부 선회체 (3) 는, 기관실 (3EG) 의 내부에, 도 2 에 나타내는 동력 발생 장치로서의 엔진 (36) 및 유압 펌프 (37) 등의 장치를 수용하고 있다. 기관실 (3EG) 은, 상부 선회체 (3) 의 후단측에 배치되어 있다. 본 실시형태를 설명할 때에, 정의하는 방향에 대해서는 후술한다.

본 실시형태에 있어서, 유압 셔블 (100) 은, 동력 발생 장치로서의 엔진 (36) 에, 예를 들어 디젤 엔진 등의 내연 기관이 사용되지만, 동력 발생 장치는 이와 같은 것에 한정되지 않는다. 유압 셔블 (100) 의 동력 발생 장치는, 예를 들어, 내연 기관과 발전 전동기와 축전 장치를 조합한, 이른바 하이브리드 방식의 장치여도 된다.

상부 선회체 (3) 는 운전실 (4) 을 갖는다. 운전실 (4) 은 상부 선회체 (3) 의 전단측에 설치되어 있다. 즉, 운전실 (4) 은 기관실 (3EG) 이 배치되어 있는 측과는 반대측에 설치되어 있다. 운전실 (4) 내에는, 도 2 에 나타내는, 제 1 표시 장치 (28) 및 조작 장치 (30) 가 배치된다. 이것들에 대해서는 후술한다. 상부 선회체 (3) 의 상방에는, 난간 (19) 및 안테나 (20, 21) 가 장착되어 있다. 안테나 (20, 21) 에 대한 상세는 후술한다.

주행 장치 (5) 는 캐터필러 밴드 (5a, 5b) 를 가지고 있다. 주행 장치 (5) 는, 좌우에 형성된 유압 모터 (5c) 의 일방 또는 양방이 구동되고, 캐터필러 밴드 (5a, 5b) 가 회전함으로써, 유압 셔블 (100) 을 전후로 주행 또는 선회시킨다. 작업기 (2) 는 상부 선회체 (3) 의 운전실 (4) 의 측방측에 장착되어 있다.

유압 셔블 (100) 은 캐터필러 밴드 (5a, 5b) 대신에 타이어를 구비하고, 도 2 에 나타내는 엔진 (36) 의 구동력을, 트랜스미션을 통해서 타이어에 전달하여 주행이 가능한 주행 장치를 구비한 것이어도 된다. 이와 같은 형태의 유압 셔블 (100) 로는, 예를 들어, 휠식 유압 셔블이 있다. 또, 유압 셔블 (100) 은, 이와 같은 타이어를 가진 주행 장치를 구비하고, 추가로 차량 본체에 작업기가 장착되고, 도 1 에 나타내는 바와 같은 상부 선회체 (3) 및 그 선회 기구를 구비하고 있지 않은 구조를 갖는, 예를 들어 백호우 로더 (backhoe loader) 여도 된다. 즉, 백호우 로더는, 차량 본체에 작업기가 장착되어, 차량 본체의 일부를 구성하는 주행 장치를 구비한 것이다.

상부 선회체 (3) 는, 작업기 (2) 및 운전실 (4) 이 배치되어 있는 측이 앞이고, 기관실 (3EG) 이 배치되어 있는 측이 뒤이다. 앞을 향하여 좌측이 상부 선회체 (3) 의 왼쪽이고, 앞을 향하여 우측이 상부 선회체 (3) 의 오른쪽이다. 또, 유압 셔블 (100) 또는 차량 본체 (1) 는, 상부 선회체 (3) 를 기준으로 하여 주행 장치 (5) 측이 아래이고, 주행 장치 (5) 를 기준으로 하여 상부 선회체 (3) 측이 위이다. 유압 셔블 (100) 이 수평면에 설치되어 있는 경우, 아래는 연직 방향, 즉 중력의 작용 방향측이고, 위는 연직 방향과는 반대측이다.

작업기 (2) 는, 붐 (6) 과 아암 (7) 과 버킷 (8) 과 붐 실린더 (10) 와 아암 실린더 (11) 와 버킷 실린더 (12) 를 갖는다. 붐 (6) 의 기단부는, 붐 핀 (13) 을 통하여 상부 선회체 (3) 의 전부 (前部) 에 요동 가능하게 장착되어 있다. 아암 (7) 의 기단부는, 아암 핀 (14) 을 통하여 붐 (6) 의 선단부에 요동 가능하게 장착되어 있다. 아암 (7) 의 선단부에는, 버킷 핀 (15) 을 통하여 버킷 (8) 이 장착되어 있다. 버킷 (8) 은 버킷 핀 (15) 을 중심으로 하여 요동한다. 버킷 (8) 은, 버킷 핀 (15) 과는 반대측에 복수의 날 (8B) 이 장착되어 있다. 날끝 (8T) 은 날 (8B) 의 선단이다.

버킷 (8) 은, 복수의 날 (8B) 을 가지고 있지 않아도 된다. 요컨대, 도 1 에 나타내는 바와 같은 날 (8B) 을 가지고 있지 않고, 날끝이 강판에 의해 스트레이트 형상으로 형성된 버킷이어도 된다. 작업기 (2) 는, 예를 들어, 단수 (單數) 의 날을 갖는 틸트 버킷을 구비하고 있어도 된다. 틸트 버킷이란, 버킷 틸트 실린더를 구비하고, 버킷이 좌우로 틸트 경사짐으로써 유압 셔블이 경사지에 있어도, 경사면, 평지를 자유로운 형태로 성형, 정지 (整地) 할 수 있고, 바닥판 플레이트에 의한 전압 (轉壓) 작업도 할 수 있는 버킷이다. 이 밖에도, 작업기 (2) 는, 버킷 (8) 대신에 법면 버킷 또는 삭암용 칩을 구비한 삭암용 어태치먼트 등을 구비하고 있어도 된다.

도 1 에 나타내는 붐 실린더 (10) 와 아암 실린더 (11) 와 버킷 실린더 (12) 는, 각각 작동유의 압력 (이하, 적절히 유압이라고 한다) 에 의해 구동되는 유압 실린더이다. 붐 실린더 (10) 는 붐 (6) 을 구동시켜, 이것을 승강시킨다. 아암 실린더 (11) 는 아암 (7) 을 구동시켜, 아암 핀 (14) 의 둘레를 회동시킨다. 버킷 실린더 (12) 는 버킷 (8) 을 구동시켜, 버킷 핀 (15) 의 둘레를 회동시킨다.

붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등의 유압 실린더와 도 2 에 나타내는 유압 펌프 (37) 사이에는, 도 2 에 나타내는 유압 제어 밸브 (38) 가 형성되어 있다. 유압 제어 밸브 (38) 는, 유압 모터 (5c) 를 구동시키기 위한 주행용 제어 밸브와, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 그리고 상부 선회체 (3) 를 선회시키는 선회 모터를 제어하기 위한 작업기용 제어 밸브를 포함한다. 도 2 에 나타내는 작업기 제어 장치 (25) 가 유압 제어 밸브 (38) 를 제어함으로써, 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12), 선회 모터 또는 유압 모터 (5c) 에 공급되는 작동유의 유량이 제어된다. 그 결과, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등의 동작이 제어된다.

상부 선회체 (3) 의 상부에는 안테나 (20, 21) 가 장착되어 있다. 안테나 (20, 21) 는 유압 셔블 (100) 의 현재 위치를 검출하기 위해서 사용된다. 안테나 (20, 21) 는, 도 2 에 나타내는 유압 셔블 (100) 의 현재 위치를 검출하기 위한 위치 검출 장치 (23) 와 전기적으로 접속되어 있다. 위치 검출 장치 (23) 는, RTK-GNSS (Real Time Kinematic - Global Navigation Satellite SYstems, GNSS는 전지구 항법 위성 시스템을 말한다) 를 이용하여 유압 셔블 (100) 의 현재 위치를 검출한다. 이하의 설명에 있어서, 안테나 (20, 21) 를 적절히 GNSS 안테나 (20, 21) 라고 한다.

GNSS 안테나 (20, 21) 가 수신한 GNSS 전파에 따른 신호는, 위치 검출 장치 (23) 에 입력된다. 위치 검출 장치 (23) 는 GNSS 안테나 (20, 21) 의 설치 위치를 검출한다.

GNSS 안테나 (20, 21) 는, 도 1 에 나타내는 바와 같이 상부 선회체 (3) 상으로서, 유압 셔블 (100) 의 좌우 방향으로 떨어진 양단 위치에 설치되는 것이 바람직하다. GNSS 안테나 (20, 21) 는, 상부 선회체 (3) 상으로서, 도 1 에 나타내는 바와 같이 난간 (19) 에 장착되어 있어도 된다. 또, GNSS 안테나 (20, 21) 는, 상부 선회체 (3) 의 후단에 형성된 카운터 웨이트 (3CW) 의 상부 또는 운전실 (4) 의 후방으로서 기계실 (3EG) 상에 설치되어도 된다. 어쨌든, GNSS 안테나 (20, 21) 는, 서로 가능한 한 떨어진 위치에 설치되는 편이, 유압 셔블 (100) 의 현재 위치의 검출 정밀도는 향상된다. 또, GNSS 안테나 (20, 21) 는, 오퍼레이터의 시야를 최대한 방해하지 않는 위치에 설치되는 것이 바람직하다.

(유압 셔블의 제어계)

도 2 를 사용하여, 유압 셔블 (100) 의 제어계에 대해 설명한다. 유압 셔블 (100) 은, 제어계로서, 작업 기계의 자세 연산 장치로서의 센서 제어 장치 (24) 와, 작업기 제어 장치 (25) 와, 엔진 제어 장치 (26) 와, 펌프 제어 장치 (27) 와, 제 1 표시 장치 (28) 와, 각속도 및 가속도를 검출하는 관성 계측 장치로서의 IMU (29) 와, 제 2 표시 장치 (39) 를 포함한다. 이들은, 상부 선회체 (3) 에 설치된다. 예를 들어, IMU (29) 는 운전실 (4) 의 하부에 설치되어 있다. 제 1 표시 장치 (28) 및 제 2 표시 장치 (39) 는, 운전실 (4) 의 내부의 운전석 근방에 설치된다.

센서 제어 장치 (24) 와, 작업기 제어 장치 (25) 와, 엔진 제어 장치 (26) 와, 펌프 제어 장치 (27) 와, 제 1 표시 장치 (28) 와, 위치 검출 장치 (23) 는, 유압 셔블 (100) 내에 설치된 차내 신호선 (41) 과 전기적으로 접속되어 있다. 센서 제어 장치 (24) 와, 작업기 제어 장치 (25) 와, 엔진 제어 장치 (26) 와, 펌프 제어 장치 (27) 와, 제 1 표시 장치 (28) 와, 위치 검출 장치 (23) 는, 차내 신호선 (41) 을 통하여 서로 통신할 수 있다. 센서 제어 장치 (24) 와 IMU (29) 와 제 2 표시 장치 (39) 는, 차내 신호선 (41) 과는 상이한 차내 신호선 (42) 과 전기적으로 접속되어 있다. 센서 제어 장치 (24) 와 IMU (29) 와 제 2 표시 장치 (39) 는, 차내 신호선 (42) 을 통하여 서로 통신할 수 있다. IMU (29) 는 차내 신호선 (42) 이 아니라, 차내 신호선 (41) 과 전기적으로 접속되어 있어도 된다.

센서 제어 장치 (24) 는, 도 1 에 나타내는 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 의 스트로크를 검출하는 센서, 붐 (6) 의 각도 및 아암 (7) 의 각도를 검출하는 센서 그리고 상부 선회체 (3) 의 선회 각도를 검출하는 센서 등과 같은 각종 센서류 (35) 가 전기적으로 접속된다. 붐 (6) 의 각도, 아암 (7) 의 각도 및 선회 각도를 검출하는 센서는 반드시 필요하지는 않다. 센서 제어 장치 (24) 는, 각종 센서류 (35) 가 검출한 신호에, 필터 처리 또는 A/D (Analog/Digital) 변환 등의 각종 신호 처리를 실시한 후, 차내 신호선 (41) 에 출력한다.

붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 의 스트로크를 검출하는 센서로서, 각 유압 실린더의 신축량을 검지하는 근접 센서나 로터리 인코더 등을 사용할 수 있다. 또, 붐 (6) 의 기단부 및 아암 (7) 의 기단부에, 도시되지 않은 인코더를 구비하고, 그 인코더에 의해 각 유압 실린더의 신축량을 검지하는 근접 센서 또는 로터리 인코더의 검출값을 리셋할 수 있도록 해도 된다.

센서 제어 장치 (24) 는 차내 신호선 (42) 으로부터, IMU (29) 가 검출한 신호, 구체적으로는 가속도 및 각속도를 취득한다. 센서 제어 장치 (24) 는, IMU (29) 로부터 취득한 가속도 및 각속도를 사용하여 작업 기계의 자세각을 구하고, 구해진 자세각에 본 실시형태에 관련된 필터 처리를 실시하여 노이즈를 제거하고 나서, 차내 신호선 (41) 에 출력한다. 자세각은 유압 셔블 (100) 의 기울기에 관한 정보이다. 자세각의 정밀도는, 제 2 표시 장치 (39) 에 의한 날끝 (8T) 의 위치 정보의 표시 정밀도 및 유압 셔블 (100) 의 작업기 제어의 정밀도에 영향을 미친다. 본 실시형태에 관련된 필터 처리를 실행함으로써, 자세각의 정밀도를 확보한다. 본 실시형태에 관련된 필터 처리에 대해서는 후술한다.

작업기 제어 장치 (25) 는 조작 장치 (30) 로부터의 입력에 기초하여, 도 1 에 나타내는 작업기 (2) 의 동작, 상부 선회체 (3) 의 동작 및 주행 장치 (5) 의 동작을 제어한다. 조작 장치 (30) 는, 조작부로서의 작업기 조작 부재 (31L, 31R) 및 주행 조작 부재 (33L, 33R) 와, 작업기 조작 검출부 (32L, 32R) 및 주행 조작 검출부 (34L, 34R) 를 갖는다. 본 실시형태에 있어서, 작업기 조작 부재 (31L, 31R) 및 주행 조작 부재 (33L, 33R) 는, 파일럿압 방식의 레버이지만, 이것에 한정되지 않는다. 작업기 조작 부재 (31L, 31R) 및 주행 조작 부재 (33L, 33R) 는, 예를 들어, 포텐셔미터를 사용하여 각 조작 부재 (레버) 의 조작량을 검출할 수 있는 전기 방식의 레버여도 된다.

작업기 조작 부재 (31L, 31R) 는 운전석의 좌우에 설치되어, 유압 셔블 (100) 의 오퍼레이터가 작업기 (2) 를 조작하기 위한 부재이며, 예를 들어, 조이스틱과 같은 손잡이 부분과 봉재를 구비한 조작 레버이다. 이와 같은 구조의 작업기 조작 부재 (31L, 31R) 는, 손잡이부를 잡아 전후좌우로 경도시키는 것이 가능하다. 예를 들어 왼쪽에 설치된 작업기 조작 부재 (31L) 를 조작함으로써, 아암 (7) 및 상부 선회체 (3) 를 동작시킬 수 있고, 오른쪽에 설치된 작업기 조작 부재 (31R) 를 조작함으로써, 버킷 (8) 및 붐 (6) 을 동작시킬 수 있다.

작업기 조작 검출부 (32L, 32R) 는, 작업기 조작 부재 (31L, 31R) 에 대한 입력, 즉 조작 내용에 따라 파일럿압을 발생시키고, 유압 제어 밸브 (38) 가 구비하는 작업용 제어 밸브에, 발생한 작동유의 파일럿압을 공급한다. 이 파일럿압의 크기에 따라, 작업용 제어 밸브가 동작되어, 도 1 에 나타내는 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등에 유압 펌프 (37) 로부터 작동유가 공급된다. 작업기 조작 부재 (31L, 31R) 가 전기 방식의 레버인 경우, 작업기 조작 검출부 (32L, 32R) 는, 작업기 조작 부재 (31L, 31R) 에 대한 입력, 즉 조작 내용을, 예를 들어 포텐셔미터 등을 사용하여 검출하고, 입력을 전기 신호 (검출 신호) 로 변환하여 작업기 제어 장치 (25) 에 보낸다. 작업기 제어 장치 (25) 는, 이 검출 신호에 기초하여 작업용 제어 밸브를 제어한다.

주행 조작 부재 (33L, 33R) 는, 운전석의 전방에 설치되어, 오퍼레이터가 유압 셔블 (100) 의 주행 장치 (5) 를 주행 또는 선회시킬 때 조작하기 위한 부재이다. 주행 조작 부재 (33L, 33R) 는, 예를 들어, 손잡이부와 봉재를 구비한 조작 레버 (이하, 적절히 주행 레버라고 호칭한다) 이다. 이와 같은 주행 조작 부재 (33L, 33R) 는, 오퍼레이터가 손잡이부를 잡아 전후로 경도시키는 것이 가능하다. 주행 조작 부재 (33L, 33R) 는, 2 개의 조작 레버가 동시에 앞으로 경도되면 유압 셔블 (100) 이 전진하고, 뒤로 경도되면 유압 셔블 (100) 은 후진한다.

주행 조작 부재 (33L, 33R) 는, 오퍼레이터가 다리로 밟음으로써 조작이 가능한 도시되지 않은 페달이어도 되며, 예를 들어 시소 (seesaw) 식의 페달이다. 페달의 전측 또는 후측 중 어느 것을 밟음으로써 전술한 조작 레버와 동일하게 파일럿압이 발생하여, 주행용 제어 밸브가 제어되고, 유압 모터 (5c) 가 구동되어 유압 셔블 (100) 을 전진 또는 후진시킬 수 있다. 2 개의 페달을 동시에, 또한 전측을 밟으면 유압 셔블 (100) 은 전진하고, 후측을 밟으면 유압 셔블 (100) 은 후진한다. 편방의 페달의 전측 또는 후측을 밟으면, 캐터필러 밴드 (5a, 5b) 의 편측만이 회전하여, 유압 셔블 (100) 의 주행 장치 (5) 를 선회시킬 수 있다.

이와 같이, 오퍼레이터는, 유압 셔블 (100) 을 주행시키고자 하는 경우, 손으로 조작 레버를 전후로 경도시키거나 또는 다리로 페달의 전측 또는 후측을 밟거나 중 어느 일방을 실행하면, 주행 장치 (5) 의 유압 모터 (5c) 를 구동시킬 수 있다. 도 2 에 나타내는 바와 같이, 주행 조작 부재 (33L, 33R) 및 주행 조작 검출부 (34L, 34R) 는 2 세트 존재한다. 좌측의 주행 조작 부재 (33L) 를 조작함으로써, 좌측의 유압 모터 (5c) 를 구동시켜 좌측의 캐터필러 밴드 (5b) 를 동작시킬 수 있다. 우측의 주행 조작 부재 (33R) 를 조작함으로써, 우측의 유압 모터 (5c) 를 구동시켜 우측의 캐터필러 밴드 (5a) 를 동작시킬 수 있다.

주행 조작 검출부 (34L, 34R) 는, 주행 조작 부재 (33L, 33R) 에 대한 입력, 즉 조작 내용에 따라 파일럿압을 발생시키고, 유압 제어 밸브 (38) 가 구비하는 주행용 제어 밸브에, 발생한 파일럿압을 공급한다. 이 파일럿압의 크기에 따라, 주행용 제어 밸브가 동작되어, 주행용의 유압 모터 (5c) 에 작동유가 공급된다. 주행 조작 부재 (33L, 33R) 가 전기 방식의 레버인 경우, 주행 조작 검출부 (34L, 34R) 는, 주행 조작 부재 (33L, 33R) 에 대한 입력, 즉 조작 내용을, 예를 들어 포텐셔미터 등을 사용하여 검출하고, 입력을 전기 신호 (검출 신호) 로 변환하여 작업기 제어 장치 (25) 에 보낸다. 작업기 제어 장치 (25) 는, 이 검출 신호에 기초하여 주행용 제어 밸브를 제어한다.

엔진 제어 장치 (26) 는, 엔진 (36) 에 공급하는 연료의 양을 조정함으로써 엔진 (36) 의 출력을 제어한다. 엔진 (36) 은, 출력 샤프트에 기계적으로 연결된 유압 펌프 (37) 를 구동시킨다. 엔진 (36) 에 연결된 유압 펌프 (37) 는, 유압 셔블 (100) 이 구비하는 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등의 유압 기기에 작동유를 공급한다. 엔진 제어 장치 (26) 에는 회전 속도 검출 센서 (36R) 및 연료 조정 다이얼 (26D) 이 전기적으로 접속되어 있다. 엔진 제어 장치 (26) 는, 회전 속도 검출 센서 (36R) 가 검출한 엔진 (36) 의 크랭크 샤프트의 회전 속도 및 연료 조정 다이얼 (26D) 의 설정 등에 기초하여, 엔진 (36) 에 공급하는 연료의 양을 제어한다. 이와 같이 하여, 엔진 제어 장치 (26) 는 엔진 (36) 의 출력을 제어한다.

펌프 제어 장치 (27) 는, 유압 셔블 (100) 이 구비하는 유압 펌프 (37) 를 제어한다. 유압 펌프 (37) 는, 예를 들어, 사판 (斜板) 의 경전각을 변경함으로써 작동유의 토출량 등을 변경하는 사판식의 유압 펌프이다. 펌프 제어 장치 (27) 는, 예를 들어, 차내 신호선 (41) 을 통하여 작업기 제어 장치 (25) 로부터, 유압 제어 밸브 (38) 의 유압 센서 (38C) 가 검출한 파일럿압을 취득한다. 펌프 제어 장치 (27) 는, 취득한 파일럿압에 기초하여 유압 펌프 (37) 의 사판의 경전각을 제어함으로써, 유압 펌프 (37) 로부터 토출되는 작동유의 유량을 제어한다. 유압 펌프 (37) 로부터 토출된 작동유는, 유압 제어 밸브 (38) 가 구비하는 작업용 제어 밸브 또는 주행용 제어 밸브를 통하여, 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12) 및 유압 모터 (5c) 및 상부 선회체 (3) 를 선회시키기 위한 선회 모터 중 적어도 1 개에 공급되고, 이들 중 적어도 1 개를 구동시킨다.

제 1 표시 장치 (28) 는 화상을 표시하는 장치이다. 제 1 표시 장치 (28) 는 표시부 (28M) 와 제어부 (28C) 를 포함한다. 제 1 표시 장치 (28) 는, 도 1 에 나타내는 유압 셔블 (100) 의 운전실 (4) 내로서, 운전석 근방에 설치된다. 본 실시형태에 있어서, 제 1 표시 장치 (28) 는, 예를 들어, 유압 셔블 (100) 의 가동 정보를 표시부 (28M) 에 표시한다. 가동 정보는, 예를 들어, 유압 셔블 (100) 의 누적 가동 시간, 연료의 잔량 또는 엔진 (36) 의 냉각수 온도 등이다. 유압 셔블 (100) 이 주변 감시용 또는 백 모니터용 카메라 등을 구비하는 경우, 제 1 표시 장치 (28) 는, 이 카메라가 촬상한 화상을 표시해도 된다.

본 실시형태에 있어서, 제 1 표시 장치 (28) 는, 각종 화상을 표시부 (28M) 에 표시하는 것 이외에, 입력 장치로서도 기능한다. 이 때문에, 제 1 표시 장치 (28) 는, 표시부 (28M) 의 하방에 입력 장치 (28I) 를 구비한다. 본 실시형태에 있어서, 입력 장치 (28I) 는, 복수의 누름 버튼식 스위치가 표시부 (28M) 의 횡방향에 대해 평행하게 배열되어 있다. 입력 장치 (28I) 가 조작됨으로써, 표시부 (28M) 에 표시되는 화상을 전환하거나, 유압 셔블 (100) 의 동작에 관한 각종 설정을 실행하거나 할 수 있다. 또한, 입력 장치 (28I) 를 표시부 (28M) 에 장착한 터치 패널에 의해 제 1 표시 장치 (28) 를 구성하도록 해도 된다. 또, 입력 장치 (28I) 는, 제 1 표시 장치 (28) 와는 별체로서, 운전석 근방의 콘솔에 설치하도록 해도 된다.

제 2 표시 장치 (39) 는 화상을 표시하는 장치이다. 제 2 표시 장치 (39) 는, 표시부 (39M) 와 제어부 (39C) 를 포함한다. 제 2 표시 장치 (39) 는, 도 1 에 나타내는 유압 셔블 (100) 의 운전실 (4) 내의 운전석 근방에 설치된다. 본 실시형태에 있어서, 제 2 표시 장치 (39) 는, 예를 들어, 유압 셔블 (100) 이 구비하는 버킷 (8) 의 날끝 (8T) 의, 시공 현장의 지형에 대한 위치 정보를 화상으로서 표시부 (28M) 에 표시한다. 이 때, 제 2 표시 장치 (39) 는, 날끝 (8T) 이 굴삭하고자 하고 있는 시공 현장의 지형에 관한 정보를, 날끝 (8T) 의 위치 정보와 함께 표시해도 된다.

본 실시형태에 있어서, 제 2 표시 장치 (39) 의 표시부 (39M) 는, 예를 들어, 액정 표시 장치이지만, 이것에 한정되는 것은 아니다. 제어부 (39C) 는, 표시부 (39M) 의 동작을 제어하거나, 날끝 (8T) 의 위치 정보를 구하거나 한다. 또, 제어부 (39C) 는, 날끝 (8T) 의 위치와 시공 현장의 지형의 상대 위치 관계를 나타내는 가이던스 화상을 표시부 (39M) 에 표시시키거나 한다. 이 때문에, 제어부 (39C) 는, 시공 현장의 지형에 대한 글로벌 좌표 위치 정보를 기억하고 있다.

본 실시형태에 있어서, 제 2 표시 장치 (39) 는, 표시부 (39M) 의 하방에 입력 장치 (39I) 를 구비한다. 본 실시형태에 있어서, 입력 장치 (39I) 는, 복수의 누름 버튼식 스위치가 표시부 (39M) 의 횡방향에 대해 평행하게 배열되어 있다. 입력 장치 (39I) 가 조작됨으로써, 표시부 (39M) 에 표시되는 가이던스 화상을 전환하거나, 가이던스의 내용을 변경하거나 할 수 있다. 본 실시형태에 있어서, 제 2 표시 장치 (39) 의 기능은, 제 1 표시 장치 (28) 가 실현해도 된다. 입력 장치 (39I) 를 표시부 (39M) 에 장착한 터치 패널에 의해 제 2 표시 장치 (39) 를 구성하도록 해도 된다. 또, 제 1 표시 장치 (28) 와 제 2 표시 장치 (39) 를 하나의 케이싱에 넣은 표시 장치로 한 것을 사용해도 된다.

IMU (29) 는, 유압 셔블 (100) 의 각속도 및 가속도를 검출한다. 유압 셔블 (100) 의 동작에 수반하여, 주행시에 발생하는 가속도, 선회시에 발생하는 각가속도 및 중력 가속도와 같은 여러 가지 가속도가 발생하지만, IMU (29) 는 적어도 중력 가속도를 포함하는 가속도를 검출하고, 각 가속도의 종류를 구별하지 않고 검출한 가속도를 출력한다. IMU (29) 는, 상세한 것에 대해서는 후술하지만, 보다 높은 정밀도로 가속도를 검출하기 위해서, 예를 들어, 유압 셔블 (100) 의 상부 선회체 (3) 의 선회 중심축 상에 형성되는 것이 바람직하지만, 전술한 바와 같이 IMU (29) 는 운전실 (4) 의 하부에 설치되어도 된다.

IMU (29) 는, 도 1 에 나타내는 좌표계 (X, Y, Z) 에 있어서, X 축 방향, Y 축 방향 및 Z 축 방향의 가속도 그리고 X 축, Y 축 및 Z 축 둘레의 각속도 (회전 각속도) 를 검출한다. 도 1 에 나타내는 예에 있어서, X 축은 유압 셔블 (100) 의 전후 방향과 평행한 축이고, Y 축은 유압 셔블 (100) 의 폭 방향과 평행한 축이며, Z 축은 X 축 및 Y 축의 양방에 직교하는 축이다. 좌표계 (X, Y, Z) 는, 예를 들어 차체 좌표계로 할 수 있다.

도 3 은, IMU (29) 의 일례를 나타내는 블록도이다. IMU (29) 는, 자이로 (29V) 와, 가속도 센서 (29A) 와, AD 변환부 (29AD) 와, 물리량 변환부 (29PT) 를 포함한다. 자이로 (29V) 는 유압 셔블 (100) 의 각속도를 검출한다. 가속도 센서 (29A) 는 유압 셔블의 가속도를 검출한다. 자이로 (29V) 에 의해 검출된 각속도 및 가속도 센서 (29A) 에 의해 검출된 가속도는 모두 아날로그량이다. AD 변환부 (29AD) 는, 이것들 아날로그량을 디지털량으로 변환한다. 물리량 변환부 (29PT) 는, AD 변환부 (29AD) 의 출력을 물리량으로 변환한다. 구체적으로는, 물리량 변환부 (29PT) 는, 자이로 (29V) 의 검출값에 대응하는 AD 변환부 (29AD) 의 출력을 각속도 (ω) 로 변환하고, 가속도 센서 (29A) 의 검출값에 대응하는 AD 변환부 (29AD) 의 출력을 가속도 (Ac) 로 변환한다. 물리량 변환부 (29PT) 는, 각속도 (ω) 및 가속도 (Ac) 를 차내 신호선 (42) 에 출력한다.

유압 셔블 (100) 의 기울기는, 피치각, 롤각 및 요잉각으로 나타낼 수 있다. 피치각은, Y 축 둘레로 유압 셔블 (100) 이 경사졌을 때의 각도이며, 롤각은 X 축 둘레로 유압 셔블 (100) 이 경사졌을 때의 각도이며, 요잉각은, Z 축 둘레로 유압 셔블 (100) 이 경사졌을 때의 각도이다. 본 실시형태에 있어서, 피치각 및 롤각을 유압 셔블 (100) 의 자세각이라고 한다. 본 실시형태에 있어서, 센서 제어 장치 (24) 는, IMU (29) 가 검출한 유압 셔블 (100) 의 각속도 및 가속도를, 차내 신호선 (42) 를 통하여 취득한다. 센서 제어 장치 (24) 는, 취득한 유압 셔블 (100) 의 각속도 및 가속도로부터 자세각을 구한다. 이하에 있어서, 자세각은 적절히 부호 θ 를 사용하여 나타낸다.

도 4 는, 센서 제어 장치 (24) 를 나타내는 블록도이다. 센서 제어 장치 (24) 는, 자세각 연산부로서의 제 1 자세각 연산부 (51) 와, 동일하게 자세각 연산부로서의 제 2 자세각 연산부 (52) 와, 제 1 상보 필터 (53) 와, 제 2 상보 필터 (54) 와, 전환부 (55) 를 포함한다. 제 1 자세각 연산부 (51) 및 제 2 자세각 연산부 (52) 는, 유압 셔블 (100) 의 각속도 (ω) 및 가속도 (Ac) 로부터 유압 셔블 (100) 의 자세각 (θ) 을 구한다. 본 실시형태에 있어서, 제 1 자세각 연산부 (51) 는, IMU (29) 가 검출한 유압 셔블 (100) 의 가속도 (Ac) 로부터 자세각 (θ) 을 구한다. 보다 구체적으로는, 제 1 자세각 연산부 (51) 는, 중력 가속도의 방향으로부터 자세각 (θ) 을 구한다. 제 2 자세각 연산부 (52) 는, IMU (29) 가 검출한 유압 셔블 (100) 의 각속도 (ω) 로부터 자세각 (θ) 을 구한다. 보다 구체적으로는, 제 2 자세각 연산부 (52) 는, 각속도 (ω) 를 적분하여 자세각 (θ) 을 구한다.

제 1 상보 필터 (53) 는 제 1 차단 주파수가 설정되어, 제 1 자세각 연산부 (51) 및 제 2 자세각 연산부 (52) 에 의해 구해진 자세각 (θ) 에 포함되는 잡음을 저감시키고, 제 1 자세각 (θ1) 을 출력한다. 제 2 상보 필터 (54) 는, 제 1 차단 주파수와는 상이한 제 2 차단 주파수가 설정되어, 제 1 자세각 연산부 (51) 및 제 2 자세각 연산부 (52) 에 의해 구해진 자세각 (θ) 에 포함되는 잡음을 저감시켜 제 2 자세각 (θ2) 을 출력한다. 제 1 상보 필터 (53) 와 제 2 상보 필터 (54) 는, 차단 주파수 (컷오프 주파수) 만이 상이하다.

제 1 상보 필터 (53) 는 필터부 (53F) 와 가산부 (53AD) 를 갖는다. 필터부 (53F) 는, 제 1 LPF (Low Pass Filter) a 와 제 1 HPF (High Pass Filter) a를 갖는다. 가산부 (53AD) 는, 제 1 LPFa 의 출력과 제 1 HPFa 의 출력을 가산하여 출력한다. 가산부 (53AD) 의 출력은 제 1 상보 필터 (53) 의 출력이다. 제 1 상보 필터 (53) 의 출력을 적절히 제 1 자세각 (θ1) 이라고 칭한다.

제 2 상보 필터 (54) 는 필터부 (54F) 와 가산부 (54AD) 를 갖는다. 필터부 (54F) 는, 제 2 LPF (Low Pass Filter) b 와 제 2 HPF (High Pass Filter) b 를 갖는다. 가산부 (54AD) 는, 제 2 LPFb 의 출력과 제 2 HPFb 의 출력을 가산하여 출력한다. 가산부 (54AD) 의 출력은 제 2 상보 필터 (54) 의 출력이다. 제 2 상보 필터 (54) 의 출력을 제 2 자세각 (θ2) 이라고 칭한다.

전환부 (55) 는 처리부 (55C) 와 전환기 (55S) 를 갖는다. 전환부 (55) 는, 유압 셔블 (100) 의 동작 상태에 따라, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환하여 출력한다. 다음으로, 이 출력에 대해 설명한다. 전환부 (55) 의 처리부 (55C) 는, 유압 셔블 (100) 의 상태, 예를 들어, 유압 셔블 (100) 이 움직이고 있는지 정지하고 있는지에 따라, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 중 어느 것을 출력하는지를 판정한다. 처리부 (55C) 의 판정 결과는, 판정 결과 출력선 (55a) 을 통하여 전환기 (55S) 에 출력되고, 전환기 (55S) 는, 처리부 (55C) 의 판정 결과에 따라, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 중 어느 일방을, 센서 제어 장치 (24) 에 의해 구해진 유압 셔블 (100) 의 자세각 (θo) 으로 하여, 자세각 출력선 (55b) 을 통하여 차내 신호선 (41) 에 출력한다.

도 5 는 상보 필터의 특성을 나타내는 도면이다. 도 5 의 세로축은 게인 (GN), 가로축은 주파수 (f) 이다. 도 5 의 곡선 (LPF 와 HPF) 은, 상보 필터의 주파수 특성을 나타낸다. 상보 필터는, LPF (Low Pass Filter) 와 HPF (High Pass Filter) 를 구비하고 있고, 도 5 로부터 알 수 있는 바와 같이, LPF 의 게인 (GN) 과 HPF 의 게인 (GN) 의 합이 1 이 되는 필터이다. 예를 들어, 상보 필터에 자세각 (θ) 을 입력하면, LPF 의 출력 LPF(θ) 와 HPF 의 출력 HPF(θ) 의 합은 1 이 된다. 즉, LPF(θ) ＋ HPF(θ) ＝ θ 가 된다. LPF 의 게인 (GN) 과 HPF 의 게인 (GN) 이 모두 0.5 가 될 때의 주파수를, 차단 주파수 (fc) 라고 한다. 센서 제어 장치 (24) 가 갖는 제 1 상보 필터 (53) 및 제 2 상보 필터 (54) 는, 전술한 바와 같이, 차단 주파수 (fc) 만이 상이하다.

도 4 에 나타내는 제 1 자세각 연산부 (51) 가 중력 가속도의 방향으로부터 구한 자세각 (θ) 은, 참의 자세각 (θtr) 과 오차 (θan) 의 합으로 구해진다. 오차 (θan) 는, 예를 들어, 충격 가속도 등과 같은 중력 가속도 이외의 가속도에 의해 발생한다. 오차 (θan) 는 고주파 성분이 주체인 잡음이다. 도 4 에 나타내는 제 2 자세각 연산부 (52) 가 각속도 (ω) 를 적분하여 구한 자세각 (θ) 은, 참의 자세각 (θtr) 과, 오차 (θwn) 의 합에서 구해진다. 오차 (θwn) 는, 적분에 의해 누적되는 드리프트에 의해 발생한다. 오차 (θwn) 는 저주파 성분이 주체인 잡음이다.

이와 같이, 제 1 자세각 연산부 (51) 가 중력 가속도의 방향으로부터 구한 자세각 (θ) 은, 고주파 성분이 주체인 오차 (θan) 를 포함하기 때문에, 제 1 상보 필터 (53) 의 제 1 LPFa 및 제 2 상보 필터 (54) 의 제 2 LPFb 에 입력된다. 제 2 자세각 연산부 (52) 가 각속도 (ω) 를 적분하여 구한 자세각 (θ) 은, 저주파 성분이 주체인 오차 (θwn) 를 포함하기 때문에, 제 1 상보 필터 (53) 의 제 1 HPFa 및 제 2 상보 필터 (54) 의 제 2 HPFb 에 입력된다.

제 1 LPFa 의 출력은 LPFa(θtr ＋ θan), 제 1 HPFa 의 출력은 HPFa(θtr ＋ θwn) 이 된다. 제 2 LPFb 의 출력은 LPFb(θtr ＋ θan), 제 2 HPFb 의 출력은 HPFb(θtr ＋ θwn) 이 된다. LPFa(θtr ＋ θan), HPFa(θtr ＋ θwn), LPFb(θtr ＋ θan) 및 HPFb(θtr ＋ θwn) 은, 모두 선형성을 갖는다고 하면, 식 (1) 내지 식 (4) 가 성립된다.

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

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

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

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

전술한 상보 필터의 특성으로부터, LPFa(θ) ＋ HPFa(θ) ＝ θ 및 LPFb(θ) ＋ HPFb(θ) ＝ θ 가 성립된다. 제 1 상보 필터 (53) 에 있어서, 필터부 (53F) 의 출력, 즉 제 1 LPFa 의 출력과 제 1 HPFa 의 출력은, 가산부 (53AD) 에 의해 가산된다. 가산부 (53AD) 의 출력, 즉 제 1 자세각 (θ1) 은, θtr ＋ LPFa(θan) ＋ HPFa(θwn) 이 된다. 제 2 상보 필터 (54) 에 있어서, 필터부 (54F) 의 출력, 즉 제 2 LPFb 의 출력과 제 2 HPFb 의 출력은, 가산부 (54AD) 에 의해 가산된다. 가산부 (54AD) 의 출력, 즉 제 2 자세각 (θ2) 은, θtr ＋ LPFb(θan) ＋ HPFb(θwn) 이 된다.

오차 (θan) 는 고주파 성분이 주체이므로, 제 1 LPFa 및 제 2 LPFb 에 의해 저감된다. 이 때문에, LPFa(θan) 및 LPFb(θan) 의 값은 작아진다. 오차 (θwn) 는 저주파 성분이 주체이므로, 제 1 HPFa 및 제 2 HPFb 에 의해 저감된다. 이 때문에, LPFa(θan) 및 HPFa(θwn) 그리고 LPFb(θan) 및 HPFb(θwn) 의 값은 작아지고, 가산부 (53AD) 의 출력인 제 1 자세각 (θ1) 및 가산부 (54AD) 의 출력인 제 2 자세각 (θ2) 은, 참의 자세각 (θtr) 에 가까운 값이 된다.

도 6 은, 오차 (θan) 및 오차 (θwn) 의 주파수 특성을 나타내는 도면이다. 도 6 의 세로축은 오차 (θan) 및 오차 (θwn) 의 스펙트럼, 가로축은 주파수 (f) 이다. 만일, IMU (29) 의 성능이 높은 것을 사용할 수 있었던 경우, IMU (29) 가 검출하는 각속도 (ω) 및 가속도 (Ac) 의 정밀도도 높기 때문에, 도 4 에 나타내는 센서 제어 장치 (24) 가 갖는 제 1 자세각 연산부 (51) 가 구하는 자세각 (θ) 의 오차 (θan) 및 제 2 자세각 연산부 (52) 가 구하는 자세각 (θ) 의 오차 (θwn) 는 작아진다. IMU (29) 의 성능이 낮은 경우, IMU (29) 가 검출하는 각속도 (ω) 및 가속도 (Ac) 의 정밀도가 낮아지기 때문에, 도 4 에 나타내는 센서 제어 장치 (24) 가 갖는 제 1 자세각 연산부 (51) 가 구하는 자세각 (θ) 의 오차 (θan) 및 제 2 자세각 연산부 (52) 가 구하는 자세각 (θ) 의 오차 (θwn) 는 커진다. 그 결과로서, 도 6 에 나타내는 바와 같이, 오차 (θwn) 와 오차 (θan) 가, 각각 상보 필터의 차단 주파수 (fc) 를 초과해도 존재하여, 차단 주파수 (fc) 를 포함하는 소정의 주파수 (f) 의 범위에서 서로 중첩된다. 오차 (θwn) 는, 차단 주파수 (fc) 보다 큰 주파수여도 존재하고, 오차 (θan) 는 차단 주파수 (fc) 보다 작은 주파수여도 존재한다.

따라서, IMU (29) 의 성능이 낮은 경우, 하나의 상보 필터에서는 잡음인 오차 (θwn) 및 오차 (θan) 를 충분히 제거할 수 없어, 자세각 (θ) 의 정밀도 저하를 초래할 가능성이 있다. 이것은, 제 2 표시 장치 (39) 에 의한 날끝 (8T) 의 위치 정보의 표시 정밀도 및 유압 셔블 (100) 의 작업기 제어의 정밀도에 영향을 미칠 가능성이 있다. 고성능의 IMU (29) 는 가격도 비싸기 때문에, 유압 셔블 (100) 의 제조 비용의 상승을 초래한다. 요컨대, 성능이 낮은 IMU (29) 를 유압 셔블 (100) 에 적용하기 위해서, 도 6 에 나타낸 특성을 고려할 필요가 있다. 그래서, 센서 제어 장치 (24) 는, 비교적 성능이 낮은 IMU (29) 를 사용한 경우에도, 자세각 (θ) 의 정밀도 저하를 억제할 수 있도록, 차단 주파수 (fc) 가 상이한 제 1 상보 필터 (53) 와 제 2 상보 필터 (54) 를 사용하고 있다.

도 7 은, 제 1 상보 필터 (53) 의 게인 (GN) 및 제 2 상보 필터 (54) 의 게인 (GN) 과 주파수 (f) 의 관계를 나타내는 도면이다. 도 7 의 세로축은 게인 (GN), 가로축은 주파수 (f) 이다. 주파수 (fch) 는 제 1 상보 필터 (53) 의 제 1 차단 주파수이고, 주파수 (fcl) 는 제 2 상보 필터 (54) 의 제 2 차단 주파수이다. 본 실시형태에 있어서, 제 1 차단 주파수 (fch) 는 제 2 차단 주파수 (fcl) 보다 높아져 있다. 즉, 제 2 차단 주파수 (fcl) 는 제 1 차단 주파수 (fch) 보다 낮아져 있다.

제 1 상보 필터 (53) 의 제 1 차단 주파수 (fch) 는, 각속도 (ω) 의 적분 오차, 즉 오차 (θwn) 를 충분히 저감시킬 수 있는 주파수로 설정된다. 제 2 상보 필터 (54) 의 제 2 차단 주파수 (fcl) 는, 중력 가속도 이외의 가속도에 의한 오차 (θan) 를 충분히 저감시킬 수 있는 주파수로 설정된다.

제 1 상보 필터 (53) 는, 제 1 HPFa 에 의해 각속도 (ω) 의 적분에 의한 오차 (θwn) 를 효과적으로 저감시킬 수 있지만, 중력 가속도 이외의 가속도에서 기인되는 오차 (θan) 를 효과적으로 저감시키는 것은 어렵다. 이 때문에, 제 1 상보 필터 (53) 는, 유압 셔블 (100) 이 정지하고 있는 상태 또는 정지하고 있는 상태에 가까운 상태, 즉 정지하고 있다고 보여지는 상태 (적절히, 준정지 상태라고 한다) 인 경우에 있어서는 자세각 (θ) 을 양호한 정밀도로 구할 수 있지만, 유압 셔블 (100) 이 준정지 상태가 아닌 동적 상태인 경우에 있어서는 자세각 (θ) 의 정밀도가 저하된다. 본 실시형태에 있어서, 동적 상태는 유압 셔블 (100) 이 움직이고 있다고 보여지는 상태이다.

제 2 상보 필터 (54) 는, 제 2 LPFa 에 의해 중력 가속도 이외의 가속도 에 의한 오차 (θan) 를 효과적으로 저감시킬 수 있지만, 각속도 (ω) 의 적분에 의한 오차 (θwn) 를 효과적으로 저감시키는 것은 어렵다. 이 때문에, 제 2 상보 필터 (54) 는, 유압 셔블 (100) 이 동적 상태인 경우에 있어서는 자세각 (θ) 을 양호한 정밀도로 구할 수 있지만, 유압 셔블 (100) 이 준정지 상태인 경우에 있어서는, 제 1 상보 필터 (53) 가 산출하는 자세각 (θ) 과 비교하여 자세각 (θ) 의 정밀도가 저하된다. 즉, 제 2 상보 필터 (54) 는, 단시간의 동특성은 우수하지만, 준정지 상태에 있어서는 동적 상태와 동일하게, 각속도 (ω) 의 적분에 의한 오차 (θwn) 가 존재한다.

도 4 에 나타내는 센서 제어 장치 (24) 가 구비하는 전환부 (55) 는, 유압 셔블 (100) 의 동작 상태가 준정지 상태인지 동적 상태인지에 따라, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환하여 출력한다. 예를 들어, 전환부 (55) 는, 유압 셔블 (100) 이 준정지 상태인 경우에는, 유압 셔블 (100) 의 자세각 (θo) 으로서, 제 1 상보 필터 (53) 가 출력하는 제 1 자세각 (θ1) 을 차내 신호선 (41) 에 출력한다. 유압 셔블 (100) 이 동적 상태인 경우, 전환부 (55) 는 유압 셔블 (100) 의 자세각 (θo) 으로서, 제 2 상보 필터 (54) 가 출력하는 제 2 자세각 (θ2) 을 차내 신호선 (41) 에 출력한다.

이와 같이, 센서 제어 장치 (24) 는, 유압 셔블 (100) 이 준정지 상태인 경우에는 제 1 상보 필터 (53) 의 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로 하므로, 준정지 상태에 있어서 자세각 (θo) 의 정밀도 저하를 억제할 수 있다. 유압 셔블 (100) 이 동적 상태인 경우, 센서 제어 장치 (24) 는, 제 2 상보 필터 (54) 의 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로 하므로, 동적 상태에 있어서도 자세각 (θo) 의 정밀도 저하를 억제할 수 있다. 결과적으로, 센서 제어 장치 (24) 는, 유압 셔블 (100) 이 준정지 상태 및 동적 상태 중 어느 것에 있어서도, 유압 셔블 (100) 의 자세각 (θo) 의 정밀도 저하를 억제할 수 있다.

유압 셔블 (100) 이 움직이고 있을 때에는 제 2 상보 필터 (54) 가 출력하는 제 2 자세각 (θ2) 이 사용되고, 예를 들어 도 1 에 나타내는 버킷 (8) 의 날끝 (8T) 의 위치가 구해진다. 또, 유압 셔블 (100) 이 정지하고 있을 때에는 제 1 상보 필터 (53) 가 출력하는 제 1 자세각 (θ1) 에 의해, 버킷 (8) 의 날끝 (8T) 의 위치가 구해진다. 이 때문에, 도 2 에 나타내는 제 2 표시 장치 (39) 가, 버킷 (8) 의 날끝 (8T) 의 위치로 대표되는 작업기 (2) 의 위치 또는 유압 셔블 (100) 의 차량 본체 (1) 의 위치 등을 구할 때의 정밀도 저하가 억제된다.

전환부 (55) 의 처리부 (55C) 는, 준정지 상태와 동적 상태를, 예를 들어, 다음 조건 A 와 조건 B 를 사용하여 판정하고, 그 판정 결과에 기초하여 전환기 (55S) 를 제어한다.

조건 A : 전환을 판정하는 시점보다 전의 소정 기간에 있어서, 제 1 자세각 (θ1) 의 표준 편차가 미리 설정된 임계값보다 작다.

조건 B : 중력 가속도 이외의 가속도의 크기가 미리 설정된 임계값보다 작다.

제 1 자세각 (θ1) 은 IMU (29) 가 검출한 각속도 (ω) 또는 가속도 (Ac) 로부터 구해지고, 중력 가속도를 포함하는 가속도는 IMU (29) 에 의해 검출된다. 즉, 처리부 (55C) 는, 유압 셔블 (100) 이 구비하는 IMU (29) 의 상태에 기초하여, 준정지 상태와 동적 상태를 판정한다.

전술한 조건 B 에 대해 설명한다. IMU (29) 는, 전술한 바와 같이, 적어도 중력 가속도를 포함하는 가속도를 검출하고, 검출된 각각의 가속도의 종류를 구별하지 않고 검출한 가속도를 출력한다. 중력 가속도는 이미 알려져 있다. 그래서, 처리부 (55C) 는, IMU (29) 가 출력한 가속도로부터, X 축 방향 또는 Y 축 방향의 가속도를 연산한다. 처리부 (55C) 는, 구해진 X 축 방향의 가속도로부터 중력 가속도의 X 축 방향 상당의 중력 가속도를 감산하면, 중력 가속도 이외의 가속도의 크기를 구할 수 있다. 처리부 (55C) 는, 중력 가속도 이외의 가속도의 크기와 미리 설정된 임계값을 비교한다. 또한, 처리부 (55C) 는, 구해진 Y 축 방향의 가속도로부터 중력 가속도의 Y 축 방향 상당의 중력 가속도를 감산하여, 중력 가속도 이외의 가속도의 크기를 구하고, 미리 설정된 임계값과 비교하여 조건 B 가 성립하는지 아닌지를 판정해도 된다.

처리부 (55C) 는, IMU (29) 로부터 취득한 가속도 (Ac) 및 제 1 상보 필터 (53) 의 출력인 제 1 자세각 (θ1) 을, 도 4 에 나타내는 가속도 송신선 (L1) 또는 제 1 자세각 송신선 (L2) 을 통하여 취득하고, 조건 A 및 조건 B 가 동시에 성립하는지 아닌지를 판정한다. 조건 A 와 조건 B 의 양방이 성립한 경우에는, 준정지 상태라고 볼 수 있다. 본 실시형태에 있어서, 준정지 상태란, 유압 셔블 (100) 이, 주행, 상부 선회체 (3) 의 선회 및 작업기 (3) 의 동작 중 어느 것도 실시하지 않고 완전하게 정지되어 있는 상태 또는 주행과 유압 셔블 (100) 의 상부 선회체 (3) 의 선회가 실시되지 않고 작업기 (3) 만이 동작하고 있는 상태이다. 이 경우, 처리부 (55C) 는, 전환기 (55S) 가 제 1 상보 필터 (53) 의 가산부 (53AD) 와 접속하도록 전환기 (55S) 를 동작시킨다. 전환기 (55S) 는, 유압 셔블 (100) 의 자세각 (θo) 으로서, 제 1 상보 필터 (53) 가 출력하는 제 1 자세각 (θ1) 을 차내 신호선 (41) 에 출력한다.

조건 A 와 조건 B 가 성립하지 않는 경우, 즉, 조건 A 및 조건 B 중 적어도 일방이 성립하지 않는 경우에는, 동적 상태, 즉 유압 셔블 (100) 이 움직이고 있다고 볼 수 있다. 이 경우, 처리부 (55C) 는, 전환기 (55S) 가 제 2 상보 필터 (54) 의 가산부 (54AD) 와 접속하도록 전환기 (55S) 를 동작시킨다. 전환기 (55S) 는 유압 셔블 (100) 의 자세각 (θo) 으로서, 제 2 상보 필터 (54) 가 출력하는 제 2 자세각 (θ2) 을 차내 신호선 (41) 에 출력한다. 조건 A 및 조건 B 를 사용하여 전환부 (55) 가 제 1 자세각 (θ1) 과 제 2 자세각 (θ2) 을 전환하도록 하면, IMU (29) 의 검출값만으로 전술한 전환이 실현될 수 있다.

본 실시형태에 있어서, 조건 A 의 소정 기간은, 예를 들어 1 초로 설정된 것이지만, 이것에 한정되는 것은 아니다. 조건 A 의 표준 편차와 비교하는 임계값은 한정되는 것은 아니지만, 예를 들어, 0.1 도로 할 수 있다. 조건 B 는, 중력 가속도 이외의 가속도가 미리 설정된 임계값보다 작은 경우에 성립하고, 미리 설정된 임계값 이상의 중력 가속도 이외의 가속도가 검출된 경우에는 성립하지 않는다. 조건 B 의 임계값은 한정되는 것은 아니지만, 예를 들어, 중력 가속도의 0.1 배 이상의 범위에서 적절히 설정할 수 있다.

(준정지 상태인지 동적 상태인지를 판정하는 변형예)

본 실시형태에 있어서, 도 4 에 나타내는 전환부 (55) 의 처리부 (55C) 는, 도 2 에 나타내는 IMU (29) 의 검출값에 기초하여, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환하고, 유압 셔블 (100) 의 자세각 (θo) 으로서 출력하였다. 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 의 선택은, 이와 같은 것에 한정되지 않고, 처리부 (55C) 는, 예를 들어, 유압 셔블 (100) 의 동작에 관한 정보 (이하, 적절히 동작 정보라고 한다) 를 사용하여 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환해도 된다.

본 실시형태에 있어서, 동작 정보는, 유압 셔블 (100) 에 어떠한 움직임이 발생한 것에 관한 정보이다. 예를 들어, 동작 정보는, 도 1 에 나타내는 상부 선회체 (3) 의 선회 여부에 대한 정보, 주행 장치 (5) 의 동작 여부에 대한 정보 또는 작업기 (2) 의 동작 여부에 대한 정보 등이 있다. 동작 정보는, 예를 들어, 상부 선회체 (3) 가 선회하고 있는 것을 검출하는 센서로부터 출력되는 검출값, 상부 선회체 (3) 를 선회시키기 위한 선회 모터에 리졸버 등의 선회 각도 센서를 형성하고, 그와 같은 각도 검출기 혹은 회전 센서로부터 출력되는 검출값, 또는 도 2 에 나타내는 작업기 조작 검출부 (32L, 32R) 에 의해 생성되는 파일럿압을 검지하는 유압 센서로부터 출력되는 검출값 등이 사용된다. 즉, 동작 정보는, 예를 들어, 상부 선회체 (3) 또는 작업기 (2) 등이 실제로 동작하고 있는지 여부의 정보여도 되고, 상부 선회체 (3) 또는 작업기 (2) 등을 동작시키기 위한 조작 부재에 대한 조작의 정보여도 된다.

도 8 은, 본 실시형태의 변형예에 있어서의 제 1 자세각 (θ1) 과 제 2 자세각 (θ2) 의 전환에 사용하는 테이블 (TB) 의 일례를 나타내는 도면이다. 본 변형예에 있어서, 전환부 (55) 의 처리부 (55C) 는, IMU (29) 의 검출값에 기초하는 준정지 상태인지 동적 상태인지에 대한 판정과, 상부 선회체 (3) 가 선회 중인지 아닌지에 대한 판정에 기초하여, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환한다. 테이블 (TB) 은, 상부 선회체 (3) 의 상태와, IMU (29) 의 검출값에 기초하는 조건 A 및 조건 B 에 대하여, 유압 셔블 (100) 의 자세각 (θo) 으로서, 출력해야 하는 자세각이 기술되어 있다. 상부 선회체 (3) 의 상태는 ON 또는 OFF 로 나타내고, ON 일 때가 선회 중이며, OFF 일 때가 정지 중이다. 조건 A 및 조건 B 는, A ＆ B 또는 NOT (A ＆ B) 로 나타내고, A ＆ B 가 준정지 상태이며, NOT (A ＆ B) 은 동적 상태이다.

IMU (29) 의 검출값에 기초하는 판정 결과가 준정지 상태, 또한 동작 정보로부터 상부 선회체 (3) 가 선회 중 (ON) 이라고 한다. 이 경우, 전환부 (55) 는, 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력한다. 상부 선회체 (3) 가 실제로 움직이고 있기 때문에, 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 사용함으로써, 자세각 (θo) 의 정밀도를 확보할 수 있다.

IMU (29) 의 검출값에 기초하는 판정 결과가 준정지 상태, 또한 동작 정보로부터 상부 선회체 (3) 가 정지 중 (OFF) 이라고 한다. 이 경우, 전환부 (55) 는, 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력한다. 준정지 상태이고, 또한 상부 선회체 (3) 가 실제로는 정지하고 있기 때문에, 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 사용함으로써, 각속도 (ω) 의 적분에 의한 오차를 저감시킬 수 있다.

IMU (29) 의 검출값에 기초하는 판정 결과가 동적 상태, 또한 동작 정보로부터 상부 선회체 (3) 가 선회 중 (ON) 이라고 한다. 이 경우, 전환부 (55) 는, 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력한다. 동적 상태이고, 또한 상부 선회체 (3) 가 실제로 움직이고 있기 때문에, 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 사용함으로써, 자세각 (θo) 의 정밀도를 확보할 수 있다.

IMU (29) 의 검출값에 기초하는 판정 결과가 동적 상태, 또한 동작 정보로부터 상부 선회체 (3) 가 정지 중 (OFF) 이라고 한다. 이 경우, 전환부 (55) 는, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 중 어느 것을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력해도 되지만, 본 변형예에서는 제 2 자세각 (θ2) 을 출력한다.

본 변형예에서는, 전환부 (55) 는, IMU (29) 의 검출값에 기초하는 준정지 상태인지 동적 상태인지의 판정과, 상부 선회체 (3) 가 선회 중인지 아닌지에 대한 판정에 기초하여, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환하였다. 이와 같이 함으로써, 전환부 (55) 는, 유압 셔블 (100) 상태를 보다 양호한 정밀도로 판정하여, 적절한 자세각을 선택할 수 있다. 본 변형예에 있어서, 전술한 처리에 한정되지 않고, 전환부 (55) 는, IMU (29) 의 검출값에 기초하는 판정 결과를 사용하지 않고 상부 선회체 (3) 가 선회 중인지 아닌지에 대한 판정에 기초하여, 제 1 자세각 (θ1) 또는 제 2 자세각 (θ2) 을 전환해도 된다. 예를 들어, 상부 선회체 (3) 가 선회 중일 때에는 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로 하고, 상부 선회체 (3) 가 정지 중일 때에는 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로 해도 된다.

도 9 는, 센서 제어 장치 (24) 의 전환부 (55) 가 출력하는 유압 셔블 (100) 의 자세각 (θo) 과, 제 1 자세각 (θ1) 과, 제 2 자세각 (θ2) 의 시간 변화의 일례를 나타내는 도면이다. 도 9 에 나타내는 굵은 실선은, 후술하는 완화항 (dif) 의 시간 변화의 일례를 나타내고 있다. 도 9 의 세로축은 자세각 (θ) 이고, 가로축은 시간 (t) 이다. 도 9 의 Sst 로 나타내는 구간은 준정지 상태이고, 제 1 자세각 (θ1) 이 유압 셔블 (100) 의 자세각 (θo) 으로서 출력된다. 도 9 의 Sdm 으로 나타내는 구간은 동적 상태이고, 제 2 자세각 (θ2) 이 유압 셔블 (100) 의 자세각 (θo) 으로서 출력된다. 도 9 에 나타내는 예에 있어서, 시간 (t1) 내지 시간 (t2) 및 시간 (t3) 이후에는 준정지 상태 (Sst) 이고, 시간 (t2) 내지 시간 (t3) 은 동적 상태 (Sdm) 이다.

유압 셔블 (100) 의 자세각 (θo) 은, 시간 (t2) 에서 제 1 자세각 (θ1) 으로부터 제 2 자세각 (θ2) 으로 전환되고, 시간 (t3) 에서 제 2 자세각 (θ2) 으로부터 제 1 자세각 (θ1) 으로 전환되고 있다. 제 2 자세각 (θ2) 은, 각속도 (ω) 를 적분하는 것에 의한 오차 (θwn) 가 축적되므로, 시간 (t2) 에 있어서, 제 1 자세각 (θ1) 과 제 2 자세각 (θ2) 은 상이한 값이 된다. 동일하게, 시간 (t3) 에 있어서, 제 2 자세각 (θ2) 과 제 1 자세각 (θ1) 은 상이한 값이 된다.

센서 제어 장치 (24) 로부터 출력되는 자세각 (θo) 을 전환부 (55) 가 제 1 자세각 (θ1) 으로부터 제 2 자세각 (θ2) 으로 전환할 때 또는 제 2 자세각 (θ2) 으로부터 제 1 자세각 (θ1) 으로 전환할 때에는, 그대로 전환하면, 전환시에 있어서 유압 셔블 (100) 의 자세각 (θo) 이 불연속이 될 가능성이 있다. 또, 전술한 바와 같이, 제 2 자세각 (θ2) 은, 각속도 (ω) 를 적분하는 것에 의한 오차 (θwn) 가 축적되므로, 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 사용하는 경우에는, 적분에 의한 오차 (θwn) 를 저감시킬 필요가 있다.

자세각 (θo) 의 전환시에 발생하는 자세각 (θo) 의 불연속 및 적분에 의한 오차 (θwn) 를 저감시키기 위해, 본 실시형태에 있어서, 전환부 (55) 의 처리부 (55C) 는, 식 (5) 내지 식 (10) 을 사용하여 자세각 (θo) 을 구하여 출력한다.

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

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

dif ＝ Ftr × dif_prev··· (7)

dif ＝ dif_prev··· (8)

dif ＝ dif_prev ＋ θ1 － θ2··· (9)

dif ＝ dif_prev ＋ θ2 － θ1··· (10)

식 (5) 는 준정지 상태에서 자세각 (θo) 을 구하는 경우에 사용되고, 식 (6) 은 동적 상태에서 자세각 (θo) 을 구하는 경우에 사용된다. 식 (5) 및 식 (6) 의 dif 는 완화항이다. 식 (7) 의 완화항 (dif) 은 준정지 상태에서 사용되고, 식 (8) 의 완화항 (dif) 은 동적 상태에서 사용된다. 식 (7) 의 Ftr 은 완화 계수이다. 완화 계수 (Ftr) 는 0 보다 크고 1 보다 작다 (0 ＜ Ftr ＜ 1). 식 (9) 의 완화항 (dif) 은 준정지 상태에서 동적 상태로 이행하는 타이밍에 사용된다. 식 (10) 의 완화항 (dif) 은, 동적 상태에서 준정지 상태로 이행하는 타이밍에 사용된다. 식 (8) 내지 식 (10) 의 dif_prev 는, 직전의 IMU (29) 의 상태 (준정지 상태 (Sst) 또는 동적 상태 (Sdm)) 에서의 완화항 (dif) 이다. dif_prev 의 초기값은 0 이다.

도 9 에 나타내는 바와 같이, 제 1 자세각 (θ1) 은, 준정지 상태 (Sst) 에서는 고정밀도를 유지하고 있지만, 동적 상태 (Sdm) 에서는 큰 오차가 발생하였다. 제 2 자세각 (θ2) 은 준정지 상태 (Sst) 및 동적 상태 (Sdm) 중 어느 것에 있어서도 적분의 누적에 의한 오차가 발생하였다. dif_prev 의 초기값은 0 이므로, 시간 (t1) 내지 시간 (t2) 의 준정지 상태 (Sst) 에서는 완화항 (dif) ＝ 0 이 된다. 그 결과, 식 (5) 로부터, 준정지 상태 (Sst) 에 있어서의 유압 셔블 (100) 의 자세각 (θo) 은 제 1 자세각 (θ1) 이 된다.

준정지 상태 (Sst) 에서 동적 상태 (Sdm) 로 전환될 때, 즉 시간 (t) ＝ t2 일 때, 처리부 (55C) 는 식 (9) 를 사용하여 완화항 (dif) 을 구한다. 전술한 바와 같이, 시간 (t) ＝ t2 일 때의 완화항 (dif) 은 0 이므로, 완화항 (dif) 은, 제 1 자세각 (θ1) 과 제 2 자세각 (θ2) 의 차인, θ1 － θ2 의 값이 된다. 이 경우의 완화항 (dif) 은, 도 9 에 나타내는 바와 같이 부 (負) 의 값이다. 시간 (t2) 일 때, 식 (5) 에 의한 자세각 (θo) 은 θ1 이고, 식 (6) 의 완화항 (dif) 에 입력되는 것이 θ1 － θ2 의 값이기 때문에, 식 (6) 에 의한 자세각 (θo) 도 θ1 이 된다. 이 때문에, 준정지 상태 (Sst) 에서 동적 상태 (Sdm) 로 전활될 때에는, 유압 셔블 (100) 의 자세각 (θo) 은 연속하여 변화한다.

시간 (t2) 내지 시간 (t3) 의 동적 상태 (Sdm) 에서는, 완화항 (dif) 의 값은 전환시에, 즉 시간 (t2) 에서 얻어진 θ1 － θ2 의 값이 그대로 유지된다. 동적 상태 (Sdm) 에 있어서의 유압 셔블 (100) 의 자세각 (θo) 은, 식 (6) 으로부터, 동적 상태 (Sdm) 에 있어서의 제 2 자세각 (θ2) 으로부터 시간 (t2) 에서 얻어져 유지되고 있는 완화항 (dif) ＝ θ1 － θ2 의 값을 가산하여 구해진다. 이 때 사용되는 완화항 (dif) 은, 식 (8) 에서 dif_prev 가 되므로, 동적 상태 (Sdm) 에서 사용되는 완화항 (dif) 은, 시간 (t2) 에서 얻어져 유지되고 있는 완화항 (dif) ＝ θ1 － θ2 의 값이 사용된다. 이와 같이, 전환부 (55) 의 처리부 (55C) 는, 제 1 자세각 (θ1) 을 제 2 자세각 (θ2) 으로 전환한 후에는, 전환시에서의 제 1 자세각 (θ1) 에서 제 2 자세각 (θ2) 을 감산한 값, 즉 전환시에 있어서의 완화항 (dif) 을 보정값으로서 사용하고, 구해진 제 2 자세각 (θ2) 을 보정하여, 자세각 (θo) 을 취득한다. 이와 같이 함으로써, 동적 상태 (Sdm) 로의 전환 이전에 발생한 제 2 자세각 (θ2) 의 적분의 축적에 의한 오차 (θwn) 가, 유압 셔블 (100) 의 자세각 (θo) 에 미치는 영향을 저감시킬 수 있다.

동적 상태 (Sdm) 에서 다시 준정지 상태 (Sst) 로 전환될 때, 즉 시간 (t3) 에 있어서, 처리부 (55C) 는 식 (10) 을 사용하여 완화항 (dif) 을 구한다. 식 (10) 의 dif_prev 는, 이미 얻어져 유지되고 있는 완화항 (dif) 이다. 요컨대, 식 (10) 의 dif_prev 는, 시간 (t2) 에서의 완화항 (dif), 즉 시간 (t2) 에서의 θ1 － θ2 의 값이다. 식 (10) 으로부터, 시간 (t3) 에 있어서의 완화항 (dif) 은, 시간 (t2) 에서 얻어져 유지되고 있는 θ1 － θ2 의 값과, 시간 (t3) 에서 얻어진 θ2 － θ1 의 값을 가산한 값이 된다. 식 (10) 을 사용함으로써, 동적 상태 (Sdm) 에서 준정지 상태 (Sst) 로 전환될 때에는, 유압 셔블 (100) 의 자세각 (θo) 은 연속하여 변화한다.

시간 (t3) 이후의 준정지 상태 (Sst) 에 있어서, 처리부 (55C) 는, 식 (5) 를 사용하여 유압 셔블 (100) 의 자세각 (θo) 을 구한다. 이 때의 완화항 (dif) 은, 식 (7) 에 의해 결정된다. 식 (7) 중의 dif_prev 는, 동적 상태 (Sdm) 에서 준정지 상태 (Sst) 로 다시 전환되는 타이밍, 즉 시간 (t3) 에 있어서의 완화항 (dif) 이다. 시간 (t3) 이후의 준정지 상태 (Sst) 에서는, 완화 계수 (Ftr) 의 효과로 완화항 (dif) 의 값은 서서히 작아져, 0 에 수속된다. 즉, 시간 (t3) 이후의 준정지 상태 (Sst) 에 있어서, 유압 셔블 (100) 의 자세각 (θo) 은 제 1 자세각 (θ1) 에 수속된다. 이와 같이, 전환부 (55) 의 처리부 (55C) 는, 제 2 자세각 (θ2) 을 제 1 자세각 (θ1) 으로 전환한 후에는, 전환시에서의 제 2 자세각의 오차, 즉 전환시에서의 완화항 (dif) 에, 0 보다 크고 1 보다 작은 계수로서의 완화 계수 (Ftr) 를 곱한 값을 보정값으로서 사용하여, 제 1 자세각 (θ1) 을 보정한다. 이와 같이 함으로써, 동적 상태 (Sdm) 에서 준정지 상태 (Sst) 로 전환된 후에는, 유압 셔블 (100) 의 자세각 (θo) 은 연속하여 변화한다.

(자세각 (θo) 을 구하는 처리의 일례)

도 10 은, 자세각 (θo) 을 구하는 처리의 일례를 나타내는 플로우 차트이다. 유압 셔블 (100) 의 자세각 (θo) 을 구할 때에, 도 2 및 도 4 에 나타내는 센서 제어 장치 (24) 는, 스텝 S101 에 있어서, 차내 통신선 (42) 을 통하여 IMU (29) 에 의한 각속도 (ω) 및 가속도 (Ac) 의 검출값을 취득한다. 스텝 S102 에 있어서, 도 2 에 나타내는 제 1 자세각 연산부 (51) 는, IMU (29) 가 검출한 가속도 (Ac) 로부터 자세각 (θ) 을 구한다. 스텝 S103 에 있어서, 도 2 에 나타내는 제 2 자세각 연산부 (52) 는, IMU (29) 가 검출한 각속도 (ω) 로부터 자세각 (θ) 을 구한다. 스텝 S102 와 스텝 S103 의 순서는 상관없다.

스텝 S104 에 있어서, 도 4 에 나타내는 제 1 상보 필터 (53) 의 제 1 LPFa 는, 가속도 (Ac) 로부터 얻어진 자세각 (θ) 에 필터 처리를 실시한다. 스텝 S105 에 있어서, 도 4 에 나타내는 제 2 상보 필터 (54) 의 제 2 LPFb 는, 가속도 (Ac) 로부터 얻어진 자세각 (θ) 에 필터 처리를 실시한다. 스텝 S106 에 있어서, 도 4 에 나타내는 제 1 상보 필터 (53) 의 제 1 HPFa 는, 각속도 (ω) 로부터 얻어진 자세각 (θ) 에 필터 처리를 실시한다. 스텝 S107 에 있어서, 도 4 에 나타내는 제 2 상보 필터 (54) 의 제 2 HPFb 는, 각속도 (ω) 로부터 얻어진 자세각 (θ) 에 필터 처리를 실시한다. 스텝 S104 와 스텝 S105 와 스텝 S106 과 스텝 S107 의 순서는 상관없다.

다음으로, 스텝 S108 로 진행되어, 제 1 상보 필터 (53) 는, 제 1 자세각 (θ1) 을 구한다. 구체적으로는, 가산부 (53AD) 가, 제 1 LPFa 의 출력과 제 1 HPFa 의 출력을 가산함으로써, 제 1 자세각 (θ1) 이 구해진다. 스텝 S109 에 있어서, 제 2 상보 필터 (54) 는 제 2 자세각 (θ2) 을 구한다. 구체적으로는, 가산부 (54AD) 가, 제 2 LPFb 의 출력과 제 2 HPFb 의 출력을 가산함으로써, 제 2 자세각 (θ2) 이 구해진다. 스텝 S108 과 스텝 S109 의 순서는 상관없다.

스텝 S110 으로 진행되어, 도 4 에 나타내는 전환부 (55) 의 처리부 (55C) 는, 유압 셔블 (100) 이 준정지 상태인 경우 (스텝 S110, 예), 스텝 S111 로 처리를 진행시킨다. 스텝 S111 에 있어서, 처리부 (55C) 는, 센서 제어 장치 (24) 가 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력하도록, 전환기 (55S) 를 제어한다. 유압 셔블 (100) 이 동적 상태인 경우 (스텝 S110, 아니오), 스텝 S112 에 있어서, 처리부 (55C) 는, 센서 제어 장치 (24) 가 제 2 자세각 (θ2) 을 유압 셔블 (100) 의 자세각 (θo) 으로서 출력하도록, 전환기 (55S) 를 제어한다.

(평가예)

도 11 은, 유압 셔블 (100) 의 동작 상태에 따라 출력되는 자세각 (θo) 을 전환한 결과의 일례를 나타내는 도면이다. 도 11 의 세로축은, 자세각 (θo) 중 하나인 피치각 (θp) 이고, 가로축은 시간 (t) 이다. 도 11 중의 실선이 유압 셔블 (100) 의 피치각 (θp) 이고, 1 점 쇄선이 제 1 자세각 (θ1) 이며, 2 점 쇄선이 제 2 자세각 (θ2) 이다. 도 11 에 나타내는 예는, 도 1 에 나타내는 유압 셔블 (100) 의 붐 (6) 을 들어 올린 상태로부터 단번에 하방을 향하여 내려서 정지시켰을 때의, 유압 셔블 (100) 의 피치각 (θp) 의 시간 변화를 나타내고 있다. 붐 (6) 을 내려서 멈추었을 때에는, 버킷 (8) 은 접지되어 있지 않다.

시간 (t1) 에서 붐 (6) 이 정지되고, 유압 셔블 (100) 의 Y 축 둘레에 있어서의 운동 (피칭), 즉 흔들림이 발생하고 있다. 유압 셔블 (100) 에 피칭이 발생하고 있지 않은 경우 (시간 (t1) 이전) 에는 제 1 자세각 (θ1) 이 자세각 (θo) 으로서 출력되고, 유압 셔블 (100) 에 피칭이 발생하고 있는 기간 (시간 (t1) 이후의 소정 기간) 에는 제 2 자세각 (θ2) 이 자세각 (θo) 으로서 출력되고 있는 것을 알 수 있다.

시간 (t2 에서 t3 까지) 은, 유압 셔블 (100) 에 스윙 오버가 발생하고 있다. 피칭이 끝났기 때문에 준정지 상태라고 판정된 후 (시간 (t2) 이후) 에는, 자세각 (θo) 이 서서히 제 1 자세각 (θ1) 에 가까워지고, 시간 (t3) 에서 자세각 (θo) 이 제 1 자세각 (θ1) 에 거의 일치하는 것을 알 수 있다. 준정지 상태라고 판정된 후에는, 적분에 의한 오차의 축적에 의해, 제 2 자세각 (θ2) 이 제 1 자세각 (θ1) 과는 상이한 값이 된다. 본 실시형태에 있어서는, 피칭이 끝난 후에는, 제 1 자세각 (θ1) 을 유압 셔블 (100) 의 자세각 (θo) 으로 하므로, 적분 오차의 영향을 억제할 수 있다.

본 실시형태는, 제 1 차단 주파수가 설정된 제 1 상보 필터 (53) 와, 제 1 차단 주파수와는 상이한 제 2 차단 주파수가 설정된 제 2 상보 필터 (54) 를 사용한다. 제 1 상보 필터 (53) 는, 각속도 (ω) 를 적분함으로써 누적되는 오차 (잡음) 를 저감시키고, 제 2 상보 필터 (54) 는, 중력 가속도 이외의 가속도에 의한 가속도에 의한 오차 (잡음) 를 저감시킨다. 본 실시형태에서는, 유압 셔블 (100) 의 동작 상태에 따라, 제 1 상보 필터 (53) 가 출력하는 자세각과, 제 2 상보 필터 (54) 가 출력하는 자세각이 전환된다. 그 결과, 유압 셔블 (100) 의 동작 상태에 따른 적절한 상보 필터에 의해 유압 셔블 (100) 의 자세각 (θo) 이 구해지므로, 동적 상태 및 준정지 상태 중 어느 것에 있어서도 자세각 (θo) 의 정밀도 저하가 억제된다.

정밀도가 높은 IMU (29) 는 고가이고, 염가의 IMU (29) 는 정밀도가 상대적으로 낮다. 본 실시형태는, 정밀도가 낮은 IMU (29) 를 사용한 경우에도, 동적 상태 및 준정지 상태 중 어느 것에 있어서도 자세각 (θo) 의 정밀도 저하를 억제할 수 있다. 이 때문에, 자세각 (θo) 의 정밀도 저하를 억제하면서, 유압 셔블 (100) 의 제조 비용을 저감시킬 수 있다.

이상, 본 실시형태 및 그 변형예를 설명했지만, 전술한 내용에 의해 본 실시형태 및 그 변형예가 한정되는 것은 아니다. 또, 전술한 구성 요소에는, 당업자가 용이하게 상정할 수 있는 것, 실질적으로 동일한 것, 이른바 균등한 범위의 것이 포함된다. 또한, 전술한 구성 요소는 적절히 조합하는 것이 가능하다. 또한, 본 실시형태 및 그 변형예의 요지를 일탈하지 않는 범위에서 구성 요소의 여러 가지의 생략, 치환 및 변경 중 적어도 1 개를 실시할 수 있다. 예를 들어, 작업기 (2) 는, 붐 (6), 아암 (7), 버킷 (8) 을 가지고 있지만, 작업기 (2) 에 장착되는 어태치먼트는 이것에 한정되지 않고, 버킷 (8) 에는 한정되지 않는다. 작업 기계는 유압 셔블 (100) 에 한정되지 않는다. 예를 들어, 본 실시형태에 관련된 자세 연산 장치를 불도저, 모터 그레이더 또는 덤프 트럭 등에 적용하여, 작업기 제어 또는 주행 제어 등을 실행할 수 있다.

본 실시형태에서는, 제 1 상보 필터 (53) 및 제 2 상보 필터 (54) 를 사용하였지만, 추가로 제 1 차단 주파수 및 제 2 차단 주파수와는 상이한 제 3 차단 주파수가 설정된 제 3 상보 필터가 추가되거나, 제 1 차단 주파수, 제 2 차단 주파수 및 제 3 차단 주파수와는 상이한 제 4 차단 주파수가 설정된 제 4 상보 필터가 추가되거나 해도 된다. 즉, 차단 주파수가 상이한 상보 필터의 수는 2 개로 한정되지 않는다.

1 : 차량 본체
2 : 작업기
3 : 상부 선회체
4 : 운전실
5 : 주행 장치
6 : 붐
7 : 아암
8 : 버킷
8T : 날끝
20, 21 : 안테나
23 : 위치 검출 장치
24 : 센서 제어 장치
25 : 작업기 제어 장치
26 : 엔진 제어 장치
27 : 펌프 제어 장치
28 : 제 1 표시 장치
28M : 표시부
29 : IMU
29A : 가속도 센서
29V : 자이로
30 : 조작 장치
35 : 센서류
36 : 엔진
37 : 유압 펌프
38 : 유압 제어 밸브
39 : 제 2 표시 장치
41, 42 : 차내 신호선
51 : 제 1 자세각 연산부
52 : 제 2 자세각 연산부
53 : 제 1 상보 필터
53F : 필터부
53AD : 가산부
54F : 필터부
54AD : 가산부
54 : 제 2 상보 필터
55 : 전환부
55C : 처리부
55S : 전환기
100 : 유압 셔블
TB : 테이블

Claims

작업 기계에 구비되어, 각속도 및 가속도를 검출하는 계측 장치와,
상기 계측 장치에 의해 검출된 각속도 및 가속도로부터 상기 작업 기계의 자세각을 구하는 자세각 연산부와,
제 1 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 1 자세각을 출력하는 제 1 상보 필터와,
상기 제 1 차단 주파수와는 상이한 제 2 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 2 자세각을 출력하는 제 2 상보 필터와,
상기 작업 기계의 상태에 따라, 상기 제 1 자세각 또는 상기 제 2 자세각을 전환하여 출력하는 전환부를 포함하는, 작업 기계의 자세 연산 장치.
제 1 항에 있어서,
상기 제 1 차단 주파수는, 상기 제 2 차단 주파수보다 높은, 작업 기계의 자세 연산 장치.
제 2 항에 있어서,
상기 전환부는,
상기 작업 기계가 준정지 상태라고 판정한 경우에는 상기 제 1 자세각을 출력하고,
상기 작업 기계가 동적 상태라고 판정한 경우에는 상기 제 2 자세각을 출력하는, 작업 기계의 자세 연산 장치.
제 1 항 내지 제 3 항 중 어느 한 항에 있어서,
상기 전환부는,
상기 제 1 자세각을 상기 제 2 자세각으로 전환한 후에는, 전환시에서의 상기 제 1 자세각으로부터 상기 제 2 자세각을 감산한 값을 보정값으로서 사용하여, 상기 제 2 자세각을 보정하는, 작업 기계의 자세 연산 장치.
제 4 항에 있어서,
상기 전환부는,
상기 제 2 자세각을 상기 제 1 자세각으로 전환한 후에는, 전환시에서의 상기 제 2 자세각의 오차에 0 보다 크고 1 보다 작은 계수를 곱한 값을 보정값으로서 사용하여, 상기 제 1 자세각을 보정하는, 작업 기계의 자세 연산 장치.
유압 셔블의 상부 선회체에 구비되어, 각속도 및 가속도를 검출하는 계측 장치와,
상기 계측 장치가 검출한 상기 각속도 및 상기 가속도로부터 상기 유압 셔블의 자세각을 구하는 자세각 연산부와,
제 1 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 1 자세각을 출력하는 제 1 상보 필터와,
상기 제 1 차단 주파수보다 낮은 제 2 차단 주파수가 설정되고, 상기 자세각 연산부에 의해 구해진 상기 자세각에 포함되는 잡음을 저감시켜 제 2 자세각을 출력하는 제 2 상보 필터와,
상기 유압 셔블이 준정지 상태라고 판정한 경우에는 상기 제 1 자세각을 출력하고, 상기 유압 셔블이 동적 상태라고 판정한 경우에는 상기 제 2 자세각을 출력하는 전환부를 포함하는, 유압 셔블의 자세 연산 장치.
제 1 항 내지 제 5 항 중 어느 한 항에 기재된 작업 기계의 자세 연산 장치와,
상기 작업 기계의 자세 연산 장치로부터 출력된 상기 제 1 자세각 또는 상기 제 2 자세각을 사용하여, 상기 작업 기계에 장착된 작업기의 위치를 구하는 제어부를 구비한, 작업 기계.