TOP > 国内特許検索 > 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム > 明細書

明細書 :非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム

発行国 日本国特許庁(JP)
公報種別 公開特許公報(A)
公開番号 特開2018-185747 (P2018-185747A)
公開日 平成30年11月22日(2018.11.22)
発明の名称または考案の名称 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム
国際特許分類 G05B  13/04        (2006.01)
B25J   5/00        (2006.01)
G05B  13/02        (2006.01)
FI G05B 13/04
B25J 5/00 E
B25J 5/00 F
G05B 13/02 J
請求項の数または発明の数 8
出願形態 OL
全頁数 50
出願番号 特願2017-088614 (P2017-088614)
出願日 平成29年4月27日(2017.4.27)
新規性喪失の例外の表示 特許法第30条第2項適用申請有り (1)平成29年2月6日 学士論文「状態ジャンプを含む非線形モデル予測制御に対するC/GMRES法の拡張」に記載 (2)平成29年2月13日開催の「国立大学法人京都大学工学部物理工学科学士発表会」にて発表
発明者または考案者 【氏名】大塚 敏之
【氏名】片山 想太郎
【氏名】佐藤 康之
【氏名】土井 将弘
出願人 【識別番号】504132272
【氏名又は名称】国立大学法人京都大学
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
個別代理人の代理人 【識別番号】100103894、【弁理士】、【氏名又は名称】家入 健
審査請求 未請求
テーマコード 3C707
5H004
Fターム 3C707KS20
3C707KS21
3C707KS34
3C707KX12
3C707LW08
3C707WA13
3C707WK04
3C707WK08
5H004GA14
5H004GA18
5H004GB16
5H004HB07
5H004HB08
5H004KC10
5H004KC27
5H004LA05
要約 【課題】不連続な状態変化を伴う非線形システムの動作を実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供する。
【解決手段】非線形システムに関する状態を示す状態パラメータが取得される。モデル予測制御のアルゴリズムを用いて非線形システムを制御するための制御入力値が算出される。算出された制御入力値を用いて非線形システムが制御される。制御入力値は、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束するペナルティ関数を用いて、算出される。
【選択図】図10
特許請求の範囲 【請求項1】
非線形システムの制御方法であって、
前記非線形システムの状態を示す状態パラメータを取得する取得ステップと、
前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記非線形システムを制御するための制御入力値を算出する算出ステップと、
前記算出された制御入力値を用いて、前記非線形システムを制御する制御ステップと
を有し、
前記算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束する拘束パラメータを用いて、前記非線形システムの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
非線形システムの制御方法。
【請求項2】
2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御装置であって、
前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する状態取得手段と、
前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出手段と、
前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御手段と
を有し、
前記算出手段は、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
二足歩行ロボットの制御装置。
【請求項3】
前記拘束パラメータは、前記モデル予測制御のアルゴリズムで用いられる評価関数に含まれている
請求項2に記載の二足歩行ロボットの制御装置。
【請求項4】
前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記二足歩行ロボットの姿勢を指定する
請求項2又は3に記載の二足歩行ロボットの制御装置。
【請求項5】
前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記2つの脚の関節部の目標角度を指定する
請求項4に記載の二足歩行ロボットの制御装置。
【請求項6】
前記拘束パラメータは、調整可能なゲインを含む
請求項2から5のいずれか1項に記載の二足歩行ロボットの制御装置。
【請求項7】
2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法であって、
前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、
前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップと、
前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップと
を有し、
前記算出ステップにおいて、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
二足歩行ロボットの制御方法。
【請求項8】
2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法を実現するプログラムであって、
前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、
前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップであって、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する、算出ステップと、
前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップと
をコンピュータに実行させるプログラム。
発明の詳細な説明 【技術分野】
【0001】
本発明は、非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムに関する。
【背景技術】
【0002】
二足歩行ロボットのような安定性の低いシステムを制御する際には、未来(有限時間後まで)のシステム挙動を予測しながら制御を行うモデル予測制御(receding horizon control;リシーディングホライズン制御)を用いることが有効である。モデル予測制御は、制御周期(サンプリング周期)ごとに各時刻から有限時間未来までの最適制御問題を解き、制御入力値を決定するフィードバック制御である。
【0003】
フィードバック制御において、二足歩行ロボットは歩行動作等を伴う非線形性の高いシステムであるので、二足歩行ロボットの制御は、非線形モデル予測制御によって行われることが好ましい。ここで、非線形モデル予測制御は、一般に、多大な計算時間を要する。したがって、非線形モデル予測制御を用いて実時間(リアルタイム)で制御入力値の最適解を決定することは困難であった。
【0004】
この技術に関連し、非特許文献1は、非線形モデル予測制御を実時間(リアルタイム)で行うことが可能な、C/GMRES法(continuation/generalized minimum residual method)と呼ばれる技術を開示する。C/GMRES法は、連続変形法(continuation method)とGMRES法とを組み合わせたアルゴリズムである。C/GMRES法は、状態変化が連続であるシステムに対し、最適解の連続性を利用して、最適解の変化率を求めながら最適解を追跡していく計算方法である。このC/GMRES法を用いることにより、非線形モデル予測制御においても、実時間でシステムを制御することが可能となる。
【先行技術文献】
【0005】

【非特許文献1】Toshiyuki OHTSUKA and Hironori A. FUJII、「Real-Time Receding-Horizon Control Algorithm for Nonlinear Systems」、計測自動制御学会論文集、1997年12月、Vol.33, No.12, p. 1131-1139
【発明の概要】
【発明が解決しようとする課題】
【0006】
非線形性を有するシステムである非線形システムでは、周囲の環境との物理的な接触を伴って移動するとき、物理的な接触により状態変化が不連続となる場合がある。一方、非特許文献1にかかる技術は、システムの状態変化が連続であることを前提としている。したがって、非特許文献1にかかる技術を用いて、物理的接触を行う可能性がある非線形システムを制御することは困難である。したがって、非線形モデル予測制御を用いて、不連続な状態変化を伴う非線形システムを実時間で制御することは困難であった。
【0007】
本発明は、不連続な状態変化を伴う非線形システムを実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供する。
【課題を解決するための手段】
【0008】
本発明にかかる非線形システムの制御方法は、前記非線形システムの状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記非線形システムを制御するための制御入力値を算出する算出ステップと、前記算出された制御入力値を用いて、前記非線形システムを制御する制御ステップとを有し、前記算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束する拘束パラメータを用いて、前記非線形システムの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。
【0009】
本発明は、上述したように、指定されたタイミングにおいて状態が不連続に変化するように非線形システムの状態を拘束する拘束パラメータを用いることで、想定しているタイミングで、不連続な状態変化を起こさせることができる。したがって、非線形システムの制御に非線形モデル予測制御の理論を容易に適用でき、さらにC/GMRES法を適用することも可能となる。したがって、本発明は、不連続な状態変化を伴う非線形システムを実時間で制御することが可能となる。
【0010】
また、本発明にかかる二足歩行ロボットの制御装置は、2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御装置であって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する状態取得手段と、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出手段と、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御手段とを有し、前記算出手段は、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。
【0011】
また、本発明にかかる二足歩行ロボットの制御方法は、2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法であって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップと、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップとを有し、前記算出ステップにおいて、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。
【0012】
また、本発明にかかるプログラムは、2つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法を実現するプログラムであって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップであって、指定されたタイミングにおいて前記2つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する、算出ステップと、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップとをコンピュータに実行させる。
【0013】
本発明は、上述したように、指定されたタイミングにおいて遊脚が着地するように二足歩行ロボットの状態を拘束する拘束パラメータを用いることで、想定しているタイミングで、遊脚の着地といった不連続な状態変化を起こさせることができる。したがって、二足歩行ロボットの制御に非線形モデル予測制御の理論を容易に適用でき、さらにC/GMRES法を適用することも可能となる。したがって、本発明は、不連続な状態変化を伴う二足歩行ロボットの動作を実時間で制御することが可能となる。
【0014】
また、好ましくは、前記拘束パラメータは、前記モデル予測制御のアルゴリズムで用いられる評価関数に含まれている。これにより、不連続な状態変化の無い非線形モデル予測制御と同様に最適化問題を扱うことが可能となる。
【0015】
また、好ましくは、前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記二足歩行ロボットの姿勢を指定する。これにより、想定した姿勢で遊脚を着地させるように二足歩行ロボットを制御することが可能となる。
【0016】
また、好ましくは、前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記2つの脚の関節部の目標角度を指定する。これにより、想定した関節角度で遊脚を着地させるように関節部を制御することが可能となる。
【0017】
また、好ましくは、前記拘束パラメータは、調整可能なゲインを含む。これにより、制御装置の性能によらないで制御を安定化させることが可能となる。
【発明の効果】
【0018】
本発明によれば、不連続な状態変化を伴う非線形システムを実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供できる。
【図面の簡単な説明】
【0019】
【図1】実施の形態1にかかるロボットシステムを示す概略図である。
【図2】実施の形態1にかかるロボットシステムの構成を示す機能ブロック図である。
【図3】非線形モデル予測制御を説明するための図である。
【図4】入力系列の更新について説明するための図である。
【図5】実施の形態1にかかるロボットをコンパス型モデルに適用する方法を説明するための図である。
【図6】実施の形態1にかかるロボットをコンパス型モデルに適用した例を示す図である。
【図7】状態ジャンプを説明するための図である。
【図8】遊脚リンクの衝突直前のロボットの状態を示す図である。
【図9】遊脚リンクの衝突直後のロボットの状態を示す図である。
【図10】実施の形態1にかかる制御装置によって行われるロボットの制御方法を示すフローチャートである。
【図11】実施の形態2にかかるロボットを示す図である。
【図12】実施の形態2にかかるロボットを膝屈曲モデルに適用した状態を示す図である。
【図13】実施の形態2にかかるロボットを膝屈曲モデルに適用した例を示す図である。
【図14】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図15】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図16】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図17】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図18】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図19】本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。
【図20】シミュレーション結果において定常状態の制御入力のグラフを示す図である。
【発明を実施するための形態】
【0020】
(実施の形態1)
以下、図面を参照して本発明の実施の形態について説明する。なお、各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。

【0021】
図1は、実施の形態1にかかるロボットシステム1を示す概略図である。また、図2は、実施の形態1にかかるロボットシステム1の構成を示す機能ブロック図である。ロボットシステム1は、ロボット100と、ロボットの動作を制御する制御装置2とを有する。

【0022】
ロボット100は、胴体102と、2つの脚である右脚110R及び左脚110Lとを有する。ロボット100は、2つの脚(右脚110R及び左脚110L)を用いて歩行動作を行うことが可能な二足歩行ロボットである。右脚110R及び左脚110Lは、ロボット100の胴体102の下部に設けられている。ここで、図1に示すように、ロボット100の前方向をX軸方向、上方向をY軸方向とする。また、以下、右脚110Rに関する構成要素の符号に「R」を付し、左脚110Lに関する構成要素の符号に「L」を付すが、それぞれの構成要素について左右を区別しない場合には、「R」及び「L」は、適宜、省略され得る。

【0023】
右脚110Rは、胴体102に近い方から順に、股関節部120Rと、上腿部112Rと、膝関節部122Rと、下腿部114Rと、足首関節部124Rと、足部116Rとを有する。同様に、左脚110Lは、胴体102に近い方から順に、股関節部120Lと、上腿部112Lと、膝関節部122Lと、下腿部114Lと、足首関節部124Lと、足部116Lとを有する。足部116R及び足部116Lの底部には、それぞれ足裏センサ118が設けられている。足裏センサ118は、足部116の底部に加わる荷重を検出する。

【0024】
股関節部120R及び股関節部120Lは、胴体102の下部に取り付けられている。そして、股関節部120R及び股関節部120Lを介して、それぞれ、上腿部112R及び上腿部112Lが胴体102と接続されている。言い換えると、右脚110R及び左脚110Lは、それぞれ、股関節部120R及び股関節部120Lを介して、胴体102と接続されている。

【0025】
また、膝関節部122Rを介して、上腿部112Rと下腿部114Rとが接続されている。同様に、膝関節部122Lを介して、上腿部112Lと下腿部114Lとが接続されている。また、足首関節部124Rを介して、下腿部114Rと足部116Rとが接続されている。同様に、足首関節部124Lを介して、下腿部114Lと足部116Lとが接続されている。

【0026】
股関節部120は、XY平面に垂直な軸(つまりロボット100の横方向に水平な軸)の周りに回転する。これにより、右脚110R及び左脚110Lは、前後に動作し得る。したがって、ロボット100は、右脚110R及び左脚110Lを交互に前に出すことにより歩行動作を行うことができる。

【0027】
膝関節部122は、XY平面に垂直な軸の周りに回転する。これにより、右脚110R及び左脚110Lは、膝関節部122で屈曲動作を行うことができる。また、足首関節部124は、XY平面に垂直な軸の周りに回転する。これにより、足部116は、下腿部114に対して上下に動作し得る。

【0028】
図2に示すように、ロボット100の各関節部(股関節部120、膝関節部122及び足首関節部124)は、角度センサ130と、モータ140とを有する。角度センサ130は、例えばエンコーダであって、各関節部の関節角度を検出する。モータ140は、各関節部を動作させる、アクチュエータとしての機能を有する。また、各関節部は、各関節部のモータ140のトルクを検出するトルクセンサ136を有してもよい。また、ロボット100の周囲の状態を検出するためのカメラが、胴体102に内蔵されていてもよい。

【0029】
制御装置2は、例えばコンピュータとしての機能を有する。制御装置2は、ロボット100の内部(例えば胴体102)に搭載されてもよい。また、制御装置2は、ロボット100と物理的に離れていてもよく、その場合、ロボット100と有線又は無線を介して通信可能に接続されてもよい。制御装置2は、ロボット100の動作、特に、右脚110R及び左脚110Lの動作を制御する。さらに具体的には、制御装置2は、各関節部のモータのトルクを制御することで、右脚110R及び左脚110Lの姿勢を制御する。つまり、ロボットシステム1において、制御装置2はマスタ装置としての機能を有し、ロボット100はスレーブ装置としての機能を有する。

【0030】
制御装置2は、主要なハードウェア構成として、CPU(Central Processing Unit)4と、ROM(Read Only Memory)6と、RAM(Random Access Memory)8とを有する。CPU4は、制御処理及び演算処理等を行う演算装置としての機能を有する。ROM6は、CPU4によって実行される制御プログラム及び演算プログラム等を記憶するための機能を有する。RAM8は、処理データ等を一時的に記憶するための機能を有する。

【0031】
また、制御装置2は、状態取得部12、非線形モデル予測制御部14、及びサーボ制御部16(以下、「各構成要素」と称する)を有する。各構成要素は、例えば、CPU4がROM6に記憶されたプログラムを実行することによって実現可能である。また、各構成要素は、必要なプログラムを任意の不揮発性記録媒体に記録しておき、必要に応じてインストールするようにして、実現するようにしてもよい。なお、各構成要素は、上記のようにソフトウェアによって実現されることに限定されず、何らかの回路素子等のハードウェアによって実現されてもよい。

【0032】
状態取得部12は、ロボット100の現在の歩行に関する状態を示すデータ(状態パラメータ)を取得する、状態取得手段としての機能を有する。状態取得部12は、各センサ(角度センサ130、足裏センサ118及びトルクセンサ136)から、各センサの検出値を取得する。そして、状態取得部12は、取得された検出値(及び検出値から得られた値)を非線形モデル予測制御部14に対して出力する。なお、「検出値から得られた値」とは、例えば、「検出値」が角度センサ130から検出された関節角度である場合、関節角度の速度(変化量,時間微分)であってもよい。この場合、状態パラメータは、関節角度及び関節角度の速度を示してもよい。

【0033】
非線形モデル予測制御部14は、ロボット100の動作を制御するための制御入力値(入力値)を算出する算出手段としての機能を有する。非線形モデル予測制御部14は、状態取得部12からの検出値(及び検出値から得られた値)の少なくとも一部を状態パラメータとして、その状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用してロボット100の動作を制御するための制御入力値を算出する。また、非線形モデル予測制御部14は、算出された制御入力値をサーボ制御部16に対して出力する。詳しくは後述する。また、非線形モデル予測制御部14は、ロボットシステム1の外部の上位コントローラ(図示せず)によって、必要な指示値(歩幅、歩行周期等)を入力されてもよい。

【0034】
サーボ制御部16は、非線形モデル予測制御部14によって算出された制御入力値を用いてロボット100の動作を制御する制御手段としての機能を有する。サーボ制御部16は、算出された制御入力値となるように、ロボット100の各関節部を制御する。また、サーボ制御部16は、サーボアンプの機能を有してもよい。また、サーボ制御部16は、トルク制御を行う場合、各関節部のトルク(関節トルク)が算出された制御入力値となるように、各関節のモータ140を制御する。このとき、サーボ制御部16は、各関節部のトルクセンサ136によって検出されたトルク値を用いてフィードバック制御を行ってもよい。

【0035】
(モデル予測制御)
ここで、本実施の形態にかかる非線形モデル予測制御部14によって行われる、モデル予測制御(非線形モデル予測制御)の手法の概要について説明する。非線形モデル予測制御とは、非線形システムに対し、各サンプリング時刻で有限時刻未来までの最適入力(制御入力値の最適解)を求め、得られた入力のうち初期値を実際の入力とする制御である。非線形モデル予測制御には、非線形最適制御である、フィードバック制御である、及び、拘束条件を組み込み易いという、3つの利点がある。

【0036】
このように、非線形モデル予測制御は、フィードバック制御であるため外乱に対して強く、拘束条件も多様に組み合わせることができる。このような特徴があるため、非線形モデル予測制御は、多くのシステムへの導入が期待されている。しかしながら、ニュートン法などの従来の反復法では、サンプリング周期内で最適解に収束させることは困難であった。

【0037】
近年、この問題に対する有効な数値計算法として、C/GMRES法が新たに考案された。C/GMRES法を用いることで有限時刻未来までの最適制御問題をサンプリング周期内で解くことが可能になった。しかしながら、以下に示すように、現状では、C/GMRES法による非線形モデル予測制御を適用できない場合もある。

【0038】
すなわち、ロボット100等の非線形システムの多くは、状態が不連続に変化する事象(「状態ジャンプ」と称する)を伴い得る。この状態ジャンプを含む問題に対し、現状のC/GMRES法による非線形モデル予測制御を適用することは困難であった。状態ジャンプを伴うシステムを直接最適化しようとすると、状態ジャンプの時刻及び入力を同時に最適化する必要がある。また、状態ジャンプにかかるラグランジュ乗数がさらに追加されることになる。

【0039】
また、これらの問題をC/GMRES法を用いて数値的に解くには、C/GMRES法のアルゴリズムを大きく変える必要がある。そこで、本願の発明者らは、ペナルティ関数(ペナルティ項)を非線形モデル予測制御の評価関数に加えるという手法を見出した。ペナルティ関数を評価関数に加えることで、状態ジャンプが発生する時刻(本実施の形態においては遊脚が着地する時刻)を指定できる。また、ラグランジュ乗数を増やさずにC/GMRES法を適用することができる。

【0040】
まず、非線形モデル予測制御の概要について説明する。制御対象として、以下の式(1)で示すような状態方程式で表される非線形システムを考える。
【数1】
JP2018185747A_000003t.gif

【0041】
ただし,x(t)は状態ベクトルであり、u(t)は制御入力ベクトル(制御入力値を示すベクトル)である。また、R及びRは、それぞれ、n次元実数ベクトル全体の集合及びm次元実数ベクトル全体の集合を示す。非線形モデル予測制御とは、式(1)で表されるシステムに対し、各時刻tにおいて、以下の式(2)で表される評価関数を最小にする入力uopt(t+τ)を求め、その初期値uopt(t)を時刻tにおける実際の制御入力値u(t)とする制御である。
【数2】
JP2018185747A_000004t.gif

【0042】
ここで、Tは時刻tにおける評価区間の長さである。関数φ(x)は終端コストと呼ばれるスカラー値関数である。関数L(x,u)はステージコストと呼ばれるスカラー値関数である。τは評価区間における時間パラメータであって、0≦τ≦Tである。なお、Tは、通常、正のスカラー値T及びα(α>0)を用いて以下の式(3)のように与えられる。
【数3】
JP2018185747A_000005t.gif

【0043】
このように、非線形モデル予測制御は、各時刻tで状態に基づいた最適入力を求めているため、フィードバック制御となっている。
また、以下の式(4)で表されるベクトル関数を拘束条件として与えることもできる。
【数4】
JP2018185747A_000006t.gif
なお、拘束条件については、等式拘束条件だけでなく、不等式拘束条件を組み込むこともできる。

【0044】
図3は、非線形モデル予測制御を説明するための図である。図3に示すように、非線形モデル予測制御では、現在時刻tにおいてT秒後までの期間のモデル挙動を予測して最適化計算を行って入力uopt(t+τ)を求める。そして、その初期値uopt(t)を現在時刻tにおける実際の制御入力値u(t)とする。

【0045】
同様に、制御周期であるサンプリング周期Δt秒後に、その時点での現在時刻tにおいてT秒後までの期間のモデル挙動を予測して最適化計算を行って入力uopt(t+τ)を求める。そして、その初期値uopt(t)を現在時刻tにおける実際の制御入力値u(t)とする。以下同様に、サンプリング周期ごとに、制御入力値u(t)が算出されることとなる。

【0046】
非線形モデル予測制御で各時刻tにおいて解くべき問題は、評価区間上の時刻τについて以下の式(5)~(8)に示すような最適制御問題である。
【数5】
JP2018185747A_000007t.gif
ただし、時刻tにおける評価区間上の時刻τ(0≦τ≦T)の状態変数ベクトルと制御入力ベクトルとを、それぞれ、x(τ;t)=x(t+τ)、u(τ;t)=u(t+τ)とした。また、添字*は、評価区間上の値であることを示す。

【0047】
式(7)のJを汎関数とみなして変分法を用いて停留条件を求めると、最適制御の必要条件(オイラー・ラグランジュ方程式)が、以下の式(9)~(14)のように得られる。
【数6】
JP2018185747A_000008t.gif

【0048】
ここで、
【数7】
JP2018185747A_000009t.gif
は、ハミルトン関数と呼ばれるスカラー値関数であり、λ(τ;t)は式(9)に対する随伴変数、μ(τ;t)は式(14)に対するラグランジュ乗数である。

【0049】
一方、実際の数値計算は、すべて離散化して行われる。したがって、式(9)~(14)は、すべて離散近似して扱わなければならない。そこで、評価区間(0≦τ≦T)をNステップに離散近似することを考える。その際の評価区間の時間刻みを、以下の式(16)で示すようにする。
【数8】
JP2018185747A_000010t.gif

【0050】
その上で、評価区間上のi番目(1≦i≦N)のステップ、つまり時刻t+iΔτにおける状態を、
【数9】
JP2018185747A_000011t.gif
と表す。u(t)、λ(t)及びμ(t)についても同様に表される。なお、T及びNは、予め定められた値である。したがって、Δτも、予め定められた値である。

【0051】
この条件の下で式(9)~(14)を離散近似すると、以下の式(17)~(22)で示すような、離散近似されたオイラー・ラグランジュ方程式が得られる。
【数10】
JP2018185747A_000012t.gif

【0052】
但し、離散近似されたハミルトン関数は、以下の式(23)で定義される。
【数11】
JP2018185747A_000013t.gif

【0053】
以上より,非線形モデル予測制御における最適入力を求める問題というのは、上記の式(17)~(22)を解いて、i=0からi=Nについて、x(t)、u(t)、λ(t)及びμ(t)を求めるという問題に帰着される。
ここで、x(t)は、上記式(17),(18)より陽に求められる。また、λ(t)は、求められたx(t)と上記式(19),(20)とから陽に求められる。したがって、x(t)、u(t)、λ(t)及びμ(t)のうちの本質的な未知量は、以下の式(24)で表されるベクトルU(t)で定義される。
【数12】
JP2018185747A_000014t.gif
なお、「:=」は、定義を意味する等号である。つまり、上記式(24)において、左辺U(t)は、右辺のベクトルで定義される。

【0054】
そして、このU(t)は、以下の式(25)で示される方程式を解くことによって得られる。
【数13】
JP2018185747A_000015t.gif

【0055】
次に、C/GMRES法について説明する。ニュートン法などの反復法では、式(25)を各サンプリング時間内で解くのは難しい。一方、U(t)が時刻tに関して連続であれば、U(t)は、サンプリング周期Δtごとに、以下の式(26)で示されるようにして更新され得る。
【数14】
JP2018185747A_000016t.gif

【0056】
したがって、U(t)を求めるためには、t=0ではU(0)を求め、t>0では、U(t)の時間微分、つまりU(t)の変化量である
【数15】
JP2018185747A_000017t.gif
を求めればよい。なお、式(26)の計算は、上記のU(t)の変化量を数値積分することに対応する。

【0057】
ここで、
【数16】
JP2018185747A_000018t.gif
を求めるために、式(25)が全てのtで成り立つことを考慮して、上記式(25)と等価である、以下の式(27)で表される方程式を扱うことを考える。

【0058】
【数17】
JP2018185747A_000019t.gif

【0059】
さらに、式(27)は、以下の式(28)で示すように書き換えられ得る。
【数18】
JP2018185747A_000020t.gif
但し、ζは正の実数で、安定化パラメータと呼ばれる。

【0060】
式(28)の全微分を実行して整理すると、Uの変化量
【数19】
JP2018185747A_000021t.gif
は、次の式(29)で表される連立方程式を解くことで得られる。

【0061】
【数20】
JP2018185747A_000022t.gif
したがって、t>0のとき、各時刻で解くべき問題は、上記式(29)で示される連立方程式のみとなる。さらに、式(29)で示される連立方程式の数値解法として、連立方程式から少ない反復回数で高精度な解を得ることが可能なGMRES法を用いることができる。

【0062】
上述した手法が、非線形モデル予測制御の実時間最適化アルゴリズムである。このU(t)の連続性を利用した変形法(連続変形法;Continuation method)とGMRES法を組み合わせたアルゴリズムを、C/GMRES法と称する。

【0063】
次に、状態ジャンプを考慮した非線形モデル予測制御について説明する。状態x(t)∈Rが以下の式(30)で表される条件を満たすと、その直後より状態ジャンプが生じるシステムを仮定する。
【数21】
JP2018185747A_000023t.gif
ただし、ξ(x(t))∈Rはベクトル値である。

【0064】
ここで、状態ジャンプが、時刻tの前後で起こるとする。つまり、tの直前及び直後の時刻をそれぞれ「t-」及び「t+」と表すと,ここで仮定されるシステムは、以下の式(31)を満たしている。
【数22】
JP2018185747A_000024t.gif

【0065】
また、ここで仮定されるシステムは、状態ジャンプ直後の状態x(t+)が、以下の式(32)で表されるように、状態ジャンプ直前の状態x(t-)から陽に求められるとする。
【数23】
JP2018185747A_000025t.gif

【0066】
さらに、状態ジャンプとともにシステムの状態方程式が切り替わり得るとする。このとき、状態ジャンプの前後の状態方程式は、以下の式(33),(34)で示すように記述される。
【数24】
JP2018185747A_000026t.gif
なお、状態方程式は必ずしも切り替わる必要はなく、f(x,u)=f(x,u)であってもよい。また、説明の明確化のため、ここで扱うシステムでは、式(4)で示したような拘束条件は存在しないと仮定する。

【0067】
非線形モデル予測制御の問題を考えるために、以下の説明では、評価区間上でのジャンプ時刻をτとする。つまり、τは、以下の式(35),(36)を満たす。
【数25】
JP2018185747A_000027t.gif

【0068】
一般的に、上記のシステムを最適化するためには、変分法により導かれた停留条件を数値的に解けばよい。システムの方程式が上記式(33),(34),(32)で与えられ、tが固定されていないとき、停留条件は、以下の式(37)~(48)で与えられる。
【数26】
JP2018185747A_000028t.gif
但し、ν∈Rは、は式(31)に対するラグランジュ乗数である。

【0069】
また、ハミルトン関数H,Hは、それぞれ以下の式(49),(50)で表される。
【数27】
JP2018185747A_000029t.gif

【0070】
上記の停留条件の中で、時刻tに関する条件を示す式は、式(41),(48)である。ここで、上述したように実際には離散化して数値計算することを考慮すると、これらの式からτを求めることは困難である。τを求めることができないと、評価区間上の状態x(τ;t)及び評価区間上の随伴変数λ(τ;t)を求めることもできない。さらに、νも新たな未知量として追加されている。したがって、仮にτが求められたとしても、本質的な未知量を算出するための式として上記の式(24)をそのまま用いることはできず、新たに未知量及び未知方程式を定義しなければならない。

【0071】
そこで、本実施の形態にかかるアルゴリズムでは、十分大きな正のスカラー値pを用いた以下の式(51)で表されるペナルティ関数が、評価関数に追加されている。
【数28】
JP2018185747A_000030t.gif

【0072】
式(51)で表されるペナルティ関数を加えた評価関数を最適化すれば、指定した時刻τに対して、以下の式(52)が成り立ち得る。
【数29】
JP2018185747A_000031t.gif

