KR20060107582A - 이동 로봇의 자세 제어 장치 - Google Patents

Abstract

이동 로봇의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력을 검출 또는 추정하는 소정 방향 성분 검출 수단을 구비하는 동시에, 상기 소정 방향 성분 검출 수단이 검출 또는 추정한 제1의 외력에 따라, 상기 소정 방향에 직교하는 방향으로 상기 아암부 링크에 제2의 외력이 작용하도록, 상기 아암부 링크를 구동하여 상기 로봇의 자세를 안정시키도록 제어하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

이동 로봇, 자세 제어 장치, 아암부 링크

Description

이동 로봇의 자세 제어 장치{ATTITUDE CONTROL DEVICE OF MOBILE ROBOT}

도 1은 본 발명의 일 실시의 형태에 관한 이동 로봇의 자세 제어 장치가 대상으로 하는 레그식 이동 로봇의 정면도,

도 2는 도 1에 도시하는 로봇의 측면도,

도 3은 도 1에 도시하는 로봇을 개략 도시하는 설명도,

도 4는 도 3에 도시하는 전자 제어 유닛(ECU) 등의 구성을 상세히 도시하는 블록도,

도 5는 본 발명의 하나의 실시의 형태에 관한 이동 로봇의 자세 제어 장치의 구성을 도시하는 블록도,

도 6은 도 5에 도시하는 장치의 목표 작업 패턴 생성기가 생성하는 보용(步容)에 있어서의 지지 레그 좌표계를 도시하는 설명도,

도 7은 도 6과 마찬가지로, 도 5에 도시하는 장치의 목표 작업 패턴 생성기가 생성하는 보용에 있어서의 지지 레그 좌표계를 도시하는 설명도,

도 8은 도 5에 도시하는 장치 내, 레그 메인 제어 장치에 적용되는 복합 컴플라이언스(compliance) 제어 장치의 구성을 도시하는 블록도,

도 9는 도 5에 도시하는 장치가 전제로 하는 원리의 설명도,

도 10은 도 9와 마찬가지로, 도 5에 도시하는 장치가 전제로 하는 원리의 설 명도,

도 11은 도 9와 마찬가지로, 도 5에 도시하는 장치가 전제로 하는 원리의 설명도,

도 12는 도 9와 마찬가지로, 도 5에 도시하는 장치가 전제로 하는 원리의 설명도,

도 13은 도 9와 마찬가지로, 도 5에 도시하는 장치가 전제로 하는 원리의 설명도,

도 14는 도 5에 도시하는 장치 내, 자세 안정화 메인 제어 장치의 구성을 도시하는 블록도,

도 15는 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 자세 안정화용 모멘트 산출기의 구성을 도시하는 블록도,

도 16은 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 대상물 반력 편차 모멘트 분리기의 구성을 도시하는 블록도,

도 17은 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 대상물 반력 평형 제어용 분배기의 구성을 도시하는 블록도,

도 18은 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 대상물 반력 평형 제어용 분배기의 구성을 도시하는, 도 17과 동일한 블록도,

도 19는 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 대상물 반력 평형 제어 장치의 구성을 도시하는 블록도,

도 20은 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 보상 모멘트 분 배기의 구성을 도시하는 블록도,

도 21은 도 14에 도시하는 자세 안정화 메인 제어 장치 내, 수정 목표 대상물 반력 산출기의 구성을 도시하는 블록도,

도 22는 도 5에 도시하는 장치의 작용 및 효과를 설명하기 위해서, 제어계를 대상물 반력에 주목하여 변형하면서 간략화하여 도시하는 블록도,

도 23은 마찬가지로 도 5에 도시하는 장치의 작용 및 효과를 설명하기 위해서, 제어계를 대상물 반력에 주목하여 변형하면서 간략화하여 도시하는 블록도,

도 24는 도 14에 도시하는 자세 안정화 메인 제어 장치의 동작을 설명하는 설명도,

도 25는 본 발명의 제2의 실시의 형태에 관한 이동 로봇의 자세 제어 장치의 구성을 도시하는, 도 14와 유사한, 자세 안정화 메인 제어 장치의 구성을 도시하는 블록도,

도 26은 본 발명의 제3의 실시의 형태에 관한 이동 로봇의 자세 제어 장치의 구성을 도시하는 설명도이다.

본 발명은 이동 로봇의 자세 제어 장치에 관한 것으로, 특히, 아암부를 구비하는 2발 레그식 이동 로봇 혹은 차륜식의 이동 로봇의 자세 제어 장치에 있어서, 로봇이 아암을 통해 대상물로부터 반력을 받는 작업을 행할 때의 자세 안정화를 도 모하도록 한 자세 제어 장치에 관한 것이다.

이동 로봇 중의 레그식 이동 로봇의 자세 제어 장치로는, 본 출원인이 이전에 제안한 일본국 특개평 10-230485호 공보에 기재되는 기술이 알려져 있다. 그 기술에 있어서는, 대상물로부터 예기치 않은 반력을 받으면, 장기적으로는 로봇의 상체의 위치가 어긋나도록 자세를 서서히 수정함으로써 로봇 전체의 중심 위치를 밀리게 하고, 보상을 위한 중력 모멘트를 발생시켜 운동 밸런스, 즉, 동력학적으로 균형잡힌 밸런스를 유지하는 동시에, 지금까지의 과도기에 있어서는, 목표 지면 반력 중심점(목표 ZMP에 일치하고 있다) 주위에 보상을 위한 전 지면 반력 모멘트를 발생시키도록 풋부를 움직여 밟음 정지시킴으로써, 운동 밸런스를 유지하도록 구성하고 있다.

이와 같이, 상기한 종래 기술에 있어서는, 대상물로부터 받는 예기치 않은 반력이 급격히 변화한 경우에는, 그 순간에는(단기적으로는), 풋부를 움직여 밟음 정지시킴으로써 운동 밸런스를 유지하는, 즉, 전 지면 반력 모멘트를 발생시킴으로써 자세 안정화가 도모된다.

또한, 본 출원인은, 대상물로부터의 반력을 받든 받지 않던 상관없이, 로봇의 자세가 기울어 있을 때에, 이를 복원시키기 위한 모멘트를 전 지면 반력 모멘트로서 적극적으로 발생시키는 기술도 제안하고 있는데(일본국 특개평 10-277969호 공보), 어느쪽 기술에 있어서도, 전 지면 반력 모멘트에는 한계가 있어, 이를 넘어 전 지면 반력 모멘트를 발생시키고자 하면, 풋부의 일부가 지면으로부터 떠버려, 그 결과, 로봇은 운동 밸런스를 무너뜨려, 최악의 경우에는 전도될 우려가 있다.

또한, 본 출원인은, 전 지면 반력 모멘트를 발생시키는 대신에 상체의 궤도를 수정함으로써 운동 밸런스를 유지하는 기술도 제안하고 있는데(일본국 특허 제 3269852호 공보), 이러한 기술에 있어서는 발의 착지 위치 등을 수정할 필요가 있으므로, 아암으로 대상물에 대한 작업을 행할 때, 상체나 발이 작업에 적절한 위치로부터 떨어져 버리는 경우가 있다.

또한, 대상물에 어떠한 운동을 부여하는 작업을 행하는 경우에 있어서는, 작업을 소기(所期)대로 수행하기 위해서는, 운동 밸런스를 유지하거나, 혹은 기울어진 자세의 복원을 행해도, 대상물의 운동에 영향을 주지 않는 것이 바람직하다.

따라서, 본 발명의 제1의 목적은, 상기한 문제를 해소하는 것에 있고, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지하도록 한 이동 로봇의 자세 제어 장치를 제공하는 것을 목적으로 한다.

또한, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 접지하는 지면으로부터 작용되는 지면 반력을 조작하지 않고, 혹은 그 조작량을 저감하면서, 운동 밸런스를 유지하여 안정된 자세를 유지하도록 한 이동 로봇의 자세 제어 장치를 제공하는 것을 제2의 목적으로 한다.

또한, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 대상물의 운동에 영향을 주지 않고 운동 밸런스를 유지하여 안정된 자세를 유지하도록 한 이동 로봇의 자세 제어 장치를 제공하는 것을 제3의 목적으로 한다.

본 발명은, 상기한 목적을 달성하기 위해서, 후술하는 청구항 1에 기재하는 바와 같이, 적어도 기체(基體)와, 이동 기구와, 대상물에 힘을 작용할 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력을 검출 또는 추정하는 소정 방향 성분 검출 수단을 구비하는 동시에, 상기 소정 방향 성분 검출 수단이 검출 또는 추정한 제1의 외력에 따라, 상기 소정 방향으로 직교하는 방향으로 상기 아암부 링크에 제2의 외력이 작용하도록, 상기 아암부 링크를 구동하여 상기 로봇의 자세를 안정시키도록 제어하도록 구성했다. 이와 같이, 이동 로봇의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력에 따라, 소정 방향으로 직교하는 방향으로 아암부 링크에 제2의 외력이 작용하도록, 아암부 링크를 구동하여 상기 로봇의 자세를 안정시키도록 제어하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

또한, 본 발명은, 후술하는 청구항 2에 기재하는 바와 같이, 적어도 기체와, 이동 기구와, 대상물에 힘을 작용할 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서, 적어도 상기 로봇의 목표 아암부 링크 위치 자세와, 상기 대상물로부터 상기 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 목표 동작 생성 수단, 상기 대상물 반력의 실제치인 실대상물 반력을 검출 혹은 추정하는 실대상물 반력 검출 수단, 적어도 상기 검출 혹은 추정된 실대상물 반력에 의거해, 상기 로봇의 자세가 안정되도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어 수단, 및 적어도 상기 수정된 목표 동작에 따라서 상기 아암부 링크를 구동하는 아암부 구동 장치를 구비하도록 구성했다. 이와 같이, 이동 로봇의 자세 제어 장치에 있어서, 적어도 로봇의 목표 아암부 링크 위치 자세와, 대상물로부터 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하고, 대상물 반력의 실제치인 실대상물 반력을 검출 혹은 추정하여, 적어도 검출 혹은 추정된 실대상물 반력에 의거하여, 로봇의 자세가 안정되도록 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하고, 수정된 목표 동작에 따라서 아암부 링크를 구동하는 아암부 구동 장치를 구비하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다. 또한, 대상물의 운동에 영향을 주지 않고, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

또한, 본 발명은, 후술하는 청구항 3에 기재하는 바와 같이, 상기 자세 안정화 제어 수단은, 적어도 상기 실대상물 반력과 상기 목표 대상물 반력과의 차이를 나타내는 대상물 반력 편차에 의거하여, 상기 로봇의 자세가 안정되도록 상기 목표 동작을 수정하도록 구성했다. 이와 같이, 적어도 실대상물 반력과 목표 대상물 반력과의 차이를 나타내는 대상물 반력 편차에 의거하여, 로봇의 자세가 안정되도록 목표 동작을 수정하도록 구성하였으므로, 상기한 효과를 한층 더 잘 얻을 수 있다.

또한, 본 발명은, 후술하는 청구항 4에 기재하는 바와 같이, 상기 자세 안정화 제어 수단은, 상기 대상물 반력 편차의 제1의 성분에 의해서 발생하는 전도력의 전부 혹은 일부를, 상기 제1의 성분으로부터 얻어지는 제2의 성분에 의해서 부정하도록, 상기 목표 동작을 수정하도록 구성했다. 이와 같이, 대상물 반력 편차의 제1의 성분에 의해서 발생하는 전도력의 전부 혹은 일부를, 제1의 성분으로부터 얻어지는 제2의 성분에 의해서 부정하도록, 목표 동작을 수정하도록 구성하였으므로, 상기한 효과를 한층 더 잘 얻을 수 있다.

또한, 본 발명은, 후술하는 청구항 5에 기재하는 바와 같이, 적어도 기체와, 이동 기구와, 대상물에 힘을 작용시킬 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서, 적어도 상기 로봇의 목표 아암부 링크 위치 자세와, 상기 대상물로부터 상기 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 목표 동작 생성 수단, 상기 로봇의 자세 기울기 편차를 검출하여, 상기 검출한 자세 기울기 편차가 제로에 근접하도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어 수단 및 적어도 상기 수정된 목표 동작에 따라서 상기 아암부 링크를 구동하는 아암부 구동 장치를 구비하도록 구성했다. 이와 같이, 이동 로봇의 자세 제어 장치에 있어서, 적어도 로봇의 목표 아암부 링크 위치 자세와, 대상물로부터 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 동시에, 로봇의 자세 기울기 편차를 검출하여, 검출한 자세 기울기 편차가 제로에 근접하도록 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하고, 수정된 목표 동작에 따라서 아암부 링크를 구동하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다. 또한, 대상물의 운동에 영향을 주지 않고, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

또한, 특허청구의 범위 및 발명의 상세한 설명란에서, 「이동 로봇」은 아암부 링크 이외에 대상물 반력을 받는 이동 로봇도 포함하는 의미로 사용한다. 또한, 「아암부 링크」에 관하여, 레그부 링크라도, 작업 대상물에 작용하는 것이면, 아암부 링크로 간주하는 것으로 한다. 예를 들면, 곤충형의 6개의 레그부 링크를 구비한 로봇에 있어서, 앞의 2개의 레그부 링크를 이용하여 물건을 들어 올리는 경우에는, 그 레그부 링크는 아암부 링크로 간주하는 것으로 한다.

또한, 「위치 자세」는 「위치」 및 「자세」의 양자를 포함해도 되고, 혹은 그 어느 하나라도 되는 의미로 사용한다. 바꾸어 말하면, 「위치 자세」는, 반드 시「위치」및「자세」의 양자를 포함하지 않으면 안되는 것이 아니다.

이하, 첨부도면을 참조하여 본 발명의 하나의 실시의 형태에 관한 이동 로봇의 자세 제어 장치를 설명한다.

도 1은 본 실시의 형태에 관한 이동 로봇의 자세 제어 장치가 대상으로 하는 레그식 이동 로봇의 정면도, 도 2는 그 측면도이다. 또한, 이동 로봇으로는, 2개의 레그부 링크와 2개의 아암부 링크를 구비한 휴머노이드(인간형) 레그식 이동 로봇을 예로 든다.

도 1에 도시하는 바와 같이, 레그식 이동 로봇(이하「로봇」이라고 한다)(1)은 다수 개, 보다 구체적으로는 2개의 레그부 링크(혹은 레그)(2)를 구비하는 동시에, 그 상방에는 상체(기체(基體))(3)가 설치된다. 상체(3)의 더 상방에는 헤드부(4)가 형성되는 동시에, 상체(3)의 양측에는 2개의 아암부 링크(혹은 아암)(5)가 연결된다. 또한, 도 2에 도시하는 바와 같이, 상체(3)의 등부에는 격납부(6)가 설치되고, 그 내부에는 전자 제어 유닛(후술) 등이 수용된다. 또한, 도 1 및 도 2에 도시하는 로봇(1)은, 내부 구조를 보호하기 위한 커버로 피복된다.

도 3은 로봇(1)을 개략으로 도시하는 설명도이다. 동 도면을 참조하여 그 내부 구조를 관절을 중심으로 설명하면, 도시와 같이, 로봇(1)은 좌우 각각의 레그부 링크(2) 및 아암부 링크(5)에, 11개의 전동 모터로 동력화된 6개의 관절을 구비한다.

즉, 로봇(1)은, 웨이스트부(대퇴부)에, 레그부 링크(2)를 연직축(Z축 혹은 연직축) 방향으로 회전시키는 관절을 구동하는 전동 모터(10R, 10L)(우측을 R, 좌 측을 L로 한다. 이하 동일)와, 레그부 링크(2)를 피치(진행)방향(Y축 방향)으로 요동시키는 관절을 구동하는 전동 모터(12R, 12L)와, 레그부 링크(2)를 롤(좌우)방향(X축 방향)으로 회전시키는 관절을 구동하는 전동 모터(14R, 14L)를 구비하는 동시에, 무릎부에 레그부 링크(2)의 하부를 피치 방향(Y축 방향)으로 회전시키는 무릎 관절을 구동하는 전동 모터(16R, 16L)를 구비하고, 또한 발목에 레그부 링크(2)의 선단측을 피치 방향(Y축 방향)으로 회전시키는 발(발목) 관절을 구동하는 전동 모터(18R, 18L)와 롤 방향(X축 방향)으로 회전시키는 발(발목) 관절을 구동하는 전동 모터(20R, 20L)를 구비한다.

상기한 바와 같이, 도 3에 있어서, 관절은 이를 구동하는 전동 모터(혹은 전동 모터에 접속되어 그 동력을 전동하는 풀리 등의 전동 요소)의 회전축선으로 표시한다. 또한, 레그부 링크(2)의 선단에는 풋부(발)(22R, 22L)가 부착된다.

이와 같이, 레그부 링크(2)의 대퇴 관절(웨이스트 관절)에는 전동 모터(10R(L), 12R(L), 14R(L))가 이들 회전축선이 직교하도록 배치되는 동시에, 발 관절(발목 관절)에는 전동 모터(18R(L), 20R(L))가 이들 회전 축선이 직교하도록 배치된다. 또한, 대퇴 관절과 무릎 관절은 대퇴 링크(24R(L))로, 무릎 관절과 발 관절은 하퇴 링크(26R(L))로 연결된다.

레그부 링크(2)는 대퇴 관절을 통해 상체(3)에 연결되는데, 도 3에서는 상체(3)를 상체 링크(28)로서 간략하게 도시한다. 상기와 같이, 상체(3)에는 아암부 링크(5)가 연결된다.

아암부 링크(5)도, 레그부 링크(2)와 마찬가지로 구성된다. 즉, 로봇(1)은, 숄더부에, 아암부 링크(5)를 피치 방향으로 회전시키는 관절을 구동하는 전동 모터(30R, 30L)와 롤 방향으로 회전시키는 관절을 구동하는 전동 모터(32R, 32L)를 구비하는 동시에, 그 자유단측을 회전시키는 관절을 구동하는 전동 모터(34R, 34L)와, 팔꿈치부에 그 이후의 부위를 회전시키는 관절을 구동하는 전동 모터(36R, 36L)를 구비하고, 또한 그 선단측에 이를 회전시키는 손목 관절을 구동하는 전동 모터(38R, 38L)를 구비한다. 또한, 손목 앞에는 핸드(엔드 이펙터)(40R, 40L)가 부착된다.

즉, 아암부 링크(5)의 어깨 관절에는 전동 모터(30R(L), 32R(L), 34R (L))가 이들 회전 축선이 직교하도록 배치된다. 또한, 어깨 관절과 팔꿈치 관절은 상부 아암 링크(42R(L))로, 팔꿈치 관절과 손목 관절은 하부 아암 링크(44R(L))로 연결된다.

또한, 헤드부(4)는, 연직축 방향의 목관절(46)과, 이와 직교하는 축 방향으로 헤드부(4)를 회전시키는 헤드부 요동 기구(48)를 통해 상체(3)에 연결된다. 도 3에 도시하는 바와 같이, 헤드부(4)의 내부에는 촬상한 화상을 나타내는 신호를 출력하는, CCD 카메라로 이루어지는 시각 센서(50)가 배치되는 동시에, 리시버 및 마이크로 폰으로 이루어지는 음성 입출력 장치(52)가 배치된다.

상기의 구성에 의해, 레그부 링크(2)는 좌우의 발에 관해서 6개의 관절을 구비하여 합계 12의 자유도가 주어지고, 6개의 관절을 적당한 각도로 구동(관절 변위)함으로써, 레그부 링크(2)에 원하는 움직임을 줄 수 있고, 로봇(1)을 임의로 3차원 공간을 보행시킬 수 있다. 또한, 아암부 링크(5)도 좌우의 아암에 대해서 5 개의 관절을 구비하여 합계 10의 자유도가 주어지고, 5개의 관절을 적당한 각도로 구동(관절 변위)함으로써 원하는 작업을 행하게 할 수 있다. 또한, 헤드부(4)는 2개의 자유도로 이루어지는 관절 혹은 요동 기구가 주어지고, 이들을 적당한 각도로 구동함으로써 원하는 방향으로 헤드부(4)를 향할 수 있다.

10R(L) 등의 전동 모터의 각각에는 로터리 인코더(도시하지 않음)가 구비되고, 전동 모터의 회전축의 회전을 통해 대응하는 관절의 각도, 각속도, 및 각가속도 중 적어도 어느 하나를 나타내는 신호를 출력한다.

풋부(22R(L))에는 공지의 6축력 센서(이하「역(力) 센서」라고 한다)(56)가 부착되고, 로봇에 작용하는 외력 중, 접지면에서 로봇(1)에 작용하는 지면 반력의 3방향 성분(Fx, Fy, Fz)과 모멘트의 3방향 성분(Mx, My, Mz)을 표시하는 신호를 출력한다.

또한, 손목 관절과 핸드(40R(L))의 사이에는 동 종의 역 센서(6축력 센서)(58)가 부착되고, 로봇(1)에 작용하는 지면 반력 이외의 외력, 구체적으로는 핸드(40R(L))에 대상물로부터 작용하는 외력(대상물 반력)의 3방향 성분(Fx, Fy, Fz)과 모멘트의 3방향 성분(Mx, My, Mz)을 표시하는 신호를 출력한다.

또한, 상체(3)에는 경사 센서(60)가 설치되고, 연직축에 대한 상체(3)의 경사(경사 각도)와 그 각속도 중 적어도 어느 하나가, 즉, 로봇(1)의 상체(3)의 경사(자세) 등의 상태량을 표시하는 신호를 출력한다.

이들 역 센서(56) 등의 출력군은, 로봇(1)의 상체(3)의 등 측의 격납부(6)(도 2에 도시한다)에 수용된, 마이크로 컴퓨터로 이루어지는 전자 제어 유닛 (Electric Control Unit. 이하 「ECU」라고 한다)(62)에 이송된다(도시의 편의를 위해 로봇(1)의 우측에 대해서만 도시한다).

도 4는 ECU(62)의 상세를 도시하는 블록도이고, 마이크로·컴퓨터로 구성된다. 여기에서 경사 센서(60) 등의 출력은 A/D 변환기(70)에서 디지털값으로 변환되고, 그 출력은 버스(72)를 통해 RAM(74)에 이송된다. 또한 각 액츄에이터에 있어서 전동 모터에 인접해 배치되는 인코더의 출력은 카운터(76)를 통해 RAM(74) 내에 입력된다.

ECU(62) 내에는 CPU로 이루어지는 연산 장치(80)가 설치되고, 연산 장치(80)가 생성한 보용에 따라서 로봇이 안정된 자세를 계속할 수 있도록, 관절 각 변위 지령(액츄에이터 변위 지령)을 산출하여, RAM(74)에 송출한다. 또한, 부호 84는 ROM을 표시한다.

또한, 연산 장치(80)는 RAM(74)으로부터 그 지령과 검출된 실측치를 독출하고, 각 관절의 구동에 필요한 제어치(조작량)를 산출하여 D/A 변환기(86)와 각 관절에 설치된 액츄에이터 구동 장치(앰프)(88)를 통해 각 관절을 구동하는, 변위 검출기(인코더) 부착 레그 액츄에이터(90)와 아암 액츄에이터(92)의 전동 모터(10R(L), 12R(L)) 등에 출력한다.

도 5는, 본 실시 형태에 관한 레그식 이동 로봇의 자세 제어 장치(주로 상기한 연산 장치(80)에 상당)의 구성 및 동작을 기능적으로 도시하는 블록도이다.

이 장치는 레그 및 아암의 동작을 통합적으로 제어하는 장치이고, 후술하는 바와 같이 조작량을 산출하여 각 액츄에이터 구동 장치(88)에 액츄에이터 변위 지 령을 출력하고, 레그 액츄에이터(90)와 아암 액츄에이터(92)를 동작시킨다. 도시와 같이, 이 장치는, 목표 작업 패턴 생성기(100), 레그 메인 제어 장치(102), 아암 메인 제어 장치(104) 및 자세 안정화 메인 제어 장치(106) 등으로 구성된다.

목표 작업 패턴 생성기(100)는, 어느 상정 조건 하에서 로봇(1)의 동력학적 평형 조건을 만족하는 보용을 포함하는, 목표 작업 패턴을 생성한다. 목표 작업 패턴은, 다수의 변수의 시간 변화 패턴에 의해서 표현된다. 이 변수는 운동을 표현하는 변수와 환경에서 받는 반력을 표현하는 변수로 구성된다.

여기서, 운동을 표현하는 변수는, 이에 의해서 각 순간에서의 자세를 하나로 결정할 수 있는 변수의 그룹이다. 구체적으로는, 목표 풋부 위치 자세, 목표 상체 위치 자세, 목표 핸드 위치 자세로 구성된다.

또한, 환경에서 받는 반력을 표현하는 변수는, 후술하는 목표 전 지면 반력 중심점, 목표 전 지면 반력 및 목표 대상물 반력으로 구성된다.

이들 각 변수는, 지지 레그 좌표계로 표시된다. 지지 레그 좌표계는, 지지 레그 발목(발관절을 구동하는 전동 모터(18, 20R(L))의 회전 축선의 교점)으로부터 풋부(22R(L))로의 수직 투영점을 원점으로 하는 좌표계이고, 도 6 및 도 7에 도시하는 바와 같이, 지지 레그가 접지하는 지면에 고정된 좌표계이고, 지지 레그 풋부의 앞방향을 X축 방향, 좌향을 Y축 방향, 연직 방향 상향을 Z축 방향으로 하는 좌표계이다.

이하, 이들 각 변수를 상세히 설명한다.

우선, 「대상물 반력」이란, 로봇(1)이 환경에서 받는 외력 중에서, 각 풋 부 지면 반력을 제외한 외력을 의미한다. 구체적으로는, 핸드(40R (L))가 작업 대상물(예를 들면 도어의 손잡이)로부터 받는 반력을 의미한다. 또한, 그 목표치를 「목표 대상물 반력」이라고 부른다.

목표 작업 패턴 생성기(100)가 출력하는 목표 대상물 반력은, 후술하는 목표 전 지면 반력 중심점 방향으로 작용하는 힘과 모멘트에 의해서 표현된다. 덧붙여서 말하면, 자세 안정화에 있어서 중요한 것은, 이 중 모멘트 성분이다.

목표 전 지면 반력과 목표 전 지면 반력 중심점(위치)에 대해서 설명하면, 작업 중에 있어 각 풋부가 지면으로부터 받아야되는 목표 지면 반력의 합력을 「광의의 목표 전 지면 반력」이라고 부른다. 광의의 목표 전 지면 반력은, 작용점과 그 점에서의 힘과 모멘트로 표현된다. 목표 전 지면 반력 중심점(위치)은, 목표 전 지면 반력을 그 점을 작용점으로 하는 힘과 모멘트로 표현하였을 때, X축 방향 모멘트 성분과 Y축 방향 모멘트 성분이 제로(0)로 되는 지면 상의 점이다.

협의의 목표 전 지면 반력은, 광의의 목표 전 지면 반력을, 목표 전 지면 반력 중심점을 작용점으로 하여, 힘과 모멘트로 표현한 경우의 힘과 모멘트를 의미한다. 목표 작업 패턴 생성기가 출력하는 목표 전 지면 반력은, 협의의 목표 전 지면 반력이다.

이후는 특별히 설명이 없는 한, 목표 전 지면 반력은, 협의의 목표 전 지면 반력을 가리킨다. 또한, 평탄한 지면을 보행하는 경우에는, 목표 전 지면 반력 중심점은, 통상, 그 지면 상에 설정된다.

보행 제어 분야에서 종래부터 공지인, M. Vukobratovic에 의해서 제창된 ZMP 의 개념도, 다음과 같이 확장한다. 즉, 로봇(1)의 운동에 의해서 생기는 관성력과 중력과 대상물 반력의 합력이, 그 점을 작용점으로 하는 힘과 모멘트로 표현되었을 때, X축 방향 모멘트 성분과 Y축 방향 모멘트 성분이 0이 되는 지면 상의 점을 「ZMP」라고 부른다. 로봇(1)이 목표의 운동을 할 때의 ZMP를 「목표 ZMP」라고 부르고, 그 위치를 「목표 ZMP 위치」라고 부른다.

목표 작업 패턴이 동력학적 평형 조건을 만족한다는 것은, 목표 작업 패턴에 의해서 생기는 상기의 관성력과 중력과 대상물 반력의 합력과 목표 전 지면 반력이, 서로 부정하여 제로가 되는 것이다. 따라서, 동력학적 평형 조건을 만족하기 위해서는, 목표 전 지면 반력 중심점(위치)과 목표 ZMP(위치)가 일치하지 않으면 안된다.

목표 작업 패턴 생성기(100)에서는, 후술하는 대국 안정화 제어가 작용하지 않는 경우, 동력학적 평형 조건을 만족하는 보용을 포함하는 목표 작업 패턴을 생성한다. 바꾸어 말하면, 이 경우, 목표 작업 패턴 생성기(100)가 생성하는 목표 전 지면 반력 중심점(위치)은 목표 ZMP(위치)에 일치한다.

목표 풋부 위치 자세, 목표 상체 위치 자세, 목표 핸드 위치 자세는, 상기한지지 레그 좌표계에서 표현된 각각의 부위의 위치와 자세를 나타낸다. 구체적으로는, 상체(3)의 위치 및 그 속도는, 상체(3)의 중심 위치 등의 대표점 및 그 (변위) 속도를 의미한다. 또한, 상체(3) 혹은 풋부(22R(L))의 자세는, X, Y, Z 공간에서의 방향을 의미한다.

이어서, 각 메인 제어 장치(102)에 대해서 설명한다. 각 메인 제어 장치 (102)의 기능은, 개략 설명하면, 레그 액츄에이터((10R(L)) 등의 전동 모터 등)를 조작하고, 목표 자세에 추종하는 자세 추종 제어와, 목표 전 지면 반력과 보상 전 지면 반력 모멘트(후술)의 합력에 추종하는 지면 반력 제어를 동시에 행하는 장치이다. 또한, 목표 지면 반력과 보상 전 지면 반력 모멘트의 합력과 목표 자세를 동시에 완전히 만족시키는 것은 불가능하므로, 적당한 조정이 행해지고, 장기적으로는 양쪽을 만족하도록 제어된다.

보다 자세히는, 레그 메인 제어 장치(102)는, 수정 목표 상체 위치 자세(후술), 목표 풋부 위치 자세, 목표 전 지면 반력 중심점(위치)과, 그 중심점에 작용하는 목표 전 지면 반력과, 보상 전 지면 반력 모멘트를 입력하고, 목표 전 지면 반력 중심점(위치)에 작용하는 실제 전 지면 반력의 모멘트 성분이, 목표 전 지면 반력 모멘트(통상은 제로)와 보상 전 지면 반력 모멘트의 합에 일치하도록, 목표 풋부 위치 자세를 수정한다. 수정된 목표 풋부 위치 자세를 「수정 목표 풋부 위치 자세」라고 부른다.

레그 메인 제어 장치(102)는, 이 수정 목표 상체 위치 자세와 수정 목표 풋부 위치 자세로부터 결정되는 목표 레그, 관절 변위에 실관절 변위가 추종하도록, 액츄에이터 변위 지령을 출력하여 레그 액츄에이터(90)를 제어한다.

이상의 기능을 실현시키기 위해서, 이 실시의 형태에 있어서는, 상기한 일본국 특개평 10-277969호 공보에 기재되는 기술(이하 「복합 컴플라이언스 제어」라고 한다)을 적용한다. 이 복합 컴플라이언스 제어를 하는 장치는, 각 메인 제어 장치의 외에, 풋부(22R(L))에 설치한 역 센서(56), 액츄에이터 구동 장치(88) 및 액츄에이터(90)를 포함하여 구성된다.

도 8에 복합 컴플라이언스 제어 장치의 블록도를 도시하는데, 그 상세한 설명은 상기 일본국 특개평 10-277969호 공보에 기재되어 있으므로 생략한다. 또한, 도 8에 도시하는 복합 컴플라이언스 제어 장치에 있어서는, 일본국 특개평 10-277969호 공보에 기재되는 복합 컴플라이언스 제어 장치와 비교하면, 상체 경사 피드백계의 자세 안정화 제어(보상 전 지면 반력 모멘트의 연산)가 생략되어 있다.

그 이유는, 동일한 처리가 후술의 자세 안정화 메인 제어 장치로 행해지고, 거기서 구해진 보상 전 지면 반력 모멘트가 입력되기 때문이다. 단, 상체 경사 편차에 의한 상반력의 변동을 억제하기 위해서, 상체 경사 피드백계를 추가해도 된다.

