JP7082477B2 - 制御装置、制御方法及びプログラム - Google Patents

制御装置、制御方法及びプログラム Download PDF

Info

Publication number
JP7082477B2
JP7082477B2 JP2017224829A JP2017224829A JP7082477B2 JP 7082477 B2 JP7082477 B2 JP 7082477B2 JP 2017224829 A JP2017224829 A JP 2017224829A JP 2017224829 A JP2017224829 A JP 2017224829A JP 7082477 B2 JP7082477 B2 JP 7082477B2
Authority
JP
Japan
Prior art keywords
boom
vector
speed
input
contraction
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.)
Active
Application number
JP2017224829A
Other languages
English (en)
Other versions
JP2019094177A (ja
Inventor
悠 吉田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Furukawa Co Ltd
Original Assignee
Furukawa Co Ltd
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 Furukawa Co Ltd filed Critical Furukawa Co Ltd
Priority to JP2017224829A priority Critical patent/JP7082477B2/ja
Publication of JP2019094177A publication Critical patent/JP2019094177A/ja
Application granted granted Critical
Publication of JP7082477B2 publication Critical patent/JP7082477B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control And Safety Of Cranes (AREA)
  • Forklifts And Lifting Vehicles (AREA)

Description

本発明は、制御装置、制御方法及びプログラムに関する。
特許文献1及び2には、油圧制御装置で油圧アクチュエータの駆動方向及び駆動速度を制御することで、ブームの先端に取り付けられた作業部の位置を制御する技術が開示されている。特許文献1には、操作レバーの傾動操作に基づき、作業部の移動方向並びに速度に関連する信号を発生し、発生させた作業部の移動方向及び速度を、三角関数を用いて起伏及び伸縮の移動方向及び速度に変換することが開示されている。
特許文献3には、ブームの先端位置を、超音波を発信する位置指令発信機の存する位置に追従移動させる技術が開示されている。
特公平7-29760 特許第4021530 特開2014-97875
本発明は、ブームの先端をなめらかに動かす技術を提供することを課題とする。
本発明によれば、
入力受付部と、
前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断部と、
前記判断部により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出部と、
前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
前記入力受付部が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
を有し、
前記動作ベクトル決定部は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、
移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御部は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置が提供される。
また、本発明によれば、
コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断工程と、
前記判断工程で超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出工程と、
前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
前記入力受付工程で受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
を実行し、
前記動作ベクトル決定工程では、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付工程では、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成工程では、
移動開始位置と前記入力受付工程で受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付工程で受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御工程では、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御方法が提供される。
また、本発明によれば、
コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断手段、
前記判断手段により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出手段、
前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
前記入力受付手段が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
として機能させ
前記動作ベクトル決定手段は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付手段は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成手段は、
移動開始位置と前記入力受付手段が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付手段が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御手段は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させないプログラムが提供される。
また、本発明によれば、
入力受付部と、
前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断部と、
前記判断部により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出部と、
前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
前記入力受付部が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
を有し、
前記動作ベクトル決定部は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成部は、
移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御部は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置が提供される。
また、本発明によれば、
コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断工程と、
前記判断工程で超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出工程と、
前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
前記入力受付工程で受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
を実行し、
前記動作ベクトル決定工程では、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付工程では、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成工程では、
移動開始位置と前記入力受付工程で受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付工程で受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御工程では、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御方法が提供される。
また、本発明によれば、
コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断手段、
前記判断手段により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出手段、
前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
前記入力受付手段が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
として機能させ
前記動作ベクトル決定手段は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
前記入力受付手段は、前記速度ベクトルを指定する入力を受付け、
前記速度ベクトル生成手段は、
移動開始位置と前記入力受付手段が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
第n(nは2以上の整数)の制御周期に対応して前記入力受付手段が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
前記アクチュエータ制御手段は、
前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させないプログラムが提供される。
本発明によれば、ブームの先端をなめらかに動かすことができる。
本実施形態の制御装置の機能ブロック図の一例である。 本実施形態の制御装置のハードウエア構成の一例を示す図である。 スプール変位、シリンダ速度特性を示す図である。 目標速度の時間変化に対するスプール変位の時間変化を示す図である。 各種変数及び定数の定義を示す図である。 作業機の構成の一例を示す図である。 本実施形態の制御装置が制御した場合のブームの先端の移動の一例を示す図である。 比較例のブームの先端の移動の一例を示す図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の制御装置が制御した場合のブームの先端の移動の一例を示す図である。 本実施形態の制御装置が制御した場合のブームの先端の移動の一例を示す図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 本実施形態の作用効果を説明するための図である。 判断部14がブームの状態に応じて閾値を変化させる処理を説明するための図である。 判断部14がブームの状態に応じて閾値を変化させる処理を説明するための図である。
<第1の実施形態>
図1に、本実施形態の制御装置10の機能ブロック図の一例を示す。図示するように、制御装置10は、入力受付部11と、速度ベクトル生成部12と、動作ベクトル決定部13と、判断部14と、算出部15と、アクチュエータ制御部16とを有する。
入力受付部11は、各種入力を受付ける。速度ベクトル生成部12は、入力受付部11が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する。動作ベクトル決定部13は、入力受付部11が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する。動作ベクトル決定部13は、例えば、速度ベクトル生成部12により生成された速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを算出する。
判断部14は、動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する。算出部15は、判断部14により超えると判断された場合、閾値を超えた動作の速度を閾値以下にする減速率を算出する。アクチュエータ制御部16は、判断部14により超えると判断された場合、動作ベクトルで示される伸縮、起伏及び旋回すべての速度を上記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
このような制御装置10により、ブームの先端をなめらかに動かす動作を実現する。以下、制御装置10の詳細を説明する。
まず、制御装置10のハードウエア構成の一例について説明する。本実施形態の制御装置10が備える各機能部は、任意のコンピュータのCPU(Central Processing Unit)、メモリ、メモリにロードされるプログラム、そのプログラムを格納するハードディスク等の記憶ユニット(あらかじめ装置を出荷する段階から格納されているプログラムのほか、CD(Compact Disc)等の記憶媒体やインターネット上のサーバ等からダウンロードされたプログラムをも格納できる)、ネットワーク接続用インターフェイスを中心にハードウエアとソフトウエアの任意の組合せによって実現される。そして、その実現方法、装置にはいろいろな変形例があることは、当業者には理解されるところである。
図2は、本実施形態の制御装置10のハードウエア構成を例示するブロック図である。図2に示すように、制御装置10は、プロセッサ1A、メモリ2A、入出力インターフェイス3A、周辺回路4A、バス5Aを有する。周辺回路4Aには、様々なモジュールが含まれる。なお、制御装置10は物理的及び/又は論理的に分かれた複数の装置で構成されてもよい。この場合、複数の装置各々が、プロセッサ1A、メモリ2A、入出力インターフェイス3A、周辺回路4A、バス5Aを有してもよい。
バス5Aは、プロセッサ1A、メモリ2A、周辺回路4A及び入出力インターフェイス3Aが相互にデータを送受信するためのデータ伝送路である。プロセッサ1Aは、例えばCPUやGPU(Graphics Processing Unit)などの演算処理装置である。メモリ2Aは、例えばRAM(Random Access Memory)やROM(Read Only Memory)などのメモリである。入出力インターフェイス3Aは、入力装置(例:キーボード、マウス、マイク、物理キー、タッチパネルディスプレイ、コードリーダ等)、外部装置、外部サーバ、外部センサ等から情報を取得するためのインターフェイスや、出力装置(例:ディスプレイ、スピーカ、プリンター、メーラ等)、外部装置、外部サーバ等に情報を出力するためのインターフェイスなどを含む。プロセッサ1Aは、各モジュールに指令を出し、それらの演算結果をもとに演算を行うことができる。
次に、本実施形態の制御装置10に制御されるブームを備える作業機について説明する。なお、作業機と制御装置10とは物理的及び/又は論理的に一体となって構成されてもよいし、物理的及び/又は論理的に分かれて構成されてもよい。
作業機は、ブームを伸縮させるアクチュエータ(例:ブーム伸縮シリンダ)、ブームを起伏させるアクチュエータ(例:ブーム起伏シリンダ)、及び、ブームを旋回させるアクチュエータ(例:ブーム旋回モータ)を有する。複数のアクチュエータの少なくとも1つ(例:複数のアクチュエータの全て)は油圧制御される。
ブームの動作は、制御弁及びスプールで制御される。すなわち、制御弁からの圧油の吐出量をスプールで変位させて調節することで、各アクチュエータへの圧油の供給量を調節する。各アクチュエータへの圧油の供給量を調節することで、各アクチュエータの動作方向及び速度を調節する。そして、各アクチュエータの動作を調節することで、ブームの先端の移動方向及び速度を調節する。
ここで、図3を用いて本実施形態の作業機が備えるスプールの特性を説明する。図3は、スプール変位とシリンダ速度の関係の一例を示す。この特性カーブに基づき、アクチュエータを所望の速度で動かすためのスプール変位を求めることができる。例えば、アクチュエータを定格の20%の速度で動かしたい場合はスプールの変位を最大変位の51%に設定する。また、アクチュエータを定格の80%の速度で動かしたい場合はスプールの変位を最大変位の94%に設定する。
ブームを伸縮させるアクチュエータ、ブームを起伏させるアクチュエータ、及び、ブームを旋回させるアクチュエータ各々の動作により、ブームの長さ(ブーム長)、起伏角度及び旋回角度が決定される。各アクチュエータの速度は、ブームの伸縮速度(単位時間当たりのブーム長の変化量)、起伏角速度(単位時間当たりのブームの起伏角の変化量)、及び、旋回角速度(単位時間当たりのブームの旋回角の変化量)に相当する。
なお、図3に示すように、制御弁のスプールはその中立付近の一定のストローク範囲内で「一定の不感帯域」を持つ。すなわち、スプール変位が不感帯に収まっている間、アクチュエータに圧油が供給されず、アクチュエータは動かない。アクチュエータに圧油を供給し、動かすためには、スプール変位が不感帯域の外に出なければならない。
図4に、線形で表したシリンダの目標速度の時間変化と、その目標速度を実現するためのスプール変位の時間変化との関係を示す。シリンダの目標速度は、所望の速度でブームの先端を移動させるためのシリンダの速度であり、一定速に安定後の終端速度と定義する。
また、作業機は、スプールの変位及び速度を検出する手段(差動トランス)を有する。さらに、作業機は、ブームの長さを検出するブーム長検出器、ブームの起伏角度を検出するブーム角度検出器、及び、ブームの旋回角度を検出する旋回角度検出器を有する。
次に、制御装置10について説明する。制御装置10は、アクチュエータの動作を制御する。図1の機能ブロック図に示すように、制御装置10は、入力受付部11と、速度ベクトル生成部12と、動作ベクトル決定部13と、判断部14と、算出部15と、アクチュエータ制御部16とを有する。
入力受付部11は、操作レバー(入力装置)を介して、ブームの先端を移動させる方向及び速度を指定する入力を受付ける。なお、入力受付部11は、タッチパネルディスプレイ、物理ボタン、マイク、キーボード、マウス等の他の入力装置を介して、ブームの先端を移動させる方向及び速度を指定する入力を受付けてもよい。
入力受付部11は、制御周期毎に、操作レバーの操作内容を検出し、検出した操作レバーの操作内容をブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、出力する。速度ベクトルは、作業機の所定位置を原点とし、作業機の所定方向をx軸(水平方向の軸)、y軸(水平方向の軸)及びz軸(垂直方向の軸)とした3次元座標系(以下、「作業機座標系」という場合がある)で示される。制御周期は、例えば1ミリ秒から100ミリ秒である。
入力受付部11は、操作レバーを傾ける方向に基づきブームの先端を移動させる方向を決定し、操作レバーを傾ける角度に基づきブームの先端を移動させる速度を決定する。例えば、予め、操作レバーを傾ける方向と、作業機座標系の所定の方向とが対応づけられていてもよい。また、予め、操作レバーを傾ける角度と、ブームの先端を移動させる速度とが対応付けられていてもよい。そして、入力受付部11は、当該対応関係に基づき、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換してもよい。例えば、2つの操作レバーを用意し、第1の操作レバーで水平方向の移動方向及び速度を指定し、第2の操作レバーで垂直方向の移動方向及び速度を指定できてもよい。
速度ベクトル生成部12は、制御周期毎に、入力受付部11が受付けた入力内容に基づき、伸縮、起伏及び旋回可能なブームの先端を移動させる方向及び速度を3次元座標系(作業機座標系)で示す速度ベクトルを生成し、出力する。以下、速度ベクトル生成部12により生成された速度ベクトルを、指示速度ベクトルという。
ここで、指示速度ベクトルの生成例を説明する。
「指示速度ベクトル生成例1」
速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルを、そのまま指示速度ベクトルとすることができる。
「指示速度ベクトル生成例2」
速度ベクトル生成部12は、指示速度ベクトルの移動方向として、入力受付部11により出力された速度ベクトルで示される移動方向をそのまま採用することができる。そして、指示速度ベクトルの速度を、以下のようにして決定してもよい。
「(ブームの先端の現在の速度)<(入力受付部11により出力された速度ベクトルで示される速度)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)+(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
ブームの動作を開始した直後等はブームの先端の速度が小さいため、当該条件を満たしやすくなる。この場合、上述のように指示速度ベクトルの速度を決定することで、急激な速度上昇を抑制し、滑らかな動作が実現される。
一方、「(ブームの先端の現在の速度)≧(入力受付部11により出力された速度ベクトルで示される速度)」を満たす場合、速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルで示される速度を指示速度ベクトルの速度として採用してもよい。ブームの動作を開始してからある程度の時間が経過し、ブームの先端の移動速度が安定している頃には当該条件を満たしやすくなる。
動作ベクトル決定部13は、速度ベクトル生成部12により出力された指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(以下の式(2)、(14)参照)。なお、動作ベクトル決定部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、指示速度ベクトルを動作ベクトルに変換してもよい(以下の式(15)参照)。
ここで、ヤコビ行列の逆行列について説明する。
ブームに伸縮、起伏、旋回を実行させると、ブームの先端が所定の方向に所定の速度で移動する。すなわち、ブームの先端に速度ベクトルが生じる。伸縮、起伏、旋回の中の複数を同時に行った場合は、各動作によってブームの先端に生じる速度ベクトルを足し合わせることによって、ブームの先端の速度ベクトルを求めることができる。
ブームの動作ベクトルと、ブームの先端の速度ベクトルとは、ヤコビ行列Jを用いて式(1)のように示される。
Figure 0007082477000001
xの上に黒点を付した文字は、作業機座標系におけるブームの先端のx軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「x´」と表す。yの上に黒点を付した文字は、作業機座標系におけるブームの先端のy軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「y´」と表す。zの上に黒点を付した文字は、作業機座標系におけるブームの先端のz軸方向の移動の速度成分を示す。明細書中では、当該速度成分を「z´」と表す。
の上に黒点を付した文字は、ブームの旋回の角速度(旋回角の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q´」と表す。qの上に黒点を付した文字は、ブームの起伏の角速度(起伏角の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q´」と表す。qの上に黒点を付した文字は、ブームの伸縮速度(ブーム長の単位時間当たりの変化量)を示す。明細書中では、当該速度成分を「q´」と表す。
ブームの伸縮、起伏、旋回の速度とブームの先端の速度の自由度が同じ(Jが正方行列)であり、かつ、ブームが特異姿勢でないとき、逆行列J-1を用いることで、式(2)に示すようにブームの先端の速度ベクトルから動作ベクトルを求めることができる。なお、ブームの特異姿勢とは、例えばブームが真上や真下を向いた状態をいう。
Figure 0007082477000002
行列計算を展開すると、式(3)のようになる。
Figure 0007082477000003
ここで、逆行列J-1を求めると以下のようになる。
「同次変換行列を使った順運動学計算(q、q、q→x、y、z)」
まず、図5及び式(4)に示すように変数及び定数を定義する。Houtrigger、Hcolumn、Dcolumn、及び、Dboomは、作業機の構成に基づき定まる固定値である。
Figure 0007082477000004
基準座標系→ターンテーブル座標系の同次変換行列は、式(5)のようになる。基準座標系は、図5に示すΣ座標系である。コラム回転中心直下の地面を原点とし、車両左側方をX軸方向、車両後方をY軸方向、鉛直上方をZ軸方向としている。ターンテーブル座標系は、図5に示すΣ座標系であり、ターンテーブル中央を原点とし、ブームの方位角がY軸と一致するよう基準座標系を並進移動およびZ軸周りに回転したものである。式(5)は、Z軸方向にHoutrigger並進移動し、Z軸回りにq回転することを意味する。
Figure 0007082477000005
ターンテーブル座標系→コラム座標系の同次変換行列は、式(6)のようになる。コラム座標系は、図5に示すΣ座標系であり、ブームフートピン中央を原点とし、ブーム長手方向がY軸方向と一致するようターンテーブル座標系を並進移動およびX軸周りに回転したものである。式(6)は、Z軸方向にHoutrigger、Y軸方向に-Dcolumn並進移動し、X軸回りにq回転することを意味する。
Figure 0007082477000006
コラム座標系→トップシーブ座標系の同次変換行列は、式(7)のようになる。トップシーブ座標系は、図5に示すΣ座標系であり、トップシーブ中央が原点となるようコラム座標系を並進移動したものである。式(7)は、Z軸方向に-Dboom、Y軸方向にq並進移動することを意味する。
Figure 0007082477000007
基準座標系→トップシーブ座標系の同次変換行列は、式(8)のようになる。
Figure 0007082477000008
式(8)を使ってトップシーブ座標系の原点(0、0、0)を基準座標系で表すと、式(9)のようになる。
Figure 0007082477000009
式(9)の各係数を計算し展開すると、式(10)のようになる。
Figure 0007082477000010
「ヤコビ行列の逆行列を使った逆運動学計算(x´、y´、z´→q´、q´、q´)
x、y、zをそれぞれ時間微分してq´、q´、q´についてまとめると、式(11)のようになる。
Figure 0007082477000011
これを行列表記にすると、式(12)のようになる。
Figure 0007082477000012
両辺に左からJ-1を掛けて、左辺及び右辺を入れ替えると式(13)のようになる。
Figure 0007082477000013
-1を計算すると、式(14)のようになる。
Figure 0007082477000014
式(13)のJ-1に式(14)を代入して展開すると、式(15)のようになる。式(15)を利用することで、ブームの先端を所定の方向に所定の速度で移動させるための複数のアクチュエータ各々の動作方向及び速度(動作ベクトル)が求まる。
Figure 0007082477000015
図1に戻り、判断部14は、動作ベクトル決定部13により決定された動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する。閾値は、伸縮、起伏及び旋回各々の許容される最大速度である。
算出部15は、動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが閾値を超えると判断された場合、閾値を超えた動作の速度を閾値以下にする減速率を算出する。算出部15は、例えば、(減速率)=(動作の閾値)/(動作ベクトルで示されるその動作の速度)に基づき算出してもよい。算出部15は、算出した減速率をアクチュエータ制御部16に入力する。
なお、複数の動作の速度が閾値を超えた場合、算出部15は閾値を超えた動作毎に減速率を算出する。そして、算出部15は、その中の最小値(減速の幅が最も大きい減速率)をアクチュエータ制御部16に入力することができる。
アクチュエータ制御部16は、動作ベクトル決定部13により決定された動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが閾値を超えると判断された場合、当該動作ベクトルで示される伸縮、起伏及び旋回すべての速度を、算出部15より入力された減速率で減速した減速後動作ベクトルを算出する。そして、アクチュエータ制御部16は、減速後動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
一方、アクチュエータ制御部16は、動作ベクトル決定部13により決定された動作ベクトルで示される伸縮、起伏及び旋回各々の速度のいずれも閾値を超えないと判断された場合、当該動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
アクチュエータ制御部16は、動作ベクトル又は減速後動作ベクトルと、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、動作ベクトル又は減速後動作ベクトルで示される方向に動作ベクトル又は減速後動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、求めた通りにスプールの変位を設定する。
ここで、参考までに、図6に、本実施形態の制御装置10の構成の一例を示す。なお、あくまで一例であり、これに限定されない。
次に、図7を用いて、本実施形態の制御装置10による処理の流れの一例及びそれに応じたブームの先端の移動を説明する。図7は、ブームを上方から観察した模式図である。ブームの先端は(1)→(2)→(3)→(4)の順に移動するものとする。1回の制御周期で(1)→(2)の変化がおき、次の制御周期で(2)→(3)の変化がおき、次の制御周期で(3)→(4)の変化がおきるものとする。なお、これらの前提は、以下で説明する図8、図12及び図13も同様である。
図7において、ブームの先端の位置が(1)で停止しているとき、入力受付部11は、最初の制御周期(第1の制御周期)の指示入力待ち状態となっている。そして、オペレータにより操作レバーを所定の方向に所定角度傾ける操作がなされると、入力受付部11は操作レバーの操作内容を検出する。そして、入力受付部11は、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、第1の制御周期の速度ベクトルとして出力する。ここでは、(1)のブームの先端から延びる矢印で示される方向の速度ベクトルが出力されたものとする。
速度ベクトル生成部12は、上述した指示速度ベクトル生成例1乃至3の何れかに基づき、第1の制御周期の指示速度ベクトルを生成し、出力する。
その後、動作ベクトル決定部13は、指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、第1の制御周期の指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(式(2)、(14)参照)。なお、動作ベクトル決定部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、第1の制御周期の指示速度ベクトルを動作ベクトル(第1の制御周期の動作ベクトル)に変換してもよい(式(15)参照)。
その後、判断部14は、第1の制御周期の動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する。
第1の制御周期の動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが閾値を超えると判断された場合、算出部15は、閾値を超えた動作の速度を閾値以下にする減速率を算出する。そして、算出部15は、算出した減速率をアクチュエータ制御部16に入力する。
なお、複数の動作の速度が閾値を超えた場合、算出部15は閾値を超えた動作毎に減速率を算出する。そして、算出部15は、その中の最小値(減速の幅が最も大きい減速率)をアクチュエータ制御部16に入力することができる。
その後、アクチュエータ制御部16は、第1の制御周期の動作ベクトルで示される伸縮、起伏及び旋回すべての速度を、算出部15より入力された減速率で減速した第1の制御周期の減速後動作ベクトルを算出する。そして、アクチュエータ制御部16は、第1の制御周期の減速後動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
すなわち、アクチュエータ制御部16は、「第1の制御周期の減速後動作ベクトル」と、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、第1の制御周期の減速後動作ベクトルで示される方向に当該減速後動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、求めた通りにスプールの変位を設定する。
一方、第1の制御周期の動作ベクトルで示される伸縮、起伏及び旋回各々の速度のいずれも閾値を超えないと判断された場合、アクチュエータ制御部16は、第1の制御周期の動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
すなわち、アクチュエータ制御部16は、「第1の制御周期の動作ベクトル」と、予め保持している「スプール変位とアクチュエータの速度との関係を示す情報(例:関数、テーブル等)」とに基づき、第1の制御周期の動作ベクトルで示される方向に当該動作ベクトルで示される速度で動かすためのスプール変位を求める。そして、求めた通りにスプールの変位を設定する。
結果、第1の制御周期の間ブームの先端は移動し、第1の制御周期後のブームの先端の位置は図示する(2)になる。なお、図示するように、ブームの動作の誤差等により、指示速度ベクトルで示される方向からズレが発生する場合がある。図の場合、図中の上下方向のズレが発生している。
入力受付部11は、第1の制御周期に対応する操作レバーの操作を検出した後の任意のタイミング以降に、次の制御周期(第2の制御周期)の指示入力待ち状態となる。そして、オペレータにより操作レバーを所定の方向に所定角度傾ける操作がなされると、入力受付部11は操作レバーの操作内容を検出する。そして、入力受付部11は、検出した操作レバーの操作内容を、ブームの先端を移動させる方向及び速度を示す速度ベクトルに変換し、第2の制御周期の速度ベクトルとして出力する。なお、オペレータは、操作レバーを継続的に所定の方向に所定の角度で傾け続けておいてもよい。
その後、速度ベクトル生成部12は、上述した指示速度ベクトル生成例1乃至3の何れかに基づき、第1の制御周期の指示速度ベクトルを生成し、出力する。
その後、動作ベクトル決定部13は、指示速度ベクトルに左からヤコビ行列の逆行列を掛けることで、第2の制御周期の指示速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する(式(2)、(14)参照)。なお、動作ベクトル決定部13は、ヤコビ行列の逆行列を用いて得られた算出式を用いて、第2の制御周期の指示速度ベクトルを動作ベクトル(第2の制御周期の動作ベクトル)に変換してもよい(式(15)参照)。
その後、判断部14は、第2の制御周期の動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する。
第2の制御周期の動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが閾値を超えると判断された場合、算出部15は、閾値を超えた動作の速度を閾値以下にする減速率を算出する。そして、算出部15は、算出した減速率をアクチュエータ制御部16に入力する。
なお、複数の動作の速度が閾値を超えた場合、算出部15は閾値を超えた動作毎に減速率を算出する。そして、算出部15は、その中の最小値(減速の幅が最も大きい減速率)をアクチュエータ制御部16に入力することができる。
その後、アクチュエータ制御部16は、第2の制御周期の動作ベクトルで示される伸縮、起伏及び旋回すべての速度を、算出部15より入力された減速率で減速した第2の制御周期の減速後動作ベクトルを算出する。そして、アクチュエータ制御部16は、第2の制御周期の減速後動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
結果、第2の制御周期の間ブームの先端は移動し、第2の制御周期後のブームの先端の位置は図示する(3)になる。なお、図示するように、ブームの動作の誤差等により、指示速度ベクトルで示される方向からズレが発生する場合がある。図の場合、図中の上下方向のズレが発生している。
以降、同様の処理を繰り返す。なお、制御周期は例えば1ミリ秒から100ミリ秒である。
次に、本実施形態の制御装置10の作用効果を説明する。
「第1の作用効果」
まず、ブームの先端を目的地まで直線的に動かす制御方法として、ブームの先端の位置を繰り返し制御する手段(位置フィードバック)が考えられる。これは、本実施形態の制御手法と異なる手段である。
位置フィードバックによる制御を行う場合、例えば、ブームの先端の現在位置と目標位置を比較し、現在位置が目標位置に到達していない場合は正の出力を出し、現在位置が目標位置を超えた場合は負の出力を出すことで、目標位置と現在位置を一致させる。目標位置は、例えば、操作レバーの操作内容(傾ける方向、傾ける角度)に基づき、スプール変位から演算される。
ここで、位置フィードバックで、図8のように旋回と伸縮でブームの先端を図の右方向に移動し続ける場合を考える。なお、この条件における基本動作方向は旋回が「右」、伸縮は「伸」である。「基本動作方向」は、操作レバーの操作内容で指定されたブームの先端の移動(図8の場合、右方向)を実現するためのブームの動作方向である。
位置フィードバックの場合、図示するように制御周期毎に目標位置を定め、制御周期毎にブームの先端の位置を目標位置と一致させる。現在位置が目標位置に到達していない場合は正の出力を出し、現在位置が目標位置を超えた場合は負の出力を出すことで、目標位置と現在位置を一致させる。
(2)乃至(4)各々の現在位置→目標位置の補正は次の通りである。(2)では、旋回補正は「左」、伸縮補正は「縮」となる。(3)では、旋回補正は「左」、伸縮補正は「伸」となる。(4)では、旋回補正は「右」、伸縮補正は「縮」となる。
位置フィードバックの場合、このように、タイミング毎に各動作の補正の方向が反転する場合がある。また、上述の通り基本動作方向は「旋回:右、伸縮:伸」に対して、補正の方向が基本動作方向から反転している場合がある。
位置制御の性能を向上させるために制御周期を短くした場合や、補正ゲインを大きくした場合、また、基本動作による移動速度が遅い場合は、基本動作方向に対する補正方向が反転する可能性が高くなり、また、制御周期毎に補正方向が反転する可能性も高まる。また、補正のために目標速度は微小なものとなる。ここで、図9に、微小な目標速度が発生した場合にこの目標速度を満足するためのスプール変位を示す。
図9に示すように目標速度が0近傍で細かく変化した場合、目標速度の符号に併せて正側の閾値と負側の閾値を超えるように目標スプール位置が激しく変化する。なお、正側の閾値及び負側の閾値は、各々、図3に示す不感帯の上限及び下限である。産業用マニピューレタのように交流電流を使用しての電気制御の場合は印加電圧やインバータの周波数などを激しく変化させることが可能だが、スプールを介して油圧を制御する場合はスプールの応答速度を超えるためスプール変位に応答遅れやオーバーシュートが発生する(図10参照)。応答遅れとオーバーシュートが生じたスプールに対して図3の特性が現れるため、実際のアクチュエータの速度は図11のように振動的になる。
図11のように動作方向が激しく入れ替わるような内容でアクチュエータを動作させた場合、例えばブームがしなるなどさらに大きなオーバーランが発生し、制御系が発振する原因となる。
これに対し、本実施形態では、図7を用いて説明したように、ブームの先端の速度をベースとした制御を行う。ブームの先端を右に動かし続けるために、単位時間(1回の制御周期)当たりにおいてブームの先端で右方向に速度が発生するような旋回及び伸縮の速度を求め、これを満たすように制御する。ブームの先端の速度を満たす旋回及び伸縮の速度はブームの現姿勢によるため、制御周期毎に更新することができる。
ブームの姿勢が大きく変わらない限り、ブームの先端の目標速度に対する旋回及び伸縮の目標速度は大きく変化せず、基本動作方向も変わらない。したがって、例えば旋回及び伸縮の速度が目標速度を超えた場合は自動車のアクセル制御のように減速することで補正され、旋回及び伸縮の動作方向の符号を変える必要はない。スプール位置についても、速度0~最大速度の範囲で制御し、反転を考慮する必要がないため、不感帯から最大速度の範囲で制御するだけでよい。目標速度が急激に変化しない限り、スプール変位量のフィードバック値もずれが発生せず、安定した制御をすることができる。
「第2の作用効果」
速度ベースで制御するための速度の算出手段として、制御周期毎の目標位置を定め、現在位置と目標位置との差分により速度を求める手段(比較例の算出手段)が考えられる。これは、本実施形態の手段と異なる手段である。
しかし、比較例の算出手段の場合、浮動小数点型の演算における微小な計算誤差による影響が問題となる。例えば、ブームの先端を垂直に上昇させたい場合、旋回を行う必要がない。しかし、比較例の算出手段の場合、浮動小数点型の演算における丸め込み等の誤差により、動作不要な旋回に対して符号が頻繁に変化する形で微小な目標速度が発生する。計算誤差の他に、ブームのガタ等により旋回位置に微小量分の狂いが生じた場合にも微小な目標速度が発生する可能性がある。符号の変化を伴う微小な目標速度は制御弁の制御に悪影響を与える。なお、他の動作に対しても浮動小数点型の演算における誤差は発生するが、基本動作速度に対して微小であるため大きな影響はない。
また、比較例の算出手段で旋回や起伏の角速度を求める場合、平方根を用いて演算する必要がある。このため、計算コストが大きくなる。
これに対し、本実施形態の制御装置10は、式(14)に示すヤコビ行列の逆行列に基づいて、ブームの先端を移動させる方向及び速度を示す速度ベクトルを、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルに変換する。
式(14)に示すヤコビ行列の逆行列の各要素は三角関数の四則演算で構成され、平方根が含まれていない。このため、各要素を求めるための計算コストは大きくならない。また、式(15)に示すように、ヤコビ行列の逆行列を用いて得られた算出式も平方根を含まない。このため、式(15)に基づく計算コストも大きくならない。
また、式(15)より、x´及びy´を0とすると、旋回の速度ベクトルを示すq´は0となる。すなわち、クレーンの先端を垂直方向に移動させる場合には、旋回の速度ベクトルを0にできる。このため、比較例の算出手段のように演算誤差による微小な目標速度が発生する不都合を回避できる。
また、式(15)より、積と和によって動作ベクトルが求まるため、計算コストが小さい。また、動作ベクトルの解が一意に求まる。
このような本実施形態の制御装置10によれば、ブームの先端を目的地まで直線的にかつなめらかに動かすことができる。
「第3の効果」
また、本実施形態の場合、動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが閾値を超える場合、閾値を超えないように速度を補正し、補正後の速度でアクチュエータを動作させることができる。そして、当該補正においては、閾値を超える動作のみを補正するのでなく、すべての動作を同じ減速率で減速することができる。
所定のルールで決定した動作ベクトルで示される伸縮、起伏及び旋回各々の速度のうち、一部の速度を変更すると、ブームが予期せぬ動きをする。結果、ブームの先端のなめらかな動きが妨げられる。動作ベクトルで示される伸縮、起伏及び旋回各々の速度のうち一部の速度を変更する必要がある場合においても、すべての動作の速度を同じ減速率で減速する本実施形態によれば、このような状況下でもブームの先端のなめらかな動きが確保される。
<第2の実施形態>
本実施形態の制御装置10は、判断部14及び算出部15の構成が第1の実施形態と異なる。その他の構成は、第1の実施形態と同様である。また、作業機の構成も第1の実施形態と同様である。
判断部14は、動作ベクトル決定部13により決定された動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する。各動作に必要な圧油の流量は、任意の手段で算出できる。例えば、各動作に必要な圧油の流量は、各動作の速度(動作ベクトルで示される速度)に、シリンダの断面積等に基づき決定された係数を掛けることで求めてもよい。判断部14は、各動作に必要な圧油の流量を算出し、その合計が閾値を超えないか判断する。閾値は、例えば油圧ポンプの最大吐出量である。
算出部15は、判断部14により超えると判断された場合、油圧ポンプの吐出量を閾値以下にする減速率を算出する。算出部15は、例えば、(減速率)=(閾値)/(動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量)に基づき算出してもよい。算出部15は、算出した減速率をアクチュエータ制御部16に入力する。
以上説明した本実施形態によれば、第1の実施形態と同様な作用効果を実現できる。また、動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超える場合、動作ベクトルで示される伸縮、起伏及び旋回すべての速度を同じ減速率で減速し、油圧ポンプの吐出量を閾値以内にすることができる。本実施形態によれば、このような状況下でもブームの先端のなめらかな動きが確保される。
<第3の実施形態>
本実施形態の制御装置10は、速度ベクトル生成部12の構成が第1及び第2の実施形態と異なる。その他の構成は、第1及び第2の実施形態と同様である。また、作業機の構成も第1及び第2の実施形態と同様である。
第1及び第2の実施形態の場合、図7を用いて説明したように、ブームの先端の移動方向が目標とする方向からずれる可能性があった。本実施形態の速度ベクトル生成部12は、当該ずれを補正するように、指示速度ベクトルを生成する。
まず、速度ベクトル生成部12は、入力受付部11により出力された速度ベクトルに基づき、制御周期毎に、ブームの先端の目標位置を決定する。目標位置は、作業機座標系で示される。
例えば、速度ベクトル生成部12は、移動開始時点のブームの先端の位置情報(作業機座標系で示すもの)を保持しておき、当該位置を起点として入力受付部11により出力された速度ベクトルで示される方向に当該速度ベクトルで示される速度で制御周期の間移動した後の位置を、最初の制御周期の目標位置として決定する。
その後は、速度ベクトル生成部12は、第n-1の制御周期の目標位置から第nの制御周期に対応して入力受付部11により出力された速度ベクトルで示される方向に当該速度ベクトルで示される速度で制御周期の間移動した後の位置を、第nの制御周期の目標位置として決定する(nは2以上の整数)。
そして、速度ベクトル生成部12は、第nの制御周期に対応して入力受付部11が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後のブームの先端の位置から第n-1の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における指示速度ベクトルとして生成する。第1の速度ベクトルは、第nの制御周期に対応して入力受付部11が受付けた入力内容に基づき、上述した指示速度ベクトル生成例1乃至3の何れかに基づき生成された指示速度ベクトルである。
図12を用いて具体例を説明する。移動開始時点のブームの先端の位置を(1)とし、第1の制御周期後の状態を(2)、第2の制御周期後の状態を(3)、第3の制御周期後の状態を(4)とする。
(2)より、第1の制御周期の目標位置に対して、第1の制御周期後の現在位置が異なっていることが分かる。速度ベクトル生成部12は、第2の制御周期の指示速度ベクトルとして、第2の制御周期に対応して決定した第1の速度ベクトルと、第1の制御周期後のブームの先端の位置から第1の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを決定する。
同様に、(3)より、第2の制御周期の目標位置に対して、第2の制御周期後の現在位置が異なっていることが分かる。速度ベクトル生成部12は、第3の制御周期の指示速度ベクトルとして、第3の制御周期に対応して決定した第1の速度ベクトルと、第2の制御周期後のブームの先端の位置から第2の制御周期のブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを決定する。
図より、合成ベクトルは、ブームの先端位置を目標軌道上に戻す方向のベクトルとなる。なお、速度ベクトル生成部12は所定の係数を掛けた第2の速度ベクトルと第1の速度ベクトルとを足し合わせた合成ベクトルを、指示速度ベクトルとしてもよい。係数を調整することで、目標軌道上に戻す補正の大きさを調整できる。係数は、予め与えられていてもよい。
次に、本実施形態の変形例を説明する。アクチュエータ制御部16は、上記合成ベクトルを変換した動作ベクトルで示されるブームの伸縮、起伏及び旋回の内、上記第1の速度ベクトルを変換した第1の動作ベクトルで示されるブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、動作方向が異なる動作は実行させないように制御してもよい。
第1の動作ベクトルは、第1の速度ベクトルに左からヤコビ行列の逆行列を掛けることで(式(2)、(14)参照)、又は、ヤコビ行列の逆行列を用いて得られた算出式を用いて(式(15)参照)、第1の速度ベクトルを変換した動作ベクトルである。
次に、本実施形態の作用効果を説明する。本実施形態の制御装置10は、第1及び第2の実施形態と同様の作用効果を実現できる。
また、図7に示す第1及び第2の実施形態の制御の場合、位置誤差が蓄積する可能性がある。上述した合成ベクトルを指示速度ベクトルとする本実施形態の制御装置10によれば、位置誤差を軽減することができる。
なお、目標軌道上を厳密に動かしたい場合は、現時点で生じている位置誤差を直ちに(例:次制御周期で)解消する必要がある。しかし、当該制御を行った場合、ブームの各動作の動作方向や速度が大きく変化する可能性がある。結果、ブームのなめらかな動きが実現されなくなる。
本実施形態の場合、位置誤差を直ちに(例:次制御周期で)解消するのでなく、複数回の制御周期で位置誤差を少しずつ解消するように制御する。このため、位置誤差を解消しつつ、ブームのなめらかな動きを実現できる。
また、第2の速度ベクトルで位置補正を行う本実施形態の場合、オペレータの入力内容に基づき決定される各アクチュエータの動作方向と、動作ベクトルで示される各アクチュエータの動作方向とが反転する可能性がある。しかし、上記変形例を採用することで、オペレータの入力内容に基づき決定される各アクチュエータの動作方向と異なる方向に動作させる不都合を回避できる。結果、ブームのなめらかの動きが実現される。
<第4の実施形態>
本実施形態の制御装置10は、入力受付部11及び速度ベクトル生成部12の構成が第1乃至第3の実施形態と異なる。その他の構成は、第1乃至第3の実施形態と同様である。また、作業機の構成も第1乃至第3の実施形態と同様である。
入力受付部11は、ブームの先端の最終目標位置を指定する入力を受付ける。最終目標位置は、作業機座標系で示される。例えば、特許文献3に記載の技術を利用してもよい。すなわち、作業機に複数の超音波受信ユニットを設置し、最終目標位置に超音波発信機を設置する。入力受付部11は、超音波発信時刻と超音波受信時刻の時間差の入力を受付ける。そして、入力受付部11は、入力された情報に基づき、ブームの先端の最終目標位置を算出し、出力する。
速度ベクトル生成部12は、制御周期毎に、ブームの先端の現在位置から最終目標位置に向かう指示速度ベクトルを生成する。
すなわち、速度ベクトル生成部12は、各制御周期後のブームの先端の現在位置を取得する。当該現在位置は、作業機座標系で示される。そして、速度ベクトル生成部12は、第nの制御周期の指示速度ベクトルの移動方向として、第n-1の制御周期後のブームの現在位置から最終目標位置に向かう方向を決定する。図13に本実施形態の制御の概念図を示す。ブームの先端の現在位置から最終目標位置に向かう指示速度ベクトルが生成されている。
ここで、指示速度ベクトルの速度の決定例を説明する。
「指示速度ベクトルの速度決定例1」
予め、固定値が与えられていてもよい。そして、速度ベクトル生成部12は当該固定値を、指示速度ベクトルの速度としてもよい。
「指示速度ベクトルの速度決定例2」
入力受付部11が速度を指定する入力を、任意の手段でオペレータから受付けてもよい。そして、速度ベクトル生成部12は、当該入力の内容に基づき、指示速度ベクトルの速度を決定してもよい。
「指示速度ベクトルの速度決定例3」
「(最終目標位置とブームの現在位置との間の距離)≦(閾値)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)-(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
ブームの先端が最終目標位置に近づいている場合、当該条件を満たすこととなる。この場合、上述のように指示速度ベクトルの速度を決定することで、最終目標位置に到達するタイミングが近付くと、徐々に速度を減少させることができる。結果、滑らかな動作が実現される。
一方、「(最終目標位置とブームの現在位置との間の距離)>(閾値)」を満たす場合、速度ベクトル生成部12は、速度ベクトル生成部12は、指示速度ベクトルの速度決定例1又は2で、指示速度ベクトルの速度を決定してもよい。最終目標位置までの距離がある程度大きい場合、当該条件を満たすこととなる。
「指示速度ベクトルの速度決定例4」
「(ブームの先端の現在の速度)<(指示速度ベクトルの速度決定例1又は2で決定された速度)」を満たす場合、速度ベクトル生成部12は、「(指示速度ベクトルの速度)=(ブームの先端の現在の速度)+(加速度)×(制御周期分の時間)」で算出してもよい。加速度は、予め与えられる固定値であってもよい。
ブームの動作を開始した直後等はブームの先端の速度が小さいため、当該条件を満たしやすくなる。この場合、上述のように指示速度ベクトルの速度を決定することで、急激な速度上昇を抑制し、滑らかな動作が実現される。
一方、「(ブームの先端の現在の速度)≧(指示速度ベクトルの速度決定例1又は2で決定された速度)」を満たす場合、速度ベクトル生成部12は、指示速度ベクトルの速度として、指示速度ベクトルの速度決定例1又は2で決定された速度を決定してもよい。ブームの動作を開始してからある程度の時間が経過し、ブームの先端の移動速度が安定している頃には当該条件を満たしやすくなる。
閾値は、例えば、「現在速度から一定の減速度で減速し、停止するまでに要する距離」であってもよい。一例として、「(閾値)=(現在速度)×(現在速度から一定の減速度で減速した場合に停止するまでに要する時間)/2」で算出してもよい。または、「(閾値)=(現在速度)×(現在速度)/(2×減速度)」で算出してもよい。なお、「現在速度から一定の減速度で減速した場合に停止するまでに要する時間」は、「(現在速度から一定の減速度で減速した場合に停止するまでに要する時間)=(現在速度)/(減速度)」で算出してもよい。
本実施形態によれば、第1乃至第3の実施形態と同様の作用効果を実現できる。また、本実施形態によれば、制御周期毎にブームの先端の現在位置を取得し、そこから最終目標位置に向かう指示速度ベクトルを生成することができる。このため、移動方向が最終目標位置に向かう方向から大きく外れることなく、ブームの先端をなめらかに移動させることができる。
<第5の実施形態>
本実施形態の制御装置10は、判断部14の構成が第1乃至第4の実施形態と異なる。その他の構成は、第1乃至第4の実施形態と同様である。また、作業機の構成も第1乃至第4の実施形態と同様である。
前の実施形態(例えば、第1の実施形態)で説明した通り、判断部14は、動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する。また、前の他の実施形態(例えば、第2の実施形態)で説明した通り、判断部14は、動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する。
そして、判断部14は、ブームの状態に応じて上記閾値を変化させることができる。例えば、判断部14は、ブームの状態と閾値との関係を示す情報(例:関数、テーブル等)を予め保持しておく。そして、判断部14は、現在のブームの状態と、上記情報とに基づき現在の閾値を決定し、上述したように閾値を超えるか否かの判断を行う。
例えば、判断部14は、ブームストロークに応じて、ブームの伸縮動作に対応する閾値(速度の閾値、吐出量の閾値)を変化させてもよい。具体的には、ブームが伸び切るタイミング、ブームが縮み切るタイミング、又、複数のシリンダを順番に動作させてブーム長を切り替える構成の場合には動作させるシリンダを切り替えるタイミングの閾値を、他のタイミングの閾値よりも小さくすることができる。
また、判断部14は、ブームの旋回角度に応じて、ブームの旋回動作に対応する閾値(速度の閾値、吐出量の閾値)を変化させてもよい。具体的には、ブームの旋回角度が可動域限界になるタイミングの閾値を、他のタイミングの閾値よりも小さくすることができる。
動作速度は閾値以下に抑制されるので、閾値を小さくしたタイミングは他のタイミングよりも動作速度が小さくなる傾向となる。ところで、ブームが伸び切るタイミング、ブームが縮み切るタイミング、又、シリンダが切り替わるタイミングには振動する可能性がある。このようなタイミングの動作速度を小さくすることで、振動を抑制することができる。また、ブームの旋回角度が可動域限界になるタイミングには振動する可能性がある。このようなタイミングの動作速度を小さくすることで、振動を抑制することができる。
ここで、図20に、ブームストロークに応じてブームの伸縮動作に対応する閾値を変化させる例を示す。図示する例では、ブームストローク0m付近、3m付近、8m付近の閾値を、他の状態の閾値よりも小さくしている。ブームストローク0m付近は、ブームが縮み切るタイミングである。ブームストローク3m付近は、シリンダが切り替わるタイミングである。ブームストローク8m付近は、ブームが伸び切るタイミングである。
図示する例の場合、ブームストローク0m及び8mでは、閾値(シリンダ速度)を最大速度の20%としている。また、ブームストローク3mでは、閾値(シリンダ速度)を最大速度の35%としている。
例えば、動作ベクトル決定部13により決定された伸縮動作のシリンダ速度が最大速度の70%であり、その時のブームストロークが0mであったとする。この場合、判断部14は、図20より閾値(シリンダ速度)を最大速度の20%と決定し、決定された伸縮動作のシリンダ速度は閾値を超えると判断する。その後、算出部15は、0.28倍(=20/70)の減速率を算出する。そして、アクチュエータ制御部16は、動作ベクトルで示される伸縮、起伏及び旋回すべての速度を0.28倍の減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる。
ちなみに、ブームストロークが0mから8mの間ずっと、動作ベクトル決定部13により決定された伸縮動作のシリンダ速度が最大速度の70%であった場合、伸縮動作のシリンダ速度は判断部14と算出部15とにより図21の実線のように決定されることとなる。
本実施形態によれば、第1乃至第3の実施形態と同様の作用効果を実現できる。
ここで、第1乃至第5の実施形態に共通の作用効果をさらに説明する。本実施形態の制御装置10によれば、全ての軸の速度に同じ減速率をかけることで、ブームの先端の移動方向を維持したまま速度を制限できる。移動方向を維持したまま速度を制限することができるため、速度超過時の減速や、各軸の可動範囲の限界、伸縮シリンダ切換時の振動を防ぐための減速を行う事でなめらかに動かす事ができる。
図14乃至図16を用いて、ブーム動作範囲限界における減速動作時の作用効果を説明する。図14に示すブームの先端の「意図する移動方向」を実現する場合、ブームを伸長し、かつ、図中右方向に旋回させることとなる。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。
ここで、ブーム伸長の制限に基づくブーム先端の到達可能範囲が図示するようになるとする。ブーム伸長動作の限界に近づくとブーム伸長動作を減速させる必要があるが、ブーム伸長動作のみを減速させると(すなわち、図中右方向の旋回動作は減速させない)、ブームの先端は図15に示すように移動することとなる。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。また、点線矢印と実線矢印の長さの違いを用いて、図中最も右側にブームが位置する際のブームの伸長動作の減速を示している。
図15に示すように、ブームの伸長動作のみを減速した場合のブームの先端の移動方向は、ブームの伸長動作を減速させなかった場合に実現されるはずであったブームの先端の移動方向と異なる。
これに対し、ブーム伸長動作の減速が必要な際に、ブームの伸長動作と旋回動作の両方を同じ減速率で減速すると、ブームの先端は図16に示すように移動することとなる。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。また、点線矢印と実線矢印の長さの違いを用いて、図中最も右側にブームが位置する際のブームの伸長動作及び旋回動作の減速を示している。
図16に示すように、ブームの伸長動作と旋回動作の両方を同じ減速率で減速すると、ブームの先端の移動方向を維持したままブームの先端の速度を制限できる。
次に、図17乃至図19を用いて、シリンダ切換に伴う減速動作時の作用効果(第5の実施形態の作用効果)を説明する。図17に示すブームの先端の「意図する移動方向」を実現する場合、ブームを伸長し、かつ、図中右方向に旋回させることとなる。シリンダ1が伸びきった後に、シリンダ2が伸び始める。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。
ここで、振動を抑制するため、シリンダ1が伸びきる前にブーム伸長動作を減速させる必要があるが、ブーム伸長動作のみを減速させると(すなわち、図中右方向の旋回動作は減速させない)、ブームの先端は図18に示すように移動することとなる。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。また、点線矢印と実線矢印の長さの違いを用いて、図中最も右側にブームが位置する際のブームの伸長動作の減速を示している。
図18に示すように、ブームの伸長動作のみを減速した場合のブームの先端の移動方向は、ブームの伸長動作を減速させなかった場合に実現されるはずであったブームの先端の移動方向と異なる。
これに対し、ブーム伸長動作の減速が必要な際に、ブームの伸長動作と旋回動作の両方を同じ減速率で減速すると、ブームの先端は図19に示すように移動することとなる。図において、ブームの伸長、旋回、ブームの先端の移動を実線矢印で示している。また、点線矢印と実線矢印の長さの違いを用いて、図中最も右側にブームが位置する際のブームの伸長動作及び旋回動作の減速を示している。
図19に示すように、ブームの伸長動作と旋回動作の両方を同じ減速率で減速すると、ブームの先端の移動方向を維持したままブームの先端の速度を制限できる。
なお、旋回動作や起伏動作のみを減速させる必要がある場合においても同様である。
以下、参考形態の例を付記する。
1. 入力受付部と、
前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断部と、
前記判断部により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出部と、
前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
を有する制御装置。
2. 1に記載の制御装置において、
前記アクチュエータ制御部は、前記判断部により超えないと判断された場合、前記動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる制御装置。
3. 入力受付部と、
前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断部と、
前記判断部により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出部と、
前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
を有する制御装置。
4. 1から3のいずれかに記載の制御装置において、
前記入力受付部が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部をさらに有し、
前記動作ベクトル決定部は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換する制御装置。
5. 4に記載の制御装置において、
前記ヤコビ行列の逆行列は、
Figure 0007082477000016
であり、
=cos(q)、C=cos(q)、S=sin(q)、S=sin(q2)、qは前記ブームの旋回角、qは前記ブームの起伏角、qは前記ブームの長さ、Dboomは前記ブームの軸とトップシーブのオフセット距離、Dcolumnはコラム回転軸と接続ピンの水平オフセット距離である制御装置。
6. 1から5のいずれかに記載の制御装置において、
前記判断部は、前記ブームの状態に応じて前記閾値を変化させる制御装置。
7. コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断工程と、
前記判断工程で超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出工程と、
前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
を実行する制御方法。
8. コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断手段、
前記判断手段により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出手段、
前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
として機能させるプログラム。
9. コンピュータが、
入力受付工程と、
前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断工程と、
前記判断工程で超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出工程と、
前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
を実行する制御方法。
10. コンピュータを、
入力受付手段、
前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断手段、
前記判断手段により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出手段、
前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
として機能させるプログラム。
1A プロセッサ
2A メモリ
3A 入出力I/F
4A 周辺回路
5A バス
10 制御装置
11 入力受付部
12 速度ベクトル生成部
13 動作ベクトル決定部
14 判断部
15 算出部
16 アクチュエータ制御部

Claims (9)

  1. 入力受付部と、
    前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
    前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断部と、
    前記判断部により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出部と、
    前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
    前記入力受付部が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
    を有し、
    前記動作ベクトル決定部は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成部は、
    移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御部は、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置。
  2. 請求項1に記載の制御装置において、
    前記アクチュエータ制御部は、前記判断部により超えないと判断された場合、前記動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させる制御装置。
  3. 入力受付部と、
    前記入力受付部が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定部と、
    前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断部と、
    前記判断部により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出部と、
    前記判断部により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御部と、
    前記入力受付部が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成部と、
    を有し、
    前記動作ベクトル決定部は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付部は、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成部は、
    移動開始位置と前記入力受付部が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付部が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御部は、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御装置。
  4. 請求項1から3のいずれか1項に記載の制御装置において、
    前記ヤコビ行列の逆行列は、
    Figure 0007082477000017
    であり、
    =cos(q)、C=cos(q)、S=sin(q)、S=sin( )、qは前記ブームの旋回角、qは前記ブームの起伏角、qは前記ブームの長さ、Dboomは前記ブームの軸とトップシーブのオフセット距離、Dcolumnはコラム回転軸と接続ピンの水平オフセット距離である制御装置。
  5. 請求項1から4のいずれか1項に記載の制御装置において、
    前記判断部は、前記ブームの状態に応じて前記閾値を変化させる制御装置。
  6. コンピュータが、
    入力受付工程と、
    前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
    前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断工程と、
    前記判断工程で超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出工程と、
    前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
    前記入力受付工程で受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
    を実行し、
    前記動作ベクトル決定工程では、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付工程では、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成工程では、
    移動開始位置と前記入力受付工程で受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付工程で受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御工程では、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御方法。
  7. コンピュータを、
    入力受付手段、
    前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
    前記動作ベクトルで示される伸縮、起伏及び旋回各々の速度の中の少なくとも1つが、伸縮、起伏及び旋回各々に対応して定められた閾値を超えないか判断する判断手段、
    前記判断手段により超えると判断された場合、前記閾値を超えた動作の速度を前記閾値以下にする減速率を算出する算出手段、
    前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
    前記入力受付手段が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
    として機能させ
    前記動作ベクトル決定手段は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付手段は、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成手段は、
    移動開始位置と前記入力受付手段が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付手段が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御手段は、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させないプログラム。
  8. コンピュータが、
    入力受付工程と、
    前記入力受付工程で受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定工程と、
    前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断工程と、
    前記判断工程で超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出工程と、
    前記判断工程で超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御工程と、
    前記入力受付工程で受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成工程と、
    を実行し、
    前記動作ベクトル決定工程では、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付工程では、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成工程では、
    移動開始位置と前記入力受付工程で受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付工程で受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御工程では、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させない制御方法。
  9. コンピュータを、
    入力受付手段、
    前記入力受付手段が受付けた入力内容に基づき、ブームの伸縮、起伏及び旋回各々の動作方向及び速度を示す動作ベクトルを決定する動作ベクトル決定手段、
    前記動作ベクトルで示される伸縮、起伏及び旋回の速度を実現する油圧ポンプの吐出量が閾値を超えないか判断する判断手段、
    前記判断手段により超えると判断された場合、前記油圧ポンプの吐出量を前記閾値以下にする減速率を算出する算出手段、
    前記判断手段により超えると判断された場合、前記動作ベクトルで示される伸縮、起伏及び旋回すべての速度を前記減速率で減速した減速後動作ベクトルで示される動作方向及び速度で、前記ブームを伸縮、起伏及び旋回させる複数のアクチュエータを動作させるアクチュエータ制御手段、
    前記入力受付手段が受付けた入力内容に基づき、前記ブームの先端を移動させる方向及び速度を3次元座標系で示す速度ベクトルを生成する速度ベクトル生成手段、
    として機能させ
    前記動作ベクトル決定手段は、前記速度ベクトルに左からヤコビ行列の逆行列を掛けることで、又は、前記ヤコビ行列の逆行列を用いて得られた算出式を用いて、前記速度ベクトルを前記動作ベクトルに変換し、
    前記入力受付手段は、前記速度ベクトルを指定する入力を受付け、
    前記速度ベクトル生成手段は、
    移動開始位置と前記入力受付手段が受付けた入力内容とに基づき、制御周期毎の前記ブームの先端の目標位置を算出し、
    第n(nは2以上の整数)の制御周期に対応して前記入力受付手段が受付けた入力内容に基づき決定される第1の速度ベクトルと、第n-1の制御周期後の前記ブームの先端の位置から第n-1の制御周期の前記ブームの先端の目標位置に向かう第2の速度ベクトルとを足し合わせた合成ベクトルを、第nの制御周期における前記速度ベクトルとして生成し、
    前記アクチュエータ制御手段は、
    前記動作ベクトルで示される前記ブームの伸縮、起伏及び旋回の内、前記第1の速度ベクトルを変換した第1の動作ベクトルで示される前記ブームの伸縮、起伏及び旋回各々と動作方向が一致する動作は実行させ、前記動作方向が異なる動作は実行させないプログラム。
JP2017224829A 2017-11-22 2017-11-22 制御装置、制御方法及びプログラム Active JP7082477B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2017224829A JP7082477B2 (ja) 2017-11-22 2017-11-22 制御装置、制御方法及びプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2017224829A JP7082477B2 (ja) 2017-11-22 2017-11-22 制御装置、制御方法及びプログラム

Publications (2)

Publication Number Publication Date
JP2019094177A JP2019094177A (ja) 2019-06-20
JP7082477B2 true JP7082477B2 (ja) 2022-06-08

Family

ID=66970834

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017224829A Active JP7082477B2 (ja) 2017-11-22 2017-11-22 制御装置、制御方法及びプログラム

Country Status (1)

Country Link
JP (1) JP7082477B2 (ja)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7476889B2 (ja) 2019-05-22 2024-05-01 株式会社タダノ 作業車両
JP7367406B2 (ja) 2019-09-05 2023-10-24 株式会社タダノ 移動式クレーンおよび遠隔端末装置
EP4082957A4 (en) * 2019-12-27 2024-03-20 Tadano Ltd CONTROL SYSTEM FOR A WORK MACHINE AND CRANE
CN114852868B (zh) * 2022-04-29 2023-06-20 中联重科股份有限公司 用于起重机臂架的控制方法、控制器、装置及起重机

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040267404A1 (en) 2001-08-31 2004-12-30 George Danko Coordinated joint motion control system
JP2006525928A (ja) 2003-05-30 2006-11-16 リープヘル−ヴェルク ネンツィング ゲーエムベーハー ロープ吊りの荷物を取り扱う最適動き案内付きクレーンまたはショベル
JP2013091935A (ja) 2011-10-24 2013-05-16 Kyokuto Kaihatsu Kogyo Co Ltd コンクリートポンプ車

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3967394B2 (ja) * 1995-10-31 2007-08-29 石川島播磨重工業株式会社 ワークの組立方法及びその装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040267404A1 (en) 2001-08-31 2004-12-30 George Danko Coordinated joint motion control system
JP2006525928A (ja) 2003-05-30 2006-11-16 リープヘル−ヴェルク ネンツィング ゲーエムベーハー ロープ吊りの荷物を取り扱う最適動き案内付きクレーンまたはショベル
JP2013091935A (ja) 2011-10-24 2013-05-16 Kyokuto Kaihatsu Kogyo Co Ltd コンクリートポンプ車

Also Published As

Publication number Publication date
JP2019094177A (ja) 2019-06-20

Similar Documents

Publication Publication Date Title
JP7082477B2 (ja) 制御装置、制御方法及びプログラム
US6430473B1 (en) Power assist apparatus and control method for same
US9827675B2 (en) Collision avoidance method, control device, and program
US8396594B2 (en) Robot, control device for robot, and control method of robot
JP6564739B2 (ja) 作業機械
JP5202735B2 (ja) 数値制御装置
CN101801616B (zh) 动力辅助装置及其控制方法
JP2020032481A (ja) ロボット制御装置、ロボット装置、ロボット制御のパラメータ調整方法、およびプログラム
JP2007098507A (ja) 作業補助装置
WO2013027250A1 (ja) ロボットシステム、ロボット及びロボット制御装置
CN115502966B (zh) 用于机器人的可变导纳控制方法
CN110621446B (zh) 机器人系统以及机器人系统的控制方法
KR102188157B1 (ko) 촉각자극 제공 장치 및 그 구동 방법
CN114536342A (zh) 一种多臂系统及其臂间防撞控制方法
JP7082476B2 (ja) 制御装置、制御方法及びプログラム
JP2010121441A (ja) 建設機械の作業機の制御装置、及び建設機械の作業機の制御方法
JP5499865B2 (ja) 多関節型ロボットの速度指令プロファイルの生成方法
JP6088601B2 (ja) 走行軸付きロボットにおけるツール先端の振れを抑制するロボット制御装置
JP4407083B2 (ja) 指令値生成方法および指令値生成システム
WO2020255724A1 (ja) ロボット制御装置及びロボット制御方法
CA2895450A1 (en) Control system and method for controlling the movement of an aerial apparatus
JP4289501B2 (ja) ロボット制御方法及びプログラム
JP7337632B2 (ja) バルブシステム、作業機械、バルブの制御方法、プログラム、および記録媒体
JPH0447087B2 (ja)
JPH07186077A (ja) マニピュレータ制御方式

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: 20211026

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20211220

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: 7082477

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150