特許 7313633 位置制御装置及び位置制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-07-14

(45)【発行日】2023-07-25

(54)【発明の名称】位置制御装置及び位置制御方法

(51)【国際特許分類】

   E02F 3/43 20060101AFI20230718BHJP

   E02F 9/20 20060101ALI20230718BHJP

【ＦＩ】

E02F3/43 E

E02F9/20 M

【請求項の数】 5

(21)【出願番号】P 2020015040

(22)【出願日】2020-01-31

(65)【公開番号】P2021121717

(43)【公開日】2021-08-26

【審査請求日】2022-10-12

(73)【特許権者】

【識別番号】504136568

【氏名又は名称】国立大学法人広島大学

(73)【特許権者】

【識別番号】000246273

【氏名又は名称】コベルコ建機株式会社

(74)【代理人】

【識別番号】100196380

【弁理士】

【氏名又は名称】森 匡輝

(72)【発明者】

【氏名】菊植 亮

【審査官】彦田 克文

(56)【参考文献】

【文献】特開昭６２－２４２０２８（ＪＰ，Ａ）

【文献】国際公開第２０１９／１５１３３５（ＷＯ，Ａ１）

【文献】特開２０１７－０５３１６０（ＪＰ，Ａ）

【文献】特開２００２－０２３８０７（ＪＰ，Ａ）

【文献】特開２０１９－０１９５６７（ＪＰ，Ａ）

【文献】特開平１０－１８３６６９（ＪＰ，Ａ）

【文献】特開２０１５－１８３７６８（ＪＰ，Ａ）

【文献】特開平０９－２９１５６０（ＪＰ，Ａ）

【文献】国際公開第２０１９／１８０８９４（ＷＯ，Ａ１）

【文献】特開２０１５－１９６９６８（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｅ０２Ｆ ３／４３

Ｅ０２Ｆ ９／２０

(57)【特許請求の範囲】

【請求項1】

制御対象である機械を駆動させる液圧アクチュエータを制御する制御部を備え、
前記制御部は、
前記機械の目標位置と目標速度との少なくともいずれか一方、及び現在位置と現在速度との少なくともいずれか一方に基づいて、前記機械の次時刻ステップの目標速度である参照速度を算出し、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記現在速度に基づいて、前記参照速度を、次時刻ステップで実現可能な範囲に収まるように飽和させ、
前記液圧アクチュエータの準静的特性、前記機械の動特性、飽和された前記参照速度及び前記現在速度に基づいて、前記液圧アクチュエータの操作量を算出する、
位置制御装置。

【請求項2】

制御対象である機械を駆動させる液圧アクチュエータを制御する制御部を備え、
前記制御部は、
前記液圧アクチュエータの準静的特性と前記機械の動特性とに基づいて、所定ステップ先の時刻ステップの前記機械の予測位置と予測速度とを算出し、
前記機械の目標位置と目標速度との少なくともいずれか一方、及び前記予測位置と前記予測速度との少なくともいずれか一方に基づいて、前記所定ステップ先の次時刻ステップの目標速度である参照速度を算出し、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記予測速度に基づいて、前記参照速度を、前記所定ステップ先の時刻ステップの次時刻ステップで実現可能な範囲に収まるように飽和させ、
前記液圧アクチュエータの準静的特性、前記機械の動特性、飽和された前記参照速度及び前記予測速度に基づいて、前記液圧アクチュエータの操作量を算出する、
位置制御装置。

【請求項3】

複数の前記液圧アクチュエータを備え、
前記制御部は、それぞれの前記液圧アクチュエータについて、前記操作量を算出して制御することにより、前記機械の所定部位を目標位置に移動させる、
請求項１又は２に記載の位置制御装置。

【請求項4】

前記液圧アクチュエータは、油圧アクチュエータである、
請求項１から３のいずれか一項に記載の位置制御装置。

【請求項5】

液圧アクチュエータで駆動される制御対象である機械の目標位置と目標速度との少なくともいずれか一方、及び現在位置と現在速度との少なくともいずれか一方に基づいて、前記機械の次時刻ステップの目標速度である参照速度を算出する参照速度算出工程と、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記現在速度に基づいて、前記参照速度を、次時刻ステップで実現可能な範囲に収まるように飽和させる参照速度修正工程と、
前記液圧アクチュエータの準静的特性、前記制御対象の動特性、飽和された前記参照速度及び前記現在速度に基づいて、前記液圧アクチュエータの操作量を算出する操作量算出工程と、を含む、
位置制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置制御装置及び位置制御方法に関する。

【背景技術】

【0002】

油圧アクチュエータで駆動されるショベル等の機械において、バケット、作業装置等を、目標位置へ自動的に移動させる制御方法が開発されている。油圧アクチュエータを含む液圧アクチュエータの応答特性は、強い非線形性を持ち、リリーフバルブの開閉によっても大きく変化する。したがって、自動的な位置決め制御を、単純な制御則で実現することは難しい。

【0003】

