JPH05277968A - 脚式移動ロボットの制御装置 - Google Patents

Abstract

(57)【要約】 【構成】 ２足歩行の脚式移動ロボットの制御装置にお
いて、ロボットの数学モデルを構築するときに、脚部リ
ンクのモデルを、大腿リンクと下腿リンクを２辺とする
３角形の１辺が鉛直方向になす角度で求める。 【効果】 膝関節が大きく曲がる歩行も正確にモデリン
グすることができ、着地誤差が減少する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は脚式移動ロボットの制
御装置に関し、より具体的には２足歩行のロボットにつ
いて数学モデルを構築して制御系を設計する様にした脚
式移動ロボットの制御装置において、モデリング手法を
改良したものに関する。

【０００２】

【従来の技術】脚式移動ロボット、特に２足歩行の脚式
移動ロボットとしては、例えば特開昭６２−９７００５
号公報記載のものが知られている。

【０００３】

【発明が解決しようとする課題】その様な２足歩行の脚
式移動ロボットを安定に制御するために、ロボットの数
学モデルを構築し、そのモデルに対して制御系を設計す
ることは行われており、本出願人も先に特願平２−２５
９８３９号（平成２年９月２８日出願）において、２足
歩行ロボットの運動を前後方向と左右方向とに分け、そ
れぞれ３リンクのモデルで近似して制御する技術を提案
している。そして、特に前後方向については歩行中に膝
関節が微小にしか曲がらないものと見做し、図１７に示
す様に、支持脚の絶対角をリンク１または２の絶対角度
とし、遊脚の絶対角をリンク４または５の絶対角で求め
てモデリングしていた。この手法は膝関節が曲がらない
歩行においては良好な結果を得ることができる。

【０００４】しかしながら、階段を昇降するときなど膝
関節が大きく曲がったりするとき、或いは平地でも膝関
節を大きく曲げて歩行するときなど、この手法では誤差
が生じる不都合があった。かと言って、他方、安易にロ
ボットモデルのリンク数を増やすと制御系の設計が複雑
となり、また実際の制御においてもコンピュータの演算
負荷が増大することとなって得策ではない。

【０００５】従って、この発明の目的は、この様な相反
する課題を解決する技術を提案することにあり、２足歩
行ロボットの前後方向モデルを、膝関節が大きく曲がる
ときも３リンクのままで、膝関節で細分した５リンクモ
デルに十分近い精度で数学的に近似できる様にした脚式
移動ロボットの制御装置を提供することにある。

【０００６】

【課題を解決するための手段】上記した課題を解決する
ためにこの発明は、基体と、それに連結される２本の脚
部とからなり、該２本の脚部がそれぞれ基体に第１の関
節を介して連結されると共に、第１の関節と脚部先端と
の間に第２の関節を備え、更に脚部先端付近に第３の関
節を備えてなる脚式移動ロボットの制御装置であって、
該２本の脚部をそれぞれ１個のリンクと見做して数学モ
デルで近似し、該リンクの鉛直方向に対する絶対角度を
含むパラメータを用いて運動方程式表現を行ってその解
を求め、求めた解に基づいて制御値を決定するものにお
いて、前記リンクの絶対角度を、前記第１の関節と第３
の関節とを結ぶ線が鉛直方向に対してなす角度で求める
様に構成した。

【０００７】

【作用】基体と連結する第１関節と脚部先端付近の第３
関節とを結ぶ線が鉛直方向に対してなす絶対角度でとる
様にしたので、即ち、図１８に示す様に、前後方向のモ
デルにおいて、支持脚の絶対角をリンク１，２を２辺と
する３角形の１辺の角度とし、同様に遊脚の絶対角をリ
ンク４，５を２辺とする３角形の１辺の角度としてモデ
リングする様に構成した。その結果、３リンクモデルで
ありながら、第２関節（膝関節）が大きく曲がる歩行に
おいても誤差を低減することができると共に、第２関節
をも含めた５リンクモデルに十分近い精度で数学的に近
似することができ、よって着地誤差が減少するなど、制
御値を一層的確に決定することができる。

【０００８】

