図面 (/)

技術 最適制御装置、最適制御方法及び最適制御プログラム

出願人 トヨタ自動車株式会社
発明者 土井将弘
出願日 2015年4月14日 (5年2ヶ月経過) 出願番号 2015-082637
公開日 2016年12月1日 (3年6ヶ月経過) 公開番号 2016-198873
状態 特許登録済
技術分野 マニプレータ
主要キーワード 最適制御装置 回転運動量 条件付き最適化 再帰的計算 再帰計算 ホライゾン 並進運動量 エンコーダ付きモータ
関連する未来課題
重要な関連分野

この項目の情報は公開日時点(2016年12月1日)のものです。
また、この項目は機械的に抽出しているため、正しく解析できていない場合があります

図面 (19)

課題

モデル予測制御において最適化問題最適解高速求解すること。

解決手段

最適制御装置は、移動ロボットの移動手段の接触点計画を設定する接触点計画手段と軌道生成手段とを備える。軌道生成手段は、予測区間において評価基準を用いて重心の状態変数を算出し、該算出した重心の状態変数に基づいて重心軌道を生成するモデル予測制御を行う。評価基準は、各接触点における接触力に基づく量の二乗が含まれる評価関数予測区間内において最小化する。評価基準、状態方程式、及び移動ロボットの線形等式表現される等式制約条件、を含む等式制約条件付き最適化問題は、直交補空間を用いて等式制約条件を含まない無制約条件の最適化問題に変換されている。軌道生成手段は、この無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、求解した最適解に基づいて重心の状態変数を算出する。

概要

背景

例えば、ロボット機械的リンク系に対してモデル予測制御(リシーディングホライゾン制御)を行う最適制御装置が知られている(特許文献1参照)。

概要

モデル予測制御において最適化問題最適解高速求解すること。最適制御装置は、移動ロボットの移動手段の接触点計画を設定する接触点計画手段と軌道生成手段とを備える。軌道生成手段は、予測区間において評価基準を用いて重心の状態変数を算出し、該算出した重心の状態変数に基づいて重心軌道を生成するモデル予測制御を行う。評価基準は、各接触点における接触力に基づく量の二乗が含まれる評価関数予測区間内において最小化する。評価基準、状態方程式、及び移動ロボットの線形等式表現される等式制約条件、を含む等式制約条件付き最適化問題は、直交補空間を用いて等式制約条件を含まない無制約条件の最適化問題に変換されている。軌道生成手段は、この無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、求解した最適解に基づいて重心の状態変数を算出する。

目的

本発明は、このような問題点を解決するためになされたものであり、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供する

効果

実績

技術文献被引用数
0件
牽制数
0件

この技術が所属する分野

(分野番号表示ON)※整理標準化データをもとに当社作成

ライセンス契約や譲渡などの可能性がある特許掲載中! 開放特許随時追加・更新中 詳しくはこちら

請求項1

二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、前記接触点計画設定手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、を備える最適制御装置であって、前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデル構築して、該予測モデルによって所定時間幅予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形状態方程式と、前記移動ロボットの線形等式表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御装置。

請求項2

請求項1記載の最適制御装置であって、前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換される、ことを特徴とする最適制御装置。

請求項3

請求項1又は2記載の最適制御装置であって、前記等式制約条件を示す式に対してQR分解を行って状態変数の変換式導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してQR分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含む、ことを特徴する最適制御装置。

請求項4

請求項1乃至3のうちのいずれか1項記載の最適制御装置であって、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をQR分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出する、ことを特徴とする最適制御装置。

請求項5

請求項1乃至4のうちいずれか1項記載の最適制御装置であって、前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含む、ことを特徴とする最適制御装置。

請求項6

請求項1乃至5のうちいずれか1項記載の最適制御装置であって、前記軌道生成手段は、前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出する、ことを特徴とする最適制御装置。

請求項7

請求項6記載の最適制御装置であって、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出する、ことを特徴とする最適制御装置。

請求項8

請求項7記載の最適制御装置であって、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用する、ことを特徴する最適制御装置。

請求項9

請求項6乃至8のうちいずれか1項記載の最適制御装置であって、前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含む、ことを特徴とする最適制御装置。

請求項10

請求項1乃至9のうちいずれか1項記載の最適制御装置であって、前記最適化問題の状態方程式は、線形時変の制御パラメータを含む、ことを特徴とする最適制御装置。

請求項11

請求項1乃至10のうちいずれか1項記載の最適制御装置であって、前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備える、ことを特徴とする最適制御装置。

請求項12

二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、を含む最適制御方法であって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御方法。

請求項13

二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、をコンピュータに実行させる最適制御プログラムであって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御プログラム。

技術分野

0001

本発明は、移動ロボットモデル予測制御を行う最適制御装置最適制御方法及び制御プログラムに関するものである。

背景技術

0002

例えば、ロボット機械的リンク系に対してモデル予測制御(リシーディングホライゾン制御)を行う最適制御装置が知られている(特許文献1参照)。

先行技術

0003

特開2000−330609号公報

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

0004

上記モデル予測制御では、ロボットの物理的な制約条件が設定される。そして、制御周期毎にこの制約条件付き最適化問題求解し、その求解した最適解に基づいてロボットの重心軌道を生成することとなる。しかし、この最適解の求解において、従来、多大な時間を要するという、問題が生じていた。

0005

本発明は、このような問題点を解決するためになされたものであり、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することを主たる目的とする。

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

0006

上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、前記接触点計画設定手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、を備える最適制御装置であって、前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデル構築して、該予測モデルによって所定時間幅予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形状態方程式と、前記移動ロボットの線形等式表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御装置である。
この一態様において、前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換されてもよい。
この一態様において、前記等式制約条件を示す式に対してQR分解を行って状態変数の変換式導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してQR分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、
前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をQR分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用してもよい。
この一態様において、前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含んでいてもよい。
この一態様において、前記最適化問題の状態方程式は、線形時変の制御パラメータを含んでいてもよい。
この一態様において、前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備えていてもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、を含む最適制御方法であって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御方法であってもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、をコンピュータに実行させる最適制御プログラムであって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、 前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御プログラムであってもよい。

発明の効果

0007

本発明によれば、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することができる。

図面の簡単な説明

0008

