JPH09146996A - キネマティクス演算方法および装置 - Google Patents
キネマティクス演算方法および装置Info
- Publication number
- JPH09146996A JPH09146996A JP7301786A JP30178695A JPH09146996A JP H09146996 A JPH09146996 A JP H09146996A JP 7301786 A JP7301786 A JP 7301786A JP 30178695 A JP30178695 A JP 30178695A JP H09146996 A JPH09146996 A JP H09146996A
- Authority
- JP
- Japan
- Prior art keywords
- joint
- acceleration
- force
- link
- torque
- 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.)
- Granted
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39077—Solve inverse geometric model by iteration, no matrixes inversion
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Transmission Devices (AREA)
- Manipulator (AREA)
Abstract
(57)【要約】
【課題】本発明は、複数のリンクが関節を介して連結さ
れたリンク機構のリンクの位置および姿勢を求めるキネ
マティクス演算方法および装置に関し、特異点の問題を
生じることを防止し、かつ汎用性を高める。 【解決手段】関節の、角度、角速度、およびトルクを数
値代入して関節の角加速度を求め、その角加速度を数値
積分して関節の角度を求めキネマティクスの演算を行な
ってその結果を表示する。
れたリンク機構のリンクの位置および姿勢を求めるキネ
マティクス演算方法および装置に関し、特異点の問題を
生じることを防止し、かつ汎用性を高める。 【解決手段】関節の、角度、角速度、およびトルクを数
値代入して関節の角加速度を求め、その角加速度を数値
積分して関節の角度を求めキネマティクスの演算を行な
ってその結果を表示する。
Description
【０００１】
【発明の属する技術分野】本発明は、複数のリンクが関
節を介して連結されたリンク機構のリンクの位置および
姿勢を求めるキネマティクス演算方法および装置に関す
る。
節を介して連結されたリンク機構のリンクの位置および
姿勢を求めるキネマティクス演算方法および装置に関す
る。
【０００２】
【従来の技術】従来より、種々のリンク機構が構築され
ており、またそれらのリンク機構解析が、例えばロボッ
トシミュレータ、アニメーション作成ツール、インタラ
ククティブロボット制御装置、機構設計ツール、エルゴ
ノミスク等、様々な分野で行なわれている。
ており、またそれらのリンク機構解析が、例えばロボッ
トシミュレータ、アニメーション作成ツール、インタラ
ククティブロボット制御装置、機構設計ツール、エルゴ
ノミスク等、様々な分野で行なわれている。
【０００３】リンク機構は、図１に示すように、複数の
リンクが関節を介して連結された構造を有している。リ
ンク機構の解析のためにモデル化された関節は、１自由
度の回転タイプと１自由度の直動タイプで表現され、人
間の肩のような多自由度の関節はこれら２つのタイプの
関節を１つの場所に複数配置することにより表現するこ
とができる。このようなリンク機構の例としては、自動
車、工作機械、ロボット、人間等を挙げることができ、
可動部をもつ、ほとんどあらゆる物を表現することがで
きる。
リンクが関節を介して連結された構造を有している。リ
ンク機構の解析のためにモデル化された関節は、１自由
度の回転タイプと１自由度の直動タイプで表現され、人
間の肩のような多自由度の関節はこれら２つのタイプの
関節を１つの場所に複数配置することにより表現するこ
とができる。このようなリンク機構の例としては、自動
車、工作機械、ロボット、人間等を挙げることができ、
可動部をもつ、ほとんどあらゆる物を表現することがで
きる。
【０００４】リンク機構の解析にあたり、回転タイプの
場合の角度、ないし直動タイプの場合の変位を与えてリ
ンク（狭義にはそのリンク機構の先端）の位置と姿勢を
求めることをキネマティクスあるいは順キネマティクス
と呼び、リンク（狭義にはリンク機構の先端）の位置情
報を与えて各関節の角度ないし変位を求めることを逆キ
ネマティクスと呼ぶ。
場合の角度、ないし直動タイプの場合の変位を与えてリ
ンク（狭義にはそのリンク機構の先端）の位置と姿勢を
求めることをキネマティクスあるいは順キネマティクス
と呼び、リンク（狭義にはリンク機構の先端）の位置情
報を与えて各関節の角度ないし変位を求めることを逆キ
ネマティクスと呼ぶ。
【０００５】尚、リンク機構の場合、回転タイプの関節
を用いる場合が多いため、以下では、その性質に反しな
い限り、かつ、相互に用語を区別して使用しない限り、
回転タイプの関節に関する物理的表現、すなわち、角
度、角速度、角加速度、トルクで代表させる場合があ
る。すなわち、直動タイプの関節の場合、角度、角速
度、角加速度、トルクを、それぞれ、変位、速度、加速
度、力と読み替えるものとする。
を用いる場合が多いため、以下では、その性質に反しな
い限り、かつ、相互に用語を区別して使用しない限り、
回転タイプの関節に関する物理的表現、すなわち、角
度、角速度、角加速度、トルクで代表させる場合があ
る。すなわち、直動タイプの関節の場合、角度、角速
度、角加速度、トルクを、それぞれ、変位、速度、加速
度、力と読み替えるものとする。
【０００６】従来より、上記のようなリンク機構の解析
が行なわれているが、従来は、幾何学的条件によりキネ
マティクスの演算が行なわれていた。この手法では、関
節構成に依存して個別に対応する必要があり、しかもマ
ニピュレータの持つ部品、組立の際の誤差に起因してそ
の解析解が破綻することもある等、数学的困難さを伴う
ことがある。
が行なわれているが、従来は、幾何学的条件によりキネ
マティクスの演算が行なわれていた。この手法では、関
節構成に依存して個別に対応する必要があり、しかもマ
ニピュレータの持つ部品、組立の際の誤差に起因してそ
の解析解が破綻することもある等、数学的困難さを伴う
ことがある。
【０００７】また、この幾何学的条件を用いて数値的に
処理する手法もあるが、極めて時間がかかるという問題
がある。さらに、解析的手法と数値的手法とを組み合わ
せた手法も提案されているが、６自由度を越えると冗長
性の問題が出てくるため６自由度以下の自由度をもつリ
ンク機構の解析にのみ有効なものであった。
処理する手法もあるが、極めて時間がかかるという問題
がある。さらに、解析的手法と数値的手法とを組み合わ
せた手法も提案されているが、６自由度を越えると冗長
性の問題が出てくるため６自由度以下の自由度をもつリ
ンク機構の解析にのみ有効なものであった。
【０００８】さらに角速度（速度を含む）の関係式（ヤ
コビアン）を用いる手法もあるが、そのリンク機構の動
きが実現可能な最高角速度（最高速度を含む）を越える
という特異点の問題がある。
コビアン）を用いる手法もあるが、そのリンク機構の動
きが実現可能な最高角速度（最高速度を含む）を越える
という特異点の問題がある。
【０００９】
【発明が解決しようとする課題】本発明は、上記事情に
鑑み、特異点の問題を生じることのない、かつ汎用性の
高いキネマティクス演算方法、およびそのキネマティク
ス演算方法が実施されたキネマティクス演算装置を提供
することを目的とする。
鑑み、特異点の問題を生じることのない、かつ汎用性の
高いキネマティクス演算方法、およびそのキネマティク
ス演算方法が実施されたキネマティクス演算装置を提供
することを目的とする。
【００１０】
【課題を解決するための手段】図２は、本発明のキネマ
ティクス演算方法の基本フローチャートである。上記目
的を達成する本発明のキネマティクス演算方法は、複数
のリンクが関節を介して連結されたリンク機構のリンク
の位置および姿勢を求めるキネマティクス演算方法にお
いて、関節の、角度（変位を含む。以下省略する。）
ｑ、角速度（速度を含む。以下省略する。）
ティクス演算方法の基本フローチャートである。上記目
的を達成する本発明のキネマティクス演算方法は、複数
のリンクが関節を介して連結されたリンク機構のリンク
の位置および姿勢を求めるキネマティクス演算方法にお
いて、関節の、角度（変位を含む。以下省略する。）
ｑ、角速度（速度を含む。以下省略する。）
【００１１】
【外１】
【００１２】（以下、「ｑドット」と称する。他の記号
についても同様である。）、角加速度（加速度を含む。
以下省略する。）
についても同様である。）、角加速度（加速度を含む。
以下省略する。）
【００１３】
【外２】
【００１４】（以下、「ｑ２ドット」と称する。他の記
号についても同様である。）およびトルク（力を含む）
τの間の関係を規定する関係式
号についても同様である。）およびトルク（力を含む）
τの間の関係を規定する関係式
【００１５】
【数１】
【００１６】に、関節の、角度初期値ｑ0 、角速度初期
値ｑ0 ドットおよびトルク初期値τ0を数値代入して関
節の角加速度ｑ２ドットを求める第１ステップと、上記
関係式（１）に、関節の、角度ｑ、角速度ｑドット、お
よびトルクτを数値代入することにより求められた関節
の角加速度ｑ２ドットを数値積分することにより、関節
の角速度ｑドットおよび角度ｑを求める第２ステップ
と、第２ステップで求められた関節の角速度ｑドットお
よび角度ｑ、および、関節の、トルク初期値τ0 、ない
しそのトルク初期値τ0 に代わる変更されたトルクτを
上記関係式（１）に数値代入して関節の角加速度ｑ２ド
ットを求める第３ステップと、第２ステップで求められ
た関節の角度ｑに基づいてリンクの位置および姿勢を求
める第４ステップとを有し、上記第２ステップと上記第
３ステップとを交互に繰り返すとともに、上記第４ステ
ップを繰り返し実行することによりリンクの位置および
姿勢を順次更新することを特徴とする。
値ｑ0 ドットおよびトルク初期値τ0を数値代入して関
節の角加速度ｑ２ドットを求める第１ステップと、上記
関係式（１）に、関節の、角度ｑ、角速度ｑドット、お
よびトルクτを数値代入することにより求められた関節
の角加速度ｑ２ドットを数値積分することにより、関節
の角速度ｑドットおよび角度ｑを求める第２ステップ
と、第２ステップで求められた関節の角速度ｑドットお
よび角度ｑ、および、関節の、トルク初期値τ0 、ない
しそのトルク初期値τ0 に代わる変更されたトルクτを
上記関係式（１）に数値代入して関節の角加速度ｑ２ド
ットを求める第３ステップと、第２ステップで求められ
た関節の角度ｑに基づいてリンクの位置および姿勢を求
める第４ステップとを有し、上記第２ステップと上記第
３ステップとを交互に繰り返すとともに、上記第４ステ
ップを繰り返し実行することによりリンクの位置および
姿勢を順次更新することを特徴とする。
【００１７】本発明のキネマティクス演算方法は、関節
の、角度ｑ、角速度ｑドット、およびトルクτを数値代
入して関節の角加速度ｑ２ドットを求め、その角加速度
ｑ２ドットを数値積分して関節の角度ｑを求めてキネマ
ティクスの演算を行なうものであるため、角速度関係式
（ヤコビアン）を用いる手法における特異点の問題を生
じることが防止され、かつ、冗長な自由度を持つリンク
機構であっても解析が可能であって、数学的な困難さも
回避される。
の、角度ｑ、角速度ｑドット、およびトルクτを数値代
入して関節の角加速度ｑ２ドットを求め、その角加速度
ｑ２ドットを数値積分して関節の角度ｑを求めてキネマ
ティクスの演算を行なうものであるため、角速度関係式
（ヤコビアン）を用いる手法における特異点の問題を生
じることが防止され、かつ、冗長な自由度を持つリンク
機構であっても解析が可能であって、数学的な困難さも
回避される。
【００１８】ここで、上記本発明のキネマティクス演算
方法において、上記関係式（１）が、関節の、角度ｑ、
角速度ｑドット、角加速度ｑ２ドットおよびトルクτに
加え、さらに、リンクに作用する力およびモーメントｐ
の間の関係を定める関係式であって、上記第１ステップ
が、関節の、角度初期値ｑ0 、角速度初期値ｑ0 ドット
およびトルク初期値τ0 に加え、さらに、リンクに作用
する力およびモーメントの初期値ｐ0 を上記関係式
（１）に数値代入して関節の角加速度ｑ２ドットを求め
るステップであり、上記第３ステップが、上記第２ステ
ップで求められた関節の角速度ｑドットおよび角度ｑ、
および、関節の、トルク初期値τ0 、ないしそのトルク
初期値τ0に代わる変更されたトルクτに加え、さら
に、リンクに作用する力およびモーメントの初期値ｐ
0 、ないしその初期値に代わる変更された力およびモー
メントｐを上記関係式（１）に数値代入して関節の角加
速度ｑ２ドットを求めるステップであってもよい。
方法において、上記関係式（１）が、関節の、角度ｑ、
角速度ｑドット、角加速度ｑ２ドットおよびトルクτに
加え、さらに、リンクに作用する力およびモーメントｐ
の間の関係を定める関係式であって、上記第１ステップ
が、関節の、角度初期値ｑ0 、角速度初期値ｑ0 ドット
およびトルク初期値τ0 に加え、さらに、リンクに作用
する力およびモーメントの初期値ｐ0 を上記関係式
（１）に数値代入して関節の角加速度ｑ２ドットを求め
るステップであり、上記第３ステップが、上記第２ステ
ップで求められた関節の角速度ｑドットおよび角度ｑ、
および、関節の、トルク初期値τ0 、ないしそのトルク
初期値τ0に代わる変更されたトルクτに加え、さら
に、リンクに作用する力およびモーメントの初期値ｐ
0 、ないしその初期値に代わる変更された力およびモー
メントｐを上記関係式（１）に数値代入して関節の角加
速度ｑ２ドットを求めるステップであってもよい。
【００１９】この場合に、上記の変更されたトルクは、
外部から直接与えてもよいが、上記リンク機構の動作領
域内に仮想物体を配置したときのその仮想物体とそのリ
ンク機構のリンクとの干渉力をそのリンクに作用する力
およびモーメントとして上記関係式（１）に数値代入し
てもよい。また、上記リンク機構が閉ループを含むリン
ク機構である場合に、その閉ループを所定位置で仮想的
に切断しその切断により生成された両端間にその両端相
互の物理量のずれに応じた力およびモーメントを作用さ
せたときのその力およびモーメントを、その両端それぞ
れを有する２つのリンクに作用する力およびモーメント
ｐとして数値代入してもよい。
外部から直接与えてもよいが、上記リンク機構の動作領
域内に仮想物体を配置したときのその仮想物体とそのリ
ンク機構のリンクとの干渉力をそのリンクに作用する力
およびモーメントとして上記関係式（１）に数値代入し
てもよい。また、上記リンク機構が閉ループを含むリン
ク機構である場合に、その閉ループを所定位置で仮想的
に切断しその切断により生成された両端間にその両端相
互の物理量のずれに応じた力およびモーメントを作用さ
せたときのその力およびモーメントを、その両端それぞ
れを有する２つのリンクに作用する力およびモーメント
ｐとして数値代入してもよい。
【００２０】このように、本発明のキネマティクス演算
方法において、関節トルクのみでなくリンクに作用する
力およびモーメントを考慮すると、そのリンク機構の動
きに制限を加えたり、その動きを周囲の環境に合わせた
り、さらに閉ループを持つリンク機構の解析も可能にな
るなど、本発明のキネマティクス演算方法の汎用性が一
層広がることになる。
方法において、関節トルクのみでなくリンクに作用する
力およびモーメントを考慮すると、そのリンク機構の動
きに制限を加えたり、その動きを周囲の環境に合わせた
り、さらに閉ループを持つリンク機構の解析も可能にな
るなど、本発明のキネマティクス演算方法の汎用性が一
層広がることになる。
【００２１】また、上記本発明のキネマティクス演算方
法において、上記第１ステップおよび上記第３ステップ
が、上記関係式（１）としてニュートン・オイラーの運
動方程式を用いて関節の角加速度を求めるステップであ
ることが好ましい。上述した本発明のキネマティクス演
算方法は、例えばアニメーションの世界にもその適用分
野が広がっており、必ずしも現実の世界の物理法則に拘
束される必要はないが、本発明のキネマティクス演算方
法において、現実の世界の物理法則を定式化したニュー
トン・オイラーの運動方程式を用いると、実世界の物理
法則に則った物体の動きを模擬した自然な動きが可能と
なる。
法において、上記第１ステップおよび上記第３ステップ
が、上記関係式（１）としてニュートン・オイラーの運
動方程式を用いて関節の角加速度を求めるステップであ
ることが好ましい。上述した本発明のキネマティクス演
算方法は、例えばアニメーションの世界にもその適用分
野が広がっており、必ずしも現実の世界の物理法則に拘
束される必要はないが、本発明のキネマティクス演算方
法において、現実の世界の物理法則を定式化したニュー
トン・オイラーの運動方程式を用いると、実世界の物理
法則に則った物体の動きを模擬した自然な動きが可能と
なる。
【００２２】この、ニュートン・オイラーの運動方程式
を用いる場合において、上記第１ステップおよび上記第
３ステップが、ニュートン・オイラーの運動方程式に基
づく、関節の角加速度から関節のトルクを求める漸化式
と、その漸化式により求められたトルクから関節の角加
速度を求める逆解法とを用いて、関節の角加速度を求め
るステップであることが好ましい。
を用いる場合において、上記第１ステップおよび上記第
３ステップが、ニュートン・オイラーの運動方程式に基
づく、関節の角加速度から関節のトルクを求める漸化式
と、その漸化式により求められたトルクから関節の角加
速度を求める逆解法とを用いて、関節の角加速度を求め
るステップであることが好ましい。
【００２３】この漸化式はウォーカー・オリン（Ｗａｌ
ｋｅｒ ＆ Ｏｒｉｎ）タイプの逆動力学の再帰アルゴ
リズムとして知られており、この漸化式とその逆解法を
用いてニュートン・オイラーの運動方程式に則った演算
を行なうことにより、ニュートン・オイラーの運動方程
式をそのまま解く場合と比べ極めて高速な演算が可能と
なる。
ｋｅｒ ＆ Ｏｒｉｎ）タイプの逆動力学の再帰アルゴ
リズムとして知られており、この漸化式とその逆解法を
用いてニュートン・オイラーの運動方程式に則った演算
を行なうことにより、ニュートン・オイラーの運動方程
式をそのまま解く場合と比べ極めて高速な演算が可能と
なる。
【００２４】また、ニュートン・オイラーの運動方程式
を用いる場合において、上記第１ステップおよび上記第
３ステップが、ニュートン・オイラーの運動方程式に基
づく、関節のトルクから関節の角加速度を求める漸化式
を用いて、関節の角加速度を求めるステップであること
も好ましい態様である。この漸化式は、フェザーストー
ン（Ｆｅａｔｈｅｒｓｔｏｎｅ）タイプの順動力学の再
帰アルゴリズムとして知られており、ニュートン・オイ
ラーの運動方程式を解くにあたってこの漸化式を用いる
ことによっても極めて高速な演算が可能となる。
を用いる場合において、上記第１ステップおよび上記第
３ステップが、ニュートン・オイラーの運動方程式に基
づく、関節のトルクから関節の角加速度を求める漸化式
を用いて、関節の角加速度を求めるステップであること
も好ましい態様である。この漸化式は、フェザーストー
ン（Ｆｅａｔｈｅｒｓｔｏｎｅ）タイプの順動力学の再
帰アルゴリズムとして知られており、ニュートン・オイ
ラーの運動方程式を解くにあたってこの漸化式を用いる
ことによっても極めて高速な演算が可能となる。
【００２５】また、本発明のキネマティクス演算方法に
おいて、上記第３ステップが、上記第２ステップで求め
られた関節の角速度ｑドットおよび角度ｑ、および、関
節の、そのリンク機構先端の現在位置と目標位置との間
の距離に応じたトルクτを上記関係式（１）に数値代入
して関節の角加速度ｑ２ドットを求めるステップであっ
てもよい。
おいて、上記第３ステップが、上記第２ステップで求め
られた関節の角速度ｑドットおよび角度ｑ、および、関
節の、そのリンク機構先端の現在位置と目標位置との間
の距離に応じたトルクτを上記関係式（１）に数値代入
して関節の角加速度ｑ２ドットを求めるステップであっ
てもよい。
【００２６】キネマティクス演算を実行するにあたって
は、関節トルクを変数として与えて、リンク（特にリン
ク機構の先端）がどのように移動するかが解析される。
このとき、変数としての関節トルクτを逐次入力しても
よいが、上記のように、リンク機構先端の現在の位置と
目標位置との間の距離に応じたトルクτを関係式（１）
に数値代入するように構成すると、リンク機構先端の目
標位置を入力するだけで関節トルクを逐次入力したのと
同等の演算を行なうことができる。
は、関節トルクを変数として与えて、リンク（特にリン
ク機構の先端）がどのように移動するかが解析される。
このとき、変数としての関節トルクτを逐次入力しても
よいが、上記のように、リンク機構先端の現在の位置と
目標位置との間の距離に応じたトルクτを関係式（１）
に数値代入するように構成すると、リンク機構先端の目
標位置を入力するだけで関節トルクを逐次入力したのと
同等の演算を行なうことができる。
【００２７】さらに、上記本発明のキネマティクス演算
方法において、上記リンク機構を表示する表示画面上
に、上記第４ステップで求められた位置および姿勢を有
するリンクからなるリンク機構を表示する第５ステップ
を有することが好ましい。リンク機構の動きを表示画面
上に表示することにより、リンク機構の動きの様子の把
握が極めて容易となる。
方法において、上記リンク機構を表示する表示画面上
に、上記第４ステップで求められた位置および姿勢を有
するリンクからなるリンク機構を表示する第５ステップ
を有することが好ましい。リンク機構の動きを表示画面
上に表示することにより、リンク機構の動きの様子の把
握が極めて容易となる。
【００２８】図３は、本発明の第１のキネマティクス演
算装置の原理ブロック図である。尚、以下のキネマティ
クス演算装置の説明において、本発明のキネマティクス
演算方法の作用と同等の作用を有する装置態様について
の作用の説明は省略する。本発明の第１のキネマティク
ス演算装置は、複数のリンクが関節を介して連結された
リンク機構のリンクの位置および姿勢を求めるキネマテ
ィクス演算装置において、図３に示すように、関節の、
角度ｑ、角速度ｑドット、角加速度ｑ２ドットおよびト
ルクτの間の関係を規定する前述の関係式（１）に、関
節の、角度ｑ、角速度ｑドットおよびトルクτを数値代
入して関節の角加速度ｑ２ドットを求める演算手段１
と、関節のトルクτを、変更自在に、演算手段１に入力
する入力手段２と、演算手段２により求められた関節の
角加速度ｑ２ドットを数値積分することにより関節の角
速度ｑドットおよび角度ｑを求めて演算手段１に入力す
る数値分析手段３と、数値積分手段３で求められた関節
の角度ｑに基づいてリンクの位置および姿勢を求めるキ
ネマティクス手段４とを備えたことを特徴とする。
算装置の原理ブロック図である。尚、以下のキネマティ
クス演算装置の説明において、本発明のキネマティクス
演算方法の作用と同等の作用を有する装置態様について
の作用の説明は省略する。本発明の第１のキネマティク
ス演算装置は、複数のリンクが関節を介して連結された
リンク機構のリンクの位置および姿勢を求めるキネマテ
ィクス演算装置において、図３に示すように、関節の、
角度ｑ、角速度ｑドット、角加速度ｑ２ドットおよびト
ルクτの間の関係を規定する前述の関係式（１）に、関
節の、角度ｑ、角速度ｑドットおよびトルクτを数値代
入して関節の角加速度ｑ２ドットを求める演算手段１
と、関節のトルクτを、変更自在に、演算手段１に入力
する入力手段２と、演算手段２により求められた関節の
角加速度ｑ２ドットを数値積分することにより関節の角
速度ｑドットおよび角度ｑを求めて演算手段１に入力す
る数値分析手段３と、数値積分手段３で求められた関節
の角度ｑに基づいてリンクの位置および姿勢を求めるキ
ネマティクス手段４とを備えたことを特徴とする。
【００２９】本発明の第１のキネマティクス演算装置
は、トルク入力手段２を備えたため、関節トルクτを随
時変更しながらキネマティクス演算を実行させ、例えば
そのリンク機構の先端を目標位置に導くことができる。
図４は、本発明の第２のキネマティクス演算装置の原理
ブロック図である。本発明の第２のキネマティクス演算
装置は、複数のリンクが関節を介して連結されたリンク
機構のリンクの位置および姿勢を求めるキネマティクス
演算装置において、関節の、角度ｑ、角速度ｑバー、角
加速度ｑ２バーおよびトルクτの間の関係を規定する、
前述の関係式（１）に、関節の、角度ｑ、角速度ｑバー
およびトルクτを数値代入して関節の角加速度ｑ２バー
を求める演算手段１と、そのリンク機構先端の目標位置
を入力する入力手段５と、関節の、そのリンク機構先端
の現在位置と、入力手段５により入力された目標位置と
の間の距離に応じたトルクτを設定する設定手段と、演
算手段１により求められた関節の角加速度ｑ２バーを数
値積分することにより関節の角速度ｑバーおよび角度ｑ
を求めて演算手段１に入力する数値積分手段３と、数値
積分手段３で求められた関節の角度ｑに基づいてリンク
の位置および姿勢を求めるキネマティクス手段４とを備
えたことを特徴とする。
は、トルク入力手段２を備えたため、関節トルクτを随
時変更しながらキネマティクス演算を実行させ、例えば
そのリンク機構の先端を目標位置に導くことができる。
図４は、本発明の第２のキネマティクス演算装置の原理
ブロック図である。本発明の第２のキネマティクス演算
装置は、複数のリンクが関節を介して連結されたリンク
機構のリンクの位置および姿勢を求めるキネマティクス
演算装置において、関節の、角度ｑ、角速度ｑバー、角
加速度ｑ２バーおよびトルクτの間の関係を規定する、
前述の関係式（１）に、関節の、角度ｑ、角速度ｑバー
およびトルクτを数値代入して関節の角加速度ｑ２バー
を求める演算手段１と、そのリンク機構先端の目標位置
を入力する入力手段５と、関節の、そのリンク機構先端
の現在位置と、入力手段５により入力された目標位置と
の間の距離に応じたトルクτを設定する設定手段と、演
算手段１により求められた関節の角加速度ｑ２バーを数
値積分することにより関節の角速度ｑバーおよび角度ｑ
を求めて演算手段１に入力する数値積分手段３と、数値
積分手段３で求められた関節の角度ｑに基づいてリンク
の位置および姿勢を求めるキネマティクス手段４とを備
えたことを特徴とする。
【００３０】本発明の第２のキネマティクス演算装置
は、目標位置を入力する入力手段５とトルクを設定する
設定手段６を備えたため、リンク機構先端の目標位置を
入力するだけで関節トルクτが設定され、関節トルクτ
を１つずついちいち入力することなしに、そのリンク機
構先端を目標位置に導くことができる。ここで、上記本
発明の第１ないし第２のキネマティクス演算装置におい
て、前述の関係式（１）が、関節の、角度ｑ、角速度ｑ
バー、角加速度ｑ２バーおよびトルクτに加え、さら
に、リンクに作用する力およびモーメントｐの間の関係
を定める関係式であって、演算手段１が、関節の、角度
ｑ、角速度ｑバーおよびトルクτに加え、さらに、リン
クに作用する力およびモーメントｐを関係式（１）に数
値代入して関節の角加速度ｑ２バーを求めるものである
ことが好ましい。
は、目標位置を入力する入力手段５とトルクを設定する
設定手段６を備えたため、リンク機構先端の目標位置を
入力するだけで関節トルクτが設定され、関節トルクτ
を１つずついちいち入力することなしに、そのリンク機
構先端を目標位置に導くことができる。ここで、上記本
発明の第１ないし第２のキネマティクス演算装置におい
て、前述の関係式（１）が、関節の、角度ｑ、角速度ｑ
バー、角加速度ｑ２バーおよびトルクτに加え、さら
に、リンクに作用する力およびモーメントｐの間の関係
を定める関係式であって、演算手段１が、関節の、角度
ｑ、角速度ｑバーおよびトルクτに加え、さらに、リン
クに作用する力およびモーメントｐを関係式（１）に数
値代入して関節の角加速度ｑ２バーを求めるものである
ことが好ましい。
【００３１】この場合に、上記本発明の第１ないし第２
のキネマティクス演算装置において、リンク機構の動作
領域内に仮想物体を配置する仮想物体配置手段７を備
え、演算手段１が、その仮想物体とリンク機構のリンク
との干渉力およびその干渉力に起因するモーメントをそ
のリンクに作用する力およびモーメントｐとして上記関
係式（１）に数値代入するものであることも好ましい態
様であり、また、演算手段１が、対象としているリンク
機構が閉ループを含むリンク機構である場合に、その閉
ループを所定位置で切断しその切断により生成された両
端間にその両端相互の物理量のずれに応じた力およびモ
ーメントを作用させたときのその力およびモーメント
を、その両端それぞれを有する２つのリンクに作用する
力およびモーメントｐとして関係式（１）に数値代入す
るものであることも好ましい態様である。
のキネマティクス演算装置において、リンク機構の動作
領域内に仮想物体を配置する仮想物体配置手段７を備
え、演算手段１が、その仮想物体とリンク機構のリンク
との干渉力およびその干渉力に起因するモーメントをそ
のリンクに作用する力およびモーメントｐとして上記関
係式（１）に数値代入するものであることも好ましい態
様であり、また、演算手段１が、対象としているリンク
機構が閉ループを含むリンク機構である場合に、その閉
ループを所定位置で切断しその切断により生成された両
端間にその両端相互の物理量のずれに応じた力およびモ
ーメントを作用させたときのその力およびモーメント
を、その両端それぞれを有する２つのリンクに作用する
力およびモーメントｐとして関係式（１）に数値代入す
るものであることも好ましい態様である。
【００３２】また、上記本発明の第１ないし第２のキネ
マティクス演算装置において、演算手段１が、関係式
（１）としてニュートン・オイラーの運動方程式を用い
て関節の角加速度ｑ２バーを求めるものであることが好
ましい。その場合に、演算手段２が、ニュートン・オイ
ラーの運動方程式に基づく、関節の角加速度ｑ２バーか
ら関節のトルクτを求める漸化式と、その漸化式により
求められたトルクτから関節の角加速度ｑ２バーを求め
る逆解法とを用いて、関節の角加速度ｑ２バーを求める
ものであることが好ましく、あるいは、演算手段１が、
ニュートン・オイラーの運動方程式に基づく、関節のト
ルクτから関節の角加速度ｑ２バーを求める漸化式を用
いて、関節の角加速度ｑ２バーを求めるものであること
も好ましい態様である。
マティクス演算装置において、演算手段１が、関係式
（１）としてニュートン・オイラーの運動方程式を用い
て関節の角加速度ｑ２バーを求めるものであることが好
ましい。その場合に、演算手段２が、ニュートン・オイ
ラーの運動方程式に基づく、関節の角加速度ｑ２バーか
ら関節のトルクτを求める漸化式と、その漸化式により
求められたトルクτから関節の角加速度ｑ２バーを求め
る逆解法とを用いて、関節の角加速度ｑ２バーを求める
ものであることが好ましく、あるいは、演算手段１が、
ニュートン・オイラーの運動方程式に基づく、関節のト
ルクτから関節の角加速度ｑ２バーを求める漸化式を用
いて、関節の角加速度ｑ２バーを求めるものであること
も好ましい態様である。
【００３３】さらに、本発明の第１ないし第２のキネマ
ティクス演算装置において、キネマティクス演算手段４
で求められた位置および姿勢を有するリンクからなるリ
ンク機構を表示する表示手段８を備えることが好まし
い。
ティクス演算装置において、キネマティクス演算手段４
で求められた位置および姿勢を有するリンクからなるリ
ンク機構を表示する表示手段８を備えることが好まし
い。
【００３４】
【発明の実施の形態】以下本発明の実施形態について説
明する。図５は、本発明のキネマティクス演算装置の一
実施形態の外観構成図である。この図５に示すように、
本発明のキネマティクス演算装置１０はコンピュータシ
ステムで構成されており、ＣＰＵを内蔵した本体部１
１、キーボード１２、表示画面１３ａを有するＣＲＴデ
ィスプレイ１３、およびマウス１４を備えている。本体
部１１には、ＣＰＵのほか、フロッピィディスク（図示
せず）を装脱自在に装填して、そのフロッピィディスク
に格納されたデータを読み込んだり、本体部１１内部の
データをそのフロッピィディスクに書き込むフロッピィ
ディスクドライブ装置や、読み込んだデータやプログラ
ム等を記憶しておくためのハードディスク装置も内蔵さ
れている。本体部１１内部に備えられたＣＰＵでは各種
プログラムが実行され、その結果がＣＲＴディスプレイ
１３の表示画面１３ａに表示され、あるいは、ハードデ
ィスクに記憶され、さらにはフロッピィディスクにダウ
ンロードされる。
明する。図５は、本発明のキネマティクス演算装置の一
実施形態の外観構成図である。この図５に示すように、
本発明のキネマティクス演算装置１０はコンピュータシ
ステムで構成されており、ＣＰＵを内蔵した本体部１
１、キーボード１２、表示画面１３ａを有するＣＲＴデ
ィスプレイ１３、およびマウス１４を備えている。本体
部１１には、ＣＰＵのほか、フロッピィディスク（図示
せず）を装脱自在に装填して、そのフロッピィディスク
に格納されたデータを読み込んだり、本体部１１内部の
データをそのフロッピィディスクに書き込むフロッピィ
ディスクドライブ装置や、読み込んだデータやプログラ
ム等を記憶しておくためのハードディスク装置も内蔵さ
れている。本体部１１内部に備えられたＣＰＵでは各種
プログラムが実行され、その結果がＣＲＴディスプレイ
１３の表示画面１３ａに表示され、あるいは、ハードデ
ィスクに記憶され、さらにはフロッピィディスクにダウ
ンロードされる。
【００３５】キーボード１２は、本体部１１に対し各種
の指示を入力操作するためのものであり、マウス１４
は、表示画面１３上に表示されたカーソルをその表示画
面１３ａ上の任意の位置に移動し、その表示画面１３ａ
に表示されたアイコンを指定することにより、本体部１
１に対し、所望の指示を入力するための操作子である。
図６は、図５に示すキネマティクス演算装置１０の内部
で実行される本発明のキネマティクス演算方法の一実施
形態を表わすフローチャートである。
の指示を入力操作するためのものであり、マウス１４
は、表示画面１３上に表示されたカーソルをその表示画
面１３ａ上の任意の位置に移動し、その表示画面１３ａ
に表示されたアイコンを指定することにより、本体部１
１に対し、所望の指示を入力するための操作子である。
図６は、図５に示すキネマティクス演算装置１０の内部
で実行される本発明のキネマティクス演算方法の一実施
形態を表わすフローチャートである。
【００３６】ここでは、先ず、被解析対象となるリンク
機構が、例えばＤ−Ｈパラメータで定義されてなるリン
ク機構データの形式で入力される（ステップａ）。Ｄ−
Ｈパラメータについては、広く知られているため、ここ
での説明は省略する（詳細は、「機械系のためのロボテ
ィクス」遠山茂樹著 総合電子出版社 参照）。
機構が、例えばＤ−Ｈパラメータで定義されてなるリン
ク機構データの形式で入力される（ステップａ）。Ｄ−
Ｈパラメータについては、広く知られているため、ここ
での説明は省略する（詳細は、「機械系のためのロボテ
ィクス」遠山茂樹著 総合電子出版社 参照）。
【００３７】本実施形態では、図５に示すキネマティク
ス演算装置１０とは別体の、やはりコンピュータシステ
ム等で実現されたリンク機構定義用ツールを用いてリン
ク機構が定義されてフロッピィディスクにダウンロード
され、そのフロッピィディスクが、今度は図５に示すキ
ネマティクス演算装置１０に装填され、そのリンク機構
データがこのキネマティクス演算装置に読み込まれる。
ス演算装置１０とは別体の、やはりコンピュータシステ
ム等で実現されたリンク機構定義用ツールを用いてリン
ク機構が定義されてフロッピィディスクにダウンロード
され、そのフロッピィディスクが、今度は図５に示すキ
ネマティクス演算装置１０に装填され、そのリンク機構
データがこのキネマティクス演算装置に読み込まれる。
【００３８】尚、図５に示すキネマティクス演算装置１
０（コンピュータシステム）自体で、例えばオペレータ
との対話処理によりリンク機構データを生成するように
構成してもよく、このキネマティクス演算装置１０とは
別体のリンク機構定義用ツールとこのキネマティクス演
算装置１０を通信回線で接続し、フロッピィディスクを
介さずに、通信回線を経由してリンク機構データを入力
してもよい。リンク機構データをどのようにして作成す
るか、あるいはどのようにしてキネマティクス演算装置
に入力するかは本発明の主題ではなく、以下では、図５
に示すキネマティクス演算装置１０にリンク機構データ
が格納されているものとして、キネマティクス演算方法
について説明する。
０（コンピュータシステム）自体で、例えばオペレータ
との対話処理によりリンク機構データを生成するように
構成してもよく、このキネマティクス演算装置１０とは
別体のリンク機構定義用ツールとこのキネマティクス演
算装置１０を通信回線で接続し、フロッピィディスクを
介さずに、通信回線を経由してリンク機構データを入力
してもよい。リンク機構データをどのようにして作成す
るか、あるいはどのようにしてキネマティクス演算装置
に入力するかは本発明の主題ではなく、以下では、図５
に示すキネマティクス演算装置１０にリンク機構データ
が格納されているものとして、キネマティクス演算方法
について説明する。
【００３９】ここでは、リンクどうしを結合する関節が
ｎ個、すなわちｎ自由度のリンク機構の動作を規定する
運動方程式を、
ｎ個、すなわちｎ自由度のリンク機構の動作を規定する
運動方程式を、
【００４０】
【数２】
【００４１】とする。この式（２）において、ｑi ２ド
ット，ｑi ドット，ｑi ，τi は、ｉ番目の関節の、そ
れぞれ角加速度、角速度、角度、およびトルクである。
またｂ i,j ，ｋi,j は、ステップ（ａ）で入力されたリ
ンク機構データに基づく定数である。図６のステップ
（ｂ）では、式（２）に初期値ｑi 0，ｑi 0ドット、τi 0
（ｉ＝１，２，…，ｎ）が数値代入される。これらの初
期値ｑi 0，ｑi 0ドット、τi 0（ｉ＝１，２，…，ｎ）
は、リンク機構データの一部として格納されていてもよ
く、あるいはキーボード１２ないしマウス１４を操作し
て入力してもよい。
ット，ｑi ドット，ｑi ，τi は、ｉ番目の関節の、そ
れぞれ角加速度、角速度、角度、およびトルクである。
またｂ i,j ，ｋi,j は、ステップ（ａ）で入力されたリ
ンク機構データに基づく定数である。図６のステップ
（ｂ）では、式（２）に初期値ｑi 0，ｑi 0ドット、τi 0
（ｉ＝１，２，…，ｎ）が数値代入される。これらの初
期値ｑi 0，ｑi 0ドット、τi 0（ｉ＝１，２，…，ｎ）
は、リンク機構データの一部として格納されていてもよ
く、あるいはキーボード１２ないしマウス１４を操作し
て入力してもよい。
【００４２】ステップ（ｃ）では、初期値が数値代入さ
れた式（２）に基づいて、各関節ｉの角加速度ｑi ２ド
ットが数値的に求められる。このステップ（ｃ）では、
後述するようにして順次角加速度ｑi ２ドットが求めら
れるが、ステップ（ｄ）ではそれらを数値積分（累算）
することにより各関節ｉの角速度ｑi ドット、およびそ
の角速度ｑi ドットを再度数値積分（累算）することに
より各関節ｉの角度ｑi が求められる。
れた式（２）に基づいて、各関節ｉの角加速度ｑi ２ド
ットが数値的に求められる。このステップ（ｃ）では、
後述するようにして順次角加速度ｑi ２ドットが求めら
れるが、ステップ（ｄ）ではそれらを数値積分（累算）
することにより各関節ｉの角速度ｑi ドット、およびそ
の角速度ｑi ドットを再度数値積分（累算）することに
より各関節ｉの角度ｑi が求められる。
【００４３】ステップ（ｅ）では、上記のようにして数
値的に求めた各関節ｉの角度ｑi と各リンクｉの長さ等
のデータに基づいて、現在解析中のリンク機構全体の位
置および姿勢（リンク機構先端の位置および姿勢を含
む）を求めるキネマティクス演算が実行される。図５に
示すキネマティクス演算装置１０には、リンク機構デー
タに基づいて、そのリンク機構の構造を模式的に表わす
リンク機構図形を描く描画ツールが内蔵されており、図
６のステップ（ｆ）では、その描画ツールで描かれたリ
ンク機構図形であって、かつ、ステップ（ｅ）で求めら
れた位置および姿勢を有するリンク機構図形が、ＣＲＴ
ディスプレイ１３の表示画面１３ａ（図５参照）に表示
される。
値的に求めた各関節ｉの角度ｑi と各リンクｉの長さ等
のデータに基づいて、現在解析中のリンク機構全体の位
置および姿勢（リンク機構先端の位置および姿勢を含
む）を求めるキネマティクス演算が実行される。図５に
示すキネマティクス演算装置１０には、リンク機構デー
タに基づいて、そのリンク機構の構造を模式的に表わす
リンク機構図形を描く描画ツールが内蔵されており、図
６のステップ（ｆ）では、その描画ツールで描かれたリ
ンク機構図形であって、かつ、ステップ（ｅ）で求めら
れた位置および姿勢を有するリンク機構図形が、ＣＲＴ
ディスプレイ１３の表示画面１３ａ（図５参照）に表示
される。
【００４４】尚、よりリアルなリンク機構図形を表示す
るために、リンク機構図形データ自体は、あらかじめ作
成されてリンク機構データとともにこのキネマティクス
演算装置１０に入力され、このキネマティクス演算装置
１０では、各関節ｉの角度がステップ（ｄ）で求められ
た角度ｑi となるようにそのリンク機構図形の各関節ｉ
の角度を変更するにとどめてもよい。
るために、リンク機構図形データ自体は、あらかじめ作
成されてリンク機構データとともにこのキネマティクス
演算装置１０に入力され、このキネマティクス演算装置
１０では、各関節ｉの角度がステップ（ｄ）で求められ
た角度ｑi となるようにそのリンク機構図形の各関節ｉ
の角度を変更するにとどめてもよい。
【００４５】表示画面１３ａにリンク機構が表示された
段階で、オペレータによる、キーボード１２ないしマウ
ス１４からの指示が入力されているか否かがモニタされ
る。この指示の１つとして、リンク機構の動きを今の段
階で停止させる指示があり、その指示が入力されると、
ステップ（ｆ）の終了時点で一連の演算が中断され、表
示画面１３ａには、現在の位置および姿勢のリンク機構
が表示されたままとなる。すなわち、オペレータには、
それまで表示画面１３ａ上で動いていたリンク機構が一
旦停止するように見える。
段階で、オペレータによる、キーボード１２ないしマウ
ス１４からの指示が入力されているか否かがモニタされ
る。この指示の１つとして、リンク機構の動きを今の段
階で停止させる指示があり、その指示が入力されると、
ステップ（ｆ）の終了時点で一連の演算が中断され、表
示画面１３ａには、現在の位置および姿勢のリンク機構
が表示されたままとなる。すなわち、オペレータには、
それまで表示画面１３ａ上で動いていたリンク機構が一
旦停止するように見える。
【００４６】そのようにリンク機構の動きを一旦停止さ
せておいて、オペレータにより、逆キネマティクスが完
了したという指示、すなわち、そのリンク機構の先端が
所望の目標位置に達したという指示が入力され（ステッ
プ（ｇ））、あるいは、新たなトルクτi の入力操作
（ステップ（ｈ））が行なわれる。逆キネマティクスが
完了した旨の指示が入力されると、リンク機構解析は終
了する。
せておいて、オペレータにより、逆キネマティクスが完
了したという指示、すなわち、そのリンク機構の先端が
所望の目標位置に達したという指示が入力され（ステッ
プ（ｇ））、あるいは、新たなトルクτi の入力操作
（ステップ（ｈ））が行なわれる。逆キネマティクスが
完了した旨の指示が入力されると、リンク機構解析は終
了する。
【００４７】また、新たなトルクτi が入力され、ある
いは入力されないまま、動作の再開が指示されると、新
たなトルクτi の入力されたときは、前述の式（２）の
関節トルクτi が入力された新たな値に変更され（ステ
ップ（ｉ））、さらに、新たなトルクτi が入力された
か否かに拘らず、前述の式（２）の角速度ｑi ドットお
よび角度ｑi が、ステップ（ｄ）で直前に求められた各
関節ｉの角速度ｑi ドットおよび角度ｑi に変更され
（ステップ（ｊ））、再度ステップ（ｃ）に戻って、式
（２）に基づいて各関節ｉの角加速度ｑi ２ドットが求
められる。
いは入力されないまま、動作の再開が指示されると、新
たなトルクτi の入力されたときは、前述の式（２）の
関節トルクτi が入力された新たな値に変更され（ステ
ップ（ｉ））、さらに、新たなトルクτi が入力された
か否かに拘らず、前述の式（２）の角速度ｑi ドットお
よび角度ｑi が、ステップ（ｄ）で直前に求められた各
関節ｉの角速度ｑi ドットおよび角度ｑi に変更され
（ステップ（ｊ））、再度ステップ（ｃ）に戻って、式
（２）に基づいて各関節ｉの角加速度ｑi ２ドットが求
められる。
【００４８】次に、図６のステップ（ｃ）における角加
速度ｑi ２ドットを求める演算の詳細について説明す
る。上記の説明ではモデル的に式（２）を示したが、実
際には、以下に示すニュートン・オイラーの運動方程式
が採用される。
速度ｑi ２ドットを求める演算の詳細について説明す
る。上記の説明ではモデル的に式（２）を示したが、実
際には、以下に示すニュートン・オイラーの運動方程式
が採用される。
【００４９】
【数３】
【００５０】であり、前述した式（１），（２）の記号
との関係では、
との関係では、
【００５１】
【数４】
【００５２】である。このニュートン・オイラーの運動
方程式は、実世界の物体の動きを表わす運動方程式であ
り、この運動方程式を用いることにより、物体の実際の
運動をシミュレートすることができ、リンク機構の、よ
り自然な動きが実現でき、特異点の問題が生じることが
防止される。
方程式は、実世界の物体の動きを表わす運動方程式であ
り、この運動方程式を用いることにより、物体の実際の
運動をシミュレートすることができ、リンク機構の、よ
り自然な動きが実現でき、特異点の問題が生じることが
防止される。
【００５３】但し、ここでは、図５に示すキネマティク
ス演算装置１０内部でのシミュレーションを行なってい
るため、各リンクｉの質量等は、現実に対応するリンク
機構が存在する場合であっても、その現実のリンク機構
の各リンクの質量等とは異なる値を代入することができ
る。上記のニュートン・オイラーの運動方程式（３）
は、ここでは一本の式にまとめてあるが、１つずつの要
素を全て書きあげると極めて複雑な式で表現され、この
式（３）の形のまま数値を代入して各関節ｉの角加速度
（角加速度ベクトルｑ２ドット）を求めるのでは、多大
な時間を要し、図５に示す表示画面１３ａ上のリンク機
構をなめらかに動かすことは不可能である。そこで上記
式（３）に基づいて角加速度ベクトルｑ２ドットを高速
に演算する方法の例として、ここでは、Ｗａｌｋｅｒ
＆ Ｑｒｉｎタイプの逆動力学の再帰アルゴリズムとそ
の逆解法を用いる手法（以下、これを「逆解法手法」と
呼ぶ）と、Ｆｅａｔｈｅｒｓｔｏｎｅタイプの順動力学
の再帰アルゴリズムを用いる手法（以下、これを「順動
力学手法」と呼ぶ）について説明する。 （逆解法手法）ここでは、上述のニュートン・オイラー
の運動方程式、すなわち、
ス演算装置１０内部でのシミュレーションを行なってい
るため、各リンクｉの質量等は、現実に対応するリンク
機構が存在する場合であっても、その現実のリンク機構
の各リンクの質量等とは異なる値を代入することができ
る。上記のニュートン・オイラーの運動方程式（３）
は、ここでは一本の式にまとめてあるが、１つずつの要
素を全て書きあげると極めて複雑な式で表現され、この
式（３）の形のまま数値を代入して各関節ｉの角加速度
（角加速度ベクトルｑ２ドット）を求めるのでは、多大
な時間を要し、図５に示す表示画面１３ａ上のリンク機
構をなめらかに動かすことは不可能である。そこで上記
式（３）に基づいて角加速度ベクトルｑ２ドットを高速
に演算する方法の例として、ここでは、Ｗａｌｋｅｒ
＆ Ｑｒｉｎタイプの逆動力学の再帰アルゴリズムとそ
の逆解法を用いる手法（以下、これを「逆解法手法」と
呼ぶ）と、Ｆｅａｔｈｅｒｓｔｏｎｅタイプの順動力学
の再帰アルゴリズムを用いる手法（以下、これを「順動
力学手法」と呼ぶ）について説明する。 （逆解法手法）ここでは、上述のニュートン・オイラー
の運動方程式、すなわち、
【００５４】
【数５】
【００５５】において、先ず、
【００５６】
【数６】
【００５７】として逆動力学を解く。このとき、
【００５８】
【数７】
【００５９】には、初期値を代入する。逆動力学の解法
については後述する。その逆動力学の演算の結果求めら
れたトルクτを、τ0 とする。また、
については後述する。その逆動力学の演算の結果求めら
れたトルクτを、τ0 とする。また、
【００６０】
【数８】
【００６１】とし、
【００６２】
【数９】
【００６３】には、上記と同じ初期値を代入して、それ
ぞれ逆動力学を解く。それらの演算結果を、それぞれ、
τ1 ，τ2 ，…，τn とする。ここで、 Δτ1 ＝τ0 −τ1 Δτ2 ＝τ0 −τ2 ・・・・ Δτn ＝τ0 −τn としたとき、角加速度ベクトルｑ２ドットは、与えられ
た（キーボード１２等から入力した）各関節のトルクの
集合としてのトルクベクトルをτとしたとき、
ぞれ逆動力学を解く。それらの演算結果を、それぞれ、
τ1 ，τ2 ，…，τn とする。ここで、 Δτ1 ＝τ0 −τ1 Δτ2 ＝τ0 −τ2 ・・・・ Δτn ＝τ0 −τn としたとき、角加速度ベクトルｑ２ドットは、与えられ
た（キーボード１２等から入力した）各関節のトルクの
集合としてのトルクベクトルをτとしたとき、
【００６４】
【数１０】
【００６５】として求められる。次に、逆動力学の解法
について説明する。この逆動力学の解法は、固定された
端リンクを持つ場合、すなわち、リンク機構の一端（リ
ンク０）が、地上等に固定されている場合と、固定され
た端リンクを持たない場合、すなわち、そのリンク機構
が例えば宇宙空間に存在する場合等の２種類に分かれ
る。 （固定された端リンクを持つ場合）ここでは、図７およ
び表１に示す表記法を用いる。
について説明する。この逆動力学の解法は、固定された
端リンクを持つ場合、すなわち、リンク機構の一端（リ
ンク０）が、地上等に固定されている場合と、固定され
た端リンクを持たない場合、すなわち、そのリンク機構
が例えば宇宙空間に存在する場合等の２種類に分かれ
る。 （固定された端リンクを持つ場合）ここでは、図７およ
び表１に示す表記法を用いる。
【００６６】
【表１】
【００６７】また、以下に示す式において、例えばωi
(i)の（ｉ）は座標系Σi からみた量であることを表わ
す。さらに、（Ｒ）は回転関節の場合、（Ｓ）は直動関
節の場合であることを表わす。固定された端リンクを持
つ場合、
(i)の（ｉ）は座標系Σi からみた量であることを表わ
す。さらに、（Ｒ）は回転関節の場合、（Ｓ）は直動関
節の場合であることを表わす。固定された端リンクを持
つ場合、
【００６８】
【数１１】
【００６９】を与えてトルクベクトルτ
【００７０】
【数１２】
【００７１】を求める逆解法は、図７および表１の表記
法を用いて、以下のように定式化される。 ステップ０ 初期化
法を用いて、以下のように定式化される。 ステップ０ 初期化
【００７２】
【数１３】
【００７３】ステップ１ フォワードルーチン（ｉ＝
１，２，…，ｎの順に演算を進めるルーチン）
１，２，…，ｎの順に演算を進めるルーチン）
【００７４】
【数１４】
【００７５】ステップ２ バックワードルーチン（ｉ＝
ｎ，ｎ−１，…，１の順に演算を進めるルーチン）
ｎ，ｎ−１，…，１の順に演算を進めるルーチン）
【００７６】
【数１５】
【００７７】このアルゴリズムに従って式（１０）まで
演算を進めることにより、
演算を進めることにより、
【００７８】
【数１６】
【００７９】の初期値から、τi の集合としてのトルク
ベクトルτが求められる。 （固定された端リンクを持たない場合）この場合は、図
８および表２，表３，表４に示す表記法を用いる。
ベクトルτが求められる。 （固定された端リンクを持たない場合）この場合は、図
８および表２，表３，表４に示す表記法を用いる。
【００８０】
【表２】
【００８１】
【表３】
【００８２】
【表４】
【００８３】固定された端リンクを持たない場合、ｑ２
ドット（式（９）参照）を与えてトルクベクトルτを求
める逆解法は、図８および表２，表３，表４の表記法を
用いて、以下のように定式化される。
ドット（式（９）参照）を与えてトルクベクトルτを求
める逆解法は、図８および表２，表３，表４の表記法を
用いて、以下のように定式化される。
【００８４】
【数１７】
【００８５】
【数１８】
【００８６】
【数１９】
【００８７】
【数２０】
【００８８】上記のアルゴリズムに従って式（１１）ま
で、演算を進めることにより、ｑ２ドット，ｑドット，
ｑの初期値から、τi の集合としてのトルクベクトルτ
が求められる。 （順動力学手法）ここでは、上述のニュートン・オイラ
ーの運動方程式、すなわち、
で、演算を進めることにより、ｑ２ドット，ｑドット，
ｑの初期値から、τi の集合としてのトルクベクトルτ
が求められる。 （順動力学手法）ここでは、上述のニュートン・オイラ
ーの運動方程式、すなわち、
【００８９】
【数２１】
【００９０】において、トルクベクトルτを与えて、直
接に、角加速度ベクトルｑ２ドットを求める。ここで
も、固定された端リンク（リンク０）を持つ場合と持た
ない場合とに分けられる。 （固定された端リンクを持つ場合）ここでは、再び図７
及び表１の表記法を採用する。 ステップ０ 初期化
接に、角加速度ベクトルｑ２ドットを求める。ここで
も、固定された端リンク（リンク０）を持つ場合と持た
ない場合とに分けられる。 （固定された端リンクを持つ場合）ここでは、再び図７
及び表１の表記法を採用する。 ステップ０ 初期化
【００９１】
【数２２】
【００９２】ステップ１ フォワードルーチン（ｉ＝
１，２，…，ｎ）
１，２，…，ｎ）
【００９３】
【数２３】
【００９４】ステップ２．１ バックワードルーチンの
初期化
初期化
【００９５】
【数２４】
【００９６】
【数２５】
【００９７】ステップ２．１ バックワードルーチン
（ｉ＝ｎ，ｎ−１，…，１）
（ｉ＝ｎ，ｎ−１，…，１）
【００９８】
【数２６】
【００９９】
【数２７】
【０１００】ステップ３．１ フォワードルーチンの初
期化
期化
【０１０１】
【数２８】
【０１０２】ステップ３．２ フォワードルーチン（ｉ
＝１，２，…，ｎ）
＝１，２，…，ｎ）
【０１０３】
【数２９】
【０１０４】上記アルゴリズムに従って式（１３）まで
演算を進めることにより、トルクベクトルτから角加速
度ベクトルｑ２ドットが求められる。 （固定された端リンクを持たない場合）ここでは、再
び、表５および表２の表記法を採用する。先ず、前述し
た固定された端リンクを持たない場合のアルゴリズムス
テップ０から開始し、ステップ２．３まで進め、リンク
０の速度Ｖ0 および角速度Ω0 を求める。
演算を進めることにより、トルクベクトルτから角加速
度ベクトルｑ２ドットが求められる。 （固定された端リンクを持たない場合）ここでは、再
び、表５および表２の表記法を採用する。先ず、前述し
た固定された端リンクを持たない場合のアルゴリズムス
テップ０から開始し、ステップ２．３まで進め、リンク
０の速度Ｖ0 および角速度Ω0 を求める。
【０１０５】次に、順動力学手法の、固定された端リン
クを持つ場合のステップ２．１以降を実行する。ただ
し、このステップ２．１以降において、表記法を合わせ
るためｐ* を Ｐi
クを持つ場合のステップ２．１以降を実行する。ただ
し、このステップ２．１以降において、表記法を合わせ
るためｐ* を Ｐi
【０１０６】
【数３０】
【０１０７】と読み替え、かつ、前述した式（１２）
を、 α0 (0)＝Ｍ0 (0)-1Ｃ0 (0) …（１４） に置き換えるものとする。以上のアルゴリズムを実行す
ることにより、固定された端リンクを持たない場合にお
いても、順動力学手法により、角加速度ベクトルｑ２ド
ットが求められる。
を、 α0 (0)＝Ｍ0 (0)-1Ｃ0 (0) …（１４） に置き換えるものとする。以上のアルゴリズムを実行す
ることにより、固定された端リンクを持たない場合にお
いても、順動力学手法により、角加速度ベクトルｑ２ド
ットが求められる。
【０１０８】尚、上記各例においては、関節トルクτ
は、オペレータが、キーボード１２ないしマウス１４を
操作して入力することを想定した例であるが、オペレー
タが関節トルク２を入力することに代わり、リンク機構
上のある点（例えば先端）の目標位置を入力し、その目
標位置と現在位置との間の距離に応じたトルクを発生さ
せ、そのトルクを、オペレータが入力したトルクと同等
に取り扱ってもよい。すなわち、この場合、図６のステ
ップ（ｋ）に代えて、リンク機構の、例えば先端の現在
位置ベクトルをＰ0 、その先端の、入力された目標位置
ベクトルをＰとしたとき、関節トルクベクトルτを、例
えば式 τ＝Ａ・（Ｐ−Ｐ0 ） …（１５） 但し、Ａは角度ベクトルｑを変数とする適当なマトリッ
クスを表わす。に，従って発生させるステップを置き、
ステップ（ｉ）で、この式（１５）に従って発生させた
トルクτに変更するように構成される。
は、オペレータが、キーボード１２ないしマウス１４を
操作して入力することを想定した例であるが、オペレー
タが関節トルク２を入力することに代わり、リンク機構
上のある点（例えば先端）の目標位置を入力し、その目
標位置と現在位置との間の距離に応じたトルクを発生さ
せ、そのトルクを、オペレータが入力したトルクと同等
に取り扱ってもよい。すなわち、この場合、図６のステ
ップ（ｋ）に代えて、リンク機構の、例えば先端の現在
位置ベクトルをＰ0 、その先端の、入力された目標位置
ベクトルをＰとしたとき、関節トルクベクトルτを、例
えば式 τ＝Ａ・（Ｐ−Ｐ0 ） …（１５） 但し、Ａは角度ベクトルｑを変数とする適当なマトリッ
クスを表わす。に，従って発生させるステップを置き、
ステップ（ｉ）で、この式（１５）に従って発生させた
トルクτに変更するように構成される。
【０１０９】このように構成した場合、オペレータによ
り関節トルクをいちいち入力する手間が省かれる。ま
た、上記各例においては、トルクベクトルτを与えて各
加速度ベクトルｑ２ドットを求めるとして説明したが、
トルクベクトルτを与えると共に、各リンクに作用する
力およびモーメントを変化させてもよい。この、リンク
に作用させる力およびモーメントを変化させるには、前
述した式（２）においては、係数ｋi, j を変化させるこ
とがそれに相当し、表１の表記法においてはＦi ，Ｎi
を変化させるがそれに相当し、表２〜表４の表記法にお
いては、Ｆi (j) ，Ｎi (j) を変化させることがそれに
相当する。
り関節トルクをいちいち入力する手間が省かれる。ま
た、上記各例においては、トルクベクトルτを与えて各
加速度ベクトルｑ２ドットを求めるとして説明したが、
トルクベクトルτを与えると共に、各リンクに作用する
力およびモーメントを変化させてもよい。この、リンク
に作用させる力およびモーメントを変化させるには、前
述した式（２）においては、係数ｋi, j を変化させるこ
とがそれに相当し、表１の表記法においてはＦi ，Ｎi
を変化させるがそれに相当し、表２〜表４の表記法にお
いては、Ｆi (j) ，Ｎi (j) を変化させることがそれに
相当する。
【０１１０】このリンクに力およびモーメントを作用さ
せるには、オペレータがキーボード１２ないしマウス１
４を操作して、力およびモーメントを作用させるリン
ク、そのリンク上の力を加えるポイント、力を加える向
き、および加える力の大きさを指定してもよいが、その
リンク機構が配置された仮想空間内に仮想物体を配置す
る手法が便利である。
せるには、オペレータがキーボード１２ないしマウス１
４を操作して、力およびモーメントを作用させるリン
ク、そのリンク上の力を加えるポイント、力を加える向
き、および加える力の大きさを指定してもよいが、その
リンク機構が配置された仮想空間内に仮想物体を配置す
る手法が便利である。
【０１１１】図９は、リンク機構が配置された仮想空間
内に仮想物体を配置した様子を示す模式図、図１０は、
リンク機構を構成する１つのリンクと仮想物体とが干渉
した状態を示す模式図である。図９に示すようにリンク
機構の移動範囲内に仮想物体を配置し、リンク機構を移
動させ、図１０に示すように、リンク機構を構成するあ
る１つのリンクと仮想物体とが、それらの間の相対速度
ｖをもって干渉量δだけ干渉したとする。このとき、干
渉力Ｆを、例えば式 Ｆ＝ｋδ＋βｖ …（１６） 但し、ｋ，βは各定数を表わす に従って発生させ、その力Ｆよびその力Ｆにより生じる
モーメントＮを演算に反映させる。
内に仮想物体を配置した様子を示す模式図、図１０は、
リンク機構を構成する１つのリンクと仮想物体とが干渉
した状態を示す模式図である。図９に示すようにリンク
機構の移動範囲内に仮想物体を配置し、リンク機構を移
動させ、図１０に示すように、リンク機構を構成するあ
る１つのリンクと仮想物体とが、それらの間の相対速度
ｖをもって干渉量δだけ干渉したとする。このとき、干
渉力Ｆを、例えば式 Ｆ＝ｋδ＋βｖ …（１６） 但し、ｋ，βは各定数を表わす に従って発生させ、その力Ｆよびその力Ｆにより生じる
モーメントＮを演算に反映させる。
【０１１２】この手法を採用すると、オペレータはマニ
ピュレータの冗長性等を監視しながら逆キネマテクスを
決定することができる。また、この手法を採用すると、
リンクに作用する力およびモーメントを変化させる役割
を仮想物体が担い、リンクに作用する力およびモーメン
トをオペレータがいちいち入力する手間が省かれる。図
１１は、閉ループを持つリンク機構の模式図である。
ピュレータの冗長性等を監視しながら逆キネマテクスを
決定することができる。また、この手法を採用すると、
リンクに作用する力およびモーメントを変化させる役割
を仮想物体が担い、リンクに作用する力およびモーメン
トをオペレータがいちいち入力する手間が省かれる。図
１１は、閉ループを持つリンク機構の模式図である。
【０１１３】このような場合、その閉ループをある場所
で切断したと考え、その切断点での拘束条件を力および
モーメントとして付加することにより、前述の演算法を
そのまま採用することができる。ここでは、破線Ａに沿
ってリンクの途中を切断した場合と、破線Ｂに沿って関
節の位置で切断した場合について説明する。
で切断したと考え、その切断点での拘束条件を力および
モーメントとして付加することにより、前述の演算法を
そのまま採用することができる。ここでは、破線Ａに沿
ってリンクの途中を切断した場合と、破線Ｂに沿って関
節の位置で切断した場合について説明する。
【０１１４】（リンクの途中を切断する場合）このと
き、拘束条件として、切断部分での位置・姿勢のずれに
比例した力およびモーメントと、相対的な速度、角速度
に比例した力およびモーメントが発生するという条件を
考える。例えば、図１１に示すｎ系とｍ系を考え、ｎ系
での力、モーメントをｆn+1 ｎ n+1 、ｍ系での力、モー
メントをｆm+1 ，ｎm+1 とする。このとき、 ｆn+1 ＝Ａf n（Ｐn −Ｐm ） ｎn+1 ＝Ａn n（Ｐn −Ｐm ） ｆm+1 ＝Ａf m（Ｐn −Ｐm ） ｎm+1 ＝Ａn m（Ｐn −Ｐm ） 但し、Ａf n，Ａn n，Ａf m，Ａn mは各定数を表わす。とし
て、例えば、前述の順動力学手法により各加速度ベクト
ルｑ２ドットを求めればよい。 （関節の位置で切断する場合）関節を直接切断すると考
えると、この手法は適用できない。そこで、その関節の
位置に例えば長さが０の仮想リンクを考え、その仮想リ
ンクを切断すると考える。
き、拘束条件として、切断部分での位置・姿勢のずれに
比例した力およびモーメントと、相対的な速度、角速度
に比例した力およびモーメントが発生するという条件を
考える。例えば、図１１に示すｎ系とｍ系を考え、ｎ系
での力、モーメントをｆn+1 ｎ n+1 、ｍ系での力、モー
メントをｆm+1 ，ｎm+1 とする。このとき、 ｆn+1 ＝Ａf n（Ｐn −Ｐm ） ｎn+1 ＝Ａn n（Ｐn −Ｐm ） ｆm+1 ＝Ａf m（Ｐn −Ｐm ） ｎm+1 ＝Ａn m（Ｐn −Ｐm ） 但し、Ａf n，Ａn n，Ａf m，Ａn mは各定数を表わす。とし
て、例えば、前述の順動力学手法により各加速度ベクト
ルｑ２ドットを求めればよい。 （関節の位置で切断する場合）関節を直接切断すると考
えると、この手法は適用できない。そこで、その関節の
位置に例えば長さが０の仮想リンクを考え、その仮想リ
ンクを切断すると考える。
【０１１５】図１２〜図１４は、関節の位置に仮想リン
クを考え、その仮想リンクを切断すると考えた場合の各
例を示した図であり、これらの図のうち、図１２，図１
３には回転タイプの関節の位置に仮想リンクを考え、そ
の仮想リンクを切断したと考えた例、図１４は直動タイ
プの関節の位置に仮想リンクを考え、その仮想リンクを
したと考えた例である。
クを考え、その仮想リンクを切断すると考えた場合の各
例を示した図であり、これらの図のうち、図１２，図１
３には回転タイプの関節の位置に仮想リンクを考え、そ
の仮想リンクを切断したと考えた例、図１４は直動タイ
プの関節の位置に仮想リンクを考え、その仮想リンクを
したと考えた例である。
【０１１６】これらの例に示すように、関節の位置での
切断をリンクの切断に置き換えることにより、実質的に
関節の位置で開ループを切断することもできる。
切断をリンクの切断に置き換えることにより、実質的に
関節の位置で開ループを切断することもできる。
【０１１７】
【発明の効果】以上説明したように、本発明によれば、
特異点の問題が生じることなく、リンク機構の自然な動
きが実現できる。
特異点の問題が生じることなく、リンク機構の自然な動
きが実現できる。
【図１】複数のリンクが関節を介して連結されたリンク
機構の例を示した図である。
機構の例を示した図である。
【図２】本発明のキネマティクス演算方法の基本フロー
チャートである。
チャートである。
【図３】本発明の第１のキネマティクス演算装置の原理
ブロック図である。
ブロック図である。
【図４】本発明の第２のキネマティクス演算装置の原理
ブロック図である。
ブロック図である。
【図５】本発明のキネマティクス演算装置の一実施形態
の外観構成図である。
の外観構成図である。
【図６】図５に示すキネマティクス演算装置の内部で実
行される、本発明のキネマティクス演算方法の一実施形
態を表わすフローチャートである。
行される、本発明のキネマティクス演算方法の一実施形
態を表わすフローチャートである。
【図７】表記法の説明図である。
【図８】表記法の説明図である。
【図９】リンク機構が配置された仮想空間内に仮想物体
を配置した様子を示す模式図である。
を配置した様子を示す模式図である。
【図１０】リンク機構を構成する１つのリンクと仮想物
体とが干渉した状態を示す模式図である。
体とが干渉した状態を示す模式図である。
【図１１】閉ループを持つリンク機構の模式図である。
【図１２】回転タイプの関節の位置に仮想リンクを考
え、その仮想リンクを切断したと考えた例を示す図であ
る。
え、その仮想リンクを切断したと考えた例を示す図であ
る。
【図１３】回転タイプの関節の位置に仮想リンクを考
え、その仮想リンクを切断したと考えた例を示す図であ
る。
え、その仮想リンクを切断したと考えた例を示す図であ
る。
【図１４】直動タイプの関節の位置に仮想リンクを考
え、その仮想リンクをしたと考えた例を示す図である。
え、その仮想リンクをしたと考えた例を示す図である。
１ 演算手段 ２ 入力手段 ３ 数値分析手段 ４ キネマティクス手段 ５ 目標位置入力手段 ６ 設定手段 ７ 仮想物体配置手段 ８ 表示手段 １０ キネマティクス演算装置 １１ 本体部 １２ キーボード １３ ＣＲＴディスプレイ １３ａ 表示画面 １４ マウス
Claims (18)
- 【請求項１】 複数のリンクが関節を介して連結された
リンク機構のリンクの位置および姿勢を求めるキネマテ
ィクス演算方法において、 関節の、角度ないし変位、角速度ないし速度、角加速度
ないし加速度、およびトルクないし力の間の関係を規定
する関係式に、関節の、角度ないし変位の初期値、角速
度ないし速度の初期値、およびトルクないし力の初期値
を数値代入して、関節の角加速度ないし加速度を求める
第１ステップと、 前記関係式に、関節の、角度ないし変位、角速度ないし
速度、およびトルクないし力を数値代入することにより
求められた関節の角加速度ないし加速度を数値積分する
ことにより、関節の角速度ないし速度、および角度ない
し変位を求める第２ステップと、 前記第２ステップで求められた関節の角速度ないし速
度、および角度ないし変位、並びに、関節の、前記トル
クないし力の初期値、ないし該初期値に代わる変更され
たトルクないし力を前記関係式に数値代入して関節の角
加速度ないし加速度を求める第３ステップと、 前記第２ステップで求められた関節の角度ないし変位に
基づいてリンクの位置および姿勢を求める第４ステップ
とを有し、 前記第２ステップと前記第３ステップとを交互に繰り返
すとともに、前記第４ステップを繰り返し実行すること
によりリンクの位置および姿勢を順次更新することを特
徴とするキネマティクス演算方法。 - 【請求項２】 前記関係式が、関節の、角度ないし変
位、角速度ないし速度、角加速度ないし加速度、および
トルクないし力に加え、さらに、リンクに作用する力お
よびモーメントの間の関係を定める関係式であって、 前記第１ステップが、関節の、角度ないし変位の初期
値、角速度ないし速度の初期値およびトルクないし力の
初期値に加え、さらに、リンクに作用する力およびモー
メントの初期値を前記関係式に数値代入して、関節の角
加速度ないし加速度を求めるステップであり、 前記第３ステップが、前記第２ステップで求められた関
節の角速度ないし速度、および角度ないし変位、およ
び、関節の、前記トルクないし力の初期値、ないし該初
期値に代わる変更されたトルクないし力に加え、さら
に、前記リンクに作用する力およびモーメントの初期
値、ないし該初期値に代わる変更された力およびモーメ
ントを前記関係式に数値代入して、関節の角加速度ない
し加速度を求めるステップであることを特徴とする請求
項１記載のキネマティクス演算方法。 - 【請求項３】 前記リンク機構の動作領域内に仮想物体
を配置したときの該仮想物体と該リンク機構のリンクと
の干渉力を該リンクに作用する力およびモーメントとし
て前記関係式に数値代入することを特徴とする請求項２
記載のキネマティクス演算方法。 - 【請求項４】 前記リンク機構が閉ループを含むリンク
機構である場合に、該閉ループを所定位置で仮想的に切
断し該切断により生成された両端間に該両端相互の物理
量のずれに応じた力およびモーメントを作用させたとき
の該力およびモーメントを、該両端それぞれを有する２
つのリンクに作用する力およびモーメントとして数値代
入することを特徴とする請求項２記載のキネマティクス
演算方法。 - 【請求項５】 前記第１ステップおよび前記第３ステッ
プが、前記関係式としてニュートン・オイラーの運動方
程式を用いて関節の角加速度ないし加速度を求めるステ
ップであることを特徴とする請求項１記載のキネマティ
クス演算方法。 - 【請求項６】 前記第１ステップおよび前記第３ステッ
プが、ニュートン・オイラーの運動方程式に基づく、関
節の角加速度ないし加速度から関節のトルクないし力を
求める漸化式と、該漸化式により求められたトルクない
し力から関節の角加速度ないし加速度を求める逆解法と
を用いて、関節の角加速度ないし加速度を求めるステッ
プであることを特徴とする請求項５記載のキネマティク
ス演算方法。 - 【請求項７】 前記第１ステップおよび前記第３ステッ
プが、ニュートン・オイラーの運動方程式に基づく、関
節のトルクないし力から関節の角加速度ないし加速度を
求める漸化式を用いて、関節の角加速度ないし加速度を
求めるステップであることを特徴とする請求項５記載の
キネマティクス演算方法。 - 【請求項８】 前記第３ステップが、前記第２ステップ
で求められた関節の角速度ないし速度、および角度ない
し変位、並びに、関節の、該リンク機構先端の現在位置
と目標位置との間の距離に応じたトルクを前記関係式に
数値代入して、関節の角加速度ないし加速度を求めるス
テップであることを特徴とする請求項１記載のキネマテ
ィクス演算方法。 - 【請求項９】 前記リンク機構を表示する表示画面上
に、前記第４ステップで求められた位置および姿勢を有
するリンクからなるリンク機構を表示する第５ステップ
を有することを特徴とする請求項１記載のキネマティク
ス演算方法。 - 【請求項１０】 複数のリンクが関節を介して連結され
たリンク機構のリンクの位置および姿勢を求めるキネマ
ティクス演算装置において、 関節の、角度ないし変位、角速度ないし速度、角加速度
ないし角速度、およびトルクないし力の間の関係を規定
する関係式に、関節の、角度ないし変位、角速度ないし
速度、およびトルクないし力を数値代入して、関節の角
加速度ないし加速度を求める演算手段と、 関節のトルクないし力を、変更自在に、前記演算手段に
入力する入力手段と、 前記演算手段により求められた関節の角加速度ないし加
速度を数値積分することにより関節の角速度ないし速
度、および角度ないし変位を求めて前記演算手段に入力
する数値積分手段と、 前記数値積分手段で求められた関節の角度ないし変位に
基づいてリンクの位置および姿勢を求めるキネマティク
ス手段とを備えたことを特徴とするキネマティクス演算
装置。 - 【請求項１１】 複数のリンクが関節を介して連結され
たリンク機構のリンクの位置および姿勢を求めるキネマ
ティクス演算装置において、 関節の、角度ないし変位、角速度ないし速度、角加速度
ないし加速度、およびトルクないし力の間の関係を規定
する関係式に、関節の、角度ないし変位、角速度ないし
速度、およびトルクないし力を数値代入して、関節の角
加速度ないし加速度を求める演算手段と、 該リンク機構先端の目標位置を入力する入力手段と、 関節の、該リンク機構先端の現在位置と、前記入力手段
により入力された目標位置との間の距離に応じたトルク
を設定して前記演算手段に入力するトルク設定手段と、 前記演算手段により求められた関節の角加速度ないし加
速度を数値積分することにより関節の角速度ないし速
度、および角度ないし変位を求めて前記演算手段に入力
する数値積分手段と、 前記数値積分手段で求められた関節の角度ないし変位に
基づいてリンクの位置および姿勢を求めるキネマティク
ス手段とを備えたことを特徴とするキネマティクス演算
装置。 - 【請求項１２】 前記関係式が、関節の、角度ないし変
位、角速度ないし速度、角加速度ないし加速度、および
トルクないし力に加え、さらに、リンクに作用する力お
よびモーメントの間の関係を定める関係式であって、前
記演算手段が、関節の、角度ないし変位、角速度ないし
速度、およびトルクないし力に加え、さらに、リンクに
作用する力およびモーメントを前記関係式に数値代入し
て、関節の角加速度ないし加速度を求めるものであるこ
とを特徴とする請求項１０又は１１記載のキネマティク
ス演算装置。 - 【請求項１３】 前記リンク機構の動作領域内に仮想物
体を配置する仮想物体配置手段を備え、 前記演算手段が、該仮想物体と該リンク機構のリンクと
の干渉力および該干渉力に起因するモーメントを該リン
クに作用する力およびモーメントとして前記関係式に数
値代入するものであることを特徴とする請求項１２記載
のキネマティクス演算装置。 - 【請求項１４】 前記演算手段が、前記リンク機構が閉
ループを含むリンク機構である場合に、該閉ループを所
定位置で切断し該切断により生成された両端間に該両端
相互の物理量のずれに応じた力およびモーメントを作用
させたときの該力およびモーメントを、該両端それぞれ
を有する２つのリンクに作用する力およびモーメントと
して前記関係式に数値代入するものであることを特徴と
する請求項１２記載のキネマティクス演算装置。 - 【請求項１５】 前記演算手段が、前記関係式としてニ
ュートン・オイラーの運動方程式を用いて関節の角加速
度ないし加速度を求めるものであることを特徴とする請
求項１０又は１１記載のキネマティクス演算装置。 - 【請求項１６】 前記演算手段が、ニュートン・オイラ
ーの運動方程式に基づく、関節の角加速度ないし加速度
から関節のトルクを求める漸化式と、該漸化式により求
められたトルクから関節の角加速度ないし加速度を求め
る逆解法とを用いて、関節の角加速度ないし加速度を求
めるものであることを特徴とする請求項１５記載のキネ
マティクス演算装置。 - 【請求項１７】 前記演算手段が、ニュートン・オイラ
ーの運動方程式に基づく、関節のトルクないし力から関
節の角加速度ないし加速度を求める漸化式を用いて、関
節の角加速度ないし加速度を求めるものであることを特
徴とする請求項１５記載のキネマティクス演算装置。 - 【請求項１８】 前記キネマティクス演算手段で求めら
れた位置および姿勢を有するリンクからなるリンク機構
を表示する表示手段を備えたことを特徴とする請求項１
０又は１１記載のキネマティクス演算装置。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP30178695A JP3247832B2 (ja) | 1995-11-20 | 1995-11-20 | キネマティクス演算装置 |
| US08/682,111 US6027238A (en) | 1995-11-20 | 1996-07-17 | Kinematic arithmetic method and apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP30178695A JP3247832B2 (ja) | 1995-11-20 | 1995-11-20 | キネマティクス演算装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH09146996A true JPH09146996A (ja) | 1997-06-06 |
| JP3247832B2 JP3247832B2 (ja) | 2002-01-21 |
Family
ID=17901163
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP30178695A Expired - Fee Related JP3247832B2 (ja) | 1995-11-20 | 1995-11-20 | キネマティクス演算装置 |
Country Status (2)
| Country | Link |
|---|---|
| US (1) | US6027238A (ja) |
| JP (1) | JP3247832B2 (ja) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011013012A (ja) * | 2009-06-30 | 2011-01-20 | A & D Co Ltd | 車輪位置計測装置 |
| JP2013521137A (ja) * | 2010-03-02 | 2013-06-10 | コミシリア ア レネルジ アトミック エ オ エナジーズ オルタネティヴズ | 輻輳した環境におけるロボット型機械の操縦を支援する方法及びシステム |
| JP2017213319A (ja) * | 2016-06-02 | 2017-12-07 | 株式会社ブリヂストン | 関節トルクの測定方法、逆動力学計算システム及び関節トルクの測定プログラム |
| CN113263503A (zh) * | 2021-07-19 | 2021-08-17 | 上海捷勃特机器人有限公司 | 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法 |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102006015981B4 (de) * | 2005-08-19 | 2014-02-13 | Siemens Aktiengesellschaft | Verfahren zum Ermitteln des Verhaltens von zum Zwecke der Bewegung eines Werkzeugs oder einer Werkzeugaufnahme im Verbund bewegbaren Maschinenachsen einer mehrachsigen Maschine |
| JP5090045B2 (ja) * | 2007-04-03 | 2012-12-05 | テルモ株式会社 | マニピュレータ及びその制御方法 |
| CN102678861B (zh) * | 2012-02-20 | 2014-09-03 | 浙江大学 | 一种确定铰接杆系机构奇异构型处相应自由节点运动方式的方法 |
| CN102678862B (zh) * | 2012-02-20 | 2014-09-03 | 浙江大学 | 一种确定铰接杆系机构运动奇异构型的方法 |
| JP5792226B2 (ja) * | 2013-05-29 | 2015-10-07 | 株式会社神戸製鋼所 | 多関節リンク機構の逆運動学解法、及びこの逆運動学解法を利用した教示データ作成装置 |
| CN105014666B (zh) * | 2015-07-13 | 2016-10-05 | 广州霞光技研有限公司 | 一种多自由度机械手自主抓取逆解工程算法 |
| CN109591023B (zh) * | 2018-12-30 | 2020-09-08 | 深圳市优必选科技有限公司 | 串联机器人的迭代计算方法及串联机器人 |
| CN113158375B (zh) * | 2021-04-26 | 2024-03-19 | 合肥工业大学 | 一种凸轮五连杆机构快速优化设计与评估方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4547888A (en) * | 1982-07-28 | 1985-10-15 | Motorola, Inc. | Recursive adaptive equalizer for SMSK data links |
| US4925312A (en) * | 1988-03-21 | 1990-05-15 | Staubli International Ag | Robot control system having adaptive feedforward torque control for improved accuracy |
| US5430643A (en) * | 1992-03-11 | 1995-07-04 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Configuration control of seven degree of freedom arms |
-
1995
- 1995-11-20 JP JP30178695A patent/JP3247832B2/ja not_active Expired - Fee Related
-
1996
- 1996-07-17 US US08/682,111 patent/US6027238A/en not_active Expired - Lifetime
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011013012A (ja) * | 2009-06-30 | 2011-01-20 | A & D Co Ltd | 車輪位置計測装置 |
| JP2013521137A (ja) * | 2010-03-02 | 2013-06-10 | コミシリア ア レネルジ アトミック エ オ エナジーズ オルタネティヴズ | 輻輳した環境におけるロボット型機械の操縦を支援する方法及びシステム |
| JP2017213319A (ja) * | 2016-06-02 | 2017-12-07 | 株式会社ブリヂストン | 関節トルクの測定方法、逆動力学計算システム及び関節トルクの測定プログラム |
| CN113263503A (zh) * | 2021-07-19 | 2021-08-17 | 上海捷勃特机器人有限公司 | 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法 |
| CN113263503B (zh) * | 2021-07-19 | 2021-11-09 | 上海捷勃特机器人有限公司 | 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP3247832B2 (ja) | 2002-01-21 |
| US6027238A (en) | 2000-02-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8140189B2 (en) | Apparatus and method for computing operational-space physical quantity | |
| Rulka | SIMPACK—A computer program for simulation of large-motion multibody systems | |
| US9031699B2 (en) | Kinematic predictor for articulated mechanisms | |
| Geike et al. | Inverse dynamic analysis of parallel manipulators with full mobility | |
| JP3247832B2 (ja) | キネマティクス演算装置 | |
| Görgülü et al. | Time efficient stiffness model computation for a parallel haptic mechanism via the virtual joint method | |
| CN112549017A (zh) | 避免关节极限的双臂机器人协作空间求解方法 | |
| Akdağ et al. | An integrated approach for simulation of mechatronic systems applied to a hexapod robot | |
| Hess-Coelho et al. | Modular modelling methodology applied to the dynamic analysis of parallel mechanisms | |
| Fisette et al. | Fully symbolic generation of complex multibody models | |
| Turner | Programming dexterous manipulation by demonstration | |
| Sadanand et al. | Virtual robots module: an effective visualization tool for robotics toolbox | |
| Schlotter | Multibody system simulation with SimMechanics | |
| Fennel et al. | Haptic rendering of arbitrary serial manipulators for robot programming | |
| Flückiger | A robot interface using virtual reality and automatic kinematics generator | |
| Chou et al. | Graph-theoretic models for simulating robot manipulators | |
| Benitez et al. | A 3D simulation environment for kinematic task of the PUMA 560 robot | |
| Yae et al. | Operator-in-the-loop simulation of a redundant manipulator under teleoperation | |
| Rodríguez et al. | A framework for the simulation and haptic display of dynamic systems subject to holonomic constraints | |
| Sychova | Motion Control of the Angular Type Educational Manipulator Using Visual Components | |
| CN117621041A (zh) | 生成机器人的动作路径的装置、方法以及记录介质 | |
| Rolland | Introduction to algebraic methods for solving the forward kinematics problem of parallel robots applied to high throughput and high accuracy | |
| JPH01111208A (ja) | ロボット教示装置 | |
| Ramadorai et al. | A generic kinematics software package | |
| Halme et al. | A Task level control and simulation system for interactive robotics |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20010703 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20011023 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20081102 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20081102 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091102 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101102 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101102 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111102 Year of fee payment: 10 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111102 Year of fee payment: 10 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121102 Year of fee payment: 11 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121102 Year of fee payment: 11 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131102 Year of fee payment: 12 |
|
| LAPS | Cancellation because of no payment of annual fees |