【実施例】以下、脚式移動ロボットとして２足歩行のロ
ボットを例にとってこの発明の実施例を説明する。図１
はそのロボット１を全体的に示す説明スケルトン図であ
り、左右それぞれの脚部リンク２に６個の関節（軸）を
備える（理解の便宜のために各関節（軸）をそれを駆動
する電動モータで例示する）。該６個の関節（軸）は上
から順に、腰の脚部回旋用の関節（軸）１０Ｒ，１０Ｌ
（右側をＲ、左側をＬとする。以下同じ）、腰のピッチ
方向（ｘ方向）の関節（軸）１２Ｒ，１２Ｌ、同ロール
方向（ｙ方向）の関節（軸）１４Ｒ，１４Ｌ、膝部のピ
ッチ方向の関節（軸）１６Ｒ，１６Ｌ、足首部のピッチ
方向の関節（軸）１８Ｒ，１８Ｌ、同ロール方向の関節
（軸）２０Ｒ，２０Ｌとなっており、その下部には足平
（足部）２２Ｒ，２２Ｌが取着されると共に、最上位に
は胴体部（基体）２４が設けられ、その内部には制御ユ
ニット２６が格納される。上記において股関節は関節
（軸）１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から
構成され、また足関節は、関節（軸）１８Ｒ（Ｌ），２
０Ｒ（Ｌ）から構成される。また、股関節と膝関節との
間は大腿リンク２８Ｒ，２８Ｌで、膝関節と足関節との
間は下腿リンク３０Ｒ，３０Ｌで連結される。

【０００９】ここで、脚部リンク２は左右の足について
それぞれ６つの自由度を与えられ、歩行中にこれらの６
×２＝１２個の関節（軸）をそれぞれ適宜な角度に駆動
することで、足全体に所望の動きを与えることができ、
任意に３次元空間を歩行することができる様に構成され
る。先に述べた様に、上記した関節は電動モータからな
り、更にはその出力を倍力する減速機などを備えるが、
その詳細は先に本出願人が別途提案した出願（特願平１
−３２４２１８号、特開平３−１８４７８２号）などに
述べられており、それ自体はこの発明の要旨とするとこ
ろではないので、これ以上の説明は省略する。

【００１０】ここで、図１に示すロボット１において、
足首部には公知の６軸力センサ３６が設けられ、足平を
介してロボットに伝達されるｘ，ｙ，ｚ方向の力成分Ｆ
ｘ，Ｆｙ，Ｆｚとその方向回りのモーメント成分Ｍｘ，
Ｍｙ，Ｍｚとを測定し、足部の着地の有無と支持脚に加
わる力の大きさと方向とを検出する。また足平の四隅に
は公知の接地スイッチ３８が設けられて、接地の有無を
検出する。更に、胴体部２４の上部には、一対の傾斜セ
ンサ４０，４２が設置され、ｘ−ｚ平面内のｚ軸に対す
る傾きとその角速度、同様にｙ−ｚ平面内のｚ軸に対す
る傾きとその角速度を検出する。また、各関節の電動モ
ータには、その回転量を検出するロータリエンコーダが
設けられる（図１において足関節の電動モータ用のもの
のみを示す）。これらセンサ３６などの出力は前記した
胴体部２４内の制御ユニット２６に送られる。

【００１１】図２は制御ユニット２６の詳細を示すブロ
ック図であり、マイクロ・コンピュータから構成され
る。そこにおいて傾斜センサ４０，４２などの出力はＡ
／Ｄ変換器５０でデジタル値に変換され、その出力はバ
ス５２を介してＲＡＭ５４に送られる。また各電動モー
タに隣接して配置されるエンコーダの出力は可逆カウン
タ５６を介してＲＡＭ５４内に入力されると共に、接地
スイッチ３８等の出力は波形整形回路６２を経て同様に
ＲＡＭ５４内に格納される。制御ユニット内には演算装
置６４が設けられており、ＲＯＭ６６に格納されている
歩行パターンを読み込んで可逆カウンタ６０から送出さ
れる実測値との偏差から電動モータの速度指令値を算出
し、Ｄ／Ａ変換器６８を介してサーボアンプに送出す
る。また図示の如く、エンコーダ出力はＦ／Ｖ変換回路
を介してサーボアンプに送出されており、図３に示す様
にマイナーループとしての速度フィードバック制御が実
現されている。

【００１２】続いて、本制御装置の動作を説明する。

【００１３】図３は図１に示したロボット１について、
公知の関節角サーボ系を構成した場合を示すブロック図
である。予めオフラインで設計しておいた平坦路を前提
とする歩行パターンを１２個の関節の角度ｑｒ、角速度
ｑｒドットの時系列データ、即ち、関節軌道に変換し、
それを各関節のサーボアンプに指令値として一定間隔で
出力する。その結果、各関節角は指令値通りにほとんど
遅れなく追従し、ロボット１は設計された歩行パターン
通りに駆動されて歩行する。これによって平坦路は支障
なく歩行するが、予期しない凹凸路面に遭遇したとき
は、支持脚の足平の角度が一定ではないことから、必ず
しも安定した歩行を確保することはできない。

【００１４】そこでこの制御においては図４に示す様
に、図３に示した関節角サーボ系に、後で述べる操作量
ｕを加えて凹凸路面に対する安定化を試みた。以下、こ
の操作量ｕの算出とそれによる歩行安定化について説明
する。