또한, 이 실시의 형태에 있어서 복합 컴플라이언스 제어에, 상기한 특허 제3269852호 공보에 기재되는 기술(이하, 「대국 안정화 제어」라고 한다)을 추가하는 것으로 한다. 단, 대국 안정화 제어는 상체의 위치나 보폭을 수정, 보다 구체적으로는 상기한 수정 목표 상체 위치 자세를 더 수정하므로, 후술의 아암부 링크(5)의 제어에 있어, 핸드(40R(L))와 대상물의 상대 위치 관계가 중요한 경우에는, 수정되는 상체의 위치나 보폭의 영향을 고려할 필요가 있다. 또한, 대국 안정화 제어가 작용하면, 목표 작업 패턴에 있어서의 목표 ZMP는, 목표 전 지면 반력 중심점으로부터 고의로 밀려진 점에 제어된다.

대국 안정화 제어가 가해짐으로써 수정 목표 상체 위치 자세가 더 수정되므로, 그에 의해 최종적으로 얻어지는 목표 상체 위치 자세를, 도 5에 도시하는 바와 같이, 「최종 수정 목표 상체 위치 자세」라고 부른다. 또한, 대국 안정화 제어를 가하는 것은 본 발명에 있어서 필수가 아니다. 즉, 대국 안정화 제어를 가하지 않아도 되고, 그 경우는, 수정 목표 상체 위치 자세를 최종 수정 목표 상체 위치 자세로서 취급하면 충분한다.

또한, 도 5에 있어서, 레그 메인 제어 장치(102)에서 수정된 최종 수정 목표 풋부 위치 자세는, 자세 안정화 메인 제어 장치(106)에 입력된다. 단, 자세 안정화 메인 제어 장치(106)에 있어서, 목표 풋부 위치 자세가 수정된 것에 의한 로봇의 중심 위치의 변화를 무시할 수 있으면, 최종 수정 목표 풋부 위치 자세를 자세 안정화 메인 제어 장치(106)에 입력시킬 필요는 없다.

이어서 아암 메인 제어 장치(104)에 관해서 설명한다. 아암 메인 제어 장치(104)의 기능은, 개략 설명하면, 아암 액츄에이터(30R(L)) 등의 전동 모터 등)(92)를 조작하고, 목표 자세에 추종하는 자세 제어와 수정 목표 대상물 반력(후술)에 추종하는 대상물 반력 제어를 동시에 행하는 것이다(여기서의「자세」는 아암부 링크(5)의 전 관절 변위 그룹을 나타낸다). 목표 자세와 수정 목표 대상물 반력을 동시에 완전히 만족시키는 것은 불가능하므로, 적당한 수법, 예를 들면, 종래부터 운동 기구(manipulator)의 컴플라이언스 제어, 소위 가상 컴플라이언스 제어로서 알려져 있는 것을 이용한다(기계 공학 편람, 엔지니어링 편, C4-100페이지).

구체적인 제어계 구성과 알고리즘을 이하에 설명하면, 아암 메인 제어계는 아암 메인 제어 장치(104) 외에, 상기한 핸드(40R(L))에 설치한 역 센서(58), 액츄에이터 구동 장치(88) 및 아암 액츄에이터(92)를 포함하여 구성된다.

아암 메인 제어 장치(104)는, 최종 목표 상체 위치 자세(혹은 수정 목표 상체 위치 자세), 목표 핸드 위치 자세 및 수정 목표 대상물 반력을 입력하고, 역 센서(58)에 의해서 검출되는 실대상물 반력과 수정 목표 대상물 반력의 차이에 따라 목표 핸드 위치 자세를 수정한다. 수정된 목표 핸드 위치 자세를 「최종 수정 목표 핸드 위치 자세」라고 부른다. 그리고, 최종 수정 목표 상체 위치 자세(혹은 수정 목표 상체 위치 자세)와 최종 수정 목표 핸드 위치 자세로부터 결정되는 목표 팔관절 변위에 실관절 변위가 추종하도록 아암 액츄에이터(92)를 제어한다.

이어서 자세 안정화 메인 제어 장치(106)에 대해서 설명한다. 자세 안정화 메인 제어 장치(106)는, 운동 밸런스 혹은 자세 밸런스를 취하기 위해서 동력학적 평형 조건을 고려하면서 제어를 한다. 그래서, 우선, 장치의 개요를 설명하기 전에, 동력학적 평형 조건에 관해서 이하에 설명한다.

실제의 로봇(1)의 자세 기울기의 거동을 결정하는 가장 큰 요인은, 목표 전 지면 반력 중심점(즉, 목표 ZMP) 방향에서의 실제 힘의 모멘트의 밸런스이다.

목표 전 지면 반력 중심점 방향에 작용하는 힘의 모멘트를 이하에 열거한다.

1) 관성력 모멘트

2) 중력 모멘트

3) 전 지면 반력 모멘트

4) 대상물 반력 모멘트

관성력 모멘트는, 목표 전 지면 반력 중심점 방향의 로봇의 각(角) 운동량의 변화에 의해서 생기는 모멘트이다. 이 값은 오일러 방정식에 의해서 구해지고, 구체적으로는 목표 전 지면 반력 중심점 방향의 로봇(1)의 각 운동량의 1층 미분치의 부호를 반전시킨 것이다. 목표 작업 패턴의 관성력 모멘트를 「목표 관성력 모멘트」라고 부른다. 실제의 로봇(1)이 작업하고 있을 때의 관성력 모멘트를 「실관성력 모멘트」라고 부른다. 중력 모멘트는, 로봇(1)의 중심에 작용하는 중력이 목표 전 지면 반력 중심점 방향에 작용하는 모멘트이다.

2개의 풋부(22R(L))에 각각 작용하는 지면 반력의 합력을 「전 지면 반력」이라고 부른다. 전 지면 반력 모멘트는, 전 지면 반력이 목표 전 지면 반력 중심점 방향에 작용하는 모멘트이다.

상술과 같이, 핸드(40R(L))가 대상물로부터 받는 반력을 「대상물 반력」이라고 부른다. 대상물 반력 모멘트는, 대상물 반력이 목표 전 지면 반력 중심점 방향에 작용하는 모멘트이다.

또한, 이상적인 레그 메인 제어 장치에 의해, 로봇(1)이 목표 작업 패턴의 운동 패턴에 충실히 추종하고 있다고 가정한다. 이 때는 실관성력 모멘트는 목표 관성력 모멘트에 일치하고, 실중력 모멘트는 목표 중력 모멘트에 일치한다.

한편, 동력학의 법칙(오일러 방정식)에 의해, 실관성력 모멘트와 실중력 모멘트와 실제 전 지면 반력 모멘트와 실대상물 반력 모멘트의 합은, 반드시 제로이다.

이 때문에, 로봇(1)이 충실히 목표 작업 패턴의 운동 패턴대로 움직이기 위해서는, 목표 관성력 모멘트와 목표 중력 모멘트와 실제 전 지면 반력 모멘트와 실대상물 반력 모멘트의 합은, 제로가 아니면 안된다. 이를 조건 1로 한다.

그런데, 실제로는 실대상물 반력 모멘트가 목표 대상물 반력 모멘트와 일치하지 않고, 차가 생긴다. 예를 들면, 도 9에 도시하는 바와 같이, 대차(대상물)(108)를 미는 작업을 하고 있을 때, 대차(108)의 실제 전도 마찰력의 절대치가 상정한 값보다도 돌연 작아진 상황이다.

이 상황에서는, 실대상물 반력이 목표 전 지면 반력 중심점의 Y축 방향에 작용하는 모멘트는, 목표 대상물 반력이 목표 전 지면 반력 중심점의 Y축 방향에 작용하는 모멘트보다도 정(正)의 방향으로 커져 조건 1을 만족하지 않게 되어, 로봇(1)은 앞으로 기울어진다. 또한, 모멘트의 방향은, 좌표축의 정 방향으로 향하여 로봇(1)을 시계 방향으로 회전시키는 모멘트를 정으로 한다.

이러한 상황에 있어서도 조건 1을 만족시키기 위해서는, 먼저 제안한 기술에서는 다음 2가지 수법을 생각할 수 있다.

수법 1) 상기 편차를 부정하도록, 실제 전 지면 반력 모멘트를 바꾼다. 구체적으로는, 목표 전 지면 반력 중심점 방향에 부(負)의 지면 반력 모멘트를 발생하도록 레그 메인 제어 장치(102)에 지령하고, 레그 메인 제어 장치(102)에 있어서 액츄에이터 변위 지령을 행하여 풋부(22R(L))의 발끝을 내려, 실제 전 지면 상반력 모멘트를 부의 방향으로 증가시킨다, 즉, 레그부 링크(2)로 밟음 정지시키는 자세를 취하게 한다.

수법 2) 상기 편차를 부정하도록, 목표 작업 패턴의 운동 패턴을 수정함으로써, 목표 관성력 모멘트와 목표 중력 모멘트를 수정한다. 구체적으로는, 목표 상체 위치 및/또는 자세를 수정함으로써, 목표 관성력 모멘트와 목표 중력 모멘트를 수정한다. 즉, 상체(3)를 전후 방향으로 이동시킨다.

실제 전 지면 반력 모멘트는, 목표 전 지면 반력 모멘트를 바꾸는 것만으로 레그 메인 제어 장치에 의해서 빠르게 변화시킬 수 있으므로, 수법 1은 단기적인 대응에 적합하다. 단, 실제 전 지면 반력 모멘트를 크게 변화시키면, 풋부(22R(L))의 접지압 분포가 기울어 접지감이 감소하고, 최악의 경우에는 풋부(22R(L))의 일부가 떠버린다. 따라서, 장기적으로는, 가능한한 원래의 목표 전 지면 반력 모멘트로 되돌려야 한다.

실제 전 지면 반력 모멘트를 원래의 목표 전 지면 반력 모멘트로 되돌리기 위해서는, 중심 위치를 어긋나게 해(이 경우는 후방으로 밀리게 한다), 목표 중력 모멘트에 의해서 상기 편차를 부정하도록, 수법 2에 의해서 목표 작업 패턴의 운동 패턴을 수정하면 된다. 단, 중심 위치를 급격히 밀리게 하면, 과대한 목표 관성력 모멘트가 역방향으로 발생하므로, 천천히 중심 위치를 밀리게 할 필요가 있다. 따라서, 수법 2는 장기적인 대응에 적합하다.

종래 기술에서 기술한, 본 출원인이 먼저 제안한 일본국 특개평 10-230485호 공보 기재의 기술은, 이들 양쪽의 수법을 동시에 행하여, 단기적으로는 수법 1을 주로 사용함으로써 빠른 변화에 대응하고, 장기적으로는 수법 2를 주로 사용함으로써 실제 전 지면 반력 모멘트를 원래의 목표 전 지면 반력 모멘트에 수속시키는 것이었다.

이에 대해, 본 발명에 관한 자세 제어 장치에 있어서는, 또한, 실대상물 반력 성분 내의 구속 방향 성분을 의도적으로 변화시키는 수법(수법 3)을 추가하도록 했다.

이하, 수법 3의 원리에 관해서 설명한다. 우선, 이해의 편의를 위해, 1개의 아암부 링크를 구비한 레그식 이동 로봇이, 그 아암부 링크의 선단에 배치된 핸드를 이용해 어느 대상물에 대해 작업을 하고 있는 경우를 생각한다. 이 때, 핸드는 대상물로부터 구속력을 받아, 구속 운동을 하는 것으로 한다.

대상물이 핸드에 의해서 공중에 지지되어, 핸드 이외의 것에 의해서 지지되지 않는다 하면, 핸드의 운동에는 3차원의 평행 이동의 자유도와 3차원의 회전 자유도의 합계 6개의 자유도가 있어, 대상물로부터는 하등 구속력을 받지 않는다.

이에 대해, 대상물이 핸드 이외의 것으로부터도 지지되면, 경우에 따라서는, 핸드의 운동은 핸드 이외의 지지되는 것에 의해서 구속력을 받는다. 구체적으로는, 핸드가 대상물, 예를 들면 정지한 강성이 높은 표면에 접촉하면, 핸드는 그 면을 관통해 움직이는 것은 불가능하고, 고로 자연스러운 위치의 구속이 발생하는, 바꾸어 말하면, 그 면으로부터 구속력을 받게 된다. 이 발명에 관한 자세 제어 장치는, 그 구속력에 착안하여, 로봇의 자세 안정화를 도모하는 제어를 하도록 했다.

설명을 계속하기 전에, 여기서, 설명에 이용하는 좌표계, 각종 공간이나 방향을 이하와 같이 정의한다. 또한, 이하에 기재하는 변수는, 특별히, 양해가 없는 한,지지 레그 좌표계로 표시되는 것으로 한다.

또한, 핸드가 어느 순간에서의 X, Y 및 Z 방향으로의 속도 성분을 각각 Vx, Vy, Vz로 한다. 같은 순간에서의 X, Y 및 Z축 방향의 회전 속도 성분을 각각 ωx, ωy, ωz로 한다. 회전은, 좌표축의 정(正)의 방향을 향해 시계 방향으로 회전 하는 방향을 정으로 한다.

이 순간에서의 핸드의 속도를, 벡터(Vx, Vy, Vz, ωx, ωy, ωz)로 표시하는 것으로 하고, 이를 「핸드 속도 벡터」라고 부른다.

그리고, 임의의 실수 Vx, Vy, Vz, ωx, ωy, ωz를 요소로 하는 핸드 속도 벡터(Vx, Vy, Vz, ωx, ωy, ωz)의 집합을 「전 운동 속도 공간」이라고 부른다. 전 운동 속도 공간은, 소위 벡터 공간으로 되어 있다.

또한, 어느 순간에 실현 불가능한 핸드 속도 벡터 모든 집합을, 이 순간의 「구속 속도 영역」이라고 부른다. 여기서 말하는 실현 불가능한 핸드 속도 벡터는, 기하학적인 관점에서 판정된다. 즉, 이 순간에 핸드와 대상물의 사이의 간섭에 의해, 거의 핸드의 변위를 발생시키지 않고 응력이 발생하는 핸드 속도 벡터를, 실현 불가능한 핸드 속도 벡터로 정의한다. 이 때, 아암 액츄에이터나 아암의 관절 배치 등에 기인하는 제약은 생각하지 않는다.

즉, 핸드 자체는, 대상물이 없으면 임의의 속도로 이동가능한 것을 전제로 한다. 또한, 이 때, 핸드 및 대상물이 충분한 강성을 구비하고 있으면, 강체로 간주한다. 따라서, 어느 핸드 속도 벡터(Vx1, Vy1, Vz1,ωx1, ωy1, ωz1)가 구속 속도 영역의 요소이면, 임의의 정의 실수(k)를 곱한 것도(kVx1, kVy1, kVz1, kωx1, kωy1, kωz1) 구속 속도 영역의 요소로 한다. 단, 편의상, 제로 벡터(0, 0, 0, 0, 0, 0)도 구속 속도 영역의 요소로 한다.

여기서 주의해야 할 점은, 구속 속도 영역을, 구속 조건하에서의 이동가능한 속도 영역과 혼동하지 않는 것이다. 또한, 구속 속도 영역은, 소위 벡터 공간에 한정되지 않는다. 예를 들면, 지면에 재치된 충분히 단단한 대상물을 핸드로 잡은 경우, 핸드를 바로 아래(지면의 법선 방향 아래쪽)로 내리는 것은 할 수 없지만, 역방향(법선 방향 위쪽)으로는 이동할 수 있다.

즉, 바로 아래 방향으로의 핸드 속도 벡터는 구속 속도 영역의 요소인데, 그 역방향은 구속 속도 영역의 요소로 되어 있지 않다. 구속 속도 영역이 벡터 공간이면, 구속 속도 영역의 임의의 요소(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1)에 대해, 역 벡터(-Vx1, -Vy1, -Vz1, -ωx1, -ωy1, -ωz1)도 구속 속도 영역의 요소가 아니면 안되는데, 이 경우의 구속 속도 영역은, 이 조건을 만족하지 않으므로, 벡터 공간이 아니다.

다음에, 구속 속도 공간을 이하와 같이 정의한다. 즉, 구속 속도 영역의 임의의 벡터(Vx1, Vyl, Vz1, ωx1, ωy1, ωz1)와 역 벡터(-Vx1, -Vyl, -Vz1, -ωx1 ,-ωy1, -ωz1)의 집합을, 구속 속도 공간으로 정의한다. 통상은, 구속 속도 공간은, 전 운동 속도 공간의 부분 벡터 공간으로 되어 있다.