例えば、標準的な線形制御理論に基づいた制御手法として非特許文献１、２の方法が提案されている。非特許文献１、２の方法では、アクチュエータ動特性の線形近似に基づくため、参照位置と目標位置とが大きく離れている場合、適切な動作をさせることは難しい。

【0004】

参照位置と目標位置とが大きく離れた場合であっても適切な制御を行うため、仮の目標位置を現在位置の近くに設定して、参照位置をなめらかに時間変化させながら、本来の目標位置へ到達させる方法が開発されている（例えば、特許文献１）。

【0005】

また、目標位置と現在位置との差が大きい場合と小さい場合とで制御則を切り替える方法（例えば、非特許文献３）、リリーフバルブの開閉によって制御則を切り替える方法（例えば、非特許文献４）等が提案されている。

【0006】

また、非特許文献５のように、アクチュエータの非線形動特性モデルを用いて、所望の速度を実現するための最適な制御入力を、収束計算によって求める方法が提案されている。

【先行技術文献】

【特許文献】

【0007】

【文献】特開２０１９－１９５６７号公報

【非特許文献】

【0008】

【文献】Yaoyao Wang, Linyi Gu, Bai Chen, Hongtao Wu, “A new discrete time delay control of hydraulic manipulators”, Proceedings of Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, Vol.231, No.3, p.168-177, 2017

【文献】Navit Niksefat, Nariman Sepehri, “A QFT fault-tolerant control for electrohydraulic positioning systems”, IEEE Transactions on Control Systems Technology, Vol.10, No.4, p.626-632, 2002

【文献】Jianpeng Shi, Long Quan, Xiaogang Zhang, Xiaoyan Xiong, “Electro-hydraulic velocity and position control based on independent metering valve control in mobile construction equipment”, Automation in Construction, Vol.94, p.73-84, 2018

【文献】小岩井一茂，▲浜▼永慎也，山本透，南條孝夫，山▲崎▼洋一郎、「油圧ショベルのイベント駆動型トルク制御」、計測自動制御学会論文集、Vol.54，No.2，p.261-268、2018年

【文献】Kwangmin Kim, Minji Kim, Dongmok Kim, Dongjun Lee, “Modeling and velocity-field control of autonomous excavator with main control valve”, Automatica, Vol.104, p.67-81, 2019

【発明の概要】

【発明が解決しようとする課題】

【0009】

特許文献１の制御方法では、時間変化する参照位置を設定するために、参照位置データを時系列データとして予め生成する必要がある。したがって、目標位置が刻一刻と変化する場合、外乱によって現在位置が参照位置から大きく逸れた場合等に対応することは難しい。

【0010】

また、非特許文献３、４のように、状況に応じて制御則を切り替える方法では、コントローラの構造が複雑化するとともに、切り替え前後において不連続性が生じないように、注意深く制御パラメータを設定することが必要となるため、設計が困難となる。

【0011】

また、非特許文献５のように、アクチュエータの非線形動特性モデルをそのまま用いる方法では、複雑な収束計算を行うため、計算コストが増大する。

【0012】

本発明は、上述の事情に鑑みてなされたものであり、目標位置の変化及び外乱による軌道のずれに対応可能であり、小さな計算コストで位置制御を行うことができる、簡素な制御アルゴリズムの位置制御装置及び位置制御方法を提供することを目的とする。

【課題を解決するための手段】

【0013】

上記目的を達成するために、この発明の第１の観点に係る位置制御装置は、
制御対象である機械を駆動させる液圧アクチュエータを制御する制御部を備え、
前記制御部は、
前記機械の目標位置と目標速度との少なくともいずれか一方、及び現在位置と現在速度との少なくともいずれか一方に基づいて、前記機械の次時刻ステップの目標速度である参照速度を算出し、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記現在速度に基づいて、前記参照速度を、次時刻ステップで実現可能な範囲に収まるように飽和させ、
前記液圧アクチュエータの準静的特性、前記機械の動特性、飽和された前記参照速度及び前記現在速度に基づいて、前記液圧アクチュエータの操作量を算出する。

【0014】

また、本発明の第２の観点に係る位置制御装置は、
制御対象である機械を駆動させる液圧アクチュエータを制御する制御部を備え、
前記制御部は、
前記液圧アクチュエータの準静的特性と前記機械の動特性とに基づいて、所定ステップ先の時刻ステップの前記機械の予測位置と予測速度とを算出し、
前記機械の目標位置と目標速度との少なくともいずれか一方、及び前記予測位置と前記予測速度との少なくともいずれか一方に基づいて、前記所定ステップ先の次時刻ステップの目標速度である参照速度を算出し、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記予測速度に基づいて、前記参照速度を、前記所定ステップ先の時刻ステップの次時刻ステップで実現可能な範囲に収まるように飽和させ、
前記液圧アクチュエータの準静的特性、前記機械の動特性、飽和された前記参照速度及び前記予測速度に基づいて、前記液圧アクチュエータの操作量を算出する。

【0015】

また、複数の前記液圧アクチュエータを備え、
前記制御部は、それぞれの前記液圧アクチュエータについて、前記操作量を算出して制御することにより、前記機械の所定部位を目標位置に移動させる、
こととしてもよい。

【0016】

また、前記液圧アクチュエータは、油圧アクチュエータである、
こととしてもよい。

【0017】

また、本発明の第３の観点に係る位置制御方法は、
液圧アクチュエータで駆動される制御対象である機械の目標位置と目標速度との少なくともいずれか一方、及び現在位置と現在速度との少なくともいずれか一方に基づいて、前記機械の次時刻ステップの目標速度である参照速度を算出する参照速度算出工程と、
前記機械の動特性、前記液圧アクチュエータに許容される最大発生力及び前記現在速度に基づいて、前記参照速度を、次時刻ステップで実現可能な範囲に収まるように飽和させる参照速度修正工程と、
前記液圧アクチュエータの準静的特性、前記制御対象の動特性、飽和された前記参照速度及び前記現在速度に基づいて、前記液圧アクチュエータの操作量を算出する操作量算出工程と、を含む。

【発明の効果】

【0018】

本発明の位置制御装置及び位置制御方法によれば、液圧アクチュエータの準静的特性、機械の動特性及び実現可能な参照速度を用いて操作量を算出して位置制御を行うので、簡素な制御アルゴリズムで位置制御を行うことができる。また、本発明の位置制御装置及び位置制御方法は、目標位置の変化及び外乱による軌道のずれに対応可能であり、小さな計算コストで位置制御を行うことが可能である。

【図面の簡単な説明】

【0019】

【図1】本発明の実施の形態１に係るショベルの概要を示す側面図である。

【図2】実施の形態１に係る制御システムの構成を示すブロック図である。

【図3】複数の油圧アクチュエータの制御方法を示す概念図である。

【図4】実施の形態１に係る位置制御の流れを示すフローチャートである。

【図5】実施の形態２に係る制御アルゴリズムによるシミュレーション結果を示すグラフである。

【図6】ｎステップ予測器の構成例を示すブロック図である。

【発明を実施するための形態】

【0020】

（実施の形態１）
以下、図を参照しつつ、本発明の実施の形態に係る位置制御装置及び位置制御方法について説明する。

【0021】

（制御アルゴリズム）
まず、本実施の形態に係る制御則、すなわち、液圧アクチュエータによって駆動される機械を制御対象とし、制御対象を目標位置へ移動させる場合の液圧アクチュエータの操作量ｕを決定する制御アルゴリズムについて説明する。

【0022】

具体的には、以下に示すシステムを考える。

【数1】

上式で、Ｍは質量、ｐは変位、τは液圧アクチュエータによって発生される力であり、ｆは、その他の全ての力を表す。式（１ａ）、（１ｂ）は、機械の動特性モデルを表す。

【0023】

式（１ｃ）は、液圧アクチュエータの準静的特性モデルを表す。ここで、本発明における準静的特性モデルとは、一定外力と定常速度を仮定したときの速度と反力の関係式で表される特性モデルをいう。操作量ｕは、液圧アクチュエータへのバルブ開度の指令値である。液圧アクチュエータ内の液体の流量と圧力の平衡関係から、液圧アクチュエータの発生力τ、速度ｖおよび操作量ｕは、近似的に式（１ｃ）を満たす。定数Ｃは、バルブに起因する係数であり、圧力差と流量との関係を示す。定数Ｆは、液圧アクチュエータのシリンダによって発生される最大力である。液圧アクチュエータの発生力τが最大力Ｆを超えると、リリーフバルブが開いて、発生力τを飽和させる。定数Ｆ_ｍは、リリーフバルブが開かないと仮定した場合に液圧アクチュエータのシリンダによって発生される最大力である。通常はＦ_ｍ≧Ｆであるが、Ｆ_ｍ＝Ｆとしてもよい。

【0024】

（１）数学的準備
Ｃ＞０、Ｆ_ｍ＞Ｆ、Ｆ∈（０，Ｆ_ｍ］、ε＞０、ｖ∈Ｒ、及びｖ_ａ∈Ｒである場合、以下の式は、互いに等価である（定理１）。

【数2】

【0025】

（２）コントローラ
続いて、本実施の形態に係る制御アルゴリズムの一般表現について説明する。上記の式（１ａ）～（１ｃ）で表されるシステムのコントローラを作成するために、プラントの公称モデルを構築する。離散時間領域では、プラントの公称モデルは、以下のように記述できる。

【数3】

ここで、ハット付きの記号は、関連する未知の量の公称値又は推定値を表し、ｋは離散時間インデックス、＾ｆ_ｋは機械に加わる外力の推定値である。コントローラのサンプリング間隔はＴとする。

【0026】

上記の定理１は、式（７）が以下の式（８）のように等価に書き換えられることを示している。

【数4】

【0027】

式（８）は、ある次時刻ステップで達成されるべき所望の速度ｖ^＊ _ｋ＋１について、｜ｖ^＊ _ｋ＋１－ｖ_ｋ－Ｔ＾ｆ_ｋ／＾Ｍ｜＜Ｔ＾Ｆ／＾Ｍを満たすならば、次の式を用いて操作量ｕ_ｋが得られることを意味する。

【数5】

【0028】

したがって、式（９）に対して所望の速度ｖ^＊ _ｋ＋１を与えるコントローラを考える。質量＾Ｍで、液圧アクチュエータの最大力が＾Ｆである場合、液圧アクチュエータの発生力の制約の下で、最小到達時間を達成するという意味において、バンバン制御が適していると考えられる。質量の推定値である＾Ｍと位置ｐとが利用可能であると仮定した場合、バンバン制御は、以下の制御則によって実現できる。

【0029】

【数6】

ここで、ｐ_ｄは目標位置である。この制御則は、一種のスライディングモード制御則とみなすことができる。

【0030】

このコントローラを、公称プラントに適用すると、下式のシステムが得られる。

【数7】

【0031】

陰的オイラー法を用いて式（１１）、（１２）を離散化すると、以下の閉形式で表すことができる。

【数8】

【0032】

ここで、ｖ_ｋ＋１の値は、バンバン制御で実現できる次時刻ステップの予想速度とみなすことができる。この値と、式（９）とを組み合わせることにより、バンバン制御に類するコントローラとして、以下の制御アルゴリズムが得られる。

【数9】

【0033】

式（１６）～（１８）を整理すると、本実施の形態に係る有限時間収束型の制御アルゴリズムとして、以下の式が得られる。

【数10】

ここで、Ａは目標位置への収束運動の目標加速度であり、Ａ＝＾Ｆ／＾Ｍと設定することが望ましい。ここで，＾Ｃはアクチュエータの特性値Ｃの公称値（推定値）であるが、最大入力での定常速度を＾Ｆで除した値に近似する値として選定することが可能である。

【0034】

上記の制御アルゴリズムにおいて、ｖ^＊ _ｋ＋１と＾ｖ^＊ _ｋ＋１とは、コントローラにおいて継続的に保存される必要のない一時的な値である。位置ｐ_ｋ及び速度ｖ_ｋは、センサによって測定される実際の値である。また、力＾ｆ_ｋは、何らかのセンサ情報から推定される推定値である。力＾ｆ_ｋの推定ができない場合、＾ｆ_ｋ＝０と設定してもよい。

【0035】

また、式（２１）において＾Ｆ_ｍ＝＾Ｆである場合、sat関数内の数値が無限大になり得る。これを回避するために、式（２１）に代えて、以下の式（２２）を用いることとしてもよい。

【0036】

【数11】

ここで、εは非常に小さい値、例えば検出できる速度の絶対値の最小値と比較して非常に小さい値とすればよい。

【0037】

本実施の形態に係る制御アルゴリズムでは、参照速度算出工程として、式（１９）を用いて、目標位置ｐ_ｄ，ｋ、現在位置ｐ_ｋから、次時刻ステップの目標速度である参照速度＾ｖ^＊ _ｋ＋１を求める。続いて、参照速度修正工程として、式（２０）を用いて、参照速度算出工程で算出された参照速度＾ｖ^＊ _ｋ＋１、現在速度ｖ_ｋから、液圧アクチュエータに許容される最大発生力に基づいて修正された参照速度、すなわち次時刻ステップで実現可能な範囲に収まる所望の速度ｖ^＊ _ｋ＋１を算出する。そして、操作量算出工程として、式（２１）を用いて、参照速度修正工程で算出された参照速度ｖ^＊ _ｋ＋１、現在速度ｖ_ｋから、液圧アクチュエータの操作量ｕ_ｋを算出する。

【0038】

上述のように、本実施の形態に係る制御アルゴリズムは、液圧アクチュエータで駆動される機械の簡易な動特性、液圧アクチュエータの準静的特性及びある種のスライディングモード制御則の３つの式を連立方程式として組み合わせて導出されたものである。本制御アルゴリズムでは、事前に時系列データを与える必要がないので、目標位置が刻一刻と変化する場合、予想外の外乱によって軌道が大きく逸れた場合等に対応することができる。

【0039】

制御アルゴリズムの導出の基礎となる液圧アクチュエータの準静的特性モデルは、リリーフバルブの開放時と閉鎖時とを統合して記述したものである。これにより、結果として得られる計算式は、場合分けを含まない簡素な構造となる。したがって、リリーフバルブが頻繁に作動する系、例えば建設機械の旋回動作制御等において、計算を簡便にすることができる。また、制御アルゴリズムの各工程は、複雑な収束計算を含まないので、計算コストを小さくすることができる。

【0040】

（制御システムの構成）
本実施の形態では、具体的な制御システムの構成の例として、複数の油圧アクチュエータで駆動されるショベルのバケットの位置制御を行う場合について説明する。

【0041】

本実施の形態に係る位置制御装置である制御ユニット３０は、図１に示すショベル１に搭載され、ショベル１の動作を制御する。

【0042】

ショベル１は、下部走行体１１、上部旋回体１２、作業装置２０を備える。下部走行体１１は、ショベル１を走行させる部分であり、例えばクローラである。上部旋回体１２は、下部走行体１１に、旋回モータ２７を介して旋回可能に取り付けられている。旋回モータ２７は、下部走行体１１及び上部旋回体１２に接続されて、上部旋回体１２を回転動作させる。上部旋回体１２には、操作者がショベル１の操作を行う運転室、作業装置２０等が配置されている。

【0043】

作業装置２０は、上部旋回体１２に回転可能に取り付けられているブーム２１、ブーム２１に回転可能に取り付けられているアーム２２、アーム２２に回転可能に取り付けられ掘削等を行うバケット２３を備える。

【0044】

また、作業装置２０は、上部旋回体１２及びブーム２１に接続されてブーム２１を動作させるブームシリンダ２４、ブーム２１及びアーム２２に接続されてアーム２２を動作させるアームシリンダ２５、アーム２２及びバケット２３に接続されてバケット２３を動作させるバケットシリンダ２６を備える。ブームシリンダ２４、アームシリンダ２５、バケットシリンダ２６、旋回モータ２７（以下、各アクチュエータ２４～２７ともいう。）は、油圧によって駆動される油圧アクチュエータである。

【0045】

制御ユニット３０は、ショベル１に搭載されており、図２のブロック図に示すように、制御部３１、記憶部３２、表示部３３、入力部３４を備える。制御ユニット３０は、位置センサ２８、速度センサ２９、各アクチュエータ２４～２７等と接続されて、ショベル１の各部の動作制御を行う。

【0046】

制御部３１は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、水晶発振器等から構成されるコンピュータ装置であり、ショベル１の動作を制御する。制御部３１は、制御部３１のＲＯＭ、記憶部３２等に記憶されている各種動作プログラム及びデータをＲＡＭに読み込んでＣＰＵを動作させることにより、図２に示される制御部３１としての各機能を実現させる。これにより、制御部３１は、演算部３１１及び動作制御部３１２として動作する。

【0047】

演算部３１１は、位置センサ２８及び速度センサ２９の出力である測定値と、バケット２３の目標位置、より詳細にはショベル１の所定部位であるバケット２３の先端部（以下、バケット先端２３ａという。）の目標位置とに基づいて、作業装置２０を動作させるための各アクチュエータ２４～２７の操作量ｕを算出する。

【0048】

動作制御部３１２は、演算部３１１で算出された操作量ｕに基づいて、各アクチュエータ２４～２７を制御し、上部旋回体１２、ブーム２１、アーム２２、バケット２３を動作させる。

【0049】

記憶部３２は、ハードディスク、フラッシュメモリ等の不揮発性メモリであり、ショベル１の動特性を表すパラメータ、操作量ｕ算出のための制御アルゴリズム等を記憶する。本実施の形態に係るショベル１の動特性は、単純化されたものであり、力を受けて運動する質点又は剛体の動特性として表される。

【0050】

表示部３３は、液晶パネル、有機ＥＬ（Electroluminescence）等の表示用デバイスであり、各種設定パラメータ、位置センサ２８及び速度センサ２９の検出値等を表示する。本実施の形態に係る表示部３３は、ショベル１の運転室内に設置された液晶パネルである。

【0051】

入力部３４は、バケット先端２３ａの目標位置等、ショベル１を動作させる各種設定パラメータを入力するための入力デバイスである。入力部３４は、例えば表示部３３上に配置されたタッチパネルである。

【0052】

位置センサ２８は、制御対象であるショベル１の作業装置２０の位置を検出するセンサであり、本実施の形態に係る位置センサ２８は、上部旋回体１２の角度を検出する旋回角度センサ２８４、ブームの角度を検出するブーム角度センサ２８１、アームの角度を検出するアーム角度センサ２８２、バケットの角度を検出するバケット角度センサ２８３を含む。制御部３１は、ブーム角度センサ２８１、アーム角度センサ２８２、バケット角度センサ２８３、旋回角度センサ２８４によって検出された各部の角度から、ブームシリンダ２４、アームシリンダ２５及びバケットシリンダ２６（以下、各シリンダ２４～２６ともいう。）の長さと、旋回モータ２７の角度とを算出し、算出された各シリンダ２４～２６の長さと旋回モータ２７の角度とに基づいて、バケット先端２３ａの位置制御を行う。

【0053】

速度センサ２９は、作業装置２０各部の速度を検出するセンサであり、本実施の形態に係る速度センサ２９は、上部旋回体１２の角速度を検出する旋回角速度センサ２９４、ブームシリンダの伸縮速度を検出するブーム速度センサ２９１、アームシリンダの伸縮速度を検出するアーム速度センサ２９２、バケットシリンダの伸縮速度を検出するバケット速度センサ２９３を含む。

【0054】

以下、本実施の形態に係る制御ユニット３０によるバケット先端２３ａの位置制御処理について説明する。図３のブロック図に示すように、油圧アクチュエータである各シリンダ２４～２６の操作量ｕを算出する制御アルゴリズムを例として説明する。なお、以下の実施の形態では、各シリンダ２４～２６のみを用いることとするが、これに限られない。目標位置ｐ_ｄの座標によって、上部旋回体１２の旋回角度を加えたコントローラを構成して、バケット先端２３ａの位置制御を行うこととしてもよい。より具体的には、図２に示すように、位置センサ２８としての旋回角度センサ２８４、速度センサ２９としての旋回角速度センサ２９４を用いて、液圧アクチュエータである旋回モータ２７を動作させ、ブーム２１、アーム２２、バケット２３の動作と組み合わせてバケット先端２３ａの位置制御を行うこととしてもよい。

【0055】

本発明の制御アルゴリズムでは、制御部３１は、与えられた制御対象の目標位置、所望の挙動、現在位置、現在速度等に基づいて、各シリンダ２４～２６の操作量ｕを算出する。所望の挙動は、例えば制御対象の目標位置への収束運動の時定数である。

【0056】

（制御の流れ）
図４のフローチャートを参照しつつ、ショベル１のバケット先端２３ａの位置制御の流れについて具体的に説明する。

【0057】

ショベル１の操作者は、入力部３４からバケット先端２３ａの目標位置ｐ_ｄを入力、設定する（ステップＳ１１）。目標位置ｐ_ｄの入力は、タッチパネルである入力部３４で座標入力することとしてもよいし、操作レバーを用いて手動で移動させたバケット先端２３ａの座標を目標位置ｐ_ｄとして設定することとしてもよい。また、記憶部３２に予め記憶されている設計面上の座標を読み出して、目標位置ｐ_ｄを設定することとしてもよい。

【0058】

また、操作者は、所望の挙動を表す時定数Ｈ等のパラメータを入力、設定する。これらのパラメータは、記憶部３２に予め記憶されている値を読み出して、設定されることとしてもよい。

【0059】

制御部３１は、入力された目標位置ｐ_ｄから、逆運動学によって目標位置ｐ_ｄにおける各シリンダ２４～２６の目標長さを算出する（ステップＳ１２）。そして、図３に示すように、各シリンダ２４～２６の長さがそれぞれ目標長さとなるように、各シリンダ２４～２６を制御して伸縮させることにより、バケット先端２３ａの位置制御を行う。

【0060】

制御部３１は、参照速度算出工程として、次時刻ステップの参照速度＾ｖ^＊ _ｋ＋１を算出する（ステップＳ１３）。より具体的には、制御部３１は、ブーム角度センサ２８１、アーム角度センサ２８２、バケット角度センサ２８３（以下、各角度センサ２８１～２８３ともいう。）から、作業装置２０各部の角度データを取得する。演算部３１１は、取得した角度データに基づいて、各シリンダ２４～２６の長さである現在位置ｐ_ｋを算出する。制御部３１は、算出した現在位置ｐ_ｋ（ｍ）、設定された目標位置ｐ_ｄ，ｋ（ｍ）、制御周期Ｔ（ｓ）、及び式（１９）に基づいて、各シリンダ２４～２６の参照速度＾ｖ^＊ _ｋ＋１を算出する。制御周期は、特に限定されないが、例えば１０ｍｓｅｃである。

【0061】

続いて、制御部３１は、参照速度修正工程として、次時刻ステップの参照速度＾ｖ^＊ _ｋ＋１を修正する（ステップＳ１４）。具体的には、制御部３１は、ブーム速度センサ２９１、アーム速度センサ２９２、バケット速度センサ２９３から、各シリンダ２４～２６の現在速度ｖ_ｋを取得する。演算部３１１は、取得した現在速度ｖ_ｋ（ｍ／ｓ）、ステップＳ１３の参照速度算出工程で算出された参照速度＾ｖ^＊ _ｋ＋１、及び式（２０）に基づいて、各シリンダ２４～２６について修正された参照速度ｖ^＊ _ｋ＋１を算出する。

【0062】

続いて、制御部３１は、操作量算出工程として、各シリンダ２４～２６の操作量ｕ_ｋを算出する（ステップＳ１５）。具体的には、演算部３１１は、現在速度ｖ_ｋ（ｍ／ｓ）、ステップＳ１４の参照速度修正工程で算出された参照速度ｖ^＊ _ｋ＋１、及び式（２１）に基づいて、操作量ｕ_ｋを算出する。

【0063】

制御部３１の動作制御部３１２は、算出した操作量ｕ_ｋを各シリンダ２４～２６へ出力し、ブーム２１、アーム２２、バケット２３を動作させてバケット先端２３ａを移動させる（ステップＳ１６）。

【0064】

次の時刻ステップに到達すると、制御部３１は、位置センサ２８から各シリンダ２４～２６の現在位置ｐ_ｋを、新たに取得する。取得した現在位置ｐ_ｋと目標位置ｐ_ｄ，ｋとの差が予め設定されている所定の値以下である場合（ステップＳ１７のＹＥＳ）、制御部３１は、位置制御を終了する。

【0065】

また、取得した現在位置ｐ_ｋと目標位置ｐ_ｄ，ｋとの差が予め設定されている所定の値よりも大きい場合（ステップＳ１７のＮＯ）、制御部３１は、ステップＳ１３の参照速度算出工程に戻る。そして、制御部３１は、制御アルゴリズムにしたがって、当該時刻ステップでの操作量ｕ_ｋを算出し、バケット先端２３ａを移動させる。これを繰り返し、制御部３１は、バケット先端２３ａの位置を目標位置ｐ_ｄとするように、位置制御を行う。

