JPH0764631A

JPH0764631A - 移動探査機の経路探索方法

Info

Publication number: JPH0764631A
Application number: JP5213053A
Authority: JP
Inventors: Hiroaki Saito; 藤浩明斉
Original assignee: Nissan Motor Co Ltd
Current assignee: Nissan Motor Co Ltd
Priority date: 1993-08-27
Filing date: 1993-08-27
Publication date: 1995-03-10

Abstract

(57)【要約】【目的】無人移動探査機が障害物等を回避して目的地
まで走行する際に、経路探索の処理時間を短縮すると共
に、より起伏が少なく平坦な地形を選択して、安定走行
できる経路探索方法を提供する。【構成】移動探査機に搭載の地形センサによるセンシ
ングで所定範囲の経路探索地図を作成し、目的地に到達
できる複数の経路を探索し、それぞれの経路について地
形の高さ，斜度，斜面方向の要素に基いて地形平坦度を
算出し、最も平坦な経路を走行上の最適経路として局所
経路計画手段により選択する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、無人移動探査機が現在
位置から目的地までに向かう間において障害物があった
場合、その障害物を避けて通る最適経路を探索するのに
利用される無人探査機の経路探索方法に関するものであ
る。

【０００２】

【従来の技術】地球のオフロード地形上や、月および火
星表面上を無人移動探査機が自律走行する場合に、走行
の障害となる岩，穴，傾斜等をセンシングし、認識して
移動探査機が安全に走行できるようにする技術が開発さ
れている。

【０００３】その移動探査機が出発地点から目的地まで
の走行経路を見つけ出すために、今までは、複数の探査
機に搭載された３次元センサにより障害物を含む地形を
センシングして地形データを求め、その結果に基づい
て、走行経路をパスグラフ方法を用いて作成していた。

【０００４】図７（ａ），（ｂ）は、従来のパスグラフ
法を用いた経路探索法の説明図であって、図７（ａ）に
おいて、無人移動探査機が自己位置（スタート位置）Ｓ
から目的地（ゴール）Ｇに向かう際に、障害物Ｘ１があ
るので、回避点ａまたはｂを通って目的地に向かう。こ
の時、経路上の距離を考えると、経路Ｓ−ａ−Ｇは２＋
４＝６であり、経路Ｓ−ｂ−Ｇは１＋６＝７であるか
ら、経路Ｓ−ａ−Ｇの方が近い。

【０００５】しかし、障害物Ｘ２があるため、図７
（ｂ）に示すように、回避点ａから障害物Ｘ２を避ける
経路を探し、回避点ｃ，ｄを通る２経路を考えることと
なり、経路ａ−Ｃ−Ｇは３＋２＝５であり、経路ａ−ｄ
−Ｇは４＋２＝６であるから、経路ａ−Ｃ−Ｇの方が近
い。

【０００６】次に、回避点ｂについても同様な手順で行
い、より短い距離となる経路を検索することとなる。

【０００７】そして、いくつかの障害物を考慮して、最
短距離となる経路を決定する。

【０００８】

【発明が解決しようとする課題】しかしながら、上述の
パスグラフ法では、経路を単に最短距離で決定してしま
うため、例えば、地形の傾斜が急なところがあって、迂
回した方が平坦なために高速かつ安定して走行できる経
路となるにもかかわらず、このような経路を選択するこ
とができないという問題があった。

【０００９】

【発明の目的】本発明は、無人移動探査機が障害物等を
回避して目的地に行く際に、経路探索の処理時間を短縮
することが可能であると共に、より起伏が少なく平坦な
地形を選択して安定走行することが可能である無人探査
機の経路探索方法を提供することを目的とするものであ
る。

【００１０】

【課題を解決するための手段】本発明に係わる移動探査
機の経路探索方法では、移動探査機に搭載した地形セン
サにより障害物等を含む地形データを取得し、これによ
ってとらえられた障害物等を回避して目的地までの経路
を探索するに際し、地形センサにより所定範囲のセンシ
ングを行って局所地図を作成する段階と、前記局所地図
を複数のグリッドに分割する段階と、前記グリッドを走
行不能領域と走行可能領域とに分けて経路検索地図を作
成する段階と、前記走行可能領域内を結んで経路検索地
図に設定される自己位置から目的地に至るまでの複数の
局所経路について各局所経路毎に地形の高さの平均値，
斜度の分散値および斜面方向平均値から決定される地形
平坦度を算出する段階と、前記地形平坦度が最小である
経路を走行最適経路として選択する段階を経る構成とし
たことを特徴としている。

【００１１】

【発明の作用】本発明に係わる移動探査機の経過探索方
法では上記した構成としており、目的地へ到達すること
ができる局所経路のそれぞれに地形の状況を表す地形平
坦度を算出して地形平坦度が最小である経路を選択する
こととしたので、最短距離が選択されない場合があると
しても、平坦な経路を選択することによって、高速かつ
安定した走行が行われることとなる。

【００１２】

【実施例】図２の（ａ）は、移動探査機が外部領域セン
シングを行っている状態の概略説明図であり、図２の
（ｂ）は移動探査機に設置した経路探索装置の基本構成
を示すブロック図であり、図３は広域地図と局所地図と
の関係を示す図であって、図２に示す無人移動探査機１
は、地球のオフロード地形上や、月および火星表面上等
を走行できる構造を有するものである。

【００１３】図２の（ａ）において、１は無人移動探査
機、２は地球、３は無人移動探査機１に搭載したアンテ
ナ、４は無人移動探査機１に搭載した地形センサ、５は
地表である。

【００１４】そして、図２の（ｂ）に示すように、無人
移動探査機１は走行方向の前方における所定範囲の外部
領域センシングを行って外部地形データを取得する地形
センサ４と、前記外部地形データをもとにして外部領域
センシング内の局所地図データを取得する外部環境認識
手段４１と前記外部環境認識手段４１で取得された局所
地図内で局所経路を計画する局所経路計画手段４２と、
前記局所経路に従って無人移動探査機１を移動させる移
動手段４３と、無人移動探査機１の移動および姿勢変化
によって位置および高さデータから変化したことを検知
して外部環境認識手段４１に補正データを送る慣性セン
サ４４を備え、局所経路計画手段４２には、地球上にあ
って広域地図を用いて作成された広域地図が通信局より
伝送される。

【００１５】そして、移動探査機１に搭載されたアンテ
ナ３からは、慣性センサ４４により移動探査機１の姿勢
位置情報等を地球２にある通信局や人工衛星等に送り、
通信局では移動探査機１の現在位置を基準とした図３の
（ａ）に示す広域地図６を作成し、それに基いた広域経
路７を作成してこれを移動探査機１の局所経路計画手段
４２に伝送する。

【００１６】そして、移動探査機１は広域経路７に沿っ
て目的地１０に向かおうとするが、その出発位置１３
で、レーザレンジファインダ等の３次元地形センサ４で
移動探査機１の進行方向の比較的広範囲の地形をセンシ
ングし、図３の（ｂ）にも示すように、センシング領域
内の詳細な局所地図８を外部環境認識手段４１で作成す
る。

【００１７】移動探査機１は、広域経路７を基準としな
がら、局所地図８内で局所経路計画手段４２により局所
経路９を計画し、サブ目的地１１へと走行不能領域（障
害物等）１４ａ，１４ｂ，１４ｃを回避しながら移動手
段４３により走行しようとする。

【００１８】ところで、移動探査機１にとって地形セン
シングと局所地図８の作成（地形環境認識）及び局所地
図８に基づく局所経路計画が重要である。

【００１９】局所経路計画手段４２における局所経路計
画では、以下のような移動探査機１の踏破能力を考慮す
る必要がある。

【００２０】（ａ）障害物乗り越え能力（ｂ）登坂能力（ｃ）軟弱地踏破能力そして、これらの踏破能力を越える領域を識別するた
め、次のような地形の特徴量をセンシング認識すること
が必要になる。

【００２１】（ａ）地形の高さ（ｂ）地形の斜度（ｃ）地形の斜面方向（ｄ）地質そして、部分自律用アルゴリズムにおいては、障害物乗
り越え能力及び登坂能力を考慮し、地形の高さ，斜度，
斜面方向により、走行不能領域の推定判断を行う。

【００２２】外部環境認識手段４１での局所地図８の作
成（地形環境認識）図３の（ｂ）に示す局所地図８の作成手法は、３次元地
形センサ（例えば、レーザレンジファインダ等）４から
の信号を処理し、得られた地形の高さ，斜度，斜面方向
の情報を用いて、移動探査機１の踏破能力（障害物乗り
越え能力及び登坂能力）を越える地形を走行不能領域と
して認識するものである。