【0073】
つまり、上記のペナルティ関数を用いることにより、指定した時刻tにおいて状態ジャンプを生じさせることが可能となる。言い換えると、上記のペナルティ関数は、指定したタイミングで状態が不連続となるようにシステムの状態を拘束する拘束パラメータである。また、式(52)で表される拘束条件がペナルティ関数について追加されているため、ラグランジュ乗数νは追加されなくてもよい。したがって、本実施の形態にかかるアルゴリズムでは、非線形モデル予測制御の最適化問題を、状態ジャンプの無い非線形モデル予測制御と同数の未知数の問題として扱うことができる。

【0074】
したがって、状態ジャンプを含む非線形モデル予測制御の問題は、式(51)を評価関数に加えることにより、状態ジャンプの無い非線形モデル予測制御と同様に扱うことができる。但し、ペナルティ関数を用いるため、あらかじめ適切な状態ジャンプ時刻tを指定する必要がある。

【0075】
このように、本実施の形態にかかるアルゴリズムでは、ペナルティ関数を用いることにより、想定しているタイミングで、不連続な状態変化を起こさせることができる。したがって、元々の非線形モデル予測制御の理論を容易に適用でき、さらにC/GMRES法を適用することも可能となる。したがって、後述するように、二足歩行ロボットのような非線形システムに対しても実時間で制御を行うことが可能となる。

【0076】
次に、ペナルティ関数を用いた場合の停留条件について説明する。ここで、仮に、連続時間で停留条件を導出した場合、以下の式(53),(54)で表される項が生じる。
【数30】
JP2018185747A_000032t.gif

【0077】
上述したように、コンピュータで数値計算を行うためには、停留条件の各方程式を離散近似して考える必要がある。しかしながら、式(53),(54)については、τの前後で離散近似を行うことができない。したがって、本実施の形態にかかる問題に対しては、状態方程式及び評価関数を離散近似したのち、変分法より停留条件を導出することとする。

【0078】
まず、以下の式(55)を満たすステップiをジャンプステップiと定義する。
【数31】
JP2018185747A_000033t.gif

【0079】
を用いると、上記の式(33),(34),(32)は、それぞれ、以下の式(56),(57),(58)で示すように、離散近似される。
【数32】
JP2018185747A_000034t.gif

【0080】
また、式(51)で表されるペナルティ項(ペナルティ関数)は、以下の式(59)で示すように、離散近似される。
【数33】
JP2018185747A_000035t.gif

【0081】
次に、変分法より停留条件を求める。離散近似された本システムに対する最適制御問題とは、式(59)が追加された、以下の式(60)で表されるような離散近似された評価関数を最小にする入力の系列u(t),・・・,uN-1(t)を求める問題である。
【数34】
JP2018185747A_000036t.gif

【0082】
ここで、式(56),(57),(58)は、それぞれ、以下の式(61),(62),(63)で示されるような等式拘束条件とみなすことができる。
【数35】
JP2018185747A_000037t.gif

【0083】
式(61),(62)のラグランジュ乗数ベクトルをλi+1(t)、式(63)のラグランジュ乗数ベクトルを
【数36】
JP2018185747A_000038t.gif
とする。このとき、ラグランジュ関数は、以下の式(64)で定義される。

【0084】
【数37】
JP2018185747A_000039t.gif

【0085】
式(64)のJに式(60)で示されたJを代入すると、以下の式(65)が得られる。
【数38】
JP2018185747A_000040t.gif

【0086】
但し、ハミルトン関数H,Hは、それぞれ以下の式(66),(67)で定義される。
【数39】
JP2018185747A_000041t.gif

【0087】
また、式(65)の変分は、以下の式(68)のように表される。
【数40】
JP2018185747A_000042t.gif

【0088】
制御入力値が最適であれば、式(68)において、任意のδx(t),δu(t)について、
【数41】
JP2018185747A_000043t.gif
が成り立つ。ここで、x(t)=x(t)よりδx(t)=0であることに注意すると、以下の式(69)~(79)で表される停留条件が導かれる。但し、添字*は、評価区間上の値であることを示す。

【0089】
【数42】
JP2018185747A_000044t.gif

【0090】
ここで、評価区間上の状態x(t)は、上記式(69),(70),(71),(72)から算出され得る。また、随伴変数λ(t)は、上記式(73),(74),(75),(76)から算出され得る。したがって、本実施の形態にかかるモデルでは、拘束条件を考えていないため、未知量を示すベクトルU(t)は、以下の式(80)で定義される。
【数43】
JP2018185747A_000045t.gif

【0091】
このU(t)は、以下の式(81)で示される方程式を解くことによって得られる。
【数44】
JP2018185747A_000046t.gif
この方程式を、上述したC/GMRES法を用いて解けばよい。

【0092】
ここで、サンプリング周期ごとの入力系列(最適解)の更新について考える。図4は、入力系列の更新について説明するための図である。図4の矢印Aに示すように、C/GMRES法では、上記式(26)で示すように前進差分近似を行っている。ここで、時刻tにおける評価区間でのiステップ目の時刻つまり(t+iΔτ)が状態ジャンプ前の時刻(つまり時刻tより前の時刻)であり、時刻t+Δtにおける評価区間でのiステップ目の時刻つまり(t+Δt+iΔτ)が状態ジャンプ後の時刻(つまり時刻tより後の時刻)であるとする。このとき、時刻tにおけるジャンプステップはiであるが、時刻t+Δtにおけるジャンプステップはi-1である。このとき、
【数45】
JP2018185747A_000047t.gif
は状態ジャンプ前の入力(以下、式Ujと表記)であり、
【数46】
JP2018185747A_000048t.gif
は状態ジャンプ後の入力(以下、式Ujと表記)となる。

【0093】
したがって、本実施の形態にかかるシステムは、状態ジャンプの前後で状態が不連続であることから、i=iのとき、単純に、
【数47】
JP2018185747A_000049t.gif
とすることはできない。つまり、式(26)’のように計算すると、上記の状態ジャンプ後の入力Ujは、最適入力として更新されていないこととなる。

【0094】
そこで、本実施の形態においては、状態ジャンプ後の入力Ujを、i≠iのuより近似することと考える。状態ジャンプ後の入力Ujは、時刻t+Δt+iΔτの評価区間上の入力である。適切に近似するためには、図4の矢印Bで示すように、この時刻に最も近い時刻における入力、つまり時刻t+(i+1)Δτの入力
【数48】
JP2018185747A_000050t.gif
から近似すればよい。

【0095】
したがって、時刻tにおけるジャンプステップと時刻t+Δtにおけるジャンプステップとが異なるとき、状態ジャンプ後の入力Ujは、以下の式(82)で示すように更新される。
【数49】
JP2018185747A_000051t.gif

【0096】
但し、i=N-1のときは、上記式(82)で示した近似は行えないので、通常通り、以下の式(83)で示すように入力は更新される。
【数50】
JP2018185747A_000052t.gif

【0097】
なお、上式は、ΔtとΔτとが、以下の式(84)を満たすことを仮定している。
Δt<2Δτ ・・・(84)
しかしながら、
2Δτ≦Δt ・・・(85)
であるときも、u(t+Δt)の系列は、実時間上のu(t)の系列から適切に近似され得る。

【0098】
なお、C/GMRES法においては、U(t)の変化量を求める際に,F(U,x,t)を用いて、以下の式(86)で示すように、前進差分近似を行っている。
【数51】
JP2018185747A_000053t.gif

【0099】
しかしながら、差分近似の差分時間をhとすると,時刻tと時刻t+hとの間で状態ジャンプが生じた場合、状態量が不連続に大きく変化してしまい、差分近似を正確に行うことができない。したがって、時刻tが以下の式(87)で示される
t<t≦t+h ・・・(87)
を満たすとき,以下の式(88)で示すように後退差分近似を行う。
【数52】
JP2018185747A_000054t.gif

【0100】
(二足歩行ロボットへの適用)
次に、上述した非線形モデル予測制御を、本実施の形態にかかるロボット100の動作の制御に適用した例について説明する。なお、実施の形態1においては、ロボット100がコンパス型モデルである例について説明するが、後述するように、非線形モデル予測制御は、ロボット100がコンパス型モデルでなくても適用可能である。

【0101】
なお、ロボット100の歩行動作は、遊脚が地面と衝突する(着地する)という動作を含む。この衝突の前後で、ロボット100の一般化速度が不連続に変化する。つまり、このとき、状態ジャンプが発生する。また、一般的に、歩行動作は、周期的な運動である。したがって、ロボット100を、予め定められた周期ごとに状態ジャンプを生じさせる(つまり遊脚を着地させる)ように制御を行うことが可能である。なお、「着地」とは、遊脚が地面と衝突(接触)することに限定されない。つまり、「着地」とは、ロボット100がその上を歩行している面(歩行面)に遊脚が接触することを意味する。

【0102】
図5は、実施の形態1にかかるロボット100をコンパス型モデルに適用する方法を説明するための図である。図5に示した例では、右脚110Rが支持脚であり、左脚110Lが遊脚(振り脚)である。制御装置2は、支持脚が地面と点接触していることを模擬するため、支持脚(図5の例では右脚110R)の足首関節部124に設けられたトルクセンサ136を用いて、支持脚の足首関節部124のトルクを0に制御する。また、制御装置2は、右脚110R及び左脚110Lの膝関節部122を、伸展状態でロックするように制御する。つまり、制御装置2は、右脚110R及び左脚110Lの膝関節部122の関節角度が伸展状態に対応する角度(例えば0)となるように、膝関節部122のモータ140を制御する。さらに、制御装置2は、遊脚(図5の例では左脚110L)の足裏センサ118を用いて、遊脚の着地を検出する。このようにして、ロボットシステム1は、コンパス型モデルを模擬することができる。

【0103】
図6は、実施の形態1にかかるロボット100をコンパス型モデルに適用した例を示す図である。図6に示す例では、ロボット100は、関節150と、支持脚リンク151と、遊脚リンク152とから構成されるコンパス型モデルにモデル化されている。ここで、関節150は、胴体102及び股関節部120に対応する。また、支持脚リンク151は、右脚110R及び左脚110Lのうちの支持脚に対応する。また、遊脚リンク152は、右脚110R及び左脚110Lのうちの遊脚に対応する。

