JP4660387B2 - 移動体の位置情報補正装置及び方法、その装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体 - Google Patents

Description

本発明は、ロボット、例えば、ホームベースを有する保安ロボット、ヘルパーロボットまたは掃除ロボットのような移動体の制御に係り、特に移動体の位置情報を補正する装置及び方法、その装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体に関する。

従来に、移動体を制御する方法では、移動体の位置を正確に分からなかった。このような従来の方法のうち一つが“Ｍｏｂｉｌｅ Ｒｏｂｏｔｓ ａｎｄ ｔｈｅｉｒ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ”という名称で出願された特許文献１に開示されている。開示された従来の方法は、ホームベースにロボットを帰還させるだけであり、ロボットの位置が分からないという問題点を有する。このような従来の方法のうち他の一つが“Ｍｅｔｈｏｄ ａｎｄ Ｓｙｓｔｅｍ ｆｏｒ ｒｏｂｏｔ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｃｏｎｆｉｎｅｍｅｎｔ”という名称で出願された特許文献２に開示されている。開示された従来の方法は、ロボットが移動できる制限区域を設定できるだけであり、ロボットの位置が分からないという問題点を有する。

前述した従来の方法に比べて、ロボットの位置が概略的に分かる従来の他の方法が“ロボット掃除機の自動充電復帰装置及び方法”という名称で出願された特許文献３に開示されている。開示された従来の方法は、ロボットが充電器側に向かっているときのみ赤外線を受信でき、充電器の位置を概略的に把握できるだけであり、ロボットが自身の正確な位置が分からないという問題点を有する。

一方、左側輪と右側輪とを有するロボットの右側輪及び左側輪の移動量を別途に測定し、別途に測定された量を利用してロボットの位置及び方位角を測定する従来の方法のうち推測航法（ｄｅａｄ ｒｅｃｋｏｎｉｎｇ ｓｙｓｔｅｍ）がある。このような従来の推測船法は、‘Ｂｏｒｅｎｓｔｅｉｎ，Ｊ’により“Ｔｈｅ ＣＬＡＰＰＥＲ：Ａ Ｄｕａｌ−ｄｒｉｖｅ Ｍｏｂｉｌｅ Ｒｏｂｏｔ Ｗｉｔｈ Ｉｎｔｅｒｎａｌ Ｃｏｒｒｅｃｔｉｏｎ ｏｆ Ｄｅａｄ−ｒｅｃｋｏｎｉｎｇ Ｅｒｒｏｒｓ”という名称で出刊された非特許文献１に開示されている。このような従来の推測航法は、ロボットの位置及び方位角のエラーが累積するという問題点を有するので、単独でロボットの位置及び方位角を認識するのに限界を有する。
米国特許第６，５３２，４０４号明細書 米国特許第６，７８１，３３８Ｂ２号明細書 韓国特許出願第２００３−５９７号明細書 ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，Ｓａｎ Ｄｉｅｇｏ，ＣＡ，Ｍａｙ ８−１３，１９９４

本発明が解決しようとする課題は、移動体の位置及び方位角のうち少なくとも一つである位置情報を簡単かつ正確に周辺により影響を受けずに補正できる移動体の位置情報補正装置及び方法を提供するところにある。

本発明が解決しようとする他の課題は、前記移動体の位置情報補正装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体を提供するところにある。

前記の課題を解決するために、移動体の位置及び方位角のうち少なくとも一つについての情報である位置情報を補正する移動体の位置情報補正装置において、少なくとも一つのビームを発生させるビーム発生部と、測定された前記位置情報を入力し、前記発生したビームを検出し、前記入力した測定された位置情報を、前記検出したビームを利用して補正する位置情報補正部と、を備え、前記位置情報補正部は、前記移動体に装着されることを特徴とする。

前記位置情報補正部は、前記ビームを検出し、この検出したビームを出力するビーム検出部と、
前記検出したビームの周波数を検出し、この検出した周波数に相応するビーム情報をあらかじめ保存したビームの情報の中から探すビーム情報探索部と、保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探されたビーム情報を利用して求めた補正された位置及び補正された方位角それぞれに更新する位置情報更新部と、を備えることが好ましい。

前記位置情報更新部は、前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出する方位角算出部と、前記移動体の測定された方位角を前記算出された方位角に更新する方位角更新部と、を備え、前記検出したビームの勾配は、前記探されたビームの情報に含まれることが好ましい。

前記方位角算出部は、前記検出したビームと前記移動体とがなす角度を測定する角度測定部と、前記検出したビームの勾配及び前記測定された角度から、式（８）に基づいて前記方位角を計算する方位角演算部と、を備えることが好ましい。

ここで、θ_ｒは前記計算された方位角を表し、θ_ｍは前記角度測定部で測定された角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す。

前記角度測定部は、最初に検出したビームと前記移動体とがなす角度、及び最後に検出したビームと前記移動体とがなす角度の平均を計算する角度平均部と、前記移動体が等速直進運動を行うか否かを検査し、前記計算された平均を前記検査された結果に応答して前記測定された角度として前記方位角演算部に出力する運動検査部と、を備え、前記角度測定部で測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点の中間時点で検出したビームと前記移動体とがなす角度に該当することが好ましい。

前記位置情報更新部は、保存されている移動体の位置を、前記探したビーム情報を利用して算出した補正された位置に更新する位置更新部を備えることが好ましい。

前記位置更新部は、式（９）及び式（１０）を利用して前記補正された位置を算出することが好ましい。

ここで、ｔｃは前記ビーム検出部で前記ビームを検出した時間を表し、ｘ_ｔｃは時間ｔｃで前記補正された位置のｘ軸座標を表し、ｙ_ｔｃは時間ｔｃで前記補正された位置のｙ軸座標を表し、ａは前記ビームの勾配を表し、ａ及びｂは前記検出した周波数に相応する前記ビーム情報であり、ｘ_ｐ及びｙ_ｐは前記ビーム発生部のｘ軸及びｙ軸座標をそれぞれ表し、ｋはスケール因子を表し、Ｒは前記ビーム検出部の中心から前記ビームを検出した地点までの距離を表す。

前記位置更新部は、式（１１）及び式（１２）を利用して前記補正された位置を算出することが好ましい。

ここで、ｔｃは前記ビーム検出部で前記ビームを検出した時間を表し、ｘ´_ｔｃは時間ｔｃで前記補正された位置のｘ軸座標を表し、ｙ´_ｔｃは時間ｔｃで前記補正された位置のｙ軸座標を表し、ａ´は前記ビームの勾配を表し、ａ´及びｂ´は前記検出された周波数に相応する前記ビーム情報であり、ｘ"_ｔｃは前記移動体の測定された位置のｘ軸座標を表し、ｙ"_ｔｃは前記移動体の測定された位置のｙ軸座標を表す。

前記移動体の位置及び方位角は、推測航法で測定されることが好ましい。

前記ビーム発生部は、赤外線形態のビームを照射することが好ましい。

前記の課題を解決するために、移動体の位置及び方位角のうち少なくとも一つについての情報である位置情報を補正する移動体の位置情報補正方法において、少なくとも一つのビームを照射するステップと、前記ビームを検出し、保存されている位置情報を、前記検出したビームを利用して補正するステップと、を含み、前記ビームを検出するステップは、前記移動体で行われることを特徴とする。

前記位置情報を補正するステップは、前記ビームを検出するステップと、前記検出したビームの周波数を検出し、この検出した周波数に相応するビーム情報をあらかじめ保存した前記ビームの情報中から探すステップと、保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探したビーム情報を利用して求めた補正された位置及び補正された方位角それぞれにより更新するステップと、を含むことが好ましい。

前記更新するステップは、前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出するステップと、前記移動体の測定した方位角を前記算出した方位角に更新するステップと、を含み、前記検出したビームの勾配は、前記探したビームの情報に含まれることが好ましい。

前記方位角を算出するステップは、前記検出したビームと前記移動体とがなす角度を測定するステップと、前記検出したビームの勾配及び前記測定した角度から、式（１３）を用いて前記方位角を計算するステップと、を含むことが好ましい。

ここで、θ_ｒは前記計算した方位角を表し、θ_ｍは前記測定した角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す。

前記角度を測定するステップは、最初に検出したビームと前記移動体とがなす角度、及び最後に検出したビームと前記移動体とがなす角度の平均を計算するステップと、前記移動体が等速直進運動を行うか否かを判断するステップと、を含み、前記移動体が前記等速直進運動を行うとき、前記測定した角度として前記計算した角度の平均を利用して前記方位角が計算し、前記測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点の中間時点で検出したビームと前記移動体とがなす角度に該当することが好ましい。

前記更新するステップは、保存されている移動体の位置を、前記探したビーム情報を利用して算出した補正した位置に更新するステップを含むことが好ましい。

前記の課題を解決するために、移動体の位置及び方位角のうち少なくとも一つについての情報である位置情報を補正する移動体の位置情報補正装置を制御する少なくとも一つのコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体において、少なくとも一つのビームを照射するステップと、前記照射したビームを検出し、前記位置情報を、前記検出したビームを利用して補正するステップと、を行うコンピュータプログラムを保存し、前記ビームを検出するステップは、前記移動体で行われることを特徴とする。

前記位置情報を補正するステップは、前記ビームを検出するステップと、前記検出したビームの周波数を検出し、この検出した周波数に相応するビーム情報をあらかじめ保存した前記ビームの情報中から探すステップと、保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探したビーム情報を利用して求めた補正した位置及び補正した方位角それぞれにより更新するステップと、を含むことが好ましい。

前記更新するステップは、前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出するステップと、前記移動体の測定した方位角を前記算出した方位角に更新するステップと、を含み、前記検出したビームの勾配は、前記探したビームの情報に含まれることが好ましい。

前記方位角を算出するステップは、前記検出したビームと前記移動体とがなす角度を測定するステップと、前記検出したビームの勾配及び前記測定した角度から、式（１４）を用いてに前記方位角を計算するステップと、を含むことが好ましい。

ここで、θ_ｒは前記計算した方位角を表し、θ_ｍは前記測定した角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す。

前記角度を測定するステップは、最初に検出した前記ビームと前記移動体とがなす角度、及び最後に検出した前記ビームと前記移動体とがなす角度の平均を計算するステップと、前記移動体が等速直進運動を行うか否かを判断するステップと、を含み、前記移動体が前記等速直進運動を行うとき、前記測定した角度として前記計算した平均を利用して前記方位角を計算し、前記測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点との中間時点で検出したビームと前記移動体とがなす角度に該当することが好ましい。

前記更新するステップは、前記移動体の測定した位置を、前記探したビーム情報を利用して算出した補正した位置に更新するステップを含むことが好ましい。

前記ビームは、赤外線であることが好ましい。

本発明に係る移動体の位置情報補正装置及び方法、その装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体は、ビームを利用して比較的簡単に移動体の位置情報を補正でき、位置情報のエラーを累積させる従来の推測航法と異なってエラーを累積させないので、位置情報をさらに正確に補正でき、赤外線を利用して位置情報を補正するので、人工的な標識がなくてもよく、夜間や照明がない場合にも位置情報を補正でき、ビームの個数を増加させて移動体の位置情報をさらに正確に補正でき、ビームの個数と位置情報の補正の正確度との間に融通性のある妥協を実行可能にし、誘拐問題、すなわち移動体が人為的に持ち上げられてある場所に移される場合にも、移動体がビームを検出して自身の位置情報を正確に補正可能にして、例えばホームベースを有する家庭用ロボットなどに適用されて推測航法で測定された位置と方位角のうち少なくとも一つを正確に補正できる。

