特開 2024-61046 制御装置、制御方法、及び制御プログラム

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024061046

(43)【公開日】2024-05-07

(54)【発明の名称】制御装置、制御方法、及び制御プログラム

(51)【国際特許分類】

   B25J 13/08 20060101AFI20240425BHJP

【ＦＩ】

B25J13/08 Z

【審査請求】未請求

【請求項の数】11

【出願形態】ＯＬ

(21)【出願番号】P 2022168713

(22)【出願日】2022-10-20

【新規性喪失の例外の表示】特許法第３０条第２項適用申請有り 令和４年１０月１２日 ｈｔｔｐｓ：／／ｍｅｄｉｕｍ．ｃｏｍ／ｓｉｎｉｃｘ－ｊａ／ｉｒｏｓ２０２２％Ｅ３％８１％ＡＢ％Ｅ７％９Ｂ％Ｂ８％Ｅ８％Ａ３％９Ｃ％Ｅ６％８０％Ａ７％Ｅ６％９Ｄ％Ａ１％Ｅ４％ＢＢ％Ｂ６％Ｅ３％８２％９２％Ｅ７％９４％Ａ８％Ｅ３％８１％８４％Ｅ３％８１％９Ｆ％Ｅ７％８９％Ａ９％Ｅ４％ＢＤ％９３％Ｅ６％９３％８Ｄ％Ｅ４％ＢＤ％９Ｃ％Ｅ３％８１％ＡＥ％Ｅ７％Ａ０％９４％Ｅ７％Ａ９％Ｂ６％Ｅ３％８１％８Ｃ％Ｅ６％８Ｅ％Ａ１％Ｅ６％８Ａ％９Ｅ％Ｅ３％８１％９５％Ｅ３％８２％８Ｃ％Ｅ３％８１％ＢＥ％Ｅ３％８１％９７％Ｅ３％８１％９Ｆ－４０５５４４８４ｆ１８ｅにて公開 令和４年１０月１３日 ｈｔｔｐｓ：／／ｗｗｗ．ｏｍｒｏｎ．ｃｏｍ／ｊｐ／ｊａ／ｎｅｗｓ／２０２２／１０／ｃ１０１３．ｈｔｍｌ にて公開 令和４年１０月１９日 ｈｔｔｐｓ：／／ｒａｓ．ｐａｐｅｒｃｅｐｔ．ｎｅｔ／ｃｏｎｆｅｒｅｎｃｅｓ／ｃｏｎｆｅｒｅｎｃｅｓ／ＩＲＯＳ２２／ｐｒｏｇｒａｍ／ＩＲＯＳ２２＿ＣｏｎｔｅｎｔＬｉｓｔＷｅｂ＿２．ｈｔｍｌ＃ｍｏａ－４＿０３ にて公開

(71)【出願人】

【識別番号】000002945

【氏名又は名称】オムロン株式会社

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】田中 一敏

(72)【発明者】

【氏名】谷合 竜典

(72)【発明者】

【氏名】片山 想太郎

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707BS09

3C707DS01

3C707KS16

3C707KS31

3C707KS33

3C707KS36

3C707KT03

3C707LS20

3C707LU06

3C707LW03

(57)【要約】

【課題】制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御する。
【解決手段】制御装置１６は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態を計算する。そして、制御装置１６は、計算された時刻ｔ＋１の状態となるように、制御対象を制御する。
【選択図】図１

【特許請求の範囲】

【請求項1】

時刻ｔの制御対象の状態を取得する取得部と、
時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する計算部と、
計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する制御部と、
を含む制御装置。

【請求項2】

前記計算部は、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件、及び前記接触点が接する接触面における接線接触速度に関する相補条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する、
請求項１に記載の制御装置。

【請求項3】

前記制御対象は、ロボット及び対象物体であり、
前記状態のデータは、前記ロボット及び前記対象物体の状態をセンシングするセンサによって得られたセンサデータに基づき計算され、
前記計算部は、時刻ｔの前記状態に基づいて、前記ロボットの接触点、前記対象物体の接触点、及び他の物体の接触点の何れかの間において発生する接触力と、前記ロボットの接触点、前記対象物体の接触点、及び前記他の物体の接触点の何れかの間の距離とに関する前記相補性条件が満たされるように、時刻ｔ＋１の前記状態を計算する、
請求項１又は請求項２に記載の制御装置。

【請求項4】

前記相補性条件は、
複数の前記接触点の各々についての、
時刻ｔの状態ｑ_ｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φ_ｉ（ｑ_ｔ）とに関する以下の相補性条件（４ａ）と、
時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における、第１水平方向の接触力λ_ｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉとに関する以下の相補性条件（４ｂ）と、
時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における、第２水平方向の接触力λ_ｆｉ－と、前記符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）及び前記接線接触速度の絶対値γ_ｉとに関する以下の相補性条件（４ｃ）と、
ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γ_ｉと、前記接触面の摩擦係数μ_ｉと、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とに関する以下の相補性条件（４ｄ）とを含んで構成されている、
請求項３に記載の制御装置。

【数1】

【請求項5】

前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（４ａ）～（４ｄ）を満たし、かつ以下の目的関数（５）が最小となるような時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、
請求項４に記載の制御装置。

【数2】

ただし、ｇ（ｑ_ｔ＋１）は前記状態ｑ_ｔ＋１に対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔ＋１の目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆは予め与えられる正定重み行列であり、λ_ｔ＋１は、複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔ＋１のベクトルである。

【請求項6】

前記相補性条件は、
複数の前記接触点の各々についての、
時刻ｔの状態ｑ_ｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、前記接触力λ_ｎｉの変化量Δλ_ｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φ_ｉ（ｑ_ｔ）と、前記状態ｑ_ｔにより前記距離φ_ｉ（ｑ_ｔ）を微分することにより得られるΦ_ｉ，ｑ（ｑ_ｔ）と、前記状態ｑ_ｔの変化量Δｑ_ｔとに関する以下の線形相補性条件（７ａ）と、
時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における第１水平方向の接触力λ_ｆｉ＋と、前記接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記一般化速度ｖ_ｔにより前記接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉと、前記状態ｑ_ｔの変化量Δｑ_ｔとに関する以下の線形相補性条件（７ｂ）と、
時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における第２水平方向の接触力λ_ｆｉ－と、前記接触力λ_ｆｉーの変化量Δλ_ｆｉーと、前記符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉと、前記一般化速度ｖ_ｔにより前記接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）と、に関する以下の線形相補性条件（７ｃ）と、
ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γ_ｉと、前記接触面の摩擦係数μ_ｉと、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記接触力λ_ｎｉの変化量Δλ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第１水平方向の接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－と、前記第２水平方向の接触力λ_ｆｉ－の変化量Δλ_ｆｉ－と、に関する以下の線形相補性条件（７ｄ）とを含んで構成されている、
請求項３に記載の制御装置。

【数3】

【請求項7】

前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（７ａ）～（７ｄ）を満たし、かつ以下の目的関数（６）が最小となるような前記状態の変化量Δｑ_ｔを計算することにより、時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、
請求項６に記載の制御装置。

【数4】