전 운동 속도 공간 내의 벡터 내에서, 구속 속도 영역의 임의의 벡터와의 내적이 제로인 (즉 그 벡터와 직교한다) 모든 벡터의 집합을 「자유 속도 공간」이라고 부른다.

여기서, 구속 속도 공간 및 자유 속도 공간 내의 벡터를 다음과 같이 정의한다. 구속 속도 공간의 요소인 방향 벡터를 「구속 방향 벡터」라고 부르고, 단지 구속 방향이라고 할 때는, 임의의 구속 방향 벡터의 방향을 가리킨다. 또한, 자유 속도 공간의 요소인 방향 벡터를 「자유 방향 벡터」 라고 부르고, 단지 자유 방향 이라고 할 때는, 임의의 자유 방향 벡터의 방향을 가리킨다.

이하에 몇 개의 대상물에 대한 작업에 관해서 구속 속도 공간을 나타낸다.

작업1) 대상물을 핸드에 의해서 공중에 들어 올리는 경우

이 경우, 구속 속도 공간은 존재하지 않는다. 또한, 편의상, 구속 속도 공간은 제로 벡터만으로 이루어지는 공간으로 한다. 자유 속도 공간은 전 운동 속도 공간에 일치한다.

작업2) 상기한 도 9에 도시하는 바와 같이, 자재 캐스터륜이 붙은 대차(108)를 핸드(40R)로 잡아 미는 경우

이 경우, 구속 속도 공간은, 임의의 실수(Vz, ωx, ωy)를 가지는 핸드 속도 벡터(0, 0, Vz, ωx, ωy, 0)의 집합이다.

작업3) 도 10에 도시하는 바와 같이, 핸드(40R)로 손잡이(110)를 잡으면서, 힌지(112)가 붙은 도어(114)를 개폐하는 경우

이 경우, 힌지축(112Z) 방향의 회전 이외의 운동은 구속된다. 힌지축을 Z축으로 하고, 이 순간의 핸드의 X 좌표를 0, Y 좌표를 -r(r은 도어(114)의 회전 반경, 보다 자세히는 손잡이(110)의 회전 반경)로 하면 , 자유 속도 공간은, 임의의 실수(Vx)를 가지는 핸드의 속도 벡터(Vx, 0, 0, 0, 0, Vx/r)의 집합이다.

또한, 구속 속도 공간은, 임의의 실수(Vx, Vy, Vz, ωx, ωy)를 가지는 핸드 속도 벡터(Vx, Vy, Vz, ωx, ωy, -r*Vx)의 집합이다.

작업 4) 도 11에 도시하는 환경에서, 원주상의 난간(120)에 핸드(40R)를 슬라이드시키면서 계단(122)을 승강하는 경우

이 경우, 난간(120)의 축 방향의 평행 이동과, 난간(120)의 축방향의 회전 이외의 운동은 구속된다. 난간(120)의 축이 식 x= z+c, y=-d(c 및 d는 정수)로 표시된다고 하면, 자유 속도 공간은, 임의의 실수(Vx 및 ωx)를 가지는 핸드 속도 벡터(Vx, 0, Vx, ωx, 0, ωx)이다. 또한, 구속 속도 공간은, 임의의 실수(Vx, Vy, ωx, ωy)를 가지는 핸드 속도 벡터(Vx, Vy, -Vx, ωx, ωy, -ωx)의 집합이다. 또한, 이해의 편의를 위해, 난간(120)과 X축이 이루는 각도를 45도로 한다.

작업5) 도 11에 도시하는 환경에서, 난간(120)을 핸드(40R)로 꽉 잡아 계단(122)을 승강하는 경우,

난간(120)을 꽉 잡을 때에는 운동의 자유도는 없고, 모든 운동이 구속된다. 따라서, 구속 속도 공간은, 전 운동 속도 공간에 일치한다.

상기와 같은 작업에 있어서, 핸드로부터 대상물에 대해 핸드의 구속 방향으로 힘을 작용시켰을 때, 대상물에는 핸드 이외의 지지하는 것으로부터 그 힘을 부정하는 구속력밖에 발생하지 않고, 자유 방향으로 실질적으로 마찰력 등이 발생하지 않으면, 핸드 및 대상물의 운동은 변화하지 않는다.

작업 3(도 10)을 예로 들면, 핸드(40R)에 의해서 도어(114)에 상향(Z축 상방)의 힘을 작용시켜도, 힌지(112)의 마찰이 실질적으로 제로이면, 힌지(112)로부터 도어(114)에 상기 상향의 힘을 부정하는 힘만이 작용하므로, 도어(114)의 회전 운동에 아무런 영향도 주지 않는다.

본 발명에 관한 자세 제어 장치는, 이러한 성질을 이용하는 것으로, 대상물의 운동 제어에 간섭하지 않고, 로봇의 자세를 안정화하고자 하는 것이다. 즉, 대 상물로부터 핸드에 작용하는 힘의 구속 방향 성분을 제어함으로써, 대상물의 운동에 영향을 주지 않고, 로봇의 운동 밸런스를 유지하거나, 기울어진 자세를 복원하도록 구성했다.

이하에, 작업 3을 예로 들어, 이 작용에 관해서 설명한다. 전술한 도 10에 도시하는 바와 같이, 로봇(1)이 도어(114)의 앞에 서서 그 개폐 작업을 행할 때, 로봇(1)이 앞으로 전도되려는 상황을 생각한다. 로봇(1)의 자세를 복원시키기 위한 하나의 방법은, 도어(114)를 앞으로 미는 것이다. 이에 따라 로봇(1)은 도어(114)로부터 반력을 받아, 자세가 복원된다.

그러나, 자세 안정화 제어를 위해 도어(114)를 밀어 버리면, 도어(114)는 가속되어, 그 운동은 원하는 개폐 운동으로부터 어긋나 버리므로, 그다지 큰 복원력을 발생시키는 것은 불가능하다. 특히, 로봇(1)의 질량에 비교해 도어의 질량이 매우 작은 경우에는, 거의 복원력을 발생시킬 수 없다. 또한, 도어(114)의 운동이 소기의 예정으로부터 변경되므로, 작업 수행상 바람직하지 않다.

로봇(1)의 자세를 복원시키기 위한 또 하나의 방법은, 도어(114)를 아래로 미는 것이다. 이에 따라 로봇(1)은 도어(114)로부터 반력을 받아, 자세가 복원된다. 또한, 구속 방향으로 밀고 있을 뿐이므로, 전술과 같이 도어의 운동은 변하지 않는다.

본 발명에 관한 자세 제어 장치는 후자의 방법을 이용하는 것이므로, 상기 작업 2, 작업 3 및 작업 4와 같이 핸드의 운동에 어떠한 구속 방향이 존재하는 것이, 본 발명을 적용하기 위한 필요 조건이 된다. 따라서, 상기 작업 1에 대해서는 적용할 수 없다.

또한, 구속 속도 공간 등을 나타내는 좌표계는, 원통 좌표계나 극좌표계 등이어도 된다. 통상의 좌표계라면 무엇을 이용해도 이들 사이에서 변환도 가능한 동시에, 그 순간에서의 구속 속도 공간이 가리키는 공간에 상위는 없기 때문이다. 작업에 따라 좌표계를 적절히 설정함으로써, 작업 중, 구속 속도 공간을 일정(보편)하게 할 수 있는 경우도 있다. 예를 들면, 상기 작업 3에서는, 원통 좌표계를 이용하여 힌지(112)의 축을 원통 좌표계의 축에 일치시킴으로써, 각 순간에서의 구속 속도 공간을 일정하게 유지할 수 있다.

이어서, n개의 아암을 구비하는 로봇이 작업을 행하는 경우에 관해서 생각한다. 이 경우, 각종 공간이나 방향의 정의를, 다음과 같이 확장한다.

우선, 하기 식과 같이, 각 핸드에 1부터 n까지의 식별 번호를 붙이고, 각각을 「제 j 핸드」(j=1, 2, …, n)라고 불러 구별한다.

좌표계는 상기한 지지 레그 좌표계를 이용하는 것으로 하고, 제 j 핸드의 어느 순간에서의 X, Y 및 Z 방향에의 속도 성분을, 각각 Vxj, Vyj, Vzj로 한다. 또한, 동일한 순간에서의 X, Y 및 Z축 방향의 회전 속도 성분을, 각각 ωxj, ωyj, ωzj로 한다.

그리고, 벡터(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2, ωz2, …, Vxn, Vyn, Vzn, ωxn, ωyn, ωzn)를 「전 핸드 속도 벡터」라고 부른다. 이 벡터는, 제1 핸드로부터 제n 핸드까지의 핸드 속도 벡터의 각 요소를 순서대로 배열한 것이다. 또한, 이 벡터 표현 대신에, j행째가 제 j 핸드의 속도 벡 터로 되어 있는, n행 6열의 행렬로 표시되어도 된다.

이후, 전 핸드 속도 벡터를 다시 「핸드 속도 벡터」라고 부른다. 또한, 임의의 실수를 요소로 하는 핸드 속도 벡터의 집합을, 다시 「전 운동 속도 공간」이라고 부른다. 이 전 운동 속도 공간은, 6*n 차원 벡터 공간으로 되어 있다.

또한, 어느 순간에 실현불가능한 핸드 속도 벡터 모든 집합을, 다시 이 순간의「구속 속도 영역」이라고 부른다. 여기서 말하는 실현불가능한 핸드 속도 벡터는, 기하학적인 관점에서 판정된다. 즉, 이 순간에, 핸드와 대상물 사이 또는 핸드와 다른 핸드 사이의 간섭에 의해, 거의 핸드의 변위를 발생시키지 않고 응력이 발생하는 핸드 속도 벡터를 실현불가능한 핸드 속도 벡터로 정의한다.

이 때, 아암 액츄에이터 능력이나 아암부 링크(5)의 관절 배치 등에 관한 제약은 생각하지 않는다. 즉, 핸드 자신은, 대상물이 없으면 임의의 속도로 이동 가능한 것을 전제로 한다. 또한, 핸드 및 대상물이 충분한 강성을 구비하고 있으면, 강체로 간주한다. 따라서, 어느 속도 벡터가 구속 속도 영역의 요소 벡터이면, 그 벡터의 전 요소를 k배(k는 임의의 정의 실수)한 벡터도, 구속 속도 영역의 요소 벡터로 한다. 또한, 편의상, 제로 벡터도 구속 속도 영역의 요소 벡터로 한다.

그리고, 구속 속도 영역의 임의의 벡터와 그 역 벡터(크기가 동일하고 방향이 반대인 벡터)로 이루어지는 집합을 다시 「구속 속도 공간」으로 정의한다. 통상은, 구속 속도 공간은, 전 운동 속도 공간의 부분 벡터 공간으로 되어 있다.

또한, 구속 속도 영역의 임의의 벡터와의 내적이 제로인(즉, 그 벡터와 직교한다) 모든 전 운동 속도 공간 내의 벡터의 집합을, 다시 「자유 속도 공간」이라 고 부른다.

여기서, 구속 속도 공간 및 자유 속도 공간 내의 벡터를, 이하와 같이 정의한다. 구속 속도 공간의 요소인 방향 벡터를, 다시 「구속 방향 벡터」라고 부르고, 단순히 구속 방향이라고 할 때는, 임의의 구속 방향 벡터의 방향을 가리킨다. 또한, 자유 속도 공간의 요소인 방향 벡터를, 다시 「자유 방향 벡터」라고 부르고, 단순히 자유 방향이라고 할 때는, 임의의 자유 방향 벡터의 방향을 가리킨다.

대상물 및 핸드의 운동의 자유도와 구속에 관한 정의를, 상기와 같이 확장한다. 이에 따라, 다수개의 아암을 구비하는 로봇이 다수개의 아암을 사용해 작업을 하는 경우라도, 전 핸드를 구속 방향으로 움직여 핸드에 대상물로부터 반작용(구속력)을 받게 함으로써, 모든 대상물의 운동에 영향을 주지 않고, 로봇의 운동 밸런스를 유지하거나, 기울어진 자세를 복원할 수 있다.

이하에, 다수개의 아암에 의한 작업의 예를 나타낸다. 우선, 도 12에 도시하는 바와 같이, 대상물이, 높이(h), 전방(b)에 존재하는 Z축 방향의 회전만이 가능한, 반경(r)의 원형인 휠(130)로서, 이 휠(130)을 제1 핸드와 제2 핸드의 계 2개의 핸드로 잡아 회전시키는 작업을 예로 든다.

어느 순간의 제1 핸드의 좌표가 (b, -r, h)이고, 제2 핸드의 좌표가 (b, r, h)인 것으로 한다. 이 순간의 자유 속도 공간은, 임의의 실수(ωz1)를 가지는 핸드 속도 벡터(r*ωz1, 0, 0, 0, 0, ωz1, -r*ωz1, 0, 0, 0, 0, ωz1)의 집합이다. 이는, 대상물의 축 방향에 2개의 핸드를 동일한 각도만큼 회전시키는 운동을 의미한다.

이 순간의 구속 속도 공간은, 임의의 실수(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2)를 가지는 핸드 속도 벡터(Vx1, Vyl, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2, -r*Vx1-ωz1+r*Vx2)의 집합이다. 예를 들면, 핸드 속도 벡터(Vx1, 0, 0, 0, 0, 0, Vx1, 0, 0, 0, 0, 0)는 구속 속도 공간의 속도 벡터이다. 이는 휠(130)의 축의 마찰이 제로이면, 양 핸드를 X 방향으로 동일한 힘으로 밀어도, 휠(130)의 회전 운동에 영향을 주지 않는 것을 의미한다.

별도의 예를 도 13을 참조하여 설명한다. 동 도면에 도시하는 바와 같이, 제1 핸드로 고정물(예를 들면 기둥)(140)을 잡으면서, 제2 핸드로 힌지(142)가 붙은 도어(144)를 개폐하는 작업을 예로 든다.

힌지(142)의 축(142Z) 방향이 Z축과 평행하고, 힌지축(142Z)의 X좌표가 b, 힌지축의 Y 좌표가 r, 이 순간의 제2 핸드의 좌표를 (b, 0, h)로 한다. 제1 핸드의 좌표를 (b, -c, h)로 하면, 자유 속도 공간은, 임의의 실수(ωz2)를 가지는 핸드 속도 벡터(0, 0, 0, 0, 0, 0, r*ωz2, 0, 0, 0, 0, ωz2)의 집합이다.

또한, 구속 속도 공간은, 그에 직교하는 벡터 집합이므로, 임의의 실수(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2)를 가지는, 핸드 속도 벡터(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, Vx2, Vy2, Vz2, ωx2, ωy2, -r*Vx2)의 집합이 된다.

예를 들면, 핸드 속도 벡터(Vx1, Vy1, Vz1, ωx1, ωy1, ωz1, 0, 0, 0, 0, 0, 0)는, 구속 속도 공간의 속도 벡터이다. 이는 제1 핸드로 고정물(140)을 어떻게 밀어도, 도어(144)의 회전 운동에 영향을 주지 않는 것을 의미한다.

상기와 같이, 다수의 핸드로부터 대상물에 대해 구속 방향으로 힘을 작용시켜도, 대상물에는 핸드 이외의 지지하는 것으로부터 그 힘을 부정하는 구속력 밖에 발생하지 않고, 따라서 자유 방향으로 마찰력 등을 발생하지 않으면, 핸드 및 대상물의 운동은 변화하지 않는다.

따라서, 상기한 바와 같이 정의를 확장함으로써, 다수의 핸드에 의한 작업에 있어서도, 1개의 핸드의 경우와 동일한 원리로, 대상물의 운동 제어에 간섭하지 않고, 로봇의 자세를 안정되게 할 수 있다. 즉, 다수의 핸드에 의한 작업에 있어서도, 대상물로부터 핸드에 작용하는 힘의 구속 방향 성분을 제어함으로써, 대상물의 운동에 영향을 주지 않고, 로봇의 운동 밸런스를 유지하거나, 기울어진 자세를 복원할 수 있다.

또한, 일반적으로, 핸드가 다수, 예를 들면 본 실시 형태와 같이 2개가 되면, 구속 속도 공간의 차원이 핸드가 1개인 경우보다도 높아지므로, 이 중에서 로봇의 자세 안정화에 효과가 높은 부분 벡터 공간을 골라내, 그 요소 벡터의 방향으로 구속력을 발생시킴으로써, 핸드가 1개인 경우보다 높은 효과를 얻을 수 있다.