以下、本発明に係る移動体の位置情報補正装置、移動体の位置情報補正方法及び移動体の位置情報補正プログラムを、図面を参照して詳細に説明する。

図１は、本実施形態に係る移動体の位置情報補正装置のブロック図である。
図１に示すように、本実施形態に係る移動体の位置情報補正装置は、ビーム発生部１０及び位置情報補正部１２を備える。

図２は、本実施形態に係る移動体の位置情報補正方法を説明するためのフローチャートである。本実施形態に係る移動体の位置情報補正方法は、ビームを発生させるステップ（ステップ２０）及びビームを利用して位置情報を補正するステップ（ステップ２２）を含む。

図１及び図２にそれぞれ示した移動体の位置情報補正装置及び移動体の位置情報補正方法は、位置情報を次のように補正する。
ここで、位置情報とは、移動体の位置及び方位角のうち少なくとも一つに関する情報である。

以下、本実施形態に係る移動体の位置情報補正装置または移動体の位置情報補正方法により補正される移動体の位置及び方位角は、推測航法で測定することが可能である。すなわち、推測航法により移動体の位置及び方位角のうち少なくとも一つを位置情報として測定し、測定された位置情報を本実施形態に係る移動体の位置情報補正装置または移動体の位置情報補正方法により補正する。

まず、ビーム発生部１０は、少なくとも一つのビームを発生させる（ステップ２０）。ここで、ビームは、例えば、赤外線であり、ビーム発生部１０は、従来のホームベースに相当する役割を担う。
ここで、移動体がロボットである場合、ホームベースは、ロボットを充電させる場所となってもよく、ロボットが自身の機能を行った後で戻らなければならない位置として定義してもよい。

ステップ２０の後に、位置情報補正部１２は、ビーム発生部１０で発生させたビームを検出（センシング、ｓｅｎｓｉｎｇ）し、検出したビームを利用して位置情報を補正し、測定した位置情報を、入力端子ＩＮ１を通じて入力し、この入力した位置情報を補正した位置情報として更新し、この更新した位置情報を、出力端子ＯＵＴ１を通じて出力する（ステップ２２）。このために、ビームを検出する位置情報補正部１２は、移動体に装着される。

図３は、本実施形態に係る移動体の位置情報補正装置の一例を示す平面図である。位置情報補正装置は、位置情報補正部１２を装着した移動体３０及びビーム発生部３２から構成される。図３に示したビーム発生部３２は、図１に示したビーム発生部１０に該当する。ここで、ｘ、ｙは、基準座標系を表す。

図３に示すように、移動体３０は、矢印方向３７に進む間にビーム発生部３２から照射されたビーム３８，３６，３４を順次に検出し、ビーム３８，３６，３４を検出するたびに自身の位置情報を補正する。

図４は、位置情報補正部の一例を示すブロック図である。
図４に示すように、位置情報補正部１２Ａは、ビーム検出部４０、ビーム情報探索部４２及び位置情報更新部４４から構成される。

図５は、図２に示したステップ２２についての一例を説明するためのフローチャートである。ステップ２２Ａは、ビームを検出するステップ（ステップ６０）、ビーム情報を探すステップ（ステップ６２）及び位置情報を更新するステップ（ステップ６４）からなる。

図４に示すように、ビーム検出部４０は、入力端子ＩＮ２を通じて入力したビームを検出し、検出したビームをビーム情報探索部４２に出力する（ステップ６０）。

図６は、ビーム検出部を装着した移動体の外観を例示的に示す図面であって、（ａ）は、移動体の平面図であり、（ｂ）は、移動体の側面図である。

図６（ａ）、（ｂ）に示すように、移動体３０の中心上端にビーム検出部７０が設けられている。ビーム検出部７０は、例えば、オムニレンズ（Ｏｍｎｉ ｌｅｎｓ：図示せず）及び２次元の位置センサ検出器（Ｐｏｓｉｔｉｏｎ Ｓｅｎｓｉｔｉｖｅ Ｄｅｔｅｃｔｏｒ：ＰＳＤ）（図示せず）である。ここで、オムニレンズは、あらゆる方向でビームを検出でき、２次元ＰＳＤは、オムニレンズで検出されたビームの像を結ばせ、結ばれた像の座標を出力する。

図７は、本発明に係る移動体の位置情報補正装置の一例を示す側面図である。
図７に示すように、本発明に係る移動体の位置情報補正装置は、ビーム発生部３２及び移動体３０から構成される。移動体３０には、位置情報補正部１２が設けられ、位置情報補正部１２は、ビーム検出部７０を備える。

図７に示すように、ビーム発生部３２は、ビームを水平方向からθほど下向きに照射する。したがって、移動体３０が低いとしても、ビーム発生部３２から発生したビームを検出することができる。

図８Ａ及び図８Ｂは、ビーム検出部の一例の概略的な外観を示す図面であって、図８Ａは、ビーム検出部の側面図であり、図８Ｂは、２次元ＰＳＤの平面図である。ここで、ｘ、ｙは、基準座標系を表す。

ビーム発生部１０から照射されたビーム７２は、図８Ａに示すビーム検出部４０のオムニレンズ７４で反射された後、スケール補正レンズ７６を通じて２次元ＰＳＤ７８に像として結像され、結像された像についての座標が２次元ＰＳＤ７８から出力される。基準座標系での移動体の正面方向がｘである場合、ｘ軸とビーム７２とがなす角度は、図８Ｂに示すようにθ_ｍとなる。

図５に戻って説明を続ける。
ステップ６０後に、ビーム情報探索部４２は、ビーム検出部４０で検出したビームの周波数を検出し、検出した周波数に相応するビーム情報をあらかじめ保存してあるビームの情報の中から探し、探したビーム情報を位置情報更新部４４に出力する（ステップ６２）。

図３に示すように、ビーム発生部３２から発生するビーム３４，３６，３８は、直線に照射されるので、照射するビームは、次の式（１５）のように表現することが可能である。