移動ロボットの動作の一例を示す図。である。
移動ロボットの機械構成の一例を示す図である。
移動ロボットの機能ブロック図である。
軸力を示す図である。
最適制御装置の機能ブロック図である。
接触点計画の概要の一例を示す図である。
接触点計画の一例を示す図である。
予測区間の例を示す図である。
予測区間での動きを表わした図である。
予測区間のシフトを説明するための図である。
予測に用いる移動ロボットのモデルを示す図である。
予測区間の離散化を説明するための図である。
直交補空間のイメージ図である。
無制約条件のLQ最適化問題に変換する際のフローを示す図である。
最適制御方法を示すフローチャートである。
接触点の座標系接触多角形とを示す図である。
接触点が不安定化する場合を例示した図である。
等式制約条件及び不等式制約条件付きLQ最適化問題の最適解の求解フローを示すフローチャートである。

実施例

0009

本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
(第1実施形態)
本実施形態は移動ロボットの最適制御装置に特徴があり、具体的には、移動ロボットの移動動作図1)を制御するための軌道生成に特徴を有するのであるが、具体的な制御(軌道生成)を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。

0010

図2は、移動ロボットの機械構成の一例を示した図である。
移動ロボット100は、股関節が3軸、膝関節が1軸、足首関節が2軸、さらに、肩関節が3軸(肩ピッチ、肩ロール、肩ヨー)、肘関節が1軸(ピッチ)、および、手首関節が3軸(手首ヨー、手首ピッチ、手首ロール)、で夫々構成されている。

0011

(移動ロボットの機械構成はこれに限定されないが、手(腕)の自由度は6以上、足(脚)の自由度も6以上は必要である。)
移動ロボット100は、各関節にエンコーダ付きモータ1、2、・・・、28を有している。
各関節のモータ1a、2a、・・・、28a(図3)は、各関節の関節角度θ1、θ2、・・・、θ28を調整できる。
一方、各関節のエンコーダ1b、2b、・・・、28bは、各関節の関節角度θ1、θ2・・・、θ28を計測することができる。

0012

また、移動ロボット100は、足先部(足平部)および手先部(手の平部)に接触力センサ25を有している。
ここで接触力とは6軸力であり、図4に示すように、x軸、y軸およびz軸方向の力fの組(fx、fy、fz)Tと、x軸回り、y軸回りおよびz軸回りの力τの組(τx、τy、τz)Tと、である。
(なお、x軸およびy軸は、鉛直方向であるz軸に垂直な面内で互いに直交する軸とする。)

0013

この移動ロボットは、移動時に、右足左足右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。
そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。また、手先、足先というのは、移動手段の一具体例である。

0014

図3は、移動ロボット100の機能ブロック図である。
移動ロボット100は、各関節のモータ1a〜24a及びエンコーダ1b〜24bと、接触力センサ25と、最適制御装置210と、を備えている。

0015

最適制御装置210には、各関節のエンコーダ1b〜24b及び接触力センサ25から、センサ検出値が入力される。また、最適制御装置210は、各関節のモータ1a〜24aに対して駆動信号を出力する。

0016

最適制御装置210は、主要なハードウェア構成として、制御処理演算処理等を行うCPU(Central Processing Unit)210aと、CPU210aによって実行される制御プログラム、演算プログラム等が記憶されたROM(Read Only Memory)210bと、処理データ等を一時的に記憶するRAM(Random Access Memory)210cと、を有するマイクロコンピュータにより構成されている。また、これらCPU210a、ROM210b、及びRAM210cは、データバス210dによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。

0017

図5は、本発明の一実施形態に係る最適制御装置210の機能ブロック図である。本実施形態に係る最適制御装置210は、移動ロボット100の接触点計画を設定する接触点計画設定部(接触点計画手段の一具体例)221と、安定に実行できる移動ロボット100の重心軌道を生成する軌道生成部(軌道生成手段の一具体例)222と、生成された重心軌道に従って移動ロボット100の全身動作を実行させる動作制御部(制御手段の一具体例)223と、を有する。

0018

ここで、軌道生成部222は、接触点計画に従った動作を実行できる重心軌道を生成するのであるが、この重心軌道生成には必要に応じた接触点変更を含む。
これら機能部の具体的な処理動作については後述する。

0019

多点接触移動のための軌道生成方法
本実施形態に係る軌道生成部222は、(1)多点接触移動を実現できる重心軌道を生成し、かつ、(2)必要に応じて接触点の変更を行っている。ここで、(1)多点接触移動を実現できる重心軌道を生成するための方法を説明する。なお、本出願人は、特願2013−254989(平成25年12月10日出願)においてこの方法を出願している。

0020

そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。ユーザとしてはロボットに接触点の計画情報だけを与え、あとは、移動ロボットが設定された接触点の計画情報に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。

0021

さて、移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。

0022

このために本実施形態に係る軌道生成部222は、モデル予測制御(所謂リシーディングホライゾン制御:Receding Horizon Control)を用いて重心軌道を生成する。
最初にモデル予測制御の概要を説明しておく。

0023

(モデル予測制御の概要説明
例えば、図1に図示したような移動動作を移動ロボットに行わせたいとする。
ここでは、2本の腕と2本の脚とを有する人型の移動ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。

0024

この場合、接触点計画設定部221は、ユーザから指令される接触点の計画情報に基づいて、この一連動作タスク)を実行できるような接触点計画を作成する。
つまり、接触点計画設定部221は、例えば、図6のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。
図6においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。

0025

この接触点計画は、具体的には図7のようになる。
接触点計画は、左手(LH)、右手(RH)、左足(LF)および右足(RF)に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。

0026

図1図6および図7対応関係を簡単に説明する。
当初(t0)左足1本だけで立ち遊脚である右足を前に振り出し、そして、右足を着地させる(t1)。
この動きに従った接触点計画を移動ロボット100に実行させるためには、左足が最初に着地している床上の接触点の座標PLF1、そのときの左足の姿勢rLF1、そして、右足が着地する床上の接触点の座標PRF1、そのときの右足の姿勢rRF1、を指定することが必要である。

0027

ここで、接触点の座標は、空間座標としてP=(Px、Py、Pz)の組で表わされる。
また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてr=(rx、ry、rz)として表わされる。
(すなわち、rx、ryおよびrzは、ロール、ピッチおよびヨー角をそれぞれ表わす。)
足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。

0028

両足で立った後、左足を振り出し(t2)、左足を前方に着地する(t4)。
その間に、左手を壁に着くようにする(t3)。
ここで、左手を着く壁上の接触点の座標PLH1、および、そのときの左手の姿勢rLH1を指定する。

0029

この接触点の座標は空間座標としてP=(Px、Py、Pz)の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてr=(rx、ry、rz)として表わされる。

0030

これ以降の接触点計画は図1図6および図7を対比して頂ければ自明と思われるので省略する。
このようにして、接触点計画設定部221は、接触点計画を時系列のデータとして作成する。

0031

軌道生成部222は、上記のように接触点計画設定部221により設定された接触点計画を実現するように重心軌道を生成する。動作制御部223は、軌道生成部222により生成された重心軌道に従って移動ロボット100の全身動作させるように、各関節のモータ1a〜24aを制御する。これにより、移動ロボット100は、設定された接触点計画に基づいて安定な重心軌道に従って、自律的に移動できる。

0032

このとき、軌道生成部222は、軌道生成にあたってモデル予測制御を行う。
すなわち、軌道生成部222は、ある時間幅を持った予測区間内で移動ロボット100が安定移動できる軌道を生成し、予測区間を微小時間(Δt)ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。

0033

例えば、図8に予測区間の例を示す。
軌道生成部222は、現在から所定時間(例えば1.6秒)先の未来までを予測区間として設定する。

0034

そして、軌道生成部222は、この予測区間の間で発散しないように安定な軌道を生成する。
この予測区間での動きをイメージしたものが図9である。
このように、軌道生成部222は、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。

0035

軌道生成部222は、次の軌道更新周期(Δt秒後)には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する(図10参照)。

0036

現在だけ、あるいは、現在から次ぎの制御周期(Δt秒)まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道が生成されるようにする。
これを繰り返すことで移動ロボットは安定に移動することができる。

0037

さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。

0038

本発明者らは、ある予測区間における安定軌道の生成問題をLQ(Linear Quadratic)最適化問題(凸二次計画問題:Quadratic Programming: QP)に帰着させるという着想を得た。
具体的には、軌道生成部222は、各接触点における接触力の二乗和と、前記6軸力(接触力)の微分値の二乗和と、を含む評価関数Jを最小化するというLQ最適化問題を解くことで、多点接触移動の安定軌道を求める。

0039

そこで、次に、この評価関数Jの導出およびその解法(LQ最適化問題への帰着)を説明する。
この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。(ここからの説明では、まず、接触点計画で指示された通りの位置(接触点)に手足を着くことだけを考える。なお、必要に応じて、スラック変数などを導入し条件式評価式緩和するなどの処置を行って接触点を変更してもよい。

0040

予測に用いる移動ロボットのモデルを改めて図11に示す。
移動ロボット全体の慣性を一つの重心Gで表わす。各接触点には6軸力を定義する。

0041

この時、重心Gの並進運動量をP、重心回り回転運動量角運動量)をL、接触点の数をnとすると、運動方程式は次のように書ける。

0042

0043

添え字iは接触点のインデックスを表す。
例えば接触点の候補が左手、右手、左足、右足の4点であれば、n=4(左手:LH=1、右手:RH=2、左足:LF=3、右足:RF=4)とすればよい。ただし、床や壁に接触していない接触点候補については接触力を0にするように拘束条件を設定しておく。例えば図11の例であれば次のようにする。

0044

0045

(1)式の第1式、第2式を微分すると次の式が得られる。((1)式はベクトルで表現しているが、これをx、y、zに分解した上で、上から順に第1式、第2式・・・第6式と称する。)

0046

0047

本実施形態では、この2式をシステムとして用いる。そして、(1)式の第3から第5式を拘束条件として定式化する。

0048

0049

さらに、予測区間内を図12のように、N個の区間に分割し、(3)式、(4)式を離散化する。(3)式を離散化すると次のようになる。

0050

0051

また、サンプリング点で常に(4)式の拘束が成り立つとすると、(4)式は次のように離散化される。

0052

0053

ここで、パラメータを次ぎのように置く。

0054

0055

θiは、6軸力としての接触力を並べたベクトルである。そして、xは、重心Gのx座標、重心Gのx軸方向速度、重心Gのy座標、重心Gのy軸方向速度、および、各接触点における接触力(6軸力)、を並べたベクトルである。このxを、重心の状態変数xと称する。さらに、uは、接触力(6軸力)の微分値を並べたベクトルである。

0056

このようにパラメータを設定すると、(5)式を次ぎの状態方程式として記述することができる。

0057

0058

この(8)式は、(j+1)のときの状態変数xを、その一つ前の状態で記述できる。(8)式を用いて予測区間内の状態変数xを順に計算していくと次のようになる。

0059

0060

したがって、時系列的に求められる状態変数xを並べて大文字のXで表わすと、状態変数の時系列データXを次のように表わすことができる。

0061

0062

この(10)式は、接触力の微分値(u[k])を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。なお、上記(10)式において、接触力を入力してもよい。この場合、状態変数xは、重心位置と重心速度のみを含むこととなる。また、上記(3)式は、G(2ドット)(2階微分)とfとの関係式となり、この関係式と、上記(5)式のf(ドット)の項を0にした式とから、上記(8)式のような線形の状態方程式が導出できる。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Jの評価基準を導入した。

0063

0064

なお、Qi、Riは、適宜設定した重みである。例えば、接触点候補すべてに力を均等配分した場合、Qiはすべて1となり、Riはすべて1×10−6と設定できる。

0065

ここで、θiは、6軸力としての接触力の成分を並べたベクトルであった。したがって、上記(11)式は、「予測区間内で、接触力(6軸力)と接触力の微分値との2乗和を最小化する」という意味の式である。上記(11)式の第1項は、接触力(6軸力)の2乗和を最小化することを意味する。

0066

この第1項には、次の作用が含まれている。
(1)各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
(2)不必要な内力打ち消すこと。
(3)接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。

0067

また、上記(11)式の第2項は、接触力(6軸力)微分値(6軸力の時間変化率)の2乗和を最小化することを意味する。

0068

この第2項には次の作用が含まれている。
(1)重心の発散を抑制すること。
(2)滑らかに接触力を切り替えていくこと。

0069

これらをQ、Rという重みによって適切に足し合わせることによって、この評価関数Jを最小化するということは、
「高い接触安定性、滑らかな接触力遷移最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。

0070

上記(11)式を離散化し一般的な形式に書き換えると、次の評価関数(12)式が得られる。

0071

次に、移動ロボットの力の釣合いの拘束を示す等式制約条件(拘束制約条件)について考える。
等式制約条件としては、
(1)移動ロボットの非接触の接触点候補に対して6軸力が0という拘束、
(2)移動ロボットの鉛直方向の力の釣り合いの拘束、および、
(3)移動ロボットのxy軸回りのモーメント力の釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。

0072

ここで、例えば、あるサンプリング点kにおいて、i番目とi+2番目の接触点が非接触であったとする。
この時、上記等式制約条件(1)乃至(3)は、下記(13)式のように記述できる。

0073

なお、係数行列Ck、dkの成分はサンプリング点によって異なり、接触点候補の接触/非接触といった情報や接触点位置は接触点計画設定部221によって設定される。例えば、上記(13)式のpix[k]、piy[k]、piz[k]は、接触点計画設定部221によって設定される。

0074

以上から、現在の状態量(状態変数の初期値)をx0とすると、上記(8)式、(12)式、及び(13)式より、最適制御装置210の軌道生成部222は、下記(14)式に示す等式制約条件付きLQ最適化問題を求解し、重心軌道を生成することとなる。



なお、上記(14)式において、1行目の式(minJ=・・)は、上述の如く、予測区間内において、接触力と接触力の微分値との2乗和を最小化するという意味の式である。2行目の式(x[k+1]=・・)は、接触力の微分値の入力と重心の状態変数と関係を示す状態方程式である。3行目の式(Ckx[k]=dk)は、移動ロボットの力の釣合いの拘束を示す等式制約条件である。

0075

ところで、上述のように移動ロボットの最適制御装置は、多点接触で安定的な動作軌道を生成するためにモデル予測制御を行っている。このモデル予測制御では、移動ロボットの物理的な制約条件(上述の等式制約条件)が設定される。そして、最適制御装置は、制御周期毎にLQ最適化問題を求解し、その求解した最適解に基づいて制御を行なっている。しかし、この最適解の求解において、従来、多大な時間を要し、モデル予測制御の周期(軌道更新の周期)に遅延が生じ、制御性能を上げることができないという問題が生じていた。

0076

これに対し本実施形態においては、直交補空間を用いて等式制約条件付きのLQ最適化問題を無制約条件のLQ最適化問題に変換する。そして、最適制御装置210の軌道生成部222は、この変換した無制約条件のLQ最適化問題をリカッチ型再帰的計算法(Riccati recursion)を用いて解き、最適解を求解する。そして、軌道生成部222は、求解した最適解に基づいて重心の状態変数を算出し、該算出した重心の状態変数に基づいて重心軌道を生成する。
直交補空間を用いて無制約条件のLQ最適化問題に変換することで、その求解に高速かつ安定的なリカッチ型再帰的計算法を用いることができる。これにより、モデル予測制御においてLQ最適化問題の最適解を高速に求解し重心軌道を生成できる。

0077

なお、上記Riccati recursionは、最適化問題を行列表現した式に変換し、その変換した行列表現の式の最適解条件(KKT(Karush-Kuhn- Tucker) 条件)を示す式に対して再帰的計算を行うことにより、最適化問題の最適解を高速に求解するものである。詳細な計算方法については、既に、非特許文献(Parallel Implementation of Riccati Recursion for Solving Linear-Quadratic, Gianluca Frison John Bagterp Jorgensen)などに開示されており、これを援用できるものとする。

0078

ここで、最初に、上述した直交補空間について詳細に説明する。直交補空間は、以下(1)−(3)のように定義される。
(1)2つの部分空間V及びUの基底{vi}ki=1および{u}mi=1に含まれるベクトルが線形独立であるとき、基底{vi∈Rn}ki=1∪で張られる部分空間をVとUの直和(direct sum)といい、U(+)Vと表記する。以下、○の中に+を(+)と表記する。特に、Rn=Rk+m=V(+)Uが成立するとき、UをVの補空間(complement)という。
(2)部分空間V⊂Rnと部分空間U⊂Rnとが、vTu=0 for all v ∈ V、all u ∈ U を満たすとき、2つの部分空間は直交するという。
(3)部分空間Vとその補空間Uが直交するとき、UをVの直交補空間(orthogonal complement)といい、V⊥と表記する。
上記定義に基づいて下記命題(4)−(5)が成立する。
(4)線形独立なm(<n)個のベクトル{yi}mi=1と直交するベクトル集合α={x∈Rn|yT1x=yT2x=・・・=yTmx=0}は、n−m次元部分空間である。
(5)非直交基底{ui∈Rn}ni=1からm個選択された基底ベクトルによって張られる部分空間V=<u1、u2、・・・、um>の直交補空間は、その双直交基底{vi∈Rn}ni=1によって、V⊥=<vm+1、vm+2、・・・、vn>で表される。

0079

上記命題を簡略して説明すると、Ck∈Rmk×nxの直交補空間C⊥k∈Rnx×(nx−mk)とは、nx×nxの線形空間のうち、Ckの残りの空間(補空間)でCkに直交する空間である。この直交補空間を用いて上記(14)式のLQ最適化問題を変換することで、図13に示す如く、等式制約条件Ckx=dk上に存在するxを、Ckに平行なベクトルζと直交しCkに終端する定数ベクトルσで表すことができる。換言すると、直交補空間を用いて、xをζに変数変換することで、ζをどのように動かしても必ず等式制約条件Ckx=dkは満たされることとなる。このため、この等式制約条件を考慮することなく無制約条件でLQ最適化問題を求解できる。

0080

次に、上述した直交補空間を用いた変換方法(以下、直交補空間変換と称す)について詳細に説明する。
本実施形態において、例えば、下記(15)式に示すQR分解(直交行列Qと上三角行列Rの積に分解)を用いて直交補空間変換を行うことができる。

0081

以上から、等式制約条件付きLQ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のLQ最適化問題を次のように導出する。
まず、等式制約条件を示す上記(13)式(Ckx[k]=dk)をQR分解することで、状態変数xの変換式である下記(16)式が導出される。

0082

次に上記状態方程式(8)式の左からCk+1を掛けると下記(17)式が導出される。
Ck+1x[k+1]=Ck+1Ax[k]+Ck+1Bu[k] ・・・(17)
さらに、上記(17)式に上記(13)式を代入して下記(18)式を導出する。
Ck+1Ax[k]+Ck+1Bu[k]=dk+1 ・・・(18)
(k=0、1、・・・、N−1)

0083

上記変換と同様に、Ck+1Bの直交補空間を用いて変数変換を行う。
Ck+1Bを下記(19)式に示すようにQR分解する。

0084

上記(19)式を用いて上記(18)式を変換し(QR分解を行い)、入力uの変換式である下記(20)式を導出する。



但し、上記(20)式における各パラメータを下記(21)式に示すように設定する。