도어의 개폐 작업의 예로 말하면, 도어의 개폐 제어에 간섭하지 않도록 로봇의 자세를 복원시키기 위해서는, 1개의 아암의 경우에는 도어에 상하 방향의 힘을 작용시키는 것이 가장 효과적인데, 2개의 아암의 예에서는, 도어를 잡지 않은 핸드로 고정물을 밀도록 함으로써, 보다 효과적으로 운동 밸런스를 유지하거나, 기울어진 자세를 복원할 수 있다.

이상의 원리를 전제로 하여, 도 5에 도시하는 자세 안정화 메인 제어 장치 (106)의 설명으로 되돌아간다.

자세 안정화 메인 제어 장치(106)는, 상기한 3개의 수법, 즉,

1) 실제 전 지면 반력 모멘트를 수정한다

2) 목표 관성력 모멘트와 목표 중력 모멘트를 수정한다

3) 실대상물 반력의 성분 내의 구속 방향 성분을 수정한다

의 3개의 수법을 병용하므로, 도 14에 도시하는 바와 같이 구성된다.

즉, 자세 안정화 메인 제어 장치(106)는, 자세 안정화용 보상 모멘트 산출기(200), 대상물 반력 평형 제어 장치(202), 대상물 반력 모멘트 편차 분리기(204), 수정 목표 대상물 반력 산출기(206) 및 각종 분배기(구체적으로는, 대상물 반력 평형 제어용 분배기(208)와 보상 모멘트 분배기(210))로 구성된다.

자세 안정화 메인 제어 장치의 각 구성 요소는 모두, 제어 주기마다(예를 들면 100msec 마다) 연산이 행해진다. ECU(62)의 성능 상, 동시에 다수의 연산 처리가 불가능한 경우에는, 도 14의 화살표 상류측에서 순서대로 연산 처리를 실행하면 된다.

이하, 자세 안정화 메인 제어 장치(106)로 처리되는 변수 및 이들 구성 요소에 관해서 설명(정의)한다.

상술한 바와 같이, 본 발명에 관한 자세 제어 장치는, 대상물 반력의 구속 방향 성분을 제어함으로써, 대상물의 운동에 영향을 주지 않고, 로봇(1)의 운동 밸런스를 유지하거나, 기울어진 자세를 복원하는 제어 장치에 관한 것이다. 그러나, 대상물 반력의 모든 구속 방향 성분이, 로봇(1)의 운동 밸런스를 유지하거나 기울 어진 자세를 복원하는 효과를 가지고 있다고는 할 수 없다.

또한, 어느 구속 방향의 목표 대상물 반력이, 작업 목적이나 대상물의 성질 상, 변경해야 하는 것이 아닌 경우도 있다. 즉, 대상물 반력의 구속 방향 성분 중에서, 로봇(1)의 운동 밸런스를 유지하거나, 기울어진 자세를 복원하기 위해서 이용해야 할 성분을, 복원하는 효과의 형편, 작업 목적 및 대상물의 성질을 고려하여 적절히 선정해야 한다.

그래서, 새롭게, 다음 공간과 방향을 정의한다. 도 24를 참조하여 설명하면, 우선, 전 운동 속도 공간 중에서, 로봇(1)의 운동 밸런스를 유지하거나, 기울어진 자세를 복원하기 위해서 이용해야 할 모든 구속 방향에 의해서 뻗어지는 벡터 공간을 「조작 공간」이라고 부른다. 조작 공간은, 구속 속도 공간의 부분 벡터 공간으로 되어 있다. 또한, 조작 공간의 임의의 방향 벡터를 「대상물 반력 조작 방향」 혹은 단순히 「조작 방향」이라고 부른다. 도시와 같이, 조작 공간은, 구속 속도 공간에서 작업 공간을 제외한, 흰 바탕 부분에 상당한다.

전 운동 속도 공간의 방향 벡터 중, 임의의 대상물 반력 조작 방향과의 내적이 제로인 (즉, 그 방향과 직교한다) 것을 「대상물 반력 비조작 방향」혹은 단순히 「비조작 방향」이라고 부르고, 모든 비조작 방향이 뻗는 공간을 「비조작 공간」이라고 부른다.

즉, 비조작 공간은, 전 운동 속도 공간의 부분 벡터 공간으로 되어 있고, 자유 속도 공간은, 비조작 공간의 부분 벡터 공간으로 되어 있다. 또한, 조작 공간의 임의의 벡터와 비조작 공간의 임의의 벡터는 직교한다.

또한, 조작 방향은, 컴플라이언스 제어의 컴플라이언스 정수 등과 마찬가지로, 제어 파라미터의 일종으로 생각된다. 이 값은, 일반적으로는 일정하지 않고, 작업이 진행함에 따라서 변화하는 성질의 것이다. 이 값을 어떻게 결정해야할 지도 중요하지만, 이 발명에 관한 자세 제어 장치의 본질이 아니므로, 이 실시의 형태에서는 설명의 편의상, 조작 방향은 미리 작업 목적, 작업 패턴 및 대상물의 성질에 따라서 결정되는 것으로 한다(조작 방향 등은 작업 중에 결정해도 된다).

이상을 전제로 하여 자세 안정화 메인 제어 장치(106)의 상기한 구성 요소에 대해서 상세히 설명한다.

자세 안정화용 보상 모멘트 산출기(200)는, 로봇(1)의 자세 기울기 편차, 보다 구체적으로는 실제 상체 기울기와 목표 상체 기울기의 차이인 상체 기울기 편차를 제로에 수속시키기 위한 자세 안정화용 보상 모멘트(실제 전 지면 반력 모멘트의 수정에 사용하는 모멘트)를 산출하는 장치이고, 상기 수법 1)를 달성하기 위한 구성이다. 이 장치는, 적어도, 경사 센서(60)에 의해서 검출된 상체 경사 각도와 목표 작업 패턴 생성기(100)가 생성한 목표 상체 경사 각도의 편차에 따라, 그 편차를 감소하도록 자세 안정화용 보상 모멘트를 산출한다.

도 15에, 자세 안정화용 보상 모멘트 산출기(200)를 가장 단순한 구성으로 도시한다. 도시의 예에서는, 입출력과 함께 1차원의 스칼라(scalar)량과 같이 나타나 있지만, 실제로는, X 방향 성분과 Y 방향 성분이 있어, 각각에 대해, 도시의 처리가 행해진다. 단, Z 방향 성분은, 자세 안정성에 관계가 없으므로 무시하기로 한다.

이하, 설명하면, 우선 실제 상체 기울기와 목표 상체 기울기의 차이인 상체 기울기 편차를 구한다. 다음에, 자세 안정화 제어 측부(200a)에서 상체 기울기 편차의 미분치를 구하고, 마지막에, 식 1에 따라 자세 안정화용 보상 모멘트를 산출한다.

자세 안정화용 보상 모멘트 =-kp*상체 기울기 편차

-kd*상체 기울기 편차의 미분치 …식1

여기서, kp와 kd는 제어 게인이다. 또한, 이외에도, H 무한대(infinity) 제어 등을 이용해도 된다.

도 14로 되돌아가 설명을 계속하면, 대상물 반력 평형 제어 장치(202)는, 검출은 가능하지만, 예기할 수 없는 대상물 반력을 받았을 때, 그 대상물 반력을 고려하면서 목표 상체 위치 자세를 수정함으로써, 목표 작업 패턴 생성기(100)에서 생성된 목표 관성력 모멘트와 목표 중력 모멘트를 수정하여 운동 밸런스를 유지하는 제어를 행하는 장치로서, 상기 수법 2)을 달성시키기 위한 구성이다.

또한, 대상물 반력 모멘트 편차 분리기(204)는, 대상물 반력을 자세 안정화에 이용하는 구속 방향의 성분과 그 이외의 성분으로 분리하는 장치이고, 수정 목표 대상물 반력 산출기(206)는 대상물 반력에 자세 안정화에 필요한 목표 구속력을 추가한 목표 대상물 반력을 산출하는 장치이다.

이들 2개의 장치(204, 206)에 각종 분배기(대상물 반력 평형 제어용 분배기(208)와 보상 모멘트 분배기(210))를 추가한 것이, 상기 수법 3)을 달성하기 위한 구성이다. 또한, 이들 장치의 상세한 것은 후술한다.

우선, 대상물 반력 모멘트 편차 분리기(204)를 설명하면, 핸드(40R(L))의 역 센서(58)로 검출되는 실 핸드력은, 핸드 기준점에 작용하는 힘과 힘의 모멘트를, 핸드 내에 국부적으로 설정된 좌표계로 표현한 것으로 한다. 여기서, 핸드 기준점은, 핸드(40R(L))의 위치를 나타내기 위한 핸드 내의 기준점이다. 또한, 핸드 내에 국부적으로 설정된 상기 좌표계의 원점은, 핸드 기준점에 설정한다.

그런데, 핸드 위치 자세는, 핸드 내에 국부적으로 설정된 상기 좌표계를 지지 레그 좌표계에서 보았을 때의 원점 위치와 좌표계의 방향을 나타낸다. 보다 구체적으로는, 상기 원점 위치는 벡터로, 상기 좌표계의 방향은 3행 3열 매트릭스로 표현된다. 혹은, 로봇 공학으로 흔히 이용되는 표현인, 상기 원점 위치와 상기 좌표계의 방향을 정리하여 표현하는 동차(同次) 행렬을 이용해도 된다.

또한, 핸드(40R(L))의 실제 위치 자세는, 상기한 아암 메인 제어 장치(104)에 의해 액츄에이터 변위 지령을 통해 구동됨으로써, 거의 최종 수정 목표 핸드 위치 자세에 있다고 생각되므로, 핸드 내에 국부적으로 설정된 상기 좌표계는, 최종 수정 목표 핸드 위치 자세를 기준으로 한 좌표계라고 생각해도 된다.

도 16은 대상물 반력 모멘트 편차 분리기(204)의 구성을 도시하는 블록도이다. 동 도면을 참조하여 설명하면, 대상물 반력 모멘트 편차 분리기(204)는 실대상물 반력 모멘트 성분 분리기(204a)를 구비하고, 그 분리기(204a)는 지지 레그 좌표계에서 본 최종 수정 목표 핸드 자세를 입력하고, 실 핸드력을 지지 레그 좌표계의 값으로 변환한다. 지지 레그 좌표계로 변환된 실 핸드력도, 핸드 기준점에 작용하는 힘과 힘의 모멘트로 표현된다. 또한, 지지 레그 좌표계로 변환된 실 핸드 력의 작용점을 전 지면 반력 중심점으로 변환한 것이, 실대상물 반력이다. 즉, 이들은 표현은 다르지만, 동일한 것을 의미한다.

핸드가 n개 있는 경우, 지지 레그 좌표계로 변환된 실 핸드력은 각 핸드의 실 핸드력의 각 성분을 순서대로 배열한 1개의 벡터로 표현된다. 즉, 전 운동 속도 공간에서의 벡터로 표현된다. 보다 구체적으로는,

(F1x, F1y, F1z, M1x, M1y, M1z, F2x, F2y, F2z, M2x, M2y, M2z, …, Mnz)의 형으로 표현된다. 단, 여기서, Fjx, Fjy, Fjz는 제 j 핸드의 기준점에 작용하는 힘의 x, y, z 성분, Mjx, Mjy, Mjz는 제 j 핸드의 기준점에 작용하는 힘의 모멘트의 x, y, z 성분을 나타낸다.

다음에, 분리기(204a)는, 지지 레그 좌표계로 변환한 실 핸드력을, 미리 선정된 조작 방향에 따라서 조작 방향 성분과 비조작 방향 성분으로 분리한다. 또한, 여기서 주의해야 할 점은, 각 성분은, 전술한 바와 같이 전 운동 속도 공간의 벡터로서, 현실의 3차원 공간의 벡터가 아닌 것이다.

구체적으로는, 분리기(204a)는, 다음과 같은 연산에 의해서 분리한다. 우선, 조작 공간이 m차원이라고 하면, 조작 공간의 직교 기저 벡터를 A1, A2, …, Am으로 한다. 또한, 비조작 공간의 직교 기저 벡터를 B1, B2, …, Be로 한다. 여기서, m과 e의 합은 핸드 개수의 6배이다.

이어서, 지지 레그 좌표계로 변환된 실 핸드력의 벡터를, F로 기술한다.

또한, 실 핸드력의 조작 방향 성분을 Fa, 실 핸드력의 비조작 방향 성분을 Fb로 기술한다. 이들 벡터는 전 운동 속도 공간의 벡터이다.

실 핸드력의 조작 방향 성분(Fa)과 실 핸드력의 비조작 방향 성분(Fb)은 식 2에 의해서 구해진다.

Fa=(A1+A2+…+Am) * F

Fb=(B1+B2+…+Be) *F …식 2

다음에, 분리기(204a)는, 최종 수정 목표 핸드 위치 자세 및 실 핸드력의 조작 방향 성분(Fa)에 의거하여, 실 핸드력의 조작 방향 성분(Fa)의 모든 성분이, 목표 전 지면 반력 중심점 방향에 작용하는 힘의 합을 구한다. 이를 「목표 전 지면 반력 중심점 방향의 실대상물 반력 조작 방향 성분 합」이라고 부른다. 이 힘은 3차원 공간에서의 힘과 힘의 모멘트로 표현된다. 좌표계로는 지지 레그 좌표계를 이용한다. 또한 목표 전 지면 반력 중심점 방향의 실대상물 반력 조작 방향 성분 내의 모멘트 성분을 「목표 전 지면 반력 중심점 방향의 실대상물 반력 모멘트 조작 방향 성분 합」이라고 부른다.

다음에, 분리기(204a)는, 최종 수정 목표 핸드 위치 자세 및 실 핸드력의 비조작 방향 성분(Fb)에 의거하여, 실 핸드력의 비조작 방향 성분(Fb)의 모든 성분이, 목표 전 지면 반력 중심점에 작용하는 힘의 합을 구한다. 이를 「목표 전 지면 반력 중심점 방향의 실대상물 반력 비조작 방향 성분 합」이라고 부른다. 이 힘도, 3차원 공간에서의 힘과 힘의 모멘트로 표현된다. 좌표계로는 지지 레그 좌표계를 이용한다. 또한 목표 전 지면 반력 중심점 방향의 실대상물 반력 비조작 방향 성분 내의 모멘트 성분을 「목표지면 반력 중심점 방향의 실대상물 반력 모멘트 비조작 방향 성분 합」이라고 부른다.

또한, 대상물 반력 모멘트 편차 분리기(204)는 목표 핸드 위치 작용점 변환기(204b)를 구비하고, 그 변환기(204b)도, 상기와 같은 처리(연산)를 목표 대상물 반력에 대해서도 행한다. 목표 대상물 반력도, 실 핸드력과 마찬가지로, 전 운동 속도 공간의 벡터이다. 구체적으로는 우선, 목표 대상물 반력은 목표 전 지면 반력 중심점을 작용점으로 하여 표현되어 있으므로, 변환기(204b)는, 이 작용점을 일단, 목표 핸드 위치(목표 핸드의 기준점 위치)를 작용점으로 하는 힘과 힘의 모멘트로 변환한다. 이를 「목표 핸드 위치를 작용점으로 하는 목표 대상물 반력」이라고 부른다.

또한, 대상물 반력 모멘트 편차 분리기(204)는 목표 대상물 반력 모멘트 성분 분리기(204c)를 구비하고, 분리기(204c)는, 상기와 같은 처리에 의해서 조작 방향 성분과 비조작 방향 성분으로 분리한다. 이들을 각각 「목표 핸드 위치를 작용점으로 하는 목표 대상물 반력의 조작 방향 성분」및「목표 핸드 위치를 작용점으로 하는 목표 대상물 반력의 비조작 방향 성분」이라고 부른다.

다음에, 분리기(204c)는, 목표 핸드 위치를 작용점으로 하는 목표 대상물 반력의 조작 방향 성분 및 목표 핸드 위치(목표 핸드의 기준점 위치)에 의거하여, 목표 대상물 반력의 조작 방향 성분의 모든 성분이, 목표 전 지면 반력 중심점에 작용하는 힘의 합을 구한다. 이를 「목표 전 지면 반력 중심점 방향의 목표 대상물 반력 조작 방향 성분 합」이라고 부른다. 이 힘은 3차원 공간에서의 힘과 힘의 모멘트로 표현된다. 좌표계로는 지지 레그 좌표계를 이용한다. 또한, 목표 전 지면 반력 중심점 방향의 목표 대상물 반력 조작 방향 성분 내의 모멘트 성분을 「목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 조작 방향 성분 합」이라고 부른다.

