KR101859263B1 - 작업 기계의 제어 시스템 및 작업 기계의 제어 방법 - Google Patents

Abstract

작업 기계의 제어 시스템은, 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어, 상기 주행 장치에 대해 선회하는 선회체를 구비하는 작업 기계를 제어한다. 작업 기계의 제어 시스템은, 상기 작업 기계의 일부의 위치인 제 1 위치를 검출하고, 상기 제 1 위치의 정보로서 출력하는 위치 검출 장치와, 상기 작업 기계의 동작을 나타내는 동작 정보를 검출해 출력하는 상태 검출 장치와, 상기 제 1 위치의 정보 및 상기 동작 정보를 이용하여, 상기 일부의 위치에 상당하는 제 2 위치를 구하고, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는 처리 장치를 포함한다.

Description

작업 기계의 제어 시스템 및 작업 기계의 제어 방법{CONSTRUCTION MACHINE CONTROL SYSTEM AND CONSTRUCTION MACHINE CONTROL METHOD}

본 발명은, 작업기를 구비한 작업 기계에 사용되는 작업 기계의 제어 시스템 및 작업 기계에 관한 것이다.

GPS (Global Positioning System) 등을 이용하여 작업 기계의 3 차원 위치를 측위하고, 얻어진 작업 기계의 위치 정보를 이용하여 작업 기계를 관리하거나, 작업 기계에 의한 시공 상태를 관리하거나, 작업 기계를 제어하거나 하는 기술이 알려져 있다 (예를 들어, 특허문헌 1).

일본 공개특허공보 2007-147588호

작업 기계의 위치를 측위하는 기기를 구비하는 작업 기계로는, 그 기기가 검출한 작업 기계의 위치 정보를 이용하여, 작업 기계의 운전실 내에 설치된 표시 장치에 작업의 가이던스 화면을 표시시키거나, 작업기의 동작을 제어하거나 하는 것이 있다. 이와 같은 작업 기계에 의한 시공은, 정보화 시공으로 불린다.

정보화 시공에 의한 건설 공사가 실시되는 경우가 있다. 정보화 시공을 실시하기 위해서, GPS 안테나 등을 탑재한 유압 셔블에 의해, 법면 성형과 같은 시공이 실시되어, 공사 기간 단축 또는 숙련 오퍼레이터 부족의 문제를 해소하는 것이 기대되고 있다.

GPS 의 측위 위성의 위치, 전리층, 대류권 또는 GPS 안테나 주변의 지형의 영향에 의해, 측위 결과에 편차가 발생하는 경우가 있다. 정보화 시공에 있어서는, 측위 결과에 근거해 버킷의 날끝 위치를 구해, 작업기의 제어 및 가이던스 화면의 표시를 실시하지만, 측위 결과의 편차의 영향에 의해, 시공면이 물결치거나, 가이던스 화면에 표시되는 버킷의 날끝이 흔들리거나 할 가능성이 있다. 결과적으로, 시공면의 마무리가 매끄럽게 되지 않거나, 시공 중에 있어서의 가이던스 화면의 시인성이 저하되거나 할 가능성이 있다.

본 발명은, 작업 기계의 위치를 측위한 결과에 기초하여 정보화 시공을 실시하는 작업 기계에 있어서, 측위 결과의 편차가 정보화 시공에 주는 영향을 저감하는 것을 목적으로 한다.

본 발명은, 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어하는 시스템으로서, 상기 작업 기계의 일부의 위치인 제 1 위치를 검출하고, 상기 제 1 위치의 정보로서 출력하는 위치 검출 장치와, 상기 작업 기계의 동작을 나타내는 동작 정보를 검출하여 출력하는 상태 검출 장치와, 상기 제 1 위치의 정보 및 상기 동작 정보를 이용하여, 상기 일부의 위치에 상당하는 제 2 위치를 구하고, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는 처리 장치를 포함하는, 작업 기계의 제어 시스템이다.

상기 처리 장치는, 상기 제 1 위치 및 상기 동작 정보로부터 얻어지는 정보인, 상기 선회체의 회전 중심축과 상기 주행 장치가 접지하는 면에 대응하는 면의 교점인 특정점의 위치를 이용하여, 상기 제 2 위치를 구하는 것이 바람직하다.

상기 처리 장치는, 상기 특정점의 위치에 평활화 처리를 실시하고, 상기 평활화 처리 후의 상기 특정점의 위치를 이용하여, 상기 제 2 위치의 정보를 구하는 것이 바람직하다.

상기 처리 장치는, 상기 동작 정보를 이용하여 상기 제 1 위치에 평활화 처리를 실시하여 상기 제 2 위치의 정보를 구하는 것이 바람직하다.

상기 처리 장치는, 상기 위치 검출 장치에 의한 상기 작업 기계의 위치의 검출이 정상, 또한 상기 작업 기계의 주행이 정지, 또한 상기 선회체가 선회하고 있지 않을 때에, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는 것이 바람직하다.

상기 처리 장치는, 상기 작업 기계의 주행이 정지, 또한 상기 선회체가 선회하고 있을 때는, 상기 제 2 위치의 정보를 구하는 처리를 중단하는 것이 바람직하다.

상기 처리 장치는, 상기 선회체의 선회가 정지했을 때는, 상기 제 2 위치를 구하는 처리를 중단하기 전에 얻어진 상기 제 2 위치의 정보를 이용하여 상기 작업기의 적어도 일부의 위치를 구하는 것이 바람직하다.

상기 처리 장치는, 상기 제 2 위치를 구하는 처리를 중단하고 있을 때에, 상기 작업 기계가 주행을 개시한 경우, 상기 제 2 위치의 정보를 구하는 처리를 정지하는 것이 바람직하다.

상기 처리 장치는, 상기 위치 검출 장치가 상기 작업 기계의 위치를 정상으로 검출하고, 또한 상기 작업 기계의 주행이 정지해 있는 경우, 상기 제 2 위치를 구하는 처리를 정지하는 것이 바람직하다.

상기 처리 장치는, 상기 동작 정보를 이용하여 상기 작업 기계의 위치를 추정하고, 추정에 의해 얻어진 상기 작업 기계의 추정 위치를 보정하여 제 2 위치로서 출력하는 위치 추정부와, 상기 제 1 위치의 정보 및 상기 동작 정보 중 적어도 일방을 이용하여, 상기 추정 위치에 포함되는 오차를 구해, 상기 위치 추정부에 출력하는 오차 연산부를 갖고, 상기 위치 추정부는, 상기 오차 연산부가 출력한 상기 오차를 이용하여 상기 추정 위치를 보정하는 것이 바람직하다.

상기 처리 장치는, 상기 위치 검출 장치에 의한 상기 작업 기계의 위치의 검출 상태와, 상기 작업 기계의 동작 상태를 이용하여, 상기 오차 연산부에 입력되는 정보를 선택하는 것이 바람직하다.

본 발명은, 전술한 작업 기계의 제어 시스템을 구비하는 작업 기계이다.

본 발명은, 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어할 시에, 상기 작업 기계가 구비하는 위치 검출 장치가 검출한 상기 작업 기계의 일부의 위치인 제 1 위치와, 상기 작업 기계가 구비하는 상태 검출 장치가 검출한 상기 작업 기계의 동작 정보를 이용하여, 상기 일부의 위치에 상당하는 상기 작업 기계의 제 2 위치를 구하고, 상기 제 2 위치를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는, 작업 기계의 제어 방법이다.

상기 제 1 위치 및 상기 동작 정보로부터 얻어지는 정보인, 상기 선회체의 회전 중심축과 상기 주행 장치가 접지하는 면에 대응하는 면의 교점인 특정점의 위치를 이용하여, 상기 제 2 위치를 구하는 것이 바람직하다.

상기 특정점의 위치에 평활화 처리를 실시하고, 상기 평활화 처리 후의 상기 특정점의 위치를 이용하여, 상기 제 2 위치를 구하는 것이 바람직하다.

상기 동작 정보를 이용하여 상기 제 1 위치에 평활화 처리를 실시하여 상기 제 2 위치를 구하는 것이 바람직하다.

상기 제 2 위치를 구하는 경우, 상기 동작 정보를 이용하여 상기 작업 기계의 위치를 추정함으로써 추정 위치를 구하고, 상기 제 1 위치 및 상기 동작 정보 중 적어도 일방을 이용하여 상기 추정 위치에 포함되는 오차를 계산하고, 상기 오차 연산부가 출력한 상기 오차를 이용하여 상기 추정 위치를 보정하는 것이 바람직하다.

본 발명은, 작업 기계의 위치를 측위한 결과에 기초하여 정보화 시공을 실시하는 작업 기계에 있어서, 측위 결과의 편차가 정보화 시공에 주는 영향을 저감할 수 있다.

도 1 은, 실시형태 1 에 관련된 작업 기계의 사시도이다.
도 2 는, 제어 시스템 및 유압 시스템의 구성을 나타내는 블록도이다.
도 3 은, 유압 셔블의 측면도이다.
도 4 는, 유압 셔블의 배면도이다.
도 5 는, 실시형태 1 에 관련된 제어 시스템의 제어 블록도이다.
도 6 은, 유압 셔블의 자세를 나타내는 평면도이다.
도 7 은, 실시형태 1 에 관련된 장치 컨트롤러가 갖는 위치 정보 연산부를 나타내는 도면이다.
도 8 은, 실시형태 1 에 관련된 제어 시스템의 처리의 일례를 나타내는 플로우 차트이다.
도 9 는, 평활화 처리 상태의 천이를 설명하기 위한 도면이다.
도 10 은, 장치 컨트롤러가 평활화 처리의 상태를 천이시키는 처리의 플로우 차트이고, 특히 평활화 처리의 중단에 관한 처리를 나타내는 것이다.
도 11 은, 장치 컨트롤러가 평활화 처리의 상태를 천이시키는 처리의 플로우 차트이고, 특히 평활화 처리의 리셋에 관한 처리를 나타내는 것이다.
도 12 는, 실시형태 2 에 관련된 제어 시스템의 제어 블록도이다.
도 13 은, 실시형태 2 에 관련된 장치 컨트롤러가 갖는 위치 정보 연산부를 나타내는 도면이다.
도 14 는, 실시형태 2 에 관련된 제어 시스템의 처리의 일례를 나타내는 플로우 차트이다.
도 15 는, 실시형태 3 에 관련된 제어 시스템의 제어 블록도이다.
도 16 은, 실시형태 3 에 관련된 장치 컨트롤러가 갖는 위치·자세 정보 연산부를 나타내는 도면이다.
도 17 은, 실시형태 3 에 관련된 장치 컨트롤러가 갖는 위치·자세 정보 연산부의 제어 블록도이다.
도 18 은, 오차 연산부가 사용하는 관측 방정식을 선택할 때에 사용되는 정보가 기술된 테이블의 일례를 나타내는 도면이다.
도 19 는, 실시형태 3 에 관련된 제어 시스템의 처리의 일례를 나타내는 플로우 차트이다.

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

실시형태 1.

＜작업 기계의 전체 구성＞

도 1 은, 실시형태 1 에 관련된 작업 기계의 사시도이다. 도 2 는, 제어 시스템 (200) 및 유압 시스템 (300) 의 구성을 나타내는 블록도이다. 작업 기계로서의 유압 셔블 (100) 은, 본체부로서의 차량 본체 (1) 와 작업기 (2) 를 갖는다. 차량 본체 (1) 는, 선회체인 상부 선회체 (3) 와 주행체로서의 주행 장치 (5) 를 갖는다. 상부 선회체 (3) 는, 기계실 (3EG) 의 내부에, 동력 발생 장치인 엔진 및 유압 펌프 등의 장치를 수용하고 있다.

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

상부 선회체 (3) 는, 운전실 (4) 을 갖는다. 운전실 (4) 은, 상부 선회체 (3) 의 타단측에 설치되어 있다. 즉, 운전실 (4) 은, 기계실 (3EG) 이 배치되어 있는 측과는 반대측에 설치되어 있다. 운전실 (4) 내에는, 도 2 에 나타내는, 표시부 (29) 및 조작 장치 (25) 가 배치된다. 상부 선회체 (3) 의 상방에는, 난간 (9) 이 장착되어 있다.

주행 장치 (5) 상에는, 상부 선회체 (3) 가 탑재되어 있다. 주행 장치 (5) 는, 크롤러 벨트 (5a, 5b) 를 갖고 있다. 주행 장치 (5) 는, 좌우에 설치된 유압 모터 (5c) 의 일방 또는 양방에 의해 구동된다. 주행 장치 (5) 의 크롤러 벨트 (5a, 5b) 가 회전함으로써, 유압 셔블 (100) 을 주행시킨다. 작업기 (2) 는, 상부 선회체 (3) 의 운전실 (4) 의 측방측에 장착되어 있다.

유압 셔블 (100) 은, 크롤러 벨트 (5a, 5b) 대신에 타이어를 구비하고, 엔진의 구동력을, 트랜스미션을 개재하여 타이어로 전달하여 주행이 가능한 주행 장치를 구비한 것이어도 된다. 이와 같은 형태의 유압 셔블 (100) 로는, 예를 들어 휠식 유압 셔블이 있다.

상부 선회체 (3) 는, 작업기 (2) 및 운전실 (4) 이 배치되어 있는 측이 전방이고, 기계실 (3EG) 이 배치되어 있는 측이 후방이다. 상부 선회체 (3) 의 전후 방향이 x 방향이다. 전방을 향해서 좌측이 상부 선회체 (3) 의 좌방이고, 전방을 향해서 우측이 상부 선회체 (3) 의 우방이다. 상부 선회체 (3) 의 좌우 방향은, 폭 방향 또는 y 방향이라고도 한다. 유압 셔블 (100) 또는 차량 본체 (1) 는, 상부 선회체 (3) 를 기준으로 해서 주행 장치 (5) 측이 하방이고, 주행 장치 (5) 를 기준으로 해서 상부 선회체 (3) 측이 상방이다. 상부 선회체 (3) 의 상하 방향이 z 방향이다. 유압 셔블 (100) 이 수평면에 설치되어 있는 경우, 하방은 연직 방향, 즉, 중력의 작용 방향측이고, 상방은 연직 방향과는 반대측이다.

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

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

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

붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등의 유압 실린더와 도 2 에 나타내는 유압 펌프 (36, 37) 사이에는, 도 2 에 나타내는 방향 제어 밸브 (64) 가 설치되어 있다. 방향 제어 밸브 (64) 는, 유압 펌프 (36, 37) 로부터 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등에 공급되는 작동유의 유량을 제어함과 함께, 작동유가 흐르는 방향을 전환한다. 방향 제어 밸브 (64) 는, 유압 모터 (5c) 를 구동하기 위한 주행용 방향 제어 밸브와, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 그리고 상부 선회체 (3) 를 선회시키는 선회 모터 (38) 를 제어하기 위한 작업기용 방향 제어 밸브를 포함한다.

조작 장치 (25) 로부터 공급되는, 소정의 파일럿 압력으로 조정된 작동유가 방향 제어 밸브 (64) 의 스풀을 동작시키면, 방향 제어 밸브 (64) 로부터 유출되는 작동유의 유량이 조정되고, 유압 펌프 (36, 37) 로부터 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12), 선회 모터 (38) 또는 유압 모터 (5c) 로 공급되는 작동유의 유량이 제어된다. 그 결과, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 등의 동작이 제어된다.

또, 도 2 에 나타내는 장치 컨트롤러 (39) 가, 도 2 에 나타내는 제어 밸브 (27) 를 제어함으로써, 조작 장치 (25) 로부터 방향 제어 밸브 (64) 로 공급되는 작동유의 파일럿압이 제어되므로, 방향 제어 밸브 (64) 로부터 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12) 또는 선회 모터 (38) 로 공급되는 작동유의 유량이 제어된다. 그 결과, 장치 컨트롤러 (39) 는, 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12) 및 상부 선회체 (3) 의 동작을 제어할 수 있다.

상부 선회체 (3) 의 상부에는, 안테나 (21, 22) 가 장착되어 있다. 안테나 (21, 22) 는, 유압 셔블 (100) 의 현재 위치를 검출하기 위해서 사용된다. 안테나 (21, 22) 는, 도 2 에 나타내는 글로벌 좌표 연산 장치 (23) 와 전기적으로 접속되어 있다. 글로벌 좌표 연산 장치 (23) 는, 유압 셔블 (100) 의 위치를 검출하는 위치 검출 장치이다. 글로벌 좌표 연산 장치 (23) 는, RTK-GNSS (Real Time Kinematic - Global Navigation Satellite Systems, GNSS 는 전지구 항법 위성 시스템을 말한다) 를 이용하여 유압 셔블 (100) 의 현재 위치, 보다 구체적으로는 유압 셔블 (100) 의 일부의 현재 위치를 검출한다. 이하의 설명에 있어서, 안테나 (21, 22) 를, 적절히 GNSS 안테나 (21, 22) 라고 칭한다. 본 실시형태에 있어서, 글로벌 좌표 연산 장치 (23) 는, 유압 셔블 (100) 의 일부의 현재 위치로서, GNSS 안테나 (21, 22) 의 적어도 1 개의 위치를 검출한다. GNSS 안테나 (21, 22) 가 수신한 GNSS 전파에 따른 신호는, 글로벌 좌표 연산 장치 (23) 에 입력된다. 글로벌 좌표 연산 장치 (23) 는, 글로벌 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 설치 위치를 구한다. 전지구 항법 위성 시스템의 일례로는, GPS (Global Positioning System) 를 들 수 있지만, 전지구 항법 위성 시스템은, 이것에 한정되는 것은 아니다.

RTK-GNSS 에서는, 측위 위성의 배치, 전리층, 대류권 또는 GNSS 안테나 주변의 지형의 영향으로 측위 상태가 변화한다. 이 측위 상태에는, 예를 들어 Fix (정밀도 ± 1 ㎝ 내지 2 ㎝ 정도), Float (정밀도 ± 10 ㎝ 내지 수 m 정도), 단독 측위 (정밀도 ± 수 m 정도), 비측위 (측위 계산 불능) 등이 있다. 이하에 있어서, 측위 상태가 Fix 인 경우를 정상이라고 칭하고, Fix 이외의 상태인 경우를 이상이라고 칭한다.

GNSS 안테나 (21, 22) 는, 도 1 에 나타내는 바와 같이, 상부 선회체 (3) 상이고, 유압 셔블 (100) 의 좌우 방향, 즉 폭 방향으로 떨어진 양단 위치에 설치되는 것이 바람직하다. 본 실시형태에 있어서, GNSS 안테나 (21, 22) 는, 상부 선회체 (3) 의 폭 방향 양측에 각각 장착된 난간 (9) 에 장착된다. GNSS 안테나 (21, 22) 가 상부 선회체 (3) 에 장착되는 위치는 난간 (9) 에 한정되는 것은 아니지만, GNSS 안테나 (21, 22) 는, 가능한 한 떨어진 위치에 설치되는 편이, 유압 셔블 (100) 의 현재 위치의 검출 정밀도는 향상되므로 바람직하다. 또, GNSS 안테나 (21, 22) 는, 오퍼레이터의 시야를 최대한 방해하지 않는 위치에 설치되는 것이 바람직하다. 예를 들어, GNSS 안테나 (21, 22) 는, 기계실 (3EG) 의 후방에 배치된 카운터웨이트 상에 배치되어도 된다.

