JP2019168394A - 身体向推定装置および身体向推定プログラム - Google Patents

Abstract

【課題】センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムを提供すること。【解決手段】移動体１は、測距センサ１６で測距された測距データＭＰから、近似楕円Ｃと、ｘｙ軸，ｙｘ軸による二次関数Ｑｘｙ，Ｑｙｘとが算出される。ユーザＨの肩幅Ｗ１及び厚みＷ２とに近い近似楕円Ｃが選択され、測距データＭＰとの近似誤差が最も小さい二次関数Ｑｘｙ，Ｑｙｘが選択される。そして、選択された近似曲線からユーザＨの位置および向きが推定される。即ち、１の形状または座標系に基づいた近似曲線がユーザＨに適合しない場合は、他の形状または座標系に基づいた近似曲線からユーザＨの位置および向きが推定される。よって、測距センサ１６とユーザＨとの位置関係が様々に変化してもユーザＨの位置および向きを検出できる。【選択図】図３

Description

本発明は、センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムに関するものである。

特許文献１の顔向き検知装置によれば、ヘッドレスト３に水平方向に設けられた複数の距離センサ１１ａ〜１１ｅによって頭部４までの距離をそれぞれ検知し、この頭部距離に基づいて近似曲線５を作成する。作成された近似曲線５の一部を楕円６により近似し、その楕円６の長軸に基づいて頭部４のヨー角を推定している。

また特許文献２の人体向き推定装置によれば、距離センサ５０を備え、距離センサ５０の測定結果に応じて対象者を楕円で近似し、その重心位置を算出する。この重心位置の変化をトラッキングして、対象者の移動方向を算出し、これを対象者の向きと推定している。同様に特許文献３の位置検出装置も、複数の距離センサ１１〜１４により対象者を測定し、その測距点群から対象者の輪郭を楕円で近似し、その重心位置の変化をトラッキングして、対象者の移動方向を算出し、これを対象者の向きと推定している。

特開２０１６−１７５５０９号公報 特開２０１１−０８１６３４号公報 特開２０１０−１６９５２１号公報

特許文献１のように、ヘッドレスト３に寄りかかる頭部４の向きであれば、各センサ１１ａ〜１１ｅと頭部４との位置関係は大凡決まっている。よって、頭部４までの距離に基づいて算出された楕円により頭部４のヨー角を推定することは可能である。

しかしながら、特許文献２，３のように、センサと対象者（身体）との位置関係が様々に変化する環境下においては、センサと対象者との位置関係によっては、測距データの取れ方が変わり、近似曲線を対象者に上手く適合できない場合があった。かかる場合には、対象者の向きを検出できなかった。

本発明は、上述した問題点を解決するためになされたものであり、センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムを提供することを目的としている。

この目的を達成するために本発明の身体向推定装置は、対象者を測距する測距手段と、その測距手段により測距された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出手段と、その二次関数算出手段により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択手段と、その二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定する向き推定手段と、を備えている。

また本発明の身体向推定プログラムは、対象者を測距した測距データを取得する測距取得機能と、その測距取得機能により取得された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出機能と、その二次関数算出機能により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択機能と、その二次関数選択機能により選択された二次関数に基づいて前記対象者の向きを推定する向き推定機能と、をコンピュータに実現させるものである。

本発明の身体向推定装置および身体向推定プログラムによれば、対象者を測距した複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する。この算出された複数の二次関数うち、複数の測距データとの近似誤差が最も小さい二次関数を選択し、その選択された二次関数に基づいて、対象者の向きを推定する。即ち、１の座標系に基づいて算出された二次関数が対象者に上手く適合できない場合には、他の座標系に基づいて算出された二次関数に基づいて対象者の向きを推定する。よって、測距手段（センサ）と対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できるという効果がある。

移動体の外観図である。 移動体の電気的構成を示すブロック図である。 移動体のフローチャートである。 ユーザの近似楕円の算出を示す図である。 （ａ）は、測距データの瞬間的なバラツキによって、サイズの異なる近似楕円が算出される場合を示す図であり、（ｂ）は、ユーザの身体の肩幅および厚さと、近似楕円の長軸および短軸とを示す図である。 （ａ）は、２種類の二次関数の算出を示す図であり、（ｂ）は、両端の測距データ間に二次関数の頂点が存在しない場合を示す図である。 （ａ）は、クラスタエッジ間に二次関数の頂点が存在し、１のクラスタエッジが二次関数の頂点より外側かつ頂点から離れている場合を示す図であり、（ｂ）は、クラスタエッジ間に二次関数の頂点が存在し、１のクラスタエッジが二次関数の頂点より外側かつ頂点Ｔに近い場合を示す図である。

以下、本発明の好ましい実施形態について、添付図面を参照して説明する。まず、図１を参照して、本実施形態における移動体１の構成を説明する。図１は、移動体１の外観図である。移動体１は、ユーザＨの前方にて、ユーザ（対象者）Ｈに対し適切な位置に移動して、ユーザＨに随行できる装置として機能する。

図１に示す通り、移動体１は、主に略円柱状の外装２と、その外装２の内部に配設され移動体１の各部を制御する制御部１０と、測距センサ１６と、車輪１７とを有する。測距センサ１６は、外装２の上部に配置され、レーザ光を全方位（３６０度）に対して照射することで、測距センサ１６と物体との距離を検知（測距）する装置である。測距センサ１６は、任意の角度毎に検出された対象物との距離を、その角度に対応付けて制御部１０へ送信する。また、測距センサ１６は上下方向に移動可能に構成され、予め測距センサ１６からのレーザ光がユーザＨの肩周辺に照射されるよう、測距センサ１６の上下方向の位置が適宜設定される。以下、測距センサ１６から検知される距離および角度のことを「測距データ」と称す。

車輪１７は、外装２の下部における左右両端にそれぞれ設けられる車輪である。左右の車輪１７にはモータ（図示せず）が接続され、後述する駆動部１８（図２参照）からの制御信号に基づいてモータを駆動させることで、移動体１が移動される。左右のモータを、同じ出力で正転および逆転させることで移動体１の前方移動および後方移動を行い、また、モータを差動させることで、移動体１の移動方向の変更を行う。即ち、移動体１は、かかる車輪１７が配設される直交方向である前後方向には移動可能である一方で、左右方向には直接移動できないので、移動体１は、非ホロノミックな拘束条件を有する車輪１７及び駆動部１８によって移動するものである。

次に、図２を参照して、移動体１の電気的構成について説明する。図２は、移動体１の電気的構成を示すブロック図である。移動体１は制御部１０を有し、その制御部１０はＣＰＵ１１と、フラッシュＲＯＭ１２と、ＲＡＭ１３とを有し、これらはバスライン１４を介して入出力ポート１５にそれぞれ接続されている。入出力ポート１５には、更に、測距センサ１６と、駆動部１８とが接続されている。

ＣＰＵ１１は、バスライン１４により接続された各部を制御する演算装置である。フラッシュＲＯＭ１２は、ＣＰＵ１１により実行されるプログラムや固定値データ等を格納した書き換え可能な不揮発性の記憶装置であり、制御プログラム１２ａが記憶される。ＣＰＵ１１によって制御プログラム１２ａが実行されると、図３のメイン処理が実行される。

ＲＡＭ１３は、ＣＰＵ１１が制御プログラム１２ａの実行時に各種のワークデータやフラグ等を書き換え可能に記憶するためのメモリであり、測距センサ１６から測距された測距データＭＰが記憶される測距データメモリ１３ａと、楕円位置向メモリ１３ｂと、二次関数位置向メモリ１３ｃと、カルマンフィルタ予測位置向メモリ１３ｄ（以下「ＫＦ予測位置向メモリ１３ｄ」と略す）と、相関結果位置向メモリ１３ｅと、推定現在位置向メモリ１３ｆと、前回値メモリ１３ｇとがそれぞれ設けられる。

楕円位置向メモリ１３ｂは、測距データメモリ１３ａの測距データＭＰから近似された近似楕円Ｃ（図４参照）から推定された、ユーザＨの位置および向きを記憶するメモリである。二次関数位置向メモリ１３ｃは、測距データメモリ１３ａの測距データＭＰから近似された二次関数Ｑｘｙ又はＱｙｘ（図６参照）から推定されたユーザＨの位置および向きを記憶するメモリである。

ＫＦ予測位置向メモリ１３ｄは、後述の前回値メモリ１３ｇの値から、カルマンフィルタ予測ステップ（数式９）に基づいて推定されたユーザＨの現在の位置および向きを記憶するメモリである。相関結果位置向メモリ１３ｅは、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃに記憶された複数のユーザＨの位置および向きのうち、最もユーザＨらしいと推定されるユーザＨの位置および向きが記憶されるメモリである。

推定現在位置向メモリ１３ｆは、相関結果位置向メモリ１３ｅの値に基づいてカルマンフィルタによって推定されたユーザＨの位置および向きが記憶されるメモリであり、前回値メモリ１３ｇは、前回の推定現在位置向メモリ１３ｆの値と、推定されたユーザＨの速度および角速度とが記憶されるメモリである。

駆動部１８は、移動体１を移動動作させるための装置であり、車輪１７（図１参照）および車輪１７の駆動源となるモータ（図示せず）等から構成される。制御部１０から制御信号が駆動部１８に入力されると、入力された制御信号に基づいてモータが回転し、該モータの回転が動力となって車輪１７が駆動し、移動体１を動作させる。

次に、図３〜７を参照して、移動体１のＣＰＵ１１で実行されるメイン処理を説明する。図３は、移動体１のメイン処理のフローチャートである。メイン処理は移動体１の電源投入直後に実行される。メイン処理はまず、測距センサ１６から取得した測距データＭＰを測距データメモリ１３ａに保存する（Ｓ１）。Ｓ１の処理の後、測距データメモリ１３ａに記憶される測距データＭＰに基づいて、ユーザＨの近似楕円Ｃを算出する（Ｓ２）。ここで図２を参照して、測距データＭＰに基づく、ユーザＨの近似楕円Ｃの算出について説明する。

図４は、ユーザＨの近似楕円Ｃの算出を示す図である。図４において、測距センサ１６から取得されたユーザＨの測距データＭＰを、白抜きの四角形で表す。測距データＭＰに基づいた最小二乗法等によって、１の近似楕円Ｃが算出される。本実施形態では、近似楕円Ｃの中心ＣＰが「ユーザＨの位置」とされ、近似楕円Ｃにおける短軸の向きＤＣ１，ＤＣ２が「ユーザＨの向き」とされる。向きＤＣ１，ＤＣ２の２つがユーザＨの向きとして算出されるのは、近似楕円ＣからはユーザＨがどの方向を向いているか（前面または背面）が区別できないからである。なお、向きＤＣ１，ＤＣ２のうち、いずれをユーザＨの最終的な向きとするかは、後述のＳ７の処理で判定される。

図３に戻る。Ｓ２の処理の後、算出された近似楕円ＣがユーザＨの体のサイズに近いものである場合は、その近似楕円ＣにおけるユーザＨの位置および向きを楕円位置向メモリ１３ｂに保存する（Ｓ３）。Ｓ２の処理では、測距データＭＰの瞬間的なバラツキや、測距データＭＰの測距誤差による揺らぎ等によって、算出される近似楕円ＣがユーザＨの体のサイズにフィットしたものや、ユーザＨの体のサイズから離れたもの等、様々となる場合がある。そこで、Ｓ３の処理では、算出された近似楕円ＣがユーザＨの体のサイズに近いかどうかが判断され、近似楕円ＣがユーザＨの体のサイズに近い場合は、該近似楕円ＣからユーザＨの位置および向きが算出される。かかるＳ３の処理における近似楕円Ｃの選択について、図５を参照して説明する。

図５（ａ）は、測距データＭＰの瞬間的なバラツキによって、サイズの異なる近似楕円Ｃが算出される場合を示す図であり、図５（ｂ）は、ユーザＨの身体の肩幅Ｗ１および厚さＷ２と、近似楕円Ｃの長軸ａ及び短軸ｂとを示す図である。図３のＳ２の処理によって、測距データＭＰから１の近似楕円Ｃが算出されるが、測距データＭＰの瞬間的なバラツキによって、図５（ａ）に示すように、ユーザＨの体のサイズに近い近似楕円Ｃ１が算出される場合と、近似楕円Ｃ１とはサイズの異なる近似楕円Ｃ２が算出される場合とが存在する。特に、近似楕円Ｃ２は、ユーザＨの肩周辺の身体の外周よりも大きな円周で近似されている。このような近似楕円Ｃ２では、ユーザＨの位置および向きを正確に算出できない。

そこで、図３のＳ３の処理では、図５（ｂ）に示す通り、近似楕円Ｃの長軸ａ及び短軸ｂと、ユーザＨの肩周辺における標準的な肩幅Ｗ１及び厚みＷ２とに基づき、算出された近似楕円Ｃが、ユーザＨの肩周辺の外周に近いかどうかを判断する。具体的には、以下の数式１の条件を満たす近似楕円Ｃが、ユーザＨの体のサイズに近いものと判断される。

本実施形態では、肩幅Ｗ１として「０．６ｍ」が例示され、厚さＷ２として「０．３ｍ」が例示される。即ち、近似楕円Ｃの長軸ａの２倍の長さがユーザＨの肩幅Ｗ１の±１０％以内かつ、短軸ｂの２倍の長さがユーザＨの厚さＷ２の±１０％以内である場合に、近似楕円Ｃは、ユーザＨのサイズに近いと判断される。このようにして、近似楕円ＣがユーザＨの体のサイズに近いと判断された場合は、その近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２が、楕円位置向メモリ１３ｂに保存される。

図３に戻る。Ｓ３の処理の後、測距データメモリに記憶される測距データＭＰに基づいて、ｘｙ座標系に基づく二次関数Ｑｘｙと、ｙｘ座標系に基づく二次関数Ｑｙｘとをそれぞれ算出する（Ｓ４）。ここで、図６（ａ）を参照して、二次関数Ｑｘｙ，Ｑｙｘの算出について説明する。

図６（ａ）は、２種類の二次関数Ｑｘｙ，Ｑｙｘの算出を模式的に示した図である。図６（ａ）に示す通り、測距データＭＰに基づいて、ｘｙ座標系において二次関数で近似された二次関数Ｑｘｙと、ｙｚ座標系において二次関数で近似された二次関数Ｑｘｙとが算出される。二次関数Ｑｘｙは以下の数式２に基づいて、二次関数Ｑｙｘは数式３に基づいてそれぞれ算出される。

ここで、数式２におけるａ_１，ｂ_１，ｃ_１は係数であり、これら係数は、数式２と測距データＭＰとの最小二乗法に基づいて決定される。また、数式３におけるａ_２，ｂ_２，ｃ_２も係数であり、これら係数は、数式３と測距データＭＰとの最小二乗法に基づいて決定される。即ち、本実施形態の二次関数Ｑｘｙのｘ軸に沿って直立する形状とされ、二次関数Ｑｙｘの軸線はｙ軸に沿って直立する形状と平行とされる。

ところで、測距センサ１６は、レーザ光を全方位に対して照射することでユーザＨとの距離が取得されるので、ユーザＨの身体の一部分が、ユーザＨの測距データＭＰとして取得される。即ち、測距センサ１６から取得される測距データＭＰは、その分布に偏りが生じる。また、図６（ａ）に示す通り、ｘｙ座標系による二次関数Ｑｘｙは、上（または下）に凸の形状であり、ｙｘ座標系による二次関数Ｑｙｘは、左（または右）に凸の形状である。

即ち、二次関数Ｑｘｙと二次関数Ｑｙｘとでは形状が異なるので、測距データＭＰの分布によって、二次関数Ｑｘｙに適合する測距データＭＰの分布や、二次関数Ｑｙｘに適合する測距データＭＰの分布が存在し得る。そこで本実施形態では、測距データＭＰから形状の異なる２つの二次関数Ｑｘｙ，Ｑｙｘを算出することで、測距データＭＰの分布により適合し得る二次関数Ｑの候補を挙げることができる。

図３に戻る。Ｓ４の処理の後、二次関数Ｑｘｙ，Ｑｙｘから、測距データメモリ１３ａの値との近似誤差が最も小さいものを選択し、その二次関数Ｑｘｙ又はＱｙｘの位置および向きを二次関数位置向メモリ１３ｃに保存する（Ｓ５）。具体的には、まず、Ｓ４の処理で算出された、二次関数Ｑｘｙ，Ｑｙｘのそれぞれについて、測距データメモリ１３ａに記憶される測距データＭＰとの近似誤差が算出される。これら二次関数Ｑｘｙ，Ｑｙｘの近似誤差が小さい方が、ユーザＨの位置および向きを算出する二次関数Ｑとして選択される。

選択された二次関数Ｑから、ユーザＨの位置および向きが算出される。本実施形態においては、二次関数Ｑの頂点Ｔと測距データＭＰとの位置関係に応じて、ユーザＨの位置および向きを算出する。ここで、二次関数ＱにおけるユーザＨの位置および向きの算出方法を、図６（ｂ），図７を参照して説明する。

図６（ｂ）は、両端の測距データＭＰ間に二次関数Ｑの頂点Ｔが存在しない場合を示す図である。ここで、図６（ｂ），図７において、測距データＭＰのうち、二次関数Ｑの一端側に該当する測距データを「クラスタエッジＥ１」と称し、二次関数Ｑの他端側に該当する測距データＭＰを「クラスタエッジＥ２」と称する。

図６（ｂ）において、二次関数Ｑの頂点Ｔは、クラスタエッジＥ１とクラスタエッジＥ２との外側に位置する。即ち、測距データＭＰは、ユーザＨの身体の一部分、具体的には、身体の前面（胸側）または背面（背中側）を測距できたが、両肩を上手く測距できなかったと判断される。

そこで、本実施形態では、二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との外側に位置する場合に、確実に測距できたクラスタエッジＥ１及びクラスタエッジＥ２に基づいて、ユーザＨの位置および向きが算出される。具体的には、クラスタエッジＥ１とクラスタエッジＥ２との中点ＥｃがユーザＨの位置とされ、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する向きＤＱ１，ＤＱ２がユーザＨの向きとされる。具体的には、クラスタエッジＥ１のｘ軸方向の座標をＸ_Ｅ１，ｙ軸方向の座標をＹ_Ｅ１とし、クラスタエッジＥ２のｘ軸方向の座標をＸ_Ｅ２，ｙ軸方向の座標をＹ_Ｅ２とすると、ユーザＨの向きＤＱ１は数式４で算出され、ユーザＨの向きＤＱ２は数式５で算出される。

これにより、測距データＭＰとしてユーザＨの身体の一部分（前面または背面）のみが測距された場合であっても、確実に測距されたクラスタエッジＥ１とクラスタエッジＥ２とに基づいてユーザＨの位置および向きが算出されるので、かかるユーザＨの位置および向きの誤差を最小限に抑えることができる。なお、ユーザＨの向きとして向きＤＱ１，ＤＱ２の２つが算出されるのは、上記した近似楕円Ｃと同様に、二次関数ＱからユーザＨがどの方向を向いているか（前面または背面）を区別できないからである。なお、向きＤＱ１，ＤＱ２のうち、いずれをユーザＨの最終的な向きとするかは後述のＳ７の処理で判定される。

次に、二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との間に存在する場合について、図７を参照して説明する。二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との間に存在する場合は、更に、クラスタエッジＥ１及びクラスタエッジＥ２が共に頂点Ｔから離れている場合（図７（ａ））と、クラスタエッジＥ１又はクラスタエッジＥ２のいずれか一方が頂点Ｔに近い場合と（図７（ｂ））で、ユーザＨの位置および向きの算出方法が異なる。

図７（ａ）は、クラスタエッジＥ１，Ｅ２間に二次関数Ｑの頂点Ｔが存在し、クラスタエッジＥ２が二次関数Ｑの頂点Ｔより外側かつ頂点Ｔから離れている場合を示す図であり、図７（ｂ）は、クラスタエッジＥ１，Ｅ２間に二次関数Ｑの頂点Ｔが存在し、クラスタエッジＥ２が二次関数Ｑの頂点Ｔより外側かつ頂点Ｔに近い場合を示す図である。以下では、クラスタエッジＥ１が頂点Ｔから十分に離れていて、クラスタエッジＥ２が頂点Ｔから離れている場合と、頂点Ｔに近づいている場合とを説明するが、クラスタエッジＥ２が頂点Ｔから十分に離れていて、クラスタエッジＥ１が頂点Ｔから離れている場合もしくは頂点Ｔに近い場合も、同様であることは言うまでもない。

クラスタエッジＥ１とクラスタエッジＥ２との間に頂点Ｔが位置する場合は、まず、クラスタエッジＥ２と頂点Ｔとの距離関係が判断される。具体的には、図７（ａ），図７（ｂ）に示す、中点Ｅｃと頂点Ｔとの距離と、中点ＥｃとクラスタエッジＥ２との距離に基づいて、クラスタエッジＥ２と頂点Ｔとの位置関係が判断される。ここで、中点Ｅｃと頂点Ｔとのｘ軸方向の距離をΔＸ_Ｔ，ｙ軸方向の距離をΔＹ_Ｔとし、中点ＥｃとクラスタエッジＥ２とのｘ軸方向の距離をΔＸ_Ｅ，ｙ軸方向の距離をΔＹ_Ｅとして、以下の数式６を満たす場合は、クラスタエッジＥ２と頂点Ｔとが離れていると判断される。

即ち、本実施形態では、中点ＥｃとクラスタエッジＥ２との距離が、中点Ｅｃと頂点Ｔとの距離の０．９倍より大きい場合は、クラスタエッジＥ２と頂点Ｔとが離れていると判断される。

まず、図７（ａ）を参照して、クラスタエッジＥ２と頂点Ｔとが離れている場合の、ユーザＨの位置および向きの算出方法を説明する。図７（ａ）に示す通り、クラスタエッジＥ２と頂点Ｔとが離れているので、クラスタエッジＥ１と頂点Ｔとの間のみならず、頂点ＴとクラスタエッジＥ２との間にも多くの測距データＭＰが測距される。従って、測距データＭＰによって、両肩を含めたユーザＨの身体の前面または背面を幅広く測距されたと判断できる。この場合、頂点Ｔ付近がユーザＨの胸部もしくは背中の中心位置と推定され、クラスタエッジＥ１とクラスタエッジＥ２とは、両肩付近の位置と推定される。

そこで本実施形態では、クラスタエッジＥ２と頂点Ｔとが離れている場合は、両肩の位置と推定されるクラスタエッジＥ１，Ｅ２とに基づいて、ユーザＨの位置および向きが算出される。具体的には、中点ＥｃがユーザＨの位置とされ、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する向きＤＱ１，ＤＱ２が、ユーザＨの向きとされる。即ち、ユーザＨの向きは、上述した数式４と数式５とにより算出される。これにより、両肩の位置と推定されるクラスタエッジＥ１，Ｅ２に基づいて、ユーザＨの位置および向きが算出されるので、かかるユーザＨの位置および向きを、より精度の高いものとできる。

次に、クラスタエッジＥ２と頂点Ｔとが近い場合、即ち、上述した数式６を満たさない場合のユーザＨの位置および向きの算出方法を図７（ｂ）を参照して説明する。図７（ｂ）に示す通り、クラスタエッジＥ２は、頂点Ｔよりも外側かつ頂点Ｔとが近い位置に存在している。かかる場合は、頂点Ｔ付近の位置がユーザＨの左肩または右肩付近の位置と推定される。

ところで、クラスタエッジＥ２は頂点Ｔよりも外側に隣接するので、測距センサ１６（移動体１）から見た場合、クラスタエッジＥ２は頂点Ｔの「裏側」に位置する。即ち、頂点Ｔが、測距センサ１６からのレーザ光の障害（邪魔）となって、クラスタエッジＥ２まで届かず、クラスタエッジＥ２は測距できない状態となっている。これによって、該当する左肩または右肩の全体が測距できないことで、クラスタエッジＥ２が正確に測距されなかった虞がある。

そこで本実施形態では、クラスタエッジＥ１，Ｅ２に加え、測距データＭＰが正確に取得された範囲に位置する頂点Ｔ、即ち左肩または右肩とされる位置とに基づいてユーザＨの向きＤＱ１，ＤＱ２が算出される。具体的には、中点Ｅｃのｘ軸方向の座標をＸ_Ｃ，ｙ軸方向の座標Ｙ_Ｃとし、頂点Ｔのｘ軸方向の座標をＸ_Ｔ，ｙ軸方向の座標をＹ_Ｔとすると、ユーザＨの向きＤＱ１は以下の数式７で算出され、向きＤＱ２は数式８で算出される。

即ち、クラスタエッジＥ２と頂点Ｔとが近い場合は、中点Ｅｃと頂点Ｔとを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２が算出される。即ち、正確に測距データＭＰが取得された範囲に位置するが、近似された二次関数Ｑ上の左肩または右肩の位置とされる頂点Ｔと、実際に測距センサ１６によって測距されたが、頂点Ｔ付近の影響によって正確に測距されなかった虞のあるクラスタエッジＥ２との両方が考慮されて向きＤＱ１，ＤＱ２が算出されるので、かかる向きＤＱ１，ＤＱ２の誤差を抑制できる。以上のように算出された、向きＤＱ１，ＤＱ２をユーザＨの向きとして、また、中点Ｅｃをユーザの位置として、それぞれ二次関数位置向メモリ１３ｃに保存される。

図３に戻る。Ｓ５の処理の後、前回値メモリ１３ｇの値から、カルマンフィルタ予測ステップに基づいてユーザＨの現在の位置および向きを予測し、ＫＦ予測位置向メモリ１３ｄに保存する（Ｓ６）。前回値メモリ１３ｇには、後述する前回のＳ９又はＳ１１の処理で推定された、最終的なユーザＨの現在位置や向き、速度や角速度が記憶される。かかる前回状態が継続した場合に、予測されるユーザＨの位置および向きが算出される。

ここで、前回値メモリ１３ｇに記憶されるユーザＨのｘ軸方向の位置をＰ_{ｘ，ｋ−１}，ｙ軸方向の位置をＰ_{ｙ，ｋ−１}，ユーザＨの向きをＰ_{θ，ｋ−１}，ユーザＨのｘ軸方向の速度をＶ_{ｘ，ｋ−１}，ｙ軸方向の速度をＶ_{ｙ，ｋ−１}，ユーザＨの角速度をＶ_{θ，ｋ−１}とし、前回のＳ９又はＳ１１の処理からの経過時間をΔｔ，加速度外乱等によるユーザＨのシステム雑音成分をｎ_ｘｓ，ｎ_ｙｓ，ｎ_θｓとすると、予測されるユーザＨのｘ軸方向の位置Ｐ_ｘｐ，ｋ，ｙ軸方向の位置Ｐ_ｙｐ，ｋ，ユーザＨの向きＰ_θｐ，ｋ，ユーザＨのｘ軸方向の速度Ｖ_ｘｐ，ｋ，ｙ軸方向の速度Ｖ_ｙｐ，ｋ，ユーザＨの角速度Ｖ_θｐ，ｋは、公知のカルマンフィルタ予測ステップである数式９によって算出される。

かかる数式９で予測されたユーザＨのｘ軸方向の位置Ｐ_ｘｐ，ｋ，ｙ軸方向の位置Ｐ_ｙｐ，ｋ，ユーザＨの向きＰ_θｐ，ｋ，ユーザＨのｘ軸方向の速度Ｖ_ｘｐ，ｋ，ｙ軸方向の速度Ｖ_ｙｐ，ｋ，ユーザＨの角速度Ｖ_θｐ，ｋがＫＦ予測位置向メモリ１３ｄに記憶される。

Ｓ６の処理の後、楕円位置向メモリ１３ｂ，二次関数位置向メモリ１３ｃ及びＫＦ予測位置向メモリ１３ｄに記憶されるユーザＨの位置および向きに基づき相関処理を行い、該当するユーザＨの位置および向きを相関結果位置向メモリ１３ｅに保存する（Ｓ７）。

具体的には、まず、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃに記憶されるユーザＨの位置および向きを、ＫＦ予測位置向メモリ１３ｄに記憶される前回値を基準とした「ゲート」で絞り込みを行う。これによって、前回値に基づくユーザＨの位置および向きから大きく異なる、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃのユーザＨの位置および向きが排除される。

そして、ゲートによって絞り込まれたユーザＨの位置および向きの中から、更に最近傍法によって最もユーザＨの位置および向きらしいと推定されるものが選択され、相関結果位置向メモリ１３ｅに記憶される。この際、ゲートによる絞り込み及び最近傍法によって、１つもユーザＨの位置および向きが選択されなかった場合は、相関結果位置向メモリ１３ｅには、ユーザＨの位置および向きが選択されなかった旨を示す値が記憶される。なお、ゲートおよび最近傍法は公知の技術なので、これらの詳細な説明は省略する。

かかるゲートによる絞り込み及び最近傍法によって、近似楕円Ｃおよび二次関数Ｑから算出されたユーザＨの位置および向きのうち、最もユーザＨらしいと推定されるユーザＨの位置および向きが選択される。

Ｓ７の処理の後、相関結果位置向メモリ１３ｅにユーザＨの位置および向きが記憶されているかを確認する（Ｓ８）。ユーザＨの位置および向きが記憶されている場合（Ｓ８：Ｙｅｓ）、相関結果位置向メモリ１３ｅの値から、公知のカルマンフィルタによってユーザの現在の位置および向きを推定し、推定現在位置向メモリ１３ｆに保存する（Ｓ９）。具体的には、相関結果位置向メモリ１３ｅに記憶されるユーザＨのｘ軸方向の位置をＰ_ｘｃ，ｙ軸方向の位置をＰ_ｙｃ，ユーザＨの向きをＰ_θｃ，ユーザＨのｘ軸方向の速度をＶ_ｘｃ，ｙ軸方向の速度をＶ_ｙｃ，ユーザＨの角速度をＶ_θｃとし、測距センサ１６の検知誤差やユーザＨの動きによる尤度関数等による観測誤差成分をｎ_ｘｐ，ｎ_ｙｐ，ｎ_θｐとすると、ユーザＨのｘ軸方向の位置Ｐ_ｘ，ｋ，ｙ軸方向の位置Ｐ_ｙ，ｋ，ユーザＨの向きＰ_θ，ｋは、数式１０によって算出される。

なお、ユーザＨのｘ軸方向の速度Ｖ_ｘｃは、前回値メモリ１３ｇに記憶されるユーザＨのｘ軸方向の位置と、相関結果位置向メモリ１３ｅに記憶されるユーザＨのｘ軸方向の位置とに基づいて算出され、同様にユーザＨのｙ軸方向の速度Ｖ_ｙｃは、前回値メモリ１３ｇのユーザＨのｙ軸方向の位置と相関結果位置向メモリ１３ｅのｙ軸方向の位置とに基づいて算出され、ユーザＨの角速度Ｖ_θｃは、前回値メモリ１３ｇのユーザＨの向きと相関結果位置向メモリ１３ｅのユーザＨの向きとに基づいて算出される。

これにより、Ｓ７の処理で相関結果位置向メモリ１３ｅに記憶された、最もユーザＨらしいと推定されるユーザＨの位置および向きに、観測誤差成分、即ち測距センサ１６の検知誤差やユーザＨの動き等、現実的に発生し得る誤差が含まれるので、実際に観測されるユーザＨの位置および向きとの誤差が小さいユーザＨの位置および向きを推定することができる。

一方、相関結果位置向メモリ１３ｅに、ユーザＨの位置および向きが記憶されていない場合（Ｓ８：Ｎｏ）、即ち、Ｓ７の処理において、相関結果位置向メモリ１３ｅにユーザＨの位置および向きが選択されなかった旨を示す値が記憶された場合は、推定現在位置向メモリ１３ｆにＫＦ予測位置向メモリ１３ｄの値を保存する（Ｓ１０）。即ち、Ｓ７の処理において、Ｓ２〜Ｓ５の処理で算出されたユーザＨの位置および向きが、いずれもユーザＨらしくないと判断された場合でも、予測値ではあるものの、前回値メモリ１３ｇの値に基づく現在のユーザＨの位置および向きが推定現在位置向メモリ１３ｆに記憶される。これにより、該ユーザＨの位置および向きが更新されない事態を抑制できる。

Ｓ９，Ｓ１０の処理の後、推定現在位置向メモリ１３ｆのユーザＨの位置および向きに基づく制御信号を駆動部１８に出力して（Ｓ１１）、駆動部１８を動作させる。これにより、移動体１は推定されたユーザＨの位置および向きに基づいて移動動作される。

Ｓ１２の処理の後、推定現在位置向メモリ１３ｆに記憶されるユーザＨの位置および向きと、ユーザＨの速度および角速度とを前回値メモリ１３ｇへ保存する（Ｓ１２）。具体的には、まず、推定現在位置向メモリ１３ｆのユーザＨの位置および向きと、前回値メモリ１３ｇに記憶されるユーザＨの位置および向きとにより、ユーザＨの速度および角速度を算出し、かかるユーザＨの速度および角速度を前回値メモリ１３ｇに保存した後に、推定現在位置向メモリ１３ｆのユーザＨの位置および向きに保存する。そして、Ｓ１２の処理の後は、Ｓ１以下の処理を繰り返す。

以上説明した通り、本実施形態の移動体１は、測距センサ１６によって測距された測距データＭＰに基づいて、近似楕円Ｃと、ｘｙ軸およびｙｘ軸による二次関数Ｑｘｙ，Ｑｙｘとが算出され、それぞれのユーザＨの位置および向きが算出される。算出された複数のユーザＨの位置および向きから、ゲート及び最近傍法によって、最もユーザＨらしいと推定されるユーザＨの位置および向きが選択され、該ユーザＨの位置および向きから、現在のユーザＨの位置および向きが推定される。

ここで、移動体１とユーザＨとの位置関係は、時々刻々と、様々に変化する。そこで、測距データＭＰから近似楕円Ｃ，二次関数Ｑｘｙ，Ｑｙｘといった複数の形状および、複数の座標系による近似曲線を算出し、１の形状または座標系に基づいた近似曲線がユーザＨに上手く適合しない場合は、より適合する他の形状あるいは座標系に基づいた近似曲線に基づいて、ユーザＨの位置および向きが推定される。これにより、測距センサ１６とユーザＨとの位置関係が様々に変化しても、ユーザＨの位置および向きを検出できる。

以上、実施形態に基づき本発明を説明したが、本発明は上述した実施形態に何ら限定されるものではなく、本発明の趣旨を逸脱しない範囲内で種々の改良変更が可能であることは容易に推察できるものである。

上記実施形態では、測距データＭＰから算出される二次関数Ｑとして、ｘｙ座標系による二次関数Ｑｘｙと、ｙｘ座標系による二次関数Ｑｙｘとの２種類とする構成とした。しかし、必ずしもこれに限られるものではなく、ｘｚ座標系やｙｚ座標系による二次関数を算出しても良いし、他の座標系による二次関数Ｑを算出しても良い。

上記実施形態では、測距データＭＰを二次関数で近似する構成とした。しかし、必ずしもこれに限られるものではなく、三次関数や四次関数等、次数が３以上の多項式関数によって近似する構成としても良い。

上記実施形態では、二次関数Ｑｘｙはｘ軸に沿って直立する形状とし、二次関数Ｑｙｘの軸線はｙ軸に沿って直立する構成とした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑｘｙ，Ｑｙｘをｘ軸またはｙ軸側に傾ける形状とする構成としても良い。

上記実施形態では、測距センサ１６によって肩周辺の胴体が測距される構成とした。しかし、これに限られるものではなく、腹部周辺や腰周辺等、ユーザＨの胴体が測距される部位であれば、他の部位でも良い。

上記実施形態では、近似楕円Ｃと二次関数Ｑとに基づいて、ユーザＨの位置および向きを推定する構成とした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑのみに基づいて、ユーザＨの位置および向きを推定しても良いし、近似楕円Ｃのみに基づいて、ユーザＨの位置および向きを推定しても良い。

上記実施形態では、近似楕円Ｃの長軸ａ及び短軸ｂがユーザＨの肩幅Ｗ１及び厚さＷ２に近似する場合、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂへ記憶する構成とした。しかし、必ずしもこれに限られるものではなく、近似楕円Ｃの円周がユーザＨの肩周辺の外周の長さと近似する場合に、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂに記憶する構成としても良いし、近似楕円Ｃの面積がユーザＨの肩周辺の断面積と近似する場合に、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂに記憶する構成としても良い。

上記実施形態では、二次関数ＱにおけるクラスタエッジＥ１，Ｅ２を、測距データＭＰのうち両端の測距データＭＰとした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑ上の点であって、測距データＭＰのうち一方の端の測距データＭＰに近似する点をクラスタエッジＥ１とし、他方の端の測距データＭＰに近似する点をクラスタエッジＥ２とする構成としても良い。

