KR20100036993A - 도립 진자형 이동 기구 - Google Patents

Abstract

본 발명의 목적은, 도립 진자형 이동 기구로 편륜 공전이 발생했을 때에 이것을 빠르게 검출할 수 있고, 공전 지속 시간이 긴 경우라도 기립 상태를 유지할 수 있는 도립 진자형 이동 기구를 제공하는 데 있다.

좌우의 차륜을 갖는 이동 기구, 이동 기구에 지지된 상체, 이동 기구를 제어하는 제어 장치를 구비한 도립 진자형 이동 기구에 있어서, 제어 장치는 차륜의 공전 검출부에서 공전이 검출되지 않은 경우에 양륜 기립 주행 제어를, 공전이 검출된 경우에 접지 차륜에 의한 접지 차륜 기립 제어를 행하고, 공전 차륜에 대해 트랙션 복귀를 촉구하는 공전 차륜 제어를 행하고, 트랙션 복귀 검출부에서 트랙션 복귀가 검출되지 않은 경우에 접지 차륜 기립 제어로 복귀되고, 트랙션 복귀가 검출된 경우에 양륜 기립 주행 제어로 복귀되는 공전 대응 제어를 행한다.

도립 진자형 이동 기구, 차륜, 주행 모터, 트랙션 복귀 검출부, 자세 방위 센서

Description

도립 진자형 이동 기구 {INVERTED PENDULUM TYPE MOVING MECHANISM}

본 발명은 편측 차륜에 공전이 발생했을 때에도, 기립 상태의 유지가 가능한 도립 진자형의 이동 기구에 관한 것이다.

좌우 대칭인 도립 진자형 이동 기구에 관해서는 특허 문헌 1에, 사람의 이동 수단으로서 사용되는 이동 기구에 관해서는 특허 문헌 2에 기재되어 있다.

특허 문헌 1의 도립 진자형 이동 기구는, 한 쌍의 차륜과, 양 차륜 사이에 가설된 차축과, 차축에 지지된 상체와, 차륜 구동 장치와, 차륜을 제어하는 제어 장치를 구비하고 있다. 이동 기구의 경사는 상체의 경사 각도 계측 수단으로 검지되고, 차륜의 회전 각도는 차륜 회전 각도 검출 수단으로 검지된다. 차륜 구동 장치는 검지된 상체의 경사 각도와 차륜의 회전 각도를 미리 설정되어 있는 제어 입력식에 대입함으로써 구동 토크를 연산하여 차륜 구동용 모터를 제어하는, 양륜 기립 제어를 행한다.

특허 문헌 2에서는 도립 진자형 이동 기구의 기립 주행 중에, 양 차륜의 가속도를 제어 주기마다 산출하여, 차륜과 바닥의 마찰력(트랙션)이 부하된 상태에서 취할 수 있는 최대 가속도보다도, 상기 가속도가 큰 경우에 당해 차륜이 공전하고 있다고 판단한다. 공전 차륜에 바닥으로부터의 마찰력이 부하되면, 그것에 추종하는 바와 같은 토크 프리 제어를 행한다. 또한, 공전 검출 중의 제어 주기마다 공전 차륜으로의 구동 토크와 공전 차륜의 가속도로부터 연산되는 관성 모멘트가, 공전 차륜의 관성 모멘트보다 크다고 판정한 경우에, 트랙션이 복귀되었다고 하여 양륜 기립 제어로 복귀되는, 트랙션 컨트롤이 이루어진다.

[특허 문헌 1] 일본 특허 출원 공개 소63-305082호 공보

[특허 문헌 2] 미국 특허 제6288505호 공보

[특허 문헌 3] 일본 특허 출원 공개 제2007-319991호 공보

도립 진자형 이동 기구가 기립 상태 유지를 위한 기립 제어를 하고 있을 때, 특히 주행 이동을 수반하고 있는 경우, 편측 차륜이 공전하는 현상이 발생하는 경우가 있다. 이 공전 발생은, (a) 주행 중인 바닥의 마찰 계수가 갑자기 낮아지고, (b) 차륜의 급한 가감속, (c) 바닥면의 미소한 돌출이나 단차로의 상승ㆍ낙하에 수반하는 일정 기간의 차륜의 부상 현상 등에 의해, 원래 상정하고 있던 바닥면과 차륜 사이의 마찰 반토크보다도 차륜으로의 구동 토크가 커져, 차륜이 급격하게 가감속하는 현상이다.

이 공전 차륜에 대해, 공전 전까지 바닥으로부터 작용하고 있던 트랙션이 감소함으로써 그것까지 도립 진자형 이동 기구의 기립 상태 유지에 기여하고 있던, 공전 차륜으로부터 이동 기구 본체로 작용하는 힘이 감소함으로써, 기립 제어가 불안정화되어 전도되는 경우가 있다. 그러나, 안정된 주행을 유지하는 동시에, 전도는 가능한 한 방지할 필요가 있다.

이 공전에 수반하는 전도를 방지하기 위해서는, 공전이 발생했을 때에 빠른 시기에 이것을 검출하여, (d) 공전 차륜의 조기 트랙션 복귀를 촉구하는 제어를 행하고, (e) 공전 중에는 접지 차륜만으로 기립 상태의 유지를 도모하는 것이 필요하다. (d)에 대해서는, 특허 문헌 2에 트랙션 컨트롤이 기재되어 있지만, (e)에 관해서는 개시되어 있지 않다.

접지 차륜으로부터 도립 진자형 이동 기구의 상체로 작용하는 힘에는 바닥과 차륜 사이의 트랙션의 반력이 포함되지만, 공전 차륜으로부터 상체로 작용하는 힘에는 상기 반력이 포함되지 않는다. 이로 인해, 상체에 작용하는 힘의 불균형으로부터 요축 주위에 회전 운동이 일어나, 기립 제어에 악영향을 미친다. 특히, 도립 진자형 이동 기구의 상체의 요축에 관한 관성 모멘트가 작은 경우, 이 영향이 현저해진다.

또한, 공전 발생 시의 경사 각도가 깊은(큰) 경우나, 공전 지속 시간이 긴 경우에는, 지금까지 양륜으로 기립 상태를 유지하고 있던 것을, 접지 차륜만으로 기립 상태를 유지할 필요가 있으므로, 접지 차륜의 구동 토크를 증가시킬 필요가 고려된다.

또한, 공전 검출도 가능한 한 완전하고 또한 확실하게 행할 필요가 있지만, 특허 문헌 2와 같이, 운동 정보에 관해서 차륜의 회전 각도 정보만을 사용하여 공전을 검출하는 방법에서는, 차륜의 회전 각도 정보에 포함되는 노이즈 성분에 반응하지 않도록, 필터 차수를 올리거나, 임계값을 올리는 수밖에 없어, 공전 검출에 지연이 발생하는 경우가 있다.

본 발명의 목적은 도립 진자형 이동 기구로 편륜 공전이 발생했을 때에 이것을 빠르게 검출할 수 있고, 공전 지속 시간이 긴 경우라도 기립 상태를 유지할 수 있는 도립 진자형 이동 기구를 제공하는 데 있다.

본 발명의 도립 진자형 이동 기구는 좌우의 차륜 및 이들 차륜을 회전 구동하는 주행 모터를 갖는 이동 기구와, 이동 기구에 지지된 상체와, 이동 기구를 제 어하는 제어 장치를 구비하고, 상기 제어 장치는 차륜의 공전 검출부 및 트랙션 복귀 검출부를 구비하고, 공전 검출부에서 공전이 검출되지 않은 경우에 양륜 기립 주행 제어를, 공전이 검출된 경우에 접지 차륜에 의한 접지 차륜 기립 제어를 행하는 동시에, 공전 차륜에 대해 트랙션 복귀를 촉구하는 공전 차륜 제어를 행하고, 트랙션 복귀 검출부에서 트랙션 복귀가 검출되지 않은 경우에 접지 차륜 기립 제어로 복귀되고, 트랙션 복귀가 검출된 경우에 양륜 기립 주행 제어로 복귀되는 공전 대응 제어를 행한다.

본 발명에 따르면, 편륜 공전 발생 시에 빠르게 이것을 검출하여, 접지 차륜에서 기립 상태를 유지하면서, 공전 차륜에 대해서는 조기의 트랙션 복귀를 지원하고, 또한 트랙션 복귀 시에 이것을 검지하여 양륜 접지 기립 제어로 바로 이행시킴으로써, 공전의 발생으로부터 종료까지 기립 상태를 유지하여 전도가 발생하지 않는 도립 진자형 이동 기구를 제공할 수 있다.