【００１５】先ず、図３の関節角サーボ系を備えたロボ
ット１について数式モデルを作成する。その際に次に示
す仮定を設ける。 〔仮定１〕ロボット１の前後方向の運動と左右方向の運
動とは相互干渉が安定化制御により十分小さく、分離す
ることができる。（図５に前後方向の、図６に左右方向
の運動を示す。） 〔仮定２〕２本の脚部リンクのうちのどちらかの足平２
２は、路面に完全に接地しているものとする。また、足
平が完全に路面に接地している方の脚を支持脚とする。 〔仮定３〕支持脚の足平は、路面に対して動かない（滑
らない）ものとする。（一般に歩行パターンは、片足支
持期において支持脚の足平が路面に対して動かない様に
設計する。例えば、いわゆるＺＭＰ（zero moment poin
t)の概念を用いて、足平が路面から離れない様にＺＭＰ
の軌道を決めて設計される。また倒立振り子の概念を用
いた歩行パターンにおいても、足首回りで回転する場合
には足平は物理的に路面から離れない。滑りに対して
は、足裏にゴムを貼るなど適宜処理することにより滑ら
ない様にすることは可能である。）

【００１６】以上の仮定から、図１に示したロボット１
の運動方程式は、数１の様に表すことができる。数１の
式から明らかな様に、仮定３が成り立てば、ロボット１
の運動は足平の角度に無関係である。

【００１７】

【数１】

【００１８】ところで、この制御においては図３に示す
関節角サーボ系を備えることから、各関節（符号ｉで代
表させる）の発生トルク（図３中のモータ発生トルク）
は、関節角ｑｉと関節角指令値ｑｉｒとから数２に示す
式を用いて計算される。（尚、数２の式中のｋｐ，ｋ
ｄ，ｆｖは、図３中のｋｐ，ｋｄにｋｖ，ｋｃ，ｋｔを
それぞれ含む値の意味で使用する。）

【００１９】

【数２】

【００２０】一般に図５，６に示す様に、関節角ｑｉと
リンク角θｉとの関係は数３で表すことができる。ここ
で、先に図１８に示した様に、ロボットの前後方向のモ
デルを腰関節と足関節とを結ぶ線を脚部リンクと仮想的
に見做し、図１９に表される如くモデリングする。この
とき、モデリング前後の各リンク角（図５、図１９）の
関係は数３−２で表される。尚、この明細書において、
「関節角」または「ｑ」はリンク間の角度（即ち、相対
角度）を、「リンク角」または「θ」は重力方向に対す
る（絶対座標における）リンクの角度（即ち、絶対角
度）を意味するものとして使用する。以下、リンク角と
しては図１９のモデリング後のものを用いる。

【００２１】

【数３】

【００２２】

【数４】

【００２３】数２の式を数３の式によってリンク角で表
してまとめると、数５に示す様になる。

【００２４】

【数５】

【００２５】尚、数５の外乱ｄ（θ０，θ０ｒ）、即
ち、足平角度θ０の実際値と目標値とによって生じる値
は数６の様に示すことができる。

【００２６】

【数６】

【００２７】ここで、数５の式を数１の式に代入して整
理すると、数７の如くになる。

【００２８】

【数７】

【００２９】この数７に示す式が、関節角サーボ系によ
るロボットの運動方程式となる。数１の式では運動を制
御する操作量は関節トルクＴであるが、数７の式におい
ては目標姿勢Θｒ（絶対角度），Θｒドット（絶対角速
度）となる。また数１の式では足平角度θ０（絶対角
度）の項が式の上に表れないが、数７の式では表れてく
る。このため、関節角サーボ系で予期しない凹凸路を歩
行させると足平角度θ０がランダムに変動し、数７の式
の運動を乱す外乱となり歩行が不安定となる。よって、
歩行可能な目標姿勢、即ち、歩行パターンは、仮定３よ
り路面の形状を既知としてのみ設計することができる。
ここで、平坦路の歩行を考えると、足平角度は零である
ことから、数８に示す様になる。

【００３０】

【数８】

【００３１】即ち、目標姿勢Θｒ，Θｒドットは平坦路
の歩行パターンとなる。このとき、ロボットの運動方程
式（数７の式）は、数９で表すことができる。

【００３２】

【数９】

【００３３】一方、関節角サーボの定数Ｋｒなどを最適
な値に設定すると目標関節軌道に実際の関節軌道がほと
んど遅れなく追従するので、数１０で示す通りとなる。
即ち、数１１で示す様になる。

【００３４】

【数１０】

【００３５】

【数１１】

【００３６】その結果、数９の式より近似的に数１２の
式が成り立つ。

【００３７】

【数１２】

