JP2610443B2 - ロボットの姿勢制御方法 - Google Patents

Description

【発明の詳細な説明】 〔目次〕 概要 産業上の利用分野 従来の技術（第12図（ａ），（ｂ），（ｃ）） 発明が解決しようとする問題点（第13図，第14図） 問題点を解決するための手段（第１図） 作用 実施例 （ａ）一実施例の説明（第２図乃至第６図） （ｂ）一実施例制御演算の説明（第７図乃至第８図） （ｃ）他の実施例の説明 発明の効果 〔概要〕 ロボットの姿勢制御方法に係り、特にロボットの外的
環境を外的センサで検出して、ロボットの姿勢を自律的
に適応制御するロボットの姿勢制御方法に関し、 ロボットが障害物回避時に最適な姿勢をとると共に、
目標位置において正確に目標姿勢となるように姿勢制御
できるようにすることを目的とし、 ロボットの外的環境を検出するセンサ、該センサによ
り検出された外的環境に基づき適応制御されるロボット
の前記姿勢を制御する方法であって、前記外的環境に基
づいてポテンシャル量を求め、該ポテンシャル量を前記
ロボットに作用する反発力として前記回転自由度に加わ
る第１のモーメントを求め、該ロボットの到達目標位置
におけるロボットの目標姿勢と現姿勢との偏差及び到達
目標位置までの距離とに基づいて第２のモーメントを求
め、前記ロボットの前記回転自由度の回転角速度に基づ
いて第３のモーメントを求め、前記第１〜第３のモーメ
ントに基づいて該ロボットを適応制御するロボットの姿
勢制御方法において、前記第２のモーメントの大きさを
決定する係数を到達目標位置までの距離の関数とし、こ
の関数を目標大位置を離れるにつれて減少するように構
成する。

〔産業上の利用分野〕

本発明は、ロボットの姿勢制御方法に係り、特にロボ
ットの外的環境を外的センサで検出して、ロボットの姿
勢を自律的に適応制御するロボットの姿勢制御方法に関
する。

近年ロボットの知能化高機能化の要求に伴ない、ロボ
ットの未知の環境、例えば対象物の存在、障害物の存在
等に適用した制御、所謂外的環境適用制御が求められれ
ている。

このような制御を行なうには、外的環境を何等かの方
法で検出するセンサ（外的センサという）、例えばテレ
ビカメラ等の視覚センサや距離センサ、触角センサ、力
センサ、温度センサ等を設け、係るセンサの出力によっ
てロボットの運動姿勢を適用制御し障害物回避等を行な
う。

〔従来の技術〕

従来上述のようなロボットの姿勢制御方法として第12
図に示すようなものがある。

これは第12図（Ａ）に示すように、ロボット１の作業
空間（運動空間）をX,Y軸で表記する２次元空間である
と、第12図（Ｂ）に示すようにこれにＰ軸を加えた３次
元の仮想空間を仮定する。

そして、第12図（Ａ）に作業空間で得たセンサ出力
（距離Ｒ）に対し、第12図（Ｂ）に示すように、仮想空
間のＲの位置にX,Y軸方向に予め定めた所定の広がりを
もちＰ軸方向に予め定めた所定の高さを持つ立体像Ｐを
生成する。即ちセンサの出力を仮想空間の立体像Ｐに変
換する。

この仮想空間では第12図（Ｂ）に示すように１次元の
距離情報が多次元の立体的な対象物として表示されるこ
ととなる。

この仮想空間での立体像ＰのＰ軸方向に高さを持って
いるので、ロボットの位置に対応する仮想空間上の位置
Ａにおいて障害物OBの立体像Ｐに起因するＰ軸方向に場
の状態量を有することとなる。この状態量を用いて制御
量を求めることができ、これを仮想空間からロボットが
受ける反発力として、この反発力に応じてロボットの姿
勢等を制御する。

例えば、自走型のロボット１が走行している時に、障
害物OBをセンサで見付け、姿勢制御によって障害物OBと
の衝突を回避する場合には、仮想空間では、第12図
（Ｃ）に示すように、立体像（制御場）が形成され、こ
の制御場ではロボット１にはセンサ21,22の位置で反発
力F₁,F₂を受けることとなり、第１の姿勢制御用のモー
メントM₁を得ることができる。

