JP2015054360A - Speed control method for two-axis robot - Google Patents

Speed control method for two-axis robot Download PDF

Info

Publication number
JP2015054360A
JP2015054360A JP2013187341A JP2013187341A JP2015054360A JP 2015054360 A JP2015054360 A JP 2015054360A JP 2013187341 A JP2013187341 A JP 2013187341A JP 2013187341 A JP2013187341 A JP 2013187341A JP 2015054360 A JP2015054360 A JP 2015054360A
Authority
JP
Japan
Prior art keywords
point
robot
speed
axis
arm
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
Application number
JP2013187341A
Other languages
Japanese (ja)
Other versions
JP6123595B2 (en
Inventor
吉朗 岩崎
Yoshiro Iwasaki
吉朗 岩崎
Original Assignee
株式会社デンソーウェーブ
Denso Wave Inc
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 株式会社デンソーウェーブ, Denso Wave Inc filed Critical 株式会社デンソーウェーブ
Priority to JP2013187341A priority Critical patent/JP6123595B2/en
Publication of JP2015054360A publication Critical patent/JP2015054360A/en
Application granted granted Critical
Publication of JP6123595B2 publication Critical patent/JP6123595B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

PROBLEM TO BE SOLVED: To provide a speed control method that can exhibit a higher performance than convention with regards to a two-axis robot comprising a direct-acting shaft consisting of a ball screw shaft.SOLUTION: A two-axis XR robot enables a movable stage to move along a direct-acting shaft when a ball screw shaft is rotated by drive means. The movable stage is loaded with a joint and an arm rotationally driven and arranged so as to turn on the base point of a rotary shaft. When this XR robot is to be actuated, an upper limit speed of the movable stage is set to be variable with a movable stage position on the direct-acting shaft with regards to at least a partial range. Concretely, a dangerous rotation speed Nd of the ball screw shaft determined according to the movable stage position is compared with a DN value to determine it on the basis of an expression where the smaller value serves as a restraint requirement of the maximum speed.

Description

本発明は、両端が固定されたボールねじ軸からなる直動軸を備えてなる2軸ロボットを速度制御する方法に関する。   The present invention relates to a method for controlling the speed of a biaxial robot having a linear motion shaft composed of a ball screw shaft fixed at both ends.
現在、生産設備等の現場において、多種多様な産業用ロボットが使用されている。この産業用ロボットとして、ボールねじ軸からなる直動軸を備えてなる2軸ロボットがある。この2軸ロボットは、溶接、塗装、部品組立、部品搬送などの作業に使用され、それが担う作業の性質上、作業の始点から終点までの動作時間の短縮化、即ち、動作速度の高速化が求められている。   Currently, a wide variety of industrial robots are used in production facilities and the like. As this industrial robot, there is a two-axis robot provided with a linear motion axis composed of a ball screw axis. This 2-axis robot is used for work such as welding, painting, parts assembly, and parts transportation. Due to the nature of the work it carries out, the operation time from the start point to the end point is shortened, that is, the operation speed is increased. Is required.
一般に、ロボットの動作速度を制御する際に基本的な動作速度パターンとして、台形状の速度パターンが用いられることが多い(例えば、特許文献1,2参照)。台形状の動作速度パターンではその上辺部分が一定の上限速度となり、上辺部分に相当する動作期間では加速度がゼロとなる。   In general, a trapezoidal speed pattern is often used as a basic operation speed pattern when controlling the operation speed of a robot (see, for example, Patent Documents 1 and 2). In the trapezoidal motion speed pattern, the upper side portion has a constant upper limit speed, and the acceleration is zero during the motion period corresponding to the upper side portion.
特開2000−94371号公報JP 2000-94371 A 特開2002−321178号公報JP 2002-321178 A
従来の台形状の動作速度パターンで規定される上限速度については、安全を考慮して最悪値を用いることが多い。しかしながら、その上限速度が必ずしも、ロボットが採用している構成に基づいた適切な限界値に近接した値として設定されておらず、本来のロボットの性能を十分に発揮させることができない場合があった。   For the upper limit speed defined by the conventional trapezoidal operation speed pattern, the worst value is often used in consideration of safety. However, the upper limit speed is not necessarily set as a value close to an appropriate limit value based on the configuration adopted by the robot, and the original robot performance may not be fully exhibited. .
本発明は上記事情に鑑みてなされたもので、その目的は、ボールねじ軸からなる直動軸を備えてなる2軸ロボットについて、従来よりも高い性能を発揮させることができる2軸ロボットの速度制御方法を提供することにある。   The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a speed of a biaxial robot that can exhibit higher performance than the conventional one with respect to a biaxial robot having a linear motion shaft composed of a ball screw shaft. It is to provide a control method.
請求項1記載の2軸ロボットの速度制御方法によれば、2軸ロボットは、両端が固定されたボールねじ軸からなる直動軸を備えるもので、ボールねじ軸が第1駆動手段により回転されると移動体が直動軸に沿って移動する。その移動体には、第2駆動手段と、第2駆動手段により回転駆動され、回転軸を基点として回動可能に配置される回動アームとが搭載されている。この2軸ロボットを動作させる際に、移動体の上限速度を、少なくとも一部の範囲について、直動軸上における移動体の位置に応じて可変設定する。   According to the speed control method of the two-axis robot according to claim 1, the two-axis robot includes a linear motion shaft composed of a ball screw shaft fixed at both ends, and the ball screw shaft is rotated by the first driving means. Then, the moving body moves along the linear motion axis. The moving body is mounted with a second driving means and a rotating arm that is rotationally driven by the second driving means and is arranged so as to be rotatable about the rotation axis. When the two-axis robot is operated, the upper limit speed of the moving body is variably set according to the position of the moving body on the linear motion axis in at least a part of the range.
すなわち、上記の構成を採用する2軸ロボットについて、上限速度,或いは限界速度,危険速度として評価される速度は、移動体の直動軸上の位置に応じて異なっている。つまり、従来のように速度を制限した状態では、ロボットの性能を十分に引き出せていなかった。そこで、移動体の上限速度を、少なくとも一部の範囲について、直動軸上における移動体の位置に応じて可変設定することで、前記位置によっては上限速度をより高く設定し、従来よりも移動時間を短縮させて作業効率を向上させることができる。   That is, for the two-axis robot adopting the above configuration, the speed evaluated as the upper limit speed, the limit speed, or the dangerous speed varies depending on the position of the moving body on the linear motion axis. That is, in the state where the speed is limited as in the prior art, the performance of the robot cannot be sufficiently obtained. Therefore, the upper limit speed of the moving body is variably set in accordance with the position of the moving body on the linear motion axis in at least a part of the range, so that the upper limit speed is set higher depending on the position, and the moving body moves more than before. The working efficiency can be improved by reducing the time.
請求項2記載の2軸ロボットの速度制御方法によれば、移動体の位置に応じて求められるボールねじ軸の危険回転速度NdとDN値とを比較して、より小さい値を示す方を最大速度の拘束条件とする。すなわち、ボールねじ軸を回転させて移動体を直線移動させる構造については、ボールねじ軸の許容回転速度が上記のように規定される。そして、DN値は、ボールねじ軸におけるねじの型式による値Kと、ボール中心径Dより決まる固定値であるが、危険回転速度Ndは、直動軸上における移動体の位置をパラメータとする演算で決まるので、その位置に応じて最大速度を設定できる。   According to the speed control method of the two-axis robot according to claim 2, the ball screw shaft critical rotation speed Nd and the DN value obtained according to the position of the moving body are compared, and the one showing the smaller value is maximized. This is a speed constraint. That is, for the structure in which the ball screw shaft is rotated to move the moving body linearly, the allowable rotational speed of the ball screw shaft is defined as described above. The DN value is a fixed value determined by the value K of the screw type on the ball screw shaft and the ball center diameter D. The dangerous rotational speed Nd is calculated using the position of the moving body on the linear motion shaft as a parameter. The maximum speed can be set according to the position.
請求項3記載の2軸ロボットの速度制御方法によれば、回動アームの先端に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する際に、始点及び終点の位置を指定すると、始点から終点に向けて手先の移動を開始させるときに、回動アームの動作によって、第1駆動手段の作用により移動体が発生する加速力を増加させる反作用を生じさせる。また、手先を終点に停止させるときには、回動アームの動作によって、第1駆動手段の作用により移動体が発生する減速力を増加させる反作用を生じさせるように、移動体及び回動アームの速度パターンを生成することで前記軌道を生成する。   According to the speed control method of the biaxial robot according to claim 3, the position of the start point and the end point is generated when generating the hand trajectory for moving the hand located at the tip of the rotating arm from the start point to the end point. When the movement of the hand is started from the start point to the end point, a reaction that increases the acceleration force generated by the moving body by the action of the first driving means is caused by the action of the rotating arm. Further, when the hand is stopped at the end point, the speed pattern of the moving body and the rotating arm is generated so that the reaction of increasing the deceleration force generated by the moving body by the action of the first driving means is caused by the operation of the rotating arm. To generate the trajectory.
従来の多軸型ロボットの制御では、例えばピック・アンド・プレイスのような単純な作業を行わせる場合、始点、終点間で動作量に変化がない軸は動かない。この状態では、手先側の回動アームは、基部側にある第1駆動手段の負荷となっているだけであり、第1駆動手段の負担が大きく、また、動作速度の上昇を図るためには障害となっている。   In conventional multi-axis robot control, for example, when a simple work such as pick-and-place is performed, an axis whose movement amount does not change between the start point and the end point does not move. In this state, the pivot arm on the hand side is only a load of the first driving means on the base side, and the burden on the first driving means is heavy, and in order to increase the operating speed It is an obstacle.
これに対して、上述したように軌道を生成すると、回動アームが移動体の動きに対し反作用を生じるように動作する。したがって、手先の起動時に必要な加速力と、停止時に必要な減速力とをそれぞれ増加させて、手先を始点から終点まで移動させる速度を向上させ、移動時間を短縮することができる。ここで、
(最大関節加速度)=(最大関節トルク+干渉トルク+重力トルク+摩擦トルク)
/イナーシャ …(A)’
(最大関節速度)=(モータ最大速度)/(減速比) …(B)
である。ただし、ここでは説明簡略化のため水平方向の動作のみを考えることとし、(A)’式において重力トルク及び摩擦トルクを無視したものを(A)式とする。例えばバンバン(Bang-Bang)制御のように、常に何れかの軸が最大トルクを出力することのみ考慮し、速度の上限を考慮しない場合は、(A)式に基づきアームを折り畳みイナーシャを小さくすることで、加速度を向上させることによる動作短縮が期待できる。
In contrast, when the trajectory is generated as described above, the rotating arm operates so as to react against the movement of the moving body. Therefore, the acceleration force required at the time of starting the hand and the deceleration force required at the time of stopping are respectively increased, the speed at which the hand is moved from the start point to the end point can be improved, and the movement time can be shortened. here,
(Maximum joint acceleration) = (maximum joint torque + interference torque + gravity torque + friction torque)
/ Inertia (A) '
(Maximum joint speed) = (Maximum motor speed) / (Reduction ratio) (B)
It is. However, here, for simplification of explanation, only the horizontal operation is considered, and in the equation (A) ′, the gravity torque and the friction torque are ignored as the equation (A). For example, if only one shaft always outputs the maximum torque, and the upper limit of speed is not considered, as in Bang-Bang control, the arm is folded based on equation (A) to reduce the inertia. Therefore, shortening of the operation by improving the acceleration can be expected.
しかしながら、現在多く普及しているロボットの大半は減速比が高く、(B)式より最大関節速度が低いため直ぐに上限に達してしまう。このため、動作時間を短縮するには加減速を急峻に立ち上げるしかない。加減速度を向上させるには、(A)式よりイナーシャを小さくすること、干渉トルク項の符号を関節トルク符号と同じ(アシストする方向)にすることが有効であると判る。つまり動作としては、加減速時は回動アームを移動体の動作方向と逆向きに振ることに対応する。   However, most of the robots that are currently widely used have a high reduction ratio, and the maximum joint speed is lower than that in the formula (B), so the upper limit is reached soon. For this reason, the only way to shorten the operation time is to start up acceleration / deceleration steeply. In order to improve the acceleration / deceleration, it can be seen that it is effective to make the inertia smaller from the equation (A) and to make the sign of the interference torque term the same as the joint torque code (direction to assist). That is, the operation corresponds to swinging the rotating arm in the direction opposite to the moving direction of the moving body during acceleration / deceleration.
このとき、干渉される直動軸を回転させる第1駆動手段は、前述した台形の速度パターンの上限を超えて加速できる。つまり、第1駆動手段が有している性能をフルに発揮でき、従来以上の加速を出すことができるので、動作時間を短縮できる。逆に言えば、従来の手法と同じ移動時間で動作させるのであれば、従来と同じ加速度を出力するのに少ないトルクで済むため、第1駆動手段の容量を下げることができ、ロボットの小型、低コスト化に貢献できる。そして、上述した軌道は、2軸ロボットの動力学に基づいて計算された結果として生成されるので、ロボットの拘束条件を満たす範囲内で速度を最大化する軌道が求められる。したがって、現実的に使用できる軌道が得られる。   At this time, the first driving means for rotating the linear motion shaft to be interfered can accelerate beyond the upper limit of the trapezoidal speed pattern described above. That is, the performance of the first driving means can be fully exhibited, and the acceleration can be increased more than before, so that the operation time can be shortened. In other words, if the operation is performed with the same movement time as the conventional method, less torque is required to output the same acceleration as the conventional method, so the capacity of the first driving means can be reduced, the robot can be reduced in size, Contributes to cost reduction. Since the above-described trajectory is generated as a result calculated based on the dynamics of the two-axis robot, a trajectory that maximizes the speed within a range that satisfies the constraint condition of the robot is obtained. Therefore, a trajectory that can be practically used is obtained.
請求項4記載の2軸ロボットの速度制御方法によれば、始点と終点との間に複数の初期経由点を自動的に設定し、初期経由点の位置を更新しながら最短時間の速度パターンを生成するので、始点から終点への移動時間が最短となる速度パターンの軌道を生成できる。   According to the speed control method of the biaxial robot according to claim 4, a plurality of initial via points are automatically set between the start point and the end point, and the speed pattern of the shortest time is set while updating the position of the initial via point. Since it is generated, it is possible to generate a trajectory of a speed pattern that minimizes the movement time from the start point to the end point.
請求項5記載の2軸ロボットの速度制御方法によれば、初期経由点を3点設定する。ここで、手先を始点から終点まで移動させることを想定すると、起動直後に移動体に加速度を付与する時点と、動作途中で加速度の方向が反転する時点と、停止直前に加速度をゼロにする時点とがある。そして、軌道の最適化を図る過程では、少なくとも上記3つの時点に対応する経由点を最適化することが重要になると考えられる。したがって、上記3つの時点に対応する経由点を設定すれば、演算量を抑制しつつ効率的に、移動時間を最短にする軌道を生成することが可能となる。これにより、従来の方法では、方法自体の限界によって引き出すことができなかった駆動手段たるモータ,アクチュエータのポテンシャルを最大限引き出しつつ、現実的に使用できる最適化された軌道が得られるようになる。   According to the two-axis robot speed control method of the fifth aspect, three initial waypoints are set. Assuming that the hand is moved from the start point to the end point, the time when acceleration is applied to the moving body immediately after startup, the time when the direction of acceleration is reversed during the operation, and the time when acceleration is zeroed immediately before stopping There is. In the course of optimizing the trajectory, it is considered important to optimize at least the waypoints corresponding to the above three points. Therefore, by setting the waypoints corresponding to the above three time points, it is possible to efficiently generate a trajectory that minimizes the travel time while suppressing the amount of calculation. As a result, in the conventional method, an optimized trajectory that can be practically used can be obtained while maximizing the potential of the motor and actuator as drive means that could not be extracted due to limitations of the method itself.
第1実施形態であり、XRロボットの動作速度をシミュレーションした結果を示す図The figure which is 1st Embodiment and shows the result of having simulated the operation speed of the XR robot XRロボットの構成を示す平面図Plan view showing the configuration of the XR robot XRロボットにおける本願の軌道生成方法の効果を説明する模式図Schematic diagram explaining the effect of the trajectory generation method of the present application in an XR robot XRロボットの機構的数値パラメータを説明する図The figure explaining the mechanical numerical parameter of the XR robot XRロボットの制御装置の構成を示す機能ブロック図Functional block diagram showing the configuration of the control device of the XR robot 第2実施形態であり、ロボットの最短時間の動作軌跡を求めるための処理手順を概略的に示す図The figure which is 2nd Embodiment and shows roughly the process sequence for calculating | requiring the movement locus | trajectory of the shortest time of a robot. ロボットのアームの移動を制御する方法の原理を従来例と比較して説明する図The figure explaining the principle of the method of controlling the movement of the arm of the robot compared with the conventional example 4軸ロボットの物理的パラメータ(動力学パラメータ)をXY平面上に示す図Diagram showing physical parameters (dynamic parameters) of the 4-axis robot on the XY plane 制御部が実行する、速度パターンを作成するためのフローチャートFlow chart for creating a speed pattern executed by the control unit 図9の処理の一部として実行される、拘束条件を加味したボブロー法による速度パターンの作成の詳細を示すフローチャートThe flowchart which shows the detail of the production | generation of the speed pattern by the Boblow method which considered the constraint condition performed as a part of process of FIG. ロボットアームの右手系,左手系を説明する図Diagram explaining the right and left hand systems of the robot arm 手先の座標位置を各軸の角度に変換して速度パターンの計算を行う理由を説明する図The figure explaining the reason for calculating the speed pattern by converting the coordinate position of the hand to the angle of each axis 加速度の拘束条件を説明する説明図Explanatory drawing explaining the constraint condition of acceleration XY平面上で第1軸,第2軸及び手先の座標が変化する軌跡を示す図The figure which shows the locus | trajectory in which the coordinate of a 1st axis | shaft, a 2nd axis | shaft, and a hand changes on XY plane. (a)は図14(a)〜(e)に示す5つの手先位置を同時にプロットした図、(b)は始点から終点まで移動する間の、第1軸,第2軸それぞれの角度,角速度,角加速度,トルクの変化を示す図(A) is a diagram in which the five hand positions shown in FIGS. 14 (a) to (e) are plotted simultaneously, and (b) is the angle and angular velocity of each of the first and second axes while moving from the start point to the end point. , Diagram showing changes in angular acceleration and torque 図1相当図1 equivalent diagram XRロボットの各軸動作を同期させる場合を説明する図The figure explaining the case where each axis operation of an XR robot is synchronized 第3実施形態を示す図9相当図FIG. 9 equivalent diagram showing the third embodiment. 図10相当図Fig. 10 equivalent
(第1実施形態)
以下、第1実施形態について図1から図5を参照して説明する。図2に示すXRロボットは、ボールねじ軸1を備え、且つ両端が固定手段2により固定ベース3に固定された直動軸J1(X軸,第1軸)と、この直動軸J1にナット部材4を介して移動可能に保持された移動ステージ5と、この移動ステージ5の内部に配置されているジョイント6(関節,モータを含む:第2駆動手段)を介して基部側が回転可能に保持されたアーム7(回動アーム)とを備える。このアーム7の先端側が手先となる。ジョイント6が回転軸(R軸,第2軸)J2を成す。ボールねじ軸1は、図4中右端側に配置されている、モータを含む駆動手段8(第1駆動手段)により回転駆動される。これにより、2軸のXRロボット9が構成されている。
(First embodiment)
Hereinafter, the first embodiment will be described with reference to FIGS. 1 to 5. The XR robot shown in FIG. 2 includes a linear motion shaft J1 (X axis, first axis) having a ball screw shaft 1 and both ends fixed to a fixed base 3 by fixing means 2, and a nut on the linear motion shaft J1. A movable stage 5 movably held via a member 4 and a base side rotatably held via a joint 6 (including a joint and a motor: second driving means) arranged inside the movable stage 5 Arm 7 (rotating arm). The tip side of the arm 7 is a hand. The joint 6 forms a rotation axis (R axis, second axis) J2. The ball screw shaft 1 is rotationally driven by driving means 8 (first driving means) including a motor, which is disposed on the right end side in FIG. Thus, a biaxial XR robot 9 is configured.
XRロボット9の動作(ジョイント6及び駆動手段8)は、それぞれ図5に示すように、ロボットコントローラRCの制御部11により作業工程プログラム等に基づいて制御されるようになっている。この制御部11(制御装置,制御手段)は、CPU,ROM,RAMなどを備えて構成され、ティーチングペンダント12が接続されている。ティーチングペンダント12には、スイッチ群13が設けられていると共に、表示手段たる表示部14が設けられている。アーム回転用モータ6Mは、ジョイント6を構成するモータであり、直線移動用モータ8Mは、駆動手段8に内蔵されているモータである。   The operation of the XR robot 9 (joint 6 and driving means 8) is controlled by the control unit 11 of the robot controller RC based on a work process program, as shown in FIG. The control unit 11 (control device, control means) includes a CPU, a ROM, a RAM, and the like, and a teaching pendant 12 is connected thereto. The teaching pendant 12 is provided with a switch group 13 and a display unit 14 as display means. The arm rotation motor 6M is a motor constituting the joint 6, and the linear movement motor 8M is a motor built in the driving means 8.
ボールねじなどの単軸は、その上限速度(限界速度、危険速度と呼ばれる)が定められているが、この速度の値はその軸の固定端からの位置によって変化する。従来の台形速度パターンで駆動させる場合、台形を成す上辺(上限速度)は一定値であり、安全率を考慮した値に設定していた。すなわち、図3に一例を示すように、上限速度は例えば回転速度2000[rpm]未満の一定値として付与されていた。   A single shaft such as a ball screw has an upper limit speed (referred to as a critical speed or a critical speed), and the value of this speed varies depending on the position from the fixed end of the shaft. In the case of driving with a conventional trapezoidal speed pattern, the upper side (upper limit speed) forming the trapezoid is a constant value and set to a value in consideration of the safety factor. That is, as shown in FIG. 3 as an example, the upper limit speed is given as a constant value less than a rotational speed of 2000 [rpm], for example.
しかし、実際の駆動手段8のようなモータの許容回転数Npは、ねじ軸の危険速度NとDN値(回転限界値)との小さい方で決まる(例えば、特開2006−198716号公報参照)。すなわち、
許容回転数Np=Min(Nd,DN値)…(1)
尚、DN値は、ねじの型式による値(メーカ値)Kと、ボール中心径Dより以下で計算される。
DN値=K/D …(2)
許容回転数について、ロボットの位置に依存するのは危険速度Ndであり、以下のように計算される。図4に示すように、ロボット走行軸(直動軸)の位置をJ1[mm](図2に示すナット部材4の位置Pに相当),メカストローク長をSt[mm],可動範囲をSf[mm]とすると、固定端からの位置La[mm]は、
La=J1+(St−Sf)/2 …(3)
となる。取り付け間距離Lb[mm]は、両端から遠い方を用いて次式とする。
Lb=Max(La,St−La) …(4)
よって、ボールねじ軸の取り付け方法(固定/支持/自由)による係数k,ねじ軸谷径d1[mm]とすると、危険速度Ndは次式となる。
Nd=k×d1×10/Lb …(5)
上記の(1)式に基づいて規定された上限速度が図3に示すものであり、上限速度がフラットになっている部分(3400rpm程度)は、DN値によって規定されている。
However, the allowable rotational speed Np of a motor such as the actual drive means 8 is determined by the smaller of the critical speed N and DN value (rotation limit value) of the screw shaft (see, for example, JP-A-2006-198716). . That is,
Allowable rotational speed Np = Min (Nd, DN value) (1)
The DN value is calculated from the value (manufacturer value) K according to the screw type and the ball center diameter D as follows.
DN value = K / D (2)
The permissible rotational speed depends on the position of the robot is the critical speed Nd, which is calculated as follows. As shown in FIG. 4, the position of the robot travel axis (linear motion axis) is J1 [mm] (corresponding to the position P of the nut member 4 shown in FIG. 2), the mechanical stroke length is St [mm], and the movable range is Sf. Assuming [mm], the position La [mm] from the fixed end is
La = J1 + (St−Sf) / 2 (3)
It becomes. The inter-mounting distance Lb [mm] is given by the following equation using the one far from both ends.
Lb = Max (La, St−La) (4)
Therefore, assuming that the coefficient k and the screw shaft valley diameter d1 [mm] depend on the ball screw shaft mounting method (fixed / supported / free), the critical speed Nd is expressed by the following equation.
Nd = k × d1 × 10 7 / Lb (5)
The upper limit speed defined based on the above equation (1) is shown in FIG. 3, and the portion where the upper limit speed is flat (about 3400 rpm) is defined by the DN value.
また、図3に示す上限速度に従い、XRロボットの動作をシミュレーションした結果を図1(a),(c)に示す。図1(b)は図3相当図であり、(c)は(a)を拡大して示したものである。図1(c)に示す一点鎖線は、XRロボットの移動ステージ5(移動体)を移動(0[mm]〜1200[mm])させる際に、従来と同様にボールねじ軸1のみを回転駆動させた場合で(上限速度は固定)、移動時間は約907msである。   Moreover, the result of having simulated the operation | movement of the XR robot according to the upper limit speed shown in FIG. 3 is shown to FIG. 1 (a), (c). FIG. 1B is a view corresponding to FIG. 3, and FIG. 1C is an enlarged view of FIG. In FIG. 1C, the alternate long and short dash line indicates that when the moving stage 5 (moving body) of the XR robot is moved (0 [mm] to 1200 [mm]), only the ball screw shaft 1 is rotationally driven as in the prior art. In this case (the upper limit speed is fixed), the movement time is about 907 ms.
これに対して破線は、移動ステージ5の移動はボールねじ軸1の回転駆動だけで行うが、(1)式に基づいて上限速度を可変させると、移動時間は約596msまで大幅に短縮される。これにより、図3においてハッチングした領域で模式的に示すように、従来では使い切れていない速度領域を有効に使用できる。つまり、より最短の軌道で手先を移動させることができると共に、省エネルギーにもなる。   On the other hand, the broken line indicates that the moving stage 5 is moved only by the rotational drive of the ball screw shaft 1, but if the upper limit speed is varied based on the equation (1), the moving time is greatly reduced to about 596 ms. . Thereby, as schematically shown by the hatched area in FIG. 3, a speed area that has not been used up conventionally can be used effectively. That is, it is possible to move the hand with a shorter path and to save energy.
以上のように本実施形態によれば、2軸のXRロボット9は、ボールねじ軸1が駆動手段8により回転されると移動ステージ5が直動軸に沿って移動するもので、その移動ステージ5には、ジョイント6と、ジョイント6により回転駆動され、回転軸を基点として回動可能に配置されるアーム7とが搭載される。このXRロボット9を動作させる際に、移動ステージ5の上限速度を、少なくとも一部の範囲について、直動軸上における移動ステージ5の位置に応じて可変設定する。具体的には、移動ステージ5の位置に応じて求められるボールねじ軸1の危険回転速度NdとDN値とを比較して、より小さい値を示す方を最大速度の拘束条件とする(1)式に基づき決定する。したがって、前記位置によっては上限速度をより高く設定することが可能となり、従来よりも移動時間を短縮させて作業効率を向上させることができる。   As described above, according to the present embodiment, the biaxial XR robot 9 is such that when the ball screw shaft 1 is rotated by the driving means 8, the moving stage 5 moves along the linear movement axis. 5 is mounted with a joint 6 and an arm 7 that is rotationally driven by the joint 6 and is disposed so as to be rotatable about a rotation axis. When the XR robot 9 is operated, the upper limit speed of the moving stage 5 is variably set in accordance with the position of the moving stage 5 on the linear motion axis in at least a part of the range. Specifically, the critical rotational speed Nd and DN value of the ball screw shaft 1 obtained according to the position of the moving stage 5 are compared, and the one showing the smaller value is set as the maximum speed constraint condition (1). Determine based on the formula. Therefore, the upper limit speed can be set higher depending on the position, and the working time can be improved by shortening the movement time as compared with the prior art.
(第2実施形態)
以下、第2実施形態について図6から図17を参照して説明する。第2実施形態では、XRロボット9の動作速度をより向上させるための軌道生成方法を示す。先ず、制御法の概要について図6及び図7を参照して説明するが、最初に図7を参照して、第2実施形態に係る制御法を従来の制御法と比較しながら説明する。
(Second Embodiment)
Hereinafter, a second embodiment will be described with reference to FIGS. In the second embodiment, a trajectory generation method for further improving the operation speed of the XR robot 9 will be described. First, the outline of the control method will be described with reference to FIGS. 6 and 7. First, the control method according to the second embodiment will be described in comparison with the conventional control method with reference to FIG.
図7(a)に示すように、第1及び第2アーム(リンク)AM1,AM2を2つのジョイントJT1,JT2で可動可能に結合した簡単な2軸(J1,J2)ロボットを想定する。このロボットのアーム手先EFを、X−Y平面上において(J1,J2)=(0°,0°)の角度位置から(J1,J2)=(90°,0°)の角度位置まで移動させることを想定する。   As shown in FIG. 7A, a simple two-axis (J1, J2) robot is assumed in which first and second arms (links) AM1, AM2 are movably coupled by two joints JT1, JT2. The arm end EF of the robot is moved from the angular position (J1, J2) = (0 °, 0 °) to the angular position (J1, J2) = (90 °, 0 °) on the XY plane. Assume that.
この場合の最も単純な手法は、図7(b)に示すように、第2のアームAM2(即ち第2軸J2)は動作させず、第1のアームAM1(即ち第1軸J1)のみを90度回転させるものである。この手法では、第1のアームAM1のみを動作させるため、負荷が偏り、第1のアームAM1の負担が大きく、また、アーム全体の動作半径が常に最大でイナーシャが大きくなる。このため、動作速度が遅くなるという問題がある。   As shown in FIG. 7B, the simplest method in this case is that the second arm AM2 (that is, the second axis J2) is not operated, and only the first arm AM1 (that is, the first axis J1) is operated. It is rotated 90 degrees. In this method, since only the first arm AM1 is operated, the load is unbalanced, the burden on the first arm AM1 is large, and the operating radius of the entire arm is always maximum and the inertia increases. For this reason, there exists a problem that operation speed becomes slow.
図7(c)は前述したバンバン(Bang-Bang)制御による動作である。この場合、第1のアームAM1と共に第2のアームAM2も動作させてアームを折り畳む。これにより、イナーシャが小さくなるため高速で動作させることができる。しかしながら、ロボットに課されている速度の制約等の拘束条件を全く考慮していないので、実際にこの様に動作させて動作時間の短縮を図ることは不可能である。   FIG. 7C shows the operation by the above-described Bang-Bang control. In this case, the second arm AM2 is operated together with the first arm AM1 to fold the arm. Thereby, since inertia becomes small, it can be operated at high speed. However, since no constraint conditions such as speed constraints imposed on the robot are taken into consideration, it is impossible to actually reduce the operation time by operating in this way.
第2実施形態では、概略的に図7(d)に示すような動作軌跡を実現させる。すなわち、手先EFの移動を開始させると、最初に第1のアームAM1(第1軸J1)を終点方向に回転させると共に(矢印A参照)、第2のアームAM2(第2軸J2)を終点方向と逆方向に回転させる(矢印A´参照)。このとき、アームの形態は所謂左手系となる。   In the second embodiment, an operation locus as schematically shown in FIG. 7D is realized. That is, when the movement of the hand EF is started, the first arm AM1 (first axis J1) is first rotated in the end point direction (see arrow A), and the second arm AM2 (second axis J2) is ended. Rotate in the opposite direction (see arrow A ′). At this time, the form of the arm is a so-called left-handed system.
その後、第1のアームAM1を回転させている途中で(矢印A参照)、第1のアームAM1を基準として見た場合に、手先位置(第2アームAM2の先端位置)が第1のアームAM1の先端位置よりも先行して終点側に近づくように、第2のアームAM2をその回転方向に振り出させる(矢印B´参照)。このとき、アームの形態は所謂右手系となる。   Thereafter, when the first arm AM1 is being rotated (see arrow A), when viewed from the first arm AM1, the hand position (the tip position of the second arm AM2) is the first arm AM1. The second arm AM2 is swung in the direction of rotation so as to approach the end point ahead of the tip position (see arrow B ′). At this time, the form of the arm is a so-called right-handed system.
すなわち、図7(b)の動作の場合、第1のアームAM1を回転中心とする半径上に常に第2のアームAM2と手先EFとが位置する。これに対して、図7(d)の動作の場合、最初は第2のアームAM2が始点方向の側に回転する(矢印A´)ことで手先の回転が遅れる。しかし、その後、第2のアームAM2が終点方向の側に、第1のアームAM1よりも先行して回転する。つまり手先EFが、第1のアームAM1よりも先行した状態となる。   That is, in the case of the operation of FIG. 7B, the second arm AM2 and the hand EF are always located on the radius with the first arm AM1 as the rotation center. On the other hand, in the case of the operation shown in FIG. 7 (d), the second arm AM2 is initially rotated toward the start point direction (arrow A '), so that the rotation of the hand is delayed. However, after that, the second arm AM2 rotates in the direction of the end point ahead of the first arm AM1. That is, the hand EF is in a state preceding the first arm AM1.
この様に、2つのアームAM1,AM2(第1軸及び第2軸)の動きを協働させることで、図7(b)の動作に比較して、2つのジョイントJT1,JT2のモータに掛かる負荷を分散させることができる。また、アームが若干折り畳まれるのでイナーシャがより小さくなり、モータに掛かる負荷そのものが減少する。   In this way, by cooperating the movements of the two arms AM1 and AM2 (the first axis and the second axis), the motors of the two joints JT1 and JT2 are applied as compared with the operation of FIG. 7B. The load can be distributed. Further, since the arm is slightly folded, the inertia becomes smaller and the load applied to the motor itself is reduced.
加えて、第2アームAM2の動作が第1アームAM1、すなわち第1軸に反作用に伴う力を生じさせる。つまり、手先EFが始点から移動するときには、第2のアームAM2の一時的な逆方向への回転(矢印A´)の反作用に伴い、矢印A″で示す付勢力が第1のアームAM1に加わる。この付勢力A″は、第1のアームAM1の終点側への回転を加勢する向きに加わる。反対に、手先FEが終点に接近してくると、第2のアームAM2のそれまでの回転方向への先行する運動(矢印B´)の反作用に伴い、矢印B″で示す抑制力が第1のアームAM1に加わる。つまり、この抑制力B″は、第1のアームAM1のそれまでの回転(矢印A)を抑制しようとする向きに働く。   In addition, the operation of the second arm AM2 generates a force associated with the reaction on the first arm AM1, that is, the first axis. That is, when the hand EF moves from the starting point, the urging force indicated by the arrow A ″ is applied to the first arm AM1 with the reaction of the temporary rotation of the second arm AM2 in the reverse direction (arrow A ′). This urging force A ″ is applied in a direction that urges the rotation of the first arm AM1 toward the end point. On the other hand, when the hand FE approaches the end point, the restraining force indicated by the arrow B ″ is increased due to the reaction of the preceding movement (arrow B ′) of the second arm AM2 in the previous rotation direction. That is, the restraining force B ″ acts in a direction to restrain the rotation (arrow A) of the first arm AM1 up to that time.
このように、手先EFが回転を開始するとき及び回転を終了させるときのそれぞれにおいて、第2のアームAM2の一時的な動作により生じる反作用の力、即ち付勢力及び抑制力が、第1のアームAM1の回転の加速及び減速を積極的にアシストする。このため、手先EFを(J1,J2)=(0°,0°)の角度位置から(J1,J2)=(90°,0°)の角度位置まで移動させるのに要する時間は、上述したアシストが無い制御と比べて、格段に短縮される。   As described above, when the hand EF starts to rotate and ends the rotation, the reaction force generated by the temporary operation of the second arm AM2, that is, the urging force and the suppressing force, is applied to the first arm. Actively assists in acceleration and deceleration of AM1 rotation. Therefore, the time required to move the hand EF from the angular position (J1, J2) = (0 °, 0 °) to the angular position (J1, J2) = (90 °, 0 °) is as described above. Compared to control without assist, the time is significantly shortened.
また、この様にアーム動作をさせるには、予めアーム、すなわち、各軸のモータを回転させるための速度パターンを生成しておく必要がある。この速度パターンを生成するに当たっては、後述する具体的な手法により、ロボットの拘束条件を満たすことを前提として、最も効率の良い動作軌道を選択する。   Further, in order to perform the arm operation in this way, it is necessary to generate a speed pattern for rotating the arm, that is, the motor of each axis in advance. In generating this speed pattern, the most efficient motion trajectory is selected by the specific method described later on the premise that the constraint condition of the robot is satisfied.
図6は、図7(d)に示すような動作軌跡を実現させるための処理手順を概略的に示したものである。先ず、ロボットの手先を、始点から終点まで移動させる間に、複数の経由点を仮に設定する(図6(a))。そして、手先の軌道を計算するためにボブロー(Bobrow)法を用いることで、各経由点を通過しながら手先を移動させるのに最速となる速度パターンを演算する(同図(b))。このボブロー法を用いた演算を行うに当たっては、実際のロボットの運動方程式が考慮されるため、各軸の動作速度やトルクの制限など、ロボットの拘束条件を満たした上で最速となる速度パターンが求められる。   FIG. 6 schematically shows a processing procedure for realizing the motion trajectory as shown in FIG. First, a plurality of waypoints are temporarily set while moving the hand of the robot from the start point to the end point (FIG. 6A). Then, by using the Bobrow method to calculate the trajectory of the hand, a speed pattern that is the fastest for moving the hand while passing through each via point is calculated ((b) in the figure). When performing calculations using this Boblow method, the equation of motion of the actual robot is taken into consideration, so the speed pattern that becomes the fastest after satisfying the constraint conditions of the robot, such as the operation speed of each axis and the torque limit, is Desired.
速度パターンを求めると、その速度パターンに従ってロボットを所定の始点から終点まで動作させた場合の動作時間を算出する(同図(c))。そして、経由点の位置を更新しながら(同図(d))上述したプロセスを繰り返し実行し、動作時間が最短に収束したと判断した時点で処理を終了する。この一連の処理は、多軸ロボットの各軸に対して実行される。その結果として得られた軌道が、ロボットの拘束条件(速度<速度制限値、且つ、トルク<トルク制限値)を満たした上で、手先を始点から終点まで最短時間で移動できる軌道となる。このようにして求めた手先の経由点及び各軸の速度パターンの情報(軌道情報)は、ロボットコントローラに格納されて実際の多軸ロボットの制御に使用される。   When the speed pattern is obtained, an operation time when the robot is operated from a predetermined start point to an end point according to the speed pattern is calculated ((c) in the figure). Then, the above-described process is repeatedly executed while updating the position of the via point ((d) in the figure), and the process is terminated when it is determined that the operation time has converged to the shortest. This series of processing is executed for each axis of the multi-axis robot. The trajectory obtained as a result becomes a trajectory that can move the hand from the start point to the end point in the shortest time after satisfying the robot constraint conditions (speed <speed limit value and torque <torque limit value). The information on the hand point and the velocity pattern of each axis (trajectory information) obtained in this way are stored in the robot controller and used for actual control of the multi-axis robot.
次に、図8から図17を参照して、上述した多軸ロボットの軌道情報を生成方法及びその軌道情報を使用した多軸ロボットの制御装置の一例を詳述する。図8は、XRロボット9のアーム7の手先に、更に第3軸,第4軸がある4軸ロボットの物理的パラメータ(動力学パラメータ)をXY平面上に示している。図中に示すm1,m2及びm3は、それぞれ第1のリンクの質量[kg]、第2のリンクの質量[kg]、及び「第3及び第4のリンク並びに負荷」の総質量[kg]である。lg2[m]は、第1のリンクの軸位置からの第2のリンクの重心までの距離,l2[m]は第2のリンクの長さである。また、I2[kgm]は第2のリンクのリンク重心回りのイナーシャ,I4[kgm]は、第3のリンク及び第4のリンクの総重心回りのイナーシャである。また、θ1は第1軸の関節位置,θ2は第2軸の関節角度である。 Next, with reference to FIGS. 8 to 17, an example of the method for generating the trajectory information of the multi-axis robot described above and an example of the control apparatus for the multi-axis robot using the trajectory information will be described in detail. FIG. 8 shows, on the XY plane, physical parameters (dynamic parameters) of a four-axis robot having a third axis and a fourth axis at the end of the arm 7 of the XR robot 9. M1, m2, and m3 shown in the figure are the mass [kg] of the first link, the mass [kg] of the second link, and the total mass [kg] of the “third and fourth links and load”, respectively. It is. lg2 [m] is the distance from the axial position of the first link to the center of gravity of the second link, and l2 [m] is the length of the second link. I2 [kgm 2 ] is an inertia around the center of gravity of the second link, and I4 [kgm 2 ] is an inertia around the total center of gravity of the third link and the fourth link. Θ1 is the joint position of the first axis, and θ2 is the joint angle of the second axis.
第2実施形態に係る軌道生成方法及び制御法は、これらの4軸のうち、何れかの2軸及び3軸以上の複数軸について実施できるが、以下の説明ではXRロボット9に対応させて第1及び第2軸のみを計算対象として説明する。   The trajectory generation method and the control method according to the second embodiment can be performed on any two axes and a plurality of three or more axes among these four axes. Only the first and second axes will be described as calculation targets.
ここで、以下の説明で使用する物理量θ,λに関して、以下のような定義を与えておくことにする。
θ :ロボットの位置を示す、各軸の関節角[rad]
θ´:ロボットの速度を示す、各軸の関節角速度[rad/s]
θ″:ロボットの加速度を示す、各軸の関節角加速度[rad/s
λ :軌道上の各位置を示す軌道パラメータ
λ´:軌道上の各位置における傾きであり、軌道の速度を示す。
λ″:軌道上の各位置における曲率を示す、軌道の加速度を示す。
上述した多軸XRロボット9の運動方程式の一般表記は(6)式となる。
但し、τはトルクベクトルである。そして、(6)式における慣性行列Hと、遠心力・コリオリ力を表す行列hとに、XRロボット9の物理的パラメータが反映されている。ここで、
また、(6)式におけるBは摩擦,gは重力であるが、以下ではこれらを無視して説明する。
尚、ロボットの運動方程式の詳細については、以下の文献(1)を参照できる。
・<文献(1)>Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9-15,2006,Beijing China “Time-Optimal Trajectory Generation of Fast-Motion Planar Parallel Manipulator” by Yanjie Liu, Chenqi Wang, Juan Li, Lining Sun
Here, the following definitions are given for the physical quantities θ and λ used in the following description.
θ: joint angle of each axis indicating the position of the robot [rad]
θ ′: the joint angular velocity [rad / s] indicating the speed of the robot
θ ″: joint angular acceleration [rad / s 2 ] of each axis indicating the acceleration of the robot
λ: Trajectory parameter indicating each position on the trajectory λ ′: Inclination at each position on the trajectory, indicating the speed of the trajectory.
λ ″: Indicates the acceleration of the trajectory indicating the curvature at each position on the trajectory.
The general notation of the equation of motion of the multi-axis XR robot 9 described above is expressed by equation (6).
Where τ is a torque vector. The physical parameters of the XR robot 9 are reflected in the inertia matrix H and the matrix h representing the centrifugal force / Coriolis force in the equation (6). here,
Further, B in equation (6) is friction and g is gravity, but in the following description, these will be ignored.
For details of the equation of motion of the robot, the following document (1) can be referred to.
・ <Reference (1)> Proceedings of the 2006 IEEE / RSJ International Conference on Intelligent Robots and Systems October 9-15,2006, Beijing China “Time-Optimal Trajectory Generation of Fast-Motion Planar Parallel Manipulator” by Yanjie Liu, Chenqi Wang , Juan Li, Lining Sun
図9は、図6に示す処理に対応して制御部11が実行するフローチャートである。先ず、制御部11は、例えばオペレータより与えられたXRロボット9の手先の始点,終点を読み込むと(ステップS1,位置指定ステップ,位置指定手段)、その始点,終点の間の3点を、初期経由点(補間点)として決定する(ステップS2,作成手段)。これらの初期経由点の決定には、例えば線形補間を用いる。例えば、始点座標が(0,0),終点座標が(30,100)であれば、初期経由点を(7.5,25),(15,50),(22.5,75)とする。   FIG. 9 is a flowchart executed by the control unit 11 corresponding to the processing shown in FIG. First, when the control unit 11 reads the start point and end point of the hand of the XR robot 9 given by an operator (step S1, position specifying step, position specifying means), the control unit 11 sets three points between the start point and end point to an initial value. It is determined as a waypoint (interpolation point) (step S2, creation means). For example, linear interpolation is used to determine these initial via points. For example, if the start point coordinates are (0, 0) and the end point coordinates are (30, 100), the initial via points are (7.5, 25), (15, 50), (22.5, 75). .
次に、制御部11は、始点,終点及び上記3点の初期経由点の間を、例えばスプライン補間等により補間処理して(ステップS3)、補間後の軌道についてボブロー法を用いて移動時間が最短となる速度パターンを作成する(ステップS4)。尚、ボブロー法の詳細については、以下の,文献(2)(3)を参照。
・<文献(2)>The International Journal of Robotics Research,Vol.4, No.3,Fall 1985 p3-17“Time-Optimal Control of Robotic Manipulators Along Specified Paths” by J. E. Bobrow, S. Dubowsky, J. S. Gibson
・<文献(3)>IEEE JOURNAL OF ROBOTICS AND AUTOMATION,VOL.4,NO.4,AUGUST 1988 P443-450 “Optimal Robot Path Planning Using the Minimum-Time Criterion” by JAMES E. BOBROW
Next, the control unit 11 interpolates between the start point, the end point, and the above three initial via points by, for example, spline interpolation (step S3), and uses the Boblow method for the trajectory after the interpolation. A speed pattern that is the shortest is created (step S4). For details of the Boblow method, see the following references (2) and (3).
・ <Reference (2)> The International Journal of Robotics Research, Vol.4, No.3, Fall 1985 p3-17 “Time-Optimal Control of Robotic Manipulators Along Specified Paths” by JE Bobrow, S. Dubowsky, JS Gibson
・ <Reference (3)> IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL.4, NO.4, AUGUST 1988 P443-450 “Optimal Robot Path Planning Using the Minimum-Time Criterion” by JAMES E. BOBROW
次いで、制御部11は、XRロボット9の手先(今の例では第2軸、すなわち第2アームの先端)を、補間した軌道について作成した速度パターンに従い移動させた場合の動作時間を計算し(ステップS5)、その動作時間と前回に計算した動作時間との差を求める。そして、制御部11は、両者の差が所定の閾値(例えば0.001s)未満に収まっているか否かを判断し(ステップS6)、収まっていなければ(NO)ステップS2で求めた経由点を、例えば滑降シンプレックス法等を用いて更新する(ステップS7)。勿論ここでは他の手法,例えば勾配法や遺伝アルゴリズム等を用いても良い。   Next, the control unit 11 calculates an operation time when the hand of the XR robot 9 (in this example, the second axis, that is, the tip of the second arm) is moved according to the speed pattern created for the interpolated trajectory ( Step S5), the difference between the operating time and the previously calculated operating time is obtained. Then, the control unit 11 determines whether or not the difference between the two is less than a predetermined threshold (for example, 0.001 s) (step S6), and if not (NO), the route point obtained in step S2 is determined. For example, it is updated using the downhill simplex method or the like (step S7). Of course, other methods such as a gradient method or a genetic algorithm may be used here.
経由点を更新すると、制御部11の処理はステップS3に戻り、ステップS7までの処理を繰り返す。そして、ステップS6において、今回と前回の動作時間の差が所定の閾値未満に収まれば(YES)処理を終了する。尚、ステップS3〜S7が軌道生成ステップ(生成手段)に対応する。   When the waypoint is updated, the process of the control unit 11 returns to step S3 and repeats the process up to step S7. In step S6, if the difference between the current operation time and the previous operation time is less than a predetermined threshold (YES), the process is terminated. Steps S3 to S7 correspond to a trajectory generation step (generation means).
次に、ステップS5で行うボブロー法を用いた計算処理について、図10を参照して説明する。先ず、ステップS3で決定した手先の軌道を示す(8)式
x=f(λ) …(8)
より、軌道パラメータλを手先位置x(ベクトル)に変換する(ステップS11)。軌道f(λ)は、例えば3次スプラインで補間した場合は次式となる。
f(λ)=aλ+bλ+cλ+d …(9)
但しλは、始点から終点までの座標の増分変数である。
続いて、逆運動学より、手先位置xを各軸(アーム)の角度qに変換する(ステップS12,変換手段)。
q=p−1(f(λ)) …(10)
但しpは、順運動学を示す関数である。ここで手先位置xを角度qに変換するのは、手先の軌道を求める際に、図11に示すように、XRロボット9のアームの形態である手系(図11(a)は左手系,図11(b)は右手系)を限定することなく求められるからである。
Next, calculation processing using the Boblow method performed in step S5 will be described with reference to FIG. First, equation (8) indicating the trajectory of the hand determined in step S3 x = f (λ) (8)
Thus, the trajectory parameter λ is converted into the hand position x (vector) (step S11). For example, when the trajectory f (λ) is interpolated by a cubic spline, the following equation is obtained.
f (λ) = aλ 3 + bλ 2 + cλ + d (9)
However, λ is an incremental variable of coordinates from the start point to the end point.
Subsequently, the hand position x is converted into an angle q of each axis (arm) by inverse kinematics (step S12, conversion means).
q = p −1 (f (λ)) (10)
Where p is a function indicating forward kinematics. Here, the hand position x is converted into the angle q when the hand trajectory is obtained, as shown in FIG. 11, the hand system in the form of the arm of the XR robot 9 (FIG. 11 (a) is the left hand system, This is because FIG. 11B can be obtained without limiting the right hand system).
すなわち、図12において説明するように(但しロボットのイメージは、水平4軸)、座標位置を用いて図10に示す計算を行うとした場合、軌道の途中でアームの形態を変化させることを想定すると(図12(a),(b)参照)、形態が変化したところで拘束条件が不連続となり、計算結果が得られなくなってしまう。そのため、座標位置を用いて計算を行う場合には、アームの形態を変化させないで行う必要がある(因みに文献(2),(3)においても、ロボットのアーム形態を、最初に決定された形態から変更することなしに最速となる速度パターンを求めている)。   That is, as described in FIG. 12 (however, the image of the robot is four horizontal axes), assuming that the calculation shown in FIG. 10 is performed using the coordinate position, it is assumed that the form of the arm is changed in the middle of the trajectory. Then (see FIGS. 12A and 12B), the constraint condition becomes discontinuous when the form changes, and the calculation result cannot be obtained. Therefore, when the calculation is performed using the coordinate position, it is necessary to perform the calculation without changing the form of the arm (in the reference (2) and (3), the form of the robot that is determined first is also used). To find the fastest speed pattern without changing from).
これに対して、第2実施形態のように、予め手先の座標位置を各軸角度に変換してから計算を行うようにすれば、軌道の途中で形態が変化しても拘束条件が不連続とならず、計算結果を得ることができる。したがって、アーム形態に制約されることなく最速となる速度パターンが求められる。   On the other hand, if the calculation is performed after the coordinate position of the hand is converted into each axis angle in advance as in the second embodiment, the constraint condition is discontinuous even if the form changes in the middle of the trajectory. The calculation result can be obtained. Therefore, a speed pattern that is the fastest without being restricted by the arm configuration is required.
そして、(6)式に(10)式を代入すると、(11)式が得られる(ステップS13)。
尚、Jはヤコビアンである。また、(11)式において摩擦B,重力gを夫々含むパラメータa3,a4の項は「ゼロ」となる。
Then, by substituting equation (10) into equation (6), equation (11) is obtained (step S13).
J is Jacobian. Further, the terms of the parameters a3 and a4 including the friction B and the gravity g in the equation (11) are “zero”.
次に、ステップS14において、制御部11は、(11)式より求められる最大加速度λ´max,最大減速度λ″minを計算する(これらについては、文献(1)の(8)〜(10)式を参照)。尚、「λ」に付した「″」の記号は文献(1)において加速度を示すためのダブルドットに替わるものである。ここで求められる最大加速度λ″max,最大減速度λ″minが、XRロボット9の動作の加速度に関する拘束条件となる。   Next, in step S14, the control unit 11 calculates the maximum acceleration λ′max and the maximum deceleration λ ″ min obtained from the equation (11) (these are (8) to (10) in the document (1). Note that the symbol “″” attached to “λ” replaces double dots for indicating acceleration in the document (1). The maximum acceleration λ ″ max and the maximum deceleration λ ″ min obtained here are constraint conditions regarding the acceleration of the operation of the XR robot 9.
次いで、制御部11は、軌道上の次の位置λ及び速度λ´を、最大加速度λ″max(又は最大減速度λ″min)を用いて(12)式,(13)式により計算する(ステップS15)。ここで、時間tはサンプリングタイム(例えば1ms)であり、λ0,λ´0は現在の位置及び速度である。また「λ」に付した「´」は一階微分を表し、文献(1)において速度を示すためのドットに替わるものである。
λ=0.5λ″maxt+λ´t …(12)
λ´=λ″maxt …(13)
Next, the control unit 11 calculates the next position λ and velocity λ ′ on the trajectory using the maximum acceleration λ ″ max (or the maximum deceleration λ ″ min) by the equations (12) and (13) ( Step S15). Here, time t is a sampling time (for example, 1 ms), and λ0 and λ′0 are current positions and velocities. Further, “′” added to “λ” represents a first-order differentiation, and is replaced with a dot for indicating speed in the document (1).
λ = 0.5λ ″ maxt 2 + λ′t (12)
λ ′ = λ ″ maxt (13)
次に、制御部11は、軌道パラメータλの速度λ´と加速度(減速度)λ″がそれぞれの拘束条件を満足するか否かを判定する(ステップS16)。つまり、制御部11は、
λ´min≦λ´≦λ´max …(14)
λ″min(λ,λ´)≦λ″≦λ″max(λ,λ´) …(15)
の両条件を同時に満足するか否かを判定する。なお、加速時か減速時かに応じて、λ´min又はλ´max、及び、λ″min又はλ″maxが使用される。
Next, the control unit 11 determines whether or not the speed λ ′ and the acceleration (deceleration) λ ″ of the trajectory parameter λ satisfy the respective constraint conditions (step S16).
λ′min ≦ λ ′ ≦ λ′max (14)
λ ″ min (λ, λ ′) ≦ λ ″ ≦ λ ″ max (λ, λ ′) (15)
It is determined whether or not both conditions are satisfied simultaneously. Note that λ′min or λ′max and λ ″ min or λ ″ max are used depending on whether the vehicle is accelerating or decelerating.
図13は、上記文献(2)のFig.2を基に模式的に表すもので、2軸ロボットについて、軌道パラメータλの加速度(減速度)λ″に関する拘束条件を満たす範囲を示している。すなわち、各軸(各関節に配置されるモータ)について規定されている最大加速度〜最大減速度の範囲でAND条件を採った許容範囲λ″min〜λ″maxが設定されている。   FIG. 13 shows FIG. 2 schematically shows a range that satisfies the constraint condition regarding the acceleration (deceleration) λ ″ of the trajectory parameter λ for a biaxial robot. That is, each axis (motors arranged at each joint) The allowable range λ ″ min to λ ″ max taking the AND condition is set in the range of the maximum acceleration to the maximum deceleration defined in (1).
また、軌道パラメータλの速度λ´に関する拘束条件の限界値λ´min、λ´maxは、
λ´min=max(q´min/f´(λ)) …(16)
λ´max=min(q´max/f´(λ)) …(17)
である。ここで、f(λ)は軌道を示し、q´min、q´maxはXRロボット9の仕様より決まる最小及び最大の関節速度である。これらの速度条件及び加速度(トルク)条件は、拘束条件として必要な最小限の条件であり、状況に応じてこれ以外の条件を追加してもよい。
Further, the limit values λ′min and λ′max of the constraint conditions regarding the speed λ ′ of the orbital parameter λ are
λ′min = max (q′min / f ′ (λ)) (16)
λ′max = min (q′max / f ′ (λ)) (17)
It is. Here, f (λ) indicates the trajectory, and q′min and q′max are the minimum and maximum joint speeds determined by the specifications of the XR robot 9. These speed conditions and acceleration (torque) conditions are the minimum conditions necessary as constraint conditions, and other conditions may be added depending on the situation.
上述のステップS16において、軌道パラメータλの速度λ´と加速度(減速度)λ″が式(14),(15)の拘束条件を共に満足していれば(ステップS16、YES)、制御部11は、位置λをステップS15で計算した値に更新する(ステップS17)。次いで、制御部11は、位置λが終点λendに到達したか否かを判断し(ステップS18)、到達していなければ(NO)、処理をステップS14に戻し、到達していれば(YES)処理を終了する。   If the speed λ ′ and the acceleration (deceleration) λ ″ of the trajectory parameter λ satisfy both of the constraint conditions of the equations (14) and (15) (step S16, YES) in the above step S16, the control unit 11 Updates the position λ to the value calculated in step S15 (step S17) Next, the control unit 11 determines whether or not the position λ has reached the end point λend (step S18). (NO), the process returns to step S14, and if it has reached (YES), the process is terminated.
一方、ステップS16において、速度λ´と加速度(減速度)λ″が式(14),(15)の少なくも一方の拘束条件を満足していない場合(ステップS16,NO)、制御部11は、ステップS19の処理を行う。すなわち、制御部11は、加速時であれば最大加速度λ″maxより任意の定数aを減じたものを新たな最大加速度λ″maxとし、減速時であれば最大減速度λ″minより任意の定数aを減じたものを新たな最大減速度λ″minとする。その後、ステップS15に戻り、軌道パラメータである位置λ及び速度λ´を再計算する。   On the other hand, in step S16, when the speed λ ′ and the acceleration (deceleration) λ ″ do not satisfy at least one of the constraint conditions of the equations (14) and (15) (NO in step S16), the control unit 11 In other words, the control unit 11 sets the maximum acceleration λ ″ max obtained by subtracting an arbitrary constant a as the new maximum acceleration λ ″ max during acceleration and the maximum during deceleration. A value obtained by subtracting an arbitrary constant a from the deceleration λ ″ min is set as a new maximum deceleration λ ″ min. Thereafter, the process returns to step S15 to recalculate the position λ and the velocity λ ′ as trajectory parameters.
以上の図10に示す処理を完了すると、図9に示すステップS3で補間されて求められた軌道上を、XRロボット9の手先が速度及び加速度(=トルク)についての拘束条件を満たしつつ移動した場合に、移動速度が最も速くなる速度パターンが得られる。すなわち、ボブロー法を用いた計算処理はXRロボット9の運動力学に基づいて行われているので、XRロボット9についての拘束条件を満たすことができる。   When the process shown in FIG. 10 is completed, the hand of the XR robot 9 moves on the trajectory obtained by interpolation in step S3 shown in FIG. 9 while satisfying the constraint conditions for speed and acceleration (= torque). In this case, a speed pattern with the fastest moving speed is obtained. That is, since the calculation process using the Boblow method is performed based on the kinematics of the XR robot 9, the constraint condition for the XR robot 9 can be satisfied.
なお、特に図示していないが、実際には、ステップS4及びS5に係る動作時間は、始点から終点までの往復の速度パターンに基づいて演算される。つまり、ステップS4にて説明したアルゴリズムに基づいて、最初に始点から終点への行きの速度パターンが演算される。次いで、終点から始点までの帰りの速度パターンが演算される。それから、行き速度パターンと帰り速度パターンとが合成され、その合成された速度パターンについて、ステップS5にて動作時間が演算される。   Although not specifically shown, in practice, the operation time according to steps S4 and S5 is calculated based on a reciprocating speed pattern from the start point to the end point. That is, the speed pattern from the start point to the end point is first calculated based on the algorithm described in step S4. Next, a return speed pattern from the end point to the start point is calculated. Then, the going speed pattern and the return speed pattern are synthesized, and the operation time is calculated for the synthesized speed pattern in step S5.
次に、図14及び図15は、以上のようにしてXRロボットの手先の移動速度が最短となる軌道を求めた一例を示す。但しここでは、ボールねじ軸の危険速度は考慮せず、上限速度一定で計算している。図14(a)は、XY平面上における第1軸,第2軸及び手先の座標が変化する軌跡を実線で示すもので、始点座標は(−0.3,0.3)であり、終点座標は(0.3,0.3)である。図15(b)には、始点から終点まで移動する間の、第1軸,第2軸それぞれの角度,角速度,角加速度,トルクの変化を示している。   Next, FIGS. 14 and 15 show an example in which the trajectory in which the moving speed of the hand of the XR robot is the shortest is obtained as described above. However, here, the critical speed of the ball screw shaft is not taken into consideration, and the calculation is performed at a constant upper limit speed. FIG. 14 (a) shows a locus in which the coordinates of the first axis, the second axis, and the hand on the XY plane change with solid lines, the start point coordinates are (−0.3, 0.3), and the end point. The coordinates are (0.3, 0.3). FIG. 15B shows changes in the angle, angular velocity, angular acceleration, and torque of each of the first axis and the second axis during the movement from the start point to the end point.
第1軸は、X軸上を−0.3[m]から(図14(a)参照)0.3[m]へ(図14(e)参照)直線的に移動する。すなわち、移動ステージが直線移動レールに沿って移動することに対応する。また、第2軸の回転角θ2(水平旋回アームの回転角)は、始点及び終点,並びにX軸上の中間点で夫々0[rad]であるが、図14(b)では、終点方向とは逆方向に0.6[rad]だけ回転した状態になる。この場合、手先位置のX軸座標は、移動ステージの座標よりも負側にあり、XRロボットのアーム形態としては右手系となる。図14(d)では逆に、−0.6[rad](始点方向)に回転した状態になる。この場合、手先位置のX軸座標は、移動ステージの座標よりも正側にあり、XRロボットのアーム形態としては左手系となる。   The first axis moves linearly on the X axis from −0.3 [m] (see FIG. 14A) to 0.3 [m] (see FIG. 14E). That is, it corresponds to the movement stage moving along the linear movement rail. Further, the rotation angle θ2 of the second axis (rotation angle of the horizontal swing arm) is 0 [rad] at the start point and the end point, and the intermediate point on the X axis, respectively, but in FIG. Is rotated by 0.6 [rad] in the opposite direction. In this case, the X-axis coordinate of the hand position is on the negative side with respect to the coordinate of the moving stage, and the XR robot arm form is a right-handed system. In contrast, in FIG. 14D, the state is rotated to −0.6 [rad] (starting point direction). In this case, the X-axis coordinate of the hand position is on the positive side with respect to the coordinate of the moving stage, and the arm form of the XR robot is a left-handed system.
上記の動きに応じて、第2軸の角速度は始点(V1)から移動した直後に正側のピーク(10[rad/s],ピーク点V2)に達し、そこから中間点における負側のピーク(−10[rad/s],ピーク点V3)に向けて直線的に変化する。また、その中間点から終点の直前における正側のピーク(10[rad/s],ピークV4)に向けて直線的に変化する。すなわち、図15(b)に示すように、中間点を中心として対称な速度パターンとなる。   In accordance with the above movement, the angular velocity of the second axis reaches the positive peak (10 [rad / s], peak point V2) immediately after moving from the starting point (V1), and from there, the negative peak at the intermediate point It changes linearly toward (−10 [rad / s], peak point V3). Moreover, it changes linearly from the intermediate point toward the positive peak (10 [rad / s], peak V4) immediately before the end point. That is, as shown in FIG. 15B, the speed pattern is symmetric with respect to the intermediate point.
また、上記の角速度パターンに対応する角加速度は、始点V1において0から正側のピーク400[rad/s]に達し、その後、ピーク点V2で負の値約−50[rad/s]に切り替わって、中間点のピーク点V3までその負の値約−50[rad/s]をキープする。さらに、中間点のピーク点V3で正の値50[rad/s]に切り替わると、ピーク点V4までその値をキープする。ピーク点V4で負側のピークである−400[rad/s]に達し終点V5で0に戻る、というパターンとなる。 The angular acceleration corresponding to the angular velocity pattern reaches a peak 400 [rad / s 2 ] from 0 at the start point V1, and then has a negative value of about −50 [rad / s 2 ] at the peak point V2. And the negative value of about −50 [rad / s 2 ] is kept until the peak point V3 at the midpoint. Furthermore, when the peak value V3 of the intermediate point is switched to a positive value 50 [rad / s 2 ], the value is kept until the peak point V4. The pattern reaches a negative peak of −400 [rad / s 2 ] at the peak point V4 and returns to 0 at the end point V5.
第2軸が上述のように動作することで、以下のような作用が発生する。第2軸は、始点より移動を開始してからX軸上の中間点に達するまでの間に、0[rad]→0.6[rad]→0[rad]と角度θ2を変化させる。これにより、XRロボットの手先が一旦、正回転方向に振られた状態から負回転方向に振り戻されるように動く。これに応答して、その動きに伴う反作用が移動ステージに生じて、移動を開始した際の第1アームの運動を加速する加速力が発生し、この付勢力が第1、第2のアームの終点方向の運動をアシスト(付勢)する。   The following actions occur when the second axis operates as described above. The second axis changes the angle θ2 from 0 [rad] → 0.6 [rad] → 0 [rad] between the start of movement from the start point to the arrival of the intermediate point on the X axis. As a result, the hand of the XR robot moves so as to be swung back in the negative rotation direction from the state once swung in the positive rotation direction. In response to this, a reaction accompanying the movement occurs in the moving stage, and an accelerating force for accelerating the movement of the first arm when the movement is started is generated. This urging force is applied to the first and second arms. Assist (bias) the movement in the end direction.
一方、X軸上の中間点から終点に達するまでの間に、第2軸は0[rad]→−0.6[rad]→0[rad]と角度θ2を変化させるが、これにより、XRロボットの手先が一旦、負回転方向に振られた状態から正回転方向に振り戻されるように動く。この場合も、その動きに伴う反作用が移動ステージ側に生じて減速力として作用する。この減速力は、終点に到達して停止するまでの減速運動をアシスト(抑制)する。   On the other hand, the second axis changes the angle θ2 from 0 [rad] → −0.6 [rad] → 0 [rad] until the end point is reached from the intermediate point on the X axis. The hand of the robot moves so as to be swung back in the positive rotation direction from the state once swung in the negative rotation direction. Also in this case, a reaction accompanying the movement occurs on the moving stage side and acts as a deceleration force. This deceleration force assists (suppresses) the deceleration movement until it reaches the end point and stops.
以上のように第2軸が動作する結果、XRロボットの手先が始点から終点まで移動する時間は、従来のように単純な動作をさせた場合に比較して短縮される。図14には、第2軸を全く回転させずに第1軸のみを動作させた場合の軌道を破線で示すが、この軌道についてシミュレートした移動時間は536[ms]であるのに対し、実線で示す軌道についてシミュレートした移動時間は514[ms]であり、4%強の短縮効果があった。したがって、移動時間を従来と同じ時間にすれば、XRロボットをより少ないエネルギーで動作させることが可能になり、省エネルギー化にも貢献できる。更に、例えば出力を低下させることでモータのワット数を低下させることも可能になり、ロボットの小型化,低コスト化を図ることもできる。   As a result of the movement of the second axis as described above, the time required for the hand of the XR robot to move from the start point to the end point is shortened compared to the case where the simple operation is performed as in the conventional case. In FIG. 14, the trajectory when only the first axis is operated without rotating the second axis is indicated by a broken line. The travel time simulated for this trajectory is 536 [ms], whereas The travel time simulated for the trajectory indicated by the solid line was 514 [ms], and there was a shortening effect of over 4%. Therefore, if the movement time is the same as the conventional time, the XR robot can be operated with less energy, which can contribute to energy saving. Furthermore, for example, it is possible to reduce the wattage of the motor by reducing the output, and the robot can be reduced in size and cost.
また、図15(a)は、図14(a)〜(e)に示す5つの手先位置を同時にプロットしたものである。また、これらの5点は、図15(b)に示す第2軸の角速度パターン上にプロットした5点(V1〜V5),すなわち角速度又は角加速度の変極点に対応している。図9に示すステップS2において、始点と終点との間に3点の初期経由点を設定したが、初期経由点を3点にすることは、図15に示す角速度又は角加速度の変極点に対応するものを選択するのが、計算効率を向上させるために有利であるとの判断に基づいている。   FIG. 15A is a plot of the five hand positions shown in FIGS. 14A to 14E simultaneously. These five points correspond to five points (V1 to V5) plotted on the angular velocity pattern of the second axis shown in FIG. 15B, that is, inflection points of angular velocity or angular acceleration. In step S2 shown in FIG. 9, three initial waypoints are set between the start point and the end point. However, setting the number of initial waypoints to 3 corresponds to the inflection point of the angular velocity or angular acceleration shown in FIG. The selection of what to do is based on the judgment that it is advantageous to improve the calculation efficiency.
例えば、文献(3)のFig.4に示されていると共に関連する記載からは、ボブロー法では、軌道を探索するための経由点数を増やすほど動作時間を短縮する効果がある。しかしながら、経由点数を増やせば、それは直接的に計算時間の増大させることに繋がるため、無制限に増やすのは実用的ではない。
そこで、第2実施形態では、動作時間の短縮を図ることと計算時間の増大を抑制することとのバランスを考慮して、初期経由点を3点にしている。初期経由点の選択は厳密に行う必要はなく、前述したように始点と終点との間の直線距離を単純に4等分するなどして決定すれば良い。図6又は図9に示す処理を繰り返し実行する過程で、経由点が変更されつつ新たな軌道が求められ、最適化される。
For example, FIG. 4 and the related description, the Boblow method has an effect of shortening the operation time as the number of via points for searching for the trajectory is increased. However, increasing the number of waypoints directly increases the calculation time, so it is not practical to increase it indefinitely.
Therefore, in the second embodiment, the number of initial via points is set to three in consideration of the balance between shortening the operation time and suppressing the increase in calculation time. The selection of the initial via point does not need to be performed strictly, and may be determined by simply dividing the linear distance between the start point and the end point into four equal parts as described above. In the process of repeatedly executing the processing shown in FIG. 6 or FIG. 9, a new trajectory is obtained and optimized while the waypoint is changed.
その結果、ピック・アンド・プレイスのような動作パターンにおいて動作時間が最短になる軌道を選択することは、3点の経由点が、図15(a)に示すような第2軸の角速度又は角加速度の3つの変極点として、どの座標位置となるかにより決まると言える。したがって、初期経由点を3点とすれば、動作時間が最短となる軌道を、計算時間の増大を抑制しつつ求めることが可能となるのである。   As a result, selecting a trajectory having the shortest operation time in an operation pattern such as pick-and-place means that three via points are angular velocities or angles of the second axis as shown in FIG. It can be said that the three inflection points of acceleration are determined by the coordinate position. Therefore, if the number of initial waypoints is 3, it is possible to obtain the trajectory with the shortest operation time while suppressing an increase in calculation time.
図16は、第1実施形態で示した図1に、第2実施形態の制御を加えた実行結果を併せて示している。二点鎖線は、第1実施形態の制御を行わず、第2実施形態のようにボブロー法を用いて計算を行い、移動の際に回転軸J2を回転させることでアーム7が生じる反力によるアシストを加えたもので、移動時間は約894msに短縮されている。また、実線は、第1実施形態の制御と第2実施形態の制御とを同時に行ったもので、移動時間は約574msに短縮されている。これは、一点鎖線で示す結果からの比較では、移動時間を約37%短縮したことになる。   FIG. 16 also shows an execution result obtained by adding the control of the second embodiment to FIG. 1 shown in the first embodiment. The two-dot chain line is calculated by using the Boblow method as in the second embodiment without performing the control of the first embodiment, and is caused by the reaction force generated by the arm 7 by rotating the rotation axis J2 during the movement. With the addition of assistance, the travel time is reduced to about 894 ms. A solid line is obtained by simultaneously performing the control of the first embodiment and the control of the second embodiment, and the moving time is shortened to about 574 ms. In comparison with the result shown by the alternate long and short dash line, this means that the movement time is shortened by about 37%.
また、第1実施形態で述べたように(1)式に基づいて上限速度を可変させる制御と、ボブロー法とを組み合わせることで、以下のような作用効果も得られる。例えば図17(a)に示すように、XRロボット9について、第1軸を0[mm]から1000[mm]に移動させると共に、第2軸を120°から90°まで−30°移動させることを想定する。このような多軸ロボットを協調動作させる場合、位置決めは全ての軸を同時発着させることが前提となる。図17(b)に示すように、上限速度が位置に応じて変化する第1軸の速度パターン(1点鎖線)を、実線で示すように、図17(c)に示す第2軸に合わせて修正変更することは、一般に困難である。
しかし、第2実施形態のようにボブロー法を用いれば、軌道空間において速度パターンを決定することになり、その結果として、各軸の発着は自動的に同期するようになる。したがって、ユーザは、各軸の動作を同期させることを特段意識する必要がなくなる。
In addition, as described in the first embodiment, by combining the control for varying the upper limit speed based on the expression (1) and the Boblow method, the following effects can be obtained. For example, as shown in FIG. 17A, for the XR robot 9, the first axis is moved from 0 [mm] to 1000 [mm], and the second axis is moved from 120 ° to 90 ° by −30 °. Is assumed. When such a multi-axis robot is operated in a coordinated manner, positioning is premised on simultaneous arrival and departure of all axes. As shown in FIG. 17 (b), the speed pattern (one-dot chain line) of the first axis in which the upper limit speed changes according to the position is aligned with the second axis shown in FIG. In general, it is difficult to correct and change.
However, if the Boblow method is used as in the second embodiment, the velocity pattern is determined in the orbital space, and as a result, the arrival and departure of the respective axes are automatically synchronized. Therefore, the user does not need to be particularly aware of synchronizing the operations of the axes.
以上のように第2実施形態によれば、制御部11は、手先を始点から移動させる際には、アーム7の動作によって移動ステージ5側に生じる反作用が、移動ステージ5の基軸であるボールねじ軸1に沿って動作させて発生する加速力を増加させるように軌道を生成し、手先を終点に停止させる際には、アーム7の動作によって移動ステージ5に生じる反作用が、移動ステージ5を動作させて発生する減速力を増加させるように軌道を生成する。   As described above, according to the second embodiment, when the control unit 11 moves the hand from the starting point, the reaction generated on the moving stage 5 side by the operation of the arm 7 is the ball screw that is the base axis of the moving stage 5. When the trajectory is generated so as to increase the acceleration force generated by operating along the axis 1 and the hand is stopped at the end point, the reaction that occurs on the moving stage 5 due to the movement of the arm 7 causes the moving stage 5 to move. The trajectory is generated so as to increase the deceleration force generated.
より具体的には、始点から終点に向けて手先の移動を開始させるときに、移動ステージ5に、アーム7の動作によって、ボールねじ軸1及び移動ステージ5が発生する加速力を増加させる反作用を生じさせ、手先を終点に停止させるときには、アーム7の動作によって、移動ステージ5が発生する減速力を増加させる反作用を生じさせるように、移動ステージ5及びジョイント6の速度パターンを生成することで手先の軌道を生成する。   More specifically, when starting the movement of the hand from the start point to the end point, the reaction that causes the moving stage 5 to increase the acceleration force generated by the ball screw shaft 1 and the moving stage 5 by the operation of the arm 7 is caused. When the hand is stopped at the end point, the speed of the moving stage 5 and the joint 6 is generated so that the reaction of increasing the deceleration force generated by the moving stage 5 is generated by the operation of the arm 7. Generate a trajectory for
従来の多軸型ロボットの制御では、例えばピック・アンド・プレイスのような単純な作業を行わせる場合、始点、終点間で動作量に変化がない軸は動かない。すなわち、図14に破線で示した動作パターンとなる。このようにアームが伸びきった状態では、第1軸(走行軸)の高加速による慣性トルクで手先側アームが動かないように第2軸は無駄な保持トルクを出力し続けており、駆動手段8の負荷となっているだけである。   In conventional multi-axis robot control, for example, when a simple work such as pick-and-place is performed, an axis whose movement amount does not change between the start point and the end point does not move. That is, the operation pattern is indicated by a broken line in FIG. In such a state where the arm is fully extended, the second shaft continues to output useless holding torque so that the hand side arm does not move due to the inertia torque due to the high acceleration of the first shaft (traveling shaft). The load is only 8.
これに対して、上述したように軌道を生成すると、アーム7が、移動ステージ5に対し反作用を生じるように動作するようになり、手先の起動時に必要な加速力と、停止時に必要な減速力とをそれぞれ増加させ、手先を始点から終点まで移動させる速度を向上させて移動時間を短縮できる。すなわち、手先を移動させる途中でアーム7を若干回転させることで、移動ステージ5が直線移動する際のイナーシャが小さくなり、手先をより少ないエネルギーで移動させることができる。よって、従来の手法で決定された軌道と同じ移動時間で動作させるのであれば、XRロボット9を従来よりも少ない消費エネルギーで動作させることができる。   On the other hand, when the trajectory is generated as described above, the arm 7 moves so as to react against the moving stage 5, and the acceleration force required when starting the hand and the deceleration force required when stopping the hand. And the speed of moving the hand from the start point to the end point can be improved to shorten the movement time. That is, by slightly rotating the arm 7 during the movement of the hand, the inertia when the moving stage 5 moves linearly is reduced, and the hand can be moved with less energy. Therefore, if the XR robot 9 is operated with the same movement time as the trajectory determined by the conventional method, the XR robot 9 can be operated with less energy consumption than in the past.
また、上述した軌道は、XRロボット9の動力学に基づいて計算された結果として生成されるので、ロボットの拘束条件を満たす範囲内で速度を最大化する軌道が求められる。したがって、現実的に使用できる最適化された軌道が得られる。加えて、ボブロー法を用いて速度パターンを演算する際に、軌道上の位置を角度に変換して演算を行うようにしたので、XRロボット9のアーム形態を左手系,右手系の何れか一方に限定することなく、左右両方の手系を用いて最速となる速度パターンを求めることができる。   Further, since the above-described trajectory is generated as a result calculated based on the dynamics of the XR robot 9, a trajectory that maximizes the speed within a range that satisfies the constraint condition of the robot is obtained. Thus, an optimized trajectory that can be used realistically is obtained. In addition, when calculating the speed pattern using the Boblow method, the position on the trajectory is converted into an angle, so the calculation is performed so that the arm form of the XR robot 9 is either left-handed or right-handed. Without being limited to the above, it is possible to obtain the speed pattern that is the fastest using both the left and right hand systems.
また、手先を始点から移動させる際には、移動ステージ5を終点方向に移動させると共に、アーム7を、その先端が終点方向とは逆方向に移動するように回転させる。その後、アーム7の先端を終点方向に回転させる。この時、アーム7の動作によって生じる反作用が起動時の加速力をアシストするようになる。   Further, when the hand is moved from the starting point, the moving stage 5 is moved in the end point direction, and the arm 7 is rotated so that the tip thereof moves in the direction opposite to the end point direction. Thereafter, the tip of the arm 7 is rotated in the end point direction. At this time, the reaction caused by the operation of the arm 7 assists the acceleration force at the time of activation.
また、手先を終点に停止させる際には、アーム7を、その先端である手先が終点を超えている位置から、先端を始点方向に移動させるように軌道を生成する。すなわち、アーム7の先端,手先を始点方向に振り返すように軌道を生成して停止させる。この時、アーム7の動作によって生じる反作用が停止時の減速力をアシストするようになる。すなわち、この様に軌道を生成することで、アーム7が、移動ステージ5に対し反作用を生じるように動作させることができる。   Further, when the hand is stopped at the end point, a trajectory is generated so that the arm 7 is moved in the direction of the start point from a position where the hand, which is the tip, exceeds the end point. That is, the trajectory is generated and stopped so that the tip and the tip of the arm 7 are turned back toward the starting point. At this time, the reaction caused by the operation of the arm 7 assists the deceleration force at the stop. That is, by generating the trajectory in this way, the arm 7 can be operated so as to react against the moving stage 5.
そして、始点と終点との間に3点の経由点を設定し、3点の位置を変更しながら各点に対応した移動時間を演算するようにした。すなわち、手先の移動時間を最短にする軌道を生成するには、軌道を最適化する必要がある。そして、最適化を行うには、始点と終点との間に1点以上の経由点を設定し、その経由点の位置を変更した場合に求められる移動速度を比較する必要がある。   Then, three waypoints are set between the start point and the end point, and the movement time corresponding to each point is calculated while changing the position of the three points. That is, in order to generate a trajectory that minimizes the movement time of the hand, it is necessary to optimize the trajectory. In order to perform optimization, it is necessary to set one or more waypoints between the start point and the end point, and to compare the movement speeds required when the position of the waypoint is changed.
ここで、上述したようにアーム7を動作させることを想定すると、アーム7の基部側軸であるボールねじ軸1及び移動ステージ5において、起動直後に加速度を付与する時点と、動作途中で加速度の方向が反転する時点と、停止直前に加速度をゼロにする時点とがある。そして、軌道の最適化を図る過程では、少なくとも上記3つの時点に対応する経由点を最適化することが重要になると考えられる(図6(a)参照)。したがって、上記3つの時点に対応する経由点を設定すれば、演算量を抑制しつつ効率的に、移動時間を最短にする軌道を生成することが可能となる。これにより、従来の方法では、方法自体の限界によって引き出すことができなかったモータ,アクチュエータのポテンシャルを最大限引き出しつつ、現実的に使用できる最適化された軌道が得られるようになる。   Assuming that the arm 7 is operated as described above, in the ball screw shaft 1 and the moving stage 5 that are the base side axes of the arm 7, the acceleration is applied immediately after activation, and the acceleration is increased during the operation. There is a time point when the direction is reversed and a time point when the acceleration is made zero immediately before stopping. In the process of optimizing the trajectory, it is considered important to optimize at least the waypoints corresponding to the above three time points (see FIG. 6A). Therefore, by setting the waypoints corresponding to the above three time points, it is possible to efficiently generate a trajectory that minimizes the travel time while suppressing the amount of calculation. Thereby, in the conventional method, an optimized trajectory that can be practically used can be obtained while maximizing the potential of the motor and actuator that could not be extracted due to the limitations of the method itself.
(第3実施形態)
図18及び図19は第3実施形態であり、図9及び図10相当図である。第3実施形態では、手先の位置xを各軸の角度qへ変換するステップの実行タイミングが変更されている。つまり、第2実施形態では、逆運動学を解いて手先の位置xを各軸の角度qへ変換していた(ステップS12参照)。これに対し、第3実施形態では、最短の速度パターンの作成前の段階、すなわち経由点の補間処理の段階で、その角度qへの変換処理を行うようにした。これにより逆運動学の演算を回避することができ、演算負荷が格段に減少する。
(Third embodiment)
18 and 19 show the third embodiment, which corresponds to FIGS. 9 and 10. In the third embodiment, the execution timing of the step of converting the hand position x into the angle q of each axis is changed. That is, in the second embodiment, the inverse kinematics is solved to convert the hand position x into the angle q of each axis (see step S12). On the other hand, in the third embodiment, the conversion process to the angle q is performed at the stage before the creation of the shortest speed pattern, that is, at the stage of the interpolation process of the via point. As a result, computation of inverse kinematics can be avoided, and computation load is greatly reduced.
図18において、制御部11は、例えばオペレータによって与えられた開始点及び終点を示す情報を読み出し(ステップS1)、3つの初期経由点を生成する(ステップS2)。次いで、制御部11は、手先空間におけるそれらの開始点、終点及び初期経由点を、関節空間における相当する角度に変換する(ステップS2A)。この変換の後、制御部11はステップS3に移行し、それらの開始点、終点及び3つの初期経由点を補間して軌道x=f(λ)を生成する。次いで、ステップS4Aでは、図19に示すように、ステップS13A、S14〜S19の処理が行われる。このうち、ステップS13Aでは、角度qをロボットの運動方程式に代入する。   In FIG. 18, the control unit 11 reads out information indicating a start point and an end point given by, for example, an operator (step S1), and generates three initial via points (step S2). Next, the control unit 11 converts the start point, end point, and initial via point in the hand space into corresponding angles in the joint space (step S2A). After this conversion, the control unit 11 proceeds to step S3, and generates a trajectory x = f (λ) by interpolating the start point, the end point, and the three initial via points. Next, in step S4A, as shown in FIG. 19, the processes of steps S13A and S14 to S19 are performed. Among these, in step S13A, the angle q is substituted into the motion equation of the robot.
このとき、補間された角度qは、
q=f(λ),q´=f´λ´,q″=f´λ″+f″λ´ …(18)
で表されるので、ロボットの運動方程式は関節空間において
と表される。上記以外の処理は第2実施形態と同様である。
At this time, the interpolated angle q is
q = f (λ), q ′ = f′λ ′, q ″ = f′λ ″ + f ″ λ ′ 2 (18)
Therefore, the motion equation of the robot is
It is expressed. Processing other than the above is the same as in the second embodiment.
したがって、第3実施形態によれば、軌道x=f(λ)を補間する前に、軌道xを関節角度qに変換する。これにより、前述した逆運動学の方程式を解く処理、即ち、前述した図10のステップS11,S12の処理が不要になる。つまり、関節角度空間においてそのままダイナミクス(順運動学)を解けばよく、その分だけ演算負荷が軽くなる。   Therefore, according to the third embodiment, the trajectory x is converted into the joint angle q before the trajectory x = f (λ) is interpolated. This eliminates the processing of solving the inverse kinematic equation described above, that is, the processing of steps S11 and S12 of FIG. That is, it is sufficient to solve the dynamics (forward kinematics) as it is in the joint angle space, and the calculation load is reduced accordingly.
本発明は上記した、または図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
初期経由点を4点以上設定しても良い。
多軸型ロボットの動力学に基づいて速度を最大化する軌跡を求める計算手法は、ボブロー法以外の手法を用いても良い。
The present invention is not limited to the embodiments described above or shown in the drawings, and the following modifications or expansions are possible.
Four or more initial waypoints may be set.
As a calculation method for obtaining a trajectory for maximizing the speed based on the dynamics of the multi-axis robot, a method other than the Boblow method may be used.
1はボールねじ軸、5は移動ステージ(移動体)、6はジョイント(第2駆動手段)、7はアーム、8は駆動手段(第1駆動手段)、9XRロボット(2軸ロボット)、11は制御部(制御装置,位置指定手段,作成手段,変換手段,生成手段,制御手段)を示す。   1 is a ball screw shaft, 5 is a moving stage (moving body), 6 is a joint (second driving means), 7 is an arm, 8 is driving means (first driving means), 9XR robot (biaxial robot), 11 is A control unit (control device, position specifying means, creation means, conversion means, generation means, control means) is shown.

Claims (5)

  1. 両端が固定されたボールねじ軸からなる直動軸と、
    前記ボールねじ軸を回転駆動する第1駆動手段と、
    前記ボールねじ軸が回転することで、前記直動軸に沿って移動可能に配置される移動体と、
    この移動体に搭載されるもので、第2駆動手段,及び前記第2駆動手段により回転駆動され、回転軸を基点として回動可能に配置される回動アームとを備えて構成される2軸ロボットを動作させる際に、
    前記移動体の上限速度を、少なくとも一部の範囲について、前記直動軸上における前記移動体の位置に応じて可変設定することを特徴とする2軸ロボットの速度制御方法。
    A linear motion shaft consisting of a ball screw shaft with both ends fixed;
    First driving means for rotationally driving the ball screw shaft;
    A moving body arranged to be movable along the linear motion axis by rotating the ball screw shaft;
    Two shafts that are mounted on the movable body and are configured to include second driving means, and a rotary arm that is rotationally driven by the second driving means and is arranged to be rotatable about the rotational axis. When operating the robot,
    A speed control method for a two-axis robot, wherein an upper limit speed of the moving body is variably set in accordance with a position of the moving body on the linear motion axis in at least a part of a range.
  2. 前記移動体の位置に応じて求められる前記ボールねじ軸の危険回転速度NdとDN値とを比較して、より小さい値を示す方を前記上限速度とすることを特徴とする請求項1記載の2軸ロボットの速度制御方法。   2. The critical rotational speed Nd and DN value of the ball screw shaft determined according to the position of the moving body are compared, and the one showing a smaller value is set as the upper limit speed. Speed control method for a 2-axis robot.
  3. 前記回動アームの先端に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する際に、
    前記始点及び前記終点の位置を指定し、
    前記始点から前記終点に向けて前記手先の移動を開始させるときに、前記回動アームの動作によって、前記第1駆動手段の作用により前記移動体が発生する加速力を増加させる反作用を生じさせると共に、
    前記手先を前記終点に停止させるときには、前記回動アームの動作によって、前記第1駆動手段の作用により前記移動体が発生する減速力を増加させる反作用を生じさせるように、前記移動体及び前記回動アームの速度パターンを生成することで前記軌道を生成することを特徴とする請求項1又は2記載の2軸ロボットの速度制御方法。
    When generating the trajectory of the hand for moving the hand located at the tip of the rotating arm from the start point to the end point,
    Specify the position of the start point and the end point,
    When the movement of the hand is started from the start point toward the end point, the operation of the rotating arm causes a reaction that increases the acceleration force generated by the moving body by the action of the first driving means. ,
    When the hand is stopped at the end point, the moving body and the rotating body are operated so as to cause a reaction to increase the deceleration force generated by the moving body by the action of the first driving means by the operation of the rotating arm. 3. The speed control method for a two-axis robot according to claim 1, wherein the trajectory is generated by generating a speed pattern of a moving arm.
  4. 前記始点と前記終点との間に複数の初期経由点を自動的に設定し、
    前記初期経由点の位置を更新しながら最短時間となる前記速度パターンを生成することを特徴とする請求項3記載の2軸ロボットの速度制御方法。
    A plurality of initial via points are automatically set between the start point and the end point,
    4. The speed control method for a two-axis robot according to claim 3, wherein the speed pattern having the shortest time is generated while updating the position of the initial waypoint.
  5. 前記初期経由点を、3点設定することを特徴とする請求項4記載の2軸ロボットの速度制御方法。   5. The speed control method for a biaxial robot according to claim 4, wherein three initial waypoints are set.
JP2013187341A 2013-09-10 2013-09-10 Speed control method for 2-axis robot Active JP6123595B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2013187341A JP6123595B2 (en) 2013-09-10 2013-09-10 Speed control method for 2-axis robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2013187341A JP6123595B2 (en) 2013-09-10 2013-09-10 Speed control method for 2-axis robot

Publications (2)

Publication Number Publication Date
JP2015054360A true JP2015054360A (en) 2015-03-23
JP6123595B2 JP6123595B2 (en) 2017-05-10

Family

ID=52819122

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2013187341A Active JP6123595B2 (en) 2013-09-10 2013-09-10 Speed control method for 2-axis robot

Country Status (1)

Country Link
JP (1) JP6123595B2 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018150958A (en) * 2017-03-10 2018-09-27 株式会社Fuji Ball screw device and control method therefor

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1190758A (en) * 1997-09-22 1999-04-06 Nisshinbo Ind Inc Control method for feed screw mechanism
JP2004362394A (en) * 2003-06-06 2004-12-24 Okuma Corp Numerically controlled device having maximum feeding speed determination function
JP2009119566A (en) * 2007-11-15 2009-06-04 Denso Wave Inc Suspended robot
JP2011208655A (en) * 2010-03-29 2011-10-20 Ube Machinery Corporation Ltd Oil pressure levitation trapezoidal screw thread
JP2014193519A (en) * 2013-03-28 2014-10-09 Denso Wave Inc Multi-axis robot trajectory forming method and multi-axis robot control device
JP2014193520A (en) * 2013-03-28 2014-10-09 Denso Wave Inc Multi-axis robot trajectory forming method and multi-axis robot control device

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1190758A (en) * 1997-09-22 1999-04-06 Nisshinbo Ind Inc Control method for feed screw mechanism
JP2004362394A (en) * 2003-06-06 2004-12-24 Okuma Corp Numerically controlled device having maximum feeding speed determination function
JP2009119566A (en) * 2007-11-15 2009-06-04 Denso Wave Inc Suspended robot
JP2011208655A (en) * 2010-03-29 2011-10-20 Ube Machinery Corporation Ltd Oil pressure levitation trapezoidal screw thread
JP2014193519A (en) * 2013-03-28 2014-10-09 Denso Wave Inc Multi-axis robot trajectory forming method and multi-axis robot control device
JP2014193520A (en) * 2013-03-28 2014-10-09 Denso Wave Inc Multi-axis robot trajectory forming method and multi-axis robot control device

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018150958A (en) * 2017-03-10 2018-09-27 株式会社Fuji Ball screw device and control method therefor

Also Published As

Publication number Publication date
JP6123595B2 (en) 2017-05-10

Similar Documents

Publication Publication Date Title
JP6268819B2 (en) Trajectory generation method for multi-axis robot
JP6167770B2 (en) Multi-axis robot trajectory generation method and multi-axis robot controller
JPH079606B2 (en) Robot controller
JP5144035B2 (en) Industrial robot tool position / posture control method and control system
JPWO2002066210A1 (en) Robot controller
JP2019135076A (en) Locus generation method and device
JP6123595B2 (en) Speed control method for 2-axis robot
JP5933450B2 (en) Robot control apparatus and control method
JP2008194760A (en) Robot arm and control method therefor
JP5633268B2 (en) Robot control device
JP6057284B2 (en) Articulated robot and semiconductor wafer transfer device
JP4647919B2 (en) Control method and control apparatus
JP6057283B2 (en) Articulated robot and semiconductor wafer transfer device
JP2015058497A (en) Horizontal multi-axis robot trajectory forming method and control device
JP2014124733A (en) Robot control device, and robot control method
JP2016055404A (en) Locus generation method, locus generation device, robot device, program, and recording medium
JP2021058950A (en) Robot control device
JPH07295615A (en) Driving control method for articulated robot
JP2013223895A (en) Robot control method and robot control device
JP3194829B2 (en) Robot motion program generation device
DK201901559A1 (en) Method of suppressing vibrations of a robot arm with external objects
JPH08328637A (en) Robot controller
JPH0957671A (en) Control device of articulated moving body and its control method
JP2012045636A (en) Robot and interpolation method
JPH07104825A (en) Controller for robot

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20160210

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20170110

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20170210

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20170307

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20170320

R150 Certificate of patent or registration of utility model

Ref document number: 6123595

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250