다음에, 분리기(204c)는, 목표 핸드 위치를 작용점으로 하는 목표 대상물 반력의 비조작 방향 성분 및 목표 핸드 위치(목표 핸드의 기준점 위치)에 의거하여, 목표 대상물 반력의 비조작 방향 성분의 모든 성분이, 목표 전 지면 반력 중심점에 작용하는 힘의 합을 구한다. 이를 「목표 전 지면 반력 중심점 방향의 목표 대상물 반력 비조작 방향 성분 합」이라고 부른다. 이 힘도, 3차원 공간에서의 힘과 힘의 모멘트로 표현된다. 좌표계로는 지지 레그 좌표계를 이용한다. 또한, 목표 전 지면 반력 중심점 방향의 목표 대상물 반력 비조작 방향 성분 내의 모멘트 성분을 「목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 비조작 방향 성분 합」이라고 부른다.

이어서, 가산점(204d)에서, 이렇게 하여 얻어진 목표 전 지면 반력 중심점 방향의 실대상물 반력 모멘트 조작 방향 성분 합으로부터 목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 조작 방향 성분 합을 뺌으로써, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합을 얻는다.

또한, 가산점(204e)에서, 목표 전 지면 반력 중심점 방향의 실대상물 반력 모멘트 비조작 방향 성분 합으로부터 목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 비조작 방향 성분 합을 뺌으로써, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합을 얻는다.

또한, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 내의 모멘트 성분 을 제외한 성분은, 로봇(1)의 자세에 그다지 관여하지 않으므로, 이 실시의 형태에서는 무시하기로 한다. 또한, 최종 수정 목표 핸드 위치 자세를 이용하는 대신, 레그부 링크(2) 및 아암부 링크(5)의 실관절각을 이용하여 키네마틱스 연산에 의해서 산출되는 실 핸드 위치 자세를 이용해도 된다.

도 14의 설명으로 되돌아가면, 상기한 바와 같이 하여 얻은 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합은, 후에 기술하는 바와 같이 보상 전 지면 반력 모멘트의 산출에 사용된다. 한편, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합은 대상물 반력 평형 제어용 분배기(208)에 입력된다.

이 대상물 반력 평형 제어용 분배기(208)를 도 17을 참조하여 설명하면, 분배기(208)는 함수(208a, 208b)를 구비하고, 대상물 반력 평형 제어용 분배기(208)는, 이들 함수(208a, 208b)에 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합을 입력하고, 대상물 반력 평형 제어로 평형을 취하는 대상물 반력 편차 모멘트 합과, 대상물 반력 평형 제어 이외로 평형을 취해야하는 대상물 반력 편차 모멘트 합을 출력시킨다.

또한, 대상물 반력 평형 제어란, 전술의 수법 2)를 달성하기 위해서 대상물 반력 평형 제어 장치(202)로 행해지는 제어로, 개략 설명하면, 상기한 바와 같이, 상체 위치를 어긋나게 해 중력 모멘트를 발생시키고, 따라서 대상물 반력 편차 모멘트 합을 부정하는 제어이다.

또한, 도 17에 도시하는 예에서는, 입출력 모두 1차원의 스칼라량과 같이 나 타나 있는데, 실제로는, X 방향 성분과 Y 방향 성분이 있어, 각각에 대해 도시하는 처리가 행해진다. 단, Z 방향 성분은, 자세 안정성에 관계가 없으므로 무시하기로 한다.

도시와 같이, 대상물 반력 평형 제어로 평형을 취해야하는 대상물 반력 편차 모멘트 합을 출력하는 함수(208a)로는, 상하한 리미터 특성(포화 특성)을 가진 함수를 이용하는 동시에, 대상물 반력 평형 제어 이외로 평형을 취해야하는 대상물 반력 편차 모멘트 합을 출력하는 함수(208b)로는 불감대 특성을 가진 함수를 이용한다.

이러한 함수 특성을 갖게 한 경우, 운동 밸런스를 유지하기 위해서, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합의 절대치가 어느 설정치 이하일 때, 그 모멘트를 대상물 반력 평형 제어에 의해서 부정하고, 그 모멘트가 설정치를 넘었을 때, 넘은 만큼을 주로 핸드의 구속 방향에 대상물 반력을 발생시켜 부정하도록 제어계가 작용한다. 즉, 상기 상하한 리미터 특성을 가지게 함으로써, 상체 위치의 편차량에 제한을 두게 해, 기하학적으로 무리한 자세가 되는 것을 방지할 수 있다.

또한, 입출력 관계를 결정하는 2개의 함수(208a, 208b)에는, 도시예 이외의 함수를 이용해도 된다. 예를 들면, 도 18에 208c, 208d로 표시하는 곡선이어도 된다. 도 17 및 도 18중 어느 것으로 해도, 로봇(1)의 특성, 대상물의 특성 및 작업 내용에 따라, 함수를 선정해야 한다.

또한, 입출력 관계를 결정하는 2개의 함수의 출력 합은, 기본적으로는 입력 에 일치시켜야 하는데, 엄밀히 일치시킬 필요는 없다. 특히, 레그 컴플라이언스 제어나 아암 컴플라이언스 제어는, 목표치의 절대치가 크면, 발생하는 힘이 목표에 대해 작아지기 쉬운 경향이 있으므로, 이를 보정하는 의미로, 입력의 절대치가 큰 영역에서는, 함수 출력의 합의 절대치를 입력의 절대치보다 크게 한 쪽이 좋은 경우도 있다. 또한, X 방향 성분과 Y 방향 성분을 독립하여 분배하는 것이 아니라, X 방향 성분과 Y 방향 성분의 양쪽을 입력으로 한 2입력 함수를 이용하여, 각 출력을 결정해도 된다.

도 14의 설명으로 되돌아가면, 대상물 반력 평형 제어용 분배기(208)로 이렇게 하여 분배된, 대상물 반력 평형 제어 이외로 평형을 취해야하는 대상물 반력 편차 모멘트 합은, 또한 승산점(214)에서 -1배되어 극성이 반전되어, 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트로서 보상 모멘트 분배기(210)에 입력된다. 또한, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합은, 대상물 반력 평형 제어 장치(202)에 입력된다.

이어서, 먼저 개략 설명한 대상물 반력 평형 제어 장치(202)에 대해서 상세히 설명한다.

우선 그 기능에 관해서 개략 설명하면, 대상물 반력 평형 제어 장치(202)는, 상술한 바와 같이 목표 상체 위치 자세 등을 입력하고, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합에 동력학적으로 평형하도록 목표 상체 위치 자세와 목표 전 지면 반력을 수정하여, 수정 목표 상체 위치 자세와 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트를 출력한다.

즉, 장치(202)는, 장기적으로는, 목표 상체 위치 자세를 밀리게 함으로써 발생하는 중력 모멘트가, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합에 동력학적으로 평형하고, 목표 전 지면 반력의 수정량이 제로로 되돌아가는 특징을 가진다.

상술과 같이, 대상물 반력 평형 제어 장치(202)는 상기 수법 2)를 해결하기 위한 구성으로, 본 출원인이 이미 일본국 특개평 10-230485호 공보에서 제안하고 있는 기술인데, 이 실시의 형태에 있어서는, 이러한 공보에 기재되는 대상물 반력 평형 제어 장치의 후반부의 구성만이 사용되는 동시에, 「대상물 반력 편차」가 입력된 부분에, 상기한 「대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합」이 입력되도록 변경된 점에서 다르다.

도 19는 대상물 반력 평형 제어 장치(202)의 구성을 도시하는 블록도로, 최종 도달 목표 중심 위치 섭동량 산출부(202a), 모델 제어측 연산기(202b), 및 섭동 동력학 모델(202c)을 구비한다.

이들에의 입력은, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합, 목표 상체 위치 자세, 최종 수정 목표 핸드 위치 자세, 최종 수정 목표 상체 위치 자세, 최종 수정 목표 풋부 위치 자세이다. 또한, 이 중, 최종 수정 목표 핸드 위치 자세, 최종 수정 목표 상체 위치 자세 및 최종 수정 목표 풋부 위치 자세는, 대상물 반력 평형 제어에 있어서 근사 연산을 이용하는 경우, 즉, 섭동 동력학 모델(202c)이 근사 모델인 경우에는 불필요하다.

또한, 상술의 자세 안정화용 보상 모멘트는, 부분적으로도 대상물 반력 평형 제어 장치(202)에는 입력되지 않는다. 자세 안정화용 보상 모멘트는 자세가 기울어진 결과 어긋난 중심 위치를 복원시키기 위해서 로봇에 외부로부터 주어져야 되는 모멘트인데, 대상물 반력 평형 제어 장치(202)의 기능은, 목표 자세의 중심을 어긋나게 해 정상적인 대상물 반력 편차를 부정하는 것이고, 자세가 기울어진 결과 어긋난 중심을 복원하는 것은 불가능하기 때문이다.

이하 상세히 설명하면, 입력된 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합은, 최종 도달 목표 중심 위치 섭동량 산출부(202a)에 입력된다.

이 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합을 장기적으로 부정하고 밸런스를 취하기 위한 중심 섭동량을 「최종 도달 목표 중심 위치 섭동량」이라고 부른다. 최종 도달 목표 중심 위치 섭동량 산출부(202a)는, 상기 합으로부터 최종 도달 목표 중심 위치 섭동량을 산출하여 출력한다.

최종 도달 목표 중심 위치 섭동량은, 모델 제어측 연산기(202b)에 입력된다. 모델 제어측 연산기(202b)에 대해서 설명하면, 최종 도달 목표 중심 위치 섭동량과, 섭동 동력학 모델(202c)이 출력하는 목표 중심 위치 섭동량과의 차를 중심 변위 편차로 하면, 모델 제어측 연산기(202b)는 이 중심 변위 편차를 제로에 수속시키기 위한 모멘트인 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트를 PD 제어측에 의해서 결정하여 출력한다.

또한, 섭동 동력학 모델(202c)은, 목표 작업 패턴의 운동(섭동)에, 어떤 구 속 조건을 부여해 둔 경우의, 모델을 위한 목표 전 지면 반력 모멘트 섭동량과 상체 위치 자세 섭동량의 관계를 나타내는 모델이다.

모델 제어측 연산기(202b)의 출력 직후의 가산점(202d)에 대해서 설명하면, 가산점(202d)에 의해서, 섭동 동력학 모델에는, 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트와, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합이 가산된 것이, 모델을 위한 목표 전 지면 반력 모멘트 섭동량(모델 입력량)으로서 입력되고, 그 입력에 대응하도록 상기 섭동 동력학 모델에 의해서 상체 위치 자세 섭동량이 산출된다. 이것이 가산점(202d)에서 목표 상체 위치 자세에 가산되어, 수정 목표 상체 위치 자세가 출력된다.

도 14의 설명으로 되돌아가면, 이와 같이, 대상물 반력 평형 제어 장치(202)의 출력은, 입력한 목표 상체 위치 자세를 수정한 수정 목표 상체 위치 자세와, 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트이다. 출력된 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트는, 동 도면에 도시하는 바와 같이, 계속해서 기술하는 보상 모멘트 분배기(210)에 입력된다.

또한, 상기에서, 장치(202)로부터 대상물 반력 평형 제어용 보상 전 지면 반력 내의 모멘트 성분만이 출력되도록 한 것은, 대상물 반력 평형 제어용 보상 전 지면 반력 중, 로봇의 자세 안정화를 위해 특히 중요한 성분은, X축 방향 모멘트 성분과 Y축 방향 모멘트 성분이기 때문이다. 모멘트 성분 이외도 고려한 제어계로 한 쪽이 보다 좋은데, 현저한 효과는 기대할 수 없다.

이어서, 보상 모멘트 분배기(210)에 대해서 설명한다. 보상 모멘트 분배기 (210)는, 도 20에 도시하는 바와 같이, 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트, 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트 및 자세 안정화용 보상 모멘트를 입력하고, 이들을 목표 구속력 모멘트와 보상 전 지면 반력 주 모멘트에 분배하는, 2개의 함수를 구비한다.

여기서, 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트는 상기한 바와 같이, 대상물 반력 평형 제어 이외의 수단으로 평형을 취해야 하는 대상물 반력 편차 모멘트 합을 승산점(214)으로 -1배한 값이다.

또한, 목표 구속력 모멘트는, 상기 입력 중, 구속 방향의 대상물 반력에 의해서 목표 전 지면 반력 중심점 방향에 발생시켜야되는 모멘트이고, 보상 전 지면 반력 주 모멘트는, 상기 입력 중, 목표 전 지면 반력 중심점 방향에 전 지면 반력에 의해서 발생시켜야되는 모멘트이다.

이하, 도 20을 참조하여 분배 처리에 대해서 설명한다. 또한, 도시하는 예에서는 입출력 모두 1차원의 스칼라량과 같이 나타나는데, 실제로는 X 방향 성분과 Y 방향 성분이 있어, 각각에 대해 처리가 행해진다. 단, Z 방향 성분은 자세 안정성에 관계가 없으므로 무시하기로 한다.

분배기(210)는 가산점(210a)을 구비하고, 가산점(210a)에서 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트와, 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트의 합을 구하여 2개의 함수에 입력한다. 여기서, 자세 안정화용 보상 모멘트를 u, 상기 합을 v로 두고, 상기한 2개의 함수 중의 1개인 함수(g1(u, v))의 값을 구하여, 이를 보상 전 지면 반력 주 모멘트로 한다. 또 한, 다른쪽 함수(g2(u, v))의 값을 구하여, 이를 목표 구속력 모멘트로 한다. 단, 각각의 함수는 2입력 1출력의 함수이다.

이 함수는, 예를 들면 이하에 표시하는 식과 같이 하면 된다. 또한, 식 중의 Cmax 및 Cmin은 설정치이다.

u+v> Cmax일 때

g1(u, v)= Cmax

u+v < Cmin일 때

g1(u, v)= Cmin

Cmin≤ u+v ≤ Cmax일 때

g1(u, v) = u+ v … 식 3

g2(u, v) = u+ v-g1(u, v) … 식 4

도 20에서는 식 3, 식 4로 표시되는 함수를, 입력 u와 입력 v를 수평면상의 직교축으로 하고, 출력치를 높이로 표시한다.

기본적으로는, 보상 모멘트 분배기의 입력 합(대상물 반력 평형 제어용 보상 전 지면 반력 모멘트와 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트와, 자세 안정화용 보상 모멘트의 합)이, 출력 합(목표 구속력 모멘트와 보상 전 지면 반력 주 모멘트의 합)에 일치하도록, 2개의 함수를 선정해야 한다. 즉, g1(U, v) + g2(u, v)= u + v 가 항등적으로 성립하도록, 2개의 함수를 선정해야 한다.

또한, 대상물 반력 평형 제어용 보상 전 지면 반력 모멘트를 u, 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트를 v, 자세 안정화용 보상 모멘트를 w로 두고, 보상 전 지면 반력 주 모멘트와 목표 구속력 모멘트를, 각각 3입력의 함수 g1(u, v, w), 함수 g2(u, v, w)의 값에 의해서 결정해도 된다.

이 경우라도, 기본적으로는, g1(u, v, w)+ g2(u, v, w)= u+v+w가 항등적으로 성립하도록 함수를 선정해야 한다. 그러나, 복합 컴플라이언스 제어 등의 제어 편차를 고려하여, 출력 합을 크게 해도 된다. 특히, 자세 안정화용 보상 모멘트는 피드백량이므로, 자세 안정화용 보상 모멘트에 영향을 받은 출력 성분의 합이 자세 안정화용 보상 모멘트와 몇십 퍼센트 달라도 그다지 문제가 되지 않는다.

도 14의 설명으로 되돌아가면, 보상 모멘트 분배기(210)로부터 출력되는, 한쪽 모멘트인 목표 구속력 모멘트는, 가산점(215)에 이송되고, 거기서 상술의 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합으로부터 감산된다.

또한, 보상 모멘트 분배기(210)로부터 출력되는, 다른쪽 모멘트인 보상 전 지면 반력 주 모멘트로부터 가산점(216)에서, 전술의 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합과 목표 구속력 모멘트의 차가 감산되어, 보상 전 지면 반력 모멘트가 출력된다.