ここで、ａは、ビーム発生部３２から照射したビーム、すなわちビーム検出部４０で検出したビームの勾配を表し、ｂは、ビームのｙ軸切片を表す。したがって、ビーム発生部３２から照射されるビーム別にａとｂとが相異なることが分かる。したがって、ビーム発生部３２から照射されるビームの周波数が相異なり、ビーム検出部４０で検出したビームの周波数をビーム情報探索部４２で検出する場合、ビーム情報探索部４２は、検出した周波数に該当するビームのａ及び／又はｂを探すことができる。このために、ビーム情報探索部４２は、ビーム別に異なる周波数別にビーム情報、すなわちａ及びｂのうち少なくとも一つをあらかじめ保存する。

ステップ６２後に、位置情報更新部４４は、測定された位置及び測定した方位角のうち少なくとも一つ、すなわち測定した位置情報を、入力端子ＩＮ３を通じて入力し、この入力した位置情報を、ビーム情報探索部４２が探したビーム情報を利用して求めた補正された位置情報により更新し、更新した位置情報を出力する（ステップ６４）。

本発明の一実施形態によれば、ステップ６４を行うために、位置情報更新部４４は、図４に示した構成とは異なり、方位角算出部５０及び方位角更新部５２のみで具現することも可能である。

方位角算出部５０は、ビーム検出部４０で検出したビームと移動体とがなす角度及びビーム検出部４０で検出したビームの勾配を利用して移動体の方位角を算出し、この算出した方位角を方位角更新部５２に出力する。

図９は、方位角算出部の一例を示すブロック図である。方位角算出部５０Ａは、角度測定部９０及び方位角演算部９２から構成される。

図１０は、図５に示したステップ６４の一例を示すフローチャートである。
図１０に示すように、ステップ６４Ａは、角度を測定するステップ（ステップ１００）、方位角を計算するステップ（ステップ１０２）及び方位角を更新するステップ（ステップ１０４）を含む。

ステップ６２後に、角度測定部９０（図９参照）は、入力端子ＩＮ４を通じてビーム検出部４０から入力した検出したビームと移動体とがなす角度を測定し、測定した角度を方位角演算部９２に出力する（ステップ１００）。

図１１は、図１０に示したステップ１００についての一例を説明するためのフローチャートである。
図１１に示すように、ステップ１００Ａは、角度の平均を計算するステップ（ステップ１１０）及び移動体の運動を検査するステップ（ステップ１１２）を含む。

本発明によれば、図９に示したように、角度測定部９０は、角度平均部９４及び運動検査部９６で具現することが可能である。

ステップ６２後に、角度平均部９４（図９参照）は、ビーム検出部４０で最初、すなわち時間ｔ_ｉで検出されたビームと移動体とがなす角度、及びビーム検出部４０で最後、すなわち時間ｔ_ｆで検出されたビームと移動体とがなす角度の平均を計算し、計算した角度の平均を運動検査部９６に出力する（ステップ１１０）。このために、角度平均部９４は、ビーム検出部４０で最初に検出したビームと最後に検出したビームとを、入力端子ＩＮ４を通じてビーム検出部４０から入力する。すなわち、角度測定部９０で測定されて方位角演算部９２に出力される角度は、最初ｔ_ｉと最後ｔ_ｆの中間時点ｔ_ｃで検出されたビームと移動体とがなす角度に該当する。

ステップ１１０後に、運動検査部９６は、入力端子ＩＮ４を通じてビーム検出部４０から入力した時間ｔ_ｉ，ｔ_ｆで検出されたビームを入力し、入力したビームを利用して移動体が等速直進運動を行うか否かを検査し、角度平均部９４で計算した平均を、検査した結果に応答して方位角演算部９２で測定された角度として出力する（ステップ１１２）。例えば、運動検査部９６は、移動体が等速直進運動を行うか否かをビーム検出部４０で検出された結果を利用して判断する（ステップ１１２）。もし、移動体が等速直進運動を行うと判断されれば、運動検査部９６は、角度平均部９４から入力した計算された平均を方位角演算部９２で測定された角度としてバイパスする。

一方、ステップ１００後に、方位角演算部９２は、入力端子ＩＮ５を通じてビーム情報探索部４２から入力した検出されたビームの勾配ａ、及び角度測定部９０から入力した測定された角度θ_ｍから次の式（１６）のように方位角θ_ｒを計算し、計算した方位角を、出力端子ＯＵＴ４を通じて方位角更新部５２に出力する（ステップ１０２）。

ここで、θ_ｈはａｒｃｔａｎ（ａ）を表す。
図１２は、図４に示した位置情報補正部１２Ａの理解を助けるための図面である。ここで、ｘ_ｍ，ｙ_ｍは、移動体３０の座標系、例えば、ロボット座標系を表す。

図１２に示すように、移動体３０のビーム検出部４０は、ビーム発生部３２から照射されるビーム１２０を検出し、この検出したビームをビーム情報探索部４２に出力する。このとき、ビーム情報探索部４２は、検出したビーム１２０の周波数を分析し、あらかじめ保存してあるビーム情報のうち分析した周波数に相応するビーム情報を探す。すなわち、ビーム情報探索部４２は、検出したビーム１２０の勾配ａをビーム情報として探す。このとき、方位角算出部５０Ａの角度測定部９０は、検出したビーム１２０と移動体３０とがなす角度θ_ｒを測定し、測定した角度θ_ｒを方位角演算部９２に出力する。角度測定部９０の角度平均部９４は、図１２に示した角度θ_ｍを測定する。

図１３Ａないし図１３Ｄは、図９に示した角度平均部９４の理解を助けるための図面であって、図１３Ａは、ビームを検出するビーム検出部４０の外観を例示的に示す図面であって、オムニレンズ７４及びＰＳＤ７８から構成され、図１３Ｂは、オムニレンズ７４の側部でビーム１４２が検出される線１４０を通じて反射されてＰＳＤ７８に結ばれた像１４６を示す図面であり、図１３Ｃは、オムニレンズ７４の側部でビーム１４４が検出される線１４０を通じて反射されてＰＳＤ７８に結ばれた像１４８を示す図面であり、図１３Ｄは、結ばれた像１４６，１４８及び円弧１５２を示す図面である。

