JP2019094176A - Control device, control method, and pogram - Google Patents
Control device, control method, and pogram Download PDFInfo
- Publication number
- JP2019094176A JP2019094176A JP2017224828A JP2017224828A JP2019094176A JP 2019094176 A JP2019094176 A JP 2019094176A JP 2017224828 A JP2017224828 A JP 2017224828A JP 2017224828 A JP2017224828 A JP 2017224828A JP 2019094176 A JP2019094176 A JP 2019094176A
- Authority
- JP
- Japan
- Prior art keywords
- boom
- velocity vector
- vector
- tip
- actuator
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims description 14
- 239000013598 vector Substances 0.000 claims abstract description 260
- 239000011159 matrix material Substances 0.000 claims abstract description 83
- 230000033001 locomotion Effects 0.000 claims description 103
- 230000008602 contraction Effects 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 description 27
- 230000008859 change Effects 0.000 description 20
- 238000012937 correction Methods 0.000 description 15
- 230000000694 effects Effects 0.000 description 9
- 230000006870 function Effects 0.000 description 7
- 230000009466 transformation Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 6
- 230000000052 comparative effect Effects 0.000 description 6
- 239000002131 composite material Substances 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000002093 peripheral effect Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007935 neutral effect Effects 0.000 description 1
- 230000003534 oscillatory effect Effects 0.000 description 1
- 238000007788 roughening Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
Images
Landscapes
- Forklifts And Lifting Vehicles (AREA)
Abstract
Description
本発明は、制御装置、制御方法及びプログラムに関する。 The present invention relates to a control device, a control method, and a program.
特許文献1及び2には、油圧制御装置で油圧アクチュエータの駆動方向及び駆動速度を制御することで、ブームの先端に取り付けられた作業部の位置を制御する技術が開示されている。特許文献1には、操作レバーの傾動操作に基づき、作業部の移動方向並びに速度に関連する信号を発生し、発生させた作業部の移動方向及び速度を、三角関数を用いて起伏及び伸縮の移動方向及び速度に変換することが開示されている。
特許文献3には、ブームの先端位置を、超音波を発信する位置指令発信機の存する位置に追従移動させる技術が開示されている。
本発明は、ブームの先端をなめらかに動かす技術を提供することを課題とする。 An object of the present invention is to provide a technique for smoothly moving the tip of a boom.
本発明によれば、
入力受付部と、
前記入力受付部が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換部と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御部と、
を有する制御装置が提供される。
According to the invention
An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device is provided.
また、本発明によれば、
コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換工程と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御工程と、
を実行する制御方法が提供される。
Moreover, according to the present invention,
The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
A control method for performing
また、本発明によれば、
コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換手段、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御手段、
として機能させるプログラムが提供される。
Moreover, according to the present invention,
Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as is provided.
本発明によれば、ブームの先端をなめらかに動かすことができる。 According to the present invention, the tip of the boom can be moved smoothly.
<第1の実施形態>
図1に、本実施形態の制御装置10の機能ブロック図の一例を示す。図示するように、制御装置10は、入力受付部11と、速度ベクトル生成部12と、変換部13と、アクチュエータ制御部14とを有する。
First Embodiment
FIG. 1 shows an example of a functional block diagram of a
入力受付部11は、各種入力を受付ける。速度ベクトル生成部12は、入力受付部11が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する。変換部13は、生成された速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを算出する。アクチュエータ制御部14は、ブームを伸縮させるアクチュエータ、ブームを起伏させるアクチュエータ及びブームを旋回させるアクチュエータを、動作ベクトルで示される動作方向及び速度で動作させる。
The
このような制御装置10により、ブームの先端をなめらかに動かす動作を実現する。以下、制御装置10の詳細を説明する。
Such a
まず、制御装置10のハードウエア構成の一例について説明する。本実施形態の制御装置10が備える各機能部は、任意のコンピュータのCPU(Central Processing Unit)、メモリ、メモリにロードされるプログラム、そのプログラムを格納するハードディスク等の記憶ユニット(あらかじめ装置を出荷する段階から格納されているプログラムのほか、CD(Compact Disc)等の記憶媒体やインターネット上のサーバ等からダウンロードされたプログラムをも格納できる)、ネットワーク接続用インターフェイスを中心にハードウエアとソフトウエアの任意の組合せによって実現される。そして、その実現方法、装置にはいろいろな変形例があることは、当業者には理解されるところである。
First, an example of the hardware configuration of the
図2は、本実施形態の制御装置10のハードウエア構成を例示するブロック図である。図2に示すように、制御装置10は、プロセッサ1A、メモリ2A、入出力インターフェイス3A、周辺回路4A、バス5Aを有する。周辺回路4Aには、様々なモジュールが含まれる。なお、制御装置10は物理的及び/又は論理的に分かれた複数の装置で構成されてもよい。この場合、複数の装置各々が、プロセッサ1A、メモリ2A、入出力インターフェイス3A、周辺回路4A、バス5Aを有してもよい。
FIG. 2 is a block diagram illustrating the hardware configuration of the
バス5Aは、プロセッサ1A、メモリ2A、周辺回路4A及び入出力インターフェイス3Aが相互にデータを送受信するためのデータ伝送路である。プロセッサ1Aは、例えばCPUやGPU(Graphics Processing Unit)などの演算処理装置である。メモリ2Aは、例えばRAM(Random Access Memory)やROM(Read Only Memory)などのメモリである。入出力インターフェイス3Aは、入力装置(例:キーボード、マウス、マイク、物理キー、タッチパネルディスプレイ、コードリーダ等)、外部装置、外部サーバ、外部センサ等から情報を取得するためのインターフェイスや、出力装置(例:ディスプレイ、スピーカ、プリンター、メーラ等)、外部装置、外部サーバ等に情報を出力するためのインターフェイスなどを含む。プロセッサ1Aは、各モジュールに指令を出し、それらの演算結果をもとに演算を行うことができる。
The
次に、本実施形態の制御装置10に制御されるブームを備える作業機について説明する。なお、作業機と制御装置10とは物理的及び/又は論理的に一体となって構成されてもよいし、物理的及び/又は論理的に分かれて構成されてもよい。
Next, a working machine provided with a boom controlled by the
作業機は、ブームを伸縮させるアクチュエータ(例:ブーム伸縮シリンダ)、ブームを起伏させるアクチュエータ(例:ブーム起伏シリンダ)、及び、ブームを旋回させるアクチュエータ(例:ブーム旋回モータ)を有する。複数のアクチュエータの少なくとも1つ(例:複数のアクチュエータの全て)は油圧制御される。 The work machine has an actuator (for example, a boom telescopic cylinder) that extends and retracts the boom, an actuator (for example, a boom hoisting cylinder) that raises and lowers the boom, and an actuator (for example, boom rotation motor) that turns the boom. At least one of the plurality of actuators (eg, all of the plurality of actuators) is hydraulically controlled.
ブームの動作は、制御弁及びスプールで制御される。すなわち、制御弁からの圧油の吐出量をスプールで変位させて調節することで、各アクチュエータへの圧油の供給量を調節する。各アクチュエータへの圧油の供給量を調節することで、各アクチュエータの動作方向及び速度を調節する。そして、各アクチュエータの動作を調節することで、ブームの先端の移動方向及び速度を調節する。 The operation of the boom is controlled by control valves and spools. That is, the amount of pressure oil supplied to each actuator is adjusted by displacing and adjusting the amount of discharge of pressure oil from the control valve using a spool. By adjusting the amount of pressure oil supplied to each actuator, the operating direction and speed of each actuator are adjusted. Then, the movement direction and speed of the tip of the boom are adjusted by adjusting the operation of each actuator.
ここで、図3を用いて本実施形態の作業機が備えるスプールの特性を説明する。図3は、スプール変位とシリンダ速度の関係の一例を示す。この特性カーブに基づき、アクチュエータを所望の速度で動かすためのスプール変位を求めることができる。例えば、アクチュエータを定格の20%の速度で動かしたい場合はスプールの変位を最大変位の51%に設定する。また、アクチュエータを定格の80%の速度で動かしたい場合はスプールの変位を最大変位の94%に設定する。 Here, the characteristics of the spool provided in the working machine of the present embodiment will be described with reference to FIG. FIG. 3 shows an example of the relationship between spool displacement and cylinder speed. Based on this characteristic curve, the spool displacement for moving the actuator at a desired speed can be determined. For example, if it is desired to move the actuator at a speed of 20% of the rated speed, the spool displacement is set to 51% of the maximum displacement. Also, if you want to move the actuator at 80% of rated speed, set the spool displacement to 94% of the maximum displacement.
ブームを伸縮させるアクチュエータ、ブームを起伏させるアクチュエータ、及び、ブームを旋回させるアクチュエータ各々の動作により、ブームの長さ(ブーム長)、起伏角度及び旋回角度が決定される。各アクチュエータの速度は、ブームの伸縮速度(単位時間当たりのブーム長の変化量)、起伏角速度(単位時間当たりのブームの起伏角の変化量)、及び、旋回角速度(単位時間当たりのブームの旋回角の変化量)に相当する。 The boom length (boom length), the elevation angle and the turning angle are determined by the operations of the actuator for extending and retracting the boom, the actuator for raising and lowering the boom, and the actuator for turning the boom. The speed of each actuator is the expansion / contraction speed of the boom (change in boom length per unit time), the elevation angular velocity (change in elevation angle of the boom per unit time), and the turning angular velocity (rotation of the boom per unit time) Corresponding to the change in angle).
なお、図3に示すように、制御弁のスプールはその中立付近の一定のストローク範囲内で「一定の不感帯域」を持つ。すなわち、スプール変位が不感帯に収まっている間、アクチュエータに圧油が供給されず、アクチュエータは動かない。アクチュエータに圧油を供給し、動かすためには、スプール変位が不感帯域の外に出なければならない。 As shown in FIG. 3, the spool of the control valve has a "constant dead zone" within a constant stroke range near its neutral position. That is, while the spool displacement is in the dead zone, no pressure oil is supplied to the actuator and the actuator does not move. In order to supply pressure oil to the actuator and move it, the spool displacement must go out of the dead zone.
図4に、線形で表したシリンダの目標速度の時間変化と、その目標速度を実現するためのスプール変位の時間変化との関係を示す。シリンダの目標速度は、所望の速度でブームの先端を移動させるためのシリンダの速度であり、一定速に安定後の終端速度と定義する。 FIG. 4 shows the relationship between the temporal change of the target velocity of the cylinder expressed linearly and the temporal change of the spool displacement for realizing the target velocity. The target velocity of the cylinder is the velocity of the cylinder for moving the tip of the boom at a desired velocity, and is defined as the terminal velocity after stabilization at a constant velocity.
また、作業機は、スプールの変位及び速度を検出する手段(差動トランス)を有する。さらに、作業機は、ブームの長さを検出するブーム長検出器、ブームの起伏角度を検出するブーム角度検出器、及び、ブームの旋回角度を検出する旋回角度検出器を有する。 The work machine also has means (differential transformer) for detecting the displacement and speed of the spool. Furthermore, the work machine has a boom length detector that detects the length of the boom, a boom angle detector that detects the elevation angle of the boom, and a pivot angle detector that detects the pivot angle of the boom.
次に、制御装置10について説明する。制御装置10は、アクチュエータの動作を制御する。図1の機能ブロック図に示すように、制御装置10は、入力受付部11と、速度ベクトル生成部12と、変換部13と、アクチュエータ制御部14とを有する。
Next, the
入力受付部11は、操作レバー(入力装置)を介して、ブームの先端を移動させる方向及び速度を指定する入力を受付ける。なお、入力受付部11は、タッチパネルディスプレイ、物理ボタン、マイク、キーボード、マウス等の他の入力装置を介して、ブームの先端を移動させる方向及び速度を指定する入力を受付けてもよい。
The
入力受付部11は、制御周期毎に、操作レバーの操作内容を検出し、検出した操作レバーの操作内容をブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、出力する。速度ベクトルは、作業機の所定位置を原点とし、作業機の所定方向をx軸(水平方向の軸)、y軸(水平方向の軸)及びz軸(垂直方向の軸)とした3次元座標系(以下、「作業機座標系」という場合がある)で示される。制御周期は、例えば1ミリ秒から100ミリ秒である。
The
入力受付部11は、操作レバーを傾ける方向に基づきブームの先端を移動させる方向を決定し、操作レバーを傾ける角度に基づきブームの先端を移動させる速度を決定する。例えば、予め、操作レバーを傾ける方向と、作業機座標系の所定の方向とが対応づけられていてもよい。また、予め、操作レバーを傾ける角度と、ブームの先端を移動させる速度とが対応付けられていてもよい。そして、入力受付部11は、当該対応関係に基づき、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換してもよい。例えば、2つの操作レバーを用意し、第1の操作レバーで水平方向の移動方向及び速度を指定し、第2の操作レバーで垂直方向の移動方向及び速度を指定できてもよい。
The
速度ベクトル生成部12は、制御周期毎に、入力受付部11が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系(作業機座標系)で示す速度ベクトルを生成し、出力する。以下、速度ベクトル生成部12により生成された速度ベクトルを、指示速度ベクトルという。
The velocity
ここで、指示速度ベクトルの生成例を説明する。 Here, an example of generation of the designated velocity vector will be described.
「指示速度ベクトル生成例1」
速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルを、そのまま指示速度ベクトルとすることができる。
"Indicated velocity vector generation example 1"
The velocity
「指示速度ベクトル生成例2」
速度ベクトル生成部12は、指示速度ベクトルの移動方向として、入力受付部11により出力された速度ベクトルで示される移動方向をそのまま採用することができる。そして、指示速度ベクトルの速度を、以下のようにして決定してもよい。
"Instructed velocity vector generation example 2"
The velocity
「(ブームの先端の現在の速度)<(入力受付部11により出力された速度ベクトルで示される速度)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)+(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
If “(the current speed of the tip of the boom) <(the speed indicated by the speed vector output from the input reception unit 11)”, the speed
ブームの動作を開始した直後等はブームの先端の速度が小さいため、当該条件を満たしやすくなる。この場合、上述のように指示速度ベクトルの速度を決定することで、急激な速度上昇を抑制し、滑らかな動作が実現される。 Immediately after the start of the operation of the boom, etc., the speed of the tip of the boom is small, so that the condition is easily satisfied. In this case, by determining the velocity of the designated velocity vector as described above, a rapid increase in velocity can be suppressed and a smooth operation can be realized.
一方、「(ブームの先端の現在の速度)≧(入力受付部11により出力された速度ベクトルで示される速度)」を満たす場合、速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルで示される速度を指示速度ベクトルの速度として採用してもよい。ブームの動作を開始してからある程度の時間が経過し、ブームの先端の移動速度が安定している頃には当該条件を満たしやすくなる。
On the other hand, when “(the current speed of the tip of the boom) ≧ (the speed indicated by the speed vector output by the input reception unit 11)”, the speed
変換部13は、速度ベクトル生成部12により出力された指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(以下の式(2)、(14)参照)。なお、変換部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、指示速度ベクトルを動作ベクトルに変換してもよい(以下の式(15)参照)。
The
ここで、ヤコビ行列の逆行列について説明する。 Here, the inverse matrix of the Jacobian matrix will be described.
ブームに伸縮、起伏、旋回を実行させると、ブームの先端が所定の方向に所定の速度で移動する。すなわち、ブームの先端に速度ベクトルが生じる。伸縮、起伏、旋回の中の複数を同時に行った場合は、各動作によってブームの先端に生じる速度ベクトルを足し合わせることによって、ブームの先端の速度ベクトルを求めることができる。 When the boom is extended, retracted, or turned, the tip of the boom moves at a predetermined speed in a predetermined direction. That is, a velocity vector is generated at the tip of the boom. In the case of simultaneously performing a plurality of stretching, undulation, and turning, the velocity vector of the tip of the boom can be determined by adding the velocity vectors generated at the tip of the boom by each operation.
ブームの動作ベクトルと、ブームの先端の速度ベクトルとは、ヤコビ行列Jを用いて式(1)のように示される。 The motion vector of the boom and the velocity vector of the tip of the boom are represented by the Jacobian matrix J as shown in equation (1).
xの上に黒点を付した文字は、作業機座標系におけるブームの先端のx軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「x´」と表す。yの上に黒点を付した文字は、作業機座標系におけるブームの先端のy軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「y´」と表す。zの上に黒点を付した文字は、作業機座標系におけるブームの先端のz軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「z´」と表す。 The letter with a black dot above x indicates the velocity component of movement of the tip of the boom in the work machine coordinate system in the x-axis direction. In the specification, the velocity component is represented as "x '". The letter with a black dot above y indicates the velocity component of movement of the tip of the boom in the y-axis direction in the work machine coordinate system. In the specification, the velocity component is referred to as "y '". The letter with a black dot above z indicates the velocity component of the movement of the tip of the boom in the z-axis direction in the work machine coordinate system. In the specification, the velocity component is referred to as "z '".
q1の上に黒点を付した文字は、ブームの旋回の角速度(旋回角の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q1´」と表す。q2の上に黒点を付した文字は、ブームの起伏の角速度(起伏角の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q2´」と表す。q3の上に黒点を付した文字は、ブームの伸縮速度(ブーム長の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q3´」と表す。 characters marked with black dots on the q 1 shows the angular velocity of the turning of the boom (the amount of change per unit turning angle time). In the specification, the velocity component is referred to as "q 1 '". The letter with a black dot above q 2 indicates the angular velocity of the boom undulation (the amount of change in undulation angle per unit time). In the specification, the velocity component is referred to as "q 2 '". characters marked with black dots on the q 3 shows the telescopic speed of the boom (variation per unit boom length time). The specification, representing the velocity component and "q 3 '."
ブームの伸縮、起伏、旋回の速度とブームの先端の速度の自由度が同じ(Jが正方行列)であり、かつ、ブームが特異姿勢でないとき、逆行列J−1を用いることで、式(2)に示すようにブームの先端の速度ベクトルから動作ベクトルを求めることができる。なお、ブームの特異姿勢とは、例えばブームが真上や真下を向いた状態をいう。 Telescopic boom, undulations, the degree of freedom of the speed of the tip of the swing speed and the boom is the same (J is a square matrix), and, when the boom is not singular, the use of the inverse matrix J -1, the formula ( As shown in 2), the motion vector can be obtained from the velocity vector of the tip of the boom. Note that the unusual posture of the boom means, for example, a state in which the boom is directed right up or down.
行列計算を展開すると、式(3)のようになる。 When the matrix calculation is expanded, it becomes like Formula (3).
ここで、逆行列J−1を求めると以下のようになる。 Here, the inverse matrix J −1 is obtained as follows.
「同次変換行列を使った順運動学計算(q1、q2、q3→x、y、z)」
まず、図5及び式(4)に示すように変数及び定数を定義する。Houtrigger、Hcolumn、Dcolumn、及び、Dboomは、作業機の構成に基づき定まる固定値である。
"Forward kinematics calculation using homogeneous transformation matrix (q 1 , q 2 , q 3 → x, y, z)"
First, variables and constants are defined as shown in FIG. 5 and equation (4). H outrigger , H column , D column , and D boom are fixed values determined based on the configuration of the working machine.
基準座標系→ターンテーブル座標系の同次変換行列は、式(5)のようになる。基準座標系は、図5に示すΣ0座標系である。コラム回転中心直下の地面を原点とし、車両左側方をX0軸方向、車両後方をY0軸方向、鉛直上方をZ0軸方向としている。ターンテーブル座標系は、図5に示すΣ1座標系であり、ターンテーブル中央を原点とし、ブームの方位角がY1軸と一致するよう基準座標系を並進移動およびZ1軸周りに回転したものである。式(5)は、Z0軸方向にHoutrigger並進移動し、Z1軸回りにq1回転することを意味する。 The homogeneous transformation matrix of the reference coordinate system → turn table coordinate system is as shown in Expression (5). The reference coordinate system is the 0 0 coordinate system shown in FIG. The ground immediately below the center of column rotation is the origin, the left side of the vehicle is the X 0 axis, the rear of the vehicle is the Y 0 axis, and the vertically upper is the Z 0 axis. Turntable coordinate system is a sigma 1 coordinate system shown in FIG. 5, the turntable center as the origin, the azimuth angle of the boom is rotated the reference coordinate system translation and Z 1 about an axis to coincide with the Y 1 axis It is a thing. Equation (5) is, Z 0 in the axial direction and H outrigger translation means to q 1 rotates in Z 1 axis.
ターンテーブル座標系→コラム座標系の同次変換行列は、式(6)のようになる。コラム座標系は、図5に示すΣ2座標系であり、ブームフートピン中央を原点とし、ブーム長手方向がY2軸方向と一致するようターンテーブル座標系を並進移動およびX2軸周りに回転したものである。式(6)は、Z1軸方向にHoutrigger、Y1軸方向に−Dcolumn並進移動し、X2軸回りにq2回転することを意味する。 The homogeneous transformation matrix of the turntable coordinate system → column coordinate system is as shown in equation (6). Column coordinate system is a sigma 2 coordinate system shown in FIG. 5, rotating the boom foot pin center as the origin, the translation and the X 2 axis around the turntable coordinate system so that the boom longitudinal direction coincides with the Y 2 axially It is Equation (6), H outrigger to Z 1 axial direction, -D column translates to Y 1 axially, it means that q 2 rotates in the X 2 axis.
コラム座標系→トップシーブ座標系の同次変換行列は、式(7)のようになる。トップシーブ座標系は、図5に示すΣ3座標系であり、トップシーブ中央が原点となるようコラム座標系を並進移動したものである。式(7)は、Z2軸方向に−Dboom、Y2軸方向にq3並進移動することを意味する。 The homogeneous transformation matrix of the column coordinate system → top sheave coordinate system is as shown in Expression (7). Top wheel coordinate system is a sigma 3 coordinate system shown in FIG. 5, it is obtained by translating the column coordinate system so that the top wheel center is the origin. Equation (7) means that q 3 translational movement to the Z 2 axial direction -D boom, the Y 2 axially.
基準座標系→トップシーブ座標系の同次変換行列は、式(8)のようになる。 The homogeneous transformation matrix of the reference coordinate system → top sheave coordinate system is as shown in equation (8).
式(8)を使ってトップシーブ座標系の原点(0、0、0)を基準座標系で表すと、式(9)のようになる。 When the origin (0, 0, 0) of the top sheave coordinate system is expressed in the reference coordinate system using the equation (8), the equation (9) is obtained.
式(9)の各係数を計算し展開すると、式(10)のようになる。 If each coefficient of Formula (9) is calculated and expanded, it will become like Formula (10).
「ヤコビ行列の逆行列を使った逆運動学計算(x´、y´、z´→q1´、q2´、q3´)」
x、y、zをそれぞれ時間微分してq1´、q2´、q3´についてまとめると、式(11)のようになる。
"Inverse kinematics calculation using the inverse matrix of the Jacobian matrix (x', y', z'→ q 1 ', q 2', q 3 ') "
If time differentiation of x, y and z is carried out and q 1 ′, q 2 ′ and q 3 ′ are put together, it becomes like Formula (11).
これを行列表記にすると、式(12)のようになる。 If this is made into matrix notation, it will become like Formula (12).
両辺に左からJ−1を掛けて、左辺及び右辺を入れ替えると式(13)のようになる。 If J- 1 is multiplied from the left to both sides and the left side and the right side are interchanged, it becomes like Formula (13).
J−1を計算すると、式(14)のようになる。 Calculating J −1 gives equation (14).
式(13)のJ−1に式(14)を代入して展開すると、式(15)のようになる。式(15)を利用することで、ブームの先端を所定の方向に所定の速度で移動させるための複数のアクチュエータ各々の動作方向及び速度(動作ベクトル)が求まる。 If Formula (14) is substituted and expand | deployed to J <-1> of Formula (13), it will become like Formula (15). By using the equation (15), the movement direction and speed (motion vector) of each of the plurality of actuators for moving the tip of the boom in a predetermined direction at a predetermined speed can be determined.
図1に戻り、アクチュエータ制御部14は、ブームを伸縮させるアクチュエータ、ブームを起伏させるアクチュエータ及びブームを旋回させるアクチュエータを、動作ベクトルで示される方向及び速度で動作させる。
Returning to FIG. 1, the
すなわち、アクチュエータ制御部14は、変換部13で算出された「動作ベクトル」と、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、動作ベクトルで示される方向に動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、求めた通りにスプールの変位を設定する。
That is, the
ここで、参考までに、図6に、本実施形態の制御装置10の構成の一例を示す。なお、あくまで一例であり、これに限定されない。
Here, for reference, FIG. 6 shows an example of the configuration of the
次に、図7を用いて、本実施形態の制御装置10による処理の流れの一例及びそれに応じたブームの先端の移動を説明する。図7は、ブームを上方から観察した模式図である。ブームの先端は(1)→(2)→(3)→(4)の順に移動するものとする。1回の制御周期で(1)→(2)の変化がおき、次の制御周期で(2)→(3)の変化がおき、次の制御周期で(3)→(4)の変化がおきるものとする。なお、これらの前提は、以下で説明する図8、図12及び図13も同様である。
Next, an example of the flow of processing by the
図7において、ブームの先端の位置が(1)で停止しているとき、入力受付部11は、最初の制御周期(第1の制御周期)の指示入力待ち状態となっている。そして、オペレータにより操作レバーを所定の方向に所定角度傾ける操作がなされると、入力受付部11は操作レバーの操作内容を検出する。そして、入力受付部11は、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、第1の制御周期の速度ベクトルとして出力する。ここでは、(1)のブームの先端から延びる矢印で示される方向の速度ベクトルが出力されたものとする。
In FIG. 7, when the position of the tip of the boom is stopped at (1), the
その後、速度ベクトル生成部12は、上述した指示速度ベクトル生成例1乃至3の何れかに基づき、第1の制御周期の指示速度ベクトルを生成し、出力する。
Thereafter, the velocity
その後、変換部13は、指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、第1の制御周期の指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(式(2)、(14)参照)。なお、変換部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、第1の制御周期の指示速度ベクトルを動作ベクトル(第1の制御周期の動作ベクトル)に変換してもよい(式(15)参照)。
Thereafter, the
その後、アクチュエータ制御部14は、「第1の制御周期の動作ベクトル」と、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、第1の制御周期の動作ベクトルで示される方向に当該動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、求めた通りにスプールの変位を設定する。
After that, the
結果、第1の制御周期の間ブームの先端は移動し、第1の制御周期後のブームの先端の位置は図示する(2)になる。なお、図示するように、ブームの動作の誤差等により、指示速度ベクトルで示される方向からズレが発生する場合がある。図の場合、図中の上下方向のズレが発生している。 As a result, the tip of the boom moves during the first control cycle, and the position of the tip of the boom after the first control cycle becomes (2) as illustrated. Note that as shown in the drawing, due to an error or the like of the operation of the boom, a shift may occur from the direction indicated by the designated speed vector. In the case of the drawing, vertical displacement in the drawing occurs.
入力受付部11は、第1の制御周期に対応する操作レバーの操作を検出した後の任意のタイミング以降に、次の制御周期(第2の制御周期)の指示入力待ち状態となる。そして、オペレータにより操作レバーを所定の方向に所定角度傾ける操作がなされると、入力受付部11は操作レバーの操作内容を検出する。そして、入力受付部11は、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、第2の制御周期の速度ベクトルとして出力する。なお、オペレータは、操作レバーを継続的に所定の方向に所定の角度で傾け続けておいてもよい。
The
その後、速度ベクトル生成部12は、上述した指示速度ベクトル生成例1乃至3の何れかに基づき、第1の制御周期の指示速度ベクトルを生成し、出力する。
Thereafter, the velocity
その後、変換部13は、指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、第2の制御周期の指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(式(2)、(14)参照)。なお、変換部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、第2の制御周期の指示速度ベクトルを動作ベクトル(第2の制御周期の動作ベクトル)に変換してもよい(式(15)参照)。
After that, the
その後、アクチュエータ制御部14は、「第2の制御周期の動作ベクトル」と、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、第2の制御周期の動作ベクトルで示される方向に当該動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、第1の制御周期終了後(第1の制御周期の動作ベクトルで制御し始めてから所定時間が経過後)に、第2の制御周期の動作ベクトルに基づき求めた通りにスプールの変位を設定する。
After that, the
結果、第2の制御周期の間ブームの先端は移動し、第2の制御周期後のブームの先端の位置は図示する(3)になる。なお、図示するように、ブームの動作の誤差等により、指示速度ベクトルで示される方向からズレが発生する場合がある。図の場合、図中の上下方向のズレが発生している。 As a result, the tip of the boom moves during the second control cycle, and the position of the tip of the boom after the second control cycle becomes (3) shown. Note that as shown in the drawing, due to an error or the like of the operation of the boom, a shift may occur from the direction indicated by the designated speed vector. In the case of the drawing, vertical displacement in the drawing occurs.
以降、同様の処理を繰り返す。なお、制御周期は例えば1ミリ秒から100ミリ秒である。 Thereafter, the same processing is repeated. The control cycle is, for example, 1 millisecond to 100 milliseconds.
次に、本実施形態の制御装置10の作用効果を説明する。
Next, the operation and effect of the
「第1の作用効果」
まず、ブームの先端を目的地まで直線的に動かす制御方法として、ブームの先端の位置を繰り返し制御する手段(位置フィードバック)が考えられる。これは、本実施形態の制御手法と異なる手段である。
"First effect"
First, as a control method of moving the tip of the boom linearly to the destination, means (position feedback) for repeatedly controlling the position of the tip of the boom can be considered. This is a means different from the control method of the present embodiment.
位置フィードバックによる制御を行う場合、例えば、ブームの先端の現在位置と目標位置を比較し、現在位置が目標位置に到達していない場合は正の出力を出し、現在位置が目標位置を超えた場合は負の出力を出すことで、目標位置と現在位置を一致させる。目標位置は、例えば、操作レバーの操作内容(傾ける方向、傾ける角度)に基づき、スプール変位から演算される。 When performing control by position feedback, for example, the current position of the tip of the boom is compared with the target position, and a positive output is output if the current position has not reached the target position, and the current position exceeds the target position Makes the target position and the current position coincide by outputting a negative output. The target position is calculated from the spool displacement, for example, based on the operation content (inclination direction, inclination angle) of the operation lever.
ここで、位置フィードバックで、図8のように旋回と伸縮でブームの先端を図の右方向に移動し続ける場合を考える。なお、この条件における基本動作方向は旋回が「右」、伸縮は「伸」である。「基本動作方向」は、操作レバーの操作内容で指定されたブームの先端の移動(図8の場合、右方向)を実現するためのブームの動作方向である。 Here, in position feedback, consider the case where the tip of the boom is continuously moved in the right direction of the figure by turning and stretching as shown in FIG. In this condition, the basic movement direction is "right" for turning and "stretching" for expansion and contraction. The “basic operating direction” is the operating direction of the boom for realizing the movement (rightward in the case of FIG. 8) of the tip of the boom specified by the operation content of the operating lever.
位置フィードバックの場合、図示するように制御周期毎に目標位置を定め、制御周期毎にブームの先端の位置を目標位置と一致させる。現在位置が目標位置に到達していない場合は正の出力を出し、現在位置が目標位置を超えた場合は負の出力を出すことで、目標位置と現在位置を一致させる。 In the case of position feedback, as shown, a target position is determined for each control cycle, and the position of the tip of the boom is matched with the target position for each control cycle. When the current position does not reach the target position, a positive output is provided, and when the current position exceeds the target position, a negative output is provided to match the target position with the current position.
(2)乃至(4)各々の現在位置→目標位置の補正は次の通りである。(2)では、旋回補正は「左」、伸縮補正は「縮」となる。(3)では、旋回補正は「左」、伸縮補正は「伸」となる。(4)では、旋回補正は「右」、伸縮補正は「縮」となる。 (2) to (4) The correction of each current position → target position is as follows. In (2), the turning correction is "left" and the expansion and contraction correction is "shrinkage". In (3), the turning correction is "left" and the expansion / contraction correction is "stretching". In (4), the turning correction is "right" and the expansion / contraction correction is "shrinkage".
位置フィードバックの場合、このように、タイミング毎に各動作の補正の方向が反転する場合がある。また、上述の通り基本動作方向は「旋回:右、伸縮:伸」に対して、補正の方向が基本動作方向から反転している場合がある。 In the case of position feedback, as described above, the direction of correction of each operation may be reversed at each timing. In addition, as described above, the direction of correction may be reversed from the basic operation direction with respect to "swing: right, expansion / contraction: extension" as described above.
位置制御の性能を向上させるために制御周期を短くした場合や、補正ゲインを大きくした場合、また、基本動作による移動速度が遅い場合は、基本動作方向に対する補正方向が反転する可能性が高くなり、また、制御周期毎に補正方向が反転する可能性も高まる。また、補正のために目標速度は微小なものとなる。ここで、図9に、微小な目標速度が発生した場合にこの目標速度を満足するためのスプール変位を示す。 When the control cycle is shortened to improve the performance of position control, or when the correction gain is increased, or when the moving speed by the basic operation is slow, there is a high possibility that the correction direction with respect to the basic operation direction is reversed. Also, the possibility that the correction direction is reversed for each control cycle is also increased. In addition, the target speed is minute for correction. Here, FIG. 9 shows a spool displacement for satisfying the target speed when a minute target speed is generated.
図9に示すように目標速度が0近傍で細かく変化した場合、目標速度の符号に併せて正側の閾値と負側の閾値を超えるように目標スプール位置が激しく変化する。なお、正側の閾値及び負側の閾値は、各々、図3に示す不感帯の上限及び下限である。産業用マニピューレタのように交流電流を使用しての電気制御の場合は印加電圧やインバータの周波数などを激しく変化させることが可能だが、スプールを介して油圧を制御する場合はスプールの応答速度を超えるためスプール変位に応答遅れやオーバーシュートが発生する(図10参照)。応答遅れとオーバーシュートが生じたスプールに対して図3の特性が現れるため、実際のアクチュエータの速度は図11のように振動的になる。 As shown in FIG. 9, when the target speed changes finely near 0, the target spool position changes drastically so as to exceed the positive side threshold and the negative side threshold along with the sign of the target speed. The positive threshold and the negative threshold are respectively the upper limit and the lower limit of the dead zone shown in FIG. In the case of electrical control using an alternating current like an industrial manipulator, it is possible to change the applied voltage and the frequency of the inverter violently, but in the case of controlling the oil pressure through the spool, the response speed of the spool is exceeded. Therefore, response delay and overshoot occur in the spool displacement (see FIG. 10). Since the characteristic of FIG. 3 appears for the response delayed and overshooted spool, the actual actuator speed becomes oscillatory as shown in FIG.
図11のように動作方向が激しく入れ替わるような内容でアクチュエータを動作させた場合、例えばブームがしなるなどさらに大きなオーバーランが発生し、制御系が発振する原因となる。 When the actuator is operated with contents such that the operation direction is strongly switched as shown in FIG. 11, a larger overrun occurs, for example, when the boom is broken, which causes the control system to oscillate.
これに対し、本実施形態では、図7を用いて説明したように、ブームの先端の速度をベースとした制御を行う。ブームの先端を右に動かし続けるために、単位時間(1回の制御周期)当たりにおいてブームの先端で右方向に速度が発生するような旋回及び伸縮の速度を求め、これを満たすように制御する。ブームの先端の速度を満たす旋回及び伸縮の速度はブームの現姿勢によるため、制御周期毎に更新することができる。 On the other hand, in the present embodiment, as described with reference to FIG. 7, control based on the speed of the tip of the boom is performed. In order to keep moving the tip of the boom to the right, find and control the speed of turning and stretching so that the velocity is generated rightward at the tip of the boom per unit time (one control cycle) . Since the speed of turning and stretching to meet the speed of the tip of the boom depends on the current position of the boom, it can be updated every control cycle.
ブームの姿勢が大きく変わらない限り、ブームの先端の目標速度に対する旋回及び伸縮の目標速度は大きく変化せず、基本動作方向も変わらない。したがって、例えば旋回及び伸縮の速度が目標速度を超えた場合は自動車のアクセル制御のように減速することで補正され、旋回及び伸縮の動作方向の符号を変える必要はない。スプール位置についても、速度0〜最大速度の範囲で制御し、反転を考慮する必要がないため、不感帯から最大速度の範囲で制御するだけでよい。目標速度が急激に変化しない限り、スプール変位量のフィードバック値もずれが発生せず、安定した制御をすることができる。
As long as the attitude of the boom does not change significantly, the target speeds of turning and extension with respect to the target speed of the tip of the boom do not change significantly, and the basic operation direction does not change. Therefore, for example, if the speed of turning and stretching exceeds the target speed, it is corrected by decelerating as in the case of accelerator control of the automobile, and it is not necessary to change the sign of the moving direction of turning and stretching. The spool position is also controlled in the range of
「第2の作用効果」
速度ベースで制御するための速度の算出手段として、制御周期毎の目標位置を定め、現在位置と目標位置との差分により速度を求める手段(比較例の算出手段)が考えられる。これは、本実施形態の手段と異なる手段である。
"Second effect"
As a speed calculation means for controlling on a speed basis, a means (comparison means of a comparative example) for determining the target position for each control cycle and obtaining the speed from the difference between the current position and the target position can be considered. This is a means different from the means of this embodiment.
しかし、比較例の算出手段の場合、浮動小数点型の演算における微小な計算誤差による影響が問題となる。例えば、ブームの先端を垂直に上昇させたい場合、旋回を行う必要がない。しかし、比較例の算出手段の場合、浮動小数点型の演算における丸め込み等の誤差により、動作不要な旋回に対して符号が頻繁に変化する形で微小な目標速度が発生する。計算誤差の他に、ブームのガタ等により旋回位置に微小量分の狂いが生じた場合にも微小な目標速度が発生する可能性がある。符号の変化を伴う微小な目標速度は制御弁の制御に悪影響を与える。なお、他の動作に対しても浮動小数点型の演算における誤差は発生するが、基本動作速度に対して微小であるため大きな影響はない。 However, in the case of the calculation means of the comparative example, the influence of the minute calculation error in the floating point type operation becomes a problem. For example, if it is desired to raise the tip of the boom vertically, it is not necessary to make a turn. However, in the case of the calculation means of the comparative example, due to an error such as rounding in the floating point type calculation, a minute target speed is generated in a form in which the sign changes frequently with respect to the turning unnecessary. In addition to the calculation error, there is a possibility that a minute target speed may be generated even when a slight amount of deviation occurs in the turning position due to the play of the boom or the like. The minute target speed accompanied by the change of sign adversely affects the control of the control valve. Although an error occurs in the floating-point type operation with respect to other operations, it does not greatly affect the basic operation speed because it is small.
また、比較例の算出手段で旋回や起伏の角速度を求める場合、平方根を用いて演算する必要がある。このため、計算コストが大きくなる。 Moreover, when calculating | requiring the angular velocity of turning or roughening with the calculation means of a comparative example, it is necessary to calculate using a square root. This increases the computational cost.
これに対し、本実施形態の制御装置10は、式(14)に示すヤコビ行列の逆行列に基づいて、ブームの先端を移動させる方向及び速度を示す速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する。
On the other hand, the
式(14)に示すヤコビ行列の逆行列の各要素は三角関数の四則演算で構成され、平方根が含まれていない。このため、各要素を求めるための計算コストは大きくならない。また、式(15)に示すように、ヤコビ行列の逆行列を用いて得られた算出式も平方根を含まない。このため、式(15)に基づく計算コストも大きくならない。 Each element of the inverse matrix of the Jacobian matrix shown in equation (14) is formed by four arithmetic operations of trigonometric functions and does not include a square root. For this reason, the calculation cost for obtaining each element does not increase. Also, as shown in equation (15), the calculation equation obtained using the inverse matrix of the Jacobian matrix does not include the square root. For this reason, the calculation cost based on Formula (15) does not increase either.
また、式(15)より、x´及びy´を0とすると、旋回の速度ベクトルを示すq1´は0となる。すなわち、クレーンの先端を垂直方向に移動させる場合には、旋回の速度ベクトルを0にできる。このため、比較例の算出手段のように演算誤差による微小な目標速度が発生する不都合を回避できる。 Further, when x ′ and y ′ are 0 from equation (15), q 1 ′ indicating the speed vector of turning is 0. That is, when moving the tip of the crane in the vertical direction, the speed vector of turning can be made zero. For this reason, it is possible to avoid the disadvantage that a minute target speed occurs due to a calculation error as in the calculation means of the comparative example.
また、式(15)より、積と和によって動作ベクトルが求まるため、計算コストが小さい。また、動作ベクトルの解が一意に求まる。 Further, according to equation (15), since the motion vector is obtained by the product and the sum, the calculation cost is small. Also, the solution of the motion vector is uniquely obtained.
このような本実施形態の制御装置10によれば、ブームの先端を目的地まで直線的にかつなめらかに動かすことができる。
According to the
<第2の実施形態>
本実施形態の制御装置10は、速度ベクトル生成部12の構成が第1の実施形態と異なる。その他の構成は、第1の実施形態と同様である。また、作業機の構成も第1の実施形態と同様である。
Second Embodiment
The
第1の実施形態の場合、図7を用いて説明したように、ブームの先端の移動方向が目標とする方向からずれる可能性があった。本実施形態の速度ベクトル生成部12は、当該ずれを補正するように、指示速度ベクトルを生成する。
In the case of the first embodiment, as described with reference to FIG. 7, there is a possibility that the moving direction of the tip of the boom may deviate from the target direction. The velocity
まず、速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルに基づき、制御周期毎に、ブームの先端の目標位置を決定する。目標位置は、作業機座標系で示される。
First, the velocity
例えば、速度ベクトル生成部12は、移動開始時点のブームの先端の位置情報(作業機座標系で示すもの)を保持しておき、当該位置を起点として入力受付部11により出力された速度ベクトルで示される方向に当該速度ベクトルで示される速度で制御周期の間移動した後の位置を、最初の制御周期の目標位置として決定する。
For example, the velocity
その後は、速度ベクトル生成部12は、第n−1の制御周期の目標位置から第nの制御周期に対応して入力受付部11により出力された速度ベクトルで示される方向に当該速度ベクトルで示される速度で制御周期の間移動した後の位置を、第nの制御周期の目標位置として決定する(nは2以上の整数)。
Thereafter, the velocity
そして、速度ベクトル生成部12は、第nの制御周期に対応して入力受付部11が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n−1の制御周期後のブームの先端の位置から第n−1の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における指示速度ベクトルとして生成する。第1の速度ベクトルは、第nの制御周期に対応して入力受付部11が受付けた入力内容に基づき、上述した指示速度ベクトル生成例1乃至3の何れかに基づき生成された指示速度ベクトルである。
Then, the velocity
図12を用いて具体例を説明する。移動開始時点のブームの先端の位置を(1)とし、第1の制御周期後の状態を(2)、第2の制御周期後の状態を(3)、第3の制御周期後の状態を(4)とする。 A specific example will be described using FIG. The position of the tip of the boom at the start of movement is (1), the state after the first control cycle is (2), the state after the second control cycle is (3), the state after the third control cycle (4)
(2)より、第1の制御周期の目標位置に対して、第1の制御周期後の現在位置が異なっていることが分かる。速度ベクトル生成部12は、第2の制御周期の指示速度ベクトルとして、第2の制御周期に対応して決定した第1の速度ベクトルと、第1の制御周期後のブームの先端の位置から第1の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを決定する。
From (2), it can be seen that the current position after the first control cycle is different from the target position of the first control cycle. The velocity
同様に、(3)より、第2の制御周期の目標位置に対して、第2の制御周期後の現在位置が異なっていることが分かる。速度ベクトル生成部12は、第3の制御周期の指示速度ベクトルとして、第3の制御周期に対応して決定した第1の速度ベクトルと、第2の制御周期後のブームの先端の位置から第2の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを決定する。
Similarly, it can be seen from (3) that the current position after the second control cycle is different from the target position of the second control cycle. The velocity
図より、合成ベクトルは、ブームの先端位置を目標軌道上に戻す方向のベクトルとなる。なお、速度ベクトル生成部12は所定の係数を掛けた第2の速度ベクトルと第1の速度ベクトルとを足し合わせた合成ベクトルを、指示速度ベクトルとしてもよい。係数を調整することで、目標軌道上に戻す補正の大きさを調整できる。係数は、予め与えられていてもよい。
From the figure, the composite vector is a vector in the direction of returning the tip position of the boom onto the target trajectory. Note that the velocity
次に、本実施形態の変形例を説明する。アクチュエータ制御部14は、上記合成ベクトルを変換した動作ベクトルで示されるブームの伸縮、起伏及び旋回の内、上記第1の速度ベクトルを変換した第1の動作ベクトルで示されるブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、動作方向が異なる動作は実行させないように制御してもよい。
Next, a modification of this embodiment will be described. The
第1の動作ベクトルは、第1の速度ベクトルに左からヤコビ行列の逆行列を掛けることで(式(2)、(14)参照)、又は、ヤコビ行列の逆行列を用いて得られた算出式を用いて(式(15)参照)、第1の速度ベクトルを変換した動作ベクトルである。 The first motion vector is a calculation obtained by multiplying the first velocity vector by the inverse matrix of the Jacobian matrix from the left (see Equations (2) and (14)) or using the inverse matrix of the Jacobian matrix Using the equation (see equation (15)), it is a motion vector obtained by converting the first velocity vector.
次に、本実施形態の作用効果を説明する。本実施形態の制御装置10は、第1の実施形態と同様の作用効果を実現できる。
Next, the operation and effect of the present embodiment will be described. The
また、図7に示す第1の実施形態の制御の場合、位置誤差が蓄積する可能性がある。上述した合成ベクトルを指示速度ベクトルとする本実施形態の制御装置10によれば、位置誤差を軽減することができる。
Further, in the case of the control of the first embodiment shown in FIG. 7, there is a possibility that position errors may be accumulated. According to the
なお、目標軌道上を厳密に動かしたい場合は、現時点で生じている位置誤差を直ちに(例:次制御周期で)解消する必要がある。しかし、当該制御を行った場合、ブームの各動作の動作方向や速度が大きく変化する可能性がある。結果、ブームのなめらかな動きが実現されなくなる。 When it is desired to move the target trajectory strictly, it is necessary to immediately eliminate the position error occurring at the present time (eg, in the next control cycle). However, when the control is performed, the operating direction and speed of each operation of the boom may change significantly. As a result, the smooth movement of the boom can not be realized.
本実施形態の場合、位置誤差を直ちに(例:次制御周期で)解消するのでなく、複数回の制御周期で位置誤差を少しずつ解消するように制御する。このため、位置誤差を解消しつつ、ブームのなめらかな動きを実現できる。 In the case of the present embodiment, the position error is controlled not to be eliminated immediately (e.g., in the next control cycle), but to be gradually eliminated in a plurality of control cycles. Therefore, it is possible to realize smooth movement of the boom while eliminating the position error.
また、第2の速度ベクトルで位置補正を行う本実施形態の場合、オペレータの入力内容に基づき決定される各アクチュエータの動作方向と、動作ベクトルで示される各アクチュエータの動作方向とが反転する可能性がある。しかし、上記変形例を採用することで、オペレータの入力内容に基づき決定される各アクチュエータの動作方向と異なる方向に動作させる不都合を回避できる。結果、ブームのなめらかの動きが実現される。 Further, in the case of the present embodiment in which the position correction is performed with the second velocity vector, the operation direction of each actuator determined based on the input content of the operator and the operation direction of each actuator indicated by the operation vector may be reversed. There is. However, by adopting the above modification, it is possible to avoid the inconvenience of operating in a direction different from the operation direction of each actuator determined based on the input content of the operator. As a result, smooth movement of the boom is realized.
<第3の実施形態>
本実施形態の制御装置10は、入力受付部11及び速度ベクトル生成部12の構成が第1及び第2の実施形態と異なる。その他の構成は、第1及び第2の実施形態と同様である。また、作業機の構成も第1及び第2の実施形態と同様である。
Third Embodiment
The
入力受付部11は、ブームの先端の最終目標位置を指定する入力を受付ける。最終目標位置は、作業機座標系で示される。例えば、特許文献3に記載の技術を利用してもよい。すなわち、作業機に複数の超音波受信ユニットを設置し、最終目標位置に超音波発信機を設置する。入力受付部11は、超音波発信時刻と超音波受信時刻の時間差の入力を受付ける。そして、入力受付部11は、入力された情報に基づき、ブームの先端の最終目標位置を算出し、出力する。
The
速度ベクトル生成部12は、制御周期毎に、ブームの先端の現在位置から最終目標位置に向かう指示速度ベクトルを生成する。
The velocity
すなわち、速度ベクトル生成部12は、各制御周期後のブームの先端の現在位置を取得する。当該現在位置は、作業機座標系で示される。そして、速度ベクトル生成部12は、第nの制御周期の指示速度ベクトルとして、第n−1の制御周期後のブームの現在位置から最終目標位置に向かう速度ベクトルを決定する。図13に本実施形態の制御の概念図を示す。ブームの先端の現在位置から最終目標位置に向かう指示速度ベクトルが生成されている。
That is, the velocity
ここで、指示速度ベクトルの速度の決定例を説明する。 Here, an example of determination of the velocity of the designated velocity vector will be described.
「指示速度ベクトルの速度決定例1」
予め、固定値が与えられていてもよい。そして、速度ベクトル生成部12は当該固定値を、指示速度ベクトルの速度としてもよい。
"Example 1 of determining the speed of commanded speed vector"
A fixed value may be given in advance. Then, the velocity
「指示速度ベクトルの速度決定例2」
入力受付部11が速度を指定する入力を、任意の手段でオペレータから受付けてもよい。そして、速度ベクトル生成部12は、当該入力の内容に基づき、指示速度ベクトルの速度を決定してもよい。
"Example 2 of determining the speed of the indicated speed vector"
The
「指示速度ベクトルの速度決定例3」
「(最終目標位置とブームの現在位置との間の距離)≦(閾値)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)−(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
"Example 3 of determining the speed of commanded speed vector"
If “(distance between final target position and current position of boom) ≦ (threshold)”, the velocity
ブームの先端が最終目標位置に近づいている場合、当該条件を満たすこととなる。この場合、上述のように指示速度ベクトルの速度を決定することで、最終目標位置に到達するタイミングが近付くと、徐々に速度を減少させることができる。結果、滑らかな動作が実現される。 If the tip of the boom is approaching the final target position, the condition is satisfied. In this case, by determining the velocity of the indicated velocity vector as described above, the velocity can be gradually decreased when the timing to reach the final target position approaches. As a result, smooth operation is realized.
一方、「(最終目標位置とブームの現在位置との間の距離)>(閾値)」を満たす場合、速度ベクトル生成部12は、速度ベクトル生成部12は、指示速度ベクトルの速度決定例1又は2で、指示速度ベクトルの速度を決定してもよい。最終目標位置までの距離がある程度大きい場合、当該条件を満たすこととなる。
On the other hand, when “(distance between final target position and current position of boom)> (threshold)”, the velocity
閾値は、例えば、「現在速度から一定の減速度で減速し、停止するまでに要する距離」であってもよい。一例として、「(閾値)=(現在速度)×(現在速度から一定の減速度で減速した場合に停止するまでに要する時間)/2」で算出してもよい。または、「(閾値)=(現在速度)×(現在速度)/(2×減速度)」で算出してもよい。なお、「現在速度から一定の減速度で減速した場合に停止するまでに要する時間」は、「(現在速度から一定の減速度で減速した場合に停止するまでに要する時間)=(現在速度)/(減速度)」で算出してもよい。 The threshold may be, for example, "the distance required to decelerate from the current speed at a constant deceleration and stop." As an example, it may be calculated by “(threshold) = (current speed) × (time required to stop when decelerating from current speed at a constant deceleration) / 2”. Alternatively, it may be calculated by “(threshold) = (current speed) × (current speed) / (2 × deceleration)”. Note that “the time required to stop when the current speed decelerates at a constant deceleration” is “(time required to stop when the current speed decelerates at a constant deceleration) = (current speed) It may be calculated by / (deceleration) ".
「指示速度ベクトルの速度決定例4」
「(ブームの先端の現在の速度)<(指示速度ベクトルの速度決定例1又は2で決定された速度)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)+(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
"Example 4 of determining the velocity of the indicated velocity vector"
If “(the current velocity of the tip of the boom) <(the velocity determined in the velocity determination example 1 or 2 of the designated velocity vector)” is satisfied, the velocity
ブームの動作を開始した直後等はブームの先端の速度が小さいため、当該条件を満たしやすくなる。この場合、上述のように指示速度ベクトルの速度を決定することで、急激な速度上昇を抑制し、滑らかな動作が実現される。 Immediately after the start of the operation of the boom, etc., the speed of the tip of the boom is small, so that the condition is easily satisfied. In this case, by determining the velocity of the designated velocity vector as described above, a rapid increase in velocity can be suppressed and a smooth operation can be realized.
一方、「(ブームの先端の現在の速度)≧(指示速度ベクトルの速度決定例1又は2で決定された速度)」を満たす場合、速度ベクトル生成部12は、指示速度ベクトルの速度として、指示速度ベクトルの速度決定例1又は2で決定された速度を決定してもよい。ブームの動作を開始してからある程度の時間が経過し、ブームの先端の移動速度が安定している頃には当該条件を満たしやすくなる。
On the other hand, when “(the current speed of the tip of the boom) ((the speed determined in speed determination example 1 or 2 of the specified speed vector)” is satisfied, the speed
本実施形態によれば、第1及び第2の実施形態と同様の作用効果を実現できる。また、本実施形態によれば、制御周期毎にブームの先端の現在位置を取得し、そこから最終目標位置に向かう指示速度ベクトルを生成することができる。このため、移動方向が最終目標位置に向かう方向から大きく外れることなく、ブームの先端をなめらかに移動させることができる。 According to this embodiment, the same operation and effect as those of the first and second embodiments can be realized. Further, according to the present embodiment, it is possible to acquire the current position of the tip of the boom for each control cycle, and to generate an indicated speed vector heading therefrom to the final target position. For this reason, the tip of the boom can be smoothly moved without the movement direction being largely deviated from the direction toward the final target position.
以下、参考形態の例を付記する。
1. 入力受付部と、
前記入力受付部が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換部と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御部と、
を有する制御装置。
2. 1に記載の制御装置において、
前記ヤコビ行列の逆行列は、
C1=cos(q1)、C2=cos(q2)、S1=sin(q1)、S2=sin(q2)、q1は前記ブームの旋回角、q2は前記ブームの起伏角、q3は前記ブームの長さ、Dboomは前記ブームの軸とトップシーブのオフセット距離、Dcolumnはコラム回転軸と接続ピンの水平オフセット距離である制御装置。
3. 1又は2に記載の制御装置において、
前記入力受付部は、制御周期毎に、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、前記制御周期毎に、前記入力受付部が受付けた入力内容に基づき前記速度ベクトルを生成する制御装置。
4. 1又は2に記載の制御装置において、
前記入力受付部は、前記ブームの先端の最終目標位置を指定する入力を受付け、
前記速度ベクトル生成部は、制御周期毎に、前記ブームの先端の現在位置から前記最終目標位置に向かう前記速度ベクトルを生成する制御装置。
5. 1又は2に記載の制御装置において、
前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、
移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n−1の制御周期後の前記ブームの先端の位置から第n−1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成する制御装置。
6. 5に記載の制御装置において、
前記アクチュエータ制御部は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置。
7. 1から6のいずれかに記載の制御装置において、
複数の前記アクチュエータは油圧制御される制御装置。
8. コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換工程と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御工程と、
を実行する制御方法。
9. コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換手段、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御手段、
として機能させるプログラム。
Hereinafter, an example of a reference form is added.
1. An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device having
2. In the control device described in 1,
The inverse of the Jacobian matrix is
C 1 = cos (q 1 ), C 2 = cos (q 2 ), S 1 = sin (q 1 ), S 2 = sin (q 2 ), q 1 is the swing angle of the boom, q 2 is the boom A controller comprising a hoisting angle, q 3 is a length of the boom, D boom is an offset distance between the axis of the boom and the top sheave, and D column is a horizontal offset distance between a column rotation axis and a connection pin.
3. In the control device according to 1 or 2,
The input receiving unit receives an input specifying the velocity vector for each control cycle,
The control device, wherein the velocity vector generation unit generates the velocity vector based on the input content accepted by the input acceptance unit for each control cycle.
4. In the control device according to 1 or 2,
The input receiving unit receives an input specifying a final target position of the tip of the boom,
The control device, wherein the velocity vector generation unit generates the velocity vector from the current position of the tip of the boom to the final target position for each control cycle.
5. In the control device according to 1 or 2,
The input receiving unit receives an input for specifying the velocity vector,
The velocity vector generation unit
Calculating a target position of the tip of the boom for each control cycle based on the movement start position and the input content received by the input reception unit;
A first velocity vector determined based on the input content received by the input receiving unit corresponding to the nth (n is an integer of 2 or more) control cycle, and the boom after the (n-1) th control cycle The control device which generates the synthetic vector which added the 2nd speed vector which goes to the end position of the tip of the boom of the (n-1) th control cycle from the position of the tip as the speed vector in the nth control cycle.
6. In the control device described in 5,
The actuator control unit
Of the expansion, contraction, and rotation of the boom indicated by the movement vector, an operation in which the movement direction coincides with each of the expansion, contraction, and rotation, indicated by the first movement vector converted from the first velocity vector And a control unit that does not execute operations with different operation directions.
7. In the control device according to any one of 1 to 6,
A controller in which the plurality of actuators are hydraulically controlled.
8. The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
Control method to execute.
9. Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as
1A プロセッサ
2A メモリ
3A 入出力I/F
4A 周辺回路
5A バス
10 制御装置
11 入力受付部
12 速度ベクトル生成部
13 変換部
14 アクチュエータ制御部
4A
Claims (9)
前記入力受付部が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換部と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御部と、
を有する制御装置。 An input reception unit,
A velocity vector generation unit for generating a velocity vector indicating a direction and velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received by the input receiving unit;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. A converter for converting into a motion vector indicating the motion direction and speed of
An actuator for extending and retracting the boom, an actuator for raising and lowering the boom, and an actuator control unit for operating the actuator for pivoting the boom with the motion direction and speed indicated by the motion vector;
A control device having
前記ヤコビ行列の逆行列は、
C1=cos(q1)、C2=cos(q2)、S1=sin(q1)、S2=sin(q2)、q1は前記ブームの旋回角、q2は前記ブームの起伏角、q3は前記ブームの長さ、Dboomは前記ブームの軸とトップシーブのオフセット距離、Dcolumnはコラム回転軸と接続ピンの水平オフセット距離である制御装置。 In the control device according to claim 1,
The inverse of the Jacobian matrix is
C 1 = cos (q 1 ), C 2 = cos (q 2 ), S 1 = sin (q 1 ), S 2 = sin (q 2 ), q 1 is the swing angle of the boom, q 2 is the boom A controller comprising a hoisting angle, q 3 is a length of the boom, D boom is an offset distance between the axis of the boom and the top sheave, and D column is a horizontal offset distance between a column rotation axis and a connection pin.
前記入力受付部は、制御周期毎に、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、前記制御周期毎に、前記入力受付部が受付けた入力内容に基づき前記速度ベクトルを生成する制御装置。 In the control device according to claim 1 or 2,
The input receiving unit receives an input specifying the velocity vector for each control cycle,
The control device, wherein the velocity vector generation unit generates the velocity vector based on the input content accepted by the input acceptance unit for each control cycle.
前記入力受付部は、前記ブームの先端の最終目標位置を指定する入力を受付け、
前記速度ベクトル生成部は、制御周期毎に、前記ブームの先端の現在位置から前記最終目標位置に向かう前記速度ベクトルを生成する制御装置。 In the control device according to claim 1 or 2,
The input receiving unit receives an input specifying a final target position of the tip of the boom,
The control device, wherein the velocity vector generation unit generates the velocity vector from the current position of the tip of the boom to the final target position for each control cycle.
前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、
移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n−1の制御周期後の前記ブームの先端の位置から第n−1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成する制御装置。 In the control device according to claim 1 or 2,
The input receiving unit receives an input for specifying the velocity vector,
The velocity vector generation unit
Calculating a target position of the tip of the boom for each control cycle based on the movement start position and the input content received by the input reception unit;
A first velocity vector determined based on the input content received by the input receiving unit corresponding to the nth (n is an integer of 2 or more) control cycle, and the boom after the (n-1) th control cycle The control device which generates the synthetic vector which added the 2nd speed vector which goes to the end position of the tip of the boom of the (n-1) th control cycle from the position of the tip as the speed vector in the nth control cycle.
前記アクチュエータ制御部は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置。 In the control device according to claim 5,
The actuator control unit
Of the expansion, contraction, and rotation of the boom indicated by the movement vector, an operation in which the movement direction coincides with each of the expansion, contraction, and rotation, indicated by the first movement vector converted from the first velocity vector And a control unit that does not execute operations with different operation directions.
複数の前記アクチュエータは油圧制御される制御装置。 The control device according to any one of claims 1 to 6.
A controller in which the plurality of actuators are hydraulically controlled.
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換工程と、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御工程と、
を実行する制御方法。 The computer is
An input reception process,
A velocity vector generation step of generating a velocity vector indicating a direction and a velocity for moving the tip of the expandable / retractable / retractable / swingable boom tip on the basis of the input content received in the input reception step;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Converting into a motion vector indicating the motion direction and speed of
An actuator control step of operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for pivoting the boom with the movement direction and speed indicated by the movement vector;
Control method to execute.
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを、前記ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する変換手段、
前記ブームを伸縮させるアクチュエータ、前記ブームを起伏させるアクチュエータ及び前記ブームを旋回させるアクチュエータを、前記動作ベクトルで示される動作方向及び速度で動作させるアクチュエータ制御手段、
として機能させるプログラム。 Computer,
Input acceptance means,
Velocity vector generating means for generating a velocity vector indicating the direction and velocity of moving the tip of the expandable / retractable / retractable and pivotable boom in a three-dimensional coordinate system based on the input contents received by the input receiving means;
The velocity vector may be expanded, contracted, or pivoted from the boom by using the velocity vector multiplied by the inverse matrix of the Jacobian matrix from the left, or using a formula obtained using the inverse matrix of the Jacobian matrix. Conversion means for converting into a motion vector indicating the motion direction and speed of
Actuator control means for operating the actuator for expanding and contracting the boom, the actuator for raising and lowering the boom, and the actuator for rotating the boom with the movement direction and speed indicated by the movement vector;
A program to function as
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2017224828A JP7082476B2 (en) | 2017-11-22 | 2017-11-22 | Controls, control methods and programs |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2017224828A JP7082476B2 (en) | 2017-11-22 | 2017-11-22 | Controls, control methods and programs |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2019094176A true JP2019094176A (en) | 2019-06-20 |
JP7082476B2 JP7082476B2 (en) | 2022-06-08 |
Family
ID=66972652
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2017224828A Active JP7082476B2 (en) | 2017-11-22 | 2017-11-22 | Controls, control methods and programs |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP7082476B2 (en) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0367309A (en) * | 1990-02-06 | 1991-03-22 | Hitachi Ltd | Industrial robot controller |
JPH07197485A (en) * | 1993-12-28 | 1995-08-01 | Komatsu Ltd | Working machine control device for construction machine |
JPH09128023A (en) * | 1995-10-31 | 1997-05-16 | Ishikawajima Harima Heavy Ind Co Ltd | Method and device for assembling work |
US20040267404A1 (en) * | 2001-08-31 | 2004-12-30 | George Danko | Coordinated joint motion control system |
JP2014014901A (en) * | 2012-07-09 | 2014-01-30 | Toshiba Corp | Robot controller |
-
2017
- 2017-11-22 JP JP2017224828A patent/JP7082476B2/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0367309A (en) * | 1990-02-06 | 1991-03-22 | Hitachi Ltd | Industrial robot controller |
JPH07197485A (en) * | 1993-12-28 | 1995-08-01 | Komatsu Ltd | Working machine control device for construction machine |
JPH09128023A (en) * | 1995-10-31 | 1997-05-16 | Ishikawajima Harima Heavy Ind Co Ltd | Method and device for assembling work |
US20040267404A1 (en) * | 2001-08-31 | 2004-12-30 | George Danko | Coordinated joint motion control system |
JP2014014901A (en) * | 2012-07-09 | 2014-01-30 | Toshiba Corp | Robot controller |
Also Published As
Publication number | Publication date |
---|---|
JP7082476B2 (en) | 2022-06-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2019094177A (en) | Control device, control method, and program | |
US6430473B1 (en) | Power assist apparatus and control method for same | |
CN108568814B (en) | Robot and robot control method | |
JP5868580B2 (en) | Crane for handling loads lifted by cables | |
JP5145356B2 (en) | Motion control system | |
CN107849856B (en) | Large manipulator with articulated mast capable of being folded and unfolded quickly | |
WO2001038048A1 (en) | Robot controller | |
JP4889809B2 (en) | Acceleration / deceleration control device | |
US11023827B2 (en) | Machine learning device, servo control device, servo control system, and machine learning method for suppressing variation in position error using feedforward control | |
WO2013027250A1 (en) | Robot system, robot, and robot control device | |
JP2011167817A (en) | Robot having learning control function | |
JP2008036742A (en) | Inverse kinematics of sixth degree of freedom robot arm by successive retrieval method, and system, control method, and program for robot using the same | |
CN114466730B (en) | Motion planning for optimizing speed of a robot while maintaining limits on acceleration and jerk | |
CN115502966B (en) | Variable admittance control method for robot | |
KR101337650B1 (en) | Realtime weaving motion control device and realtime weaving motion control method | |
JP2019094176A (en) | Control device, control method, and pogram | |
JP2008194760A (en) | Robot arm and control method therefor | |
JP2002234700A (en) | Attitude angle control device for manipulator | |
JP4407083B2 (en) | Command value generation method and command value generation system | |
Lin et al. | Design and oscillation suppression control for cable-suspended robot | |
CN112428262A (en) | Parallel redundant flexible cable mechanism servo control method based on hyper-ellipsoid mapping analysis algorithm | |
JP2005034991A (en) | Working range limiting apparatus for robot and mobile robot | |
CN114536335B (en) | Control method and system of robot mechanical structure, electronic equipment and storage medium | |
JP2022114472A (en) | Direct teaching device and direct teaching method for robot | |
US20210162597A1 (en) | Multi axis robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20201007 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210929 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20211102 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20211217 |
|
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: 20220510 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20220527 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7082476 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |