ところで、上記の如きコンプライアンス制御を行う移動体の動作時において、該移動体に実際に作用する全床反力と、目標全床反力との定常的な偏差は、移動体の目標運動を作成するときに使用した想定床面の位置や姿勢の実際の床面に対する誤差に起因するものと考えられる。従って、特許文献1に見られる如きコンプライアンス制御を行いつつ、実際の全床反力の観測値と、目標全床反力との偏差を基に、特許文献2に見られる如き手法によって、実際の床面の位置や姿勢を推定することが考えられる。
しかるに、この場合、次のような不都合を生じる恐れがある。
すなわち、特許文献1に見られる如き従来のコンプライアンス制御の技術では、目標全床反力中心点周りに発生する実際の床反力モーメントを、目標モーメントに追従させるための各脚リンクの先端部(足平部)の目標位置及び姿勢の修正は、両脚リンクの先端部の上下方向の位置を互いに逆向きに変化させる修正(以降、本欄では第1の修正という)と、各脚リンクの先端部の姿勢を変化させる修正(以降、本欄では第2の修正という)とを合成したものとされる。
この場合、第1の修正における各脚リンクの先端部の位置の修正量と、第2の修正における各脚リンクの先端部の姿勢の修正量とは各別に決定される。
しかるに、かかる従来のコンプライアン制御の技術では、第1の修正に起因して生じる床反力の変化と、第2の修正に起因する床反力の変化とが相互に干渉しやすい。このため、第1の修正と第2の修正とを合成してなる各脚リンクの先端部の位置及び姿勢の修正によって実際に生じる床反力モーメントの変化が、目標モーメントに対して過不足を生じやすいという不都合がある。
その結果、実際の全床反力の観測値と、目標全床反力との偏差を基に、特許文献2に見られる如き手法によって、実際の床面の位置や姿勢を推定した場合には、その推定精度を十分に高めることが困難となる恐れがある。
また、上記過不足の発生を極力抑制するためには、第1の修正による各脚リンクの先端部の位置の修正量を決定するためのゲイン(第1の修正によって目標全床反力中心点周りに発生させようとするモーメントの目標モーメントに対する度合いを規定するゲイン)と、第2の修正による各脚リンクの先端部の姿勢の修正量を決定するためのゲイン(第2の修正によって目標全床反力中心点周りに発生させようとするモーメントの目標モーメントに対する度合いを規定するゲイン)とを適切に調整しておく必要がある。そして、その調整作業には、一般に多大な工数を必要とするという不都合がある。
また、特許文献1に見られる如き技術に特許文献2に見られる如き技術を適用する場合には、特許文献1に見られるコンプライアンス制御の技術とは別に、床面推定用のコンプライアンスモデルの設定が必要となる。このため、移動体の制御処理が煩雑なものとなるとという不都合もある。
本発明にかかる背景に鑑みてなされたものであり、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うようにしつつ、移動体の動作環境の実際の床面の位置や姿勢を適切に推定することができる床面推定装置を提供することを目的とする。
まず、本発明の基礎となる技術事項を、以下に一般化して説明する。
図1に示す如く、基体102にN個(N≧2)の脚リンク103_1〜103_Nが連結された移動体101を想定する。この移動体101の脚リンク103_1〜103_Nのそれぞれは、複数の関節を備えており、これらの関節の変位によって、空間的な運動が可能である。なお、各関節は、回転型又は直動型の関節である。
そして、移動体101は、N個の脚リンク103_1〜103_Nのうちのいずれか1つ又は複数の脚リンクを支持脚として、その支持脚の先端部を接地(床面に接触)させることで移動体101の自重を該支持脚を介して床面に支えることが可能であると共に、脚リンク103_1〜103_Nの空間的な運動を行なうことによって、移動体101が床面上で移動可能である。以降、これらの脚リンク103_1〜103_Nのそれぞれを総称的に第i脚リンク103_i(i=1,2,…,N)、又は、脚リンク103_iという。
この移動体101の任意の運動状態において、床面から移動体101に作用するトータルの床反力である全床反力(ベクトル)を↑FMt、この↑FMtのうちの並進力ベクトルである全並進床反力を↑Ft、↑FMtのうちのモーメントベクトルである全床反力モーメントを↑Mtと表記する。
これらの全並進床反力↑Ft及び全床反力モーメント↑Mtは、それぞれ慣性座標系(床に対して固定された座標系)で見た3成分の縦ベクトルとして表現されるものとする。そして、全床反力↑FMtは、↑Ft及び↑Mtの各成分を並べた6成分の縦ベクトル(=[↑Ft,↑Mt]T)として表現されるものとする。また、全床反力↑FMtの作用点Ptは床面上の点とする。
なお、本明細書では、“↑”は、ベクトル(縦ベクトル)を表現する記号として用いる。また、上付き添え字“T”は、転置を意味する。また、本明細書では、理解の便宜上、並進力、モーメント、位置、姿勢等を表現する慣性座標系としては、例えば移動体101の前後方向の水平軸をX軸、鉛直方向をZ軸、これらのX軸及びZ軸に直交する方向(移動体101の左右方向)をY軸とする3軸直交座標系が用いられるものとする。
また、第i脚リンク103_i(i=1,2,…,N)に床面から作用する床反力(以下、脚床反力という)を↑FM_i、この↑FM_iのうちの並進力ベクトルである並進床反力を↑F_i、↑FM_iのうちのモーメントベクトルである床反力モーメントを↑M_iと表記する。
これらの並進床反力↑F_i及び床反力モーメント↑M_iは、全床反力↑FMtの場合と同様に、それぞれ慣性座標系で見た3成分の縦ベクトルとして表現されるものとする。そして、第i脚床反力↑FM_iは、↑F_i及び↑M_iの各成分を並べた6成分の縦ベクトル(=[↑F_i,↑M_i]T)として表現されるものとする。また、第i脚床反力↑FM_iの作用点P_iは、第i脚リンク103_iの接地面内(第i脚リンク103_iの先端部と床面との接触面内)の床面上の点とする。
補足すると、第i脚リンク103_iが非接地状態である場合(↑FM_i=ゼロベクトルとなる場合)には、第i脚床反力↑FM_iの作用点P_iは存在しないこととなるが、便宜上、該作用点P_iは任意に設定された点(例えば第i脚リンク103_iの先端部の代表点など)とする。
このとき、全床反力↑FMtと脚床反力↑FM__i(i=1,2,…,N)との間の関係は一般的に、次式(1)により表される。
なお、式(1)のただし書きにおいて、行列AA_iの成分“I”、“0”はそれぞれ、単位行列、零行列である。また、“×”は外積(ベクトル積)を表す算術符号である。
ここで、第i脚床反力↑FM_iの作用点P_iに摂動分の床反力↑ΔFM_i(=[↑ΔF_i,↑ΔM_i]T)を付加し、それによって、全床反力↑FMtの作用点Ptに摂動分の床反力↑ΔFMt(=[↑ΔFt,↑ΔMt]T)を付加することを想定する。なお、作用点P_iに摂動分の床反力↑ΔFM_iを付加するということは、作用点P_iに作用する床反力を↑FM_iから↑FM_i+↑ΔFM_iに変更することを意味する。同様に、作用点Ptに摂動分の床反力↑ΔFMtを付加するということは、作用点Ptに作用する床反力を↑FMtから↑FMt+↑ΔFMtに変更することを意味する。
以降、↑ΔFM_iを第i脚摂動床反力、↑ΔFM_iのうちの並進力ベクトル↑ΔF_i及びモーメントベクトル↑ΔM_iをそれぞれ、第i脚摂動並進床反力、第i脚摂動床反力モーメントという。また、↑ΔFMtを摂動全床反力、↑ΔFMtのうちの並進力ベクトル↑ΔFt及びモーメントベクトル↑ΔMtをそれぞれ、摂動全並進床反力、摂動全床反力モーメントという。
上記摂動全床反力↑ΔFMtと、第i脚摂動床反力↑ΔFM_i(i=1,2,…,N)との間の関係は、上記式(1)に基づいて、次式(2)により与えられる。
この式(2)におけるAA_i(i=1,2,…,N)は、式(1)のただし書きで定義したものと同じである。
また、第i脚摂動床反力↑ΔFM_iが、第i脚リンク103_iの接地面(以降、第i脚接地面という)の位置及び姿勢のばね性変位によって発生するものと仮定する。より詳しくは、第i脚接地面の位置のばね性変位(並進変位)によって、第i脚摂動並進床反力↑ΔF_iが発生し、第i脚接地面の姿勢のばね性変位(回転変位)によって、第i脚摂動床反力モーメント↑M_iが発生するものと仮定する。なお、第i脚接地面の位置及び姿勢のばね性変位は、第i脚接地面での床の弾性変形又は第i脚リンク103_iの先端部の弾性変形による並進変位及び回転変位にそれぞれ相当する。
このばね性変位による第i脚接地面の位置の変位量(第i脚接地面の3軸方向の並進変位量のベクトル。以降、ばね性並進変位量という)と姿勢の変位量(第i脚接地面の3軸周りの回転変位量のベクトル。以降、ばね性回転変位量という)とをそれぞれ↑Xorg_i、↑Xrot_iと表記する。このとき、↑Xorg_iと↑ΔF_iとの間の関係、及び↑Xrot_iと↑ΔM_iとの間の関係は、それぞれ、次式(3)、(4)により表されることとなる。
↑ΔF_i=Korg_i・↑Xorg_i ……(3)
↑ΔM_i=Krot_i・↑Xrot_i ……(4)
式(3)のKorg_iは、第i脚接地面のばね性並進変位量↑Xorg_iの各成分毎のばね定数を対角成分として構成される3次の対角行列(以下、並進ばね定数行列Korg_iという)、式(4)のKrot_iは、第i脚接地面のばね性回転変位量↑Xrot_iの各成分毎のばね定数を対角成分として構成される3次の対角行列(以下、回転ばね定数行列Krot_iという)である。
また、移動体101と床面との全ての接触面(接地状態の全ての脚リンク103の接地面)を代表する単一の仮想的な接触面としての代表接触面を想定し、この代表接触面にて全床反力↑FMtが移動体101に作用するものとする。該代表接触面は、所謂、支持多角形に相当するものである。
そして、第i脚接地面の場合と同様の考え方によって、摂動全床反力↑ΔFMtが、代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定する。より詳しくは、代表接触面の位置のばね性変位(並進変位)によって、摂動全並進床反力↑ΔFtが発生し、代表接触面の姿勢のばね性変位(回転変位)によって、摂動全床反力モーメント↑ΔMtが発生するものと仮定する。
従って、代表接触面の位置のばね性並進変位量(ばね性変位による3軸方向の並進変位量のベクトル)と姿勢のばね性回転変位量(ばね性変位による3軸周りの回転変位量のベクトル)とをそれぞれ↑Xc_org、↑Xc_rotとすると、↑Xc_orgと↑ΔFtとの間の関係、及び↑Xc_rotと↑ΔMtとの間の関係は、それぞれ、次式(5)、(6)により表されることとなる。
↑ΔFt=Kc_org・↑Xc_org ……(5)
↑ΔMt=Kc_rot・↑Xc_rot ……(6)
式(5)のKc_orgは、代表接触面のばね性並進変位量↑Xc_orgの各成分毎のばね定数を対角成分として構成される3次の対角行列(以下、並進ばね定数行列Kc_orgという)、式(6)のKc_rotは、代表接触面のばね性回転変位量↑Xc_rotの各成分毎のばね定数を対角成分として構成される3次の対角行列(以下、回転ばね定数行列Kc_rotという)である。
補足すると、上記代表接触面は、移動体101の単一の脚リンク103_iが接地している状態では、代表接触面は、その単一の脚リンク103_iの接地面(第i脚接地面)に一致するものとされる。そして、この状態では、代表接触面の位置及び姿勢のばね性変位は、第i脚接地面の位置及び姿勢のばね性変位と同じであり、第i脚接地面での床の弾性変形又は第i脚リンク103_iの先端部の弾性変形による並進変位及び回転変位にそれぞれ相当する。
また、移動体101の2つ以上の脚リンク103が接地している状態では、上記代表接触面は、接地している全ての脚リンク103の接地部分をひとまとめにした単一の仮想的な接地部の接地面としての意味を持つ。そして、この状態での代表接触面の位置及び姿勢のばね性変位は、当該仮想的な接地部の接地面での床の弾性変形あるいは当該仮想的な接地部の弾性変形による並進変位及び回転変位にそれぞれ相当する。
前記式(2)において、第i脚床反力↑FM_i(i=1,2,…,N)の作用点P_iにそれぞれ付加する第i脚摂動並進床反力↑ΔF_iによって全床反力↑FMtの作用点Ptに付加される摂動全並進床反力↑ΔFtに着目すると、次式(7)が得られる。
↑ΔFt=↑ΔF_1+↑ΔF_2+……+↑ΔF_N ……(7)
この式(7)と、前記式(3),(5)とから次式(8)が得られる。
Kc_org・↑Xc_org=Korg_1・↑Xorg_1+Korg_2・↑Xorg_2
+……+Korg_N・↑Xorg_N ……(8)
一方、摂動全床反力↑ΔFMtの付加前の全床反力↑FMtの作用点Ptを、全床反力中心点(COP)とし、第i脚摂動床反力↑ΔFM_iの付加前の第i脚床反力↑FM_iの作用点P_iを第i脚接地面の床反力中心点としたとき、任意の基準点に対する作用点Ptの位置ベクトル(これを↑Ptと表記する)と、作用点P_i(i=1,2,…,N)のそれぞれの位置ベクトル(これを↑P_iと表記する)との間の関係は、次式(9)により与えられる。
なお、全床反力中心点は、全床反力↑FMtの作用点であって、その点周りの全床反力モーメント↑Mtの水平成分(水平軸周り成分)がゼロとなるような点である。同様に、第i脚接地面の床反力中心点は、第i脚床反力↑FM_iの作用点であって、その点周りの床反力モーメント↑M_iの水平成分(水平軸周り成分)がゼロとなるような点である。
↑Pt=r_1・↑P_1+r_2・↑P_2+……+r_N・↑P_N ……(9)
この式(9)におけるr_i(i=1,2,…,N)は、r_i≡Fn_i/Fntにより定義される重み係数である。ここで、Fn_i(i=1,2,…,N)は、第i脚並進床反力↑F_iのうちの床面に垂直な垂直抗力成分の絶対値(以降、床面垂直抗力成分という)、Fntは全並進床反力↑Ftのうちの床面に垂直な垂直抗力成分の絶対値(床面垂直抗力成分)であり、Fnt=Fn_1+Fn_2+……+Fn_Nである。従って、重み係数r_iは、全並進床反力↑Ftの床面垂直抗力成分Fntに対する、第i脚床反力↑F_iの床面垂直抗力成分Fn_iの比率であり、その値は、0≦r_i≦1の範囲内の値である。
なお、本明細書では、Fn_i,Fntは、それぞれ、↑F_iの鉛直成分(Z軸成分)、↑Ftの鉛直成分(Z軸成分)に一致又はほぼ一致するものとする。
そして、この式(9)と同様の関係が、代表接触面の位置のばね性並進変位量↑Xc_orgと、第i脚接地面の位置のばね性並進変位量↑Xorg_iとの間にも成立するものと仮定する。すなわち、次式(10)が成立するものと仮定する。
↑Xc_org=r_1・↑Xorg_1+r_2・↑Xorg_2+……+r_N・↑Xorg_N ……(10)
このとき、この式(10)と前記式(8)とから、次式(11)が得られる。
Korg_i=r_i・Kc_org ……(11)
本発明では、上記式(10)が、代表接触面のばね性並進変位量↑Xc_orgと第i脚接地面(i=1,2,…,N)のばね性並進変位量↑Xorg_iとの間の関係を表す基本式、式(11)が、代表接触面の並進ばね定数行列Kc_orgと第i脚接地面(i=1,2,…,N)の並進ばね定数行列Korg_iとの関係を表す基本式とされる。
従って、第i脚接地面の並進ばね定数行列Korg_iの各対角成分(3軸方向のそれぞれの並進変位に関するばね定数)は、重み係数r_iに比例し、重み係数r_iの値が大きいほど(“1”に近いほど)、すなわち、全並進床反力↑Ftの床面垂直抗力成分Fntに対する第i脚並進床反力↑F_iの床面垂直抗力成分Fn_iの比率が大きいほど、大きくなるものとされる。換言すれば、Fntに対するFn_iの比率が大きいほど、要求される第i脚摂動並進床反力に対する第i脚接地面のばね性並進変位量↑Xorg_iの変化の感度が高くなるものとされる。
また、単一の第i脚リンク103_iだけが接地している場合(r_i=1となる場合)には、第i脚接地面と代表接触面とが一致することに対応して、第i脚接地面の並進ばね定数行列Korg_iと、代表接触面の並進ばね定数行列Kc_orgとが互いに一致する。さらにこの場合、↑ΔF_i=↑ΔFtとなるので、第i脚接地面のばね性並進変位量↑Xorg_iと、代表接触面のばね性並進変位量↑Xc_orgとが互いに一致することとなる。
そして、2つ以上の脚リンク103が接地している場合には、代表接触面のばね性並進変位量↑Xc_orgは、非接地状態の脚リンクを除く各脚リンク103_iに対応する第i脚接地面のばね性並進変位量↑Xorg_iの重み付き平均値(r_iを重み係数とする重み付き平均値)となる。
次に、前記式(2)において、第1〜第N脚床反力↑FM_1〜↑FM_Nのそれぞれの並進床反力↑F_1〜↑F_Nを一定に保持した状態で、第i脚床反力↑FM_i(i=1,2,…,N)の作用点P_iに第i脚摂動床反力モーメント↑ΔM_iを付加することによって全床反力↑FMtの作用点Ptに付加される摂動全床反力モーメント↑ΔMtに着目すると、次式(12)が得られる。
↑ΔMt=↑ΔM_1+↑ΔM_2+……+↑ΔM_N ……(12)
そして、この式(12)と、前記式(4),(6)とから次式(13)が得られる。
Kc_rot・↑Xc_rot=Krot_1・↑Xrot_1+Krot_2・↑Xrot_2
+……+Krot_N・↑Xrot_N ……(13)
一方、第i脚床反力↑FM_iの作用点P_iを第i脚接地面の床反力中心点としたとき、第1〜第N脚床反力↑FM_1〜↑FM_Nのそれぞれの並進床反力↑F_1〜↑F_Nを一定に保持した状態で、第i脚床反力↑FM_iの作用点P_i(床反力中心点)に第i脚摂動床反力モーメント↑ΔM_iを付加するということは、第i脚接地面での床反力中心点の水平方向位置を点P_iからずらすことと等価である。同様に、全床反力↑FMtの作用点Ptを全床反力中心点(COP)としたとき、全床反力↑FMtの作用点Ptに摂動全床反力モーメント↑ΔMtが付加されるということは、全床反力中心点の水平方向位置が点Ptからずれることと等価である。
この場合、第i脚接地面での床反力中心点の水平方向位置の変位量(2成分の変位量ベクトル)を↑ΔRpt_i、全床反力中心点の水平方向位置の変位量(2成分の変位量ベクトル)を↑ΔCOPとすると、↑ΔCOP・Fnt≒↑ΔMtの水平軸周り成分(X軸及びY軸周り成分)、↑ΔRpt_i・Fn_i≒↑ΔM_iの水平軸周り成分(X軸及びY軸周り成分)となる。このため、上記式(12)から、次式(14)が得られる。
↑ΔCOP=r_1・↑ΔRpt_1+r_2・↑ΔRpt_2
+……+r_N・↑ΔRpt_N ……(14)
なお、式(14)のr_i(i=1,2,…,N)は、前記した重み係数r_i(≡Fn_i/Fnt)である。
また、第1〜第N脚接地面は、共通の床面上の部分であるから、第1〜第N脚接地面のそれぞれに作用する垂直抗力Fn_iが互いに同じである場合、第i脚接地面の床反力中心点の水平方向位置の変位量↑ΔRpt_iに対応する摂動床反力モーメント(=↑ΔRpt_i・Fn_i)と、第i脚接地面のばね性変位による回転変位量↑Xrot_iの水平軸周り成分との間の関係は、いずれの第i脚接地面についても同じになると考ええられる。
そこで、第1〜第N脚接地面のそれぞれに作用する床面垂直抗力成分Fn_iが互いに同じ値(これをFnaとおく)である場合、次式(15)が成立すると考えられる。
↑ΔRpt_i・Fna=Krot・↑Xrot_i_xy ……(15)
なお、↑Xrot_i_xyは、第i脚接地面のばね性回転変位量↑Xrot_iの水平軸周り成分(X軸及びY軸周り成分)であり、Krotは、2次の対角行列(X軸周り及びY軸周りの回転に関するばね定数行列)である。
そして、上記式(15)が、代表接触面についても同様に成立するものと仮定すると、次式(16)が得られる。
↑ΔCOP・Fna=Krot・↑Xc_rot_xy ……(16)
なお、↑Xc_rot_xyは、代表接触面のばね性回転変位量↑Xc_rotの水平軸周り成分(X軸及びY軸周り成分)である。
上記式(14)〜(16)により、次式(17)が得られる。
Krot・↑Xc_rot_xy=r_1・Krot・↑Xrot_1_xy+r_2・Krot・↑Xrot_2_xy
+……+r_N・Krot・↑Xrot_N_xy ……(17)
さらに、この式(17)から次式(18)が得られる。
↑Xc_rot_xy=r_1・↑Xrot_1_xy+r_2・↑Xrot_2_xy
+……+r_N・↑Xrot_N_xy ……(18)
そこで、本実施形態では、代表接触面のばね性回転変位量↑Xc_orgと、第i脚接地面のばね性回転変位量↑Xorg_iとの間に、それらの全ての成分(3成分)について、上記式(18)と同じ関係が成立するものと仮定する。すなわち、次式(19)が成立するものと仮定する。
↑Xc_rot=r_1・↑Xrot_1+r_2・↑Xrot_2
+……+r_N・↑Xrot_N ……(19)
このとき、この式(19)と前記式(13)とから、次式(20)が得られる。
Krot_i=r_i・Kc_rot ……(20)
本実施形態では、上記式(19)が、代表接触面のばね性回転変位量↑Xc_rotと第i脚接地面(i=1,2,…,N)のばね性回転変位量↑Xrot_iとの間の関係を表す基本式、式(20)が、代表接触面の回転ばね定数行列Kc_rotと第i脚接地面(i=1,2,…,N)の回転ばね定数行列Krot_iとの関係を表す基本式とされる。
従って、第i脚接地面の回転ばね定数行列Krot_iの各対角成分(3軸周りのそれぞれの回転変位に関するばね定数)は、重み係数r_iに比例し、重み係数r_iの値が大きいほど(“1”に近いほど)、すなわち、全並進床反力↑Ftの床面垂直抗力成分Fntに対する第i脚並進床反力↑F_iの床面垂直抗力成分Fn_iの比率が大きいほど、大きくなるものとされる。換言すれば、Fntに対するFn_iの比率が大きいほど、要求される第i脚摂動床反力モーメントに対する第i脚接地面のばね性回転変位量↑Xrot_iの変化の感度が高くなるものとされる。
また、単一の第i脚リンク100_iだけが接地している場合(r_i=1となる場合)には、第i脚接地面と代表接触面とが一致することに対応して、第i脚接地面の回転ばね定数行列Krot_iと、代表接触面の回転ばね定数行列Kc_rotとが互いに一致する。さらにこの場合、↑ΔM_i=↑ΔMtとなるので、第i脚接地面のばね性回転変位量↑Xrot_iと、代表接触面のばね性回転変位量↑Xc_rotとが互いに一致することとなる。
そして、2つ以上の脚リンク103が接地している場合には、代表接触面のばね性回転変位量↑Xc_rotは、非接地状態の脚リンクを除く各脚リンク103_iに対応する第i脚接地面のばね性回転変位量↑Xrot_iの重み付き平均値(r_iを重み係数とする重み付き平均値)となる。
次に、前記式(2)に式(3)〜(6)を適用し、さらに、式(11),(20)を適用することによって、次式(21)が得られる。
なお、前記式(11)、(20)は、全床反力↑FMtの作用点Ptが全床反力中心点、第i脚床反力↑FM_iの作用点P_iが第i脚接地面の床反力中心点であることを前提としているので、式(21)におけるVV_i(i=1,2,…,N)は、全床反力中心点としての作用点Ptに対する第i脚接地面の床反力中心点の位置ベクトルを改めて↑V_iとしたとき、次のように定義される行列である。
VV_i:VV_i・↑F_i=V_i×↑F_iとなる行列
ただし、↑V_i:全床反力中心点に対する第i脚接地面の床反力中心点の位置ベクトル
なお、この場合、上記全床反力中心点は、より詳しくは、摂動全床反力↑ΔFMtが付加される前の全床反力↑FMtに対応する全床反力中心点であり、第i脚接地面の床反力中心点は、第i脚摂動床反力↑ΔFM_iが付加される前の第i脚床反力↑FM_iに対応する床反力中心点である。
さらに、上記式(21)から、次式(22)が得られる。
ここで、移動体101の基体102の位置(3軸方向の位置)及び姿勢(3軸周りの姿勢角)と移動体101の各関節の変位量とを成分として構成される一般化変数ベクトルを↑q、代表接触面の位置のばね性並進変位量↑Xc_org及び姿勢のばね性回転変位量↑Xc_rotから成る変位量ベクトル(=[↑Xc_org,↑Xc_rot]T)を↑Xc、第i脚接地面の位置のばね性並進変位量↑Xorg_i及び姿勢のばね性回転変位量↑Xrot_iから成る変位量ベクトル(=[↑Xorg_i,↑Xrot_i]T)を↑X_iと表記する。以降、↑Xcを代表接触面のばね性並進・回転変位量、↑X_iを第i脚接地面のばね性並進・回転変位量という。なお、上記一般化変数ベクトル↑qは、より詳しくは、基体102の位置及び姿勢の6個の成分と、移動体101の各関節の変位量とを並べた縦ベクトルである。
この場合、代表接触面のばね性並進・回転変位量↑Xcを代表接触面の位置及び姿勢の単位時間あたりの変位量(時間的変化率)とみなすと、代表接触面の位置及び姿勢の単位時間あたりの変位量と、一般化変数ベクトル↑qの単位時間当たりの変化量(時間的変化率)↑Δqとの間の関係を表現するヤコビアン行列は、↑Xcと↑Δqとの間の関係を次式(23)により表す行列Jcとして表現される。なお、↑Δqは、一般化変数ベクトル↑qの各成分の単位時間当たりの変化量を並べた縦ベクトルである。
↑Xc=Jc・↑Δq ……(23)
同様に、第i脚接地面のばね性並進・回転変位量↑X_iを第i脚接地面の位置及び姿勢の単位時間あたりの変位量(時間的変化率)とみなすと、第i脚接地面の位置及び姿勢の単位時間あたりの変位量と、一般化変数ベクトル↑qの単位時間当たりの変化量(時間的変化率)↑Δqとの間の関係を表現するヤコビアン行列は、↑X_iと↑Δqとの間の関係を次式(24)により表す行列J_iとして表現される。
↑X_i=J_i・↑Δq ……(24)
補足すると、代表接触面の位置の単位時間当たりの変位量と↑Δqとの間の関係を表現するヤコビアン行列(↑Xc_orgと↑Δqとの間の関係を次式(25a)により表現する行列)をJc_org、代表接触面の姿勢の単位時間当たりの変位量と↑Δqとの間の関係を表現するヤコビアン行列(↑Xc_rotと↑Δqとの間の関係を次式(25b)により表現する行列)をJc_rotと表記すると、式(25c)に示す如くJc=[Jc_org,Jc_rot]Tとなる。
↑Xc_org=Jc_org・↑Δq ……(25a)
↑Xc_rot=Jc_rot・↑Δq ……(25b)
Jc=[Jc_org,Jc_rot]T ……(25c)
同様に、第i脚接地面の位置の単位時間当たりの変位量と↑Δqとの間の関係を表現するヤコビアン行列(↑Xorg_iと↑Δqとの間の関係を次式(26a)により表す行列)をJorg_i、第i脚接地面の姿勢の単位時間当たりの変位量と↑Δqとの間の関係を表現するヤコビアン行列(↑Xrot_iと↑Δqとの間の関係を次式(26b)により表す行列)をJrot_iと表記すると、式(26c)に示す如くJ_i=[Jorg_i,Jrot_i]Tとなる。
↑Xorg_i=Jorg_i・↑Δq ……(26a)
↑Xrot_i=Jrot_i・↑Δq ……(26b)
J_i=[Jorg_i,Jrot_i]T ……(26c)
前記式(22)の両辺を微分し、前記式(23)、(24)を適用することで、次式(27)が得られる。
従って、代表接触面の位置及び姿勢の変位に関するヤコビアン行列Jc(以降、代表接触面ヤコビアン行列Jcという)は、各脚接地面の位置及び姿勢の変位に関するヤコビアン行列J_i(i=1,2,…,N)から、上記の式(27)に基づいて決定できることとなる。
この場合、第i脚接地面の位置及び姿勢は、第i脚リンク103_iの先端部の位置及び姿勢であるから、ヤコビアン行列J_i(以降、脚リンクヤコビアン行列J_iという)は、換言すれば、第i脚リンク103_iの先端部の位置及び姿勢の単位時間当たりの変化量と、↑Δqとの間の関係を表現するヤコビアン行列である。そして、このようなヤコビアン行列J_iは、移動体101の各関節の実際の変位量の観測値及びその時間的変化率を基に特定できる。
そして、このように各脚リンクヤコビアン行列J_i(i=1,2,…,N)を特定すれば、それらのJ_iを用いて、上記式(27)により、代表接触面ヤコビンア行列Jcを決定できることとなる。
一方、移動体101の所要の状態を実現するための要求操作量として、ある値の摂動全床反力↑ΔFMtが与えられたとき、この↑ΔFMtは前記式(5),(6)に基づいて、代表接触面のばね性並進・回転変位量↑Xc(=[↑Xc_org,↑Xc_rot]T)の要求値に変換できることとなる。この変換は、次式(28)により与えられる。
そして、代表接触面ヤコビアン行列Jcの擬似逆行列をJc-1とおくと、代表接触面のばね性並進・回転変位量変位量↑Xcの要求値から、次式(29)により、摂動全床反力↑ΔFMtの要求値を実現するための、移動体101の各関節の変位量の修正量を決定できることとなる。
↑Δq=Jc-1・↑Xc ……式(29)
そこで、移動体101の任意の運動状態において、移動体101に作用する全床反力を制御するための要求操作量として、摂動全床反力↑ΔFMtの要求値が与えられたとき、前記式(27)に基づいて代表接触面ヤコビアン行列Jcを求めると共に、このJcの擬似逆行列Jc-1を決定する。さらに、前記式(28)に基づいて、↑ΔFMtの要求値に対応する代表接触面の変位量ベクトル↑Xcを求める。そして、この↑Xcと、Jc-1とから式(29)によって、摂動全床反力↑ΔFMtの要求値を実現するための各関節の変位量の修正量を決定できることとなる。
ひいては、各脚リンク103_iの先端部の位置及び姿勢の修正量自体をそれぞれ決定することなく、一括的に、各関節の変位量を決定できることとなる。
次に、移動体101に実際に作用する全床反力を、該移動体101の目標運動を実現するための目標全床反力に追従させるようにように、上記摂動全床反力↑ΔFMtの要求値を決定することを想定する。そして、この場合、↑ΔFMtの要求値を、実際の全床反力の観測値と、目標全床反力との偏差を積分したものとして決定し、あるいは、少なくとも該偏差を積分してなる積分項と、該偏差に比例する比例項とを合成することによって決定するものとする。
この場合、実際の全床反力の観測値と、目標全床反力との偏差を積分したもの(以降、これを↑ΔFMt_intと表記する)は、移動体101の目標運動で想定されている想定床面の位置及び姿勢と、実際の床面の位置及び姿勢との定常的な差分に応じたものとなる。このため、当該偏差の積分値↑ΔFMt_intを、前記式(28)により変換することで算出される↑Xc、すなわち、↑ΔFMt_intから、次式(28−1)により算出される↑Xc_intは、想定床面の位置及び姿勢と、実際の床面の位置及び姿勢との定常的な誤差分に相当するものとなると考えられる。
なお、式(28−1)においては、↑Xc_intのうちの並進変位量の成分、回転変位量の成分をそれぞれ↑Xc_org_int、↑Xc_rot_intと表記し、↑ΔFMt_intのうちの並進力成分、モーメント成分をそれぞれ↑ΔFt_int、↑ΔMt_intと表記している。
従って、想定床面の位置及び姿勢を上記↑Xc_intにより補正することで、実際の床面の位置及び姿勢を推定できることとなる。
以上が、本発明の基礎とする技術事項である。
以上説明したことを基礎として、以下に本発明を説明する。
本発明の第1の態様は、基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差を積分してなる値を、該偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量として決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Jcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列J_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式(27)により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された前記要求変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする(第1発明)。
あるいは、本発明の第2の態様は、基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量を、少なくとも該偏差に比例する比例項と、該偏差を積分してなる積分項とを合成することにより決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Jcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列J_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式(27)により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段と、
前記全床反力要求修正量のうちの前記積分項と前記代表接触面のばね定数とから、該積分項に対応する該代表接触面の位置及び姿勢の変位量である代表接触面定常変位量を算出する代表接触面定常変位量算出手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面定常変位量算出手段により算出された前記代表接触面定常変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする(第2発明)。
ここで、前記式(27)と、この式(27)の変数の意味とを改めて記載すると、次の通りである。
なお、本発明において、「床面」は、屋内の通常の床面に限らず、屋外の地面もしくは路面であってもよい。また、想定床面、あるいは、実際の床面の位置及び姿勢は、移動体の各脚リンクの先端部の接地面もしくはその近辺の床面の位置及び姿勢を意味する。
上記第1発明及び第2発明によれば、前記移動体に実際に作用する全床反力(以降、実全床反力ということがある)の観測値と、前記目標全床反力との偏差をゼロに近づけるためのフィードバック操作量(制御入力)としての前記全床反力要求修正量が、該偏差に応じて決定される。
この場合、第1発明では、該偏差を積分してなる値(積分項)が、そのまま全床反力要求修正量として決定される。また、第2発明では、少なくとも該偏差に比例する比例項(該偏差に所定値のゲインを乗じてなる値)と、該偏差を積分してなる積分とを合成することにより前記全床反力要求修正量が決定される。
このように決定される全床反力要求修正量は、第1発明及び第2発明のいずれにおいても、前記摂動全床波力↑ΔFMtの要求値に相当するものである。
なお、第2発明では、比例項及び積分項だけを合成したものを前記全床反力要求修正量としてもよいが、比例項及び積分項以外の項、例えば微分項をさらに合成することで、前記全床反力要求修正量を決定してもよい。
また、第1発明及び第2発明における前記目標全床反力としては、例えば、実際の床面のモデルとしての想定床面上で、前記移動体の目標運動に対して適宜の動力学モデルの動力学的関係が成立するように作成されたものを採用できる。あるいは、移動体の目標運動に対して上記動力学的関係が成立するように作成した基準の全床反力を、移動体の運動に関する所定の状態量(移動体の特定の部位(基体等)もしくは全体重心点の位置、姿勢、又はそれらの変化速度等の状態量)の目標値と実際の値との偏差に応じて、該偏差をゼロに近づけるように補正したものであってもよい。
第1発明及び第2発明では、上記の如く決定された全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量が代表接触面位置姿勢変位量算出手段により算出される。
これにより、実全床反力を目標全床反力に追従させるためのフィードバック操作量(制御入力)としての全床反力要求修正量が、単一の仮想面たる代表接触面の位置及び姿勢の要求変位量に変換される。
ここで、代表接触面の位置及び姿勢の変位量は、前記ばね性並進・回転変位量↑Xcに相当するものであり、前記一般化変数ベクトル↑qの単位時間当たりの変化量↑Δqに対して、代表接触面ヤコビアン行列Jcによって表現される前記式(23)の関係を有する。
従って、全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量に、代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を乗じることによって(すなわち、前記式(29)によって)、当該要求変位量を実現するための(ひいては、全床反力要求修正量を実現するための)移動体の関節変位修正量を決定できることとなる。
そこで、本発明では、前記代表接触面ヤコビアン行列算出手段によって、代表接触面ヤコビアン行列Jcを算出する。この場合、代表接触面ヤコビアン行列Jcは、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列J_i(i=1,2,…,N)と、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式(27)によって算出される。
なお、式(27)の演算を行なうために用いる各脚リンクに実際に作用する床反力の値や、その床反力作用点の位置、全床反力作用点の位置は、移動体に搭載した力センサ等により計測した観測値でよいことはもちろんであるが、実際の値を精度よく近似し得るものであれば、前記目標床反力や適当なモデル等を基に、近似的に推定もしくは予測した値であってもよい。
また、上記脚リンクヤコビアン行列に関して補足すると、本発明では、各脚リンクは、その先端部を接地させた状態で、該先端部に作用する床反力モーメントを変化させることができる構造の脚リンク(例えば、先端部がアクチュエータにより姿勢を変化させることができる足平部により構成され、該先端部と床面との接触が面接触状態となる脚リンク)と、該先端部に作用する床反力モーメントを変化させることができない構造の脚リンク(例えば該先端部と床面との接触状態が点状となる脚リンク)とのいずれの構造の脚リンクであってもよい。
この場合、各脚リンクが、その先端部での床反力モーメントを変化させることができる構造のものである場合には、脚リンクヤコビアン行列は、該脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。また、各脚リンクが、その先端部での床反力モーメントを変化させることができない構造のものである場合には、脚リンクヤコビアン行列は、該脚リンクの先端部の位置の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。
これらの脚リンクヤコビアン行列は、移動体の各関節の実際の変位量の観測値を基に、公知の手法によって求めることができる。
そして、本発明においては、前記関節変位修正量決定手段が、上記の如く算出された代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を、代表接触面の位置及び姿勢の要求変位量に乗じることによって、前記関節変位修正量が決定される。これにより、結果的に、前記全床反力要求修正量を実現するための関節変位修正量が決定されることとなる。
さらに、前記関節変位制御手段が、前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、上記の如く決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する。
これにより、実全床反力を目標全床反力に追従させるように、移動体の各関節の変位量の制御が行われ、所謂、コンプライアンス制御が実現される。
かかる第1発明及び第2発明によれば、全床反力要求修正量を代表接触面の位置及び姿勢の要求変位量に変換し、この要求変位量に応じて移動体の各関節の関節変位修正量を求めることにより、個々の脚リンクの先端部の位置や姿勢の修正と実全床反力の変化との関係やそれらの相互の関係を考慮して各脚リンクの先端部の位置や姿勢の修正量を決定するような処理を実行することなく、実全床反力を目標全床反力に追従させるための移動体の各関節の変位量の修正量(関節変位修正量)を一括的に決定できる。そのため、関節変位修正量を決定する処理を効率よく短時間で行なうことができる。
この場合、前記式(27)における各脚リンクの重み係数r_iは、床反力の垂直抗力成分が相対的に大きい脚リンクほど、より大きい値(“1”に近い値)に設定されることとなるため、全床反力要求修正量に対する各脚リンクの負担分は、結果的に、床反力の垂直抗力成分が相対的に大きい脚リンクほど、大きくなる。従って、床反力の垂直抗力成分が相対的に小さい脚リンクの先端部の位置や姿勢を不必要に修正したりすることなく、全床反力要求修正量を確実に実現し得るような関節変位修正量を決定できる。
また、各脚リンクの重み係数r_iは連続的に変化するので、代表接触面ヤコビアン行列Jcが、不連続的な変化を生じることがない。このため、移動体の各関節の変位量を滑らかに連続的に変化させることができる。ひいては、移動体の運動を滑らかに行なうことができる。
よって、第1発明及び第2発明によれば、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うことができる。
一方、第1発明において、上記の如く移動体のコンプライアンス制御が行われている状態において、前記全床反力要求修正量決定手段により決定される全床反力要求修正量は、実全床反力の観測値と、目標全床反力との偏差を積分してなる値(積分項)であるから、該全床反力要求修正量と前記ばね定数とから、前記代表接触面位置姿勢変位量算出手段により算出される代表接触面の位置及び姿勢の要求変位量は、前記目標運動で想定されている床面である想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するものとなる。すなわち、該要求変位量は、前記式(28−1)における↑Xc_intに相当するものである。
また、第2発明においては、上記の如く移動体のコンプライアンス制御が行われている状態において、前記全床反力要求修正量決定手段により決定される全床反力要求修正量のうちの積分項と前記ばね定数とから、前記代表接触面位置姿勢変位量算出手段と同様の演算によって算出される代表接触面の位置及び姿勢の変位量(全床反力要求修正量の代わりに前記積分項を用いて算出される代表接触面の位置及び姿勢の変位量)は、前記想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するもの、すなわち、前記式(28−1)における↑Xc_intに相当するものとなる。
そこで、第1発明においては、前記想定床面の位置及び姿勢から、前記代表接触面位置姿勢変位量算出手段により算出された前記要求変位量だけ修正してなる位置及び姿勢を実際の床面の位置及び姿勢として推定する。
また、第2発明においては、前記代表接触面定常変位量算出手段によって、前記全床反力要求修正量のうちの前記積分項と前記代表接触面のばね定数とから、該積分項に対応する該代表接触面の位置及び姿勢の変位量である代表接触面定常変位量を算出する。そして、前記想定床面の位置及び姿勢から、前記代表接触面定常変位量算出手段により算出された前記代表接触面定常変位量だけ修正してなる位置及び姿勢を実際の床面の位置及び姿勢として推定する。
これにより、第1発明及び第2発明によれば、実際の床面の位置及び姿勢を推定することができる。この場合、前記のコンプライアンス制御によって、前記全床反力要求修正量を確実に実現し得るような関節変位修正量を決定して、前記移動体の動作を制御できることから、第1発明における全床反力要求修正量、又は第2発明における代表接触面定常変位量は、想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するものとしての信頼性が高いものとなる。従って、実際の床面の位置及び姿勢を精度よく推定することができることとなる。
よって第1発明及び第2発明によれば、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うようにしつつ、移動体の動作環境の実際の床面の位置や姿勢を適切に推定することができる。
なお、第2発明においては、前記全床反力要求修正量は、少なくとも前記比例項と積分項とを合成したものであるので、想定床面の位置及び姿勢の実際の床面に対する定常的な誤差の影響だけでなく、想定床面で想定されていない実際の床面の局所的な凹凸等に起因して生じる、実全床反力の観測値と目標全床反力との一時的な偏差の影響を補償するよにして、実全床反力を目標全床反力に追従させるための関節変位修正量を決定できる。
上記第1発明又は第2発明において、前記代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1は、公知の適宜の手法により算出することができる。
ただし、前記算出された代表接触面ヤコビアン行列の擬似逆行列Jc-1は、あらかじめ設定された重み行列Wと前記算出された代表接触面ヤコビアン行列Jcとから次式(30)により算出される行列であると共に、該式(28)におけるkの値(実数値)を、次式(31)により表される行列式DETが所定の正の閾値以上の値になるように決定する擬似逆行列演算用パラメータ決定手段を備えており、
Jc-1=W-1・JcT・(Jc・W-1・JcT+k・I)-1 ……(30)
DET=det(Jc・W-1・JcT+k・I) ……(31)
ただし、W:あらかじめ定められた重み行列(対角行列)
前記擬似逆行列演算用パラメータ決定手段は、kの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式DETの値を算出することと、算出した行列式DETの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのkの暫定値を、前記式(30)により擬似逆行列を算出するために用いるkの値として決定する手段であると共に、前記判断結果が否定的である場合のkの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式DETの絶対値と前記所定の閾値との偏差の絶対値のn乗根(n:Jc・W-1・JcTの次数)に比例する値に設定することが好ましい(第3発明)。
なお、式(30)において、Iは単位行列である。また、重み係数行列Wは、各関節の変位量の変化に対する代表接触面の位置及び姿勢の変化の応答性等を考慮し、代表接触面の位置及び姿勢の要求変位量を実現するための各関節の変位量の修正度合いを各関節毎に調整するものである。なお、重み係数行列Wは単位行列であってもよい。
また、kは、式(30)の右辺の括弧内の行列の行列式、すなわち、式(31)に示される行列式DETの大きさが小さくなり過ぎるのを防止するための調整パラメータであり、ゼロ以上の実数値である。この調整パラメータkについて補足すると、擬似逆行列Jc-1は、基本的には、k=0とした場合の上記の式(30)によって算出することができる。
ただし、この場合、上記行列式DETの大きさが小さくなり過ぎる(ゼロに近い値となる)場合がある。そして、このような場合には、式(30)の右辺の括弧内の行列の逆行列が発散し、適切な擬似逆行列Jc-1を決定することができなくなる。これを防止するために、式(30)では、右辺の括弧内の第1項に、単位行列Iのk倍の行列を付加するようにしている。
一方、前記行列式DETの大きさが小さくなり過ぎないようにするための適切な調整パラメータkの値はJcに応じて変化する。また、行列式DETの大きさは、kの値の変化に対して非線形な変化を呈する。
このため、第3発明では、上記行列式DETの絶対値が前記所定の閾値以上となる(過小にならない)kの値を、前記擬似逆行列演算パラメータ決定手段により探索的に決定する。
具体的には、kの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式DETの値を算出することと、算出した行列式DETの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのkの暫定値を、前記式(30)により擬似逆行列を算出するために用いるkの値として決定する。
この場合、この処理において、kの暫定値の増加量が一定であると、kの値が最終的に決定されるまでに時間がかかりすぎたり、あるいは、移動体の制御装置の制御処理周期毎に決定されるkの値が頻繁に変動し、ひいては、算出される擬似逆行列Jc-1の不連続的な変動が生じやすい。
一方、本願発明者の知見によれば、Jc・W-1・JcTの次数をnとしたとき、行列式DETの値は、kの値のn乗に比例して変化する。
そこで、第3発明では、前記擬似逆行列演算パラメータ決定手段の処理において、前記判断結果が否定的である場合のkの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式DETの絶対値と前記所定の閾値との偏差の絶対値のn乗根(n:Jc・W-1・JcTの次数)に比例する値に設定することとした。
これにより、第3発明によれば、移動体の制御装置の制御処理周期毎に、擬似逆行列Jc-1を算出するために用いる適切なkの値(DETの絶対値が所定の閾値以上となるkの値)を効率よく短時間で決定できると共に、擬似逆行列Jc-1が滑らかに変化していくようにすることができる。ひいては、移動体の各関節の変位量を滑らかに変化させるように、関節変位修正量を決定できることとなる。
また、第3発明によれば、前記重み係数行列Wによって、各関節の変位量の変化に対する代表接触面の位置及び姿勢の変化の応答性等を考慮して、代表接触面の位置及び姿勢の要求変位量を実現するための各関節の変位量の修正度合いを各関節毎に調整することができる。
本発明の一実施形態を図2〜図5を参照して説明する。
図2を参照して、本実施形態で例示する移動体1は、例えば2つの脚リンクを備える脚式移動ロボットである。この移動体1は、基体としての上体2と、左右一対の脚リンク3R,3Lと、左右一対の腕リンク4R,4Lと、頭部5とを有する。
なお、本実施形態の説明では、移動体1の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“R”を付加し、ロボット1の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“L”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“R”,“L”を省略することがある。
左右一対の脚リンク3R,3Lは、互いに同じ構造のものである。具体的には、各脚リンク3は、上体2に股関節部13を介して連結された大腿部14と、この大腿部14に膝関節部15を介して連結された下腿部16と、この下腿部16に足首関節部17を介して連結された足平部18とを、該脚リンク3を構成する複数の要素リンクとして備える。
各脚リンク3の先端部である足平部18の底面には弾性部材18aが取り付けられている。そして、床面上での移動体1の移動時に、各足平部18は、その弾性部材18aを介して床面に接地(着床)するようになっている。
各脚リンク3の股関節部13は、ヨー軸(図2のZ軸)、ピッチ軸(図2のY軸)及びロール軸(図1のX軸)の各軸周りの回転自由度をそれぞれ有する3つの関節19,20,21により構成されている。また、膝関節部15は、ピッチ軸周りの回転自由度を有する関節22により構成されている。また、足首関節部17は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する2つの関節23,24により構成されている。
従って、各脚リンク3は、本実施形態では、上体2(基体)に対して、6自由度の運動自由度を有する。なお、上記の説明における各脚リンク3の関節19〜24の回転軸(ロール軸、ピッチ軸、ヨー軸)は、脚リンク3を上下方向に延在させた状態での回転軸を示している。
左右一対の腕リンク4R,4Lは、互いに同じ構造のものである。具体的には、各腕リンク4は、上体2に肩関節部25を介して連結された上腕部26と、この上腕部26に肘関節部27を介して連結された前腕部28と、この前腕部28に手首関節部29を介して連結されたハンド30とを、該腕リンク4を構成する複数の要素リンクとして備える。
この場合、肩関節部25は、ピッチ軸、ロール軸、及びヨー軸の各軸周りの回転自由度をそれぞれ有する3つの関節31,32,33により構成されている。また、肘関節部27は、ピッチ軸周りの回転自由度を有する関節34により構成されている。また、手首関節部29は、ヨー軸、ピッチ軸及びロール軸の各軸周りの回転自由度をそれぞれ有する3つの関節35,36,37により構成されている。
従って、各腕リンク4は、本実施形態では、上体2に対して、7自由度の運動自由度を有する。なお、上記の説明における各腕リンク4の関節31〜37の回転軸(ロール軸、ピッチ軸、ヨー軸)は、腕リンク4を上下方向に延在させた状態での回転軸を示している。
頭部5は、上体2の上方に配置されており、頸関節部38を介して上体2に連結されている。この場合、頸関節部38は、ピッチ軸周り及びヨー軸周りの回転自由度をそれぞれ有する2つの関節39,40により構成されている。従って、頭部5は、上側上体6に対して、2自由度の運動自由度を有する。なお、関節31,37の上記の回転軸(ピッチ軸、ヨー軸)は、頸関節部38を上下方向に伸ばした状態での回転軸を示している。
図2では図示を省略するが、移動体1には、上記した各関節をそれぞれ駆動する関節アクチュエータとしての複数の電動モータ41(図3に示す)が搭載されている。各電動モータ41は、それぞれに対応する関節に、減速機を含む動力伝達機構(図示省略)を介して駆動力(回転駆動力)を伝達するように接続されている。
なお、関節アクチュエータは、電動モータ以外のアクチュエータ、例えば油圧式のアクチュエータであってもよい。
以上の構成を有する移動体1は、各脚リンク3の6個の関節19〜24を電動モータ41により駆動することによって、各脚リンク3の空間的な運動が行なわれる。この運動によって移動体1が床上を移動することが可能となっている。例えば、人の歩行動作と同様の形態(歩容)で脚リンク3R,3Lを運動させることによって、移動体1の歩行動作を行なうことが可能である。
補足すると、本実施形態の移動体1は、腕リンク4R,4L及び頭部5を有する人型のロボットであるが、これらの腕リンク4R,4L又は頭部5を備えないものであってもよい。また、例えば、上体2を下側上体と上側上体とで構成し、これらを関節を介して連結してもよい。
本実施形態では、以上の構造を有する移動体1の動作制御を行なうために、CPU、RAM、ROM等を含む電子回路ユニットにより構成された制御装置50と、各種のセンサとが備えられている。
この場合、センサとしては、図2に示すように、移動体1の基体である上体2の姿勢角(鉛直方向に対する傾斜角度やヨー軸周りの方位角)やその時間的変化率(角速度)等を計測するために該上体2に搭載された姿勢センサ51と、各足平部18がその接地時に受ける外力(床反力)を計測するために各脚リンク3の足首関節部24と足平部18との間に介装された力センサ52(52R,52L)と、各ハンド30がその接触対象物体から受ける外力を計測するために各腕リンク4の手首関節部29とハンド30との間に介装された力センサ53(53R,53L)とが備えられている。
上記姿勢センサ51は、例えば3軸周りの角速度を検出するジャイロセンサと3軸方向の加速度を検出する加速度センサとから構成される。また、各力センサ52,53は、それぞれ、例えば3軸方向の並進力及び3軸周りのモーメントを検出する6軸力センサにより構成される。
なお、本実施形態で主に説明する移動体1の歩行動作においては、各腕リンク4の力センサ53(53R,53L)の出力は不要である。従って、これらの力センサ53を省略してもよい。
また、図2では図示を省略するが、移動体1には、各関節の変位量(回転角度)を検出するセンサとして、例えばロータリーエンコーダ54(図2に示す)が備えられている。なお、各関節の変位量(回転角度)を検出するセンサとしては、ポテンショメータ等の他のセンサを用いてもよい。
制御装置50には、上記の各センサの出力が入力される。そして、制御装置50は、これらの入力値から認識される移動体1の動作状態の観測値(上体2の姿勢角やその時間的変化率(角速度)の計測値、上体2の移動速度の計測値、各足平部18に作用する床反力の計測値、各関節の変位量やその時間的変化率の計測値など)を使用して、移動体1に目標とする運動を行なわせるための各関節の変位量の目標値(以下、関節変位指令という)を決定すると共に、この関節変位指令に応じて、各関節の実際の変位量(実変位量)を電動モータ41を介して制御する。さらに、制御装置50は、移動体1の動作制御と並行して、その動作制御のために算出するパラメータの値を利用して、移動体1の動作環境の実際の床面の位置及び姿勢を推定する処理も実行する。
かかる制御処理を実行する制御装置50は、実装されるプログラムにより実現される主要な機能として、図4に示す如く、基準歩容生成部61、姿勢安定化補償力決定部62、目標全床反力修正部63、全床反力要求修正量決定部64、代表接触面並進・回転変位量算出部65、代表接触面ヤコビアン行列算出部66、目標関節変位量決定部67、関節変位修正量決定部68、関節変位制御部69、床面推定部70を備える。
そして、制御装置50は、上記基準歩容生成部61、姿勢安定化補償力決定部62、目標全床反力修正部63、全床反力要求修正量決定部64、代表接触面並進・回転変位量算出部65、代表接触面ヤコビアン行列算出部66、目標関節変位量決定部67、関節変位修正量決定部68及び関節変位制御部69の処理を所定の演算処理周期で逐次実行することにより、各関節の関節変位指令を逐次決定し、この関節変位指令に応じて、図示しないモータ駆動回路を介して電動モータ41を制御する。
さらに、制御装置50は、上記の制御処理と並行して、床面推定部70の処理を実行することにより、移動体1の運動を行なう実際の床面の位置及び姿勢を推定する。
以下に、移動体1の歩行動作を行なわせる場合を例にとって、上記各機能部の詳細な処理を含めて制御装置50の処理を説明する。
[基準歩容生成部61の処理]
本実施形態では、制御装置50は、基準歩容生成部61により、移動体1の基準の目標歩容としての基準歩容を生成する。
この基準歩容は、移動体1の各部の目標とする空間的な位置及び空間的な姿勢(空間的な向き)の軌道を規定する(ひいては移動体1の各関節の目標とする変位量の軌道を規定する)目標運動と、移動体1に床面から作用させるべきトータルの床反力(全床反力)の軌道を規定する目標全床反力とから構成される。なお、「軌道」は、瞬時値の時系列を意味する。
基準歩容のうちの目標運動は、移動体1の移動環境の実際の床面の形状(床面の各所の位置及び姿勢)に合致するものとしてあらかじめ形状が定められた床面モデルである想定床面上で、移動体1を移動させ得るように生成されるものである。なお、本実施形態では、目標運動を作成するために使用する想定床面は、床面推定部70で実際の床面を表すものとして推定された床面(推定床面)に応じて定期的に(例えば移動体1の移動時に1歩毎又は複数歩毎に、あるいは、所定時間毎に)更新される。
この目標運動は、本実施形態の移動体1では、各脚リンク3の足平部18の目標位置及び目標姿勢の軌道である目標足平位置姿勢軌道と、上体2(基体)の目標位置及び目標姿勢の軌道である目標上体位置姿勢軌道と、上体2に対する各腕リンク4の全体の相対的な目標姿勢の軌道である目標腕姿勢軌道と、上体2に対する頭部5の相対的な目標姿勢の軌道である目標頭部姿勢軌道とから構成される。
なお、足平部18の「位置」は、足平部18の空間的な位置を代表的に示すものとして任意に定められた該足平部18の代表点の位置(例えば足平部18の底面の特定の位置の点)の位置を意味する。上体2の「位置」についても同様である。また、足平部18の「姿勢」は、該足平部18の空間的な向きを意味する。上体2の「姿勢」についても同様である。
また、上記の目標足平位置姿勢及び目標上体位置姿勢は、床面に対して固定された慣性座標系としてのグローバル座標系で見た位置及び姿勢として表される。このグローバル座標系としては、例えば移動体1の支持脚(移動体1の重量を床に支える脚)としての1つの脚リンク3の接地面(床との接触面)内の点を原点とし、該脚リンク3の足平部18の前後方向の水平軸をX軸、鉛直方向の軸をZ軸、これらに直交する方向の水平軸をY軸とする支持脚座標系が用いられる。
この場合、移動体1の歩行動作においては、支持脚が切替わる毎に、グローバル座標系の原点の位置とX軸及びY軸の方向とが更新されることとなる。ただし、グローバル座標系は、床面に対して定常的に固定された座標系であってもよい。なお、以降の説明では、説明の便宜上、X軸、Y軸、Z軸は特にことわらない限り、上記支持脚座標系の3軸を意味するものとする。
本実施形態では、基準歩容生成部61は、目標足平位置姿勢軌道等の目標運動の構成要素を、公知の歩容生成手法、例えば、本願出願人が特許第3726081号等に提案した手法と同様の手法によって生成する。
具体的には、移動体1の歩行動作を行なための目標運動では、目標腕姿勢軌道と目標頭部姿勢軌道とは、例えば上体2に対する相対的な目標腕姿勢と目標頭部姿勢とを定常的に一定に保つように決定される。そして、目標足平位置姿勢軌道と、目標上体位置姿勢軌道とが、例えば、本願出願人が特許第3726081号等に提案した手法によって生成される。
この目標足平位置姿勢軌道及び目標上体位置姿勢軌道の生成処理を概略的に説明すると、各足平部18の想定床面に対する着床予定位置及び着床予定時刻等、目標足平位置姿勢軌道を規定するパラメータが、移動体1の外部から制御装置50に与えられる移動体1の移動方向や移動速度の要求、あるいは移動計画等に応じて決定される。なお、上記パラメータを制御装置50に外部から入力し、あるいは、あらかじめ制御装置50の記憶装置に記憶保持しておいてもよい。
そして、このパラメータにより規定される目標足平位置姿勢軌道に応じて、ZMP(Zero Moment Point)の目標位置である目標ZMPの軌道が決定される。目標ZMP軌道は、目標ZMPが目標足平位置姿勢軌道に応じて規定される想定床面上の支持多角形内の概ね中央付近の位置に極力収まり、且つ、滑らかに(ステップ状の変化を生じることなく)変位していくように決定される。
さらに、移動体1の動力学(外力としての床反力と移動体1の運動との間の関係)を表現する適宜の動力学モデルを用いて、目標ZMP軌道を満足するように目標上体位置姿勢軌道が決定される。なお、目標ZMP軌道を満足するということは、移動体1の運動によって発生する慣性力と移動体1に作用する重力との合力が目標ZMP周りに発生するモーメントの水平成分(X軸周り及びY軸周りの成分)がゼロになることを意味する。
補足すると、目標運動を生成する手法は、想定床面上で移動体1が実現可能な目標運動を生成し得るものであれば、本願出願人が特許第3726081号等に提案した手法以外の他の公知の手法によって生成してもよい。
また、上体2に対する相対的な目標腕姿勢や目標頭部姿勢を、必要に応じて所望の形態で変化させるように決定するようにしてもよい。例えば両腕リンク4R,4Lを移動体1の歩行動作に合せて前後に振るようにしてもよい。その場合、目標腕姿勢や目標頭部姿勢の変化に伴う移動体1の全体重心の位置の変化や、該全体重心の周りの角運動量の変化を考慮した動力学モデルを用いて目標上体位置姿勢軌道を生成するようにしてもよい。
また、目標運動の構成要素は、上記のものに限られるものではない。例えば移動体1が腕リンク4R,4L、あるいは頭部5を備えない場合には、目標腕姿勢軌道や目標頭部姿勢軌道は不要である。また、腕リンク4R,4Lや頭部5とは別に、上体2に対して可動な部位が移動体1に備えられている場合には、その部位の目標位置や目標姿勢の軌道も目標運動の構成要素して付加される。また、例えば、各脚リンク3が7自由度以上の自由度を有する場合には、目標足平位置姿勢軌道に加えて、例えば各脚リンク3の中間部の部位の目標位置姿勢軌道を目標運動の構成要素として付加してもよい。
目標運動の構成要素は、それによって移動体1の各関節の変位量の軌道を規定し得るものであればよく、移動体1の構造等に応じて適宜定めればよい。
基準歩容のうちの目標全床反力は、移動体1を想定床面上で目標運動に従って運動させるために、該想定床面から動力学的に移動体1に作用させることが必要となる全床反力の軌道を規定するものである。
この目標全床反力は、全床反力の作用点としての全床反力中心点(COP)の目標位置である目標全床反力中心点の軌道と、この目標全床反力中心点を作用点として想定床面から移動体1に作用させる並進力(ベクトル)の目標である目標並進床反力の軌道と、目標全床反力中心点周りに想定床面から移動体1に作用させるモーメント(ベクトル)の目標である目標床反力モーメントの軌道とから構成される。
なお、以降の説明では、便宜上、「目標全床反力」は、特にことわらない限り、目標並進床反力と目標床反力モーメントとの組を意味するものとし、目標全床反力中心点と区別する。
基準歩容生成部61は、目標全床反力中心点軌道を、目標ZMP軌道に一致させるように決定する。また、基準歩容生成部61は、目標並進床反力が、移動体1の目標運動における該移動体1の全体重心の並進運動によって発生する慣性力と移動体1に作用する重力との合力に釣り合うように目標並進床反力軌道を決定する。
また、基準歩容生成部61は、目標全床反力中心点周りの目標床反力モーメントが、移動体1の目標運動によって発生する慣性力(全体重心の並進運動による慣性力と全体重心周りの回転運動による慣性力)と移動体1に作用する重力との合力によって目標全床反力中心点周りに発生するモーメントに釣り合うように目標床反力モーメント軌道を決定する。なお、この場合、目標床反力モーメントの水平軸周り成分(X軸周り及びY軸周りの成分)はゼロとなる。
以降の説明では、基準歩容の目標全床反力のうちの目標並進床反力(ベクトル)を↑Ft_cmd0、目標床反力モーメント(ベクトル)を↑Mt_cmd0と表記する。本実施形態では、↑Ft_cmd0,↑Mt_cmd0は、それぞれ、X軸成分、Y軸成分、及びZ軸成分の3成分からなる縦ベクトルである。また、↑Ft_cmd0,↑Mt_cmd0を合わせたものを目標全床反力↑FMt_cmd0と表記する。この場合、↑FMt_cmd0は、↑Ft_cmd0,↑Mt_cmd0の各成分を並べた縦ベクトル(6成分の縦ベクトル)であるとする。すなわち、↑FMt_cmd0=[↑Ft_cmd0,↑Mt_cmd0]Tであるとする。
補足すると、前記基準歩容は、移動体1の移動動作中に生成する必要はなく、該移動体1の移動開始前に事前に作成しておき、それを制御装置50の記憶装置に事前に記憶保持したり、外部から制御装置50に適宜、無線通信によって入力するようにしてもよい。このようにした場合、制御装置50に基準歩容生成部61を備える必要はない。
[目標関節変位量決定部67の処理]
制御装置50は、前記の如く生成された基準歩容のうちの目標運動を、目標関節変位量決定部67に入力し、該目標関節変位量決定部67の処理を実行する。この目標関節変位量決定部67は、基準歩容の目標運動によって規定される移動体1の各関節の変位量である基準の目標変位量を算出する機能部である。この場合、目標関節変位量決定部67は、入力された目標運動から、逆運動学の演算処理によって移動体1の各関節の目標変位量を算出する。
なお、基準歩容を事前に作成しておく場合には、この基準歩容の目標運動に対応する各関節の目標変位量も事前に作成しておいてもよい。その場合には、制御装置50に目標関節変位量決定部67を備える必要はない。
[姿勢安定化補償力決定部62の処理]
制御装置50は、目標関節変位量決定部67の処理と並行して(又は該処理の前もしくは後に)、姿勢安定化補償力決定部62、目標全床反力修正部63、全床反力要求修正量決定部64、代表接触面位置・姿勢変位量算出部65、代表接触面ヤコビアン行列算出部66及び関節変位修正量決定部68の処理を実行する。これらの処理は、コンプライアンス制御のための操作量(制御入力)を決定するための処理であり、以降、コンプライアンス操作量決定処理という。そして、制御装置50は、このコンプライアンス操作量決定処理によって、前記基準歩容に対応する各関節の基準の目標変位量を修正するための関節変位修正量を決定する。
このコンプライアンス操作量決定処理では、制御装置50は、まず、姿勢安定化補償力決定部62の処理を実行する。この姿勢安定化補償力決定部62は、基準歩容の目標上体姿勢と、移動体1の上体2の実際の姿勢(実上体姿勢)の観測値との偏差をゼロに近づけるために移動体1に付加的に作用させるべき摂動床反力である姿勢安定化補償力↑Mdmdを、基準歩容の目標全床反力を修正するための修正量として決定する機能部である。
上記姿勢安定化補償力↑Mdmdは、本実施形態では、基準歩容の目標全床反力中心点周りに付加する水平軸周り(X軸周り及びY軸周り)の摂動モーメントとされる。従って、姿勢安定化補償力↑Mdmdは、基準歩容の目標全床反力のうち、目標床反力モーメントの水平軸周り成分を修正する操作量である。換言すれば、姿勢安定化補償力↑Mdmdは、全床反力中心点の目標を基準歩容の目標全床反力中心点からずらす機能を有する操作量である。
そして、姿勢安定化補償力決定部62は、姿勢安定化補償力↑Mdmdを、前記姿勢センサ51の出力により示される実上体姿勢の観測値と、基準歩容の目標上体姿勢との偏差から所定のフィードバック制御則により決定する。そのフィードバック制御則としては、例えばPD則が用いられる。
すなわち、本実施形態では、実上体姿勢の観測値と目標上体姿勢との偏差に所定値のゲインを乗じた比例項と、該偏差の時間的変化率に所定値のゲインを乗じた微分項とを加え合わせることにより姿勢安定化補償力(モーメント)↑Mdmdが決定される。この場合、↑MdmdのX軸周り成分は、上記偏差のX軸周り成分に応じて決定され、↑MdmdのY軸周り成分は、上記偏差のY軸周り成分に応じて決定される。
なお、実上体姿勢の観測値と目標上体姿勢との偏差は、移動体1の実際の全体重心と基準歩容の目標運動における全体重心との間の水平方向の位置偏差にほぼ比例する。従って、移動体1の実際の全体重心の水平位置の観測値と、基準歩容の目標運動における移動体1の全体重心の水平位置との偏差を、実上体姿勢の目標上体姿勢からのずれの程度を表す指標として用い、この偏差に応じてPD則等のフィードバック制御則により姿勢安定化補償力(モーメント)↑Mdmdを決定するようにしてもよい。この場合、移動体1の実際の全体重心の位置は、例えば、移動体1の各関節の実変位量の観測値(計測値)と、移動体1の剛体リンクモデルとを用いて推定することが可能である。
補足すると、本実施形態では、↑MdmdのX軸周り成分及びY軸周り成分の2成分であるが、Z軸周り成分をさらに含めてもよい。その場合、例えば、基準歩容の目標上体姿勢のうちのZ軸周り成分と、実上体姿勢の観測値のうちのZ軸周り成分との偏差に応じてPD則等のフィードバック制御則により、↑MdmdのZ軸周り成分を決定するようにすればよい。
[目標全床反力修正部63の処理]
次いで、制御装置50は、目標全床反力修正部63の処理を実行する。この目標全床反力修正部63は、上記の如く決定した姿勢安定化補償力↑Mdmdにより基準歩容の目標全床反力↑FMt_cmd0を補正する機能部である。この目標全床反力修正部63は、上記の如く決定された姿勢安定化補償力↑Mdmdを基準歩容の目標床反力に加え合わせることによって、修正後目標床反力を決定する。この場合、修正後目標床反力は、基準歩容の目標全床反力中心点周りの目標床反力モーメントのうちのX軸周り成分及びY軸周り成分だけを基準歩容の目標床反力から修正したものである。
以降、修正後目標床反力のうちの並進床反力(3成分の縦ベクトル)及び床反力モーメント(3成分の縦ベクトル)をそれぞれ↑Ft_cmd1、↑Mt_cmd1と表記し、これらを合わせた6成分の縦ベクトル(=[↑Ft_cmd1,↑Mt_cmd1]T)を修正後目標床反力↑FMt_cmd1と表記する。この場合、↑Ft_cmd1=↑Ft_cmd0である。また、↑Mt_cmd1=↑Mt_cmd0+↑Mdmdである(ただし、本実施形態では↑MdmdのZ軸周り成分はゼロとする)。
[全床反力要求修正量決定部64の処理]
次いで、制御装置50は、全床反力要求修正量決定部64の処理を実行する。この全床反力要求修正量決定部64は、上記の如く決定した修正後目標床反力↑FMt_cmd1に、移動体1に作用する実際の全床反力(実全床反力)の観測値を追従させるように全床反力の要求修正量を決定する機能部である。
この全床反力の要求修正量は、前記式(2)の摂動全床反力↑ΔFMtの要求値に相当するものであり、全床反力のうちの並進床反力の要求修正量(3成分の縦ベクトル)としての摂動全並進床反力↑ΔFtの要求値と、目標全床反力中心点周りの全床反力モーメントの要求修正量(3成分の縦ベクトル)としての摂動全床反力モーメント↑ΔMtの要求値とから構成される。以降の説明では、摂動全並進床反力↑ΔFtの要求値を↑ΔFt_dmd、摂動全床反力モーメント↑ΔMtの要求値を↑ΔMt_dmdと表記し、これらを合わせた6成分の縦ベクトルとしての摂動全床反力↑ΔFMtの要求値を↑ΔFMt_dmd(=[↑ΔFt_dmd,↑ΔMt_dmd]T)と表記する。
また、移動体1に作用する実全床反力のうちの並進床反力の観測値(3成分の縦ベクトル)を↑Ft_act、実全床反力のうちの床反力モーメントの観測値(3成分の縦ベクトル)を↑Mt_actと表記し、さらに、これらの↑Ft_act,↑Mt_actを合わせた6成分の縦ベクトルとしての実全床反力の観測値を↑FMt_act(=[↑Ft_act,↑Mt_act]T)と表記する。
全床反力要求修正量決定部64は、以下に説明する処理によって、上記摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを決定する。
すなわち、全床反力要求修正量決定部64は、まず、修正後目標床反力↑FMt_cmd1と、実全床反力の観測値↑FMt_actとの偏差↑Dfmt(=↑FMt_cmd1−↑FMt_act)を算出する。この場合、実全床反力の観測値↑FMt_actは、前記力センサ52R,52Lの出力により示される各脚リンク3毎の床反力(6軸力)の観測値を、目標全床反力中心点を作用点として合成することにより算出される。
ただし、本実施形態では、偏差↑Dfmtが過剰に変動するのを防止するために、全床反力要求修正量決定部64は、前記力センサ52R,52Lの出力により示される各脚リンク3毎の床反力(6軸力)の観測値を合成してなる実全床反力の値に、ローパス特性のフィルタリング処理を施すことによって、目標全床反力中心点に作用する実全床反力の観測値↑FMt_actを算出する。なお、この場合、各脚リンク3毎の床反力(6軸力)の観測値に、それぞれローパス特性のフィルタリング処理を施したものを合成することによって実全床反力の観測値↑FMt_actを算出するようにしてもよい。
次いで、全床反力要求修正量決定部64は、次式(41)に示す如く、上記偏差↑Dfmtに応じて決定した比例項(式(41)の右辺の第1項)と積分項(式(41)の右辺の第2項)と加え合わせることによって、摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを決定する。
上記比例項は、偏差↑Dfmtに所定値のゲインKcmpを乗じたものである。また、上記積分項は、偏差↑Dfmtにローパス特性のフィルタリング処理を施してなる値↑Dfmt_filtに所定値のゲインKestmを乗じてなる値を積分したものである。ここで、↑Dfmt_filtを得るためのローパス特性のフィルタリング処理は、その高周波側のカットオフ周波数が、前記偏差↑Dfmtを算出するときに用いたローパス特性のフィルタリング処理の高周波側のカットオフ周波数よりも低周波数となる特性のフィルタリング処理である。
↑ΔFMt_dmd=Kcmp・↑Dfmt+∫(Kestm・↑Dfmt_filt) ……(41)
なお、ゲインKcmp,Kestmは、スカラーでもよいが、対角行列であってもよい。
ここで、式(41)の積分項は、修正後目標床反力↑FMt_cmd1と、実全床反力の観測値↑FMt_actとの定常偏差に相当し、これは、実床面に対する想定床面の位置及び姿勢の誤差分に相当するものである。
なお、本実施形態では、上記式(41)により摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを算出したが、比例項を省略して要求値↑ΔFMt_dmdを算出するようにしてもよい。また、積分項の演算では、↑ΔDfmt_filtの代わりに、↑Dfmtをそのまま用いるようにしてもよい。
[代表接触面並進・回転変位量算出部65の処理]
以上の如く全床反力要求修正量決定部64の処理を実行した後、制御装置50は、代表接触面並進・回転変位量算出部65の処理を実行する。この代表接触面並進・回転変位量算出部65は、上記の如く決定された摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを、前記式(28)によって、仮想的な代表接触面のばね性並進・回転変位量↑Xc(=[↑Xc_org,↑Xc_rot]T)に変換する機能部である。
この場合、本実施形態では、移動体1の脚リンク3R,3Lの接地面を代表する代表接触面の並進ばね定数行列Kc_orgの各対角成分(代表接触面の3軸方向の並進変位に関する各成分のばね定数)と、該代表接触面の回転ばね定数行列Kc_rotの各対角成分(代表接触面の3軸周りの回転変位に関する各成分のばね定数)とはあらかじめ定められた定数値とされている。
そして、代表接触面並進・回転変位量算出部65は、これらのばね定数行列Kc_org,Kc_rotを用いて、前記式(28)に基づいて、代表接触面のばね性並進・回転変位量↑Xcを算出する。具体的には、式(28)の右辺の↑ΔFMtに↑ΔFMt_dmdを代入してなる次式(42)により、代表接触面のばね性並進・回転変位量↑Xcを算出する。
この式(42)におけるKc_org-1は、並進ばね定数行列Kc_orgの逆行列(Kc_orgの各対角成分の逆数値を対角成分とする行列)、Kc_rot-1は、並進ばね定数行列Kc_rotの逆行列(Kc_rotの各対角成分の逆数値を対角成分とする行列)である。これらのKc_org-1,Kc_rot-1は、それぞれ、制御装置50で並進ばね定数行列Kc_org、回転ばね定数行列Kc_rotから算出するようにしてもよいが、Kc_org-1,Kc_rot-1をあらかじめ制御装置50の記憶装置に記憶保持しておくようにしてもよい。
[代表接触面ヤコビアン行列算出部66の処理]
コンプライアンス操作量決定処理では、制御装置50は、上記の如く代表接触面のばね性並進・回転変位量↑Xcを算出するのと並行して(又は↑Xcの算出処理の前もしくは後に)、代表接触面ヤコビアン行列算出部66の処理を実行する。
この代表接触面ヤコビアン行列算出部66は、前記式(23)の関係を表現する代表接触面ヤコビアン行列Jcを算出する機能部である。なお、本実施形態における一般化変数ベクトル↑qは、より詳しくは、基体としての上体2の位置及び姿勢の6個の成分と、移動体1の各関節の変位量とを並べた縦ベクトルである。そして、この↑qの各成分の単位時間当たりの変化量を並べた縦ベクトルが、式(23)の↑Δqである。
本実施形態では、代表接触面ヤコビアン行列算出部66は、前記式(27)において、N=2とした次式(43)により代表接触面ヤコビアン行列Jcを算出する。なお、本実施形態では、両脚リンク3R,3Lのうちのいずれか一方と他方とが、それぞれ式(27)における第1脚リンク、第2脚リンクに相当する。以降の説明では、便宜上、例えば脚リンク3Rを第1脚リンク、脚リンク3Lを第2脚リンクとする。
この場合、式(43)の右辺の演算に必要な変数値は、次のように決定される。
重み係数r_i(i=1,2)に関し、ロータリエンコーダ54の出力により示される各関節の実際の変位量(実関節変位量)の観測値(計測値)に基づいて、床面に対する各脚リンク3の足平部18の現在の実姿勢の観測値が決定される。さらに、各脚リンク3の足平部18の実姿勢の観測値と、前記力センサ52R,52Lの出力とを基に、各脚リンク3毎の現在の実際の床面垂直抗力成分Fn_1,Fn_2(実床面垂直抗力成分Fn_1,Fn_2)の観測値が求められる。そして、各脚リンク3毎の実床面垂直抗力成分Fn_1,Fn_2の観測値から、式(43−1)により示される定義式に従って、重み係数r_i(i=1,2)が決定される。なお、この場合、重み係数r_iの値が頻繁に変動するのを防止するために、Fn_iの観測値にローパスフィルタ等のフィルタリング処理を施してもよい。
補足すると、重み係数r_iは、各脚リンク3毎の実床面垂直抗力成分Fn_iの観測値に基づいて決定することが望ましいが、該観測値の代わりに、実床面垂直抗力成分Fn_iの近似的な推定値もしくは予測値を使用して、重み係数r_iを決定するようにしてもよい。例えば基準歩容の構成要素として、各脚リンク3毎の目標床反力を含み、且つ、各脚リンク3毎の実床反力が目標床反力に近似的に一致すると考えられる場合には、実床面垂直抗力成分Fn_iの観測値の代わりに目標値を使用して、重み係数r_iを決定するようにしてもよい。
また、行列A_i(i=1,2)に関する係数行列Rkは、代表接触面に関してあらかじめ定められた並進ばね定数行列Kc_orgと、回転ばね定数行列Kc_rot(又はその逆行列Kc_rot-1)とから上記式(43−3)に従って決定される。なお、係数行列Rkは、制御装置50の記憶装置にあらかじめ記憶保持しておいてもよい。
また、行列A_i(i=1,2)に関する行列VV_iを決定するために必要となる位置ベクトル↑V_iは、例えば、次のように決定される。
ロータリエンコーダ54の出力により示される各関節の現在の実変位量の観測値(計測値)と、前記力センサ52R,52Lの出力とを基に、各脚リンク3の接地面における現在の実際の床反力中心点(実床反力中心点)の位置ベクトル(以降、これを↑Va_i(i=1,2)と表記する)が特定される。なお、この位置ベクトルの基準点は任意の点でよい。
次いで、各脚リンク3に対応する実床反力中心点を結ぶ線分を前記重み係数r_i(i=1,2)の比率で内分してなる内分点の位置ベクトル↑Vc(=r_1・↑Va_1+r_2・↑Va_2)を、実全床反力中心点の位置ベクトルとして算出する。
そして、↑V_i=↑Va_i−↑Vc(i=1,2)により、実全床反力中心点に対する各脚リンク3の接地面の実床反力中心点の位置ベクトル↑V_1,↑V_2がそれぞれ決定される。
なお、↑V_1=↑Va_1−↑Vc=r_2・(↑Va_1−↑Va_2)、↑V_2=↑Va_2−↑Vc=r_1・(↑Va_2−↑Va_1)となるので、これらの式に基づいて、位置ベクトル↑V_1,↑V_2を決定するようにしてもよい。この場合、↑Vcを算出する必要はない。
そして、行列A_i(i=1,2)に関する行列VV_iは、定義に従って、VV_i・↑F_i=↑V_i×↑F_iとなるように決定される。
なお、非接地状態の脚リンク3(Fn_i=0となる脚リンク3)については、実床反力中心点が存在しないので、該脚リンク3に対応する↑V_iは不定となる。この場合、Fn_i=0となる脚リンク3に対応する重み係数r_iはゼロであるので、A_iやJ_iの成分値によらずに、r_i・A_i・J_i=0となる。従って、非接地状態の脚リンク3に対応する行列VV_i(ひいては行列A_i)を決定する必要はない。このことは、以下に説明する脚リンクヤコビアン行列J_iについても同様である。
式(43)の右辺の演算に必要な変数値のうち、第i脚リンク3に関する脚リンクヤコビアン行列J_iは、次のように決定される。脚リンクヤコビアン行列J_iは、第i脚接地面のばね性並進・回転変位量ベクトル(=[↑Xorg_i,↑Xrot_i]Tと、一般化変数ベクトル↑qの単位時間当たりの変化量↑Δqとの間の関係を前記式(24)により表現する行列である。
ここで、↑X_iのうちのばね性並進変位量↑Xorg_iは、第i脚接地面の単位時間当たりの並進変位量であるとみなすと、第i脚リンク3の足平部18の現在の接地部分の位置の時間的変化率(並進速度)に一致すると考えてよい。また、↑X_iのうちのばね性回転変位量↑Xrot_iは、第i脚接地面の単位時間当たりの回転変位量であるとみなすと、第i脚リンク3の足平部18の現在の姿勢の時間的変化率(角速度)に一致すると考えてよい。
そこで、本実施形態では、第i脚リンク3の足平部18の現在の接地部分の位置及び該足平部18の姿勢(より詳しく当該位置及び姿勢を成分とする縦ベクトル)の単位時間当たりの変化量(時間的変化率)が、前記式(24)の左辺の↑X_iに一致するものとして、第i脚リンク3に関する脚リンクヤコビアン行列J_iを決定する。なお、第i脚リンク3の足平部18の現在の接地部分の位置としては、例えば、該第i脚リンク3の接地面(第i脚接地面)の実床反力中心点に相当する該足平部18の点の位置が用いられる。
この場合、脚リンクヤコビアン行列J_iは、移動体1の現在の動作状態(これは各関節の現在の実変位量の計測値により特定される)を↑qの微小変化(摂動)の起点として、公知の手法によって決定される。例えば、移動体1の幾何学モデル(剛体リンクモデル)を用いて、あるいは、解析的な演算によって、一般化変数ベクトル↑qの各成分の微小変化(現在状態からの微小変化)に対する第i脚リンク3の足平部18の接地部分の位置の変化と該足平部18の姿勢の変化とを算出することで、脚リンクヤコビアン行列J_iが決定される。
なお、この場合、第i脚リンク3の足平部18の接地部分の現在の位置は、第i脚リンク3の接地面の現在の実床反力中心点の位置の観測値に一致するものとされる。そして、この点の位置の変化(↑qの各成分の微小変化に対する変化)が、該足平部18の接地部分の位置の変化として算出される。
また、第i脚リンク3の足平部18の現在の姿勢は、ロータリエンコーダ54の出力が示す各関節の実変位量の観測値(計測値)を基に算出される該足平部18の現在の実姿勢の観測値に一致するものとされて、該足平部18の姿勢の変化(↑qの各成分の微小変化に対する変化)が算出される。
本実施形態では、代表接触面ヤコビアン行列算出部66は、以上の如く決定した重み係数r_1,r_2、行列VV_1,VV_2、及び脚リンクヤコビアン行列J_1,J_2を用いて、前記式(43)の右辺の演算を行なうことで、代表接触面ヤコビアン行列Jcを算出する。
[関節変位修正量決定部68の処理]
コンプライアンス操作量決定処理では、制御装置50は、以上の如く、代表接触面のばね性並進・回転変位量↑Xcと、代表接触面ヤコビアン行列Jcとを算出した後、関節変位修正量決定部68の処理を実行する。
この関節変位修正量決定部68は、代表接触面のばね性並進・回転変位量↑Xcに対応する各関節の変位量の修正量(基準歩容の目標運動に対応する目標関節変位量からの修正量)である関節変位修正量を決定する機能部である。
この関節変位修正量決定部68の処理では、まず、代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1が算出される。ここで、一般化変数ベクトル↑qの成分数、ひいては、↑Δqの成分数は、ばね性並進・回転変位量↑Xcの成分数よりも多いので、代表接触面ヤコビアン行列Jcは、正方行列ではない。従って、Jcの逆行列は存在しない。このため、関節変位修正量決定部68は、代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を算出する。
そして、関節変位修正量決定部68は、前記式(29)で示す如く、この擬似逆行列Jc-1を代表接触面のばね性並進・回転変位量↑Xcに乗じることによって、前記一般化変数ベクトル↑qの各成分の要求修正量(要求摂動量)を構成要素とする一般化変数要求修正量ベクトル↑Δq_dmdを算出する。そして、この一般化変数要求修正量ベクトル↑Δq_dmdの成分のうち、各関節の変位量の要求修正量を示す成分が、関節変位修正量として決定される。
この場合、関節変位修正量決定部68は、前記擬似逆行列Jc-1を前記式(30)により算出する。
すなわち、本実施形態では、擬似逆行列Jc-1は、重み付きの擬似逆行列(重み付きのSR−Inverse)として算出される。
具体的には、擬似逆行列Jc-1を算出する処理は、前記調整パラメータkの値の決定処理を含めて、図5のフローチャートに示す如く実行される。
まずSTEP1において、代表接触面ヤコビアン行列Jcの転置行列JcTと、重み係数行列Wの逆行列W-1とが算出される。なお、重み係数行列Wは、本実施形態ではその各対角成分(重み係数)の値があらかじめ定められた行列である。ただし、移動体1の動作状態に応じて適宜、重み係数行列Wの各対角成分の値を変化させるようにしてもよい。
次いで、STEP2において、調整パラメタータkの候補値の初期値として、kの値の標準値であるゼロが設定される。
次いで、STEP3において、調整パラメタータkの現在の候補値を用いて、前記式(31)により行列式DETの値が算出される。
次いで、STEP4において、この行列式DETの大きさ(絶対値)が、あらかじめ定められた所定の下限閾値DET_L(>0)よりも小さいか否か(DETの大きさが過小であるか否か)が判断される。
このSTEP4の判断結果が否定的である場合(DETの大きさが過小でない適切な大きさである場合)には、STEP10の処理が実行される。この処理では、kの現在の候補値が調整パラメタータkの値として確定され、この調整パラメータkの値を用いて、前記式(30)の演算により、擬似逆行列Jc-1が算出される。
一方、STEP4の判断結果が肯定的である場合(DETの大きさが過小である場合)には、STEP5〜8の処理(詳細は後述する)によって調整パラメータkの候補値の増加量Δk(>0)が決定される。そして、STEP9において、調整パラメータkの候補値が、現在値から上記増加量Δkだけ増加させた値に更新される。以降、STEP4の判断結果が否定的となるまで、STEP3〜9の処理が繰り返される。これにより、行列式DETの大きさが所定の下限閾値DET_L以上の適切な大きさとなるような調整パラメータkの値が探索的に決定される。
ここで、上記増加量Δkの決定処理を以下に説明する。行列式DETの大きさが所定の下限閾値DET_L以上の適切な大きさとなるような調整パラメータkの値を探索的に決定する処理では、例えば、調整パラメータkの候補値の増加量Δkをあらかじめ定めた一定値として、調整パラメータkの候補値を更新することが一般的に考えられる。
ただし、その場合、増加量Δkを大きめの値に設定しておくと、k=0として算出した前記行列式DETの大きさが過小となる状況で、制御装置50の各演算処理周期で逐次決定されるkの値が、Δkの整数倍の刻み幅の離散的な値となるために、DET≧DET_Lとなるような最小のkの値に対して必要以上に過大な値となりやすい。このため、制御装置50の各演算処理周期で逐次決定されるkの値に対応する行列式DETの大きさと前記下限閾値DET_Lとの差のばらつきを生じやすい。ひいては、前記式(30)により決定した擬似逆行列Jc-1を用いて前記式(29)により関節変位修正量を決定した場合に、その関節変位修正量の不連続的な変動が生じやすい。その結果、移動体1の運動の滑らかさが損なわれることがある。
一方、増加量Δkを十分に小さい値に設定することで、上記の不都合を解消することができる。ただし、その場合、前記STEP4の判断結果が否定的となる適切な調整パラメータkの値が決定されるまでに要する時間が長くなりやすい。このため、制御装置50の各演算処理周期の時間内で適切な調整パラメータkの値を決定できない場合が生じる恐れがある。
そこで、本実施形態では、STEP5〜8の処理において、調整パラメータkの候補値の増加量Δkを、STEP3で算出した行列式DETの大きさと、前記下限閾値DET_Lとの偏差の大きさに応じて、可変的に設定するようにした。
具体的には、STEP5において、次式(32)により算出される値が、調整パラメータkの増加量Δkの暫定値Δkpとして設定される。
Δkp=G・(DET_L−|DET|)1/n ……(32)
すなわち、前記下限閾値DET_Lから行列式DETの絶対値を差し引いてなる偏差(=DET_L−|DET|)のn乗根に、所定値のゲインG(>0)を乗じてなる値(偏差(=DET_L−|DET|)のn乗根に比例する値)が増加量Δkの暫定値Δkpとして設定される。なお、nは行列(Jc・W-1・Jc-1)の次数である。また、ゲインGの値は、あらかじめ定めれられた定数値である。
このように増加量Δkの暫定値Δkpを決定するのは、次の理由による。行列式det(Jc・W-1・JcT)の値(k=0の場合のDETの値)と、行列式DET(=det(Jc・W-1・JcT+k・I))の値との差分は、kのn次関数(kn+a・kn-1+……という形の関数)となるので、kの値の変化に対する行列式DETの値の変化は、ほぼkn(kのn乗)に比例すると考えられる。そこで、本実施形態では、上記(32)により算出される値を、増加量Δkの暫定値Δkpとして設定する。
そして、STEP6〜8において、増加量Δkが過剰に小さい値になるのを防止するためのリミット処理が暫定値Δkpに対して施され、これにより増加量Δkが確定される。具体的には、STEP6において、増加量Δkの暫定値Δkpと所定の下限値Δkmin(>0)とが比較され、Δkp≧Δkminである場合には、STEP7において、暫定値Δkpがそのまま増加量Δkとして決定される。また、Δkp<Δkminである場合には、STEP8において、Δkminが増加量Δkとして決定される。
これにより、増加量Δkは、Δkminを下限値として、行列式DETの大きさと、前記下限閾値DET_Lとの偏差の大きさに応じて前記式(32)により算出される値に決定されることとなる。
関節変位修正量決定部68は、以上説明した如く算出した擬似逆行列Jc-1を用いて、前記式(29)の右辺の演算を行なうことで、一般化変数要求修正量ベクトル↑Δq_dmdを算出する。そして、関節変位修正量決定部68は、この一般化変数要求修正量ベクトル↑Δq_dmdの成分のうち、各関節の変位量の要求修正量を示す成分を、関節変位修正量として決定する。このようにして決定される関節変位修正量が、実全床反力を修正後目標全床反力↑FMt_cmd1に近づけるための各関節の変位量の要求修正量としてのコンプライアンス操作量である。
[関節変位制御部69の処理]
以上説明したコンプラインス操作量決定処理(姿勢安定化補償力決定部62、目標全床反力修正部63、全床反力要求修正量決定部64、代表接触面並進・回転変位量算出部65、代表接触面ヤコビアン行列算出部66及び関節変位修正量決定部68の処理)とを実行した後、次に、制御装置50は、関節変位制御部69の処理を実行する。
この関節変位制御部69は、目標関節変位量決定部67で決定した各関節の目標関節変位量(基準歩容の目標運動に対応する目標関節変位量)に、目標関節変位量修正部68で決定した関節変位修正量を加え合わせることにより、各関節の最終的な関節変位指令としての修正後目標関節変位量を決定する。
そして、関節変位制御部69は、上記の如く決定した修正後目標関節変位量に各関節の実変位量を一致させるように、電動モータ41(関節アクチュエータ)を図示しないサーボアンプ等のモータ駆動回路を介して制御する。
[床面推定部70の処理]
制御装置50は、移動体1の各関節の駆動制御と並行して、床面推定部70の処理を実行する。この床面推定部70は、移動体1の各脚リンク3が接地する実際の床面(実床面)の位置及び姿勢の推定値である床面推定値を算出する機能部である。
この処理のために床面推定部70には、前記全床反力要求修正量決定部64から、摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdの組成成分である前記式(41)の右辺の積分項(第2項)の値、すなわち、修正後目標床反力↑FMt_cmd1と、実全床反力の観測値↑FMt_actとの偏差↑Dfmt(=↑FMt_cmd1−↑FMt_act)にローパス特性のフィルタリング処理を施してなる値↑Dfmt_filtに、所定値のゲインKestm(スカラー又は対角行列)を乗じてなる値を積分したもの(=∫(Kestm・↑Dfmt_filt))が入力される。以降、この積分項の値を次式(33)で示す如く、ΔFMt_intと表記する。
↑ΔFMt_int≡∫(Kestm・↑Dfmt_filt) ……(33)
ここで、前記した移動体1の各関節の駆動制御によって、各関節の変位量は、実全床反力の観測値↑FMt_actを修正後目標床反力↑FMt_cmd1に追従させるように(上記偏差↑Dfmtをゼロに近づけるように)制御されるので、上記積分項の値↑ΔFMt_intは、基準歩容の作成のために使用された想定床面の位置及び姿勢の、実床面に対する定常的な誤差に起因して発生するものとなる。
従って、この↑ΔFMt_intに対応する代表接触面のばね性並進・回転変位量が基準歩容の目標運動の作成のために使用された想定床面の位置及び姿勢の、実際の床面(実床面)に対する定常的な誤差分に相当するものと考えられる。
そこで、床面推定部70は、与えられた積分項の値↑ΔFMt_intを、前記式(28−1)により、ばね性並進・回転変位量に変換したものを、想定床面の位置及び姿勢の誤差分↑Xc_intとして得る。この場合、より詳しくは、積分項の値↑ΔFMt_intのうちの並進力成分↑ΔFt_intに、代表接触面の並進ばね定数行列Kc_orgの逆行列Kc_org-1を乗じたもの(=Kc_org-1・↑ΔFt_int)が想定床面の位置の誤差分↑Xc_org_intとされる。また、積分項の値↑ΔFMt_intのうちのモーメント成分↑ΔMt_intに、代表接触面の回転ばね定数行列Kc_rotの逆行列Kc_rot-1を乗じたもの(=Kc_rot-1・↑ΔMt_int)が想定床面の姿勢の誤差分↑Xc_rot_intとされる。
そして、床面推定部70は、上記の如く算出した誤差分↑Xc_intに応じて想定床面の位置及び姿勢を補正することにより、実床面の位置及び姿勢の推定値としての床面推定値を決定する。
このように床面推定部70により決定された床面推定値は、本実施形態では、前記基準歩容生成部61に与えられる。そして、基準歩容生成部61では、床面推定値に応じて、基準歩容を作成するために使用する想定床面を定期的に(例えば、移動体1の歩行動作時の1歩毎、又は複数歩毎に)更新する。
なお、基準歩容の作成に使用する想定床面は、床面推定値だけに応じて決定する必要はない。例えば、移動体1に搭載した視覚センサや、外部から与えられる床情報に基づいて、実床面の位置及び姿勢を推定できるような場合には、その推定された床面の位置及び姿勢の信頼性の評価や補完的な修正のために、上記床面推定値を使用するようにしてもよい。
以上が、本実施形態における制御装置50の処理の詳細である。
ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、全床反力要求修正量決定部64によって、本発明における全床反力要求修正量決定手段が実現される。この場合において、本実施形態では、修正後目標全床反力↑FMt_cmd1が、本発明における目標全床反力に相当する。
また、代表接触面並進・回転変位量算出部65、代表接触面ヤコビアン行列算出部66、関節変位修正量決定部68、関節変位制御部69によって、それぞれ、本発明における代表接触面位置姿勢変位量算出手段、代表接触面ヤコビアン行列算出手段、関節変位修正量決定手段、関節変位制御手段が実現される。
また、関節変位修正量決定部68において実行される処理のうち、図5のSTEP2〜9の処理によって、本発明における擬似逆行列演算用パラメータ決定手段が実現される。
また、床面推定部70は、本発明(第2の態様)における代表接触面定常変位量算出手段としての機能を有する。この場合、該床面推定部70が、前記積分項の値↑ΔFMt_intから、前記式(28−1)により、想定床面の位置及び姿勢の誤差分↑Xc_intを算出する処理によって、代表接触面定常変位量算出手段が実現される。そして、この誤差分↑Xc_intが本発明における代表接触面定常変位量に相当する。
以上説明した実施形態によれば、制御装置50は、各制御処理周期において、修正後目標全床反力↑FMt_cmd1に実全床反力を追従させるための操作量(制御入力)たる摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを、代表接触面のばね性並進・回転変位量↑Xcに変換する。さらに、制御装置50は、この↑Xcに代表接触面ヤコビアン行列Jcの擬似逆行列Jc-1を乗じることによって、↑Xcを実現するための(ひいては実全床反力を修正号目標全床反力↑FMt_cmd1に追従させるための)コンプライアンス操作量である移動体1の各関節の関節変位修正量を決定する。そして、制御装置50は、基準歩容の目標運動に対応する各関節の目標関節変位量を、関節変位修正量により修正してなる修正後目標関節変位量に応じて各関節の変位量を電動モータ41(関節アクチュエータ)を介して制御する。
これにより、本実施形態によれば、個々の脚リンク3の先端部(足平部18)の位置や姿勢の修正と実全床反力の変化との関係やそれらの相互の関係を考慮して各脚リンク3の先端部の位置や姿勢の修正量を決定するような処理を実行することなく、実全床反力を修正後目標全床反力↑FMt_cmd1に追従させるための各関節の関節変位修正量を一括的に決定できる。そのため、コンプライアンス制御用の関節変位修正量を決定する処理を効率よく短時間で行なうことができる。
この場合、代表接触面ヤコビアン行列Jcを算出するための前記式(43)における各脚リンク3_iの重み係数r_iは、床面垂直抗力成分Fn_iが相対的に大きい脚リンク3_iほど、より大きい値(“1”に近い値)に設定されることとなる。
このため、摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdに対する各脚リンク3_iの負担分は、結果的に、床面垂直抗力成分Fn_iが相対的に大きい脚リンク3_iほど、大きくなる。換言すれば、摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdが、できるだけ、床面垂直抗力成分Fn_iが相対的に大きい脚リンクの先端部の位置や姿勢の修正によって実現することができるように代表接触面ヤコビアン行列Jcが決定される。
従って、床面垂直抗力成分Fn_iが相対的に小さい脚リンク3_iの先端部の位置や姿勢を不必要に修正したりすることなく、摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを確実に実現し得るような関節変位修正量、すなわち、実全床反力を修正後目標全床反力↑FMt_cmd1に追従させる上で適切な関節変位修正量を決定できる。
また、各脚リンク3_iの重み係数r_iは連続的に変化するので、代表接触面ヤコビアン行列Jcが、不連続的な変化を生じることがない。このため、移動体1の各関節の変位量を滑らかに連続的に変化させることができる。ひいては、移動体1の運動を滑らかに行なうことができる。
また、前記実施形態では、関節変位修正量決定部68で、前記行列式DETの値が過小になるのを防止するための調整パラメータkの値を探索する処理(図5のSTEP2〜9の処理)において、調整パラメータkの増加量Δkを、下限閾値DET_Lから行列式DETの絶対値を差し引いてなる偏差(=DET_L−|DET|)のn乗根に比例する値に設定する。
これにより、制御装置50の各制御処理周期において、|DET|≧DET_Lとなるようにするための適切な調整パラメータkの値を、該kの値の不連続な変動が生じないようにしつつ、効率よく短時間で決定できる。このため、代表接触面並進・回転変位量↑Xcから関節変位修正量を決定するための擬似逆行列Jc-1を滑らかに変化させていくようにすることができる。ひいては、移動体1の各関節の変位量を滑らかに変化させるように、関節変位修正量を決定できる。
また、上記の如く実全床反力を修正後目標全床反力↑FMt_cmd1に追従させるように、移動体1の各関節の駆動制御を行いつつ、前記摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdの構成要素である前記積分項の値↑ΔFMt_intに対応する代表接触面の並進・回転変位量↑Xc_intを前記式(28−1)により算出することによって、想定床面の位置及び姿勢の実床面に対する定常的な誤差分としての↑Xc_intを高い信頼性で求めることができる。このため、実床面の位置及び姿勢の推定を精度よく行なうことができる。
次に、以上説明した実施形態の変形態様をいくつか説明する。
前記実施形態では、移動体1は、2つの脚リンク3R,3Lを備えた脚式移動ロボットであるが、3つ以上の脚リンクを備えたロボットであってもよい。その場合、制御装置50の処理のうち、代表接触面ヤコビアン行列算出部66以外の処理は前記実施形態と同じでよい。そして、代表接触面ヤコビアン行列算出部66の処理では、前記式(27)のNの値を、脚リンクの個数とした式に基づいて、代表接触面ヤコビアン行列Jcを算出すればよい。なお、この場合の演算に必要な重み係数r_i、行列A_i、脚リンクヤコビアン行列J_iの算出の仕方は、前記実施形態と同様である。
前記実施形態の移動体1は、各脚リンク3の先端部は、これに作用する床反力モーメントを変化させる(床反力中心点を該先端部の接地面で変化させる)ことができる足平部18により構成されたものであるが、各脚リンクの先端部は、床反力モーメントを変化させることができない構造のものであってもよい。
例えば、図6に示す如く、各脚リンク103_i(i=1,2,…,N)の先端部が球体状のもの(より一般的には、床面との接触面が実質的に点状になるもの(該接触面が微小面積となるものを含む))であってもよい。
この場合には、各脚リンク103_iの床面との点状の接触面(接地点)が床反力中心点に合致することとなるので、該脚リンク103_iに床反力モーメントを作用させること(ひいては変化させること)が実質的にできない。そして、この場合には、該脚リンク103_iに付加し得る摂動床反力モーメント↑M_iは常にゼロであるから、該脚リンク103_iの接地面のばね性回転変位量↑Xrot_i(=Krot_i・↑M_i)も常にゼロとなる。ひいては、このばね性変位量↑Xrot_iと、一般化変数ベクトル↑qの単位時間当たりの変化量↑Δqとの間の関係を表すヤコビアン行列Jrot_iも常にゼロ行列となる。
従って、この場合の脚リンクヤコビアン行列J_iは、前記式(26c)によって、J_i=[Jorg_i,0]Tとなる。このため、前記式(27)により代表接触面ヤコビアン行列Jcを算出する処理は、次式(27a)によりJcを算出する処理と等価である。
_ この式(27a)におけるr_i、Rk、VV_iは、それぞれ、前記式(22)に示したものと同じある。また、Jorg_iは、前記式(26a)に示したヤコビアン行列、すなわち、第i脚接地面のばね性並進変位量↑Xorg_iと、一般化変数ベクトル↑qの単位時間当たりの変化量↑Δqとの間の関係を表すヤコビアン行列である。
そして、この式(27a)の演算を行なうためのr_i、Rk、VV_iの値は、前記実施形態と同様の仕方で決定できる。また、J_i=[Jorg_i,0]Tであるから、前記実施形態で脚リンクヤコビアン行列J_iを算出する場合と同様の仕方でJorg_iを算出することができる。
また、移動体は、例えば図7に示す如く、各脚リンク103_i(i=1,2,…,N)の先端部に車輪を備える構造の移動体であってもよい。
また、前記実施形態では、基準歩容の目標全床反力を、目標上体姿勢と実上体姿勢との偏差に応じて決定した姿勢安定化補償力(床反力モーメント)↑Mdmdにより補正して修正後目標全床反力を、実全床反力の追従目標としたが、基準歩容の目標全床反力を実床反力の追従目標としてもよい。その場合には、姿勢安定化補償力決定部62及び目標全床反力修正部63は不要となる。そして、この場合には、全床反力要求修正量決定部64において、前記修正後目標全床反力↑FMt_cmd1の代わりに、基準歩容の目標全床反力↑FMt_cmd0を用いて摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを決定するようにすればよい。
また、前記実施形態では、修正後目標床反力↑FMt_cmd1と、実全床反力の観測値↑FMt_actとの偏差↑Dfmt(=↑FMt_cmd1−↑FMt_act)に応じた比例項と積分項とを合成する(加え合わせる)ことによって、前記摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdを決定するようにしたが、積分項(=↑ΔFMt_int)をそのまま、前記式前記摂動全床反力↑ΔFMtの要求値↑ΔFMt_dmdとして決定するようにしてもよい。
このようにした場合には、前記代表接触面並進・回転変位量算出部65により↑ΔFMt_dmdから算出される代表接触面のばね性並進・回転変位量↑ΔXcが、想定床面の位置及び姿勢の実床面に対する定常的な誤差分↑Xc_intに相当するものとなる。
従って、この場合には、床面推定部70においては、前記式(28−1)により↑Xc_intを改めて算出する必要はなく、前記代表接触面並進・回転変位量算出部65により算出された代表接触面のばね性並進・回転変位量↑ΔXcに応じて、想定床面の位置及び姿勢を修正することで、床面推定値を決定するようにすればよい。このようにすることで、本願発明の第1の態様の一実施形態が構築されることとなる。