図１３Ａに示すように、オムニレンズ７４の側部にある線１４０を通じて最初の時間ｔ_ｉで検出されたビーム１４２は、図１３Ｂに示したようにＰＳＤ７８に像１４６として結像される。このとき、移動体が矢印方向１４３に移動する場合、オムニレンズ７４の側部にある線１４０を通じて最後の時間ｔ_ｆで検出されたビーム１４４は、図１３Ｃに示すようにＰＳＤ７８に像１４８として結像される。

結局、図１３Ｄに示したように、移動体が矢印方向１４３に等速直進運動を行えば、ＰＳＤ７８で結像された像１４６，１４８は、円弧１５２をなす。したがって、円弧の中間時点ｔｃ、すなわち最初の時間ｔ_ｉと最後の時間ｔ_ｆの中間でビーム１５０がＰＳＤ７８の中心を経る場合、角度平均部９４は、次の式（１７）のようにθ_ｍを計算できる。

ここで、θ_ｍｉは時間ｔ_ｉで測定された角度を表し、θ_ｍｆは時間ｔ_ｆで測定された角度を表す。
一方、図１０に示すように、ステップ１０２後に、方位角更新部５２は、入力端子ＩＮ３を通じて入力した移動体の測定された方位角を方位角算出部５０で算出された方位角に更新し、更新された方位角を、出力端子ＯＵＴ２を通じて出力する（ステップ１０４）。

本発明の他の実施形態によれば、ステップ６４を行うために、位置情報更新部４４は、図４に示した構成と異なり、位置更新部５４のみで具現することも可能である。

図１４は、図５に示したステップ６４の一例を説明するためのフローチャートである。ステップ６４Ｂは、位置を更新するステップ（ステップ１６０）を含む。

ステップ６２後に、位置更新部５４は、ビーム情報探索部４２で探されたビーム情報を利用して補正された位置を算出し、入力端子ＩＮ３を通じて移動体の測定された位置を入力し、算出された補正された位置に測定された位置を更新し、更新された位置を、出力端子ＯＵＴ３を通じて出力する（ステップ１６０）。

図１５Ａないし図１５Ｃは、位置更新部５４の一実施形態を説明するための図面であって、図１５Ａは、位置情報補正装置の平面図であり、図１５Ｂは、ビーム検出部４０の外観を示す図面であり、図１５Ｃは、ＰＳＤの外観を示す図面である。ここで、ｘ，ｙは、基準座標系を表す。

本発明の一実施形態によれば、位置更新部５４は、次の式（１８）及び式（１９）を利用して補正された位置を算出できる。

図１５Ａないし図１５Ｃに示すように、ｘ_ｔｃは、時間ｔ_ｃで移動体３０の補正された位置のｘ軸座標を表し、ｙ_ｔｃは、時間ｔ_ｃで移動体３０の補正された位置のｙ軸座標を表す。すなわち、ｘ_ｔｃ，ｙ_ｔｃは、図１５Ａに示した移動体３０の補正された位置２０４のｘ軸及びｙ軸座標をそれぞれ表す。ａ，ｂは、ビーム情報探索部４２で検出された周波数に相応して探されたビーム情報を表し、ｘ_ｐ，ｙ_ｐは、図１５Ａに示したビーム発生部３２でビームが発生する地点２０２のｘ軸及びｙ軸座標をそれぞれ表す。ここで、ｋは、スケール因子を表し、Ｒは、図１５Ｃに示すようにビーム検出部７８の中心７９からビームが検出された地点までの距離を表す。図１５Ｂに示すように、移動体３０から近い距離に位置したビーム発生部３２から発生したビーム２１０により形成されるＲは、移動体３０から遠い距離に位置したビーム発生部３２から発生したビーム２１２により形成されるＲより大きいことが分かる。したがって、式（１９）から分かるように、移動体３０とビーム発生部３２との距離２００は、ｋによりＲと関係がある。

結局、式（１８）において、ａ，ｂは、ビーム情報探索部４２で探されるビーム情報であり、式（１９）において、ｘ_ｐ，ｙ_ｐは、移動体３０の位置更新部５４にあらかじめ与えられる情報であり、Ｒは、位置更新部５４がビーム検出部４０で検出されたビームを利用して測定した値である。したがって、このような値ａ，ｂ，ｘ_ｐ，ｙ_ｐ，Ｒから、式（１８）及び式（１９）により移動体の補正された位置ｘ_ｔｃ，ｙ_ｔｃを求めることができることがわかる。

図１６は、位置更新部の他の実施形態を説明するための図面である。
本発明の他の実施形態によれば、位置更新部５４は、次のような式（２０）及び式（２１）を利用して補正された位置を算出できる。

図１６に示すように、ｘ´_ｔｃは、時間ｔｃで移動体３０の補正された位置のｘ軸座標を表し、ｙ´_ｔｃは、時間ｔｃで移動体３０の補正された位置のｙ軸座標を表す。すなわち、ｘ´_ｔｃ及びｙ´_ｔｃは、図１６に示した移動体３０の補正された位置２３０のｘ軸及びｙ軸座標をそれぞれ表す。ａ´は、ビーム２４０の勾配を表し、ａ´及びｂ´は、ビーム情報探索部４２で検出された周波数に相応して探されたビーム情報を表し、ｘ"_ｔｃは、移動体３０の測定された位置のｘ軸座標を表し、ｙ"_ｔｃは、移動体３０の測定された位置のｙ軸座標を表す。すなわち、ｘ"_ｔｃ及びｙ"_ｔｃは、図１６に示した移動体３０の測定された位置２３２のｘ軸及びｙ軸座標をそれぞれ表す。