이하, 본 발명의 일 실시예에 관한 도립 진자형 이동 기구에 대해, 도 1 내지 도 6을 사용하여 설명한다.

우선, 본 실시예의 이동 로봇(101)의 구성을, 도 1을 참조하여 설명한다. 도 1a는 본 실시예의 이동 로봇의 구성을 설명하는 정면도, 도 1b는 도 1a의 이동 로봇의 측면도, 도 1c는 평면도(상면도)이다.

이동 로봇(101)은 도립 진자형의 이동 로봇이고, 이동 기구(102)와 상 체(103)로 크게 구별된다. 이동 기구(102)는 좌우의 차륜(104 및 105)과, 이들을 회전 구동하는 좌우의 주행 모터(106 및 107)를 구비하고 있다. 상체(103)는 이동 기구(102)의 상부에 회전 가능하게 지지되어 있다. 이동 기구(102)의 상부에는 연직 방향을 기준으로 한 상체(103)의 경사를 검출하는 자세 방위 센서(108), 이동 로봇의 요축 주위의 회전량(선회량)을 검출하는 자세 방위 센서(109)가 설치되어 있다. 상체(103)는 작업용 머니퓰레이터(110), 대인 인터페이스 기능을 갖는 헤드부(111) 등의 작업 장치, 로봇 전체를 제어하는 제어 장치(112)를 구비하고 있다.

다음에, 이동 로봇(101)의 제어계의 구성에 대해, 도 2 및 도 3을 참조하여 설명한다. 도 2는 이동 로봇의 제어계의 구성도이다. 도 3은 차륜의 공전 발생에 대처하기 위한 공전 대응의 제어 플로우이다. 제어 장치(112)는 이동 목표 생성부(201), 동작 계획부(202), 목표 모터 구동 토크 연산부(203), 좌측 모터 구동 장치(204), 우측 모터 구동 장치(205), 공전 검출부(208), 트랙션 복귀 검출부(209), 경로 제어부(210)를 구비하고 있다.

이동 목표 생성부(201)에서는 이동 로봇(101)의 이동 목표인, 도착 위치, 이동 시간, 이동 속도, 최대 이동 가속도, 최대 모터 구동 토크 등을 생성한다. 동작 계획부(202)에서는 이동 목표 생성부(201)로부터 도착 위치, 이동 시간, 이동 속도, 최대 이동 가속도, 최대 모터 구동 토크를 목표로 하여 받고, 이동 로봇의 시계열마다의(시계열에 따름) 목표 위치, 목표 속도, 목표 경사 각도, 이동 모터 구동 토크를 생성한다. 생성 방법은, 예를 들어 특허 문헌 3의 방법을 사용할 수 있다.

경로 제어부(210)에서는 이동 목표 생성부(201)로부터 도착(도달) 위치를 취득하여, 도착 위치까지의 경로를 생성하고, 그 경로 상에 있어서의 선회 각도 목표값 및 선회 각속도 목표값을 계산한다. 이하, 선회 각도 목표값 및 선회 각속도 목표값을, 정리하여 선회 목표값이라고 부른다.

목표 모터 구동 토크 연산부(203)에서는 공전 검출부(208)로부터 차륜의 공전 정보를, 트랙션 복귀 검출부(209)로부터 공전 차륜의 트랙션 복귀 정보를, 동작 계획부(202)로부터 이동 목표값을, 경로 제어부(210)로부터 선회 목표값을 각각 취득한다. 또한, 목표 모터 구동 토크 연산부(203)는 좌우의 인코더(각속도 센서)(206 및 207)로부터 좌우 차륜의 각속도(dθ_L/dt 및 dθ_R/dt)를, 자세 방위 센서(108)로부터 상체(103)의 연직 방향으로부터 경사 각속도(dθ₁/dt)를, 자세 방위 센서(109)로부터 상체(103)의 요 선회 각속도(dθ_y/dt)를 각각 취득한다. 목표 모터 구동 토크 연산부(203)는 이들의 취득한 정보를 사용하여, 각 제어 주기에 있어서, 도 3에 도시하는 공전 대응 제어 플로우에 따라서, 좌우의 모터 구동 장치(204 및 205)로 목표 모터 구동 토크(τ_{L_r} 및 τ_{R_r})를 지정한다.

다음에, 좌우의 모터 구동 장치(204 및 205)는 목표 모터 구동 토크 연산부(203)로부터 목표 모터 구동 토크(τ_{L_r} 및 τ_{R_r})를, 좌우의 인코더(206 및 207)로부터 좌우 차륜의 각속도(dθ_L/dt 및 dθ_R/dt)를 각각 취득하고, 좌우의 주행 모터(106 및 107)의 모터 구동 토크(τ_L 및 τ_R)가 목표 모터 구동 토크(τ_{L_r} 및 τ _{R_r})에 동등해지도록 각각 제어를 행한다.

목표 모터 구동 토크 연산부(203)는, 도 3의 공전 대응 제어 플로우와 같이 공전 검출부(208)가 차륜의 공전을 검출하지 않았던 경우에는 양륜 기립 주행 제어(301)를 행한다. 한편, 공전 검출부(208)가 차륜의 공전을 검출한 경우에는, 비공전 검출측의 접지 차륜에 대해 접지 차륜 기립 제어(302)를 행하고, 계속해서 공전 차륜 제어(303)를 행하고, 트랙션 복귀 검출부(209)에서 공전 차륜의 트랙션이 복귀되어 있는지 여부의 정보를 수취한다. 트랙션이 복귀되어 있지 않은 경우에는 다음의 제어 주기로 접지 차륜 기립 제어(302)로 복귀되고, 트랙션이 복귀된 경우에는 다음의 제어 주기로 공전 검출부(208)에 의한 편륜 공전 검출로 복귀된다. 상기한 양륜 기립 주행 제어(301)에 있어서의 구동 토크 산출 방법은, 예를 들어 특허 문헌 3의 방법을 사용할 수 있다.

다음에, 공전 검출부(208)에 있어서의 편륜 공전 검출 방법을 도 4의 흐름도를 사용하여 설명한다. 도 1d와 같이, XY 평면 상에서 상체(103)의 무게 중심을 지나는 요축(Z축)의 반시계 방향의 회전 변위 각도를 θ_y로 했을 때, S401에서 좌우 차륜의 인코더(206 및 207)의 출력 신호로부터 연산 가능한 단시간 내의 차륜 각도 적산값(Δθ_L 및 Δθ_R)을 사용하여, 단시간 내의 회전 변위 각도(선회 이동량)(Δθ_{y_odo})를 식 1로부터 산출한다. 여기서, 식 1의 r은 차륜 반경, w는 차륜의 트레드 폭이다.

[식 1]

한편, S402에서 자세 방위 센서(109)의 출력 신호의 단시간 내 적분값으로부터, 단시간 내의 회전 변위 각도(선회 이동량)(Δθ_{y_gyro})를 산출한다. 다음에, S403에서 Δθ_{y_odo}와 Δθ_{y_gyro}의 차분(Δθ_{y_diff} ＝ aㆍΔθ_{y_gyro} － Δθ_{y_odo})을 산출하고, 그 절대값을 미리 설정된 Δθ_{y_threshold}와 비교한다. 여기서, a는 Δθ_{y_gyro}의 Δθ_{y_odo}에 대한 가중 계수이다.

양 차륜에 충분한 트랙션이 발생하고 있는 경우, 양 회전 이동량은 근사값을 취하므로 Δθ_{y_diff}의 절대값은 Δθ_{y_threshold} 미만의 값으로 되어, 양륜이 접지되어 있다고 판단한다. 한편, 편측의 차륜의 트랙션이 빠진 경우, 예를 들어 좌측 차륜이 공전한 경우, 공전 차륜측으로부터 상체로의 반력이 없어지므로 요축 주위에 회전력이 발생하고, 우측 모터 구동 토크 τ_R이 양의 값이면 Δθ_{y_gyro}는 양의 값으로, τ_R이 음의 값이면 Δθ_{y_gyro}는 음의 값으로 된다.

공전 차륜에 대해서는, 좌측 모터 구동 토크 τ_L이 양의 값이면 dθ_L/dt는 급격한 가속을, τ_L이 음의 값이면 dθ_L/dt는 급격한 감속을 하므로, 식 1로부터 산출되는 Δθ_{y_odo}는 실제의 회전 이동량에 대해 외관상 반대의 방향으로 큰 값을 취한다. 즉, Δθ_{y_odo}와 Δθ_{y_gyro}는 공전 발생 시에 역부호의 값을 취하게 되고, S403에서 Δθ_{y_diff}의 절대값은 공전 발생 후 빠르게 Δθ_{y_threshold}보다 큰 값을 취하고, 차륜의 회전 각속도 정보만을 사용했을 때보다도 신속하게 공전 검출이 가능해진다.

S403의 조건을 만족시킨 경우에는, S404에서 Δθ_{y_diff}의 부호 판정을 행하고, 또한 S405, S406에서 구동 토크의 부호 판정에 의해, 어떤 차륜이 공전하고 있는지를 판정한다(S407, S408).

다음에, 도 3의 접지 차륜 기립 제어(302)에 있어서의 이동 로봇(101)의 모터 구동 토크의 산출 방법에 대해 설명한다. 이하에서는, 좌측 차륜(104)이 공전한 예를 나타내지만, 우측 차륜(105)이 공전한 경우도 동일한 방법으로 대응 가능하다.

접지 차륜 기립 제어(302)에서는, 도 5에 도시하는 제어계에서, 이동 로봇(101)의 기립 상태를 유지하기 위해 필요한 접지 차륜(우측 차륜)(105)에 관한 모터 구동 토크 τ_{R_r}이 연산된다. 본 실시 형태에서는, 도 5에서 사용하는 이동 기구(102)의 운동에 관한 상태량에 대해, 편륜 공전에 수반하는 상체(103)의 요축 주위의 선회 운동 성분을 제외한 접지 차륜의 운동 정보를 사용하는 것으로 한다. 또한, 기립 제어에 필요한 피드백 게인을, 편륜 공전 상태를 고려하여 설정한다. 이하에, 이 상태 변수 및 피드백 게인 행렬(K)의 연산 방법에 대해 설명한다.

접지 차륜 기립 제어(302)에서 사용하는 이동 기구(102)에 관한 운동 정보에 대해, 접지 차륜(105)의 인코더(207)로부터 얻어지는 차륜 회전 각속도 정보(dθ _R/dt)와, 자세 방위 센서(109)로부터 얻어지는 요축 주위의 선회 운동 정보(dθ_y/dt)를 사용하여, 상체(103)의 요축 주위의 선회 운동 성분을 제외한 보정 속도 정보(보정 회전 각속도)(dθ_c/dt)를 다음 식에 의해 산출한다.

[식 2]

도 5의 기립 제어계에 있어서, 식 2에서 얻어지는 속도 정보 및 그 적산값을 상태량으로서 사용함으로써, 제어계를 불안정화시키는 상체(103)의 요 선회 운동 정보 성분의 혼입을 작게 하는 것이 가능하다. 이는, 특별히 제어 장치(112)의 제어 주기가 긴(느린) 경우, 상체(103)의 요축 주위의 관성 모멘트가 작은 경우에, 효과적이다.

다음에, 도 5에 도시되어 있는 접지 차륜 기립 제어(302)의 블록선도 중에 있어서의 피드백 게인(K)을 구하는 수순을 도시한다. 이동 로봇(101)의 차륜의 편륜이 공전 중에는, 공전 차륜에는 트랙션이 전혀 부하되어 있지 않다고 하여, 기립 상태 유지를 위해 필요한 힘의 균형 관계를, 도 1b에 있어서의 상체(103)의 무게 중심을 지나는 XZ 평면 상에서 고려된다.

여기서, 이동 로봇(101)은 이동 기구(102) 및 상체(103)로 구성되고, 이동 기구(102)는 좌우의 차륜(104, 105), 좌우의 주행 모터(106, 107) 및 이들 차륜과 주행 모터를 접속하는 차축으로 이루어지고, 차륜 1개당의 질량을 m₀, 차축의 주위 의 관성 모멘트를 J₀으로 한다. 상체(103)는 그 이외의 부분으로 하고, 그 질량을 m_l, 차축으로부터 본 무게 중심의 경사에 관한 관성 모멘트를 J_l로 하고, 차축과 무게 중심 사이의 거리를 l로 하는 질점으로 대표한다. 차륜의 반경을 r, 각 차륜 구동부와 상체(103) 사이의 점성 저항을 D로 한다. 이들 파라미터 m₀, m_l, J₀, J_l, l, r, D는 실제 기기를 측정해도 좋고, 설계값으로부터 계산해도 좋다.

XZ 평면 상에서, 차륜(104, 105)과 상체(103)가 이루는 회전 각도를 각각 θ_L, θ_R로 하고, 상체(103)의 연직 방향에서의 기울기를 θ_l로 한다. 주행 모터(106, 107)의 구동 토크를 각각 τ_L, τ_R로 한다. 간단하기 하기 위해, 이동 로봇(101)의 총 질량을 M_all ＝ (m_l ＋ 2m₀)로 한다.

이때, θ_l과, 상술한 접지 차륜의 보정 회전 각속도(dθ_c/dt)를 적산한 θ_c에 관한 운동 방정식에 대해, θ_l이 충분히 작다고 간주하고 선형 간략(근사)한 것을 식 3a, 식 3b에 나타낸다.

[식 3]

또한, 식 3a, 식 3b를 상태 공간 표현한 것이 식 4이다. 단, τ_{R_offset} ＝ τ_R-Dㆍdθ_R/dt로 해 두고, 공전 차륜의 각 가속도에 의한 상체(103)로의 반토크의 영 향은 작다고 하여 무시한다.

[식 4]

이 상태 공간 표현에 관해서는, 공지의 다양한 제어 이론에 기초하여 상태 피드백 게인 행렬(K)을 산출하고, 상태 피드백 제어를 실시함으로써, 기립 상태를 유지할 수 있다. 이 제어계를 도시한 것이 도 5이다. 단, 도 5의 fr은 동작 계획부(202)로부터의 동작 계획 목표값이고, 경로 제어부(210)로부터의 선회 목표값은 차단되어 있다.

따라서, 접지 차륜(105)에 관한 목표 구동 토크 τ_{R_r}은 식 5와 같이, θ_c, θ_l, dθ_c/dt, dθ_l/dt에 대해 상태 피드백 게인 행렬(K)의 각 성분 k₁, k₂, k₃, k₄를 곱하여 가산한 것에, 차륜의 점성 저항을 캔슬하는 토크를 보충하게 한 것으로 된다.

[식 5]

이상과 같이, 상체(103)의 무게 중심 연직 하점에 있어서의 보정 회전 각속도(dθ_c/dt)와 그 적산값(θ_c)을 이동 기구(102)에 관한 상태량으로 하고, 식 4의 상태 공간 표현으로부터 구해지는 피드백 게인 행렬(K)을 사용한 도 5에서 나타내는 상태 피드백 제어를 실시함으로써, 양륜 접지 상태를 가정한 기립 제어에 비해, 기립 상태 유지를 보다 길게 유지하는 것이 가능한 제어계가 구축된다.

다음에, 도 3의 공전 차륜 제어(303) 및 트랙션 복귀 검출부(209)에 대해 설명한다. 공전 차륜 제어(303)에서는 공전 발생 후의 트랙션 복귀를 촉구하고, 또한 용이한 트랙션 복귀의 검출이 구해진다. 따라서, 본 실시 형태에서는, τ_{R_r}을 식 6과 같이 제어한다.

[식 6]

여기서, dθ_{L_ref}/dt는 접지 차륜의 각속도(dθ_R/dt)와, 요축 주위의 자세 각속도(선회 각속도)(dθ_y/dt)로부터 식 6b에 의해 구해지는 바닥과 공전 차륜의 상대 속도이다. F_friction은 원하는 트랙션 복귀 검출량이고, Dㆍ(dθ_{L_ref}/dt)/r보다도 작은 값으로 설정해 둔다. 이때, 공전 차륜의 운동 방정식은 식 7로 된다.

[식 7]

이 운동 방정식에 의해, 토크 지령을 식 6과 같이 설정해 두면, 공전 차륜에 트랙션이 전혀 작용하지 않는 상황이 계속되면, 대략 바닥면과의 상대 속도로부터 (rㆍF_friction)/D만큼 느린 속도에 근접해 가는 것을 알 수 있다. 이에 의해, 바닥면과 공전 차륜 사이에 마찰력이 작용할 때, 마찰 계수가 정지 마찰 계수에 근접하므로, 트랙션 복귀가 촉구된다.

공전 차륜 제어(303)에 의해, F_friction 이상의 마찰력이 부하되면 자동적으로 공전 차륜 각속도(rㆍdθ_L/dt)가 바닥과의 상대 속도와 일치하게 되므로, 공전 차륜 각속도(rㆍdθ_L/dt)가 바닥과의 상대 속도와 동등한 값으로 되면, F_friction 이상의 트랙션이 복귀되었다고 판정할 수 있다.