【００３８】ここで、しばしば述べた様に、平坦路に未
知の凹凸が存在する場合を考えると、足平角度θ０が零
ではなくて不明なため、歩行可能な歩行パターンを作成
するのが困難である。そこで路面の凹凸が十分小さいと
し（段差、階段などの大きい凹凸は前記したロボット１
に視覚手段を設ければ検知し易い）、平坦路の歩行パタ
ーンΘｒを用いると（即ち、外乱ｄ（θｒ，θ０）＝
０）、凹凸による姿勢のズレ（偏差）をΔΘとすると
き、ロボットの姿勢Θは数１３の様に表すことができ
る。

【００３９】

【数１３】

【００４０】従って、このズレを補正するために、数５
の式の関節トルクに補正量Ｕを数１４で示す式の様に導
入する。

【００４１】

【数１４】

【００４２】この数１４の式は平坦路の歩行における、
即ち（θ０ｒ，θ０＝０）の状態における関節角サーボ
系による発生トルクに補正量Ｕを加えた形になっている
ことが分かる。

【００４３】この補正量Ｕの算出について次に説明す
る。数１３，数１４の式をロボットの運動方程式である
数１の式に代入すると、数１５の式となる。即ち、平坦
路歩行パターンΘｒとズレΔΘで表した運動方程式とな
る。

【００４４】

【数１５】

【００４５】路面の凹凸が十分小さいとすると、ズレΔ
Θも十分小さくなることから、数１６の式の様に変形す
ることができる。

【００４６】

【数１６】

【００４７】ここで、ズレΔΘの２次以上の項を無視す
ると、数１７の式となる。

【００４８】

【数１７】

【００４９】関節角サーボが高速に追従するとき成立す
る数１２に示した式より、数１７の式は近似的に数１８
に示す様になる。

【００５０】

【数１８】

【００５１】さらに、Ｊ（Θｒ）はΘによる変化が小さ
いこと、Ｘ（Θｒ）は値が小さいこと、関節角サーボの
ゲインＦｐ，Ｆｖの値が大きいことから、数１９に示す
様な大小関係が成り立つ。

【００５２】

【数１９】

【００５３】よって、数１８の式は数２０の様に表すこ
とができる。

【００５４】

【数２０】

【００５５】即ち、数２０の式は歩行パターンからのズ
レΔΘに対する運動方程式を表し、安定化のための補正
量ＵはそのズレΔΘの運動を制御する操作量となってい
る。よって、ズレΔΘを零にする操作量Ｕを導き出せ
ば、凹凸のある路面であってもロボットの姿勢は平坦路
の歩行パターンに収束するため、安定して歩行すること
になる。

【００５６】次に、ズレΔΘを零にする操作量Ｕの算出
について説明する。ここで操作量Ｕを数２１の様に表し
てみる。ここで、ΔＦは遊脚に作用する外力の変動分で
あるが、これは前記した６軸力センサ３６から検出する
ことができる。

【００５７】

【数２１】

【００５８】数２１の式を数２０の式に代入すると、数
２２の如くとなる。

【００５９】

【数２２】

【００６０】従って、Ｕを数２１の式を用いて計算すれ
ば、数２２の式はズレΔΘに関する線形方程式となり、
また最適に制御値を決定することができる。ただし、外
力変動分ΔＦが十分小さく、数２２の式において外乱と
みなすことができるならば、ΔＦ≒０とし、数２１の式
の第２項を計算することなく以下の操作量Ｕθを算出す
ることによっても、ある程度の安定化は可能である。こ
れは例えば、片足支持期では遊脚に外力が作用しないた
めΔＦ≒０とできる様な場合である。

【００６１】以下、その操作量Ｕθの算出について説明
する。

【００６２】数２２の式はΔΘに関する線形方程式であ
るから、いわゆる状態方程式表現を用いて数２３の様に
表すことができる。

【００６３】

【数２３】

【００６４】ここで、最適レギュレータ理論により、数
２４の式に示す２次形式の評価関数Ｊを最小にするｕは
数２５の様に表現することができる。

【００６５】

【数２４】

【００６６】

【数２５】

【００６７】評価関数Ｊを最小にすることは、数２４の
式から明らかな様に、できるだけ少ない操作量ｕで状態
変数ｘをできるだけ小さくすることである。即ち、路面
の凹凸によるズレΔΘをできるだけ小さくすることにな
る。よって、数２５の式から補正量（操作量）ｕを計算
することにより、安定な歩行が実現できることになる。

【００６８】一方、外力変動分ΔＦやマトリックスＡ，
Ｂのモデル化誤差を外乱ｄと考えると、数２３に示した
式は、数２６の様に表すことができる。

【００６９】

【数２６】

【００７０】また、最適レギュレータ理論によれば、数
２７に示す２次形式の評価関数Ｊを最小にするｕは数２
８の様に計算することができる。

【００７１】

【数２７】

【００７２】

【数２８】