結局、式（２０）において、ａ´及びｂ´は、ビーム情報探索部４２で探されるビーム情報であり、式（２１）において、ｘ"_ｔｃ及びｙ"_ｔｃは、前述したように推測航法などで測定されて入力端子ＩＮ３を通じて外部から位置更新部５４に入力される情報である。したがって、このような値ａ´，ｂ´，ｘ"_ｔｃ，ｙ"_ｔｃから、式（２０）及び式（２１）により移動体の補正された位置ｘ´_ｔｃ及びｙ´_ｔｃを求めることができることがわかる。すなわち、式（２０）は、ビーム２４０により基準座標系で形成される直線を表す式であり、式（２１）は、ビーム２４０と直交する線２４２を表す式である。

本発明のさらに他の実施形態によれば、ステップ６４を行うために、位置情報更新部４４は、図４に示したように方位角算出部５０、方位角更新部５２及び位置更新部５４で具現することも可能である。この場合、図１０に示したステップ６４Ａと図１４に示したステップ６４Ｂとを同時に行ってもよく、ステップ６４Ａがステップ６４Ｂより先に行われてもよく、ステップ６４Ｂがステップ６４Ａより先に行われてもよい。

一方、前述した本発明の実施形態は、コンピュータで実行されうるプログラムで作成可能であり、コンピュータで読み取り可能な記録媒体を利用して前記プログラムを動作させる汎用デジタルコンピュータで具現することが可能である。コンピュータで読み取り可能な記録媒体は、磁気記録媒体（例えば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フレキシブルディスク、ハードディスクなど）、光学的読み取り媒体（例えば、ＣＤ−ＲＯＭ、ＤＶＤなど）及びキャリアウェーブ（例えば、インターネットを通じた伝送）のような記録媒体を含む。

以下、本発明に係る移動体の位置情報補正装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体を次のように説明する。

移動体の測定された位置及び方位角のうち少なくとも一つについての情報である位置情報を補正する移動体の前述した位置情報補正装置を制御する少なくとも一つのコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体は、少なくとも一つのビームを発生させるステップ、及び発生したビームを検出させ、検出されたビームを利用して位置情報を補正させるステップを行うコンピュータプログラムを保存する。このとき、位置情報を補正させるステップは、ビームを検出させるステップ、検出したビームの周波数を検出させ、検出された周波数に相応するビーム情報をあらかじめ保存したビームの情報のうちから探すステップ、及び測定された位置及び測定された方位角のうち少なくとも一つを、探したビーム情報を利用して求めた補正された位置及び補正された方位角それぞれにより更新するステップを行う。

ここで、更新するステップは、検出されたビームと移動体とがなす角度及び検出されたビームの勾配を利用して、移動体の方位角を算出させるステップ、及び移動体の測定された方位角を算出された方位角に更新させるステップを行い、検出されたビームの勾配は、探されたビームの情報に含まれる。方位角を算出させるステップは、検出されたビームと移動体とがなす角度を測定させるステップ、及び検出されたビームの勾配及び測定された角度から、前述した式（１６）のように方位角を計算させるステップを行う。角度を測定させるステップは、最初に検出されたビームと移動体とがなす角度、及び最後に検出されたビームと移動体とがなす角度の平均を計算させるステップ、及び移動体が等速直進運動を行うか否かを判断するステップを行い、移動体が等速直進運動を行うとき、測定された角度として計算された平均が利用されて方位角が計算され、測定された角度は、最初と最後の中間時点で検出されたビームと移動体とがなす角度に該当する。更新させるステップは、移動体の測定された位置を、探されたビーム情報を利用して算出した補正された位置に更新させるステップを行う。

結局、ビーム発生部１０から発生するビームを移動体が経るたびに、すなわち移動体がビームを検出するたびに、位置情報補正部１２は、移動体の測定された位置情報、すなわち測定された位置及び測定された方位角のうち少なくとも一つを補正できる。したがって、移動体の測定された位置情報は、ビーム発生部１０から発生するビームの個数が増加するほどさらに正確に補正されることが可能である。

例えば、移動体をロボットとし、ビーム発生部１０は、ビームを赤外線の形態に発生させると仮定し、ロボットを４０坪台のアパートの居間と台所で走行させ、ロボットが５ｍ走行するときに一つ以上の赤外線を検出すると仮定し、ビームは、少なくとも二つ以上であると仮定しつつ、従来の推測航法と本発明とを比較すれば次の表（１）の通りである。

ここで、‘ｎｏ ｔｕｎｉｎｇ’は、移動体の車輪の間の距離及び車輪の直径などの数値を精密に調整していないことを意味し、‘ｔｕｎｉｎｇ’は、移動体の車輪の間の距離及び車輪の直径などの数値を精密に調整した状態を意味する。

表１から分かるように、本発明は、従来の推測航法より位置エラー及び方位角エラーをはるかに少なくなる。

本発明は、ロボットなどの移動体の制御関連の技術分野に適用可能である。

本発明による移動体の位置情報補正装置のブロック図である。 本発明による移動体の位置情報補正方法を説明するためのフローチャートである。 図１に示した位置情報補正装置の平面図を例示的に示す図面である。 図１に示した位置情報補正部の本発明による実施形態のブロック図である。 図２に示したステップ２２についての本発明による実施形態を説明するためのフローチャートである。 図４に示したビーム検出部を装着した移動体の外観を例示的に示す図面である。 図１に示した位置情報補正装置の側面図を例示的に示す図面である。 図４に示したビーム検出部の概略的な外観を例示的に示す図面である。 図４に示したビーム検出部の概略的な外観を例示的に示す図面である。 図４に示した方位角算出部の本発明による実施形態のブロック図である。 図５に示したステップ６４の本発明による実施形態のフローチャートである。 図１０に示したステップ１００についての本発明による実施形態を説明するためのフローチャートである。 図４に示した位置情報補正部の理解を助けるための図面である。 図９に示した角度平均部の理解を助けるための図面である。 図９に示した角度平均部の理解を助けるための図面である。 図９に示した角度平均部の理解を助けるための図面である。 図９に示した角度平均部の理解を助けるための図面である。 図５に示したステップ６４についての本発明による実施形態を説明するためのフローチャートである。 位置更新部の一実施形態を説明するための図面である。 位置更新部の一実施形態を説明するための図面である。 位置更新部の一実施形態を説明するための図面である。 位置更新部の他の実施形態を説明するための図面である。

