JPH0656565B2 - マニピユレ−タ制御装置 - Google Patents
マニピユレ−タ制御装置Info
- Publication number
- JPH0656565B2 JPH0656565B2 JP61267288A JP26728886A JPH0656565B2 JP H0656565 B2 JPH0656565 B2 JP H0656565B2 JP 61267288 A JP61267288 A JP 61267288A JP 26728886 A JP26728886 A JP 26728886A JP H0656565 B2 JPH0656565 B2 JP H0656565B2
- Authority
- JP
- Japan
- Prior art keywords
- point
- trajectory
- auxiliary
- distance
- arc
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 （産業上の利用分野） 本発明はマニピュレータを制御する装置に係り、特に円
弧軌道を含んで目標位置に移動する場合に円弧軌道と円
滑に接続される動作を行うように改良した制御装置に関
する。
弧軌道を含んで目標位置に移動する場合に円弧軌道と円
滑に接続される動作を行うように改良した制御装置に関
する。
（従来の技術及び解決しようとする問題点） 第７図は出発点Ｐｓから経由点Ｐｍを経由して到着点Ｐ
Ｅに移動する場合の説明図である。以下、出発点Ｐｓ若
しくは現在位置Ｐｓから経由点Ｐｍまでの軌道を前軌
道、経由点Ｐｍから到着点ＰＥ若しくは目標点ＰＥまで
の軌道を後軌道という。
Ｅに移動する場合の説明図である。以下、出発点Ｐｓ若
しくは現在位置Ｐｓから経由点Ｐｍまでの軌道を前軌
道、経由点Ｐｍから到着点ＰＥ若しくは目標点ＰＥまで
の軌道を後軌道という。
従来は前軌道と後軌道とを円滑に接続する制御が困難な
ので、経由点Ｐｍで一旦停止し、再び後軌道上を移動さ
せていた。
ので、経由点Ｐｍで一旦停止し、再び後軌道上を移動さ
せていた。
しかし停止を伴うので、動作時間が長くなると共にきめ
細い動作制御が行えない問題点があった。
細い動作制御が行えない問題点があった。
また円弧を含む前後軌道において、経由点Ｐｍの近傍で
両軌道に接する円弧で接続させることも行われている
が、接続点で曲率が急変するためマニピュレータに大き
な力が作用して無用の振動等を発生させることがあっ
た。
両軌道に接する円弧で接続させることも行われている
が、接続点で曲率が急変するためマニピュレータに大き
な力が作用して無用の振動等を発生させることがあっ
た。
（発明の目的） 本発明はこのような問題点を解決したもので、経由点近
傍での経由動作を円滑に行うと共に、経由動作に接続す
る動作が直線軌道及び円弧軌道であっても効率のよいマ
ニピュレータの運動を行わせる制御装置を提供すること
を目的とする。
傍での経由動作を円滑に行うと共に、経由動作に接続す
る動作が直線軌道及び円弧軌道であっても効率のよいマ
ニピュレータの運動を行わせる制御装置を提供すること
を目的とする。
（問題点を解決するための手段） このような目的を達成する本発明は、マニピュレータの
現在位置より、外部より指定された経由点を経由して外
部より指定された目標点に移動するものであって、前軌
道と後軌道の少なくとも一方に円弧軌道を含むものに関
する。
現在位置より、外部より指定された経由点を経由して外
部より指定された目標点に移動するものであって、前軌
道と後軌道の少なくとも一方に円弧軌道を含むものに関
する。
比較手段は前軌道と後軌道のいずれか短い方の移動距離
が、所定の最高速度で移動するに足るものであるかを静
止距離と比較して判断する。
が、所定の最高速度で移動するに足るものであるかを静
止距離と比較して判断する。
補助点演算手段は移動距離が最高速度に達するためには
短すぎる場合は経由点から該移動距離の半分だけ離れた
前軌道側に第１の補助点，後軌道側に第２の補助点を選
定する。移動距離が静止距離よりも長い場合は経由点か
ら静止距離だけ離れた前軌道側に第１の補助点，後軌道
側に第２の補助点を選点する。前軌道が直線の場合は第
３の補助点を第１の補助点と経由点との中点に選定し、
前軌道が円弧の場合は第１の経由点で該円弧に接する直
線上で、該円弧上における第１の経由点と経由点との距
離の半分に相当する距離第１の経由点から離れた点を第
３の補助点として選定する。後軌道が直線の場合は第４
の補助点を第２の補助点と経由点との中点に選定し、後
軌道が円弧の場合は第２の経由点で該円弧に接する直線
上で、該円弧上における第２の経由点と経由点との距離
の半分に相当する距離第２の経由点から離れた点を第４
の補助点として選定する。
短すぎる場合は経由点から該移動距離の半分だけ離れた
前軌道側に第１の補助点，後軌道側に第２の補助点を選
定する。移動距離が静止距離よりも長い場合は経由点か
ら静止距離だけ離れた前軌道側に第１の補助点，後軌道
側に第２の補助点を選点する。前軌道が直線の場合は第
３の補助点を第１の補助点と経由点との中点に選定し、
前軌道が円弧の場合は第１の経由点で該円弧に接する直
線上で、該円弧上における第１の経由点と経由点との距
離の半分に相当する距離第１の経由点から離れた点を第
３の補助点として選定する。後軌道が直線の場合は第４
の補助点を第２の補助点と経由点との中点に選定し、後
軌道が円弧の場合は第２の経由点で該円弧に接する直線
上で、該円弧上における第２の経由点と経由点との距離
の半分に相当する距離第２の経由点から離れた点を第４
の補助点として選定する。
経由軌道演算手段は、第１の補助点で前軌道に滑らかに
接し、第３の補助点、経由点並びに第４の補助点の近傍
を通過して、第２の補助点で後軌道に滑らかに接する所
定の近似スプライン曲線で定められる経由軌道を演算し
ている。
接し、第３の補助点、経由点並びに第４の補助点の近傍
を通過して、第２の補助点で後軌道に滑らかに接する所
定の近似スプライン曲線で定められる経由軌道を演算し
ている。
（作用） 上述の近似スプライン曲線は第１から第４の補助点及び
経由点を用いて決定され、第１の補助点と第２の補助点
で同一速度で加速度零になっている。そこで、前軌道か
ら経由軌道に入るのが円滑に進行すると共に経由軌道か
ら後軌道に入る際にも無用の力を発生させない。
経由点を用いて決定され、第１の補助点と第２の補助点
で同一速度で加速度零になっている。そこで、前軌道か
ら経由軌道に入るのが円滑に進行すると共に経由軌道か
ら後軌道に入る際にも無用の力を発生させない。
比較手段は第１及び第２の補助点におけるマニピュレー
タの移動速度を間接的に算出し、補助点演算手段は現在
位置，指定経由点，指定目標点及び指定最高速度に適合
させた経由軌道を演算するための補助点を算定する。
タの移動速度を間接的に算出し、補助点演算手段は現在
位置，指定経由点，指定目標点及び指定最高速度に適合
させた経由軌道を演算するための補助点を算定する。
（実施例） 以下図面を用いて本発明を説明する。
第１図は本発明の一実施例を示す構成ブロック図であ
る。図において、１０は例えば６自由度の関節機構より
なるマニピュレータ、２０はマニピュレータ１０の動作
に必要な情報を送信するホストコンピュータで、目標位
置ＰＥと経由位置Ｐｍ及び軌道が円弧か直線かを指定す
る。３０はマニピュレータ１０の現在位置Ｐｓを検出す
る位置検出器で、例えば関節の各回転角をエンコーダを
用いて検出して演算により求めている。４０はマニピュ
レータ１０の位置制御を行うコントローラ、５０はコン
トローラ４０の指令に応じてマニピュレータ１０を移動
させる駆動回路である。
る。図において、１０は例えば６自由度の関節機構より
なるマニピュレータ、２０はマニピュレータ１０の動作
に必要な情報を送信するホストコンピュータで、目標位
置ＰＥと経由位置Ｐｍ及び軌道が円弧か直線かを指定す
る。３０はマニピュレータ１０の現在位置Ｐｓを検出す
る位置検出器で、例えば関節の各回転角をエンコーダを
用いて検出して演算により求めている。４０はマニピュ
レータ１０の位置制御を行うコントローラ、５０はコン
トローラ４０の指令に応じてマニピュレータ１０を移動
させる駆動回路である。
R41は位置検出器３０の出力する現在位置Ｐｓを記憶す
るレジスタ、R42はホストコンピュータ１０より指令さ
れる経由点に関する情報を記憶するレジスタで、直線運
動が指令されると経由点Ｐｍが記憶されるが、円弧運動
が指令されると経由点Ｐｍ及び他の一点が記憶されて円
弧軌道の中心Ｏ１及び半経Ｒ１が決定しうるようになっ
ている。R43はホストコンピュータ１０より指令される
目標点に関する情報を記憶するレジスタで、直線運動が
指定されると目標点ＰＥが記憶されるが、円弧運動が指
定されると目標点ＰＥ及び他の一点が記憶されて円弧軌
道の中心Ｏ２及び半径Ｒ２が決定しうるようになってい
る。尚、マニピュレータ１０の動作に固有の所定最大速
度Ｖmax及びこの最大速度Ｖmaxから静止するまでに必要
な静止距離ｌは別のレジスタに記憶され、適宜呼び出し
て使用する。
るレジスタ、R42はホストコンピュータ１０より指令さ
れる経由点に関する情報を記憶するレジスタで、直線運
動が指令されると経由点Ｐｍが記憶されるが、円弧運動
が指令されると経由点Ｐｍ及び他の一点が記憶されて円
弧軌道の中心Ｏ１及び半経Ｒ１が決定しうるようになっ
ている。R43はホストコンピュータ１０より指令される
目標点に関する情報を記憶するレジスタで、直線運動が
指定されると目標点ＰＥが記憶されるが、円弧運動が指
定されると目標点ＰＥ及び他の一点が記憶されて円弧軌
道の中心Ｏ２及び半径Ｒ２が決定しうるようになってい
る。尚、マニピュレータ１０の動作に固有の所定最大速
度Ｖmax及びこの最大速度Ｖmaxから静止するまでに必要
な静止距離ｌは別のレジスタに記憶され、適宜呼び出し
て使用する。
A41はレジスタR41，R42，R44から位置情報を受けとり、
現在位置から経由点までの軌道上における距離｜ＰｓＰ
ｍ｜と経由点から目標点までの軌道上における距離｜Ｐ
ｍＰＥ｜のいずれか短い移動距離を選択する距離比較手
段で、この移動距離Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ）
と静止距離ｌの２倍との大小の比較を行うので、全体と
して次の処理をする。
現在位置から経由点までの軌道上における距離｜ＰｓＰ
ｍ｜と経由点から目標点までの軌道上における距離｜Ｐ
ｍＰＥ｜のいずれか短い移動距離を選択する距離比較手
段で、この移動距離Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ）
と静止距離ｌの２倍との大小の比較を行うので、全体と
して次の処理をする。
Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ｜，２ｌ）
(1) A42は経由軌道の計算に必要な補助点を演算する補助点
演算手段である。この補助点の選定を第２図及び第３図
に基づいて説明する。第２図は補助点の選定の規準を前
軌道と後軌道の長さ及び軌道の形状によって区分してあ
らわしたものである。図中前後軌道がいずれも直線の場
合は本実施例をそのまま用いてもよいが、通常はもって
簡便な方法を用いており、ここでは円弧軌道を含む場合
の共通条件を表示し、その下の欄で各組合せに特有の条
件をあらわしている。第３図は円弧−円弧軌道の組合せ
を用いた、具体例である。尚図中Ｐ２＝Ｐｍとする。補
助点演算手段A42は前軌道上に経由点Ｐｍから次の距離
離れた点に第１の補助点Ｐ０を選定し、後軌道上に経由
点Ｐｍから第１の補助点Ｐ０と同じ条件の距離離れた点
に第２の補助点Ｐ４を選定する。この距離は、 Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ｜）≧２ｌ であるときは、静止距離ｌとし、 Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ｜）＜２ｌ であるときは移動距離Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ
｜）の半分とする。次に第１の補助点Ｐ０と経由点Ｐｍ
との間に第３の補助点Ｐ１を定める。前軌道が直線であ
る場合は第１の補助点Ｐ０と経由点Ｐｍの中点を第３の
補助点Ｐ１とする。前軌道が円弧である場合は第１の補
助点Ｐ０において円弧に接するように第３の補助点Ｐ１
の方向を定める。
(1) A42は経由軌道の計算に必要な補助点を演算する補助点
演算手段である。この補助点の選定を第２図及び第３図
に基づいて説明する。第２図は補助点の選定の規準を前
軌道と後軌道の長さ及び軌道の形状によって区分してあ
らわしたものである。図中前後軌道がいずれも直線の場
合は本実施例をそのまま用いてもよいが、通常はもって
簡便な方法を用いており、ここでは円弧軌道を含む場合
の共通条件を表示し、その下の欄で各組合せに特有の条
件をあらわしている。第３図は円弧−円弧軌道の組合せ
を用いた、具体例である。尚図中Ｐ２＝Ｐｍとする。補
助点演算手段A42は前軌道上に経由点Ｐｍから次の距離
離れた点に第１の補助点Ｐ０を選定し、後軌道上に経由
点Ｐｍから第１の補助点Ｐ０と同じ条件の距離離れた点
に第２の補助点Ｐ４を選定する。この距離は、 Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ｜）≧２ｌ であるときは、静止距離ｌとし、 Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ｜）＜２ｌ であるときは移動距離Min（｜ＰｓＰｍ｜，｜ＰｍＰＥ
｜）の半分とする。次に第１の補助点Ｐ０と経由点Ｐｍ
との間に第３の補助点Ｐ１を定める。前軌道が直線であ
る場合は第１の補助点Ｐ０と経由点Ｐｍの中点を第３の
補助点Ｐ１とする。前軌道が円弧である場合は第１の補
助点Ｐ０において円弧に接するように第３の補助点Ｐ１
の方向を定める。
Ｏ１Ｐ１⊥Ｐ０Ｐ１
(2) 第３の補助点Ｐ１の第１の補助点Ｐ０からの距離は、円
弧軌道上における第１の補助点Ｐ０と経由点Ｐｍとの距
離の半分にとる。最後に第２の補助点Ｐ４と経由点Ｐｍ
との間に第４の補助点Ｐ３を定める。後軌道が直線であ
る場合は第２の補助点Ｐ０と経由点Ｐｍの中点を第４の
補助点Ｐ３とする。後軌道が円弧である場合は第２の補
助点Ｐ４において円弧に接するように第４の補助点Ｐ３
の方向を定める。
(2) 第３の補助点Ｐ１の第１の補助点Ｐ０からの距離は、円
弧軌道上における第１の補助点Ｐ０と経由点Ｐｍとの距
離の半分にとる。最後に第２の補助点Ｐ４と経由点Ｐｍ
との間に第４の補助点Ｐ３を定める。後軌道が直線であ
る場合は第２の補助点Ｐ０と経由点Ｐｍの中点を第４の
補助点Ｐ３とする。後軌道が円弧である場合は第２の補
助点Ｐ４において円弧に接するように第４の補助点Ｐ３
の方向を定める。
Ｏ２Ｐ４⊥Ｐ３Ｐ４
(3) 第４の補助点Ｐ３の第２の補助点Ｐ４からの距離は、円
弧軌道上における第２の補助点Ｐ４と経由点Ｐｍとの距
離の半分にとる。
(3) 第４の補助点Ｐ３の第２の補助点Ｐ４からの距離は、円
弧軌道上における第２の補助点Ｐ４と経由点Ｐｍとの距
離の半分にとる。
A43は前軌道における現在位置Ｐｓから第１の補助点Ｐ
０までと、後軌道における第２の補助点Ｐ４から目標点
ＰＥまでの速度パターンを演算する速度演算手段で、例
えば時間に関する三次式を用いて位置Ｐｓ，Ｐｏ，
Ｐ４，ＰＥにおける加速度を零にしている。
０までと、後軌道における第２の補助点Ｐ４から目標点
ＰＥまでの速度パターンを演算する速度演算手段で、例
えば時間に関する三次式を用いて位置Ｐｓ，Ｐｏ，
Ｐ４，ＰＥにおける加速度を零にしている。
A44は補助点演算手段A42で求めた点Ｐｏ，Ｐ１，Ｐ
２（＝Ｐｍ）、Ｐ３，Ｐ４を用いて点Ｐｏ，Ｐ４で同一
速度かつ加速度零で移動する経由軌道を演算する経由軌
道演算手段で、例えば８次のBezier（ベジュエ）曲線な
どの８次のスプライン曲線を定める。
２（＝Ｐｍ）、Ｐ３，Ｐ４を用いて点Ｐｏ，Ｐ４で同一
速度かつ加速度零で移動する経由軌道を演算する経由軌
道演算手段で、例えば８次のBezier（ベジュエ）曲線な
どの８次のスプライン曲線を定める。
A45は前軌道では速度演算手段A43、経由軌道では経由軌
道演算手段A44、後軌道では速度演算手段A43を用いて軌
道を出力する選択器で、駆動回路５０に出力している。
道演算手段A44、後軌道では速度演算手段A43を用いて軌
道を出力する選択器で、駆動回路５０に出力している。
第４図は第１図の装置によるマニピュレータ１０の動作
説明図で、ここでは前軌道が直線、後軌道が円弧の場合
を示している。第４図において、(イ)は経由点Ｐｍの両
側に等速区間が存在する場合、(ロ)は経由点Ｐｍの現在
位置Ｐｓ側に等速区間が存在する場合、(ハ)は経由点Ｐ
ｍの目標位置ＰＥ側に等速区間が存在する場合、(ニ)は
経由点Ｐｍの両側に等速区間が存在しない場合を示し、
以下これらを分説する。
説明図で、ここでは前軌道が直線、後軌道が円弧の場合
を示している。第４図において、(イ)は経由点Ｐｍの両
側に等速区間が存在する場合、(ロ)は経由点Ｐｍの現在
位置Ｐｓ側に等速区間が存在する場合、(ハ)は経由点Ｐ
ｍの目標位置ＰＥ側に等速区間が存在する場合、(ニ)は
経由点Ｐｍの両側に等速区間が存在しない場合を示し、
以下これらを分説する。
図において、Ｐ５は現在位置Ｐｓ又は経由点Ｐｍから加
速して最大速度Ｖmaxに達する位置、Ｐ６は最大速度Ｖm
axから減速して目標点ＰＥ又は経由点Ｐｍで停止する減
速開始位置、Ｐｄは減速で開始する位置で、その後曲線
軌道に入る。Ｐｄは最大速度Ｖmaxよりも小さい最大速
度に達する点である。
速して最大速度Ｖmaxに達する位置、Ｐ６は最大速度Ｖm
axから減速して目標点ＰＥ又は経由点Ｐｍで停止する減
速開始位置、Ｐｄは減速で開始する位置で、その後曲線
軌道に入る。Ｐｄは最大速度Ｖmaxよりも小さい最大速
度に達する点である。
(イ)経由点の両側の等速区間が存在する場合、 現在位置Ｐｓから位置Ｐ５までの間で最大速度Ｖmaxま
で加速される。この速度及び移動距離は例えば次式で与
えられる。
で加速される。この速度及び移動距離は例えば次式で与
えられる。
ここに、Ｔｓ１は加速時間で加速度の最大値をＡmaxと
すると、 Ｔｓ１＝（３／２）・Ｖmax／Ａmax となり、加速に要する距離ｌは次式で与えられる。
すると、 Ｔｓ１＝（３／２）・Ｖmax／Ａmax となり、加速に要する距離ｌは次式で与えられる。
位置Ｐ５から位置Ｐ０までは速度Ｖmaxの等速運動を行
う。
う。
接続点Ｐ０から接続点Ｐ４までは、８次のスプライン曲
線のうちで８次Bezier曲線で表わせる軌道（点Ｐ０，Ｐ
１，Ｐｍ，Ｐ３，Ｐ４及びこれらの中点の合計９点で定
められるが、中点の４つは５つの与点で決定できるので
結局補助点演算手段４２で求めた補助点４つ及び経由点
Ｐｍにより定まる。）上を次の如く移動する。
線のうちで８次Bezier曲線で表わせる軌道（点Ｐ０，Ｐ
１，Ｐｍ，Ｐ３，Ｐ４及びこれらの中点の合計９点で定
められるが、中点の４つは５つの与点で決定できるので
結局補助点演算手段４２で求めた補助点４つ及び経由点
Ｐｍにより定まる。）上を次の如く移動する。
Ｐ(t)＝Ｋ１(t／T)８ ＋Ｋ２(t／T)７ ＋Ｋ３(t／T)６ ＋Ｋ４(t／T)５ ＋Ｋ５(t／T)４ ＋Ｋ６(t／T)３ ＋Ｋ７(t／T)＋Ｐ０(０≦ｔ≦Ｔ) (7) ここで、 Ｔ＝４｜Ｐ０−Ｐ１｜／Ｖ０ (8) とする。ここにＶ０は第１の補助点Ｐ０における初速度
である。
である。
即ち、経由点Ｐｍで停止せずに等速でＰ０，Ｐｍ，Ｐ４
と移動するのと同一の所要時間でＰ０からＰ４を経由動
作する。また、 ｜Ｐ′（０）｜＝｜Ｐ′（Ｔ）｜＝Ｖmax
(9) Ｐ″（０）＝Ｐ″（Ｔ）＝０
(10) であるから、接続点Ｐ０，Ｐ４で加速度零で、速度は滑
らかに接続されていると共に速度の絶対値は等しくなっ
ている。
と移動するのと同一の所要時間でＰ０からＰ４を経由動
作する。また、 ｜Ｐ′（０）｜＝｜Ｐ′（Ｔ）｜＝Ｖmax
(9) Ｐ″（０）＝Ｐ″（Ｔ）＝０
(10) であるから、接続点Ｐ０，Ｐ４で加速度零で、速度は滑
らかに接続されていると共に速度の絶対値は等しくなっ
ている。
位置Ｐ４から位置Ｐ６までは速度Ｖmaxの等速運動を行
う。
う。
位置Ｐ６から目標位置ＰＥまでの間で最大速度Ｖmaxか
ら減速されて停止する。この速度及び移動距離は、次式
で与えられる。
ら減速されて停止する。この速度及び移動距離は、次式
で与えられる。
(ロ)経由点の現在位置側に等速区間が存在する場合 現在位置Ｐｓから加速し距離ｌ離れた位置Ｐ５で最高速
度Ｖmaxに達し、位置Ｐ６で減速を開始し経由軌道Ｐ０
−Ｐ４を経て目標位置ＰＥで停止する。
度Ｖmaxに達し、位置Ｐ６で減速を開始し経由軌道Ｐ０
−Ｐ４を経て目標位置ＰＥで停止する。
位置Ｐ０，Ｐ４における速度ＶＥは、区間Ｐ４ＰＥの長
さによって定められる。区間Ｐ４ＰＥの運動は、 で表わされる。ここに Ｔｓ３＝(３／２)・ＶＥ／Ａmax になっている。これから、 ｜Ｐ４−ＰＥ｜＝(３／４)・▲Ｖ2 E▼／Ａmax となり、速度ＶＥは次式で与えられる。
さによって定められる。区間Ｐ４ＰＥの運動は、 で表わされる。ここに Ｔｓ３＝(３／２)・ＶＥ／Ａmax になっている。これから、 ｜Ｐ４−ＰＥ｜＝(３／４)・▲Ｖ2 E▼／Ａmax となり、速度ＶＥは次式で与えられる。
区間Ｐ０Ｐ４では、(7)式に準じた８次Bezier曲線で移
動する。減速開始位置Ｐ６は次の如く定める。区間Ｐ６
Ｐ０では次式で運動する。
動する。減速開始位置Ｐ６は次の如く定める。区間Ｐ６
Ｐ０では次式で運動する。
従って区間Ｐ６Ｐ０の長さは次式で与えられる。
(ハ)経由点の目標位置側に等速区間が存在する場合 現在位置Ｐｓから位置Ｐ０まで加速し速度Ｖｓに至る。
この運動は次式で与えられる。
この運動は次式で与えられる。
ここで速度Ｖｓは(18)式を積分して、 で与えられる。
区間Ｐ０Ｐ４では速度Ｖｓの(7)式に準じた８次Bezier
曲線で移動する。区間Ｐ４Ｐ５では、速度Ｖｓから加速
して最高速度Ｖmaxに至る。この区間の運動は次式で与
えられる。
曲線で移動する。区間Ｐ４Ｐ５では、速度Ｖｓから加速
して最高速度Ｖmaxに至る。この区間の運動は次式で与
えられる。
この式を積分すると、区間長さは次の如くなる。
位置Ｐ５からＰ６までは等速運動がなされ、その後減速
して目標位置ＰＥで停止するのは(イ)と同じである。
して目標位置ＰＥで停止するのは(イ)と同じである。
(ニ)経由点の両側に等速区間がない場合 移動距離が短いために許容された加速度Ａmaxでは最大
速度に達しない場合である。この場合は、(ロ)及び(ハ)の
等速区間を含まない側の動作を組合せたものとなる。
速度に達しない場合である。この場合は、(ロ)及び(ハ)の
等速区間を含まない側の動作を組合せたものとなる。
尚、区間の変わり目は動作開始からの経過時間又は移動
距離で判定できる。
距離で判定できる。
第５図は８次Bezier曲線上を移動する速度と発生する加
速度を示したものである。(9)式に示す如く速度は前後
軌道となめらかに接続されると共に経由軌道上では前後
軌道上の移動速度より小さくなっている。(10)式に示す
如く、曲線上を移動するため加速度が作用するが接続点
では零になっている。
速度を示したものである。(9)式に示す如く速度は前後
軌道となめらかに接続されると共に経由軌道上では前後
軌道上の移動速度より小さくなっている。(10)式に示す
如く、曲線上を移動するため加速度が作用するが接続点
では零になっている。
尚、上記実施例においては経由点が一つの場合を示して
いるが、複数であっても各経由位置に隣接する点を現在
位置及び目標位置とみなして曲線軌道を演算すればよ
く、速度パターンを停止ではなく軌道の条件に応じて適
宜設定すればよい。
いるが、複数であっても各経由位置に隣接する点を現在
位置及び目標位置とみなして曲線軌道を演算すればよ
く、速度パターンを停止ではなく軌道の条件に応じて適
宜設定すればよい。
また、曲線として８次のBezier曲線を示しているが、曲
線が０次，１次の微係数が連続で、２次の微係数が零で
直線と接続される曲線であれば他の近似スプライン曲線
でもよく、また９次以上の多項式であらわしてもよい。
尚、ここで近似スプライン曲線とは、制御点として与え
られた点列Ｐ０、…、Ｐ４を必ずしも通らないもので、
この点列を通過する内挿スプライン曲線とは異なる。
線が０次，１次の微係数が連続で、２次の微係数が零で
直線と接続される曲線であれば他の近似スプライン曲線
でもよく、また９次以上の多項式であらわしてもよい。
尚、ここで近似スプライン曲線とは、制御点として与え
られた点列Ｐ０、…、Ｐ４を必ずしも通らないもので、
この点列を通過する内挿スプライン曲線とは異なる。
第６図は円弧軌道の始点と終点との間を補間して、軌道
上の移動距離の求め方を説明するものである。図におい
て、Ｏ１は中心、Ｒ１は半径、Ｐ０は始点、Ｐ１は終
点、Ｐ(t)は円弧軌道上の補間される点、 は円弧のなす平面の法線ベクトルである。点Ｐ０を中心
Ｏ１を通る法線まわりにθ１まわすと点Ｐ１に一致す
る。
上の移動距離の求め方を説明するものである。図におい
て、Ｏ１は中心、Ｒ１は半径、Ｐ０は始点、Ｐ１は終
点、Ｐ(t)は円弧軌道上の補間される点、 は円弧のなす平面の法線ベクトルである。点Ｐ０を中心
Ｏ１を通る法線まわりにθ１まわすと点Ｐ１に一致す
る。
円弧上の移動距離ｄ(t)は直線補間の式(12)と同じよう
にして設定できる。
にして設定できる。
ｄ(t)＝-0.5×VsTs1(t／Ts1)4＋VsTs1(t／Ts1)3
(21) これにより回転角度θ(t)は次式であらわせる。
(21) これにより回転角度θ(t)は次式であらわせる。
θ(t)＝ｄ(t)／Ｒ (2
2) そこで、円弧上の任意の点Ｐ(t)は、 Ｐ(t)＝Ｒ（１，θ(t)）・Ｐ０ (23) であらわせる。ここにＲ（１，θ(t)）は公知の一般
化回転マトリクスで、１ ｔ＝（ｎ１，ｎ２，ｎ３）と
して、次式であらわせる。
2) そこで、円弧上の任意の点Ｐ(t)は、 Ｐ(t)＝Ｒ（１，θ(t)）・Ｐ０ (23) であらわせる。ここにＲ（１，θ(t)）は公知の一般
化回転マトリクスで、１ ｔ＝（ｎ１，ｎ２，ｎ３）と
して、次式であらわせる。
ｒ１１＝ｎ１ ２＋（１−ｎ１ ２）ｃｏｓθ(t) ｒ１２＝ｎ１ｎ２（１−ｃｏｓθ(t)）−ｎ３ｓｉｎθ
(t) ｒ１３＝ｎ１ｎ３（１−ｃｏｓθ(t)）＋ｎ２ｓｉｎθ
(t) ｒ２１＝ｎ１ｎ２（１−ｃｏｓθ(t)）＋ｎ３ｓｉｎθ
(t) ｒ２２＝ｎ２ ２＋（１−ｎ２ ２）ｃｏｓθ(t) ｒ２３＝ｎ２ｎ３（１−ｃｏｓθ(t)）−ｎ１ｓｉｎθ
(t) ｒ３１＝ｎ１ｎ３（１−ｃｏｓθ(t)）−ｎ２ｓｉｎθ
(t) ｒ３２＝ｎ２ｎ３（１−ｃｏｓθ(t)）＋ｎ１ｓｉｎθ
(t) ｒ３３＝ｎ３ ２＋（１−ｎ３ ２）ｃｏｓθ(t) 次に、(7)式の８次のBezier曲線の速度Ｐ′(t)及び加速
度Ｐ″(t)は次式で与えられる。
(t) ｒ１３＝ｎ１ｎ３（１−ｃｏｓθ(t)）＋ｎ２ｓｉｎθ
(t) ｒ２１＝ｎ１ｎ２（１−ｃｏｓθ(t)）＋ｎ３ｓｉｎθ
(t) ｒ２２＝ｎ２ ２＋（１−ｎ２ ２）ｃｏｓθ(t) ｒ２３＝ｎ２ｎ３（１−ｃｏｓθ(t)）−ｎ１ｓｉｎθ
(t) ｒ３１＝ｎ１ｎ３（１−ｃｏｓθ(t)）−ｎ２ｓｉｎθ
(t) ｒ３２＝ｎ２ｎ３（１−ｃｏｓθ(t)）＋ｎ１ｓｉｎθ
(t) ｒ３３＝ｎ３ ２＋（１−ｎ３ ２）ｃｏｓθ(t) 次に、(7)式の８次のBezier曲線の速度Ｐ′(t)及び加速
度Ｐ″(t)は次式で与えられる。
そこでｔ＝Ｏ，Ｔにおける速度，加速度を求めると次の
如くなる。
如くなる。
Ｐ′(o)＝４（Ｐ１−Ｐｏ）／Ｔ (2
7) Ｐ′(T)＝４（Ｐ４−Ｐ３）／Ｔ (2
8) Ｐ″(o)＝Ｐ″(T)＝０ (2
9) そこで、 とするならば、両点における速度が一致する。
7) Ｐ′(T)＝４（Ｐ４−Ｐ３）／Ｔ (2
8) Ｐ″(o)＝Ｐ″(T)＝０ (2
9) そこで、 とするならば、両点における速度が一致する。
（発明の効果） 以上説明したように本発明によれば次の効果がある。
(A)加速特性で定まる長さで経由軌道を円弧軌道と直線
軌道又は円弧軌道相互を接続するので、経由点のごく近
傍を通過させることができ障害物等を回避する場合の動
作時間が短くなる。
軌道又は円弧軌道相互を接続するので、経由点のごく近
傍を通過させることができ障害物等を回避する場合の動
作時間が短くなる。
(B)経由軌道は速度をなめらかに変化させるので、マニ
ピュレータに不用な力がかからず、振動や音の発生しな
い円滑な動作を行うことができる。
ピュレータに不用な力がかからず、振動や音の発生しな
い円滑な動作を行うことができる。
第１図は本発明の一実施例を示す構成ブロック図、第２
図は第１図の装置が補助点を選定する場合の説明図、第
３図は補助点選定の具体例、第４図は第１図の装置によ
るマニピュレータの動作説明図、第５図はベジュエ曲線
上の速度と加速度の説明図、第６図は円弧軌道の補間方
式の説明図、第７図は従来例の説明図である。
図は第１図の装置が補助点を選定する場合の説明図、第
３図は補助点選定の具体例、第４図は第１図の装置によ
るマニピュレータの動作説明図、第５図はベジュエ曲線
上の速度と加速度の説明図、第６図は円弧軌道の補間方
式の説明図、第７図は従来例の説明図である。
Claims (1)
- 【請求項１】マニピュレータの現在位置（Ｐｓ）より、
外部より指定された経由点（Ｐｍ）を直線軌道若しくは
円弧軌道で経由して、外部より指定された目標点
（ＰＥ）に前記マニピュレータを移動させると共に、現
在位置から経由点までが直線軌道の時は当該経由点から
目標点までを円弧軌道で移動させ、現在位置から経由点
までが円弧軌道の時は当該経由点から目標点までを直線
軌道若しくは円弧軌道で移動させるマニピュレータ制御
装置において、 前記現在位置から前記経由点までの軌道上の距離と、前
記経由点から前記目標点までの軌道上の距離のいずれか
短いほうの移動距離と、前記移動で指定された所定の最
大速度から静止するまでの静止距離との大小を比較する
比較手段（Ａ４１）と、 この比較手段で静止距離よりも十分移動距離が大きいと
判断する場合には静止距離、十分には大きくないと判断
する場合には前記移動距離の半分だけ前記２軌道上の前
記経由点から離れた点であって、現在位置側を第１の補
助点（Ｐ０）、目標点側を第２の補助点（Ｐ４）として
選定すると共に、下記の第３及び第４の補助点（Ｐ１、
Ｐ３）を演算する補助点演算手段（Ａ４２）と、 上記第１および第３の補助点、前記経由点、上記第４及
び第２の補助点により、この順で定められる近似スプラ
イン曲線を演算すると共に、この近似スプライン曲線は
第１の補助点で前記直線又は円弧軌道と滑らかに接続さ
れ、第２の補助点で前記直線又は円弧軌道と滑らかに接
続される経由軌道演算手段（Ａ４４）とを備え、 前記現在位置から前記第１の補助点までを前記直線軌道
若しくは円弧軌道上で移動し、前記第１の補助点から前
記第２の補助点までを前記経由軌道演算手段で求めた経
由軌道上で移動し、前記第２の補助点から前記目標点ま
でを前記直線軌道若しくは円弧軌道上で移動させること
を特徴とするマニピュレータ制御装置。 記 第３の補助点は、現在位置と経由点を結ぶ軌道が直線
である場合には第１の補助点と経由点との中点、現在位
置と経由点を結ぶ軌道が円弧である場合には第１の補助
点と経由点との中点の軌道上における距離だけ第１の補
助点から経由点側に離れた点であって、第１の補助点に
おいて該円弧に接する直線上の点とする。 第４の補助点は、経由点と目標点を結ぶ軌道が直線で
ある場合には第２の補助点と経由点との中点、経由点と
目標点を結ぶ軌道が円弧である場合には第２の補助点と
経由点との中点の軌道上における距離だけ第２の補助点
から経由点側に離れた点であって、第２の補助点におい
て該円弧に接する直線上の点とする。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61267288A JPH0656565B2 (ja) | 1986-11-10 | 1986-11-10 | マニピユレ−タ制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61267288A JPH0656565B2 (ja) | 1986-11-10 | 1986-11-10 | マニピユレ−タ制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS63120306A JPS63120306A (ja) | 1988-05-24 |
| JPH0656565B2 true JPH0656565B2 (ja) | 1994-07-27 |
Family
ID=17442747
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP61267288A Expired - Fee Related JPH0656565B2 (ja) | 1986-11-10 | 1986-11-10 | マニピユレ−タ制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0656565B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114378807B (zh) * | 2020-10-20 | 2024-02-09 | 北京配天技术有限公司 | 一种动力装置的运行控制方法、系统、装置及存储介质 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS583002A (ja) * | 1981-06-30 | 1983-01-08 | Fujitsu Ltd | ロボツトの制御方式 |
| JPS58195208A (ja) * | 1982-05-07 | 1983-11-14 | Fanuc Ltd | ロボツト制御方式 |
| JPS58195209A (ja) * | 1982-05-10 | 1983-11-14 | Hitachi Ltd | ロボツトハンド経路補間方式 |
| JPS5968014A (ja) * | 1982-10-13 | 1984-04-17 | Hitachi Ltd | ロボツトの経路補間装置 |
| JPS60262213A (ja) * | 1984-06-06 | 1985-12-25 | Nippei Toyama Corp | 産業用ロボツトの運動制御方法 |
-
1986
- 1986-11-10 JP JP61267288A patent/JPH0656565B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPS63120306A (ja) | 1988-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3286334B2 (ja) | 移動体の制御装置 | |
| EP0346538A1 (en) | Control method for an unmanned vehicle | |
| JPH04324505A (ja) | 自律移動機械、移動機械の制御装置及び方法 | |
| JPH0434773B2 (ja) | ||
| KR101567456B1 (ko) | 속도 계획 장치를 구비한 도립 진자형 이동체 | |
| JP3266747B2 (ja) | 車両の誘導走行制御装置 | |
| JPH0656565B2 (ja) | マニピユレ−タ制御装置 | |
| JP3991637B2 (ja) | 無人車 | |
| JP2645551B2 (ja) | ロボット制御装置 | |
| JPS59111508A (ja) | 点追従方式による車両の自動誘導方法 | |
| JPH03286315A (ja) | 自動操舵装置 | |
| JP2023110178A (ja) | 全方位移動体の軌道生成装置、軌道生成方法及びプログラム | |
| JP7804559B2 (ja) | 軌道生成装置、軌道生成方法、プログラムおよび移動体 | |
| JPH07129243A (ja) | 無人車の操舵制御装置 | |
| JP2837986B2 (ja) | 走行体の旋回制御システム | |
| JP2518183B2 (ja) | ロボット制御装置 | |
| JPH061405B2 (ja) | 位置制御装置 | |
| JP2001277160A (ja) | ロボットの経路生成方法 | |
| JP2979824B2 (ja) | クレーンの振れ止め制御装置 | |
| KR20020079132A (ko) | 차선방법을 이용한 이동 로봇의 장애물 회피 방법 | |
| JPS63242769A (ja) | 車体傾斜装置の異常検知方法 | |
| JP2837985B2 (ja) | 走行体のコース誘導システム | |
| JP2002041147A (ja) | 搬送台車の制御方法及び制御装置 | |
| JPH0991020A (ja) | 教示ロボット制御方法 | |
| JPS63253413A (ja) | リニアモ−タ搬送台車 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |