位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法
【課題】移動体の正確な位置および姿勢を時間遅れなく出力する。
【解決手段】位置姿勢出力装置１０は、移動体のオドメトリを計算する計算部３から、オドメトリを第１の間隔で取得する。オドメトリ及びオドメトリ以外の情報を用いて移動体の位置及び姿勢を推定する推定部５から、移動体の位置及び姿勢の推定結果を第１の間隔より長い第２の時間間隔で取得する。移動体の位置及び姿勢の推定結果が取得された場合に、更新後の移動体の位置及び姿勢の値が、オドメトリの差分に基づく位置及び姿勢と、移動体の位置及び姿勢の推定結果との間に遷移するように、記憶部１３に記憶された移動体の位置及び姿勢を更新する。オドメトリが第１の間隔で取得される度に、当該オドメトリと今回の推定に用いられたオドメトリとの差分を記憶部１３に記憶された移動体の位置及び姿勢に足し合わせた上で出力する。
【解決手段】位置姿勢出力装置１０は、移動体のオドメトリを計算する計算部３から、オドメトリを第１の間隔で取得する。オドメトリ及びオドメトリ以外の情報を用いて移動体の位置及び姿勢を推定する推定部５から、移動体の位置及び姿勢の推定結果を第１の間隔より長い第２の時間間隔で取得する。移動体の位置及び姿勢の推定結果が取得された場合に、更新後の移動体の位置及び姿勢の値が、オドメトリの差分に基づく位置及び姿勢と、移動体の位置及び姿勢の推定結果との間に遷移するように、記憶部１３に記憶された移動体の位置及び姿勢を更新する。オドメトリが第１の間隔で取得される度に、当該オドメトリと今回の推定に用いられたオドメトリとの差分を記憶部１３に記憶された移動体の位置及び姿勢に足し合わせた上で出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法に関する。
【背景技術】
【０００２】
移動体の位置および姿勢を計算する技術が知られている。例えば、オドメトリを計算する方法が挙げられる。かかる「オドメトリ」は、車輪の回転角度から位置および姿勢の変化量を計算した上でそれまでに求められていた位置および姿勢に累積して足し合わせることによって算出される。このように、オドメトリを計算する場合には、車輪が想定外の挙動を取ることによって生じる誤差や車輪等に設定されるパラメータの誤差も累積加算されるとともに、オドメトリの計算誤差も蓄積する。このため、移動体の移動量が大きくなるにしたがってオドメトリと実際の位置および姿勢との誤差も大きくなる。
【０００３】
このことから、オドメトリと他の情報とを併用して移動体の位置および姿勢を計算する技術も提案されている。一例としては、オドメトリセンサと撮像センサとを併用する位置推定装置が挙げられる。この位置推定装置では、オドメトリセンサを用いて予測された移動体の位置及び／又は姿勢をもとに撮像センサを用いて撮像された画像から目印やランドマークなどの特徴点を追跡できるか否かによって予測の正誤を判断する。このとき、位置推定装置は、位置及び／又は姿勢の予測が正しくないと判断した場合に、位置及び／又は姿勢の予測を修正する。また、位置推定装置は、位置及び／又は姿勢の予測が正しいと判断した場合に、移動体の位置及び／又は姿勢を更新する。
【０００４】
他の一例としては、ジャイロを用いることなく車両の３次元の位置・姿勢を標定する標定装置が挙げられる。この標定装置では、慣性センサとオドメトリとにより計測された角速度と速度とに基づいて車両の自己姿勢角と自己位置とを計測する。そして、標定装置は、先に計測された車両の自己姿勢角および自己位置を用いて予測された白線の消失点予測値と、画像センサによって撮像された画像に映っている白線の消失点実測値との差分である消失点残差に基づいて姿勢誤差推定量を算出する。その上で、標定装置は、姿勢誤差推定量を用いて自己姿勢角を補正する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００７−３０３８８６号公報
【特許文献２】特開２００９−２０４３８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の従来技術では、以下に説明するように、移動体の正確な位置および姿勢を時間遅れなく出力することができないという問題がある。
【０００７】
すなわち、上記の位置推定装置および標定装置では、オドメトリとともに画像等の他の情報を用いて移動体の位置および姿勢を推定するので、画像処理を実行するためにオドメトリの計算に比べて長い処理時間が必要となる。このため、上記の位置推定装置および標定装置では、移動体の位置および姿勢を推定し始めてからその推定結果が出力されるまでのタイムラグが大きくなる。よって、上記の位置推定装置および標定装置では、推定結果に対応する位置および姿勢が移動体によって実際に取られていた時点から遅れて位置および姿勢の推定結果が出力されてしまう。
【０００８】
また、上記の位置推定装置および標定装置では、画像等の他の情報を用いて移動体の位置および姿勢を推定する場合に、移動体の位置および姿勢の推定精度にばらつきが生じてしまう。なぜなら、上記の位置推定装置では、画像から目印やランドマークなどの特徴点を追跡し、また、上記の標定装置では、画像から白線の消失点を検出するが、画像には実際の特徴点や実際の白線の消失点と混同されやすいノイズが含まれる場合があるからである。かかる「ノイズ」が画像に含まれる場合には、ノイズを特徴点や白線の消失点と誤認し、移動体の位置および姿勢の推定結果に誤りが生じてしまう。
【０００９】
開示の技術は、上記に鑑みてなされたものであって、移動体の正確な位置および姿勢を時間遅れなく出力できる位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本願の開示する位置姿勢出力装置は、移動体の位置および姿勢を記憶する記憶部を有する。前記位置姿勢出力装置は、前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得する第１の取得部を有する。前記位置姿勢出力装置は、前記オドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得する第２の取得部を有する。さらに、前記位置姿勢出力装置は、前記第２の取得部によって前記推定結果が取得された場合に、前記記憶部に記憶された移動体の位置および姿勢を更新する更新部を有する。前記更新部は、更新後の移動体の位置および姿勢の値が、オドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新する。前記オドメトリの差分に基づく位置および姿勢は、前記記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られる。前記位置姿勢出力装置は、前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する出力部を有する。
【発明の効果】
【００１１】
本願の開示する位置姿勢出力装置の一つの態様によれば、移動体の正確な位置および姿勢を時間遅れなく出力できるという効果を奏する。
【図面の簡単な説明】
【００１２】
【図１】図１は、実施例１に係るロボットの機能的構成を示すブロック図である。
【図２】図２は、実施例１に係る位置姿勢出力装置の機能的構成を示すブロック図である。
【図３】図３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図４】図４は、内部状態量の更新方法の一例を示す図である。
【図５】図５は、フィルタリング結果の算出方法を示す図である。
【図６】図６は、実施例１に係る出力処理の手順を示すフローチャートである。
【図７】図７は、実施例１に係る算出処理の手順を示すフローチャートである。
【図８】図８は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。
【図９】図９は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。
【図１０】図１０は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値の一例を示す図である。
【図１１】図１１は、内部状態量の更新方法の応用例を示す図である。
【図１２】図１２は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図１３】図１３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図１４】図１４は、応用例に係る出力処理の手順を示すフローチャートである。
【図１５】図１５は、実施例１及び実施例２に係る位置姿勢出力プログラムを実行するコンピュータの一例について説明するための図である。
【発明を実施するための形態】
【００１３】
以下に、本願の開示する位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法の実施例を図面に基づいて詳細に説明する。なお、この実施例は開示の技術を限定するものではない。そして、各実施例は、処理内容を矛盾させない範囲で適宜組み合わせることが可能である。
【実施例１】
【００１４】
［ロボットの構成］
まず、本実施例に係る位置姿勢出力装置を含むロボットの機能的構成について説明する。図１は、実施例１に係るロボットの機能的構成を示すブロック図である。図１に示すロボット１は、オフィス、廊下やショッピングモール等の空間において所定の目標の位置、例えば予め定められた位置や外部から指示入力された位置に向かって自律走行する装置である。
【００１５】
図１に示すように、ロボット１は、計算部３と、センサ５ａと、推定部５と、経路計画部７と、移動制御部９と、位置姿勢出力装置１０とを有する。なお、ロボット１は、図１に示した機能部以外にも既知のロボットが有する各種の機能部、例えば各種の入力デバイスや音声出力デバイスなどの他、車軸や車輪、これらを駆動する駆動部などの機能を有するものとする。
【００１６】
計算部３は、ロボット１のオドメトリを計算する処理部である。一態様としては、計算部３は、ロボット１の車輪の回転角度から位置および姿勢の変化量を計算した上でそれまでに求めていた位置および姿勢に累積して足し合わせることによってオドメトリを計算する。このように、計算部３によって計算されたオドメトリは、後述の位置姿勢出力装置１０へ出力される。
【００１７】
推定部５は、撮像センサやＬＲＦ（Laser Range Finder）などのセンサ５ａによって採取されるセンサ情報を用いて、ロボット１の位置および姿勢を推定する処理部である。一態様としては、推定部５は、センサ５ａによって出力されたセンサ情報を予め作成された地図情報との間でマップマッチングすることにより、ロボット１の位置および姿勢を推定する。かかる位置および姿勢の推定には、上述したマップマッチングの他にも公知の任意の手法を採用することができる。このように、推定部５によって推定された位置および姿勢の推定結果は、後述の位置姿勢出力装置１０へ出力される。なお、以下では、位置および姿勢のことを「位置・姿勢」と記載する場合がある。
【００１８】
経路計画部７は、ロボット１が移動する経路を計画する処理部である。一態様としては、経路計画部７は、ロボット１が目標の位置に到達または通過できるように、後述の位置姿勢出力装置１０によって出力されたロボット１の位置・姿勢から、ロボット１を移動させる速度や角速度などの移動制御用のパラメータを決定する。このように、経路計画部７によって決定された移動制御用のパラメータは、後述の移動制御部９へ出力される。
【００１９】
移動制御部９は、ロボット１の移動を制御する処理部である。一態様としては、移動制御部９は、経路計画部７によって出力された移動制御用のパラメータにしたがってロボット１の車輪を駆動させたり、車輪の回転に制動を掛けたり、あるいはロボット１本体の向きを回転させたりする。
【００２０】
位置姿勢出力装置１０は、計算部３および推定部５によって互いに異なる周期で出力されるロボット１のオドメトリおよび位置・姿勢をフィルタリングした上で経路計画部７へ出力する装置である。一態様としては、位置姿勢出力装置１０は、計算部３によって計算されたオドメトリおよび推定部５によって推定された位置・姿勢をもとに、内部状態量を更新しつつ、内部状態量を参照してロボット１の位置・姿勢を出力する。ここで言う「内部状態量」とは、位置姿勢出力装置１０によって内部的に管理されるロボット１の状態量を指す。なお、以下では、位置姿勢出力装置１０が出力する位置・姿勢のことを「フィルタリング結果」と記載する場合がある。
【００２１】
［位置姿勢出力装置の構成］
続いて、本実施例に係る位置姿勢出力装置の機能的構成について説明する。図２は、実施例１に係る位置姿勢出力装置の機能的構成を示すブロック図である。図２に示すように、位置姿勢出力装置１０は、第１の取得部１１と、第２の取得部１２と、記憶部１３と、更新部１４と、算出部１５と、出力部１６とを有する。
【００２２】
このうち、第１の取得部１１は、計算部３によって出力されるロボット１のオドメトリを取得する処理部である。また、第２の取得部１２は、推定部５によって出力されるロボット１の位置・姿勢の推定結果を取得する処理部である。さらに、第２の取得部１２は、ロボット１の位置・姿勢の推定結果とともに、その推定に用いられたオドメトリの値も併せて取得する。
【００２３】
ここで、図３を用いて、オドメトリおよび位置・姿勢の推定結果が入力される周期の違いについて説明する。図３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。図３に示す「t」は、時刻を指し、「t＿new」は、計算部３によって最新のオドメトリが入力された時刻を指す。また、図３に示す「t’」は、推定部５によって位置・姿勢が推定された時刻を指し、図３に示す「t’-1」は、前回に推定部５によって位置・姿勢が推定された時刻を指す。なお、図３の例では、オドメトリおよび位置・姿勢の推定結果の両者がともに一定の間隔で入力される場合を想定する。
【００２４】
図３に示すように、計算部３によって計算されたオドメトリが「50msec」の間隔で入力される一方で、推定部５によって推定された位置・姿勢の推定結果が「300msec」の間隔で入力される。このように、オドメトリが入力される周期が位置・姿勢の推定結果が入力される周期よりも短いのは、オドメトリが移動速度の時間積分という簡易な演算によって算出できるからである。また、位置・姿勢の推定結果が入力される周期がオドメトリが入力される周期よりも長いのは、画像やレーザー光の反射光などを解析する処理にオドメトリの計算に比べて長い処理時間を要するからである。なお、ここでは、オドメトリの入力周期が「50msec」であり、位置・姿勢の推定結果が「300msec」である場合を例示したが、これらの入力周期はあくまでも一例である。各々の入力周期は、任意の値であってかまわないが、オドメトリの入力周期は位置・姿勢の推定結果の入力周期よりも短いことが好ましい。
【００２５】
記憶部１３は、ロボット１の内部状態量を記憶するものである。かかる内部状態量の一態様としては、浮動小数点型のｘ座標位置（ｍ）、ｙ座標位置（ｍ）および姿勢θ（rad）が対応付けられたデータが採用される。一例としては、第２の取得部１２によって位置・姿勢の推定結果が取得される度に、後述の更新部１４によって内部状態量が更新される。他の一例としては、第１の取得部１１によってオドメトリが取得される度に、後述の出力部１６によって内部状態量が参照される。なお、内部状態量の初期状態には、ローカル座標系の原点として位置・姿勢（x,y,θ）＝（0,0,0）を設定することもできるし、グローバル座標系におけるロボット１の所在位置およびその姿勢を設定することもできる。
【００２６】
この記憶部１３には、半導体メモリ素子や記憶装置を採用できる。例えば、半導体メモリ素子の一例としては、ＶＲＡＭ（Video Random Access Memory）、ＲＡＭ（Random Access Memory)、ＲＯＭ（Read Only Memory）やフラッシュメモリ（flash memory）などが挙げられる。また、記憶装置の一例としては、ハードディスク、光ディスクなどの記憶装置が挙げられる。
【００２７】
更新部１４は、内部状態量を更新する処理部である。一態様としては、更新部１４は、第２の取得部１２によって位置・姿勢の推定結果が取得された場合に、処理を起動する。このとき、更新部１４は、更新後の内部状態量が、更新前の内部状態量に今回の推定に用いられたオドメトリおよび前回の推定に用いられたオドメトリの差分が加算された値と、今回の位置・姿勢の推定結果との間に遷移するように、内部状態量を更新する。
【００２８】
ここで、図４を用いて、内部状態量の更新方法について説明する。図４は、内部状態量の更新方法の一例を示す図である。図４に示す「E(t)」は、位置・姿勢の推定結果を指し、例えば、「E(t’)」と表記された場合には今回推定された位置・姿勢の推定結果を表し、「E(t’-1)」と表記された場合には前回推定された位置・姿勢の推定結果を表す。また、図４に示す「z(t)」は、内部状態量を指し、例えば、「z(t’)」と表記された場合には更新後の内部状態量を表し、「z(t’-1)」と表記された場合には更新前の内部状態量を表す。また、図４に示す「E＿odo(t)」は、位置・姿勢の推定に用いられたオドメトリを指し、例えば、「E＿odo(t’)」と表記された場合には今回の推定に用いられたオドメトリを表し、「E＿odo(t’-1)」と表記された場合には前回の推定に用いられたオドメトリを表す。
【００２９】
図４に示すように、更新部１４は、推定部５によって推定された位置・姿勢の推定結果「E(t’)」をそのまま内部状態量「z(t’)」と同定する更新を必ずしも実行しない。その理由の一因としては、推定部５による位置・姿勢の推定結果にはセンサ５ａによって採取されたセンサ情報に含まれるノイズが原因となって推定精度にばらつきが生じるからである。それゆえ、更新部１４は、位置・姿勢の推定結果「E(t’)」だけに依存せずに内部状態量z(t)を更新するために、今回の推定に用いられたオドメトリと前回の推定に用いられたオドメトリとの差分「E＿odo(t’)-E＿odo(t’-1)」を使用する。なぜなら、オドメトリの値は、ロボット１の移動量が大きくなるに伴って実際の位置・姿勢との間で誤差も大きくなるが、オドメトリの差分は位置・姿勢の推定結果が出力される周期という局所的な範囲で見れば誤差は小さく、実値と近い値と推定できるからである。
【００３０】
このため、更新部１４は、更新前の内部状態量z(t’-1)に上記のオドメトリの差分「E＿odo(t’)-E＿odo(t’-1)」を加算することによってオドメトリの差分に基づく内部状態量、すなわち図中の白丸の位置・姿勢を算出する。そして、更新部１４は、今回の位置・姿勢の推定結果E(t’)から上記のオドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」を減算した上でその減算値「D」に後述の定数ω（０＜ω≦１）を乗算する。これによって、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へ、更新後の内部状態量z(t’)をどれだけシフトさせるかを表すシフト値dsを導出する。その上で、更新部１４は、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」にシフト値dsを足し合わせることによって、更新後の内部状態量z(t’)を算出する。なお、上記の定数ωは、ωの値が「１」に近づくほど、内部状態量z(t')は位置・姿勢の推定結果に近づく。また、ωの値が「０」に近づくほど、内部状態量z(t')は更新前の内部状態量z(t'-1)にオドメトリの差分を加えた値に近づく。
【００３１】
より詳細には、更新部１４は、推定部５によって位置・姿勢の推定結果「E(t’)」が出力さると、下記の式（１）に次の値を代入することによってシフト値dsを算出する。すなわち、更新部１４は、更新前の内部状態量z(t’-1)、今回の位置・姿勢の推定結果「E(t’)」、今回の推定に用いられたオドメトリ「E＿odo(t’)」および前回の推定に用いられたオドメトリ「E＿odo(t’-1)」を下記の式（１）に代入する。その上で、更新部１４は、式（１）を用いて算出したシフト値dsを下記の式（２）に代入することによって更新後の内部状態量z(t’)を算出した上でその更新後の内部状態量z(t’)を記憶部１３に上書きして更新する。
【００３２】
【数１】
【数２】
【００３３】
このように、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフト値dsの分、シフトさせる内部状態量z(t)の更新を実行する。これによって、位置・姿勢の推定結果がばらつくという弱点をオドメトリの差分によって補完することができる結果、内部状態量z(t)を実際の位置・姿勢と近い値に保つことができる。さらに、内部状態量z(t)を更新する方式としてKalman Filterを使用する方法も考えられるが、上記の更新方法では、Kalman Filterを使用する場合のように、入力の共分散行列を受け取る必要がなく、Kalman Filterを使用する場合よりも効率的に更新できる。
【００３４】
算出部１５は、シフト値dsに乗ずる定数ωを算出する処理部である。ここでは、一例として、ロボット１を予め計画された走行経路で自律走行された場合における位置・姿勢の出力結果のログを用いて定数ωを算出する場合を例示する。なお、上記のログとして、後述の出力部１６によって出力されたロボット１のフィルタリング結果のログを用いることとしてもよいし、また、外部装置から供給されたロボット１または他のロボットの位置・姿勢のログを用いることとしてもかまわない。
【００３５】
これを説明すると、まず、算出部１５は、算出対象とする定数ωを「１」とし、後述の自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅを「０」とするパラメータの初期化を実行する。その後、算出部１５は、ロボット１の位置・姿勢の出力結果のログを取得する。そして、算出部１５は、ログ取得したロボット１の位置の推移、すなわち位置の点列に含まれる位置と、予め計画された移動軌跡のうち最も近い位置との距離の自乗和Ｓを計算する。続いて、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きいか否かを判定する。なお、自乗和Ｓの計算が始めて実行された場合には、パラメータの初期化によって１回前の計算結果Ｓ＿ｐｒｅが「０」に設定されている。
【００３６】
このとき、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロであるか、もしくは今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅ以下である場合には、次のように、定数ωおよび１回前の計算結果Ｓ＿ｐｒｅを変更して自乗和Ｓの計算を繰り返す。すなわち、算出部１５は、定数ωをω／２に変更するとともに１回前の計算結果Ｓ＿ｐｒｅを今回計算したＳに変更した上で、変更後の定数ωを用いて出力された位置・姿勢のログを改めて取得し、自乗和Ｓを再び計算する。その後、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きい場合には、今回計算したωの２倍の２ωを定数ωとして決定する。
【００３７】
このように定数ωを算出することによって、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフトさせる重みを適切に決定できる結果、内部状態量z(t)を正確に算出できる。
【００３８】
なお、ここでは、ロボット１を実際に自律走行させた位置・姿勢のログを用いて定数ωを算出する場合を例示したが、ロボット１の管理者から定数ωとして経験的に得られる理想値を受け付け、その理想値を採用することとしてもよい。一例としては、今回の位置・姿勢の推定結果「E(t’)」よりもオドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」寄りへシフトさせる値、例えば「0.3」を定数ωとして採用するのが好ましい。
【００３９】
出力部１６は、記憶部１３に記憶された内部状態量z(t)を用いて、フィルタリング結果を算出した上で経路計画部７へ出力する処理部である。一態様としては、出力部１６は、最新のオドメトリと今回の推定に用いられたオドメトリとのオドメトリの差分を内部状態量に足し合わせることによってフィルタリング結果を算出する。
【００４０】
図５は、フィルタリング結果の算出方法を示す図である。図５に示すように、出力部１６は、第１の取得部１１によってオドメトリが取得される度に、次のようにして、フィルタリング結果を算出する。すなわち、出力部１６は、下記の式（３）のように、内部状態量z(t’)に最新のオドメトリodo(t＿new)と今回の推定に用いられたオドメトリE＿odo(t’)とのオドメトリの差分を足し合わせることによってフィルタリング結果f(t＿new)を算出する。そして、出力部１６は、フィルタリング結果f(t＿new)を経路計画部７へ出力する。
【００４１】
【数３】
【００４２】
このように、上記のオドメトリの差分「odo(t＿new)-E＿odo(t’)」を用いてフィルタリング結果を算出することにより、位置・姿勢の推定結果が入力されずともロボット１の位置・姿勢を導出できる。このため、位置・姿勢の推定結果が入力されるのを待機せずに、位置・姿勢の推定結果が入力されるよりも短い周期、すなわちオドメトリが入力される周期でフィルタリング結果を出力できる。したがって、フィルタリング結果に対応する位置・姿勢がロボット１によって実際に取られていた時点からフィルタリング結果を出力するまでの遅れを低減できる結果、時間遅れを抑制できる。
【００４３】
なお、第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６には、各種の集積回路や電子回路を採用できる。例えば、集積回路の一例としては、ＡＳＩＣ（Application Specific Integrated Circuit）が挙げられる。また、電子回路の一例としては、ＣＰＵ（Central Processing Unit）やＭＰＵ（Micro Processing Unit）などが挙げられる。
【００４４】
［計算例］
次に、内部状態量の更新およびフィルタリング結果の算出に関する計算例を説明する。なお、E(t)、E＿odo(t)、f(t)、odo(t)、z(t)、dsのデータ型は、それぞれ浮動小数点型のｘ座標位置（m）、ｙ座標位置（m）、姿勢θ（rad）を含んで構成されるものとする。さらに、ここでは、内部状態量として初期の位置・姿勢（x,y,θ）として（0,0,0）が設定される場合を想定して以下の説明を行う。加えて、以下では、これらデータの要素を区別して説明する場合には、「E(t).x」、「E(t).y」「E(t).θ」のように記載する場合がある。
【００４５】
また、本計算例では、一例として、オドメトリの座標系における姿勢が「θ＝π」であり、位置・姿勢の推定に用いられる座標系における姿勢が「θ＝０」である場合を想定する。以下では、オドメトリの座標系のことを「オドメトリ座標系」と記載し、位置・姿勢の推定に用いられる座標系のことを「位置姿勢推定座標系」と記載する場合がある。なお、内部状態量は、位置・姿勢推定座標系と同様の座標系が採用されているものとする。
【００４６】
このとき、内部状態量z(t’-1)の位置・姿勢（x,y,θ）が（5.0,5.0,1.57）であり、前回の推定に用いられたオドメトリE＿odo(t’-1)が（1.0,1.0,3.12）であったものとする。さらに、時刻t'において今回の位置・姿勢の推定結果E(t’)が（6.0,6.0,1.44）であり、今回の推定に用いられたオドメトリE＿odo(t’)が（2.0,2.0,3.0）であったものとする。
【００４７】
このような前提の下、更新部１４は、上記の式（１）にしたがってシフト値dsの（x,y,θ）を算出する。まず、更新部１４は、位置・姿勢の推定計算に用いられたオドメトリの差分E＿odo(t’)-E＿odo(t’-1)を（1.0,1.0,-0.12）と算出する。
【００４８】
さらに、更新部１４は、z(t’-1)+E＿odo(t’)-E＿odo(t’-1)を算出するが、前述のように、オドメトリ座標系における姿勢と位置・姿勢推定座標系における姿勢は異なるので、オドメトリ座標系から位置・姿勢推定座標系へと座標変換する。座標変換式を以下に記す。
θ(t’)=E(t’).θ-E＿odo(t’).θ
dx=(E＿odo(t’).x-E＿odo(t’-1).x)・cosθ(t’)-(E＿odo(t’).y-E＿odo(t’-1).y)・sinθ(t’)
dy=(E＿odo(t’).x-E＿odo(t’-1).x)・sinθ(t’)+(E＿odo(t’).y-E＿odo(t’-1).y)・cosθ(t’)
【００４９】
上記の座標変換式をE＿odo(t’)-E＿odo(t’-1)に適用することにより、更新部１４は、z(t’-1)+E＿odo(t’)-E＿odo(t’-1)を計算する。
すなわち、
z(t’-1).x+dx=5.0+1.0・cos(-1.56)-1.0・sin(-1.56)=5.0+0.01+1.00=6.01
z(t’-1).y+dy=5.0+1.0・sin(-1.56)+1.0・cos(-1.56)=5.0-1.00+0.01=4.01
z(t’-1).θ+E＿odo(t’).θ-E＿odo(t’-1).θ=1.45
となる。
【００５０】
したがって、更新部１４は、E(t’)-(z(t’-1)+E＿odo(t’)-E＿odo(t’-1))により計算される(x,y,θ)を（-0.01,1.99,-0.01）と算出する。
【００５１】
ここで定数ωの値が「0.3」であったとすると、更新部１４は、上記の式（１）により、シフト値dsを（-0.003,0.66,-0.003）と算出する。
【００５２】
さらに、更新部１４は、上記の式（２）にしたがって内部状態量z(t’)を計算する。
すなわち、
z(t’).x=6.01-0.003=6.007
z(t’).y=4.01+0.66=4.67
z(t’).θ=1.45-0.003=1.497
と算出する。
【００５３】
その後、出力部１６は、時刻t＿newにおいて最新のオドメトリが入力された場合に、上記の式（３）を用いてフィルタリング結果を計算する。この場合も、出力部１６は、内部状態量z(t’)とオドメトリの差分odo(t＿new)-E＿odo(t’)の座標系は異なるので、座標変換を実行する。
【００５４】
すなわち、出力部１６は、上記の同様に、
θ(t’)=E(t’).θ(t’)-E＿odo(t’).θ(t’)
dx=(odo(t＿new).x-E＿odo(t’).x)・cosθ(t’)-(odo(t＿new).y-E＿odo(t’).y)・sinθ(t’)
dy=(odo(t＿new).x-E＿odo(t’).x)・sinθ(t’)+(odo(t＿new).y-E＿odo(t’).y)・cosθ(t’)
の座標変換式を適用する。
【００５５】
そして、出力部１６は、最新のオドメトリodo(t＿new)が（2.5,2.5,2.8）であるとしたとき、下記のように、フィルタリング結果を計算する。
f(t＿new).x=6.007+0.5・cos(-1.56)-0.5・sin(-1.56)=6.507
f(t＿new).y=4.67+0.5・cos(-1.56)+0.5・sin(-1.56)=4.17
f(t＿new).θ=1.497-0.2=1.297
【００５６】
［処理の流れ］
次に、本実施例に係る位置姿勢出力装置の処理の流れについて説明する。なお、ここでは、位置姿勢出力装置が実行する（１）出力処理を説明した後に、（２）算出処理を説明することとする。
【００５７】
（１）出力処理
図６は、実施例１に係る出力処理の手順を示すフローチャートである。この出力処理は、ロボット１の電源が作動した場合に起動する処理であり、電源がＯＦＦ状態になるまで繰り返し実行される。
【００５８】
図６に示すように、更新部１４は、内部状態量の初期の位置・姿勢（x,y,θ）として（0,0,0）を設定する（ステップＳ１０１）。
【００５９】
そして、位置・姿勢の推定結果が取得された場合（ステップＳ１０２肯定）には、更新部１４は、次のような処理を実行する（ステップＳ１０３）。すなわち、更新部１４は、更新前の内部状態量z(t’-1)、今回の位置・姿勢の推定結果「E(t’)」、今回の推定に用いられたオドメトリ「E＿odo(t’)」および前回の推定に用いられたオドメトリ「E＿odo(t’-1)」を上記の式（１）に代入してシフト値dsを算出する。なお、位置・姿勢の推定結果が取得されていない場合（ステップＳ１０２否定）には、下記のステップＳ１０５の処理に移行する。
【００６０】
その後、更新部１４は、式（１）を用いて算出したシフト値dsを下記の式（２）に代入することによって更新後の内部状態量z(t’)を算出した上でその更新後の内部状態量z(t’)を記憶部１３に上書きして更新する（ステップＳ１０４）。
【００６１】
そして、最新のオドメトリが取得されるのを待機し（ステップＳ１０５）、最新のオドメトリが取得されると、出力部１６は、次のような処理を実行する（ステップＳ１０６）。すなわち、出力部１６は、下記の式（３）のように、内部状態量z(t’)に最新のオドメトリodo(t＿new)と今回の推定に用いられたオドメトリE＿odo(t’)との差分を足し合わせることによってフィルタリング結果f(t＿new)を算出して経路計画部７へ出力する。
【００６２】
その後、上記のステップＳ１０２の処理に戻り、位置姿勢出力装置１０は、ロボット１の電源がＯＦＦ状態になるまで、上記のステップＳ１０２〜ステップＳ１０６までの処理を繰り返し実行する。
【００６３】
（２）算出処理
図７は、実施例１に係る算出処理の手順を示すフローチャートである。この算出処理は、ロボット１の管理者によって定数ωの算出が指示された場合等に処理が起動する。
【００６４】
図７に示すように、算出部１５は、算出対象とする定数ωを「１」とし、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅを「０」とするパラメータの初期化を実行する（ステップＳ３０１）。その後、算出部１５は、ロボット１の位置・姿勢の出力結果のログを取得する（ステップＳ３０２）。
【００６５】
そして、算出部１５は、ログ取得したロボット１の位置の推移、すなわち位置の点列に含まれる位置と、予め計画された移動軌跡のうち最も近い位置との距離の自乗和Ｓを計算する（ステップＳ３０３）。
【００６６】
続いて、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きいか否かを判定する（ステップＳ３０４）。なお、自乗和Ｓの計算が始めて実行された場合には、パラメータの初期化によって１回前の計算結果Ｓ＿ｐｒｅが「０」に設定されている。
【００６７】
このとき、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロであるか、もしくは今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅ以下である場合（ステップＳ３０４否定）には、次のような処理を実行する（ステップＳ３０５）。すなわち、算出部１５は、定数ωをω／２に変更するとともに１回前の計算結果Ｓ＿ｐｒｅを今回計算したＳに変更する。その上で、算出部１５は、変更後の定数ωを用いて出力された位置・姿勢のログを改めて取得し（ステップＳ３０２）、自乗和Ｓを再び計算する（ステップＳ３０３）。
【００６８】
その後、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きい場合（ステップＳ３０４肯定）には、算出部１５は、次のような処理を実行する。すなわち、算出部１５は、今回計算したωの２倍の２ωを定数ωとして決定し（ステップＳ３０６）、処理を終了する。
【００６９】
［実施例１の効果］
上述してきたように、本実施例に係る位置姿勢出力装置１０は、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフト値dsの分、シフトさせる内部状態量z(t)の更新を実行する。これによって、位置・姿勢の推定結果がばらつくという弱点をオドメトリの差分によって補完することができる結果、内部状態量z(t)を実際の位置・姿勢と近い値に保つことができる。また、本実施例に係る位置姿勢出力装置１０は、オドメトリの差分「odo(t＿new)-E＿odo(t’)」を用いてフィルタリング結果を算出することにより、位置・姿勢の推定結果が入力されずともロボット１の位置・姿勢を導出できる。このため、位置・姿勢の推定結果が入力されるのを待機せずに、位置・姿勢の推定結果が入力されるよりも短い周期、すなわちオドメトリが入力される周期でフィルタリング結果を出力できる。したがって、本実施例に係る位置姿勢出力装置１０によれば、ロボット１の正確な位置および姿勢を時間遅れなく出力することが可能になる。
【００７０】
ここで、本実施例に係る位置姿勢出力装置１０を用いた実験結果を引用して位置・姿勢の精度を検証する。図８及び図９は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。このうち、図８の例では、ロボット１をＹ軸と平行に直線移動させた場合の実験結果を示す。また、図９の例では、ロボット１をＸ軸と平行に直線移動させた場合の実験結果を示す。なお、図８及び図９のグラフでは、点線が推定部５による位置・姿勢の推定結果を示し、実線が出力部１６によるフィルタリング結果を示す。
【００７１】
図８及び図９に示すように、推定部５によって推定された位置・姿勢の推定結果の位置は、数十cmのオーダーでばらついている。その一方で、出力部１６によって出力されたフィルタリング結果の位置のばらつきは十cm程度である。このため、推定部５によって推定された位置・姿勢の推定結果よりも出力部１６によって出力されたフィルタリング結果の方がロボット１の実際の挙動に近いことがわかる。それゆえ、本実施例に係る位置姿勢出力装置１０がロボット１の正確な位置および姿勢を出力できることが検証できたと言える。
【００７２】
さらに、図１０を用いて、時間遅れの低減について検証する。図１０は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値の一例を示す図である。この図１０の例では、ロボット１をＸ座標値が増加するように移動物体を移動させた場合を想定する。図１０に示すブラフの横軸は、ロボット１が移動を開始してからの経過時間を示し、縦軸は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値を示す。なお、図１０に示すグラフでは、点線が推定部５による位置・姿勢の推定結果を示し、実線が出力部１６によるフィルタリング結果を示し、一点鎖線が内部状態量を示す。
【００７３】
図１０に示すように、位置・姿勢の推定結果のＸ座標値のばらつきに対し、内部状態量のばらつきは小さい。また、フィルタリング結果は、最新のオドメトリを反映した値となっているので、内部状態量のＸ座標値よりもＸ座標値が大きい。さらに、内部状態量が長い周期、本例では約300msで更新されているのに対し、フィルタリング結果は短い周期、本例では約50msで更新されている。このように、出力部１６によって出力されたフィルタリング結果は、Ｘ座標値のばらつきが小さく、さらに、フィルタリング結果が出力された時点とロボット１がその位置に所在した時点とのライムラグも小さいことがわかる。それゆえ、本実施例に係る位置姿勢出力装置１０がロボット１の位置および姿勢を時間遅れなく出力できることが検証できたと言える。
【実施例２】
【００７４】
さて、これまで開示の装置に関する実施例について説明したが、本発明は上述した実施例以外にも、種々の異なる形態にて実施されてよいものである。そこで、以下では、本発明に含まれる他の実施例を説明する。
【００７５】
［内部状態量の更新契機］
上記の実施例１では、第２の取得部１２によって位置・姿勢の推定結果が取得される周期で内部状態量zを更新する場合を説明したが、開示の装置は先の例に限定されない。他の一例としては、第１の取得部１１によってオドメトリが取得される周期で更新する内部状態量z’を新たに使用することもできる。これによって、フィルタリング結果が前回に得られたフィルタリング結果との間で極端に大きな変化をとる場合に、フィルタリング結果を時間的に滑らかに変化させることができる。なお、内部状態量z’のデータ型についても、内部状態量zと同様に、浮動小数点型のｘ座標位置（m）、ｙ座標位置（m）、姿勢θ（rad）を含んで構成されるものとする。
【００７６】
これを説明すると、開示の装置は、上記の実施例１と同様に、第２の取得部１２によって位置・姿勢の推定結果が取得され場合に、内部状態量zを更新する。このとき、開示の装置は、上記の式（１）および式（２）の代わりに、下記の式（４）及び式（５）を用いて、シフト値ds(t)の値を更新するとともに、内部状態量z(t)を更新する。なお、下記の式に示す「N」は、位置・姿勢の推定結果の平均入力周期をオドメトリの平均入力周期で除算した値を指す。また、下記の式に示す「n」は、最新のオドメトリが入ってくるときに１つインクリメントさせるカウンタであり、ds(t)の値を更新した後で０にリセットされる。また、下記の式に示すds(t’)は、上記の実施例１におけるシフト値dsに対応する。
【００７７】
【数４】
【数５】
【００７８】
図１１は、内部状態量の更新方法の応用例を示す図である。図１１に示すように、オドメトリの入力周期と位置・姿勢の推定結果の入力周期が一定であると仮定した場合には、オドメトリが入力されるタイミングでds(t’)/Nずつ内部状態量z'を内部状態量zへ近づける方式を採用することができる。なぜなら、オドメトリが入力されるステップを１ステップとしたとき、Ｎステップ後には内部状態量z’が内部状態量zにds(t’)だけ近付くからである。
【００７９】
ところが、オドメトリの入力周期と位置・姿勢の推定結果の入力周期が必ずしも一定であるとは限らない。この場合には、開示の装置は、位置・姿勢の推定結果が入力された時点からオドメトリの入力回数を数えるカウンタnを用意することで内部状態量z’の更新の過不足を補正する。
【００８０】
これを説明すると、開示の装置は、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いか否か、すなわちカウンタnの値がN以上であるか否かを判定する。図１２及び図１３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。これら図１２及び図１３の例では、位置・姿勢の推定結果の平均入力周期を300ms、オドメトリの平均入力周期を50msと仮定し、Nを6としている。
【００８１】
このとき、nの値がN以上である場合には、図１２に示すように、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いと推定できる。この場合には、開示の装置は、内部状態量z’がds(t’)だけ内部状態量zに近付いたとみなし、下記の式（６）用いて、内部状態量z’を更新する。この式（６）を適用した場合には、内部状態量z’の更新時に「ds(t’)/N」が足し合わせられないので、「ds(t’)/N」の項を足しすぎるのを防止できる。一方、nの値がN未満である場合には、図１３に示すように、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が少ないと推定できる。この場合には、開示の装置は、下記の式（７）を適用し、内部状態量z’を内部状態量zに近づける。なお、図１３の例のように、位置・姿勢の推定結果の入力間隔に比べ、オドメトリの入力回数が小さい場合に対応するために、ds(t’)の値を計算する際には上記の式（４）の第二項ds(t’-1)/N・(N-n)を加える。
【００８２】
【数６】
【数７】
【００８３】
このように内部状態量zおよび内部状態量z'を更新しつつ、開示の装置は、最新のオドメトリが入力されると、下記の式（８）を用いて、フィルタリング結果を算出する。すなわち、開示の装置は、下記の式（８）を用いて、前回にフィルタリング結果として出力した内部状態量z'(t＿new-1)に最新のオドメトリとその１つ前のオドメトリの差分「odo(t＿new)-odo(t＿new-1)」を足し合わせる。これによって、開示の装置は、最新のオドメトリの入力に対応して出力する今回のフィルタリング結果を算出する。
【００８４】
【数８】
【００８５】
図１４は、応用例に係る出力処理の手順を示すフローチャートである。この出力処理は、上記の実施例１に示した出力処理と同様に、ロボット１の電源が作動した場合に起動する処理であり、電源がＯＦＦ状態になるまで繰り返し実行される。
【００８６】
図１４に示すように、開示の装置は、内部状態量zおよび内部状態量z’の初期の位置・姿勢（x,y,θ）として（0,0,0）を設定する（ステップＳ５０１）。続いて、開示の装置は、シフト値dsに初期値｛0,0,0｝を設定する（ステップＳ５０２）。
【００８７】
そして、位置・姿勢の推定結果が取得された場合（ステップＳ５０３肯定）には、開示の装置は、上記の式（４）を用いて、シフト値dsを更新する（ステップＳ５０４）。さらに、開示の装置は、上記の式（５）を用いて、内部状態量zを更新し（ステップＳ５０５）、最新のオドメトリが取得されるのを待機する（ステップＳ５０６）。なお、シフト値dsおよび内部状態量zの更新後には、カウンタnが「０」に初期化される。
【００８８】
一方、位置・姿勢の推定結果が取得されていない場合（ステップＳ５０３否定）には、ステップＳ５０４及びステップＳステップＳ５０５の処理を実行せずに、下記のステップＳ５０６の処理に移行する。
【００８９】
ここで、開示の装置は、カウンタnの値がN以上であるか否かを判定する（ステップＳ５０７）。このとき、nの値がN未満である場合（ステップＳ５０７否定）には、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が少ないと推定できる。この場合には、開示の装置は、下記の式（７）を適用し、内部状態量z’を内部状態量zに近づけ（ステップＳ５０８）、カウンタnをインクリメントし（ステップＳ５０９）、上記のステップＳ５０３の処理に戻る。
【００９０】
一方、nの値がN以上である場合（ステップＳ５０７肯定）には、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いと推定できる。この場合には、開示の装置は、内部状態量z’がds(t’)だけ内部状態量zに近付いたとみなし、下記の式（６）用いて、内部状態量z’を更新し（ステップＳ５１０）、上記のステップＳ５０３の処理に戻る。
【００９１】
このように、図１４に示した出力処理を実行することによって、フィルタリング結果が前回に得られたフィルタリング結果との間で極端に大きな変化をとる場合に、フィルタリング結果を時間的に滑らかに変化させることができる。
【００９２】
［分散および統合］
また、図示した各装置の各構成要素は、必ずしも物理的に図示の如く構成されていることを要しない。すなわち、各装置の分散・統合の具体的形態は図示のものに限られず、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。例えば、センサ５ａ、計算部３、推定部５、経路計画部７または移動制御部９をロボット１の外部装置としてネットワーク経由で接続するようにしてもよい。また、センサ５ａ、計算部３、推定部５、経路計画部７または移動制御部９を別の装置がそれぞれ有し、ネットワーク接続されて協働することで、上記のロボット１の機能を実現するようにしてもよい。
【００９３】
［位置姿勢出力プログラム］
また、上記の実施例で説明した各種の処理は、予め用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することによって実現することができる。そこで、以下では、図１５を用いて、上記の実施例と同様の機能を有する位置姿勢出力プログラムを実行するコンピュータの一例について説明する。
【００９４】
図１５は、実施例１及び実施例２に係る位置姿勢出力プログラムを実行するコンピュータの一例について説明するための図である。図１５に示すように、コンピュータ１００は、操作部１１０ａと、スピーカ１１０ｂと、カメラ１１０ｃと、ディスプレイ１２０と、通信部１３０とを有する。さらに、このコンピュータ１００は、ＣＰＵ１５０と、ＲＯＭ１６０と、ＨＤＤ１７０と、ＲＡＭ１８０と有する。これら１１０〜１８０の各部はバス１４０を介して接続される。
【００９５】
ＨＤＤ１７０には、図１５に示すように、上記の実施例１で示した第１の取得部１１と、第２の取得部１２と、更新部１４と、算出部１５と、出力部１６と同様の機能を発揮する位置姿勢出力プログラム１７０ａが予め記憶される。この位置姿勢出力プログラム１７０ａについては、図２に示した各々の第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６の各構成要素と同様、適宜統合又は分離しても良い。すなわち、ＨＤＤ１７０に格納される各データは、常に全てのデータがＨＤＤ１７０に格納される必要はなく、処理に必要なデータのみがＨＤＤ１７０に格納されれば良い。
【００９６】
そして、ＣＰＵ１５０が、位置姿勢出力プログラム１７０ａをＨＤＤ１７０から読み出してＲＡＭ１８０に展開する。これによって、図１５に示すように、位置姿勢出力プログラム１７０ａは、位置姿勢出力プロセス１８０ａとして機能する。この位置姿勢出力プロセス１８０ａは、ＨＤＤ１７０から読み出した各種データを適宜ＲＡＭ１８０上の自身に割り当てられた領域に展開し、この展開した各種データに基づいて各種処理を実行する。なお、位置姿勢出力プロセス１８０ａは、図２に示した第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６にて実行される処理、例えば図６、図７や図１４に示す処理を含む。また、ＣＰＵ１５０上で仮想的に実現される各処理部は、常に全ての処理部がＣＰＵ１５０上で動作する必要はなく、処理に必要な処理部のみが仮想的に実現されれば良い。
【００９７】
なお、上記の位置姿勢出力プログラム１７０ａについては、必ずしも最初からＨＤＤ１７０やＲＯＭ１６０に記憶させておく必要はない。例えば、コンピュータ１００に挿入されるフレキシブルディスク、いわゆるＦＤ、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させる。そして、コンピュータ１００がこれらの可搬用の物理媒体から各プログラムを取得して実行するようにしてもよい。また、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介してコンピュータ１００に接続される他のコンピュータまたはサーバ装置などに各プログラムを記憶させておき、コンピュータ１００がこれらから各プログラムを取得して実行するようにしてもよい。
【符号の説明】
【００９８】
１ ロボット
３ 計算部
５ 推定部
５ａ センサ
７ 経路計画部
９ 移動制御部
１０ 位置姿勢出力装置
１１ 第１の取得部
１２ 第２の取得部
１３ 記憶部
１４ 更新部
１５ 算出部
１６ 出力部
【技術分野】
【０００１】
本発明は、位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法に関する。
【背景技術】
【０００２】
移動体の位置および姿勢を計算する技術が知られている。例えば、オドメトリを計算する方法が挙げられる。かかる「オドメトリ」は、車輪の回転角度から位置および姿勢の変化量を計算した上でそれまでに求められていた位置および姿勢に累積して足し合わせることによって算出される。このように、オドメトリを計算する場合には、車輪が想定外の挙動を取ることによって生じる誤差や車輪等に設定されるパラメータの誤差も累積加算されるとともに、オドメトリの計算誤差も蓄積する。このため、移動体の移動量が大きくなるにしたがってオドメトリと実際の位置および姿勢との誤差も大きくなる。
【０００３】
このことから、オドメトリと他の情報とを併用して移動体の位置および姿勢を計算する技術も提案されている。一例としては、オドメトリセンサと撮像センサとを併用する位置推定装置が挙げられる。この位置推定装置では、オドメトリセンサを用いて予測された移動体の位置及び／又は姿勢をもとに撮像センサを用いて撮像された画像から目印やランドマークなどの特徴点を追跡できるか否かによって予測の正誤を判断する。このとき、位置推定装置は、位置及び／又は姿勢の予測が正しくないと判断した場合に、位置及び／又は姿勢の予測を修正する。また、位置推定装置は、位置及び／又は姿勢の予測が正しいと判断した場合に、移動体の位置及び／又は姿勢を更新する。
【０００４】
他の一例としては、ジャイロを用いることなく車両の３次元の位置・姿勢を標定する標定装置が挙げられる。この標定装置では、慣性センサとオドメトリとにより計測された角速度と速度とに基づいて車両の自己姿勢角と自己位置とを計測する。そして、標定装置は、先に計測された車両の自己姿勢角および自己位置を用いて予測された白線の消失点予測値と、画像センサによって撮像された画像に映っている白線の消失点実測値との差分である消失点残差に基づいて姿勢誤差推定量を算出する。その上で、標定装置は、姿勢誤差推定量を用いて自己姿勢角を補正する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００７−３０３８８６号公報
【特許文献２】特開２００９−２０４３８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の従来技術では、以下に説明するように、移動体の正確な位置および姿勢を時間遅れなく出力することができないという問題がある。
【０００７】
すなわち、上記の位置推定装置および標定装置では、オドメトリとともに画像等の他の情報を用いて移動体の位置および姿勢を推定するので、画像処理を実行するためにオドメトリの計算に比べて長い処理時間が必要となる。このため、上記の位置推定装置および標定装置では、移動体の位置および姿勢を推定し始めてからその推定結果が出力されるまでのタイムラグが大きくなる。よって、上記の位置推定装置および標定装置では、推定結果に対応する位置および姿勢が移動体によって実際に取られていた時点から遅れて位置および姿勢の推定結果が出力されてしまう。
【０００８】
また、上記の位置推定装置および標定装置では、画像等の他の情報を用いて移動体の位置および姿勢を推定する場合に、移動体の位置および姿勢の推定精度にばらつきが生じてしまう。なぜなら、上記の位置推定装置では、画像から目印やランドマークなどの特徴点を追跡し、また、上記の標定装置では、画像から白線の消失点を検出するが、画像には実際の特徴点や実際の白線の消失点と混同されやすいノイズが含まれる場合があるからである。かかる「ノイズ」が画像に含まれる場合には、ノイズを特徴点や白線の消失点と誤認し、移動体の位置および姿勢の推定結果に誤りが生じてしまう。
【０００９】
開示の技術は、上記に鑑みてなされたものであって、移動体の正確な位置および姿勢を時間遅れなく出力できる位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本願の開示する位置姿勢出力装置は、移動体の位置および姿勢を記憶する記憶部を有する。前記位置姿勢出力装置は、前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得する第１の取得部を有する。前記位置姿勢出力装置は、前記オドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得する第２の取得部を有する。さらに、前記位置姿勢出力装置は、前記第２の取得部によって前記推定結果が取得された場合に、前記記憶部に記憶された移動体の位置および姿勢を更新する更新部を有する。前記更新部は、更新後の移動体の位置および姿勢の値が、オドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新する。前記オドメトリの差分に基づく位置および姿勢は、前記記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られる。前記位置姿勢出力装置は、前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する出力部を有する。
【発明の効果】
【００１１】
本願の開示する位置姿勢出力装置の一つの態様によれば、移動体の正確な位置および姿勢を時間遅れなく出力できるという効果を奏する。
【図面の簡単な説明】
【００１２】
【図１】図１は、実施例１に係るロボットの機能的構成を示すブロック図である。
【図２】図２は、実施例１に係る位置姿勢出力装置の機能的構成を示すブロック図である。
【図３】図３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図４】図４は、内部状態量の更新方法の一例を示す図である。
【図５】図５は、フィルタリング結果の算出方法を示す図である。
【図６】図６は、実施例１に係る出力処理の手順を示すフローチャートである。
【図７】図７は、実施例１に係る算出処理の手順を示すフローチャートである。
【図８】図８は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。
【図９】図９は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。
【図１０】図１０は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値の一例を示す図である。
【図１１】図１１は、内部状態量の更新方法の応用例を示す図である。
【図１２】図１２は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図１３】図１３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。
【図１４】図１４は、応用例に係る出力処理の手順を示すフローチャートである。
【図１５】図１５は、実施例１及び実施例２に係る位置姿勢出力プログラムを実行するコンピュータの一例について説明するための図である。
【発明を実施するための形態】
【００１３】
以下に、本願の開示する位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法の実施例を図面に基づいて詳細に説明する。なお、この実施例は開示の技術を限定するものではない。そして、各実施例は、処理内容を矛盾させない範囲で適宜組み合わせることが可能である。
【実施例１】
【００１４】
［ロボットの構成］
まず、本実施例に係る位置姿勢出力装置を含むロボットの機能的構成について説明する。図１は、実施例１に係るロボットの機能的構成を示すブロック図である。図１に示すロボット１は、オフィス、廊下やショッピングモール等の空間において所定の目標の位置、例えば予め定められた位置や外部から指示入力された位置に向かって自律走行する装置である。
【００１５】
図１に示すように、ロボット１は、計算部３と、センサ５ａと、推定部５と、経路計画部７と、移動制御部９と、位置姿勢出力装置１０とを有する。なお、ロボット１は、図１に示した機能部以外にも既知のロボットが有する各種の機能部、例えば各種の入力デバイスや音声出力デバイスなどの他、車軸や車輪、これらを駆動する駆動部などの機能を有するものとする。
【００１６】
計算部３は、ロボット１のオドメトリを計算する処理部である。一態様としては、計算部３は、ロボット１の車輪の回転角度から位置および姿勢の変化量を計算した上でそれまでに求めていた位置および姿勢に累積して足し合わせることによってオドメトリを計算する。このように、計算部３によって計算されたオドメトリは、後述の位置姿勢出力装置１０へ出力される。
【００１７】
推定部５は、撮像センサやＬＲＦ（Laser Range Finder）などのセンサ５ａによって採取されるセンサ情報を用いて、ロボット１の位置および姿勢を推定する処理部である。一態様としては、推定部５は、センサ５ａによって出力されたセンサ情報を予め作成された地図情報との間でマップマッチングすることにより、ロボット１の位置および姿勢を推定する。かかる位置および姿勢の推定には、上述したマップマッチングの他にも公知の任意の手法を採用することができる。このように、推定部５によって推定された位置および姿勢の推定結果は、後述の位置姿勢出力装置１０へ出力される。なお、以下では、位置および姿勢のことを「位置・姿勢」と記載する場合がある。
【００１８】
経路計画部７は、ロボット１が移動する経路を計画する処理部である。一態様としては、経路計画部７は、ロボット１が目標の位置に到達または通過できるように、後述の位置姿勢出力装置１０によって出力されたロボット１の位置・姿勢から、ロボット１を移動させる速度や角速度などの移動制御用のパラメータを決定する。このように、経路計画部７によって決定された移動制御用のパラメータは、後述の移動制御部９へ出力される。
【００１９】
移動制御部９は、ロボット１の移動を制御する処理部である。一態様としては、移動制御部９は、経路計画部７によって出力された移動制御用のパラメータにしたがってロボット１の車輪を駆動させたり、車輪の回転に制動を掛けたり、あるいはロボット１本体の向きを回転させたりする。
【００２０】
位置姿勢出力装置１０は、計算部３および推定部５によって互いに異なる周期で出力されるロボット１のオドメトリおよび位置・姿勢をフィルタリングした上で経路計画部７へ出力する装置である。一態様としては、位置姿勢出力装置１０は、計算部３によって計算されたオドメトリおよび推定部５によって推定された位置・姿勢をもとに、内部状態量を更新しつつ、内部状態量を参照してロボット１の位置・姿勢を出力する。ここで言う「内部状態量」とは、位置姿勢出力装置１０によって内部的に管理されるロボット１の状態量を指す。なお、以下では、位置姿勢出力装置１０が出力する位置・姿勢のことを「フィルタリング結果」と記載する場合がある。
【００２１】
［位置姿勢出力装置の構成］
続いて、本実施例に係る位置姿勢出力装置の機能的構成について説明する。図２は、実施例１に係る位置姿勢出力装置の機能的構成を示すブロック図である。図２に示すように、位置姿勢出力装置１０は、第１の取得部１１と、第２の取得部１２と、記憶部１３と、更新部１４と、算出部１５と、出力部１６とを有する。
【００２２】
このうち、第１の取得部１１は、計算部３によって出力されるロボット１のオドメトリを取得する処理部である。また、第２の取得部１２は、推定部５によって出力されるロボット１の位置・姿勢の推定結果を取得する処理部である。さらに、第２の取得部１２は、ロボット１の位置・姿勢の推定結果とともに、その推定に用いられたオドメトリの値も併せて取得する。
【００２３】
ここで、図３を用いて、オドメトリおよび位置・姿勢の推定結果が入力される周期の違いについて説明する。図３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。図３に示す「t」は、時刻を指し、「t＿new」は、計算部３によって最新のオドメトリが入力された時刻を指す。また、図３に示す「t’」は、推定部５によって位置・姿勢が推定された時刻を指し、図３に示す「t’-1」は、前回に推定部５によって位置・姿勢が推定された時刻を指す。なお、図３の例では、オドメトリおよび位置・姿勢の推定結果の両者がともに一定の間隔で入力される場合を想定する。
【００２４】
図３に示すように、計算部３によって計算されたオドメトリが「50msec」の間隔で入力される一方で、推定部５によって推定された位置・姿勢の推定結果が「300msec」の間隔で入力される。このように、オドメトリが入力される周期が位置・姿勢の推定結果が入力される周期よりも短いのは、オドメトリが移動速度の時間積分という簡易な演算によって算出できるからである。また、位置・姿勢の推定結果が入力される周期がオドメトリが入力される周期よりも長いのは、画像やレーザー光の反射光などを解析する処理にオドメトリの計算に比べて長い処理時間を要するからである。なお、ここでは、オドメトリの入力周期が「50msec」であり、位置・姿勢の推定結果が「300msec」である場合を例示したが、これらの入力周期はあくまでも一例である。各々の入力周期は、任意の値であってかまわないが、オドメトリの入力周期は位置・姿勢の推定結果の入力周期よりも短いことが好ましい。
【００２５】
記憶部１３は、ロボット１の内部状態量を記憶するものである。かかる内部状態量の一態様としては、浮動小数点型のｘ座標位置（ｍ）、ｙ座標位置（ｍ）および姿勢θ（rad）が対応付けられたデータが採用される。一例としては、第２の取得部１２によって位置・姿勢の推定結果が取得される度に、後述の更新部１４によって内部状態量が更新される。他の一例としては、第１の取得部１１によってオドメトリが取得される度に、後述の出力部１６によって内部状態量が参照される。なお、内部状態量の初期状態には、ローカル座標系の原点として位置・姿勢（x,y,θ）＝（0,0,0）を設定することもできるし、グローバル座標系におけるロボット１の所在位置およびその姿勢を設定することもできる。
【００２６】
この記憶部１３には、半導体メモリ素子や記憶装置を採用できる。例えば、半導体メモリ素子の一例としては、ＶＲＡＭ（Video Random Access Memory）、ＲＡＭ（Random Access Memory)、ＲＯＭ（Read Only Memory）やフラッシュメモリ（flash memory）などが挙げられる。また、記憶装置の一例としては、ハードディスク、光ディスクなどの記憶装置が挙げられる。
【００２７】
更新部１４は、内部状態量を更新する処理部である。一態様としては、更新部１４は、第２の取得部１２によって位置・姿勢の推定結果が取得された場合に、処理を起動する。このとき、更新部１４は、更新後の内部状態量が、更新前の内部状態量に今回の推定に用いられたオドメトリおよび前回の推定に用いられたオドメトリの差分が加算された値と、今回の位置・姿勢の推定結果との間に遷移するように、内部状態量を更新する。
【００２８】
ここで、図４を用いて、内部状態量の更新方法について説明する。図４は、内部状態量の更新方法の一例を示す図である。図４に示す「E(t)」は、位置・姿勢の推定結果を指し、例えば、「E(t’)」と表記された場合には今回推定された位置・姿勢の推定結果を表し、「E(t’-1)」と表記された場合には前回推定された位置・姿勢の推定結果を表す。また、図４に示す「z(t)」は、内部状態量を指し、例えば、「z(t’)」と表記された場合には更新後の内部状態量を表し、「z(t’-1)」と表記された場合には更新前の内部状態量を表す。また、図４に示す「E＿odo(t)」は、位置・姿勢の推定に用いられたオドメトリを指し、例えば、「E＿odo(t’)」と表記された場合には今回の推定に用いられたオドメトリを表し、「E＿odo(t’-1)」と表記された場合には前回の推定に用いられたオドメトリを表す。
【００２９】
図４に示すように、更新部１４は、推定部５によって推定された位置・姿勢の推定結果「E(t’)」をそのまま内部状態量「z(t’)」と同定する更新を必ずしも実行しない。その理由の一因としては、推定部５による位置・姿勢の推定結果にはセンサ５ａによって採取されたセンサ情報に含まれるノイズが原因となって推定精度にばらつきが生じるからである。それゆえ、更新部１４は、位置・姿勢の推定結果「E(t’)」だけに依存せずに内部状態量z(t)を更新するために、今回の推定に用いられたオドメトリと前回の推定に用いられたオドメトリとの差分「E＿odo(t’)-E＿odo(t’-1)」を使用する。なぜなら、オドメトリの値は、ロボット１の移動量が大きくなるに伴って実際の位置・姿勢との間で誤差も大きくなるが、オドメトリの差分は位置・姿勢の推定結果が出力される周期という局所的な範囲で見れば誤差は小さく、実値と近い値と推定できるからである。
【００３０】
このため、更新部１４は、更新前の内部状態量z(t’-1)に上記のオドメトリの差分「E＿odo(t’)-E＿odo(t’-1)」を加算することによってオドメトリの差分に基づく内部状態量、すなわち図中の白丸の位置・姿勢を算出する。そして、更新部１４は、今回の位置・姿勢の推定結果E(t’)から上記のオドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」を減算した上でその減算値「D」に後述の定数ω（０＜ω≦１）を乗算する。これによって、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へ、更新後の内部状態量z(t’)をどれだけシフトさせるかを表すシフト値dsを導出する。その上で、更新部１４は、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」にシフト値dsを足し合わせることによって、更新後の内部状態量z(t’)を算出する。なお、上記の定数ωは、ωの値が「１」に近づくほど、内部状態量z(t')は位置・姿勢の推定結果に近づく。また、ωの値が「０」に近づくほど、内部状態量z(t')は更新前の内部状態量z(t'-1)にオドメトリの差分を加えた値に近づく。
【００３１】
より詳細には、更新部１４は、推定部５によって位置・姿勢の推定結果「E(t’)」が出力さると、下記の式（１）に次の値を代入することによってシフト値dsを算出する。すなわち、更新部１４は、更新前の内部状態量z(t’-1)、今回の位置・姿勢の推定結果「E(t’)」、今回の推定に用いられたオドメトリ「E＿odo(t’)」および前回の推定に用いられたオドメトリ「E＿odo(t’-1)」を下記の式（１）に代入する。その上で、更新部１４は、式（１）を用いて算出したシフト値dsを下記の式（２）に代入することによって更新後の内部状態量z(t’)を算出した上でその更新後の内部状態量z(t’)を記憶部１３に上書きして更新する。
【００３２】
【数１】
【数２】
【００３３】
このように、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフト値dsの分、シフトさせる内部状態量z(t)の更新を実行する。これによって、位置・姿勢の推定結果がばらつくという弱点をオドメトリの差分によって補完することができる結果、内部状態量z(t)を実際の位置・姿勢と近い値に保つことができる。さらに、内部状態量z(t)を更新する方式としてKalman Filterを使用する方法も考えられるが、上記の更新方法では、Kalman Filterを使用する場合のように、入力の共分散行列を受け取る必要がなく、Kalman Filterを使用する場合よりも効率的に更新できる。
【００３４】
算出部１５は、シフト値dsに乗ずる定数ωを算出する処理部である。ここでは、一例として、ロボット１を予め計画された走行経路で自律走行された場合における位置・姿勢の出力結果のログを用いて定数ωを算出する場合を例示する。なお、上記のログとして、後述の出力部１６によって出力されたロボット１のフィルタリング結果のログを用いることとしてもよいし、また、外部装置から供給されたロボット１または他のロボットの位置・姿勢のログを用いることとしてもかまわない。
【００３５】
これを説明すると、まず、算出部１５は、算出対象とする定数ωを「１」とし、後述の自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅを「０」とするパラメータの初期化を実行する。その後、算出部１５は、ロボット１の位置・姿勢の出力結果のログを取得する。そして、算出部１５は、ログ取得したロボット１の位置の推移、すなわち位置の点列に含まれる位置と、予め計画された移動軌跡のうち最も近い位置との距離の自乗和Ｓを計算する。続いて、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きいか否かを判定する。なお、自乗和Ｓの計算が始めて実行された場合には、パラメータの初期化によって１回前の計算結果Ｓ＿ｐｒｅが「０」に設定されている。
【００３６】
このとき、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロであるか、もしくは今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅ以下である場合には、次のように、定数ωおよび１回前の計算結果Ｓ＿ｐｒｅを変更して自乗和Ｓの計算を繰り返す。すなわち、算出部１５は、定数ωをω／２に変更するとともに１回前の計算結果Ｓ＿ｐｒｅを今回計算したＳに変更した上で、変更後の定数ωを用いて出力された位置・姿勢のログを改めて取得し、自乗和Ｓを再び計算する。その後、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きい場合には、今回計算したωの２倍の２ωを定数ωとして決定する。
【００３７】
このように定数ωを算出することによって、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフトさせる重みを適切に決定できる結果、内部状態量z(t)を正確に算出できる。
【００３８】
なお、ここでは、ロボット１を実際に自律走行させた位置・姿勢のログを用いて定数ωを算出する場合を例示したが、ロボット１の管理者から定数ωとして経験的に得られる理想値を受け付け、その理想値を採用することとしてもよい。一例としては、今回の位置・姿勢の推定結果「E(t’)」よりもオドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」寄りへシフトさせる値、例えば「0.3」を定数ωとして採用するのが好ましい。
【００３９】
出力部１６は、記憶部１３に記憶された内部状態量z(t)を用いて、フィルタリング結果を算出した上で経路計画部７へ出力する処理部である。一態様としては、出力部１６は、最新のオドメトリと今回の推定に用いられたオドメトリとのオドメトリの差分を内部状態量に足し合わせることによってフィルタリング結果を算出する。
【００４０】
図５は、フィルタリング結果の算出方法を示す図である。図５に示すように、出力部１６は、第１の取得部１１によってオドメトリが取得される度に、次のようにして、フィルタリング結果を算出する。すなわち、出力部１６は、下記の式（３）のように、内部状態量z(t’)に最新のオドメトリodo(t＿new)と今回の推定に用いられたオドメトリE＿odo(t’)とのオドメトリの差分を足し合わせることによってフィルタリング結果f(t＿new)を算出する。そして、出力部１６は、フィルタリング結果f(t＿new)を経路計画部７へ出力する。
【００４１】
【数３】
【００４２】
このように、上記のオドメトリの差分「odo(t＿new)-E＿odo(t’)」を用いてフィルタリング結果を算出することにより、位置・姿勢の推定結果が入力されずともロボット１の位置・姿勢を導出できる。このため、位置・姿勢の推定結果が入力されるのを待機せずに、位置・姿勢の推定結果が入力されるよりも短い周期、すなわちオドメトリが入力される周期でフィルタリング結果を出力できる。したがって、フィルタリング結果に対応する位置・姿勢がロボット１によって実際に取られていた時点からフィルタリング結果を出力するまでの遅れを低減できる結果、時間遅れを抑制できる。
【００４３】
なお、第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６には、各種の集積回路や電子回路を採用できる。例えば、集積回路の一例としては、ＡＳＩＣ（Application Specific Integrated Circuit）が挙げられる。また、電子回路の一例としては、ＣＰＵ（Central Processing Unit）やＭＰＵ（Micro Processing Unit）などが挙げられる。
【００４４】
［計算例］
次に、内部状態量の更新およびフィルタリング結果の算出に関する計算例を説明する。なお、E(t)、E＿odo(t)、f(t)、odo(t)、z(t)、dsのデータ型は、それぞれ浮動小数点型のｘ座標位置（m）、ｙ座標位置（m）、姿勢θ（rad）を含んで構成されるものとする。さらに、ここでは、内部状態量として初期の位置・姿勢（x,y,θ）として（0,0,0）が設定される場合を想定して以下の説明を行う。加えて、以下では、これらデータの要素を区別して説明する場合には、「E(t).x」、「E(t).y」「E(t).θ」のように記載する場合がある。
【００４５】
また、本計算例では、一例として、オドメトリの座標系における姿勢が「θ＝π」であり、位置・姿勢の推定に用いられる座標系における姿勢が「θ＝０」である場合を想定する。以下では、オドメトリの座標系のことを「オドメトリ座標系」と記載し、位置・姿勢の推定に用いられる座標系のことを「位置姿勢推定座標系」と記載する場合がある。なお、内部状態量は、位置・姿勢推定座標系と同様の座標系が採用されているものとする。
【００４６】
このとき、内部状態量z(t’-1)の位置・姿勢（x,y,θ）が（5.0,5.0,1.57）であり、前回の推定に用いられたオドメトリE＿odo(t’-1)が（1.0,1.0,3.12）であったものとする。さらに、時刻t'において今回の位置・姿勢の推定結果E(t’)が（6.0,6.0,1.44）であり、今回の推定に用いられたオドメトリE＿odo(t’)が（2.0,2.0,3.0）であったものとする。
【００４７】
このような前提の下、更新部１４は、上記の式（１）にしたがってシフト値dsの（x,y,θ）を算出する。まず、更新部１４は、位置・姿勢の推定計算に用いられたオドメトリの差分E＿odo(t’)-E＿odo(t’-1)を（1.0,1.0,-0.12）と算出する。
【００４８】
さらに、更新部１４は、z(t’-1)+E＿odo(t’)-E＿odo(t’-1)を算出するが、前述のように、オドメトリ座標系における姿勢と位置・姿勢推定座標系における姿勢は異なるので、オドメトリ座標系から位置・姿勢推定座標系へと座標変換する。座標変換式を以下に記す。
θ(t’)=E(t’).θ-E＿odo(t’).θ
dx=(E＿odo(t’).x-E＿odo(t’-1).x)・cosθ(t’)-(E＿odo(t’).y-E＿odo(t’-1).y)・sinθ(t’)
dy=(E＿odo(t’).x-E＿odo(t’-1).x)・sinθ(t’)+(E＿odo(t’).y-E＿odo(t’-1).y)・cosθ(t’)
【００４９】
上記の座標変換式をE＿odo(t’)-E＿odo(t’-1)に適用することにより、更新部１４は、z(t’-1)+E＿odo(t’)-E＿odo(t’-1)を計算する。
すなわち、
z(t’-1).x+dx=5.0+1.0・cos(-1.56)-1.0・sin(-1.56)=5.0+0.01+1.00=6.01
z(t’-1).y+dy=5.0+1.0・sin(-1.56)+1.0・cos(-1.56)=5.0-1.00+0.01=4.01
z(t’-1).θ+E＿odo(t’).θ-E＿odo(t’-1).θ=1.45
となる。
【００５０】
したがって、更新部１４は、E(t’)-(z(t’-1)+E＿odo(t’)-E＿odo(t’-1))により計算される(x,y,θ)を（-0.01,1.99,-0.01）と算出する。
【００５１】
ここで定数ωの値が「0.3」であったとすると、更新部１４は、上記の式（１）により、シフト値dsを（-0.003,0.66,-0.003）と算出する。
【００５２】
さらに、更新部１４は、上記の式（２）にしたがって内部状態量z(t’)を計算する。
すなわち、
z(t’).x=6.01-0.003=6.007
z(t’).y=4.01+0.66=4.67
z(t’).θ=1.45-0.003=1.497
と算出する。
【００５３】
その後、出力部１６は、時刻t＿newにおいて最新のオドメトリが入力された場合に、上記の式（３）を用いてフィルタリング結果を計算する。この場合も、出力部１６は、内部状態量z(t’)とオドメトリの差分odo(t＿new)-E＿odo(t’)の座標系は異なるので、座標変換を実行する。
【００５４】
すなわち、出力部１６は、上記の同様に、
θ(t’)=E(t’).θ(t’)-E＿odo(t’).θ(t’)
dx=(odo(t＿new).x-E＿odo(t’).x)・cosθ(t’)-(odo(t＿new).y-E＿odo(t’).y)・sinθ(t’)
dy=(odo(t＿new).x-E＿odo(t’).x)・sinθ(t’)+(odo(t＿new).y-E＿odo(t’).y)・cosθ(t’)
の座標変換式を適用する。
【００５５】
そして、出力部１６は、最新のオドメトリodo(t＿new)が（2.5,2.5,2.8）であるとしたとき、下記のように、フィルタリング結果を計算する。
f(t＿new).x=6.007+0.5・cos(-1.56)-0.5・sin(-1.56)=6.507
f(t＿new).y=4.67+0.5・cos(-1.56)+0.5・sin(-1.56)=4.17
f(t＿new).θ=1.497-0.2=1.297
【００５６】
［処理の流れ］
次に、本実施例に係る位置姿勢出力装置の処理の流れについて説明する。なお、ここでは、位置姿勢出力装置が実行する（１）出力処理を説明した後に、（２）算出処理を説明することとする。
【００５７】
（１）出力処理
図６は、実施例１に係る出力処理の手順を示すフローチャートである。この出力処理は、ロボット１の電源が作動した場合に起動する処理であり、電源がＯＦＦ状態になるまで繰り返し実行される。
【００５８】
図６に示すように、更新部１４は、内部状態量の初期の位置・姿勢（x,y,θ）として（0,0,0）を設定する（ステップＳ１０１）。
【００５９】
そして、位置・姿勢の推定結果が取得された場合（ステップＳ１０２肯定）には、更新部１４は、次のような処理を実行する（ステップＳ１０３）。すなわち、更新部１４は、更新前の内部状態量z(t’-1)、今回の位置・姿勢の推定結果「E(t’)」、今回の推定に用いられたオドメトリ「E＿odo(t’)」および前回の推定に用いられたオドメトリ「E＿odo(t’-1)」を上記の式（１）に代入してシフト値dsを算出する。なお、位置・姿勢の推定結果が取得されていない場合（ステップＳ１０２否定）には、下記のステップＳ１０５の処理に移行する。
【００６０】
その後、更新部１４は、式（１）を用いて算出したシフト値dsを下記の式（２）に代入することによって更新後の内部状態量z(t’)を算出した上でその更新後の内部状態量z(t’)を記憶部１３に上書きして更新する（ステップＳ１０４）。
【００６１】
そして、最新のオドメトリが取得されるのを待機し（ステップＳ１０５）、最新のオドメトリが取得されると、出力部１６は、次のような処理を実行する（ステップＳ１０６）。すなわち、出力部１６は、下記の式（３）のように、内部状態量z(t’)に最新のオドメトリodo(t＿new)と今回の推定に用いられたオドメトリE＿odo(t’)との差分を足し合わせることによってフィルタリング結果f(t＿new)を算出して経路計画部７へ出力する。
【００６２】
その後、上記のステップＳ１０２の処理に戻り、位置姿勢出力装置１０は、ロボット１の電源がＯＦＦ状態になるまで、上記のステップＳ１０２〜ステップＳ１０６までの処理を繰り返し実行する。
【００６３】
（２）算出処理
図７は、実施例１に係る算出処理の手順を示すフローチャートである。この算出処理は、ロボット１の管理者によって定数ωの算出が指示された場合等に処理が起動する。
【００６４】
図７に示すように、算出部１５は、算出対象とする定数ωを「１」とし、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅを「０」とするパラメータの初期化を実行する（ステップＳ３０１）。その後、算出部１５は、ロボット１の位置・姿勢の出力結果のログを取得する（ステップＳ３０２）。
【００６５】
そして、算出部１５は、ログ取得したロボット１の位置の推移、すなわち位置の点列に含まれる位置と、予め計画された移動軌跡のうち最も近い位置との距離の自乗和Ｓを計算する（ステップＳ３０３）。
【００６６】
続いて、算出部１５は、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きいか否かを判定する（ステップＳ３０４）。なお、自乗和Ｓの計算が始めて実行された場合には、パラメータの初期化によって１回前の計算結果Ｓ＿ｐｒｅが「０」に設定されている。
【００６７】
このとき、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロであるか、もしくは今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅ以下である場合（ステップＳ３０４否定）には、次のような処理を実行する（ステップＳ３０５）。すなわち、算出部１５は、定数ωをω／２に変更するとともに１回前の計算結果Ｓ＿ｐｒｅを今回計算したＳに変更する。その上で、算出部１５は、変更後の定数ωを用いて出力された位置・姿勢のログを改めて取得し（ステップＳ３０２）、自乗和Ｓを再び計算する（ステップＳ３０３）。
【００６８】
その後、自乗和Ｓの１回前の計算結果Ｓ＿ｐｒｅがゼロでなく、かつ今回計算した自乗和Ｓが１回前の計算結果Ｓ＿ｐｒｅよりも大きい場合（ステップＳ３０４肯定）には、算出部１５は、次のような処理を実行する。すなわち、算出部１５は、今回計算したωの２倍の２ωを定数ωとして決定し（ステップＳ３０６）、処理を終了する。
【００６９】
［実施例１の効果］
上述してきたように、本実施例に係る位置姿勢出力装置１０は、オドメトリの差分に基づく内部状態量「z(t’-1)+E＿odo(t’)-E＿odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフト値dsの分、シフトさせる内部状態量z(t)の更新を実行する。これによって、位置・姿勢の推定結果がばらつくという弱点をオドメトリの差分によって補完することができる結果、内部状態量z(t)を実際の位置・姿勢と近い値に保つことができる。また、本実施例に係る位置姿勢出力装置１０は、オドメトリの差分「odo(t＿new)-E＿odo(t’)」を用いてフィルタリング結果を算出することにより、位置・姿勢の推定結果が入力されずともロボット１の位置・姿勢を導出できる。このため、位置・姿勢の推定結果が入力されるのを待機せずに、位置・姿勢の推定結果が入力されるよりも短い周期、すなわちオドメトリが入力される周期でフィルタリング結果を出力できる。したがって、本実施例に係る位置姿勢出力装置１０によれば、ロボット１の正確な位置および姿勢を時間遅れなく出力することが可能になる。
【００７０】
ここで、本実施例に係る位置姿勢出力装置１０を用いた実験結果を引用して位置・姿勢の精度を検証する。図８及び図９は、位置・姿勢の推定結果とフィルタリング結果に関する実験結果の一例を示す図である。このうち、図８の例では、ロボット１をＹ軸と平行に直線移動させた場合の実験結果を示す。また、図９の例では、ロボット１をＸ軸と平行に直線移動させた場合の実験結果を示す。なお、図８及び図９のグラフでは、点線が推定部５による位置・姿勢の推定結果を示し、実線が出力部１６によるフィルタリング結果を示す。
【００７１】
図８及び図９に示すように、推定部５によって推定された位置・姿勢の推定結果の位置は、数十cmのオーダーでばらついている。その一方で、出力部１６によって出力されたフィルタリング結果の位置のばらつきは十cm程度である。このため、推定部５によって推定された位置・姿勢の推定結果よりも出力部１６によって出力されたフィルタリング結果の方がロボット１の実際の挙動に近いことがわかる。それゆえ、本実施例に係る位置姿勢出力装置１０がロボット１の正確な位置および姿勢を出力できることが検証できたと言える。
【００７２】
さらに、図１０を用いて、時間遅れの低減について検証する。図１０は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値の一例を示す図である。この図１０の例では、ロボット１をＸ座標値が増加するように移動物体を移動させた場合を想定する。図１０に示すブラフの横軸は、ロボット１が移動を開始してからの経過時間を示し、縦軸は、位置・姿勢の推定結果、内部状態量およびフィルタリング結果のＸ座標値を示す。なお、図１０に示すグラフでは、点線が推定部５による位置・姿勢の推定結果を示し、実線が出力部１６によるフィルタリング結果を示し、一点鎖線が内部状態量を示す。
【００７３】
図１０に示すように、位置・姿勢の推定結果のＸ座標値のばらつきに対し、内部状態量のばらつきは小さい。また、フィルタリング結果は、最新のオドメトリを反映した値となっているので、内部状態量のＸ座標値よりもＸ座標値が大きい。さらに、内部状態量が長い周期、本例では約300msで更新されているのに対し、フィルタリング結果は短い周期、本例では約50msで更新されている。このように、出力部１６によって出力されたフィルタリング結果は、Ｘ座標値のばらつきが小さく、さらに、フィルタリング結果が出力された時点とロボット１がその位置に所在した時点とのライムラグも小さいことがわかる。それゆえ、本実施例に係る位置姿勢出力装置１０がロボット１の位置および姿勢を時間遅れなく出力できることが検証できたと言える。
【実施例２】
【００７４】
さて、これまで開示の装置に関する実施例について説明したが、本発明は上述した実施例以外にも、種々の異なる形態にて実施されてよいものである。そこで、以下では、本発明に含まれる他の実施例を説明する。
【００７５】
［内部状態量の更新契機］
上記の実施例１では、第２の取得部１２によって位置・姿勢の推定結果が取得される周期で内部状態量zを更新する場合を説明したが、開示の装置は先の例に限定されない。他の一例としては、第１の取得部１１によってオドメトリが取得される周期で更新する内部状態量z’を新たに使用することもできる。これによって、フィルタリング結果が前回に得られたフィルタリング結果との間で極端に大きな変化をとる場合に、フィルタリング結果を時間的に滑らかに変化させることができる。なお、内部状態量z’のデータ型についても、内部状態量zと同様に、浮動小数点型のｘ座標位置（m）、ｙ座標位置（m）、姿勢θ（rad）を含んで構成されるものとする。
【００７６】
これを説明すると、開示の装置は、上記の実施例１と同様に、第２の取得部１２によって位置・姿勢の推定結果が取得され場合に、内部状態量zを更新する。このとき、開示の装置は、上記の式（１）および式（２）の代わりに、下記の式（４）及び式（５）を用いて、シフト値ds(t)の値を更新するとともに、内部状態量z(t)を更新する。なお、下記の式に示す「N」は、位置・姿勢の推定結果の平均入力周期をオドメトリの平均入力周期で除算した値を指す。また、下記の式に示す「n」は、最新のオドメトリが入ってくるときに１つインクリメントさせるカウンタであり、ds(t)の値を更新した後で０にリセットされる。また、下記の式に示すds(t’)は、上記の実施例１におけるシフト値dsに対応する。
【００７７】
【数４】
【数５】
【００７８】
図１１は、内部状態量の更新方法の応用例を示す図である。図１１に示すように、オドメトリの入力周期と位置・姿勢の推定結果の入力周期が一定であると仮定した場合には、オドメトリが入力されるタイミングでds(t’)/Nずつ内部状態量z'を内部状態量zへ近づける方式を採用することができる。なぜなら、オドメトリが入力されるステップを１ステップとしたとき、Ｎステップ後には内部状態量z’が内部状態量zにds(t’)だけ近付くからである。
【００７９】
ところが、オドメトリの入力周期と位置・姿勢の推定結果の入力周期が必ずしも一定であるとは限らない。この場合には、開示の装置は、位置・姿勢の推定結果が入力された時点からオドメトリの入力回数を数えるカウンタnを用意することで内部状態量z’の更新の過不足を補正する。
【００８０】
これを説明すると、開示の装置は、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いか否か、すなわちカウンタnの値がN以上であるか否かを判定する。図１２及び図１３は、オドメトリおよび位置・姿勢の推定結果が入力される周期の一例を示す図である。これら図１２及び図１３の例では、位置・姿勢の推定結果の平均入力周期を300ms、オドメトリの平均入力周期を50msと仮定し、Nを6としている。
【００８１】
このとき、nの値がN以上である場合には、図１２に示すように、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いと推定できる。この場合には、開示の装置は、内部状態量z’がds(t’)だけ内部状態量zに近付いたとみなし、下記の式（６）用いて、内部状態量z’を更新する。この式（６）を適用した場合には、内部状態量z’の更新時に「ds(t’)/N」が足し合わせられないので、「ds(t’)/N」の項を足しすぎるのを防止できる。一方、nの値がN未満である場合には、図１３に示すように、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が少ないと推定できる。この場合には、開示の装置は、下記の式（７）を適用し、内部状態量z’を内部状態量zに近づける。なお、図１３の例のように、位置・姿勢の推定結果の入力間隔に比べ、オドメトリの入力回数が小さい場合に対応するために、ds(t’)の値を計算する際には上記の式（４）の第二項ds(t’-1)/N・(N-n)を加える。
【００８２】
【数６】
【数７】
【００８３】
このように内部状態量zおよび内部状態量z'を更新しつつ、開示の装置は、最新のオドメトリが入力されると、下記の式（８）を用いて、フィルタリング結果を算出する。すなわち、開示の装置は、下記の式（８）を用いて、前回にフィルタリング結果として出力した内部状態量z'(t＿new-1)に最新のオドメトリとその１つ前のオドメトリの差分「odo(t＿new)-odo(t＿new-1)」を足し合わせる。これによって、開示の装置は、最新のオドメトリの入力に対応して出力する今回のフィルタリング結果を算出する。
【００８４】
【数８】
【００８５】
図１４は、応用例に係る出力処理の手順を示すフローチャートである。この出力処理は、上記の実施例１に示した出力処理と同様に、ロボット１の電源が作動した場合に起動する処理であり、電源がＯＦＦ状態になるまで繰り返し実行される。
【００８６】
図１４に示すように、開示の装置は、内部状態量zおよび内部状態量z’の初期の位置・姿勢（x,y,θ）として（0,0,0）を設定する（ステップＳ５０１）。続いて、開示の装置は、シフト値dsに初期値｛0,0,0｝を設定する（ステップＳ５０２）。
【００８７】
そして、位置・姿勢の推定結果が取得された場合（ステップＳ５０３肯定）には、開示の装置は、上記の式（４）を用いて、シフト値dsを更新する（ステップＳ５０４）。さらに、開示の装置は、上記の式（５）を用いて、内部状態量zを更新し（ステップＳ５０５）、最新のオドメトリが取得されるのを待機する（ステップＳ５０６）。なお、シフト値dsおよび内部状態量zの更新後には、カウンタnが「０」に初期化される。
【００８８】
一方、位置・姿勢の推定結果が取得されていない場合（ステップＳ５０３否定）には、ステップＳ５０４及びステップＳステップＳ５０５の処理を実行せずに、下記のステップＳ５０６の処理に移行する。
【００８９】
ここで、開示の装置は、カウンタnの値がN以上であるか否かを判定する（ステップＳ５０７）。このとき、nの値がN未満である場合（ステップＳ５０７否定）には、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が少ないと推定できる。この場合には、開示の装置は、下記の式（７）を適用し、内部状態量z’を内部状態量zに近づけ（ステップＳ５０８）、カウンタnをインクリメントし（ステップＳ５０９）、上記のステップＳ５０３の処理に戻る。
【００９０】
一方、nの値がN以上である場合（ステップＳ５０７肯定）には、位置・姿勢の推定結果が入力される間隔に比べ、オドメトリの入力回数が多いと推定できる。この場合には、開示の装置は、内部状態量z’がds(t’)だけ内部状態量zに近付いたとみなし、下記の式（６）用いて、内部状態量z’を更新し（ステップＳ５１０）、上記のステップＳ５０３の処理に戻る。
【００９１】
このように、図１４に示した出力処理を実行することによって、フィルタリング結果が前回に得られたフィルタリング結果との間で極端に大きな変化をとる場合に、フィルタリング結果を時間的に滑らかに変化させることができる。
【００９２】
［分散および統合］
また、図示した各装置の各構成要素は、必ずしも物理的に図示の如く構成されていることを要しない。すなわち、各装置の分散・統合の具体的形態は図示のものに限られず、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。例えば、センサ５ａ、計算部３、推定部５、経路計画部７または移動制御部９をロボット１の外部装置としてネットワーク経由で接続するようにしてもよい。また、センサ５ａ、計算部３、推定部５、経路計画部７または移動制御部９を別の装置がそれぞれ有し、ネットワーク接続されて協働することで、上記のロボット１の機能を実現するようにしてもよい。
【００９３】
［位置姿勢出力プログラム］
また、上記の実施例で説明した各種の処理は、予め用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することによって実現することができる。そこで、以下では、図１５を用いて、上記の実施例と同様の機能を有する位置姿勢出力プログラムを実行するコンピュータの一例について説明する。
【００９４】
図１５は、実施例１及び実施例２に係る位置姿勢出力プログラムを実行するコンピュータの一例について説明するための図である。図１５に示すように、コンピュータ１００は、操作部１１０ａと、スピーカ１１０ｂと、カメラ１１０ｃと、ディスプレイ１２０と、通信部１３０とを有する。さらに、このコンピュータ１００は、ＣＰＵ１５０と、ＲＯＭ１６０と、ＨＤＤ１７０と、ＲＡＭ１８０と有する。これら１１０〜１８０の各部はバス１４０を介して接続される。
【００９５】
ＨＤＤ１７０には、図１５に示すように、上記の実施例１で示した第１の取得部１１と、第２の取得部１２と、更新部１４と、算出部１５と、出力部１６と同様の機能を発揮する位置姿勢出力プログラム１７０ａが予め記憶される。この位置姿勢出力プログラム１７０ａについては、図２に示した各々の第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６の各構成要素と同様、適宜統合又は分離しても良い。すなわち、ＨＤＤ１７０に格納される各データは、常に全てのデータがＨＤＤ１７０に格納される必要はなく、処理に必要なデータのみがＨＤＤ１７０に格納されれば良い。
【００９６】
そして、ＣＰＵ１５０が、位置姿勢出力プログラム１７０ａをＨＤＤ１７０から読み出してＲＡＭ１８０に展開する。これによって、図１５に示すように、位置姿勢出力プログラム１７０ａは、位置姿勢出力プロセス１８０ａとして機能する。この位置姿勢出力プロセス１８０ａは、ＨＤＤ１７０から読み出した各種データを適宜ＲＡＭ１８０上の自身に割り当てられた領域に展開し、この展開した各種データに基づいて各種処理を実行する。なお、位置姿勢出力プロセス１８０ａは、図２に示した第１の取得部１１、第２の取得部１２、更新部１４、算出部１５及び出力部１６にて実行される処理、例えば図６、図７や図１４に示す処理を含む。また、ＣＰＵ１５０上で仮想的に実現される各処理部は、常に全ての処理部がＣＰＵ１５０上で動作する必要はなく、処理に必要な処理部のみが仮想的に実現されれば良い。
【００９７】
なお、上記の位置姿勢出力プログラム１７０ａについては、必ずしも最初からＨＤＤ１７０やＲＯＭ１６０に記憶させておく必要はない。例えば、コンピュータ１００に挿入されるフレキシブルディスク、いわゆるＦＤ、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させる。そして、コンピュータ１００がこれらの可搬用の物理媒体から各プログラムを取得して実行するようにしてもよい。また、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介してコンピュータ１００に接続される他のコンピュータまたはサーバ装置などに各プログラムを記憶させておき、コンピュータ１００がこれらから各プログラムを取得して実行するようにしてもよい。
【符号の説明】
【００９８】
１ ロボット
３ 計算部
５ 推定部
５ａ センサ
７ 経路計画部
９ 移動制御部
１０ 位置姿勢出力装置
１１ 第１の取得部
１２ 第２の取得部
１３ 記憶部
１４ 更新部
１５ 算出部
１６ 出力部
【特許請求の範囲】
【請求項１】
移動体の位置および姿勢を記憶する記憶部と、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得する第１の取得部と、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得する第２の取得部と、
前記第２の取得部によって前記推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、前記記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新する更新部と、
前記第１の取得部によってオドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する出力部と
を有することを特徴とする位置姿勢出力装置。
【請求項２】
前記オドメトリの差分に基づく位置および姿勢から前記推定部による移動体の位置および姿勢の推定結果へ前記更新後の移動体の位置および姿勢を遷移させる度合いを定める定数を算出する算出部をさらに有し、
前記更新部は、
前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分に前記算出部によって算出された定数を乗算した乗算値を用いて、前記記憶部に記憶された移動体の位置および姿勢を更新することを特徴とする請求項１に記載の位置姿勢出力装置。
【請求項３】
前記記憶部は、
前記第２の取得部によって前記推定結果が取得された場合に更新される移動体の位置および姿勢とともに、前記第１の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢をさらに記憶するものであって、
前記更新部は、
前記第１の取得部によってオドメトリが前記第１の間隔で取得される度に、前記第１の間隔と前記第２の間隔との比に基づいて、前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分を平滑化した上で、前記記憶部に記憶された移動体の位置および姿勢のうち前記第１の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢に足し合わせる更新をさらに実行することを特徴とする請求項１または２に記載の位置姿勢出力装置。
【請求項４】
コンピュータに、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得し、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行させることを特徴とする位置姿勢出力プログラム。
【請求項５】
コンピュータが、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得し、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行することを特徴とする位置姿勢出力方法。
【請求項１】
移動体の位置および姿勢を記憶する記憶部と、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得する第１の取得部と、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得する第２の取得部と、
前記第２の取得部によって前記推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、前記記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新する更新部と、
前記第１の取得部によってオドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する出力部と
を有することを特徴とする位置姿勢出力装置。
【請求項２】
前記オドメトリの差分に基づく位置および姿勢から前記推定部による移動体の位置および姿勢の推定結果へ前記更新後の移動体の位置および姿勢を遷移させる度合いを定める定数を算出する算出部をさらに有し、
前記更新部は、
前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分に前記算出部によって算出された定数を乗算した乗算値を用いて、前記記憶部に記憶された移動体の位置および姿勢を更新することを特徴とする請求項１に記載の位置姿勢出力装置。
【請求項３】
前記記憶部は、
前記第２の取得部によって前記推定結果が取得された場合に更新される移動体の位置および姿勢とともに、前記第１の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢をさらに記憶するものであって、
前記更新部は、
前記第１の取得部によってオドメトリが前記第１の間隔で取得される度に、前記第１の間隔と前記第２の間隔との比に基づいて、前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分を平滑化した上で、前記記憶部に記憶された移動体の位置および姿勢のうち前記第１の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢に足し合わせる更新をさらに実行することを特徴とする請求項１または２に記載の位置姿勢出力装置。
【請求項４】
コンピュータに、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得し、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行させることを特徴とする位置姿勢出力プログラム。
【請求項５】
コンピュータが、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第１の間隔で取得し、
前記計算部によって計算されたオドメトリおよび前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第１の間隔よりも長い第２の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に前記推定部によって今回の推定に用いられたオドメトリおよび今回以前の推定に用いられたオドメトリの差分を加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第１の間隔で取得される度に、当該オドメトリと前記推定部によって今回の推定に用いられたオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行することを特徴とする位置姿勢出力方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−２５５７０５（Ｐ２０１２−２５５７０５Ａ）
【公開日】平成２４年１２月２７日（２０１２．１２．２７）
【国際特許分類】
【出願番号】特願２０１１−１２８５４９（Ｐ２０１１−１２８５４９）
【出願日】平成２３年６月８日（２０１１．６．８）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２４年１２月２７日（２０１２．１２．２７）
【国際特許分類】
【出願日】平成２３年６月８日（２０１１．６．８）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]