【0104】
関節150の質量をmとする。また、図6の矢印で示すように、関節150の周りに、制御入力値として入力トルクuが入力される。ここで、支持脚リンク151及び遊脚リンク152の物理的性質は、互いに同じであるとする。支持脚リンク151及び遊脚リンク152の長さを、lとする。また、支持脚リンク151及び遊脚リンク152の質量を、mとする。

【0105】
また、鉛直方向に対する支持脚リンク151の角度をθとし、鉛直方向に対する遊脚リンク152の角度をθとする。但し、図6において時計回り(各リンクの下端を中心に関節150が前方に回る方向)を正とする。したがって、図6の状態では、θ<0である。

【0106】
次に、図6に例示したコンパス型モデルの歩行動作に関して、以下のような仮定があるとする。
・遊脚リンク152と地面90との衝突(着地)は一瞬である。
・遊脚リンク152と地面90との衝突は完全非弾性衝突である。
・リンクと地面との摩擦係数は∞である。
・両脚(両リンク)が同時に地面90から力を受けることはない。

【0107】
上記の仮定より、両脚(支持脚リンク151及び遊脚リンク152)が同時に地面90に着くことはない。また、衝突時に遊脚リンク152の速度は0となる。したがって、本モデルの歩行制御に必要な方程式は、片脚支持期の運動方程式(状態方程式)と、遊脚リンク152の衝突時の方程式(衝突方程式)との2つである。

【0108】
片脚(つまり支持脚リンク151)だけが地面90に接触しているとき、ラグランジュの運動方程式より、以下の式(89)で示す方程式が導き出される。
【数53】
JP2018185747A_000055t.gif

【0109】
但し、qは、以下の式(S1)で示される一般化座標ベクトルである。
【数54】
JP2018185747A_000056t.gif
また、M(q)は慣性行列、H(q、q(ドット))は重力とコリオリ力の項、Nuはqに対する一般化力である。なお、Rn×mは、n×mの実数行列全体の集合を示す。

【0110】
ここで、式(89)に示した運動方程式の詳細を、以下の式(E1),(E2),(E3)に示す。なお、以下の式において、Iは、支持脚リンク151及び遊脚リンク152の重心(重心151m及び重心152m)周りの慣性モーメントである。また、lは、関節150から各リンクの重心(重心151m及び重心152m)までの長さである。
【数55】
JP2018185747A_000057t.gif

【0111】
また、状態ベクトルx∈Rを、以下の式(S2)に示す。
【数56】
JP2018185747A_000058t.gif

この場合、状態方程式は、上記式(89)より、以下の式(90)で表される。
【数57】
JP2018185747A_000059t.gif
なお、本実施の形態かかるコンパス型モデルでは、歩行の拘束条件として、ZMP(zero moment point)は考慮されないものとする。

【0112】
次に、衝突方程式について説明する。衝突方程式の説明の前に、状態ジャンプについて説明する。
図7は、状態ジャンプを説明するための図である。図7は、右脚110R又は左脚110Lの状態を示す図である。ここでは、右脚110Rの状態を示すとする。図7は、横軸が右脚110Rの角度を示し、縦軸が右脚110Rの角速度を示す、グラフ(位相線図)である。

【0113】
状態Iにおいて、右脚110Rが地面から離れて遊脚となる。したがって、状態Iから、右脚110R(遊脚リンク152)の角度及び角速度は、(θ,θ(ドット))である。このとき、状態Iから後述する状態IIまでの期間では、角度及び角速度は、連続的に変化している。そして、状態IIで、遊脚であった右脚110R(遊脚リンク152)が地面90に着地する。そして、直ちに、状態は状態IIIに移行して右脚110Rは支持脚(支持脚リンク151)となる。このとき、状態IIから状態IIIに遷移するときに、角度はほとんど変わらないが、角速度が急激に変化する。したがって、状態IIから状態IIIに遷移する際に、状態ジャンプが発生している。

【0114】
状態IIIから、右脚110R(支持脚リンク151)の角度及び角速度は、(θ,θ(ドット))である。このとき、状態IIIから後述する状態IVまでの期間では、角度及び角速度は、連続的に変化している。そして、状態IVで、遊脚であった左脚110L(遊脚リンク152)が地面90に着地する。そして、直ちに、状態は状態Iに移行して右脚110Rは遊脚(遊脚リンク152)となる。このとき、状態IVから状態Iに遷移するときに、角度はほとんど変わらないが、角速度が急激に変化する。したがって、状態IVから状態Iに遷移する際に、状態ジャンプが発生している。
このように、遊脚が地面に着地すると、状態を示すパラメータ(図7の例では脚の角度及び角速度)が、不連続に変化する。この現象が、状態ジャンプである。

【0115】
次に、衝突前後の一般化座標及び一般化速度の定義について説明する。
図8は、遊脚リンク152の衝突直前のロボット100の状態を示す図である。図8に示すように、衝突直前の一般化座標及び一般化速度は、それぞれ以下の式(G1)及び式(G2)で表される。
【数58】
JP2018185747A_000060t.gif

【0116】
図9は、遊脚リンク152の衝突直後のロボット100の状態を示す図である。図9に示すように、衝突直後の一般化座標及び一般化速度は、それぞれ以下の式(G3)及び式(G4)で表される。
【数59】
JP2018185747A_000061t.gif

【0117】
ここで、遊脚リンク152と地面90との衝突について、以下の2つの角運動量についての保存則を適用する。
・衝突前に遊脚であった脚(遊脚リンク152)の先端まわりの系全体の角運動量。
・関節150まわりの、衝突前に支持脚であった脚(支持脚リンク151)の角運動量。
上記の角運動量保存則は、上記式(G1),(G2),(G3),(G4)より、以下の式(91)で表される。
【数60】
JP2018185747A_000062t.gif

【0118】
なお、Q及びQは、それぞれ以下の式(E4),(E5)で表される。
【数61】
JP2018185747A_000063t.gif

【0119】
ここで、衝突前後で、一般化座標は変わらない。したがって、衝突直前の一般化座標q及び衝突直後の一般化座標qについて、以下の式(92)が成り立つ。
【数62】
JP2018185747A_000064t.gif

【0120】
また、上記式(91),(92)は、衝突直前の状態空間ベクトルx及び衝突直後の状態空間ベクトルxを用いて、以下の式(93)で表される。
【数63】
JP2018185747A_000065t.gif
但し、Iは2×2の単位行列である。

【0121】
また、Z(q)は、以下の式(94)を満たす2×2行列である。
【数64】
JP2018185747A_000066t.gif

【0122】
次に、実施の形態1にかかるコンパス型モデルに、上述した非線形モデル予測制御を適用することを考える。上述したように、歩行動作とは、「連続して脚を前に出す」という動作である。本モデルにおいては、脚(支持脚リンク151及び遊脚リンク152)の開き角を目標値に近づける、という評価関数を設定し、遊脚リンク152が着地するたびに各リンクの座標を入れ替えることで歩行制御を行う。

【0123】
このとき、上述した評価関数Jの終端コストφ及びステージコストLは、それぞれ、次の式(95),(96)のように表される。
【数65】
JP2018185747A_000067t.gif
但し、s、q及びrは、それぞれ重みを表す正のスカラー値である。また、θrefは、脚の開き角の目標値(目標角度)である。

【0124】
また、遊脚の着地の度にθとθとを入れ替え、θ(ドット)とθ(ドット)とを入れ替えることを考慮すると、状態ジャンプの方程式である式(93)は、以下の式(97)のように書き換えられる。
【数66】
JP2018185747A_000068t.gif

【0125】
但し、I'∈R2×2は、以下の行列である。
【数67】
JP2018185747A_000069t.gif

【0126】
次に、歩行周期をTstepとすると、ジャンプ時刻tは、整数kを用いて以下の式(98)で表される。
=kTstep ・・・(98)
なお、評価区間中に状態ジャンプが2回以上生じないように、式(3)で示したT(t)を設定する。このとき,周期Tstepごとに式(91)で表される脚(遊脚リンク152)と地面90との衝突が起こり、それ以外のときは、式(97)で表される運動方程式でモデルの状態を記述することができる。つまり、モデルの状態は、以下の式(99),(100)で表される。
【数68】
JP2018185747A_000070t.gif

【0127】
ここで、時刻tで状態ジャンプ、つまり脚(遊脚リンク152)と地面90との衝突が起こらなければならない。また、支持脚リンク151及び遊脚リンク152の長さが互いに同じであることから、遊脚リンク152が地面90に着地したとき、θ=-θである。したがって、状態ジャンプが起こる条件は、以下の式(101)で表される。
【数69】
JP2018185747A_000071t.gif

【0128】
したがって、評価関数に追加されるペナルティ関数(ペナルティ項)は、以下の式(51)’で表される。ここで、pは、ゲイン(重み)であって、十分大きな正のスカラー値である。このゲインpは、制御装置2のコンピュータの性能等に応じて、適宜調整可能である。
【数70】
JP2018185747A_000072t.gif

【0129】
さらに、上記のペナルティ項とは別に、遊脚リンク152の着地時の脚の開き角を目標値に近づくけるような項を付け加えることを考える。したがって、以下の式(51)”で示す項も、ペナルティ項として評価関数に加える。ここで、pは、ゲイン(重み)であって、十分大きな正のスカラー値である。このゲインpは、制御装置2のコンピュータの性能等に応じて、適宜調整可能である。
【数71】
JP2018185747A_000073t.gif

【0130】
式(51)’及び式(51)”を足し合わせると、実施の形態1にかかるペナルティ項は、以下の式(102)で表される。これにより、ペナルティ項(拘束パラメータ)は、予め指定したタイミング(時刻t)において遊脚が着地するようにロボット100の状態を指定することとなる。言い換えると、ペナルティ項(拘束パラメータ)は、予め指定したタイミング(時刻t)において遊脚が着地したときのロボット100の姿勢(各関節部の関節角度)を指定することとなる。
【数72】
JP2018185747A_000074t.gif
したがって、実施の形態1にかかるコンパス型モデルにおける非線形モデル予測制御を用いたロボット100の制御では、式(51)で示したペナルティ関数として、上記式(102)で表したものが適用される。