【００２３】次に、地形の高さ，斜度及び斜面方向のそ
れぞれの求め方を示す。

【００２４】（１）地形の高さの求め方図４は３次元地形センサ４による距離データの概念説明
図であって、図４に示すように、３次元地形センサ４に
より求められる距離から、センシング領域内の各測定点
の慣性座標系における位置及び地表の高さを求める。移
動探査機１の移動および姿勢変化により，位置，高さデ
ータは変化するが、その補正は、移動探査機１に搭載し
た慣性センサ４４のデータを用いる。

【００２５】図４（ａ）に示すように、センシングによ
り得られる測定点Ｐｉｊ（ｉ＝１，・・・ｎ，ｊ＝１，
・・・，ｍ，Ｐｉｊは、センシング領域内におけるｉ，
ｊ番目の測定点）の位置ｘ，ｙと高さｚは、Ｐｉｊとセ
ンサ４間の距離Ｒ，センサ４からＰｉｊを見た横方向の
角度θ，奥行き方向の角度φより得られ、数式１〜３の
ように表現できる。なお、Ｐｉｊの範囲は、θ，φを３
次元センサ４の視野の範囲で変化させることにより決定
される。

【００２６】

【数式１】ｘ＝Ｒｓｉｎθ

【００２７】

【数式２】ｙ＝Ｒｃｏｓθｃｏｓφ

【００２８】

【数式３】ｚ＝ｈｓ−Ｒｃｏｓθｓｉｎφ 但し、ｈｓは地形センサ４の地上高である。

【００２９】（２）地形の斜度及び斜面方向の求め方センシング領域内の斜度及び斜面方向は、求めようとす
る地点に平面を当てはめた時の法線ベクトルを求めるこ
とに等しい。ここでは、最小二乗法を用いて、あてはま
る平面を算出した。

【００３０】図５（ａ）に示すように斜度及び斜面方向
を求める地点の座標をＱ_０（ｘ_０，ｙ_０，ｚ_０）、その
近傍地点の座標をＱ_１（ｘ_１、ｙ_１，ｚ_１）・・・Ｑ_ｎ
（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）、あてはめる平面Ｓの方程式を数
式４で表す。

【００３１】

【数式４】ｚ＝ａｘ＋ｂｙ＋ｃ平面Ｓと各地点間の誤差を（ｅ_０，・・・，ｅ_ｎ）とす
ると、これらの関係は数式５で表される。

【００３２】

【数式５】Ｚ＝Ｘ・Ｂ＋Ｅ但し、Ｚ，Ｘ，Ｂ，Ｅは数式６〜９で表すものである。

【００３３】

【数式６】Ｚ＝［Ｚ_０Ｚ_１・・・Ｚ_ｎ］^ｔ

【００３４】

【数式７】

【００３５】

【数式８】Ｂ＝［ａｂｃ］^ｔ

【００３６】

【数式９】Ｅ＝［ｅ_０ｅ_１・・・ｅ_ｎ］^ｔ数式５において、二乗誤差Ｅ^ｔＥを最小にするよう、最
小二乗法でＢを求めると、数式１０が得られる。

【００３７】

【数式１０】Ｂ＝（Ｘ^ｔ・Ｘ）^−１・Ｘ^ｔ・Ｚ数式１０で求められるＢが数式１１で表されるときの平
面Ｓの法線ベクトルｎは数式１２で表される。

【００３８】

【数式１１】Ｂ＝［ａ，ｂ，ｃ］^ｔ

【００３９】

【数式１２】ｎ＝［ａ，ｂ，−１］^ｔ図５（ｂ）は平面Ｓの法線ベクトルｎの概念説明図であ
り、求める斜度α及び斜面方向βは数式１３，１４で表
される。

【００４０】

【数式１３】

【００４１】

【数式１４】

【００４２】（３）走行不能領域の識別前記（１），（２）の処理で得られた高さ，斜度，斜面
方向のデータを用い、移動探査機１の登坂能力及び乗り
越え能力から決定される走行不能領域１４ａ，１４ｂ，
１４ｃの識別手法について述べる。

