特開 2022-102735 膝継手、姿勢計算装置、膝継手の制御方法、膝継手の制御用プログラムおよび姿勢計算装置の制御用プログラム

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

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

(11)【公開番号】P2022102735

(43)【公開日】2022-07-07

(54)【発明の名称】膝継手、姿勢計算装置、膝継手の制御方法、膝継手の制御用プログラムおよび姿勢計算装置の制御用プログラム

(51)【国際特許分類】

   A61F 2/70 20060101AFI20220630BHJP

   A61F 2/64 20060101ALI20220630BHJP

   B25J 11/00 20060101ALI20220630BHJP

【ＦＩ】

A61F2/70

A61F2/64

B25J11/00 Z

【審査請求】未請求

【請求項の数】16

【出願形態】ＯＬ

(21)【出願番号】P 2020217642

(22)【出願日】2020-12-25

(71)【出願人】

【識別番号】503405689

【氏名又は名称】ナブテスコ株式会社

(74)【代理人】

【識別番号】100105924

【弁理士】

【氏名又は名称】森下 賢樹

(72)【発明者】

【氏名】橋本 浩明

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

3C707

4C097

【Ｆターム（参考）】

3C707AS38

3C707KS20

3C707KS24

3C707XK03

3C707XK06

3C707XK13

3C707XK27

4C097AA07

4C097BB02

4C097BB06

4C097CC16

4C097TA06

4C097TB01

(57)【要約】

【課題】角速度センサの検知結果を用いて膝継手の姿勢を制御する際に、角度センサの検知結果を用いて膝継手の姿勢を補正できるようにする膝継手を提供する。
【解決手段】膝継手１００は、大腿接続部１０２と、下腿部１０４と、大腿接続部１０２と下腿部１０４とを連結し、大腿接続部１０２に対する下腿部１０４の所定軸回りの回転動作を制限又は補助する駆動補助部１０６と、下腿部１０４の角速度を検知する角速度センサ１０８と、静止座標系に対する下腿部１０４の角度を検知する角度センサ１１０と、角速度センサ１０８の検知結果をもとに導出した第１の多元数と、角度センサ１１０の検知結果をもとに導出し第１の多元数と同一の項数を有する第２の多元数に基づいて下腿部１０４の姿勢を計算する姿勢計算部１１２と、姿勢計算部１１２の計算結果に基づいて駆動補助部１０６を制御する制御部とを備える。
【選択図】図２

【特許請求の範囲】

【請求項1】

使用者の大腿部に対応するソケットが連結される大腿接続部と、
前記大腿接続部に接続され所定軸回りに回転可能に設けられた下腿部と、
前記大腿接続部と前記下腿部とを連結し、前記大腿接続部に対する前記下腿部の前記所定軸回りの回転動作を制限又は補助する駆動補助部と、
前記下腿部の角速度を検知する角速度センサと、
静止座標系に対する前記下腿部の角度を検知する角度センサと、
前記角速度センサの検知結果をもとに導出した第１の多元数と、前記角度センサの検知結果をもとに導出し前記第１の多元数と同一の項数を有する第２の多元数とに基づいて前記下腿部の姿勢を計算する姿勢計算部と、
前記姿勢計算部の計算結果に基づいて前記駆動補助部を制御する制御部とを備える、膝継手。

【請求項2】

前記姿勢計算部は、前記第１の多元数及び前記第２の多元数を用いて前記下腿部の姿勢の計算結果を補正する補正部を備える、請求項１に記載の膝継手。

【請求項3】

前記補正部は、前記角速度センサの検知結果に関する第１の重み付け係数、及び前記角度センサの検知結果に関する第２の重み付け係数を含む相補フィルタを用いて前記下腿部の姿勢の計算結果を補正し、前記第１の重み付け係数及び前記第２の重み付け係数は変更可能である、請求項２に記載の膝継手。

【請求項4】

前記補正部は、繰り返し補正を行い、
前記第１の重み付け係数及び前記第２の重み付け係数は、前記補正部による直前の補正からの経過時間に応じて変更される、請求項３に記載の膝継手。

【請求項5】

前記姿勢計算部は、
前記角速度センサの検知結果をもとに四元数を導出し、当該四元数の任意の項を０として四元数を三元数に変換する第１の導出部と、
前記角度センサの検知結果をもとに前記第２の多元数としての三元数を導出する第２の導出部とを備える、請求項１乃至４のいずれか１項に記載の膝継手。

【請求項6】

前記制御部は、予め決定された回数だけ前記第１の多元数のみに基づいて前記駆動補助部を制御する度に、前記補正部により補正された計算結果に基づいて前記駆動補助部を制御する、請求項２乃至５のいずれか１項に記載の膝継手。

【請求項7】

直前の駆動補助部の制御が補正された計算結果に基づく場合、前記第１の導出部は、直前の駆動補助部の制御で用いられた計算結果を用いて前記三元数を導出する、請求項５に記載の膝継手。

【請求項8】

前記姿勢計算部は、静止座標系における鉛直軸方向の値を０として姿勢を計算する、請求項１乃至７のいずれか１項に記載の膝継手。

【請求項9】

前記姿勢計算部は、前記大腿接続部に対する前記下腿部の角度が所定角度未満のときに第１の座標系を用いて姿勢を計算し、前記ソケットに対する前記下腿部の角度が所定角度以上のときに前記第１の座標系を回転させた第２の座標系を用いて姿勢を計算する、請求項１乃至８のいずれか１項に記載の膝継手。

【請求項10】

前記角度センサの検知結果を用いて前記角速度センサを校正する校正部を備える、請求項１乃至９のいずれか１項に記載の膝継手。

【請求項11】

移動体の角速度を検知する角速度センサの検知結果をもとに導出した第１の多元数と、前記移動体の角度を検知する角度センサの検知結果をもとに導出し前記第１の多元数と同一の項数を有する第２の多元数の項数に基づいて前記移動体の姿勢を計算する、姿勢計算装置。