ただし、ｇ_ｔは前記状態ｑ_ｔに対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆ予め与えられる正定重み行列であり、λ_ｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλ_ｔは前記λ_ｔの変化量である。

【請求項8】

前記相補性条件には、前記相補性条件を緩和するための以下の緩和変数ｓ_ｉが含まれている、
請求項５に記載の制御装置。

【数5】

【請求項9】

前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（９ａ）～（９ｄ）を満たし、かつ以下の目的関数（１０）が最小となるような前記状態の変化量Δｑ_ｔと前記接触力の変化量Δλ_ｔと緩和変数ｓ_ｉを各成分として持つベクトルｓ_ｔとを計算することにより、時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、
請求項８に記載の制御装置。

【数6】

ただし、ｇ_ｔは前記状態ｑ_ｔに対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆ予め与えられる正定重み行列であり、λ_ｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλ_ｔは前記λ_ｔの変化量である。

【請求項10】

時刻ｔの制御対象の状態を取得し、
時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、
計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、
処理をコンピュータが実行する制御方法。

【請求項11】

時刻ｔの制御対象の状態を取得し、
時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、
計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、
処理をコンピュータに実行させるための制御プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、制御装置、制御方法、及び制御プログラムに関する。

【背景技術】

【0002】

従来、ロボットが外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測する技術が知られている（例えば、特許文献１を参照）。

【0003】

また、ロボットが物体に接触する際の目標軌道を達成するために、どのように物体に接触すべきかに関する技術が知られている（例えば、非特許文献１を参照）。非特許文献１では、ロボットが物体に接触する際の大域的な最適化モデルが提案されている。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００７－３００５４号公報

【非特許文献】

【0005】

【非特許文献1】Bernardo Aceituno-Cabezas and Alberto Rodriguez, "A Global Quasi-Dynamic Model for Contact-Trajectory Optimization", Robotics: Science and Systems 2020 Corvalis, Oregon, USA, July 12-16, 2020, ［令和4年9月27日検索］，インターネット＜URL：https://www.researchgate.net/publication/342882025>

【発明の概要】

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

【0006】

ところで、例えば、ロボットが物体と接触するような場面においては、ロボットと物体との間の接触態様（以下、単に接触モードとも称する。）が逐次変化する。これにより、ロボットの動作に関する物理的な制約も逐次変化する。このため、ロボットと物体との接触が多い場合、そのロボットの動作を適切に制御することは難しい。

【0007】

本発明は、上記の点に鑑みてなされたものであり、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することを目的とする。

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

【0008】

上記目的を達成するために、本開示に係る制御装置は、時刻ｔの制御対象の状態を取得する取得部と、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する計算部と、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する制御部と、を含む制御装置である。

【0009】

また、前記計算部は、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件、及び前記接触点が接する接触面における接線接触速度に関する相補条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算するようにすることができる。

【0010】

なお、制御対象とは、制御装置によって制御される機械と、その機械により作用を及ぼされる対象物体のことを示し、機械はロボットを含み得る。また、状態とは、前記機械および前記対象物の位置および姿勢を含み得る。また、時刻ｔ＋１とは、時刻ｔからの計算上の単位時間進んだ時刻を示し、制御装置のサイクルタイム、コンピュータの計算のサイクルタイム、コンピュータによるシミュレーションのサイクルタイムのことを含み得る。また、目標値とは、前記機械または／および対象物体が到達するあらかじめ設定した状態であり、座標および姿勢を含みうる。また、最小とは、目的関数から計算される値であるが、計算の手法により誤差を含み得る。

【0011】

また、前記制御対象は、ロボット及び対象物体であり、前記状態のデータは、前記ロボット及び前記物体の状態をセンシングするセンサによって得られたセンサデータに基づき計算され、前記計算部は、時刻ｔの前記状態に基づいて、前記ロボットの接触点、前記対象物体の接触点、及び他の物体の接触点の何れかの間において発生する接触力と、前記ロボットの接触点、前記対象物体の接触点、及び前記他の物体の接触点の何れかの間の距離とに関する前記相補性条件が満たされるように、時刻ｔ＋１の前記状態を計算するようにすることができる。

【0012】

また、前記相補性条件は、複数の前記接触点の各々についての、時刻ｔの前記状態ｑ_ｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φ_ｉ（ｑ_ｔ）とに関する以下の相補性条件（４ａ）と、時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における、第１水平方向の接触力λ_ｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉとに関する以下の相補性条件（４ｂ）と、時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における、第２水平方向の接触力λ_ｆｉ－と、前記符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）及び前記接線接触速度の絶対値γ_ｉとに関する以下の相補性条件（４ｃ）と、ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γ_ｉと、前記接触面の摩擦係数μ_ｉと、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とに関する以下の相補性条件（４ｄ）とを含んで構成されるようにすることができる。

【0013】

【数1】

【0014】

また、前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（４ａ）～（４ｄ）を満たし、かつ以下の目的関数（５）が最小となるような時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、ようにすることができる。

【0015】

【数2】

【0016】

ただし、ｇ（ｑ_ｔ＋１）は前記状態ｑ_ｔ＋１に対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔ＋１の目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆは予め与えられる正定重み行列であり、λ_ｔ＋１は、複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔ＋１のベクトルである。

【0017】

また、前記相補性条件は、複数の前記接触点の各々についての、時刻ｔの前記状態ｑ_ｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、前記接触力λ_ｎｉの変化量Δλ_ｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φ_ｉ（ｑ_ｔ）と、前記状態ｑ_ｔにより前記距離φ_ｉ（ｑ_ｔ）を微分することにより得られるΦ_ｉ，ｑ（ｑ_ｔ）と、前記状態ｑ_ｔの変化量Δｑ_ｔとに関する以下の線形相補性条件（７ａ）と、時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における第１水平方向の接触力λ_ｆｉ＋と、前記接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記一般化速度ｖ_ｔにより前記接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉと、前記状態ｑ_ｔの変化量Δｑ_ｔとに関する以下の線形相補性条件（７ｂ）と、時刻ｔの前記状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の前記接触点が接する前記接触面における第２水平方向の接触力λ_ｆｉ－と、前記接触力λ_ｆｉーの変化量Δλ_ｆｉーと、前記符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、前記接線接触速度の絶対値γ_ｉと、前記一般化速度ｖ_ｔにより前記接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）と、に関する以下の線形相補性条件（７ｃ）と、ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γ_ｉと、前記接触面の摩擦係数μ_ｉと、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記接触力λ_ｎｉの変化量Δλ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第１水平方向の接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－と、前記第２水平方向の接触力λ_ｆｉ－の変化量Δλ_ｆｉ－と、に関する以下の線形相補性条件（７ｄ）とを含んで構成されるようにすることができる。

【0018】

【数3】

【0019】

また、前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（７ａ）～（７ｄ）を満たし、かつ以下の目的関数（６）が最小となるような前記状態の変化量Δｑ_ｔを計算することにより、時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、ようにすることができる。

【0020】

【数4】

【0021】

ただし、ｇ_ｔは前記状態ｑ_ｔに対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆ予め与えられる正定重み行列であり、λ_ｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλ_ｔは前記λ_ｔの変化量である。

【0022】