一方ロボットは移動目標位置P_rにおいて、所定の動作
等を実行するためには所定の目標姿勢φ_ｒになっていな
ければならない。そこで現在におけるロボットの姿勢φ
を検出して、予め設定した目標位置における目標姿勢と
の差（φ_ｃ＝φ_ｒ−φ）を検出して、目標姿勢に向けこ
の角度差φ_ｃに比例した量を算出し、この量に基づいて
ロボットの重心周りに第２のモーメントM₂を得ることが
できる。

また現在における回転角速度を検出し、それに比例
した量を算出しこの量に基づいてロボットの重心周りに
第３のモーメントM₃を得ることができる。

この３つのモーメントM₁,M₂,M₃をロボット１に与える
ことにより、障害物を回避しつつ移動し、目標位置に到
達したときには所定の目標角度にロボットが向いている
ようにロボットの姿勢を制御するようにしている。

〔発明が解決しようとする問題点〕

ところで、上述した従来のロボットの姿勢制御方法に
あっては目標角度にロボットを向かせようとする第２の
モーメントは常にロボットに作用するから、この第２の
モーメントの大きさを決定する比例定数を大きくする
と、第13図に示すように、障害物を回避するのに最適な
姿勢をとろうとするのを妨げることとなる。同図は、ロ
ボット１が出発位置Ｓで図中Ｙ方向を向いているのを障
害物OBを避けて目標位置P_rまで移動して、目標位置では
Ｘ方向を向くように制御すべく比例定数として所定の値
を与えシミュレーションしたものである。同図におい
て、障害物の付近においてロボットの動きは障害物を避
けるには適当ではないものとなっている。

またこの比例定数を小さくすると、第14図に示すよう
に、ロボット１は目標位置Ｐにおいて目標角度に収束し
ないこととなる。同図は、比例定数を上述の場合の1/8
程度に設定した他はロボットには第13図の場合と同一の
条件を与えてシミュレーションしたものである。同図に
おいてロボットは目標位置において目標角度であるＸ方
向を向いていない。

従って、上記の比例定数は最適なものとする必要が有
る。

しかしながら、この比例定数の最適値の選択は難し
く、とくに障害物付近に目標位置を設定した場合には最
適値を選択することは困難なものとなるという問題点が
ある。

本発明は、ロボットが障害物回避時に最適な姿勢をと
ると共に、目標位置において正確に目標姿勢となるよう
姿勢制御できるロボットの姿勢制御方法を提供すること
を目的とする。

〔問題点を解決するための手段〕

本発明において、上記の問題点を解決するための手段
は、姿勢を変更可能な回転自由度を有するロボット１、
該ロボット１の外的環境を検出するセンサ22,23,……,2
n、該センサにより検出された外的環境に基づき適応制
御される前記ロボット１の前記姿勢を制御する方法であ
って、前記外的環境に基づいてポテンシャル量を求め、
該ポテンシャル量を前記ロボットに作用する反発力とし
て前記回転自由度に加わる第１のモーメントM₁を求め、
該ロボット１の到達目標位置におけるロボット１の目標
姿勢φ_ｒと現姿勢φとの偏差及び到達目標位置までの距
離R_cとに基づいて第２のモーメントM₂を求め、前記ロボ
ット１の前記回転自由度の回転角速度φ/dtに基づいて
第３のモーメントM₃を求め、前記第１〜第３のモーメン
トM₁,M₂,M₃に基づいて該ロボットを適応制御するロボッ
トの姿勢制御方法において、前記第２のモーメントM₂の
大きさを決定する係数を到達目標位置までの距離R_cの関
数κ（R_c）とし、この関数κ（R_c）を目標位置を離れる
につれて減少するようにしたことである。

〔作用〕

本発明によれば上述した第２のモーメントM₂の大きさ
は目標位置の遠方では小さいから、ロボットは第１のモ
ーメントM₁により障害回避に最適な姿勢に制御され、目
標に近付くと第２のモーメントM₂は大きなものとなるか
ら目標位置においてロボットは正確に目標角度を向くよ
うに制御される。

〔実施例〕

（ａ）一実施例の説明 第２図は本発明の一実施例説明図、第３図は第２図に
おける実空間と仮想空間の関係図、第４図は第２図にお
ける仮想空間による動作説明図である。

本実施例において、ロボットは仮想空間の反発力から
並進変位の制御量と姿勢を決定する第１のモーメントM₁
を、また該ロボット１の到達目標位置Ｐにおけるロボッ
トの目標姿勢φ_ｒと現姿勢φとの違いによって第２のモ
ーメントM₂を発生して制御を実行する。

第２図（Ａ）において、２次元X,Yの実空間（作業空
間）を移動するロボット１の側面に第２図（Ｃ）の如
く、各々距離センサである超音波センサ21〜28を２個づ
つ合計８個設け、超音波センサ21〜28で外部環境を把握
しながら、障害物OB1、OB2を避けながら実空間を移動
（並進移動，姿勢制御）する例を示している。

先ず、超音波センサ（以下センサと称す）21〜28の検
出距離r_iを、ロボット固有（重心を原点）の座標Ｘ−Ｙ
で表し、センサが検出できる方向を定義する単位ベクト
ルｅ＝（e_xi,e_yi），（ｉ＝１〜８）と、第２図（Ｃ）
に示す如く設定するセンサの取付位置A₁〜A₈の位置ベク
トル_ｉ＝（S_xi,S_yi），（ｉ＝１〜８）とを用いて、
センサｉを捉えた障害物位置（X_i,Y_i）を表わす第
（１）式。

（X_i,Y_i）＝r_i＊（e_xi,e_yi）＋（S_xi,S_yi） ‥‥（１） これで、センサ21〜28の検出距離r_iより、ロボット１
の各センサ21〜28からの障害物の相対位置ベクトル_ｉ
＝（X_i,Y_i）が得られる。

次に、これを仮想空間において展開する。

第３図に示す如く、実空間RSと仮想空間ISとにおい
て、実空間RSのX,Y2次元座標と仮想空間ISのX,Y2次元座
標とは一致しており、仮想空間ISは実空間RSのX,Y平面
に対し、新たに直交するＰ軸（ポテンシャル軸）を加え
たものである。

従って、２次元軸空間RSの点Ａの位置は、仮想空間IS
の２次元平面Ｘ−Ｙの位置と対応する。

前述のセンサ21〜28の検出距離r_iによる相対位置ベク
トル_ｉを用いて、仮想空間ISにおけるＸ−Ｙ面の_ｉ
の示す位置を中心に、広がりをもった多次元正規分布曲
面をを障害物OB1,OB2の立体像P_iとして生成する。

この立体像P_iは、 P_i）A_i＝K_i×exp（−R²/2σ^２） ‥‥（２） 但し、R²＝（Ｘ−X_i）^２＋（Ｙ−Y_i）^２、A_iはセンサ
取付位置、σは標準偏差である。

この仮想空間ISでは、各センサ取付位置A_i（ｉ＝１〜
８）においての立体像P_iが順次生成され、重ね合された
立体像群（ポテンシャル曲面という）Ｐは、 Ｐ＝ΣP_i）A_i ‥‥（３） となる。

従って、第２図（Ａ）の例では、仮想平面では第２図
（Ｂ）の如く障害物OB1,OB2の位置にX,Y方向に広がりを
持ち、Ｐ軸方向に高さを持つ立体像P_a,P_bが生成され、
これらの重ね合せによるポテンシャル曲面が形成され
る。

この仮想平面ISにおいて、ロボット１の位置でのＰ軸
方向の高さは、立体像P_a,P_bによるポテンシャル面PSの
Ｐ軸方向の高さPAとなる。

このようなポテンシャル面PSにおいて、ロボット１が
ポテンシャル面PSに沿ってポテンシャルの低い方に転が
るような反発力RFを生成するとともに、到達目標位置に
おけるロボットの目標姿勢と現姿勢との違いによる復元
力を生成して、ロボット１の並進変位及び姿勢制御を行
ない、障害物を回避する。

次にこのようにセンサ21〜28の出力から生成されたポ
テンシャル曲面からの反発力を求める方法について説明
する。

第４図及び第５図は反発力換算説明図、第６図は仮想
空間による動作説明図である。

第３図のＡ点における仮想空間ISのポテンシャル面PS
を拡大すると、第４図（Ａ）となる。

即ち、Ｘ方向に傾き Ｙ方向に傾き を持つ面である。

これから、係るポテンシャル面PSに沿って転がろうと
する反発力を求める。

ここでＰ軸の負方向に仮想重力速度Ｇを第４図（Ａ）
に示す如く設定し、Ｘ軸,Y軸方向の反発力の制御量とし
て加速度_r,_ｒを求める。