【００４３】図６は走行不能領域識別のフローチャート
であって、図３および図６に示すように、まず、斜度デ
ータから移動探査機１の登坂能力以上の斜度を持つ領域
を、走行不能領域Ａｉ（ｉ＝１，・・・，Ｎ）として抽
出する。その他は、走行可能領域１５とする。

【００４４】次に、各走行不能領域Ａｉにおいて、最大
高さＨｍａｘと最小高さＨｍｉｎを高さデータより求
め、これらの値が数式１５の条件を満たす場合は走行可
能領域１５に変更する。これは、小石等の斜度は大きい
が乗り越えられる高さの地形は走行可能とするためであ
る。

【００４５】

【数式１５】Ｈｍａｘ−Ｈｍｉｎ＜Ｔｚ但し、Ｔｚは移動探査機障害物乗り越え能力のしきい値
である。数式１５の条件判定を全てのＡｉについて行
う。

【００４６】以上の処理により識別された、走行可能領
域１５と走行不能領域１４ａ，１４ｂ，１４ｃを記録し
たセンシング領域内の局所地図８（図３の（ｂ）参照）
が経路計画に使用される。

【００４７】経路計画（１）経路探索地図作成図３において、外部環境認識手段４１での地形環境認識
で作成された局所地図８に対して移動探査機１の現在位
置（出発点）１３からサブ目的地１１までの局所経路９
を局所経路計画手段４２で見い出すための経路計画につ
いて述べる。

【００４８】この手法には、（イ）迷路脱出法，（ロ）
ポテンシャル法，（ハ）グラフ探索法等があるが、ここ
では、確実に経路が作成できるグラフ探索法を用いる。

【００４９】図１の（ａ）は、経路探索手順の説明図で
あって、図１の（ａ）に示す各グリッドは、局所地図８
を分割しており、その一辺の長さは、移動探査機１の最
大長（対角線）に相当するものとなっている。

【００５０】各グリッドを走行不能領域（障害物等）１
４と走行可能領域１５とに二値化し、経路探索地図１６
とする。この方法によれば、画素数の少ない地図で経路
探索を行うことができるため、高速で処理することがで
きる。

【００５１】（２）経路探索経路探索手順を図１の（ａ）に基づいて説明する。

【００５２】（ａ）経路探索地図１６内に、経路探索の
目標点であるサブゴール１７を定める。このサブゴール
１７は、広域経路７上の地点である。

【００５３】（ｂ）自己位置１８とサブゴール１７とを
結ぶ直線経路３０を算出する。

【００５４】（ｃ）直線経路３０が走行不能領域１４に
侵入しなければ、直線経路３０を局所経路集合に登録
し、経路探索を終了する。

【００５５】（ｄ）走行不能領域１４へ侵入する場合、
侵入直前の走行可能領域（侵入点）１９，走行不能領域
１４から脱出する領域（脱出点）２０を求める。次に、
侵入点１９から走行不能領域１４に沿い、右回り及び左
回りに走行可能領域１５を脱出点２０まで辿る。脱出点
２０に辿ることができた場合、自己位置１８とサブゴー
ル１７とを結ぶ直線とのユークリッド距離が最大である
点を回避点２１，２２とし、局所経路候補として登録す
る。右回り，左回り共に脱出点まで辿ることができなか
った場合は、回避点を生成しない。

【００５６】（ｅ）回避点２１，２２と自己位置１８及
び、回避点２１，２２とサブゴール１７を結ぶ直線経路
３０を算出し、（ｃ）の処理に戻る。

【００５７】なお、サブゴール１７に到達する経路が作
成されなかった場合は、サブゴール１７が走行不能領域
１４に囲まれていると見なす。

【００５８】本方法では、サブゴール１７に向かう直線
経路３０上にある一つの走行不能領域１４において１回
の探索について、最大二箇所の回避点２１，２２を生成
するのみであり、回避点の総数が少なくなるため、経路
探索処理が高速で終了する。

【００５９】上述の説明では、障害物５が１個の場合の
説明であるが、障害物５が複数個ある場合にも、同様な
手法にて、複数の回避点と、複数の局所経路を得ること
ができる。

【００６０】（３）経路決定移動探査機１の走行経路は、走行距離が短い経路より
も、より起伏が少なく平坦な地形である方が望ましい。
上述の走行経路は、経路探索で作成された局所経路候補
の中からより最適な経路を以下の方法で選択した。