また、前記相補性条件には、前記相補性条件を緩和するための以下の緩和変数ｓ_ｉが含まれている、ようにすることができる。

【0023】

【数5】

【0024】

また、前記計算部は、時刻ｔの前記状態ｑ_ｔに基づいて、前記相補性条件（９ａ）～（９ｄ）を満たし、かつ以下の目的関数（１０）が最小となるような前記状態の変化量Δｑ_ｔと前記接触力の変化量Δλ_ｔと緩和変数ｓ_ｉを各成分として持つベクトルｓ_ｔとを計算することにより、時刻ｔ＋１の前記状態ｑ_ｔ＋１を計算する、ようにすることができる。

【0025】

【数6】

【0026】

ただし、ｇ_ｔは前記状態ｑ_ｔに対応する前記制御対象の物理量を表し、ｇ_ｒｅｆは前記状態ｑ_ｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑ_ｇ，Ｑ_ｖ，及びＱ_ｆ予め与えられる正定重み行列であり、λ_ｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λ_ｎｉと、前記第１水平方向の接触力λ_ｆｉ＋と、前記第２水平方向の接触力λ_ｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλ_ｔは前記λ_ｔの変化量である。

【0027】

また、本開示の制御方法は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、処理をコンピュータが実行する制御方法である。

【0028】

また、本開示の制御プログラムは、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、処理をコンピュータに実行させるための制御プログラムである。

【発明の効果】

【0029】

本開示の制御装置、方法、及びプログラムによれば、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することができる。

【図面の簡単な説明】

【0030】

【図1】本実施形態の概要を説明するための図である。

【図2】本実施形態に係る制御システムが実行する処理の流れを説明するための図である。

【図3】本実施形態で用いる状態ｑ_ｔを説明するための図である。

【図4】接触点と接触面との間の距離と接触力との間の関係を説明するための図である。

【図5】接触点と接触面との間のクーロン摩擦係数を説明するための図である。

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

【図7】本実施形態に係る制御装置のハードウェア構成を示すブロック図である。

【図8】本実施形態における制御処理の流れを示すフローチャートである。

【図9】本実施例を説明するための図である。

【図10】本実施例の結果を表す図である。

【図11】本実施例の結果を説明するための図である。

【図12】本実施例の結果を表す図である。

【図13】従来技術を説明するための図である。

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

【0031】

以下、本開示の実施形態の一例を、図面を参照しつつ説明する。本実施形態では、本開示に係る制御装置を搭載した制御システムを例に説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

【0032】

＜実施形態の概要＞
図１３は、従来技術の一例を説明するための図である。図１３に示されるように、従来技術では、時刻ｔ_１における物体Ｂとロボットの把持部Ｇとの間の接触モードに応じた制御１、時刻ｔ_２における接触モードに応じた制御２、及び時刻ｔ_３における接触モードに応じた制御３が実行される。この場合には、対象物体Ｂとロボットの把持部Ｇとの間の接触モードに応じてロボットの動作を制御する必要がある。しかし、その接触モードの数は膨大なものとなるため、膨大な数の接触モードに応じてロボットを連続的に制御することは難しい、という課題がある。

【0033】

そこで、本実施形態では、物体間の接触に関する相補性条件が満たされるように、ロボットの動作を連続的に制御する。これにより、ロボットの動作をリアルタイムに制御することができる。

【0034】

図１は、本実施形態の概要を説明するための図である。図１に示されるように、本実施形態では、対象物体Ｂとロボットの把持部Ｇと他の物体である壁Ｗ又は床面ＦＬとの接触に関する相補性条件が考慮される。対象物体Ｂとロボットの把持部Ｇとは、本開示の制御対象の一例である。

【0035】

具体的には、図１に示されるように、時刻ｔ_１においては、ロボットの把持部Ｇは対象物体Ｂと接触していないため把持部Ｇの接触点ｐ_Ｇと対象物体Ｂとの間の距離はゼロよりも大きく、ロボットの把持部Ｇの接触点ｐ_Ｇにおいて発生する接触力はゼロである。なお、時刻ｔ_１においては、接触点ｐ_Ｇは対象物体Ｂと実際に接触していないため、接触候補点でもある。

【0036】

一方、図１に示されるように、時刻ｔ_２においては、ロボットの把持部Ｇは対象物体Ｂと接触しているため把持部Ｇの接触点ｐ_Ｇと対象物体Ｂとの間の距離はゼロとなり、ロボットの把持部Ｇの接触点ｐ_Ｇにおいてゼロよりも大きい接触力λ_４が発生する。

【0037】

更に、図１に示されるように、時刻ｔ_３においては、ロボットの把持部Ｇは対象物体Ｂと接触していないため把持部Ｇの接触点ｐ_Ｇと対象物体Ｂとの間の距離はゼロよりも大きく、ロボットの把持部Ｇの接触点ｐ_Ｇにおいて発生する接触力はゼロとなる。

【0038】

なお、図１に示されるように、対象物体Ｂには複数の接触点ｐ_１，ｐ_２，ｐ_３，ｐ_４がある。例えば、時刻ｔ_１においては、接触点ｐ_１と床面ＦＬとの間において接触力λ_１が発生し、接触点ｐ_２と床面ＦＬとの間において接触力λ_２が発生する。また、時刻ｔ_２においては、接触点ｐ_１と床面ＦＬとの間において接触力λ_１が発生し、接触点ｐ_３と壁Ｗとの間において接触力λ_３が発生する。また、時刻ｔ_３においては、接触点ｐ_１と床面ＦＬとの間において接触力λ_１が発生し、接触点ｐ_３と床面ＦＬとの間において接触力λ_３が発生する。

【0039】

図１に示されるように、ロボットの把持部Ｇと対象物体Ｂと壁Ｗ又は床面ＦＬとの間において、各々の距離がゼロよりも大きい場合には接触力がゼロとなる。一方、ロボットの把持部Ｇと対象物体Ｂと壁Ｗ又は床面ＦＬとの間において、各々の距離がゼロである場合には接触力がゼロよりも大きくなる。このため、物体間の距離と物体間で発生する接触力との間には相補性条件が成立する。

【0040】

そこで、本実施形態では、物体間の接触に関する相補性条件が満たされるように、ロボットの動作を連続的に制御する。これにより、図１３の従来技術のような複数の接触モードに基づくロボットの制御をすることなく、ロボットの動作をリアルタイムに制御することができる。なお、２つのスカラー変数であるａとｂとの相補性条件は以下の式（１）によって表現される。

【0041】

【数7】

【0042】

なお、ａ，ｂは次式を満たす。

【0043】

【数8】

【0044】

なお、以下では、本実施形態において提案される手法を、ＬＣＱＰ（Ｌｉｎｅａｒ ｃｏｍｐｌｅｍｅｎｔａｒｉｔｙ ｑｕａｄｒａｔｉｃ ｐｒｏｇｒａｍ）とも称する。

【0045】

図２は、本実施形態に係る制御システムが実行する処理の流れを説明するための図である。図１に示されるように、本実施形態の制御装置１６は、時刻ｔの制御対象の状態ｑ_ｔを取得し、目標値との差異が小さくなるような時刻ｔ＋１の制御対象の状態ｑ_ｔ＋１を計算する。その際、制御装置１６は、物体間の接触に関する相補性条件が満たされ、かつ目標値と時刻ｔ＋１の制御対象の状態ｑ_ｔ＋１との間の差異を表す目的関数が小さくなるように、時刻ｔ＋１の制御対象の状態ｑ_ｔ＋１を計算する。そして、制御装置１６は、時刻ｔ＋１の制御対象の状態ｑ_ｔ＋１を制御入力としてロボット１４へ出力する。これにより、制御対象の変化に応じて接触モードを逐次設定することなく、ロボットの動作をリアルタイムに制御することができる。また、本実施形態では、物体間の接触に関する相補性条件が満たされているか否かを判定するのみでよいため、ロボットの動作をリアルタイムに制御する際の計算コストを低減させることができる。

【0046】

図３は、本実施形態で用いる状態ｑ_ｔを説明するための図である。図３は、システム全体が把持部Ｇと対象物体Ｂとにより構成される準静的定式化の一例を示す図でもある。図３に示されるように、本実施形態では、把持部Ｇの所定位置を原点とする把持部座標系における座標（ｘ_ａ，ｙ_ａ）と、把持部Ｇの姿勢を表す姿勢角θ_ａと、把持部Ｇが備えるグリッパー間の距離ｒ_ａとを各成分として持つ状態ベクトルｑ_ａが設定される。また、図３に示されるように、本実施形態では、対象物体Ｂの所定位置を原点とする対象物体座標系における座標（ｘ_０，ｙ_０）と、対象物体Ｂの姿勢を表す姿勢角θ_０とを各成分として持つ状態ベクトルｑ_ｕが設定される。なお、把持部Ｇの全自由度（DOF : Degree of freedom）をＮ_ｕとし、対象物体Ｂの全自由度をＮ_ａとした場合、一般化座標における状態ベクトルｑ_ａと状態ベクトルｑ_ｕとは、以下の式によって定義される。なお、以下で説明する数式内の太字はベクトル又は行列を表す。

【0047】

【数9】

【0048】

制御対象の状態ｑと各物体の速度ｖは、次式によって表される。なお、速度ｖは一般化速度でもある。

【0049】

【数10】

【0050】

なお、準静的モデルを離散時間へ拡張することにより、以下の式が導出される。ｈは０以上の予め設定される係数である。

【0051】

【数11】

【0052】

次に、接触点と接触面との間において発生する力である接触力について説明する。図３に示されるように、対象物体Ｂの隅と把持部Ｇが備えるグリッパーの隅を接触点ｐ_Ｇ１，ｐ_Ｇ２として設定する。ここで、ｉ番目の接触点と接触面との間において発生する各時刻ｔの接触力λ_ｔ，ｉが以下のように定義される。

【0053】

【数12】

【0054】

λ_ｆｉは、ｉ番目の接触点が接する接触面における水平方向の接触力である。λ_ｎｉは、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力である。なお、本実施形態では、対象物体Ｂがボックスである場合を例に説明するが、他の種類の物体（例えば、球状の物体）に関しても同様に相補性条件を定式化することができる。

【0055】

次に、対象物体Ｂに相当するオブジェクトｏにかかる力とモーメントとの釣り合いを導出するために、接触点と接触面とのペアの集合Ｃ_ｏを導入する。接触点と接触面とのペアを識別するためのインデックスｉは、次式によって定義される。

【0056】

また、接触点と接触面とのペアを識別するためのインデックスｉは、次式によって定義される接触面の回転行列Ｒ_ｉ（ｑ_ｔ）及び接触面の質量中心からの相対位置［ｒ_{ｏ，ｉ，ｘ}（ｑ_ｔ），ｒ_{ｏ，ｉ，ｙ}（ｑ_ｔ）］^Ｔに関連付けられる。

【0057】

【数13】

【0058】

準静的な仮定下（例えば、速度と加速度とが十分に小さい）において、オブジェクトｏに関するニュートンの第２法則は、以下の式（３）によって表される。

【0059】

【数14】

【0060】

なお、上記式のｒ^～ _ｏ，ｉ（ｑ_ｔ）は次式によって表される。

【0061】

【数15】

【0062】

上記式（３）が表すベクトルの１次元目の成分と２次元目の成分とは、世界座標系においてオブジェクトｏに作用する力を表す。同様に、上記式（３）の３次元目の成分がモーメントを表す。また、上記式（３）のかっこ内に記載されている力とモーメントとはオブジェクトｏにかかる接触力を表す。また、次式によって定義される力Ｆ_ｏは重力等の時間的に一定の力を表す。上記式（３）に示されている各値は、既知の技術によって計算可能である。

【0063】

【数16】

【0064】

［接触に関する相補性条件］
本実施形態では、接触点と接触面とのペアを識別するためのインデックスｉの各々において、時刻ｔに発生する接触力ベクトルλ_ｔ，ｉが定義される。上述したように、この接触力ベクトルλ_ｔ，ｉは、ｉ番目の接触点と接触面とのペアにおける接触面の垂直方向に発生する接触力λ_ｎｉと、ｉ番目の接触点と接触面とのペアにおける接触面の水平方向に発生する接触力λ_ｆｉとを各成分として持つ。図４は、ｉ番目の接触点と接触面との間の距離φ_ｉ（ｑ）と接触力λ_ｎｉとの間の関係を説明するための図である。図４に示されるように、距離φ_ｉ（ｑ）＝０である場合には接触力λ_ｎｉ＞０となり、距離φ_ｉ（ｑ）＞０である場合には接触力λ_ｎｉ＝０となる。

【0065】

また、上記図１に示されるように、ロボット１４の把持部Ｇの接触点と対象物体Ｂとが接触し、把持部Ｇの接触点と対象物体Ｂとの間の距離がゼロである場合に接触力λ_ｎｉが正となる。そのため、本実施形態では、時刻ｔにおけるｉ番目の接触点と接触面との間の距離φ_ｉ（ｑ_ｔ）を導入する。距離φ_ｉ（ｑ_ｔ）は次式によって表される。

【0066】

【数17】

【0067】

また、対象物体Ｂ又は把持部Ｇが接触点において滑らないためには、接触力ベクトルλ_ｔ，ｉが、接触点と接触面との摩擦によって形成される摩擦円錐内に位置している必要がある。そのため、本実施形態では、時刻ｔにおけるｉ番目の接触点と接触面とにおける接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を導入する。接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）は、次式によって表される。

【0068】

【数18】

【0069】

更に、物体間の接触に関する相補性条件を表現するために、本実施形態では、接触面の水平方向に発生する非負の２つの接触力λ_ｆｉ＋，λ_ｆｉ－を導入する。２つの接触力λ_ｆｉ＋，λ_ｆｉ－は、次式によって表される。

【0070】

【数19】

【0071】

図５は、接触点と接触面との間のクーロン摩擦係数を説明するための図である。図５に示される黒丸は接触点を表し、実線の矢印は接触力を表し、破線の矢印は接触速度を表す。対象物体Ｂ又は把持部Ｇが接触点において滑らないためには、鉛直方向に発生する接触力λ_ｎｉと水平方向に発生する接触力λ_ｆｉ＋，λ_ｆｉ－とが、図５に示されるような関係である必要がある。図５に示される実線の矢印は接触力を表し、破線の矢印は対象物体Ｂが滑ることにより生じる接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）の絶対値γ_ｉを表す。また、μ_ｉは接触点と接触面との間の摩擦係数を表し、μ_ｉ＞０である。なお、絶対値γ_ｉは次式によって定義される。

【0072】

【数20】

【0073】

図５の（Ａ）の場合、接触力ベクトルλ_ｔ，ｉが摩擦円錐Ｃの右側の境界に位置しているため、対象物体Ｂの接触点において滑りが発生する。このため、図５の（Ａ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。

【0074】

【数21】

【0075】

上記式に示されているように、図５の（Ａ）の場合、接触力λ_ｎｉが正の値となり、第１水平方向に発生する接触力λ_ｆｉ＋が正の値となり、第１水平方向とは逆向きの第２水平方向に発生する接触力λ_ｆｉ－がゼロとなり、第１水平方向を正とする接触速度ψ_ｉ（ｑ，ｑ^・）＝ψ_ｉ（ｑ_ｔ，ｖ_ｔ）が負となり、接触速度の絶対値γ_ｉが正となる。また、図５の（Ａ）の場合、接触点において発生する摩擦力μ_ｉλ_ｎｉから第１水平方向に発生する接触力λ_ｆｉ＋と第２水平方向に発生する接触力λ_ｆｉ－とを差し引いた値がゼロとなる。これにより、図５の（Ａ）の場合、対象物体Ｂの接触点において滑りが発生する。

【0076】

また、図５の（Ｂ）の場合、接触力ベクトルλ_ｔ，ｉが摩擦円錐Ｃの内側に位置しているため、対象物体Ｂに滑りは発生しない。このため、図５の（Ｂ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。

【0077】

【数22】

【0078】

上記式に示されているように、図５の（Ｂ）の場合、接触力λ_ｎｉが正の値をとり、第１水平方向に発生する接触力λ_ｆｉ＋が正の値をとり、第２水平方向に発生する接触力λ_ｆｉ－がゼロとなり、第１水平方向を正とする接触速度ψ_ｉ（ｑ，ｑ^・）＝ψ_ｉ（ｑ_ｔ，ｖ_ｔ）がゼロとなり、接触速度の絶対値γ_ｉがゼロとなる。また、図５の（Ｂ）の場合、接触点において発生する摩擦力μ_ｉλ_ｎｉから第１水平方向に発生する接触力λ_ｆｉ＋と第２水平方向に発生する接触力λ_ｆｉ－とを差し引いた値が正の値となる。これにより、図５の（Ｂ）の場合、対象物体Ｂに滑りは発生しない。

【0079】

また、図５の（Ｃ）の場合、接触力ベクトルλ_ｔ，ｉが摩擦円錐Ｃの左側の境界に位置しているため、対象物体Ｂの接触点において滑りが発生する。このため、図５の（Ｃ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。

【0080】

【数23】

【0081】

上記式に示されているように、図５の（Ｃ）の場合、接触力λ_ｎｉが正の値となり、第１水平方向に発生する接触力λ_ｆｉ＋がゼロとなり、第２水平方向に発生する接触力λ_ｆｉ－が正の値となり、第１水平方向を正とする接触速度ψ_ｉ（ｑ，ｑ^・）＝ψ_ｉ（ｑ_ｔ，ｖ_ｔ）が正の値となり、接触速度の絶対値γ_ｉが正の値となる。また、図５の（Ｃ）の場合、接触点において発生する摩擦力μ_ｉλ_ｎｉから第１水平方向に発生する接触力λ_ｆｉ＋と第２水平方向に発生する接触力λ_ｆｉ－とを差し引いた値がゼロとなる。これにより、図５の（Ｃ）の場合、対象物体Ｂの接触点において滑りが発生する。

【0082】

図５に示されるように、接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）がゼロではない場合に、第１水平方向に発生する接触力λ_ｆｉ＋及び第２水平方向に発生する接触力λ_ｆｉ－の何れか一方がゼロとなる。これは、接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）がゼロではない場合に、γ_ｉ＋ψ_ｉ（ｑ_ｔ，ｖ_ｔ）の値及びγ_ｉ－ψ_ｉ（ｑ_ｔ，ｖ_ｔ）の値の何れか一方がゼロではないためである。なお、前述したように、ψ_ｉ（ｑ_ｔ，ｖ_ｔ）は、第１方向を正とする符号付きの接触速度である。また、図５に示されるように、接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）は、第１水平方向に発生する接触力λ_ｆｉ＋又は第２水平方向に発生する接触力λ_ｆｉ－とは逆向きの符号となる。

【0083】

このため、物体間の接触に関する相補性条件として、次式が成立する。

【0084】

【数24】

【0085】

上記式（４ａ）は、物体間の距離φ_ｉ（ｑ_ｔ）がゼロである場合には接触力λ_ｎｉが正となり、物体間の距離φ_ｉ（ｑ_ｔ）が正の値である場合には接触力λ_ｎｉがゼロとなる、ということを表す相補性条件の式である。また、上記式（４ｂ）～（４ｄ）は、接触力ベクトルλ_ｔ，ｉが摩擦円錐Ｃの内側に位置している、ということを表す相補性条件である。また、上記式（４ｄ）は、図５の（Ａ）及び（Ｃ）にも示されているように、接触力ベクトルλ_ｔ，ｉが摩擦円錐Ｃの境界に位置している、ということを表す相補性条件である。

【0086】

このため、上述した物体間の接触に関する相補性条件は、相補性条件（４ａ）と、相補性条件（４ｂ）と、相補性条件（４ｃ）と、相補性条件（４ｄ）とを含んで構成される。相補性条件（４ａ）は、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件の一例である。相補性条件（４ｂ）～（４ｄ）は、接触点が接する接触面における接線接触速度に関する相補条件の一例である。

【0087】

上記式（４ａ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔにおける、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、ｉ番目の接触点と接触面との間の距離φ_ｉ（ｑ_ｔ）とに関する相補性条件である。

【0088】

上記式（４ｂ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の接触点が接する接触面における、第１水平方向の接触力λ_ｆｉ＋と、接触面の第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、接線接触速度の絶対値γ_ｉとに関する相補性条件である。

【0089】

上記式（４ｃ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の接触点が接する接触面における、第２水平方向の接触力λ_ｆｉ－と、符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）及び接線接触速度の絶対値γ_ｉとに関する相補性条件である。

【0090】

上記式（４ｄ）は、複数の接触点の各々についての、ｉ番目の接触点が接する接触面における、接線接触速度の絶対値γ_ｉと、接触面の摩擦係数μ_ｉと、接触面の鉛直方向に発生する接触力λ_ｎｉと、第１水平方向の接触力λ_ｆｉ＋と、第２水平方向の接触力λ_ｆｉ－とに関する相補性条件である。

【0091】

なお、上記の提案手法を３次元運動の場合に適用すると、多面体円錐の対角線毎に上記式（４ｂ）と（４ｃ）のペアが導入される。

【0092】

次に、本実施形態のＬＣＱＰを定式化する。ここで、接触力ベクトルλ_ｔが定義される。接触力ベクトルλ_ｔは、次式のλ_ｆｉ＋，λ_ｆｉ－，λ_ｎｉを各成分として持つベクトルである。

【0093】

【数25】

【0094】

また、以下の式（５）が導入される。本実施形では、上記式（２）～（４ｄ）が表す条件を満たしつつ、以下の式（５）が表す目的関数が最小となるような時刻_ｔ＋１の状態ｑ_ｔ＋１及び接触力ベクトルλ_ｔ＋１が計算される。

【0095】

【数26】

【0096】

ここで、上記式（５）のｇは、一般微分可能関数であり、状態ｑを、ロボット１４の把持部Ｇの位置及び姿勢と対象物体Ｂの位置及び姿勢の空間へ写像するための関数である。このため、ｇ（ｑ）は、状態ｑに対応する制御対象の物理量である。ｇ_ｒｅｆは、物理量の目標値ベクトルを表す。また、Ｑ_ｇ，Ｑ_ｖ，Ｑ_ｆは、予め与えられる正定重み行列である。Ｑ_ｇはタスクの優先度を表し、Ｑ_ｖ，Ｑ_ｆは、正規化項である。

【0097】

本実施形態のＬＣＱＰでは、既知の勾配降下法と同様の考え方に基づき、上記式（５）に示されている制約付き非線形目的関数を最適化する。具体的には、本実施形では、上記式（５）の目的関数が小さくなるような降下ステップを生成することにより、ロボット１４を動作させる制御を実行する。なお、各降下ステップは、線形化された式（５）の目的関数を解くことによって計算される。これにより、次の時間ｔ＋１の把持部Ｇ及び対象物体Ｂの位置と力とを表す状態ｑ_ｔ＋１及び接触力ベクトルλ_ｔ＋１が得られる。

【0098】

上記式（５）の目的関数が小さくなるような降下ステップの生成を繰り返すことにより、最終的に式（５）が最小化されるような目標状態ｑ^＊に到達する。

【0099】

本実施形態では、問題を扱いやすくするために、Δｑ_ｔ及びΔλ_ｔに基づいて定義される以下の式を導入する。

【0100】

【数27】

【0101】

Δｑ_ｔ及びΔλ_ｔが十分に小さい場合、上記式（５）の目的関数はΔｑ_ｔとΔλ_ｔの２次関数によって以下の式（６）のように近似される。

【0102】

【数28】

【0103】

ここで、上記式（６）のうちの各変数は以下の式によって定義される。

【0104】

【数29】

【0105】

上記式（２）を上記式（６）へ組み込むと、上記式（３）～（４ｄ）は以下のように線形化することができる。

【0106】

具体的には、時刻ｔにおけるΔｑ_ｔ，Δλ_ｎｉ，Δλ_ｆｉ＋，及びΔλ_ｆｉ－を用いて時刻ｔ＋１におけるｑ_ｔ＋１，λ_ｎｉ，λ_ｆｉ＋，及びλ_ｆｉ－を近似することにより、以下の式（７ａ）～（７ｄ）が導出される。

【0107】

【数30】

【0108】

なお、上記式（７ａ）～（７ｄ）における未定義の変数は次式によって定義される。

【0109】

【数31】

【0110】

なお、ＬＣＱＰが実行される際には、次式の関係式が上記式（７ａ）～（７ｄ）に代入される。

【0111】

【数32】

【0112】

このため、上述した物体間の接触に関する相補性条件は、線形相補性条件（７ａ）と、相補性条件（７ｂ）と、線形相補性条件（７ｃ）と、線形相補性条件（７ｄ）とを含んで構成される。

【0113】

上記式（７ａ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔにおける、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力λ_ｎｉと、接触力λ_ｎｉの変化量Δλ_ｎｉと、ｉ番目の接触点と接触面との間の距離φ_ｉ（ｑ_ｔ）と、状態ｑ_ｔにより距離φ_ｉ（ｑ_ｔ）を微分することにより得られるΦ_ｉ，ｑ（ｑ_ｔ）と、状態ｑ_ｔの変化量Δｑ_ｔとに関する線形相補性条件である。

【0114】

上記式（７ｂ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の接触点が接する接触面における第１水平方向の接触力λ_ｆｉ＋と、接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、接触面の第１水平方向を正とする符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、一般化速度ｖ_ｔにより接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）と、接線接触速度の絶対値γ_ｉと、状態ｑ_ｔの変化量Δｑ_ｔとに関する線形相補性条件である。

【0115】

上記式（７ｃ）は、複数の接触点の各々についての、時刻ｔの状態ｑ_ｔ及び時刻ｔの一般化速度ｖ_ｔにおける、ｉ番目の接触点が接する接触面における第２水平方向の接触力λ_ｆｉ－と、接触力λ_ｆｉーの変化量Δλ_ｆｉーと、符号付きの接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）と、接線接触速度の絶対値γ_ｉと、一般化速度ｖ_ｔにより接線接触速度ψ_ｉ（ｑ_ｔ，ｖ_ｔ）を微分することにより得られるΨ_ｉ，ｖ（ｑ_ｔ，ｖ_ｔ）とに関する線形相補性条件である。

【0116】

上記式（７ｄ）は、複数の接触点の各々についての、ｉ番目の接触点が接する接触面における、接線接触速度の絶対値γ_ｉと、接触面の摩擦係数μ_ｉと、接触面の鉛直方向に発生する接触力λ_ｎｉと、接触力λ_ｎｉの変化量Δλ_ｎｉと、第１水平方向の接触力λ_ｆｉ＋と、第１水平方向の接触力λ_ｆｉ＋の変化量Δλ_ｆｉ＋と、第２水平方向の接触力λ_ｆｉ－と、第２水平方向の接触力λ_ｆｉ－の変化量Δλ_ｆｉ－とに関する以下の線形相補性条件である。

【0117】

（力とモーメントの線形化）
上記式（３）が表す力とモーメントとの間の関係式は、次式（８）のように線形化される。

【0118】

【数33】

【0119】

（物体間の接触に関する相補性条件の緩和）
なお、実際には、物体間の接触に関する相補性条件（７ａ）～（７ｃ）を満たすような解が存在しない場合があり得る。例えば、センサ等によって測定された状態がノイズを含んでいるような場合又は対象物体Ｂを表すモデルが現実の対象物体Ｂを表現しきれていない場合（例えば、対象物体Ｂの形状モデルのモデリングエラー又は対象物体Ｂの変形等）により、物体間の接触に関する相補性条件（７ａ）～（７ｃ）が満たされないという場合もあり得る。

【0120】

また、物体間の接触に関する相補性条件（７ａ）～（７ｄ）により、作動するオブジェクトであるロボット１４の把持部Ｇが、対象物体Ｂと接触することできないという状況も発生し得る。例えば、非ゼロの鉛直方向の接触力λ_ｎｉが計算された場合であっても、その接触点の実際の位置は接触面からわずかにずれているという場合もあり得る。

【0121】