단, 공전 발생 직후에 일단 공전이 가속하고, 그 후 공전 차륜 제어(303)가 기능하여 공전 차륜이 감속하고 있을 때, 일시적으로 공전 차륜 속도와 바닥의 상 대 속도가 일치하는 상황이 있을 수 있다. 따라서, 본 실시 형태에서는, 도 6의 흐름도에 따라서 트랙션 복귀 검출부(209)가 트랙션 복귀의 검출을 행한다.

우선, 제어 장치(112)의 각 제어 주기에 있어서, S601에서 접지 차륜 속도(dθ_R/dt) 및 자세 방위 센서 출력(dθ_gyro/dt)으로부터 산출되는 공전 차륜 속도(dθ_R/dt-wㆍdθ_gyro/dt)와, dθ_L/dt의 차분이 임계값(ε_{v_threshold}) 미만이라고 판정한 경우, 즉 F_friction 이상의 마찰력이 부하되어 있다고 판정한 경우, S603에서 마찰력의 부하 계속 시간을 카운트하여, S604에서 마찰 부하 계속 시간이 트랙션 복귀 판정에 충분한 시간 길이 이상인지 판정한다. 트랙션 복귀라고 판정한 경우에는, S605에서 마찰 부하 계속 시간을 초기화한다. S601의 조건을 만족시키고 있지 않다고 판단한 경우에는, 마찰력이 부하되어 있지 않다고 판정하여, S602에서 마찰 부하 계속 시간을 초기화한다. S601의 조건을 만족시키면서 S6O4의 조건을 만족시키고 있지 않은 경우에는, 현상의 마찰 부하 계속 시간을 유지한 상태로 다음의 제어 주기로 복귀된다. S604의 복귀 판정 시간 길이는 트랙션 누락 시의 공전 차륜 감속 중에, 일시적으로 S601의 조건을 만족시키는 시간 간격보다 큰 값에 대응하도록 설정된다. 마찰 부하 계속 시간은 이동 로봇(101)의 기동 시에 자동적으로 초기화되는 것으로 한다. 이상의 실시 형태에 의해, 공전 차륜의 트랙션 복귀를 고정밀도로 검출하는 것이 가능해진다.

이상, 본 실시 형태에 따르면, 도립 진자형 이동 기구에 있어서, 항시 좌우 차륜의 회전 차로부터 산출되는 선회량(회전량)과, 자세 방위 센서로부터 산출되는 선회량을 비교함으로써, 편륜의 공전 발생을 빠르게 검출할 수 있다. 공전 검출 중에는, 접지 차륜에는 편륜 접지를 전제로 한 운동 방정식으로부터 유도되어, 편륜 공전에 수반하는 상체의 요축 주위의 선회 운동 성분을 제외한 접지 차륜의 운동 정보를 상태량으로서 이용한 기립 제어가 적용된다. 공전 차륜은 트랙션 복귀와 그 검출을 원조(서포트)하도록, 공전 차륜과 바닥의 상대 속도, 기대하는 바닥과 공전 차륜 사이의 마찰력, 차륜의 직경, 차륜의 점성 저항값 등에 기초하여 출력 토크가 제어된다. 공전 차륜의 각속도가 도립 진자형 이동 기구의 이동 속도와 일정 시간 동일하게 된 경우, 공전 차륜의 트랙션이 복귀되었다고 판정하여, 양륜 접지 기립 제어로 복귀함으로써, 편륜의 공전 발생 중에도 안정된 기립 상태를 유지하여 전도를 억제할 수 있다.

도 1a는 본 발명의 일 실시예에 관한 이동 로봇의 기구 구성을 설명하는 정면도.

도 1b는 도 1a의 이동 로봇의 측면도.

도 1c는 도 1a의 이동 로봇의 평면도(상면도).

도 1d는 도 1a의 이동 로봇의 요 선회 운동의 변위를 도시하는 평면도.

도 2는 도 1의 이동 로봇의 제어계 구성도.

도 3은 도 1의 이동 로봇의 공전 대처 제어 방법을 도시하는 흐름도.

도 4는 도 2의 공전 검출부에 의한 공전 검출 방법을 도시하는 흐름도.

도 5는 도 3의 접지 차륜 기립 제어의 블록선도.