【0131】
図10は、実施の形態1にかかる制御装置2によって行われるロボット100の制御方法を示すフローチャートである。図10に示した制御方法は、上述した非線形モデル予測制御を用いている。したがって、実施の形態1にかかるロボット100の制御方法は、式(69)~(79)で表される停留条件から、式(80)で表されるベクトルU(t)を求め、このベクトルU(t)の各成分の値を、式(81)で示される方程式を解くことによって算出する。これにより、ロボット100の動作を制御するための制御入力値が算出される。ここで、図10に示したフローチャートにおいて、S102は、状態パラメータを取得する取得ステップに対応し、S104~S114は、制御入力値を算出する算出ステップに対応し、S116は、ロボット100の動作を制御する制御ステップに対応する。

【0132】
まず、制御装置2は、ロボット100の状態を示す状態ベクトル(状態パラメータ)を取得して、状態観測を行う(ステップS102)。具体的には、制御装置2の状態取得部12は、股関節部120R及び股関節部120Lの角度センサ130から、股関節部120R及び股関節部120Lの関節角度を取得する。そして、状態取得部12は、これらの関節角度から、現在時刻tにおけるθ及びθ(図6)を算出する。なお、状態取得部12は、例えば、支持脚である方の脚にかかる股関節部120の関節角度と、胴体102の傾きとから、鉛直方向に対する支持脚リンク151の角度θを取得できる。同様に、状態取得部12は、例えば、遊脚である方の脚にかかる股関節部120の関節角度と、胴体102の傾きとから、鉛直方向に対する遊脚リンク152の角度θを取得できる。なお、胴体102の傾きは、例えばジャイロセンサ等の傾斜センサを用いて取得可能である。

【0133】
また、状態取得部12は、θ及びθの変化量θ(ドット)及びθ(ドット)を算出する。例えば、状態取得部12は、時刻tの1つ前のサンプリング周期(制御周期)における時刻t-Δtにおけるθ及びθと時刻tにおけるθ及びθとの差分から、それぞれ変化量θ(ドット)及びθ(ドット)を算出してもよい。

【0134】
これにより、状態取得部12は、時刻tにおける状態ベクトルx(t)を取得する。さらに、状態取得部12は、式(72)で示すように、このx(t)を、時刻tについての評価区間における、状態ベクトルの初期値とする。つまり、x(t)=x(t)とする。なお、この式(72)についての処理は、非線形モデル予測制御部14が行ってもよい。

【0135】
次に、制御装置2の非線形モデル予測制御部14は、時刻tについての評価区間における状態変数を、各i=1~Nについて更新する(ステップS104)。具体的には、非線形モデル予測制御部14は、式(69)~(71)で示したように、状態変数x(t)を更新する。なお、実施の形態1にかかるコンパス型モデルの遊脚の着地の例では、状態方程式に関するfは、ジャンプステップの前後で同じであるとする。つまり、f(x,u)=f(x,u)である。

【0136】
そして、非線形モデル予測制御部14は、式(90)に示した状態方程式を用いて、式(69)~(70)で示すように、i≠iにおける状態変数を更新する。また、非線形モデル予測制御部14は、i=iにおいては、式(97)に示した状態ジャンプの方程式を用いて、式(71)で示すように、状態変数を更新する。また、制御装置2のRAM8は、得られた状態変数x(t)を記憶する。

【0137】
次に、制御装置2は、時刻tについての評価区間における随伴変数を、各i=1~Nについて更新する(ステップS106)。具体的には、非線形モデル予測制御部14は、式(73)~(76)で示したように、S104で更新された各xを用いて、随伴変数λ(t)を更新する。また、制御装置2のRAM8は、得られた随伴変数λ(t)を記憶する。

【0138】
なお、上述したように、f(x,u)=f(x,u)であるので、H=Hである。そして、非線形モデル予測制御部14は、式(90)に示した状態方程式及び式(96)に示したステージコストLの式を用いてハミルトン関数Hを算出し、式(73)~(74)で示すように、i≠iにおける随伴変数を更新する。また、非線形モデル予測制御部14は、i=iにおいては、式(97)に示した状態ジャンプの方程式、式(102)に示したペナルティ関数の式及び式(96)に示したステージコストLの式を用いて、式(75)で示すように、随伴変数を更新する。また、非線形モデル予測制御部14は、i=Nにおいては、式(95)に示した終端コストの式を用いて、式(76)で示すように、随伴変数を更新する。

【0139】
次に、制御装置2は、ベクトル関数Fを導出する(ステップS108)。具体的には、非線形モデル予測制御部14は、式(80)で示されたベクトルU(t)を算出するため、S104及びS106の処理で得られた状態変数x(t)及び随伴変数λ(t)を用いて、式(81)で示されたベクトル関数F(U,x)の方程式を導出する。なお、上述したように、実施の形態1においては、H=Hである。

【0140】
次に、制御装置2は、ベクトルU(t)の全微分を計算する(ステップS110)。具体的には、非線形モデル予測制御部14は、式(29)を変形した以下の式(103)から、C/GMRES法を用いて、Uの全微分(U(ドット))、つまりU(t)の変化率を算出する。言い換えると、非線形モデル予測制御部14は、制御周期ごとに、制御入力値の最適解の変化率を算出する。
【数73】
JP2018185747A_000075t.gif
これにより、式(80)から明らかなように、i=0~N-1について、u(t)の時間微分(u(ドット))が得られることとなる。RAM8は、得られたu(t)の時間微分の値を記憶する。

【0141】
次に、制御装置2は、入力系列u(t)の更新を行う(ステップS112)。具体的には、非線形モデル予測制御部14は、式(26)及び式(82)から、以下の式(104)及び(105)により、入力系列u(t)の更新を行う。
【数74】
JP2018185747A_000076t.gif

【0142】
ここで、式(104),(105)から、非線形モデル予測制御部14は、ある時刻tにおける入力系列u(t)及びu(t)の変化率を用いて、次のサンプリング周期Δtである時刻t+Δtにおける入力系列を算出する。したがって、時刻tにおける入力系列u(t)を算出するためには、現在の時刻tの1つ前のサンプリング周期Δtの時刻t-Δtにおける入力系列u(t-Δt)及びu(t-Δt)の時間微分を用いることとなる。これにより、式(80)で示したU(t)の各成分の値が得られる。言い換えると、入力系列u(t)のi=0~N-1それぞれの値が得られる。RAM8は、入力系列u(t)のi=0~N-1それぞれの値を記憶する。

【0143】
次に、制御装置2は、入力値(制御入力値)を決定する(ステップS114)。具体的には、非線形モデル予測制御部14は、以下の式(106)により、入力値uを決定する。
【数75】
JP2018185747A_000077t.gif
つまり、非線形モデル予測制御部14は、U(t)の成分のうちの1番目の成分を入力値と決定する。非線形モデル予測制御部14は、決定された入力値を、サーボ制御部16に対して出力する。

【0144】
次に、制御装置2は、ロボット100の制御を行う(ステップS116)。具体的には、サーボ制御部16は、S114で決定された入力値から、各関節部に指示する関節トルクを決定する。さらに具体的には、サーボ制御部16は、支持脚リンク151に対応する脚の股関節部120の関節トルクτと、遊脚リンク152に対応する脚の股関節部120の関節トルクτとを、以下の式(107),(108)によって決定する。
τ=u ・・・(107)
τ=-u ・・・(108)

【0145】
なお、胴体102(関節150)の姿勢が崩れることを防止するため、サーボ制御部16は、以下の式(107),(108)のように関節トルクτ及び関節トルクτを決定してもよい。
【数76】
JP2018185747A_000078t.gif
ここで、θtorsoは、鉛直方向に対する胴体102の前方への傾き角度である。また、k及びkは、ゲイン(重み)であって、予め定められた定数である。このゲインは、制御装置2のコンピュータの性能に応じて、適宜調整可能である。
サーボ制御部16は、決定された関節トルクとなるように、各関節部(股関節部120)のモータ140を制御する。

【0146】
以上のように、実施の形態1にかかる制御装置2は、非線形モデル予測制御のアルゴリズムを用いてロボット100の動作を制御するに際し、指定したタイミングで遊脚が着地するようにロボット100の状態を拘束するペナルティ関数(拘束パラメータ)を用いている。これにより、想定しているタイミングで、遊脚の着地という不連続な状態変化を起こさせることができる。したがって、元々の非線形モデル予測制御の理論を容易に適用でき、さらにC/GMRES法を適用することも可能となる。したがって、二足歩行ロボットのような非線形システムに対しても実時間で制御を行うことが可能となる。

【0147】
また、実施の形態1にかかるペナルティ関数は、上記式(102)で示すように、予め指定したタイミングにおいて遊脚が着地したときのロボット100の姿勢を指定している。これにより、想定した姿勢で遊脚を着地させるようにロボット100を制御することが可能となる。さらに、実施の形態1にかかるペナルティ関数は、予め指定したタイミングにおいて遊脚が着地したときのロボット100の各関節部の関節角度を指定している。これにより、想定した関節角度で遊脚を着地させるようにロボット100の関節部を制御することが可能となる。

【0148】
また、式(102)で示すように、ペナルティ関数は、調整可能なゲイン(p及びp)を含む。指定されたタイミングで状態変化を確実に起こさせるため、ゲインは十分大きなスカラー値とする必要がある。しかしながら、制御装置2のコンピュータの性能等によっては、ゲインがあまりにも大きすぎると感度が過大となるため、不安定な制御となる可能性がある。したがって、ゲインを調整することにより、制御を安定化させることが可能となる。

【0149】
(実施の形態2)
次に、実施の形態2について説明する。実施の形態2では、ロボット100がより人間に似たヒューマノイドロボットである点で、実施の形態1と異なる。そして、実施の形態2では、膝を曲げて二足歩行を行うモデル(膝屈曲モデル)について、上述した非線形モデル予測制御を適用する。その他の点については、実施の形態1と実質的に同様であるので、説明を省略する。