도 2 에 나타내는 바와 같이, 유압 셔블 (100) 의 유압 시스템 (300) 은, 엔진 (35) 과, 유압 펌프 (36, 37) 를 구비한다. 유압 펌프 (36, 37) 는, 엔진 (35) 에 의해 구동되어, 작동유를 토출한다. 유압 펌프 (36, 37) 로부터 토출된 작동유는, 붐 실린더 (10) 와 아암 실린더 (11) 와 버킷 실린더 (12) 로 공급된다. 또, 유압 셔블 (100) 은, 선회 모터 (38) 를 구비한다. 선회 모터 (38) 는, 유압 모터이고, 유압 펌프 (36, 37) 로부터 토출된 작동유에 의해 구동된다. 선회 모터 (38) 는, 상부 선회체 (3) 를 선회시킨다. 도 2 에서는, 2 개의 유압 펌프 (36, 37) 가 도시되어 있지만, 유압 펌프는 1 개여도 된다. 선회 모터 (38) 는, 유압 모터에 한정하지 않고, 전기 모터여도 된다.

작업 기계의 제어 시스템인 제어 시스템 (200) 은, 글로벌 좌표 연산 장치 (23) 와, 각속도 및 가속도를 검출하는 상태 검출 장치인 IMU (Inertial Measurement Unit : 관성 계측 장치) (24) 와, 조작 장치 (25) 와, 처리 장치로서의 장치 컨트롤러 (39) 와, 처리 장치로서의 표시 컨트롤러 (28) 와, 표시부 (29) 를 포함한다. 조작 장치 (25) 는, 도 1 에 나타내는 작업기 (2), 상부 선회체 (3) 및 주행 장치 (5) 중 적어도 1 개를 조작하기 위한 장치이다. 조작 장치 (25) 는, 작업기 (2) 등을 구동시키기 위해서 오퍼레이터에 의한 조작을 받아들이고, 조작량에 따른 파일럿 유압을 출력한다.

조작 장치 (25) 는, 오퍼레이터의 좌측에 설치되는 좌조작 레버 (25L) 와, 오퍼레이터의 우측에 배치되는 우조작 레버 (25R) 를 갖는다. 좌조작 레버 (25L) 및 우조작 레버 (25R) 는, 전후좌우의 동작이 2 축의 동작에 대응되어 있다. 예를 들어, 우조작 레버 (25R) 의 전후 방향의 조작은, 붐 (6) 의 조작에 대응되어 있다. 예를 들어, 우조작 레버 (25R) 의 좌우 방향의 조작은, 버킷 (8) 의 조작에 대응되어 있다. 예를 들어, 좌조작 레버 (25L) 의 전후 방향의 조작은, 아암 (7) 의 조작에 대응하고 있다. 예를 들어, 좌조작 레버 (25L) 의 좌우 방향의 조작은, 상부 선회체 (3) 의 선회에 대응하고 있다.

본 실시형태에 있어서, 조작 장치 (25) 는, 파일럿 유압 방식이 사용된다. 조작 장치 (25) 에는, 유압 펌프 (36) 로부터, 도시하지 않은 감압 밸브에 의해 소정의 파일럿 압력으로 감압된 작동유가 붐 조작, 버킷 조작, 아암 조작, 선회 조작 및 주행 조작에 기초하여 공급된다.

우조작 레버 (25R) 의 전후 방향의 조작에 따라, 파일럿 유로 (450) 에 파일럿 유압이 공급 가능하게 되고, 오퍼레이터에 의한 붐 (6) 의 조작이 받아들여진다. 우조작 레버 (25R) 의 조작량에 따라 우조작 레버 (25R) 가 구비하는 밸브 장치가 열리고, 파일럿 유로 (450) 에 작동유가 공급된다. 또, 압력 센서 (66) 는, 그때의 파일럿 유로 (450) 내에 있어서의 작동유의 압력을 파일럿압으로서 검출한다. 압력 센서 (66) 는, 검출한 파일럿압을, 붐 조작 신호 (MB) 로서 장치 컨트롤러 (39) 에 송신한다.

조작 장치 (25) 와 붐 실린더 (10) 사이의 파일럿 유로 (450) 에는, 압력 센서 (68), 제어 밸브 (이하, 적절히 개입 밸브라고 칭한다) (27C) 및 셔틀 밸브 (51) 가 설치된다. 우조작 레버 (25R) 의 좌우 방향의 조작에 따라, 파일럿 유로 (450) 에 파일럿 유압이 공급 가능하게 되고, 오퍼레이터에 의한 버킷 (8) 의 조작이 받아들여진다. 우조작 레버 (25R) 의 조작량에 따라 우조작 레버 (25R) 가 구비하는 밸브 장치가 열리고, 파일럿 유로 (450) 에 작동유가 공급된다. 또, 압력 센서 (66) 는, 그때의 파일럿 유로 (450) 내에 있어서의 작동유의 압력을 파일럿압으로서 검출한다. 압력 센서 (66) 는, 검출한 파일럿압을, 버킷 조작 신호 (MT) 로서 장치 컨트롤러 (39) 에 송신한다.

좌조작 레버 (25L) 의 전후 방향의 조작에 따라, 파일럿 유로 (450) 에 파일럿 유압이 공급 가능하게 되고, 오퍼레이터에 의한 아암 (7) 의 조작이 받아들여진다. 좌조작 레버 (25L) 의 조작량에 따라 좌조작 레버 (25L) 가 구비하는 밸브 장치가 열리고, 파일럿 유로 (450) 에 작동유가 공급된다. 또, 압력 센서 (66) 는, 그때의 파일럿 유로 (450) 내에 있어서의 작동유의 압력을 파일럿압으로서 검출한다. 압력 센서 (66) 는, 검출한 파일럿압을, 아암 조작 신호 (MA) 로서 장치 컨트롤러 (39) 에 송신한다.

좌조작 레버 (25L) 의 좌우 방향의 조작에 따라, 파일럿 유로 (450) 에 파일럿 유압이 공급 가능하게 되고, 오퍼레이터에 의한 상부 선회체 (3) 의 선회 조작이 받아들여진다. 좌조작 레버 (25L) 의 조작량에 따라 좌조작 레버 (25L) 가 구비하는 밸브 장치가 열리고, 파일럿 유로 (450) 에 작동유가 공급된다. 또, 압력 센서 (66) 는, 그때의 파일럿 유로 (450) 내에 있어서의 작동유의 압력을 파일럿압으로서 검출한다. 압력 센서 (66) 는, 검출한 파일럿압을, 선회 조작 신호 (MR) 로서 장치 컨트롤러 (39) 에 송신한다.

우조작 레버 (25R) 가 조작됨으로써, 조작 장치 (25) 는, 우조작 레버 (25R) 의 조작량에 따른 크기의 파일럿 유압을 방향 제어 밸브 (64) 에 공급한다. 좌조작 레버 (25L) 가 조작됨으로써, 조작 장치 (25) 는, 좌조작 레버 (25L) 의 조작량에 따른 크기의 파일럿 유압을 제어 밸브 (27) 에 공급한다. 이 파일럿 유압에 의해, 방향 제어 밸브 (64) 의 스풀이 이동한다.

파일럿 유로 (450) 에는, 제어 밸브 (27) 가 설치되어 있다. 우조작 레버 (25R) 및 좌조작 레버 (25L) 의 조작량은, 파일럿 유로 (450) 에 설치되는 압력 센서 (66) 에 의해 검출된다. 압력 센서 (66) 가 검출한 파일럿 유압은, 장치 컨트롤러 (39) 에 입력된다. 장치 컨트롤러 (39) 는, 입력된 파일럿 유압에 따른, 파일럿 유로 (450) 의 제어 신호 (N) 를 제어 밸브 (27) 에 출력해, 파일럿 유로 (450) 를 개폐한다. 우조작 레버 (25R) 또는 좌조작 레버 (25L) 의 조작 방향과 조작 대상 (버킷 (8), 아암 (7), 붐 (6), 상부 선회체 (3)) 의 관계는, 상기에 한정되는 것은 아니고, 상이한 관계여도 된다.

조작 장치 (25) 는, 주행용 레버 (25FL, 25FR) 를 갖는다. 본 실시형태에 있어서, 조작 장치 (25) 는, 파일럿 유압 방식이 사용되므로, 유압 펌프 (36) 로부터, 감압된 작동유가 방향 제어 밸브 (64) 로 공급되고, 파일럿 유로 (450) 내의 작동유의 압력에 근거해 방향 제어 밸브 (64) 의 스풀이 구동된다. 그러면, 유압 셔블 (100) 의 주행 장치 (5) 가 구비하는 유압 모터 (5c, 5c) 에, 유압 펌프 (36, 37) 로부터 작동유가 공급되어, 주행 가능해진다. 파일럿 유로 (450) 내의 작동유의 압력, 즉 파일럿압은, 압력 센서 (27PC) 에 의해 검출된다.

유압 셔블 (100) 의 오퍼레이터가 주행 장치 (5) 를 동작시키는 경우, 오퍼레이터는 주행용 레버 (25FL, 25FR) 를 조작한다. 오퍼레이터에 의한 주행용 레버 (25FL, 25FR) 의 조작량은 압력 센서 (27PC) 에서 검출되고, 장치 컨트롤러 (39) 에 조작 신호 (MD) 로서 출력된다.

좌조작 레버 (25L) 및 우조작 레버 (25R) 의 조작량이, 예를 들어 포텐셔미터 및 홀 IC 등에 의해 검출되고, 장치 컨트롤러 (39) 는, 이들 검출값에 기초하여 방향 제어 밸브 (64) 및 제어 밸브 (27) 를 제어함으로써, 작업기 (2) 를 제어해도 된다. 이와 같이, 좌조작 레버 (25L) 및 우조작 레버 (25R) 는, 전기 방식이어도 된다.

제어 시스템 (200) 은, 제 1 스트로크 센서 (16) 와 제 2 스트로크 센서 (17) 와 제 3 스트로크 센서 (18) 를 갖는다. 예를 들어, 제 1 스트로크 센서 (16) 는 붐 실린더 (10) 에, 제 2 스트로크 센서 (17) 는 아암 실린더 (11) 에, 제 3 스트로크 센서 (18) 는 버킷 실린더 (12) 에, 각각 설치된다. 제 1 스트로크 센서 (16) 는, 붐 실린더 (10) 의 신장에 대응하는 변위량을 검출해, 장치 컨트롤러 (39) 에 출력한다. 제 2 스트로크 센서 (17) 는, 아암 실린더 (11) 의 신장에 대응하는 변위량을 검출해, 장치 컨트롤러 (39) 에 출력한다. 제 3 스트로크 센서 (18) 는, 버킷 실린더 (12) 의 신장에 대응하는 변위량을 검출해, 장치 컨트롤러 (39) 에 출력한다.

장치 컨트롤러 (39) 는, CPU (Central Processing Unit) 등의 프로세서인 처리부 (39P) 와, RAM (Random Access Memory) 및 ROM (Read Only Memory) 등의 기억 장치인 기억부 (39M) 를 갖는다. 장치 컨트롤러 (39) 에는, 글로벌 좌표 연산 장치 (23) 의 검출값, IMU (24) 의 검출값, 압력 센서 (27PC, 66, 68) 의 검출값, 제 1 스트로크 센서 (16) 의 검출값, 제 2 스트로크 센서 (17) 의 검출값 및 제 3 스트로크 센서 (18) 의 검출값이 입력된다. 장치 컨트롤러 (39) 는, 글로벌 좌표 연산 장치 (23) 의 검출값 및 IMU (24) 의 검출값으로부터, 유압 셔블 (100) 의 위치에 관련된 위치 정보 (IPL) 를 구해 표시 컨트롤러 (28) 에 출력한다. 장치 컨트롤러 (39) 는, 도 2 에 나타내는 압력 센서 (66) 의 검출값에 기초하여, 제어 밸브 (27) 및 개입 밸브 (27C) 를 제어한다.

도 2 에 나타내는 방향 제어 밸브 (64) 는, 예를 들어 비례 제어 밸브이고, 조작 장치 (25) 로부터 공급되는 작동유에 의해 제어된다. 방향 제어 밸브 (64) 는, 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12) 및 선회 모터 (38) 등의 유압 액츄에이터와, 유압 펌프 (36, 37) 사이에 배치된다. 방향 제어 밸브 (64) 는, 유압 펌프 (36, 37) 로부터 붐 실린더 (10), 아암 실린더 (11), 버킷 실린더 (12) 및 선회 모터 (38) 로 공급되는 작동유의 유량을 제어한다.

글로벌 좌표 연산 장치 (23) 는, 도 2 에 나타내는 보정 데이터 수신 장치 (26) 로부터, 보정 데이터 (C1) 를 수신한다. 보정 데이터 수신 장치 (26) 는, 글로벌 좌표 연산 장치 (23) 에 접속된다. 보정 데이터 (C1) 는, 유압 셔블 (100) 의 외부에 설치된 GNSS 수신기에서 생성된, RTK-GNSS 에서 이용 가능한 정보이고, 보정 데이터 수신 장치 (26) 와 공통 규격의 통신 기능을 갖는 장치로부터 송신된 정보이다. 또, 보정 데이터 수신 장치 (26) 를 전화 회선용의 모뎀으로 하고, 보정 데이터 전송 서비스를 이용하여 외부로부터 보정 데이터 (C1) 를 얻도록 해도 된다. 보정 데이터 수신 장치 (26) 는 보정 데이터 (C1) 를 글로벌 좌표 연산 장치 (23) 에 출력한다. GNSS 안테나 (21), GNSS 안테나 (22) 는, 복수의 측위 위성으로부터 신호를 수신하고, 글로벌 좌표 연산 장치 (23) 에 출력한다.

글로벌 좌표 연산 장치 (23) 는, GNSS 안테나 (21), GNSS 안테나 (22) 로부터 입력된 측위 위성의 신호와, 보정 데이터 수신 장치 (26) 로부터 수신한 보정 데이터 (C1) 에 기초하여, GNSS 안테나 (21) 의 위치인 기준 위치 데이터 (P1) 와 GNSS 안테나 (22) 의 위치인 기준 위치 데이터 (P2) 를 측위한다. 글로벌 좌표 연산 장치 (23) 는, GNSS 안테나 (21) 의 위치인 기준 위치 데이터 (P1) 와 GNSS 안테나 (22) 의 위치인 기준 위치 데이터 (P2) 의 상대 위치로부터 선회체 방위 데이터 (Q) 를 산출한다. GNSS 안테나 (21, 22) 및 글로벌 좌표 연산 장치 (23) 에 의해 GPS 컴퍼스를 구성하고, 선회체 방위 데이터 (Q) 를 얻도록 해도 된다.

GNSS 안테나 (21, 22) 가 설치되는 부분은, 유압 셔블 (100) 의 일부이다. 따라서, 기준 위치 데이터 (P1, P2) 는, 유압 셔블 (100) 의 일부, 구체적으로는 GNSS 안테나 (21, 22) 가 설치되는 부분의 위치를 나타내는 정보이다. 이하에 있어서, GNSS 안테나 (21, 22) 가 설치되는 부분의 위치를, 적절히 제 1 위치라고 칭한다. 기준 위치 데이터 (P1, P2) 는, 제 1 위치의 정보이다.

본 실시형태에 있어서, 선회체 방위 데이터 (Q) 는, GNSS 안테나 (21, 22) 가 취득한 기준 위치 데이터 (P), 즉 기준 위치 데이터 (P1) 및 기준 위치 데이터 (P2) 의 적어도 일방으로부터 결정되는 방위가, 글로벌 좌표의 기준 방위 (예를 들어 북쪽) 에 대해 이루는 각, 즉 방위각이다. 방위각은, 유압 셔블 (100) 의 요각이기도 하다. 선회체 방위 데이터 (Q) 는, 상부 선회체 (3), 즉 작업기 (2) 가 향하고 있는 방위를 나타내고 있다.

글로벌 좌표 연산 장치 (23) 는, CPU 등의 프로세서인 처리부와, RAM 및 ROM 등의 기억 장치인 기억부를 갖는다. 글로벌 좌표 연산 장치 (23) 는, 측위한 2 개의 기준 위치 데이터 (P1, P2), 즉 기준 위치 데이터 (P) 와 선회체 방위 데이터 (Q) 를, 장치 컨트롤러 (39) 에 출력한다.

표시 컨트롤러 (28) 는, CPU 등의 프로세서인 처리부 (28P) 와, RAM 및 ROM 등의 기억 장치인 기억부 (28M) 를 갖는다. 표시 컨트롤러 (28) 는, 표시부 (29) 에, 예를 들어 후술하는 가이던스 화면 등의 화상을 표시하는 것 외에, 장치 컨트롤러 (39) 로부터 얻어지는 유압 셔블 (100) 의 위치 정보 (IPL) 를 이용하여, 버킷 (8) 의 날끝 (8T) 의 3 차원 위치인 날끝 위치를 나타내는 버킷 날끝 위치 데이터 (S) 를 생성한다. 표시부 (29) 는, 예를 들어 액정 표시 장치 등이지만, 이것에 한정되는 것은 아니다. 표시부 (29) 는, 예를 들어 입력부와 표시부를 일체화한 터치 패널을 사용할 수 있다. 본 실시형태에 있어서는, 표시부 (29) 에 인접해, 스위치 (29S) 가 설치되어 있다. 스위치 (29S) 는, 후술하는 굴삭 제어를 실행시키거나, 실행 중의 굴삭 제어를 정지시키거나 하기 위한 입력 장치이다. 표시부 (29) 에 터치 패널을 사용하는 경우, 스위치 (29S) 는 터치 패널의 입력부에 삽입되어도 된다.

표시 컨트롤러 (28) 는, 작업기 (2) 가 굴삭하는 대상인 목표 시공면의 화상과, 버킷 날끝 위치 데이터 (S) 를 이용하여 생성한 버킷 (8) 의 화상을 가이던스 화면으로서 표시부 (29) 에 표시시킬 수 있다. 표시 컨트롤러 (28) 는, 가이던스 화면에 의해, 목표 시공면과 버킷 (8) 의 위치 관계를 유압 셔블 (100) 의 오퍼레이터에게 인식시켜, 정보화 시공을 실시할 때의 오퍼레이터의 부담을 경감할 수 있다.

IMU (24) 는, 유압 셔블 (100) 의 동작을 나타내는 동작 정보 (MI) 를 검출하는 상태 검출 장치이다. 유압 셔블 (100) 의 동작은, 상부 선회체 (3) 의 동작 및 주행 장치 (5) 의 동작 중 적어도 일방을 포함한다. 본 실시형태에 있어서, 동작 정보 (MI) 는, 유압 셔블 (100) 의 자세를 나타내는 정보를 포함하고 있어도 된다. 유압 셔블 (100) 의 자세를 나타내는 정보는, 유압 셔블 (100) 의 롤각, 피치각 및 방위각이 예시된다.

본 실시형태에 있어서, IMU (24) 는, 유압 셔블 (100) 의 각속도 및 가속도를 검출한다. 유압 셔블 (100) 의 동작에 따라, 유압 셔블 (100) 에는, 주행 시에 발생하는 가속도, 선회 시에 발생하는 각가속도 및 중력가속도와 같은 여러 가지 가속도가 발생하지만, IMU (24) 는 적어도 중력가속도를 포함하는 가속도를 검출하고, 각 가속도의 종류를 구별하지 않고 검출한 가속도를 출력한다. 중력가속도는, 중력에 대응한 가속도이다. IMU (24) 는, 도 1 에 나타내는 차체 좌표계 (x, y, z) 에 있어서, x 축 방향, y 축 방향 및 z 축 방향의 가속도 (a) 와, x 축, y 축 및 z 축 둘레의 각속도 (회전 각속도)(ω) 를 검출한다. 이들이 동작 정보 (MI) 가 된다. 차체 좌표계란, 유압 셔블 (100) 을 기준으로 한, (x, y, z) 로 나타내는 3 차원 좌표계이다.