도 6은 도 2의 트랙션 복귀 검출부의 트랙션 복귀 검출 방법을 도시하는 흐름도.

<도면의 주요 부분에 대한 부호의 설명>

101 : 이동 로봇

102 : 이동 기구

103 : 상체

104, 105 : 차륜

106, 107 : 주행 모터

108, 109 : 자세 방위 센서

110 : 머니퓰레이터

111 : 헤드부

112 : 제어 장치

201 : 이동 목표 생성부

202 : 동작 계획부

203 : 목표 모터 구동 토크 연산부

204 : 좌측 모터 구동 장치

205 : 우측 모터 구동 장치

206, 207 : 인코더

208 : 공전 검출부

209 : 트랙션 복귀 검출부

210 : 경로 제어부

Claims (6)

1. 좌우의 차륜 및 이들 차륜을 회전 구동하는 주행 모터를 갖는 이동 기구와, 상기 이동 기구에 지지된 상체와, 상기 이동 기구를 제어하는 제어 장치를 구비한 도립 진자형 이동 기구에 있어서,

   상기 제어 장치는 차륜의 공전 검출부 및 트랙션 복귀 검출부를 구비하고, 상기 공전 검출부에서 공전이 검출되지 않은 경우에는 양륜 기립 주행 제어를 행하고, 상기 공전 검출부에서 공전이 검출된 경우에는 접지 차륜에 의한 접지 차륜 기립 제어를 행하는 동시에,

   공전 차륜에 대해 트랙션 복귀를 촉구하는 공전 차륜 제어를 행하고, 상기 트랙션 복귀 검출부에서 트랙션 복귀가 검출되지 않은 경우에는 상기 접지 차륜 기립 제어로 복귀되고, 상기 트랙션 복귀 검출부에서 트랙션 복귀가 검출된 경우에는 상기 양륜 기립 주행 제어로 복귀됨으로써 공전 대응 제어를 행하는, 도립 진자형 이동 기구.
2. 제1항에 있어서, 상기 접지 차륜 기립 제어는 상기 이동 기구가 구비한 접지 차륜의 회전 운동을 검출 가능한 각속도 센서의 정보와, 상기 상체가 구비한 상기 상체의 연직 방향에 대한 경사각 운동을 검출 가능한 제1 자세 방위 센서의 정보와, 상기 상체가 구비한 상기 상체의 요축 주위의 선회각 운동을 검출 가능한 제2 자세 방위 센서의 정보에 기초하여 기립 상태 제어를 행하는, 도립 진자형 이동 기 구.
3. 제2항에 있어서, 상기 각속도 센서의 정보 및 상기 제2 자세 방위 센서의 정보는, 상기 상체의 요축 주위의 선회 운동 성분을 제외한 상기 접지 차륜의 운동 정보로 되도록 연산되어 상기 기립 상태 제어에 사용되는, 도립 진자형 이동 기구.
4. 제1항에 있어서, 상기 공전 검출부는 상기 좌우 차륜의 회전 운동을 각각 검출 가능한 각속도 센서의 정보의 차로부터 산출되는 요 선회 운동량과, 상기 상체가 구비한 요 선회 운동을 검출 가능한 자세 방위 센서의 정보로부터 산출되는 요 선회 운동량과의 차분값을 임계값과 비교함으로써 상기 차륜의 공전을 검출하는, 도립 진자형 이동 기구.
5. 제1항에 있어서, 상기 공전 차륜 제어는 상기 이동 기구가 구비한 접지 차륜의 회전 운동을 검출 가능한 각속도 센서의 정보 및 상기 상체가 구비한 요 선회 운동을 검출 가능한 자세 방위 센서의 정보로부터 산출되는, 공전 차륜과 바닥의 상대 속도에 차륜의 점성 저항을 곱한 값으로부터, 원하는 최저 복귀 트랙션량에 차륜 반경을 곱한 값을 뺀 값을, 공전 차륜으로의 구동 토크로 하는, 도립 진자형 이동 기구.
6. 제1항에 있어서, 상기 트랙션 복귀 검출부는 공전 차륜 속도와, 공전 차륜과 바닥의 상대 속도의 차가 일정한 임계값 이내에 있는 마찰 부하 계속 시간이, 미리 결정된 복귀 판정 시간 길이를 초과했을 때에, 트랙션이 복귀되었다고 판정하는, 도립 진자형 이동 기구.

Images