【0150】
図11は、実施の形態2にかかるロボット100を示す図である。実施の形態1にかかるロボット100と同様に、実施の形態2にかかるロボット100は、胴体102と、右脚110Rと、左脚110Lとを有する。右脚110R及び左脚110Lの構成については、実施の形態1のものと同様である。また、胴体102は、腰部102aと、胸部102bと、腰関節部102cとを有する。また、胴体102の上側には、頭部104が設けられている。この頭部104に、カメラ等のセンサが設けられていてもよい。

【0151】
また、ロボット100は、胴体102(胸部102b)の右側及び左側に、それぞれ右腕160R及び左腕160Lを有する。実施の形態2にかかるロボット100は、右腕160R及び左腕160Lを用いて、所定の動作を行うことが可能である。なお、図11には図示されていないが、右腕160R及び左腕160Lの先端に、対象物を把持することが可能なエンドエフェクタが設けられていてもよい。

【0152】
右腕160Rは、胴体102に近い方から順に、肩関節部170Rと、上腕部162Rと、肘関節部172Rと、前腕部164Rとを有する。同様に、左腕160Lは、胴体102に近い方から順に、肩関節部170Lと、上腕部162Lと、肘関節部172Lと、前腕部164Lとを有する。肩関節部170R及び肩関節部170Lは、胴体102の右側及び左側にそれぞれ取り付けられている。そして、肩関節部170R及び肩関節部170Lを介して、それぞれ、上腕部162R及び上腕部162Lが胴体102と接続されている。言い換えると、右腕160R及び左腕160Lは、それぞれ、肩関節部170R及び肩関節部170Lを介して、胴体102と接続されている。

【0153】
また、肘関節部172Rを介して、上腕部162Rと前腕部164Rとが接続されている。同様に、肘関節部172Lを介して、上腕部162Lと前腕部164Lとが接続されている。また、肩関節部170は、互いに直交した3軸の周りをそれぞれ回転するように構成され得る。また、肘関節部172は、1軸の周りを回転する。また、図2に示したように、肩関節部170及び肘関節部172は、角度センサ130と、モータ140とを有する。

【0154】
図12は、実施の形態2にかかるロボット100を膝屈曲モデルに適用した状態を示す図である。図12に示した例では、右脚110Rが支持脚であり、左脚110Lが遊脚である。制御装置2は、支持脚が地面と点接触していることを模擬するため、支持脚(図12の例では右脚110R)の足首関節部124に設けられたトルクセンサ136を用いて、足首関節部124のトルクを0に制御する。さらに、制御装置2は、遊脚(図12の例では左脚110L)の足裏センサ118を用いて、遊脚の着地を検出する。ここで、実施の形態1とは異なり、実施の形態2では、膝関節部122は、ロックされていない。このようにして、ロボットシステム1は、膝屈曲モデルを模擬することができる。

【0155】
図13は、実施の形態2にかかるロボット100を膝屈曲モデルに適用した例を示す図である。図13に示す例では、ロボット100は、支持脚下腿リンク201と、支持脚上腿リンク202と、胴体リンク203と、遊脚上腿リンク204と、遊脚下腿リンク205と、関節211,212,213,214,215とから構成される、膝屈曲モデルにモデル化されている。ここで、胴体リンク203は、ロボット100の胴体102から上の構成要素に対応する。

【0156】
また、支持脚下腿リンク201は、右脚110R及び左脚110Lのうちの支持脚にかかる下腿部114に対応する。支持脚上腿リンク202は、右脚110R及び左脚110Lのうちの支持脚にかかる上腿部112に対応する。遊脚上腿リンク204は、右脚110R及び左脚110Lのうちの遊脚にかかる上腿部112に対応する。遊脚下腿リンク205は、右脚110R及び左脚110Lのうちの遊脚にかかる下腿部114に対応する。

【0157】
また、関節211は、支持脚にかかる足首関節部124に対応する。関節212は、支持脚にかかる膝関節部122に対応する。関節213は、股関節部120に対応する。関節214は、遊脚にかかる膝関節部122に対応する。関節215は、遊脚にかかる足首関節部124に対応する。ここで、関節211は、地面90に接触している。この関節211の位置、つまり支持脚の先端の位置を、(X,Y)とする。また、関節215の位置、つまり遊脚の先端の位置を、(X,Y)とする。

【0158】
ここで、各リンク及び各関節211~215を区別するパラメータk(k=1~5)を設ける。k=1は、支持脚下腿リンク201及び関節211に対応する。同様に、k=2は、支持脚上腿リンク202及び関節212に対応する。k=3は、胴体リンク203及び関節213に対応する。k=4は、遊脚上腿リンク204及び関節214に対応する。そして、k=5は、遊脚下腿リンク205及び関節215に対応する。

【0159】
したがって、支持脚下腿リンク201、支持脚上腿リンク202、胴体リンク203、遊脚上腿リンク204、及び遊脚下腿リンク205を、それぞれ、リンク#1、#2、#3、#4、#5と示すことがある。そして、各リンクについて一般化して示すときに、リンク#kと示すことがある。関節211~215についても同様に、それぞれ、関節#1、#2、#3、#4、#5と示すことがある。そして、各関節について一般化して示すときに、関節#kと示すことがある。

【0160】
支持脚下腿リンク201、支持脚上腿リンク202、胴体リンク203、遊脚上腿リンク204、及び遊脚下腿リンク205の長さ(リンク長)を、それぞれ、l、l、l、l、lとする。また、支持脚下腿リンク201、支持脚上腿リンク202、胴体リンク203、遊脚上腿リンク204、及び遊脚下腿リンク205の質量(リンク質量)を、それぞれ、m、m、m、m、mとする。また、支持脚下腿リンク201、支持脚上腿リンク202、胴体リンク203、遊脚上腿リンク204、及び遊脚下腿リンク205の各リンクの重心周りの慣性モーメントを、それぞれ、I、I、I、I、Iとする。なお、l、m及びIは、予め定められた値である。

【0161】
また、支持脚下腿リンク201、支持脚上腿リンク202、胴体リンク203、遊脚上腿リンク204、及び遊脚下腿リンク205の鉛直方向に対する角度(リンク角度)を、それぞれ、θ、θ、θ、θ、θとする。なお、リンク角度θ、θ、θ、θ、θは、ロボット100の各関節部(股関節部120、膝関節部122及び足首関節部124)の角度センサ130で検出された関節角度から、幾何学的に一意に算出可能である。したがって、リンク角度θ、θ、θ、θ、θは、ロボット100の各関節部の関節角度に対応する。

【0162】
また、支持脚下腿リンク201の重心201mの位置を、(Xc1,Yc1)とする。支持脚上腿リンク202の重心202mの位置を、(Xc2,Yc2)とする。胴体リンク203の重心203mの位置を、(Xc3,Yc3)とする。遊脚上腿リンク204の重心204mの位置を、(Xc4,Yc4)とする。遊脚下腿リンク205の重心205mの位置を、(Xc5,Yc5)とする。また、k=1~5それぞれについて、関節#kから、リンク#kの重心#k(質点)までの距離を、rとする。このrは、予め定められた値である。

【0163】
ここで、重心#kの位置(Xck,Yck)は、以下の式(109),(110)で表される。なお、式(109),(110)の第2項については、k<4の場合は0とする。
【数77】
JP2018185747A_000079t.gif

【0164】
式(109),(110)について時間微分を行うことで、以下の式(111),(112)で示すように、重心#kの速度が得られる。
【数78】
JP2018185747A_000080t.gif

【0165】
この場合、ラグランジアンΓを以下の式(113)のように表すことができる。なお、gは重力加速度である。
【数79】
JP2018185747A_000081t.gif

【0166】
このとき、ラグランジュの運動方程式は、以下の式(114)のように表される。
【数80】
JP2018185747A_000082t.gif

【0167】
なお、q及びηは、以下に示すベクトルである。
【数81】
JP2018185747A_000083t.gif
なお、ηは、関節トルクベクトルである。ここで、ηは、足首関節部124のトルクである。また、ηは、支持脚の膝関節部122のトルクである。また、ηは、支持脚の股関節部120のトルク、言い換えると、支持脚に対する胴体リンク203のトルクである。ηは、遊脚の股関節部120のトルクである。また、ηは、遊脚の膝関節部122のトルクである。

【0168】
そして、式(114)から、以下の式(115)を導き出すことができる。つまり、実施の形態2にかかる膝屈曲モデルでは、コンパス型モデルにおける式(89)の方程式が、以下の式(115)で示すように修正される。なお、M(q)及びH(q、q(ドット))は、膝屈曲モデルに対応するように修正され得る。
【数82】
JP2018185747A_000084t.gif

【0169】
ここで、Nは、以下の式(116)で表される行列である。
【数83】
JP2018185747A_000085t.gif

【0170】
また、実施の形態2にかかる膝屈曲モデルにおいても、式(91)が成り立つので、Q及びQについても、膝屈曲モデルに対応するように修正され得る。また、式(95)及び式(96)に示した評価関数Jの終端コストφ及びステージコストLについても、膝屈曲モデルに対応するように修正され得る。
また、実施の形態2にかかる膝屈曲モデルにおけるペナルティ項については、式(102)で示したものを、以下の式(117)で示したものに修正する。なお、qrefは、遊脚が着地するときの目標姿勢を示す。言い換えると、qrefは、遊脚が着地するときの各関節の関節角度に対応するリンク角度θ、θ、θ、θ、θの目標値(目標角度)のベクトルを示す。具体的には、qrefは、遊脚が着地した時点において、X-Xが実現したい歩幅となり、Y=Yとなる(遊脚の先端の高さが支持脚の先端の高さと同じとなる)ようなqである。
【数84】
JP2018185747A_000086t.gif