そこで、本実施形態では、これらの問題を解決するために、物体間における小さな貫通を許可する。具体的には、本実施形態では、上記式（７ａ）～（７ｄ）に対して、緩和変数ｓ_ｉを導入することにより、物体間の接触に関する相補性条件を緩和させる。緩和変数ｓ_ｉが導入された相補性条件は次式によって表される。

【0122】

【数34】

【0123】

なお、上記式（９ａ）～（９ｄ）を利用する場合には、ｐ_ｉｓ_ｉ ^２を上記式（６）の目的関数へ追加する。ｐ_ｉｓ_ｉ ^２が追加された目的関数は、以下の式（１０）によって表される。なお、以下の式におけるｐ_ｉは予め設定される係数である。

【0124】

【数35】

【0125】

本実施形態では、上記式（１０）を最小化するような時刻ｔのΔｑ_ｔ，Δλ_ｔ，及びｓ_ｔを計算する。時刻ｔのΔｑ_ｔ及びΔλ_ｔは、次の時刻ｔ＋１におけるｑ_ｔ＋１＝ｑ_ｔ＋Δｑ_ｔ及びλ_ｔ＋１＝λ_ｔ＋Δλ_ｔを計算する際に用いられる。ｓ_ｔは、緩和変数ｓ_ｉを各成分として持つベクトルである。ｑ_ｔ＋１及びλ_ｔ＋１には、作動システム（本実施形態では、ロボット１４の把持部Ｇ）の位置と力とが要素として含まれているため、これに基づいてロボット１４の把持部Ｇの動作が制御される。

【0126】

なお、ＬＣＱＰによって計算された値は、次式によって把持部Ｇの位置に関する制御信号ｑ^～ _ｔ＋１へと変換される。

【0127】

【数36】

【0128】

ここで、Ｋ＞０はユーザによって定義されるゲインである。上記の変換により、実際にはロボット１４がＬＣＱＰによって計算された力の指示信号を正確に実行できないという事態の発生が抑制される。

【0129】

図６は、ＬＣＱＰによって制御される本実施形態の制御システム１０の概略構成を表すブロック図である。図６に示されるように、制御システム１０は、カメラ１２と、ロボット１４と、制御装置１６とを備えている。本実施形態に係る制御装置１６は、物体間（例えば、ロボット１４の把持部Ｇと対象物体Ｂと壁Ｗ及び床面ＦＬとの間）の接触に関する相補性条件が満たされるように、ロボット１４の動作を連続的に制御する。

【0130】

カメラ１２は、制御対象であるロボット１４の把持部Ｇと対象物体Ｂとを逐次撮像する。そして、カメラ１２は、ロボット１４の把持部Ｇと対象物体Ｂとを撮像することにより得られた画像データを制御装置１６へ出力する。

【0131】

図７は、本実施形態に係る制御装置１６のハードウェア構成を示すブロック図である。図７に示されるように、制御装置１６は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）４２、メモリ４４、記憶装置４６、入出力Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）４８、記憶媒体読取装置５０、及び通信Ｉ／Ｆ５２を有する。各構成は、バス５４を介して相互に通信可能に接続されている。

【0132】

記憶装置４６には、後述する各処理を実行するための制御プログラムが格納されている。ＣＰＵ４２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ４２は、記憶装置４６からプログラムを読み出し、メモリ４４を作業領域としてプログラムを実行する。ＣＰＵ４２は、記憶装置４６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

【0133】

メモリ４４は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置４６は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

【0134】

入出力Ｉ／Ｆ４８は、カメラ１２とロボット１４とからのデータの入力、及びカメラ１２とロボット１４とへのデータの出力を行うインタフェースである。また、例えば、キーボードやマウス等の、各種の入力を行うための入力装置、及び、例えば、ディスプレイやプリンタ等の、各種の情報を出力するための出力装置が接続されてもよい。出力装置として、タッチパネルディスプレイを採用することにより、入力装置として機能させてもよい。

【0135】

記憶媒体読取装置５０は、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の各種記憶媒体に記憶されたデータの読み込みや、記憶媒体に対するデータの書き込み等を行う。

【0136】

通信Ｉ／Ｆ５２は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0137】

次に、制御装置１６の機能構成について説明する。図６に示されるように、制御装置１６は、機能的には、取得部２４と、計算部２６と、制御部２８とを含む。また、制御装置１６の所定の記憶領域には、データ記憶部１８と、相補性条件記憶部２０と、目的関数記憶部２２とが設けられている。各機能構成は、ＣＰＵ４２が記憶装置４６に記憶された各プログラムを読み出し、メモリ４４に展開して実行することにより実現される。

【0138】

データ記憶部１８には、カメラ１２によって撮像された画像データが格納される。相補性条件記憶部２０には、上述した物体間の接触に関する相補性条件のデータが格納されている。目的関数記憶部２２には、上述した目的関数に関するデータが格納されている。

【0139】

取得部２４は、制御対象であるロボット１４の把持部Ｇと対象物体Ｂからセンシングされたセンサデータの一例である画像データを逐次取得する。そして、取得部２４は、取得した画像データを、データ記憶部１８へ格納する。

【0140】

また、取得部２４は、時刻ｔのロボット１４及び対象物体Ｂの状態ｑ_ｔを取得する。具体的には、取得部２４は、データ記憶部１８に格納されている時刻ｔの画像データを読み出し、時刻ｔの状態ｑ_ｔを表すデータを計算する。なお、取得部２４は、既知の技術を用いて、画像データから状態ｑ_ｔを表すデータを計算する。

【0141】

計算部２６は、取得部２４によって計算された時刻ｔの状態ｑ_ｔに基づいて、上述した物体間の接触に関する相補性条件を満たし、かつ時刻ｔ＋１のロボット１４及び対象物体Ｂの状態ｑ_ｔ＋１と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態ｑ_ｔ＋１を計算する。なお、この際、計算部２６は、相補性条件記憶部２０に格納されている物体間の接触に関する相補性条件のデータと、目的関数記憶部２２に格納されている目的関数に関するデータとを参照して計算処理を実行する。

【0142】

なお、本実施形態では、物体間の接触に関する相補性条件は、ロボット１４の把持部Ｇの接触点、対象物体Ｂの接触点、及び他の物体である壁Ｗ又は床面ＦＬの接触点の何れかの間において発生する接触力と、ロボット１４の把持部Ｇの接触点、対象物体Ｂの接触点、及び壁Ｗ又は床面ＦＬの接触点の何れかの間の距離とに関する相補性条件である。物体間の接触に関する相補性条件は、上述した式（４ａ）～（４ｄ）、式（７ａ）～（７ｄ）、及び式（９ａ）～（９ｄ）の何れかである。

【0143】

このため、例えば、計算部２６は、物体間の接触に関する相補性条件式（９ａ）～（９ｄ）を満たし、かつ状態ｑ_ｔを引数とするｇ_ｔとｇ_ｒｅｆとの差を要素として含む上記式（１０）の目的関数が最小となるような状態Δｑ_ｔと接触力Δλ_ｔとのペアを計算する。そして、計算部２６は、状態Δｑ_ｔと接触力Δλ_ｔとのペアに基づいて、時刻ｔ＋１のｑ_ｔ＋１＝ｑ_ｔ＋Δｑ_ｔ及びλ_ｔ＋１＝λ_ｔ＋Δλ_ｔを計算する。