단, 아암 액츄에이터(92)의 컴플라이언스 제어의 능력이 높고, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합이 거의 목표 구속력 모멘트에 일치하도록 제어되는 경우, 가산점(216)에서 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합과 목표 구속력 모멘트의 차 를 빼지 않고, 보상 전 지면 반력 주 모멘트를 그대로 보상 전 지면 반력 모멘트로 해도 된다.

또한, 가산점(216)을 삭제하고, 보상 전 지면 반력 모멘트를 보상 전 지면 반력 주 모멘트로 하고, 그 대신에, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합으로부터, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 조작 방향 성분 합과 목표 구속력 모멘트의 차(가산점(215)의 출력)를 빼도 된다.

상기한 목표 구속력 모멘트는, 수정 목표 대상물 반력 산출기(206)에도 입력된다.

도 21에, 수정 목표 대상물 반력 산출기(206)의 구성을 도시한다. 동 도면에 도시하는 바와 같이, 수정 목표 대상물 반력 산출기(206)는, 목표 핸드 위치 자세(혹은 최종 수정 목표 핸드 위치 자세), 목표 구속력 모멘트, 목표 전 지면 반력 중심점 위치, 조작 방향 및 목표 대상물 반력을 입력하고, 이들 입력에 따라서 수정 목표 대상물 반력을 산출하여 출력한다.

이하, 그 처리에 관해서 설명하면, 산출기(206)는 목표 핸드력 수정량 결정기(206a), 변환기(206b) 및 가산점(206c)을 구비한다. 목표 핸드력 수정량 결정기(206a)는, 목표 핸드 위치(혹은 최종 수정 목표 핸드 위치)를 작용점으로 하도록 표현된 목표 대상물 반력의 수정량인 목표 핸드력 수정량을 이하와 같이 결정한다.

즉, 결정기(206a)는, 목표 핸드력 수정량이 목표 핸드 위치(혹은 최종 수정 목표 핸드 위치)에 작용하였을 때에, 목표 전 지면 반력 중심점에 작용하는 모멘트 의 합과 목표 구속력 모멘트의 차가 제로에 근접하도록, 목표 핸드력 수정량을 결정한다. 단, 결정기(206a)는, 목표 핸드력 수정량이, 조작 방향의 힘과 힘의 모멘트로서, 비조작 방향 성분을 포함하지 않도록 결정한다. 또한, 목표 핸드력 수정량은, 지지 레그 좌표계로 표현된다.

가장 간단한 결정법을 이하에 도시한다. 우선, 조작 방향 내에 있는 2개의 벡터를 자유롭게 선택한다. 각 벡터는, 조작 공간의 직교 기저 벡터(A1, A2, …, Am)를 적당히 선형 결합시킴으로써 얻어진다. 이 벡터를 각각 Q1, Q2로 한다.

이어서 목표 핸드력 수정량을, aQ1+bQ2의 형으로 한다. 여기서, a, b는 계수이다.

목표 핸드력 수정량(aQ1+bQ2)이 목표 핸드 위치(혹은 최종 수정 목표 핸드 위치)에 작용하였을 때, 목표 전 지면 반력 중심점에 작용하는 합력의 모멘트(M)를, 역학 연산에 의해 구한다.

구해진 M의 X 성분을 Mx, Y 성분을 My로 하면, Mx, My와 a, b의 관계는, 다음 연립 1차 방정식의 형으로 된다. 여기서, k11, k12, k21 및 k22은 계수이다.

Mx = ak11 + bk12

My = ak21 + bk22 … 식 5

다음에, Mx가 목표 구속력 모멘트의 X 성분, My가 목표 구속력 모멘트의 Y 성분에 일치하도록, a 및 b를 결정한다. 구체적으로는, 식 5의 연립 방정식을 a 및 b에 관해서 풀고, Mx에 목표 구속력 모멘트의 X 성분의 값, My에 목표 구속력 모멘트의 Y 성분의 값을 대입함으로써, a 및 b를 얻는다.

다음에, 상기 벡터 Q1, Q2 및 상기 a, b를 이용하여 목표 핸드력 수정량(aQ1 + bQ2)의 값을 얻는다. 얻어진 수정량은 변환기(206b)에 입력되고, 변환기(206b)는, 목표 핸드력 수정량을, 작용점을 목표 핸드 위치(혹은 최종 수정 목표 핸드 위치)로부터 전 지면 반력 중심점으로 변경한 표현으로 변환함으로써, 전 지면 반력 중심점을 작용점으로 하는 목표 대상물 반력 수정량을 출력한다.

마지막에, 가산점(206c)에서, 전 지면 반력 중심점을 작용점으로 하는 목표 대상물 반력에, 전 지면 반력 중심점을 작용점으로 하는 목표 대상물 반력 수정량을 가산함으로써, 전 지면 반력 중심점을 작용점으로 하는 수정 목표 대상물 반력이 출력된다. 또한, 수정 목표 대상물 반력은, 전 운동 속도 공간에서의 벡터이다.

도 14의 설명으로 되돌아가면, 이상과 같이 하여 얻은 수정 목표 대상물 반력 및 상기한 수정 목표 상체 위치 자세 및 보상 전 지면 반력 모멘트가, 자세 안정화 메인 제어 장치(106)의 최종적인 출력으로 된다. 그리고, 도 5에 도시하는 바와 같이, 수정 목표 대상물 반력이 아암 메인 제어 장치(104)에, 보상 전 지면 반력 모멘트가 각 메인 제어 장치(102)에, 수정 목표 상체 위치 자세가 그 양쪽에 입력된다.

레그 메인 제어 장치(102)와 아암 메인 제어 장치(104)의 기능을 재설명하면, 레그 메인 제어 장치(102)에서는, 전술과 같이, 목표 전 지면 반력 중심점 위치에 작용하는 실제 전 지면 반력의 모멘트 성분이, 목표 전 지면 반력 모멘트(통상은 제로)와 보상 전 지면 반력 모멘트의 합에 일치하도록 목표 풋부 위치 자세를 수정하고, 또한, 수정 목표 상체 위치 자세와 수정된 목표 풋부 위치 자세로부터 결정되는 목표 레그 관절 변위에 실관절 변위가 추종하도록, 액츄에이터 변위 지령을 출력하여 레그 관절 액츄에이터를 제어한다.

또한, 아암 메인 제어 장치(104)에서는, 전술과 같이, 핸드(40R(L))의 역 센서(58)에 의해서 검출되는 실대상물 반력(실제 핸드력)과 수정 목표 대상물 반력의 차에 따라, 차가 제로에 근접하도록, PD 제어 등에 의해서 목표 핸드 위치 자세를 수정한다. 또한, 수정 목표 상체 위치 자세(혹은 최종 수정 목표 상체 위치 자세)와 수정된 목표 핸드 위치 자세로부터 결정되는 목표 아암 관절 변위에 실관절 변위가 추종하도록, 액츄에이터 변위 지령을 출력하여 아암 관절 액츄에이터를 제어한다.

이상과 같이, 자세 안정화 메인 제어 장치(106)는, 목표 전 지면 반력 중심점에 작용하는 대상물 반력 편차 모멘트를, 조작 방향 성분과 비조작 방향 성분으로 분리하여, 상기 비조작 방향 성분에 의해서 발생하는 동력학적 언밸런스를, 그 일부를 대상물 반력 평형 제어 장치에 의해서 부정하고, 나머지를, 목표 전 지면 반력 중심점에 작용해야 할 목표 대상물 반력 모멘트 조작 방향 성분을 수정함으로써, 바꿔 말하면, 아암의 동작을 수정하여 상기 조작 방향 성분을 변화시킴으로써 부정하도록 구성했다.

바꾸어 말하면, 앞의 제안 기술에 있어서, 대상물로부터 받는 예기치 않은 반력이 급격히 변화했을 때, 대상물 반력 평형 제어 장치(202)에 의해서 행해진, 풋부(22R(L))(및 레그부 링크(2))를 움직이게 해 밟음 정지시키는 (버티게 하는) 동작을, 아암부 링크(5)의 동작에 의해서 경감, 혹은 없앨 수 있다. 이 때문에, 전 지면 반력 모멘트가 한계를 넘어서 발생할 염려가 없고, 따라서 로봇(1)의 운동 밸런스를 유지할 수 있어, 로봇이 기울거나, 전도하기도 하는 것을 방지할 수 있다.

또한, 조작 방향 성분이 변화하도록 아암의 동작을 수정하는 것으로부터, 로봇(1)의 운동 밸런스를 유지하거나, 기울어진 자세를 복원해도, 대상물의 운동에 영향을 주지 않는다.

마지막에, 상기한 제어계의 안정성에 관해서 설명한다.

목표 전 지면 반력 중심점에 작용해야 할 목표 대상물 반력 모멘트 조작 방향 성분의 수정량인 목표 구속력 모멘트와, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트의 조작 방향 성분 합과 비조작 방향 성분 합의 관계는, 상기한 도 14에 도시하게 된다.

이해의 편의를 위해, 자세 안정화 메인 제어 장치(106)에 있어서 대상물 반력 모멘트 편차 분리기(204)가 분리하는, 목표 전 지면 반력 중심점에 작용하는 대상물 반력 편차 모멘트의 조작 방향 성분 합과 비조작 방향 성분 합 중, 비조작 방향 성분 합에 의해서 발생하는 동력학적 언밸런스를 모두 목표 전 지면 반력 중심점에 작용해야 할 목표 대상물 반력 모멘트 조작 방향 성분을 수정함으로써 부정하도록 구성했다고 가정한다. 즉, 대상물 반력 평형 제어를 동작시키지 않는 경우를 상정한다.

그 경우, 아암 제어계(도 5에 도시하는 아암 메인 제어 장치(104)나 액츄에 이터(92), 액츄에이터 구동 장치(88) 등으로 이루어지는 제어계)와 자세 안정화 메인 제어 장치(106)로 이루어지는 제어계는, 대상물 반력에 주목하면, 도 22와 같이 간략화할 수 있다. 또한, 도 22에 있어서의 외란 모멘트는 대상물에 상정해 있던 성질과 다른 성질을 가지고 있으므로, 대상물이 예기치 않은 거동을 나타냄으로써 발생하는 대상물 반력, 즉, 상정 외의 대상물 반력이다. 예를 들면, 대상물에 발생하는 마찰이, 상정하고 있던 것과 다름으로써 발생한다. 외란 모멘트 중의 조작 방향 성분을 「외란 모멘트 조작 방향 성분」, 비조작 방향 성분을 「외란 모멘트 비조작 방향 성분」이라고 부른다.

도 22에서 가산점(220)으로 입력되는 외란 모멘트 비조작 방향 성분이 변화하면, 이를 부정하도록 목표 구속력 모멘트가 결정되고, 가산점(222)으로 가산됨으로써, 목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 조작 방향 성분 합이 수정되고, 조작 방향 아암 컴플라이언스 제어부에서 목표 핸드 위치 자세가 수정량만큼 수정되고, 또한 아암부 링크(5)의 액츄에이터 변위 지령이 수정되어, 로봇(1)의 아암부 링크(5)가 구동되어 운동 밸런스가 유지된다. 또한, 외란 모멘트 조작 방향 성분이 변화해도, 아암의 컴플라이언스 제어에의 목표치는 변화하지 않는다.

한편, 자세 안정화 메인 제어 장치(106)가, 상기 비조작 방향 성분 합에 의해서 발생하는 동력학적 언밸런스를, 목표 전 지면 반력 중심점에 작용해야 할 목표 대상물 반력 모멘트 비조작 방향 성분 합을 수정함으로써 부정하도록 구성했다고 가정하면, 아암 제어계와 자세 안정화 메인 제어 장치(106)로 이루어지는 제어 계는, 대상물 반력에 주목하면, 도 23과 같이 간략화할 수 있다.

이 경우, 자세 안정화 메인 제어 장치를 포함하는 피드 백 루프는 정 귀환으로 되어 발산한다.

즉, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합으로부터 자세 안정화 메인 제어 장치에 의해서 목표 구속력 모멘트가 산출되고, 이것이 가산점(226)으로 입력(목표 전 지면 반력 중심점 방향의 목표 대상물 반력 모멘트 비조작 방향 성분 합)에 가산되어 목표 전 지면 반력 중심점 방향의 수정 목표 대상물 반력 모멘트 비조작 방향 성분 합이 산출되고, 이것이 가산점(228)으로 목표 전 지면 반력 중심점 방향의 실대상물 반력 모멘트 비조작 방향 성분 합으로부터 감산되고, 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합이 산출되기까지의 피드 백 루프는 정 귀환으로 되어, 발산한다.

즉, 도 23에 도시하는 구성에 있어서는, 일단 외란 모멘트 비조작 방향 성분이 발생하면, 이를 부정하려고 목표 전 지면 반력 중심점 방향의 수정 목표 대상물 반력 모멘트 비조작 방향 성분 합이 끝없이 증가 혹은 감소하여 발산하게 된다.

이에 대해, 본 실시의 형태에 관한 이동 로봇의 자세 제어 장치에 있어서는, 도 14 등에 도시하는 바와 같이, 목표 전 지면 반력 중심점 방향의 대상물 반력 모멘트를 조작 방향 성분 합과 비조작 방향 성분 합으로 분리하고, 또한, 도 22에 도시하는 바와 같이, 정 귀환이 되지 않도록 피드 백 루프를 구성함으로써, 이러한 발산을 막도록 했다. 즉, 비조작 방향측에서 피드백된 값이 조작 방향측에 가산되 도록 구성함으로써, 그러한 발산을 막도록 했다.

도 25는, 본 발명의 제2의 실시의 형태에 관한 이동 로봇의 자세 제어 장치이다.

제1의 실시의 형태와 상이한 점에 초점을 두고 설명하면, 제2의 실시의 형태에 있어서는, 대상물 반력 평형 제어용 분배기(208)의 출력 중, 대상물 반력 평형 제어로 평형을 취해야 하는 대상물 반력 편차 모멘트 합을 제로(동 도면에 파선으로 표시한다)로 하는 동시에, 분배기(208)의 출력을 모두 그 이외의 수단으로 평형을 취해야 하는 대상물 반력 편차 모멘트 합으로 했다.

또한, 나머지 구성 및 효과는 제1의 실시의 형태의 그것과 다르지 않다. 제2의 실시의 형태에 관한 이동 로봇의 자세 제어 장치는 상기와 같이 구성하였으므로, 제1의 실시의 형태에 관한 장치와 동일한 효과를 얻을 수 있다.

도 26은 본 발명의 제3의 실시의 형태에 관한 이동 로봇의 자세 제어 장치이다.

제1의 실시의 형태와 상이한 점에 초점을 두고 설명하면, 제3의 실시의 형태에 있어서는, 2발의 레그식 이동 로봇(1)에 대신해, 도시와 같은 차륜식의 이동 로봇에 관해서 자세 제어를 행하도록 했다.

이하, 설명하면, 도시하는 차륜식의 이동 로봇(300)은, 원통형의 기체(상체)(302)와, 그에 상하(Z축 방향)로 스트로크하는 액티브 서스펜션(도시하지 않음)을 통해 부착되는 4개의 차륜(도면에서 2개만 표시한다)(304)을 구비한다. 기체(302)의 상부에는 마운트(306)가 회전 자유롭게 부착되는 동시에, 그 위에 1개의 굴곡 자유로운 아암(아암부 링크)(310)이 부착된다.

즉, 아암(310)은, 마운트(306)에 부착되는 제1 링크(310a)와, 제1 링크(310a)에 관절(도시하지 않음)을 통해 부착되는 제2 링크(310b)와, 제2 링크(310b)의 자유단측에 관절(도시하지 않음)을 통해 부착되는 핸드(310c)로 이루어진다. 관절은, 각각 전동 모터 등의 액츄에이터를 내장한다.

기체(302)의 내부에는, 마이크로 컴퓨터로 이루어지는 전자 제어 유닛(ECU)(312)이 격납된다. 기체(302)의 중심 위치의 부근에는 경사 센서(도시하지 않음)가 배치되고, Z축에 대한 기체(302)의 기울기와 그 각속도에 따른 출력이 발생한다. 차륜(304)의 각각에는 가중 센서(도시하지 않음)가 배치되고, 접지면에서 4개의 차륜(304)에 각각 작용하는 지면 반력(가중)(F1, F2, …)을 검출한다.

여기서, 마운트(306)의 Z축 방향의 회전 각도를 θ1, 제1 링크(310a)의 Z축에 대한 각도를 θ2, 제1 링크(310a)에 대한 제2 링크(310b)의 상대 각도를 θ3, 제2 링크(310b)에 대한 핸드(310c)의 상대 각도를 θ4로 하면, 이동 로봇(300)의 목표 동작은, θ1에서 θ4의 동작 패턴과 ZMP로 표시할 수 있다.