【００７３】数２８の式も、数２５の式と同様に、安定
な歩行を実現する補正量（操作量）ｕの算出手法であ
る。

【００７４】ここで、ｕを計算するためにはゲインＫ
ｘ，Ｋｉとロボットの状態量ｘとを求める必要がある
が、ゲインＫｘ，Ｋｉは、周知の如く、最適レギュレー
タ理論によりリカッチ方程式を解くことで求められる。
また状態量ｘはリンクの角度と角速度であるが、これは
ロボットの胴体部２４に取りつけた傾斜センサ４０，４
２と各関節に配置したロータリエンコーダにより求める
ことができる。よって、例えば前後方向の運動の場合、
数３の式は数２９の様に書くことができる。

【００７５】

【数２９】

【００７６】ここで、胴体部（基体）２４の傾斜角θ３
は傾斜センサ４０，４２で検出できることから、この検
出値をψとすると、関節角ｑｉを用いれば、数２９の式
より数３０の式が成り立つ。

【００７７】

【数３０】

【００７８】よって、ズレΔΘは数３１の式で表すこと
ができる。

【００７９】

【数３１】

【００８０】よって、数３１の式は、マトリックス表現
で数３２の様に表すことができる。

【００８１】

【数３２】

【００８２】従って、例えば数２５の式は数３２に示し
た変換式を用いて数３３の様に表すことができる。

【００８３】

【数３３】

【００８４】数３３の式は、操作量ｕを胴体部２４の傾
斜角と関節角のズレから算出することを示している。こ
こで、マトリックスＥの構成要素を見ると、傾斜角と傾
斜角速度の列に１の要素が多いことや、関節角のズレは
サーボ系の偏差と等価であり、サーボ系はこの偏差を小
さくする様に作用することなどを考えると、操作量ｕは
胴体部２４の傾斜角と傾斜角速度のみから簡便に算出す
る数３４の式を考えることができる。

【００８５】

【数３４】

【００８６】図７から図１１は実験データであるが、こ
れらから数３４で操作量ｕを算出した場合でも、この制
御系は安定となり、制御系の応答性を決める固有値も数
３３の式を用いる場合とほとんど変わらないことが確認
できた。即ち、図７から図９はロボットを片足支持状態
にして支持脚の足首関節角を３度前方にステップ状に変
化させたときの各リンク角の応答を示す実験データであ
り、うち図７は関節角サーボのみの場合を、図８は数３
４の式による安定化制御の場合を、図９は数３３の式に
よる安定化制御の場合を示す。また図１０と図１１は左
右方向に同様に３度傾けた場合の実験データを示してお
り、うち図１０が関節角制御のみの場合を、図１１が数
３３の式による安定化制御の場合を示す。この実験結果
から明らかな様に、数３３の式による場合と数３４の式
による場合とで応答に余り差が見られなかった。またい
ずれの式によるにせよ、この安定化制御を行うことでロ
ボットは転倒することはなかったが、関節角サーボのみ
の場合には勢い余って転倒することが確認された。

【００８７】尚、上記は数２８に示す式にも妥当し、同
式も全く同様に簡略化することができる。

【００８８】以上を前提として、図１２から図１４のフ
ロー・チャートを参照して制御値の決定を説明する。
尚、これらのフロー・チャートは、図１５に示す歩行の
状態遷移図において、右足から歩行を始めて３歩目から
定常歩行に移行し、左足で終わる場合を想定し、それら
に沿って経時的に片足支持期、コンプライアンス制御期
（遊脚着地時の）、両足支持期とに分けて記載したもの
である。

【００８９】図１２は最初の片足支持期の制御値決定フ
ロー・チャートであり、先ずＳ１０において関節角指令
値ｑｒ，ｑｒドットを読み込む。即ち、ロボット１の制
御ユニット２６中のＲＯＭ６６には図４に示す様に、歩
行パターンとして各リンク角Θｒ（先に述べた様に絶対
角）が格納されており、それが経時的に関節角度ｑｒと
角速度ｑｒドット（共に相対角度）に座標変換され、Ｓ
１０ではその変換された値（時刻ｔにおける）を、１２
個の関節のうち最初の関節について読み出す。

【００９０】次いでＳ１２に進んでその関節について実
際の角度ｑを入力し、Ｓ１４に進んで図示の式から関節
角サーボ系の操作量を算出する。これは具体的には前記
したサーボモータの速度指令値の形で算出される。次い
でＳ１６において傾斜角ψ、傾斜角速度ψドットを入力
し、Ｓ１８において実際角度ｑと目標角度ｑｒとから関
節角偏差Δｑを算出し、Ｓ２０において数３３あるいは
数３４の式のいずれかを用いて補正量ｕを算出する
（尚、数３４の式を用いるときはＳ１８はジャンプす
る）。この補正量ｕもサーボモータの速度指令値の形で
算出される。続いてＳ２２においてＳ１４で算出した値
にＳ２０で算出した値を加算し、その値をサーボアンプ
に出力する。続いて、Ｓ２４で遊脚が着地したと判断さ
れるまで、次の関節について同様に制御値を決定し、１
２の関節について制御値を決定し終わると、次の時刻ｔ
＋１について同様の作業を繰り返す。尚、遊脚の着地は
前記した着地スイッチ３８から検出する。