【００６１】（ａ）地形の状況を表す地形平坦度の算出経路探索は移動探索機１のサイズを一辺とした経路探索
地図１６で作成されるため、その各グリッド毎に、対応
する局所地図８から、地形の特徴を以下の地形要素とし
て算出する。

【００６２】・高さ地形要素（高さの平均値）・斜度地形要素（斜度の分散値）・斜面方向地形要素（斜面方向平均値）各地形要素は移動探査機１の踏破能力で規格化する。

【００６３】（ｂ）走行最適経路の選択局所経路候補の各経路毎に、上記各要素に基づいて、地
形平坦度を算出する。移動探査機１の位置からサブゴー
ル１７に至るまでの局所経路９の候補の中から、地形平
坦度が最小である経路を走行最適経路として選択する。

【００６４】本処理を局所経路計画手段４２で行うこと
により、走行可能経路１５の中から、移動探査機１がよ
り安定して走行できる経路を選択することができる。

【００６５】移動探索機１は、障害物等の走行不能領域
１４を回避しながらサブゴール１７に至り、さらに、い
くつかのサブゴール１７を経て走行最適経路を走行し、
図３の（ｂ）に示す局所経路９のサブ目的地１１に到達
し、さらに、移動探査機１に搭載した３次元地形センサ
４により広範囲をセンシングして、次の局所地図８を外
部環境認識手段４１で作成し、順次前述した方法を繰返
して図３に示す広域経路７の目的地１０に到達する。

【００６６】

【発明の効果】本発明に係わる移動探査機の経路探索方
法によれば、目的地へ到達することができる局所経路の
それぞれに地形の状況を表す地形平坦度を算出して、地
形平坦度が最少である経路を選択するようにしたので、
平坦な経路を選択し、高速かつ安定した走行を行うこと
ができ、最短距離でない場合であっても短時間のうちに
目的地に到達することが可能であるという著しく優れた
効果がもたらされ、さらに局所地図を分離するグリッド
幅を移動探査機の最大長とすることによって、画素数の
少ない地図で経路探索を行うことができるため、高速で
処理することができるという効果ももたらせる。

【図面の簡単な説明】

【図１】（ａ）は本発明に係わる経路探索の手順を示す
説明図であり、（ｂ）は本発明に係わる走行最適経路探
索方法を示すブロック図である。

【図２】移動探査機がセンシングを行っている状態を示
す概略説明図および移動探査機に設置した経路決定装置
の基本構成を示すブロック図である。

【図３】広域地図と局所地図の関係を示す説明図（図の
（ａ））および局所地図を拡大して示す説明図（図の
（ｂ））である。

【図４】３次元センサによる距離データの概念を示す障
害物の座標の説明図（図４の（ａ）およびセンシング領
域の説明図（図４の（ｂ））である。

【図５】（ａ）平面Ｓの概念説明図である。（ｂ）平面Ｓの法線ベクトルｎの概念説明図である。

【図６】走行不能領域識別のフローチャートである。

【図７】（ａ），（ｂ）は従来のパスグラフ法を用いた
経路探索法の説明図である。

【符号の説明】

１無人移動探査機２通信局（地球）４地形センサ（３次元地形センサ）５障害物６広域地図７広域経路８局所地図９局所経路１４走行不能領域１５走行可能領域４１外部環境認識手段４２局所経路計画手段４３移動手段４４慣性センサ

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０５Ｄ 1/00 Ｂ 9323−3ＨＧ０６Ｔ 1/00

Claims

【特許請求の範囲】

【請求項１】移動探査機に搭載した地形センサにより
障害物等を含む地形データを取得し、これによってとら
えられた障害物等を回避して目的地までの経路を探索す
るに際し、地形センサにより所定範囲のセンシングを行
って局所地図を作成する段階と、前記局所地図を複数の
グリッドに分割する段階と、前記グリッドを走行不能領
域と走行可能領域とに分けて経路検索地図を作成する段
階と、前記走行可能領域内を結んで経路検索地図に設定
される自己位置から目的地に至るまでの複数の局所経路
について各局所経路毎に地形の高さの平均値，斜度の分
散値および斜面方向平均値から決定される地形平坦度を
算出する段階と、前記地形平坦度が最小である経路を走
行最適経路として選択する段階を経ることを特徴とする
移動探査機の経路探索方法。
【請求項２】グリッド幅は移動探査機の最大長とした
請求項１記載の移動探査機の経路探索方法。