또한, 도시하는 이동 로봇(300)의 자세 제어에 있어서, 제1 링크(310a)의 Z축에 대한 각도 θ2를 제어하는 것으로 하면, θ2가 제1의 실시의 형태의 레그식 이동 로봇(1)의 목표 상체 위치에, θ1, θ3, θ4의 그룹이 목표 상체 자세에 상당한다. 또한, θ4이 핸드(310c)의 위치 자세에 상당한다. 액티브 서스펜션을 제어하는 전자 제어 유닛(ECU)(312)의 동작이 제1의 실시의 형태의 도 8에 도시하는 구성 중의 복합 컴플라이언스 동작 결정부의 동작에 상당하고, 가중(F)을 F1에서 F4 로 분배하여 제어한다.

따라서, 제3의 실시의 형태에 있어서 차륜식의 이동 로봇(300)에 대해서 상기와 같은 제어를 행할 때, 제1의 실시의 형태에서 기술한 레그식 이동 로봇(1)의 자세 제어와 동일한 효과를 얻을 수 있다.

이상과 같이, 제1에서 제3의 실시의 형태에 관한 이동 로봇의 자세 제어 장치에 있어서는, 적어도 기체(상체(3))와, 이동 기구(레그부 링크(2) 혹은 차륜(304))와, 대상물에 힘을 작용할 수 있는, 적어도 1개의 아암부 링크(5)(혹은 아암(310))로 이루어지는 이동 로봇(1, 300)의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력을 검출 또는 추정하는 소정 방향 성분 검출 수단을 구비하는 동시에, 상기 소정 방향 성분 검출 수단이 검출 또는 추정한 제1의 외력에 따라, (목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합)에 따라, 상기 소정 방향으로 직교하는 방향으로 상기 아암부 링크에 제2의 외력(대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트)이 작용하도록, 바꾸어 말하면, 상기 목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합에 의거해 얻어지는 대상물 반력 평형 제어 이외의 수단으로 평형을 취해야 하는 대상물 반력 편차 모멘트 합의 극성을 반전하여 얻어지는, 즉 이에 직교하는 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트 합의 극성을 반전하여 얻어지는, 즉, 이에 직교하는 대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트가 작용하도록, 상기 아암부 링크를 구동 하고, 바꾸어 말하면 목표치를 수정하는 것을 필요로 하지 않고, 상기 로봇의 자세를 안정시키도록 제어하도록 구성했다.

또한, 적어도 기체(상체(3))와, 이동 기구(레그부 링크(2) 혹은 차륜(304))와, 대상물에 힘을 작용시킬 수 있는, 적어도 1개의 아암부 링크(5)(혹은 아암(310))로 이루어지는 이동 로봇(1, 300)의 자세 제어 장치에 있어서, 적어도 상기 로봇의 목표 아암부 링크 위치 자세(목표 핸드 위치 자세)와, 상기 대상물로부터 상기 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작(목표 대상물 반력 혹은 목표 위치 자세)을 생성하는 목표 동작 생성 수단(목표 작업 패턴 생성기(100)), 상기 대상물 반력의 실제치인 실대상물 반력(실 핸드력)을 검출 혹은 (업저버를 통해) 추정하는 실대상물 반력 검출 수단(검출에 대해서는 역 센서(58), 하중 센서(ECU62, 312), 추정에 대해서는 업저버), 적어도 상기 검출 혹은 추정된 실대상물 반력에 의거하여, 상기 로봇의 자세가 안정되도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어 수단(자세 안정화 메인 제어 장치(106)) 및 적어도 상기 수정된 목표 동작에 따라서 상기 아암부 링크를 구동하는 아암부 구동 장치(액츄에이터(90, 92))를 구비하도록 구성했다.

또한, 상기 자세 안정화 제어 수단은, 적어도 상기 실대상물 반력과 상기 목표 대상물 반력과의 차를 나타내는 대상물 반력 편차(목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합)에 의거하여, 상기 로봇의 자세가 안정되도록 상기 목표 동작을 수정하도록 구성했다.

또한, 상기 자세 안정화 제어 수단은, 상기 대상물 반력 편차의 제1의 성분(목표 전 지면 반력 중심점 방향의 대상물 반력 편차 모멘트 비조작 방향 성분 합)에 의해서 발생하는 전도력의 전부 혹은 일부를, 상기 제1의 성분으로부터 얻어지는 제2의 성분(대상물 반력 평형 제어 이외의 수단에 요구되는 대상물 반력 보상 모멘트)에 의해서 부정하도록, 상기 목표 동작을 수정하도록 구성했다.

또한, 적어도 기체(상체(3))와, 이동 기구(레그부 링크(2) 혹은 차륜(304))와, 대상물에 힘을 작용시킬 수 있는, 적어도 1개의 아암부 링크(5)(혹은 아암(310))로 이루어지는 이동 로봇(1, 300)의 자세 제어 장치에 있어서, 적어도 상기 로봇의 목표 아암부 링크 위치 자세(목표 핸드 위치 자세)와, 상기 대상물로부터 상기 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작(목표 대상물 반력 혹은 목표 위치 자세)을 생성하는 목표 동작 생성 수단(목표 작업 패턴 생성기(100)), 상기 로봇의 자세 기울기 편차(상체 경사 각도)를 검출하고, 상기 검출한 자세 기울기 편차, 보다 구체적으로는 검출치에 따라서 산출되는 자세 안정화용 보상 모멘트)이 제로에 근접하도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어 수단(자세 안정화 메인 제어 장치(106)) 및 적어도 상기 수정된 목표 동작에 따라서 상기 아암부 링크를 구동하는 아암부 구동 장치(액츄에이터(90, 92))를 구비하도록 구성했다.

또한, 상기에 있어서, 목표 전 지면 반력 중심점 방향의 실대상물 반력 비조작 방향 성분 합은, 실대상물 반력의 모든 성분이, 목표 전 지면 반력 중심점 방향 에 작용하는 힘의 합으로부터, 목표 전 지면 반력 중심점 방향의 실대상물 반력 조작 방향 성분 합을 뺌으로써 구해도 된다. 왜냐하면, 목표 전 지면 반력 중심점 방향의 실대상물 반력 조작 방향 성분 합과 목표 전 지면 반력 중심점 방향의 실대상물 반력 비조작 방향 성분 합의 합은, 실대상물 반력의 모든 성분이, 목표 전 지면 반력 중심점 방향에 작용하는 힘의 합과 동일하기 때문이다.

반대로, 목표 전 지면 반력 중심점 방향의 실대상물 반력 조작 방향 성분 합은, 실대상물 반력의 모든 성분이, 목표 전 지면 반력 중심점 방향에 작용하는 힘의 합으로부터, 목표 전 지면 반력 중심점 방향의 실대상물 반력 비조작 방향 성분 합을 뺌으로써 구해도 된다.

또한, 목표 전 지면 반력 중심점 방향의 대상물 반력 조작 방향 성분 합과, 목표 전 지면 반력 중심점 방향의 대상물 반력 비조작 방향 성분 합에 대해서도 동일한 방법으로 구해도 된다.

또한, 자세 안정화 메인 제어 장치(106)에 있어서, 목표 대상물 반력을 수정하는 대신에 목표 핸드 위치 자세를 변경해도 된다. 보다 상세히 설명하면, 아암 메인 제어 장치(104)는, 통상, 대상물 반력과 핸드 위치 자세를 목표치에 일치시키도록 제어하는 것은 불가능하므로, 대상물 반력의 제어와 핸드 위치 자세의 제어에 트레이드 오프(trade off)가 작용하고, 목표 핸드 위치 자세를 변경하는 것만으로도 실대상물 반력이 변화한다.

따라서, 이 때의 목표 핸드 위치 자세의 변경량과 실대상물 반력의 변화의 비(즉 아암 메인 제어계의 컴플라이언스 정수)를 알고 있으면, 상술의 실시의 형태 에서 구해진 목표 대상물 반력의 수정량에, 그 비를 곱함으로써, 목표 핸드 위치 자세의 수정량을 구할 수 있기 때문이다.

또한, 레그식 이동 로봇으로서 2개의 레그부 링크와 2개의 아암부 링크를 구비한 휴머노이드 로봇을 예로 들어 설명했는데, 2개 이외의 개수의 레그부 링크를 구비한 레그식 이동 로봇이어도 되고, 아암부 링크의 개수도 1개 혹은 3개 이상이어도 된다.

또한, 레그부 링크라도, 지면 이외의 대상물에 작용하는 것이면, 아암부 링크로 간주해도 된다. 예를 들면, 곤충형의 6개의 레그부 링크를 구비한 로봇에 있어서, 헤드부 측 앞의 2개의 레그부 링크를 이용하여 물건을 들어 올리는 경우, 그 레그부 링크를 아암부 링크로 간주할 수 있다.

또한, 로봇의 동력학적 언밸런스를 X축 방향과 Y축 방향에 대한 기울기로서 설명했는데, 움직이지 않는 레그의 흔들림 등에 기인하는 Z축 방향의 스핀에 대해서도 동일하게 적용할 수 있다.

또한, 상체 링크의 휘어짐이나 비틀기를 위한 액츄에이터를 설치하지 않았는데, 이를 추가할 때, 상체 액츄에이터 제어 장치도 필요해진다. 단, 상체 링크의 휘어짐이나 비틀기는, 아암 또는 레그가 붙어 있는 측에 관절을 추가한 것과 둥가이므로, 개념상, 아암 또는 레그의 액츄에이터로 간주할 수 있다. 즉, 상체 액츄에이터 제어 장치는, 아암 또는 레그의 제어 장치의 일부로서 포함된다고 생각할 수 있다.

또한, 아암부 링크의 제어에, 가상 컴플라이언스 제어 장치 이외의 별도의 수단, 예를 들면, 전동 액츄에이터를 전류 지령형의 앰프에 의해서 제어하는 수단을 이용하여 관절 토크를 제어하고, 그 결과, 간접적으로 대상물 반력을 제어하도록 해도 된다. 그 제어에는 핸드에 역 센서를 배치할 필요는 없지만, 대상물 반력 제어 장치용으로 역시 역 센서를 설치하는 것이 바람직하다.

또한, 핸드(40R(L))의 역 센서(58) 대신에, 관절 토크로부터 실대상물 반력을 추정하는 추정기를 아암 메인 제어 장치에 구비해도 된다. 이 추정기는, 종래 기술인 외란 업저버를 이용하면 된다.

또한, 목표 대상물 반력을 제로에 설정해도 좋은 경우도 있다. 예를 들면, 상기한 작업 4에서는 난간과 핸드의 사이의 마찰이 거의 제로이면, 목표 대상물 반력을 제로로 설정하는 것이 좋다. 또한, 작업 2에서 대차의 자재 캐스터륜의 마찰이 불명확하면, 목표 대상물 반력을 제로에 설정하면 좋다. 또한, 목표 대상물 반력이 항상 제로인 경우에는, 대상물 반력 편차는 실대상물 반력과 일치하므로, 상기에 있어서 대상물 반력 편차에 대신해 실대상물 반력을 이용해도 된다.

또한, 마루가 평면이 아닌 경우라도, 본 출원인이 일본국 특개평 5-318340호 공보에서 제안한 가상 평면을 상정하는 기술을 이용하여, 목표 전 지면 반력 중심점이나 목표 ZMP를 가상 평면 상에 구함으로써, 적용이 가능해진다.

또한, 상기에 있어서, 블록도는 연산 처리 순서를 바꾸는 등, 다양한 변형이 가능하다.

청구항 1에 있어서는, 이동 로봇의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력에 따라, 소정 방향으로 직교하는 방향으로 아암부 링크에 제2의 외력이 작용하도록, 아암부 링크를 구동하여 상기 로봇의 자세를 안정시키도록 제어하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

청구항 2에 있어서는, 이동 로봇의 자세 제어 장치에 있어서, 적어도 로봇의 목표 아암부 링크 위치 자세와, 대상물로부터 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하고, 대상물 반력의 실제치인 실대상물 반력을 검출 혹은 추정하여, 적어도 검출 혹은 추정된 실대상물 반력에 의거하여, 로봇의 자세가 안정되도록 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하고, 수정된 목표 동작에 따라서 아암부 링크를 구동하는 아암부 구동 장치를 구비하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다. 또한, 대상물의 운동에 영향을 주지 않고, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

청구항 5에 있어서는, 이동 로봇의 자세 제어 장치에 있어서, 적어도 로봇의 목표 아암부 링크 위치 자세와, 대상물로부터 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 동시에, 로봇 의 자세 기울기 편차를 검출하여, 검출한 자세 기울기 편차가 제로에 근접하도록 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하고, 수정된 목표 동작에 따라서 아암부 링크를 구동하도록 구성하였으므로, 이동 로봇이 대상물로부터 반력을 받는 작업을 하고 있을 때, 자세가 불안정하게 되거나, 혹은 대상물로부터 예기치 않은 반력을 받더라도, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다. 또한, 대상물의 운동에 영향을 주지 않고, 운동 밸런스를 유지하여 안정된 자세를 유지할 수 있다.

Claims (5)

1. 적어도 기체(基體)와, 이동 기구와, 대상물에 힘을 작용할 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서, 상기 아암부 링크에 실제로 작용하는 대상물로부터의 반력 중의 설정된 소정 방향의 성분인 제1의 외력을 검출 또는 추정하는 소정 방향 성분 검출 수단을 구비하는 동시에, 상기 소정 방향 성분 검출 수단이 검출 또는 추정한 제1의 외력에 따라, 상기 소정 방향에 직교하는 방향으로 상기 아암부 링크에 제2의 외력이 작용하도록, 상기 아암부 링크를 구동하여 상기 로봇의 자세를 안정시키도록 제어하는 것을 특징으로 하는 이동 로봇의 자세 제어 장치.
2. 적어도 기체와, 이동 기구와, 대상물에 힘을 작용시킬 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서,

   a. 적어도 상기 로봇의 목표 아암부 링크 위치 자세와, 상기 대상물로부터 상기 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 목표 동작 생성 수단,

   b. 상기 대상물 반력의 실제치인 실대상물 반력을 검출 혹은 추정하는 실대상물 반력 검출 수단,

   c. 적어도 상기 검출 혹은 추정된 실대상물 반력에 의거하여, 상기 로봇의 자세가 안정되도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치 자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어수단, 및

   d. 적어도 상기 수정된 목표 동작에 의거해 상기 아암부 링크를 구동하는 아암부 구동 장치를 구비한 것을 특징으로 하는 이동 로봇의 자세 제어 장치.
3. 청구항 2에 있어서, 상기 자세 안정화 제어 수단은, 적어도 상기 실대상물 반력과 상기 목표 대상물 반력의 차를 나타내는 대상물 반력 편차에 의거하여, 상기 로봇의 자세가 안정되도록 상기 목표 동작을 수정하는 것을 특징으로 하는 이동 로봇의 자세 제어 장치.
4. 청구항 3에 있어서, 상기 자세 안정화 제어 수단은, 상기 대상물 반력 편차의 제1의 성분에 의해서 발생하는 전도력의 전부 혹은 일부를, 상기 제1의 성분으로부터 얻어지는 제2의 성분에 의해 부정하도록, 상기 목표 동작을 수정하는 것을 특징으로 하는 이동 로봇의 자세 제어 장치.
5. 적어도 기체와, 이동 기구와, 대상물에 힘을 작용할 수 있는, 적어도 1개의 아암부 링크로 이루어지는 이동 로봇의 자세 제어 장치에 있어서,

   a. 적어도 상기 로봇의 목표 아암부 링크 위치 자세와, 상기 대상물로부터 아암부 링크에 작용하는 대상물 반력의 목표치인 목표 대상물 반력으로 이루어지는 목표 동작을 생성하는 목표 동작 생성 수단,

   b. 상기 로봇의 자세 기울기 편차를 검출하고, 상기 검출한 자세 기울기 편 차가 제로에 근접하도록 상기 목표 동작 중의 적어도 상기 목표 아암부 링크 위치자세 또는 상기 목표 대상물 반력을 수정하는 자세 안정화 제어 수단 및

   c. 적어도 상기 수정된 목표 동작에 의거해 상기 아암부 링크를 구동하는 아암부 구동 장치를 구비한 것을 특징으로 하는 이동 로봇의 자세 제어 장치.

Images