【0066】

以上、詳細に説明したように、本実施の形態に係る位置制御装置及び位置制御方法では、制御対象である機械の動特性、液圧アクチュエータの準静的特性及びスライディングモード制御則に基づいて作成された参照速度算出工程、参照速度修正工程及び操作量算出工程を含む制御アルゴリズムによって位置制御を行う。これにより、液圧アクチュエータの準静的特性、機械の動特性及び実現可能な参照速度を用いて操作量を算出して、位置制御を行うので、場合分けを含まない簡素な制御アルゴリズムで位置制御を行うことができる。

【0067】

また、本実施の形態に係る位置制御装置及び位置制御方法では、事前に時系列データを与える必要がないので、目標位置の変化及び外乱による軌道のずれに対応可能である。また、本発明に係る制御アルゴリズムは、複雑な収束計算を含まないので、小さな計算コストで位置制御を行うことが可能である。

【0068】

（実施の形態２）
上記実施の形態１では、バンバン制御のような有限時間収束型の制御アルゴリズムを用いることとしたが、これに限られず、他の制御アルゴリズムを用いることもできる。本実施の形態では、目標位置へ、滑らかで指数関数的な収束を実現する制御アルゴリズムについて説明する。

【0069】

（制御アルゴリズム）
本実施の形態に係る漸近収束型のコントローラは、以下の制御則によって実現できる。

【数12】

ここで、Ｈは収束の時定数である。この制御則も、実施の形態１に係る制御則（式（１０））と同様に、一種のスライディングモード制御則とみなすことができる。

【0070】

このコントローラを、公称プラントに適用すると、以下のように表すことができる。

【数13】

【0071】

陰的オイラー法を用いて式（２４）、（２５）を離散化すると、以下の閉形式で表すことができる。

【数14】

【0072】

ここで、ｖ_ｋ＋１の値は、式（２３）で表されるコントローラで実現できる予想速度とみなすことができる。この値と、式（９）とを組み合わせることにより、漸近収束型コントローラとして、以下の制御アルゴリズムが得られる。

【数15】

【0073】

また、式（３０）において、Ｆ_ｍ＝Ｆである場合、sat関数内の数値が無限大になり得る。これを回避するために、式（３０）に代えて、式（２２）を用いることとしてもよい。

【0074】

（数値シミュレーション）
図５は、上記の漸近収束型制御アルゴリズムを用いて、数値シミュレーションを行った場合のシミュレーション結果を示すグラフである。本シミュレーションでは、質量Ｍ＝１．１×１０^３ｋｇの質点が油圧シリンダにより駆動される系を考える。油圧シリンダの物理モデルは、式（１Ｃ）で表される準静的特性モデルである。

【0075】

ここで、油圧シリンダの特性を表す定数Ｃの値は２×１０^－５ｍ／（Ｎ・ｓ）、油圧シリンダの最大推力（最大発生力）を表す定数ＦとＦ_ｍの値はＦ＝Ｆ_ｍ＝３６×１０^３Ｎとした。

【0076】

質点Ｍには、周波数１Ｈｚ、振幅１０×１０^３Ｎの周期的外力が持続的に加わることとした。さらに、時刻ｔ＝２．０～３．０ｓの間に、３８×１０^３Ｎの大きさの外力がマイナス方向に加わることとした。

【0077】

制御器のパラメータは、Ｈ＝１．０ｓ、＾Ｍ＝１．０×１０^３ｋｇ、＾Ｃ＝３×１０^－５ｍ／（Ｎ・ｓ）、＾Ｆ＝＾Ｆ_ｍ＝４０×１０^３Ｎとした。これらのパラメータの値は、＾Ｍ＝Ｍ、＾Ｃ＝Ｃ、＾Ｆ＝Ｆ、＾Ｆ_ｍ＝Ｆ_ｍとなるように選択されることが理想的であるが、制御対象の値であるＭ、Ｃ、Ｆ、Ｆ_ｍを正確に知ることは困難であるので、本シミュレーションでは、意図的に誤差を有するように設定している。

【0078】

図５は、目標位置ｐ_ｄ＝１ｍとした場合のシミュレーション結果を示している。位置ｐのグラフは、目標位置ｐ_ｄ＝１ｍへ向かう質点Ｍの挙動を示しており、時刻ｔ＝２．０～３．０ｓの大きな外乱が取り除かれた後、正しく収束していることがわかる。また、制御器のパラメータに誤差を設定し、かつ未知の外乱が加わっている状況であっても正しく収束しており、本発明に係る制御アルゴリズムによって適切な制御が行われていることがわかる。なお、アクチュエータの発生力ｆ_ｍが正弦波状に振動しているのは、制御器が正弦波状の外力に抗う力を、発生しているからである。

【0079】

以上、説明したように、本実施の形態に係る位置制御装置及び位置制御方法の漸近収束型コントローラでは、簡素な制御アルゴリズムで位置制御を行うことができるとともに、目標位置の変化及び外乱による軌道のずれに対応可能であり、小さな計算コストで位置制御を行うことが可能である。また、滑らかで指数関数的な収束を実現する制御アルゴリズムによって制御対象の位置制御を行うことができる。

