特開 2023-143259 検出装置、検出方法およびプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023143259

(43)【公開日】2023-10-06

(54)【発明の名称】検出装置、検出方法およびプログラム

(51)【国際特許分類】

   G01S 13/42 20060101AFI20230928BHJP

【ＦＩ】

G01S13/42

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022050541

(22)【出願日】2022-03-25

(71)【出願人】

【識別番号】000005290

【氏名又は名称】古河電気工業株式会社

(71)【出願人】

【識別番号】391045897

【氏名又は名称】古河ＡＳ株式会社

(74)【代理人】

【識別番号】100143959

【弁理士】

【氏名又は名称】住吉 秀一

(72)【発明者】

【氏名】眞子 凌

(72)【発明者】

【氏名】山村 隆介

(72)【発明者】

【氏名】大谷 栄介

【テーマコード（参考）】

5J070

【Ｆターム（参考）】

5J070AB17

5J070AC01

5J070AC02

5J070AC06

5J070AC12

5J070AC13

5J070AD02

5J070AE07

5J070AF02

5J070AH12

5J070AH31

5J070AH45

5J070AK13

(57)【要約】

【課題】検知範囲の広いレーダなどのセンサ装置を用いて、凹凸の多い走路においても走路障害物を精度よく検出することが可能な検出装置、検出方法およびプログラムを提供する。
【解決手段】上記課題は、移動体（２）の周囲の走路（３、４）の検出点を周期的に検出するセンサ装置（１０）と、走路（３）を複数の距離領域に分割した距離領域ごとに、検出点（３２）に基づいて走路障害物（５２）が存在するか否かを判定する走路障害物判定部（２５、２５’）と、を備える検出装置（１、６）等により解決することができる。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体の周囲の走路の検出点を周期的に検出するセンサ装置と、
前記走路を複数の距離領域に分割した距離領域ごとに、前記検出点に基づいて走路障害物が存在するか否かを判定する走路障害物判定部と、
を備える、検出装置。

【請求項2】

前記走路障害物判定部は、点群密度判定部を備え、該点群密度判定部は、
前記距離領域ごとに、前記検出点の点群密度と、所定の点群密度閾値とを比較し、
前記点群密度閾値より小さな点群密度を有する距離領域が存在し、かつ、該距離領域と前記移動体との距離が、所定の危険度閾値より小さいときに、前記走路障害物が存在すると判定する、
請求項１に記載の検出装置。

【請求項3】

前記点群密度閾値は、所定区間で検出された検出点の点群密度の平均値に基づいて設定される、請求項２に記載の検出装置。

【請求項4】

前記点群密度閾値は、所定期間に検出された検出点の点群数の平均値に基づいて設定される、請求項２に記載の検出装置。

【請求項5】

前記走路障害物判定部は、前記センサ装置の検出情報に基づいて、前記走路の環境変化を検知する走路データモニタリング部を備え、
前記点群密度判定部は、前記環境変化が検知されたときに、前記点群密度閾値を更新する、
請求項２から４までのいずれか１項に記載の検出装置。

【請求項6】

前記走路障害物判定部は、高さ判定部を備え、該高さ判定部は、
前記距離領域ごとに、前記検出点の高さの平均値と、所定の高さ閾値とを比較し、
前記高さ閾値より小さな前記平均値を有する距離領域が存在するときに、前記走路障害物が存在すると判定する、
請求項１から５までのいずれか１項に記載の検出装置。

【請求項7】

前記走路障害物判定部は、前記走路障害物が存在するときに警告を発する、請求項１から６までのいずれか１項に記載の検出装置。

【請求項8】

センサ装置により、移動体の周囲の走路の検出点を周期的に検出するステップと、
前記走路を複数の距離領域に分割した距離領域ごとに、前記検出点に基づいて走路障害物が存在するか否かを判定するステップと、
を含む、検出方法。

【請求項9】

コンピュータに、
センサ装置により、移動体の周囲の走路の検出点を周期的に検出する機能と、
前記走路を複数の距離領域に分割した距離領域ごとに、前記検出点に基づいて走路障害物が存在するか否かを判定する機能と、
を実現させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、検出装置、検出方法およびプログラムに関し、特に、移動体の周囲の走路障害物を検出可能な検出装置、検出方法およびプログラムに関する。

【背景技術】

【0002】

移動体の走行路の凹型障害物などの走路障害物の検出方法として、移動体に取り付けたレーダやソナーなどのセンサ装置により、移動体の周囲の走路を監視する方法がある。例えば特許文献１には、路面方向に向けられたソナーによって路面までの距離を検出し、その距離の変化を捉えることにより、路面の凹みを検出する自律移動装置に関する技術が記載されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１３－２３５４０９号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

農業用車両や建設用車両のように、あぜ道、圃場、土手道などの比較的未整備の走路上を走行する移動体では、走行路の段差や溝などの走路障害物の検出が不可欠である。しかしながら、特許文献１の記載された技術は、指向性の鋭いソナーを用いて、走路とソナーとの距離がほぼ一定な整備された走路を走行することを前提としているため、農道や建設現場のように凹凸の多い走路では安定した検知結果を得ることが困難である。

【0005】

本願発明は、センサと走路までの距離変動が検出結果に与える影響が小さく、検知範囲の広いレーダなどのセンサ装置を用いて、凹凸の多い走路においても走路障害物を精度よく検出することが可能な検出装置、検出方法およびプログラムを提供することを目的とする。

【課題を解決するための手段】

【0006】

上記課題は、移動体の周囲の走路の検出点を周期的に検出するセンサ装置と、走路を複数の距離領域に分割した距離領域ごとに、検出点に基づいて走路障害物が存在するか否かを判定する走路障害物判定部と、を備える検出装置により解決することができる。

【0007】

ここで、「移動体」は、１台の車両である必要はなく、トラクターとトレーラの組み合わせなどの移動する物体の集合体を含む。また、「センサ装置」とは、レーダ装置や、ＬｉＤＡＲ、カメラなどの移動体に取り付けて走路の位置を検出できる装置である。「走路障害物」とは、下り段差や溝、崖などのように、急峻に高さが低下する高低差を含む地形（凹型障害物）を含む走路上および走路の周辺の障害物を意味する。

【0008】

走路障害物判定部は、点群密度判定部を備え、該点群密度判定部は、距離領域ごとに、検出点の点群密度と、所定の点群密度閾値とを比較し、点群密度閾値より小さな点群密度を有する距離領域が存在し、かつ、該距離領域と移動体との距離が、所定の危険度閾値より小さいときに、走路障害物が存在すると判定することが望ましい。

【0009】

ここで、「点群密度」は、距離に対する検出点の数であり、単位距離当たりの検出点数（線密度）のみならず、走路を等間隔で複数の距離領域に分割した各距離領域に存在する検出点数で示される指標を含む。

【0010】

点群密度閾値は、所定区間で検出された検出点の点群密度の平均値に基づいて設定してもよい。また、点群密度閾値は、所定期間に検出された検出点の点群数の平均値に基づいて設定してもよい。

【0011】

また、走路障害物判定部は、センサ装置の検出情報に基づいて、走路の環境変化を検知する走路データモニタリング部を備え、点群密度判定部は、環境変化が検知されたときに、前記点群密度閾値を更新することが望ましい。

【0012】

さらに、走路障害物判定部は、高さ判定部を備え、該高さ判定部は、距離領域ごとに、検出点の高さの平均値と、所定の高さ閾値とを比較し、高さ閾値より小さな平均値を有する距離領域が存在するときに、走路障害物が存在すると判定することが望ましい。

【0013】

さらに、走路障害物判定部は、走路障害物が存在するときに警告を発することが望ましい。

【0014】

また、上記課題は、センサ装置により、移動体の周囲の走路の検出点を周期的に検出するステップと、走路を複数の距離領域に分割した距離領域ごとに、検出点に基づいて走路障害物が存在するか否かを判定するステップとを含む、検出方法によっても解決することができる。

【0015】

さらに、上記課題は、コンピュータに、センサ装置により、移動体の周囲の走路の検出点を周期的に検出する機能と、走路を複数の距離領域に分割した距離領域ごとに、検出点に基づいて走路障害物が存在するか否かを判定する機能と、を実現させるためのプログラムによっても解決することができる。

【発明の効果】

【0016】

本発明によれば、検知範囲の広いレーダなどのセンサ装置を用いて、凹凸の多い走路においても走路障害物を精度よく検出することが可能な検出装置、検出方法およびプログラムを提供することができる。

【図面の簡単な説明】

【0017】

【図1】本発明に係る検出装置の概略構成図である。

【図2】レーダ装置による検出の説明図である。

【図3】本発明に係る検出方法およびプログラムのフローチャートである。

【図4】レーダ装置による検出のフローチャートである。

【図5】ＩＭＵ情報に基づく高さ補正の説明図である。

【図6】走路障害物判定１のフローチャートである。

【図7】点群密度判定のフローチャートである。

【図8】点群密度判定の説明図である。

【図9】危険度判定のフローチャートである。

【図10】危険度判定の説明図である。

【図11】点群密度判定の変形例のフローチャートである。

【図12】走路障害物判定２のフローチャートである。

【図13】走路障害物判定２の説明図である。

【図14】本発明に係る検出装置の変形例の概略構成図である。

【図15】検出装置の変形例における点群密度判定のフローチャートである。

【図16】環境変化判定のフローチャートである。

【図17】環境変化判定の説明図である。

【発明を実施するための形態】

【0018】

本発明の実施態様である検出装置１の概略構成図を図１に示す。検出装置１は、移動体２の周囲の走路の検出点を周期的に検出するセンサ装置の一種であるレーダ装置１０と、レーダ装置１０に接続され、検出点に基づいて、凹型障害物などの走路障害物が存在するか否かを判定する走路障害物判定部２５とを備える。

【0019】

検出装置１は、走路障害物の検出結果を受けて走路障害物回避のための自動操舵などの処理を行うための上位装置２９に接続されている。図１には、１台のレーダ装置１０のみを表示しているが、検出装置１は複数台のレーダ装置を備えてもよい。また、装置間の接続は、有線で構成してもよいし、無線ネットワークを介して接続してもよい。

【0020】

レーダ装置１０は、所定の周波数帯の信号を、周期的に（例えば５０ｍｓごとに）送受信するものであって、農機や、建機、一般自動車等の移動体２の周囲（例えば、移動体の前方方向（フロントまたはフロントコーナー）、移動体の後方方向（リアまたはリアコーナー）または移動体の側方（サイド）、またはそれらの組み合わせ）に搭載され、移動体周囲に存在する走路の状態を検出する。具体的には、図２に示すように移動体２からみた走路の位置を検出することができる。図２（ａ）は、レーダ装置１０が搭載された移動体２を上面からみた平面図、図２（ｂ）は、移動体２の側方からみた側面図である。レーダ装置１０により複数の検出点３２が検出され、それぞれの検出点３２について、高さを含めた三次元の位置情報が検出される。これら一群の検出点３２に基づいて、レーダ装置１０の検知範囲３１を定めることができる。検出した情報は、移動体２周辺に存在する溝や崖などの段差といった走路障害物を検出し、移動体２の車輪が溝にはまったり、移動体２が崖から転落することなどを防ぐために利用される。また、走路両端の溝を検出することで道幅の検出に使用することもできる。

【0021】

レーダ装置１０は、周期的な送信信号の信号パターンを生成する信号生成部１４、信号生成部１４に接続され、信号生成部１４の動作を制御する送信制御部１３、信号生成部１４に接続され、信号生成部１４からの信号パターンに基づいて送信信号を発振する発振部１５、発振部１５に接続され、発振部１５で発振された送信波を走路に向かう方向に放射する送信アンテナ１１、送信波が走路４０により反射された反射波を受信する受信アンテナ１２、受信アンテナ１２に接続され、受信アンテナ１２で受信した信号をデジタル信号へ変換するＡＤＣ１７、デジタル信号に基づいてレーダ装置１０に対する物標４０の相対距離を算出する距離演算部１８、デジタル信号に基づいて移動体２の速度を算出する速度演算部１９、検出部２１で利用する検出閾値を設定する閾値設定部２０、距離方向のピークを検出する検出部２１、検出部２１で検出された距離において、到来角度（水平角、仰角）を求める角度演算部２２、検出方向のずれをＩＭＵ（Ｉｎｔｅｒｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）情報を利用して補正する補正部２３、および、補正部２３による補正結果から走路以外の点群情報を排除して走路の検知精度を高めるフィルタ処理を行うフィルタ部２４を備える。発振部１５とＡＤＣ１７とは、スイッチ１６により直接接続することができる。検出装置１のレーダ装置１０は、ＦＣＭ（Ｆａｓｔ Ｃｈｉｒｐ Ｍｏｄｕｌａｔｉｏｎ）方式を採用しているが、ＦＣＭ方式に限定されず、パルス方式でもよい。

【0022】

走路障害物判定部２５は、走路を複数の距離領域に分割した距離領域ごとに、検出点に基づいて走路障害物が存在するか否かを判定する。走路障害物判定部２５は、走路障害物の判定方法が異なる２種類の判定部（点群密度判定部２６、高さ判定部２７）とを備える。走路障害物判定部２５は、点群密度判定部２６および高さ判定部２７の一方または両方を用いて、走路障害物が存在するか否かを判定する。走路障害物判定部２５は、各部の機能をプログラムで記述し、コンピュータによって実行することにより、各部の機能を実施しているが、各部を個別のハードウェアやソフトウェアによって構成してもよい。プログラムは、コンピュータの記憶装置に格納されている。

【0023】

次に、図３のフローチャートに基づいて、検出装置１の動作、すなわち本発明の実施態様である検出方法およびプログラムについて説明する。

【0024】

はじめに、レーダ装置１０により、移動体２の周囲の走路の検出点を周期的に検出して、検出情報を走路障害物判定部２５に提供する（ステップ１０１）。検出情報には、移動体２の速度や、検出点ごとの、走路の高さを含む三次元位置情報および反射波の受信強度を含む。検出情報は、各周期に対応するフレームごとに、時系列的に、検出装置１のメモリに記憶される。

【0025】

次に、走路障害物判定部２５が、移動体２が進行する予定の走路を複数の距離領域に分割し、分割された距離領域ごとに、レーダ装置１０による検出点に基づいて、走行に支障をきたす走路障害物が走路上または走路周辺に存在するか否かを判定する（ステップ１０２）。走路障害物判定には、点群密度判定部２６による走路障害物判定１と、高さ判定部２７による走路障害物判定２との２種類の走路障害物判定処理があるが、一方の走路障害物判定処理のみを実施して、走路障害物判定が存在するか否かを判定してもよいし、双方の走路障害物判定の判定結果に基づいて判定してもよい。双方の走路障害物判定を行う場合には、両者を並行して実施してもよいし、順次実施してもよい。順次実施する場合、判定処理の順序は問わない。

【0026】

走路障害物判定部２５により、走路障害物が存在しないと判定された場合には（ステップ１０３）、処理を終了する。これに対して、走路障害物が存在すると判定された場合には、検出装置１は、警告を発して（ステップ１０４）、処理を終了する。警告は、上位装置２９に障害物が検出されたことを送信してもよいし、移動体２の表示装置を介して移動体２のユーザに直接警告を発出してもよい。

【0027】

次に、レーダ検出（ステップ１０１）および走路障害物判定（ステップ１０２）の各処理について、詳細に説明する。

【0028】

図４は、上述したレーダ検出（ステップ１０１）の動作を具体的に示すフローチャートである。はじめに、送信制御部１３、信号生成部１４および発振部１５が所定の周波数の送信信号を生成し、送信アンテナ１１から移動体２の前方の走路に向けて送信波を放射する送信処理を行う（ステップ２０１）。次に、送信波が１または複数の物標（走路）４０によって反射された反射波を、受信アンテナ１２により受信信号として受信し、ＡＤＣ１７によりデジタル信号に変換する受信処理を行う（ステップ２０２）。

【0029】

次に、距離演算部１８は、デジタル信号に基づいて、レーダ装置１０と物標の一つである走路４０との相対距離を求める距離演算を行う（ステップ２０３）。また、速度演算部１９は、デジタル信号に基づいて速度演算を行う（ステップ２０４）。具体的には、速度演算部１９は、受信信号に基づいて、１または複数の対象物との相対速度情報を算出する。速度演算により対象物の速度を検出することができる。その後、閾値設定部２０は、検出部２１で利用するピークを検出のための検出閾値を設定する（ステップ２０５）。具体的には、受信アンテナ１２の取り付け位置、高さ、角度などに基づいて閾値を調整する。そして、検出部２１は、ＣＦＡＲ（Ｃｏｎｓｔａｎｔ Ｆａｌｓｅ Ａｌａｒｍ Ｒａｔｅ） などの所定の方式を用いて、距離方向のピークを検出する（ステップ２０６）。次に、角度演算部２２は、検出部２１で検出された距離において、反射波の到来角度（水平角、仰俯角）を求める角度演算処理を行い、レーダ装置１０の位置を原点として放射方向を基準軸とする仮想空間において、物標（走路など）４０が存在する方向を算出する（ステップ２０７）。

【0030】

さらに、補正部２３が、走路の凹凸などの影響でレーダ装置が傾いた場合に乗じる検出位置のずれをＩＭＵ情報を利用することで補正する（ステップ２０８）。図５のＡで示すように、移動体２が平坦な走路を走行している場合にはレーダ装置１０と走路３の位置関係が変わらないので高さはぶれずに検出することできる。図中のグラフから明らかなとおり、検出点３２は所定の範囲６０内に位置している。これに対して、図５のＢで示すように移動体２が凸部がある走路を走行すると、レーダ装置１０で得られる物標（走路）４０の位置はレーダからの相対的な位置によって決まるため、ずれが生ずる。例えば、レーダ装置１０が上向くと走路は本来の位置よりも低く検出され、本来位置しなければならない範囲６０から大幅にずれた検出点３３が検出される。このずれを解消するために移動体２のＩＭＵ情報から移動体の姿勢の変化(三次元の移動量)を認識し、レーダ装置１０から見た座標情報をグローバル座標(スタート位置を原点とした座標)に直す。

【0031】

その後、フィルタ部２４が、補正部２３による補正結果から走路以外の点群情報を排除して走路の検知精度を高めるフィルタ処理を行って（ステップ２０９）、処理を終了する。フィルタ部２４におけるフィルタ処理では、走路の高さ方向の標準偏差が他のフレームまたは距離と比較して大きい場合にはそのフレームのデータは除くなどの処理をしてもよい。

【0032】

以上の処理により、レーダ装置１０は、各検出点の三次元位置情報および反射波の受信強度と、対象物の速度とを含む検出情報を検出することができる。レーダ装置１０の検出情報は、各周期に対応するフレームごとに、時系列的に、検出装置１のメモリに記憶される。

【0033】

次に、図６に示したフローチャートに基づいて、２つの走路障害物判定のうち走路障害物判定１について、詳細な処理の説明を行う。走路障害物判定１は、走路を複数の距離領域に分割した距離領域ごとに、検出点の点群密度と、所定の点群密度閾値とを比較し、点群密度閾値より小さな点群密度を有する距離領域が存在し、かつ、点群密度閾値より小さな点群密度を有する距離領域と移動体との距離が、所定の危険度閾値より小さいときに、走路障害物が存在すると判定する。走路障害物判定１は、点群密度判定部２６を構成するコンピュータ（プロセッサ）が、メモリに記憶されたプログラムを実行することにより実施する。

【0034】

はじめに、検出装置１のメモリに記憶された検出点の位置情報に基づいて、検出点の点群密度が低い距離領域が存在するか否かを判定する点群密度判定を実施する（ステップ３０１）。点群密度が低い距離領域が存在しない場合には（ステップ３０２）、走路障害物が存在しないと判定する（ステップ３０６）。これに対して、点群密度が低い距離領域が存在する場合には、当該距離領域の危険度を判定する危険度判定を実施する（ステップ３０３）。すなわち、点群密度が低くなる原因として、レーダ装置１０の検知範囲３１を超えた距離領域であるため点群が検出されなかったことと、段差・溝・崖などの走路障害物が存在することが考えられる。危険度判定によって両者を判別し、走路障害物の存在のみを検出する。すなわち、点群密度が低い距離領域が検知範囲３１を超えた距離領域である場合は、現時点における危険度が低いと判定し（ステップ３０４）、走路障害物が存在しないと判定して（ステップ３０６）、処理を終了する。これに対して、点群密度が低い距離領域が検知範囲３１内の距離領域である場合には、危険度が高いと判定し、走路障害物が存在すると判定して（ステップ３０５）、処理を終了する。

【0035】

次に、走路障害物判定１のうち、点群密度判定（ステップ３０１）と危険度判定（ステップ３０３）の各処理について、詳細に説明する。

【0036】

図７は、点群密度判定のフローチャートである。点群密度判定は、走路を複数の距離領域に分割した距離領域ごとに、検出点の点群密度と、所定の点群密度閾値とを比較し、点群密度閾値より小さな点群密度を有する距離領域を、点群密度の低い距離領域と判定する。

【0037】

はじめに、検出装置１のメモリに記憶された検出点の点群情報を保持して貯めておく（ステップ４０１）。また、走路を複数の距離領域に分割し（ステップ４０２）、保持した点群をそれぞれの位置情報に基づいて、分割した距離領域振り分けて、距離領域ｄごとの点群密度ｄｅｎ_ｄを算出する（ステップ４０３）。このとき、各距離領域の幅を等間隔とすると、各距離領域に振り分けられた検出点の数を点群密度として扱うことができるため、コンピュータの計算量を軽減することができる。検出装置１では、距離領域を所定の幅（例えば０．１ｍ）ごとに分割し、分割された距離領域ごとに単位長さ当たりの点群数を求めることで、各距離領域の点群密度を求めているが、分割方法や分割する幅の大きさはこれに限られるものではなく、移動体２の速度に応じて変更してもよい。例えば、速度が速い場合には幅を大きく、遅い場合には幅を小さくしてもよい。距離領域の原点となる基準点は移動体２のスタート位置としてもよいし、現時点の移動体２の位置としてもよい。

【0038】

図８（ａ）は、移動体２が走路障害物５２のひとつである段差に向かって走行している状態を、走路上方からみた平面図であり、図８（ｂ）は、点群密度ｄｅｎ_ｄの算出結果である。図８（ｂ）において、横軸は移動体２のスタート位置からの距離であり、進行方向３ｍの位置に走路障害物５２が存在している。各領域の点群密度は色の濃淡で示し、色が濃いほど点群密度が高い。なお、上述したように距離領域の幅は０．１ｍで処理を行っているが、煩雑さを避けるため、図８（ｂ）では色の濃淡を０．５ｍごとに表示した。図から明らかなように、走路障害物５２の手前の上側走路５１では、点群密度ｄｅｎ_ｄが高いが、走路障害物５２付近では、点群密度ｄｅｎ_ｄが低くなっていることがわかる。

【0039】

点群密度ｄｅｎ_ｄの算出に続いて、点群密度の高低を判定するための点群密度閾値ｔｈ_ｄｅｎを設定する（ステップ４０４）。点群密度閾値ｔｈ_ｄｅｎは、適宜設定可能であるが、検出装置１では、所定区間で検出された検出点の点群密度の平均値に基づいて設定している。すなわち、走行した走路の所定区間の点群密度平均値ｄｅｎ_ｒｅｆと、重畳したフレーム数ａｃｃｕｍ_{ｆｒａｍｅ}に基づいて次式で求めている。
ｔｈ_ｄｅｎ ＝ ｄｅｎ_ｒｅｆ ／ ａｃｃｕｍ_{ｆｒａｍｅ} ・・・（１）

【0040】

ここで、重畳したフレーム数は、例えば閾値作成に利用した所定区間の点群数のフレーム(時間)間変化が所定以下となるまでのフレーム数とする。フレーム間変化は、例えば過去と現在のフレームの点群密度の変化率を用いる。また、変化率に限らず変化量などを利用してもよい。なお過去のフレームは１フレームに限定するものではなく、複数フレームの点群密度を利用してもよい。

【0041】

次に、距離領域ごとに、点群密度ｄｅｎ_ｄと点群密度閾値ｔｈ_ｄｅｎを比較する（ステップ４０５）。点群密度ｄｅｎ_ｄが点群密度閾値ｔｈ_ｄｅｎよりも小さい距離領域ｄが存在する場合には、検出点の点群密度が低い距離領域が存在すると判定して（ステップ４０６）、処理を終了する。これに対して、全ての距離領域の点群密度ｄｅｎ_ｄが点群密度閾値ｔｈ_ｄｅｎ以上であった場合には、検出点の点群密度が低い距離領域が存在しないと判定して（ステップ４０７）、処理を終了する。

【0042】

続いて、走路障害物判定１のうち、危険度判定（ステップ３０３）の処理について、詳細に説明する。図９は、危険度判定のフローチャートである。危険度判定は、点群密度判定で点群密度が低いと判定された距離領域についてその危険度を判定する。点群密度が低くなる原因として、レーダ装置１０の検知範囲３１を超えた距離領域であるため点群が検出されなかったことと、段差・溝・崖などの走路障害物が存在することとが考えられる。危険度判定によって両者を判別し、走路障害物の存在のみを検出する。すなわち、図１０で示すように、レーダ装置１０の検知範囲３１の範囲内にある距離領域４２の点群密度が低いと判定された場合には、走路障害物の存在により点群密度が低いと判定された可能性が高いことから、危険度が高いと判定する。これに対して、レーダ装置１０の検知範囲３１の範囲外の距離領域４３で点群密度が低いと判定された場合には、いずれの原因であるかを判別できない。ただし、現時点ではレーダ装置１０が取り付けられた移動体２と、点群密度が低いと判定された距離領域４３とはまだ十分に離れており、今後、移動体２の走行により距離領域４３がレーダ装置１０の検知範囲３１の範囲内に入れば、原因を判別することができるため、現時点では危険度は低いと判定する。

【0043】

はじめに、点群密度が低いと判定された距離領域ｄと移動体２との間の距離ｒ_ｄを算出する（ステップ５０１）。次に、危険度を判定するための危険度閾値ｔｈ_ｄｉｓｔを設定する（ステップ５０２）。危険度閾値ｔｈ_ｄｉｓｔは、例えばレーダ装置１０の検知範囲３１に基づいて設定する。この設定によって点群密度が低いと判定された距離領域のうち、走路障害物が存在する距離領域のみを検出することができる。レーダ装置１０の検知範囲３１は、レーダ装置１０の仕様に基づいて設定してもよいし、例えばレーダ装置の位置を原点とした距離方向のヒストグラムにおいて点群密度の最大値に対して所定割合点群密度が低下した地点までを検知範囲３１として定めてもよい。ただし、移動体２が判断してから停止できる距離かつ検知できる距離であることが必要である。

【0044】

次に、距離ｒ_ｄと危険度閾値ｔｈ_ｄｉｓｔを比較する（ステップ５０３）。距離ｒ_ｄが危険度閾値ｔｈ_ｄｉｓｔよりも小さい場合には、危険度が高いと判定して（ステップ５０４）、処理を終了する。これに対して、距離ｒ_ｄが危険度閾値ｔｈ_ｄｉｓｔ以上の場合には、移動体２に直ちに危険を及ぼさないので、現時点での危険度は低いと判定して（ステップ５０５）、処理を終了する。

【0045】

以上により、点群密度に基づいて走路障害物が存在するか否かを判定することできる。これにより、検知範囲の広いレーダなどのセンサ装置を用いて、凹部または凸部が多い走路においても走路障害物を精度よく検出することが可能となる。

【0046】

図１１に点群密度判定処理の変形例のフローチャートを示す。変形例では、点群密度閾値を、所定期間に検出された検出点の点群数の平均値に基づいて設定する点が、図７に示した点群密度判定処理とは異なる。

【0047】

はじめに、検出装置１のメモリに記憶された検出点の点群情報を保持して貯めておく（ステップ４２１）。このとき、現在のフレームにおける検出点の点群情報のみならず、過去の所定期間にわたる検出点の点群情報、例えば直近のｎフレーム分（ｎは自然数）の検出点の点群情報を保持する。また、走路を複数の距離領域に分割し（ステップ４２２）、保持した点群のうち、現在のフレームの点群をそれぞれの位置情報に基づいて、分割した距離領域に振り分けて、距離領域ｄごとの点群数ｃｎｔ_ｄを算出する（ステップ４２３）。このとき、各距離領域の幅を等間隔となるように分割する。すると、各距離領域に振り分けられた検出点の点群数ｃｎｔ_ｄにより、現在のフレームの距離領域ｄごとの点群密度を示すことができる。

【0048】

次に、点群密度の高低を判定するための点群密度閾値ｔｈ_ｃｎｔを設定する（ステップ４２４）。点群密度閾値ｔｈ_ｃｎｔは、ステップ４２１で保持した点群のうち、直近の過去ｎフレーム分の保持した点群をそれぞれの位置情報に基づいて、分割した距離領域に振り分けて、平均した点群数を単位フレームあたりの個数に規格化した値である。すなわち、過去ｎフレーム分の保持した点群の点群数をｃｎｔ_ｐとすると、点群密度閾値ｔｈ_ｃｎｔは次式にように算出できる。
ｔｈ_ｃｎｔ ＝ ｃｎｔ_ｐ ／ ｎ ・・・（２）

【0049】

次に、距離領域ごとに、点群数ｃｎｔ_ｄと点群密度閾値ｔｈ_ｃｎｔとを比較する（ステップ４２５）。点群数ｃｎｔ_ｄが点群密度閾値ｔｈ_ｃｎｔよりも小さい距離領域ｄが存在する場合には、検出点の点群密度が低い距離領域が存在すると判定して（ステップ４２６）、処理を終了する。これに対して、全ての距離領域の点群数ｃｎｔ_ｄが点群密度閾値ｔｈｃｎｔ以上であった場合には、検出点の点群密度が低い距離領域が存在しないと判定して（ステップ４２７）、処理を終了する。

【0050】

次に、図１２および図１３を参照しながら２つの走路障害物判定のうち走路障害物判定２について、詳細な処理の説明を行う。図１２は、走路障害物判定２のフローチャートである。また、図１３（ａ）、（ｂ）は、走路障害物判定２の一例であり、各図とも横軸は移動体２の移動方向の距離を、縦軸は上側走路５１の高さを基準とした高さを示す。また、各図には、走路障害物の一類型である段差５２、段差５２の上側走路５１、および段差５２の下側走路５３の位置も示した。すなわち、移動体２から、移動体２の進行方向に３．０ｍ離れた位置に段差５２があり、段差５２より手前には上側走路５１が、段差５２以降の走路には、上側走路５１より０．２５ｍ低い下側走路５３がある。

【0051】

走路障害物判定２は、走路を複数の距離領域に分割した距離領域ごとに、検出点の高さの平均値と、所定の高さ閾値とを比較し、高さ閾値より小さな高さ平均値を有する距離領域が存在する場合に、走路障害物が存在すると判定する。走路障害物判定２は、高さ判定部２７を構成するコンピュータ（プロセッサ）が、メモリに記憶されたプログラムを実行することにより実施する。

【0052】

はじめに、検出装置１のメモリに検出点の点群情報を保持して貯めておく（ステップ６０１）。また、走路を複数の距離領域に分割し（ステップ６０２）、保持した点群をそれぞれの位置情報に基づいて、分割した距離領域に振り分けて距離領域ｄごとの高さの平均値ｈ_ａｖｅを算出する（ステップ６０３）。検出装置１では、各距離領域の幅を０．１ｍに設定した。図１３（ａ）には検出点３２の分布および距離領域ｄごとの高さの平均値７０を、図１３（ｂ）には各距離区間の検出点の高さの平均値７０を、それぞれ示す。

【0053】

次に、走路障害物の有無を判定するための高さ閾値ｔｈ_ｈを設定する（ステップ６０４）。高さ閾値ｔｈ_ｈは適宜設定可能であるが、検出装置１では、所定区間の走路の高さの平均値ｈ_ｒｅｆと所定のオフセットｈ_{ｏｆｆｓｅｔ}とに基づき、次式のように設定した。
ｔｈ_ｈ ＝ ｈ_ｒｅｆ － ｈ_{ｏｆｆｓｅｔ} ・・・（３）

【0054】

図１３の各図には、高さ閾値ｔｈ_ｈを線７６で示した。なお、所定区間の走路の高さの平均値ｈ_ｒｅｆを求める際に用いる所定区間の大きさは移動体２の速度に応じて変更してもよい。また所定区間の始点は移動体２を基準としてもよいし、スタート位置を基準としてもよい。また、所定のオフセットｈ_{ｏｆｆｓｅｔ}は検出した点群の高さ方向の標準偏差に基づいて定めてもよいし、任意の値に設定してもよい。

【0055】