k=0のときは、上記(20)式における各パラメータを下記(22)式に示すように設定する。

0085

上記(16)式の左からDTkを掛けて下記(23)式を導出する。



但し、上記(23)式において、正規直交性から下記(24)式が成立する。

0086

上より、上記(8)式を上記(16)式、(20)式、及び(23)式を用いて変形し、状態方程式の変換式である下記(25)式を導出する。



但し、上記(25)式における各パラメータを下記(26)式に示すように設定する。



k=0のときは、上記(25)式における各パラメータを下記(27)式に示すように設定する。

0087

また、上記(16)式及び(20)式を用いて、上記(12)式に示す評価関数JのΣの項は、下記(28)式に示すように変形できる。



但し、k=0のときは、下記(29)式が成立する。



また、k=Nのときは、下記(30)式が成立する。

0088

上記(16)式及び(20)式を用いて上記(12)式に示す評価関数Jを変形し、評価関数の変換式である下記(31)式を導出する。



但し、上記(31)式における各パラメータを下記(32)式に示すように設定する。

0089

以上のように、等式制約条件付きLQ最適化問題に対して直交補空間変換を行い、下記(33)式に示す無制約条件のLQ最適化問題を導出できる。すなわち、直交補空間変換を行うことで、上記(14)式に示す等式制約条件付きLQ最適化問題を、下記(33)式に示す無制約条件のLQ最適化問題に変換できる。本実施形態に係る軌道生成部222は、下記(33)式に示す無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて最適解を高速に求解できる。

0090

次に、上記直交補空間変換により変換した無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記(33)式を行列表現すると、下記(34)式及び(35)式のように表現できる。

0091

上記(34)式及び(35)式の最適解条件(KKT条件)は、下記(36)式となる。但し、下記(37)式は、上記(35)式のラグランジュ乗数である。

0092

軌道生成部222は、上記(36)式に示す式に対して、次のように、再帰的計算を行うことで、上記無制約条件のLQ最適化問題を高速かつ安定的に求解する。
まず、軌道生成部222は、上記(36)式の行列内の各パラメータの順番を入れ替えることで、下記(38)式のように表現する。

0093

そして、軌道生成部222は、上記(38)式に対して、下記(39)式に示す再帰計算を繰り返す。

0094

上記再帰計算を繰り返すことで、上記(38)式は、下記(40)式のように変形される。

0095

さらに、軌道生成部222は、上記(40)式に対して、下記(41)式に示す再帰計算を行うことで、上記(33)式に示すLQ最適化問題の最適解ζを高速で求解する。

0096

最後に、軌道生成部222は、上記求解した最適解ζと、上記(16)式及び(20)式(下記2式)と、を用いて、上記(14)式に示す等式制約条件付きLQ最適化問題のパラメータを復元し、x[k]及びu[k]を算出する。
x[k]=Dkζ[k]+ek
u[k]=Nkζ[k]+Mkv[k]+lk

0097

軌道生成部222は、算出したx[k](重心Gのx座標、重心Gのx軸方向速度、重心Gのy座標、重心Gのy軸方向速度、および、各接触点における接触力(6軸力) )の時系列データに基づいて、重心軌道を生成する。このようにして、予測区間内において、等式制約条件を満たし、かつ評価関数Jを最小化する重心軌道が高速に生成される。すなわち、予測区間内において移動ロボットの安定な移動を実現する重心軌道を高速に生成することができる。

0098

図14は、上述した直交補空間変換を行った上記(33)式に示す無制約条件のLQ最適化問題に変換する際のフローを示す図である。
等式制約条件の上記(13)式に対してQR分解を行って、状態変数の変換式である上記(16)式が導出される(ステップS101)。

0099

上記入力(接触力(6軸力)の微分値)uと重心の状態変数xとの関係を示す状態方程式(8)式から導出した(18)式に対してQR分解を行って、入力uの変換式である上記(20)式が導出される(ステップS102)。

0100

状態方程式(8)式を、導出された状態変数xの変換式(16)式、入力uの変換式(20)式、及び、状態変数xの変換式(16)式から導出した(23)式を用いて変形し、状態方程式の変換式である(25)式が導出される(ステップS103)。

0101

上記導出した状態変数xの変換式(16)式と、入力uの変換式(20)式に基づいて、上記(12)式に示す評価関数を変形し、評価関数の変換式である上記(31)式が導出される(ステップS104)。変換後の無制約条件のLQ最適化問題は、上述の如く、上記導出された評価関数の変換式(31)式と、状態方程式の変換式(25)式と、を含むこととなる。

0102

図15は、本実施形態に係る最適制御装置による最適制御方法を示すフローチャートである。
接触点計画設定部221は接触点計画(等式制約条件のCk及びdk)を設定する(ステップS201)。

0103

軌道生成部222は、接触点計画設定部221により設定された接触点計画に基づいて、上記(33)式のLQ最適化問題を行列表現し、その最適解条件に対して再帰的計算を行うことで、LQ最適化問題の最適解ζを求解する(ステップS202)。

0104

軌道生成部222は、求解した最適解ζと、上記(16)式及び(20)式と、を用いて、上記(14)式に示す等式制約条件付きLQ最適化問題のパラメータを復元し、重心の状態変数x[k]及び入力u[k]を算出する(ステップS203)。
軌道生成部222は、算出したx[k]の時系列データに基づいて、重心軌道を生成する(ステップS204)。

0105

動作制御部223は、軌道生成部222により生成された重心軌道に従って移動ロボット100の全身動作させるように、各関節のモータ1a〜24aを制御する(ステップS205)。

0106

以上、本実施形態において、軌道生成部222は、等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて重心の状態変数を算出し、算出した重心の状態変数に基づいて重心軌道を生成する。これにより、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる。

0107

(第2実施形態)
本実施形態において、軌道生成部222は、上記等式制約条件に加えて不等式制約条件を加えたLQ最適化問題を求解する。ここで、接触点の安定性の拘束を示す不等式制約条件について説明する。

0108

移動ロボットの接触点が安定して接触を保つ為の不等式制約条件を導入する。
図16に接触点の座標系(上添え字l(エル)がついている)と、接触多角形(接触点の支持多角形)と、を示した。

0109

接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢riに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力(6軸力)θilを次のように表わす。
θil=[fixl、fiyl、fizl、τixl、τiyl、τizl]T

0110

すると、接触力(6軸力)θilは、接触面の姿勢行列Φi=rot(ri)を用いて下記(42)式のように表現できる。
なお、rotは、オイラー角を姿勢行列に変換する関数である。

