JP2018030210A - Simulation device, control system, robot system, simulation method, program and recording medium - Google Patents
Simulation device, control system, robot system, simulation method, program and recording medium Download PDFInfo
- Publication number
- JP2018030210A JP2018030210A JP2016164833A JP2016164833A JP2018030210A JP 2018030210 A JP2018030210 A JP 2018030210A JP 2016164833 A JP2016164833 A JP 2016164833A JP 2016164833 A JP2016164833 A JP 2016164833A JP 2018030210 A JP2018030210 A JP 2018030210A
- Authority
- JP
- Japan
- Prior art keywords
- robot
- force
- contact
- jacobian
- joint
- 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.)
- Pending
Links
Images
Abstract
Description
本発明は、拘束を有するロボットのシミュレーションの技術に関する。 The present invention relates to a technique for simulating a robot having constraints.
近年、ロボット開発において、ロボットの動作を詳細にシミュレーションするために、様々な技術開発が行われている。拘束力の導入もその一つであり、パラレルリンクを用いたロボットのシミュレーションにおいて高速な演算が可能となっている。 In recent years, in the development of robots, various technical developments have been performed in order to simulate the robot operation in detail. The introduction of restraining force is one of them, and high-speed computation is possible in a robot simulation using a parallel link.
ここで、拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。拘束力の一例として、ギヤとギヤとが速度伝達比ＺＡ：ＺＢで伝達する力が挙げられる。また、拘束力の他の例として、物体上の任意点の自由度を、別の物体の座標系に拘束する力が挙げられる。この他の例では、例えば物体上の任意点の並進自由度を、別の物体に拘束する拘束力を計算することで、物体と別の物体を球面軸で繋いだような動作をシミュレーションすることができ、閉リンクロボットのシミュレーションが可能となる。 Here, the restraining force refers to a force that restrains the movement of another object with respect to a certain object. As an example of the restraining force, there is a force transmitted between the gears at a speed transmission ratio Z A : Z B. Another example of the restraining force is a force that restrains the degree of freedom of an arbitrary point on an object to the coordinate system of another object. In this other example, for example, by calculating the constraint force that constrains the translational freedom of an arbitrary point on an object to another object, the motion of connecting the object and another object with a spherical axis is simulated. It is possible to simulate a closed link robot.
一方、ロボットは、ワークの搬送や組み立てなどの生産ラインにも適用されている。また、ギヤやパラレルリンク機構などのロボットを、接触力のみを用いてシミュレーションする方法もある。このようなロボットの動作をシミュレーションするには、接触力の計算が必要となる。接触力の計算方式に関しては、従来はモデル同士の侵入量から接触力を疑似的に計算する方式が主流であったが、現在は解析的でより現実に即した計算方式が提案されている。 On the other hand, robots are also applied to production lines such as workpiece transfer and assembly. There is also a method of simulating a robot such as a gear or a parallel link mechanism using only the contact force. In order to simulate the operation of such a robot, it is necessary to calculate the contact force. Regarding the contact force calculation method, a method for calculating the contact force in a pseudo manner based on the amount of intrusion between models has been the mainstream. However, an analytical and more realistic calculation method has been proposed at present.
ロボットの接触力の計算方法の従来例として非特許文献１が知られている。非特許文献１では、ロボットに作用する接触力を、コリオリ力、遠心力項などの影響を考慮して算出する方法について記載されている。まず、接触点におけるロボットの運動方程式を導出し、次に、接触点が満たすべき条件を考慮して接触力を算出している。ここで、満たすべき条件とは、接触点における接触力の向き、接触点における速度、法線方向の接触力と接線方向の接触力との関係などである。また、接触力を算出する方法として、ガウスサイデル法に似た繰り返し計算を用いて、満たすべき条件に合致した接触力を計算している。 Non-Patent Document 1 is known as a conventional example of a method for calculating a contact force of a robot. Non-Patent Document 1 describes a method for calculating a contact force acting on a robot in consideration of influences such as a Coriolis force and a centrifugal force term. First, the equation of motion of the robot at the contact point is derived, and then the contact force is calculated in consideration of the condition that the contact point should satisfy. Here, the conditions to be satisfied are the direction of the contact force at the contact point, the speed at the contact point, the relationship between the contact force in the normal direction and the contact force in the tangential direction, and the like. In addition, as a method for calculating the contact force, a contact force that meets a condition to be satisfied is calculated using an iterative calculation similar to the Gauss-Sider method.
しかしながら、上記非特許文献１に記載の方法では、ロボットの接触力を求める際に、関節における拘束力については考慮されていなかった。そのため、拘束（具体的には速度拘束）を有するロボットがワークと接触するような場合のシミュレーションを行うことができなかった。 However, in the method described in Non-Patent Document 1, the binding force at the joint is not considered when the contact force of the robot is obtained. For this reason, it has not been possible to perform a simulation in a case where a robot having a constraint (specifically, a velocity constraint) comes into contact with a workpiece.
また、パラレルリンク機構などの拘束を有するロボットを、接触力を用いてシミュレーションする方法では、ロボットの拘束機構についても接触点の接触力について連立方程式を立てる必要がある。拘束機構については多数の接触点が存在するため、接触点の数が増えるほど連立方程式の数が増えることになり、拘束を有するロボットについて接触点のみでシミュレーションを行う場合、計算負荷が高く、多くの計算時間が必要であった。 In the method of simulating a robot having a constraint such as a parallel link mechanism using the contact force, it is necessary to establish simultaneous equations for the contact force at the contact point for the constraint mechanism of the robot. Since there are many contact points for the restraint mechanism, the number of simultaneous equations increases as the number of contact points increases. Calculation time was required.
そこで、本発明は、ロボットの接触力と拘束力とを共に計算し、計算負荷を低減することを目的とする。 Therefore, an object of the present invention is to calculate both the contact force and the restraining force of a robot and reduce the calculation load.
関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出処理と、
前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出処理と、を実行することを特徴とする。
A simulation device including a calculation unit for calculating a contact force with which an object comes into contact with the robot and a binding force in the joint of the robot, which is used for simulating an operation of a robot having a joint,
The computing unit is
In a joint space where the number of joints of the robot is a degree of freedom, when the speed of the joints of the robot is q (•), the robot has a degree of freedom based on the constraint conditions of the joints of the robot, and restrains the restraint points of the robot. a restraining force f a in a first coordinate space relative to the direction, a first Jacobian J a showing the relationship between the binding tau a in the joint space,
Based on the coordinate value of the contact point between the robot and the object in the world coordinate system, the contact force f in the second coordinate space represented by orthogonal coordinates with reference to the normal direction and the tangential direction of the contact point of the robot. A second Jacobian calculation process for calculating a second Jacobian J B indicating the relationship between B and the contact force τ B in the joint space;
A third Jacobian calculation process for obtaining the third Jacobian J the combined first Jacobian J A and the second Jacobian J B,
In the joint space, the acceleration of the joint of the robot is q (··), the mass matrix relating to the mass of the robot is A, the Coriolis force acting on the robot and the Coriolis force / centrifugal force term relating to the centrifugal force are b, A gravity term related to the working gravity is g, and a generated force generated in the joint of the robot is τ act, and the first motion equation of the robot in the joint space
The third equation of motion is obtained by integrating the second equation of motion in the time domain and linearizing the predetermined time Δt.
The linearized third equation of motion is a simultaneous equation with the binding force f A and the contact force f B as unknowns, and the binding force f A and the contact force f B are calculated by solving the simultaneous equations. Performing force calculation processing.
本発明によれば、ロボットが物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。 According to the present invention, when simulating a scene in which a robot comes into contact with an object, a joint that is a restraining mechanism is modeled by a restraining force, so that a joint that is a restraining mechanism is modeled by a contact force. The number of forces to be determined by the equation of motion is reduced. Therefore, the calculation load is reduced and high-speed simulation is possible.
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、実施形態に係るロボットシステムを示す説明図である。図１に示すように、ロボットシステム１００は、ロボット２００と、制御システム６００と、を備えている。
Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings. FIG. 1 is an explanatory diagram illustrating a robot system according to an embodiment. As shown in FIG. 1, the
ロボット２００は、垂直多関節のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２０２と、を備えている。
The
ロボットアーム２０１の基端は、台座１５０に固定された固定端である。ロボットアーム２０１の先端は、自由端である。ロボットアーム２０１は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に、手先（ロボットアーム２０１の先端）を向けることができる。
The base end of the
ロボットアーム２０１は、複数の関節、例えば６つ関節Ｊ１〜Ｊ６を有している。ロボットアーム２０１は、各関節Ｊ１〜Ｊ６をそれぞれ回転駆動する複数（６つ）の駆動機構を有している。
The
ロボットアーム２０１は、複数のリンク２１００〜２１０６を有し、複数のリンク２１００〜２１０６が関節Ｊ１〜Ｊ６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１００〜２１０６が順に直列に連結されている。
The
図２は、ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。以下、関節Ｊ２を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６については、同様の構成であるため、説明を省略する。 FIG. 2 is a partial cross-sectional view showing one joint among a plurality of joints of the robot arm. Hereinafter, the joint J 2 will be described as an example, and the other joints J 1 and J 3 to J 6 have the same configuration, and thus the description thereof is omitted.
関節Ｊ２を駆動する駆動機構２３０は、駆動源である電動モータ（以下、「モータ」という）２３１と、モータ２３１の出力を減速する減速機２３３と、を有している。
The
モータ２３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ２３１の回転軸２３２の回転が減速機２３３で減速されて関節（リンク）を駆動する。例えば減速機２３３の減速比が５０：１であれば、モータ２３１が５０回転で減速機２３３の出力側に配置されているリンク２１０２が１回転する。また、リンク２１０１とリンク２１０２とは、クロスローラベアリング２３７を介して回転自在に結合されている。
The
減速機２３３は、本実施形態では波動歯車減速機である。減速機２３３は、モータ２３１の回転軸２３２に結合された、入力軸であるウェブジェネレータ２４１と、リンク２１０２に固定された、出力軸であるサーキュラスプライン（ギヤ）２４２と、を備えている。なお、サーキュラスプライン２４２は、リンク２１０２に直結されているが、リンク２１０２に一体に形成されていてもよい。
The
また、減速機２３３は、ウェブジェネレータ２４１とサーキュラスプライン２４２との間に配置され、リンク２１０１に固定されたフレクスプライン（ギヤ）２４３を備えている。フレクスプライン２４３は、ウェブジェネレータ２４１の回転に対して減速比Ｎで減速され、サーキュラスプライン２４２に対して相対的に回転する。従って、モータ２３１の回転は、減速機２３３で１／Ｎの減速比で減速されて、フレクスプライン２４３が固定されたリンク２１０１に対してサーキュラスプライン２４２が固定されたリンク２１０２を相対的に回転運動させ、関節Ｊ２を回転させる。
Also, the
図３は、実施形態に係る制御システムを示すブロック図である。制御システム６００は、ロボット２００（図１）の動作を制御するロボット制御装置３００と、シミュレーションシステム５５０と、を有している。シミュレーションシステム５５０は、コンピュータで構成されたシミュレーション装置５００と、シミュレーション装置５００に接続された、キーボード５２１、マウス５２２等の操作入力部５２０、モニタ５２３及び外部記憶装置５２４と、を備えている。
FIG. 3 is a block diagram illustrating a control system according to the embodiment. The
シミュレーション装置５００は、オフライン教示によってユーザの操作に従ってロボット２００の動作プログラムを作成し、作成した動作プログラムに基づき、ロボット２００に対応する仮想ロボットを三次元仮想空間内で動作させるシミュレーションを行う。
The
シミュレーション装置５００は、演算部としてのＣＰＵ（Central Processing Unit）５０１を備えている。また、シミュレーション装置５００は、記憶部として、ＲＯＭ（Read Only Memory）５０２、ＲＡＭ（Random Access Memory）５０３、ＨＤＤ（Hard Disk Drive）５０４を備えている。また、シミュレーション装置５００は、記録ディスクドライブ５０５及び複数のインタフェース５１１〜５１５を備えている。
The
ＣＰＵ５０１には、ＲＯＭ５０２、ＲＡＭ５０３、ＨＤＤ５０４、記録ディスクドライブ５０５及びインタフェース５１１〜５１５が、バス５１０を介して相互に通信可能に接続されている。
A
ＲＯＭ５０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ５０３は、ＣＰＵ５０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ５０４は、ＣＰＵ５０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ５０１に、後述する各種演算処理を実行させるためのプログラム５３１を記録するものである。ＣＰＵ５０１は、ＨＤＤ５０４に記録（格納）されたプログラム５３１に基づいてシミュレーション方法の各工程を実行する。
The
また、ＨＤＤ５０４には、ロボット言語（命令文）で記述された動作プログラムが格納される。更に、ＨＤＤ５０４には、ロボット２００を構成する部品の三次元データ（ＣＡＤデータ）、三次元有限要素法の計算式、パラメータ値等の各種情報が予め格納されている。
The
記録ディスクドライブ５０５は、記録媒体の一例である記録ディスク５３２に記録された各種データを読み出すことができる。
The
インタフェース５１１にはキーボード５２１が接続され、インタフェース５１２にはマウス５２２が接続されている。これらキーボード５２１及びマウス５２２からなる操作入力部５２０により、各種の操作入力を受付可能となっている。
A
インタフェース５１３には、表示部であるモニタ５２３が接続されており、モニタ５２３には、データ入力（編集）画像、仮想ロボットが配置された仮想三次元空間の表示画像などの各種画像が表示可能である。
A
インタフェース５１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置５２４が接続可能に構成されている。なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ５０４であり、ＨＤＤ５０４にプログラム５３１が格納される場合について説明するが、これに限定するものではない。プログラム５３１は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム５３１を供給するための記録媒体としては、図３に示すＲＯＭ５０２や、記録ディスク５３２、外部記憶装置等５２４を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ＨＤＤ、ＳＳＤ、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＲＯＭ等を用いることができる。また、記憶部としてＨＤＤ５０４の場合について説明するが、ＳＳＤや書き換え可能な不揮発性メモリであってもよい。
The
インタフェース５１５には、ロボット制御装置３００が接続されている。ロボット制御装置３００は、動作プログラムに基づき作成された軌道データをシミュレーション装置５００から取得し、軌道データに従ってロボット２００の動作を制御する。
The
ここで、ロボットアーム２０１の基端には、互いに直交する３軸ｘ０，ｙ０，ｚ０からなるワールド座標系Σ０が設定されている。また、ロボット２００の手先、つまりロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定される。ＴＣＰは、ロボットアーム２０１の基端に設定されたワールド座標系Σ０を基準に、位置を表す３つのパラメータと、姿勢を表す３つのパラメータで表される。このＴＣＰの目標値である教示点が、オフライン教示により、記憶部であるＨＤＤ５０４に格納される動作プログラムに記述される。
Here, at the base end of the
ＣＰＵ５０１は、動作プログラムに記述された補間方法に従い教示点間を補間し、逆運動学計算により各関節の値に変換したロボット２００（ロボットアーム２０１）の軌道データを生成する。教示点間を補間する補間方法としては、直線補間、円弧補間、関節補間、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線など、種々の方法がある。
The
ロボットアーム２０１の動作の制御として、位置制御とインピーダンス制御がある。位置制御では、ロボット２００の手先に設定したＴＣＰがワールド座標系Σ０の３軸ｘ０，ｙ０，ｚ０で目標位置（軌道）を辿るようにロボットアーム２０１の動作を制御する。一方、インピーダンス制御では、ロボット２００の手先に係る力（接触力）が所定値（より具体的には０）に近づくようにロボットアーム２０１の動作を制御する。よって、インピーダンス制御の設計段階におけるシミュレーションにおいて、接触力を計算する必要がある。接触力とは、物体と物体が接触した際に生じる反発力と摩擦力をいう。
Control of the operation of the
ところで、本実施形態のロボット２００は、拘束を有するロボットである。拘束を有するロボットの動作をシミュレーションする際には、接触力と拘束力の計算が必要である。ここで、拘束を有するロボットとは、ある動作軸とある動作軸との間に速度拘束（回転速度と回転速度との関係性）を有するものを指す。例えば、パラレルリンク機構を使ったものは拘束を有するロボットであるが、関節機構にモータ軸とリンク軸がギヤで速度拘束されているものについても、拘束を有するロボットである。拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。
By the way, the
本実施形態では、拘束を有するロボット２００がワーク（物体）と接触するときを想定し、ＣＰＵ５０１の演算処理により、ロボット２００にワークが接触する接触力及びロボット２００の関節における拘束力を求める。
In the present embodiment, assuming that the
以下、オフライン教示におけるロボット２００のダイナミクスシミュレーションについて説明する。図４は、実施形態に係るシミュレーション方法を示すフローチャートである。
Hereinafter, the dynamic simulation of the
ＣＰＵ５０１は、ユーザの操作によりロボット２００の動作プログラムを作成する（Ｓ１）。ＣＰＵ５０１は、ロボット２００のリンク２１００〜２１０６の相対位置情報や、各リンク２１００〜２１０６の重さ、慣性モーメント、重心位置、初期の関節角度、速度などの情報を読み込むことで、初期設定を行う（Ｓ２）。
The
ここで、ＣＰＵ５０１は、ロボット２００の三次元データ（ＣＡＤデータ）に基づく仮想モデル（仮想ロボット）を仮想三次元空間に配置する。ＣＰＵ５０１は、この仮想ロボットが配置された仮想三次元空間をモニタ５２３に表示させる。
Here, the
次に、ＣＰＵ５０１は、ロボット２００のバネ要素やダンパー要素などの受動要素のトルクを計算する（Ｓ３）。また、ＣＰＵ５０１は、モータトルクを計算する（Ｓ４）。即ち、これらステップＳ３，Ｓ４の計算により、力発生手段（モータ、バネ要素、ダンパー要素）の発生力としてのトルクが計算される。そして、ＣＰＵ３０１は、これらステップＳ３，Ｓ４の計算結果を用いて、ロボット２００の接触力及び拘束力を計算する（Ｓ５）。
Next, the
次に、ＣＰＵ５０１は、ステップＳ３，Ｓ４，Ｓ５の計算結果を用いて順動力学計算を行い、関節の加速度を算出する（Ｓ６）。
Next, the
ＣＰＵ５０１は、関節の加速度に基づき、関節の速度及び位置を計算してこれら関節の加速度、速度及び位置等からなる関節情報を更新し（Ｓ７）、３Ｄ画面情報（仮想ロボットの位置姿勢）を更新する。
The
次に、ＣＰＵ５０１は、ＴＣＰが終点に到達したか否かを判断し（Ｓ８）、終点に到達していなければ（Ｓ８：Ｎｏ）、予め決められた時間、仮想ロボットの動作を進め（Ｓ９）、ステップＳ３の処理に戻る。ＣＰＵ５０１は、ＴＣＰが終点に到達していれば（Ｓ８：Ｙｅｓ）、仮想ロボットを用いたロボット２００の動作のシミュレーションを終了する。
Next, the
ユーザは、このシミュレーション結果に基づき動作プログラムを必要に応じて適宜修正する。ユーザは、再度シミュレーション装置５００にシミュレーションを行わせるなどして、動作プログラムを確定させる。動作プログラムが確定したら、ユーザの操作により、動作プログラムに基づく軌道データをロボット制御装置３００に送る。ロボット制御装置３００は、実際のロボット２００を、軌道データに従って動作させ、組み付け動作などをロボット２００に行わせる。このように、オフライン教示で軌道データを作成することで、オンライン教示に比べ、ロボット２００の教示作業が容易となる。
The user modifies the operation program as needed based on the simulation result. The user confirms the operation program by causing the
次に、ロボット２００の動作をシミュレーションするのに用いる、ステップＳ５のロボット２００に物体が接触する接触力及びロボット２００の関節における拘束力を求める処理について詳細に説明する。
Next, a process for obtaining the contact force with which the object comes into contact with the
図５は、接触力及び拘束力の計算方法を示すフローチャートである。なお、以下の説明で、ヤコビアンとは、ヤコビ行列のことである。また、実施形態中の拘束力τｃｏｎｓｔｒａｉｎは、拘束力τＡに相当し、実施形態中の接触力τｃｏｎｔａｃｔは、接触力τＢに相当する。また、実施形態中の拘束力ｆｃｏｎｓｔｒａｉｎは、拘束力ｆＡに相当し、実施形態中の接触力ｆｃｏｎｔａｃｔは、接触力ｆＢに相当する。更に、実施形態中の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、第１のヤコビアンＪＡに相当し、実施形態中の接触ヤコビアンＪｃｏｎｔａｃｔは、第２のヤコビアンＪＢに相当し、実施形態中の接触・拘束ヤコビアンＪは、第３のヤコビアンＪに相当する。 FIG. 5 is a flowchart illustrating a method for calculating the contact force and the binding force. In the following description, Jacobian is a Jacobian matrix. Further, the constraint force τ constraint in the embodiment corresponds to the constraint force τ A, and the contact force τ contact in the embodiment corresponds to the contact force τ B. Furthermore, binding f constrain in embodiment corresponds to binding f A, the contact force f contact in embodiments corresponds to the contact force f B. Furthermore, constraint Jacobian J constrain in embodiment corresponds to the first Jacobian J A, contact Jacobian J contact in embodiment corresponds to the second Jacobian J B, the contact-bound in embodiments Jacobian J corresponds to the third Jacobian J.
まずＣＰＵ５０１は、関節空間におけるロボット２００の運動方程式（第１の運動方程式）を計算する（Ｓ１１）。ここで、関節空間とは、ロボット２００の関節の数を自由度とする空間、即ち各関節位置（角度）を座標軸とする空間である。自由度とは、ある物理系の状態を記述するのに必要な変数の最小個数である。
First, the
関節空間において、ロボット２００の関節の変位をｑ、ロボット２００の関節の加速度をｑ（・・）、ロボット２００の質量に関する質量行列をＡ、ロボット２００に働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂとする。また、関節空間において、ロボット２００に働く重力に関する重力項をｇ、ロボット２００の関節に発生する発生力（即ち、モータトルクなどの能動要素のトルク、スプリング要素やダンパー要素などの受動要素のトルク）をτａｃｔとする。また、関節空間において、ロボット２００の拘束力をτｃｏｎｓｔｒａｉｎ、ロボット２００の接触力をτｃｏｎｔａｃｔとする。ここで、関数を時間で一次微分したものを（・）、関数を時間で二次微分したものを（・・）で表記する。
In the joint space, q is the displacement of the joint of the
関節空間におけるロボット２００の第１の運動方程式は、加速度ｑ（・・）、質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇ、発生力τａｃｔ、拘束力τｃｏｎｓｔｒａｉｎ、接触力τｃｏｎｔａｃｔを用いて以下の式（１）で表される。
The first motion equation of the
ここで、第１の運動方程式である式（１）を計算するとは、式（１）における質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇを計算することである。また、式（１）は、拘束を取り去ったロボット２００の運動方程式である。
Here, calculating the equation (1) which is the first equation of motion means calculating the mass matrix A, the Coriolis force / centrifugal force term b, and the gravity term g in the equation (1). Equation (1) is an equation of motion of the
次に、ＣＰＵ５０１は、拘束空間（第１の座標空間）における拘束力ｆｃｏｎｓｔｒａｉｎと、関節空間における拘束力τｃｏｎｓｔｒａｉｎとの関係を示す拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを計算する（Ｓ１２）。拘束空間とは、ロボット２００の関節の拘束条件に基づく自由度を持ち、ロボット２００の拘束点の拘束方向を基準とする座標空間である。
Next, the
このとき、ＣＰＵ５０１は、以下の式（２）を満たすように、拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを算出する（第１のヤコビアン算出工程、第１のヤコビアン算出処理）。ここで、ｑ（・）は、関節空間におけるロボット２００の関節の速度である。
At this time, the
具体例として、ギヤの拘束のように、回転部品の軸ｑ１と回転部品の軸ｑ２がある減速比Ｒで拘束される場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。 As a specific example, the constraint Jacobian J constraint when the rotation component axis q 1 and the rotation component axis q 2 are constrained at a certain reduction ratio R, such as gear constraint , can be calculated as follows.
この場合の拘束点は軸ｑ１と軸ｑ２の位置であり、このヤコビアンのサイズは、１つのギヤの拘束につき「１×自由度」である。 This constraint point in this case is the position of the axis q 1 and axis q 2, the size of the Jacobian is "1 × DOF" per restraining one gear.
異なる具体例として、部品同士を固定する拘束のように、部品１の任意点Ｐ１と部品２の任意点Ｐ２の位置を拘束する場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。
As a different specific example, the constraint Jacobian J constraint in the case of constraining the position of the arbitrary point P 1 of the component 1 and the arbitrary point P 2 of the
ここで、ＪＰ１は任意点Ｐ１のヤコビアンを表し、ＪＰ２は任意点Ｐ２のヤコビアンを表し、関節の速度と任意点の速度を関係づける一般的なヤコビアンである。この場合の拘束点は任意点Ｐ１と任意点Ｐ２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分＋回転成分）×自由度」である。 Here, J P1 represents the Jacobian of the arbitrary point P 1, J P2 represents the Jacobian of the arbitrary point P 2, which is a general Jacobian that relates velocity joints speed and an arbitrary point. The constraint points in this case are the positions of the arbitrary point P 1 and the arbitrary point P 2 , and the size of this Jacobian is “(translation component + rotation component) × degree of freedom” per one fixed point.
さらにこの例において、部品１の任意点Ｐ１と部品２の任意点Ｐ２を並進方向にのみ固定する拘束のような場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。
Further, in this example, the constraint Jacobian J constraint in the case of the constraint in which the arbitrary point P 1 of the component 1 and the arbitrary point P 2 of the
ここで、Ｊ’Ｐ１は任意点Ｐ１のヤコビアンの回転成分を表し、Ｊ’Ｐ２は任意点Ｐ２のヤコビアンの並進成分を表している。この場合の拘束点は任意点Ｐ１と任意点Ｐ２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分）×自由度」である。 Here, J 'P1 represents the rotational component of the Jacobian of the arbitrary point P 1, J' P2 represents the translation component of the Jacobian of the arbitrary point P 2. In this case, the constraint points are the positions of the arbitrary point P 1 and the arbitrary point P 2 , and the size of the Jacobian is “(translational component) × degree of freedom” per one fixed point.
次に、ＣＰＵ５０１は、接触点の検出計算を行い（Ｓ１３）、接触点が存在するか否かを判断する（Ｓ１４）。ＣＰＵ５０１は、接触点が存在する場合、接触ヤコビアンＪｃｏｎｔａｃｔを計算する（Ｓ１５：第２のヤコビアン算出工程、第２のヤコビアン算出処理）。具体的に説明すると、ＣＰＵ５０１は、ロボット２００と物体との接触点のワールド座標系Σ０における座標値に基づき、接触ヤコビアンＪｃｏｎｔａｃｔを算出する。接触ヤコビアンＪｃｏｎｔａｃｔは、接触空間（第２の座標空間）における接触力ｆｃｏｎｔａｃｔと、関節空間における接触力τｃｏｎｔａｃｔとの関係を示すヤコビアンである。接触空間は、ロボット２００の接触点の法線方向及び接線方向を基準とする三次元直交座標（デカルト座標）で表された座標空間である。
Next, the
以下、更に具体的に説明する。なお、ｘｃｏｎｔａｃｔは接触空間における変位（接触点の相対位置）、ｘｃｏｎｓｔｒａｉｎは拘束空間における変位（拘束点の相対位置）とする。よって、ｘｃｏｎｔａｃｔ（・）は接触空間における速度、ｘｃｏｎｔａｃｔ（・・）は接触空間における加速度であり、ｘｃｏｎｓｔｒａｉｎ（・）は拘束空間における速度、ｘｃｏｎｓｔｒａｉｎ（・・）は拘束空間における加速度である。 More specific description will be given below. Note that x contact is a displacement in the contact space (relative position of the contact point), and x constrain is a displacement in the constraint space (relative position of the constraint point). Therefore, x contact (·) is the velocity in the contact space, x contact (··) is the acceleration in the contact space, x constrain (·) is the velocity in the constrained space, and x constrain (··) is the acceleration in the constrained space. is there.
部品ａの接触点ｘｃｏｎｔａｃｔ ａ，ｉと、部品ｂの接触点ｘｃｏｎｔａｃｔ ｂ，ｉとに接触が発生したとする。ｉは、接触点に付した番号である。このとき、それぞれの接触点の速度は以下のように表される。 A contact point x contact a, i of the component a, the contact points x contact b of the component b, the contact on the i to have occurred. i is a number given to the contact point. At this time, the speed of each contact point is expressed as follows.
ここで、右辺のＪｃｏｎｔａｃｔ ａ，ｉ及びＪｃｏｎｔａｃｔ ｂ，ｉは、ｉ番目の接触点のヤコビアンである。これを用いると、ｉ番目の接触における接触点の相対速度ｘ（・）ｃｏｎｔａｃｔ ｉは、以下の式（３）のようになる。 Here, J contact a, i and J contact b, i on the right side are Jacobians of the i-th contact point. When this is used, the relative velocity x (•) contact i of the contact point in the i-th contact is expressed by the following equation (3).
次に、ｉ番目の接触における法線ベクトルをｎｃｏｎｔａｃｔ ｉとし、接線ベクトルをｔｃｏｎｔａｃｔ ｘ，ｉとｔｃｏｎｔａｃｔ ｙ，ｉとする。これらの表記を用いて、ｉ番目の接触における法線方向速度と接線方向速度を表すヤコビアンＪｃｏｎｔａｃｔ ｎ，ｉ及びＪｃｏｎｔａｃｔ ｔ，ｉは以下の式（４），（５）のようになる。 Next, let the normal vector in the i-th contact be n contact i, and let the tangent vectors be t contact x, i and t contact y, i . Using these notations, Jacobians J contact n, i and J contact t, i representing the normal direction velocity and the tangential direction velocity at the i-th contact are expressed by the following equations (4) and (5).
各接触における法線方向ヤコビアンＪｃｏｎｔａｃｔ ｎ，ｉと接線方向ヤコビアンＪｃｏｎｔａｃｔ ｔ，ｉを積みあげることによって、すべての接触点に対する接触ヤコビアンＪｃｏｎｔａｃｔが以下の式（６）のように表現できる。 By accumulating the normal direction Jacobian J contact n, i and the tangential direction Jacobian J contact t, i in each contact, the contact Jacobian J contact for all contact points can be expressed as the following equation (6).
次に、ＣＰＵ５０１は、拘束ヤコビアンＪｃｏｎｓｔｒａｉｎ及び接触ヤコビアンＪｃｏｎｔａｃｔを合わせた接触・拘束ヤコビアンＪを求める（Ｓ１６：第３のヤコビアン算出工程，第３のヤコビアン算出処理）。接触・拘束ヤコビアンＪは、以下の式（７）で表される。
Next, the
次に、ＣＰＵ５０１は、接触・拘束ヤコビアンＪを用いて、関節空間における第１の運動方程式（式（１））を、以下の接触・拘束空間（第１の座標空間及び第２の座標空間）における第２の運動方程式（式（８））に変換する（Ｓ１７：変換工程，変換処理）。
Next, the
なお、式（８）は、以下の式で表される。 In addition, Formula (8) is represented by the following formula | equation.
ただし、上式のパラメータは以下の通りである。 However, the parameters of the above formula are as follows.
即ち、ＣＰＵ５０１は、ヤコビアンＪを用いて第１の運動方程式（式（１））を第２の運動方程式（式（８））に変換することで、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数を求める。なお、加速度ｘ（・・）は、ｘｃｏｎｓｔｒａｉｎ（・・），ｘｃｏｎｔａｃｔ（・・）からなる。
That is, the
次に、ＣＰＵ５０１は、式（８）に示す第２の運動方程式を、時間領域で積分することで線形化して、以下の第３の運動方程式を求める（Ｓ１８：線形化工程，線形化処理）。
Next, the
つまり、線形化した第３の運動方程式において、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数は既知の値であり、ｆｃｏｎｓｔｒａｉｎ，ｆｃｏｎｔａｃｔが未知数である。第３の運動方程式にこれら係数を適用すれば、第２の運動方程式を第３の運動方程式に変換したことになる。ここで、Δｔは接触力と拘束力が働く所定の時間である。 That is, in the third equation of motion linearized, mass matrix lambda, Coriolis force / centrifugal force term mu, each coefficient of the gravity term p, and generation force f act is a known value, f constrain, is f contact unknowns It is. If these coefficients are applied to the third equation of motion, the second equation of motion is converted to the third equation of motion. Here, Δt is a predetermined time during which the contact force and the binding force are applied.
式（８）に示す第２の運動方程式を時間領域で積分し線形化した第３の運動方程式は、以下のようにも表される。 The third equation of motion obtained by integrating and linearizing the second equation of motion shown in Equation (8) in the time domain is also expressed as follows.
以上の第３の運動方程式は、接触・拘束空間における拘束点の拘束力ｆｃｏｎｓｔｒａｉｎ及び接触点の接触力ｆｃｏｎｔａｃｔを未知数とする連立方程式となる。 More third equation of motion, the simultaneous equations to unknown contact force f contact the binding f constrain and contact points of constraint points in the contact-bound space.
次に、ＣＰＵ５０１は、線形化した連立方程式を解くことにより、接触・拘束空間における拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを算出する（Ｓ１９：力算出工程、力算出処理）。このように、本実施形態では、拘束力ｆｃｏｎｓｔｒａｉｎ及び接触力ｆｃｏｎｔａｃｔを同時に求めることができる。
Next,
ここで、接触・拘束空間における接触力ｆｃｏｎｔａｃｔ及び拘束力ｆｃｏｎｓｔｒａｉｎを連立させて計算する際には、下記の制約条件を満たすように計算を行う。 Here, when the contact force f contact and the constraint force f constraint in the contact / restraint space are calculated simultaneously, the calculation is performed so as to satisfy the following constraint conditions.
制約条件１．接触時に発生する接触空間における接触力の法線方向成分は、物体同士が離れる方向に発生する。 Constraint 1 The normal direction component of the contact force in the contact space generated at the time of contact is generated in a direction in which the objects are separated from each other.
制約条件２．接触時に発生する接触空間における速度は、物体同士が離れる方向に発生する。
制約条件３．接触時に発生する接触空間における接触力の法線方向成分と速度の関係について、反発係数＝０の場合においては、接触力が発生した場合には速度は０になり、逆に接触力が０になる場合に速度は０にならない。 Constraint 3 Regarding the relationship between the normal direction component of the contact force in the contact space generated at the time of contact and the speed, when the restitution coefficient = 0, the speed is 0 when the contact force is generated, and conversely the contact force is 0. In this case, the speed does not become zero.
制約条件４．接触時に発生する接触空間における接触力の接線方向成分は、法線方向接触力と摩擦係数ｋを掛けた最大摩擦力以内となる。 Constraint 4 The tangential direction component of the contact force in the contact space generated at the time of contact is within the maximum friction force multiplied by the normal direction contact force and the friction coefficient k.
制約条件５．拘束力によって発生する拘束空間における速度は、常に０となる。 Constraint 5 The speed in the constrained space generated by the constraining force is always zero.
接触力ｆｃｏｎｔａｃｔに関する制約は、接触力の物理的な関係性であり、これらを用いて線形相補性問題に帰着することができる。拘束力ｆｃｏｎｓｔｒａｉｎに関する制約は、拘束力の物理的な関係性を表している。以上の制約をまとめると、以下のようになる。 The constraint on the contact force f contact is the physical relationship of the contact force, which can be used to reduce the linear complementarity problem. The constraint on the constraint force f constraint represents a physical relationship between the constraint forces. The above restrictions are summarized as follows.
式（１７）は、接触点において法線方向の接触力は、物体同士が離れる方向にしか作用しないことを意味している。式（１８）は、接触点において物体同士がめり込む方向に速度が発生しないことを意味している。式（１９）は、反発係数＝０の場合においては、接触力が発生した場合には接触空間における速度は０になり、逆に接触力が０になる場合には接触空間における速度は０にならないことを意味している。式（２０）は、接線方向接触力が法線方向接触力の最大摩擦係数以内であることを意味している。式（２１）は、拘束空間での速度が常に０となることを意味している。 Expression (17) means that the contact force in the normal direction at the contact point acts only in the direction in which the objects are separated from each other. Equation (18) means that no speed is generated in the direction in which the objects sink into each other at the contact point. Equation (19) shows that when the restitution coefficient = 0, the velocity in the contact space is zero when the contact force is generated, and conversely, the velocity in the contact space is zero when the contact force is zero. It means not to be. Equation (20) means that the tangential contact force is within the maximum friction coefficient of the normal contact force. Equation (21) means that the velocity in the constrained space is always zero.
したがって、ＣＰＵ５０１は、ステップＳ１９では、式（１７）〜式（２１）の関係式を満たすように、拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを求める。具体的に説明すると、ＣＰＵ５０１は、以下の演算処理１〜５を行う。
Accordingly,
ｌ＝０とする。ｆに初期解ｆｌを代入する（演算処理１）。 Let l = 0. Substitute the initial solution fl for f (arithmetic processing 1).
ｆｌのｉ成分ｆｉ ｌを以下の式に従い更新する（演算処理２）。 The component i f i l of f l to update according to the following formula (processing 2).
ここで、ｍｉ，ｊは式（１３）の行列Ｍの（ｉ，ｊ）成分である。Ｚｉは式（１４）のベクトルｚの成分ｉである。 Here, m i, j is the (i, j) component of the matrix M in equation (13). Z i is the component i of the vector z in equation (14).
次に、ｆｉ ｌ＋１がｆｃｏｎｔａｃｔの法線方向の力に相当する成分であり、かつ、ｆｉ ｌ＋１＜０ならｆｉ ｌ＋１＝０とする（演算処理３）。 Next, f i l + 1 is a component corresponding to the force in the normal direction of f contact , and if f i l + 1 <0, f i l + 1 = 0 is set (arithmetic processing 3).
次に、演算処理４として、ｆｉ ｌ＋１とｆｉ＋１ ｌ＋１がｆｃｏｎｔａｃｔの接線方向の力に相当する成分である場合、以下の処理４−１，４−２を実行する。即ち、処理４−１として、以下の計算をする。 Next, as processing 4, when f i 1 + 1 and f i + 1 1 + 1 are components corresponding to the force in the tangential direction of f contact , the following processes 4-1 and 4-2 are executed. That is, as the process 4-1, the following calculation is performed.
また、処理４−２として、以下の計算をする。 Further, as processing 4-2, the following calculation is performed.
次に、演算処理５として、以下の式の演算結果を満たす場合は、ｆｌ＋１をｆとして終了する。 Next, as the calculation process 5, when the calculation result of the following expression is satisfied, fl + 1 is set to f, and the process is terminated.
以上の式を満たさない場合は、ｌ＝ｌ＋１として、演算処理２へ戻る。以上の演算処理により、式（１７）〜式（２１）を満たす式（１２）に対する拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを求めることができる。即ち、拘束を有するロボットの接触力を求めることが可能になる。
If the above equation is not satisfied, l = 1 + 1 and the process returns to the
関節空間における拘束力τｃｏｎｓｔｒａｉｎ及び接触力τｃｏｎｔａｃｔを足し合わせた関節への入力トルクをτとする。ＣＰＵ５０１は、拘束力ｆｃｏｎｓｔｒａｉｎ及び接触力ｆｃｏｎｔａｃｔを接触・拘束ヤコビアンＪを用いて、入力トルクτに変換する（Ｓ２０：力変換工程，力変換処理）。
An input torque to the joint obtained by adding the restraining force τ constraint and the contact force τ contact in the joint space is τ. The
以上、ロボット２００が物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。
As described above, when simulating a scene in which the
［実施例］
（実施例１）ギヤ拘束
図６は、実施例１におけるロボットを示す模式図である。ダイナミクスシミュレーション方法について、図６に示すような軸間に拘束を有するロボットについて接触力及び拘束力を計算する具体例を下記に示す。
[Example]
(Example 1) Gear restraint FIG. 6 is a schematic diagram showing a robot in Example 1. As shown in FIG. Regarding the dynamics simulation method, a specific example of calculating contact force and restraint force for a robot having constraints between axes as shown in FIG. 6 is shown below.
図６に示すように、ロボット５０の関節である回転ジョイント４１が、駆動源であるモータに駆動される第１のギヤである入力ギヤ３１で構成され、回転ジョイント４２が、入力ギヤ３１に噛合する第２のギヤである出力ギヤ３２で構成されている。したがって、回転ジョイント４１が入力軸であり、回転ジョイント４２が出力軸である。出力ギヤ３２には、リンク３３が接続されている。したがって、リンク３３は出力ギヤ３２と共に回転する。
As shown in FIG. 6, a rotary joint 41 that is a joint of the
実施例１では、入力ギヤ３１と出力ギヤ３２との変位が、ある一定の比率で動作するという拘束を有している。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）が含まれている。例えば、入力ギヤ３１と出力ギヤ３２との速度伝達比が２：１の拘束条件で拘束されているものとする。拘束を有するロボットのリンク３３に、物体３４と接触したときの接触力を求める。
In the first embodiment, the displacement between the
ロボットのリンク間の相対位置情報を表１にまとめる。 Table 1 summarizes the relative position information between the links of the robot.
また、リンクの緒元を表２にまとめる。 Table 2 summarizes the origins of the links.
回転ジョイント４１と回転ジョイント４２に関する拘束条件を表３にまとめる。 Table 3 summarizes the constraint conditions related to the rotary joint 41 and the rotary joint 42.
ここでリンク３３は出力ギヤ３２に固定されているものとし、以降では一体部品として扱う。まず図６の状態における関節変位は、以下のようになる。
Here, the
各関節速度は、以下のようになる。 Each joint speed is as follows.
このときの質量行列Ａは、以下のようになる。 The mass matrix A at this time is as follows.
重力項ｇは、以下のようになる。 The gravity term g is as follows.
また、遠心力／コリオリ力ｂは、以下のようになる。 Further, the centrifugal force / Coriolis force b is as follows.
以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。 As described above, terms relating to A, b, and g of the following equation of motion of the robot in a state where the constraint is removed are obtained (S11).
ここで発生力τｍｏｔｏｒは、駆動源であるモータの駆動力である。なお、実施例１では、これらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは第１軸と第２軸の速度伝達比（ギヤ比）が２：１のギヤで接続されているという拘束なので、以下のように算出される（Ｓ１２）。 Here, the generated force τ motor is a driving force of a motor that is a driving source. In the first embodiment, the calculation procedures of A, b, and g are omitted, but the values of A, b, and g are the moment of inertia and weight of each link, the position of the center of gravity, and the distance between the links. It is calculated from the relative position information. The constraint Jacobian J constrain is a constraint that the speed transmission ratio (gear ratio) of the first shaft and the second shaft is connected by a gear of 2: 1, and is calculated as follows (S12).
次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表４及び表５に示す。 Next, contact point detection calculation is performed (S13). The contact point information is shown in Tables 4 and 5.
接触点情報と法線方向の情報から接触ヤコビアンＪｃｏｎｔａｃｔは以下のように算出される（Ｓ１５）。 The contact Jacobian J contact is calculated from the contact point information and the normal direction information as follows (S15).
これらの接触ヤコビアンＪｃｏｎｔａｃｔと拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを用いて、接触・拘束ヤコビアンＪを算出する（Ｓ１６）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される。 Using these contact Jacobian J contact and constrained Jacobian J constrain , a contact / restraint Jacobian J is calculated (S16). Further, the equation of motion is converted using the contact / restraint Jacobian J to calculate the equation (12). At this time, M and z are calculated as follows.
ここで接触力と拘束力が働く時間を表すΔｔについては、３．７５Ｅ−０８［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める（Ｓ１９）。 Here, Δt representing the time during which the contact force and the restraint force are applied is set to 3.75E-08 [s]. By the method described above, f is obtained by setting the friction coefficient to 0.2 (S19).
ｆの第１成分が拘束力ｆｃｏｎｓｔｒａｉｎを表し、第２成分と第３成分が接触力ｆｃｏｎｔａｃｔの法線方向成分、第４〜第７成分までが接触力ｆｃｏｎｔａｃｔの接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。 The first component of f represents the binding force fconstrain , the second component and the third component correspond to the normal direction component of the contact force fcontact , and the fourth to seventh components correspond to the tangential direction component of the contact force fcontact. ing. When x (•) is calculated from the calculated f, it is as follows.
拘束点ｘｃｏｎｓｔｒａｉｎにおける速度である第１成分は０となっており、式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点ｘｃｏｎｔａｃｔにおける法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。 The first component, which is the velocity at the constraint point x constraint, is 0, and it can be confirmed that the formula (21) is satisfied. In addition, the second component and the third component have zero normal direction velocity at the contact point x contact . Looking at the corresponding components of f, all take positive values, and it can be confirmed that the equations (17), (18), and (19) are satisfied.
また、ｆの接触力の法線方向と接線方向の力を比較すると、接触１においては、以下の不等式が成り立つ。 Further, when the normal force direction and the tangential force of the contact force of f are compared, the following inequality is established for the contact 1.
接触２においても、以下の不等式が成り立つ。
In
いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力を示すトルクτは、以下のように求まる（Ｓ２０）。 It can be confirmed that both satisfy the formula (20). At this time, the torque τ indicating the force of each axis obtained by adding the contact force and the restraining force is obtained as follows (S20).
以上説明したとおり、図６に示すような、軸間に拘束を有するロボット５０に対して、接触力を計算することができるため、ギヤ３１，３２のダイナミクスを考慮したロボットの接触シミュレーションを行うことができる。
As described above, since the contact force can be calculated with respect to the
実施例１では、ロボット５０と物体３４が接触した瞬間を具体例として挙げている。接触力と拘束力とを、連立方程式を用いて解いており、ギヤの拘束１つにつき１個の変数、接触１つにつき３個の変数を解く必要がある。実施例１では接触点が２つあるため、７個の変数を解いている。この変数の個数は、拘束ヤコビアン、接触ヤコビアンの行数に対応している。また、接触力計算では式（１７）〜式（２０）を考慮しなければならない分、計算量が大きい。つまり、ある１つの接触点の接触力の計算量は、ある１つのギヤの拘束力の計算量に比べて、最低３倍以上の計算量が必要である。さらに、実際にギヤを接触でシミュレーションする場合、複数の接触点が発生する可能性がある。仮に実施例１において拘束力を廃しギヤを接触力でシミュレーションした場合では、ギヤ間において最低２つの接触点が発生するため、最低６倍以上の計算量が必要である。さらに、ギヤの数が増えるほどその影響は大きくなる。そのため、拘束を有するロボットの接触問題をシミュレーションする場合は、実施例１のように接触力と拘束力を連立させて計算することにより、計算時間を短縮することができる。これにより、高速なシミュレーションが可能となる。
In the first embodiment, the moment when the
（実施例２）角度の伝達誤差つきギヤ拘束
実施例２では、ロボットで一般的に使用される波動歯車減速機などの角度の伝達誤差を持つギヤを模した軸間拘束を有するロボットに対して、接触力の計算方法を適用したときの具体例を示す。
(Embodiment 2) Gear constraint with angular transmission error In the second embodiment, for a robot having an inter-axis constraint imitating a gear having an angular transmission error, such as a wave gear reducer generally used in a robot. A specific example when the contact force calculation method is applied will be described.
ロボットで使用される波動歯車減速機など柔軟物を含むギヤの伝達力計算を行うには、接触力計算に加えて変形計算を行う必要があり、計算コストがかかる。そのため、実施例２のように拘束力でシミュレーションを実現するのが有効である。 In order to calculate the transmission force of a gear including a flexible object such as a wave gear reducer used in a robot, it is necessary to perform a deformation calculation in addition to the contact force calculation, which requires a calculation cost. Therefore, it is effective to realize the simulation with the restraint force as in the second embodiment.
なお、ロボットの基本構成、リンクの配置、リンクの緒元は実施例１と同一とし、説明を省略する。 The basic configuration of the robot, the arrangement of the links, and the specifications of the links are the same as those in the first embodiment, and a description thereof is omitted.
実施例２は、実施例１と同様にギヤで接続されたリンクを模しており、回転ジョイント４１が入力軸、回転ジョイント４２が出力軸となる。そして、実施例２では、以下のような比率と伝達誤差で表されるギヤで接続されている。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）と、入力ギヤ３１と出力ギヤ３２との間の角度の伝達誤差が含まれているものとする。
The second embodiment is similar to the first embodiment in that the links are connected by gears. The rotary joint 41 is an input shaft and the rotary joint 42 is an output shaft. And in Example 2, it connects by the gear represented by the following ratios and transmission errors. That is, it is assumed that the constraint condition includes a speed transmission ratio (gear ratio) between the
ここで、右辺第一項がギヤ比を、右辺第二項が角度伝達誤差を表している。拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは以下のように算出される。まず、上記の入力軸と出力軸の関係について微分し、速度の関係式に変換する。 Here, the first term on the right side represents the gear ratio, and the second term on the right side represents the angle transmission error. The restricted Jacobian J constrain is calculated as follows. First, the relationship between the input shaft and the output shaft is differentiated and converted to a speed relational expression.
その後、式をまとめて以下のような式で拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを算出する。 After that, the constrained Jacobian J constrain is calculated using the following formula.
実施例２では、表６のようなギヤ比と角度伝達誤差パラメータを選択した。 In Example 2, the gear ratio and the angle transmission error parameter as shown in Table 6 were selected.
角度伝達誤差以外のパラメータについては実施例１と同様であるため、ロボットのリンク間の相対位置情報とリンクの緒元は省略する。まず図６の状態における各関節変位は、以下のようになる。 Since parameters other than the angle transmission error are the same as those in the first embodiment, the relative position information between the links of the robot and the specifications of the links are omitted. First, each joint displacement in the state of FIG. 6 is as follows.
各関節速度は、以下のようになる。 Each joint speed is as follows.
このときの質量行列Ａは、以下のようになる。 The mass matrix A at this time is as follows.
重力項ｇは、以下のようになる。 The gravity term g is as follows.
また、遠心力／コリオリ力ｂは、以下のようになる。 Further, the centrifugal force / Coriolis force b is as follows.
以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。 As described above, terms relating to A, b, and g of the following equation of motion of the robot in a state where the constraint is removed are obtained (S11).
なお、実施例２ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンは第１軸と第２軸の変位比率が１０：１のギヤで接続されており、かつ角度伝達誤差を含むという拘束であり、前段で計算した式に値を入れると以下のようになる（Ｓ１２）。 Although the calculation procedure of A, b, and g is omitted in the second embodiment, the numerical values of A, b, and g are the moment of inertia and weight of each link, the position of the center of gravity, and the relative value between the links. It is calculated from the position information. The constraint Jacobian is a constraint in which the displacement ratio of the first axis and the second axis is connected by a gear of 10: 1 and includes an angle transmission error. (S12).
次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表７及び表８に示す。 Next, contact point detection calculation is performed (S13). Table 7 and Table 8 show the contact point information.
接触点情報と法線方向の情報から接触ヤコビアンＪｃｏｎｔａｃｔは以下のように算出される（Ｓ１４）。 The contact Jacobian J contact is calculated from the contact point information and the normal direction information as follows (S14).
これらの接触ヤコビアンＪｃｏｎｔａｃｔと拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを用いて、接触・拘束ヤコビアンを算出する（Ｓ１５）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される（Ｓ１７）。 Using these contact Jacobian J contact and constrained Jacobian J constrain , a contact / restraint Jacobian is calculated (S15). Further, the equation of motion is converted using the contact / restraint Jacobian J to calculate the equation (12). At this time, M and z are calculated as follows (S17).
ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。 Here, the predetermined time Δt during which the contact force and the restraining force are applied is set to 1.00E-07 [s]. By the method described above, f is obtained with a friction coefficient of 0.2.
ｆの第１成分が拘束力を表し、第２成分と第３成分が接触力の法線方向成分、第４〜第７成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。 The first component of f represents the binding force, the second component and the third component correspond to the normal direction component of the contact force, and the fourth to seventh components correspond to the tangential component of the contact force. When x (•) is calculated from the calculated f, it is as follows.
拘束点における速度である第１成分は０となっており式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点における法線方向の速度も０となっている。このとき、ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。 The first component, which is the speed at the constraint point, is 0, and it can be confirmed that the formula (21) is satisfied. In addition, the second component and the third component have zero normal speed at the contact point. At this time, looking at the corresponding components of f, all of them take a positive value, and it can be confirmed that the expressions (17), (18), and (19) are satisfied.
また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。 Further, when the normal force direction and the tangential force of the contact force of f are compared, the following inequality is established.
よって、式（２０）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。 Therefore, it can confirm that Formula (20) is satisfy | filled. Further, when the normal force direction and the tangential force of the contact force of f are compared, the following inequality is established.
よって、式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。 Therefore, it can confirm that Formula (20) is satisfy | filled. At this time, the force (torque) τ of each axis obtained by adding the contact force and the restraining force is obtained as follows.
以上説明したとおり、ギヤの角度伝達誤差を考慮したロボットの接触シミュレーションを行うことができる。 As described above, it is possible to perform a robot contact simulation in consideration of a gear angle transmission error.
［実施例３］
次に、実施例３のダイナミクスシミュレーション方法について説明する。図７（ａ）は、実施例３におけるロボットを示す模式図である。図７（ａ）に示すロボット８０は、パラレルリンク型のロボットである。図７（ａ）に示すような、拘束点の相対速度が常に０になるような拘束を有するロボット８０に対して、接触計算方法を適用したときの具体例について説明する。図７（ａ）に示すように、ロボット８０は、複数のリンク６１〜６４が、関節である回転ジョイント７１〜７５で回転可能に連結されている。
[Example 3]
Next, a dynamics simulation method according to the third embodiment will be described. FIG. 7A is a schematic diagram illustrating the robot in the third embodiment. A
図７（ｂ）は、図７（ａ）に示すロボットの１つの回転ジョイントを、互いに拘束関係にある２つの拘束点とした模式図である。ロボット８０は、回転ジョイント７４を取り去り、代わりに拘束点８１と拘束点８２とがｘ０方向とｚ０方向の座標が常時一致するという拘束条件が設けられた、２本別々のロボットと考えてもよい。２本のロボットのうち、１本目はリンク６１〜６３を有し、２本目はリンク４４のみによって構成される。図７（ａ）に示すロボット８０は、拘束を有するロボットである。
FIG. 7B is a schematic diagram in which one rotation joint of the robot shown in FIG. 7A is used as two constraint points that are in a constraint relationship with each other.
図７（ｃ）は、実施例３においてロボットに物体が接触した状態を示す模式図である。この拘束を有するロボット８０が、図７（ｃ）に示すような物体９１と接触したときの接触力を求める。
FIG. 7C is a schematic diagram illustrating a state where an object contacts the robot in the third embodiment. The contact force when the
ロボット８０のリンク間の相対位置情報を表９にまとめる。また、リンクの緒元は実施例１と同様であるため省略する。表１０には、拘束点８１と拘束点８２に関する拘束条件をまとめる。
The relative position information between the links of the
図７（ｃ）の状態における各関節変位は、以下のようになる。 Each joint displacement in the state of FIG.7 (c) is as follows.
各関節速度は、以下のようになる。 Each joint speed is as follows.
このときの質量行列Ａは、以下のようになる。 The mass matrix A at this time is as follows.
重力項ｇは、以下のようになる。 The gravity term g is as follows.
また、遠心力／コリオリ力ｂは、以下のようになる。 Further, the centrifugal force / Coriolis force b is as follows.
以上により、拘束を取り去った状態におけるロボット８０の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。
As described above, terms relating to A, b, and g of the equation of motion of the
なお、実施例３ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、表１と表２に示した各リンクの慣性モーメントや重さ、重心位置、また、各リンク間の相対位置情報から計算されるものである。 In Example 3, the calculation procedures of A, b, and g are omitted. However, the numerical values of A, b, and g are the inertia moments and weights of the links shown in Tables 1 and 2, It is calculated from the position of the center of gravity and the relative position information between the links.
拘束ヤコビアンは、表１０の拘束点情報と各軸の姿勢から以下のように算出される（Ｓ１２）。具体的には、拘束点８１と拘束点８２の位置を計算し、各点におけるヤコビアンを計算し、それぞれのヤコビアンの差を取り、そこから拘束する軸成分を取得する。本シミュレーションでは、ｙ並進成分とｚ並進成分を拘束することとした。
The constrained Jacobian is calculated as follows from the constrained point information in Table 10 and the posture of each axis (S12). Specifically, the positions of the
次に接触点情報を表１１及び表１２に示す。 Next, Table 11 and Table 12 show the contact point information.
接触点情報と法線方向の情報から接触ヤコビアンは以下のように算出される（Ｓ１５）。 The contact Jacobian is calculated from the contact point information and the normal direction information as follows (S15).
これらの接触ヤコビアンと拘束ヤコビアンを用いて、Ｍとｚは以下のように算出される。 Using these contact Jacobian and constrained Jacobian, M and z are calculated as follows.
ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。 Here, the predetermined time Δt during which the contact force and the restraining force are applied is set to 1.00E-07 [s]. By the method described above, f is obtained with a friction coefficient of 0.2.
ｆの第１成分と第２成分が拘束力を表し、第３成分と第４成分が接触力の法線方向成分、第５〜第８成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。 The first component and the second component of f represent the binding force, the third component and the fourth component correspond to the normal component of the contact force, and the fifth to eighth components correspond to the tangential component of the contact force. . When x (•) is calculated from the calculated f, it is as follows.
拘束点における速度である第１成分と第２成分は０となっており、式（２１）を満たしていることが確認できる。また、第３成分と第４成分は接触点における法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると１つ目の接触について、以下の不等式が成り立つ。 The first component and the second component, which are the velocities at the restraint points, are 0, and it can be confirmed that the formula (21) is satisfied. In addition, the third component and the fourth component have zero normal speed at the contact point. Looking at the corresponding components of f, all take positive values, and it can be confirmed that the equations (17), (18), and (19) are satisfied. Further, when the force in the normal direction and the tangential direction of the contact force of f are compared, the following inequality holds for the first contact.
２つ目の接触について、以下の不等式が成り立つ。 The following inequality holds for the second contact.
いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。 It can be confirmed that both satisfy the formula (20). At this time, the force (torque) τ of each axis obtained by adding the contact force and the restraining force is obtained as follows.
以上説明したとおり、図７（ｂ）に示すような拘束点８１，８２の相対速度が常に０になるような拘束を有するロボットに対して接触力を計算することができる。このため、パラレルリンクを考慮したロボットの接触シミュレーションを行うことができる。 As described above, the contact force can be calculated for a robot having a constraint such that the relative speeds of the constraint points 81 and 82 are always 0 as shown in FIG. Therefore, it is possible to perform a robot contact simulation in consideration of the parallel link.
なお、シミュレーション中の接触や拘束の数やその種類は、これらの実施例に限定されるものではない。 Note that the number and types of contacts and restraints during simulation are not limited to these examples.
本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。 The present invention is not limited to the embodiments described above, and many modifications are possible within the technical idea of the present invention. In addition, the effects described in the embodiments are merely a list of the most preferable effects resulting from the present invention, and the effects of the present invention are not limited to those described in the embodiments.
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。 The present invention supplies a program that realizes one or more functions of the above-described embodiments to a system or apparatus via a network or a storage medium, and one or more processors in a computer of the system or apparatus read and execute the program This process can be realized. It can also be realized by a circuit (for example, ASIC) that realizes one or more functions.
また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。 In the above-described embodiment, the case where the robot arm is a vertically articulated robot arm has been described. However, the present invention is not limited to this. The robot arm may be various robot arms such as a horizontal articulated robot arm, a parallel link robot arm, and an orthogonal robot.
また、上述の実施形態では、シミュレーション装置とロボット制御装置とが別のコンピュータで構成される場合について説明したが、これに限定するものではなく、シミュレーション装置とロボット制御装置とが１つのコンピュータで構成されていてもよい。 In the above-described embodiment, the case where the simulation apparatus and the robot control apparatus are configured by separate computers has been described. However, the present invention is not limited to this, and the simulation apparatus and the robot control apparatus are configured by one computer. May be.
１００…ロボットシステム、２００…ロボット、３００…ロボット制御装置、５００…シミュレーション装置、５０１…ＣＰＵ（演算部）、６００…制御システム
DESCRIPTION OF
Claims (9)
前記演算部は、
前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出処理と、
前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出処理と、を実行することを特徴とするシミュレーション装置。 A simulation device including a calculation unit for calculating a contact force with which an object comes into contact with the robot and a binding force in the joint of the robot, which is used for simulating an operation of a robot having a joint,
The computing unit is
In a joint space where the number of joints of the robot is a degree of freedom, when the speed of the joints of the robot is q (•), the robot has a degree of freedom based on the constraint conditions of the joints of the robot, and restrains the restraint points of the robot. a restraining force f a in a first coordinate space relative to the direction, a first Jacobian J a showing the relationship between the binding tau a in the joint space,
Based on the coordinate value of the contact point between the robot and the object in the world coordinate system, the contact force f in the second coordinate space represented by orthogonal coordinates with reference to the normal direction and the tangential direction of the contact point of the robot. A second Jacobian calculation process for calculating a second Jacobian J B indicating the relationship between B and the contact force τ B in the joint space;
A third Jacobian calculation process for obtaining the third Jacobian J the combined first Jacobian J A and the second Jacobian J B,
In the joint space, the acceleration of the joint of the robot is q (··), the mass matrix relating to the mass of the robot is A, the Coriolis force acting on the robot and the Coriolis force / centrifugal force term relating to the centrifugal force are b, A gravity term related to the working gravity is g, and a generated force generated in the joint of the robot is τ act, and the first motion equation of the robot in the joint space
The third equation of motion is obtained by integrating the second equation of motion in the time domain and linearizing the predetermined time Δt.
The linearized third equation of motion is a simultaneous equation with the binding force f A and the contact force f B as unknowns, and the binding force f A and the contact force f B are calculated by solving the simultaneous equations. And a force calculating process.
前記拘束力ｆＡ及び前記接触力ｆＢを前記第３のヤコビアンＪを用いて、前記拘束力τＡ及び前記接触力τＢを足し合わせた関節の入力トルクτに変換する力変換処理を更に実行することを特徴とする請求項１に記載のシミュレーション装置。 The computing unit is
Using said binding f A and the said contact force f B third Jacobian J, further the binding tau A and the contact force tau force conversion processing for converting B the added combined with the input torque tau of the joint The simulation apparatus according to claim 1, wherein the simulation apparatus is executed.
前記拘束条件には、前記第１のギヤと前記第２のギヤとの速度伝達比が含まれていることを特徴とする請求項１又は２に記載のシミュレーション装置。 A joint of the robot has a first gear driven by a drive source and a second gear meshing with the first gear;
The simulation apparatus according to claim 1, wherein the constraint condition includes a speed transmission ratio between the first gear and the second gear.
前記シミュレーション装置によるシミュレーションにより作成した前記ロボットの動作プログラムに基づき、前記ロボットの動作を制御するロボット制御装置と、を備えた制御システム。 The simulation apparatus according to any one of claims 1 to 4,
And a robot control device that controls the operation of the robot based on an operation program of the robot created by simulation by the simulation device.
請求項５に記載の制御システムと、を備えたロボットシステム。 The robot;
A control system comprising the control system according to claim 5.
前記演算部が、前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
前記演算部が、前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出工程と、
前記演算部が、前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出工程と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
前記演算部が、所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
前記演算部が、前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出工程と、を備えたことを特徴とするシミュレーション方法。 A calculation method is used for simulating the operation of a robot having a joint, and is a simulation method for obtaining a contact force with which an object contacts the robot and a binding force at the joint of the robot,
In the joint space in which the number of joints of the robot is a degree of freedom, and the speed of the joint of the robot is q (·), the arithmetic unit has a degree of freedom based on a constraint condition of the joint of the robot. a restraining force f a in the coordinate space, a first Jacobian J a showing the relationship between the binding tau a in the joint space,
Based on the coordinate value in the world coordinate system of the contact point between the robot and the object, the arithmetic unit is a second coordinate represented by orthogonal coordinates based on the normal direction and the tangential direction of the contact point of the robot A second Jacobian calculating step of calculating a second Jacobian J B indicating a relationship between the contact force f B in the space and the contact force τ B in the joint space;
The arithmetic unit is a third Jacobian calculation step of obtaining a third Jacobian J the combined first Jacobian J A and the second Jacobian J B,
In the joint space, the acceleration of the joint of the robot is q (··), the mass matrix relating to the mass of the robot is A, the Coriolis force acting on the robot and the Coriolis force / centrifugal force term relating to the centrifugal force are b, A gravity term related to the working gravity is g, and a generated force generated in the joint of the robot is τ act, and the first motion equation of the robot in the joint space
The arithmetic unit sets a predetermined time as Δt, integrates the second equation of motion in the time domain, and linearizes the third equation of motion.
The arithmetic unit, the previously described linearizing third equation of motion and simultaneous equations to unknown said restraining force f A and the contact force f B, the restraining force f A and the contact by solving the simultaneous equations simulation method characterized by comprising: a power calculating step of calculating the force f B, the.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016164833A JP2018030210A (en) | 2016-08-25 | 2016-08-25 | Simulation device, control system, robot system, simulation method, program and recording medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016164833A JP2018030210A (en) | 2016-08-25 | 2016-08-25 | Simulation device, control system, robot system, simulation method, program and recording medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2018030210A true JP2018030210A (en) | 2018-03-01 |
Family
ID=61304370
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2016164833A Pending JP2018030210A (en) | 2016-08-25 | 2016-08-25 | Simulation device, control system, robot system, simulation method, program and recording medium |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2018030210A (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110795876A (en) * | 2019-10-23 | 2020-02-14 | 珠海格力智能装备有限公司 | Method for establishing finite element equivalent model of speed reducer |
| CN112464486A (en) * | 2020-12-02 | 2021-03-09 | 华北水利水电大学 | Numerical simulation method for transmission error of cycloidal pin gear speed reducer for robot |
| CN112818482A (en) * | 2021-01-19 | 2021-05-18 | 中国科学院沈阳自动化研究所 | Modeling and control method of elastic smooth curved surface constrained elongated soft robot |
| CN112818481A (en) * | 2021-01-19 | 2021-05-18 | 中国科学院沈阳自动化研究所 | Elastic plane constrained slender soft robot modeling and control method |
| JP2021082222A (en) * | 2019-11-22 | 2021-05-27 | キヤノン株式会社 | Information processing method, robot system, article manufacturing method, and information processing unit |
| CN113263503A (en) * | 2021-07-19 | 2021-08-17 | 上海捷勃特机器人有限公司 | Control unit of robot system, robot system and control method of robot system |
-
2016
- 2016-08-25 JP JP2016164833A patent/JP2018030210A/en active Pending
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110795876A (en) * | 2019-10-23 | 2020-02-14 | 珠海格力智能装备有限公司 | Method for establishing finite element equivalent model of speed reducer |
| JP2021082222A (en) * | 2019-11-22 | 2021-05-27 | キヤノン株式会社 | Information processing method, robot system, article manufacturing method, and information processing unit |
| CN112464486A (en) * | 2020-12-02 | 2021-03-09 | 华北水利水电大学 | Numerical simulation method for transmission error of cycloidal pin gear speed reducer for robot |
| CN112464486B (en) * | 2020-12-02 | 2024-03-19 | 华北水利水电大学 | Transmission error numerical simulation method of cycloidal pin gear speed reducer for robot |
| CN112818482A (en) * | 2021-01-19 | 2021-05-18 | 中国科学院沈阳自动化研究所 | Modeling and control method of elastic smooth curved surface constrained elongated soft robot |
| CN112818481A (en) * | 2021-01-19 | 2021-05-18 | 中国科学院沈阳自动化研究所 | Elastic plane constrained slender soft robot modeling and control method |
| CN112818482B (en) * | 2021-01-19 | 2023-07-21 | 中国科学院沈阳自动化研究所 | Modeling and control method of elastic smooth curved surface constrained slender soft robot |
| CN112818481B (en) * | 2021-01-19 | 2023-11-07 | 中国科学院沈阳自动化研究所 | Modeling and control method of elastic plane constraint slender soft robot |
| CN113263503A (en) * | 2021-07-19 | 2021-08-17 | 上海捷勃特机器人有限公司 | Control unit of robot system, robot system and control method of robot system |
| CN113263503B (en) * | 2021-07-19 | 2021-11-09 | 上海捷勃特机器人有限公司 | Control unit of robot system, robot system and control method of robot system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2018030210A (en) | Simulation device, control system, robot system, simulation method, program and recording medium | |
| US9120227B2 (en) | Human motion tracking control with strict contact force constraints for floating-base humanoid robots | |
| Eski et al. | Fault detection on robot manipulators using artificial neural networks | |
| JP5896789B2 (en) | Robot control apparatus, robot apparatus, robot control method, program, and recording medium | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| CN107530879B (en) | Multi-axis machine simulator, design support device for operation command device, design support device for motor control device, and motor capacity selection device | |
| Wang et al. | Kinematics and dynamics of a 6 degree-of-freedom fully parallel manipulator with elastic joints | |
| CN110035871A (en) | The system and method for being used to indicate robot | |
| EP2105263A2 (en) | Real time control method and device for robots in virtual projection | |
| Pedrammehr et al. | Closed-form dynamics of a hexarot parallel manipulator by means of the principle of virtual work | |
| Vallés et al. | Mechatronic development and dynamic control of a 3-DOF parallel manipulator | |
| Enferadi et al. | A Kane’s based algorithm for closed-form dynamic analysis of a new design of a 3RSS-S spherical parallel manipulator | |
| RU2308764C2 (en) | Method for moving a virtual jointed object in virtual space with prevention of collisions of jointed object with elements of environment | |
| Cazalilla et al. | Hybrid force/position control for a 3-DOF 1T2R parallel robot: Implementation, simulations and experiments | |
| Karimi Eskandary et al. | Trajectory-planning and normalized-variable control for parallel pick-and-place robots | |
| Zobova et al. | Multi-physics modelling of a compliant humanoid robot | |
| Filipovic | Relation between Euler–Bernoulli equation and contemporary knowledge in robotics | |
| JP2020075329A (en) | Simulation device, control system, robot system, simulation method, program and recording medium | |
| Bottin et al. | Comparison of under-actuated and fully actuated serial robotic arms: A case study | |
| Hwang et al. | Dynamic analysis and control of industrial robotic manipulators | |
| Ángel et al. | RoboTenis: design, dynamic modeling and preliminary control | |
| JP6540810B2 (en) | Hand force sense measurement device, hand force sense measurement method, and hand force sense measurement program | |
| Boujnah et al. | Motion simulation of a manipulator robot modeled by a CAD software | |
| Dupac | A path planning approach of 3D manipulators using zenithal gnomic projection and polar piecewise interpolation | |
| Denisov et al. | Designing simulation model of humanoid robot to study servo control system |