IMU (24) 가 검출하는 동작 정보 (MI) 에는, 상부 선회체 (3) 의 회전 중심축이 되는 z 축을 중심으로 하여 상부 선회체 (3) 가 선회할 때의 각속도 (ω) 가 포함된다. 선회 시의 각속도 (ω) 는, GNSS 안테나 (21, 22) 의 위치를 나타내는 정보로부터 취득된 상부 선회체 (3) 의 선회 각도를 시간으로 미분함으로써 구해져도 된다. 선회 시의 각속도 (ω) 를 시간으로 적분함으로써, 선회 각도를 구할 수 있다.

IMU (24) 는, 상부 선회체 (3) 에 장착되어 있다. IMU (24) 는, 보다 높은 정밀도로 가속도 등을 검출하기 위해서, 예를 들어 유압 셔블 (100) 의 상부 선회체 (3) 의 선회 중심축 상에 설치되는 것이 바람직하지만, IMU (24) 는 운전실 (4) 의 하부에 설치되어도 된다.

도 3 은, 유압 셔블 (100) 의 측면도이다. 도 4 는, 유압 셔블 (100) 의 배면도이다. 차량 본체 (1) 의 좌우 방향, 즉 폭 방향에 대한 경사각 (θ4) 은 유압 셔블 (100) 의 롤각이고, 차량 본체 (1) 의 전후 방향에 대한 경사각 (θ5) 은 유압 셔블 (100) 의 피치각이고, z 축 둘레에 있어서의 상부 선회체 (3) 의 각도는 유압 셔블 (100) 의 방위각이다. 롤각은 IMU (24) 에 의해 검출된 x 축 둘레의 각속도를 시간으로 적분함으로써, 피치각은 IMU (24) 에 의해 검출된 y 축 둘레의 각속도를 시간으로 적분함으로써, 방위각은 IMU (24) 에 의해 검출된 z 축 둘레의 각속도를 시간으로 적분함으로써 구해진다. z 축 둘레의 각속도는, 유압 셔블 (100) 의 선회 시의 각속도 (ω) 이다. 즉, 선회 시의 각속도 (ω) 를 시간으로 적분함으로써 유압 셔블 (100), 보다 구체적으로는 상부 선회체 (3) 의 방위각이 얻어진다.

IMU (24) 는, 소정의 주기로 유압 셔블 (100) 의 가속도 및 각속도를 갱신한다. IMU (24) 의 갱신 주기는, 글로벌 좌표 연산 장치 (23) 에 있어서의 갱신 주기보다 짧은 것이 바람직하다. IMU (24) 가 검출한 가속도 및 각속도는, 동작 정보 (MI) 로서 장치 컨트롤러 (39) 에 출력된다. 장치 컨트롤러 (39) 는, IMU (24) 로부터 취득한 동작 정보 (MI) 에 필터 처리 및 적분과 같은 처리를 실시하여, 롤각인 경사각 (θ4), 피치각인 경사각 (θ5) 및 방위각을 구한다. 장치 컨트롤러 (39) 는, 구한 경사각 (θ4), 경사각 (θ5) 및 방위각을, 유압 셔블 (100) 의 위치에 관련된 위치 정보 (IPL) 로서, 표시 컨트롤러 (28) 에 출력한다.

표시 컨트롤러 (28) 는, 글로벌 좌표 연산 장치 (23) 로부터 기준 위치 데이터 (P) 및 선회체 방위 데이터 (Q) 를 취득한다. 선회체 방위 데이터 (Q) 는, 유압 셔블 (100) 의 방위를 나타내는 정보이고, 본 실시형태에 있어서는, 상부 선회체 (3) 의 방위를 나타내는 정보이다. 구체적으로는, 선회체 방위 데이터 (Q) 는, 상부 선회체 (3) 의 방위각이다. 본 실시형태에 있어서, 표시 컨트롤러 (28) 는, 작업기 위치 데이터로서, 버킷 날끝 위치 데이터 (S) 를 생성한다. 버킷 날끝 위치 데이터 (S) 는, 장치 컨트롤러 (39) 에 의해 생성되어도 된다. 그리고, 표시 컨트롤러 (28) 는, 버킷 날끝 위치 데이터 (S) 와, 목표 시공 정보 (T) 를 이용하여, 굴삭 대상의 목표 형상을 나타내는 목표 굴삭 지형 데이터 (U) 를 생성한다. 목표 시공 정보 (T) 는, 표시 컨트롤러 (28) 의 기억부 (28M) (목표 시공 정보 격납부 (28C)) 에 기억되어 있다. 목표 시공 정보 (T) 는, 유압 셔블 (100) 이 구비하는 작업기 (2) 의 굴삭 대상의 굴삭 후에 있어서의 마무리의 목표가 되는 정보이고, 설계 데이터로부터 얻어지는 목표 시공면의 정보를 포함한다. 목표 굴삭 지형 데이터 (U) 는, 차체 좌표계에 있어서 날끝 (8T) 의 현시점에 있어서의 날끝 위치를 지나는 수직선과, 목표 시공면의 교점을 굴삭 대상 위치로 했을 때, 굴삭 대상 위치의 전후에 있어서의 단수 또는 복수의 변곡점의 위치를 나타내는 정보와 그 전후의 선의 각도 정보이다.

장치 컨트롤러 (39) 는, 제 1 스트로크 센서 (16) 가 검출한 붐 실린더 길이로부터, 차체 좌표계에 있어서의 수평면과 직교하는 방향 (z 축 방향) 에 대한 붐 (6) 의 경사각 (θ1) (도 3 참조) 을 산출한다. 장치 컨트롤러 (39) 는, 제 2 스트로크 센서 (17) 가 검출한 아암 실린더 길이로부터, 붐 (6) 에 대한 아암 (7) 의 경사각 (θ2)(도 3 참조) 을 산출한다. 장치 컨트롤러 (39) 는, 제 3 스트로크 센서 (18) 가 검출한 버킷 실린더 길이로부터, 아암 (7) 에 대한 버킷 (8) 의 경사각 (θ3) 을 산출한다. IMU (24) 는, 선회 시의 각속도 (ω) 를 장치 컨트롤러 (39) 에 출력한다.

장치 컨트롤러 (39) 는, 전술한 바와 같이, 도 1 에 나타내는 z 축을 중심으로 해서 상부 선회체 (3) 가 선회할 때에 있어서의 상부 선회체 (3) 의 선회 시의 각속도 (ω) 를, IMU (24) 로부터 취득한다. 또, 장치 컨트롤러 (39) 는, 압력 센서 (66) 로부터 붐 조작 신호 (MB), 버킷 조작 신호 (MT), 아암 조작 신호 (MA) 및 선회 조작 신호 (MR) 를 취득한다.

장치 컨트롤러 (39) 는, 표시 컨트롤러 (28) 로부터, 목표 굴삭 지형 데이터 (U) 를 취득한다. 장치 컨트롤러 (39) 는, 자신이 구한 작업기 (2) 의 각도 (θ1, θ2, θ3) 로부터, 버킷 (8) 의 날끝 (8T) 의 위치 (이하, 적절히 날끝 위치라고 칭한다) 를 구한다. 장치 컨트롤러 (39) 의 기억부 (39M) 는, 작업기 (2) 의 데이터 (이하, 적절히 작업기 데이터라고 한다) 를 기억하고 있다. 작업기 데이터는, 붐 (6) 의 길이 (L1), 아암 (7) 의 길이 (L2) 및 버킷 (8) 의 길이 (L3) 와 같은 설계 치수를 포함한다. 도 3 에 나타내는 바와 같이, 붐 (6) 의 길이 (L1) 는, 붐 핀 (13) 으로부터 아암 핀 (14) 까지의 길이에 상당한다. 아암 (7) 의 길이 (L2) 는, 아암 핀 (14) 으로부터 버킷 핀 (15) 까지의 길이에 상당한다. 버킷 (8) 의 길이 (L3) 는, 버킷 핀 (15) 으로부터 버킷 (8) 의 날끝 (8T) 까지의 길이에 상당한다. 날끝 (8T) 은, 도 1 에 나타내는 날 (8B) 의 선단이다. 또, 작업기 데이터는, 차체 좌표계의 위치 PL 에 대한 붐 핀 (13) 까지의 위치의 정보를 포함한다. 장치 컨트롤러 (39) 는, 길이 (L1, L2, L3), 경사각 (θ1, θ2, θ3) 및 위치 PL 을 이용하여, 위치 PL 에 대한 날끝 위치를 구할 수 있다.

장치 컨트롤러 (39) 는, 목표 굴삭 지형 데이터 (U) 를 따라 버킷 (8) 의 날끝 (8T) 이 이동하도록, 조작 장치 (25) 로부터 입력된 붐 조작 신호 (MB), 버킷 조작 신호 (MT) 및 아암 조작 신호 (MA) 를, 목표 굴삭 지형 데이터 (U) 와 버킷 (8) 의 날끝 (8T) 의 거리 및 버킷 (8) 의 날끝 (8T) 의 속도에 근거해 조정한다. 장치 컨트롤러 (39) 는, 목표 굴삭 지형 데이터 (U) 를 따라 버킷 (8) 의 날끝 (8T) 이 이동하도록 작업기 (2) 를 제어하기 위한 제어 신호 (N) 를 생성해, 도 2 에 나타내는 제어 밸브 (27) 에 출력한다. 이와 같은 처리에 의해, 작업기 (2) 가 목표 굴삭 지형 데이터 (U) 에 근접하는 속도는, 목표 굴삭 지형 데이터 (U) 에 대한 거리에 따라 제한된다.

장치 컨트롤러 (39) 로부터의 제어 신호 (N) 에 따라, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 의 각각에 대해 2 개씩 설치된 제어 밸브 (27) 가 개폐된다. 좌조작 레버 (25L) 또는 우조작 레버 (25R) 의 조작과 제어 밸브 (27) 의 개폐 지령에 근거해, 방향 제어 밸브 (64) 의 스풀이 동작해, 붐 실린더 (10), 아암 실린더 (11) 및 버킷 실린더 (12) 에 작동유가 공급된다.

글로벌 좌표 연산 장치 (23) 는, 글로벌 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 기준 위치 데이터 (P1, P2) 를 검출한다. 본 실시형태에 있어서, 글로벌 좌표계란, 예를 들어 GNSS 에 있어서의 좌표계이다. 도 3 에서는, 글로벌 좌표계는, (Xg, Yg, Zg) 로 나타내는 3 차원 좌표계이다. 현장 좌표계는, 유압 셔블 (100) 의 작업 에어리어 (GA) 에 설치된 기준이 되는, 예를 들어 기준 말뚝 (60) 의 위치 PG 를 기준으로 한, (X, Y, Z) 로 나타내는 3 차원 좌표계이다. 도 3 에 나타내는 바와 같이, 위치 PG 는, 예를 들어 작업 에어리어 (GA) 에 설치된 기준 말뚝 (60) 의 선단 (60T) 에 위치한다. 글로벌 좌표계 (Xg, Yg, Zg) 와 현장 좌표계 (X, Y, Z) 는, 서로 변환할 수 있다.

도 2 에 나타내는 표시 컨트롤러 (28) 는, 글로벌 좌표 연산 장치 (23) 에 의한 검출 결과에 기초하여, 글로벌 좌표계에서의 차체 좌표계의 위치를 구한다. 본 실시형태에 있어서, 예를 들어 차체 좌표계의 위치 PL 은, 선회체의 회전 중심축인 z 축과 주행 장치 (5) 가 접지하는 면에 상당하는 면의 교점이다. 본 실시형태에 있어서, 위치 PL 의 좌표는, 차체 좌표계에 있어서, (0, 0, 0) 이 된다. 주행 장치 (5) 가 접지하는 면은, 크롤러 벨트 (5a, 5b) 가 접하는 작업 에어리어 (GA) 의 표면 (GD) 이다. 주행 장치 (5) 가 접지하는 면에 상당하는 면은, 작업 에어리어 (GA) 의 표면 (GD) 이어도 되고, 크롤러 벨트 (5a, 5b) 가 접지하는 부분에 의해 규정되는 평면 (CP) 이어도 된다. 크롤러 벨트 (5a, 5b) 가 접지하는 부분에 의해 규정되는 평면 (CP) 은, 차체 좌표계 (x, y, z) 에 있어서, 유압 셔블 (100) 의 설계 치수로부터 일의적으로 결정된다.

위치 PL 은, z 축과 평면 (CP) 의 교점에 한정되는 것은 아니다. 본 실시형태에 있어서, 후술하는 유사 부동점의 위치는, 위치 PL 과 일치하고 있어도 되고, 일치하고 있지 않아도 된다. 차체 좌표계의 위치 PL 은, 다른 장소여도 되고, 예를 들어 붐 핀 (13) 의 축 방향의 길이의 중심점을 위치 PL 로 해도 된다. 위치 PL 은, z 축 상, 또한 상부 선회체 (3) 가 선회하기 위한 스윙 써클 상에 위치하고 있어도 된다. 전술한 바와 같이, 장치 컨트롤러 (39) 는, 위치 PL 에 대한 날끝 위치, 즉 차체 좌표계에서의 날끝 위치를 구하므로, 글로벌 좌표계에서의 위치 PL 의 좌표가 얻어지면, 차체 좌표계에서의 날끝 위치의 좌표를, 글로벌 좌표계에서의 날끝 위치의 좌표로 변환할 수 있다.

장치 컨트롤러 (39) 는, 버킷 (8) 이 목표 굴삭 지형을 침식하는 것을 억제하기 위해서, 작업기 (2) 가 굴삭 대상에 접근하는 방향의 속도가 제한 속도 이하가 되도록 제어한다. 이 제어를, 적절히 굴삭 제어라고 한다. 굴삭 제어는, 표시 컨트롤러 (28) 로부터 취득된 목표 굴삭 지형 데이터 (U) 및 버킷 날끝 위치 데이터 (S) 에 기초하여, 작업기 (2) 와 굴삭 대상의 상대 위치를 연산하면서 작업기 (2) 가 굴삭 대상에 접근하는 방향의 속도를 제한 속도 이하가 되도록 하는 제어이다. 이와 같은 제어를 실행함으로써, 굴삭 대상을 목표 형상 (목표 시공 정보 (T) 가 나타내는 형상) 으로 시공할 수 있다. 다음으로, 제어 시스템 (200) 에 대해, 보다 상세하게 설명한다.

＜제어 시스템 (200)＞

도 5 는, 실시형태 1 에 관련된 제어 시스템 (200) 의 제어 블록도이다. 본 실시형태에 있어서, 제어 시스템 (200) 의 장치 컨트롤러 (39) 와 표시 컨트롤러 (28) 는, 신호선을 개재하여 서로 정보를 교환할 수 있다. 또, 장치 컨트롤러 (39) 는, 글로벌 좌표 연산 장치 (23) 로부터 신호선을 개재하여 정보를 취득할 수 있다. 제어 시스템 (200) 내에서 정보를 전달하는 신호선은, CAN (Controller Area Network) 과 같은 차내 신호선이 예시된다. 본 실시형태에 있어서, 제어 시스템 (200) 은, 장치 컨트롤러 (39) 와, 표시 컨트롤러 (28) 가 별개의 장치이지만, 양자는 1 개의 장치로 실현되어도 된다.

표시 컨트롤러 (28) 는, 날끝 위치 산출부 (28A) 와, 목표 굴삭 지형 데이터 생성부 (28B) 와, 목표 시공 정보 격납부 (28C) 를 갖는다. 날끝 위치 산출부 (28A) 및 목표 굴삭 지형 데이터 생성부 (28B) 는, 기억부 (28M) 에 기억된 컴퓨터 프로그램을 처리부 (28P) 가 실행함으로써 실현된다. 목표 시공 정보 격납부 (28C) 는, 기억부 (28M) 의 기억 영역의 일부에 의해 실현된다.

날끝 위치 산출부 (28A) 는, 장치 컨트롤러 (39) 로부터 취득하는 위치 정보 (IPL) 에 기초하여, 상부 선회체 (3) 의 선회 중심축이 되는 z 축을 통과하는, 유압 셔블 (100) 의 선회 중심의 위치를 나타내는 선회 중심 위치 데이터 (XR) 를 생성한다. 날끝 위치 산출부 (28A) 가 장치 컨트롤러 (39) 로부터 취득하는 위치 정보 (IPL) 는, 기준 위치 데이터 (P1, P2) 에 근거하는 기준 위치 데이터 (P1c, P2c) 및 유압 셔블 (100) 의 자세각을 포함한다. 자세각은, 롤각 (θ4), 피치각 (θ5) 및 방위각 (θdc) 이다.

날끝 위치 산출부 (28A) 는, 선회 중심 위치 데이터 (XR) 와 작업기 (2) 의 경사각 (θ1, θ2, θ3) 과, 붐 (6) 의 길이 (L1), 아암 (7) 의 길이 (L2) 및 버킷 (8) 의 길이 (L3) 에 기초하여, 버킷 (8) 의 날끝 (8T) 의 현재 위치를 나타내는 버킷 날끝 위치 데이터 (S) 를 생성하고, 목표 굴삭 지형 데이터 생성부 (28B) 에 출력한다. 버킷 날끝 위치 데이터 (S) 는, 작업기 (2) 의 위치를 나타내는 정보이다. 본 실시형태에 있어서, 작업기 (2) 의 위치는, 날끝 위치, 즉 버킷 (8) 의 날끝 (8T) 의 3 차원 위치에 한정되는 것은 아니고, 작업기 (2) 의 특정 부분의 위치이면 된다. 예를 들어, 작업기 (2) 의 위치는, 버킷 (8) 의 뒷 부분의 위치여도 되고, 법면 버킷의 바닥 부분의 위치여도 되고, 작업기 (2) 의 어태치먼트를 장착하는 부분의 위치여도 된다.

목표 굴삭 지형 데이터 생성부 (28B) 는, 목표 시공 정보 격납부 (28C) 에 격납된 목표 시공 정보 (T) 와, 날끝 위치 산출부 (28A) 로부터의 버킷 날끝 위치 데이터 (S) 를 취득한다. 목표 굴삭 지형 데이터 생성부 (28B) 는, 차체 좌표계에 있어서 날끝 (8T) 의 현시점에 있어서의 날끝 위치를 통과하는 수직선과 목표 시공면의 교점을 굴삭 대상 위치로서 설정한다. 목표 굴삭 지형 데이터 생성부 (28B) 는, 목표 시공 정보 (T) 와 버킷 날끝 위치 데이터 (S) 에 기초하여, 목표 굴삭 지형 데이터 (U) 를 생성하고, 후술하는 장치 컨트롤러 (39) 의 처리부 (39P) 가 갖는 작업기 제어부 (39C) 에 출력한다.

장치 컨트롤러 (39) 의 처리부 (39P) 는, 자세각 연산부 (39A) 와, 위치 정보 연산부 (39B) 와, 작업기 제어부 (39C) 를 갖는다. 자세각 연산부 (39A), 위치 정보 연산부 (39B) 및 작업기 제어부 (39C) 는, 기억부 (39M) 에 기억된 컴퓨터 프로그램을 처리부 (39P) 가 실행함으로써 실현된다. 본 실시형태에 있어서, 작업기 제어부 (39C) 는, 장치 컨트롤러 (39) 와는 별개의 제어 장치여도 된다.

자세각 연산부 (39A) 에는, IMU (24) 의 검출값인 가속도 (a)(ax, ay, az) 및 각속도 (ω)(ωx, ωy, ωz), 즉 동작 정보 (MI) 와, 글로벌 좌표 연산 장치 (23) 의 검출값인 선회체 방위 데이터 (Q)(방위각 (θda)) 가 입력된다. 또, 처리부 (39P) 의 자세각 연산부 (39A) 및 위치 정보 연산부 (39B) 에는, 압력 센서 (66, 27PC) 의 검출값 STr, STd 가 입력된다.

글로벌 좌표 연산 장치 (23) 는, 전파의 수신 상태 또는 장치 컨트롤러 (39) 와의 통신 상태를 나타내는 정보인 상태 정보 (SR) 를 생성해, 장치 컨트롤러 (39) 의 처리부 (39P) 및 표시 컨트롤러 (28) 의 처리부 (28P) 에 출력한다. 상태 정보 (SR) 는, 글로벌 좌표 연산 장치 (23) 가 전파를 수신할 수 없게 된 경우, 전파의 수신 상태가 저하된 경우 또는 글로벌 좌표 연산 장치 (23) 와 장치 컨트롤러 (39) 의 통신에 불량이 발생한 경우 등에, 각각의 경우의 수신 상태 또는 통신 상태를 나타내는 정보를 갖는다. 수신 상태 또는 통신 상태를 나타내는 정보는, 글로벌 좌표 연산 장치 (23) 에 의한 측위 상태를 나타낸다. 측위 상태에는, 전술한 바와 같이, 측위의 정밀도가 좋은 상태 (Fix), 측위 불가능 상태 (비측위), 측위는 할 수 있지만 정보가 적은 상태 및 측위의 정밀도가 나쁜 상태 (Float, 단독 측위) 등을 들 수 있다. 이와 같이, 글로벌 좌표 연산 장치 (23) 는, RTK-GNSS 에 의한 측위에 문제가 발생했는지의 여부를 판정하는 측위 상태 판단 장치이다.

본 실시형태에 있어서, 측위 상태 판단 장치, 즉 글로벌 좌표 연산 장치 (23) 는, 측위의 정밀도가 좋은 상태 (Fix) 를 측위 상태가 정상이라고 판정한다. 또, 글로벌 좌표 연산 장치 (23) 는, 측위 불가능 상태 (비측위), 측위는 할 수 있지만 정보가 적은 상태 및 측위의 정밀도가 나쁜 상태 (Float, 단독 측위) 를, 측위 상태가 이상이라고 판정한다. 즉, 글로벌 좌표 연산 장치 (23) 는, 측위 상태가 Fix 인 경우를 정상으로서 판정하고, Fix 이외의 상태인 경우를 이상으로서 판정한다.

표시 컨트롤러 (28) 는, 상태 정보 (SR) 를 취득하면, 측위 상태에 따른 정보를, 도 2 에 나타내는 표시부 (29) 에 표시한다. 상태 정보 (SR) 가 측위 불가능의 상태를 나타내는 경우, 표시 컨트롤러 (28) 는, 도 2 에 나타내는 표시부 (29) 에, RTK-GNSS 에 의한 측위에 이상이 발생한 것을 표시한다.

자세각 연산부 (39A) 는, IMU (24) 의 검출값으로부터, 유압 셔블 (100) 의 롤각인 경사각 (θ4) 과, 유압 셔블 (100) 의 피치각인 경사각 (θ5) 을 구해, 위치 정보 연산부 (39B) 및 표시 컨트롤러 (28) 의 날끝 위치 산출부 (28A) 에 출력한다. 자세각 연산부 (39A) 는, IMU (24) 가 검출한 z 축 둘레의 각속도 (ω) 를 적분하여 방위각 (θdi) 을 구할 수 있다. 롤각 (θ4), 피치각 (θ5) 및 방위각 (θdi) 이 자세각이다.

자세각 연산부 (39A) 는, 위치 검출 장치인 글로벌 좌표 연산 장치 (23) 의 상태에 따라, 자신이 구한 방위각 (θdi) 또는 글로벌 좌표 연산 장치 (23) 로부터 취득한 방위각 (θda) 을 전환해, 표시 컨트롤러 (28) 의 날끝 위치 산출부 (28A) 또는 위치 정보 연산부 (39B) 에 방위각 (θdc) 으로서 출력한다. 요컨대, RTK-GNSS 에 의한 측위가 정상인 경우에는, 글로벌 좌표 연산 장치 (23) 로부터 취득한 방위각 (θda) 을 이용하여 버킷 날끝 위치 데이터 (S) 가 구해지고, RTK-GNSS 에 의한 측위가 이상인 경우에는, IMU (24) 가 검출한 z 축 둘레의 각속도 (ω) 를 적분하여 구한 방위각 (θdi) 을 이용하여 버킷 날끝 위치 데이터 (S) 가 구해진다. 또, 자세각 연산부 (39A) 로부터 표시 컨트롤러 (28) 로 보내지는 경사각 (θ4), 경사각 (θ5) 및 방위각 (θdc) 은, 유압 셔블 (100) 의 위치에 관련된 위치 정보 (IPL) 이다. 이하에 있어서, 경사각 (θ4) 을 적절히 롤각 (θ4) 이라 칭하고, 경사각 (θ5) 을 적절히 피치각 (θ5) 이라 칭한다.

본 실시형태에 있어서, 위치 정보 (IPL) 는, 전술한 바와 같이, 작업 기계인 유압 셔블 (100) 의 위치에 관련된 정보이다. 위치 정보 (IPL) 에는, 유압 셔블 (100) 의 위치 그 자체의 정보 외에, 유압 셔블 (100) 의 위치를 구하기 위해서 필요한 정보도 포함한다. 유압 셔블 (100) 의 위치 그 자체의 정보는, 기준 위치 데이터 (P1, P2) 및 버킷 날끝 위치 데이터 (S) 가 예시되고, 유압 셔블 (100) 의 위치를 구하기 위해서 필요한 정보는, 경사각 (θ4), 경사각 (θ5) 및 방위각 (θda, θdi 또는 θdc) 이 예시된다.

위치 정보 연산부 (39B) 는, 글로벌 좌표 연산 장치 (23) 로부터 취득한 기준 위치 데이터 (P1, P2) 와, IMU (24) 로부터 취득한 동작 정보 (MI) 를 이용하여, 기준 위치 데이터 (P1, P2) 가 나타내는 위치에 상당하는 위치를 구한다. 기준 위치 데이터 (P1, P2) 는, 제 1 위치의 정보이다. 이하에 있어서, 위치 정보 연산부 (39B) 가 기준 위치 데이터 (P1, P2) 및 동작 정보 (MI) 로부터 구한 위치를, 적절히 제 2 위치라고 칭한다. 제 2 위치의 정보는, 기준 위치 데이터 (P1i, P2i) 이다. 기준 위치 데이터 (P1i, P2i) 는, 위치 정보 연산부 (39B) 가 생성한다. 이하에 있어서, 글로벌 좌표 연산 장치 (23) 가 출력한 기준 위치 데이터 (P1, P2) 를 적절히, 제 1 기준 위치 데이터 (P1, P2) 라고 칭하고, 위치 정보 연산부 (39B) 에 의해 생성된 기준 위치 데이터 (P1i, P2i) 를 적절히, 제 2 기준 위치 데이터 (P1i, P2i) 라고 칭한다.

제 2 기준 위치 데이터 (P1i, P2i) 는, IMU (24) 의 검출값인 가속도 (a)(ax, ay, az) 및 각속도 (ω)(ωx, ωy, ωz) 로부터 자세각 연산부 (39A) 가 구한 롤각 (θ4) 및 피치각 (θ5) 과, 자세각 연산부 (39A) 로부터 출력되는 방위각 (θdc) 에 의해 구해진다. 이 방위각 (θdc) 은, 자세각 연산부 (39A) 가 글로벌 좌표 연산 장치 (23) 로부터 취득한 방위각 (θda) 또는 방위각 (θda) 에 선회 시의 각속도 (ω) 의 적분에 의해 얻어진 각도를 가산한 방위각이다. 이 경우, 글로벌 좌표 연산 장치 (23) 는, 제 1 기준 위치 데이터 (P1, P2) 로부터 방위각 (θda) 을 구해, 자세각 연산부 (39A) 에 출력한다. 선회 시의 각속도 (ω) 가 0 인 경우, 자세각 연산부 (39A) 로부터 출력되는 방위각 (θdc) 은, 글로벌 좌표 연산 장치 (23) 로부터 취득한 방위각 (θda) 과 동일하다. 이와 같이, 위치 정보 연산부 (39B) 는, 제 1 기준 위치 데이터 (P1, P2) 및 동작 정보 (MI) 를 이용하여 제 2 기준 위치 데이터 (P1i, P2i) 를 생성한다. 본 실시형태에 있어서, 자세각 연산부 (39A) 는, 글로벌 좌표 연산 장치 (23) 로부터 제 1 기준 위치 데이터 (P1, P2) 를 취득하고, 이것을 이용하여 방위각 (θda) 을 구해도 된다.

측위 위성의 배치, 전리층, 대류권 또는 GNSS 안테나 주변의 지형의 영향에 의해, 글로벌 좌표 연산 장치 (23) 에 의한 측위 결과가 불규칙한 경우가 있다. 측위 결과가 불규칙하면, 굴삭 제어에 있어서는 시공면이 물결쳐 설계면대로 시공되지 않을 가능성이 있다. 또, 측위 결과가 불규칙하면, 가이던스 화면에 표시되는 버킷 (8) 의 날끝이 흔들려, 오퍼레이터의 시인성이 저하될 가능성이 있다. 도 2 에 나타내는 조작 장치 (25) 가 파일럿압 방식인 경우, 좌조작 레버 (25L) 또는 우조작 레버 (25R) 에 유격이 발생해, 오퍼레이터가 위화감을 느낄 가능성도 있다.

글로벌 좌표 연산 장치 (23) 로부터 출력되는 제 1 기준 위치 데이터 (P1, P2) 에, 로우 패스 필터 처리 또는 이동 평균과 같은 평활화 처리를 실시함으로써, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 편차에 의한 영향을 저감하는 것이 생각된다. 유압 셔블 (100) 은, 굴삭 시에 있어서의 자세각의 변동에 의해 GNSS 안테나 (21, 22) 의 위치도 변동한다. 이 때문에, 제 1 기준 위치 데이터 (P1, P2) 가 직접 평활화 처리되면, 평활화 처리 후에 있어서의 GNSS 안테나 (21, 22) 의 위치 산출에 있어서 평활화 처리를 하는 것에 의한 시간 지연이 발생한다. 그 결과, 평활화 처리 후에 있어서의 GNSS 안테나 (21, 22) 의 위치가, 현실의 GNSS 안테나 (21, 22) 의 위치와는 상이할 가능성이 있다.

유압 셔블 (100) 이 작업을 위해서 가동되고 있는 시간에 있어서 절대 위치가 변화하지 않는 부동점이 유압 셔블 (100) 에 존재하고, GNSS 안테나 (21, 22) 의 위치와 부동점의 상대 관계를 알면, 제어 시스템 (200) 의 장치 컨트롤러 (39) 는 부동점의 절대 위치를 GNSS 안테나 (21, 22) 의 위치로부터 산출할 수 있다. 장치 컨트롤러 (39) 는, 부동점의 절대 위치에 평활화 처리를 적용하고, 편차가 저감된 부동점을 얻는다. 장치 컨트롤러 (39) 는, 평활화 처리를 적용한 후의 부동점의 절대 위치로부터 GNSS 안테나 (21, 22) 의 위치를 계산하면, 평활화 처리의 시간 지연이 영향을 주는 일 없이 RTK-GNSS 에 의한 측위의 편차를 저감할 수 있다.

실제로는 유압 셔블 (100) 의 엔진 (35) 이 가동되고 있는 한, 작업기 (2) 의 동작 등에 의해 진동이 발생하기 때문에, 부동점이라 간주할 수 있는 근사적인 위치가 유사 부동점으로서 선정된다. 제어 시스템 (200) 의 장치 컨트롤러 (39) 는, 선정된 유사 부동점을, 전술한 부동점과 동일하게 취급함으로써, 유사 부동점을 이용하여 RTK-GNSS 에 의한 측위를 백업할 수 있다. 유사 부동점을 부동점으로 간주할 수 있는 것은, 유압 셔블 (100) 이 이동하고 있지 않은 경우, 즉 도 1 에 나타내는 크롤러 벨트 (5a, 5b) 가 움직이고 있지 않은 경우이다.

본 실시형태에 있어서, 제어 시스템 (200), 보다 구체적으로는 제어 시스템 (200) 의 장치 컨트롤러 (39) 는, 전술한 유사 부동점에 평활화 처리를 실시하고, 평활화 처리를 실시한 유사 부동점을 이용하여, GNSS 안테나 (21, 22) 의 위치, 즉 제 2 위치를 구한다. 유사 부동점은, 후술하는 바와 같이, 유압 셔블 (100) 이 작업을 위해서 가동되고 있는 시간에 있어서, 시간의 경과에 의해 절대 위치가 변화하지 않는다고 간주할 수 있으므로, 평활화 처리에 의한 지연의 영향은 무시할 수 있다. 그 결과, 장치 컨트롤러 (39) 는, 평활화 처리 후에 있어서의 GNSS 안테나 (21, 22) 의 위치와, 현실의 GNSS 안테나 (21, 22) 의 위치를 일치시킬 수 있다. 이와 같이, 장치 컨트롤러 (39) 는, 유사 부동점에 평활화 처리를 실시함으로써, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 편차에 의한 영향을 저감할 수 있다. 그 결과, 장치 컨트롤러 (39) 는, 굴삭 제어를 사용한 시공 시의 정밀도 저하 및 가이던스 화면의 시인성 저하를 억제할 수 있다.

위치 정보 연산부 (39B) 는, 도 2 에 나타내는 좌조작 레버 (25L) 의 파일럿압을 검출하는 압력 센서 (66) 의 검출값 STr (전술한 선회 조작 신호 (MR) 에 대응) 및 주행용 레버 (25FL) 및 주행용 레버 (25FR) 의 파일럿압을 검출하는 압력 센서 (27PC) 의 검출값 STd (전술한 조작 신호 (MD) 에 대응) 를 취득한다. 위치 정보 연산부 (39B) 는, 취득한 검출값 STr, STd 에 기초하여, 평활화 처리를 실행할지의 여부를 비롯한 각종 판정을 실시한다.

다음으로, 유사 부동점에 대해 설명한다. 본 실시형태에 있어서, 유사 부동점은, 도 3 및 도 4 에 나타내는 유압 셔블 (100) 의 위치 PL 이다. 또한, 전술한 바와 같이 차체 좌표계의 원점으로서 위치 PL 을 정하고 있지만, 다른 위치에 차체 좌표계의 원점을 정해도 된다. 따라서, 유사 부동점은, 이하의 설명에 있어서 특정점이라 칭하는 경우도 있다. 작업 중의 유압 셔블 (100) 에 회전 (이하, 선회라고 칭하는 경우도 있다) 이 생기는 움직임이 있었을 경우, 그 회전의 지점 (支點) 은 움직이지 않기 때문에, 유사 부동점이 그 지점에 있으면, 제어 시스템 (200) 에 의해 구해지는 위치, 예를 들어 작업기 (2) 의 특정 부분의 위치 또는 버킷 (8) 의 날끝 (8T) 의 위치를 포함하는 작업기 (2) 의 위치의 오차가 가장 작아진다. 유사 부동점을 회전의 지점으로 할 수 없는 경우라도, 유사 부동점을 가능한 한 지점 근처에 설정하면, 제어 시스템 (200) 에 의해 구해지는 위치 (작업기 (2) 의 위치) 의 오차를 작게 할 수 있다.

상부 선회체 (3) 가 선회할 때의 지점은 선회 중심축, 즉 z 축이므로, 유사 부동점을 z 축 상으로 한다. 롤각 (θ4) 의 방향 및 피치각 (θ5) 의 방향에 있어서의 회전의 지점은 일정점은 아니지만, 반드시 유압 셔블 (100) 이 접지하는 면 상에 있다고 생각된다. 본 실시형태에 있어서, 전술한 바와 같이, 위치 PL 은, 선회체의 회전 중심축인 z 축과 주행 장치 (5) 가 접지하는 면에 상당하는 면의 교점이다. 본 실시형태에서는, 유사 부동점을, 유압 셔블 (100) 이 접지하는 면 상으로 함으로써, 작업 중의 유압 셔블 (100) 에 회전이 생기는 움직임이 있었을 경우라도, 유사 부동점은 부동이라고 생각된다. 이 때문에, RTK-GNSS 에 의한 측위의 편차가 발생했을 때에 있어서, 제어 시스템 (200) 에 의해 구해지는 위치, 보다 구체적으로는 GNSS 안테나 (21, 22) 의 절대 위치의 편차를 저감할 수 있다.

유압 셔블 (100) 은 여러 가지 작업을 실시할 수 있지만, 전술한 바와 같이 유압 셔블 (100) 에 회전이 생겨도 유사 부동점은 부동이라고 생각되는 경우로서, 예를 들어 법면의 시공 등의 작업이 있다. 이 경우, 주행 장치 (5) 는 정지한 채, 작업기 (2) 또는 상부 선회체 (3) 의 조작만으로 굴삭 또는 고르기가 실시되는 경우가 있다. 정보화 시공을 가능하게 하는 유압 셔블 (100) 을 이용하여, 이와 같은 법면 시공 등의 시공을 실시하는 경우, 제어 시스템 (200) 은, 유사 부동점 및 RTK-GNSS 에 의한 측위 결과를 이용하여 유압 셔블 (100) 의 제 2 위치, 구체적으로는 GNSS 안테나 (21, 22) 의 위치를 구한다. 이와 같이 함으로써, 제어 시스템 (200) 은, 굴삭 제어의 정밀도 저하 및 가이던스 화면의 시인성 저하를 억제할 수 있다.

＜유사 부동점 구하는 방법＞

유압 셔블 (100) 의 제어 시스템 (200), 구체적으로는 장치 컨트롤러 (39) 가, GNSS 안테나 (21, 22) 의 절대 위치로부터 유사 부동점을 구하는 방법, 및 유사 부동점으로부터 GNSS 안테나 (21, 22) 의 절대 위치를 다시 구하는 방법을 설명한다.

식 (1) 은, 차체 좌표계에 있어서의 위치 PL 의 위치 벡터와 GNSS 안테나 (21, 22) 의 위치 벡터의 차분을, 도 3 에 나타내는 현장 좌표계 (X, Y, Z) 에 있어서의 위치 PL 의 위치 벡터와 GNSS 안테나 (21, 22) 의 위치 벡터의 차분으로 변환하는 식이다. 식 (2) 는, 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 측정값 Ral 로부터 현장 좌표계에 있어서의 위치 PL 의 위치 벡터 Rfl 을 계산하는 식이고, 식 (1) 의 변형이다. 식 (3) 은, 현장 좌표계에 있어서의 위치 PL 의 위치 벡터 Rfl 로부터 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 측정값 Ral 을, 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 계산값 Ralc 를 구하는 식으로서 나타낸 것이다.

Rfl - Ral = Clb(Rfb - Rab)···(1)

Rfl = Clb(Rfb - Rab) ＋ Ral···(2)

Ralc = Clb(Rab - Rfb) ＋ Rfl···(3)

여기서,

Rfb : 차체 좌표계에 있어서의 위치 PL 의 위치 벡터의 교정값

Rab : 차체 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 교정값

Rfl : 현장 좌표계에 있어서의 위치 PL 의 위치 벡터의 계산값

Ral : 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 측정값

Ralc : 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 계산값

Clb : 차체 좌표계로부터 현장 좌표계로의 좌표 회전 행렬

교정값이란, 유압 셔블 (100) 의 각 위치 및 치수를 계측함으로써 얻어진, 위치 PL 및 GNSS 안테나 (21, 22) 의 위치의 값이고, 장치 컨트롤러 (39) 의 기억부 (39M) 및 표시 컨트롤러 (28) 의 기억부 (28M) 의 적어도 일방에 기억되어 있다. 교정값은, 유압 셔블 (100) 의 설계 치수에 근거하는 것이어도 되지만, 설계 치수는, 유압 셔블 (100) 마다 편차가 생길 가능성이 있다. 이 때문에, 교정값은, 계측 (캘리브레이션) 에 기초하여 얻어진 것이 바람직하다.