0111

接触点が安定して接触を保つ為には、
(1)接触点が離れないこと、
(2)接触点が滑らないこと、
(3)接触点が剥がれないこと、
という3つの制約条件を満たす必要がある。
上記3つの制約条件が理解しやすいように、図17に、接触点が不安定化する場合を例示した。

0112

(1)接触点が離れない為には、接触面の鉛直力が正であれば良い。即ち、下記(43)式を満たす必要がある。

0113

(2)接触点が滑らない為には、接触面に平行な2軸力が摩擦力以下であれば良い。即ち下記(44)式がその条件である。ただし接触面の摩擦係数をμiとする。

0114

(3)接触点が剥がれない為の条件は、接触多角形のh個の頂点座標
(xi1l, yi1l),・・・・・(xihl, yihl)
を用いて下記(45)式のように表される。
(ただし接触多角形の頂点反時計回りに順に与えられているとする)。

0115

以上、(43)、(44)、(45)式をまとめると次のようになる。

0116

(46)式に(42)式を代入し、k番目のサンプリング点としてインデックスを付け加える。すなわち、下記(47)式は、k番目の接触点が安定な接触を維持するための条件式である。したがって、安定な多点接触動作を実現するためには、全サンプリング点の全接触点において下記(47)式が成立する必要がある。

0117

k番目のサンプリング点において全接触点が上記(47)式を満足する為の条件は、下記(48)式のように表現できる。

0118

(不等式制約条件)
(Pkx[x]≦qk)・・・(49)
なお、上記不等式制約条件の一般式(49)式の右辺qkをqk=Oと置けば、上記導出した(48)式と一致する。上記(14)式に示す等式制約条件付きLQ最適化問題に上記(48)を加えることで、下記(50)式に示す等式制約条件及び不等式制約条件付きLQ最適化問題が導出される。

0119

本実施形態において、上記実施形態1で行った直交補空間変換に加えて、さらに、上記(49)式に示す不等式制約条件の変換を行う。具体的には、上記(16)式を上記(49)式に代入することで、下記(51)式を導出する。

0120

以上から、本実施形態において、直交補空間変換を行うことで、上記(50)式に示す等式制約条件及び不等式制約条件付きLQ最適化問題を、下記(52)式に示す無制約条件のLQ最適化問題に変換できる。本実施形態に係る軌道生成部222は、下記(52)式に示す無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて最適解を求解する。

0121

次に、上記直交補空間変換により変換した無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記実施形態1と同様に、上記(52)式を行列表現すると、下記(53)式乃至(55)式のように表現できる。

0122

下記(56)式に示す、上記(54)式及び(55)式のラグランジュ乗数を導入する。

0123

続いて、下記(57)式に示すように、上記(55)式にスラック変数を導入する。

0124

上記(56)式及び(57)式の導入により、上記(53)式乃至(55)式に示すLQ最適化問題の最適解条件(KKT)は、下記(58)式で表現できる。

0125

上記(53)式乃至(55)式は、凸2次計画問題と称されるLQ最適化問題であり。内点法やアクティブセット法などの既知の求解法(収束演算)を用いて効率的に求解できる。これら求解法はニュートン法をベースにした求解法であり、ニュートン法の収束演算中でニュートンの方向計算を行い、リカッチ型再帰的計算法による連立一次方程式の求解を行うこととなる。また、凸2次計画問題の計算量の大部分は、この連立方程式の計算が占めているため、この計算の高速化が非常に有効となる。

0126

上述の如く、直交補空間変換を行うことで、上記(50)式に示す等式制約条件及び不等式制約条件付きLQ最適化問題を、上記(52)式に示す無制約条件のLQ最適化問題に変換する。これにより、このLQ最適化問題の連立一次方程式の求解に、安定かつ高速のリカッチ型再帰的計算法を用いることができる。したがって、LQ最適化問題の最適解を高速に求解できる。

0127

本実施形態において、軌道生成部222は、リカッチ型再帰的計算法による連立一次方程式の求解を、例えば、内点法やアクティブセット法などの収束演算の中で行う。以下、本実施形態において、内点法を用いた求解法を説明するがこれに限定されない。アクティブセット法を用いた求解法も、内点法と同様の手法で求解できる。

0128

内点法は、上記(58)式をニュートン法とラインサーチにより効率的に解くことにより、最適解を求解する手法である。なお、本実施形態においては、内点法の中で、最もスタンダードな主双対内点法を用いる場合について説明するが、これに限定されない。

0129

まず、上記(58)式にニュートン法を適用すると、下記(59)式及び(60)式が導出される。但し、下記(60)式において、(○の中に×)は要素同志の積を意味し、Λ≒diag(λ)、Z≒diag(z)とする。

0130

次に、下記(61)式及び(62)式に示すようにcomplementary measure μとステップ幅αpを導入する。

0131

ここで、complementary measure μは収束演算の残差総計、ステップ幅αpはλ≧0、z≧0を満足する範囲でニュートン方向への最大のスッテップ幅を求めていると理解すると分かり易い。

0132

なお、主双対内点法のアルゴリズムを簡略して記載すると以下のようになる。

0133

軌道生成部222は、上記主双対内点法を用いて収束演算を行い、最適解ζを求解する。軌道生成部222は、上記実施形態1と同様に、上記求解した最適解ζと、上記(16)式及び(20)式と、を用いて、上記(50)式に示す等式制約条件及び不等式条件付きLQ最適化問題のパラメータを復元し、その最適解であるx[k]及びu[k]を得る。

0134

ここで、主双対内点法のアルゴリズム内に示した上記(59)式の求解について詳細に説明する。まず、下記(63)式が成立する。

0135

上記(63)式を用いて、上記(59)式は、下記(64)式のように表現できる。

0136

ここで、Θ≒Z−1Λ及びrz′≒rz−Z−1rλと置くと、上記(64)式は、下記(65)式のように表現できる。



さらに、上記行列(65)式の各係数を並べ替えると、下記(66)式のように表現できる。

0137

ここで、下記(67)式が成立する。

0138

上記(67)式を用いて、上記(66)式は、下記(68)式のように表現できる。



但し、上記(68)式において、パラメータを下記(69)式のように設定している。

0139

上記(68)式は、上述した実施形態1のリカッチ型再帰的計算法で示した(38)式と同様の形となっている。したがって、軌道生成部222は、上記(68)式に示す連立1次方程式についても、上記実施形態1と同様に、リカッチ型再帰的計算法を用いて高速かつ安定的に求解できる。