Ｘ軸方向について考えると、第４図（Ｂ）の如くポテ
ンシャル曲面PSの斜度はαであるから、加速度は、 となり、 とすれば、第（１）式は、_ｒ ＝Ｋ・sin2α ‥‥（５） となって計算によって求められる。

同様にＹ軸の加速度は、_ｒ ＝Ｋ・sin2β ‥‥（６） として得られる。

これをX,Y軸方向の制御量とすれば、第４図（Ｃ）に
示す如く、ロボット１を球と仮定し、Ｘ軸,Y軸の加速度
_r,_ｒの合成によってポテンシャル曲面PSに沿って転
がり力（反発力）RFが発生する。

即ち、第２図（Ｂ）の仮想空間において、ロボット１
はＰ軸ポテンシャル値の小さい方向に反発力が加わり、
従って、実空間では、ロボット１は障害物OB1,OB2の間
を障害物OB1,OB2を避けて移動することができる。

この原理は、ロボット１の重心に注目し、Ａ点を重
心の位置とすることによって、ロボット１の並進変位
運動の制御に用いることができる。

これをロボット１の姿勢制御に用いるには、次のよう
にする。

第５図（Ａ），（Ｂ）に示す如く、ロボット１全体の
質点が重心にあるという仮定から質点はセンサ21〜28
の取付け位置A₁〜A₈に分散存在することにする。

また、ｎ（＝８）個に分散した質量m_i（M_s/n）の和は
仮想空間におけるロボット１の仮想質量M_sに等しく。

ロボットモデルの重心は、力学的釣合い条件から求め
られる通常の力学的重心に一致する。

これらの仮定によって、センサ出力から発生する仮想
空間からの反発力F_iは、センサ取付位置に分散配置され
た質点A_iの中心に第１のモーメントM₁として作用し、重
心にはこの第１のモーメントM₁及び後述する仮想吸引
力、第２のモーメントM₂と第３のモーメントM₃が作用す
る。

慣性モーメントＩは、第５図（Ａ）の如く、並進変位
のＸ−Ｙ軸に対し、点Ａ（重心Ｏ）におけるＰ軸回りの
ものである。従って、姿勢制御の第１のモーメントM₁を
求めるには、先づ、各センサ取付け位置A₁〜A₈での反発
力の制御量_ri,_riを求める。

即ち、最初に第４図同様の方法で各センサ取付け位置
A_i（A₁〜A₈）のＸ方向の傾きα_i,Y方向の傾きβ_ｉを求
める。

そして、各センサ取付け位置A_iでの加速度_ri,_ri
を第（５）式，第（６）式を変形して、次のように得
る。_ri ＝Ｋ・sin2α_ｉ ‥‥（７）_ri ＝Ｋ・sin2β_ｉ ‥‥（８） ここで、各センサ取付け位置A_iで受ける反発力F_iは、 F_i＝（F_xi,F_yi） ＝m_i（_ri,_ri） ‥‥（９） である。但し、m_iは分散された質点の質量である。

従って、第（７）式，第（８）式，第（９）式より、 となる。

このため、ロボットの重心回りの第１のモーメント
M₁は、 と定義される。

従って、第（11）式に於ける第（９）式，第（10）式
各センサの取付け位置A_iの仮想空間ISからの反発力F_iを
求め、第（11）式によって重心からのセンサ位置ベク
トル_ｉとによって、姿勢制御のための第１のモーメン
トM₁が求められる。

同様に、重心での並進力Ｆ（F_x,F_y）は、 であるので、第（10）式より求めた各センサ取付け位置
A_iでの反発力F_xi,F_yiによって、並進変位の力（第４図
の転がり力）が得られる。

即ち、第６図に示すように、仮想的に仮想空間ISから
各センサ21〜28の出力で生成したポテンシャル面PSから
ロボット１は各センサ取付け位置A_iで図のような反発力
F_iを得、第（11）式から姿勢制御の第１のモーメントM₁
を、第（12）式から並進変位力を得ることとなる。

次に、到達目標位置におけるロボットの目標姿勢φ_ｒ
と現姿勢φとの違いによる復元力によって第２のモーメ
ントM₂を得る方法について説明する。この例において、
予め設定されている目標地点P_rの座標（X_r,Y_r）と、ロ
ボットに設けている加速度計の値を２回積分して得た現
在地点の座標Ｐとを比較して位置偏差R_cを求め、又予め
設定されている目標位置における目標姿勢φ_ｒと、ロボ
ットに設けた回転速度計の値を積分して求めた現在の姿
勢φとを比較して角度偏差φ_ｃを求め復元力である第２
のモーメントM₂の値を関数として求めるようにしてい
る。

M₂＝κ（R_c）（φ_ｒ−φ） ‥‥（13） ここで、κ（R_c）は定義域が［0,∞］、値域［0,M］
（Ｍは正の実数）、ｄκ（R_c）、/dR_c≦０、且つ、R_c＝
０付近ではＭに近い値を取り、R_cが増加すると急激に減
少し０に近い値を取る関数を選択している。この関数κ
（R_c）としては、 κ（R_c）＝Ｍ・exp（−R_c ²/A²） ‥‥（14） M,Aは定数 を選択することができる。

また、現在の回転角速度による粘性力によって第３
のモーメントM₃は次の方法によって得る。ロボットに設
けられた回転速度計により回転角速度を得、これによ
り第３のモーメントM₃は、 M₃＝Ｄ・φ_ｎ ‥‥（Ａ） 但しＤは捻り粘性係数（定数）として求めることがで
きる。

これにより、ロボット１は第（11）式から姿勢制御の
第１のモーメントM₁を、第（12）式から並進変位力を、
また第（13）式，第（Ａ）式から姿勢制御の第２のモー
メントM₂、第３のモーメントM₃を得、図中の矢印の如く
例えばロボット１は障害物OB1,OB2を回避し、またこれ
らの間を姿勢を制御しつつ移動して、目標位置に目標角
度で到達することとなる。

したがってロボット１の姿勢は、 なる方程式で表わされる。ここでＩはロボットの慣性モ
ーメントである。

（ｂ）一実施例制御演算の説明 上述の姿勢制御及び並進変位制御は、センサの出力か
ら演算によって実行される。

第７図は係る制御量演算のブロック図であり、各演算
をステップをブロックで示してある。

図中、第２図で示したものと同一のものは同一の記号
であり、30は座標変換部であり、前述のセンサ21〜2nの
検出距離r₁〜r_nを、第（１）式によりセンサ21〜2nの実
空間上での方向ベクトル_１〜_ｎとセンサ取付け位置
ベクトル_１〜_ｎを用いて前述の相対位置座標
_１（X₁,Y₁）〜_ｎ（X_n,Y_n）に変換するもの、31は制御
偏差発生部であり、与えられたロボット１の目標位置
X_r,Y_rと現在位置との位置誤差_ｃ及び角度偏差_ｃを
発生するものであり、ロボット１の内部センサからの各
軸（X,Y軸）の検出した現加速度，を２回積分部31a
で２回積分して現在位置を得、同時にロボット１の内部
に設けられたジャイロで検出したロボット１の現回転速
度を積分部31cで積分して得た現回転角φを用いて変
換行列生成部31dで変換行列を得て、上述の現在位置に
この変換行列を乗算部31bで乗じて座標変換し、差分部3
1eで目的位置X_r,Y_rと現在位置との差を求めて位置誤差
_ｃを発生するとともに、差分部31fで目標姿勢φ_ｒと
積分部31cからの現回転角φとの差を求めて_ｃを発生
するものである。

32は像変換部であり、入力される位置座標_１〜_ｎ
及び位置偏差_ｃを基本関数を用いて立体像に変換する
ものであり、具体的には多次元正規分布関数を基本関数
として用い、正規分布の立体像に変換するものである。

即ち、立体像P_iは、第（２）式の演算によって生成さ
れる。

33は像演算部であり、像変換された各立体像P_iを加算
して、仮想空間でのポテンシャル面を作成するものであ
り、ポテンシャル面（値）Ｐは第（３）式の演算によっ
て得られる。

34は換算部であり、前述の像演算部33で演算された仮
想空間でのポテンシャル値Ｐから偏微分によって、前述
の各センサ取付位置でのX,Y方向の曲面斜度α_i,β_ｉを
求め、反発力F_iを得、後述する仮想吸引力と合成し、並
進変位の制御量（速度）_r,_ｒを、後述する第２のモ
ーメントと合成して姿勢制御量_ｒを演算するものであ
り、斜度計算機構34a,仮想重力計算機構34b,外積部34c1
〜34cn,除算部34d,加算器34e,回転角積分部34f,加算部3
4g,並進積分部34hを有している。