좌표 회전 행렬 Clb 는, 롤각 (θ4), 피치각 (θ5) 및 요각, 즉 방위각 (θd) 을 이용하여 식 (4) 와 같이 표현된다. 롤각 (θ4), 피치각 (θ5) 및 방위각 (θd) 은, IMU (24) 에 의해 검출된 x 축 둘레의 각속도 (ωx), y 축 둘레의 각속도 (ωy) 및 z 축 둘레의 각속도 (ωz) 를 자세각 연산부 (39A) 가 시간으로 적분됨으로써 구해진다. 식 (4) 중의 sx 는 sinθ4, sy 는 sinθ5, sz 는 sinθd, cx 는 cosθ4, cy 는 cosθ5, cz 는 cosθd 이다.

제어 시스템 (200) 은, 식 (2) 를 사용함으로써, 유사 부동점인 특정점 (본 실시형태에서는 위치 PL) 의 위치를 구할 수 있다. 또, 제어 시스템 (200) 은, 식 (3) 을 사용함으로써, 유사 부동점인 특정점의 위치를 이용하여, GNSS 안테나 (21, 22) 의 절대 위치, 즉 현장 좌표계에 있어서의 위치 또는 글로벌 좌표계에 있어서의 위치를 구할 수 있다. 제어 시스템 (200) 은, 식 (2) 및 식 (3) 을 사용함으로써, GNSS 안테나 (21, 22) 의 절대 위치를 구할 수 있다.

＜평활화 처리＞

본 실시형태에 있어서, 장치 컨트롤러 (39) 는, 유사 부동점인 특정점의 위치에 평활화 처리를 실시한다. 본 실시형태에 있어서, 평활화 처리에는, 예를 들어 식 (5) 로 나타내는 로우 패스 필터가 사용된다.

Rft = {(M - 1) × Rftpr ＋ Rfl}/M···(5)

식 (5) 중의 Rft 는, 금회의 제어 주기에 있어서의 로우 패스 필터의 출력이고, Rftpr 은 전회의 제어 주기에 있어서의 로우 패스 필터 (이하, 적절히 필터라고 칭한다) 의 출력이다. 이들은, 모두 특정점의 위치 벡터이다. M 은, 평균화 정수 (定數) 이다. 본 실시형태에 있어서, 평균화 정수 (M) 의 초기값은 1 이고, M 의 값이 설정값 Mmax 에 도달할 때까지, 1 회의 제어 주기가 종료할 때마다, M 은 1 씩 증가한다.

본 실시형태에 있어서, 장치 컨트롤러 (39) 는, 평활화 처리를 개시하면, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 을 기억부 (39M) 에 일시적으로 기억시킨다. 기억부 (39M) 는, 다음의 제어 주기의 필터 처치가 실행될 때까지 또는 실행 중의 평활화 처리가 리셋될 때까지, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 을 기억한다.

장치 컨트롤러 (39) 는, 식 (2) 를 이용하여, 특정점의 위치를 나타내는 위치 벡터 Rfl 을 구하고, 얻어진 위치 벡터 Rfl 을 식 (5) 에 부여한다. 장치 컨트롤러 (39) 는, 식 (5) 에 의해, 제어의 1 주기마다, 특정점의 위치 벡터 Rfl 에 평활화 처리, 구체적으로는 로우 패스 필터 처리를 실시한다. 장치 컨트롤러 (39) 는, 로우 패스 필터 처리 후, 평활화 처리 후에 있어서의 특정점의 위치 벡터로서, 금회의 제어 주기에 있어서의 로우 패스 필터의 출력 Rft 를 출력한다. 평활화 처리 후에 있어서의 특정점의 위치 벡터를, 이하에 있어서는 적절히 위치 벡터 Rft 라고 칭한다. 위치 벡터 Rft 는, 제 2 기준 위치 데이터 (P1i, P2i) 이다. 제 2 기준 위치 데이터 (P1i, P2i) 는, 평활화 처리에 의해 얻어진 정보이다. 이와 같이, 장치 컨트롤러 (39) 는, 식 (5) 로 나타내는 로우 패스 필터의 기능을 실현함으로써, 특정점의 위치에 평활화 처리를 실시하고, 평활화 처리 후의 특정점의 위치를 이용하여, 제 2 위치를 구한다.

장치 컨트롤러 (39) 는, 초회의 평활화 처리 또는 평활화 처리를 리셋한 후에 있어서, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 을 식 (5) 의 Rfl 로 하고, 평균화 정수 (M) 를 1 로 한다. 초회의 평활화 처리란, 장치 컨트롤러 (39) 가 평활화 처리를 개시하는 경우에, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 을 갖고 있지 않은 경우이다.

장치 컨트롤러 (39) 는, 어떠한 이유로 평활화 처리를 중단한 경우, 금회의 제어 주기에 있어서의 필터의 출력 Rft 를 출력하지 않고, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 및 전회의 제어 주기에 있어서의 평균화 정수 (M) 를 유지한다. 이 경우, 장치 컨트롤러 (39) 는, 기억부 (39M) 에, 전회의 제어 주기에 있어서의 필터의 출력 Rftpr 에 추가로, 전회의 제어 주기에 있어서의 평균화 정수 (M) 를 일시적으로 기억시킨다. 장치 컨트롤러 (39) 는, 중단하고 있던 평활화 처리를 재개하는 경우, 현재의 제어 주기에 있어서의 특정점의 위치 벡터 Rfl 과, 기억부 (39M) 에 기억하고 있던, 중단 전에 있어서의 필터의 출력 Rftpr 및 평균화 정수 (M) 를 식 (5) 에 부여한다. 이 처리에 의해, 장치 컨트롤러 (39) 는, 특정점의 위치 벡터 Rfl 을 평활화한다.

장치 컨트롤러 (39) 는, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 이상값을 회피하기 위해, 평활화 처리를 개시한 경우 (초회의 평활화 처리의 개시를 제외한다) 또는 평활화 처리의 중단으로부터 복귀하는 경우에, 판정 처리를 실행한다. 판정 처리를 실행할 때, 장치 컨트롤러 (39) 는, 식 (6) 을 이용하여 차분 ΔRfl 을 구한다. 식 (6) 중, Rfl 은, 금회의 제어 주기에 있어서의 특정점의 위치 벡터이고, Rftpr 은, 평활화 처리의 개시 전 또는 평활화 처리의 중단으로부터 복귀, 즉 재개하는 시점에 있어서 기억부 (39M) 에 기억되어 있는 필터의 출력이다.

ΔRfl =｜Rfl - Rftpr｜···(6)

판정 처리에 있어서, 장치 컨트롤러 (39) 는, 차분 ΔRfl 이 미리 정해진 임계값 미만이면, 금회의 제어 주기에 있어서의 특정점의 위치 벡터 Rfl 은 정상이라고 판정해, 금회의 제어 주기에 있어서의 특정점의 위치 벡터 Rfl 을 이용하여 평활화 처리를 실행한다. 장치 컨트롤러 (39) 는, 차분 ΔRfl 이 미리 정해진 임계값 이상이면, 금회의 제어 주기에 있어서의 특정점의 위치 벡터 Rfl 은 이상이라고 판정한다. 이와 같이, 이상으로 판정된 경우, 장치 컨트롤러 (39) 는, 금회의 제어 주기에 있어서의 특정점의 위치 벡터 Rfl 대신에, 기억부 (39M) 에 기억되어 있는 필터의 출력 Rftpr 을 이용하여, 식 (5) 의 필터의 출력 Rft 를 구한다. 이와 같은 처리에 의해, 장치 컨트롤러 (39) 는, 글로벌 좌표 연산 장치 (23) 의 측위 결과에 이상값이 발생한 경우에는, 이상값에 기인한 버킷 (8) 의 날끝 위치의 변동을 억제할 수 있다. 차분 ΔRfl 이 미리 정해진 임계값 이상이 되는 상태가, 미리 정해진 설정값 Nt 초 계속된 경우, 장치 컨트롤러 (39) 는, 타임 아웃 처리를 실행한다. 구체적으로는, 장치 컨트롤러 (39) 는, 평활화 처리를 리셋한다.

글로벌 좌표 연산 장치 (23) 의 측위 결과에 이상값이 발생하는 경우에는, 글로벌 좌표 연산 장치 (23) 가 출력하는 제 1 기준 위치 데이터 (P1, P2) 의 좌표값이 이상값을 나타내는 경우, 글로벌 좌표 연산 장치 (23) 와 장치 컨트롤러 (39) 사이에서 통신 에러가 발생한 경우 및 RTK-GNSS 에 의한 측위에 문제가 발생한 경우 중 어느 하나가 발생한 경우, 또는 복수가 동시에 발생한 경우가 포함된다. GNSS 안테나 (21, 22) 가 측위 위성으로부터의 전파를 수신할 수 없게 되거나 또는 전파를 수신하기 어려워지는 것에 의해, RTK-GNSS 에 의한 측위에 문제가 발생한다.

본 실시형태에 있어서, 로우 패스 필터는, 식 (5) 에 나타내는 것으로 한정되지 않는다. 평활화 처리는, 로우 패스 필터에는 한정되지 않고, 예를 들어 특정점의 위치를 이동 평균하는 처리여도 된다.

본 실시형태에 있어서, 장치 컨트롤러 (39) 는, RTK-GNSS 에 의한 측위가 정상인 경우에 평활화 처리를 실행한다. 장치 컨트롤러 (39) 가 특정점에 평활화 처리를 실시할 때, 장치 컨트롤러 (39) 가 갖는 자세각 연산부 (39A) 는, 롤각 (θ4), 피치각 (θ5) 및 방위각 (θdc) 을 구하고, 위치 정보 연산부 (39B) 및 표시 컨트롤러 (28) 의 날끝 위치 산출부 (28A) 에 출력한다. 글로벌 좌표 연산 장치 (23) 가 수신한 기준 위치 데이터 (P1, P2) 에 의해 방위각 (θdc), 즉 선회체 방위 데이터 (Q) 가 얻어지므로, 차체 좌표계에 대해 구해진 작업기 (2) 의 위치를, 현장 좌표계에 있어서의 작업기 (2) 의 위치로서 구할 수 있다.

도 6 은, 유압 셔블의 자세를 나타내는 평면도이다. 자세각 연산부 (39A) 에 의해 구해진 방위각 (θdc) 은, 현장 좌표계 (X, Y, Z) 의 Y 축에 대한 상부 선회체 (3) 의 전후축인 x 축의 기울기를 나타낸다. 방위각 (θdc) 에 의해 유압 셔블 (100) 의 방위 D1 이 정해진다.

위치 정보 연산부 (39B) 는, 자세각 연산부 (39A) 에 의해 구해진 롤각 (θ4), 피치각 (θ5) 및 방위각 (θdc) 으로부터 좌표 회전 행렬 Clb 를 구한다. 이 경우, 위치 정보 연산부 (39B) 는, 자세각 연산부 (39A) 에 의해 구해진 방위각 (θdc) 을 식 (4) 의 θd 에 부여하여, 좌표 회전 행렬 Clb 를 구한다. 또, 위치 정보 연산부 (39B) 는, RTK-GNSS 에 의한 측위가 정상인 상태에서 글로벌 좌표 연산 장치 (23) 로부터 취득한 기준 위치 데이터 (P1, P2) 로부터 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터의 측정값 Ral 을 구한다. 그리고, 위치 정보 연산부 (39B) 는, 구한 좌표 회전 행렬 Clb 와 위치 벡터의 측정값 Ral 을 식 (2) 에 부여하여, 현장 좌표계에 있어서의 위치 PL 의 위치 벡터 Rfl 을 구한다. 위치 벡터 Rfl 은 계산값이다.

위치 벡터 Rfl 이 얻어지면, 위치 정보 연산부 (39B) 는, 식 (5) 에 위치 벡터 Rfl 을 부여함으로써, 위치 벡터 Rfl 에 평활화 처리를 실시한다. 위치 정보 연산부 (39B) 는, 평활화 처리 후에 있어서의 위치 벡터 Rfl, 즉 로우 패스 필터의 출력 Rft 를 식 (3) 의 Rfl 로서 부여하고, 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터, 즉 제 2 기준 위치 데이터 (P1i, P2i) 를 구한다. 현장 좌표계에 있어서의 GNSS 안테나 (21, 22) 의 위치 벡터는, 식 (3) 에 나타내는 계산값 Ralc 이다. 위치 정보 연산부 (39B) 는, 제 2 기준 위치 데이터 (P1i, P2i) 를, 기준 위치 데이터 (P1c, P2c) 로서 표시 컨트롤러 (28) 의 날끝 위치 산출부 (28A) 에 출력한다.

다음으로, 장치 컨트롤러 (39) 의 처리부 (39P) 가 갖는 작업기 제어부 (39C) 에 대해 설명한다. 작업기 제어부 (39C) 는, 표시 컨트롤러 (28) 로부터 취득한 목표 굴삭 지형 데이터 (U) 에 기초하여, 작업기 (2) 가 목표 굴삭 지형 데이터 (U) 에 근접하는 속도를 제어하기 위한 제어 신호 (N) 를 생성한다. 작업기 제어부 (39C) 는, 생성한 제어 신호 (N) 를 제어 밸브 (27) 에 부여하여, 제어 밸브 (27) 를 개폐함으로써, 작업기 (2) 가 목표 굴삭 지형 데이터 (U) 에 근접하는 속도를 제어한다.

도 7 은, 실시형태 1 에 관련된 장치 컨트롤러 (39) 가 갖는 위치 정보 연산부 (39B) 를 나타내는 도면이다. 위치 정보 연산부 (39B) 는, 판정부 (40A) 와, 특정점 연산부 (40B) 와, 평활화 처리부 (40C) 와, 위치 계산부 (40D) 를 포함한다. 판정부 (40A) 는, 장치 컨트롤러 (39) 가 평활화 처리를 실행할지 정지시킬지, 실행 중의 평활화를 중단할지의 여부, 중단 중의 평활화 처리를 재개시킬지의 여부, 및 평활화 처리를 리셋할지에 대해 판정한다. 이들 판정은, 압력 센서 (66) 의 검출값 STr 및 압력 센서 (27PC) 의 검출값 STd 에 기초하여 실시된다.

특정점 연산부 (40B) 는, 식 (2) 를 이용하여 특정점의 위치 벡터 Rfl 을 구한다. 평활화 처리부 (40C) 는, 식 (5) 를 이용하여, 특정점 연산부 (40B) 에 의해 구해진 특정점의 위치 벡터 Rfl 에 평활화 처리를 실시한다. 위치 계산부 (40D) 는, 평활화 처리 후에 있어서의 위치 벡터 Rft 를 식 (3) 의 Rfl 에 부여하여 제 2 기준 위치 데이터 (P1i, P2i) 를 구하고, 이들을 기준 위치 데이터 (P1c, P2c) 로서 표시 컨트롤러 (28) 에 출력한다. 다음으로, 본 실시형태에 관련된 제어 시스템 (200) 이 평활화 처리를 실행해, 버킷 (8) 의 날끝 위치를 구하는 처리의 일례를 설명한다.

＜제어 시스템 (200) 의 처리의 일례＞

도 8 은, 실시형태 1 에 관련된 제어 시스템 (200) 의 처리의 일례를 나타내는 플로우 차트이다. 스텝 S101 에 있어서, 제어 시스템 (200) 의 장치 컨트롤러 (39) 가 갖는 위치 정보 연산부 (39B) 의 판정부 (40A) 는, 장치 컨트롤러 (39) 가 평활화 처리를 실행하기 위해서 필요한 실행 조건이 성립되었는지의 여부를 판정한다. 실행 조건은, RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 이 비주행, 또한 상부 선회체 (3) 가 비선회인 경우에 성립한다.

개시 조건이 성립한 경우 (스텝 S101, 예), 장치 컨트롤러 (39) 는, 스텝 S102 에 있어서 특정점을 구한다. 구체적으로는, 장치 컨트롤러 (39) 가 갖는 위치 정보 연산부 (39B) 의 특정점 연산부 (40B) 가, 특정점, 구체적으로는 특정점의 위치 벡터 Rfl 을 구한다. 스텝 S103 에 있어서, 장치 컨트롤러 (39) 는, 특정점 연산부 (40B) 에 의해 구해진 특정점의 위치 벡터 Rfl 에 평활화 처리를 실시한다. 스텝 S104 에 있어서, 장치 컨트롤러 (39) 가 갖는 위치 정보 연산부 (39B) 의 위치 계산부 (40D) 는, 평활화 처리 후에 있어서의 위치 벡터 Rfl 인 위치 벡터 Rft 를 이용하여 제 2 기준 위치 데이터 (P1i, P2i) 를 구한다. 그리고, 위치 계산부 (40D) 는, 구한 제 2 기준 위치 데이터 (P1i, P2i) 를 기준 위치 데이터 (P1c, P2c) 로서 표시 컨트롤러 (28) 에 출력한다.

스텝 S105 에 있어서, 표시 컨트롤러 (28) 의 처리부 (28P) 는, 장치 컨트롤러 (39) 로부터 취득한 유압 셔블 (100) 의 위치 정보 (IPL) 를 이용하여, 버킷 (8) 의 날끝 (8T) 의 3 차원 위치인 날끝 위치를 구한다. 구체적으로는, 처리부 (28P) 는, 날끝 위치를 나타내는 버킷 날끝 위치 데이터 (S) 를 생성한다. 위치 정보 (IPL) 는, 기준 위치 데이터 (P1c, P2c), 롤각 (θ4), 피치각 (θ5) 및 방위각 (θdc) 이다. 다음으로, 스텝 S101 로 돌아가 설명한다. 스텝 S101 에 있어서, 개시 조건이 성립하지 않은 경우 (스텝 S101, 아니오), 장치 컨트롤러 (39) 는 처리를 종료한다. 요컨대, 판정부 (40A) 는, RTK-GNSS 에 의한 측위가 정상인 것, 유압 셔블 (100) 이 비주행인 것, 및 상부 선회체 (3) 가 비선회인 것 중 어느 하나가 성립하고 있지 않은 경우, 실행 조건이 성립하고 있지 않다고 판정한다. 다음으로, 평활화 처리 상태의 천이에 대해 설명한다.

＜평활화 처리 상태의 천이＞

도 9 는, 평활화 처리 상태의 천이를 설명하기 위한 도면이다. 본 실시형태에 있어서, 평활화 처리는, 상태 1 (ON, 평활화 처리의 실행), 상태 2 (OFF, 평활화 처리의 정지), 상태 3 (중단, 실행 중의 평활화 처리의 중단) 및 상태 4 (리셋, 평활화 처리의 리셋) 의 4 개의 상태 사이를 천이한다.

장치 컨트롤러 (39) 는, RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 의 주행이 정지 (비주행), 또한 상부 선회체 (3) 가 비선회인 경우에, 평활화 처리 상태를 상태 1 로 천이시킨다. 즉, 상태 1 은, 전술한 실행 조건이 성립한 경우에 있어서의 평활화 처리 상태이다. 상태 1 에 있어서, 장치 컨트롤러 (39) 는, 제 2 위치의 정보인 제 2 기준 위치 데이터 (P1i, P2i) 를 이용하여 날끝 위치를 구한다.

장치 컨트롤러 (39) 는, 실행 조건이 성립하지 않은 경우, 보다 구체적으로는 유압 셔블이 주행하고 있는 경우에, 평활화 처리를 상태 1 로부터 상태 2 로 천이시킨다 (I). 즉, 장치 컨트롤러 (39) 는, 유압 셔블 (100) 이 주행하고 있을 때는, 제 2 위치를 구하는 처리, 즉 평활화 처리를 정지한다. 장치 컨트롤러 (39) 는, 상태 2 에 있어서 RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 이 비주행, 또한 상부 선회체 (3) 가 비선회인 경우에, 평활화 처리를 상태 2 로부터 상태 1 로 천이시킨다 (I).