0140

すなわち、軌道生成部222は、上記(68)式に対して、下記(70)式に示す再帰計算を繰り返す。

0141

さらに、軌道生成部222は、下記(71)式に示す再帰計算を行うことで、(Δvk、Δζk、Δyk)を算出する。



軌道生成部222は、算出したΔζkを上記(67)式に代入することで、Δzkを算出する。軌道生成部222は、算出したΔzk=[Δz1T、Δz2T、・・・ΔzNT]Tを上記(63)式に代入することで、Δλを算出する。以上により、上記(59)式の求解が完了する。

0142

図18は、上述した等式制約条件及び不等式制約条件付きLQ最適化問題の最適解の求解フローを示すフローチャートである。
まず、軌道生成部222は、解ベクトル初期解(η=η0、y=y0、z=z0、λ=λ0)を行う(ステップS301)。

0143

軌道生成部222は、繰返パラメータn=0を設定する(ステップS302)。
軌道生成部222は、上記(60)式を用いて、残差[rη、ry、rz、rλ]を算出する(ステップS303)。

0144

軌道生成部222は、リカッチ型再帰的計算法によるニュートン方向[Δη、Δy、Δz、Δλ]の計算を行う(ステップS304)。
軌道生成部222は、上記(62)式を用いて、ステップ幅αpを算出する(ステップS305)。

0145

軌道生成部222は、上記算出したニュートン方向[Δη、Δy、Δz、Δλ]とステップ幅αpとに基づいて、下記式を用いて解ベクトルの更新を行う(ステップS306)。
[η、y、z、λ]=[η、y、z、λ]+βαp[Δη、Δy、Δz、Δλ]

0146

軌道生成部222は、上記(61)式を用いて、complementary measure μを算出する(ステップS307)。
軌道生成部222は、条件(μ>μmin and n<nmax)を満足するか否かを判定する(ステップS308)。

0147

軌道生成部222は、条件(μ>μmin and n<nmax)を満足すると判定したとき( ステップS308のYES)、n=n+1を設定し、上記(ステップS303)の処理に戻る。

0148

軌道生成部222は、条件(μ>μmin and n<nmax)を満足しないと判定したとき(ステップS308のNO)、上記収束したときのηに基づいて、上記(53)乃至(55)式からの最適解ζを算出する。そして、軌道生成部222は、この最適解ζと、上記(16)式及び(20)式と、を用いて、上記(52)式に示す等式制約条件及び不等式制約条件付きLQ最適化問題のパラメータを復元し、x[k]及びu[k]を算出する(ステップS309)。

0149

(第3実施形態)
上記実施形態1に係る軌道生成部222は、線形不変な等式制約条件付き最適化問題を求解しているが、本実施形態3に係る軌道生成部222は、線形時変な等式制約条件付き最適化問題を求解する。

0150

例えば、サンプリング間隔が変化するような場合を考えると、Δtは固定ではなく、Δtkのようにサンプリング点毎に変化することとなる。この場合、上記(8)式は、下記(72)式のように線形時変の制御システムとして表現できる。

0151

ここで、線形時変の最適化問題の状態方程式は、例えば、下記(73)式に示す関係が成立する。すなわち、(73)式に示す状態方程式は、線形時変の制御パラメータAk、Bkを含むこととなる。

0152

上記(73)式、(12)式、及び(13)式より、軌道生成部222は、下記(74)式に示す等式制約条件付きLQ最適化問題を求解し、重心軌道を生成することとなる。



但し、上記(74)式において、下記(75)式が成立するものとする。

0153

以上から、実施形態1と同様に、等式制約条件付きLQ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のLQ最適化問題を導出する。
まず、上記(13)式(等式制約条件:Ckx[k]=dk)より、状態変数xの変換式である下記(76)式が上記実施形態1と同様に導出される。

0154

Ck+1Bの直交補空間を用いて変数変換を行う。Ck+1Bを下記(77)式に示すようにQR分解する。

0155

上記(77)式を用いて上記(20)式と同様に、入力uの変換式である下記(78)式を導出する。



但し、上記(78)式における各パラメータを下記(79)式に示すように設定する。



k=0のときは、上記(78)式における各パラメータを下記(80)式に示すように設定する。

0156

以上より、上記(8)式を上記(73)式、(76)式、及び(78)式を用いて変形し、状態方程式の変換式である下記(81)式を導出する。



但し、上記(81)式における各パラメータを下記(82)式に示すように設定する。



k=0のときは、上記(81)式における各パラメータを下記(83)式に示すように設定する。

0157

上記実施形態1と同様に、上記(12)式に示す評価関数Jを変形し、評価関数の変換式である下記(84)式を導出する。



但し、上記(84)式における各パラメータを下記(85)式に示すように設定する。

0158

以上から、本実施形態に係る軌道生成部222は、実施形態1と同様に、上記直交補空間を用いて変換した下記(86)式に示す無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。

0159

最後に、軌道生成部222は、上記求解した最適解ζと、上記(16)式及び(20)式と、を用いて、上記(74)式に示す等式制約条件付きLQ最適化問題のパラメータを復元し、x[k]及びu[k]を算出する。

0160

(第4実施形態)
本実施形態4に係る軌道生成部222は、線形時変な、所定の区間内だけ接触力が変化しないように設定した、入力を含む等式制約条件付き最適化問題を求解する。

0161

例えば、未来のサンプリング区間において、移動ロボットが一定の力で物体を押して動かす等の、接触力を変動させたくない区間が存在する場合を想定する。より具体的には、2番目の接触点と最後から2番目の接触点をある区間内だけ接触力を変化しないようにした場合、当該区間における等式制約条件を下記(87)式に示すように入力Ek(u)を含むこととなる。

0162

なお、上記接触力を変動させたくない区間以外の区間においては、Ckは下記(88)式のように設定できる。

0163

上記式より、軌道生成部222は、下記(89)式に示す入力を含む等式制約条件付きLQ最適化問題を求解し、重心軌道を生成することとなる。



但し、下記(90)式が成立する。

0164

まず、等式制約条件内の入力uに対する係数Ekの転置行列をQR分解すると下記(91)式が導出される。

0165

上記(91)式を用いて、上記入力を含む等式制約条件の(87)式を、下記(92)式に示すように変換できる。

0166

上記(92)式を上記状態方程式(8)式に代入することで、下記(93)式が導出される。