【0080】

上記各実施の形態では、速度センサ２９を用いて各シリンダ２４～２６の伸縮速度を計測することとしたが、これに限られない。例えば、各角度センサ２８１～２８３で計測された角度データに基づいて、演算部３１１が各シリンダ２４～２６の速度を算出することとしてもよい。

【0081】

また、上記各実施の形態では、各シリンダ２４～２６について適用される制御アルゴリズムは、有限時間収束型の制御アルゴリズム又は漸近収束型の制御アルゴリズムのいずれか一方を選択して適用することとしたが、これに限られない。例えば、ブームシリンダ２４及びアームシリンダ２５に有限時間収束型制御アルゴリズムを適用し、バケットシリンダ２６に漸近収束型制御アルゴリズムを適用することとしてもよく、その他の制御アルゴリズムと組み合わせて位置制御を行うこととしてもよい。これにより、液圧アクチュエータを含む各部の動特性に適した制御アルゴリズムを、液圧アクチュエータごとに選択して適用することができる。

【0082】

また、上記実施の形態に係る有限時間収束型及び漸近収束型のいずれの制御アルゴリズムにおいても、参照速度の算出には現在速度を用いないこととしているが、現在速度を用いる別のアルゴリズムによって参照速度を求めることも可能である。すなわち、機械の目標位置と目標速度との少なくともいずれか一方、及び現在位置と現在速度との少なくともいずれか一方に基づいて、参照速度を算出することとしてもよい。

【0083】

また、機械の動特性モデルのパラメータ＾Ｍ及び推定外力＾ｆは、時間の経過とともに変化することとしてもよい。例えば、図１のショベルの動作制御の場合、＾Ｍは慣性パラメータであるので、各シリンダ２４～２６の長さ（ショベルの各関節の角度）によって変化する。この場合、設計時の公称値と各関節の角度情報から、＾Ｍを推定することができる。また、各リンクの速度の測定値、設計時の公称値より得られる遠心力、コリオリ力、摩擦力などの和として、推定外力＾ｆを推定することができる。

【0084】

なお、一般に、油圧システムには応答の遅れ（無駄時間）が存在する。具体的には、コントローラから送信された指令値が液圧アクチュエータのバルブ開度に反映されるまでに、０．０１秒から０．１秒程度の無駄時間が存在する。この場合、上述の制御アルゴリズムに係る現在位置及び現在速度を、所定ステップ先の時刻ステップの予測位置と予測速度に読み替えて実装することにより、無駄時間の悪影響を低減することができる。この予測位置と予測速度は、機械の動特性モデル（式（１ａ）、（１ｂ））及び液圧アクチュエータの準静的特性モデル（式（１ｃ））を用いて得ることができる。

【0085】

上述の予測位置を用いた実装形態は、図６のように表すことができる。図６内の予測器は、プラント公称モデルを表す式（７）から導くことができる。式（７）は、定理１を用いて下記のように等価に書き直すことができる。

【数16】

【0086】

上記の式（３１）は、１ステップ先の速度ｖ_ｋ＋１を予測する予測器として用いることができる。また、式（３１）で得られたｖ_ｋ＋１を式（１５）に代入することにより、１ステップ先の位置ｐ_ｋ＋１を予測する予測器として用いることができる。図６に示すように、この予測器をｎ個直列に接続して用いることにより、ｎステップ先の予測位置と予測速度を得ることができる。図６中のｎステップ予測器は、以下に示すアルゴリズムで表すことができる。

【数17】

【0087】

液圧アクチュエータと液圧アクチュエータで駆動される可動部とを備える制御対象が、ｎステップの無駄時間を内包すると推定されるとき、上記のアルゴリズムで得られる＾ｐ及び＾ｖを制御則（２８）～（３０）のｐ_ｋ及びｖ_ｋにそれぞれ代えて用いることにより、時間遅れの影響を低減することができる。

【産業上の利用可能性】

【0088】

本発明は、液圧アクチュエータで動作する機械の位置制御に好適である。特に、油圧アクチュエータによって動作する建設機械の自動位置決め制御に好適である。

【符号の説明】

【0089】

１ ショベル、１１ 下部走行体、１２ 上部旋回体、２０ 作業装置、２１ ブーム、２２ アーム、２３ バケット、２３ａ バケット先端、２４ ブームシリンダ、２５ アームシリンダ、２６ バケットシリンダ、２７ 旋回モータ、２８ 位置センサ、２８１ ブーム角度センサ、２８２ アーム角度センサ、２８３ バケット角度センサ、２８４ 旋回角度センサ、２９ 速度センサ、２９１ ブーム速度センサ、２９２ アーム速度センサ、２９３ バケット速度センサ、２９４ 旋回角速度センサ、３０ 制御ユニット、３１ 制御部、３１１ 演算部、３１２ 動作制御部、３２ 記憶部、３３ 表示部、３４ 入力部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】