(57)【要約】【課題】地図生成・自己位置姿勢推定を高精度に行う自律移動装置を提供する。【解決手段】ＬＲＦを搭載した移動体に設けられる自律移動装置であって、ＬＲＦにより計測した距離データを取得する距離データ取得部１１と、移動体の移動量及び角度姿勢変化量を取得する位置姿勢変化量計測部１２と、移動体の傾きを計測する傾き計測部１３と、前記移動体の傾きに基づき距離データを補正し、補正済み距離データとする距離データ補正部１４と、補正済み距離データに基づき、ＬＲＦによる現在の計測範囲の局所地図を生成する局所地図生成部１５と、局所地図、移動体の自己位置・姿勢に基づき、大域地図を生成する大域地図生成部１６と、直前の自己位置・姿勢に移動量及び姿勢変化量を反映して更新したデータを、大域地図及び補正済み距離データに基づき補正して、最新の自己位置・姿勢を求める自己位置姿勢推定部１７とを備える。【選択図】図１
2018005709.tif 000002
【特許請求の範囲】
【請求項１】
移動経路上にある物体までの距離を計測する距離センサを搭載した移動体に設けられる自律移動装置であって、
前記距離センサにより計測した距離データを取得する距離データ取得部と、
前記移動体の水平面上での移動量及び姿勢変化量を取得する位置姿勢変化量計測部と、
前記移動体のロール角及びピッチ角を計測する傾き計測部と、
前記ロール角及び前記ピッチ角に基づき前記距離データを補正し、補正済み距離データとする距離データ補正部と、
前記補正済み距離データに基づき、前記距離センサによる現在の計測範囲の局所地図を生成する局所地図生成部と、
前記局所地図、前記移動体の自己位置及び自己姿勢に基づき、大域地図を生成する大域地図生成部と、
直前の前記自己位置及び前記自己姿勢に前記移動量及び前記姿勢変化量を反映して更新したデータを、前記大域地図及び前記補正済み距離データに基づき補正して、最新の前記自己位置及び前記自己姿勢を求める自己位置姿勢推定部とを備える
ことを特徴とする自律移動装置。
【請求項２】
前記距離データ補正部は、
前記ロール角及び前記ピッチ角を用いて、前記距離データを補正し、前記補正済み距離データとする
ことを特徴とする請求項１に記載の自律移動装置。
【請求項３】
前記距離データ補正部は、
計測角度が等間隔の前記距離データから得られる計測点を、前記ロール角及び前記ピッチ角に基づき前記距離センサの中心点を基準とした水平面へ投影し、該水平面に投影した該計測点の点群を順に結んだ線と、前記距離センサの中心点から前記計測角度毎の方向に伸ばした直線との交点を求め、前記距離センサの中心点から該交点までの距離を算出し、これを前記補正済み距離データとする
ことを特徴とする請求項２に記載の自律移動装置。
【請求項４】
前記距離データ補正部は、
前記水平面投影前の前記計測点の前記水平面からの高さが所定範囲外のデータを削除する
ことを特徴とする請求項３に記載の自律移動装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、２次元のレーザレンジファインダ（ＬＲＦ）を用いて周辺環境を計測しながら動作する移動体（ロボット車両や自動搬送車（ＡＧＶ））において、移動体が傾いた場合のＬＲＦからの距離データを補正する自律移動装置に関するものである。
【背景技術】
【０００２】
２次元のＬＲＦを用いて周辺環境を計測しながら動作する移動体は、ＬＲＦからの距離データを基に２次元の地図生成及び自己位置姿勢推定を行っている（この場合の「位置姿勢」は、２次元なので位置(ｘ，ｙ)と姿勢θ（＝水平面上での向き)となる）。しかしながら、ＬＲＦからの距離データを基に地図生成・自己位置姿勢推定を行う際、路面状況により移動体の姿勢（この場合の「姿勢」は３次元なのでロール・ピッチ・ヨーそれぞれ(ψ，φ，θ)とする）が傾く（ロールψ及びピッチφが変化する）と、この距離データが歪み、地図生成及び自己位置姿勢推定の精度が低下してしまう。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第５１４１５０７号公報
【特許文献２】特願２０１５‐２２５４９３号
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記特許文献１では、移動体が傾いているとき、又は、移動体の傾きが変動している間、ＬＲＦからの距離データに基づく地図生成・自己位置姿勢推定を一時中断する（自己位置姿勢推定は、駆動系からのオドメトリ情報を用いて行う）ことで、精度低下を抑える技術が開示されている。
【０００５】
しかしながら、この技術では、路面において長距離にわたり凹凸が存在する場合など、長い間傾いている状態、又は、その傾きの変動が長く続く場合に、ＬＲＦからの距離データに基づく地図生成・自己位置姿勢推定が長時間行われないことになる。これにより、特に地図生成については、生成精度の低下や地図分断の可能性もある。
【０００６】
本発明は、上述の技術的課題に鑑みて、移動体が傾いているとき、及び、移動体の傾きが変動している間にも、２次元の地図生成及び自己位置姿勢推定を高精度に行うことができる、自律移動装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決する第１の発明に係る自律移動装置は、
移動経路上にある物体までの距離を計測する距離センサを搭載した移動体に設けられる自律移動装置であって、
前記距離センサにより計測した距離データを取得する距離データ取得部と、
前記移動体の水平面上での移動量及び姿勢変化量を取得する位置姿勢変化量計測部と、
前記移動体のロール角及びピッチ角を計測する傾き計測部と、
前記ロール角及び前記ピッチ角に基づき前記距離データを補正し、補正済み距離データとする距離データ補正部と、
前記補正済み距離データに基づき、前記距離センサによる現在の計測範囲の局所地図を生成する局所地図生成部と、
前記局所地図、前記移動体の自己位置及び自己姿勢に基づき、大域地図を生成する大域地図生成部と、
直前の前記自己位置及び前記自己姿勢に前記移動量及び前記姿勢変化量を反映して更新したデータを、前記大域地図及び前記補正済み距離データに基づき補正して、最新の前記自己位置及び前記自己姿勢を求める自己位置姿勢推定部とを備える
ことを特徴とする。
【０００８】
上記課題を解決する第２の発明に係る自律移動装置は、
上記第１の発明に係る自律移動装置において、
前記距離データ補正部は、
前記ロール角及び前記ピッチ角を用いて、前記距離データを補正し、前記補正済み距離データとする
ことを特徴とする。
【０００９】
上記課題を解決する第３の発明に係る自律移動装置は、
上記第２の発明に係る自律移動装置において、
前記距離データ補正部は、
計測角度が等間隔の前記距離データから得られる計測点を、前記ロール角及び前記ピッチ角に基づき前記距離センサの中心点を基準とした水平面へ投影し、該水平面に投影した該計測点の点群を順に結んだ線と、前記距離センサの中心点から前記計測角度毎の方向に伸ばした直線との交点を求め、前記距離センサの中心点から該交点までの距離を算出し、これを前記補正済み距離データとする
ことを特徴とする。
【００１０】
上記課題を解決する第４の発明に係る自律移動装置は、
上記第３の発明に係る自律移動装置において、
前記距離データ補正部は、
前記水平面投影前の前記計測点の前記水平面からの高さが所定範囲外のデータを削除する
ことを特徴とする。
【発明の効果】
【００１１】
本発明に係る自律移動装置によれば、移動体が傾いているとき、及び、移動体の傾きが変動している間にも、地図生成・自己位置姿勢推定を高精度に行うことができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施例１に係る自律移動装置の構成を説明するブロック図である。
【図２】本発明の実施例１に係る自律移動装置の「補正済み距離データ」を算出するまでの動作を説明するフローチャートである。
【図３】本発明の実施例１においてＬＲＦが傾いた状態で取得した距離データを示す概念図である。
【図４】本発明の実施例１においてＬＲＦが傾いた状態で取得した距離データを水平面上へ投影した状態を示す概念図である。
【図５】本発明の実施例１において水平面に投影した隣り合う計測点間を結んだ状態を示す概念図である。
【図６】本発明の実施例１においてＬＲＦを中心に角度ψiで直線を伸ばし、交点を求める様子を示す概念図である。
【図７】本発明の実施例２に係る自律移動装置の「補正済み距離データ」を算出するまでの動作を説明するフローチャートである。
【図８】本発明の実施例２における計測点と所定範囲（上限値及び下限値）との位置関係を説明する模式図である。（ａ）は計測点が所定範囲内にある状態を示し、（ｂ）は計測点が所定範囲外にある状態を示している。
【発明を実施するための形態】
【００１３】
以下、本発明に係る自律移動装置を、実施例にて図面を用いて説明する。
【００１４】
［実施例１］
まず、本実施例に係る自律移動装置の構成について、図１を用いて説明する。図１は、該補正システムの構成を説明するブロック図である。
【００１５】
本実施例に係る自律移動装置は、移動経路上にある物体までの距離を計測する距離センサとしてのＬＲＦを搭載した移動体（ロボット車両や自動搬送車（ＡＧＶ））に設けられるものであり、ＬＲＦ距離データ取得部１１、位置・姿勢変化量計測部１２、傾き計測部１３、距離データ補正部１４、局所地図生成部１５、大域地図生成部１６、自己位置姿勢推定部１７、及び、記憶部１８を備える。
【００１６】
ＬＲＦ距離データ取得部１１では、２次元のＬＲＦにより計測した距離データを取得し、記憶部１８へ保管する。
【００１７】
位置・姿勢変化量計測部１２では、移動体の駆動部分から、水平面（２次元）上での移動体の移動量及び姿勢変化量を取得し、このデータ（位置・姿勢変化量データ）を記憶部１８へ保管する。
【００１８】
傾き計測部１３では、ジャイロセンサ・加速度センサ等の情報から、移動体の３次元での姿勢を計測し、この姿勢の傾き（ロール角及びピッチ角）のデータ（傾きデータ）を記憶部１８へ保管する。
【００１９】
距離データ補正部１４では、傾き計測部１３で計測した傾きデータに基づき、ＬＲＦ距離データ取得部１１で取得した距離データを補正し、このデータを「補正済み距離データ」として記憶部１８へ保管する。
【００２０】
局所地図生成部１５では、距離データ補正部１４で補正した補正済み距離データに基づき、ＬＲＦによる現在の計測範囲の局所地図を生成し、記憶部１８へ保管する。なお、「局所地図」とは、現在ＬＲＦで見えている範囲の２次元の地図であり、移動体が該範囲の障害物などの情報を基に、行きたい方向（行くべき方向）へ進むために用いる地図である。局所地図の具体的な生成手順については、上記特許文献２に記載されたとおりである。
【００２１】
大域地図生成部１６では、局所地図生成部１５で生成した局所地図、及び、局所地図生成時の自己位置・姿勢データ（移動体の水平面上での自己位置及び自己姿勢のデータを指す。詳細は後述する。）に基づき、大域地図を生成し、記憶部１８へ保管する。なお、「大域地図」とは、例えば工場などの施設全体の２次元の地図であり、上記「局所地図」をつなぎ合わせて生成する。移動体は、この「大域地図」上で自分がどこにいるか、そして目的地はどこかを知り、目的地に向かうための経路を決定する。また、大域地図の具体的な生成手順については、上記特許文献２に記載されたとおりである。
【００２２】
なお、スタート時には、水平面上での自己位置・姿勢データを例えば座標（ｘ，ｙ，θ）=（０，０，０）として（あるいは特別な座標を設定しておいても構わない）、大域地図の作成を開始する。
【００２３】
自己位置姿勢推定部１７では、直前の自己位置・姿勢データに位置・姿勢変化量データを反映して更新したデータを、大域地図生成部１６で生成した大域地図と補正済み距離データとをマッチングすることにより補正して、最新の自己位置・姿勢データを生成し、記憶部１８へ保管する。自己位置・姿勢データの具体的な生成手順については、上記特許文献２に記載されたとおりである。
【００２４】
次に、本実施例に係る自律移動装置の「補正済み距離データ」を算出するまでの動作について、図２のフローチャートを用いて説明する。
【００２５】
本実施例では、「移動体の周辺環境の障害物には、壁などの垂直な（鉛直方向に延伸した）ものが多い」と仮定し、傾き計測部１３から取得する傾きデータを用いて、ＬＲＦ距離データ取得部１１で取得した距離データを水平面上に投影して補正する。
【００２６】
ステップＳ１では、ＬＲＦ距離データ取得部１１により、ＬＲＦから距離データを取得し、ステップＳ２では、傾き計測部１３により、傾きデータを取得する。また、下記ステップＳ３〜Ｓ１０については、「距離データ補正部１４」により行われる。
【００２７】
ステップＳ３では、距離データから得られる計測点を、傾きデータを基に、ＬＲＦの中心点を基準とした水平面へ投影する。このステップＳ３について、以下詳述する。
【００２８】
図３は、ＬＲＦが傾いた状態で取得した距離データを示す概念図である。計測角度（ψi）が等間隔の距離データから取得できるそれぞれの計測点ｐiは、ＬＲＦの中心点を基準とした極座標系でｐi（ψi，ｌi）（ただし、正面がψi＝０，０≦ｉ≦Ｎ）となるとすると、ＬＲＦの中心点を基準としたｘ‐ｙ‐ｚ座標系では（ｌiｃｏｓψi，ｌiｓｉｎψi，０）となる。
【００２９】
移動体がロール角φピッチ角θで傾いた場合、ｘ‐ｙ平面が水平なＬＲＦの中心点を基準とする座標系において、回転後のｐiの位置（ｘi，ｙi，ｚi）は、
【数１】
2018005709.tif 000003
となる。
【００３０】
図４は、ＬＲＦが傾いた状態で取得した距離データ（図３）を、水平面上へ投影した状態を示す概念図である。傾いた距離データを水平面に投影した座標は、
ｐ´i（ｌiｃｏｓθｃｏｓψi＋ｌiｓｉｎθｓｉｎφｓｉｎψi，ｌiｃｏｓφｓｉｎψi）となる。
なお、水平面からの高さはｌiｃｏｓθｓｉｎφｓｉｎψi−ｌiｓｉｎθｃｏｓψiとなる。
以上がステップＳ３の説明である。
【００３１】
ステップＳ４では、上記ｉを０とする。ステップＳ５では、ｉがＮ未満であればステップＳ６へ移行し、ｉがＮとなると動作を終了する（下記ステップＳ１０により、下記ステップＳ６〜Ｓ９のサイクルを一順するたびに、ｉは０，１，２，３，…，Ｎと上がっていく）。
【００３２】
ステップＳ６では、図５の概念図に示すように、水平面に投影した隣り合う計測点間を結び、この線分の内、ＬＲＦの中心点から角度ψi方向に伸ばした直線との交わる線分を探索する。
【００３３】
ステップＳ７では、上記ステップＳ６において該当する線分があった場合は、下記ステップＳ８へ移行し、該当する線分がなかった場合には、ステップＳ９へ移行する。
【００３４】
ステップＳ８では、図６の概念図に示すように、上記ステップＳ６における線分と、ＬＲＦの中心点から角度ψi方向に伸ばした直線との交点を求め、ＬＲＦの中心点から該交点までの距離を算出し、角度ψiの「補正済み距離データ」とし、下記ステップＳ１０へ移行する。
【００３５】
すなわち、上記ステップＳ６〜Ｓ８では、上記ステップＳ３において水平面に投影した点ｐ´i，とその隣の点ｐ´i+1の、ＬＲＦの中心点から見た角度は、回転した歪みにより等間隔ではなくなる。そのため、水平面に投影した距離データの点群を順に結んだ線と、ＬＲＦの中心点から角度ψi方向に伸ばした直線との交点を求め、ＬＲＦの中心点から該交点までの距離を算出し、「補正済み距離データ」とするというものである。
【００３６】
ステップＳ９では、上記ステップＳ７において該当する線分がなかった場合に、ψi方向の距離データを「−１」とし、ステップＳ１０へ移行する。なお、この「−１」は、距離データがないことを示し、地図生成や自己位置姿勢推定には用いられない（距離データが削除される）。
そして、ステップＳ１０では、上記ｉの値を１つ繰り上げステップＳ５へ移行する。
【００３７】
すなわち、本実施例では、「移動体の周辺環境の障害物には、壁などの垂直な（鉛直方向に延伸した）ものが多い」と仮定し、傾き計測部１３から取得する傾きデータのロール角及びピッチ角を用いて、ＬＲＦ距離データ取得部１１で取得した距離データを水平面上に投影して補正する。
【００３８】
より詳しくは、距離データ補正部１４において、計測角度が等間隔の距離データから得られる計測点を、移動体のロール角及びピッチ角に基づきＬＲＦの中心点を基準とした水平面へ投影し、該水平面に投影した該計測点の点群を順に結んだ線と、ＬＲＦの中心点から計測角度（ψi）毎の方向に伸ばした直線との交点を求め、ＬＲＦの中心点から該交点までの距離を算出し、これを「補正済み距離データ」とするものである。
【００３９】
本実施例によれば、移動体の傾きによりＬＲＦの距離データが歪むことによっておこる、地図生成時の精度及び自己位置姿勢推定の精度の劣化を、改善することができるため、移動体が傾いているとき、又は、移動体の傾きが変動している間も、地図生成・自己位置姿勢推定を高精度に行うことができる。
【００４０】
［実施例２］
実施例１の場合、移動体の傾きが小さくても距離の遠いところでは、大きく高さの異なる障害物を検出することがある。例えば、ＬＲＦの探索方向が下向きになったとき、遠方の道路面を検出する場合がある。
【００４１】
本実施例では、実施例１と同等の処理を行う際に、ＬＲＦの中心点を基準とした水平面へ投影する前の高さ（計測点ｐiの高さｚi）を用い、この高さが所定範囲外であれば、距離データとして使用しないようにすることで、余分な検出を削除することができる。
【００４２】
図７は、本実施例に係る自律移動装置の「補正済み距離データ」を算出するまでの動作を説明するフローチャートである。図７に示すように、実施例１において説明したステップＳ８を、下記ステップＳ８´‐１〜Ｓ８´‐４に変更する。なお、その他のステップは実施例１と同一のため、以下ではステップＳ８´‐１〜Ｓ８´‐４を中心に説明する。また、これらのステップについては、実施例１同様「距離データ補正部１４」によって行われる。
【００４３】
ステップＳ８´‐１では、ステップＳ６（実施例１参照）における線分と、ＬＲＦの中心点から角度ψi方向に伸ばした直線との交点を求め、ＬＲＦの中心点から該交点までの距離を算出し、これを角度ψiの距離データとし、ステップＳ１０（実施例１参照）へ移行する。
【００４４】
ステップＳ８´‐２では、上記ステップＳ８´‐１で求めた交点の水平面投影前の、水平面からの高さ（計測点ｐiの高さｚi）を算出する。
【００４５】
ステップＳ８´‐３では、上記ステップＳ８´‐２で求めた高さが、所定範囲内であるか否かを判断する。ここで、図８は、本実施例における計測点と所定範囲の上限値及び下限値との位置関係を説明する模式図であり、図中のＡは傾いた移動体を示している。図８（ａ）に示すように計測点ｐiの高さｚiが所定範囲内であれば、下記ステップＳ８´‐４へ、図８（ｂ）に示すように計測点ｐiの高さｚiが所定範囲外であれば、ステップＳ９（実施例１参照）へ、それぞれ移行する。
【００４６】
すなわち、ステップＳ６（実施例１参照）において線分がないと判断した場合に加え、上記ステップＳ８´‐３では、上記ステップＳ８´‐２において算出した高さが所定範囲外である場合にも、距離データを「−１」（ステップＳ９）として、距離データがないものとし、データを削除する。
【００４７】
ステップＳ８´‐４では、ＬＲＦの中心点から上記ステップＳ８´‐１で求めた（水平面上の）交点までの距離を算出し、これを角度ψiの「補正済み距離データ」とする。
【００４８】
すなわち、本実施例では、実施例１にて説明した処理に加え、移動体が傾いたときの水平面からの高さに所定範囲（上限値及び下限値）を設定することで、計測点の水平面からの高さが所定範囲外である場合のデータを、余分なデータ（例えば道路面の検出など）として、水平面に投影したデータから削除することで、走行可能な路面を障害物として検出することを防ぐことができる。
【産業上の利用可能性】
【００４９】
本発明は、ＬＲＦを用いて周辺環境を計測しながら動作する移動体において、移動体が傾いた場合のＬＲＦからの距離データを補正する自律移動装置として好適である。
【符号の説明】
【００５０】
１１  ＬＲＦ距離データ取得部
１２  位置・姿勢変化量計測部
１３  傾き計測部
１４  距離データ補正部
１５  局所地図生成部
１６  大域地図生成部
１７  自己位置姿勢推定部
１８  記憶部
【図１】
2018005709.tif 000004
【図２】
2018005709.tif 000005
【図３】
2018005709.tif 000006
【図４】
2018005709.tif 000007
【図５】
2018005709.tif 000008
【図６】
2018005709.tif 000009
【図７】
2018005709.tif 000010
【図８】
2018005709.tif 000011