斜度計算機構34aは、前述の第４図で説明したX,Y方向
の偏微粉を各センサ取付位置A_iで求め、曲面斜度α_i,β
_ｉを求めるものであり、次式により得られる。

センサ取付けベクトル_１〜_ｎより取付け位置A₁〜
A_nを得て求める。

更に、 を演算し、各取付け位置A_iの曲面斜度α_i,β_ｉを得る。
又、仮想重力計算機構34bは、第４図で説明した第
（７）式及び（８）式によって加速度_ri,_riを求め
第（９）式によって反発力F_iを演算するものである。

外積部34sc1〜34cnは求めた反発力F₁〜F_nとセンサ取
付位置ベクトル_１〜_ｎとの外積_ｉ×_ｉを求める
ものであり、除算部34dは後述する第２のモーメントM₂
を質量ｍ分の１の値にするもの、加算部34eは外積部34c
1〜34cnの外積の和をとり、第（11）式の第１のモーメ
ントM₁を求め且つ除算部34dからの第２のモーメントM₂
と加算するもの、34fは加算部34eの出力（合成加速度）
を して積分し、回転制御量_ｒを出力するものである。

加算部34gは、仮想重量計算機構34bからの反発力F_iの
和をとり、第（12）式の並進力Ｆ（F_x,F_y）を得且つ後
述する仮想吸引力発生部35からの仮想吸引力を加算する
もの、並進積分34hは、加算部34gの合成加速度_r,_ｒ
を1/M_sして積分して並進制御量_r,_ｒを求めるもので
ある。

35は仮想吸引発生部であり、位置偏差_ｃから目的位
置への並進方向の吸引力である誘導加速力を発生するも
のであり、位置偏差_ｃ（X_r−Ｘ）を比例定数で倍
してバネ力としてフィードバックする比例部35aと、位
置偏差_ｃを一階微分し（Ｓはラプラス演算子）、偏差
速度を得、これを比例定数でＣ倍して粘性力として得る
微分部35bと、比例部35aのバネ力と微分部35bの粘性力
とを引いて誘導加速力を発する減算部35cおを有するも
のである。

36は第２モーメント発生部であり、角度偏差φ_ｃと位
置偏差_ｃとから、第２のモーメントM₂を発生するもの
であり、第２のモーメントM₂を第（13）及び第（14）式
から求める演算部36aと、角度偏差_ｃを一階微分し、
偏差回転速度を得、これを比例定数でＤ倍して粘性力と
しての第３のモーメントM₃を第（Ａ）式より得る微分部
36bと、演算部36aと微分部36bの粘性力とを加える加算
部36cを有するものである。

次に、第７図実施例の動作について第８図，第９図を
用いて説明する。

ロボット１の各センサ21〜2nの出力は座標変換部30で
座標変換され、ロボット１からの相対座標_ｉ（X_i,
Y_i）に変換される。一方、制御偏差発生部31で位置偏差
_ｃ（X_c,Y_c）及び角度偏差_ｃを発生し、このうち
_ｃは、第（２）式によって像変換部32で立体像P_iに変換
される。この時に、_１〜_ｎに対するゲインK_iは正
に、一方、_ｃに対するゲインK_iは負にとると、第８図
に示す如く仮想空間ISでは、障害物OB1,OB2の像P_a,P_bは
Ｐ軸の正方向の高さをもつ正規分布形状をなし、一方、
目標位置（X_r,Y_r）の像、即ち位置誤差による像P_cはＰ
軸の負方向に高さをもつ正規分布形状をなす。従って、
像演算部33で第（３）式でこれらの和をとると、第８図
のポテンシャル面が得られ、仮想空間ISでは、障害物の
存在する位置にはK_i＞０となる凸部分を生成し、反対に
目標位置にはK_i＜０となる凹部分が生成される。

このことは、後述する並進及び姿勢制御量によって、
ロボット１が仮想空間IS上のポテンシャル面に沿って高
い方から低い方に向って転がっていくことになり、結局
障害物を避けた移動軌道IMが形成されており、実空間で
係る軌道IMに対応する軌道RMに沿って目標位置に向って
並進及び姿勢制御され移動する。

このため、換算部34においては、第（13）式，第（1
4）式によって、曲面斜度α_i,β_ｉを求め、第（７）
式，第（８）式によって曲がり加速度_ri,_riを求め
て、第（９）式によって反発力F_iを得る。