【請求項12】

前記角速度センサの検知結果をもとに前記第１の多元数としての三元数を導出する第１の導出部と、
前記角度センサの検知結果をもとに前記第２の多元数としての三元数を導出する第２の導出部と、
前記第１の導出部で導出された三元数及び前記第２の導出部で導出された三元数を用いて前記移動体の姿勢の計算結果を補正する補正部とを備える、請求項１１に記載の姿勢計算装置。

【請求項13】

前記角度センサの検知結果を用いて前記角速度センサを校正する校正部を備える、請求項１１又は１２に記載の姿勢計算装置。

【請求項14】

使用者の大腿部に対応するソケットが連結される大腿接続部と、
前記大腿接続部に接続され所定軸回りに回転可能に設けられた下腿部と、
前記大腿接続部と前記下腿部とを連結し、前記大腿接続部に対する前記下腿部の前記所定軸回りの回転動作を制限又は補助する駆動補助部と、
前記下腿部の角速度を検知する角速度センサと、
静止座標系に対する前記下腿部の角度を検知する角度センサと、
前記角速度センサの検知結果をもとに導出した第１の多元数と、前記角度センサの検知結果をもとに導出し前記第１の多元数と同一の項数を有する第２の多元数とに基づいて前記下腿部の姿勢を計算する姿勢計算部と、
前記姿勢計算部の計算結果に基づいて前記駆動補助部を制御する制御部とを備える膝継手の制御方法であって、
前記角速度センサの検知結果をもとに第１の多元数を導出し、かつ前記角度センサの検知結果をもとに前記第１の多元数と同一の項数を有する第２の多元数を導出するステップと、
前記第１の多元数及び前記第２の多元数に基づいて前記膝継手の姿勢を計算するステップと、
計算された前記膝継手の姿勢に基づいて前記駆動補助部を制御するステップとを備える、膝継手の制御方法。

【請求項15】

使用者の大腿部に対応するソケットが連結される大腿接続部と、
前記大腿接続部に接続され所定軸回りに回転可能に設けられた下腿部と、
前記大腿接続部と前記下腿部とを連結し、前記大腿接続部に対する前記下腿部の前記所定軸回りの回転動作を制限又は補助する駆動補助部と、
前記下腿部の角速度を検知する角速度センサと、
静止座標系に対する前記下腿部の角度を検知する角度センサと、
前記角速度センサの検知結果をもとに導出した第１の多元数と、前記角度センサの検知結果をもとに導出し前記第１の多元数と同一の項数を有する第２の多元数に基づいて前記下腿部の姿勢を計算する姿勢計算部と、
前記姿勢計算部の計算結果に基づいて前記駆動補助部を制御する制御部とを備える膝継手の制御用プログラムであって、
前記姿勢計算部に、前記角速度センサの検知結果をもとに第１の多元数を導出させる第１の多元数導出部と、
前記姿勢計算部に、前記角度センサの検知結果をもとに前記第１の多元数とは異なる項数を有する第２の多元数を導出させる第２の多元数導出部と、
前記姿勢計算部に、前記第１の多元数及び前記第２の多元数に基づいて前記膝継手の姿勢を計算させる計算指示部と、
前記制御部に、前記計算指示部の計算結果に基づいて前記駆動補助部を制御させる駆動指示部とを備える、膝継手の制御用プログラム。

【請求項16】

移動体の角速度を検知する角速度センサの検知結果をもとに導出した第１の多元数と、前記移動体の角度を検知する角度センサの検知結果をもとに導出し前記第１の多元数と同一の項数を有する第２の多元数に基づいて前記移動体の姿勢を計算する姿勢計算装置の制御用プログラムであって、
前記姿勢計算装置に、前記角速度センサの検知結果をもとに第１の多元数を導出させる第１の多元数導出部と、
前記姿勢計算装置に、前記角度センサの検知結果をもとに第２の多元数を導出させる第２の多元数導出部と、
前記姿勢計算装置に、前記第１の多元数及び前記第２の多元数に基づいて前記移動体の姿勢を計算させる計算指示部とを備える、姿勢計算装置の制御用プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、膝継手、姿勢計算装置、膝継手の制御方法、膝継手の制御用プログラムおよび姿勢計算装置の制御用プログラムに関する。

【背景技術】

【0002】

従来、装着者の歩行を補助する義足について種々の開発が行われている。義足についての技術開発の結果、義足は単に装着者の体重を支持するだけではなく、装着者の歩行等の動作に合わせて所望の姿勢を取れるようになるまで発展している。義足の姿勢を算出するにあたり、角速度センサを用いて回転及び姿勢を加味した制御（四元数を用いた姿勢制御）を行う技術が提案されている（例えば特許文献１）。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０２０－１１０３３２号公報

【発明の概要】

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

【0004】

四元数を用いて義足の姿勢制御を行うことにより、義足の回転のみならず姿勢を計算できるようになり、装着者の歩行動作に合わせた制御を行える技術が発展した。しかしながら、角速度センサの検知結果は必ずしも正しいわけではなく、検知結果の誤差が発生した場合、精度の高い姿勢制御を行えない。角速度センサの検知結果の誤差を修正するために予めしたフィルタを用いることが考えられる。しかしながらフィルタは様々な動作場面を想定して場面毎に設定されており、汎用性が不足している。

【0005】

本発明は、角速度センサの検知結果を用いて膝継手の姿勢を制御する際に、様々な場面に応じて膝継手の姿勢を補正できるようにする膝継手を提供することを目的とする。

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

【0006】

上記目的を達成するための膝継手は、使用者の大腿部に対応するソケットが連結される大腿接続部と、大腿接続部に接続され所定軸回りに回転可能に設けられた下腿部と、大腿接続部と下腿部とを連結し、大腿接続部に対する下腿部の所定軸回りの回転動作を制限又は補助する駆動補助部と、下腿部の角速度を検知する角速度センサと、静止座標系に対する下腿部の角度を検知する角度センサと、角速度センサの検知結果をもとに導出した第１の多元数と、角度センサの検知結果をもとに導出し第１の多元数と同一の項数を有する第２の多元数とに基づいて下腿部の姿勢を計算する姿勢計算部と、姿勢計算部の計算結果に基づいて前記駆動補助部を制御する制御部とを備える。

【図面の簡単な説明】

【0007】

【図1】実施形態による電子制御式膝継手を含む大腿義足の概略図である。

【図2】膝継手の概略構成を示すブロック図である。

【図3】膝継手の一連の動作を示すフロー図である。

【図4】図３のステップＳ２の処理を具体的に示すフロー図である。

【図5】値ｑａと値ｑｇとの関係を示すグラフである。

【図6】同膝継手の概略図を示す。

【図7】姿勢計算部による一連の処理を示すブロック線図である。

【図8】膝継手を制御するための制御用プログラムのブロック図である。

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

【0008】

以下、本発明の実施形態について説明する。本発明は、装置自身が外部からの機械的な支援を受けずに装置自身に備えられた角速度センサの検出結果を用いて単独で姿勢制御を行う様々なものに適用可能である。このような装置としては、以下の実施形態で説明する義足用の電子制御式膝継手に加えて、ドローン、自律走行自動運搬車等の移動体がある。

【0009】

以下の説明においては、方向を示す用語として固定系及び回転系の二種類の三次元直交座標系を用いることがある。静止座標系は義足の装着者が直立している状態に基づいて３軸が定められる。Ｘ軸は装着者の幅方向に延び、Ｙ軸は装着者の前後方向に延び、Ｚ軸は装着者の高さ方向に延びる。

【0010】

図１は、実施形態による電子制御式膝継手（以下、単に「膝継手」という）を含む大腿義足の概略図である。大腿義足１０は、ソケット１２と、膝継手１００と、足部１４とを備える。ソケット１２は、装着者の断端を収納する。足部１４は、装着者の足として機能する。膝継手１００は、ソケット１２と足部１４との間に連結される。膝継手１００は、自身の姿勢等の状態に応じてソケット１２に対する足部１４の角度を制御する膝関節としての役割を果たす。

【0011】

膝継手１００は、大腿接続部１０２と、下腿部１０４と、駆動補助部１０６とを備える。大腿接続部１０２は、ソケット１２に対して回転不能に連結される。下腿部１０４は、大腿接続部１０２に対してＸ軸回りに回転可能に連結される。下腿部１０４は、足部１４に対して回転不能に連結される。駆動補助部１０６は、大腿接続部１０２と下腿部１０４との間に連結される。駆動補助部１０６は、空圧式又は油圧式のシリンダのような伸縮式の駆動機構により構成される。駆動補助部１０６の一端は大腿接続部１０２に連結され、他端は下腿部１０４に連結される。駆動補助部１０６の駆動又は停止は、後述する制御部により制御される。駆動補助部１０６が伸びると、大腿接続部１０２と下腿部１０４とのＸ軸回りの角度が増加して１８０度に近付き、大腿接続部１０２と下腿部１０４とが直線状に並ぶ。駆動補助部１０６が縮むと、大腿接続部１０２と下腿部１０４とのＸ軸回りの角度が減少し、大腿接続部１０２と下腿部１０４とが所定の角度（例えば９０度）をなして装着者の着席動作等に対応できるようになる。駆動補助部１０６の伸縮を制限すると、大腿接続部１０２と下腿部１０４との角度が現在の角度で固定される。膝継手１００は、駆動補助部１０６を伸縮又は伸縮を制限することで大腿接続部１０２と下腿部１０４との角度を制御し、ひいてはソケット１２に対する足部１４の角度を制御する。

【0012】

図２は、膝継手の概略構成を示すブロック図である。図２に示す各ブロックは、ハードウェア的には、コンピュータのプロセッサ、ＣＰＵ、メモリをはじめとする素子や電子回路、機械装置で実現でき、ソフトウェア的にはコンピュータプログラム等によって実現されるが、ここでは、それらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックはハードウェア、ソフトウェアの組合せによっていろいろなかたちで実現できることは、当業者には理解されるところである。これらハードウェアは、大腿接続部１０２又は下腿部１０４のいずれかに収容してもよいし、独立して設けられた制御装置として膝継手１００に取り付けてもよい。

【0013】

図２に示すように膝継手１００は、大腿接続部１０２、駆動補助部１０６、及び下腿部１０４に加えて角速度センサ１０８と、角度センサ１１０と、姿勢計算部１１２と、制御部１１４とを備える。

【0014】

角速度センサ１０８は、下腿部１０４の角速度を検出する。角速度センサ１０８としてはジャイロセンサを用いることができる。角速度センサ１０８は下腿部１０４に収容される。角速度センサ１０８は、大腿接続部１０２との連結部を中心にＸ軸、Ｙ軸及びＺ軸回りにおける下腿部１０４の角速度を検出し、検出結果を姿勢計算部１１２に供給する。

【0015】

角度センサ１１０は、静止座標系（鉛直または方位）に対する下腿部１０４の角度を検出する。角度センサ１１０としては、加速度センサ又は地磁気センサを用いることができる。角度センサ１１０の検出結果は姿勢計算部１１２に供給される。なお、角度センサ１１０と角速度センサ１０８とを合わせた６軸慣性センサを用いてもよい。

【0016】

姿勢計算部１１２は、角速度センサ１０８及び角度センサ１１０の検出結果に基づいて下腿部１０４の姿勢を計算する。

【0017】

姿勢計算部１１２は、第１の導出部１１６と、第２の導出部１１８と、補正部１２２とを備える。姿勢計算部１１２は、第１の導出部１１６より導出された三元数及び第２の導出部１１８で導出された三元数に基づいて下腿部１０４の姿勢を計算する。下腿部１０４の姿勢の求め方については後述する。

【0018】

第１の導出部１１６は、角速度センサ１０８の検知結果をもとに、第１の多元数を導出する。第１の多元数は、膝継手１００と共に回転する回転座標系が静止座標系に対してどの程度回転したかを示す四元数（クォータニオン）を導出し、その四元数を三元数に変換したものである。導出された三元数は補正部１２２に供給される。四元数は、単位ベクトル又は回転角に関する項を４つ有する式として表現される。以下の数式１は、一般的な回転を表す四元数である。なお、四元数を導出するとは、計算の論理を説明するために用いる表現であり、以下で説明する一連の計算過程において必ずしも四元数を導き出し、情報として保有し、又は書き出したりする必要はない。

【数1】

ここでｎは大きさ１の単位ベクトルを表し、ｑ０^２＋ｑ１^２＋ｑ２^２＋ｑ３^２＝１であり、θは回転角を表す。

【0019】

第２の導出部１１８は、角度センサ１１０の検知結果をもとに、第２の多元数を導出する。第２の多元数は、膝継手１００と共に回転する回転座標系が静止座標系に対してどの程度回転したかを示す。第２の多元数は、四元数よりも少ない項を有する三元数である。

【0020】

補正部１２２は、補正が要求される場合に、角速度センサ１０８及び角度センサ１１０の検知結果から算出された下腿部１０４の姿勢を補正する。補正部１２２は、カルマンフィルタ、相補フィルタ等のフィルタを用いて補正を行う。補正部１２２は、下腿部１０４の姿勢が計算される度、予め決定された時間周期毎、又は下腿部１０４の姿勢が所定回数計算された後等、所定の条件に従って補正を行う。

【0021】

姿勢計算部１１２は、補正部１２２で補正されていない下腿部１０４の姿勢に関する情報、及び補正部１２２で補正された下腿部１０４の姿勢に関する情報を姿勢情報として制御部１１４に供給する。

【0022】

制御部１１４は、姿勢計算部１１２から供給された姿勢情報に基づいて駆動補助部１０６を制御する。制御部１１４が用いる下腿部１０４の姿勢情報は、姿勢計算部１１２で計算され、又は姿勢計算部１１２で計算されて補正された値である。例えば姿勢情報により示される下腿部１０４の姿勢から、装着者の着地動作が推定されるような場合には、制御部１１４は駆動補助部１０６を伸ばして着地に備え、さらに装着者の重心が義足側にあるときに下腿部１０４が大腿接続部１０２に対してＸ軸回りに回転しないよう駆動を制限する。

【0023】

膝継手１００はさらに校正部１２４を備える。校正部１２４は、第１の導出部１１６で得られた三元数を用いて角速度センサ１０８のドリフトを推定し、ドリフトを打ち消すよう角速度センサ１０８を校正する。

【0024】

次に、膝継手１００の作用について説明する。図３は、膝継手の一連の動作を示すフロー図である。膝継手１００の電源がオン状態になり一連の動作を開始すると、ステップＳ１において膝継手１００はセンサの値を取得する。この処理は、姿勢計算部１１２が角速度センサ１０８及び角度センサ１１０の検出結果を取得することで実行される。次いでステップＳ２において膝継手１００は、下腿部１０４の姿勢を算出する。この処理については後述する。次いでステップＳ３において膝継手１００は、駆動補助部１０６を制御する。この処理は、制御部１１４４がステップＳ２において得られた姿勢情報に基づいて駆動補助部１０６を制御することで実行される。

【0025】

図４は、図３のステップＳ２における姿勢計算部の一連の処理を示すフロー図である。一連の処理が開始するとステップＳ１１において第１の導出部１１６は、角速度センサ１０８の検知結果に基づいて第１の多元数を導出する。次いでステップＳ１２において第２の導出部１１８は、角度センサ１１０の検知結果に基づいて第２の多元数を導出する。なお、ステップＳ１１とステップＳ１２は逆の順序で実行してもよい。ステップＳ１１及びステップＳ１２の処理により、第１の多元数と第２の多元数が相補に補正可能な状態になる。次いでステップＳ１３において姿勢計算部１１２は、下腿部１０４の姿勢を計算して姿勢情報を生成する。ステップＳ１３における処理は、得られた姿勢情報を補正部１２２において補正することを含む。

【0026】

次に姿勢計算部１１２による下腿部１０４の姿勢の求め方について詳述する。下腿部１０４の姿勢とは、回転座標系における下腿部１０４の向き及びどのような回転でその向きになったかをいう。下腿部１０４の姿勢を求めるために、角速度センサ１０８の検出値を積分して傾斜角を求める。なお、説明の便宜上、従来用いられていた計算方法についても詳述する。従来、傾斜角を求めるためには、一般的には数式２乃至８が用いられていた。

【0027】

まず、静止座標系における下腿部１０４の任意の点ｒ（ｘ，ｙ，ｚ）を四元数ｑで回転させることは以下の数式２により表される。

【数2】

ここでｑ＊は、共役四元数（共役クォータニオン）を表す。

【0028】

数式２は四元数ｑに基づいて点ｒを一度回転させたことを表す。これを応用して、四元数ｑ１，ｑ２，ｑ３・・・ｑｎで点を連続的に回転させた場合、四元数ｑは以下の数式３により表される。

【数3】

数式３に示すように姿勢計算部１１２は、過去の回転を示す四元数を次の計算の際にフィードバックする。

【0029】

数式３を用いて、時刻ｔの四元数ｑ（ｔ）が、時間Δｔ後に四元数ｑ（ｔ＋Δｔ）になり、この間の回転を四元数ｑ（Δｔ）で表すと、以下の数式４により表される。

【数4】

【0030】

