知財求人 - 知財ポータルサイト「IP Force」
特開2024-137661障害物検出装置、移動体、プログラムおよび、障害物検出方法
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024137661
(43)【公開日】2024-10-07
(54)【発明の名称】障害物検出装置、移動体、プログラムおよび、障害物検出方法
(51)【国際特許分類】
G05D 1/617 20240101AFI20240927BHJP
G05D 1/43 20240101ALI20240927BHJP
G05D 1/242 20240101ALI20240927BHJP
【ＦＩ】
G05D1/617
G05D1/43
G05D1/242
【審査請求】未請求
【請求項の数】12
【出願形態】ＯＬ
(21)【出願番号】P 2023201928
(22)【出願日】2023-11-29
(31)【優先権主張番号】P 2023047196
(32)【優先日】2023-03-23
(33)【優先権主張国・地域又は機関】JP
(71)【出願人】
【識別番号】000006747
【氏名又は名称】株式会社リコー
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】保坂 健人
(72)【発明者】
【氏名】工藤 宏一
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA01
5H301BB14
5H301CC03
5H301CC06
5H301CC10
5H301GG08
5H301GG09
5H301GG10
5H301HH19
5H301LL01
5H301LL07
5H301LL08
5H301LL11
5H301LL14
(57)【要約】
【課題】３次元のセンサデータを用いて、移動体の路面への進入角の変動の影響を受けにくい路面障害物検知を行う。
【解決手段】走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、を備える。
【選択図】図３
【解決手段】走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、を備える。
【選択図】図３
【特許請求の範囲】
【請求項1】
走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、
前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、
を備えることを特徴とする障害物検出装置。
前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、
を備えることを特徴とする障害物検出装置。
【請求項2】
前記三次元の走行路面データは、前記移動体を基準とした三次元のデータであり、
前記高さマップは、前記高さ情報として前記走行路面からの最大高さを保持した２次元マップである、
ことを特徴とする請求項１に記載の障害物検出装置。
前記高さマップは、前記高さ情報として前記走行路面からの最大高さを保持した２次元マップである、
ことを特徴とする請求項１に記載の障害物検出装置。
【請求項3】
前記障害物検知部は、前記走行路面データに、平面検出により振動補正を施した上で時間により重ね合わせて得られた、補正後の前記走行路面データに基づき、前記高さマップを生成する、
ことを特徴とする請求項２に記載の障害物検出装置。
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項4】
前記三次元測距センサは、前記移動体の走行方向の先端部手前までの前記走行路面を検知できる高さかつ傾斜角度に設置されている、
ことを特徴とする請求項２に記載の障害物検出装置。
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項5】
前記障害物検知部は、
高低差となだらかさの特徴に基づいて前記走行路面に高さ障害物があるかを判定し、
近傍の高さの差に基づいて前記走行路面に溝障害物があるかを判定する、
ことを特徴とする請求項２に記載の障害物検出装置。
高低差となだらかさの特徴に基づいて前記走行路面に高さ障害物があるかを判定し、
近傍の高さの差に基づいて前記走行路面に溝障害物があるかを判定する、
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項6】
前記障害物検知部は、前記高さマップの生成結果を得るための、前記振動補正結果に基づくボクセルグリッドを生成する、
ことを特徴とする請求項２に記載の障害物検出装置。
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項7】
前記障害物検知部は、前記振動補正結果の点群分布に基づいてノイズ除去を行う、
ことを特徴とする請求項２に記載の障害物検出装置。
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項8】
前記移動体の駆動部材は、所定の接地長を有する履帯式走行体であり、
前記障害物検知部は、前記駆動部材に合わせた判定のパラメータを有する、
ことを特徴とする請求項２に記載の障害物検出装置。
前記障害物検知部は、前記駆動部材に合わせた判定のパラメータを有する、
ことを特徴とする請求項２に記載の障害物検出装置。
【請求項9】
前記三次元測距センサは、３６０度全周囲の全面的な前記走行路面データを取得することができる、
ことを特徴とする請求項１に記載の障害物検出装置。
ことを特徴とする請求項１に記載の障害物検出装置。
【請求項10】
走行路面を走行する駆動部材と、
前記駆動部材の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、
前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、
を備えることを特徴とする移動体。
前記駆動部材の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、
前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、
を備えることを特徴とする移動体。
【請求項11】
コンピュータを、
走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部、
として機能させるプログラム。
走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部、
として機能させるプログラム。
【請求項12】
走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知ステップ、
を含む障害物検出方法。
を含む障害物検出方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、障害物検出装置、移動体、プログラムおよび、障害物検出方法に関する。
【背景技術】
【0002】
従来、屋外にて動作させる移動ロボットには、自律走行を行うために障害物を検知して停止や回避を行う、安全技術が必須である。この安全技術には、ＲＧＢＤカメラやステレオカメラ、レーダー、超音波センサなどの様々なセンサが用いられている。近年、安全技術に用いられるセンサとして、レーザ光を照射して、その反射光の情報をもとに対象物までの距離や対象物の形などを計測する技術であるＬｉＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）、特に二次元平面の障害物が検知できる２Ｄ－ＬｉＤＡＲが用いられることが多い。
【0003】
特許文献１には、距離測定装置による一次元のレーザ走査をＫ回行い、ａ回前（Ｋ－ａ回）の走査からＫ回目までのレーザ走査の全てについて、連続して凹形状の障害物として検出する技術が開示されている。
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、従来技術によれば、路面に対し移動ロボットから見て左右方向、つまり２次元のレーザ光のラインが照射された結果としてセンサデータを取得する構成であるため、急な坂道等で移動ロボットの路面に対する進入角度が急となり路面に２次元ラインのレーザ光が十分路面に当たらなくなった場合、センサデータを取得できず路面障害物の検知が困難な場合があった。
【0005】
本発明は、上記に鑑みてなされたものであって、３次元のセンサデータを用いて、移動体の路面への進入角の変動の影響を受けにくい路面障害物検知を行うことを目的とする。
【課題を解決するための手段】
【0006】
上述した課題を解決し、目的を達成するために、本発明は、走行路面を走行する移動体の進行方向前面に配置され、前記走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように設けられた三次元測距センサと、前記三次元測距センサが取得したデータから得られる三次元の走行路面データに基づいて生成された前記走行路面からの高さ情報を保持した高さマップによって前記走行路面上の障害物を検知する障害物検知部と、を備えることを特徴とする。
【発明の効果】
【0007】
本発明によれば、３次元のセンサデータを用いて、移動体の路面への進入角の変動の影響を受けにくい路面障害物検知を行うことができる、という効果を奏する。
【図面の簡単な説明】
【0008】
【発明を実施するための形態】
【0009】
以下に添付図面を参照して、障害物検出装置、移動体、プログラムおよび、障害物検出方法の実施の形態を詳細に説明する。
【0010】
【0011】
なお、本明細書において、Ｙ方向は走行装置１の横幅方向を意味する。また、Ｘ方向は走行装置１の進行方向を意味し、Ｚ方向は走行装置１の高さ方向を示す。また、これらＸ，Ｙ，Ｚで表される座標をロボット座標と呼ぶことがある。すなわち、ロボット座標は走行装置１を中心とする座標である。
【0012】
移動体としての走行装置１は、履帯式走行体１１ａ，１１ｂおよび本体１０を有する。
【0013】
履帯式走行体１１ａ，１１ｂは、走行装置１の駆動部材となるユニットである。また、履帯式走行体１１ａ，１１ｂは、金属またはゴム製のベルトを用いた履帯（クローラ）式の走行体である。
【0014】
履帯式の走行体は、自動車のようなタイヤで走行する走行体と比較して接地面積が広く、例えば、足場の悪い環境においても、安定した走行を行うことができる。また、タイヤで走行する走行体は、回転動作を行う際に旋回スペースを必要とするのに対して、履帯式の走行体を備えた走行装置は、いわゆる超信地旋回を行うことができるため、限られたスペースでも回転動作をスムーズに行うことができる。
【0015】
ここで、超信地旋回とは、左右の履帯を互いに逆方向に等速回転させることによって、車体の中心を軸としてその場で旋回することを指す。このような旋回方法は、スピンターン（ｓｐｉｎ ｔｕｒｎ）等とも呼ばれる。
【0016】
２つの履帯式走行体１１ａ，１１ｂは、本体１０を挟んで、走行装置１が走行可能な状態で設置される。なお、履帯式走行体の数は、２つに限定されるものではなく、３つ以上であってもよい。例えば、走行装置１は、３つの履帯式走行体を平行に三列に整列させる等、走行装置１が走行可能な状態で設置されてもよい。また例えば、走行装置１は、４つの履帯式走行体を自動車のタイヤのように前後左右に配列させてもよい。
【0017】
履帯式走行体１１（１１ａ，１１ｂ）は、三角形の形状を有する。三角形の形状の履帯式走行体１１によれば、例えば前後のサイズに制約がある場合、前後の限られたサイズの中で接地面積を大きくすることができる。これにより、上述のように、走行時の安定性を向上させることができる。一方で、下側（転輪側）よりも上側（駆動輪側）の方が長い、いわゆる戦車タイプの履帯は、前後のサイズに制約がある場合には全体的に接地面積が小さくなって不安定になる。このように、履帯式走行体１１は、比較的に小型の走行装置１の走行性を高める場合に有効である。
【0018】
本体１０は、履帯式走行体１１ａ，１１ｂを走行可能な状態で支持する支持体であるとともに、走行装置１を駆動させるための制御を行う制御装置である。また、本体１０は、履帯式走行体１１ａ，１１ｂを駆動させるための電力を供給する図示しないバッテリを搭載する。
【0019】
走行装置１の本体１０は、電源ボタン１２、起動ボタン１３、非常停止ボタン１４、状態表示ランプ１５、およびバンパー１６，１７を備える。
【0020】
電源ボタン１２は、走行装置１の周辺にいる人が、走行装置１の電源をオンまたはオフする際に押下する操作手段である。起動ボタン１３は、走行装置１の周辺にいる人が、２つの履帯式走行体１１ａ，１１ｂをスタートさせる際に押下する操作手段である。非常停止ボタン１４は、走行装置１の周辺にいる人が、走行中の走行装置１を停止させる際に押下する操作手段である。
【0021】
状態表示ランプ１５は、走行装置１の状態を通知するための通知手段である。状態表示ランプ１５は、例えば、バッテリ残量の低下等の走行装置１の状態が変化した場合、周囲の人に、走行装置１の状態変化を知らせるために点灯する。また、状態表示ランプ１５は、例えば走行装置１の走行を妨げる障害物の存在等が検知された場合等、異常発生のおそれがある場合に点灯する。
【0022】
なお、図１は、走行装置１に状態表示ランプ１５が１つ備えられている例を示すが、状態表示ランプ１５の数は、２つ以上であってもよい。また、通知手段は、状態表示ランプ１５のみならず、スピーカから発せられる警告音等によって走行装置１の状態を通知する構成であってもよい。
【0023】
バンパー１６，１７は、本体１０の前面および後面にそれぞれ設けられ、走行装置１の本体１０および履帯式走行体１１ａ，１１ｂなどを保護する。
【0024】
走行装置１は、高さ０．５ｍの位置に、本体１０の進行方向を正面として、水平方向検知用の測距センサ１１２を備える。水平方向検知用の測距センサ１１２は、光による検知と２次元方向の測距を行う２Ｄ－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）方式のセンサであり、ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）方式や回転ミラー方式が有る。
【0025】
測距センサ１１２は、レーザ光を物体に照射し、物体に当たって跳ね返ってくるまでの時間の計測結果に基づいて、物体までの距離および物体が存在する方向を測定する。測距センサ１１２は、前方Ｘ方向を中心として、２７０度の範囲を計測できるものである。走行装置１は、測距センサ１１２を用いて水平方向の広範囲の物体との距離を計測し、障害物情報として用いる。
【0026】
加えて、走行装置１は、本体１０の進行方向を正面として、高さ０．９ｍ、俯角３０度に、斜め方向検知用の測距センサ１１３を備える。この設置位置は、走行装置１の前面にあるバンパー１６の手前までの走行路面を検知できるように考慮したものである。斜め方向検知用の測距センサ１１３は、３次元方向の測距が可能な３Ｄ－ＬｉＤＡＲ方式の三次元測距センサであり、ＭＥＭＳ方式や回転ミラー方式が有る。
【0027】
３Ｄ－ＬｉＤＡＲとしては、センサ検出面中心から見て円錐７０．４度の範囲を計測でき、距離としては９０ｍまで計測できる、非反復走査方式のものを使用する。非反復走査方式とは、水平と垂直方向に少しずつ位相をずらしながら花を描くようにスキャンする方式であり、時間の蓄積により計測範囲内の点群カバー率が高くなることが特徴である。測距センサ１１３は、水平な走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように、水平よりも下方に角度をつけて取り付けられる。
【0028】
測距センサ１１３は、レーザ光を路面障害物等の物体に照射し、物体に当たって跳ね返ってくるまでの時間の計測結果に基づいて、物体までの距離および物体が存在する方向を測定し、測定した結果をデータとして取得する。
【0029】
ところで、測距センサ１１３の設置位置には、履帯式走行体１１ａ，１１ｂの履帯の幅および長さによって、また、検知したい物体の大きさ、幅、深さ、高さによって、適正値がある。
【0030】
【0031】
図２に示すように、走行装置１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１、メモリ１０２、補助記憶装置１０３、カメラ１１１、測距センサ１１２，１１３、衛星測位システム１１４、ＩＭＵ１１５、バッテリ１２１、モータドライバ１２２ａ，１２２ｂ、走行用モータ１３２ａ，１３２ｂ、ブレーキドライバ１２３ａ，１２３ｂ、ブレーキ用モータ１３３ａ，１３３ｂ、電源ＳＷ１４１、起動ＳＷ１４２、及び非常停止ＳＷ１４３を有する。
【0032】
ＣＰＵ１０１は、走行装置１全体の制御を行う。ＣＰＵ１０１は、メモリ１０２に記憶された走行プログラムに従って動作することにより、障害物検知部１５５（図３参照）として機能する。ＣＰＵ１０１は、メモリ１０２に記憶された走行プログラムに従って動作することにより、マップ生成部が生成した障害物マップを用いて走行する。メモリ１０２は、ＣＰＵ１０１により走行プログラム等を実行するための一時記憶領域である。補助記憶装置１０３は、ＣＰＵ１０１により実行される走行プログラム等を格納する。
【0033】
走行装置１で実行される走行プログラムは、インストール可能な形式または実行可能な形式のファイルでＣＤ－ＲＯＭ、フレキシブルディスク（ＦＤ）、ＣＤ－Ｒ、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）等のコンピュータで読み取り可能な記録媒体に記録されて提供される。
【0034】
また、走行装置１で実行される走行プログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成しても良い。また、走行装置１で実行される走行プログラムをインターネット等のネットワーク経由で提供または配布するように構成しても良い。また、走行装置１で実行される走行プログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。
【0035】
モータドライバ１２２（１２２ａ，１２２ｂ）は、２つの履帯式走行体１１ａ，１１ｂにそれぞれ設けられた走行用モータ１３２（１３２ａ，１３２ｂ）のドライバである。ブレーキドライバ１２３（１２３ａ，１２３ｂ）は、２つの履帯式走行体１１ａ，１１ｂにそれぞれ設けられたブレーキ用モータ１３３（１３３ａ，１３３ｂ）のドライバである。モータドライバ１２２及びブレーキドライバ１２３は、ＣＰＵ１０１からのコマンドを受けて走行用モータ１３２及びブレーキ用モータ１３３をそれぞれ制御する。
【0036】
電源ＳＷ１４１は、走行装置１の電源をオンまたはオフするスイッチである。電源ＳＷ１４１は、上述の電源ボタン１２（図１参照）の押下と連動して作動する。起動ＳＷ１４２は、２つの履帯式走行体１１ａ，１１ｂをスタートさせるスイッチである。起動ＳＷ１４２は、上述の起動ボタン１３（図１参照）の押下と連動して作動する。非常停止ＳＷ１４３は、２つの履帯式走行体１０ａ，１０ｂを非常停止させるスイッチである。非常停止ＳＷ１４３は、上述の非常停止ボタン１４（図１参照）の押下と連動して作動する。
【0037】
カメラ１１１は、全天球カメラ、ステレオカメラ、赤外線カメラ等を含む。測距センサ１１２は、上述の通り、水平方向検知用の２Ｄ－ＬｉＤＡＲ方式のセンサである。測距センサ１１３は、上述の通り、斜め方向検知用の３Ｄ－ＬｉＤＡＲ方式のセンサである。
【0038】
衛星測位システム１１４は、衛星からの電波を受信して、その受信結果に基づいて、地球上における走行装置１の位置を測定する。衛星測位システム１１４は、ＲＴＫ（Real Time Kinematic）－ＧＮＳＳ（Global Navigation Satellite System）を使用する。ＲＴＫ－ＧＮＳＳによる位置推定においては、ＲＴＫ－ＧＮＳＳ測位における高精度測位解（Ｆｉｘ解）が得られているときは、数ｃｍの精度を得ることができる。
【0039】
衛星測位システム１１４は、ＧＮＳＳ受信用の２つのアンテナを備える。衛星測位システム１１４は、２つのアンテナで得られた位置情報に一定距離以上のずれがあるときには、位置情報の信頼性（精度）が低くなっていると判断する。
【0040】
ＩＭＵ１１５は、３軸の加速度センサ及び回転角速度センサなどを備える。走行装置１は、ＩＭＵ１１５の計測データを用いて走行装置１の本体１０の傾き量を検出して、測距センサ１１３によって測定された走行路面に対する高低差を、走行装置１の傾き量に基づき補正する。
【0041】
なお、ＩＭＵ１１５は、測距センサ１１３の近傍に設けられていることが好ましい。測距センサ１１３がアームを介して本体１０に設けられている場合には、ＩＭＵ１１５はアームに設けられていることが好ましい。また、本実施形態においては、ＩＭＵ１１５は、走行装置１の姿勢を推定する自己位置推定用のＩＭＵを兼ねる。
【0042】
これにより、不整地を走行時に本体１０が前後に揺れることに起因して、測距センサ１１３が検出する高さ位置が変化してしまう、という課題を解決することができる。
【0043】
なお、障害物検知部１５５として機能するＣＰＵ１０１は、走行装置１の履帯式走行体１１ａ，１１ｂに合わせた判定のパラメータを有している。これにより、走行装置１の履帯式走行体１１ａ，１１ｂが踏破できる溝の幅や乗り越えられる障害物の高さは、障害物として検知させない設定ができる、という効果がある。
【0044】
続いて、走行装置１の自律走行にかかる動作について説明する。
【0045】
図３は、走行装置１の自律走行にかかる機能構成を示すブロック図である。図３に示すように、走行装置１の自律走行においては、衛星測位システム１１４を用いた走行装置１の自己位置推定にかかる機能と、走行装置１の走行制御にかかる機能と、を含む。
【0046】
まず、走行装置１の自己位置推定について説明する。
【0047】
図３に示すように、走行装置１は、走行装置１の自己位置推定にかかる機能として、オドメトリ計算部１５１と、自己位置推定部１５２と、を備える。
【0048】
走行装置１の自己位置推定に関しては、上述のように衛星測位システム１１４を用いた高精度位置情報を利用するが、位置更新周期が２００ｍｓ～１ｓ程度とばらつきが大きい。そのため、オドメトリ計算部１５１は、２つの履帯式走行体１１ａ，１１ｂをそれぞれ駆動する走行用モータ１３２ａ，１３２ｂの回転角ωから走行装置１の自己位置を推定するオドメトリ計算を行う。走行用モータ１３２ａ，１３２ｂの回転角ωは、走行用モータ１３２ａ，１３２ｂのモータ駆動用のホールセンサパルスや、エンコーダ、タコメータなどの外部センサから得ることができる。
【0049】
自己位置推定部１５２は、自己位置推定結果（Ｘ，Ｙ，θ）を出力する。より詳細には、自己位置推定部１５２は、オドメトリ計算部１５１で計算したオドメトリ情報を、ＲＴＫ－ＧＮＳＳを含む衛星測位システム１１４で得られた位置情報とＩＭＵ１１５で計測された姿勢変化とを用いて補正することで、安定した自己位置推定を行う。
【0050】
ところで、ＲＴＫ－ＧＮＳＳによる位置情報は、反射物などの影響で急に数十ｃｍ～数ｍずれることが有る。したがって、自己位置推定部１５２は、このような急な位置推定のずれにより不安定な動作をしないように、前述の２つのアンテナで取得された位置情報が一定以上ずれていない時には、上述したようなオドメトリ計算部１５１で計算したオドメトリ情報の補正を行う。一方、自己位置推定部１５２は、前述の２つのアンテナで取得された位置情報が一定以上ずれていたり、ＲＴＫ－ＧＮＳＳ測位における高精度測位解（Ｆｉｘ解）が得られていなかったりする場合には、オドメトリ計算部１５１でのオドメトリの計算結果のみで自己位置推定を行う。
【0051】
次に、走行装置１の速度制御について説明する。
【0052】
図３に示すように、走行装置１は、走行装置１の速度制御にかかる機能として、グローバルプランナ部１５３と、ローカルプランナ部１５４と、障害物検知部１５５と、ステアリングコントロール部１５６と、を備える。
【0053】
グローバルプランナ部１５３は、スタート地点からゴールまでの大域的なプラニング（ＷＰリスト）を行い、到達可能なＷＡＹＰＯＩＮＴ（ＷＰ（ｎ））のシーケンスを出力する。ＷＡＹＰＯＩＮＴは、走行装置１の走行経路上の点の集合である。ＷＡＹＰＯＩＮＴは、位置（ｘ，ｙ，ｚ）以外に方向および速度（Ｖｍａｘ）の情報を有している。
【0054】
ローカルプランナ部１５４は、グローバルプランナ部１５３から得られるＷＡＹＰＯＩＮＴ（ＷＰ（ｎ））の情報を基にして、実際にＷＡＹＰＯＩＮＴとＷＡＹＰＯＩＮＴとを結ぶ経路を生成するとともに、目標方向（ＷＰベクトル）を計算する。
【0055】
障害物検知部１５５は、測距センサ１１２，１１３による検知結果に基づいて、障害物検知を行う。障害物検知部１５５は、検知した障害物に応じて、ステアリングコントロール部１５６に対して、回避方向（θ）、速度指示（Ｖ）、停止指示（Ｖ）を出力する。障害物検知部１５５はまた、障害物検知の結果を用いて、走行装置１の走行路面の障害物マップを生成する。
【0056】
ステアリングコントロール部１５６は、走行装置１の速度，角速度を演算するとともに、ステアリング制御するための左右の履帯式走行体１１ａ，１１ｂの速度（ＶＬ，ＶＲ）を計算する。ステアリングコントロール部１５６は、左右の履帯式走行体１１ａ，１１ｂの速度（ＶＬ，ＶＲ）を、シリアルＩＦ（Ｉｎｔｅｒｆａｃｅ）１５７およびモータドライバ１２２ａ，１２２ｂに渡し、走行用モータ１３２ａ，１３２ｂを制御する。
【0057】
ここで、ステアリングコントロール部１５６の速度制御にかかる機能構成について説明する。図４は、ステアリングコントロール部１５６の速度制御にかかる機能構成を示すブロック図である。
【0058】
左右の履帯式走行体１１ａ，１１ｂを独立駆動するクローラ型ロボットである走行装置１は、左右の履帯式走行体１１ａ，１１ｂの速度差によって旋回する。走行負荷の異なる様々な路面で安定した走行をするためには、負荷に依らず左右の履帯式走行体１１ａ，１１ｂを指定した速度で回転させる必要がある。
【0059】
このため、図４に示すように、ステアリングコントロール部１５６は、ステアリングを制御するメジャーループと、左右それぞれの履帯式走行体１１ａ，１１ｂの速度を制御するマイナーループとに分けて、走行装置１の走行制御を実行する。
【0060】
図４に示すように、ステアリングコントロール部１５６は、位置ＰＩＤ（Proportional-Integral-Differential）１６１と、速度ＰＩＤ（Proportional-Integral-Differential）１６２ａ，１６２ｂと、速度演算部１６３ａ，１６３ｂと、を備える。
【0061】
位置ＰＩＤ１６１は、設定値と測定値との差分（位置偏差）から、ステアリング制御するための左右の履帯式走行体１１ａ，１１ｂの操作量を計算する。
【0062】
速度ＰＩＤ１６２ａ，１６２ｂは、速度偏差から操作量の変化分を求める。
【0063】
速度演算部１６３ａ，１６３ｂは、走行用モータ１３２ａ，１３２ｂに取り付けられたホールセンサの信号（モータパルス）に基づいて速度演算部１６３ａ，１６３ｂにてモータ速度の推定を行う。
【0064】
ステアリングコントロール部１５６は、メジャーループでは、走行用モータ１３２ａ，１３２ｂのモータパルスを用いたオドメトリ計算部１５１でのオドメトリの計算結果を、ＲＴＫ－ＧＮＳＳを含む衛星測位システム１１４で得られた位置情報とＩＭＵ１１５で計測された姿勢変化（θｇｐｓ）および速度変化（Ｖｇｐｓ）とを用いて自己位置推定部１５２で補正した自己位置推定結果と、目標地点への目標方向（θｓｅｔ）と目標位置（Ｐｓｅｔ）との差分（位置偏差）を計算する。
【0065】
より詳細には、ステアリングコントロール部１５６は、計算した差分（位置偏差）を位置ＰＩＤ１６１に出力する。
【0066】
位置ＰＩＤ１６１は、設定値と測定値との差分（位置偏差）から、ステアリング制御するための左右の履帯式走行体１１ａ，１１ｂの操作量ＶＬｓｅｔ，ＶＲｓｅｔ（指示速度Ｖおよび走行用モータ１３２ａ，１３２ｂの回転角ω）を計算する。
【0067】
走行装置１は、左右の履帯式走行体１１ａ，１１ｂをメジャーループからの操作量ＶＬｓｅｔ，ＶＲｓｅｔに従って制御されることで、安定した走行を実現することができる。
【0068】
加えて、ステアリングコントロール部１５６は、マイナーループでは、走行用モータ１３２ａ，１３２ｂに取り付けられたホールセンサの信号（モータパルス）に基づいて速度演算部１６３ａ，１６３ｂにてモータ速度の推定を行う。そして、ステアリングコントロール部１５６は、速度演算部１６３ａ，１６３ｂからの速度推定結果と目標速度との差分（速度偏差）に基づき、速度ＰＩＤ１６２ａ，１６２ｂにて走行用モータ１３２ａ，１３２ｂに対するモータ印加電圧を制御して、速度をフィードバック制御する。このようにマイナーループで速度をフィードバック制御するのは、走行用モータ１３２ａ，１３２ｂに対する外乱トルクによる速度変動を抑制するためである。
【0069】
（走行装置の障害物検知処理の例）
次に、走行装置１の自律走行の制御処理のうち、走行装置１の走行路面における路面障害物の検知について説明する。
次に、走行装置１の自律走行の制御処理のうち、走行装置１の走行路面における路面障害物の検知について説明する。
【0070】
まず、走行装置１の走行路面における路面障害物の検知の概要について説明する。走行装置１は、斜め下向きに設置した３Ｄ－ＬｉＤＡＲである測距センサ１１３から得られるデータに対して平面検出による振動補正を施した上で時間により重ね合わせることで詳細な路面データを取得する。得られた振動補正済みの詳細な路面データを前後左右斜めの高さの関係性が把握可能なように、高さ情報としての最大高さを２次元マップのセルに保持した高さ（Ｈｅｉｇｈｔ）マップに変換する。そして、この高さ（Ｈｅｉｇｈｔ）マップの高低差から障害物を判定する。
【0071】
ここで、図５は縁石の検知処理の概要について説明する図である。走行装置１は、図５（ａ）に示す測距センサ１１３から得られる３ＤＬｉＤＡＲの生データを２秒間蓄積した後、図５（ｂ）に示す高さ（Ｈｅｉｇｈｔ）マップを生成する。走行装置１は、高さ（Ｈｅｉｇｈｔ）マップデータを用いて、高低差となだらかさから縁石であるかを判断する。すなわち、走行装置１の障害物検知部１５５は、具体的には、図５（ｂ）の高さ（Ｈｅｉｇｈｔ）マップに対して、地面との高さの差が閾値以上（高さ障害物マップ）、かつ、近傍の２次元マップのセルとの微分値が一定以上（微分障害物マップ）であれば縁石として検知する。
【0072】
ここで、図６は溝の検知処理の概要について説明する図である。走行装置１は、図６（ａ）に示す測距センサ１１３から得られる３ＤＬｉＤＡＲの生データを２秒間蓄積した後、図６（ｂ）に示す高さ（Ｈｅｉｇｈｔ）マップを生成する。障害物検知部１５５は、高さ（Ｈｅｉｇｈｔ）マップデータを用いて、地面と高さの差が閾値以下の２次元マップのセルをクラスタリングして溝であるかを判断する。すなわち、走行装置１の障害物検知部１５５は、地面との高さの差が閾値以下の２次元マップのセルをクラスタリングし、クラスタの２次元マップのセルの中に近傍との微分値が一定以上のものがあればそのクラスタを溝として検知する。溝候補のクラスタ内に高いエッジ情報が一つでもあれば、溝候補クラスタすべてを溝として判断している。
【0073】
こうすることで、縁石や溝といった路面障害物に加えて、空中物体も障害物として検知でき、正面や急な進入角での検知も対応可能となる。基本的には作成した高さ（Ｈｅｉｇｈｔ）マップから障害物かどうか判断するが、縁石のような凸の特徴を持った障害物（以降、縁石障害物と呼ぶ）と溝のような凹の特徴を持った障害物（以降、溝障害物と呼ぶ）ではそれぞれ専用の処理がある。
【0074】
図７は路面障害物の検知処理結果の例を示す図である。図７（ａ）は縁石の検知処理結果の例を示し、図７（ｂ）は溝の検知処理結果の例を示している。図７（ａ）が進入角４５度での縁石検知結果、図７（ｂ）が正面での溝検知結果である。黒い２次元マップのセルが、路面障害物を示している。
【0075】
それぞれ処理を行い、その処理結果を統合することで、凸凹両方の路面障害物の検知を実現している。以降では、フローチャートを用いて、詳細な障害物検知処理のフローを説明する。
【0076】
図８は、走行装置１における路面障害物の検知処理の手順の一例を示すフローチャートである。
【0077】
図８に示すように、障害物検知部１５５は、点群データ保存処理（ステップＳ１）、高さ（Ｈｅｉｇｈｔ）マップ生成処理（ステップＳ２）、高さ障害物マップ生成処理（ステップＳ３）、微分障害物マップ生成処理（ステップＳ４）、溝障害物ラベリングマップ生成処理（ステップＳ５）、障害物マップ生成処理（ステップＳ６）を順に行うことで、障害物検知を行っている。それぞれの処理については以降、順に詳細に説明する。
【0078】
まず、点群データ保存処理（ステップＳ１）について説明する。図９は、点群データ保存処理の手順の一例を示すフローチャートである。
【0079】
図９に示すように、障害物検知部１５５は、まず、斜め方向検知用の測距センサ１１３による走査を開始する（ステップＳ１１）。
【0080】
次いで、障害物検知部１５５は、取得した測距センサ１１３の点群データを走行装置１のロボット座標に変換し、走行路面データである路面点群データを生成する（ステップＳ１２）。このように三次元測距センサから得られたデータに基づき、障害物検知部１５５は、走行路面上の走行装置１を基準とした三次元データである走行路面データを生成する。
【0081】
続いて、障害物検知部１５５は、オドメトリ情報を用いて、すでに保存してある路面点群データを移動する（ステップＳ１３）。なお、走行装置装置１が移動していない等の場合には、オドメトリ情報が無い場合もある。
【0082】
続いて、障害物検知部１５５は、取得した路面点群データを用いて平面検出を実施し、その平面検出結果から地面の傾きを算出する（ステップＳ１４）。具体的には、障害物検知部１５５は、取得した路面点群データのうち、一定矩形範囲（縦幅ｘ：１．０ｍ、横幅ｙ：１．４ｍ）内の抽出し、そのデータを用いて平面検出を実施する。障害物検知部１５５は、検出した平面からPitch角を算出、それを地面の傾きとして出す。
【0083】
続いて、障害物検知部１５５は、算出した地面の傾きを用いて、取得した路面点群データを傾き補正する（ステップＳ１５）。
【0084】
続いて、障害物検知部１５５は、走行装置１自身に当たったセンサデータを除外するために、傾き補正済みの取得した路面点群データのうち走行装置１の矩形内にある路面点群データを除去する（ステップＳ１６）。
【0085】
続いて、障害物検知部１５５は、傾き補正済みの取得した路面点群データを保存する（ステップＳ１７）。障害物検知部１５５は、保存路面点群データに格納するが、保存するのは最新２秒分のみであり、それより古いデータは消去する。
【0086】
以上の手順を施すことで、不整地走行による走行装置１の前後揺れを補正でき、詳細な路面点群データである、補正後の走行路面データを得ることができる。
【0087】
次に、高さ（Ｈｅｉｇｈｔ）マップ生成処理（ステップＳ２）について説明する。図１０は、高さ（Ｈｅｉｇｈｔ）マップ生成処理の手順の一例を示すフローチャートである。図１０に示すように、高さ（Ｈｅｉｇｈｔ）マップ生成処理は、前述で生成した保存路面点群データを用いて以下の順に処理を行う。
【0088】
図１０に示すように、障害物検知部１５５は、まず、検知範囲の路面点群データを抽出する（Pass Through Filter）（ステップＳ２１）。検知範囲は、バンパー前面から見て縦幅ｘ：２．０ｍ、ロボット中心から横幅ｙ：２．８ｍの矩形である。
【0089】
続いて、障害物検知部１５５は、検知範囲内の路面点群データを用いて、ボクセルグリッドデータの生成を行う（ステップＳ２２）。分解能は、２．５ｃｍとする。
【0090】
続いて、障害物検知部１５５は、ノイズ除去処理を実施する（Radius Outlier Filter）（ステップＳ２３）。ここで、図１１はノイズ除去処理の概要について示す図である。具体的には、障害物検知部１５５は、図１１に示すように、例えば、対象点から半径ｄｍ以内に他の点がｎ点未満であればその点（図１１中、バツ印で示す）を除去する。
【0091】
続いて、障害物検知部１５５は、ノイズ除去を行ったボクセルグリッドデータを用いて、高さ（Ｈｅｉｇｈｔ）マップを生成する（ステップＳ２４）。分解能は、平面（ｘｙ）分解能５．０ｃｍ、高さ（ｚ）分解能２．５ｃｍとする。一例として各ボクセルに含まれる点群のうちｚ値が最大の点群データのｚ値を、当該ボクセルの高さの値として保持する。そして、あるｘｙ値の複数のボクセルのうち、最大のｚ値を持つボクセルのｚ値を、当該ｘｙ値で示される位置の高さの値として保持するマップを生成し、走行路面からの最大高さを２次元マップ上に保持した高さマップとする。
【0092】
以上の処理を施すことで、ノイズ除去済みの高さ（Ｈｅｉｇｈｔ）マップを得ることができる。この高さ（Ｈｅｉｇｈｔ）マップの効果としては、点群数削減による処理負荷軽減（処理高速化）と、大き目の高さ（Ｈｅｉｇｈｔ）マップ化による側溝板（グレーチング）踏破可能という効果がある。
【0093】
屋外で走行する場合、側溝は、走行装置１が落下する危険性があるため、路面障害物として検知する必要があるが、場所によっては踏破できるように網目状の側溝板（グレーチング）を設置している現場も多くある。グレーチングがある箇所は、走行装置１では踏破可能であるため、路面障害物として検知する対象ではなく、走行可能な領域として検知してほしい。グレーチングは網目状のため、点群を細かく見てしまうと、平面の部分と溝として検知してしまう部分の両方存在し、溝の誤検知結果のために踏破できないことが危惧される。そこで、２次元マップのセルの分解能をある程度荒くした高さ（Ｈｅｉｇｈｔ）マップに変換することで、局所的に見ないようにし、グレーチングを溝として誤検知させず、走行路面として検知させることができる。本実施形態では走行装置１の履帯の大きさを考慮して、高さ（Ｈｅｉｇｈｔ）マップの分解能は５．０ｃｍとして設定している。
【0094】
ここで、ステップＳ２３におけるノイズ除去処理の詳細に関して説明する。
【0095】
図１２は、ノイズ除去処理結果の例について説明する図である。測距センサ１１３である３Ｄ－ＬｉＤＡＲは、雨天時に雨粒を誤検知してしまうことがあり、降雨量が多くなると、図１２（ａ）に示すように、本来は空中に障害物がないにも関わらず、空中に点群データがばらばらと誤検知されてしまう。そのため、そのまま高さ（Ｈｅｉｇｈｔ）マップの変換処理を実行すると、雨粒を障害物として検知してしまい、障害物がない場所であるにも関わらず、停止や回避を行ってしまうことが危惧される。
【0096】
そこで、本実施形態においては、図１１に示したようなノイズ除去処理を施すことで、雨粒をノイズとして除去し、雨天時でも障害物検知の機能を発揮させることが見込める。図１１に示したようなノイズ除去処理は、対象の点に対して、半径ｄｍ以内にｎ点未満である点は除去するという処理である。図１２（ｂ）に示すデータは、図１１のノイズ除去処理をｄ＝０．０６ｍ、ｎ＝２で施した結果であり、図１２（ａ）に示したデータと比べて、空中の雨粒が除去できている。これにより、雨天時の雨粒による誤検知データをこのノイズ除去により除去することで、雨天時でも対応可能（障害物検知機能を発揮させられる）、という効果がある。
【0097】
次に、高さ障害物マップ生成処理（ステップＳ３）について説明する。図１３は、高さ障害物マップ生成処理の手順の一例を示すフローチャートである。図１３に示すように、高さ障害物マップ生成処理は、高さ（Ｈｅｉｇｈｔ）マップデータを用いて以下の順に処理を行う。
【0098】
図１３に示すように、障害物検知部１５５は、閾値を±６ｃｍに設定し、高さ障害物マップの生成する（ステップＳ３１）。具体的には、障害物検知部１５５は、＋６ｃｍの閾値より高いセルは通常障害物として２次元マップのセルに１００の値を設定、－６ｃｍより低いセルは溝障害物として２次元マップのセルに９９の値を設定、点群データが存在しないセル、いわゆる未知障害物のセルは－１を設定、それ以外は走行可能セルとして０に設定する。この処理を施すことで、通常障害物、溝障害物、未知障害物、走行可能領域セルの４種類に分けた高さ障害物マップを得ることができ、２次元マップのセルの値によって、処理の条件分岐を行うことができる。
【0099】
次に、微分障害物マップ生成処理（ステップＳ４）について説明する。図１４は、微分障害物マップ生成処理の手順の一例を示すフローチャートである。図１４に示すように、微分障害物マップ生成処理は、高さ（Ｈｅｉｇｈｔ）マップデータを用いて以下の順に処理を行う。
【0100】
図１４に示すように、障害物検知部１５５は、まず、高さ（Ｈｅｉｇｈｔ）マップ内の未知障害物を補正する（ステップＳ４１）。高さ（Ｈｅｉｇｈｔ）マップには障害物のオクルージョンなどの影響で、点群データが存在しない、いわゆる未知障害物が存在する。その未知障害物に対して、障害物検知部１５５は、４近傍の２次元マップのセルの平均高さを未知障害物セルに格納して、補正する。障害物検知部１５５は、４近傍がすべて未知障害物の場合は高さ０の値を格納する。
【0101】
続いて、障害物検知部１５５は、補正した高さ（Ｈｅｉｇｈｔ）マップにメディアンフィルタ処理を実行する（ステップＳ４２）。
【0102】
続いて、障害物検知部１５５は、メディアンフィルタ処理を施した補正した高さ（Ｈｅｉｇｈｔ）マップにラプラシアンフィルタ処理を実行して、微分マップを生成する（ステップＳ４３）。
【0103】
続いて、障害物検知部１５５は、ラプラシアンフィルタにより生成した微分マップを用いて、閾値を±０．２０に設定し、微分障害物マップを生成する（ステップＳ４４）。閾値＋０．２０より大きいセル、または閾値－０．２０より小さいセルは、微分障害物として２次元マップのセルに１００を設定、それ以外は走行可能セルとして０に設定する。
【0104】
以上の処理を施すことで、微分障害物、走行可能領域セルの２種類に分けた微分障害物マップを得ることができる。微分マップとは、高さ（Ｈｅｉｇｈｔ）マップの近傍の高さの差を数値として表したマップであり、近傍の高さの差が大きいほどセルに大きい値が入る。例えば、縁石のような近傍セル同士で急に高さの差が大きくなる、つまりエッジが立っているような場所は値が大きくなる。これにより、高さ（Ｈｅｉｇｈｔ）マップのなだらかさを知ることができる。
【0105】
なお、ステップＳ４１の補正処理はメディアンフィルタを実施するためのものであり、ステップＳ４２のメディアンフィルタ処理は測距センサ１１３のノイズによる高さ（Ｈｅｉｇｈｔ）マップの高さのばらつきを平滑化するためのものである。
【0106】
このような微分マップに閾値を設定して、分類したものを微分障害物マップと呼んでおり、閾値以上のセルは縁石のようにエッジが立っている場所を示しており、障害物の候補箇所ということになる。
【0107】
次に、溝障害物ラベリングマップ生成処理（ステップＳ５）について説明する。図１５は、溝障害物ラベリングマップ生成処理の手順の一例を示すフローチャートである。図１５に示すように、溝障害物ラベリングマップ生成処理は、高さ障害物マップデータを用いて以下の順に処理を行う。
【0108】
図１５に示すように、障害物検知部１５５は、まず、高さ障害物マップの拡張処理を実行する（ステップＳ５１）。より詳細には、障害物検知部１５５は、高さ障害物マップにある未知障害物セルの周囲８近傍に溝障害物がある場合、その対象の未知障害物を溝障害物に変換する。
【0109】
続いて、障害物検知部１５５は、拡張した高さ障害物マップを用いて溝障害物ラベリングマップを生成する（ステップＳ５２）。より詳細には、障害物検知部１５５は、溝障害物の８近傍にてラベリングを実行する。
【0110】
以上の処理を施すことで、高さ障害物マップにある溝障害物のラベリング（クラスタリング）を行うことができる。
【0111】
なお、ステップＳ５１の拡張処理は、溝を３Ｄ－ＬｉＤＡＲで見る場合にはオクルージョンの関係で未知障害物が発生することが多く、溝障害物セルの近くに存在する未知障害物は同じ溝障害物であるという仮定のもと、その未知障害物を溝障害物に拡張するために行っている。
【0112】
次に、障害物マップ生成処理（ステップＳ６）について説明する。図１６は、障害物マップ生成処理の手順の一例を示すフローチャートである。図１６に示すように、障害物マップ生成処理は、これまでの処理で得られた、高さ障害物マップ、微分障害物マップ、溝障害物ラベリングマップデータの３つを用いて以下の順に処理を行う。
【0113】
図１６に示すように、障害物検知部１５５は、高さ障害物マップデータ、微分障害物マップデータ、溝障害物ラベリングマップデータの３つを統合し、障害物マップの生成を実行する（ステップＳ６１）。より詳細には、障害物検知部１５５は、微分障害物マップと高さ障害物マップの両マップの同セルを見て、両マップの同セルともに９９（溝障害物）または１００（縁石障害物）の値が入っていた場合にａｎｄを取り、そのセルを障害物セルとする。このとき、溝障害物の場合、障害物セルとなったラベルデータは全て障害物セルに変換する。
【0114】
続いて、障害物検知部１５５は、溝障害物をより見つけやすくするために、高さ（Ｈｅｉｇｈｔ）マップの対象点近傍の高さの差から検知する、障害物マップ追加処理を実行する（ステップＳ６２）。
【0115】
ここで、図１７は障害物マップ追加処理の概要について説明する図、図１８は障害物マップ追加処理適用結果の例について説明する図である。障害物検知部１５５は、対象のセルに対して、走行装置１の横方向（Ｙ方向）の左右３セルの平均高さを算出する。障害物検知部１５５は、算出した平均高さを用いて、走行装置１の進行方向（Ｘ方向）の前後３セルの平均高さを比較する。例えば、障害物検知部１５５は、図１７に示すように、３セルの中で（最大－最小）が６ｃｍより大きければ障害物セルとする。
【0116】
正面の溝障害物の場合は、前述までの処理だけでは図１８（ａ）に示すように、検知の感度が低いため、より高感度に検知できるように、ステップＳ６２に示した障害物マップ追加処理も実行する。障害物マップ追加処理も実行することで、図１８（ｂ）に示すように、正面の溝障害物に対して感度よく検知することが可能となる。
【0117】
以上の処理を施すことで、溝障害物も縁石障害物も含めた、障害物マップを得ることができる。
【0118】
なお、ステップＳ６１の処理は、高低差となだらかさから障害物を判断するということで、高低差に当たる高さ障害物マップとなだらかさに当たる微分障害物マップの両方を統合することを行っている。また、溝障害物に対しては、必ずしも高低差となだらかさのａｎｄで判別できるわけではないため、溝障害物のラベリング（クラスタ）マップデータを用いて、同ラベルマップの中で一つでも微分障害物マップのエッジセルとａｎｄが取れる場合はそのラベルのセルをすべて溝障害物として判断している。
【0119】
このように本実施形態によれば、３Ｄ－ＬｉＤＡＲ方式の測距センサ１１３を斜め下向きにスキャンさせ、測距センサ１１３から得られるデータを平面検出による振動補正を施した上で時間により重ね合わせることで詳細な路面データを取得する。得られた振動補正済みの詳細な路面データを前後左右斜めの高さの関係性が把握可能なように、最大高さをセルに保持した高さ（Ｈｅｉｇｈｔ）マップに変換する。そして、この高さ（Ｈｅｉｇｈｔ）マップの高低差から障害物を判定し、前後左右斜め方向に対応した障害物検知を行うので、３次元のセンサデータを用いて、移動体の路面への進入角の変動の影響を受けにくい路面障害物検知を行うことができる。
【0120】
また、本実施形態によれば、高低差となだらかさの特徴に基づいて走行路面に障害物があるかを判定し、近傍の高さの差に基づいて走行路面に障害物があるかを判定する。これにより、走行路面にある障害物を不足なく検知することができる、という効果がある。
【0121】
以上、第１の実施の形態について説明したが、その各部の具体的な構成、処理の内容等は、第１の実施の形態で説明したものに限るものではない。
【0122】
［実施例］
次いで、第１の実施の形態にかかる走行装置１の実施例について説明する。
次いで、第１の実施の形態にかかる走行装置１の実施例について説明する。
【0123】
ここで、図１９は走行装置１の減速・停止範囲イメージを示す図である。実施例として、走行装置１の走行中の停止について考える。走行装置１の自律または手動走行中に障害物検知よる停止を行う場合、図１９に示すように、減速範囲及び停止範囲を定めておく。走行装置１は、その範囲内に障害物があった場合に、減速または停止の速度を出力することで、障害物検知による停止を実現できる。
・範囲の横幅は、ロボットの横幅（robot_width）＋マージン幅（margin_width）で設定。
・進行方向側（図５などにおける縦方向）は、減速開始距離（decel_dist）と停止開始距離（stop_dist）の両方を設定。
・原則、減速開始距離（decel_dist） ＞ 停止開始距離（stop_dist）。
・範囲の横幅は、ロボットの横幅（robot_width）＋マージン幅（margin_width）で設定。
・進行方向側（図５などにおける縦方向）は、減速開始距離（decel_dist）と停止開始距離（stop_dist）の両方を設定。
・原則、減速開始距離（decel_dist） ＞ 停止開始距離（stop_dist）。
【0124】
ここで、図２０は走行装置１の旋回半径による検知範囲変更を示す図である。このとき、走行装置１は、減速範囲または停止範囲を、図２０（ａ）に示すように、走行時の軌道、具体的には旋回半径ｒによって変更させるようにすることで、指令の進行軌道上の障害物を見ることが可能となる。なお、旋回半径ｒは、速度指令のｖとωから算出可能である（ｒ＝ｖ／ω）。また、超信地旋回は、旋回半径が０であることから、走行装置１の中心からボディの対角までを半径とした円が停止判定範囲となるため、図２０（ｂ）に示すように範囲が極端に狭くなり、狭いところでも旋回しやすくなる。ω＝０の場合は、旋回半径が∞になってしまうため、例外処理を行う。直進の指令であるので、走行装置１は、図１９に示すように、前方の範囲として設定する。
【0125】
ここで、図２１は検知領域とコストマップによる代表点を示す図、図２２は障害物との距離計算方法を示す図である。走行装置１は、前方の範囲内にある障害物に対して障害物との距離を算出し、その距離に応じて、速度そのまま／減速／停止を判断させることで、障害物検知による走行の停止を実現する。なお、障害物との距離算出には以下の方法を用いている。
・コストマップのセル中心を代表点座標とし、図２１に示すように変更した検知範囲内にある代表点の中から障害物との距離ｄを算出する。
・各障害物との距離は旋回半径に依存するため、円弧形状で計算する。
・図２２に示すように検知範囲内の障害物との距離が最も小さいものが障害物までの距離となる。
・コストマップのセル中心を代表点座標とし、図２１に示すように変更した検知範囲内にある代表点の中から障害物との距離ｄを算出する。
・各障害物との距離は旋回半径に依存するため、円弧形状で計算する。
・図２２に示すように検知範囲内の障害物との距離が最も小さいものが障害物までの距離となる。
【0126】
また、走行装置１は、障害物との距離による停止判定は以下のように適用して、実現している。
・算出した障害物までの距離ｄを図１９の減速・停止範囲に適用することで、「そのまま／減速／停止」を判断する。
d＞decel_distであれば速度指令はそのまま
stop_dist＜d＜decel_distであれば減速
d＜stop_distであれば停止（速度０）
・減速判定時は、軌道（旋回半径r）自体はそのままにする必要があるため、直進速度ｖ、角速度ωともに一定割合（decel_rate）をかけて速度を減速させる。（例：decel_rate ＝ ５０％）
・停止判定時は直進、旋回ともに停止させたいので、直進速度ｖ、角速度ωともに０を指示する。
・算出した障害物までの距離ｄを図１９の減速・停止範囲に適用することで、「そのまま／減速／停止」を判断する。
d＞decel_distであれば速度指令はそのまま
stop_dist＜d＜decel_distであれば減速
d＜stop_distであれば停止（速度０）
・減速判定時は、軌道（旋回半径r）自体はそのままにする必要があるため、直進速度ｖ、角速度ωともに一定割合（decel_rate）をかけて速度を減速させる。（例：decel_rate ＝ ５０％）
・停止判定時は直進、旋回ともに停止させたいので、直進速度ｖ、角速度ωともに０を指示する。
【0127】
このような障害物検知による停止に関しても、本発明の障害物検知結果を用いれば、縁石や溝といった路面障害物や空中に出っ張っている障害物、空中に浮いている障害物などに対しても軌道を考慮して安全に停止させることができる。
【0128】
（第２の実施の形態）
次に、第２の実施の形態について説明する。
次に、第２の実施の形態について説明する。
【0129】
第２の実施の形態は、第１の実施の形態で説明した測距センサ１１３に代えて、測距センサ１１３とは視野角が異なる測距センサ２００を用いるものとなっている。以下、第２の実施の形態の説明では、第１の実施の形態と同一部分の説明については省略し、第１の実施の形態と異なる箇所について説明する。
【0130】
【0131】
斜め方向検知用の測距センサ２００は、３Ｄ－ＬｉＤＡＲ方式の三次元測距センサとして、３６０度全周囲の視野角を有する非反復走査方式のものを使用する。斜め方向検知用の測距センサ２００は、水平な走行路面に対してレーザ光の照射角度が所定の俯角の角度になるように取り付けられる。走行装置１は、本体１０の進行方向を正面として、高さ０．８ｍの位置であって、俯角５２度および仰角７度となるように、斜め方向検知用の測距センサ２００を備える。この設置位置は、走行装置１の前面にあるバンパー１６の手前までの走行路面を検知できるように考慮したものである。なお、斜め方向検知用の測距センサ２００の計測範囲の関係上、足元と高さ方向に死角が存在する。バンパー１６からの死角は、０．３２５ｍである。
【0132】
ここで、図２５は実際の計測データ例を示す図である。図２５に示すように、本実施形態の測距センサ２００は、３６０度全周囲の全面的な点群データを取得することができ、障害物の検知適用範囲（図２４参照）の拡大が図れる。第１の実施の形態では、センサ検出面中心から見て円錐７０度ほどの範囲しか計測できなかった。ただし、図２５に示すように、本実施形態の走行装置１は、測距センサ２００の取り付けの制限等により一部死角が存在する。
【0133】
本実施形態の測距センサ２００を用いることで、走行装置１の全周囲の全面的な点群データを取得することができるので、第１の実施の形態の測距センサ１１３では見ることのできなかった側方や後方の障害物を検知することができる。そのため、本実施形態の走行装置１は、側方や後方の障害物に対しても軌道を考慮して安全に回避や停止を行うことができる。言い換えれば、本実施形態の走行装置１は、側方や後方にある障害物を検知できるため、側方や後方にある障害物を考慮した回避動作が可能となり、安全性が増す。
【0134】
走行装置１の全周囲の全面的な点群データを取得するという意味では、第１の実施の形態の測距センサ１１３を複数個搭載することで、３６０度全周囲の障害物検知は可能となるが、複数のセンサを使用することでコストがかかってしまう。また、複数のセンサを搭載する分、クローラロボット自体の重量も増えてしまい、クローラロボットのハード面でも負荷が増大してしまう。
【0135】
そこで、本実施形態の測距センサ２００を使用することで、１個のセンサで障害物の検知範囲を拡大できると同時に、コストの低減、ハード面の負荷の低減を図ることができる。
【0136】
以上のことより、本実施形態の走行装置１は、本実施形態の測距センサ２００を使用することで、「安全性」と「コスト」のバランスを図ることができる。
【0137】
なお、走行路面を検知するための３Ｄ－ＬｉＤＡＲ方式の三次元測距センサを第１の実施の形態の測距センサ１１３から３６０度全周囲の全面的な点群データを取得する本実施形態の測距センサ２００に変更した場合でも、障害物検知処理の方法は変わらない。本実施形態の測距センサ２００から得られる三次元の走行路面データに基づいて生成された走行路面からの高さ情報を保持した高さマップに変換することによって、高さマップの高低差から障害物を検知する。
【0138】
しかしながら、本実施形態の測距センサ２００によれば、検知範囲が３６０度全周囲に拡大したことにより取得する情報量が増え、障害物検知処理の処理負荷は増えることになる。そのため、三次元データを二次元の高さマップに変換してから障害物を検知するという処理が、処理負荷軽減の面で「高さマップ化」をより活かすことができる。言うまでもないが、三次元データのまま障害物検知の処理を行った場合、処理負荷はかなり大きくなるため、走行装置１を自律走行させながらリアルタイムに障害物を検知することは難しい。そのため、三次元データを二次元の高さマップに変換してから処理することで、処理負荷を軽減することができ実装が可能となるが、第１の実施の形態の測距センサ１１３よりも情報量が増大する本実施形態の測距センサ２００の方が、処理負荷軽減の面で高さマップ化の効果をより得ることができる。
【0139】
このように本実施形態によれば、３６０度全周囲の全面的な点群データを取得する本実施形態の測距センサ２００を使用することで、「安全性」も増しつつ、高さマップ化による処理速度軽減の効果をより得ることができるので、高さマップ化の処理をより活かすことが可能となる。
【0140】
上記で説明した各実施の形態の各機能は、一又は複数の処理回路によって実現することが可能である。ここで、本明細書における「処理回路」とは、電子回路により実装されるプロセッサのようにソフトウェアによって各機能を実行するようプログラミングされたプロセッサや、上記で説明した各機能を実行するよう設計されたＡＳＩＣ（Application Specific Integrated Circuit）、ＤＳＰ（Digital Signal Processor）、ＦＰＧＡ（Field ProGrammable Gate Array）や従来の回路モジュール等のデバイスを含むものとする。
【符号の説明】
【0141】
１ 移動体、障害物検出装置
１１ａ，１１ｂ 駆動部材
１１３ 三次元測距センサ
１５５ 障害物検知部
１１ａ，１１ｂ 駆動部材
１１３ 三次元測距センサ
１５５ 障害物検知部
【先行技術文献】
【特許文献】
【0142】
【特許文献1】特開２０００－０７５０３２号公報
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】
【図24】
【図25】