【0171】
但し、Rは、以下の式(118)で表されるペナルティ重み行列である。なお、p~pは、予め定められた値である。
【数85】
JP2018185747A_000087t.gif

【0172】
そして、図10に示したフローチャートにおいて、S102の処理で、状態取得部12は、股関節部120、膝関節部122及び足首関節部124の角度センサ130から、各関節部の関節角度を取得する。そして、状態取得部12は、これらの関節角度から、現在時刻tにおけるθ~θ(図13)及びこれらの変化量を算出して、状態ベクトルx(t)を取得する。そして、非線形モデル予測制御部14は、修正された各関数を用いてS104~S114の計算を行って、関節トルクη~ηを算出することができる。そして、サーボ制御部16は、算出された関節トルクη~ηから、各関節部のモータを制御することができる。

【0173】
以上のように、状態方程式及びペナルティ項等を、実施の形態1にかかるコンパス型モデルにおけるものから、実施の形態2にかかる膝屈曲モデルにおけるものに置き換えることができる。これにより、実施の形態2にかかる膝屈曲モデルにかかるロボット100の制御においても、上記のコンパス型モデルの制御で用いた非線形モデル予測制御のアルゴリズムを用いることが可能となる。したがって、実施の形態2にかかる膝屈曲モデルについても、実施の形態1と同様に、実時間で非線形モデル予測制御を行うことが可能となる。

【0174】
(シミュレーション結果)
次に、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを用いて非線形システムについて行ったシミュレーション結果について説明する。以下に説明するシミュレーションは、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、実施の形態1にかかるコンパス型モデルにかかるロボット100に適用したものである。

【0175】
表1は、シミュレーションで用いたコンパス型モデルの物理パラメータを示す。また、表2は、シミュレーションで用いた非線形モデル予測制御の評価関数の重みを示す。また、脚の開き角の目標値をθref=0.32[rad]とし、歩行周期をTstep=0.8[s]とする。また、式(S2)に示した状態ベクトルの初期値を、x(t)=[-0.166,0.165,0.6,0.75]とする。

【0176】
【表1】
JP2018185747A_000088t.gif

【0177】
【表2】
JP2018185747A_000089t.gif

【0178】
表3は、C/GMRES法の数値計算に用いる各パラメータを示す。ここで、hdirは、式(86)で示したGMRES法における前進差分近似の差分時間hである。また、rtolは、シミュレーション開始時における最適性条件残差の許容値である。また、シミュレーション時間は10[s]とした。また、式(3)に示した評価区間T(t)において、T=0.8[s]、α=1.0とした。

【0179】
【表3】
JP2018185747A_000090t.gif

【0180】
図14~図19は、本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。また、図20は、シミュレーション結果において定常状態の制御入力のグラフを示す図である。図14は、表1~表3に示した条件下において、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、実施の形態1にかかるコンパス型モデルにかかるロボット100に適用したシミュレーション結果を示す。図14~図17は、それぞれ、θ、θ、θ(ドット)及びθ(ドット)のシミュレーション結果を示す。また、図18は、制御入力値uのシミュレーション結果を示す。また、図19は、式(81)で表されるベクトルFの大きさである||F||(エラーノルム)のシミュレーション結果を示す。

【0181】
図20のt=8.0[s]の近傍及びt=8.8[s]の近傍においてグラフが垂直に立っている箇所で、状態ジャンプが生じていることが分かる。このように、本シミュレーションでは、定常状態において、周期的な状態ジャンプを生じさせることに成功している。また、図18のu(t)のグラフから、t=5[s]以降で、定常的な歩行をシミュレーションしていることが分かる。

【0182】
(変形例)
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、ロボット100の片方の脚は、股関節部120、膝関節部122及び足首関節部124を有するとしたが、このような構成に限られない。ロボット100の脚は、3個よりも少ない数の関節部を有してもよいし、3個よりも多い数の関節部を有してもよい。この場合、状態ベクトル及び関節トルクベクトル(制御入力値)は、関節部の数に応じて、適宜、変更され得る。そして、状態方程式及びペナルティ関数等の関数も、関節部の数に応じて、適宜、変更され得る。

【0183】
また、上述した実施の形態においては、非線形システムが二足歩行ロボットである例について説明したが、本実施の形態にかかる非線形モデル予測制御のアルゴリズムは、二足歩行ロボット以外の非線形システムについても適用可能である。つまり、本実施の形態にかかる非線形システムの制御方法は、以下に例示するような、状態ジャンプを伴う任意の非線形システムに対して、適用可能である。そして、上述したように、非線形モデル予測制御において状態ジャンプが発生するタイミングを指定するようにすればよい。

【0184】
例えば、本実施の形態にかかる非線形システムは、図11に示したロボット100の腕(右腕160R又は左腕160L)のようなロボットハンド又はロボットアーム等であってもよい。この例における状態ジャンプは、ロボットハンド又はロボットアームが、周辺環境又は操作対象等の物体を押圧するとき、物体を把持し又は離すとき、球体等の物体を叩く又は打ち返すとき等に、発生し得る。なお、球体等の物体を打ち返す非線形システムの例として、例えば、卓球ロボットがある。

【0185】
また、例えば、本実施の形態にかかる非線形システムは、腕及び脚を同時に床等に着地して移動可能な人型ロボット又は動物型ロボット等であってもよい。この例における状態ジャンプは、人型ロボット又は動物型ロボットが、腕と脚とを同時に、壁、床又はテーブル等に接触して移動するとき、又は、人型ロボット又は動物型ロボットが、梯子又は壁等を登るとき等に、発生し得る。

【0186】
また、例えば、本実施の形態にかかる非線形システムは、ドローン等の無人航空機などであってもよい。この例における状態ジャンプは、無人航空機が、操作対象又は検査対象の物体に接触するとき又はその物体から離れるとき、輸送対象又は捕獲対象の物体を把持し又は離すとき等に、発生し得る。

【0187】
また、例えば、本実施の形態にかかる非線形システムは、加工機械の工具等であってもよい。この例における状態ジャンプは、加工機械の工具が、加工対象等の物体に接触し又は離れるとき等に、発生し得る。

【0188】
また、例えば、本実施の形態にかかる非線形システムは、自動車のトランスミッション等であってもよい。この例における状態ジャンプは、トランスミッションのクラッチが、接触状態(動力の伝達状態)となったとき又は離間状態(動力の遮断状態)なったとき等に、発生し得る。

【0189】
また、例えば、本実施の形態にかかる非線形システムは、ハイブリッド車の動力源等であってもよい。この例における状態ジャンプは、ハイブリッド車の動力源が、モータとエンジンとの間で切り替わるとき等に、発生し得る。また、例えば、本実施の形態にかかる非線形システムは、電気自動車又はハイブリッド車等のバッテリーであってもよい。この例における状態ジャンプは、バッテリーが充電と放電の間で切り替わるとき等に、発生し得る。

【0190】
また、例えば、本実施の形態にかかる非線形システムは、自動車等の自動運転システムであってもよい。この例における状態ジャンプは、自車の車線変更又は合流等によって先行車両や後続車両の有無が変化するとき等に、発生し得る。また、この例における状態ジャンプは、物体との衝突が避けられない場合に衝突後の状況まで含めて可能な範囲で最善の動作を行うように制御するとき等に、発生し得る。

【0191】
また、例えば、本実施の形態にかかる非線形システムは、飛行機等であってもよい。この例における状態ジャンプは、飛行機の離着陸において、接地の前後を含めて運動を最適化するように制御するとき等に、発生し得る。具体的には、所望の経路で着陸しつつ、着陸後すみやかに減速するようにエンジン及び機体を制御するような場合である。

【0192】
また、例えば、本実施の形態にかかる非線形システムは、列車等であってもよい。この例における状態ジャンプは、列車の連結において、連結の前後を含めて運動を最適化するように制御するとき等に、発生し得る。具体的には、連結時の衝撃及び駆動モータの負荷を軽減するようにモータを制御するような場合である。

【0193】
上述したような任意の非線形システムについて、図10で示したような制御方法が実行され得る。この場合、非線形システムの制御方法は、式(69)~(79)で表される停留条件から、式(80)で表されるベクトルU(t)を求め、このベクトルU(t)の各成分の値を、式(81)で示される方程式を解くことによって算出する。これにより、非線形システムを制御するための制御入力値が算出される。そして、図10に示したフローチャートにおいて、S102は、非線形システムの状態パラメータを取得する取得ステップに対応し、S104~S114は、非線形システムを制御するための制御入力値を算出する算出ステップに対応し、S116は、非線形システムを制御する制御ステップに対応する。そして、算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように非線形システムの状態を拘束する拘束パラメータを用いて、制御入力値が算出される。このとき、非線形システムの制御周期ごとに、モデル予測制御のアルゴリズムにおける予め定められた評価区間における制御入力値の最適解の変化率が算出され、変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出され、最適解から、現在の前記制御入力値が算出される。

【0194】
上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)、CD-ROM(Read Only Memory)、CD-R、CD-R/W、半導体メモリ(例えば、マスクROM、PROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM(Random Access Memory))を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【0195】
1・・・ロボットシステム、2・・・制御装置、12・・・状態取得部、14・・・非線形モデル予測制御部、16・・・サーボ制御部、100・・・ロボット、102・・・胴体、110L・・・左脚、110R・・・右脚、112・・・上腿部、114・・・下腿部、116・・・足部、118・・・足裏センサ、120・・・股関節部、122・・・膝関節部、124・・・足首関節部、130・・・角度センサ、136・・・トルクセンサ、140・・・モータ、150・・・関節、151・・・支持脚リンク、152・・・遊脚リンク、201・・・支持脚下腿リンク、202・・・支持脚上腿リンク、203・・・胴体リンク、204・・・遊脚上腿リンク、205・・・遊脚下腿リンク、211,212,213,214,215・・・関節
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7
【図9】
8
【図10】
9
【図11】
10
【図12】
11
【図13】
12
【図14】
13
【図15】
14
【図16】
15
【図17】
16
【図18】
17
【図19】
18
【図20】
19