数式４において、微小時間Δｔの間のベクトルｎの変化が微小角Δθによるものとすると、傾斜は以下の数式５により表される。

【数5】

【0031】

数式５において微小時間Δｔを限り無く０に近付けると、角速度センサ１０８から得られる出力は四元数ｑで回転させられた値となるので、これを数式２に代入すると、以下の数式６が得られる。

【数6】

ここでωは角速度センサで計測された角速度を表す。

【0032】

数式６について四元数ｑを時間ｔで微分すると、数式７が得られる。

【数7】

【0033】

数式７を整理すると、数式８が得られる。

【数8】

数式８は、角速度センサ１０８の検出値を積分して傾斜角を求めるために用いられていた式である。

【0034】

数式８を用いれば一応、下腿部１０４の傾斜角を求めることができる。しかしながら、角速度センサ１０８の検出値にドリフトが含まれる状態で数式８により傾斜角を反復計算すると、ドリフトによる誤差が蓄積して正確な傾斜角が得られないことがある。また数式５においては、Δｔを限り無く０に近付けている。つまり、数式８は姿勢計算部１１２による計算間隔が短い場合にのみ成立する。

【0035】

ドリフトによる誤差をゼロにすることは困難であるため、角速度センサ１０８以外のセンサ、つまり角度センサ１１０の検知結果に基づいて数式８で得られた結果を補正できれば良い。しかしながら、角度センサ１１０はＺ軸回り（ヨー軸回り）の回転を正確に検知できないため、角度センサ１１０の検知結果をそのまま利用して角速度センサ１０８の検知結果を補正することは困難である。

【0036】

実施形態の膝継手１００では、Ｚ軸回りの回転は、装着者が向いている方向を変えるように旋回したこと（例えば、北向きに歩いている最中に方向転換をして西に向いて歩くこと）を意味する。つまり、装着者の向きと、駆動補助部１０６の伸縮動作は、直接関係するものではないため旋回動作は膝継手１００の制御（つまり駆動補助部１０６の伸縮動作）に影響を及ぼさない。発明者等はこの点に着目し、Ｚ軸回りの回転を無視する（０とみなす）ように角速度センサ１０８の検知結果から導出された四元数の項数を減らして角速度センサ１０８の検知結果を角度センサ１１０の検知結果と同一項数にすれば、角度センサ１１０の検知結果から導出された三元数の項数との間で補正を行えるようになるという新たな知見を得た。これにより、角速度センサ１０８の検知結果を角度センサ１１０の検知結果で補正でき、又は反対に角度センサ１１０の検知結果を角速度センサ１０８の検知結果で補正できるようになる。以下、この点についてさらに詳細に説明する。

【0037】

角速度センサ１０８の検知結果から得られた現在の下腿部１０４の回転が四元数ｑ’＝（ｑ０’＋ｑ１’ｉ＋ｑ２’ｊ＋ｑ３’ｋ）により表されるものとする。Ｚ軸回りの回転を無視する場合、四元数ｑ’により下腿部１０４を任意の角度θで回転させてもよいことを意味する。したがって、四元数ｑ’をＺ軸回りに角度θ回転させた値ｑ’’は、以下の数式９により表される。なお値ｑ’’は、Ｚ軸回りの回転を無視し四元数ｑ’に対して項数を１つ減らした三元数を示す。

【数9】

【0038】

数式９の４つの項のうち、例えばｋの項を０にしようとすると、以下の数式１０のような関係が得られる。

【数10】

数式１０より、θ／２＝－αとなる。

【0039】

数式１０に基づけば角度θの正弦及び余弦は、以下の数式１１により表される。

【数11】

【0040】

数式１１を数式９に代入すると以下の数式１２が得られる。

【数12】

数式１２は、以下の数式１３のようにも表せる。

【数13】

【0041】

数式１２及び数１３は、ｋの項を０とみなして下腿部１０４のＺ軸回りの回転を無視しているが、この三元数は膝継手１００の性質を考慮すると元の四元数と等価であるとも言える。数式１３は、Ｘ軸をＺ軸回りに所定角度回転させた新たな軸を定め、下腿部１０４を新たな軸回りに角度θ回転させていることを表す。

【0042】

第１の導出部１１６が現在の角速度センサ１０８の検知結果から導出した四元数ｑに基づいて、時間変化後の三元数ｑ’’を求める場合、例えば以下の計算方法を用いる。まず、数式１４に示すように、数式８に現在の四元数ｑを代入し、時間変化後の四元数ｑ’を算出する。

【数14】

なお、ｑ３＝０である点は上述の通りである。

【0043】

次に数式１４を以下の数式１５に代入する。

【数15】

これにより、四元数ｑ’と等価な三元数ｑ’’が得られる。数式１５において、項ｄｔ^２の値は十分に小さいため０に近似すると数式１５は以下の数式１６のように表せる。

【数16】

【0044】

三元数ｑ１’’及びｑ２’’についても同様の計算をすると、以下の数式１７及び数式１８が得られる。

【数17】

【数18】

【0045】

数式１６乃至数式１８を用いることにより、前回の三元数ｑと現在の角速度センサ１０８の検知結果から得られる出力ωに基づいて下腿部１０４の傾斜角度を三元数ｑ’’として算出できる。

【0046】

次に、第２の導出部１１８が角度センサ１１０の検知結果から下腿部１０４の姿勢を算出する方法について説明する。この場合、角度センサ１１０は３軸の加速度センサにより構成されているものとする。角度センサ１１０の出力をａ（ａｘ，ａｙ，ａｚ）とし、ベクトルの大きさが１になるよう予め正規化するものとする。角度センサ１１０の検知結果から得られる三元数ｑａを、ｑａ＝（ｑａ０，ｑａ１，ｑａ２）とし、重力加速度を（０，０，－１）を三元数ｑａで回転させた回転座標系で測定した加速度はａとなる。したがって、静止座標系にある点を三元数ｑａで回転された座標系から見た場合の式ｑ＊ｒｑより、以下の数式１９の関係が成立する。

【数19】

【0047】

数式１９を整理すると、以下の数式２０乃至２２が得られる。

【数20】

【数21】

【数22】

【0048】

数式２２より、数式２３の関係が得られる。

【数23】

【0049】

数式２３を数式２０及び数式２１のそれぞれに代入すると、以下の数式２４及び２５が得られる。

【数24】

【数25】

数式２３乃至２５により、三元数ｑａが得られる。

【0050】

以上のように、Ｚ軸回りの回転を無視して下腿部１０４の姿勢を算出することで、角度センサ１１０の検知結果と、角速度センサ１０８の検知結果とを同じ次元として取り扱える。姿勢計算部１１２が角度センサ１１０の検知結果及び角速度センサ１０８の検知結果を三元数として取り扱うことで、例えば互いの値を相補に補正する場合の計算を単純化できる。つまり角速度センサ１０８の検知結果と、角度センサ１１０の検知結果とを同じ次元として取り扱うことで、角速度センサ１０８の検知結果を角度センサ１１０の検知結果を用いて補正できる状態を作り出せる。これにより、様々な場面に応じて角速度センサの検出値を補正できる環境が整う。例えば、角速度センサ１０８の検知結果にドリフトによる誤差が含まれていても、角度センサ１１０の検知結果を用いて誤差を補正できるようになる。姿勢計算部１１２は駆動中に姿勢を繰り返し計算し続けるが、例えば装着者が長時間立ち止まり前回の姿勢計算時から時間が空いた場合、角速度センサ１０８の検知結果に誤差が生じ易い。また、膝継手１００全体のエネルギー消費量を減らすような省エネモードが搭載されていることがある。非省エネモードでは例えば５ｍｓ毎に下腿部１０４の姿勢を計算するのに対して、省エネモードでは例えば１００ｍｓ毎に下腿部１０４の姿勢を計算するとする。省エネモードに入ったときに計算頻度が少なくなり、角速度センサ１０８の検知結果の誤差が大きくなることがある。このような場合に、角度センサ１１０の検知結果により下腿部１０４の姿勢を補正できるようにすることは非常に有益である。

【0051】

次に、補正部１２２による補正について詳述する。補正部１２２は、膝継手１００の駆動中に繰り返し下腿部１０４の姿勢の計算結果を補正する。補正部１２２は、姿勢計算部１１２が下腿部１０４の姿勢を計算する度に補正を行ってもよいし、所定条件が満たされたときに補正を行ってもよい。所定条件としては、省エネモードから復帰したとき、及び前回補正を行ったときから所定時間経過したときがある。また、所定条件としては、前回補正を行ってから予め決定された回数だけ補正をしていない計算結果に基づいて制御部１１４が駆動補助部１０６を制御したときがある。このような場合には、角速度センサ１０８の検知結果に含まれるドリフトの量が多い可能性が高いからである。

【0052】

所定条件として前回補正を行ってから所定時間経過したときに補正を行うことが設定されている場合、補正部１２２は、前回の補正を行ったときからの経過時間に応じて変化する第１の重み付け係数Ａ及び第２の重み付け係数Ｂを用いることができる。第１及び第２の重み付け係数Ａ，Ｂは、対応する多元数をどの程度、補正結果に反映させるかを決定する係数である。数式１６乃至１８で得られた角速度センサ由来の三元数をｑｇとし、数式２３乃至２５で得られた角度センサ由来の三元数をｑａとした場合、補正部１２２は以下の数式２６により下腿部１０４の姿勢の計算結果を補正できる。

【数26】

ここでＡ＋Ｂ＝１である。

【0053】

前回の補正からの経過時間が長い場合、第２の重み付け係数Ｂの値を大きくして角度センサ１１０に基づく三元数の重みを多くするよう変更し、第１の重み付け係数Ａの値を小さくして角速度センサ１０８に基づく三元数の重みを少なくするよう変更できる。これにより角速度センサ１０８のドリフトの影響を少なくした補正値ｑｈを得られる。

【0054】

姿勢計算部１１２が下腿部１０４の姿勢を計算する度に補正を行う場合、又は所定条件として、省エネモードから復帰したとき、前回補正を行ってから予め決定された回数だけ補正をしていない計算結果に基づいて制御部１１４が駆動補助部１０６を制御したときに補正を行うことが設定されている場合、補正部１２２は以下の方法により補正を行うことができる。

【0055】

この場合、補正部１２２は、相補フィルタを用いて下腿部１０４の姿勢を補正してもよい。角度センサ１１０の検知結果からは、歩行動作以外の動作からくる振動と、重力加速度の変化による振動を区別できない。したがって振動が発生した場合、角度センサ１１０の検知結果には大量のノイズが含まれることとなる。一方で角速度センサ１０８の検知結果は振動の影響を受けないため、短期的に見ればノイズが小さいと考えられる。したがって、例えば短い時間間隔で補正を行うような場合には、角速度センサ１０８の検知結果を重視し、角速度センサ１０８のドリフトの影響を角度センサ１１０の検知結果で打ち消すような補正をするのがよい。この場合、補正部１２２は以下の数式２７により下腿部１０４の姿勢を補正する。

【数27】

ここでαは予め決定された定数である。数式２７の相補フィルタを繰り返し使用することで、角度センサ１１０の誤差は定数αにより小さくなり、かつ角速度センサ１０８の誤差はα（ｑａ－ｑｇ）により繰り返し補正される。これにより振動の影響を抑えつつ、ドリフトの影響を少なくできる。

【0056】

別の例として補正部１２２は、カルマンフィルタを用いて下腿部１０４の姿勢を補正してもよい。カルマンフィルタは、以下の数式２８により表される。

【数28】