【００９１】尚、ここで留意されるべきことは、Ｓ２２
においてサーボアンプに出力するに際し、関節に優先順
位を設けたことである。即ち、関節の優先順位を、支持
脚の足関節１８，２０Ｒ（Ｌ）、支持足の膝関節１６Ｒ
（Ｌ）、支持足の股関節１０，１２，１４Ｒ（Ｌ）、遊
脚の股関節１０，１２，１４Ｌ（Ｒ）、遊脚の膝関節１
６Ｌ（Ｒ）、遊脚の足関節１８，２０Ｌ（Ｒ）とし、そ
の順で操作量を加算する様にした。即ち、安定した姿勢
の保持に寄与することことが大きい関節を優先する様に
したものであり、この結果、時間的に余裕がないときに
は、例えば、支持脚の足関節と膝関節についてのみ操作
量ｕを加算することになる。言うまでもなく、時間的に
余裕があるときは、この順序で全ての関節に加算する。

【００９２】図１２フロー・チャートのＳ２４で遊脚着
地と判断されるときは図１３フロー・チャートに従って
着地時のコンプライアンス制御に移行する。同図を参照
して説明すると、先ずＳ１００からＳ１０４において同
様にサーボ制御値を算出した後、Ｓ１０６に進んで前記
した６軸力センサ３６の出力から着地した遊脚に加わっ
た足首トルクを入力し、Ｓ１０８でコンプライアンス量
ｍを算出する。これはインピーダンス制御を速度分解制
御で実現した、いわゆる仮想的なコンプライアンス制御
手法を用いて行う。続いてＳ１１０においてサーボ制御
値ｖｃと仮想コンプラアンス制御値ｍと補正量ｕとを加
算した値をサーボアンプに出力し、Ｓ１１２でコンプラ
イアンス制御が終了と判断されるまで、それぞれの関節
について経時的に繰り返す。尚、この終了判断は、着地
から所定時間が経過したか否かで判断する。またＳ１１
０の補正量ｕの値は、図１２フロー・チャートのＳ２０
で算出した値を保存しておいて使用する。

【００９３】図１３フロー・チャートにおいてＳ１１２
でコンプライアンス制御終了と判断されたときは図１４
フロー・チャートのＳ２００からＳ２１４に従って両脚
支持期の制御値を決定するが、これはＳ２１０の補正量
Ｕの算出が数２１の式から決定される点を除けば、図１
２フロー・チャートと同様である。尚、ここで数２１に
示した式を用いて精緻に操作量を算出する様にしたが、
ΔＦを外乱と考えれば数３３または数３４から算出して
も良く、逆に遊脚に外力が作用するときは、図１２フロ
ー・チャートのＳ２０において数２１を含めた式の中か
ら算出しても良い。

【００９４】尚、ここで前述した評価関数Ｊの重みの決
定について、図５に示すモデルを前提として付言する
と、先に数２４に示した式の評価関数Ｊを前後方向の運
動の場合に具体的に表すと、一般的には数３５の様にな
る。

【００９５】

【数３５】

【００９６】この評価関数Ｊは姿勢のズレΔθｉと操作
量ｕｉの重み付き自乗和を表している。最適レギュレー
タ理論によれば、重みの大きい項の姿勢のズレは重みの
小さい項のそれよりも小さい値になる様にフィードバッ
クゲインが算出される。よって、重みの付けかたで違っ
たフィードバックゲインが算出され、姿勢のズレの修正
の時間応答が相違してくる。最適レギュレータ理論で
は、この重みの付けかたに関する定まった選定法がな
い。一般にはシミュレーションや実験により選定する。

【００９７】そこで、この制御においては２足歩行の特
徴を考慮して重みＱ，Ｒを選定する様にした。それに関
して階段を昇降するときの様に遊脚の着地位置を正確に
制御したい場合を例にとって説明する。

【００９８】図１６に示す様に、遊脚の目標足先位置
（ｘ，ｙ）にズレΔｘ，Δｙが生じた場合を考える。こ
れらは数３６から数３９の様に示すことができる。

【００９９】

【数３６】

【０１００】

【数３７】

【０１０１】

【数３８】

【０１０２】

【数３９】

【０１０３】図１６に示す様に、遊脚の目標足先位置
（ｘ，ｙ）からのズレΔｘ，Δｙの位置上のズレΔｐ
は、数４０の様に自乗和で表すことができる。