장치 컨트롤러 (39) 는, 상태 1 에 있어서, 실행 조건이 성립하지 않게 된 경우, 보다 구체적으로는 RTK-GNSS 에 의한 측위가 이상이 된 경우 및 상부 선회체 (3) 가 선회하고 있는 경우 중 적어도 일방이 성립한 경우에, 평활화 처리를 상태 1 로부터 상태 3 으로 천이시킨다 (II). 상태 3 에 있어서, 장치 컨트롤러 (39) 는, 제 2 위치의 정보인 제 2 기준 위치 데이터 (P1i, P2i) 를 구하는 처리, 즉 평활화 처리를 중단한다. 장치 컨트롤러 (39) 는, 상태 3 에 있어서 RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 이 비주행, 또한 상부 선회체 (3) 가 정지, 즉 비선회인 경우에, 평활화 처리를 상태 3 으로부터 상태 1 로 천이시킨다 (II). 이 경우, 장치 컨트롤러 (39) 는, 중단하고 있던 평활화 처리를 재개한다. 장치 컨트롤러 (39) 는, 중단하고 있던 평활화 처리를 재개하는 경우, 평활화 처리를 중단하기 전에 얻어진 제 2 기준 위치 데이터 (P1i, P2i) 를 이용하여 날끝 위치를 구한다.

장치 컨트롤러 (39) 는, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 이상값을 회피하는 처리 또는 타임 아웃 처리를 실행하는 경우에, 평활화 처리를 상태 1 로부터 상태 4 로 천이시킨다 (III). 장치 컨트롤러 (39) 는, 상태 4 에 있어서 RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 이 비주행, 또한 상부 선회체 (3) 가 비선회, 또한 평활화 처리의 리셋이 완료된 경우에, 평활화 처리를 상태 4 로부터 상태 1 로 천이시킨다 (III).

장치 컨트롤러 (39) 는, 평활화 처리의 중단 중에 유압 셔블 (100) 이 주행을 개시한 경우에, 평활화 처리를 상태 3 으로부터 상태 2 로 천이시킨다 (IV). 즉, 장치 컨트롤러 (39) 는, 제 2 위치의 정보를 구하는 처리인 평활화 처리를 정지한다. 장치 컨트롤러 (39) 는, 상태 4 에 있어서 유압 셔블 (100) 이 주행, 또는 상부 선회체 (3) 가 선회, 또는 RTK-GNSS 에 의한 측위가 이상인 경우에, 평활화 처리를 상태 4 로부터 상태 2 로 천이시킨다 (V).

＜유압 셔블 (100) 상태 및 RTK-GNSS 에 의한 측위 상태의 판정＞

위치 정보 연산부 (39B) 가 평활화 처리 상태를 천이시킬 때에, 도 7 에 나타내는 위치 정보 연산부 (39B) 의 판정부 (40A) 는, 유압 셔블 (100) 상태 및 RTK-GNSS 에 의한 측위 상태를 판정한다. 판정부 (40A) 는, 압력 센서 (27PC) 가, 주행용 레버 (25FL) 및 주행용 레버 (25FR) 의 적어도 일방의 파일럿압을 검출한 경우, 유압 셔블 (100) 은 주행하고 있다고 판정한다. 상부 선회체 (3) 를 선회 조작하기 위한 조작 레버인 좌조작 레버 (25L) 가 좌우 어느 방향으로 조작되어, 압력 센서 (66) 가 파일럿압을 검출한 경우, 판정부 (40A) 는, 상부 선회체 (3) 는 선회하고 있다고 판정한다. 판정부 (40A) 는, 글로벌 좌표 연산 장치 (23) 가 생성한 상태 정보 (SR) 가, RTK-GNSS 에 의한 측위 상태가 이상인 것을 나타내는 경우, 측위 상태가 이상이라고 판정한다.

＜평활화 처리 상태를 천이시키는 처리＞

도 10 은, 장치 컨트롤러 (39) 가 평활화 처리 상태를 천이시키는 처리의 플로우 차트로서, 특히 평활화 처리의 중단에 관한 처리를 나타내는 것이다. 스텝 S201 에 있어서, 장치 컨트롤러 (39) 가 평활화 처리를 실행하고 있는 경우, 장치 컨트롤러 (39) 가 갖는 위치 정보 연산부 (39B) 의 판정부 (40A) 는, 평활화 처리를 중단하는 조건이 성립했는지의 여부를 판정한다. 평활화 처리를 중단하는 조건은, RTK-GNSS 에 의한 측위가 이상이 된 경우 및 상부 선회체 (3) 가 선회하고 있는 경우 중 적어도 일방이 성립한 경우이다. 평활화 처리를 중단하는 조건이 성립했다고 판정부 (40A) 가 판정한 경우 (스텝 S201, 예), 스텝 S202 에 있어서, 장치 컨트롤러 (39) 의 위치 정보 연산부 (39B) 는 평활화 처리를 중단한다 (II).

스텝 S203 에 있어서, 판정부 (40A) 는, 유압 셔블 (100) 이 주행하고 있는지의 여부를 판정한다. 유압 셔블 (100) 이 주행하고 있다고 판정부 (40A) 가 판정한 경우 (스텝 S203, 예), 스텝 S204 에 있어서, 위치 정보 연산부 (39B) 는, 중단하고 있던 평활화 처리를 정지시킨다 (IV). 다음으로, 스텝 S201 로 돌아가 설명한다. 평활화 처리를 중단하는 조건이 성립하지 않는다고 판정부 (40A) 가 판정한 경우 (스텝 S201, 아니오), 장치 컨트롤러 (39) 는 처리를 종료한다.

다음으로, 스텝 S203 으로 돌아가 설명한다. 유압 셔블 (100) 이 주행하고 있지 않다고 판정부 (40A) 가 판정한 경우 (스텝 S203, 아니오), 스텝 S205 에 있어서, 판정부 (40A) 는, 실행 조건이 성립했는지의 여부를 판정한다. 실행 조건이 성립했다고 판정부 (40A) 가 판정한 경우 (스텝 S205, 예), 스텝 S206 에 있어서, 위치 정보 연산부 (39B) 는, 평활화 처리를 중단했을 때의 정보를 이용하여, 평활화 처리를 실행한다 (II). 평활화 처리를 중단했을 때의 정보는, 기억부 (39M) 에 기억되어 있던 중단 전에 있어서의 필터의 출력 Rftpr 및 평균화 정수 (M) 이다. 실행 조건이 성립하지 않는다고 판정부 (40A) 가 판정한 경우 (스텝 S205, 아니오), 위치 정보 연산부 (39B) 는, 스텝 S202 로 돌아가 스텝 S202 이후의 처리를 실행한다.

도 11 은, 장치 컨트롤러 (39) 가 평활화 처리 상태를 천이시키는 처리의 플로우 차트로서, 특히 평활화 처리의 리셋에 관한 처리를 나타내는 것이다. 스텝 S301 에 있어서, 장치 컨트롤러 (39) 가 평활화 처리를 실행하고 있는 경우, 판정부 (40A) 는, 평활화 처리를 리셋하는 조건이 성립했는지의 여부를 판정한다. 평활화 처리를 리셋하는 조건은, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 이상값을 회피하는 처리로서, 이상값이 발생한 상태가, 미리 정해진 시간 (설정값 Nt 초) 계속되었을 때에, 타임 아웃 처리가 실행되는 경우이다. 평활화 처리를 리셋하는 조건이 성립했다고 판정부 (40A) 가 판정한 경우 (스텝 S301, 예), 스텝 S302 에 있어서, 장치 컨트롤러 (39) 의 위치 정보 연산부 (39B) 는 평활화 처리를 리셋한다 (III).

스텝 S303 에 있어서, 판정부 (40A) 는, 평활화 처리의 복귀 조건이 성립했는지의 여부를 판정한다. 평활화 처리의 복귀 조건은, 상태 4 에 있어서 RTK-GNSS 에 의한 측위가 정상, 또한 유압 셔블 (100) 이 비주행, 또한 상부 선회체 (3) 가 비선회, 또한 평활화 처리의 리셋이 완료된 경우이다. 평활화 처리의 복귀 조건이 성립했다고 판정부 (40A) 가 판정한 경우 (스텝 S303, 예), 스텝 S304 에 있어서, 위치 정보 연산부 (39B) 는, 평활화 처리를 실행한다 (III).

다음으로, 스텝 S301 로 돌아가 설명한다. 평활화 처리를 리셋하는 조건이 성립하지 않는다고 판정부 (40A) 가 판정한 경우 (스텝 S301, 아니오), 위치 정보 연산부 (39B) 는, 스텝 S305 에 있어서 실행 중의 평활화 처리를 계속한다. 다음으로, 스텝 S303 으로 돌아가 설명한다. 평활화 처리의 복귀 조건이 성립하고 있지 않다고 판정부 (40A) 가 판정한 경우 (스텝 S303, 아니오), 스텝 S306 에 있어서 평활화 처리를 정지한다 (V).

본 실시형태는, 글로벌 좌표 연산 장치 (23) 로부터의 제 1 위치의 정보, 즉 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터의 동작 정보 (MI) 를 이용하여, 유압 셔블 (100) 일부의 위치에 상당하는 제 2 위치를 구하고, 얻어진 제 2 위치의 정보를 이용하여, 작업기 (2) 의 적어도 일부의 위치를 구한다. 본 실시형태에서는, 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터 얻어지는 특정점, 즉 상부 선회체 (3) 의 회전 중심축인 z 축과 주행 장치 (5) 가 접지하는 면에 대응하는 면의 교점의 정보를 이용하여, 제 2 위치가 구해진다. 특정점은, 유압 셔블 (100) 의 작업 중에 있어서, 시간의 경과에 의해 절대 위치가 변화하지 않는다고 간주할 수 있으므로, 장치 컨트롤러 (39) 는, 특정점의 위치에 평활화 처리를 실시하고, 평활화 처리 후의 특정점의 위치를 이용하여 제 2 위치를 구해도, 평활화 처리에 의한 지연의 영향은 무시할 수 있다. 그 결과, 본 실시형태는, 제 2 위치와, 유압 셔블 (100) 일부의 위치를 일치시킬 수 있으므로, 작업 기계의 위치를 측위한 결과에 기초하여 정보화 시공을 실시하는 작업 기계에 있어서, 측위 결과의 편차가 정보화 시공에 주는 영향을 저감할 수 있다. 일례로는, 굴삭 제어를 이용한 시공 시의 정밀도 저하 및 가이던스 화면의 시인성 저하가 억제된다.

이상, 실시형태 1 에 대해 설명했지만, 실시형태 1 의 구성은, 이하의 실시형태에 있어서도 적절히 적용할 수 있다.

실시형태 2.

도 12 는, 실시형태 2 에 관련된 제어 시스템 (200a) 의 제어 블록도이다. 도 13 은, 실시형태 2 에 관련된 장치 컨트롤러 (39a) 가 갖는 위치 정보 연산부 (39Ba) 를 나타내는 도면이다. 제어 시스템 (200a) 은, 실시형태 1 의 제어 시스템 (200) 과 동일하지만, IMU (24) 의 검출값인 동작 정보 (MI) 가 장치 컨트롤러 (39a) 의 처리부 (39Pa) 가 갖는 위치 정보 연산부 (39Ba) 에 입력되는 점, 및 위치 정보 연산부 (39Ba) 의 구성이 상이하다. 장치 컨트롤러 (39a) 는, 실시형태 1 과 마찬가지로, CPU 등의 프로세서 및 RAM 및 ROM 등의 기억 장치에 의해 실현된다. 장치 컨트롤러 (39a) 의 처리부 (39Pa) 의 기능은, 그 기능을 실현하기 위한 컴퓨터 프로그램을 기억부 (39M) 로부터 처리부 (39Pa) 가 읽어들여 실행함으로써, 실현된다.

위치 정보 연산부 (39Ba) 는, 판정부 (40A) 와, 속도 계산부 (40E) 와, 평활화 처리부 (40Ba) 를 갖는다. 판정부 (40A) 는, 실시형태 1 에 관련된 장치 컨트롤러 (39) 의 판정부 (40A) 와 동일하므로 설명을 생략한다. 속도 계산부 (40E) 는, IMU (24) 로부터 취득한 동작 정보 (MI) 인 각속도 (ω) 및 IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계로부터 GNSS 안테나 (21, 22) 에 발생하는 속도 v 를 구한다. 요컨대, 어느 각속도 (ω) 가 생기고 있다는 것은, 차량 본체 (1) 에 움직임이 생기고 있고, IMU (24) 와 동일한 차량 본체 (1) 상에 설치되어 있는 GNSS 안테나 (21, 22) 가 움직이고 있다는 것이 된다. IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계 (예를 들어 설계 치수) 는 이미 알려져 있다. 이 때문에, 각속도 (ω) 및 상대 위치 관계로부터, GNSS 안테나 (21, 22) 의 움직임 (이동 거리) 이 구해지므로, 결과적으로 GNSS 안테나 (21, 22) 가 소정 시간에 이동한 거리, 즉 속도 v 가 구해진다. 시간 dt 는, 제어의 1 주기이다.

＜평활화 처리＞

본 실시형태에 있어서, 장치 컨트롤러 (39a), 보다 구체적으로는 평활화 처리부 (40Ba) 는, 속도 v 를 이용하여 제 1 위치, 보다 구체적으로는 제 1 위치의 정보인 기준 위치 데이터 (P1, P2) 에 평활화 처리를 실시한다. 본 실시형태에 있어서, 평활화 처리에는, 식 (7) 로 나타내는 로우 패스 필터가 사용된다.

P_i = {P ＋ (M - 1) × (P_i-1 ＋ vdt)}/M···(7)

식 (7) 중의 P 는, 금회의 제어 주기에 있어서의 제 1 위치의 정보인 제 1 기준 위치 데이터 (P1, P2) 이다. P_i-1 은, 전회의 제어 주기에 있어서의 로우 패스 필터의 출력, 즉 전회의 제어 주기에 있어서 평활화 처리가 실시된 제 1 위치의 정보인 제 1 기준 위치 데이터 (P1, P2) 이다. 제 1 기준 위치 데이터 (P1, P2) 는, 글로벌 좌표 연산 장치 (23) 가 출력한다. 식 (7) 중의 P_i 는, 금회의 제어 주기에 있어서의 로우 패스 필터의 출력이고, 제 2 위치의 정보인 제 2 기준 위치 데이터 (P1i, P2i) 이다. 식 (7) 중의 v 는, IMU (24) 가 검출한 각속도 (ω) 및 IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계로부터 속도 계산부 (40E) 에 의해 구해진 GNSS 안테나 (21, 22) 의 속도이다. 식 (7) 중의 dt 는, 장치 컨트롤러 (39a) 에 의한 제어의 1 주기이다. vdt 는, 장치 컨트롤러 (39a) 제어의 1 주기에 유압 셔블 (100) 이 이동한 거리이다. M 은, 평균화 정수이다. 평균화 정수 (M) 는, 실시형태 1 과 동일하다. 장치 컨트롤러 (39a) 의 평활화 처리부 (40Ba) 는, 식 (7) 로 나타내는 로우 패스 필터의 기능을 실현함으로써, 동작 정보 (MI) 를 이용하여 제 1 위치에 평활화 처리를 실시하여 제 2 위치를 구한다.

본 실시형태에 있어서, 장치 컨트롤러 (39a) 의 위치 정보 연산부 (39Ba) 가 갖는 속도 계산부 (40E) 는, 제어의 1 주기마다 속도 v 를 구하고, 평활화 처리부 (40Ba) 는, 속도 v 를 이용하여 제어의 1 주기마다 제 1 기준 위치 데이터 (P1, P2) 에 평활화 처리를 실시한다. 속도 v 는, IMU (24) 의 검출값인 각속도 (ω) 및 IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계로부터 구해진다. 장치 컨트롤러 (39a) 의 위치 정보 연산부 (39Ba) 는, 글로벌 좌표 연산 장치 (23) 로부터 출력된 제 1 기준 위치 데이터 (P1, P2) 에, IMU (24) 의 검출값을 이용하여 평활화 처리를 실시한다. 이와 같이 위치 정보 연산부 (39Ba) 는, IMU (24) 의 검출값을 이용하여 평활화 처리를 실시한다. 이 때문에, 위치 정보 연산부 (39Ba) 는, 굴삭 시에 있어서의 유압 셔블 (100) 의 자세 변화가 GNSS 안테나 (21, 22) 의 위치의 변화에 주는 영향을, IMU (24) 의 검출값에 의해 반영시켜, 제 2 기준 위치 데이터 (P1i, P2i) 를 구할 수 있다. 그 결과, 장치 컨트롤러 (39a) 는, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 편차에 의한 영향을 저감할 수 있기 때문에, 굴삭 제어를 사용한 시공 시의 정밀도 저하 및 가이던스 화면의 시인성 저하를 억제할 수 있다.

＜제어 시스템 (200a) 의 처리의 일례＞

도 14 는, 실시형태 2 에 관련된 제어 시스템 (200a) 의 처리의 일례를 나타내는 플로우 차트이다. 스텝 S401 에 있어서, 제어 시스템 (200a) 의 장치 컨트롤러 (39a) 가 갖는 위치 정보 연산부 (39Ba) 의 판정부 (40A) 는, 장치 컨트롤러 (39a) 가 평활화 처리를 실행하기 위해서 필요한 실행 조건이 성립되었는지의 여부를 판정한다. 실행 조건은, 실시형태 1 에서 설명한 바와 같다.

개시 조건이 성립한 경우 (스텝 S401, 예), 장치 컨트롤러 (39a) 는, 스텝 S402 에 있어서 IMU (24) 로부터 각속도 (ω) 를 취득하고, 글로벌 좌표 연산 장치 (23) 로부터 제 1 기준 위치 데이터 (P1, P2) 를 취득한다. 스텝 S403 에 있어서, 장치 컨트롤러 (39a) 의 위치 정보 연산부 (39Ba) 가 갖는 평활화 처리부 (40Ba) 는, 속도 v 를 이용하여 제 1 기준 위치 데이터 (P1, P2) 에 평활화 처리를 실시한다. 속도 v 는, 속도 계산부 (40E) 에 의해, 각속도 (ω) 및 IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계로부터 구해진다. IMU (24) 와 GNSS 안테나 (21, 22) 의 상대 위치 관계는, 교정값으로서 계측 (캘리브레이션) 에 기초하여 얻어진 것이 바람직하다.

스텝 S404 에 있어서, 장치 컨트롤러 (39a) 가 갖는 평활화 처리부 (40Ba) 는, 필터 출력, 즉 제 2 기준 위치 데이터 (P1i, P2i) 를 기준 위치 데이터 (P1c, P2c) 로서 표시 컨트롤러 (28) 에 출력한다. 스텝 S405 에 있어서, 표시 컨트롤러 (28) 의 처리부 (28P) 는, 장치 컨트롤러 (39) 로부터 취득한 유압 셔블 (100) 의 위치 정보 (IPL) 를 이용하여, 버킷 (8) 의 날끝 (8T) 의 3 차원 위치인 날끝 위치를 구한다. 다음으로, 스텝 S401 로 돌아가 설명한다. 스텝 S401 에 있어서, 개시 조건이 성립하지 않은 경우 (스텝 S401, 아니오), 장치 컨트롤러 (39a) 는 처리를 종료한다. 본 실시형태에 있어서, 평활화 처리 상태를 천이시키는 처리에 대해서는, 실시형태 1 과 동일하다.