以降の式変換の方法は、上記実施形態3と同一であるため、省略して説明する。以上から、線形時変な、入力を含む等式制約条件付き最適化問題を直交補空間変換を行い、下記(94)に示す無制約条件のLQ最適化問題を導出する。

0167

軌道生成部222は、導出した上記(94)式に示す無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。最後に、軌道生成部222は、上記求解した最適解ζと、上記(16)式及び(20)式と、を用いて、上記(89)式に示す入力を含む等式制約条件付きLQ最適化問題のパラメータを復元し、x[k]及びu[k]を算出する。なお、軌道生成部222は、上記同様に、線形不変な、入力を含む等式制約条件付き最適化問題を求解してもよい。

0168

(第5実施形態)
本実施形態4に係る軌道生成部222は、線形時変な、所定の区間内だけ接触力が変化しないように設定した入力を含む等式制約条件、及び、所定の区間内だけ接触力に制限をかけるように設定した入力を含む不等式制約条件付き最適化問題を求解する。

0169

例えば、移動ロボットの手先や足先の急激な接触力の変動を防ぐように、接触力の変化に制限をかけたい場合を想定する。例えば、2番目の接触点の接触力の増加量と、最後から2番目の接触点の接触力の減少量と、をある区間内だけ、制限したい場合、制限をかけたい区間における不等式制約条件は、下記(95)式のように入力Γku[k]を含むこととなる。

0170

但し、上記(95)式において、Δflmは各接触点の6軸力の制限値を縦に並べたベクトルである。また、上記制限をかけたい区間以外の区間においては、Pkは下記(96)式のように設定できる。

0171

上記式より、軌道生成部222は、下記(97)式に示す入力を含む等式制約条件付きLQ最適化問題を求解し、重心軌道を生成することとなる。



但し、下記(98)式が成立する。

0172

不等式制約条件を示す上記(95)式に上記(92)式を代入することで、不等式制約条件を下記(99)式に変換する。



但し、上記(99)式のパラメータを下記(100)式のように設定する。

0173

上記(33)式と上記(99)式から、軌道生成部222は、下記(101)式に示す無制約条件のLQ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを求解することとなる。



以降に示す、上記無制約条件のLQ最適化問題に対するリカッチ型再帰的計算法による最適解ζの求解方法は、上記実施形態2において説明した求解方法と略同一であるため、相違点のみを説明する。

0174

上記(53)式乃至(55)式の係数行列Pが下記(102)式に置き換わる。

0175

したがって、上記(66)式は、下記(103)式に置き換わる。

0176

ここで、下記(104)式が成立する。

0177

上記(104)式を用いて、上記(103)式を下記(105)式のように変形できる。

0178

但し、上記(105)式の各パラメータを下記(106)式のように設定する。



実施形態2の上記(68)式と上記(105)式との相違は、Sk(ハット)がS′k(ハット)となっているだけで、その他のパラメータは同一である。したがって、軌道生成部222は、以降の計算について、上記実施形態2と同一の計算を行い、最適解ζを高速に求解できる。最後に、軌道生成部222は、上記求解した最適解ζと、上記(16)式及び(20)式と、を用いて、入力を含む等式制約条件及び不等式制約条件付きLQ最適化問題のパラメータを復元し、x[k]及びu[k]を算出する。なお、軌道生成部222は、上記同様に、線形不変な、入力を含む等式制約条件及び不等式制約条件付き最適化問題を求解してもよい。また、軌道生成部222は、線形不変あるいは線形時変な、等式制約条件及び入力を含む不等式制約条件付き最適化問題を求解してもよい。

0179

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図15図18に示す処理を、CPU210aにコンピュータプログラムを実行させることにより実現することも可能である。

0180

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク磁気テープハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)、CD−ROM(Read Only Memory)、CD−R、CD−R/W、半導体メモリ(例えば、マスクROMPROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM(random access memory))を含む。

0181

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

0182

1a-28a…モータ、1b-28b…エンコーダ、25…接触力センサ、100…移動ロボット、210…最適制御装置、221…接触点計画設定部、222…軌道生成部、223…動作制御部。

ページトップへ

この技術を出願した法人

この技術を発明した人物

ページトップへ

関連する挑戦したい社会課題

関連する公募課題

該当するデータがありません

ページトップへ

技術視点だけで見ていませんか?

この技術の活用可能性がある分野

分野別動向を把握したい方- 事業化視点で見る -

(分野番号表示ON)※整理標準化データをもとに当社作成

ページトップへ

おススメ サービス

おススメ astavisionコンテンツ

新着 最近 公開された関連が強い技術

  • 株式会社トライフォース・マネジメントの「 力覚センサ」が 公開されました。( 2020/04/09)

    【課題】ロボットの安全性および信頼性を向上させるとともに、ロボットの見栄えを向上させる。【解決手段】ロボットに装着される力覚センサ1であって、検出対象となる力またはモーメントの作用を受ける受力体10と... 詳細

  • セイコーエプソン株式会社の「 制御装置、ロボットシステム、及びロボット」が 公開されました。( 2020/04/09)

    【課題】吐出面に対して吐出物を吐出するニードルを有するロボットにおいて、吐出不良の発生を抑制する。【解決手段】ロボットを制御する制御装置であって、制御部を備え、検出部と発信部との間に、ニードルが配置さ... 詳細

  • 学校法人福岡大学の「 ロボットセンサ」が 公開されました。( 2020/04/09)

    【課題】多関節ロボットの作業を効率化すつつ、安全性を確保することができるロボットセンサを提供する。【解決手段】多関節ロボットの少なくとも先端関節の先のアーム部表面における複数面に配設されるToF(Ti... 詳細

この 技術と関連性が強い技術

関連性が強い 技術一覧

この 技術と関連性が強い人物

関連性が強い人物一覧

この 技術と関連する社会課題

関連する挑戦したい社会課題一覧

この 技術と関連する公募課題

該当するデータがありません

astavision 新着記事

サイト情報について

本サービスは、国が公開している情報(公開特許公報、特許整理標準化データ等)を元に構成されています。出典元のデータには一部間違いやノイズがあり、情報の正確さについては保証致しかねます。また一時的に、各データの収録範囲や更新周期によって、一部の情報が正しく表示されないことがございます。当サイトの情報を元にした諸問題、不利益等について当方は何ら責任を負いかねることを予めご承知おきのほど宜しくお願い申し上げます。

主たる情報の出典

特許情報…特許整理標準化データ(XML編)、公開特許公報、特許公報、審決公報、Patent Map Guidance System データ