値Ｋは、検出結果の分散に応じて可変な変数である。例えば装着者が立ち止まっている場合には例えばターミナルインパクトによる振動が発生しないため、予期せぬ振動が生じ難いものと考えられる。このような場合には、観測値（ｑａ）の分散が小さくなり、これにより値Ｋが大きくなる。したがって、補正値ｑｈに対する項Ｋ（ｑａ－ｑｇ）の影響を大きくできる。反対にターミナルインパクトのような急激な振動が発生した場合、観測値（ｑａ）の分散が大きくなりＫの値が小さくなる。これにより補正値ｑｈに対する項Ｋ（ｑａ－ｑｇ）の影響を小さくできる。このように短期的にはノイズの乗りやすい角度センサ１１０の観測値の分散に応じて補正量を増減させることで、より精度の高い補正を行える。

【0057】

補正部１２２がカルマンフィルタを用いて下腿部１０４の姿勢を補正する場合、補正部１２２は角度センサ１１０の観測値の分散（つまり急激な振動）に応じて姿勢計算部１１２の計算結果を補正する機能を有する、とも言える。補正部１２２は、所定期間内における振幅の変化や周波数の変化に基づいて変数Ｋを変化させてもよい。

【0058】

数式２８で表されるカルマンフィルタの代替として、以下の数式２９で表されるフィルタを用いてもよい。

【数29】

μは定数である。

【0059】

数式２９は、角度センサ１１０の信頼性が高い場合には項（ｑａ－ｑｇ）の値が小さくなり、角度センサ１１０の信頼性が低い場合には項（ｑａ－ｑｇ）の値が大きくなるようになっている。

【0060】

数式２９は、以下の数式３０乃至３２のようにも表せる。

【数30】

【数31】

【数32】

【0061】

図５は、値ｑａと値ｑｇとの関係を示すグラフである。具体的には図５は、値ｑａと値ｑｇとをノルム空間上に配置し、ベクトルの大きさが同一であるものとして表している。図５に示すように補正値ｑｈは、値ｑｇから値ｑａに向けて固定値μだけ移動させたものである。図５から分かるように、ノルム空間上において補正値ｑｈは、必ず円弧の内側に入り大きさが値１よりも小さくなる。しかしながら角度センサ１１０の出力ａ（ａｘ，ａｙ，ａｚ）は予め正規化されて値１で固定されており、数式２９において値ｑａに近付けている。したがって、この段階で補正部１２２が補正値ｑｈを正規化する計算を行わなくても、補正値ｑｈは値１に近い値（値１より僅かに小さい値）で安定する。これにより補正部１２２の計算量を減らせる。

【0062】

姿勢計算部１１２で得られた姿勢情報、又は補正された姿勢情報は、姿勢計算部１１２の判断により制御部１１４に供給される。例えば、補正にあたり所定条件が設定されている場合、姿勢計算部１１２は所定条件が満たされていない間は補正されていない姿勢情報を制御部１１４に供給する。姿勢計算部１１２は所定条件が満たされた場合にのみ、姿勢情報を補正し、補正された姿勢情報を制御部１１４に供給する。なお、姿勢計算部１１２が全ての場合において姿勢情報、及び補正された姿勢情報を計算して制御部１１４に供給し、制御部１１４において所定条件を判断し、使用する姿勢情報を判断してもよい。

【0063】

次に校正部１２４により角速度センサ１０８を校正する方法について説明する。校正部１２４は、第１の導出部１１６にて得られた三元数を用いて角速度センサ１０８のドリフトを推定し、バイアスをゼロに近付ける。例えば装着者が動いていないにも関わらず角速度センサ１０８の検知結果が得られた場合、ドリフトが発生しているとみなせる。この場合、校正部１２４は、角速度センサ１０８の検知結果の平均値を角速度センサ１０８の検知結果として取り扱う。また、校正部１２４は、数式１６～１８で得られた値に対して数式２９のフィルタをかけることにより、角速度センサ１０８の検知結果を校正してもよい。校正部１２４は、角速度センサ１０８の検知結果が得られる度に校正を行ってもよいし、上述した所定条件が満たされたときに校正を行ってもよい。

【0064】

校正部１２４を備えることにより、角速度センサ１０８の検知結果の精度を向上させられる。特に大腿義足１０のような特定の装置では、使用者が装置を正しい位置で保持しながら校正をすることが困難である。したがって校正部１２４により継続的に角速度センサ１０８を校正できるようにすることで、使用者の負担を軽減させられる効果もある。

【0065】

次に、ステップＳ３における制御部の処理について説明する。上述したように姿勢情報は、三元数として供給される。制御部１１４は、供給された三元数を含む姿勢情報に基づいて下腿部１０４の姿勢を制御する。制御部１１４は、以下の例で得られたピッチ角ｐ及びロール角ｒに基づいて駆動補助部１０６の伸縮量を制御する。

【0066】

ピッチ角ｐを回転してからロール角ｒを回転することを表す四元数は、数式１及び３を用いて以下の数式３３により表される。

【数33】

【0067】

数式１２を用いて数式３３を三元数に変換すると以下の数式３４～３６のようになる。

【数34】

【数35】

【数36】

【0068】

数式３６をｐについて整理すると以下の数式３７が得られる。

【数37】

【0069】

また数式３４及び３５を整理すると、以下の数式３８が得られる。

【数38】

数式３８より、ｒ＝ａｔａｎ２（２ｑ０ｑ１，２ｑ０^２－１）となる。

【0070】

制御部１１４は、数式３７及び３８で得られた結果に基づいて駆動補助部１０６を制御する。

【0071】

また、例えば数式１７のように分母に値ｑ０を含む項が存在する場合、姿勢計算部１１２は以下のような処理を行ってもよい。

【0072】

例えば数式１７では、値ｑ０が十分に大きいものとしてｄｔの二乗項を無視する計算を行っているが、値ｑ０が小さい場合（つまり固定座標系において膝継手１００が１８０度近く回転している場合）には、分母に値ｑ０を含む項を無視できなくなる可能性がある。このため、膝継手１００が静止座標系において１８０度近く回転している場合には、姿勢計算部１１２は、以下のように座標系を回転させて姿勢を計算する。