加算部34gでは反発力F_iの和がとられ、並進制御力Ｆ
がえられ、同様に加算部34eでは反発力F_iと取付けベク
トル_ｉとの外積の和がとられ、第１のモーメントM₁が
得られる。

上記の仮想吸引力において、制御偏差発生部31からの
制御位置偏差_ｃが、仮想吸引力発生部35で位置偏差
_ｃに比例するバネ力とに比例する粘性力を発生し、これ
から誘導力を発生している。加算部34hで仮想重力によ
る並進制御力と合成し、合成したものを積分部34hで1/M
して積分して、制御量として_r,_ｒを求め、これを並
進速度指令としてロボット１のサーボ系（図示せず）に
与える。

これによってロボット１は軌道RM（第８図参照）に沿
って障害物を避けながら、目的位置（X_r,Y_r）に向って
移動することになる。

ここで、仮想空間IS上でも、位置偏差に応じてポテン
シャル面（第８図の像P_c）が形成されており、目標位置
への誘導力は零ではない。従って、誘導制御量（仮想吸
引力）を発生しなくてもロボット１は目標位置に誘導さ
れるといえる。

しかし、位置偏差が大の場合には、ロボットが目標へ
向おうとする力は非常に小さくなることから、ロボット
を誘導することが難しくなる。

そこで、目標位置の像P_cの影響を仮想平面IS（第８
図）で大きくしようとして、目標位置に生成する局所関
数（第（２）式）の形状を変更すると、安定に誘導制御
を行うには、位置制御偏差やロボットの速度によって、
動的な形状変更を必要とされ、制御態様の複雑化を招
き、誘導特性を所望する特性に設定することが難しい。

このため、第７図に示す如く、位置偏差に比例するバ
ネ力と偏差速度に比例する粘性力を発生し、この２つの
値によって位置偏差に対する補償と、誘導速度に対する
補償を行うようにしたものである。

これによって初期制御偏差が大きくても、適度な接近
速度を保ちながら、確実に誘導制御できる。これととも
に前述のバネ定数や粘性係数Ｃを調整することによっ
て、誘導特性を変更できる。

また、加算部34eで第１のモーメントM₁と第２のモー
メントM₂,第３のモーメントM₃を合成し、合成したもの
を積分部34fでm/I倍して積分し、制御量として_ｒを求
め、同様に回転速度指令としてロボット１のサーボ系に
与える。

これらは、全てロボット制御装置のプロセッサのプロ
グラムによる演算によって実現でき、仮想空間ISは概念
上では存在するが、結局第（２）式，第（３）式で示す
値が仮想空間ISでのポテンシャル面を定義することにな
り、単なるデータで示される。

従って、プロセッサは、各センサ21〜2nの出力を座標
変換し、更に位置誤差を求めて、第（13）式を実行し、
X,Y方向の傾きを求め、更に第（14）式で曲面斜度α，
βを演算し、第（７）式，第（８）式で加速度_ri,
_riを演算し、且つ上述した演算を行ない並進速度_r,
_ｒ及び回転速度_ｒを各サンプリング時刻毎にサーボ系
に出力すればよいことになる。このような制御を行なう
ことによって、第９図乃至第11図に示すような動作が実
現できる。

第９図において、ロボット１は障害物OBを回避しつ
つ、出発位置Ｓから目標位置Ｒまで移動するが、出発位
置Ｓにおいてロボット１はＹ方向正側を向いており、目
標位置ＲにおいてはＸ方向正側を向いて停止すべく、上
述の方式では制御した場合のシミュレーションである。
この例において、ロボット１は障害物のまわりを障害物
に対して側部を向けた適正な姿勢で回避して、目標位置
Ｒにおいて目標姿勢になって停止している。

第10図は目標位置P_rでのロボット１の目標姿勢をＹ方
向負側を向くよう設定してシミュレーションしたもの
で、第９図に示した場合と同様に良好な姿勢制御が行な
われる。

第11図は、障害物OBを２ケ所に設け、ロボット１の出
発位置Ｓでの姿勢をＹ軸に対して30゜傾けた状態とし、
目標位置P_rにおいて、Ｙ方向正側を向くように設定して
シミュレーションしたものである。この例においてもロ
ボットは２つの障害物OBの中間地点を両障害物に側面を
向けた適正な姿勢で通過して、目標位置の近傍で方向を
変えて、目標位置においては、目標姿勢となって停止す
るように姿勢制御が行なわれる。