符号の説明

１０ ビーム発生部
１２ 位置情報補正部
４０ ビーム検出部
４２ ビーム情報探索部
４４ 位置情報更新部
５０ 方位角算出部
５２ 方位角更新部
５４ 位置更新部

Claims (23)

1. 移動体の位置及び方位角のうち少なくとも１つについての情報である位置情報を補正する移動体の位置情報補正装置において、前記位置情報は推測航法により測定され、該当位置情報補正装置は、
   相異なる周波数のビームを発生させる水平面内で相異なる勾配の方向に同じ場所から発生させるビーム発生部と、
   発生したビームを検出し、ビームの周波数はビームの勾配により異なることを示すあらかじめ保存されたビーム情報を参照し、前記位置情報のうち前記方位角については検出されたビーム周波数に対応する勾配を用いて補正し、前記位置情報のうち前記位置については検出されたビームの周波数に対応する勾配及び前記ビーム発生部からビームが発生される地点に対する情報を用いて補正する位置情報補正部と、を備え、
   前記位置情報補正部は、前記移動体に装着されることを特徴とする移動体の位置情報補正装置。
2. 前記位置情報補正部は、
   前記ビームを検出するビーム検出部と、
   前記検出したビームの周波数を検出し、検出した該周波数に相応するビームの情報をあらかじめ保存されたビーム情報の中から探すビーム情報探索部と、
   保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探されたビーム情報を利用して求めた補正された位置及び補正された方位角それぞれに更新する位置情報更新部と、
   を備えることを特徴とする請求項１に記載の移動体の位置情報補正装置。
3. 前記位置情報更新部は、
   前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出する方位角算出部と、
   前記移動体の測定された方位角を前記算出された方位角に更新する方位角更新部と、
   を備え、
   前記検出したビームの勾配は、前記探されたビームの情報に含まれることを特徴とする請求項２に記載の移動体の位置情報補正装置。
4. 前記方位角算出部は、
   前記検出したビームと前記移動体とがなす角度を測定する角度測定部と、
   前記検出したビームの勾配及び前記測定された角度から、式（１）に基づいて前記方位角を計算する方位角演算部と、
   を備えることを特徴とする請求項３に記載の移動体の位置情報補正装置。
   （ここで、θ_ｒは前記計算された方位角を表し、θ_ｍは前記角度測定部で測定された角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す）。
5. 前記角度測定部は、
   最初に検出したビームと前記移動体とがなす角度、及び最後に検出したビームと前記移動体とがなす角度の平均を計算する角度平均部と、
   前記移動体が等速直進運動を行うか否かを検査し、前記移動体が等速直線運動を行うと検査されれば、前記計算された平均を前記測定された角度として前記方位角演算部に出力する運動検査部と、
   を備え、
   前記角度測定部で測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点の中間時点で検出したビームと前記移動体とがなす角度に該当することを特徴とする請求項４に記載の移動体の位置情報補正装置。
6. 前記位置情報更新部は、
   保存されている移動体の位置を、前記探したビーム情報を利用して算出した補正された位置に更新する位置更新部を備えることを特徴とする請求項２または請求項３に記載の移動体の位置情報補正装置。
7. 前記位置更新部は、式（２）及び式（３）を利用して前記補正された位置を算出することを特徴とする請求項６に記載の移動体の位置情報補正装置。
   （ここで、ｔｃは前記ビーム検出部で前記ビームを検出した時間を表し、ｘ_ｔｃは時間ｔｃで前記補正された位置のｘ軸座標を表し、ｙ_ｔｃは時間ｔｃで前記補正された位置のｙ軸座標を表し、ａは前記ビームの勾配を表し、ａ及びｂは前記検出した周波数に相応する前記ビーム情報であり、ｘ_ｐ及びｙ_ｐは前記ビーム発生部のｘ軸及びｙ軸座標をそれぞれ表し、ｋはスケール因子を表し、Ｒは前記ビーム検出部の中心から前記ビームを検出した地点までの距離を表す）。
8. 前記位置更新部は、式（４）及び式（５）を利用して前記補正された位置を算出することを特徴とする請求項６に記載の移動体の位置情報補正装置。
   （ここで、ｔｃは前記ビーム検出部で前記ビームを検出した時間を表し、ｘ´_ｔｃは時間ｔｃで前記補正された位置のｘ軸座標を表し、ｙ´_ｔｃは時間ｔｃで前記補正された位置のｙ軸座標を表し、ａ´は前記ビームの勾配を表し、ａ´及びｂ´は前記検出された周波数に相応する前記ビーム情報であり、ｘ"_ｔｃは前記移動体の測定された位置のｘ軸座標を表し、ｙ"_ｔｃは前記移動体の測定された位置のｙ軸座標を表す）。
9. 前記位置情報補正部は推測航法で測定された前記移動体の位置及び方位角を補正することを特徴とする請求項１に記載の移動体の位置情報補正装置。
10. 前記ビーム発生部は、赤外線形態のビームを照射することを特徴とする請求項１に記載の移動体の位置情報補正装置。
11. 移動体の位置及び方位角のうち少なくとも１つについての情報である位置情報を補正する移動体の位置情報補正方法において、前記位置情報は推測航法により測定され、該当位置情報補正方法は、
    相異なる周波数のビームを発生させる水平面内で相異なる勾配の方向に同じ場所から発生させるステップと、
    発生したビームを検出し、ビームの周波数はビームの勾配により異なることを示すあらかじめ保存されたビーム情報を参照し、前記位置情報のうち前記方位角については検出されたビーム周波数に対応する勾配を用いて補正し、前記位置情報のうち前記位置については検出されたビームの周波数に対応する勾配及び前記ビーム発生部からビームが発生される地点に対する情報を用いて補正するステップと、を備え、
    前記ビームを検出するステップは、前記移動体で行われることを特徴とする移動体の位置情報補正方法。
