センシング対象値推定装置およびその推定方法
【課題】センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることのできるセンシング対象値推定装置およびその推定方法を提供すること。
【解決手段】本発明の積算値推定装置２０は，センサ出力値の変遷を記録する第１ループメモリ２１と，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱を検出するとともにダイナミックレンジ内への復帰を検出するレンジオーバー開始／レンジ内復帰判定部２７と，これらによって検出された逸脱から復帰までの間について，第１ループメモリ２１に記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するセンシング対象値推定部２８とを有する。
【解決手段】本発明の積算値推定装置２０は，センサ出力値の変遷を記録する第１ループメモリ２１と，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱を検出するとともにダイナミックレンジ内への復帰を検出するレンジオーバー開始／レンジ内復帰判定部２７と，これらによって検出された逸脱から復帰までの間について，第１ループメモリ２１に記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するセンシング対象値推定部２８とを有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，例えばロボットの腕や足等に設置された角速度センサや加速度センサ等のセンサの出力値からそのセンシング対象値を推定するセンシング対象値推定装置およびその推定方法に関する。さらに詳細には，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定するセンシング対象値推定装置およびその推定方法に関するものである。
【背景技術】
【０００２】
従来より，ロボットを指示に従って駆動させるために，ロボットの各部には各種のセンサが取り付けられている。例えば，肩部や肘部等の関節には角速度センサが設けられ，関節の回転角速度を累積して，上腕や下腕の角度やその先端部の位置等が求められている。むろん，このような例はロボットにも限らないし，角速度にも限らない。このようなセンサには，一般に，それぞれ所定の検出可能範囲（ダイナミックレンジ）があり，その範囲を超えた入力を受けた場合には，正確に検出することはできない。すなわち，入力がダイナミックレンジを超えている場合には，一般に，ダイナミックレンジの上限値または下限値が出力されるようになっている。そして，入力が再びダイナミックレンジ内に入れば，センサの出力値も正しく出力される。
【０００３】
これに対して，特許文献１には，地磁気方位センサを用いて車両の走行方位を検出する車両用方位計において，外乱により地磁気方位センサへの入力がそのダイナミックレンジを超えたときの処理方法について開示されている。この文献の技術によれば，外乱によりセンサへの入力がダイナミックレンジを超えている時間の長さを基に，外乱の大きさとその発生時点とを予測できるとされている。
【特許文献１】特開平２−２１８９１１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら，前記した従来の技術は，地磁気を利用して水平面内での車両の向きを測定する車両用方位計に特化された技術である。一般に，磁気外乱によってセンサ出力値は大きく変化するが，車両の姿勢自体は大きく変化したわけではない。また外乱の大きさによっては，車両の着磁という後遺症が発生するおそれがあるため，その外乱の大きさを取得することが重要となっている。
【０００５】
これに対して，例えばロボットの腕部等の場合では，他の物体への衝突や足部の振動等による外乱は，腕の姿勢自体を変化させてしまう。特に，センサの出力値を積算して角度や位置を求めるもののように，履歴に応じた信号処理を持つ処理系では，ダイナミックレンジの上限値あるいは下限値となっているセンサ出力値をそのまま利用して計算を続けることは，誤差がますます蓄積することになるので好ましくない。
【０００６】
また，ダイナミックレンジの大きいセンサを選択するという方法も考えられるが，一般にダイナミックレンジの大きいセンサは，分解能が低いという問題点がある。ダイナミックレンジの大きさと分解能とをともに満たすセンサは無く，あったとしてもごく高価で複雑な構造のものであって，一般的ではない。さらに，ダイナミックレンジの異なる複数のセンサを備えてそれらを使い分けることもできるが，センサの個数が増加するためコスト高となり，また制御も煩雑なものとなる。
【０００７】
本発明は，前記した従来のセンシング対象値推定装置およびその推定方法が有する問題点を解決するためになされたものである。すなわちその課題とするところは，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることのできるセンシング対象値推定装置およびその推定方法を提供することにある。
【課題を解決するための手段】
【０００８】
この課題の解決を目的としてなされた本発明のセンシング対象値推定装置は，センサ出力値の変遷を記録する第１メモリと，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出するレンジ検出部と，レンジ検出部により検出された逸脱から復帰までの間について，第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定する推定部とを有するものである。
【０００９】
本発明のセンシング対象値推定装置では，センサ出力値の変遷が第１メモリに記録され，センサ出力値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰がレンジ検出部で検出される。さらに，検出された逸脱から復帰までの間のセンシング対象値が推定部で推定されるので，センシング対象値がセンサのダイナミックレンジを超えていても，推定部によって適切に推定される。従って，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることのできるセンシング対象値推定装置となっている。
【００１０】
さらに本発明では，推定部は，復帰後に，第１メモリに記録されている逸脱前と復帰後とのセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するものであることが望ましい。またさらに，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部を有し，予測部で決定した予測関数によりセンシング対象値の変遷を推定するものであることが望ましい。
【００１１】
さらに本発明では，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，予測部で決定した予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定する減衰関数決定部とを有し，外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については減衰関数決定部が決定した減衰関数によりセンシング対象値の変遷を推定するものであることが望ましい。
センシング対象値を変化させる要因の一部が外部装置にある場合，その外部装置から外部情報を受けることによりセンシング対象値の変遷をある程度推定できる。例えば，モータ等によって回転されているセンシング対象では，モータを停止したという外部情報に基づいて，その後のセンシング対象値を減衰関数で推定すれば，推定精度を向上させることができる。
【００１２】
さらに本発明では，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定する逆算部とを有し，予測部で決定した予測関数と逆算部が決定した逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定するものであることが望ましい。
このようにすれば，逸脱前と復帰後のそれぞれのセンサ出力値の変遷に基づいて，それぞれ逸脱後と復帰前の適切な推定値が選択されるので，より推定精度を向上させることができる。
【００１３】
さらに本発明では，推定部は，逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定するベジェ関数決定部を有し，ベジェ関数決定部が決定したベジェ関数によりセンシング対象値の変遷を推定するものであることが望ましい。
このようにすれば，逸脱前と復帰後との両方のセンサ出力値の変遷に基づいた関数が使用されているので，不自然に急激な変化が無い推定値が得られる。
【００１４】
さらに本発明では，予測部は，センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて予測関数を決定するとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，予測部における予測関数の決定が，センシング対象値のダイナミックレンジ内からの逸脱時点とそこから所定時間さかのぼった時点とのセンサ出力値に基づいて行われる。さらに，センシング対象値のダイナミックレンジ内からの逸脱時点から所定時間さかのぼった時点までの間にさらにダイナミックレンジ内からの逸脱があった場合には，逸脱していない範囲のセンサ出力値の波形に基づいて決定される。従って，複数回の逸脱が断続的に発生したとしても，適切な予測関数が得られる。
【００１５】
さらに本発明では，逆算部は，センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて逆算関数を決定するとともに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，逆算部における逆算関数の決定が，センシング対象値のダイナミックレンジ内への復帰時点とそこから所定時間経過した時点とのセンサ出力値に基づいて行われる。さらに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，逸脱していない範囲のセンサ出力値の波形に基づいて決定される。従って，複数回の逸脱が断続的に発生したとしても，適切な逆算関数が得られる。
【００１６】
さらに本発明では，ベジェ関数決定部は，センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいてベジェ関数を決定するとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，複数回の逸脱が断続的に発生したとしても，適切なベジェ関数が得られる。
【００１７】
さらに本発明では，センシング対象値のダイナミックレンジ内からの逸脱前とダイナミックレンジ内への復帰後についてはセンサ出力値を記録し，逸脱から復帰までの間については推定部による推定値を記録する第２メモリと，第２メモリに記録された値の積算値を求める積算部とを有することが望ましい。
このようにすれば，第２メモリには，センシング対象値がダイナミックレンジ内ではセンサ出力値が，ダイナミックレンジ外では推定部による推定値が記録される。この記録された値が積算部で積算されるので，より適切な積算値が得られる。
【００１８】
また，本発明は，センサ出力値の変遷を第１メモリに記録し，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定し，検出された逸脱から復帰までの間について，決定された予測関数によりセンシング対象値の変遷を推定するセンシング対象値推定方法にも及ぶ。
【００１９】
さらに本発明のセンシング対象値推定方法では，決定された予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定し，外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については決定された減衰関数によりセンシング対象値の変遷を推定することが望ましい。
【００２０】
さらに本発明のセンシング対象値推定方法では，第１メモリに記録されている復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定し，決定された予測関数と決定された逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定することが望ましい。
【００２１】
また，本発明は，センサ出力値の変遷を第１メモリに記録し，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，第１メモリに記録されている逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定し，検出された逸脱から復帰までの間について，決定されたベジェ関数によりセンシング対象値の変遷を推定するセンシング対象値推定方法にも及ぶ。
【００２２】
また，本発明のセンシング対象値推定方法は，予測関数の決定を，センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて行うとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【００２３】
また，本発明のセンシング対象値推定方法は，逆算関数の決定を，センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【００２４】
また，本発明のセンシング対象値推定方法は，ベジェ関数の決定を，センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【発明の効果】
【００２５】
本発明のセンシング対象値推定装置およびその推定方法によれば，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることができる。
【発明を実施するための最良の形態】
【００２６】
以下，本発明を具体化した最良の形態について，添付図面を参照しつつ詳細に説明する。本形態は，二足歩行ロボットに設けられ，腕部の回転角速度を測定する角速度センサと，そのダイナミックレンジを超えた入力があった場合でもセンシング対象値（ここでは，腕部の角速度）を推定する推定装置に本発明を適用したものである。
【００２７】
本形態の二足歩行ロボット１は，そのスケルトンモデルを図１に示すように，肩部・肘部等の関節Ａ〜関節Ｑまでの計１７箇所の関節部を有している。さらに，各関節Ａ〜Ｑには，それぞれ駆動用のモータと角速度等を検出するセンサとを有している。例えば，右手の肘部に相当する関節Ｐには角速度センサ１１を有し，これにより右肘関節Ｐの回転角速度が検出されている。
【００２８】
また，この二足歩行ロボット１では，この角速度センサ１１の出力値を積算することにより，上腕に対する下腕の角度を求めることができる。同様に肩関節等の各自由度についても，各角速度センサの出力値を積算して，その基部に対する角度が求められる。これらを合わせることにより，各部の位置が得られる。例えば，本体部分に対する右手先の位置関係は，右肩関節（Ｏ，Ｎ）の角度と右肘関節Ｐの角度等を合わせて算出できる。
【００２９】
従って，本形態の二足歩行ロボット１では，このようなセンサ出力値を順次積算した積算値を求めている。ここで，センサにそのダイナミックレンジを超えた入力が印加された場合は，センサ出力値はダイナミックレンジの上限値あるいは下限値に保持される。このため，そのセンサ出力値をそのまま積算すると，例えば手先位置等の算出結果の誤差が大きいものとなってしまう。そこで本形態では，このような場合にはセンサ出力値をそのまま積算するのでなく，その間のセンシング対象値を推定してその推定値を積算する。
【００３０】
次に，本形態の二足歩行ロボット１において，各センサ出力値あるいはセンシング対象値の推定値の積算値を求めるための積算値推定装置２０の概略構成を，図２のブロック図を参照して説明する。本形態の積算値推定装置２０は，図２に示すように，角速度センサ１１からそのセンサ出力値の入力を受けるとともに，タイマ１２から時刻の入力を受け，算出した積算値を表示部１３へ表示するものである。このタイマ１２や表示部１３は，二足歩行ロボット１に備えられていても良いし，別に備えて情報の送受信のみを行うようにしても良い。
【００３１】
また，積算値推定装置２０は，図２に示すように，第１ループメモリ２１，推定部２２，第２ループメモリ２３，積算器２４，平滑器２５を有している。また，推定部２２は，レンジオーバー開始／レンジ内復帰判定部２７，センシング対象値推定部２８を有している。ここで，推定部２２は，角速度センサ１１がレンジオーバー状態である場合に，センシング対象値である角速度を推定する部分である。ここで，レンジオーバー開始／レンジ内復帰判定部２７がレンジ検出部に相当し，センシング対象値推定部２８は予測部，減衰関数決定部，逆算部，ベジェ関数決定部を含んでいる。
【００３２】
この積算値推定装置２０では，第１ループメモリ２１は，角速度センサ１１の出力値の入力を受け，そのまま蓄積する。また，推定部２２は，第１ループメモリ２１に蓄積されたセンサ出力値を受けて，まず，レンジオーバー開始／レンジ内復帰判定部２７によって角速度センサ１１がレンジオーバーしているかどうかを判定する。レンジオーバーしていないと判定されたときはセンサ出力値をそのまま第２ループメモリ２３に書き込む。この場合は，そのままシフトレジスタ等の積算器２４を通して積算値が算出される。さらに，平滑器２５によって平滑化し，その結果を表示部１３へ出力して表示させる。
【００３３】
あるいは，レンジオーバー開始／レンジ内復帰判定部２７によって，角速度センサ１１がレンジオーバーしたと判定された場合には，タイマ１２からそのときの時刻ｔ１を取得する。また積算器２４による積算を停止し，とりあえず一定値を出力する。この区間の積算値については，推定したセンシング対象値を用いて後で修正する。また，レンジオーバー開始／レンジ内復帰判定部２７によって，角速度センサ１１のセンサ出力値がレンジ内に復帰したかどうかを判定する。復帰したと判断されたときは，タイマ１２によってその時刻ｔ２を取得する。さらに，センシング対象値推定部２８によって時刻ｔ１〜ｔ２間の角速度を推定する。この推定方法については後述する。
【００３４】
そして，センシング対象値推定部２８によって推定されたセンシング対象値が第２ループメモリ２３に入力される。以後は，レンジオーバーしていないときと同様に，積算器２４によって第２ループメモリ２３の値を積算する。また，平滑器２５は，レンジオーバーによる積算値の変化が滑らかになるように，例えば移動平均やローパスフィルタによる平滑化を行う。さらにその結果を表示部１３へ出力して表示させる。
【００３５】
次に，センシング対象値推定部２８による推定方法について説明する。推定方法としては，各種の条件に応じて（１）１本直線近似（図３参照），（２）２本直線近似（図５参照），（３）ベジェ曲線近似（図７参照），（４）減衰曲線近似（図９参照）等から使い分ける。これらの図では，角速度センサ１１のダイナミックレンジｄをＤ１≦ｄ≦Ｄ２とし，レンジオーバー開始時刻ｔ１からレンジ内復帰時刻ｔ２までのセンシング対象値を推定している。図中太線で表示しているのは角速度センサ１１の出力値である。なおここでは，レンジオーバー直前とレンジ復帰直後のセンサ出力値が直線的に変化しているように図示したが，このようなものに限らない。
【００３６】
（１）１本直線近似は，図３に示すように，レンジオーバー開始時刻ｔ１からレンジ内復帰時刻ｔ２までのセンシング対象値を１本の直線Ｌ１（ｔ）によって近似する方法である。すなわち，時刻（ｔ１−Δｔａ）〜時刻ｔ１間のセンサ出力値を用いて，例えば最小自乗法等により求めた近似直線Ｌ１（ｔ）によって近似する。この推定方法を実施する場合の概略構成を図４に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をＬ１推定部３１が担っている。
【００３７】
次に，図４に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００３８】
センサ出力値がレンジオーバーとなった時刻ｔ１において，Ｌ１推定部３１は，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，第１ループメモリ２１から時刻（ｔ１−Δｔａ）および時刻ｔ１に対応するセンサ出力値を読み出す。これらのセンサ出力値から近似直線Ｌ１（ｔ）を求める。ここで，Δｔａは，角速度センサ１１のサンプリングタイムの定数倍，例えば１〜１００倍程度の所定時間とする。さらに，Ｌ１推定部３１では，近似直線Ｌ１（ｔ）を用いて，レンジオーバー中（時刻ｔ１〜時刻ｔ２間）のセンシング対象値推定値を逐次計算する。
【００３９】
レンジオーバー開始後すぐに，近似直線Ｌ１（ｔ）を求め，レンジ内復帰するまで同じ直線で近似する。レンジオーバー中はＬ１推定部３１で得られたセンシング対象値推定値を，逐次第２ループメモリ２３に書き込む。この後，この推定値を用いて積算，平滑を行い，表示部１３に表示する。センサ出力値がレンジ内復帰したら，第１ループメモリ２１のデータがそのまま第２ループメモリ２３に逐次コピーされる。このようにすることで，レンジ内復帰を待たずダイナミックレンジを超えている時間中も実時間で推定近似を行うことができる。この方法は，測定対象のモデルが全く不明な場合に適している。
【００４０】
（２）２本直線近似は，図５に示すように，時刻ｔ１直前のセンサ出力値の変化を延長した直線と，時刻ｔ２直後のセンサ出力値の変化を逆方向に延長した直線との２本の直線による近似方法である。この推定方法を実施する場合の概略構成を図６に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をΔｔｂ決定部３２とＬ１Ｌ２推定部３３とが担っている。
【００４１】
次に，図６に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００４２】
レンジオーバー開始時に，Ｌ１Ｌ２推定部３３は，時刻（ｔ１−Δｔａ）および時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。そして，これらのセンサ出力値を用いて，例えば最小自乗法等により第１の近似直線Ｌ１（ｔ）を求める。ここで，時間Δｔａは，（１）１本直線近似の場合のものと同様に，あらかじめ設定された時間である。
【００４３】
レンジ内復帰後に，第２の近似直線Ｌ２（ｔ）を求める。そのために，時刻ｔ２に到達しレンジ内復帰した後，あらかじめ設定した時間Δｔｂ０だけ待つ。時刻ｔ２〜時刻（ｔ２＋Δｔｂ０）間に再度レンジオーバーとなった場合は，再度レンジオーバーとなった時刻ｔ３を記録する。あるいは，時刻（ｔ２＋Δｔｂ０）までに次のレンジオーバーが発生しなければ，時刻（ｔ２＋Δｔｂ０）において，時刻ｔ３＝（ｔ２＋Δｔｂ０）とおく。これらの時刻ｔ１，ｔ２，ｔ３をΔｔｂ決定部３２に送る。Δｔｂ決定部３２は，あらかじめ設定された時間Δｔｂ０と時刻ｔ１，ｔ２，ｔ３とから，新たなΔｔｂを決定する。
【００４４】
次に，Δｔｂ決定部３２におけるΔｔｂおよび第４点のセンサ出力値の決定方法について説明する。まず，ｔ２＋Δｔｂ０≦ｔ３の場合は，あらかじめ設定した時間Δｔｂ０＝Δｔｂとする。この場合は，第４点は時刻（ｔ２＋Δｔｂ）＝（ｔ２＋Δｔｂ０）であり，その時刻（ｔ２＋Δｔｂ）におけるセンサ出力値を第４点のセンサ出力値とする。
【００４５】
一方，ｔ２＋Δｔｂ０＞ｔ３であった場合には，次の２種類の決定法のうちいずれかを使用する。第１の決定法では，時刻ｔ２〜時刻ｔ３間でセンサ出力値がピーク値（最小）となった時刻を第４点の時刻ｔ４とし，Δｔｂ＝ｔ４−ｔ２とする。またその時刻ｔ４におけるセンサ出力値を第４点の時刻（ｔ２＋Δｔｂ）におけるセンサ出力値とする。
【００４６】
第２の決定法では，時刻ｔ２と時刻ｔ３の中間の時刻を第４点の時刻ｔ４とする。すなわち，Δｔｂ＝（ｔ３−ｔ２）／２とする。また，時刻ｔ２〜時刻ｔ３間のセンサ出力値を平均した値のｕ倍を，この第４点の時刻ｔ４＝（ｔ２＋Δｔｂ）におけるセンサ出力値として，センシング対象値推定処理に使用する。ここで，ｕは１０以内の正の数であり，センサ出力波形から決定する。このｕは，主として簡単な自然数比となる。例えば，自然数そのものや，その逆数等である。
【００４７】
なお，第１の決定法は計算量が少なく，容易にΔｔｂおよび第４点のセンサ出力値を得ることができる。一方，第２の決定法は，平均化処理によってΔｔｂおよび第４点のセンサ出力値を得る手法であるので，センサ出力値にノイズが乗り，時刻ｔ２〜時刻ｔ３間のセンサ出力値の変動が大きい場合に効果的である。このように，Δｔｂをあらかじめ設定したものに限らず，時刻ｔ３を考慮して求めることにより，近似する直線をより適切に求めることができる。
【００４８】
以上のように決定したΔｔｂおよび第４点のセンサ出力値を用いて，Ｌ１Ｌ２推定部３３は，第２の近似直線Ｌ２（ｔ）を求める。まず，時刻ｔ２のセンサ出力値を第１ループメモリ２１から読み出す。この値とΔｔｂ決定部３２で決定されたΔｔｂおよび第４点のセンサ出力値を用いて，第２の近似直線Ｌ２（ｔ）を求める。さらに，第１の近似直線Ｌ１（ｔ）と第２の近似直線Ｌ２（ｔ）との交点の時刻ｔｇを求める。これにより，図５に示したように，時刻ｔ１〜時刻ｔｇ間のセンシング対象値を第１の近似直線Ｌ１（ｔ）で，時刻ｔｇ〜時刻ｔ２間のセンシング対象値を第２の近似直線Ｌ２（ｔ）で近似することができる。
【００４９】
なおここでは，第２の近似直線Ｌ２（ｔ）を決定する際に，レンジ内復帰後の時刻ｔ２〜時刻（ｔ２＋Δｔｂ０）間に再度レンジオーバーとなった場合について考慮し，Δｔｂを改めて決定する方法を説明した。一方，第１の近似直線Ｌ１（ｔ）を決定する際にも同様に，レンジオーバー開始前の時刻（ｔ１−Δｔａ）〜時刻ｔ１間にもレンジオーバーしていたという場合もあり得る。従って，第１の近似直線Ｌ１（ｔ）をより精密に推定するためには，Δｔａについても同様に決定し直すことが望ましい。また，（１）１本直線近似の場合にも同様に，Δｔａを決定し直すとさらによい。
【００５０】
Ｌ１Ｌ２推定部３３は，レンジオーバー開始時に近似直線Ｌ１（ｔ）を求め，その後レンジ内復帰までの間，継続的に近似直線Ｌ１（ｔ）によりセンシング対象値推定値を生成していても良い。この場合，レンジオーバー中（時刻ｔ１〜時刻ｔ２）には，一旦近似直線Ｌ１（ｔ）によるセンシング対象値推定値が第２ループメモリ２３に書き込まれる。レンジ内復帰後，時刻（ｔ２＋Δｔｂ）において，Ｌ１Ｌ２推定部３３で得られた時刻ｔ１〜時刻ｔ２間のセンシング対象値推定値により，時刻ｔｇ〜時刻ｔ２間の第２ループメモリ２３のデータは上書きされる。
【００５１】
この後，積算器２４によってセンシング対象値の積算を行う。上記のように第２ループメモリ２３に上書きされることにより，再計算された時刻ｔｇ〜時刻ｔ２間の積算値は急激に変化し，積算結果が不連続になる。この不連続性を平滑器２５によって安定させた後，表示部１３に表示する。
【００５２】
この（２）２本直線近似は，測定対象の慣性が比較的小さい場合に適している。すなわち，レンジオーバーしても比較的早くピークに達し，すぐにレンジ内に復帰するようなセンサであり，小型のロボットの関節に設けられる角速度センサ等である。また，測定対象の共振周期が，オーバーレンジ時間（ｔ２−ｔ１）より小さい場合や，センサのサンプリングタイムが運動の周期に比して長く，時間あたりのサンプリング数を十分得られない場合に適している。
【００５３】
（３）ベジェ曲線近似は，図７に示すように，レンジオーバー中のセンシング対象値の推定値をその前後のセンサ出力値を利用したベジェ曲線Ｌ３（ｔ）で近似したものである。（２）２本直線近似の場合と同様のΔｔａ，Δｔｂを使って，時刻（ｔ１−Δｔａ），時刻ｔ１，時刻ｔ２，時刻（ｔ２＋Δｔｂ）の４点でのセンサ出力値を用いてベジェ曲線Ｌ３（ｔ）を算出する。従って，（２）２本直線近似の場合と同様に，レンジ内復帰後Δｔｂ０だけ経過してから推定開始する。この推定方法を実施する場合の概略構成を図８に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をΔｔｂ決定部３２とベジェ曲線Ｌ３推定部３４とが担っている。
【００５４】
次に，図８に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。また，（２）２本直線近似の場合と同様にΔｔｂ決定部３２によってΔｔｂおよび第４点のセンサ出力値が決定される。
【００５５】
ベジェ曲線Ｌ３推定部３４は，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，時刻（ｔ１−Δｔａ）と時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。また，レンジ内復帰した後，ベジェ曲線Ｌ３推定部３４は，時刻ｔ２に対応するセンサ出力値を第１ループメモリ２１から読み出す。これらと，Δｔｂ決定部３２で決定されたΔｔｂおよび第４点のセンサ出力値との４つのセンサ出力値を用いて，ベジェ曲線Ｌ３推定部３４はベジェ曲線Ｌ３を求める。さらに，求められたベジェ曲線Ｌ３によりセンシング対象値推定値を求め，第２ループメモリ２３に書き込む。この後，積算器２４によってセンシング対象値の積算を行う。さらに，平滑器２５によって平滑化を行い，表示部１３に表示する。また，（２）２本直線近似において記載したように，Δｔａについても同様に決定し直すとさらによい。
【００５６】
なお，ベジェ曲線（ｔ，Ｌ３（ｔ））は，４点のデータＰ1（ｘ1，ｙ1），Ｐ2（ｘ2，ｙ2），Ｐ3（ｘ3，ｙ3），Ｐ4（ｘ4，ｙ4）からｍを０〜１の範囲で移動させることにより次式のように求められる。ここで，ｘ1＝ｔ１−Δｔａ，ｘ2＝ｔ１，ｘ3＝ｔ２，ｘ4＝ｔ２＋Δｔｂであり，ｙ1〜ｙ4は，それらの時刻におけるセンサ出力値である。
ｔ＝（１−ｍ）3・ｘ1＋３（１−ｍ）2・ｍ・ｘ2＋３（１−ｍ）・ｍ2・ｘ3＋ｍ3・ｘ4…（式１）
Ｌ３（ｔ）＝（１−ｍ）3・ｙ1＋３（１−ｍ）2・ｍ・ｙ2＋３（１−ｍ）・ｍ2・ｙ3＋ｍ3・ｙ4…（式２）
ただし，０≦ｍ≦１である。ここで，ｍ＝０のときは点（ｔ，Ｌ３（ｔ））は点Ｐ1（ｘ1，ｙ1）に一致し，ｍ＝１のときは点（ｔ，Ｌ３（ｔ））は点Ｐ4（ｘ4，ｙ4）に一致する。
【００５７】
この方法では，ベジェ曲線が時刻（ｔ２＋Δｔｂ）において初めて決定されることから，時刻ｔ１から時刻（ｔ２＋Δｔｂ）までの間はダイナミックレンジＤ１あるいはＤ２を維持して積分した値を出力する。または，この間は，（１）１本直線近似と同様に，近似直線Ｌ１（ｔ）によってセンシング対象値の推定を行っても良い。
【００５８】
この方法は，推定のための関数を曲線で近似するため，測定対象の慣性が比較的大きく，動きがゆっくりしている場合に適している。また，測定対象の共振周期が，オーバーレンジ時間（ｔ２−ｔ１）より大きいものに適している。さらに，センサのサンプリングタイムが運動の周期に比して短く，時間あたりのサンプリング数を十分得られる場合にも適している。また，ロール角，ピッチ角等の本質的に限界があり必ず復帰するものに適している。なお，ここでは近似の手法としてベジェ曲線を用いたが，他に，２次曲線近似，正弦波近似，スプライン近似等であっても同様の効果が得られる。
【００５９】
（４）減衰曲線近似は，図９に示すように，レンジオーバー開始時刻ｔ１から，（１）１本直線近似と同様に近似直線Ｌ１（ｔ）で近似し，外部情報を受けた後は減衰曲線Ｌ４（ｔ）で近似する近似方法である。外部情報としては，例えば，対応する駆動モータの電流値，電圧値，抵抗値等が利用できる。これらの外部情報によって，駆動モータの回転トルクが消失したと判断できる時刻ｔｓを得る。時刻ｔｓより後では，摩擦力等によって角速度が減衰するため，一般的に徐々に停止に向かう。または，モータトルクと慣性力や抵抗力との組み合わせによってモータの回転がある程度以下に減少したと判断できたとき，その時刻ｔｓを外部情報としてもよい。あるいは，データとして外部からの情報が無くても，センサが取り付けられている測定対象の物理特性等から求めても良い。この推定方法を実施する場合の概略構成を図１０に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能を外部情報入力部３５とＬ１Ｌ４推定部３６とが担っている。
【００６０】
次に，図１０に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００６１】
Ｌ１Ｌ４推定部３６は，レンジオーバー後すぐに，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，時刻（ｔ１−Δｔａ）と時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。そしてこれらのセンサ出力値を用いて，（１）１本直線近似の場合と同様に近似直線Ｌ１（ｔ）を求める。その後，近似直線Ｌ１（ｔ）からセンシング対象値推定値を求め，第２ループメモリ２３に記録する。また，（２）２本直線近似において記載したように，Δｔａを決定し直すとさらによい。
【００６２】
外部情報入力部３５から例えば回転トルクが減少したというようなトリガー信号が入力されると，Ｌ１Ｌ４推定部３６は，そのときの時刻ｔｓを記録する。さらに，時刻ｔｓにおけるセンシング対象値推定値Ｌ１（ｔｓ）を求め，近似減衰曲線Ｌ４（ｔ）を生成する。この近似減衰曲線Ｌ４（ｔ）を用いて，時刻ｔｓ〜時刻ｔ２のセンシング対象値推定値を求め，第２ループメモリ２３に記録する。この後，積算器２４によってセンシング対象値の積算を行う。さらに，平滑器２５によって平滑化を行い，表示部１３に表示する。
【００６３】
減衰曲線Ｌ４（ｔ）としては，例えば指数関数を用いる。
Ｌ４（ｔ）＝ｍａｘ（（Ｌ１（ｔｓ）−β）・ｅ-α(t-ts)＋β，Ｄ２）…（式３）：センサ出力値がＤ２の場合
Ｌ４（ｔ）＝ｍｉｎ（（β−Ｌ１（ｔｓ））・ｅ-α(t-ts)−β，Ｄ１）…（式４）：センサ出力値がＤ１の場合
ただし，αは減衰係数である。
時刻ｔｓでは，ｅの乗数は０であり，Ｌ４（ｔｓ）＝Ｌ１（ｔｓ）となる。時刻ｔｓを超えると，式３は漸近線βを持つ減衰曲線となる。外部トルクが０になれば，運動体の動きは止まる方向に向かうので，漸近線はセンサの零点出力βとなる。ただし，対象モデルによっては，βをセンサの零点出力以外の値とする方が適切な場合もある。
【００６４】
なお，レンジ内復帰する時刻ｔ２以前に，減衰曲線の値がセンサダイナミックレンジの上限値Ｄ２または下限値Ｄ１に達した場合は，その後，式３ではＬ４（ｔ）＝Ｄ２，式４ではＬ４（ｔ）＝Ｄ１を維持する。また，レンジ内復帰時の減衰曲線の値がちょうどレンジ境界値となるように減衰係数αを定めても良い。ただし，この場合には復帰後に推定することになる。またあるいは，レンジ内復帰時に減衰曲線の値がまだレンジ境界値まで戻っていない場合には，このまま推定値とセンサ出力値とをつなぐとこの箇所にギャップができる。そこで，例えば（２）２本直線近似の第２の近似直線Ｌ２（ｔ）を併用して，ギャップをならすようにしても良い。
【００６５】
この方法は，例えば，モータが停止すると所定の位置に復帰していくような，モデルが分かっているものに適している。また，外部情報としては，モータトルクのみでなく，傾斜角センサ等の他の検出器の出力値や，通信等による外部からの信号によっても良い。また，この方法が選択されていても，外部情報が入力される以前にレンジ内に復帰した場合は，上記の（１）〜（３）のいすれかの方法を用いて推定する。
【００６６】
次に，図１１〜図１５のフローチャートを参照して，この積算値推定装置２０による処理を説明する。まず，図１１のメイン処理について説明する。この処理は二足歩行ロボット１が動作されている間，繰り返し実行される。この処理が実行されるとまず，各時刻ｔにおける角速度センサ１１のセンサ出力値を取得する（Ｓ１０１）。さらに，取得したセンサ出力値を第１ループメモリ２１に蓄積する（Ｓ１０２）。
【００６７】
次に，そのセンサ出力値から，レンジオーバー開始／レンジ内復帰判定部２７により，角速度センサ１１がレンジオーバー状態であるかどうかを判断する（Ｓ１０３）。レンジオーバー状態であると判断された場合は（Ｓ１０３：Ｙｅｓ），推定部２２によりセンシング対象値推定処理を行う（Ｓ１０４）。この処理については後述する。
【００６８】
あるいは，レンジオーバー状態でない場合は（Ｓ１０３：Ｎｏ），そのまま第２ループメモリ２３に記録する（Ｓ１０５）。さらに，積算器２４によってセンサ出力値を積算する（Ｓ１０６）。さらに，積算結果を平滑器２５によって平滑化する（Ｓ１０７）。そして，求められた積算値を表示部１３に表示し（Ｓ１０８），この処理を終了する。
【００６９】
次に，Ｓ１０４で行われるセンシング対象値推定処理を図１２〜図１５の４通りについて説明する。これらはそれぞれ，図１２に示す第１の方法が上記の（１）１本直線近似に相当する。また，図１３に示す第２の方法が上記の（２）２本直線近似に相当する。また，図１４に示す第３の方法が上記の（３）ベジェ曲線近似に相当する。また，図１５に示す第４の方法が上記の（４）減衰曲線近似に相当する。二足歩行ロボット１の性能やセンサの種類等に応じて，あらかじめこれらの処理のうちから１つが選択されている。また，これらはいずれも，推定部２２のセンシング対象値推定部２８によって実行される。
【００７０】
次に，第１の方法について図１２に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ２０１）。そして，レンジオーバー前のデータを基に，近似直線Ｌ１（ｔ）を求める（Ｓ２０２）。そして，そのＬ１（ｔ）を用いてその時点でのセンシング対象値を推定する（Ｓ２０３）。この場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７１】
次に，第２の方法について図１３に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ３０１）。次に，レンジ内復帰判定部２７によってセンサ出力値がレンジ内に復帰したかどうかが判断される（Ｓ３０２）。復帰していない場合は（Ｓ３０２：Ｎｏ），レンジオーバー前のデータを基に，第１の近似直線Ｌ１（ｔ）を求める（Ｓ３０３）。そして，そのＬ１（ｔ）を用いてその時点でのセンシング対象値を推定する（Ｓ３０４）。この場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７２】
あるいは，センサ出力値がレンジ内復帰した場合は（Ｓ３０２：Ｙｅｓ），その復帰時刻ｔ２を取得する（Ｓ３０５）。さらに，あらかじめ設定した時間Δｔｂ０だけ時間が経過するのを待ち，新たな時間Δｔｂを決定する（Ｓ３０６）。得られた時間Δｔｂおよび第４点のセンサ出力値と，復帰時刻ｔ２でのセンサ出力値とから，第２の近似直線Ｌ２（ｔ）を求める（Ｓ３０７）。
【００７３】
さらに，第１の近似直線Ｌ１（ｔ）と第２の近似直線Ｌ２（ｔ）との接合点ｔｇを求める（Ｓ３０８）。次に，第２の近似直線Ｌ２（ｔ）を用いて時刻ｔｇ〜ｔ２間のセンシング対象値を推定する。そして，この第２の推定センシング対象値を，先に第１の近似直線Ｌ１（ｔ）を用いて求めた推定センシング対象値（Ｓ３０４の結果）の時刻ｔｇ〜ｔ２間のデータと置き換える（Ｓ３０９）。これで，この処理を終了してメイン処理に戻る。
【００７４】
次に，第３の方法について図１４に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ４０１）。次に，レンジ内復帰判定部２７によってセンサ出力値がレンジ内に復帰したかどうかが判断される（Ｓ４０２）。復帰していない場合は何もせず，メイン処理に戻る。この方法では，復帰後にセンシング対象値の推定を行うので，復帰するまではメイン処理中のセンシング対象値推定処理（Ｓ１０４）以降の処理は行わない。
【００７５】
あるいは，センサ出力値がレンジ内復帰した場合は（Ｓ４０２：Ｙｅｓ），その復帰時刻ｔ２を取得する（Ｓ４０３）。さらに，あらかじめ設定した時間Δｔｂ０だけ時間が経過するのを待ち，新たな時間Δｔｂを決定する（Ｓ４０４）。復帰時刻ｔ２と時刻（ｔ２＋Δｔｂ）との２点のセンサ出力値を第１ループメモリ２１から読み出す（Ｓ４０５）。あるいは，新たに決定された第４点のセンサ出力値を時刻（ｔ２＋Δｔｂ）におけるセンサ出力値に代えて使用する。また，レンジオーバー開始時刻ｔ１でのセンサ出力値とその時間Δｔａ前でのセンサ出力値とを第１ループメモリ２１から読み出す（Ｓ４０６）。Ｓ４０５とＳ４０６の実行順序は逆でも良い。
【００７６】
さらに，得られた４点のセンサ出力値から，ベジェ曲線Ｌ３（ｔ）を作成する（Ｓ４０７）。さらに，Ｓ４０７で求めたベジェ曲線Ｌ３（ｔ）を用いて時刻ｔ１〜ｔ２間のセンシング対象値を推定する（Ｓ４０８）。これでこの処理を終了してメイン処理に戻る。さらに，Ｓ４０８で得られた推定値を用いて，メイン処理中のレンジオーバー中には停止していた部分を算出実行し，表示する。なお，この方法でも，実時間では第２の方法等で推定しておき，レンジ内復帰後改めて推定し直して，推定センシング対象値を置き換えるようにしても良い。
【００７７】
次に，第４の方法について図１５に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ５０１）。レンジオーバー前のデータを基に，近似直線Ｌ１（ｔ）を求める（Ｓ５０２）。次に，外部情報が入力されたかどうかが判断される（Ｓ５０３）。入力されていない場合は（Ｓ５０３：Ｎｏ），Ｓ５０２で求めた近似直線Ｌ１（ｔ）を用いて，その時点でのセンシング対象値を推定する（Ｓ５０４）。このままレンジ内復帰した場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７８】
あるいは，モータ停止等の外部情報が入力されたら（Ｓ５０３：Ｙｅｓ），その外部情報の入力開始時刻ｔｓを取得する。さらに，その時刻ｔｓにおける推定センシング対象値Ｌ１（ｔｓ）を取得する（Ｓ５０５）。そしてそのＬ１（ｔｓ）を起点とする減衰曲線Ｌ４（ｔ）を作成し，それによってセンシング対象値を推定する（Ｓ５０６）。この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。なお，減衰曲線Ｌ４（ｔ）による推定値が，レンジ内復帰時刻ｔ２より前にレンジ上限値Ｄ２または下限値Ｄ１に到達した場合は，その後の推定値はその上限値Ｄ２または下限値Ｄ１で固定する。
【００７９】
以上詳細に説明したように，本形態の二足歩行ロボット１によれば，積算値推定装置２０が推定部２２を有しているので，センサへダイナミックレンジを超えた入力があった場合でも，センシング対象値を推定することができる。さらに，推定部２２は，レンジオーバー開始／レンジ内復帰判定部２７，センシング対象値推定部２８を有し，センシング対象値推定部２８ではその推定方法を（１）１本直線近似，（２）２本直線近似，（３）ベジェ曲線近似，（４）減衰曲線近似等から使い分ける。従って，そのセンサの取り付けられている部材のモデルに応じて，適切に推定される。これにより，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることができる。また，エンコーダを用いず，回転角速度の累積から角度を得ることができるので，ロボットの骨格に当たるリンクがたわんでも正しい角度が得られるというメリットがある。
【００８０】
なお，本形態は単なる例示にすぎず，本発明を何ら限定するものではない。したがって本発明は当然に，その要旨を逸脱しない範囲内で種々の改良，変形が可能である。
例えば，上記の実施の形態では，（２）２本直線近似において，予測関数や逆算関数として直線を選択しているが，これらは曲線近似としてもよい。
また例えば，本形態では二足歩行ロボット１の肘関節に設けられた角速度センサ１１のセンサ出力値を対象としたものとして記載しているが，センサの種類やそのセンサが設置される対象等は，どのようなものでも良い。また，本形態では積算器を用いてセンサ出力値を積算することにより肘角度を求めるとしているが，積算の必要のないセンサでは，この積算処理を省略すればよい。
【図面の簡単な説明】
【００８１】
【図１】本形態の二足歩行ロボットのスケルトンモデルを示す概略構成図である。
【図２】本形態の積算値推定装置の概略構成を示すブロック図である。
【図３】１本直線近似によるセンシング対象値推定方法を示す説明図である。
【図４】１本直線近似による積算値推定装置の概略構成を示すブロック図である。
【図５】２本直線近似によるセンシング対象値推定方法を示す説明図である。
【図６】２本直線近似による積算値推定装置の概略構成を示すブロック図である。
【図７】ベジェ曲線近似によるセンシング対象値推定方法を示す説明図である。
【図８】ベジェ曲線近似による積算値推定装置の概略構成を示すブロック図である。
【図９】減衰曲線近似によるセンシング対象値推定方法を示す説明図である。
【図１０】減衰曲線近似による積算値推定装置の概略構成を示すブロック図である。
【図１１】センシング対象値推定方法のメイン処理を示すフローチャート図である。
【図１２】第１のセンシング対象値推定方法を示すフローチャート図である。
【図１３】第２のセンシング対象値推定方法を示すフローチャート図である。
【図１４】第３のセンシング対象値推定方法を示すフローチャート図である。
【図１５】第４のセンシング対象値推定方法を示すフローチャート図である。
【符号の説明】
【００８２】
１１ 加速度センサ
２０ 積算値推定装置
２１ 第１ループメモリ
２２ 対象値推定部
２３ 第２ループメモリ
２４ 積算器
２７ レンジオーバー開始／レンジ内復帰判定部
２８ センシング対象値推定部
【技術分野】
【０００１】
本発明は，例えばロボットの腕や足等に設置された角速度センサや加速度センサ等のセンサの出力値からそのセンシング対象値を推定するセンシング対象値推定装置およびその推定方法に関する。さらに詳細には，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定するセンシング対象値推定装置およびその推定方法に関するものである。
【背景技術】
【０００２】
従来より，ロボットを指示に従って駆動させるために，ロボットの各部には各種のセンサが取り付けられている。例えば，肩部や肘部等の関節には角速度センサが設けられ，関節の回転角速度を累積して，上腕や下腕の角度やその先端部の位置等が求められている。むろん，このような例はロボットにも限らないし，角速度にも限らない。このようなセンサには，一般に，それぞれ所定の検出可能範囲（ダイナミックレンジ）があり，その範囲を超えた入力を受けた場合には，正確に検出することはできない。すなわち，入力がダイナミックレンジを超えている場合には，一般に，ダイナミックレンジの上限値または下限値が出力されるようになっている。そして，入力が再びダイナミックレンジ内に入れば，センサの出力値も正しく出力される。
【０００３】
これに対して，特許文献１には，地磁気方位センサを用いて車両の走行方位を検出する車両用方位計において，外乱により地磁気方位センサへの入力がそのダイナミックレンジを超えたときの処理方法について開示されている。この文献の技術によれば，外乱によりセンサへの入力がダイナミックレンジを超えている時間の長さを基に，外乱の大きさとその発生時点とを予測できるとされている。
【特許文献１】特開平２−２１８９１１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら，前記した従来の技術は，地磁気を利用して水平面内での車両の向きを測定する車両用方位計に特化された技術である。一般に，磁気外乱によってセンサ出力値は大きく変化するが，車両の姿勢自体は大きく変化したわけではない。また外乱の大きさによっては，車両の着磁という後遺症が発生するおそれがあるため，その外乱の大きさを取得することが重要となっている。
【０００５】
これに対して，例えばロボットの腕部等の場合では，他の物体への衝突や足部の振動等による外乱は，腕の姿勢自体を変化させてしまう。特に，センサの出力値を積算して角度や位置を求めるもののように，履歴に応じた信号処理を持つ処理系では，ダイナミックレンジの上限値あるいは下限値となっているセンサ出力値をそのまま利用して計算を続けることは，誤差がますます蓄積することになるので好ましくない。
【０００６】
また，ダイナミックレンジの大きいセンサを選択するという方法も考えられるが，一般にダイナミックレンジの大きいセンサは，分解能が低いという問題点がある。ダイナミックレンジの大きさと分解能とをともに満たすセンサは無く，あったとしてもごく高価で複雑な構造のものであって，一般的ではない。さらに，ダイナミックレンジの異なる複数のセンサを備えてそれらを使い分けることもできるが，センサの個数が増加するためコスト高となり，また制御も煩雑なものとなる。
【０００７】
本発明は，前記した従来のセンシング対象値推定装置およびその推定方法が有する問題点を解決するためになされたものである。すなわちその課題とするところは，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることのできるセンシング対象値推定装置およびその推定方法を提供することにある。
【課題を解決するための手段】
【０００８】
この課題の解決を目的としてなされた本発明のセンシング対象値推定装置は，センサ出力値の変遷を記録する第１メモリと，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出するレンジ検出部と，レンジ検出部により検出された逸脱から復帰までの間について，第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定する推定部とを有するものである。
【０００９】
本発明のセンシング対象値推定装置では，センサ出力値の変遷が第１メモリに記録され，センサ出力値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰がレンジ検出部で検出される。さらに，検出された逸脱から復帰までの間のセンシング対象値が推定部で推定されるので，センシング対象値がセンサのダイナミックレンジを超えていても，推定部によって適切に推定される。従って，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることのできるセンシング対象値推定装置となっている。
【００１０】
さらに本発明では，推定部は，復帰後に，第１メモリに記録されている逸脱前と復帰後とのセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するものであることが望ましい。またさらに，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部を有し，予測部で決定した予測関数によりセンシング対象値の変遷を推定するものであることが望ましい。
【００１１】
さらに本発明では，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，予測部で決定した予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定する減衰関数決定部とを有し，外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については減衰関数決定部が決定した減衰関数によりセンシング対象値の変遷を推定するものであることが望ましい。
センシング対象値を変化させる要因の一部が外部装置にある場合，その外部装置から外部情報を受けることによりセンシング対象値の変遷をある程度推定できる。例えば，モータ等によって回転されているセンシング対象では，モータを停止したという外部情報に基づいて，その後のセンシング対象値を減衰関数で推定すれば，推定精度を向上させることができる。
【００１２】
さらに本発明では，推定部は，逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定する逆算部とを有し，予測部で決定した予測関数と逆算部が決定した逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定するものであることが望ましい。
このようにすれば，逸脱前と復帰後のそれぞれのセンサ出力値の変遷に基づいて，それぞれ逸脱後と復帰前の適切な推定値が選択されるので，より推定精度を向上させることができる。
【００１３】
さらに本発明では，推定部は，逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定するベジェ関数決定部を有し，ベジェ関数決定部が決定したベジェ関数によりセンシング対象値の変遷を推定するものであることが望ましい。
このようにすれば，逸脱前と復帰後との両方のセンサ出力値の変遷に基づいた関数が使用されているので，不自然に急激な変化が無い推定値が得られる。
【００１４】
さらに本発明では，予測部は，センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて予測関数を決定するとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，予測部における予測関数の決定が，センシング対象値のダイナミックレンジ内からの逸脱時点とそこから所定時間さかのぼった時点とのセンサ出力値に基づいて行われる。さらに，センシング対象値のダイナミックレンジ内からの逸脱時点から所定時間さかのぼった時点までの間にさらにダイナミックレンジ内からの逸脱があった場合には，逸脱していない範囲のセンサ出力値の波形に基づいて決定される。従って，複数回の逸脱が断続的に発生したとしても，適切な予測関数が得られる。
【００１５】
さらに本発明では，逆算部は，センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて逆算関数を決定するとともに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，逆算部における逆算関数の決定が，センシング対象値のダイナミックレンジ内への復帰時点とそこから所定時間経過した時点とのセンサ出力値に基づいて行われる。さらに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，逸脱していない範囲のセンサ出力値の波形に基づいて決定される。従って，複数回の逸脱が断続的に発生したとしても，適切な逆算関数が得られる。
【００１６】
さらに本発明では，ベジェ関数決定部は，センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいてベジェ関数を決定するとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることが望ましい。
このようにすれば，複数回の逸脱が断続的に発生したとしても，適切なベジェ関数が得られる。
【００１７】
さらに本発明では，センシング対象値のダイナミックレンジ内からの逸脱前とダイナミックレンジ内への復帰後についてはセンサ出力値を記録し，逸脱から復帰までの間については推定部による推定値を記録する第２メモリと，第２メモリに記録された値の積算値を求める積算部とを有することが望ましい。
このようにすれば，第２メモリには，センシング対象値がダイナミックレンジ内ではセンサ出力値が，ダイナミックレンジ外では推定部による推定値が記録される。この記録された値が積算部で積算されるので，より適切な積算値が得られる。
【００１８】
また，本発明は，センサ出力値の変遷を第１メモリに記録し，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定し，検出された逸脱から復帰までの間について，決定された予測関数によりセンシング対象値の変遷を推定するセンシング対象値推定方法にも及ぶ。
【００１９】
さらに本発明のセンシング対象値推定方法では，決定された予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定し，外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については決定された減衰関数によりセンシング対象値の変遷を推定することが望ましい。
【００２０】
さらに本発明のセンシング対象値推定方法では，第１メモリに記録されている復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定し，決定された予測関数と決定された逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定することが望ましい。
【００２１】
また，本発明は，センサ出力値の変遷を第１メモリに記録し，センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，第１メモリに記録されている逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定し，検出された逸脱から復帰までの間について，決定されたベジェ関数によりセンシング対象値の変遷を推定するセンシング対象値推定方法にも及ぶ。
【００２２】
また，本発明のセンシング対象値推定方法は，予測関数の決定を，センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて行うとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【００２３】
また，本発明のセンシング対象値推定方法は，逆算関数の決定を，センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【００２４】
また，本発明のセンシング対象値推定方法は，ベジェ関数の決定を，センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることが望ましい。
【発明の効果】
【００２５】
本発明のセンシング対象値推定装置およびその推定方法によれば，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることができる。
【発明を実施するための最良の形態】
【００２６】
以下，本発明を具体化した最良の形態について，添付図面を参照しつつ詳細に説明する。本形態は，二足歩行ロボットに設けられ，腕部の回転角速度を測定する角速度センサと，そのダイナミックレンジを超えた入力があった場合でもセンシング対象値（ここでは，腕部の角速度）を推定する推定装置に本発明を適用したものである。
【００２７】
本形態の二足歩行ロボット１は，そのスケルトンモデルを図１に示すように，肩部・肘部等の関節Ａ〜関節Ｑまでの計１７箇所の関節部を有している。さらに，各関節Ａ〜Ｑには，それぞれ駆動用のモータと角速度等を検出するセンサとを有している。例えば，右手の肘部に相当する関節Ｐには角速度センサ１１を有し，これにより右肘関節Ｐの回転角速度が検出されている。
【００２８】
また，この二足歩行ロボット１では，この角速度センサ１１の出力値を積算することにより，上腕に対する下腕の角度を求めることができる。同様に肩関節等の各自由度についても，各角速度センサの出力値を積算して，その基部に対する角度が求められる。これらを合わせることにより，各部の位置が得られる。例えば，本体部分に対する右手先の位置関係は，右肩関節（Ｏ，Ｎ）の角度と右肘関節Ｐの角度等を合わせて算出できる。
【００２９】
従って，本形態の二足歩行ロボット１では，このようなセンサ出力値を順次積算した積算値を求めている。ここで，センサにそのダイナミックレンジを超えた入力が印加された場合は，センサ出力値はダイナミックレンジの上限値あるいは下限値に保持される。このため，そのセンサ出力値をそのまま積算すると，例えば手先位置等の算出結果の誤差が大きいものとなってしまう。そこで本形態では，このような場合にはセンサ出力値をそのまま積算するのでなく，その間のセンシング対象値を推定してその推定値を積算する。
【００３０】
次に，本形態の二足歩行ロボット１において，各センサ出力値あるいはセンシング対象値の推定値の積算値を求めるための積算値推定装置２０の概略構成を，図２のブロック図を参照して説明する。本形態の積算値推定装置２０は，図２に示すように，角速度センサ１１からそのセンサ出力値の入力を受けるとともに，タイマ１２から時刻の入力を受け，算出した積算値を表示部１３へ表示するものである。このタイマ１２や表示部１３は，二足歩行ロボット１に備えられていても良いし，別に備えて情報の送受信のみを行うようにしても良い。
【００３１】
また，積算値推定装置２０は，図２に示すように，第１ループメモリ２１，推定部２２，第２ループメモリ２３，積算器２４，平滑器２５を有している。また，推定部２２は，レンジオーバー開始／レンジ内復帰判定部２７，センシング対象値推定部２８を有している。ここで，推定部２２は，角速度センサ１１がレンジオーバー状態である場合に，センシング対象値である角速度を推定する部分である。ここで，レンジオーバー開始／レンジ内復帰判定部２７がレンジ検出部に相当し，センシング対象値推定部２８は予測部，減衰関数決定部，逆算部，ベジェ関数決定部を含んでいる。
【００３２】
この積算値推定装置２０では，第１ループメモリ２１は，角速度センサ１１の出力値の入力を受け，そのまま蓄積する。また，推定部２２は，第１ループメモリ２１に蓄積されたセンサ出力値を受けて，まず，レンジオーバー開始／レンジ内復帰判定部２７によって角速度センサ１１がレンジオーバーしているかどうかを判定する。レンジオーバーしていないと判定されたときはセンサ出力値をそのまま第２ループメモリ２３に書き込む。この場合は，そのままシフトレジスタ等の積算器２４を通して積算値が算出される。さらに，平滑器２５によって平滑化し，その結果を表示部１３へ出力して表示させる。
【００３３】
あるいは，レンジオーバー開始／レンジ内復帰判定部２７によって，角速度センサ１１がレンジオーバーしたと判定された場合には，タイマ１２からそのときの時刻ｔ１を取得する。また積算器２４による積算を停止し，とりあえず一定値を出力する。この区間の積算値については，推定したセンシング対象値を用いて後で修正する。また，レンジオーバー開始／レンジ内復帰判定部２７によって，角速度センサ１１のセンサ出力値がレンジ内に復帰したかどうかを判定する。復帰したと判断されたときは，タイマ１２によってその時刻ｔ２を取得する。さらに，センシング対象値推定部２８によって時刻ｔ１〜ｔ２間の角速度を推定する。この推定方法については後述する。
【００３４】
そして，センシング対象値推定部２８によって推定されたセンシング対象値が第２ループメモリ２３に入力される。以後は，レンジオーバーしていないときと同様に，積算器２４によって第２ループメモリ２３の値を積算する。また，平滑器２５は，レンジオーバーによる積算値の変化が滑らかになるように，例えば移動平均やローパスフィルタによる平滑化を行う。さらにその結果を表示部１３へ出力して表示させる。
【００３５】
次に，センシング対象値推定部２８による推定方法について説明する。推定方法としては，各種の条件に応じて（１）１本直線近似（図３参照），（２）２本直線近似（図５参照），（３）ベジェ曲線近似（図７参照），（４）減衰曲線近似（図９参照）等から使い分ける。これらの図では，角速度センサ１１のダイナミックレンジｄをＤ１≦ｄ≦Ｄ２とし，レンジオーバー開始時刻ｔ１からレンジ内復帰時刻ｔ２までのセンシング対象値を推定している。図中太線で表示しているのは角速度センサ１１の出力値である。なおここでは，レンジオーバー直前とレンジ復帰直後のセンサ出力値が直線的に変化しているように図示したが，このようなものに限らない。
【００３６】
（１）１本直線近似は，図３に示すように，レンジオーバー開始時刻ｔ１からレンジ内復帰時刻ｔ２までのセンシング対象値を１本の直線Ｌ１（ｔ）によって近似する方法である。すなわち，時刻（ｔ１−Δｔａ）〜時刻ｔ１間のセンサ出力値を用いて，例えば最小自乗法等により求めた近似直線Ｌ１（ｔ）によって近似する。この推定方法を実施する場合の概略構成を図４に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をＬ１推定部３１が担っている。
【００３７】
次に，図４に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００３８】
センサ出力値がレンジオーバーとなった時刻ｔ１において，Ｌ１推定部３１は，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，第１ループメモリ２１から時刻（ｔ１−Δｔａ）および時刻ｔ１に対応するセンサ出力値を読み出す。これらのセンサ出力値から近似直線Ｌ１（ｔ）を求める。ここで，Δｔａは，角速度センサ１１のサンプリングタイムの定数倍，例えば１〜１００倍程度の所定時間とする。さらに，Ｌ１推定部３１では，近似直線Ｌ１（ｔ）を用いて，レンジオーバー中（時刻ｔ１〜時刻ｔ２間）のセンシング対象値推定値を逐次計算する。
【００３９】
レンジオーバー開始後すぐに，近似直線Ｌ１（ｔ）を求め，レンジ内復帰するまで同じ直線で近似する。レンジオーバー中はＬ１推定部３１で得られたセンシング対象値推定値を，逐次第２ループメモリ２３に書き込む。この後，この推定値を用いて積算，平滑を行い，表示部１３に表示する。センサ出力値がレンジ内復帰したら，第１ループメモリ２１のデータがそのまま第２ループメモリ２３に逐次コピーされる。このようにすることで，レンジ内復帰を待たずダイナミックレンジを超えている時間中も実時間で推定近似を行うことができる。この方法は，測定対象のモデルが全く不明な場合に適している。
【００４０】
（２）２本直線近似は，図５に示すように，時刻ｔ１直前のセンサ出力値の変化を延長した直線と，時刻ｔ２直後のセンサ出力値の変化を逆方向に延長した直線との２本の直線による近似方法である。この推定方法を実施する場合の概略構成を図６に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をΔｔｂ決定部３２とＬ１Ｌ２推定部３３とが担っている。
【００４１】
次に，図６に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００４２】
レンジオーバー開始時に，Ｌ１Ｌ２推定部３３は，時刻（ｔ１−Δｔａ）および時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。そして，これらのセンサ出力値を用いて，例えば最小自乗法等により第１の近似直線Ｌ１（ｔ）を求める。ここで，時間Δｔａは，（１）１本直線近似の場合のものと同様に，あらかじめ設定された時間である。
【００４３】
レンジ内復帰後に，第２の近似直線Ｌ２（ｔ）を求める。そのために，時刻ｔ２に到達しレンジ内復帰した後，あらかじめ設定した時間Δｔｂ０だけ待つ。時刻ｔ２〜時刻（ｔ２＋Δｔｂ０）間に再度レンジオーバーとなった場合は，再度レンジオーバーとなった時刻ｔ３を記録する。あるいは，時刻（ｔ２＋Δｔｂ０）までに次のレンジオーバーが発生しなければ，時刻（ｔ２＋Δｔｂ０）において，時刻ｔ３＝（ｔ２＋Δｔｂ０）とおく。これらの時刻ｔ１，ｔ２，ｔ３をΔｔｂ決定部３２に送る。Δｔｂ決定部３２は，あらかじめ設定された時間Δｔｂ０と時刻ｔ１，ｔ２，ｔ３とから，新たなΔｔｂを決定する。
【００４４】
次に，Δｔｂ決定部３２におけるΔｔｂおよび第４点のセンサ出力値の決定方法について説明する。まず，ｔ２＋Δｔｂ０≦ｔ３の場合は，あらかじめ設定した時間Δｔｂ０＝Δｔｂとする。この場合は，第４点は時刻（ｔ２＋Δｔｂ）＝（ｔ２＋Δｔｂ０）であり，その時刻（ｔ２＋Δｔｂ）におけるセンサ出力値を第４点のセンサ出力値とする。
【００４５】
一方，ｔ２＋Δｔｂ０＞ｔ３であった場合には，次の２種類の決定法のうちいずれかを使用する。第１の決定法では，時刻ｔ２〜時刻ｔ３間でセンサ出力値がピーク値（最小）となった時刻を第４点の時刻ｔ４とし，Δｔｂ＝ｔ４−ｔ２とする。またその時刻ｔ４におけるセンサ出力値を第４点の時刻（ｔ２＋Δｔｂ）におけるセンサ出力値とする。
【００４６】
第２の決定法では，時刻ｔ２と時刻ｔ３の中間の時刻を第４点の時刻ｔ４とする。すなわち，Δｔｂ＝（ｔ３−ｔ２）／２とする。また，時刻ｔ２〜時刻ｔ３間のセンサ出力値を平均した値のｕ倍を，この第４点の時刻ｔ４＝（ｔ２＋Δｔｂ）におけるセンサ出力値として，センシング対象値推定処理に使用する。ここで，ｕは１０以内の正の数であり，センサ出力波形から決定する。このｕは，主として簡単な自然数比となる。例えば，自然数そのものや，その逆数等である。
【００４７】
なお，第１の決定法は計算量が少なく，容易にΔｔｂおよび第４点のセンサ出力値を得ることができる。一方，第２の決定法は，平均化処理によってΔｔｂおよび第４点のセンサ出力値を得る手法であるので，センサ出力値にノイズが乗り，時刻ｔ２〜時刻ｔ３間のセンサ出力値の変動が大きい場合に効果的である。このように，Δｔｂをあらかじめ設定したものに限らず，時刻ｔ３を考慮して求めることにより，近似する直線をより適切に求めることができる。
【００４８】
以上のように決定したΔｔｂおよび第４点のセンサ出力値を用いて，Ｌ１Ｌ２推定部３３は，第２の近似直線Ｌ２（ｔ）を求める。まず，時刻ｔ２のセンサ出力値を第１ループメモリ２１から読み出す。この値とΔｔｂ決定部３２で決定されたΔｔｂおよび第４点のセンサ出力値を用いて，第２の近似直線Ｌ２（ｔ）を求める。さらに，第１の近似直線Ｌ１（ｔ）と第２の近似直線Ｌ２（ｔ）との交点の時刻ｔｇを求める。これにより，図５に示したように，時刻ｔ１〜時刻ｔｇ間のセンシング対象値を第１の近似直線Ｌ１（ｔ）で，時刻ｔｇ〜時刻ｔ２間のセンシング対象値を第２の近似直線Ｌ２（ｔ）で近似することができる。
【００４９】
なおここでは，第２の近似直線Ｌ２（ｔ）を決定する際に，レンジ内復帰後の時刻ｔ２〜時刻（ｔ２＋Δｔｂ０）間に再度レンジオーバーとなった場合について考慮し，Δｔｂを改めて決定する方法を説明した。一方，第１の近似直線Ｌ１（ｔ）を決定する際にも同様に，レンジオーバー開始前の時刻（ｔ１−Δｔａ）〜時刻ｔ１間にもレンジオーバーしていたという場合もあり得る。従って，第１の近似直線Ｌ１（ｔ）をより精密に推定するためには，Δｔａについても同様に決定し直すことが望ましい。また，（１）１本直線近似の場合にも同様に，Δｔａを決定し直すとさらによい。
【００５０】
Ｌ１Ｌ２推定部３３は，レンジオーバー開始時に近似直線Ｌ１（ｔ）を求め，その後レンジ内復帰までの間，継続的に近似直線Ｌ１（ｔ）によりセンシング対象値推定値を生成していても良い。この場合，レンジオーバー中（時刻ｔ１〜時刻ｔ２）には，一旦近似直線Ｌ１（ｔ）によるセンシング対象値推定値が第２ループメモリ２３に書き込まれる。レンジ内復帰後，時刻（ｔ２＋Δｔｂ）において，Ｌ１Ｌ２推定部３３で得られた時刻ｔ１〜時刻ｔ２間のセンシング対象値推定値により，時刻ｔｇ〜時刻ｔ２間の第２ループメモリ２３のデータは上書きされる。
【００５１】
この後，積算器２４によってセンシング対象値の積算を行う。上記のように第２ループメモリ２３に上書きされることにより，再計算された時刻ｔｇ〜時刻ｔ２間の積算値は急激に変化し，積算結果が不連続になる。この不連続性を平滑器２５によって安定させた後，表示部１３に表示する。
【００５２】
この（２）２本直線近似は，測定対象の慣性が比較的小さい場合に適している。すなわち，レンジオーバーしても比較的早くピークに達し，すぐにレンジ内に復帰するようなセンサであり，小型のロボットの関節に設けられる角速度センサ等である。また，測定対象の共振周期が，オーバーレンジ時間（ｔ２−ｔ１）より小さい場合や，センサのサンプリングタイムが運動の周期に比して長く，時間あたりのサンプリング数を十分得られない場合に適している。
【００５３】
（３）ベジェ曲線近似は，図７に示すように，レンジオーバー中のセンシング対象値の推定値をその前後のセンサ出力値を利用したベジェ曲線Ｌ３（ｔ）で近似したものである。（２）２本直線近似の場合と同様のΔｔａ，Δｔｂを使って，時刻（ｔ１−Δｔａ），時刻ｔ１，時刻ｔ２，時刻（ｔ２＋Δｔｂ）の４点でのセンサ出力値を用いてベジェ曲線Ｌ３（ｔ）を算出する。従って，（２）２本直線近似の場合と同様に，レンジ内復帰後Δｔｂ０だけ経過してから推定開始する。この推定方法を実施する場合の概略構成を図８に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能をΔｔｂ決定部３２とベジェ曲線Ｌ３推定部３４とが担っている。
【００５４】
次に，図８に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。また，（２）２本直線近似の場合と同様にΔｔｂ決定部３２によってΔｔｂおよび第４点のセンサ出力値が決定される。
【００５５】
ベジェ曲線Ｌ３推定部３４は，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，時刻（ｔ１−Δｔａ）と時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。また，レンジ内復帰した後，ベジェ曲線Ｌ３推定部３４は，時刻ｔ２に対応するセンサ出力値を第１ループメモリ２１から読み出す。これらと，Δｔｂ決定部３２で決定されたΔｔｂおよび第４点のセンサ出力値との４つのセンサ出力値を用いて，ベジェ曲線Ｌ３推定部３４はベジェ曲線Ｌ３を求める。さらに，求められたベジェ曲線Ｌ３によりセンシング対象値推定値を求め，第２ループメモリ２３に書き込む。この後，積算器２４によってセンシング対象値の積算を行う。さらに，平滑器２５によって平滑化を行い，表示部１３に表示する。また，（２）２本直線近似において記載したように，Δｔａについても同様に決定し直すとさらによい。
【００５６】
なお，ベジェ曲線（ｔ，Ｌ３（ｔ））は，４点のデータＰ1（ｘ1，ｙ1），Ｐ2（ｘ2，ｙ2），Ｐ3（ｘ3，ｙ3），Ｐ4（ｘ4，ｙ4）からｍを０〜１の範囲で移動させることにより次式のように求められる。ここで，ｘ1＝ｔ１−Δｔａ，ｘ2＝ｔ１，ｘ3＝ｔ２，ｘ4＝ｔ２＋Δｔｂであり，ｙ1〜ｙ4は，それらの時刻におけるセンサ出力値である。
ｔ＝（１−ｍ）3・ｘ1＋３（１−ｍ）2・ｍ・ｘ2＋３（１−ｍ）・ｍ2・ｘ3＋ｍ3・ｘ4…（式１）
Ｌ３（ｔ）＝（１−ｍ）3・ｙ1＋３（１−ｍ）2・ｍ・ｙ2＋３（１−ｍ）・ｍ2・ｙ3＋ｍ3・ｙ4…（式２）
ただし，０≦ｍ≦１である。ここで，ｍ＝０のときは点（ｔ，Ｌ３（ｔ））は点Ｐ1（ｘ1，ｙ1）に一致し，ｍ＝１のときは点（ｔ，Ｌ３（ｔ））は点Ｐ4（ｘ4，ｙ4）に一致する。
【００５７】
この方法では，ベジェ曲線が時刻（ｔ２＋Δｔｂ）において初めて決定されることから，時刻ｔ１から時刻（ｔ２＋Δｔｂ）までの間はダイナミックレンジＤ１あるいはＤ２を維持して積分した値を出力する。または，この間は，（１）１本直線近似と同様に，近似直線Ｌ１（ｔ）によってセンシング対象値の推定を行っても良い。
【００５８】
この方法は，推定のための関数を曲線で近似するため，測定対象の慣性が比較的大きく，動きがゆっくりしている場合に適している。また，測定対象の共振周期が，オーバーレンジ時間（ｔ２−ｔ１）より大きいものに適している。さらに，センサのサンプリングタイムが運動の周期に比して短く，時間あたりのサンプリング数を十分得られる場合にも適している。また，ロール角，ピッチ角等の本質的に限界があり必ず復帰するものに適している。なお，ここでは近似の手法としてベジェ曲線を用いたが，他に，２次曲線近似，正弦波近似，スプライン近似等であっても同様の効果が得られる。
【００５９】
（４）減衰曲線近似は，図９に示すように，レンジオーバー開始時刻ｔ１から，（１）１本直線近似と同様に近似直線Ｌ１（ｔ）で近似し，外部情報を受けた後は減衰曲線Ｌ４（ｔ）で近似する近似方法である。外部情報としては，例えば，対応する駆動モータの電流値，電圧値，抵抗値等が利用できる。これらの外部情報によって，駆動モータの回転トルクが消失したと判断できる時刻ｔｓを得る。時刻ｔｓより後では，摩擦力等によって角速度が減衰するため，一般的に徐々に停止に向かう。または，モータトルクと慣性力や抵抗力との組み合わせによってモータの回転がある程度以下に減少したと判断できたとき，その時刻ｔｓを外部情報としてもよい。あるいは，データとして外部からの情報が無くても，センサが取り付けられている測定対象の物理特性等から求めても良い。この推定方法を実施する場合の概略構成を図１０に示す。この図では，図２と同じ構成については同じ符号を付し，センシング対象値推定部２８としての機能を外部情報入力部３５とＬ１Ｌ４推定部３６とが担っている。
【００６０】
次に，図１０に示した各構成の作用を説明する。角速度センサ１１からの出力信号は，逐次第１ループメモリ２１に蓄積される。センサ出力値がダイナミックレンジ内ならば第１ループメモリ２１内のデータはそのまま第２ループメモリ２３に逐次コピーされる。レンジオーバー開始／レンジ内復帰判定部２７は，第１ループメモリ２１に蓄積されたセンサ出力値を読み出し，レンジオーバー開始時刻ｔ１とレンジ内復帰時刻ｔ２とを記録する。
【００６１】
Ｌ１Ｌ４推定部３６は，レンジオーバー後すぐに，時刻ｔ１およびあらかじめ決められた時間Δｔａを用いて，時刻（ｔ１−Δｔａ）と時刻ｔ１に対応するセンサ出力値を第１ループメモリ２１から読み出す。そしてこれらのセンサ出力値を用いて，（１）１本直線近似の場合と同様に近似直線Ｌ１（ｔ）を求める。その後，近似直線Ｌ１（ｔ）からセンシング対象値推定値を求め，第２ループメモリ２３に記録する。また，（２）２本直線近似において記載したように，Δｔａを決定し直すとさらによい。
【００６２】
外部情報入力部３５から例えば回転トルクが減少したというようなトリガー信号が入力されると，Ｌ１Ｌ４推定部３６は，そのときの時刻ｔｓを記録する。さらに，時刻ｔｓにおけるセンシング対象値推定値Ｌ１（ｔｓ）を求め，近似減衰曲線Ｌ４（ｔ）を生成する。この近似減衰曲線Ｌ４（ｔ）を用いて，時刻ｔｓ〜時刻ｔ２のセンシング対象値推定値を求め，第２ループメモリ２３に記録する。この後，積算器２４によってセンシング対象値の積算を行う。さらに，平滑器２５によって平滑化を行い，表示部１３に表示する。
【００６３】
減衰曲線Ｌ４（ｔ）としては，例えば指数関数を用いる。
Ｌ４（ｔ）＝ｍａｘ（（Ｌ１（ｔｓ）−β）・ｅ-α(t-ts)＋β，Ｄ２）…（式３）：センサ出力値がＤ２の場合
Ｌ４（ｔ）＝ｍｉｎ（（β−Ｌ１（ｔｓ））・ｅ-α(t-ts)−β，Ｄ１）…（式４）：センサ出力値がＤ１の場合
ただし，αは減衰係数である。
時刻ｔｓでは，ｅの乗数は０であり，Ｌ４（ｔｓ）＝Ｌ１（ｔｓ）となる。時刻ｔｓを超えると，式３は漸近線βを持つ減衰曲線となる。外部トルクが０になれば，運動体の動きは止まる方向に向かうので，漸近線はセンサの零点出力βとなる。ただし，対象モデルによっては，βをセンサの零点出力以外の値とする方が適切な場合もある。
【００６４】
なお，レンジ内復帰する時刻ｔ２以前に，減衰曲線の値がセンサダイナミックレンジの上限値Ｄ２または下限値Ｄ１に達した場合は，その後，式３ではＬ４（ｔ）＝Ｄ２，式４ではＬ４（ｔ）＝Ｄ１を維持する。また，レンジ内復帰時の減衰曲線の値がちょうどレンジ境界値となるように減衰係数αを定めても良い。ただし，この場合には復帰後に推定することになる。またあるいは，レンジ内復帰時に減衰曲線の値がまだレンジ境界値まで戻っていない場合には，このまま推定値とセンサ出力値とをつなぐとこの箇所にギャップができる。そこで，例えば（２）２本直線近似の第２の近似直線Ｌ２（ｔ）を併用して，ギャップをならすようにしても良い。
【００６５】
この方法は，例えば，モータが停止すると所定の位置に復帰していくような，モデルが分かっているものに適している。また，外部情報としては，モータトルクのみでなく，傾斜角センサ等の他の検出器の出力値や，通信等による外部からの信号によっても良い。また，この方法が選択されていても，外部情報が入力される以前にレンジ内に復帰した場合は，上記の（１）〜（３）のいすれかの方法を用いて推定する。
【００６６】
次に，図１１〜図１５のフローチャートを参照して，この積算値推定装置２０による処理を説明する。まず，図１１のメイン処理について説明する。この処理は二足歩行ロボット１が動作されている間，繰り返し実行される。この処理が実行されるとまず，各時刻ｔにおける角速度センサ１１のセンサ出力値を取得する（Ｓ１０１）。さらに，取得したセンサ出力値を第１ループメモリ２１に蓄積する（Ｓ１０２）。
【００６７】
次に，そのセンサ出力値から，レンジオーバー開始／レンジ内復帰判定部２７により，角速度センサ１１がレンジオーバー状態であるかどうかを判断する（Ｓ１０３）。レンジオーバー状態であると判断された場合は（Ｓ１０３：Ｙｅｓ），推定部２２によりセンシング対象値推定処理を行う（Ｓ１０４）。この処理については後述する。
【００６８】
あるいは，レンジオーバー状態でない場合は（Ｓ１０３：Ｎｏ），そのまま第２ループメモリ２３に記録する（Ｓ１０５）。さらに，積算器２４によってセンサ出力値を積算する（Ｓ１０６）。さらに，積算結果を平滑器２５によって平滑化する（Ｓ１０７）。そして，求められた積算値を表示部１３に表示し（Ｓ１０８），この処理を終了する。
【００６９】
次に，Ｓ１０４で行われるセンシング対象値推定処理を図１２〜図１５の４通りについて説明する。これらはそれぞれ，図１２に示す第１の方法が上記の（１）１本直線近似に相当する。また，図１３に示す第２の方法が上記の（２）２本直線近似に相当する。また，図１４に示す第３の方法が上記の（３）ベジェ曲線近似に相当する。また，図１５に示す第４の方法が上記の（４）減衰曲線近似に相当する。二足歩行ロボット１の性能やセンサの種類等に応じて，あらかじめこれらの処理のうちから１つが選択されている。また，これらはいずれも，推定部２２のセンシング対象値推定部２８によって実行される。
【００７０】
次に，第１の方法について図１２に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ２０１）。そして，レンジオーバー前のデータを基に，近似直線Ｌ１（ｔ）を求める（Ｓ２０２）。そして，そのＬ１（ｔ）を用いてその時点でのセンシング対象値を推定する（Ｓ２０３）。この場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７１】
次に，第２の方法について図１３に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ３０１）。次に，レンジ内復帰判定部２７によってセンサ出力値がレンジ内に復帰したかどうかが判断される（Ｓ３０２）。復帰していない場合は（Ｓ３０２：Ｎｏ），レンジオーバー前のデータを基に，第１の近似直線Ｌ１（ｔ）を求める（Ｓ３０３）。そして，そのＬ１（ｔ）を用いてその時点でのセンシング対象値を推定する（Ｓ３０４）。この場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７２】
あるいは，センサ出力値がレンジ内復帰した場合は（Ｓ３０２：Ｙｅｓ），その復帰時刻ｔ２を取得する（Ｓ３０５）。さらに，あらかじめ設定した時間Δｔｂ０だけ時間が経過するのを待ち，新たな時間Δｔｂを決定する（Ｓ３０６）。得られた時間Δｔｂおよび第４点のセンサ出力値と，復帰時刻ｔ２でのセンサ出力値とから，第２の近似直線Ｌ２（ｔ）を求める（Ｓ３０７）。
【００７３】
さらに，第１の近似直線Ｌ１（ｔ）と第２の近似直線Ｌ２（ｔ）との接合点ｔｇを求める（Ｓ３０８）。次に，第２の近似直線Ｌ２（ｔ）を用いて時刻ｔｇ〜ｔ２間のセンシング対象値を推定する。そして，この第２の推定センシング対象値を，先に第１の近似直線Ｌ１（ｔ）を用いて求めた推定センシング対象値（Ｓ３０４の結果）の時刻ｔｇ〜ｔ２間のデータと置き換える（Ｓ３０９）。これで，この処理を終了してメイン処理に戻る。
【００７４】
次に，第３の方法について図１４に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ４０１）。次に，レンジ内復帰判定部２７によってセンサ出力値がレンジ内に復帰したかどうかが判断される（Ｓ４０２）。復帰していない場合は何もせず，メイン処理に戻る。この方法では，復帰後にセンシング対象値の推定を行うので，復帰するまではメイン処理中のセンシング対象値推定処理（Ｓ１０４）以降の処理は行わない。
【００７５】
あるいは，センサ出力値がレンジ内復帰した場合は（Ｓ４０２：Ｙｅｓ），その復帰時刻ｔ２を取得する（Ｓ４０３）。さらに，あらかじめ設定した時間Δｔｂ０だけ時間が経過するのを待ち，新たな時間Δｔｂを決定する（Ｓ４０４）。復帰時刻ｔ２と時刻（ｔ２＋Δｔｂ）との２点のセンサ出力値を第１ループメモリ２１から読み出す（Ｓ４０５）。あるいは，新たに決定された第４点のセンサ出力値を時刻（ｔ２＋Δｔｂ）におけるセンサ出力値に代えて使用する。また，レンジオーバー開始時刻ｔ１でのセンサ出力値とその時間Δｔａ前でのセンサ出力値とを第１ループメモリ２１から読み出す（Ｓ４０６）。Ｓ４０５とＳ４０６の実行順序は逆でも良い。
【００７６】
さらに，得られた４点のセンサ出力値から，ベジェ曲線Ｌ３（ｔ）を作成する（Ｓ４０７）。さらに，Ｓ４０７で求めたベジェ曲線Ｌ３（ｔ）を用いて時刻ｔ１〜ｔ２間のセンシング対象値を推定する（Ｓ４０８）。これでこの処理を終了してメイン処理に戻る。さらに，Ｓ４０８で得られた推定値を用いて，メイン処理中のレンジオーバー中には停止していた部分を算出実行し，表示する。なお，この方法でも，実時間では第２の方法等で推定しておき，レンジ内復帰後改めて推定し直して，推定センシング対象値を置き換えるようにしても良い。
【００７７】
次に，第４の方法について図１５に基づいて説明する。この処理が実行されると，まず角速度センサ１１がレンジオーバー開始した時刻ｔ１を取得する（Ｓ５０１）。レンジオーバー前のデータを基に，近似直線Ｌ１（ｔ）を求める（Ｓ５０２）。次に，外部情報が入力されたかどうかが判断される（Ｓ５０３）。入力されていない場合は（Ｓ５０３：Ｎｏ），Ｓ５０２で求めた近似直線Ｌ１（ｔ）を用いて，その時点でのセンシング対象値を推定する（Ｓ５０４）。このままレンジ内復帰した場合は，この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。
【００７８】
あるいは，モータ停止等の外部情報が入力されたら（Ｓ５０３：Ｙｅｓ），その外部情報の入力開始時刻ｔｓを取得する。さらに，その時刻ｔｓにおける推定センシング対象値Ｌ１（ｔｓ）を取得する（Ｓ５０５）。そしてそのＬ１（ｔｓ）を起点とする減衰曲線Ｌ４（ｔ）を作成し，それによってセンシング対象値を推定する（Ｓ５０６）。この値を推定センシング対象値として，この処理を終了しメイン処理に戻る。なお，減衰曲線Ｌ４（ｔ）による推定値が，レンジ内復帰時刻ｔ２より前にレンジ上限値Ｄ２または下限値Ｄ１に到達した場合は，その後の推定値はその上限値Ｄ２または下限値Ｄ１で固定する。
【００７９】
以上詳細に説明したように，本形態の二足歩行ロボット１によれば，積算値推定装置２０が推定部２２を有しているので，センサへダイナミックレンジを超えた入力があった場合でも，センシング対象値を推定することができる。さらに，推定部２２は，レンジオーバー開始／レンジ内復帰判定部２７，センシング対象値推定部２８を有し，センシング対象値推定部２８ではその推定方法を（１）１本直線近似，（２）２本直線近似，（３）ベジェ曲線近似，（４）減衰曲線近似等から使い分ける。従って，そのセンサの取り付けられている部材のモデルに応じて，適切に推定される。これにより，センサのダイナミックレンジを超えた入力があった場合のセンシング対象値を推定し，検出精度を向上させることができる。また，エンコーダを用いず，回転角速度の累積から角度を得ることができるので，ロボットの骨格に当たるリンクがたわんでも正しい角度が得られるというメリットがある。
【００８０】
なお，本形態は単なる例示にすぎず，本発明を何ら限定するものではない。したがって本発明は当然に，その要旨を逸脱しない範囲内で種々の改良，変形が可能である。
例えば，上記の実施の形態では，（２）２本直線近似において，予測関数や逆算関数として直線を選択しているが，これらは曲線近似としてもよい。
また例えば，本形態では二足歩行ロボット１の肘関節に設けられた角速度センサ１１のセンサ出力値を対象としたものとして記載しているが，センサの種類やそのセンサが設置される対象等は，どのようなものでも良い。また，本形態では積算器を用いてセンサ出力値を積算することにより肘角度を求めるとしているが，積算の必要のないセンサでは，この積算処理を省略すればよい。
【図面の簡単な説明】
【００８１】
【図１】本形態の二足歩行ロボットのスケルトンモデルを示す概略構成図である。
【図２】本形態の積算値推定装置の概略構成を示すブロック図である。
【図３】１本直線近似によるセンシング対象値推定方法を示す説明図である。
【図４】１本直線近似による積算値推定装置の概略構成を示すブロック図である。
【図５】２本直線近似によるセンシング対象値推定方法を示す説明図である。
【図６】２本直線近似による積算値推定装置の概略構成を示すブロック図である。
【図７】ベジェ曲線近似によるセンシング対象値推定方法を示す説明図である。
【図８】ベジェ曲線近似による積算値推定装置の概略構成を示すブロック図である。
【図９】減衰曲線近似によるセンシング対象値推定方法を示す説明図である。
【図１０】減衰曲線近似による積算値推定装置の概略構成を示すブロック図である。
【図１１】センシング対象値推定方法のメイン処理を示すフローチャート図である。
【図１２】第１のセンシング対象値推定方法を示すフローチャート図である。
【図１３】第２のセンシング対象値推定方法を示すフローチャート図である。
【図１４】第３のセンシング対象値推定方法を示すフローチャート図である。
【図１５】第４のセンシング対象値推定方法を示すフローチャート図である。
【符号の説明】
【００８２】
１１ 加速度センサ
２０ 積算値推定装置
２１ 第１ループメモリ
２２ 対象値推定部
２３ 第２ループメモリ
２４ 積算器
２７ レンジオーバー開始／レンジ内復帰判定部
２８ センシング対象値推定部
【特許請求の範囲】
【請求項１】
センサ出力値の変遷を記録する第１メモリと，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出するレンジ検出部と，
前記レンジ検出部により検出された逸脱から復帰までの間について，前記第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定する推定部とを有することを特徴とするセンシング対象値推定装置。
【請求項２】
請求項１に記載のセンシング対象値推定装置において，
前記推定部は，復帰後に，前記第１メモリに記録されている逸脱前と復帰後とのセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項３】
請求項１に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部を有し，
前記予測部で決定した予測関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項４】
請求項１に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，
前記予測部で決定した予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定する減衰関数決定部とを有し，
外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については前記減衰関数決定部が決定した減衰関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項５】
請求項２に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，
復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定する逆算部とを有し，
前記予測部で決定した予測関数と前記逆算部が決定した逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項６】
請求項２に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定するベジェ関数決定部を有し，
前記ベジェ関数決定部が決定したベジェ関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項７】
請求項３から請求項５までのいずれか１つに記載のセンシング対象値推定装置において，
前記予測部は，
センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて予測関数を決定するとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項８】
請求項５に記載のセンシング対象値推定装置において，前記逆算部は，
センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて逆算関数を決定するとともに，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項９】
請求項６に記載のセンシング対象値推定装置において，
前記ベジェ関数決定部は，
センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいてベジェ関数を決定するとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項１０】
請求項１から請求項９までのいずれか１つに記載のセンシング対象値推定装置において，
センシング対象値のダイナミックレンジ内からの逸脱前とダイナミックレンジ内への復帰後についてはセンサ出力値を記録し，逸脱から復帰までの間については前記推定部による推定値を記録する第２メモリと，
前記第２メモリに記録された値の積算値を求める積算部とを有することを特徴とするセンシング対象値推定装置。
【請求項１１】
センサ出力値の変遷を第１メモリに記録し，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，
第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定し，
検出された逸脱から復帰までの間について，決定された予測関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１２】
請求項１１に記載のセンシング対象値推定方法において，
決定された予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定し，
外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については決定された減衰関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１３】
請求項１１に記載のセンシング対象値推定方法において，
第１メモリに記録されている復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定し，
決定された予測関数と決定された逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１４】
センサ出力値の変遷を第１メモリに記録し，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，
第１メモリに記録されている逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定し，
検出された逸脱から復帰までの間について，決定されたベジェ関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１５】
請求項１１に記載のセンシング対象値推定方法において，前記予測関数の決定を，
センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて行うとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【請求項１６】
請求項１３に記載のセンシング対象値推定方法において，前記逆算関数の決定を，
センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【請求項１７】
請求項１４に記載のセンシング対象値推定方法において，前記ベジェ関数の決定を，
センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【請求項１】
センサ出力値の変遷を記録する第１メモリと，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出するレンジ検出部と，
前記レンジ検出部により検出された逸脱から復帰までの間について，前記第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定する推定部とを有することを特徴とするセンシング対象値推定装置。
【請求項２】
請求項１に記載のセンシング対象値推定装置において，
前記推定部は，復帰後に，前記第１メモリに記録されている逸脱前と復帰後とのセンサ出力値の変遷に基づいてセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項３】
請求項１に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部を有し，
前記予測部で決定した予測関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項４】
請求項１に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，
前記予測部で決定した予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定する減衰関数決定部とを有し，
外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については前記減衰関数決定部が決定した減衰関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項５】
請求項２に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定する予測部と，
復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定する逆算部とを有し，
前記予測部で決定した予測関数と前記逆算部が決定した逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項６】
請求項２に記載のセンシング対象値推定装置において，前記推定部は，
逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定するベジェ関数決定部を有し，
前記ベジェ関数決定部が決定したベジェ関数によりセンシング対象値の変遷を推定するものであることを特徴とするセンシング対象値推定装置。
【請求項７】
請求項３から請求項５までのいずれか１つに記載のセンシング対象値推定装置において，
前記予測部は，
センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて予測関数を決定するとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項８】
請求項５に記載のセンシング対象値推定装置において，前記逆算部は，
センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて逆算関数を決定するとともに，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項９】
請求項６に記載のセンシング対象値推定装置において，
前記ベジェ関数決定部は，
センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいてベジェ関数を決定するとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いるものであることを特徴とするセンシング対象値推定装置。
【請求項１０】
請求項１から請求項９までのいずれか１つに記載のセンシング対象値推定装置において，
センシング対象値のダイナミックレンジ内からの逸脱前とダイナミックレンジ内への復帰後についてはセンサ出力値を記録し，逸脱から復帰までの間については前記推定部による推定値を記録する第２メモリと，
前記第２メモリに記録された値の積算値を求める積算部とを有することを特徴とするセンシング対象値推定装置。
【請求項１１】
センサ出力値の変遷を第１メモリに記録し，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，
第１メモリに記録されている逸脱前のセンサ出力値の変遷に基づいてセンシング対象値の予測関数を決定し，
検出された逸脱から復帰までの間について，決定された予測関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１２】
請求項１１に記載のセンシング対象値推定方法において，
決定された予測関数の外部情報を受けた時点での値を初期値とする減衰関数を決定し，
外部情報を受ける前については予測関数によりセンシング対象値の変遷を推定し，外部情報を受けた後については決定された減衰関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１３】
請求項１１に記載のセンシング対象値推定方法において，
第１メモリに記録されている復帰後のセンサ出力値の変遷に基づいてセンシング対象値の逆算関数を決定し，
決定された予測関数と決定された逆算関数との交差時以前については予測関数によりセンシング対象値の変遷を推定し，交差時以後については逆算関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１４】
センサ出力値の変遷を第１メモリに記録し，
センサ出力値に基づいてセンシング対象値のダイナミックレンジ内からの逸脱とダイナミックレンジ内への復帰とを検出し，
第１メモリに記録されている逸脱前および復帰後のセンサ出力値の変遷に基づいてベジェ関数を決定し，
検出された逸脱から復帰までの間について，決定されたベジェ関数によりセンシング対象値の変遷を推定することを特徴とするセンシング対象値推定方法。
【請求項１５】
請求項１１に記載のセンシング対象値推定方法において，前記予測関数の決定を，
センシング対象値のダイナミックレンジ内からの逸脱時点およびそこから所定時間さかのぼった時点でのセンサ出力値に基づいて行うとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【請求項１６】
請求項１３に記載のセンシング対象値推定方法において，前記逆算関数の決定を，
センシング対象値のダイナミックレンジ内への復帰時点およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【請求項１７】
請求項１４に記載のセンシング対象値推定方法において，前記ベジェ関数の決定を，
センシング対象値のダイナミックレンジ内からの逸脱時点，そこから所定時間さかのぼった時点，センシング対象値のダイナミックレンジ内への復帰時点，およびそこから所定時間経過した時点でのセンサ出力値に基づいて行うとともに，
前記所定時間さかのぼった時点でセンシング対象値がダイナミックレンジ内から逸脱していた場合には，前記所定時間さかのぼった時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用い，
センシング対象値がダイナミックレンジ内へ復帰してから所定時間経過前に再びダイナミックレンジ内から逸脱した場合には，前記所定時間経過した時点でのセンサ出力値に替えて，復帰から逸脱までのセンサ出力値の波形に基づいて決定した値を用いることを特徴とするセンシング対象値推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２００７−７８６１０（Ｐ２００７−７８６１０Ａ）
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願番号】特願２００５−２６９７７４（Ｐ２００５−２６９７７４）
【出願日】平成１７年９月１６日（２００５．９．１６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願日】平成１７年９月１６日（２００５．９．１６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]