このように本実施例によれば、目標位置から離れてい
るときはロボット１を目標姿勢とさせる第２のモーメン
トM₂の大きさは小さいものであるため、ロボットの姿勢
は主として障害物回避のための第１のモーメントM₁によ
って決定される。そして、ロボット１の目標位置に近づ
くと、第２のモーメントが大きくはたらき、目標位置が
たとえ障害物に近くとき第２のモーメントM₂が作用し
て、ロボットを目標位置において、目標姿勢にして停止
させることができる。

（３）他の実施例の説明 上述の実施例では、仮想間の反発力から並進変位と姿
勢制御の両方の制御量を求めているが、姿勢制御量のみ
を求め、並進変位制御量は他の方法によって求めるよう
にしてもよい。

又、上述の実施例では、センサとして超音波距離セン
サを用いているが他の周知の距離センサを用いてもよ
い。

更に、移動ロボットに限らず、アームを有する作業ロ
ボットの作業にも適用でき、例えば、ハンドを目標位置
に移動させる場合や、ハンドの把持した物品を相手物品
に嵌合させる作業や、物体の倣い動作を行なう作業等に
も適用でき、これら作業に応じてセンサを適切な他の力
センサや温度センサ等を用いることもできる。

〔発明の効果〕

以上説明したように、本発明によれば、ロボットの姿
勢を外的センサが外的環境に基づいて求めたポテンシャ
ル量からロボットに作用する反発力として回転自由度に
加わる第１のモーメントと、ロボットの到達目標位置に
おけるロボットの目標姿勢と現姿勢との偏差及び到達目
標位置までの距離とに基づいて求めた第２のモーメント
と、ロボットの前記回転自由度の回転角速度に基づいて
求めた第３のモーメントとに基づいて該ロボットを適応
制御するロボットの姿勢制御方法において、第２のモー
メントの大きさを決定する係数を到達目標位置までの関
数とし、この関数を目標位置を離れるにつれて減少する
ようにしたから、ロボットを目標位置から離れた地点に
おいては障害物を回避するのに最適な姿勢で通過でき、
また目標位置において、指令された姿勢で停止させるこ
とができ、ロボットを良好に制御することができるとい
う効果を奏する。

【図面の簡単な説明】

第１図は本発明の原理説明図、第２図は本発明の一実施
例説明図、第３図は第２図における実空間と仮想空間の
関係図、第４図及び第５図は第２図における反発力換算
説明図、第６図は仮想空間による動作説明図、第７図は
本発明に一実施例ブロック図、第８図は第７図による動
作説明図、第９図乃至第11図は本発明の実施例になるシ
ュミレーションの結果を示す図、第12図（ａ），
（ｂ），（ｃ）は従来のロボットの姿勢制御方式を示す
説明図、第13図及び第14図は従来のロボットの姿勢制御
方式によるシュミレーションの結果を示す図である。 １……ロボット 21,22,〜2n……センサ M₁……第１のモーメント M₂……第２のモーメント

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】姿勢を変更可能な回転自由度を有するロボ
   ット（１）、 該ロボット（１）の外的環境を検出するセンサ（22），
   （23），……，（2n）、 該センサにより検出された外的環境に基づき適応制御さ
   れる前記ロボット（１）の前記姿勢を制御する方法であ
   って、 前記外的環境に基づいてポテンシャル量を求め、該ポテ
   ンシャル量を前記ロボットに作用する反発力として前記
   回転自由度に加わる第１のモーメント（M₁）を求め、 該ロボット（１）の到達目標位置におけるロボット
   （１）の目標姿勢（φ_ｒ）と現姿勢（φ）との偏差及び
   到達目標位置までの距離（R_c）とに基づいて第２のモー
   メント（M₂）を求め、 前記ロボット（１）の前記回転自由度の回転角速度（φ
   /dt）に基づいて第３のモーメント（M₃）を求め、 前記第１〜第３のモーメント（M₁,M₂,M₃）に基づいて該
   ロボットを適応制御するロボットの姿勢制御方法におい
   て、 前記第２のモーメント（M₂）の大きさを決定する係数を
   到達目標位置までの距離（R_c）の関数（κ（R_c））と
   し、 この関数（κ（R_c））を目標位置を離れるにつれて減少
   するようにしたことを特徴とするロボットの姿勢制御方
   法。