12. 前記位置情報を補正するステップは、
    前記ビームを検出するステップと、
    前記検出したビームの周波数を検出し、検出した該周波数に相応するビームの情報をあらかじめ保存された前記ビーム情報中から探すステップと、
    保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探したビーム情報を利用して求めた補正された位置及び補正された方位角それぞれにより更新するステップと、
    を含むことを特徴とする請求項１１に記載の移動体の位置情報補正方法。
13. 前記更新するステップは、
    前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出するステップと、
    前記移動体の測定した方位角を前記算出した方位角に更新するステップと、
    を含み、
    前記検出したビームの勾配は、前記探したビームの情報に含まれることを特徴とする請求項１２に記載の移動体の位置情報補正方法。
14. 前記方位角を算出するステップは、
    前記検出したビームと前記移動体とがなす角度を測定するステップと、
    前記検出したビームの勾配及び前記測定した角度から、式（６）を用いて前記方位角を計算するステップと、
    を含むことを特徴とする請求項１３に記載の移動体の位置情報補正方法。
    （ここで、θ_ｒは前記計算した方位角を表し、θ_ｍは前記測定した角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す）。
15. 前記角度を測定するステップは、
    最初に検出したビームと前記移動体とがなす角度、及び最後に検出したビームと前記移動体とがなす角度の平均を計算するステップと、
    前記移動体が等速直進運動を行うか否かを判断するステップと、
    を含み、
    前記移動体が前記等速直進運動を行うと判断されれば、前記測定した角度として前記計算した角度の平均を利用して前記方位角を計算し、前記測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点の中間時点で検出したビームと前記移動体とがなす角度に該当することを特徴とする請求項１４に記載の移動体の位置情報補正方法。
16. 前記更新するステップは、
    保存されている移動体の位置を、前記探したビーム情報を利用して算出した補正した位置に更新するステップを含むことを特徴とする請求項１２または請求項１３に記載の移動体の位置情報補正方法。
17. 移動体の位置及び少なくとも方位角のうち少なくとも一ついついての情報である位置情報を補正する移動体の位置情報補正装置を制御する少なくとも一つのコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体において、前記位置情報は推測航法により測定され、前記コンピュータプログラムは、
    相異なる周波数のビームを発生させる水平面内で相異なる勾配の方向に同じ場所から発生させるステップをビーム発生部に行わせ、
    発生したビームを検出し、ビームの周波数はビームの勾配により異なることを示すあらかじめ保存されたビーム情報を参照し、前記位置情報のうち前記方位角については検出されたビーム情報に含まれた周波数に対応する勾配を用いて補正し、前記位置情報のうち前記位置については検出されたビームの周波数に対応する勾配及び前記ビーム発生部からビームが発生される地点に対する情報を用いて補正させるステップを、前記移動体の位置情報補正部に行わせることを特徴とするコンピュータで読み取り可能な記録媒体。
18. 前記位置情報を補正するステップは、
    前記ビームを検出するステップと、
    前記検出したビームの周波数を検出し、検出した該周波数に相応するビームの情報をあらかじめ保存された前記ビーム情報中から探すステップと、
    保存されている移動体の位置及び保存されている移動体の方位角のうち少なくとも一つを、前記探したビーム情報を利用して求めた補正した位置及び補正した方位角それぞれにより更新するステップと、
    を含むことを特徴とする請求項１７に記載のコンピュータで読み取り可能な記録媒体。
19. 前記更新するステップは、
    前記検出したビームと前記移動体とがなす角度及び前記検出したビームの勾配を利用して、前記移動体の前記方位角を算出するステップと、
    前記移動体の測定した方位角を前記算出した方位角に更新するステップと、
    を含み、
    前記検出したビームの勾配は、前記探したビームの情報に含まれることを特徴とする請求項１８に記載のコンピュータで読み取り可能な記録媒体。
20. 前記方位角を算出するステップは、
    前記検出したビームと前記移動体とがなす角度を測定するステップと、
    前記検出したビームの勾配及び前記測定した角度から、式７を用いて前記方位角を計算するステップと、
    を含むことを特徴とする請求項１９に記載のコンピュータで読み取り可能な記録媒体。
    （ここで、θ_ｒは前記計算した方位角を表し、θ_ｍは前記測定した角度を表し、θ_ｈはａｒｃｔａｎ（ａ）であり、ａは前記検出したビームの勾配に該当する角度を表す）。
21. 前記角度を測定するステップは、
    最初に検出した前記ビームと前記移動体とがなす角度、及び最後に検出した前記ビームと前記移動体とがなす角度の平均を計算するステップと、
    前記移動体が等速直進運動を行うか否かを判断するステップと、
    を含み、
    前記移動体が前記等速直進運動を行うと判断されれば、前記測定した角度として前記計算した平均を利用して前記方位角を計算し、前記測定した角度は、前記最初にビームを検出した時点と前記最後にビームを検出した時点との中間時点で検出したビームと前記移動体とがなす角度に該当することを特徴とする請求項２０に記載のコンピュータで読み取り可能な記録媒体。
22. 前記更新するステップは、
    前記移動体の測定した位置を、前記探したビーム情報を利用して算出した補正した位置に更新するステップを含むことを特徴とする請求項１８または請求項１９に記載のコンピュータで読み取り可能な記録媒体。
23. 前記ビームは、赤外線であることを特徴とする請求項１９に記載のコンピュータで読み取り可能な記録媒体。