본 실시형태는, 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터의 동작 정보 (MI) 를 이용하여 제 2 위치를 구하고, 얻어진 제 2 위치의 정보를 이용하여, 작업기 (2) 의 적어도 일부의 위치를 구한다. 실시형태 1 에서는, 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터 얻어지는 특정점에 정보를 이용하여 제 2 위치가 구해졌지만, 본 실시형태는, IMU (24) 에 의해 검출된 동작 정보 (MI), 보다 구체적으로는 각속도 등으로부터 속도를 구하고, 얻어진 속도를 이용하여 제 1 위치의 정보인 제 1 기준 위치 데이터 (P1, P2) 에 평활화 처리를 실시해, 제 2 위치를 구한다. 본 실시형태는, 굴삭 시에 있어서의 유압 셔블 (100) 의 자세 변화가 GNSS 안테나 (21, 22) 의 위치 변화에 주는 영향을, IMU (24) 의 검출값, 구체적으로는 각속도에 의해 반영시켜, 제 2 위치를 구할 수 있다. 그 결과, 본 실시형태는, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 편차가 정보화 시공에 주는 영향을 저감할 수 있다.

이상, 실시형태 2 에 대해 설명했지만, 실시형태 2 의 구성은, 이하의 실시형태에 있어서도 적절히 적용할 수 있다.

실시형태 3.

도 15 는, 실시형태 3 에 관련된 제어 시스템 (200b) 의 제어 블록도이다. 도 16 은, 실시형태 3 에 관련된 장치 컨트롤러 (39b) 가 갖는 위치·자세 정보 연산부 (39Bb) 를 나타내는 도면이다. 실시형태 3 은, 위치 자세 연산 수법에 칼만 필터를 사용한다. 제어 시스템 (200b) 은, 실시형태 1 의 제어 시스템 (200) 과 동일하지만, 위치·자세 정보 연산부 (39Bb) 를 갖는 점, 및 IMU (24) 의 검출값인 동작 정보 (MI) 가 장치 컨트롤러 (39b) 의 처리부 (39Pb) 가 갖는 위치·자세 정보 연산부 (39Bb) 에 입력되는 점이 상이하다. 장치 컨트롤러 (39b) 는, 실시형태 1 과 마찬가지로, CPU 등의 프로세서 및 RAM 및 ROM 등의 기억 장치에 의해 실현된다. 장치 컨트롤러 (39b) 의 처리부 (39Pb) 의 기능은, 그 기능을 실현하기 위한 컴퓨터 프로그램을 기억부 (39M) 로부터 처리부 (39Pb) 가 읽어들여 실행함으로써, 실현된다.

위치·자세 정보 연산부 (39Bb) 는, 위치 추정부 (40F) 와, 오차 연산부 (40Bb) 와, 선택부 (40Ab) 와, 특정점 연산부 (40B) 를 갖는다. 위치 추정부 (40F) 는, IMU (24) 에 의해 검출된 동작 정보 (MI) 를 이용하여 유압 셔블 (100) 의 위치, 속도, 방위각 및 자세각과 같은 위치 자세 추정값을 추정한다. 유압 셔블 (100) 의 위치는, GNSS 안테나 (21, 22) 의 위치이다. 본 실시형태에 있어서, 위치 추정부 (40F) 는, 유압 셔블 (100) 의 위치, 속도, 방위각 및 자세각과 같은 위치 자세값을 추정하여 위치 자세 추정값을 구할 때에, 관성 항법을 사용한다. 위치 추정부 (40F) 는, 추정에 의해 얻어진 유압 셔블 (100) 의 위치를 제 2 위치, 구체적으로는 제 2 기준 위치 데이터 (P1i, P2i) 로서 출력한다. 또, 위치 추정부 (40F) 는, 오차 연산부 (40Bb) 가 출력한 오차를 이용하여 제 2 위치를 보정한다.

오차 연산부 (40Bb) 는, 제 1 기준 위치 데이터 (P1, P2), 유압 셔블 (100) 의 속도 V, 방위각 (θda), 특정점 (본 실시형태에서는 위치 PL) 및 정지 시의 각속도 (ω) = 0 중 적어도 1 개를 관측값으로서 사용하여, 위치 추정부 (40F) 에 의해 추정된 유압 셔블 (100) 의 위치, 속도, 방위각 및 자세각 또는 그것들이 갖는 오차를 구하고, 위치 추정부 (40F) 에 출력한다. 즉, 오차 연산부 (40Bb) 는, 위치 자세 추정값을 보정하기 위한 정보를 위치 추정부 (40F) 에 송신한다. 위치 추정부 (40F) 는, 위치 자세 추정값을 보정하기 위한 정보를 이용하여, 먼저 구해져 있던 위치 자세 추정값의 오차를 보정한다. 그 후, 위치 추정부 (40F) 는, 보정된 위치 자세 추정값으로부터 제 2 위치 데이터를 산출한다. 오차 연산부 (40Bb) 가 사용하는 관측값 중, 제 1 기준 위치 데이터 (P1, P2), 유압 셔블 (100) 의 속도 V, 방위각 (θda) 은, 글로벌 좌표 연산 장치 (23) 로부터 얻어진다. 오차 연산부 (40Bb) 는, 글로벌 좌표 연산 장치 (23) 로부터 얻은 글로벌 좌표계의 제 1 기준 위치 데이터 (P1, P2) 및 속도 V 를, 현장 좌표계로 변환한다. 특정점, 본 실시형태에서는 위치 PL 및 특정점의 위치 벡터 Rfl 은, 특정점 연산부 (40B) 가 구한다. 본 실시형태에 있어서, 오차 연산부 (40Bb) 는, 칼만 필터를 포함한다.

선택부 (40Ab) 는, 유압 셔블 (100) 의 상태에 따라, 오차 연산부 (40Bb) 가 사용하는 관측값을 선택한다. 유압 셔블 (100) 의 상태에는, 정정 (靜定) 상태, 비정정 상태, 상부 선회체 (3) 가 선회하고 있는 상태, 유압 셔블 (100) 이 주행하고 있는 상태가 포함된다.

도 17 은, 실시형태 3 에 관련된 장치 컨트롤러 (39b) 가 갖는 위치·자세 정보 연산부 (39Bb) 의 제어 블록도이다. 위치 추정부 (40F) 는, IMU (24) 에 의해 측정된 각속도를 적분해 차체의 자세각의 추정값 및 방위각의 추정을 산출한다. 위치 추정부 (40F) 는, IMU (24) 에 의해 측정된 가속도를 적분해, 유압 셔블 (100) 의 추정 속도 및 추정 위치를 산출한다.

선택부 (40Ab) 는, 거동 검출부 (42a) 와, 판정기 (42b) 와, 선택기 (42c) 를 포함한다. 거동 검출부 (42a) 에는, 차체 정보 (IFb) 와, IMU (24) 의 검출값인 각속도 (ω) 및 가속도 (a) 가 입력된다. 본 실시형태에 있어서, 차체 정보 (IFb) 는, 도 2 에 나타내는 좌조작 레버 (25L) 및 우조작 레버 (25R) 의 파일럿압을 검출하는 압력 센서 (66) 의 검출값 STr 및 주행용 레버 (25FL) 및 주행용 레버 (25FR) 의 파일럿압을 검출하는 압력 센서 (27PC) 의 검출값 STd 이다. 거동 검출부 (42a) 는, 각속도 (ω), 가속도 (a) 및 차체 정보 (IFb) 를 이용하여, 유압 셔블 (100) 의 상태를 검출하고, 검출 결과에 따른 신호를, 판정기 (42b) 에 출력한다.

판정기 (42b) 에는, 거동 검출부 (42a) 로부터의 신호, 차체 정보 (IFb) 및 글로벌 좌표 연산 장치 (23) 가 출력한 상태 정보 (SR) 가 입력된다. 판정기 (42b) 는, 입력된 정보에 기초하여 선택기 (42c) 를 동작시켜, 오차 연산부 (40Bb) 에 입력하는 관측값을 선택한다. 선택기 (42c) 에는, 관측값, 즉 글로벌 좌표 연산 장치 (23) 가 수신한 제 1 기준 위치 데이터 (P1, P2), 유압 셔블 (100) 의 속도 Vc, 방위각 (θda), 특정점 연산부 (40B) 가 구한 특정점의 위치 벡터 Rfl 및 유압 셔블 (100) 이 선회하고 있지 않을 때의 각속도 (ω) = 0 이 입력된다. 글로벌 좌표 연산 장치 (23) 는, 측위 위성으로부터의 전파 (신호) 를 이용하여, 제 1 기준 위치 데이터 (P1, P2) 를 구함과 동시에, 유압 셔블 (100) 의 속도 Vc 를 구한다. 방위각 (θda) 은, 글로벌 좌표 연산 장치 (23) 가 제 1 기준 위치 데이터 (P1, P2) 로부터 구한다.

오차 연산부 (40Bb) 는, 선택부 (40Ab) 의 선택기 (42c) 로부터, 유압 셔블 (100) 의 상태에 따른 관측값이 입력된다. 오차 연산부 (40Bb) 는, 칼만 필터를 포함한다. 오차 연산부 (40Bb) 는, 관측 벡터를 취득하고, 상태 방정식으로 사전에 예측한 상태 벡터를 보정하고, 사후의 추정값을 구한다. 이 처리를 반복함으로써 보다 확실한 추정값을 구한다. 식 (8) 은 칼만 필터의 계산식이다. X_k｜k (X 는 굵은 글씨) 는 사후 추정에 의해 얻어진 상태 벡터, X_k｜k-1 (X 는 굵은 글씨) 은 사전 추정에 의해 얻어진 상태 벡터, K (K 는 굵은 글씨) 는 칼만 게인, z_k (z 는 굵은 글씨) 는 관측 벡터, H_k (H 는 굵은 글씨) 는 관측 행렬이다. 오차 연산부 (40Bb) 는, 식 (8) 을 이용하여 사후 추정에 의해 얻어진 상태 벡터를 구한다.

칼만 게인 (K) (K 는 굵은 글씨) 은, 식 (9) 로 구해진다. P_k｜k-1 (P 는 굵은 글씨) 은 추정 오차의 공분산, R_k (R 은 굵은 글씨) 는 관측 오차의 공분산이다. 추정 오차의 공분산 P_k｜k-1 및 관측 오차의 공분산 R_k 의 설정에 의해, 상태 벡터 X_k｜k (X 는 굵은 글씨) 및 관측 벡터 z_k (z 는 굵은 글씨) 의 가중치가 결정된다.

상태 벡터에 대해 설명한다. 예측값을 통상자체로 나타내고, 보정값을 이탤릭체로 나타내면, 에러 상태 벡터는 식 (10) 부터 식 (14) 로 정의된다. 여기서,

δΨⁿ _nb (Ψ 는 굵은 글씨) : 항법 좌표계에 있어서의 유압 셔블 (100) 의 각도 오차 벡터 [rad]

δωb (ωb 는 굵은 글씨) : IMU (24) 의 각속도 바이어스 오차 벡터 [rad/s]

δP^l _lb (P 는 굵은 글씨) : 현장 좌표계에 있어서의 현장 좌표계를 기준으로 한 차체 좌표 원점의 위치 오차 벡터 [m]

δVⁿ _eb (V 는 굵은 글씨) : 현장 좌표계에 있어서의 ECEF (Earth Centered Earth Fixed : 지구 중심 지구 고정) 좌표계를 기준으로 한 차체 좌표 원점의 속도 오차 벡터 [m/s]

δAb (Ab 는 굵은 글씨) : IMU (24) 의 가속도 바이어스 오차 벡터 [m/s²]

C_b ⁿ (C 는 굵은 글씨) : 차체 좌표계로부터 항법 좌표계로의 좌표 회전 행렬

Ψⁿ _nb (Ψ 는 굵은 글씨) : 항법 좌표계에 있어서의 유압 셔블 (100) 의 각도 벡터 [rad]

ωb (ωb 는 굵은 글씨) : IMU (24) 의 각속도 벡터 [rad/s]

Ab (Ab 는 굵은 글씨) : IMU (24) 의 가속도 벡터 [m/s²]

P^l _lb (P 는 굵은 글씨) : 현장 좌표계에 있어서의 현장 좌표계를 기준으로 한 차체 좌표 원점의 위치 벡터 [m]

Vⁿ _eb (V 는 굵은 글씨) : 현장 좌표계에 있어서의 ECEF 좌표계를 기준으로 한 차체 좌표 원점의 속도 벡터 [m/s]

I : 단위 행렬

상태 방정식에 대해 설명한다. 식 (11) 부터 식 (19) 는, 에러 상태 모델에 근거하는 상태 방정식이다. 잡음항은 생략되어 있다. 여기서,

ωⁿ _ie (ω 는 굵은 글씨) : 항법 좌표계에 있어서의 지구 자전 속도 벡터 [rad/s]

Aⁿ _ib (A 는 굵은 글씨) : 항법 좌표계에 있어서의 관성 좌표계를 기준으로 한 차체 좌표 원점의 가속도 벡터 [m/s²]

이탤릭체로 표시한 관측값에 대한 관측 방정식을 식 (20) 부터 식 (24) 에 나타낸다. 잡음항은 생략되어 있다. 식 (20) 은 GNSS 안테나 (21, 22) 의 위치의 관측 방정식이고, 식 (21) 은 GNSS 안테나 (21, 22) 의 속도의 관측 방정식이다. 식 (22) 는, 특정점의 속도의 관측 방정식이다. 정지 시 및 선회 시에 사용된다. 식 (23) 은, 정지 시에 있어서의 유압 셔블 (100) 의 가속도의 관측 방정식이다. 식 (23) 은, 유압 셔블 (100) 의 비선회 시에 있어서의 GPS 컴퍼스에 의한 방위각의 관측 방정식이다. 여기서,

P^l _la (이탤릭체) : 현장 좌표계에 있어서의 현장 좌표계를 기준으로 한 GNSS 안테나 (21, 22) 의 위치 [m]

Vⁿ _ea (이탤릭체) : 항법 좌표계에 있어서의 ECEF 좌표계를 기준으로 한 GNSS 안테나 (21, 22) 의 속도 [m/s]

Vⁿ _eq (이탤릭체) : 항법 좌표계에 있어서의 ECEF 좌표계를 기준으로 한 특정점의 속도 [m/s]

Ψ_z (이탤릭체) : GPS 컴퍼스에 의한 유압 셔블 (100) 의 방위각 측정값 [rad]

δC_b ^nT : 자세각 오차의 회전 행렬 (δC_b ^nT = I - [δΨⁿ _nb ^{^}])

δΨ_z : 유압 셔블 (100) 의 방위각의 오차 (δΨⁿ _nb 의 Z 성분) [rad]

R^b _ba : 차체 좌표계에 있어서의 차체 좌표계를 기준으로 한 GNSS 안테나 (21, 22) 의 위치 [m]

R^b _bq : 차체 좌표계에 있어서의 차체 좌표계를 기준으로 한 특정점의 위치 [m]

ω^b _nb : 차체 좌표계에 있어서의 항법 좌표계를 기준으로 한 차체 좌표계의 각속도 벡터 [rad/s]

Ψ_z : 관성 항법 연산에 의한 유압 셔블 (100) 의 방위각 [rad]

전술한 식 (10) 중의 [δΨⁿ _nb ^＾](Ψ 는 굵은 글씨) 와, 식 (15) 및 식 (18) 중의 [ωⁿ _ie ^＾](ω 는 굵은 글씨) 와, 식 (18) 중의 [Aⁿ _ib](A 는 굵은 글씨) 에 대해 설명한다. δΨⁿ _nb (Ψ 는 굵은 글씨), ωⁿ _ie (ω 는 굵은 글씨) 및 Aⁿ _ib (A 는 굵은 글씨) 가, (α, β, γ) 의 3 차원 좌표계에 있어서의 벡터 또는 롤 방향, 피치 방향, 요 방향의 벡터라고 한다. 롤 방향은 α 축 둘레의 방향, 피치 방향은 β 축 둘레의 방향, 요 방향은 γ 축 둘레의 방향이다. 이 경우, [δΨⁿ _nb ^＾](Ψ 는 굵은 글씨) 는 식 (25) 로, [ωⁿ _ie ^＾](ω 는 굵은 글씨) 는 식 (26) 으로, [Aⁿ _ib](A 는 굵은 글씨) 는 식 (27) 로 나타낸다. δΨ_α, δΨ_β, δΨ_γ 는, 이 순서로, α 축 둘레, β 축 둘레, γ 축 둘레에 있어서의 유압 셔블 (100) 의 각도 오차이다. ωi_α, ωi_β, ωi_γ 는, 이 순서로, α 축 둘레, β 축 둘레, γ 축 둘레에 있어서의 지구 자전 속도이다. Ai_α, Ai_β, Ai_γ 는, 이 순서로, α 축 둘레, β 축 둘레, γ 축 둘레에 있어서의 유압 셔블 (100) 의 차체좌 원점의 가속도이다.

오차 연산부 (40Bb) 는, 사전 추정에 있어서, 식 (15) 부터 식 (19) 로 나타내는 상태 방정식을 품으로써, 식 (10) 부터 식 (14) 로 나타내는 상태 벡터의 사전 추정값, 즉 상태 벡터 X_k｜k-1 을 구할 수 있다. 본 실시형태에 있어서, 상태 벡터는, 항법 좌표계에 있어서의 유압 셔블 (100) 의 각도 벡터 Ψⁿ _nb (Ψ 는 굵은 글씨), IMU (24) 의 각속도 벡터 ωb (ωb 는 굵은 글씨), IMU (24) 의 가속도 벡터 Ab (Ab 는 굵은 글씨), 현장 좌표계에 있어서의 현장 좌표계를 기준으로 한 차체 좌표 원점의 위치 벡터 P^l _lb (P 는 굵은 글씨) 및 현장 좌표계에 있어서의 ECEF 좌표계를 기준으로 한 차체 좌표 원점의 속도 벡터 δV^l _eb (V 는 굵은 글씨) 이다. 오차 연산부 (40Bb) 는, 사전 추정에 의해 상태 벡터 X_k｜k-1 을 구하는 경우, 위치 추정부 (40F) 가 구한 자세각 (롤각 (θ4), 피치각 (θ5), 방위각 (θdc)), 제 2 기준 위치 데이터 (P1i, P2i) 및 속도 V 를 취득하고, 위치, 속도 및 자세의 예측값으로서 사용한다.

관측 행렬은, 관측 방정식의 야코비안에 의해 얻어진다. 오차 연산부 (40Bb) 는, 식 (20) 부터 식 (24) 를 이용하여 관측 벡터 z_k (z 는 굵은 글씨) 를 구하고, 또 식 (9) 로부터 칼만 게인 (K) (K 는 굵은 글씨) 을 구한다. 그리고, 오차 연산부 (40Bb) 는, 식 (8) 에, 사전 추정에 의한 상태 벡터 X_k｜k-1, 관측 벡터 z_k (z 는 굵은 글씨) 및 칼만 게인 (K) (K 는 굵은 글씨) 을 부여해 품으로써, 사후의 추정값인 상태 벡터 X_k｜k 를 구할 수 있다.

각속도 바이어스 오차 (FBa) 의 벡터는, IMU (24) 의 각속도 바이어스 오차 벡터 δωb (ωb 는 굵은 글씨) 이다. 차체 각도 오차 (FBb) 의 벡터는, 항법 좌표계에 있어서의 유압 셔블 (100) 의 각도 오차 벡터 δΨⁿ _nb (Ψ 는 굵은 글씨) 이다. 차체 속도 오차 (FBc) 의 벡터는, 현장 좌표계에 있어서의 ECEF 좌표계를 기준으로 한 차체 좌표 원점의 속도 오차 벡터 δV^l _eb (V 는 굵은 글씨) 이다. 차체 위치 오차 (FBd) 의 벡터는, 현장 좌표계에 있어서의 현장 좌표계를 기준으로 한 차체 좌표 원점의 위치 오차 벡터 δP^l _lb (P 는 굵은 글씨) 이다. 가속도 바이어스 오차 (FBe) 의 벡터는, IMU (24) 의 가속도 바이어스 오차 벡터 δAb (Ab 는 굵은 글씨) 이다.

