ロボットの安定化制御装置
【課題】 台車移動型ロボットの転倒を防止しながらアクチュエータを駆動するに当たり、計算量を少なくするとともに、アクチュエータの作動を少なくすることによってマニピュレータにおける無駄なエネルギーを消費しないようにしたロボットの安定化制御装置を提供する。
【解決手段】 安定化制御装置１は、ロボット４の挙動に基づいて、ＺＭＰ算出部１１で実ＺＭＰを求める。また、ＺＭＰ比較部１３で、起立状態が不安定となるとして予め記憶したＺＭＰ限界値と実ＺＭＰとを比較する。目標角度算出部１４では、実ＺＭＰおよびＺＭＰ限界値の比較結果に基づいて、ロボット４のＺＭＰの目標変化分を設定する。
【解決手段】 安定化制御装置１は、ロボット４の挙動に基づいて、ＺＭＰ算出部１１で実ＺＭＰを求める。また、ＺＭＰ比較部１３で、起立状態が不安定となるとして予め記憶したＺＭＰ限界値と実ＺＭＰとを比較する。目標角度算出部１４では、実ＺＭＰおよびＺＭＰ限界値の比較結果に基づいて、ロボット４のＺＭＰの目標変化分を設定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、台車に搭載されたマニピュレータを有するロボットの安定化制御装置に係り、特に、ＺＭＰを安定性判別の規範として台車移動型ロボットを安定化制御するロボットの安定化制御装置に関する。
【背景技術】
【０００２】
従来、ロボットとしてはたとえば工場などに固定して設けられるもののほか、移動可能とされたロボットもある。移動可能なロボットとして、たとえば、台車に搭載されたマニピュレータを有する台車移動型ロボットや二足歩行型ロボットなどがある。移動可能なロボットでは、ロボットが移動し、または作業等することによって転倒することが懸念される。移動可能なロボットの転倒を防止するため、二足歩行型ロボットの歩行制御において、ＺＭＰ規範が知られている
ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）とは、ロボットが位置する床面上において、このロボットの各部材の重力によるモーメントと慣性力によるモーメントと（もし外力が作用していれば、さらに外力によるモーメントと）の総和がゼロとなる点のことである。ＺＭＰ規範とは、このＺＭＰがロボットの接地面が形成する支持多角形（安定領域と呼ばれる）の辺上または内部にあればロボットは転倒せずに安定に移動することが可能であるとする規範である。このようなＺＭＰ規範は、二足歩行ロボットのほか、台車移動型ロボットに対しても有効なものである。
【０００３】
このようなＺＭＰ規範を利用する技術として、特開２００１−２７７１５８号公報（特許文献１）に開示された脚式ロボットの動作制御システムがある。この動作制御システムは、まず、ロボットの動作を生成するために、事前に目標ＺＭＰ軌道を計画する。次に、ロボットの運動時に、ロボットの運動と床反力との関係を示す動力学モデルを用いて実ＺＭＰを算出する。続いて、実ＺＭＰを目標ＺＭＰに追従させるために、算出された実ＺＭＰと目標ＺＭＰとの差に基づいて、ロボットの状態を補正するというものである。
【０００４】
また、下記非特許文献１に開示された技術がある。この技術は、マニピュレータの機能を拡張するために、移動機構（ヴィークル）を設けたものに関するものであり、ヴィークル搭載型マニピュレータの安定化運動を実現するために、まず、ＺＭＰ規範を用いた安定度および有効安定領域といった安定性を図っている。その安定性維持と安定性回復の制御方策として、安定性のポテンシャル場を用いてＺＭＰ軌道を導出し、このＺＭＰ軌道に基づく安定化制御を行うというものである。
【０００５】
さらに、下記非特許文献２に開示された技術がある。この技術は、やはりヴィークルを設けたものであり、ロボットの転倒防止のために、定重心安定制御を行うものである、定重心安定制御を行うことにより、ロボットの転倒を効果的に防止している。
【特許文献１】特開２００１−２７７１５８号公報
【非特許文献１】ヴィークル搭載型マニュピレータの安定化制御−安定規範とマニュピレータによる補償運動− 黄強，菅野重樹，加藤一郎 計測自動制御学会論文集Ｖｏｌ．３１，Ｎｏ．７，８６１〜８７０ページ １９９５年７月
【非特許文献２】マン・ロボット強調作業型マニピュレータの基礎的研究「第三報、マニピュレータ／ヴィークルシステムの定重心制御」 日本機械学会論文集Ｃ、Ｖｏｌ．５８，Ｎｏ．５５１，２１５２〜２１５８ページ
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、上記特許文献１に開示された技術では、ランダムに作成された目標ＺＭＰに実ＺＭＰを追従させているため、関節を駆動するアクチュエータが常時働くことになるので、エネルギーの消耗が多い。さらには、アクチュエータを頻繁に正負方向に移動させることから、アクチュエータの寿命を短くするという問題がある。
【０００７】
また、上記非特許文献１に開示された技術では、上記特許文献１で掲げた問題点のほか、ＺＭＰの軌道を求めるために、非線形微分方程式を解くことが必要となるので、計算量が多くなり、ロボットへの実装が難しくなるという問題がある。
【０００８】
さらに、上記非特許文献２に開示された技術では、マニピュレータ／ヴィークルシステムの重心を一定にするため、ヴィークルの移動加速度の増減に伴って、マニピュレータの姿勢を常時調整することが必要となる。したがって、アクチュエータが常時働くことになるので、エネルギー消費が大きくなってしまうという問題もあった。
【０００９】
そこで、本発明の課題は、ロボットの転倒を防止しながらアクチュエータを駆動するに当たり、計算量を少なくするとともに、アクチュエータの作動を少なくすることによってマニピュレータにおける無駄なエネルギーを消費しないようにしたロボットの安定化制御装置を提供することにある。
【課題を解決するための手段】
【００１０】
上記課題を解決した本発明に係るロボットの安定化制御装置は、台車に搭載されたマニピュレータを有する台車移動型ロボットの安定化制御を行うロボットの安定化制御装置であって、台車移動型ロボットの挙動に基づいて求められる実ＺＭＰと台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値とを比較し、実ＺＭＰおよびＺＭＰ限界値に基づいて、台車移動型ロボットのＺＭＰの目標変化分を設定するものである。
【００１１】
本発明に係る安定化制御装置においては、台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値と実ＺＭＰとを比較して、ＺＭＰの目標変化分を設定している。仮に、実ＺＭＰが安定領域内にあると、ＺＭＰの目標変化分を０に設定している。このため、アクチュエータの作動を少なくするとともに、マニピュレータにおける無駄なエネルギー消費を防止することができる。また、非線形微分方程式を解く必要もないので、計算量が増大することもないようにすることができる。さらに、アクチュエータを頻繁に正負方向に動作させることもないので、アクチュエータの寿命の低下を防止することができる。
【００１２】
ここで、台車は、３点以上の複数の接地点を有しており、複数の接地点で囲まれた領域の境界線に対応してＺＭＰ限界値が設定される態様とすることができる。
【００１３】
このように、３点以上の複数の接地点で囲まれた領域の境界線に対応してＺＭＰ限界値が設定されることにより、台車移動型ロボットが安定となる状態を確実に維持することができる。
【００１４】
さらに、台車移動型ロボットにおける台車の移動速度が０である場合に、ＺＭＰ限界値を０に設定する態様とすることができる。
【００１５】
このように、台車移動速度が０である場合にＺＭＰ限界値を０に設定することにより、台車移動型ロボットの最終姿勢を直立状態とすることができる。
【００１６】
また、マニピュレータは、複数のリンクと、複数のリンクを接続する関節とを有しており、台車移動型ロボットにおけるＺＭＰの変化分とマニピュレータにおける関節角度の変化分とを関係づけるＺＭＰヤコビアン行列を用いて、ＺＭＰから関節における関節角度を算出する態様とすることもできる。
【００１７】
このように、ＺＭＰに対応するＺＭＰヤコビアン行列を用いて、ＺＭＰから関節における関節角度を算出することにより、関節角度を容易に算出することができる。
【発明の効果】
【００１８】
本発明に係るロボットの安定化制御装置によれば、ロボットの転倒を防止しながらアクチュエータを駆動するに当たり、計算量を少なくするとともに、アクチュエータの作動を少なくすることによってマニピュレータおける無駄なエネルギーを消費しないようにすることができる。
【発明を実施するための最良の形態】
【００１９】
以下、図面を参照して本発明の実施形態について説明する。以下の各実施形態において、同一の機能を有するものについては同一の番号を付し、重複する説明は省略することがある。図１は、本発明の第一の実施形態に係るロボットの安定化制御装置のブロック構成図である。
【００２０】
図１に示すように、本実施形態に係る安定化制御装置１は、実ＺＭＰ算出部１１、ＺＭＰ限界値設定部１２、ＺＭＰ比較部１３、および目標角速度算出部１４を備えている。また、安定化制御装置１には、角度センサ２および加速度センサ３が接続されており、さらに、ロボット４におけるマニピュレータ５が接続されている。この安定化制御装置１によって、ロボット４におけるマニピュレータ５の関節角度を調節する。
【００２１】
安定化制御装置１によって制御されるロボット４について、図２を参照して説明する。図２に示すように、ロボット４は、台車６を有し、台車６の上にマニピュレータ５が設けられている、いわゆる台車移動型ロボットである。台車６には、４つの車輪６Ａが取り付けられており、この車輪６Ａが回転することによって、台車６が走行移動する。
【００２２】
また、マニピュレータ５は、第一リンク５Ａ、第二リンク５Ｂ、およびハンド部材５Ｃを備えている。第一リンク５Ａにおける一端部は、台車６に対して回転可能に取り付けられており、第一リンク５Ａの他端部には、第二リンク５Ｂの一端部が回転可能に取り付けられている。また、第二リンク５Ｂの他端部には、ハンド部材５Ｃが固定されている。
【００２３】
図１に示す角度センサ２は、ロボット４におけるマニピュレータ５に取り付けられており、台車６と第一リンク５Ａとがなす第一関節角度θ１および第一リンク５Ａと第二リンク５Ｂとがなす第二関節角度θ２を検出している。角度センサ２は、検出した第一関節角度θ１および第二関節角度θ２を安定化制御装置１における実ＺＭＰ算出部１１に出力している。
【００２４】
また、加速度センサ３は、たとえばロボット４における台車６に取り付けられたエンコーダなどからなり、台車６の走行加速度を検出している。この加速度センサ３は、検出した台車６の加速度を安定化制御装置１における実ＺＭＰ算出部１１に出力している。
【００２５】
安定化制御装置１における実ＺＭＰ算出部１１は、角度センサ２から出力されたマニピュレータ５における関節角度θ１，θ２、加速度センサ３から出力された台車６の加速度情報に基づいて、実ＺＭＰを算出する。また、実ＺＭＰ算出部１１は、算出した実ＺＭＰをＺＭＰ比較部１３に出力する。
【００２６】
ＺＭＰ限界値設定部１２は、予め記憶している台車６の寸法や環境外乱に基づいてＺＭＰ限界値を設定する。ＺＭＰ限界値設定部１２は、設定したＺＭＰ限界値をＺＭＰ比較部１３に出力する。なお、ＺＭＰ限界値を設定するにあたり、図示しないカメラなどを用いてロボット４の周囲を撮像し、撮像した画像を画像処理して得られる走路の傾斜などの周辺情報を加味して、ＺＭＰ限界値を設定することもできる。
【００２７】
ＺＭＰ比較部１３は、実ＺＭＰ算出部１１から出力された実ＺＭＰとＺＭＰ限界値設定部１２から出力されたＺＭＰ限界値とを比較し、ＺＭＰが安定領域にあるか否かを判断し、その判断結果に基づいてＺＭＰの変化分（目標変化分）Δｘｚｍｐを算出する。ＺＭＰ比較部１３は、算出したＺＭＰの変化分Δｘｚｍｐを目標角速度算出部１４に出力する。
【００２８】
目標角速度算出部１４は、ＺＭＰ比較部１３から出力されたＺＭＰの変化分Δｘｚｍｐに基づいて、マニピュレータ５の関節角度θ１，θ２を算出する。目標角速度算出部１４は、算出した関節角度θ１，θ２をマニピュレータ５に出力し、マニピュレータ５の関節角度を調整する。
【００２９】
以上の構成を有する本実施形態に係る安定化制御装置における制御手順について説明する。図３は、本実施形態に係る安定化制御の手順を示すフローチャート、図４は、ロボットの安定化制御系のブロック線図である。ここでは、ロボット４がｘ方向に沿って走行し、ロボット４がｘ方向に転倒するのを防止する例について説明する。
【００３０】
安定化制御を開始すると、最初に、角度センサ２によってロボット４から検出され、角度センサ２から出力された関節角度θ１，θ２を入力し（Ｓ１）、加速度センサ３によってロボット４から検出され、加速度センサ３から出力された台車６の加速度を入力する（Ｓ２）。
【００３１】
続いて、ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出する（Ｓ３）。ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出するにあたり、ＺＭＰについて説明すると、ＺＭＰは、床面においてロボット４の各部の重力によるモーメント、慣性力によるモーメント、および外力によるモーメントの総和が０となる点である。いま、図５に示すように、重力、慣性力、および外力の合力が下向きであり、かつ台車の各車輪６Ａの接地点からなり３点以上の複数である４点の接地点が形成する支持多角形９Ａ（この支持多角形内の領域を「安定領域」という）内にＺＭＰが存在すれば、ロボットは転倒することなく安定して運動することができる。一方、ＺＭＰが安定領域内に存在しなければ、ロボット４は不安定となり、理論上転倒することになる。
【００３２】
そこで、マニピュレータ５を動かしたとしてもＺＭＰが安定領域内に存在することになるようにするため、関節角度θ１，θ２および台車加速度αを用いてＺＭＰの変化分Δｘｚｍｐを求めて、その変化分Δｘｚｍｐに基づいて、目標関節角速度を算出する。この目標関節角速度から関節角度が定められる。ＺＭＰの変化分Δｘｚｍｐの算出は、図６に示すフローに従って行われる。図６は、ＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである。
【００３３】
図６に示すように、ＺＭＰの変化分Δｘｚｍｐを算出する際には、まず、ＺＭＰの限界値ｘｚｍｐ＿ｍを設定する（Ｓ１１）。ＺＭＰの限界値ｘｚｍｐ＿ｍは、ＺＭＰ限界値設定部１２に記憶されているＺＭＰ限界値を読み出すことによって設定される。次に、実ＺＭＰの位置ｘｚｍｐを算出する（Ｓ１２）。実ＺＭＰの位置ｘｚｍｐは、実ＺＭＰ算出部１１において、角度センサ２から出力された関節角度θ１，θ２および加速度センサ３から出力された台車６の加速度αに基づいて算出する。
【００３４】
実ＺＭＰを算出するにあたり、ｘ方向における関節角度θ１，θ２、台車加速度α、およびロボット４のＺＭＰの位置ｘｚｍｐの関係は、下記（１）式で表される。
【００３５】
【数１】
上記（１）式にステップＳ１で入力された関節角度θ１，θ２およびステップＳ２で入力された台車の加速度αを代入することにより、上記（１）式を変形した下記（２）式に基づいて実ＺＭＰの位置ｘｚｍｐの位置を算出することができる。
【００３６】
【数２】
ロボット４には、台車６の加速度の増減によって生じる慣性力が作用しているため、台車加速度の変化に伴い、ＺＭＰの位置が変動している。ここで、ＺＭＰが安定領域内に存在すれば、ロボット４は転倒しないので、安定化のための補正等を行う必要はない。一方、ＺＭＰが安定領域を外れると、ロボットは不安定となり、安定化のための補正を行う必要が生じる。
【００３７】
そこで、実ＺＭＰの位置ｘｚｍｐを算出したら、実ＺＭＰの位置ｘｚｍｐが安定領域内にあるか否かの判断を行う。そのため、まず、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっているか否かを判断する（Ｓ１３）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となってないと判断した場合には、実ＺＭＰの位置ｘｚｍｐが図５に示す安定領域９Ａから外れていると考えられる。このままマニピュレータ５を駆動すると、理論上は実ＺＭＰの位置ｘｚｍｐが安定領域から外れてロボット４が転倒することになる。
【００３８】
そこで、この場合には、ＺＭＰの変化分Δｘｚｍｐを下記（３）式によって求める（Ｓ１４）。
【００３９】
Δｘｚｍｐ＝ｘｚｍｐ＿ｍ−ｘｚｍｐ ・・・（３）
この（３）式からΔｘｚｍｐを求めることにより、マニピュレータ５を動かした後であっても、ロボット４のＺＭＰは安定領域内に位置することになるため、ロボット４の転倒を防止することができる。
【００４０】
一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっているか否かを判断する（Ｓ１５）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以下となってないと判断した場合には、実ＺＭＰの位置ｘｚｍｐが図５に示す安定領域９Ａから外れていると考えられる。このままマニピュレータ５を駆動すると、理論上は実ＺＭＰの位置ｘｚｍｐが安定領域から外れてロボット４が転倒することになる。
【００４１】
そこで、この場合には、ＺＭＰの変化分Δｘｚｍｐを下記（４）式によって求める（Ｓ１６）。
【００４２】
Δｘｚｍｐ＝−ｘｚｍｐ−ｘｚｍｐ＿ｍ ・・・（４）
この（４）式からΔｘｚｍｐを求めることにより、マニピュレータ５を動かした後であっても、ロボット４のＺＭＰは安定領域内に位置することになるため、ロボット４の転倒を防止することができる。
【００４３】
一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐが安定領域９Ａ内にあると考えられる。この場合には、ロボット４は安定しており、転倒の危険性が少なくなっているので、Δｘｚｍｐ＝０に設定する（Ｓ１７）。このようにして、Δｘｚｍｐを設定する。
【００４４】
このように、実ＺＭＰの位置ｘｚｍｐとＺＭＰ限界値ｘｚｍｐ＿ｍとを比較し、その比較結果に基づいてＺＭＰの変化分Δｘｚｍｐを求めることにより、ＺＭＰが安定領域内に存在することになるようにすることができる。したがって、ロボットの転倒を防止することができる。
【００４５】
こうしてＺＭＰの変化分Δｘｚｍｐを算出したら、図３に示すフローに戻り、目標関節角を算出する（Ｓ４）。目標関節角速度を算出する際には、ステップＳ３で求めたΔｘｚｍｐに調節用の係数Ｋを乗じる。それから、係数Ｋを乗じたＺＭＰの変化分Δｘｚｍｐと、関節角度の変化分を関係づけるＺＭＰヤコビアン行列（ヤコビ行列）を用いて目標関節角速度を算出する。本実施形態では、ロボットの制御系として速度制御モードを採用しており、ＺＭＰの速度ｖｘｚｍｐと関節角速度ｖθとの関係は、下記（５）式より求められる。
【００４６】
ｖｘｚｍｐ＝Ｊｚｍｐｖθ ・・・（５）
ここで、ＪｚｍｐはＺＭＰヤコビアン行列、ｖθは［ｖθ１，ｖθ２］Ｔである。
【００４７】
また、Ｊｚｍｐの擬似逆行列をＪ＋ｚｍｐとすると、慣性座標系から見た関節角速度ｖθは、下記（６）式によって算出することができる。
【００４８】
ｖθ＝Ｊ＋ｚｍｐ・ｖｘｚｍｐ ・・・（６）
この（６）式を用いて、ロボット４におけるマニピュレータ５の目標関節角速度ｖθｒｅｆを算出する（Ｓ４）。この目標関節角速度ｖθｒｅｆをロボット４に出力して（Ｓ５）、ロボット４におけるマニピュレータ５における各関節の角速度の制御を行う。この制御では、現在のマニピュレータ５における各関節の角速度ｖθをフィードバックし、目標関節角速度ｖθｒｅｆを加算するフィードバック制御を行う。
【００４９】
このように、本実施形態に係る安定化制御装置では、マニピュレータ５を駆動した後のＺＭＰの位置ｘｚｍｐが安定領域にある場合には、ＺＭＰの変化分Δｘｚｍｐを変えず、ＺＭＰの位置ｘｚｍｐが安定領域から外れる場合にのみＺＭＰの変化分Δｘｚｍｐを調整するようにしている。このため、実ＺＭＰを目標ＺＭＰに追従させる場合と比較すると、その計算量を削減することができる。また、マニピュレータ５を頻繁に動作させることを回避することができるので、エネルギー消費の少ない安定化制御を行うことができる。特に、台車移動加速度が不安定となる限界値の上下にＺＭＰが脈動する場合にマニピュレータ５の頻繁な動作を防止することができる。さらに、マニピュレータ５を頻繁に正負に動作させることを回避することができるので、アクチュエータの寿命の低下を防止することができる。
【００５０】
続いて、本実施形態に係る安定化制御装置を用いたロボットの制御について行ったシミュレーションについて説明する。図２に示すロボット４を用いて、ロボット４を走行させながらマニピュレータ５を作動させるシミュレーションを行った。このシミュレーションでは、ＺＭＰの限界値ｘｚｍｐ＿ｍを６０．０ｍｍに設定した。そのシミュレーションの結果を図７に示す。
【００５１】
図７（ａ）に示すように、台車６の加速度を経時変化させた場合において、図７（ｂ）に、ＺＭＰの位置ｘｚｍｐの経時変化、図７（ｃ）に第一関節角度θ１の経時変化、図７（ｄ）に第二関節角度θ２の経時変化をそれぞれ示す。このうち、図７（ａ）には、台車が不安定となる加速度の限界値（上限値および下限値）を折れ線Ｌ１，Ｌ２で示している。また、図７（ｂ）には、折れ線Ｌ３で示す本実施形態の制御による経時変化と、折れ線Ｌ４で示す従来の制御による経時変化の例を示している。
【００５２】
図７（ｂ）から分かるように、従来の制御では、台車６の加速度の変化により、ＺＭＰの位置ｘｚｍｐは、ロボット４が不安定となるＺＭＰの限界値を超えることがあり、この場合には、台車６が転倒する恐れが高くなるものであった。これに対して、本実施形態に係る安定化制御を行った場合には、台車６の加速度が変化した場合でも、ＺＭＰの位置ｘｚｍｐは、ロボット４が不安定となるＺＭＰの位置の限界値を超えることはなく、ロボット４の転倒を効果的に防止することができる。
【００５３】
また、台車６の加速度が不安定となる限界値の上下に脈動する場合であっても、図７（ｃ）、（ｄ）に示すように、マニピュレータ５を頻繁に正負方向に動作させることもないようにすることができる。したがって、マニピュレータ５の作動によるエネルギーの消費を低減することができ、アクチュエータの寿命の低下を防止することもできる。
【００５４】
次に、本発明の第二の実施形態について説明する。本実施形態に係るロボットの安定化制御は、上記第一の実施形態と同様のロボット４を対象として行われる。また、上記の第一の実施形態と比較して、ＺＭＰ限界値設定部１２に記憶されたＺＭＰ限界値が一部異なっている点で主に異なる。
【００５５】
本実施形態に係るロボットの安定化制御装置は、ＺＭＰ限界値設定部１２には、上記第一の実施形態と同様のＺＭＰ限界値のほか、台車６の速度が「０」の場合のＺＭＰ限界値として、ＺＭＰ限界値「０」が記憶されている。
【００５６】
次に、本実施形態に係る安定化制御の手順を説明するが、上記第一の実施形態と比較して、ＺＭＰの変化分Δｘｚｍｐの算出が主に異なるので、Δｘｚｍｐの算出手順について主に説明する。図８は、ロボットの安定化制御系のブロック線図、図９は、Δｘｚｍｐを算出する手順を示すフローチャートである。
【００５７】
安定化制御を開始すると、上記第一の実施形態と同様、角度センサ２から出力された関節角度θ１，θ２を入力し、加速度センサ３から出力された台車６の加速度を入力する。その後に、ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出する。
【００５８】
たとえば、台車６の上に胴体が設けられた人間型ロボットでは、胴体が直立した状態を保持するときの作業性が最も良好なものとなる。そこで、ロボットが直立状態を保持するときにＺＭＰの初期位置が０であると仮定すると、台車６が静止状態を保持する（台車移動速度＝０である）とき、ロボット４の姿勢を直立状態に回復するために、ＺＭＰを０にすることが必要となる。
【００５９】
したがって、台車６が移動している間、台車６、移動速度を計測する。そして、台車移動速度が０ではないとき、上記第一の実施形態と同様にしてＺＭＰの変化分Δｘｚｍｐを算出する。また、台車移動速度が０であるときには、ＺＭＰの変化分Δｘｚｍｐを０に設定する。
【００６０】
このため、Δｘｚｍｐを設定するにあたり、図９に示すように、上記第一の実施形態と同様、まずＺＭＰの限界値ｘｚｍｐ＿ｍを設定し（Ｓ２１）、次に、上記第一の実施形態と同様にして、実ＺＭＰの位置ｘｚｍｐを算出する（Ｓ２２）。
【００６１】
ＺＭＰの限界値ｘｚｍｐ＿ｍを設定し、実ＺＭＰの位置ｘｚｍｐを算出したら、続いて、台車の移動速度を計測する（Ｓ２３）。台車６の移動速度の計測は、速度センサを用いて直接行うほか、台車６の加速度を積分することによって行うこともできる。こうして、台車６の移動速度を計測したら、台車移動速度が０であるか否かを判断する（Ｓ２４）。その結果、台車の移動速度が０である、換言すれば、台車が停止した状態にあるときには、ＺＭＰの限界値ｘｚｍｐ＿ｍを０に再設定する（Ｓ２５）。一方、台車移動速度が０でないと判断した場合には、設定されたＺＭＰの限界値ｘｚｍｐ＿ｍをそのまま用いる。
【００６２】
その後は、上記第一の実施形態と同様、実ＺＭＰの位置ｘｚｍｐが安定領域内にあるか否かの判断を行うために、まず、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっているか否かを判断する（Ｓ２６）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となってないと判断した場合には、上記（３）式からΔｘｚｍｐを求める（Ｓ２７）。
【００６３】
また、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっているか否かを判断する（Ｓ２８）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となってないと判断した場合には、上記（４）式からΔｘｚｍｐを求める（Ｓ２７）。一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、Δｘｚｍｐ＝０に設定する。
【００６４】
このように、実ＺＭＰの位置ｘｚｍｐとＺＭＰ限界値ｘｚｍｐ＿ｍとを比較し、その比較結果に基づいてＺＭＰの変化分Δｘｚｍｐを求めることにより、ＺＭＰが安定領域内に存在することになるようにすることができる。したがって、ロボットの転倒を防止することができる。
【００６５】
また、台車６の移動速度が０である場合には、ＺＭＰ限界値ｘｚｍｐ＿ｍを０に設定しているので、人間型ロボットのように、台車の上に胴体が設けられたロボットでは、台車６が停止しているときに胴体が起立状態を保持しようとする。したがって、ロボットの作業性を良好なものとすることができる。
【００６６】
続いて、本実施形態に係る安定化制御装置を用いたロボットの制御について行ったシミュレーションについて説明する。このシミュレーションでは、ＺＭＰの限界値ｘｚｍｐ＿ｍを６０．０ｍｍに設定した。そのシミュレーションの結果を図１０に示す。
【００６７】
図１０（ａ）に曲線Ｃ１で示すように、台車６の速度を経時変化させた場合において、図１０（ｂ）に、ＺＭＰの位置ｘｚｍｐの経時変化、図１０（ｃ）に第一関節角度θ１の経時変化、図１０（ｄ）に第二関節角度θ２の経時変化をそれぞれ示す。このうち、図１０（ａ）には、台車の速度のほか、台車の加速度を折れ線Ｌ１０で示している。また、図１０（ｂ）には、折れ線Ｌ１１で示す本実施形態の制御による経時変化と、折れ線Ｌ１２で示す上記第一の実施形態の制御による経時変化と、折れ線Ｌ１３で示す従来の制御による経時変化の例を示している。
【００６８】
さらに、図１０（ｃ）には、折れ線Ｌ１４で示す本実施形態の制御による経時変化と、折れ線Ｌ１５で示す従来の制御による経時変化の例を示し、図１０（ｄ）には、折れ線Ｌ１６で示す本実施形態の制御による経時変化と、折れ線Ｌ１７で示す従来の制御による経時変化の例を示している。
【００６９】
図１０から（ｂ）から分かるように、従来の制御では、台車の加速度の変化により、ＺＭＰの位置ｘｚｍｐは、ロボットが不安定となるＺＭＰの限界値を超えることがあり、この場合には、台車が転倒する恐れが高くなるものであった。これに対して、本実施形態に係る安定化制御を行った場合には、台車の加速度が変化した場合でも、ＺＭＰの位置ｘｚｍｐは、ロボットが不安定となるＺＭＰの位置の限界値を超えることはなく、ロボットの転倒を効果的に防止することができる。
【００７０】
また、台車の加速度が不安定となる限界値の上下に脈動する場合であっても、図１０（ｃ）、（ｄ）に示すように、マニピュレータを頻繁に正負方向に動作させることもないようにすることができる。したがって、マニピュレータの作動によるエネルギーの消費を低減することができ、アクチュエータの寿命の低下を防止することもできる。
【００７１】
さらに、図１０（ｂ）〜（ｄ）から分かるように、本実施形態に係る安定化制御では、上記第一の実施形態と比較して、台車の移動速度が０となった後は、ロボットの最終姿勢を早期に直立状態に回復することができる。したがって、良好な作業性を維持することができる。
【００７２】
以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。たとえば、上記実施形態では４つの車輪が接地する点で囲まれる部分を安定領域として設定したが、３つ以上の接地点を囲む領域であれば、安定領域とすることができる。また、接地する点で囲まれる範囲を若干増減させる形で安定領域を設定することもできる。
【００７３】
さらに、上記実施形態では、ｘ方向についてのみ安定化制御を行っているが、ｙ方向、あるいはｘ−ｙ平面上で安定化制御を行う態様とすることもできる。他方、上記実施形態では、ヤコビアン行列を用いて関節角度を算出するようにしているが、他の方法によって関節角度を算出する態様とすることもできる。さらに、上記実施形態では、関節が２つであるマニピュレータを有するロボットを制御対象としているが、関節が１つまたは３つ以上であるマニピュレータを有するロボットを制御対象とすることもできる。
【図面の簡単な説明】
【００７４】
【図１】第一の実施形態に係るロボットの安定化制御装置のブロック構成図である。
【図２】ロボットの概略を示す側面図である。
【図３】本実施形態に係る安定化制御の手順を示すフローチャートである。
【図４】ロボットの安定化制御系のブロック線図である。
【図５】安定領域を説明する概念図である。
【図６】ＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである。
【図７】（ａ）は台車の経時変化を示すグラフ、（ｂ）はＺＭＰの経時変化を示すグラフ、（ｃ）は第一関節の関節角度の経時変化を示すグラフ、（ｄ）は第二関節の関節角度の経時変化を示すグラフである。
【図８】第二の実施形態に係るロボットの安定化制御系のブロック線図である。
【図９】第二の実施形態におけるＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである
【図１０】第二の実施形態に係るものであり、（ａ）は台車の経時変化を示すグラフ、（ｂ）はＺＭＰの経時変化を示すグラフ、（ｃ）は第一関節の関節角度の経時変化を示すグラフ、（ｄ）は第二関節の関節角度の経時変化を示すグラフである。
【符号の説明】
【００７５】
１…安定化制御装置、２…角度センサ、３…加速度センサ、４…ロボット、５…マニピュレータ、５Ａ…第一リンク、５Ｂ…第二リンク、５Ｃ…ハンド部材、６…台車、６Ａ…車輪、１１…ＺＭＰ算出部、１２…限界値設定部、１３…ＺＭＰ比較部、１４…目標角速度算出部。
【技術分野】
【０００１】
本発明は、台車に搭載されたマニピュレータを有するロボットの安定化制御装置に係り、特に、ＺＭＰを安定性判別の規範として台車移動型ロボットを安定化制御するロボットの安定化制御装置に関する。
【背景技術】
【０００２】
従来、ロボットとしてはたとえば工場などに固定して設けられるもののほか、移動可能とされたロボットもある。移動可能なロボットとして、たとえば、台車に搭載されたマニピュレータを有する台車移動型ロボットや二足歩行型ロボットなどがある。移動可能なロボットでは、ロボットが移動し、または作業等することによって転倒することが懸念される。移動可能なロボットの転倒を防止するため、二足歩行型ロボットの歩行制御において、ＺＭＰ規範が知られている
ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）とは、ロボットが位置する床面上において、このロボットの各部材の重力によるモーメントと慣性力によるモーメントと（もし外力が作用していれば、さらに外力によるモーメントと）の総和がゼロとなる点のことである。ＺＭＰ規範とは、このＺＭＰがロボットの接地面が形成する支持多角形（安定領域と呼ばれる）の辺上または内部にあればロボットは転倒せずに安定に移動することが可能であるとする規範である。このようなＺＭＰ規範は、二足歩行ロボットのほか、台車移動型ロボットに対しても有効なものである。
【０００３】
このようなＺＭＰ規範を利用する技術として、特開２００１−２７７１５８号公報（特許文献１）に開示された脚式ロボットの動作制御システムがある。この動作制御システムは、まず、ロボットの動作を生成するために、事前に目標ＺＭＰ軌道を計画する。次に、ロボットの運動時に、ロボットの運動と床反力との関係を示す動力学モデルを用いて実ＺＭＰを算出する。続いて、実ＺＭＰを目標ＺＭＰに追従させるために、算出された実ＺＭＰと目標ＺＭＰとの差に基づいて、ロボットの状態を補正するというものである。
【０００４】
また、下記非特許文献１に開示された技術がある。この技術は、マニピュレータの機能を拡張するために、移動機構（ヴィークル）を設けたものに関するものであり、ヴィークル搭載型マニピュレータの安定化運動を実現するために、まず、ＺＭＰ規範を用いた安定度および有効安定領域といった安定性を図っている。その安定性維持と安定性回復の制御方策として、安定性のポテンシャル場を用いてＺＭＰ軌道を導出し、このＺＭＰ軌道に基づく安定化制御を行うというものである。
【０００５】
さらに、下記非特許文献２に開示された技術がある。この技術は、やはりヴィークルを設けたものであり、ロボットの転倒防止のために、定重心安定制御を行うものである、定重心安定制御を行うことにより、ロボットの転倒を効果的に防止している。
【特許文献１】特開２００１−２７７１５８号公報
【非特許文献１】ヴィークル搭載型マニュピレータの安定化制御−安定規範とマニュピレータによる補償運動− 黄強，菅野重樹，加藤一郎 計測自動制御学会論文集Ｖｏｌ．３１，Ｎｏ．７，８６１〜８７０ページ １９９５年７月
【非特許文献２】マン・ロボット強調作業型マニピュレータの基礎的研究「第三報、マニピュレータ／ヴィークルシステムの定重心制御」 日本機械学会論文集Ｃ、Ｖｏｌ．５８，Ｎｏ．５５１，２１５２〜２１５８ページ
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、上記特許文献１に開示された技術では、ランダムに作成された目標ＺＭＰに実ＺＭＰを追従させているため、関節を駆動するアクチュエータが常時働くことになるので、エネルギーの消耗が多い。さらには、アクチュエータを頻繁に正負方向に移動させることから、アクチュエータの寿命を短くするという問題がある。
【０００７】
また、上記非特許文献１に開示された技術では、上記特許文献１で掲げた問題点のほか、ＺＭＰの軌道を求めるために、非線形微分方程式を解くことが必要となるので、計算量が多くなり、ロボットへの実装が難しくなるという問題がある。
【０００８】
さらに、上記非特許文献２に開示された技術では、マニピュレータ／ヴィークルシステムの重心を一定にするため、ヴィークルの移動加速度の増減に伴って、マニピュレータの姿勢を常時調整することが必要となる。したがって、アクチュエータが常時働くことになるので、エネルギー消費が大きくなってしまうという問題もあった。
【０００９】
そこで、本発明の課題は、ロボットの転倒を防止しながらアクチュエータを駆動するに当たり、計算量を少なくするとともに、アクチュエータの作動を少なくすることによってマニピュレータにおける無駄なエネルギーを消費しないようにしたロボットの安定化制御装置を提供することにある。
【課題を解決するための手段】
【００１０】
上記課題を解決した本発明に係るロボットの安定化制御装置は、台車に搭載されたマニピュレータを有する台車移動型ロボットの安定化制御を行うロボットの安定化制御装置であって、台車移動型ロボットの挙動に基づいて求められる実ＺＭＰと台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値とを比較し、実ＺＭＰおよびＺＭＰ限界値に基づいて、台車移動型ロボットのＺＭＰの目標変化分を設定するものである。
【００１１】
本発明に係る安定化制御装置においては、台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値と実ＺＭＰとを比較して、ＺＭＰの目標変化分を設定している。仮に、実ＺＭＰが安定領域内にあると、ＺＭＰの目標変化分を０に設定している。このため、アクチュエータの作動を少なくするとともに、マニピュレータにおける無駄なエネルギー消費を防止することができる。また、非線形微分方程式を解く必要もないので、計算量が増大することもないようにすることができる。さらに、アクチュエータを頻繁に正負方向に動作させることもないので、アクチュエータの寿命の低下を防止することができる。
【００１２】
ここで、台車は、３点以上の複数の接地点を有しており、複数の接地点で囲まれた領域の境界線に対応してＺＭＰ限界値が設定される態様とすることができる。
【００１３】
このように、３点以上の複数の接地点で囲まれた領域の境界線に対応してＺＭＰ限界値が設定されることにより、台車移動型ロボットが安定となる状態を確実に維持することができる。
【００１４】
さらに、台車移動型ロボットにおける台車の移動速度が０である場合に、ＺＭＰ限界値を０に設定する態様とすることができる。
【００１５】
このように、台車移動速度が０である場合にＺＭＰ限界値を０に設定することにより、台車移動型ロボットの最終姿勢を直立状態とすることができる。
【００１６】
また、マニピュレータは、複数のリンクと、複数のリンクを接続する関節とを有しており、台車移動型ロボットにおけるＺＭＰの変化分とマニピュレータにおける関節角度の変化分とを関係づけるＺＭＰヤコビアン行列を用いて、ＺＭＰから関節における関節角度を算出する態様とすることもできる。
【００１７】
このように、ＺＭＰに対応するＺＭＰヤコビアン行列を用いて、ＺＭＰから関節における関節角度を算出することにより、関節角度を容易に算出することができる。
【発明の効果】
【００１８】
本発明に係るロボットの安定化制御装置によれば、ロボットの転倒を防止しながらアクチュエータを駆動するに当たり、計算量を少なくするとともに、アクチュエータの作動を少なくすることによってマニピュレータおける無駄なエネルギーを消費しないようにすることができる。
【発明を実施するための最良の形態】
【００１９】
以下、図面を参照して本発明の実施形態について説明する。以下の各実施形態において、同一の機能を有するものについては同一の番号を付し、重複する説明は省略することがある。図１は、本発明の第一の実施形態に係るロボットの安定化制御装置のブロック構成図である。
【００２０】
図１に示すように、本実施形態に係る安定化制御装置１は、実ＺＭＰ算出部１１、ＺＭＰ限界値設定部１２、ＺＭＰ比較部１３、および目標角速度算出部１４を備えている。また、安定化制御装置１には、角度センサ２および加速度センサ３が接続されており、さらに、ロボット４におけるマニピュレータ５が接続されている。この安定化制御装置１によって、ロボット４におけるマニピュレータ５の関節角度を調節する。
【００２１】
安定化制御装置１によって制御されるロボット４について、図２を参照して説明する。図２に示すように、ロボット４は、台車６を有し、台車６の上にマニピュレータ５が設けられている、いわゆる台車移動型ロボットである。台車６には、４つの車輪６Ａが取り付けられており、この車輪６Ａが回転することによって、台車６が走行移動する。
【００２２】
また、マニピュレータ５は、第一リンク５Ａ、第二リンク５Ｂ、およびハンド部材５Ｃを備えている。第一リンク５Ａにおける一端部は、台車６に対して回転可能に取り付けられており、第一リンク５Ａの他端部には、第二リンク５Ｂの一端部が回転可能に取り付けられている。また、第二リンク５Ｂの他端部には、ハンド部材５Ｃが固定されている。
【００２３】
図１に示す角度センサ２は、ロボット４におけるマニピュレータ５に取り付けられており、台車６と第一リンク５Ａとがなす第一関節角度θ１および第一リンク５Ａと第二リンク５Ｂとがなす第二関節角度θ２を検出している。角度センサ２は、検出した第一関節角度θ１および第二関節角度θ２を安定化制御装置１における実ＺＭＰ算出部１１に出力している。
【００２４】
また、加速度センサ３は、たとえばロボット４における台車６に取り付けられたエンコーダなどからなり、台車６の走行加速度を検出している。この加速度センサ３は、検出した台車６の加速度を安定化制御装置１における実ＺＭＰ算出部１１に出力している。
【００２５】
安定化制御装置１における実ＺＭＰ算出部１１は、角度センサ２から出力されたマニピュレータ５における関節角度θ１，θ２、加速度センサ３から出力された台車６の加速度情報に基づいて、実ＺＭＰを算出する。また、実ＺＭＰ算出部１１は、算出した実ＺＭＰをＺＭＰ比較部１３に出力する。
【００２６】
ＺＭＰ限界値設定部１２は、予め記憶している台車６の寸法や環境外乱に基づいてＺＭＰ限界値を設定する。ＺＭＰ限界値設定部１２は、設定したＺＭＰ限界値をＺＭＰ比較部１３に出力する。なお、ＺＭＰ限界値を設定するにあたり、図示しないカメラなどを用いてロボット４の周囲を撮像し、撮像した画像を画像処理して得られる走路の傾斜などの周辺情報を加味して、ＺＭＰ限界値を設定することもできる。
【００２７】
ＺＭＰ比較部１３は、実ＺＭＰ算出部１１から出力された実ＺＭＰとＺＭＰ限界値設定部１２から出力されたＺＭＰ限界値とを比較し、ＺＭＰが安定領域にあるか否かを判断し、その判断結果に基づいてＺＭＰの変化分（目標変化分）Δｘｚｍｐを算出する。ＺＭＰ比較部１３は、算出したＺＭＰの変化分Δｘｚｍｐを目標角速度算出部１４に出力する。
【００２８】
目標角速度算出部１４は、ＺＭＰ比較部１３から出力されたＺＭＰの変化分Δｘｚｍｐに基づいて、マニピュレータ５の関節角度θ１，θ２を算出する。目標角速度算出部１４は、算出した関節角度θ１，θ２をマニピュレータ５に出力し、マニピュレータ５の関節角度を調整する。
【００２９】
以上の構成を有する本実施形態に係る安定化制御装置における制御手順について説明する。図３は、本実施形態に係る安定化制御の手順を示すフローチャート、図４は、ロボットの安定化制御系のブロック線図である。ここでは、ロボット４がｘ方向に沿って走行し、ロボット４がｘ方向に転倒するのを防止する例について説明する。
【００３０】
安定化制御を開始すると、最初に、角度センサ２によってロボット４から検出され、角度センサ２から出力された関節角度θ１，θ２を入力し（Ｓ１）、加速度センサ３によってロボット４から検出され、加速度センサ３から出力された台車６の加速度を入力する（Ｓ２）。
【００３１】
続いて、ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出する（Ｓ３）。ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出するにあたり、ＺＭＰについて説明すると、ＺＭＰは、床面においてロボット４の各部の重力によるモーメント、慣性力によるモーメント、および外力によるモーメントの総和が０となる点である。いま、図５に示すように、重力、慣性力、および外力の合力が下向きであり、かつ台車の各車輪６Ａの接地点からなり３点以上の複数である４点の接地点が形成する支持多角形９Ａ（この支持多角形内の領域を「安定領域」という）内にＺＭＰが存在すれば、ロボットは転倒することなく安定して運動することができる。一方、ＺＭＰが安定領域内に存在しなければ、ロボット４は不安定となり、理論上転倒することになる。
【００３２】
そこで、マニピュレータ５を動かしたとしてもＺＭＰが安定領域内に存在することになるようにするため、関節角度θ１，θ２および台車加速度αを用いてＺＭＰの変化分Δｘｚｍｐを求めて、その変化分Δｘｚｍｐに基づいて、目標関節角速度を算出する。この目標関節角速度から関節角度が定められる。ＺＭＰの変化分Δｘｚｍｐの算出は、図６に示すフローに従って行われる。図６は、ＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである。
【００３３】
図６に示すように、ＺＭＰの変化分Δｘｚｍｐを算出する際には、まず、ＺＭＰの限界値ｘｚｍｐ＿ｍを設定する（Ｓ１１）。ＺＭＰの限界値ｘｚｍｐ＿ｍは、ＺＭＰ限界値設定部１２に記憶されているＺＭＰ限界値を読み出すことによって設定される。次に、実ＺＭＰの位置ｘｚｍｐを算出する（Ｓ１２）。実ＺＭＰの位置ｘｚｍｐは、実ＺＭＰ算出部１１において、角度センサ２から出力された関節角度θ１，θ２および加速度センサ３から出力された台車６の加速度αに基づいて算出する。
【００３４】
実ＺＭＰを算出するにあたり、ｘ方向における関節角度θ１，θ２、台車加速度α、およびロボット４のＺＭＰの位置ｘｚｍｐの関係は、下記（１）式で表される。
【００３５】
【数１】
上記（１）式にステップＳ１で入力された関節角度θ１，θ２およびステップＳ２で入力された台車の加速度αを代入することにより、上記（１）式を変形した下記（２）式に基づいて実ＺＭＰの位置ｘｚｍｐの位置を算出することができる。
【００３６】
【数２】
ロボット４には、台車６の加速度の増減によって生じる慣性力が作用しているため、台車加速度の変化に伴い、ＺＭＰの位置が変動している。ここで、ＺＭＰが安定領域内に存在すれば、ロボット４は転倒しないので、安定化のための補正等を行う必要はない。一方、ＺＭＰが安定領域を外れると、ロボットは不安定となり、安定化のための補正を行う必要が生じる。
【００３７】
そこで、実ＺＭＰの位置ｘｚｍｐを算出したら、実ＺＭＰの位置ｘｚｍｐが安定領域内にあるか否かの判断を行う。そのため、まず、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっているか否かを判断する（Ｓ１３）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となってないと判断した場合には、実ＺＭＰの位置ｘｚｍｐが図５に示す安定領域９Ａから外れていると考えられる。このままマニピュレータ５を駆動すると、理論上は実ＺＭＰの位置ｘｚｍｐが安定領域から外れてロボット４が転倒することになる。
【００３８】
そこで、この場合には、ＺＭＰの変化分Δｘｚｍｐを下記（３）式によって求める（Ｓ１４）。
【００３９】
Δｘｚｍｐ＝ｘｚｍｐ＿ｍ−ｘｚｍｐ ・・・（３）
この（３）式からΔｘｚｍｐを求めることにより、マニピュレータ５を動かした後であっても、ロボット４のＺＭＰは安定領域内に位置することになるため、ロボット４の転倒を防止することができる。
【００４０】
一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっているか否かを判断する（Ｓ１５）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以下となってないと判断した場合には、実ＺＭＰの位置ｘｚｍｐが図５に示す安定領域９Ａから外れていると考えられる。このままマニピュレータ５を駆動すると、理論上は実ＺＭＰの位置ｘｚｍｐが安定領域から外れてロボット４が転倒することになる。
【００４１】
そこで、この場合には、ＺＭＰの変化分Δｘｚｍｐを下記（４）式によって求める（Ｓ１６）。
【００４２】
Δｘｚｍｐ＝−ｘｚｍｐ−ｘｚｍｐ＿ｍ ・・・（４）
この（４）式からΔｘｚｍｐを求めることにより、マニピュレータ５を動かした後であっても、ロボット４のＺＭＰは安定領域内に位置することになるため、ロボット４の転倒を防止することができる。
【００４３】
一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐが安定領域９Ａ内にあると考えられる。この場合には、ロボット４は安定しており、転倒の危険性が少なくなっているので、Δｘｚｍｐ＝０に設定する（Ｓ１７）。このようにして、Δｘｚｍｐを設定する。
【００４４】
このように、実ＺＭＰの位置ｘｚｍｐとＺＭＰ限界値ｘｚｍｐ＿ｍとを比較し、その比較結果に基づいてＺＭＰの変化分Δｘｚｍｐを求めることにより、ＺＭＰが安定領域内に存在することになるようにすることができる。したがって、ロボットの転倒を防止することができる。
【００４５】
こうしてＺＭＰの変化分Δｘｚｍｐを算出したら、図３に示すフローに戻り、目標関節角を算出する（Ｓ４）。目標関節角速度を算出する際には、ステップＳ３で求めたΔｘｚｍｐに調節用の係数Ｋを乗じる。それから、係数Ｋを乗じたＺＭＰの変化分Δｘｚｍｐと、関節角度の変化分を関係づけるＺＭＰヤコビアン行列（ヤコビ行列）を用いて目標関節角速度を算出する。本実施形態では、ロボットの制御系として速度制御モードを採用しており、ＺＭＰの速度ｖｘｚｍｐと関節角速度ｖθとの関係は、下記（５）式より求められる。
【００４６】
ｖｘｚｍｐ＝Ｊｚｍｐｖθ ・・・（５）
ここで、ＪｚｍｐはＺＭＰヤコビアン行列、ｖθは［ｖθ１，ｖθ２］Ｔである。
【００４７】
また、Ｊｚｍｐの擬似逆行列をＪ＋ｚｍｐとすると、慣性座標系から見た関節角速度ｖθは、下記（６）式によって算出することができる。
【００４８】
ｖθ＝Ｊ＋ｚｍｐ・ｖｘｚｍｐ ・・・（６）
この（６）式を用いて、ロボット４におけるマニピュレータ５の目標関節角速度ｖθｒｅｆを算出する（Ｓ４）。この目標関節角速度ｖθｒｅｆをロボット４に出力して（Ｓ５）、ロボット４におけるマニピュレータ５における各関節の角速度の制御を行う。この制御では、現在のマニピュレータ５における各関節の角速度ｖθをフィードバックし、目標関節角速度ｖθｒｅｆを加算するフィードバック制御を行う。
【００４９】
このように、本実施形態に係る安定化制御装置では、マニピュレータ５を駆動した後のＺＭＰの位置ｘｚｍｐが安定領域にある場合には、ＺＭＰの変化分Δｘｚｍｐを変えず、ＺＭＰの位置ｘｚｍｐが安定領域から外れる場合にのみＺＭＰの変化分Δｘｚｍｐを調整するようにしている。このため、実ＺＭＰを目標ＺＭＰに追従させる場合と比較すると、その計算量を削減することができる。また、マニピュレータ５を頻繁に動作させることを回避することができるので、エネルギー消費の少ない安定化制御を行うことができる。特に、台車移動加速度が不安定となる限界値の上下にＺＭＰが脈動する場合にマニピュレータ５の頻繁な動作を防止することができる。さらに、マニピュレータ５を頻繁に正負に動作させることを回避することができるので、アクチュエータの寿命の低下を防止することができる。
【００５０】
続いて、本実施形態に係る安定化制御装置を用いたロボットの制御について行ったシミュレーションについて説明する。図２に示すロボット４を用いて、ロボット４を走行させながらマニピュレータ５を作動させるシミュレーションを行った。このシミュレーションでは、ＺＭＰの限界値ｘｚｍｐ＿ｍを６０．０ｍｍに設定した。そのシミュレーションの結果を図７に示す。
【００５１】
図７（ａ）に示すように、台車６の加速度を経時変化させた場合において、図７（ｂ）に、ＺＭＰの位置ｘｚｍｐの経時変化、図７（ｃ）に第一関節角度θ１の経時変化、図７（ｄ）に第二関節角度θ２の経時変化をそれぞれ示す。このうち、図７（ａ）には、台車が不安定となる加速度の限界値（上限値および下限値）を折れ線Ｌ１，Ｌ２で示している。また、図７（ｂ）には、折れ線Ｌ３で示す本実施形態の制御による経時変化と、折れ線Ｌ４で示す従来の制御による経時変化の例を示している。
【００５２】
図７（ｂ）から分かるように、従来の制御では、台車６の加速度の変化により、ＺＭＰの位置ｘｚｍｐは、ロボット４が不安定となるＺＭＰの限界値を超えることがあり、この場合には、台車６が転倒する恐れが高くなるものであった。これに対して、本実施形態に係る安定化制御を行った場合には、台車６の加速度が変化した場合でも、ＺＭＰの位置ｘｚｍｐは、ロボット４が不安定となるＺＭＰの位置の限界値を超えることはなく、ロボット４の転倒を効果的に防止することができる。
【００５３】
また、台車６の加速度が不安定となる限界値の上下に脈動する場合であっても、図７（ｃ）、（ｄ）に示すように、マニピュレータ５を頻繁に正負方向に動作させることもないようにすることができる。したがって、マニピュレータ５の作動によるエネルギーの消費を低減することができ、アクチュエータの寿命の低下を防止することもできる。
【００５４】
次に、本発明の第二の実施形態について説明する。本実施形態に係るロボットの安定化制御は、上記第一の実施形態と同様のロボット４を対象として行われる。また、上記の第一の実施形態と比較して、ＺＭＰ限界値設定部１２に記憶されたＺＭＰ限界値が一部異なっている点で主に異なる。
【００５５】
本実施形態に係るロボットの安定化制御装置は、ＺＭＰ限界値設定部１２には、上記第一の実施形態と同様のＺＭＰ限界値のほか、台車６の速度が「０」の場合のＺＭＰ限界値として、ＺＭＰ限界値「０」が記憶されている。
【００５６】
次に、本実施形態に係る安定化制御の手順を説明するが、上記第一の実施形態と比較して、ＺＭＰの変化分Δｘｚｍｐの算出が主に異なるので、Δｘｚｍｐの算出手順について主に説明する。図８は、ロボットの安定化制御系のブロック線図、図９は、Δｘｚｍｐを算出する手順を示すフローチャートである。
【００５７】
安定化制御を開始すると、上記第一の実施形態と同様、角度センサ２から出力された関節角度θ１，θ２を入力し、加速度センサ３から出力された台車６の加速度を入力する。その後に、ｘ方向のＺＭＰの変化分Δｘｚｍｐを算出する。
【００５８】
たとえば、台車６の上に胴体が設けられた人間型ロボットでは、胴体が直立した状態を保持するときの作業性が最も良好なものとなる。そこで、ロボットが直立状態を保持するときにＺＭＰの初期位置が０であると仮定すると、台車６が静止状態を保持する（台車移動速度＝０である）とき、ロボット４の姿勢を直立状態に回復するために、ＺＭＰを０にすることが必要となる。
【００５９】
したがって、台車６が移動している間、台車６、移動速度を計測する。そして、台車移動速度が０ではないとき、上記第一の実施形態と同様にしてＺＭＰの変化分Δｘｚｍｐを算出する。また、台車移動速度が０であるときには、ＺＭＰの変化分Δｘｚｍｐを０に設定する。
【００６０】
このため、Δｘｚｍｐを設定するにあたり、図９に示すように、上記第一の実施形態と同様、まずＺＭＰの限界値ｘｚｍｐ＿ｍを設定し（Ｓ２１）、次に、上記第一の実施形態と同様にして、実ＺＭＰの位置ｘｚｍｐを算出する（Ｓ２２）。
【００６１】
ＺＭＰの限界値ｘｚｍｐ＿ｍを設定し、実ＺＭＰの位置ｘｚｍｐを算出したら、続いて、台車の移動速度を計測する（Ｓ２３）。台車６の移動速度の計測は、速度センサを用いて直接行うほか、台車６の加速度を積分することによって行うこともできる。こうして、台車６の移動速度を計測したら、台車移動速度が０であるか否かを判断する（Ｓ２４）。その結果、台車の移動速度が０である、換言すれば、台車が停止した状態にあるときには、ＺＭＰの限界値ｘｚｍｐ＿ｍを０に再設定する（Ｓ２５）。一方、台車移動速度が０でないと判断した場合には、設定されたＺＭＰの限界値ｘｚｍｐ＿ｍをそのまま用いる。
【００６２】
その後は、上記第一の実施形態と同様、実ＺＭＰの位置ｘｚｍｐが安定領域内にあるか否かの判断を行うために、まず、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっているか否かを判断する（Ｓ２６）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となってないと判断した場合には、上記（３）式からΔｘｚｍｐを求める（Ｓ２７）。
【００６３】
また、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となっているか否かを判断する（Ｓ２８）。その結果、実ＺＭＰの位置ｘｚｍｐがｘ方向に負のＺＭＰ限界値−ｘｚｍｐ＿ｍ以上となってないと判断した場合には、上記（４）式からΔｘｚｍｐを求める（Ｓ２７）。一方、実ＺＭＰの位置ｘｚｍｐがｘ方向に正のＺＭＰ限界値ｘｚｍｐ＿ｍ以下となっていると判断した場合には、Δｘｚｍｐ＝０に設定する。
【００６４】
このように、実ＺＭＰの位置ｘｚｍｐとＺＭＰ限界値ｘｚｍｐ＿ｍとを比較し、その比較結果に基づいてＺＭＰの変化分Δｘｚｍｐを求めることにより、ＺＭＰが安定領域内に存在することになるようにすることができる。したがって、ロボットの転倒を防止することができる。
【００６５】
また、台車６の移動速度が０である場合には、ＺＭＰ限界値ｘｚｍｐ＿ｍを０に設定しているので、人間型ロボットのように、台車の上に胴体が設けられたロボットでは、台車６が停止しているときに胴体が起立状態を保持しようとする。したがって、ロボットの作業性を良好なものとすることができる。
【００６６】
続いて、本実施形態に係る安定化制御装置を用いたロボットの制御について行ったシミュレーションについて説明する。このシミュレーションでは、ＺＭＰの限界値ｘｚｍｐ＿ｍを６０．０ｍｍに設定した。そのシミュレーションの結果を図１０に示す。
【００６７】
図１０（ａ）に曲線Ｃ１で示すように、台車６の速度を経時変化させた場合において、図１０（ｂ）に、ＺＭＰの位置ｘｚｍｐの経時変化、図１０（ｃ）に第一関節角度θ１の経時変化、図１０（ｄ）に第二関節角度θ２の経時変化をそれぞれ示す。このうち、図１０（ａ）には、台車の速度のほか、台車の加速度を折れ線Ｌ１０で示している。また、図１０（ｂ）には、折れ線Ｌ１１で示す本実施形態の制御による経時変化と、折れ線Ｌ１２で示す上記第一の実施形態の制御による経時変化と、折れ線Ｌ１３で示す従来の制御による経時変化の例を示している。
【００６８】
さらに、図１０（ｃ）には、折れ線Ｌ１４で示す本実施形態の制御による経時変化と、折れ線Ｌ１５で示す従来の制御による経時変化の例を示し、図１０（ｄ）には、折れ線Ｌ１６で示す本実施形態の制御による経時変化と、折れ線Ｌ１７で示す従来の制御による経時変化の例を示している。
【００６９】
図１０から（ｂ）から分かるように、従来の制御では、台車の加速度の変化により、ＺＭＰの位置ｘｚｍｐは、ロボットが不安定となるＺＭＰの限界値を超えることがあり、この場合には、台車が転倒する恐れが高くなるものであった。これに対して、本実施形態に係る安定化制御を行った場合には、台車の加速度が変化した場合でも、ＺＭＰの位置ｘｚｍｐは、ロボットが不安定となるＺＭＰの位置の限界値を超えることはなく、ロボットの転倒を効果的に防止することができる。
【００７０】
また、台車の加速度が不安定となる限界値の上下に脈動する場合であっても、図１０（ｃ）、（ｄ）に示すように、マニピュレータを頻繁に正負方向に動作させることもないようにすることができる。したがって、マニピュレータの作動によるエネルギーの消費を低減することができ、アクチュエータの寿命の低下を防止することもできる。
【００７１】
さらに、図１０（ｂ）〜（ｄ）から分かるように、本実施形態に係る安定化制御では、上記第一の実施形態と比較して、台車の移動速度が０となった後は、ロボットの最終姿勢を早期に直立状態に回復することができる。したがって、良好な作業性を維持することができる。
【００７２】
以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。たとえば、上記実施形態では４つの車輪が接地する点で囲まれる部分を安定領域として設定したが、３つ以上の接地点を囲む領域であれば、安定領域とすることができる。また、接地する点で囲まれる範囲を若干増減させる形で安定領域を設定することもできる。
【００７３】
さらに、上記実施形態では、ｘ方向についてのみ安定化制御を行っているが、ｙ方向、あるいはｘ−ｙ平面上で安定化制御を行う態様とすることもできる。他方、上記実施形態では、ヤコビアン行列を用いて関節角度を算出するようにしているが、他の方法によって関節角度を算出する態様とすることもできる。さらに、上記実施形態では、関節が２つであるマニピュレータを有するロボットを制御対象としているが、関節が１つまたは３つ以上であるマニピュレータを有するロボットを制御対象とすることもできる。
【図面の簡単な説明】
【００７４】
【図１】第一の実施形態に係るロボットの安定化制御装置のブロック構成図である。
【図２】ロボットの概略を示す側面図である。
【図３】本実施形態に係る安定化制御の手順を示すフローチャートである。
【図４】ロボットの安定化制御系のブロック線図である。
【図５】安定領域を説明する概念図である。
【図６】ＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである。
【図７】（ａ）は台車の経時変化を示すグラフ、（ｂ）はＺＭＰの経時変化を示すグラフ、（ｃ）は第一関節の関節角度の経時変化を示すグラフ、（ｄ）は第二関節の関節角度の経時変化を示すグラフである。
【図８】第二の実施形態に係るロボットの安定化制御系のブロック線図である。
【図９】第二の実施形態におけるＺＭＰの変化分Δｘｚｍｐを算出する手順を示すフローチャートである
【図１０】第二の実施形態に係るものであり、（ａ）は台車の経時変化を示すグラフ、（ｂ）はＺＭＰの経時変化を示すグラフ、（ｃ）は第一関節の関節角度の経時変化を示すグラフ、（ｄ）は第二関節の関節角度の経時変化を示すグラフである。
【符号の説明】
【００７５】
１…安定化制御装置、２…角度センサ、３…加速度センサ、４…ロボット、５…マニピュレータ、５Ａ…第一リンク、５Ｂ…第二リンク、５Ｃ…ハンド部材、６…台車、６Ａ…車輪、１１…ＺＭＰ算出部、１２…限界値設定部、１３…ＺＭＰ比較部、１４…目標角速度算出部。
【特許請求の範囲】
【請求項１】
台車に搭載されたマニピュレータを有する台車移動型ロボットの安定化制御を行うロボットの安定化制御装置であって、
前記台車移動型ロボットの挙動に基づいて求められる実ＺＭＰと前記台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値とを比較し、前記実ＺＭＰおよび前記ＺＭＰ限界値に基づいて、前記台車移動型ロボットのＺＭＰの目標変化分を設定することを特徴とするロボットの安定化制御装置。
【請求項２】
前記台車は、３点以上の複数の接地点を有しており、
前記複数の接地点で囲まれた領域の境界線に対応して前記ＺＭＰ限界値が設定される請求項１に記載のロボットの安定化制御装置。
【請求項３】
前記台車移動型ロボットにおける前記台車の移動速度が０である場合に、前記ＺＭＰ限界値を０に設定する請求項１または請求項２に記載のロボットの安定化制御装置。
【請求項４】
前記マニピュレータは、複数のリンクと、前記複数のリンクを接続する関節とを有しており、
前記台車移動型ロボットにおけるＺＭＰの変化分と前記マニピュレータにおける関節角度の変化分とを関係づけるＺＭＰヤコビアン行列を用いて、前記ＺＭＰから前記関節における関節角度を算出する請求項１〜請求項３のうちのいずれか１項に記載のロボットの安定化制御装置。
【請求項１】
台車に搭載されたマニピュレータを有する台車移動型ロボットの安定化制御を行うロボットの安定化制御装置であって、
前記台車移動型ロボットの挙動に基づいて求められる実ＺＭＰと前記台車移動型ロボットの起立状態が不安定となるＺＭＰ限界値とを比較し、前記実ＺＭＰおよび前記ＺＭＰ限界値に基づいて、前記台車移動型ロボットのＺＭＰの目標変化分を設定することを特徴とするロボットの安定化制御装置。
【請求項２】
前記台車は、３点以上の複数の接地点を有しており、
前記複数の接地点で囲まれた領域の境界線に対応して前記ＺＭＰ限界値が設定される請求項１に記載のロボットの安定化制御装置。
【請求項３】
前記台車移動型ロボットにおける前記台車の移動速度が０である場合に、前記ＺＭＰ限界値を０に設定する請求項１または請求項２に記載のロボットの安定化制御装置。
【請求項４】
前記マニピュレータは、複数のリンクと、前記複数のリンクを接続する関節とを有しており、
前記台車移動型ロボットにおけるＺＭＰの変化分と前記マニピュレータにおける関節角度の変化分とを関係づけるＺＭＰヤコビアン行列を用いて、前記ＺＭＰから前記関節における関節角度を算出する請求項１〜請求項３のうちのいずれか１項に記載のロボットの安定化制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００６−１５０５６７（Ｐ２００６−１５０５６７Ａ）
【公開日】平成１８年６月１５日（２００６．６．１５）
【国際特許分類】
【出願番号】特願２００４−３４８９２１（Ｐ２００４−３４８９２１）
【出願日】平成１６年１２月１日（２００４．１２．１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成１８年６月１５日（２００６．６．１５）
【国際特許分類】
【出願日】平成１６年１２月１日（２００４．１２．１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]