【０１０４】

【数４０】

【０１０５】この式から明らかな様に、目標足先位置か
らのズレΔｐ² は、各リンクの姿勢ズレΔθｉにかかる
係数に比例している。即ち、その係数が大きい順に姿勢
ズレΔθｉを小さくすれば、位置ズレΔｐ² を効率的に
小さくすることができる。一方、評価関数Ｊは重みの大
きい姿勢ズレΔθｉほど小さくすることができる。よっ
て、評価関数Ｊを数４１の式の様に選ぶことによって最
適に足先位置のズレを修正することができる。即ち、所
定の位置に正確に着地させたいときなどは、全てのリン
ク角についての重みを等しくするのではなく、足先位置
のズレに対する寄与度に応じて重みを変えることとす
る。例えば、数３５において、Δθ１の変動を小さくし
たいときは重みｑ１を大きくし、アクチュエータの最大
発生トルクを考慮し、ｕ２の発生トルクを大きくしたい
ときはｒ２を小さくすれば良い。この様に、安定化制御
とは別に、重みＱ，Ｒの大きさを最適に制御したい位置
（ここでは関節角度位置などの関節座標系での位置では
ない、作業座標系での位置）に応じて変えることによ
り、例えば足先の位置を最適（効果的）に制御すること
が可能となる。尚、この選定手法は、足先位置のみなら
ず、重心位置など他の部位の位置にも適用可能である。

【０１０６】

【数４１】

【０１０７】この実施例は上記の如く、関節角サーボ系
を備えた２足歩行の脚式移動ロボットの制御装置におい
て、予め設定した歩行パターンの目標角度と実際の角度
との偏差を絶対角度で求め、それに応じて操作量ｕを求
めてサーボ操作量に足し込む様に構成したので、歩行パ
ターンが予期しない凹凸路面を歩行するときも常に安定
した姿勢を保持することができ、また線形性を損なう様
な歩行パターンに対しても高速なコンピュータを用いて
非線形補償する必要がなく、常に滑らかな歩行を実現す
ることができる。また操作量ｕの算出に際しても、遊脚
に作用する外力の変動分に応じて精緻に決定する手法に
加えて、胴体部の傾斜角度と傾斜角速度とから簡便に算
出する様にも構成したので、その簡便に算出する手法に
よるときは簡易に算出することができる。

【０１０８】更に、先に述べた如く、図１８に示す様
に、前後方向のモデルにおいて、支持脚の絶対角をリン
ク１，２を２辺とする３角形の１辺の角度とし、同様に
遊脚の絶対角度をリンク４，５を２辺とする３角形の１
辺の角度としてモデリングする様にした。その結果、図
１７に示した手法に比較して、３リンクのままで５リン
クのモデルに十分近い精度で近似することができる。具
体的には図１７に示す手法に比較して以下の効果を得る
ことができる。尚、これらの効果は膝関節が曲がるほど
顕著となる。

【０１０９】先ず、遊脚の着地位置の誤差を小さくする
ことができる。即ち、両脚部の角度が、腰から足首を結
ぶ線の角度で定義されるために、制御誤差が零ならば、
脚部の角度誤差も零である。誤差は脚部の長さだけなの
で、着地位置はかなり正確である。図１７に示した手法
の場合には大腿リンクの延長線上に着地位置があるとし
て制御するため、図１８に示したこの手法との脚部の角
度差をΔΘとすれば、「脚部の長さ×ｓｉｎ（ΔΘ／
２）」の差が生じることになる。例えば、脚部長さを１
ｍで膝関節の角度を１０度としたとき、８ｃｍの着地位
置の誤差を抑えることができる。

【０１１０】また制御系の安定限界が上がり、制御応答
を向上させることができる。前述の如く、最適レギュレ
ータを用いて制御系の安定性を高めているが、モデリン
グ自体に誤差があると、高い周波数域でかえって発振し
やすくなる不都合がある。またシミュレーションにおい
て制御入力の大きさと制御量の応答の兼ね合いを調整し
ながらフィードバックゲインを決めるが、そのときもモ
デリング誤差が大きいと、意図する応答が得られない。
しかし、この制御においては図１８で示す様に構成した
ことでモデリング精度を上げて上記した効果を得てい
る。

【０１１１】尚、上記した実施例として関節角サーボ系
を含んだ制御装置を例にとって説明してきたが、この発
明は先に本出願人が特願平２−２５９８３９号で提案し
た関節角サーボ系を含まないものについても妥当する。

【０１１２】更には、この発明を２足歩行の脚式移動ロ
ボットについて説明したが、それに限られるものではな
く、１足ないしは３足以上の脚式移動ロボットにも妥当
するものである。

【０１１３】