上記実施形態では、近似楕円ＣにおけるユーザＨの位置を近似楕円Ｃの中心ＣＰとし、二次関数Ｑｘｙ，ＱｙｘにおけるユーザＨの位置をクラスタエッジＥ１，Ｅ２の中点Ｅｃとする構成とした。しかし、必ずしもこれに限られるものではなく、近似楕円Ｃ及び二次関数ＱにおけるユーザＨの位置をいずれも近似楕円Ｃの中心ＣＰとしても良いし、クラスタエッジＥ１，Ｅ２の中点Ｅｃとしても良い。また、測距データＭＰに基づく最小二乗法等、他の手法によって算出されたユーザＨの位置を用いても良い。

上記実施形態では、クラスタエッジＥ１とクラスタエッジＥ２との間に頂点Ｔが存在し、かつクラスタエッジＥ２と頂点Ｔとが近い場合は、中点Ｅｃと頂点Ｔとを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２を算出する構成とした。しかし、必ずしもこれに限られるものではなく、かかる場合に、クラスタエッジＥ２と頂点Ｔとが離れている場合と同様に、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２を算出する構成としても良い。

上記実施形態に挙げた数値は一例であり、他の数値を採用することは当然可能である。

１０ 制御部（身体向推定装置）
１２ａ 制御プログラム（身体向推定プログラム）
Ｈ ユーザ（対象者）
１６ 測距センサ（測距手段、測距取得機能）
ＭＰ 測距データ
Ｑ，Ｑｘｙ，Ｑｙｘ 二次関数
Ｃ，Ｃ１，Ｃ２ 近似楕円
Ｅ１，Ｅ２ クラスタエッジ
Ｓ２ 楕円算出手段
Ｓ３ 向き推定手段、向き推定機能
Ｓ４ 二次関数算出手段、二次関数算出機能
Ｓ５ 二次関数選択手段、二次関数選択機能、
向き推定手段、向き推定機能
第１向き推定手段、第２向き推定手段
Ｓ７〜Ｓ１０ 向き推定手段、向き推定機能
Ｔ 頂点

Claims (11)

1. 対象者を測距する測距手段と、
   その測距手段により測距された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出手段と、
   その二次関数算出手段により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択手段と、
   その二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定する向き推定手段と、
   を備えていることを特徴とする身体向推定装置。
2. 前記二次関数選択手段により選択された二次関数の算出に用いられた測距データのうち、その二次関数上の一方の端に該当する測距データと、他方の端に該当する測距データとの２点をクラスタエッジと称し、或いは、前記二次関数選択手段により選択された二次関数上の点であって、複数の前記測距データのうち、一方の端の測距データに近似される点と、他方の端の測距データに近似される点との２点をクラスタエッジと称し、
   前記向き推定手段は、前記クラスタエッジを結ぶ直線に直交する方向を、前記対象者の向きとして推定する第１向き推定手段を備えていることを特徴とする請求項１記載の身体向推定装置。
3. 前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在しない場合に、前記第１向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項２記載の身体向推定装置。
4. 前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在し且つ第１所定条件を満たす場合に、前記第１向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項２又は３に記載の身体向推定装置。
5. 前記二次関数選択手段により選択された二次関数の算出に用いられた測距データのうち、その二次関数上の一方の端に該当する測距データと、他方の端に該当する測距データとの２点をクラスタエッジと称し、或いは、前記二次関数選択手段により選択された二次関数上の点であって、複数の前記測距データのうち、一方の端の測距データに近似される点と、他方の端の測距データに近似される点との２点をクラスタエッジと称し、
   前記向き推定手段は、前記クラスタエッジの中点と前記二次関数選択手段により選択された二次関数の頂点とを結ぶ直線に直交する方向を、前記対象者の向きとして推定する第２向き推定手段を備えていることを特徴とする請求項１から４のいずれかに記載の身体向推定装置。
6. 前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在し且つ第２所定条件を満たす場合に、前記第２向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項５記載の身体向推定装置。
7. 前記二次関数算出手段は、複数の前記測距データを基にして近似される二次関数を、ｘｙ座標系とｙｘ座標系との少なくとも２つの座標系に基づいて、それぞれ算出するものであることを特徴とする請求項１から６のいずれかに記載の身体向推定装置。
8. 前記測距手段により測距された複数の測距データに基づいて近似楕円を算出する楕円算出手段を備え、
   前記向き推定手段は、その楕円算出手段により算出された楕円または前記二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定するものであることを特徴とする請求項１から７のいずれかに記載の身体向推定装置。
9. 前記向き推定手段は、前記楕円算出手段により算出された楕円に基づいて推定された前記対象者の向きと、前記二次関数選択手段により選択された二次関数に基づいて推定された前記対象者の向きと、前記対象者の前回推定された向きに基づいて予測された前記対象者の今回予測向きとのうち、いずれかの向きを前記対象者の今回予測向きに基づく相関処理により前記対象者の向きとして推定するものであることを特徴とする請求項８記載の身体向推定装置。
10. 前記向き推定手段は、前記対象者の前記推定された向きを、更にカルマンフィルタによって更新し、その更新後の向きを前記対象者の向きとして推定するものであることを特徴とする請求項１から９のいずれかに記載の身体向推定装置。
11. 対象者を測距した測距データを取得する測距取得機能と、
    その測距取得機能により取得された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出機能と、
    その二次関数算出機能により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択機能と、
    その二次関数選択機能により選択された二次関数に基づいて前記対象者の向きを推定する向き推定機能と、
    をコンピュータに実現させることを特徴とする身体向推定プログラム。
    

Images