【0144】

制御部２８は、計算部２６によって計算された時刻ｔ＋１の状態ｑ_ｔ＋１となるように、制御対象であるロボット１４の把持部Ｇの動作を制御する。例えば、制御部２８は、既知の技術を利用することにより、ロボット１４の把持部Ｇの動作を制御する。

【0145】

次に、本実施形態に係る制御システム１０の作用について説明する。

【0146】

まず、対象物体Ｂが置かれている環境下において、制御システム１０が所定の指示信号を受け付けると、制御装置１６のＣＰＵ４２は記憶装置４６から制御プログラムを読み出して、メモリ４４に展開して実行する。これにより、ＣＰＵ４２が制御装置１６の各機能構成として機能し、図８に示す制御処理が実行される。なお、図８に示される制御処理は繰り返し実行される。

【0147】

なお、以下では、物体間の接触に関する相補性条件として上記式（９ａ）～（９ｄ）を用い、目的関数として上記式（１０）を用いる場合を例に説明する。このため、相補性条件記憶部２０には、上記式（９ａ）～（９ｄ）に関するデータが格納されている。また、目的関数記憶部２２には、上記式（１０）に関するデータが格納されている。

【0148】

ステップＳ１００において、取得部２４は、データ記憶部１８に格納されている時刻ｔの画像データを読み出し、時刻ｔの状態ｑ_ｔを表すデータを取得する。

【0149】

ステップＳ１０２において、計算部２６は、相補性条件記憶部２０から上記式（９ａ）～（９ｄ）に関するデータを読み出す。また、ステップＳ１０２において、計算部２６は、目的関数記憶部２２から上記式（１０）に関するデータを読み出す。そして、ステップＳ１０２において、ステップＳ１００で取得された時刻ｔの状態ｑ_ｔに基づいて、上記式（９ａ）～（９ｄ）の物体間の接触に関する相補性条件を満たし、かつ時刻ｔ＋１のロボット１４及び対象物体Ｂの状態ｑ_ｔ＋１と目標値との差を表す上記式（１０）の目的関数が最小となるような時刻ｔ＋１の状態ｑ_ｔ＋１を計算する。

【0150】

ステップＳ１０４において、制御部２８は、ステップＳ１０２で計算された時刻ｔ＋１の状態ｑ_ｔ＋１となるように、制御対象であるロボット１４の把持部Ｇの動作を制御する。

【0151】

図８に示される制御処理が繰り返され、ロボット１４に対して制御信号が繰り返し出力されることにより、対象物体Ｂが所望の位置及び姿勢となる。

【0152】

以上説明したように、本実施形態に係る制御システムの制御装置は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態を計算する。そして、制御装置は、計算された時刻ｔ＋１の状態となるように、制御対象を制御する。これにより、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することができる。

【0153】

また、本実施形態の制御装置によれば、制御対象であるロボットを連続的に制御することができる。

【0154】

また、本実施形態では、上記式（４ａ）～（４ｄ）の相補性条件を線形化し、上記式（７ａ）～（７ｄ）の線形相補性条件を生成する。これにより、相補性条件の演算に要する計算コストを低減させることができ、ロボット１４の把持部Ｇと対象物体Ｂの状態に応じたロボットの動作命令をオンラインで演算することができる。これにより、例えば、ロボット１４の把持部Ｇの先端で対象物体Ｂを押して（把持せずに）立ち上げるという動作を実現させることができる。

【0155】

また、本実施形態では、上記式（７ａ）～（７ｄ）の線形相補性条件に対して緩和変数ｓ_ｉを導入する。これにより、現実世界における物体間の接触とセンサデータによって検知された物体間の接触とに齟齬があったとしても、物体間の接触に関する相補性条件を満たしているか否かを計算することが可能となり、ロボット１４の把持部Ｇの動作を適切に制御することができる。

【0156】

［実施例］
次に、実施例について説明する。本実例では、提案したＬＣＱＰの有効性を検証するため、PyBullet 3D physics simulator（E. Coumans and Y. Bai, “Pybullet, a python module for physics simulation for games, robotics and machine learning,” 2016-2021. [Online]. Available: http://pybullet.org）を利用したシミュレーションを行う。

【0157】

図９は、本実施例を説明するための図である。図９に示されるように、本実施例では、ロボットの把持部Ｇの先端を、横たわっている対象物体Ｂであるボックスに接触させ、ボックスを立たせるようなシミュレーションを行った。なお、本実施例では、上記式（７ａ）～（７ｄ）に緩和変数を導入した相補性条件を利用する。

【0158】

図１０，図１２は、本実施例の結果を表す図である。図１１は、本実施例の結果を説明するための図である。

【0159】

図１０に示されるように、ロボットの把持部Ｇが対象物体Ｂであるボックスに触れ、対象物体Ｂを立たせることができていることがわかる。また、図１２は、図１１に示す各接触点に関連するパラメータを各時刻でモニタリングした際の結果を表す図である。図１２（１）～（５）に示されるように、接触点ｐ１～ｐ４の各パラメータにおいて、接触に関する相補性条件が満たされていることがわかる。例えば、図１０及び図１１に示されているように、接触点ｐ_３は（ａ）～（ｂ）の間において壁面Ｗと接触する。図１２の（１）を参照すると、（ａ）～（ｂ）の間においてλ_ｎ及びλ_ｆが発生する。その一方で、図１２の（１）の（ａ）～（ｂ）の間においては、Φ及びψはゼロに近い値となる。このように、図１２（１）～（５）に示されているように、接触点ｐ１～ｐ４の各パラメータにおいて、接触に関する相補性条件が満たされていることがわかる。

【0160】

なお、上記実施形態では、本開示のＬＣＱＰをロボット制御に適用した例について説明したが、これに限定されるものではない。ＬＣＱＰはパラメータの多い制御系においてリアルタイムでの最適化が求められる対象に応用可能と考えられる。例えば、接触状態の切り替わる物体操作の他、脚式ロボット、又は車輪ロボットの移動に応用することが可能である。接触状態の切り替わる物体操作を行うロボットは、物体の持ち替えの省略、狭隘環境でのピックアンドプレイス等が必要な場面も多いため、ＬＣＱＰを適用することが可能である。また、実験自動化、部品組立など、自律してマニピュレータが作業を実行する場合にもＬＣＱＰを適用させることができる。また、車輪ロボットは、壁又は床面等と接触して走行するため、壁又は床面との間で摩擦力が発生するため、接触に関する相補性条件を利用するＬＣＱＰを適用させることが可能である。

【0161】

また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した各処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、各処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0162】

また、上記実施形態では、各プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、ブルーレイディスク、ＵＳＢメモリ等の記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【符号の説明】

【0163】

１０ 制御システム
１４ ロボット
１６ 制御装置
１８ データ記憶部
２０ 相補性条件記憶部
２２ 目的関数記憶部
２４ 取得部
２６ 計算部
２８ 制御部
４２ ＣＰＵ
４４ メモリ
４６ 記憶装置
５０ 記憶媒体読取装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】