【発明の効果】請求項１項にあっては、基体と、それに
連結される２本の脚部とからなり、該２本の脚部がそれ
ぞれ基体に第１の関節を介して連結されると共に、第１
の関節と脚部先端との間に第２の関節を備え、更に脚部
先端付近に第３の関節を備えてなる脚式移動ロボットの
制御装置であって、該２本の脚部をそれぞれ１個のリン
クと見做して数学モデルで近似し、該リンクの鉛直方向
に対する絶対角度を含むパラメータを用いて運動方程式
表現を行ってその解を求め、求めた解に基づいて制御値
を決定するものにおいて、前記リンクの絶対角度を、前
記第１関節と第３関節とを結ぶ線が鉛直方向に対してな
す角度で求める様に構成したので、３リンクでありなが
ら５リンクモデルに十分近い精度で近似することがで
き、遊脚の着地位置の誤差を低減することができると共
に、制御系の安定限界と制御の応答性とを向上させるこ
とができ、更に階段の様な膝関節が大きく曲がる個所の
歩行についても正確にモデル化することができる。

【図面の簡単な説明】

【図１】この発明に係る脚式移動ロボットの制御装置を
全体的に示す概略図である。

【図２】図１に示す制御ユニットの説明ブロック図であ
る。

【図３】図２に示す制御ユニットの制御のうちの関節角
サーボ系を示すブロック線図である。

【図４】図３に示す関節角サーボ系に、この発明で算出
する第２の操作量を加算する状態を示すブロック図であ
る。

【図５】図１に示すロボットの前後方向の運動を示す説
明図である。

【図６】図１に示すロボットの左右方向の運動を示す説
明図である。

【図７】図３に示す関節角サーボ系のみを備えた場合
に、ロボットの支持脚の足首関節角を前後方向にステッ
プ状に３度傾けたときの応答性を示す実験データ図であ
る。

【図８】図４に示す第２の操作量を加算した場合の図７
と同様の実験データ図である。

【図９】図８と同様のものであって、別の手法で算出し
た第２の操作量を加算した場合の実験データ図である。

【図１０】図７と同様のものであって、ロボットの支持
脚の足首関節角を左右方向にステップ状に３度傾けたと
きの応答性を示す実験データ図である。

【図１１】図１０と同様のものであって、図４に示す第
２の操作量を加算した場合の実験データ図である。

【図１２】図２の制御装置の動作を示すフロー・チャー
トであって、片足支持期の制御値決定を示すフロー・チ
ャートである。

【図１３】図１２と同様のものであって、着地時のコン
プライアンス制御期の制御値決定を示すフロー・チャー
トである。

【図１４】図１２と同様のものであって、両足支持期の
制御値決定を示すフロー・チャートである。

【図１５】図１２から図１４に示した制御時期を説明す
る歩行の状態遷移図である。

【図１６】図４に示す第２の操作量を算出するときの評
価関数の重みの決定手法を説明する説明図である。

【図１７】本出願人が先の出願で提案した３リンクモデ
ルを示す説明図である。

【図１８】この発明で用いる３リンクモデルへのモデリ
ングを示す説明図である。

【図１９】この発明で用いる３リンクモデルを示す説明
図である。

【符号の説明】

１ 脚式移動ロボット（２足歩行ロボ
ット） ２ 脚部リンク １０Ｒ，１０Ｌ 脚部回旋用の関節（軸） １２Ｒ，１２Ｌ 股部のピッチ方向の関節（軸） １４Ｒ，１４Ｌ 股部のロール方向の関節（軸） １６Ｒ，１６Ｌ 膝部のピッチ方向の関節（軸） １８Ｒ，１８Ｌ 足首部のピッチ方向の関節（軸） ２０Ｒ，２０Ｌ 足首部のロール方向の関節（軸） ２２Ｒ，２２Ｌ 足平（足部） ２４ 胴体部 ２６ 制御ユニット ３６ ６軸力センサ ４０，４２ 傾斜センサ

Claims (1)

【特許請求の範囲】
1. 【請求項１】 基体と、それに連結される２本の脚部と
   からなり、該２本の脚部がそれぞれ基体に第１の関節を
   介して連結されると共に、第１の関節と脚部先端との間
   に第２の関節を備え、更に脚部先端付近に第３の関節を
   備えてなる脚式移動ロボットの制御装置であって、該２
   本の脚部をそれぞれ１個のリンクと見做して数学モデル
   で近似し、該リンクの鉛直方向に対する絶対角度を含む
   パラメータを用いて運動方程式表現を行ってその解を求
   め、求めた解に基づいて制御値を決定するものにおい
   て、前記リンクの絶対角度を、前記第１の関節と第３の
   関節とを結ぶ線が鉛直方向に対してなす角度で求めるこ
   とを特徴とする脚式移動ロボットの制御装置。