사후의 추정에 의해 얻어진 상태 벡터 X_k｜k 는, 전술한 바와 같이, 각속도 바이어스 오차 (FBa), 차체 각도 오차 (FBb), 차체 속도 오차 (FBc), 차체 위치 오차 (FBd) 및 가속도 바이어스 오차 (FBe) 에 대응한다. 오차 연산부 (40Bb) 는, 사후의 추정에 의해 구한 상태 벡터 X_k｜k 를 위치 추정부 (40F) 에 부여한다. 위치 추정부 (40F) 는, 오차 연산부 (40Bb) 로부터 취득한 상태 벡터 X_k｜k 를 이용하여, 위치 자세 추정값을 보정한다. 보다 구체적으로는, 위치 추정부 (40F) 는, 상태 벡터 X_k｜k 를 이용하여, 위치 자세 추정값에 포함되는 오차를 보정 (위치 자세 추정값을 보정) 한다. 위치 추정부 (40F) 가 상태 벡터 X_k｜k 를 이용하여 위치 자세 추정값 (추정 위치) 을 보정한 위치가, 유압 셔블 (100) 의 제 2 위치가 된다. 이와 같이, 위치 추정부 (40F) 는, 동작 정보를 이용하여 유압 셔블의 위치를 추정하고, 얻어진 추정 위치를 상태 벡터 X_k｜k 를 이용하여 보정함으로써, 유압 셔블의 제 2 위치를 구한다.

도 18 은, 오차 연산부 (40Bb) 가 사용하는 관측 방정식을 선택할 때에 사용되는 정보가 기술된 테이블 (44) 의 일례를 나타내는 도면이다. 테이블 (44) 은, 도 15 에 나타내는 장치 컨트롤러 (39b) 의 기억부 (39M) 에 기억되어 있다. 본 실시형태에 있어서, 오차 연산부 (40Bb) 가 상태 벡터 X_k｜k 를 추정할 때에 사용하는 관측값은, 유압 셔블 (100) 의 상태에 따라 선택된다. 이 때문에, 오차 연산부 (40Bb) 에 의해 사용되는 관측값에 따라, 오차 연산부 (40Bb) 에 의해 사용되는 관측 방정식이 상이하다. 오차 연산부 (40Bb) 는, 상태 벡터 X_k｜k 를 추정할 때에, 유압 셔블 (100) 의 상태에 따라 선택된 관측값에 대응한 관측 방정식을, 식 (20) 부터 식 (24) 중에서 선택한다.

유압 셔블 (100) 의 상태는, 도 18 에 나타내는 바와 같이, RTK-GNSS 에 의한 측위 상태를 나타내는 상태 A, 상태 B 및 상태 C 와, 유압 셔블 (100) 의 동작 상태, 즉 차체 상태를 나타내는 차체 정정 1, 차체 정정 2 및 차체 주행의 조합에 의해 결정된다. RTK-GNSS 에 의한 측위 상태는, 글로벌 좌표 연산 장치 (23) 에 의한 유압 셔블 (100) 위치의 검출 상태이다. 본 실시형태에서는, 측위 상태가 3 개, 차체 상태가 3 개이므로, 유압 셔블 (100) 의 상태는, 합계 9 개가 된다. 측위 상태 및 동작 상태의 내용의 상세를 하기에 나타낸다. 본 실시형태에 있어서, 측위 상태 및 동작 상태의 수 및 내용은 한정되지 않는다.

상태 A : 측위 상태가 Fix

상태 B : 측위 상태가 Fix, 비측위 이외의 상태

상태 C : 측위 상태가 비측위

차체 정정 1 : 유압 셔블 (100) 이 정지하고, 또한 상부 선회체 (3) 도 정지하고 있다.

차체 정정 2 : 유압 셔블 (100) 이 정지하고, 또한 상부 선회체 (3) 가 선회하고 있다.

차체 주행 : 유압 셔블이 주행하고 있다.

선택부 (40Ab) 의 판정기 (42b) 는, 거동 검출부 (42a) 로부터의 신호 및 차체 정보 (IFb) 로부터 동작 상태를 판정하고, 글로벌 좌표 연산 장치 (23) 가 출력 한 상태 정보 (SR) 로부터 측위 상태를 판정한다. 판정기 (42b) 는, 기억부 (39M) 에 기억되어 있는 테이블 (44) 에서 판정한 동작 조건 및 측위 조건에 근거해, 오차 연산부 (40Bb) 에 의해 사용되는 관측 방정식으로부터, 오차 연산부 (40Bb) 에 입력하는 관측값을 결정한다. 그리고, 결정된 관측값이 오차 연산부 (40Bb) 에 입력되도록, 선택기 (42c) 를 동작시킨다.

판정기 (42b) 는, 식 (20) 의 관측 방정식이 사용되는 경우, 글로벌 좌표 연산 장치 (23) 가 수신한 GNSS 안테나 (21, 22) 의 위치에 대응한 제 1 기준 위치 데이터 (P1, P2) 를, 오차 연산부 (40Bb) 에 입력하는 관측값으로 한다. 판정기 (42b) 는, 식 (21) 의 관측 방정식이 사용되는 경우, 글로벌 좌표 연산 장치 (23) 가 수신한 GNSS 안테나 (21, 22) 의 위치에 대응한 제 1 기준 위치 데이터 (P1, P2) 및 글로벌 좌표 연산 장치 (23) 가 구한 GNSS 안테나 (21, 22) 의 속도 Va 를 현장 좌표계로 변환하고, 오차 연산부 (40Bb) 에 입력하는 관측값으로 한다. 판정기 (42b) 는, 식 (22) 의 관측 방정식이 사용되는 경우, 도 16 에 나타내는 특정점 연산부 (40B) 가 구한 특정점의 위치 벡터 Rfl 을, 오차 연산부 (40Bb) 에 입력하는 관측값으로 한다. 판정기 (42b) 는, 식 (23) 의 관측 방정식이 사용되는 경우, 정지 시의 각속도, 즉 유압 셔블 (100) 이 정지하고 있을 때의 각속도 (ω) = 0 을, 오차 연산부 (40Bb) 에 입력하는 관측값으로 한다. 판정기 (42b) 는, 식 (24) 의 관측 방정식이 사용되는 경우, 글로벌 좌표 연산 장치 (23) 가 구한, GPS 컴퍼스에 의한 유압 셔블 (100) 의 방위각 (θda) 을, 오차 연산부 (40Bb) 에 입력하는 관측값으로 한다.

오차 연산부 (40Bb) 는, 선택기 (42c) 로부터 입력된 관측값을 이용하여, 입력된 관측값에 대응하는 관측 방정식을 이용하여 관측 벡터 z_k (z 는 굵은 글씨) 를 구한다. 이와 같이, 오차 연산부 (40Bb) 는, 유압 셔블 (100) 상태, 즉 측위 상태 및 차체 상태에 따라 관측 벡터 z_k (z 는 굵은 글씨) 를 구할 때에 사용하는 관측 방정식을 변경하므로, 유압 셔블 (100) 의 상태에 따라서는 불필요한 관측 방정식을 사용하지 않도록 할 수 있다. 그 결과, 오차 연산부 (40Bb) 는, 연산의 부하를 경감할 수 있다.

유압 셔블 (100) 이 정지하고, 또한 상부 선회체 (3) 가 선회하지 않는 경우, 오차 연산부 (40Bb) 는, 유압 셔블 (100) 의 특정점의 속도가 0, 또한 각속도 (ω) = 0 이라는 관측값을 이용하여 관측 벡터 z_k (z 는 굵은 글씨) 를 구할 수 있다. 그 결과, 글로벌 좌표 연산 장치 (23) 의 측위 결과의 편차에 의한 영향을 저감할 수 있다.

도 19 는, 실시형태 3 에 관련된 제어 시스템 (200b) 의 처리의 일례를 나타내는 플로우 차트이다. 스텝 S501 에 있어서, 장치 컨트롤러 (39b) 의 위치·자세 정보 연산부 (39Bb) 는, 다음의 시각, 본 실시형태에서는 다음의 제어 주기에 있어서의 유압 셔블 (100) 의 상태 벡터를 추정하고, 또 관측값을 취득한다.

스텝 S502 에 있어서, 위치·자세 정보 연산부 (39Bb) 의 오차 연산부 (40Bb) 는, 측위 상태 및 차체 상태에 따라 관측 벡터 z_k (z 는 굵은 글씨) 를 구할 때에 사용하는 관측 방정식을 선택한다. 스텝 S503 에 있어서, 오차 연산부 (40Bb) 는, 사후의 추정값인 상태 벡터 X_k｜k 를 구하고, 이것에 대응하는 각속도 바이어스 오차 (FBa), 차체 각도 오차 (FBb), 차체 속도 오차 (FBc), 차체 위치 오차 (FBd) 및 가속도 바이어스 오차 (FBe) 를, 위치 추정부 (40F) 에 부여한다. 위치 추정부 (40F) 는, 오차 연산부 (40Bb) 로부터 취득한 각속도 바이어스 오차 (FBa), 차체 각도 오차 (FBb), 차체 속도 오차 (FBc), 차체 위치 오차 (FBd) 및 가속도 바이어스 오차 (FBe) 를 이용하여, IMU (24) 가 검출한 각속도 (ω) 및 가속도 (a) 와, 위치 추정부 (40F) 가 각속도 (ω) 로부터 구한 각도와, 위치 추정부 (40F) 가 가속도 (a) 로부터 구한 속도 및 위치를 보정한다.

스텝 S504 에 있어서, 위치·자세 정보 연산부 (39Bb) 는, 전술한 보정에 의해 얻어진 제 2 기준 위치 데이터 (P1i, P2i) 를, 기준 위치 데이터 (P1c, P2c) 로서 표시 컨트롤러 (28) 의 날끝 위치 산출부 (28A) 에 출력한다. 스텝 S505 에 있어서, 표시 컨트롤러 (28) 의 처리부 (28P) 는, 장치 컨트롤러 (39) 로부터 취득한 유압 셔블 (100) 의 위치 정보 (IPL) 를 이용하여, 버킷 (8) 의 날끝 (8T) 의 3 차원 위치인 날끝 위치를 구한다.

본 실시형태는, 제 1 위치의 정보인 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터의 동작 정보 (MI) 를 이용하여 제 2 위치를 구하고, 얻어진 제 2 위치의 정보를 이용하여, 작업기 (2) 의 적어도 일부의 위치를 구한다. 실시형태 1 에서는, 제 1 기준 위치 데이터 (P1, P2) 및 IMU (24) 로부터 얻어지는 특정점의 정보를 이용하여 제 2 위치가 구해졌지만, 본 실시형태에 있어서도, 특정점 (유사 부동점) 이 정지하고 있다는 조건을 관측 방정식에 추가함으로써, 실시형태 1 과 마찬가지로 제 2 위치를 구할 수 있다.

본 실시형태에 있어서, 유압 셔블 (100) 의 위치는 관성 항법에 의해 추정되고, 유압 셔블 (100) 의 위치 자세 오차에 포함되는 오차 및 IMU (24) 의 오차 등은 칼만 필터에 의해 구해진다. 본 실시형태는, 관성 항법에 의해 다음 시각의 유압 셔블 (100) 의 위치를 추정하고, 제 1 위치의 정보 및 동작 정보 (MI) 를 이용하여 칼만 필터가 구한 오차에 의해, 추정된 유압 셔블 (100) 의 위치를 보정한다. 실시형태 1 및 실시형태 2 에서는, 글로벌 좌표 연산 장치 (23) 에 의해 얻어진 위치의 정보를 평활화 처리하지만, 본 실시형태에서는, 관성 항법에 의해 미리 추정된 위치를 칼만 필터에 의해 구해진 오차를 이용하여 보정하거나, 또는 관성 항법에 의해 미리 추정된 상태 벡터를 칼만 필터에 의해 구해진 상태 벡터를 이용하여 보정한다. 이 때문에, 본 실시형태는, 평활화 처리의 지연의 영향을 배제할 수 있으므로, 작업 기계의 위치를 측위한 결과에 기초하여 정보화 시공을 실시하는 작업 기계에 있어서, 측위 결과의 편차가 정보화 시공에 주는 영향을 보다 확실하게 저감할 수 있다.

이상, 실시형태 1 내지 실시형태 3 을 설명했지만, 전술한 내용에 의해 실시형태 1 내지 실시형태 3 이 한정되는 것은 아니다. 또, 전술한 구성 요소에는, 당업자가 용이하게 상정할 수 있는 것, 실질적으로 동일한 것, 이른바 균등 범위의 것이 포함된다. 또한, 전술한 구성 요소는 적절히 조합할 수 있다.

또한, 실시형태 1 내지 실시형태 3 의 요지를 일탈하지 않는 범위에서 구성 요소의 여러 가지의 생략, 치환 및 변경 중 적어도 1 개를 실시할 수 있다. 예를 들어, 장치 컨트롤러 (39) 가 실행하는 각 처리는, 장치 컨트롤러 (39), 표시 컨트롤러 (28), 펌프 컨트롤러 또는 이들 이외의 컨트롤러가 실행해도 된다. 작업 기계는 유압 셔블 (100) 에 한정되지 않고, 휠 로더 또는 불도저와 같은 다른 작업 기계여도 된다. 도 5 에 나타내는 자세각 연산부 (39A) 및 위치·자세 정보 연산부 (39B) 는, 장치 컨트롤러 (39) 에 구비되지만, 어느 일방 또는 양방이 표시 컨트롤러 (28) 에 구비되어도 되고, 표시 컨트롤러 (28) 이외의 컨트롤러에 구비되어도 된다.

1 : 차량 본체
2 : 작업기
3 : 상부 선회체
5 : 주행 장치
8 : 버킷
8B : 날
8T : 날끝
21, 22 : 안테나 (GNSS 안테나)
23 : 글로벌 좌표 연산 장치
25 : 조작 장치
28 : 표시 컨트롤러
28A : 날끝 위치 산출부
28B : 목표 굴삭 지형 데이터 생성부
28C : 목표 시공 정보 격납부
28M : 기억부
28P : 처리부
39, 39a, 39b : 장치 컨트롤러
39A : 자세각 연산부
39B, 39Ba : 위치 정보 연산부
39Bb : 위치·자세 정보 연산부
39C : 작업기 제어부
39M : 기억부
39P, 39Pa, 39Pb : 처리부
40A : 판정부
40Ab : 선택부
40B : 특정점 연산부
40Ba, 40C : 평활화 처리부
40D : 위치 계산부
40E : 속도 계산부
40F : 위치 추정부
40Bb : 오차 연산부
41a, 41g, 41p : 차체 좌표계 변환부
41b, 41c, 41h, 41j : 가감산기
41d, 41f : 갱신부
41i : 현장 좌표계 변환부
41k : 속도 보정부
41m : 적분기
41n : 위치 보정부
42a : 거동 검출부
42b : 판정기
42c : 선택기
44 : 테이블
60 : 기준 말뚝
100 : 유압 셔블
200, 200a, 200b : 제어 시스템
FBa : 각속도 바이어스 오차
FBb : 차체 각도 오차
FBc : 차체 속도 오차
FBd : 차체 위치 오차
FBe : 가속도 바이어스 오차
K : 칼만 게인
M : 평균화 정수
MI : 동작 정보
P, P1c, P2c : 기준 위치 데이터
P1, P2 : 제 1 기준 위치 데이터
P1i, P2i : 제 2 기준 위치 데이터
SR : 상태 정보
a : 가속도
g : 중력가속도
ω : 각속도
θ4 : 롤각
θ5 : 피치각
θda, θdc, θdi : 방위각

Claims (6)

1. 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어하는 시스템으로서,
   상기 작업 기계의 일부의 위치인 제 1 위치를 검출하고, 상기 제 1 위치의 정보로서 출력하는 위치 검출 장치와,
   상기 작업 기계의 동작을 나타내는 동작 정보를 검출해 출력하는 상태 검출 장치와,
   상기 제 1 위치 및 상기 동작 정보로부터 얻어지는 정보인, 상기 선회체의 회전 중심축과 상기 주행 장치가 접지하는 면에 대응하는 면의 교점인 특정점의 위치에 평활화 처리를 실시하고, 상기 평활화 처리 후의 상기 특정점의 위치를 이용하여 상기 일부의 위치에 상당하고, 또한 상기 제 1 위치 측위 결과의 편차에 의한 영향을 저감한 제 2 위치를 구하고, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는 처리 장치를 포함하는, 작업 기계의 제어 시스템.
2. 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어하는 시스템으로서,
   상기 작업 기계의 일부의 위치인 제 1 위치를 검출하고, 상기 제 1 위치의 정보로서 출력하는 위치 검출 장치와,
   상기 작업 기계의 동작을 나타내는 동작 정보를 검출해 출력하는 상태 검출 장치, 및
   상기 위치 검출 장치에 접속되고, 상기 작업 기계의 위치를 검출하기 위한 안테나의 속도를 상기 동작 정보에 기초하여 구하고, 상기 안테나의 속도를 이용하여 상기 제 1 위치에 평활화 처리를 실시하여, 상기 일부의 위치에 상당하고, 또한 상기 제 1 위치의 측위 결과의 편차에 의한 영향을 저감한 제 2 위치를 구하고, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는 처리 장치를 포함하는, 작업 기계의 제어 시스템.
3. 제 1 항 또는 제 2 항에 있어서,
   상기 처리 장치는,
   상기 위치 검출 장치에 의한 상기 작업 기계의 위치 검출이 정상, 또한 상기 작업 기계의 주행이 정지, 또한 상기 선회체가 선회하고 있지 않을 때에, 상기 제 2 위치의 정보를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는, 작업 기계의 제어 시스템.
4. 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어할 시에,
   상기 작업 기계가 구비하는 위치 검출 장치가 검출한 상기 작업 기계의 일부의 위치인 제 1 위치와, 상기 작업 기계가 구비하는 상태 검출 장치가 검출한 상기 작업 기계의 동작 정보를 이용하여, 상기 선회체의 회전 중심축과 상기 주행 장치가 접지하는 면에 대응하는 면의 교점인 특정점의 위치를 구하고,
   상기 특정점의 위치에 평활화 처리를 실시하고, 상기 평활화 처리 후의 상기 특정점의 위치를 이용하여, 상기 일부의 위치에 상당하고, 또한 상기 제 1 위치 측위 결과의 편차에 의한 영향을 저감한 상기 작업 기계의 제 2 위치를 구하고,
   상기 제 2 위치를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는, 작업 기계의 제어 방법.
5. 주행 장치와, 작업구를 갖는 작업기와, 상기 작업기가 장착되고, 또한 상기 주행 장치에 장착되어 선회하는 선회체를 구비하는 작업 기계를 제어할 시에,
   상기 작업 기계가 구비하는 위치 검출 장치가 검출한 상기 작업 기계의 일부의 위치인 제 1 위치에, 상기 작업 기계가 구비하는 상태 검출 장치가 검출한 상기 작업 기계의 동작 정보에 기초하여 구한 상기 작업 기계의 위치를 검출하기 위한 안테나의 속도를 이용하여 평활화 처리를 실시하여, 상기 일부의 위치에 상당하고, 또한 상기 제 1 위치의 측위 결과의 편차에 의한 영향을 저감한 상기 작업 기계의 제 2 위치를 구하고,
   상기 제 2 위치를 이용하여, 상기 작업기의 적어도 일부의 위치를 구하는, 작업 기계의 제어 방법.
6. 제 1 항 또는 제 2 항에 있어서,
   상기 처리 장치가 구한 상기 작업기의 일부 위치에 기초하여 상기 작업기의 동작을 제어하거나, 또는 상기 작업 기계에 설치된 표시 장치에 상기 작업기의 일부의 위치를 표시하는, 작업 기계의 제어 시스템.

Images