次に、距離領域ごとに、高さの平均値ｈ_ａｖｅ７０と高さ閾値ｔｈ_ｈ７６を比較する（ステップ６０５）。高さの平均値ｈ_ａｖｅ７０が高さ閾値ｔｈ_ｈ７６よりも小さい距離領域が存在する場合には、走路障害物が存在すると判定して（ステップ６０６）、処理を終了する。これに対して、全ての距離領域の高さの平均値ｈ_ａｖｅ７０が高さ閾値ｔｈ_ｈ７６以上であった場合には、走路障害物が存在しないと判定して（ステップ６０７）、処理を終了する。図１３に示した例においては、上側走路５１の距離領域の高さ平均値ｈ_ａｖｅ７０は、いずれも高さ閾値ｔｈ_ｈ７６以上であるが、下側走路５３の距離領域の高さ平均値ｈ_ａｖｅ７０は、いずれも高さ閾値ｔｈ_ｈ７６より小さいため、走路障害物が存在すると判定される。

【0056】

以上のような検出方法により、検知範囲の広いレーダなどのセンサ装置を用いて、凹部または凸部が多い走路においても走路障害物を精度よく検出することが可能となる。

【0057】

続いて、図１４～１７を参照しながら、本発明に係る検出装置、検出方法およびプログラムの変形例について説明する。図１４は変形例の検出装置６の概略構成図であり、図１５は検出装置６の動作を示すフローチャートである。検出装置６は、走路障害物判定部２５’が、点群密度判定部２６’および高さ判定部２７に加え、走路データモニタリング部２８を備える点が検出装置１と異なる。このため、検出装置１と同一の機能を有する検出装置６の構成要素には、検出装置１と同一の参照符号を付した。

【0058】

走路データモニタリング部２８は、レーダ装置１０で検出された検出情報に基づいて、走路の環境変化を検知し、その結果を点群密度判定部２６’に提供する。点群密度判定部２６’は、移動体２の周囲の走路を複数の距離領域に分割した距離領域ごとに、検出点の点群密度と、所定の点群密度閾値とを比較し、点群密度閾値より小さな点群密度を有する距離領域が存在し、かつ、該点群密度閾値より小さな点群密度を有する距離領域と移動体２との距離が、所定の危険度閾値より小さいときに、走路障害物が存在すると判定する点は、検出装置１の点群密度判定部２６と同じであるが、環境変化が検知されたときに、点群密度閾値を更新する点が点群密度判定部２６とは異なる。検出装置６の点群密度判定部２６’、高さ判定部２７、走路データモニタリング部２８は、その機能をプログラムで記述し、コンピュータによって実行することによりその機能を実施しているが、個別のハードウェアやソフトウェアによって構成してもよい。プログラムは、コンピュータの記憶装置に格納されている。

【0059】

次に、検出装置６の動作、すなわち本発明の実施態様である検出方法およびプログラムについて説明する。検出装置６の動作は、図６で示した検出装置１の走路障害物判定１の処理のうち、点群密度判定（ステップ３０１）の動作が、検出装置１の動作とは異なる。このため、以下では、検出装置６の点群密度判定部２６’により実施される点群密度判定処理について説明する。

【0060】

図１５は、検出装置６の点群密度判定のフローチャートである。なお、図１５に示すステップ４０１からステップ４０７は、上述した図７に示すステップ４０１からステップ４０７と同一の工程である。はじめに、検出装置６のメモリに記憶された検出点の点群情報を保持して貯めておく（ステップ４０１）。また、走路を複数の距離領域に分割し（ステップ４０２）、保持した点群をそれぞれの位置情報に基づいて、分割した距離領域振り分けて、距離領域ｄごとの点群密度ｄｅｎ_ｄを算出する（ステップ４０３）。

【0061】

次に、点群密度の高低を判定するための点群密度閾値ｔｈ_ｄｅｎを設定する。まず、点群密度閾値ｔｈ_ｄｅｎ設定が初回の設定であるか否かを判定する（ステップ４０８）。初回の設定である場合には、既存の点群密度閾値ｔｈ_ｄｅｎが存在しないため、点群密度閾値ｔｈ_ｄｅｎを設定する（ステップ４０４）。具体的な点群密度閾値ｔｈ_ｄｅｎの設定方法は、図７のステップ４０４で説明した検出装置１の設定方法と同様である。これに対して、初回の設定でない場合には、走路データモニタリング部２８による環境変化判定処理を行う（ステップ４０９）。この処理では、例えば走路の状態（材質や舗装状態など）が変化するなどの環境変化が生じた場合に、所定の指標を用いて検出される点群の検出情報が平常時と異なるか否か、すなわち環境変化の有無を判定する。環境変化が有ると判断された場合には（ステップ４１０）、点群密度閾値ｔｈ_ｄｅｎを再度設定（更新）する（ステップ４０４）。一方、環境変化が無いと判断された場合には、点群密度閾値ｔｈ_ｄｅｎの設定は行わずに、既存の点群密度閾値ｔｈ_ｄｅｎを利用する。

【0062】

次に、距離領域ごとに、点群密度ｄｅｎ_ｄと点群密度閾値ｔｈ_ｄｅｎを比較する（ステップ４０５）。点群密度ｄｅｎ_ｄが点群密度閾値ｔｈ_ｄｅｎよりも小さい距離領域が存在する場合には、検出点の点群密度が低い距離領域が存在すると判定して（ステップ４０６）、処理を終了する。これに対して、全ての距離領域の点群密度ｄｅｎ_ｄが点群密度閾値ｔｈ_ｄｅｎ以上であった場合には、検出点の点群密度が低い距離領域が存在しないと判定して（ステップ４０７）、処理を終了する。