【0073】

図６は、膝継手の概略図を示す。姿勢計算部１１２は、図６に示すように値ｑ０が大きい場合には下半球にある静止座標系Ｇ１（第１の座標系）を用いて姿勢を計算し、値ｑ０が小さい場合には上半球にある静止座標系Ｇ１を変換させた静止座標系Ｇ２（第２の座標系）を用いて姿勢を計算する。静止座標系Ｇ１及び静止座標系Ｇ２を用いて姿勢を計算する場合、回転軸の向きは維持し、回転角度について静止座標系Ｇ１における回転角度に１８０度を加算して計算を行えばよい。

【0074】

座標系を変換する場合、以下の数式３９～４４を用いる。

【数39】

【数40】

【数41】

【数42】

【数43】

【数44】

θは元の変換前の三元数ｑの回転角度を示し、αは回転軸とＸ軸のなす角度を示す。また、変換後の三元数ｑ’は回転軸が等しく、回転角度πが加算されたものとして示す。また、数式４２～４４において、ｑ０’は正の値をとるので、角度θが正の値をとるときは式中の符号を全て反転させる必要がある。

【0075】

数式４０及び数式４２を整理すると、以下の数式４５が得られる。

【数45】

【0076】

また数式４０と数式４３を整理すると、以下の数式４６が得られる。

【数46】

【0077】

また数式４０と数式４４を整理すると、以下の数式４７が得られる。

【数47】

【0078】

このような座標の変換により、値ｑ０が小さい場合でも精度の高い補正値を得られる。

【0079】

図７は、姿勢計算部による一連の処理を示すブロック線図である。なお、図７は図示を簡略化するために、姿勢情報を計算する度に補正する態様を例示するものである。図７に示すように、姿勢計算部１１２は、要素１１２０において角度センサ１１０の検知結果から三元数を導出する。この処理は、第２の導出部１１８が数式２３～２５を用いて実行する。姿勢計算部１１２は、要素１１２２において角速度センサ１０８の検知結果から三元数を導出する。この処理は、第１の導出部１１６が数式１６～１８を用いて実行する。姿勢計算部１１２は要素１１２４において、要素１１２０の出力と、要素１１２２の出力の差を導出する。この差は、要素１１２６においてフィルタにかけられ（例えば数式２９）、補正値として要素１２２８に出力される。要素１１２８では、要素１１２２で得られた三元数と、要素１１２６で得られた補正値との和が導出される。つまり要素１１２２においては、一度要素１１２２において算出された下腿部１０４の姿勢情報を補正値により補正している。補正された姿勢情報は、姿勢計算部１１２の出力ｑ１１２として制御部１１４に供給される。また、要素１１２８の出力ｑ１１２は、要素１１２２にフィードバックされ、次回の計算に使用される。この場合、補正結果は数式３０のｑｇ１に代入される。

【0080】

図８は、膝継手を制御するための制御用プログラムのブロック図である。図８に示すように制御用プログラム２００は、第１の多元数導出部２０２と、第２の多元数導出部２０４と、計算指示部２０８と、駆動指示部２１０とを備える。

【0081】

第１の多元数導出部２０２は、角速度センサ１０８の検知結果をもとに三元数を導出する指示を第１の導出部１１６に供給する。第２の多元数導出部２０４は、角度センサ１１０の検知結果をもとに三元数を導出する指示を第２の導出部１１８に供給する。計算指示部２０８は、得られた２つの三元数に基づいて膝継手１００の姿勢を計算する指示を姿勢計算部１１２に供給する。計算指示部２０８からの指示には、補正部１２２に所定条件に応じて姿勢情報を補正させる指示を含む。駆動指示部２１０は、計算指示部２０８の計算結果に基づいて駆動補助部１０６を制御させる指示を制御部１１４に供給する。

【0082】

以上のように膝継手１００によれば、角速度センサ１０８及び角度センサ１１０から得られた２つの検知結果のうちの一方を用いて他方を補正できる環境を整えられる。補正できる環境が整うことで、簡易なフィルタを用いて両者の検知結果を相補に補正できるようになる。

【0083】

また、第１の重み付け係数Ａ及び第２の重み付け係数Ｂを用いることで、角速度センサ１０８の検知結果の信頼性が低下している可能性が高いときに角度センサ１１０の検知結果に重みをおいた補正を行える。これにより、姿勢の算出精度を高められる。

【0084】

また、所定条件を満たしていない場合、制御部１１４が角速度センサ１０８の検知結果のみに基づいて駆動補助部１０６を制御することにより計算に要する時間を短くでき、膝継手１００の応答速度を向上させられる。

【0085】

本発明は上述の実施形態に限られるものではなく、実施形態の各構成は本発明の趣旨を逸脱しない範囲で適宜変更可能である。

【0086】

特に本発明は膝継手に限定されるものではなく、Ｚ軸回りの旋回を無視しても姿勢の計算に影響がない装置に適用可能である。本発明は、姿勢制御にあたり進行方向を加味する必要がない装置、又はピッチ方向の回転を無視しても姿勢計算に影響がない回転体に適用可能である。移動体の姿勢を計算する姿勢計算装置として本発明を実施する場合、移動体の角速度から導出される三元数と、静止座標系における移動体の角度の検知かっかから導出される三元数とを用いて姿勢を計算すればよい。

【符号の説明】

【0087】

１０ 大腿義足， １００ 膝継手， １０２ 大腿接続部， １０４ 下腿部， １０６ 駆動補助部， １０８ 角速度センサ， １１０ 角度センサ， １１２ 姿勢計算部， １１４ 制御部， １１６ 第１の導出部， １１８ 第２の導出部， １２２ 補正部， １２４ 校正部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】