【0063】

次に、図１６のフローチャートを参照しながら、環境変化判定ついて、詳細な処理の説明を行う。はじめに、過去の所定期間にわたる検出点の点群情報、例えば直近のｎフレーム分（ｎは自然数）の環境変化指標ｅｎｖ_ｆａｃを保持して貯めておく（ステップ７０１）。環境変化指標ｅｎｖ_ｆａｃは、走路の環境変動に依存するレーダ装置１０の検出情報であり、例えば受信強度を用いる。すなわち、同一状態の走路を走行している場合には、受信強度に大きな変化は生じにくい。一方、走行している走路が反射しやすい走路に切り替わった場合には受信強度が強くなり、反射しにくい走路に切り替わった場合には受信強度が弱くなる。このような環境変化が生じた場合には、点群密度判定処理に用いる点群密度閾値を再設定した方が走路障害物の判定精度を向上させることができる。

【0064】

環境変化指標ｅｎｖ_ｆａｃの保持が完了すると、蓄積した所定フレーム分の環境変化指標ｅｎｖ_ｆａｃから判定指標ｊｕｄ_ｉｎｄを算出する（ステップ７０２）。判定指標ｊｕｄ_ｉｎｄは、適宜選定可能であるが、検出装置６では、受信強度の標準偏差を利用している。具体的には、受信強度の標準偏差を所定間隔のフレーム期間でモニタリングする。図１７に算出された判定指標ｊｕｄ_ｉｎｄの時間変化を、横軸に時間（フレーム）を、縦軸に判定指標ｊｕｄ_ｉｎｄの大きさをとって示す。図では、移動体２が、互いに状態（材質や舗装状態など）が異なる走路３から走路４に向けて走行している。すなわち、レーダ装置１０が受信した反射波の振幅の強度がある一定以上に異なる状態であるか否かを判定することにより、状態が異なることを判定する。レーダ装置１０で検知される受信強度を所定期間ごとのフレーム期間に分割すると、期間８５では、移動体２が、状態が同一の走路３のみを走行しているため、判定指標ｊｕｄ_ｉｎｄ７２、すなわち受信強度の標準偏差は小さくなる。期間８６では、移動体２が、状態が異なる走路３から走路４に移行するため、期間中の判定指標ｊｕｄ_ｉｎｄ７２は大きくなる。期間８７では、移動体２が、状態が同一の走路４のみを走行しているため、判定指標ｊｕｄ_ｉｎｄ７２は小さくなる。

【0065】

次に、環境変化閾値ｔｈ_ｓｔｄを設定する（ステップ７０３）。環境変化閾値ｔｈ_ｓｔｄは、選定した判定指標ｊｕｄ_ｉｎｄや、想定される判定指標ｊｕｄ_ｉｎｄの変動に応じて適宜設定可能であり、過去フレームの標準偏差の大きさを基準とした相対的な閾値を設定してもよい。図１７の例では、線７７に示す値に環境変化閾値ｔｈ_ｓｔｄを設定した。そして、フレーム期間ごとに、判定指標ｊｕｄ_ｉｎｄと環境変化閾値ｔｈ_ｓｔｄとを比較する（ステップ７０４）。そして、判定指標ｊｕｄ_ｉｎｄが環境変化閾値ｔｈ_ｓｔｄを超える場合には、環境変化があると判定する（ステップ７０５）。一方、判定指標ｊｕｄ_ｉｎｄが環境変化閾値ｔｈ_ｓｔｄ以下の場合には、環境変化がないと判定する（ステップ７０６）。

【0066】

なお、判定指標ｊｕｄ_ｉｎｄは、受信強度の平均値を用いてもよい。前述した標準偏差を用いる場合には、受信強度の時間的な変化を見ていたが、平均値を利用する場合には距離軸で変化を捉える。すなわち、所定距離ごとの点群情報から、距離領域ごとに受信強度を評価する。初期位置の受信強度を１としたときの変化の大きさを見る。環境変化閾値をもとに変化量が大きいと判断される場合に環境変化があると判断する。なお、環境変化閾値は任意の設定値としてもよいし、既に走行した距離の受信強度平均値を基準とした相対的な閾値を設定してもよい。

【0067】

以上のような、走路の環境変化をモニタリングして、所定の環境変化が生じた場合に点群密度閾値を更新する検出方法により、凹凸多い走路においても走路障害物を精度よく検出することが可能となる。

【0068】

以上、本発明にかかる検出装置、検出方法およびプログラムについて説明を行ったが、本発明は上記の実施の形態に限定されるものではなく、本発明の概念及び特許請求の範囲に含まれるあらゆる態様を含む。

【符号の説明】

【0069】

１、６ 検出装置
２ 移動体
３、４ 走路
１０ センサ装置（レーダ装置）
１１ 送信アンテナ
１２ 受信アンテナ
１３ 送信制御部
１４ 信号生成部
１５ 発振部
１６ スイッチ
１７ ＡＤＣ
１８ 距離演算部
１９ 速度演算部
２０ 閾値設定部
２１ 検出部
２２ 角度演算部
２３ 補正部
２４ フィルタ部
２５、２５’ 走路障害物判定部
２６、２６’ 点群密度判定部
２７ 高さ判定部
２８ 走路データモニタリング部
２９ 上位装置
３１ 検知範囲
３２、３３ 検出点
４０ 物標（走路）
４２、４３ 領域
５１ 上側走路
５２ 走路障害物（段差、凹型障害物）
５３ 下側走路
７０ 検出点の平均値
７２ 判定指標
７５、７６、７７ 閾値
８０ 警告表示
８５、８６、８７ 期間

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】