JP6406289B2 - 路面形状測定装置、測定方法、及び、プログラム - Google Patents

Description

本発明は、起伏などを有する路面の表面形状を測定する路面形状測定装置に関する。

従来、車両などの移動体の周辺に存在する障害物を認識し、障害物と移動体との位置関係などに基づいて、移動体の運転者に代わって移動体の制御を行う先進運転支援システム（Ａｄｖａｎｃｅｄ Ｄｒｉｖｅｒ Ａｓｉｓｔａｎｃｅ Ｓｙｓｔｅｍ、ＡＤＡＳ）が知られている。その一方、移動体の移動においては、移動体の周辺に存在する障害物だけでなく、移動体が移動する路面状態を把握することも重要である。例えば、路面の起伏が激しい場合などには、当該起伏が激しい路面を回避するよう移動体を制御したり、起伏が激しい路面が存在することを通知したりすることが重要である。

例えば、特許文献１には、路面プロファイルと路面の各点の３次元の地理座標を測定し、当該路面プロファイルと地理座標とを関連付けて路面の凹凸に関するデータを発生させる路面性状測定装置が開示されている。この装置では、上記のようにして得られた路面の凹凸に関するデータを実際の路面画像上に反映した画像として表示することにより、路面プロファイルの把握が容易となる。

特開２００４−２９４１５２号公報

上記の路面性状測定装置では、路面プロファイルは路面上の点と装置が取り付けられた移動体との間の距離として測定されている。すなわち、この装置で得られる路面の凹凸に関するデータは、３次元の座標値にて表される点データである。
このような場合に装置自身が路面の形状を把握するためには、装置は、凹凸に関するデータを多数必要とし、その結果、膨大な計算を実行する必要がある。

本発明の課題は、測定装置などにより測定したデータを用いて路面形状を把握する装置において、少ない計算量で適切に路面形状を把握することにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る路面形状測定装置は、起伏を有する路面の形状を測定する。路面形状測定装置は、路面情報取得部と、路面形状判定部と、を備える。路面情報取得部は、路面上の複数の点について、路面位置と路面高さとからなる路面情報を取得する。路面位置は、複数の区画領域に分けられた基準平面上における路面の位置である。路面高さは、当該路面位置における路面の基準平面からの高さを表す。路面形状判定部は、区画領域に含まれる路面情報を用いて、当該区画領域ごとに表面形状を推定することにより、路面の一部または全体の形状を判定する。

上記の路面形状測定装置では、路面情報取得部が、路面上の複数の点について、基準平面上における路面の位置（路面位置）と、当該路面位置における路面の高さ（路面高さ）とを、路面情報として取得する。その後、路面形状判定部が、基準平面を区画することにより定義された複数の区画領域の各区画領域に含まれる路面情報を用いて、当該各区画領域の表面形状を推定する。当該区画領域の表面形状に基づいて、路面形状判定部は、路面の一部または全体の形状を判定する。
このように、上記の路面形状測定装置では、区画領域に含まれる路面情報を用いて、区画領域ごとにその表面形状（すなわち、路面の一部の形状）を推定している。また、当該区画領域ごとに推定された表面形状に基づいて、路面の一部又は全体の形状を判定している。
これにより、基準平面全体にて取得された全ての路面情報を用いて路面全体の形状を判定する場合と比較して、より少ない計算量により適切に路面形状を把握できる。

路面情報取得部は、エネルギー測定部と、距離画像取得部と、座標変換部と、を有していてもよい。エネルギー測定部は、路面にて反射したエネルギーを測定する。距離画像取得部は、距離画像を取得する。距離画像は、複数の画素で構成され、複数の画素のそれぞれが、上記のエネルギーに基づいて測定される路面と路面情報取得部との間の距離を画素値として有する画像である。座標変換部は、距離画像の各画素を基準平面の位置に対応付けて路面位置を算出し、当該路面位置における路面高さを当該各画素が有する画素値に基づいて算出することにより、路面情報を算出する。
これにより、路面情報取得部は、路面にて反射してエネルギーを用いて、路面情報を算出できる。

区画領域に所定の数以上の路面情報が存在していれば、路面形状判定部は、当該区画領域の表面形状を推定することを決定してもよい。一方、区画領域に所定の数以上の路面情報が存在していなければ、路面形状判定部は、当該区画領域の表面形状を推定しないことを決定してもよい。
これにより、適切な表面形状を推定できる程度の数の路面情報が集まった時点などの適切なタイミングにて、路面の表面形状を適切に推定できる。

路面形状判定部は、表面形状を推定しようとしている区画領域に含まれる複数の路面情報の数が前回の表面形状の推定時よりも増加していれば、前回推定した表面形状を、当該数が増加した路面情報を用いて更新してもよい。
これにより、より多くの路面情報を用いてより適切に推定された表面形状を、路面の形状とできる。

路面形状判定部は、１つの区画領域に対して推定された表面形状と、当該１つの区画領域に隣接する区画領域に対して推定された表面形状とが、基準平面内で不連続となれば、当該１つの区画領域に対して推定された表面形状を、当該隣接する区画領域に対して推定された表面形状と連続的に接続するよう補正してもよい。
これにより、互いに隣接し合った区画領域において推定された複数の表面形状の間に不連続が生じた場合に、当該不連続を解消して、各区画領域において適切な表面形状を推定できる。

区画領域の大きさは、各々、基準平面における路面情報取得部と当該区画領域との間の距離に基づいて決定されてもよい。
これにより、路面情報取得部からの基準平面における位置によって、１つの路面情報が存在しうる基準平面での領域の大きさが変化しても、できるだけ多数の路面情報を１つの区画領域に対して取得できる。

路面情報取得部は、基準平面上を移動してもよい。この場合、路面情報取得部は、移動中の異なる時刻に測定された路面情報を累積する。路面形状判定部は、当該累積された路面情報を用いて表面形状を推定する。
これにより、各区画領域により多くの路面情報を含めて、各区画領域において表面形状をより適切に推定できる。

路面形状判定部は、区画領域に含まれる路面情報に対して所定の関数をフィッティングすることにより算出される表面形状関数を、当該区画領域における表面形状と推定してもよい。これにより、表面形状を、路面情報と一致する表面形状関数といった数学モデルにより表現できる。

路面形状判定部は、取得された時刻がより新しい路面情報と一致する表面形状関数を、当該表面形状関数を算出中の区画領域の表面形状を表すものと推定してもよい。
これにより、より信頼性の高い路面情報を用いて、より適切な表面形状を推定できる。

上記の所定の関数は、路面の段差を表す第１形状関数と、路面における斜面を表す第２形状関数と、を含んでいてもよい。この場合、路面形状判定部は、第１形状関数と第２形状関数のうち、区画領域に含まれる路面情報とより一致する方を、上記の表面形状関数と推定する。
これにより、予め決められた第１形状関数と第２形状関数とを用いて、より簡単に表面形状関数を決定できる。

上記の路面形状測定装置は、路面状態判定部をさらに備えていてもよい。路面状態判定部は、路面形状判定部にて推定された表面形状に基づき、路面の路面状態を判定する。これにより、路面の実際の路面状態を判定できる。

本発明の他の見地に係る測定方法は、起伏を有する路面の形状を測定する方法である。測定方法は、以下のステップを含む。
◎路面上の複数の点について、複数の区画領域に分けられた基準平面上における路面の路面位置と、路面位置における路面の基準平面からの高さを表す路面高さと、からなる路面情報を取得するステップ。
◎区画領域ごとに、当該区画領域に含まれる路面情報を用いて表面形状を推定することにより、路面の一部または全体の形状を判定するステップ。

上記の測定方法では、区画領域に含まれる路面情報を用いて、区画領域ごとにその表面形状（すなわち、路面の一部の形状）を推定している。また、当該区画領域ごとに推定された表面形状に基づいて、路面の一部又は全体の形状を判定している。
これにより、基準平面全体にて取得された全ての路面情報を用いて路面全体の形状を判定する場合と比較して、より少ない計算量により適切に路面形状を把握できる。

本発明のさらに他の見地に係るプログラムは、上記の測定方法をコンピュータに実行させるプログラムである。

測定装置などにより測定したデータを用いて路面形状を把握する際に、少ない計算量で適切に路面形状を把握できる。

路面形状測定装置が用いられる移動体システムの構成を示す図。 物体検出センサの構成を示す図。 制御部の構成を示す図。 基準平面を区画領域に区画した状態の一例を模式的に示す図。 第１形状関数の一例を示す図。 第２形状関数の一例を示す図。 路面形状測定装置の全体的な動作を示すフローチャート。 路面形状判定処理の流れを示すフローチャート。 路面情報を累積する様子の一例を模式的に示す図（その１）。 路面情報を累積する様子の一例を模式的に示す図（その２）。 路面情報を累積する様子の一例を模式的に示す図（その３）。 表面形状を判定する処理の流れを示すフローチャート。 新しい路面情報と古い路面情報が混在した場合の、ＲＡＮＳＡＣを用いた表面形状関数の決定方法の一例を示す図（その１）。 新しい路面情報と古い路面情報が混在した場合の、ＲＡＮＳＡＣを用いた表面形状関数の決定方法の一例を示す図（その２）。

１．第１実施形態
（１）路面形状測定装置が用いられる移動体システムの構成
以下、第１実施形態に係る路面形状測定装置１００が用いられる移動体システム１の構成について、図１を用いて説明する。図１は、路面形状測定装置が用いられる移動体システムの構成を示す図である。第１実施形態に係る路面形状測定装置１００は、例えば、自動車などの移動体の本体１１に取り付けられ、当該移動体が移動する路面ＲＳの起伏を測定するための装置である。路面形状測定装置１００は、必要に応じて、運転者による移動体の操作をアシストする。

移動体システム１は、本体１１を備える。本体１１は、移動体システム１の本体を構成する。移動体システム１は、車輪１２ａ、１２ｂ、１２ｃ、１２ｄを備える。車輪１２ａ、１２ｂは、本体１１の直進方向（図１）の前部において、駆動部１３（例えば、エンジン及び／又は電動モータ）の出力回転軸に、減速機構を介して軸回りに回転可能に取り付けられている。一方、車輪１２ｃ、１２ｄは、本体１１の直進方向の後部に、軸回りに回転可能に取り付けられている。

移動体システム１は、物体検出センサ１４（エネルギー測定部の一例）を備える。図１に示すように、物体検出センサ１４は、本体１１の直進方向の最前部に、検出面ＤＳ（後述）が路面に対して平行または平行から若干路面向きに取り付けられる。この結果、物体検出センサ１４は、本体１１の前方に存在する路面の幅広い範囲から反射される反射光Ｌｒを測定する。
本実施形態において、物体検出センサ１４は、ＴＯＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）センサである。物体検出センサ１４としては、ＴＯＦセンサに限られず、超音波や光などのエネルギーによる信号を出力し、当該信号が路面ＲＳにて反射されて発生する反射信号を検出できるセンサを用いることができる。このようなセンサとしては、例えば、レーザレンジファインダ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ、ＬＲＦ）などがある。本実施形態における物体検出センサ１４の構成については、後ほど詳しく説明する。

本実施形態において、物体検出センサ１４は、本体１１の直進方向の最前部にのみ取り付けられているが、これに限られず、本体１１の直進方向の最後部などの他の位置に取り付けられてもよい。これにより、例えば、本体１１を前進方向以外へ移動させる際に、前進方向以外の方向の路面状態を確認できる。

移動体システム１は、制御部１５を備える。制御部１５は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）と、記憶装置（ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）又はＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）など）と、各種インターフェース（例えば、Ａ／Ｄ、Ｄ／Ａ変換器など）を備えたコンピュータシステムである。制御部１５は、物体検出センサ１４からの検出信号を入力し、当該検出信号に基づいて、本体１１が移動しようとする方向の路面ＲＳの状態を判定する。

本実施形態において、制御部１５は、車輪１２ａ、１２ｂに設けられたブレーキの駆動機構、駆動部１３の駆動機構（例えば、アクセルやモータ制御装置）、及び／又はハンドルの駆動機構などに接続されている。制御部１５は、本体１１が移動しようとしている方向の路面の状態に基づいて、必要に応じて移動体システム１の運転者に代わって、駆動機構を制御する。制御部１５の構成については、後ほど詳しく説明する。

移動体システム１は、位置測定部１６を備える。位置測定部１６は、本体１１が移動したときの実空間における相対的な位置変化を測定する。位置測定部１６は、例えば、本体１１の速度を測定する速度センサ、加速度を測定する加速度センサ、及び／又は、本体１１の角速度を測定するジャイロセンサーなどを用いることができる。その他、例えば、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）装置を、位置測定部１６として用いることができる。

上記の構成を備えることにより、移動体システム１は、物体検出センサ１４により検出された検出信号に基づいて、本体１１が移動しようとする路面ＲＳの状態を判定できる。また、移動体システム１は、上記の判定された路面ＲＳの状態に基づいて、移動体システム１の運転者による運転をアシストできる。本実施形態においては、物体検出センサ１４と制御部１５とが、路面形状測定装置１００を構成する。

（２）物体検出センサの構成
次に、本実施形態に係る路面形状測定装置１００にて用いられる物体検出センサ１４の構成について、図２を用いて説明する。図２は、物体検出センサの構成を示す図である。
物体検出センサ１４は、出力部１４１を有する。出力部１４１は、例えば、検出対象で路面ＲＳに向けて、赤外領域の測定光Ｌｍを出力する光源である。本実施形態において、出力部１４１は、本体１１が移動しようとしている路面ＲＳの広い範囲に広がった測定光Ｌｍを出力することが好ましい。これにより、物体検出センサ１４は、移動しようとしている方向にある広い範囲の路面ＲＳに対して、同時に測定光Ｌｍを照射できる。

物体検出センサ１４は、複数の検出部１４３−１、１４３−２、・・・１４３−ｎを有する。複数の検出部１４３−１、１４３−２、・・・１４３−ｎのそれぞれは、例えば、検出面ＤＳ（半導体基板）上の所定の位置に配置され、測定光Ｌｍが路面ＲＳにて反射されることにより発生する反射光Ｌｒを検出する。検出部１４３−１〜１４３−ｎは、例えば、電荷結合素子（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）、又は、ＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ ＭＯＳ）素子である。

また、図２に示すように、複数の検出部１４３−１〜１４３−ｎは、検出面ＤＳ上の上下方向及び左右方向に配置されてアレイを形成している。これにより、複数の検出部１４３−１〜１４３−ｎは、検出面ＤＳ上に、ＣＣＤイメージセンサ又はＣＭＯＳイメージセンサを形成できる。
複数の検出部１４３−１〜１４３−ｎのそれぞれには、当該検出部と外部の制御部１５とを接続／切断するためのスイッチング素子（例えば、ＭＯＳ−ＦＥＴ）が接続されている。また、スイッチング素子にはアドレス線が接続されており、アドレス線に信号が印加されたときに、当該スイッチング素子はＯＮされ、ＯＮされたスイッチング素子に接続された検出部と、制御部１５とが信号送受信可能となる。

検出面ＤＳが複数の検出部１４３−１〜１４３−ｎを有することにより、物体検出センサ１４は、路面ＲＳの広い範囲から発生した反射光Ｌｒを検出できる。どの検出部が路面ＲＳのどの位置からの反射光Ｌｒを検出するかは、当該検出部の検出面ＤＳの中心からの相対的な配置位置により決定される。

例えば、図２のように本体１１の高さ方向に出力部１４１と検出面ＤＳとが並んで配置される場合、本体１１の直進方向にある路面ＲＳからの反射光Ｌｒは、検出面ＤＳの中心に近い検出部にて検出される。一方、物体検出センサ１４から近い又は遠い位置にある路面ＲＳからの反射光Ｌｒは、検出面ＤＳの中心から離れた検出面ＤＳの下部又は上部に存在する検出部にて検出される。
さらに、本体１１の直進方向から外れる方向にある路面ＲＳからの反射光Ｌｒは、検出面ＤＳの左側又は右側に存在する検出部にて検出される。
このように、複数の検出部１４３−１〜１４３−ｎのそれぞれは、広い範囲の路面ＲＳの異なる位置からの反射光Ｌｒを検出できる。

物体検出センサ１４は、レンズ１４５を有している。レンズ１４５は、反射光Ｌｒを、検出面ＤＳのうち複数の検出部１４３−１〜１４３−ｎが形成された領域に集光する。これにより、実空間にある路面ＲＳの像を、複数の検出部１４３−１〜１４３−ｎが形成された領域に結像できる。

上記の構成を有することにより、物体検出センサ１４は、基準平面ＳＦ上の本体１１が移動しようとする方向に存在する広い範囲の路面ＲＳからの反射光Ｌｒを測定できる。また、複数の検出部１４３−１〜１４３−ｎのそれぞれが、路面ＲＳの異なる位置からの反射光Ｌｒを検出できるので、物体検出センサ１４にて検出された反射光Ｌｒの測定結果に基づいて、路面ＲＳと物体検出センサ１４との間の距離を画素値として有する複数の画素にて構成される距離画像（後述）を取得できる。

なお、以下の説明においては、実空間（Ｘ−Ｙ−Ｚ座標系）において、路面ＲＳが配置される基準平面ＳＦはＸ−Ｙ座標系で表現されるものとする。また、基準平面ＳＦからの高さ方向はＺ軸の座標値として表現されるものとする。

（３）制御部の構成
以下、本実施形態に係る路面形状測定装置１００の制御部１５の構成を、図３を用いて説明する。図３は、制御部の構成を示す図である。以下に説明する制御部１５の各要素の機能の一部又は全部は、制御部１５を構成するコンピュータシステムにて実行可能なプログラムとして実現されていてもよい。このとき、当該プログラムは、コンピュータシステムの記憶装置に形成された記憶領域に記憶されていてもよい。また、制御部１５の各要素の機能の一部又は全部は、カスタムＩＣなどによりハードウェア的に実現されていてもよい。
制御部１５は、記憶部１５１を有する。記憶部１５１は、各種データを記憶する、例えば、コンピュータシステムの記憶装置に設けられた記憶領域の一部である。

制御部１５は、距離画像取得部１５２を有する。距離画像取得部１５２は、距離画像Ｄ１を取得する。距離画像取得部１５２は、以下のようにして距離画像Ｄ１を取得する。
出力部１４１から測定光Ｌｍを出力した後、距離画像取得部１５２は、最初に、１番目の検出部１４３−１に対応するアドレス線に信号を印加することで、１番目の検出部１４３−１と距離画像取得部１５２とを接続し、検出部１４３−１が反射光Ｌｒを検出したか否かを示す信号（例えば、電流又は電圧信号）を入力する。

当該信号を入力した後、距離画像取得部１５２は、反射光Ｌｒを検出したか否かを示す信号を入力した時刻と、出力部１４１が測定光Ｌｍを出力した時刻と、の差分を信号検知情報ＤＴとして算出する。すなわち、信号検知情報ＤＴは、路面ＲＳと物体検出センサ１４との間の距離に対応する情報である。
なお、反射光Ｌｒが検出されなかった場合には、検出部１４３−１からの信号の入力がないので、信号検知情報ＤＴは無限大（あるいは非常に大きな値）に設定される。

次に、距離画像取得部１５２は、信号検知情報ＤＴから路面ＲＳと物体検出センサ１４との間の距離を算出する。当該距離ｄはｃ＊ＤＴ／２（ｃ：光速）により算出できる。

次に、距離画像取得部１５２は、第１位置情報（ｘ，ｙ，ｄ）を生成する。第１位置情報は、検出部１４３−１〜１４３−ｎのいずれかの配置位置を第１座標に投影した座標値（ｘ，ｙ）と、上記の距離ｄとを関連付けることで生成された、距離画像Ｄ１の１つの単位、すなわち、１つの画素に対応する。

その後、距離画像取得部１５２は、信号を印加するアドレス線を順次変更することにより、上記の工程を他の全ての検出部１４３−１、１４３−２、１４３−３、・・・１４３−ｎに対して実行して、ｎ個の第１位置情報（画素）の集合体（ｘ１、ｙ１、ｄ１）、（ｘ２、ｙ２、ｄ２）、・・・、（ｘｎ、ｙｎ、ｄｎ）を生成する。当該ｎ個の第１位置情報の集合体は、距離画像Ｄ１として、記憶部１５１に記憶される。

なお、信号検知情報から算出された距離ｄと関連付ける上記の座標値は、検出部ごとに予め決められており、例えば、各検出部の検出面ＤＳにおける配置関係に基づいて決定される。例えば、検出部１４３−１の水平方向に隣接する検出部１４３−２に割り当てられた座標値（ｘ２，ｙ２）において、ｙ２はｙ１と等しくなる。一方、例えば、検出部１４３−１の水平方向に隣接する検出部１４３−ｍに割り当てられた座標値（ｘｍ，ｙｍ）において、ｘｍはｘ１と等しくなる。

また、物体検出センサ１４の複数の検出部１４３−１〜１４３−ｎのそれぞれは、路面ＲＳの異なる位置からの反射光Ｌｒを検出するので、距離画像Ｄ１は、物体検出センサ１４にて検出された路面ＲＳを第１座標上に投影できる。また、上記のように、距離画像Ｄ１を構成する第１位置情報（画素）には、各検出部にて検出された路面ＲＳと物体検出センサ１４との間の距離が関連付けられている。
従って、上記のようにして得られた距離画像Ｄ１は、複数の画素（第１位置情報）で構成され、複数の画素のそれぞれが、反射光Ｌｒに基づいて測定される路面ＲＳと物体検出センサ１４との間の距離を画素値として有する、物体検出センサ１４にて測定された路面ＲＳを第１座標に投影する画像であると言える。

制御部１５は、座標変換部１５３を有する。座標変換部１５３は、路面情報Ｉを算出する。路面情報Ｉは、路面ＲＳを定義する基準平面ＳＦ（図２）における路面ＲＳの位置を表す路面位置と、当該路面位置に存在する路面ＲＳの前記基準ＳＦからの高さを表す路面高さと、からなる情報である。
具体的には、座標変換部１５３は、距離画像Ｄ１に含まれる各画素（第１位置情報）を、基準平面ＳＦにおける位置（座標値）に対応付けることで、路面情報Ｉに含まれる路面位置を算出する。複数の検出部１４３−１〜１４３−ｎと実空間とは、予めキャリブレーションを行うことにより対応付けされる。一方、路面情報Ｉに含まれる路面高さは、路面高さを算出しようとしている路面位置に対応する画素（第１位置情報）に関連付けられている画素値（距離ｄ）に基づいて算出される。

上記のように、物体検出センサ１４は複数（ｎ個）の検出部１４３−１〜１４３−ｎを有しているので、座標変換部１５３は、ｎ個の路面ＲＳ上の点について、ｎ個の路面情報Ｉ−１〜Ｉ−ｎを算出する。

本実施形態においては、物体検出センサ１４と、距離画像取得部１５２と、座標変換部１５３と、が路面情報取得部１４’を構成する。

制御部１５は、位置決定部１５４を有する。本体１１が移動した後に、位置決定部１５４は、物体検出センサ１４（路面情報取得部１４’）の基準平面ＳＦ上の位置を決定する。具体的には、位置決定部１５４は、移動前の基準平面ＳＦにおける物体検出センサ１４の位置と、位置測定部１６にて測定された本体１１の実空間における相対的な位置変化情報と、本体１１への物体検出センサ１４の設置位置とに基づいて、移動後の物体検出センサ１４の基準平面ＳＦ上の位置を決定する。

例えば、位置測定部１６がＧＰＳ装置である場合には、位置決定部１５４は、ＧＰＳ装置にて測定された位置（例えば、緯度及び経度）から、本体１１の基準平面ＳＦにおける位置を決定できる。
一方、位置測定部１６から速度の実測値を入力する場合には、所定の時間内における本体１１の速度を累積又は時間で積分して本体１１の移動距離を算出し、移動前の基準平面ＳＦの位置から当該移動距離だけ移動したときの位置を、移動後の物体検出センサ１４の位置と決定できる。
さらに、位置測定部１６から加速度の実測値を入力する場合には、当該加速度から速度を算出した上で、当該速度を累積又は時間で積分することにより、移動距離を算出できる。その後、移動前の位置から移動距離だけ移動したときの位置を、移動後の物体検出センサ１４の位置と決定できる。

制御部１５は、路面形状判定部１５５を有する。路面形状判定部１５５は、区画領域に含まれる路面情報Ｉを用いて、当該区画領域ごとに表面形状を推定する。これにより、路面形状判定部１５５は、路面ＲＳの一部または全体の形状を判定できる。本実施形態においては、基準平面ＳＦは、例えば、図４に示すように、複数の正方形の区画領域Ｇ１〜Ｇ８７に区画される。図４は基準平面を区画領域に区画した状態の一例を模式的に示す図である。

区画領域Ｇ１〜Ｇ８７は、例えば、各区画領域の中心座標と、区画領域の大きさとにより定義できる。例えば、ｐ番目の区画領域Ｇｐの中心座標を（ＸＧｐ，ＹＧｐ）（基準平面ＳＦ上にて予め決められた固定値）とし、当該区画領域Ｇｐの大きさをＷとした場合、区画領域Ｇｐは、ＸＧｐ−Ｗ／２≦Ｘ＜ＸＧｐ＋Ｗ／２、ＹＧｐ−Ｗ／２≦Ｙ≦ＹＧｐ＋Ｗ／２の２つの不等式で表される領域にて定義できる。
上記の各区画領域の中心座標と、当該各区画領域の大きさとは、区画領域情報ＧＩとして記憶部１５１に記憶されている。

物体検出センサ１４（検出面ＤＳ）を路面ＲＳに向けて本体１１に取り付けているため、本体１１から直進方向に向かって遠い位置に存在する路面ＲＳの測定点の密度は、本体１１から近い位置に存在する路面ＲＳの測定点の密度よりも小さくなる。
従って、本実施形態において、区画領域Ｇ１〜Ｇ８７の大きさは、各々、基準平面ＳＦにおける物体検出センサ１４と当該区画領域との間の距離に基づいて決定される。具体的には、図４に示すように、本体１１からＹ軸方向（本体１１の直進方向）に遠い位置にある区画領域の大きさを、他の区画領域の大きさよりも大きくしている。
このように、本体１１から直進方向に向かって遠い位置に存在する区画領域の大きさを大きくすることにより、各区画領域により多くの路面情報Ｉを含めることができる。本体１１の移動により距離が変化するので、本体１１の位置変化情報を用いて適宜区画領域の大きさを見直すようにしてもよい。

または、上記にかかわらず、区画領域Ｇ１〜Ｇ８７の大きさを全領域で同一としておいてもよい。これにより、本体１１が移動するに伴って（中心座標が固定である）区画領域の大きさを変更する必要がなくなる。
上記の各区画領域の大きさ、個数、形状などは、物体検出センサ１４の特性や本体１１への取付状態などを考慮して、適宜変更できる。

また、路面形状判定部１５５は、所定の関数を、区画領域内に含まれる路面情報Ｉに対してフィッティングし、当該フィッティングの結果算出された表面形状関数Ｆを、当該区画領域に存在する路面ＲＳの表面形状を表す関数であると推定する。これにより、路面ＲＳの表面形状を、路面情報Ｉと一致する表面形状関数Ｆといった数学モデルにて表現できる。

本実施形態において、路面形状判定部１５５は、区画領域内に含まれる路面情報Ｉに対してフィッティングさせる所定の関数として、第１形状関数Ｆ１と第２形状関数Ｆ２とを用いる。路面形状判定部１５５は、第１形状関数Ｆ１と第２形状関数Ｆ２のうち、区画領域に含まれる路面情報Ｉとより一致する方を、当該区画領域の表面形状を表す表面形状関数Ｆであると推定する。これにより、予め決められた第１形状関数Ｆ１と第２形状関数Ｆ２とを用いて、より簡単に表面形状関数Ｆを決定できる。

第１形状関数Ｆ１及び第２形状関数Ｆ２は、路面ＲＳの典型的な形状を表す関数として以下のように定義できる。
第１形状関数Ｆ１は、図５Ａに示すように、実空間座標系（Ｘ−Ｙ−Ｚ座標系）において、路面ＲＳの段差を表す関数として定義できる。図５Ａは、第１形状関数の一例を示す図である。図５Ａに示す第１形状関数Ｆ１は、例えば、面Ｚ＝ａ（−Ｘ１≦Ｘ≦Ｘ１、０≦Ｙ≦Ｙ１）、面Ｙ＝０（−Ｘ１≦Ｘ≦Ｘ１、ｂ≦Ｚ≦ａ）、面Ｚ＝ｂ（−Ｘ１≦Ｘ≦Ｘ１、−Ｙ１≦Ｙ≦０）（Ｘ１：区画領域のＸ軸方向の境界を示す値、Ｙ１：区画領域のＹ軸方向の境界を示す値）と３つの面にて表すことができる。

第１形状関数Ｆ１を路面情報Ｉに対してフィッティングさせる場合には、上記の値ａ及びｂを変えることにより、第１形状関数Ｆ１の段差の高さや基準平面（面Ｚ＝０）からの高さを変えることができる。また、例えば、図５Ａに示す第１形状関数Ｆ１をＺ軸回りに回転することにより、段差の向きを変更できる。さらに、例えば、面Ｙ＝０（Ｘ−Ｚ平面に平行な面）をＹ軸方向に移動することにより、区画領域における段差の位置を変更できる。その他、各面を任意の軸回りに回転したり、面をシフトしたりすることにより、任意の第１形状関数Ｆ１を生成できる。
または、段差を表す第１形状関数Ｆ１を、シグモイド関数などの段差を表す関数により、連続的な１つの面として定義することもできる。

一方、第２形状関数Ｆ２は、図５Ｂに示すように、路面ＲＳにおける斜面（又は傾斜のない平面）を表す関数として定義できる。図５Ｂは、第２形状関数の一例を示す図である。第２形状関数Ｆ２は、１つの面で表現できるため、例えば、区画領域中の座標（Ｘ０，Ｙ０，Ｚ０）を通り、法線ベクトルが（ｃ，ｄ，ｅ）である面として、例えば、ｃ＊（Ｘ−Ｘ０）＋ｄ＊（Ｙ−Ｙ０）＋ｅ＊（Ｚ−Ｚ０）＝０（−Ｘ１≦Ｘ≦Ｘ１，−Ｙ１≦Ｙ≦Ｙ１）と定義できる。
第２形状関数Ｆ２を路面情報Ｉとフィッティングさせる際には、上記の第２形状関数Ｆ２の法線ベクトルの値、及び／又は、第２形状関数Ｆ２が通る座標点を適宜変更して、任意の第２形状関数Ｆ２を生成できる。

制御部１５は、路面状態判定部１５６を有する。路面状態判定部１５６は、路面形状判定部１５５にて推定された表面形状に基づき、路面ＲＳの路面状態を判定する。具体的には、路面状態判定部１５６は、第１形状関数Ｆ１と第２形状関数Ｆ２とを用いたフィッティングの結果、区画領域に含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２が存在せず、表面形状関数Ｆを決定できなかった場合に、当該区画領域には路面ＲＳ以外の障害物が存在すると判定する。
その他、例えば、路面ＲＳの斜面を表す第２形状関数Ｆ２において、第２形状関数Ｆ２の法線ベクトルがＸ軸及び／又はＹ軸に平行に近いベクトルとして算出され（例えば、法線ベクトルのＸ軸方向成分（ｃ値）、及び／又は、法線ベクトルのＹ軸方向成分（ｄ値）の絶対値が、法線ベクトルのＺ軸方向成分の絶対値（ｅ値）よりもはるかに大きい場合）、Ｚ座標値の値が極端に大きい路面情報Ｉが含まれていた場合にも、当該区画領域には障害物が存在すると判定してもよい。

また、路面状態判定部１５６は、各区画領域において決定された表面形状関数Ｆを参照し、本体１１が移動しようとしている方向の区画領域に、本体１１が通過することができないほどの段差や斜面がある場合に、本体１１が通過できない路面ＲＳが存在すると判定してもよい。
例えば、第１形状関数Ｆ１にて表される表面形状関数Ｆにおいて、３つの面のうち、基準平面ＳＦから極端に高さが高い又は低い面が存在する場合に、本体１１が通過できないほどの段差が路面ＲＳに存在すると判定できる。または、第２形状関数Ｆ２にて表される表面形状関数Ｆおいて、法線ベクトルのＸ軸方向成分（ｃ値）、及び／又は、Ｙ軸方向成分（ｄ値）が、Ｚ軸方向成分（ｅ値）よりもはるかに大きい場合に、基準平面ＳＦに対して大きな角度をなす斜面が路面ＲＳに存在すると判定できる。

本体１１が移動しようとする方向に障害物、及び／又は、本体１１が通過できない路面ＲＳが存在すると判定した場合、路面状態判定部１５６は、例えば、移動体システム１に備わるディスプレイ（例えば、ＧＰＳ装置の地図表示画面の一部）に当該判定結果を表示する異常警報を出力する。その他、ランプなどを点灯させる異常警報を移動体システム１に出力してもよい。これにより、移動体システム１の運転者は、本体１１の近傍に障害物や、本体１１が通過できない路面ＲＳが存在することを視覚的に認識できる。
また、路面状態判定部１５６は、上記の異常警報を後述する移動制御部１５７にも出力する。

制御部１５は、移動制御部１５７を有する。移動制御部１５７は、路面状態判定部１５６から上記の異常警報を入力したときに、ブレーキの駆動機構、駆動部１３の駆動機構、及び／又はハンドルの駆動機構などを制御する移動制御信号を出力し、必要に応じて移動体システム１の運転者に代わって、移動体システム１を制御する。
例えば、移動制御部１５７は、異常警報を入力したときに、障害物や通過できない路面ＲＳが存在する区画領域を避ける方向に本体１１を移動させるか、又は、当該区画領域の手前で本体１１を停止させる移動制御信号を出力する。
これにより、移動体システム１は、障害物への衝突や、通過できない路面ＲＳを通過することにより移動体システム１（本体１１）が損傷することを回避できる。

上記の構成を有することにより、制御部１５は、基準平面ＳＦ全体にて取得された全ての路面情報Ｉを用いて路面ＲＳ全体の形状を判定する場合と比較して、より少ない計算量にて適切に路面形状を把握できる。その結果、路面形状の算出のための処理速度を向上でき、例えば、本体１１が回避すべき領域を通過してしまう前に、路面形状を適切に把握して、当該領域を回避する指令を移動体システム１に出力できる。

（４）路面形状測定装置の動作
（４−１）路面形状測定装置の全体的な動作
以下、移動体システム１に備わった路面形状測定装置１００の動作について説明する。まず、路面形状測定装置１００の全体的な動作について、図６を用いて説明する。図６は、路面形状測定装置の全体的な動作を示すフローチャートである。
最初に、制御部１５は、本体１１が移動している路面ＲＳの形状を判定する（ステップＳ１）。具体的には、制御部１５は、基準平面ＳＦにおいて予め決められた複数の区画領域毎に表面形状関数Ｆを決定することにより、路面ＲＳの一部又は全部の形状を数学的モデルとして判定する。ステップＳ１における路面ＲＳの形状の判定処理については、後ほど詳しく説明する。

路面ＲＳの形状を判定後、路面状態判定部１５６は、本体１１が移動しようとしている路面ＲＳの状態を判定する（ステップＳ２）。
ステップＳ１において判定した各区画領域の表面形状関数Ｆを参照し、本体１１が移動しようとしている方向の区画領域に、障害物、大きな段差、及び／又は急峻な斜面を表す表面形状関数Ｆが存在する、すなわち、本体１１が移動しようとしている方向に回避すべき障害物や路面が存在する場合（ステップＳ２において「Ｙｅｓ」の場合）、路面状態判定部１５６は、上記の異常警報を出力する（ステップＳ３）。

異常警報が出力されることにより、例えば、移動体システム１に備わるディスプレイなどに、本体１１が移動しようとしている方向に回避すべき障害物や路面があることを表示できる。その結果、回避すべき障害物や路面の存在を、移動体システム１の運転者に視覚的に通知できる。
異常警報が出力されると、移動制御部１５７が、必要に応じて、ブレーキの駆動機構、駆動部１３の駆動機構、及び／又はハンドルの駆動機構などを制御する移動制御信号を出力し、本体１１が障害物に衝突しないよう、及び／又は、回避すべき路面を通過しないよう、移動体システム１の移動を制御する。

一方、本体１１が移動しようとしている方向の区画領域に、回避すべき障害物や路面が存在しないと判定した場合（ステップＳ２において「Ｎｏ」の場合）、路面状態判定部１５６は、異常警報を出力しない。これにより、移動体システム１の運転者は、移動しようとしている方向を通過しても問題ないと判断できる。その結果、移動体システム１は、運転者の操作にしたがって移動を継続できる。

上記のステップＳ１〜Ｓ３を実行後、移動体システム１の運転者などにより、路面形状測定装置１００の動作を停止するよう指令されない限り（ステップＳ４において「Ｎｏ」の場合）、プロセスはステップＳ１に戻り、路面形状測定装置１００は動作を継続する。
一方、移動体システム１の運転者などにより、路面形状測定装置１００の動作を停止するよう指令された場合（ステップＳ４において「Ｙｅｓ」の場合）、路面形状測定装置１００は動作を停止する。

上記のステップＳ１〜Ｓ４を実行することにより、本体１１が移動しようとしている方向に回避すべき障害物や路面が存在する場合には、当該障害物や路面の存在について警告することができる。その結果、必要に応じて、例えば、障害物に衝突したり、通過不可能な路面を通過したりしないよう、移動体システム１を制御できる。

（４−２）路面形状判定処理
次に、上記のステップＳ１において実行される路面形状判定処理について、図７を用いて説明する。図７は、路面形状判定処理の流れを示すフローチャートである。
路面形状判定処理を開始すると、まず、路面情報取得部１４’が、路面ＲＳの形状を判定するために用いる路面情報Ｉを取得する。具体的には、以下のようにして路面情報Ｉを取得する。
距離画像取得部１５２が、まず、上記にて説明した方法により、ｎ個の第１位置情報（画素）の集合体（ｘ１、ｙ１、ｄ１）、（ｘ２、ｙ２、ｄ２）、・・・（ｘｎ、ｙｎ、ｄｎ）を、距離画像Ｄ１として取得して、記憶部１５１に記憶する（ステップＳ１１）。

距離画像Ｄ１を取得後、座標変換部１５３が、距離画像Ｄ１に含まれるｎ個の第１位置情報を、本体１１が移動する実空間座標系（Ｘ−Ｙ−Ｚ座標系）の座標値に変換することにより、路面情報Ｉを算出する（ステップＳ１２）。以下、第１位置情報（ｘｐ，ｙｐ，ｄｐ）を例にとって、第１位置情報を実空間座標系の座標値に変換する具体的な方法について説明する。
座標変換部１５３は、まず、位置決定部１５４から、本体１１の基準平面ＳＦ上の位置を取得する。座標変換部１５３は、物体検出センサ１４の本体１１への取付位置を、本体１１の基準平面ＳＦ上の位置に加味して、基準平面ＳＦにおける物体検出センサ１４の位置を算出する。例えば、基準平面ＳＦにおける物体検出センサ１４の位置が（Ｘｓ１，Ｙｓ１）と算出されたとする。

次に、座標変換部１５３は、第１位置情報（ｘｐ，ｙｐ，ｄｐ）の第１座標の座標値（ｘｐ，ｙｐ）を、基準平面ＳＦ上の位置（座標値）に対応付ける。第１座標の座標値（ｘｐ，ｙｐ）に対応する基準平面ＳＦの物体検出センサ１４に対する相対的な位置（言い換えると、基準平面ＳＦの原点を物体検出センサ１４とした場合の、第１座標の所定の座標値に対応する基準平面ＳＦ上の位置（座標値））は、第１座標の座標値（ｘｐ，ｙｐ）に対応する検出部の検出面ＤＳにおける配置位置に基づいて、キャリブレーションにより予め対応付けられている。
例えば、座標値（ｘｐ，ｙｐ）に対応する基準平面ＳＦの相対的な位置が、座標値（Ｘｐ，Ｙｐ）に対応付けられているとする。

その後、座標変換部１５３は、位置決定部１５４にて決定された物体検出センサ１４の基準平面ＳＦでの位置（Ｘｓ１，Ｙｓ１）に、上記の座標値（Ｘｐ，Ｙｐ）を加算することにより、第１座標の座標値（ｘｐ，ｙｐ）に対応する基準平面ＳＦ上の位置を、路面位置（Ｘｐ＋Ｘｓ１，Ｙｐ＋Ｙｓ１）として算出できる。

次に、座標変換部１５３は、路面位置（Ｘｐ＋Ｘｓ１，Ｙｐ＋Ｙｓ１）に存在する路面ＲＳの基準平面ＳＦからの高さである路面高さ（Ｚ座標値）を、以下のようにして、距離画像Ｄ１の画素値である距離ｄｐを用いて算出できる。
まず、路面位置（Ｘｐ＋Ｘｓ１，Ｙｐ＋Ｙｓ１）に存在する路面ＲＳと検出部（物体検出センサ１４）との間の距離がｄｐであるので、座標変換部１５３は、当該路面ＲＳからみた検出部（物体検出センサ１４）の高さを、ｓｑｒｔ｛ｄｐ^２−（Ｘｐ^２＋Ｙｐ^２）｝（ｓｑｒｔ｛｝：｛｝内の数値の平方根）と算出できる。

次に、座標変換部１５３は、上記のように算出された高さと、物体検出センサ１４とその直下の路面ＲＳとの間の高さＨと、路面位置（Ｘｓ１，Ｙｓ１）における路面ＲＳの路面高さＺｓ１とから、路面位置（Ｘｐ＋Ｘｓ１，Ｙｐ＋Ｙｓ１）に存在する路面ＲＳの路面高さＺｐを、Ｚｐ＝Ｚｓ１＋Ｈ−ｓｑｒｔ｛ｄｐ^２−（Ｘｐ^２＋Ｙｐ^２）｝と算出できる。
なお、高さＨは、物体検出センサ１４の本体１１への取付位置により決まる一定値である。また、本体１１が位置（Ｘｓ１，Ｙｓ１）に到達するまでに当該位置又はその最近傍おける路面情報Ｉは取得済みであるため、路面位置（Ｘｓ１，Ｙｓ１）における路面高さＺｓ１は、上記の路面高さＺｐの算出時点では既知となっている。

上記のようにして、１つの第１位置情報（距離画像Ｄ１の１つの画素）に対して、当該第１位置情報（画素）に対応する路面位置と、当該路面位置における路面ＲＳの基準平面ＳＦからの高さである路面高さと、により構成される１つの路面情報を算出できる。
上記の座標変換を距離画像Ｄ１に含まれるｎ個の第１位置情報（ｘ１、ｙ１、ｄ１）〜（ｘｎ、ｙｎ、ｄｎ）のそれぞれに対して実行することにより、座標変換部１５３は、路面ＲＳ上の複数（ｎ個）の点について、複数（ｎ個）の路面情報Ｉ−１〜Ｉ−ｎを算出できる。

路面情報Ｉ−１〜Ｉ−ｎを算出後、路面情報取得部１４’は、前回の路面形状判定処理までに算出された路面情報に、今回算出した路面情報Ｉ−１〜Ｉ−ｎを累積する（ステップＳ１３）。具体的には、路面情報取得部１４’は、前回までに算出した路面情報を累積することにより生成され記憶部１５１に記憶された路面情報データＤ２に、今回算出した路面情報Ｉ−１〜Ｉ−ｎを追加する。これにより、路面情報取得部１４’は、異なる時刻（路面形状判定処理の実行毎）に測定された路面情報Ｉを累積できる。
なお、路面情報Ｉを累積する際には、毎回基準座標を更新するように構成してもよい。この場合は本体１１の移動を考慮して、位置測定部１６から得られる位置変化情報を用いて追加前の路面情報データＤ２に対して補正を行った後に、今回算出した路面情報Ｉ−１〜Ｉ−ｎを追加すればよい。

上記のように、異なる時刻にて取得した路面情報を累積することにより、図８Ａ〜図８Ｃに示すように、本体１１が直進方向（Ｙ軸方向）に移動するに従って、各区画領域Ｇ１〜Ｇ８７に含まれる路面ＲＳの測定点（すなわち、路面情報Ｉ）の個数を増加できる。図８Ａ〜図８Ｃは、路面情報を累積する様子の一例を模式的に示す図である。

路面情報Ｉ−１〜Ｉ−ｎを路面情報データＤ２に累積後、路面形状判定部１５５は、各区画領域Ｇ１〜Ｇ８７に含まれる路面情報Ｉを用いて、区画領域毎に路面ＲＳの表面形状を推定する。区画領域毎に表面形状を推定する際、どの区画領域から表面形状の推定を開始するかは任意に決定できる。例えば、互いに隣接した区画領域の表面形状を連続的になるよう補正することを考慮して、多くの路面情報Ｉを含む区画領域から表面形状を推定できる。これにより、多くの路面情報Ｉを用いてより正確な表面形状をまず推定し、その後算出された表面形状を、最初に推定された正確な表面形状に接続できる。すなわち、後に算出した表面形状を、正確な表面形状に接続できる。
しかし、表面形状を推定する区画領域の順番は、上記に限られず、例えば、区画領域Ｇ１からＧ８７へと順番に推定してもよいし、その逆の順番に推定してもよい。

以下、ｐ番目の区画領域Ｇｐを推定する場合を例にとって、区画領域Ｇｐにおける路面ＲＳの表面形状の推定方法を説明していく。
路面形状判定部１５５は、まず、記憶部１５１に記憶されている路面情報データＤ２を参照し、区画領域Ｇｐに所定の数以上の路面情報Ｉが含まれているか否かを判定する（ステップＳ１４）。

具体的には、路面形状判定部１５５は、例えば、路面情報データＤ２に含まれる路面情報Ｉのうち、Ｘ座標値及びＹ座標値が、それぞれ、ＸＧｐ−Ｗ／２≦Ｘ＜ＸＧｐ＋Ｗ／２、及び、ＹＧｐ−Ｗ／２≦Ｙ≦ＹＧｐ＋Ｗ／２の範囲内にある路面情報Ｉを、区画領域Ｇｐに含まれる路面情報Ｉと判定する。路面形状判定部１５５は、上記のように判定された路面情報Ｉの個数を計数して、計数された路面情報Ｉの個数が所定の数以上となっているか否か判定する。

なお、上記の所定の数は、区画領域Ｇｐの表面形状を確定するのに必要な最低限の路面情報Ｉの個数とすることができる。本実施形態においては、３次元の数学モデル（第１形状関数Ｆ１、第２形状関数Ｆ２）を用いて区画領域Ｇｐの表面形状を推定するので、上記の所定の数としては、３が選択される。

表面形状を推定しようとする対象の区画領域Ｇｐに所定の数よりも少ない路面情報Ｉしか含まれていないと判定された場合（ステップＳ１４において「Ｎｏ」の場合）、路面形状判定部１５５は、区画領域Ｇｐにおいては表面形状を推定しないと決定し、プロセスはステップＳ１８に進む。
一方、区画領域Ｇｐに所定の数以上の路面情報Ｉが含まれていると判定された場合（ステップＳ１４において「Ｙｅｓ」の場合）、区画領域Ｇｐにおいて表面形状を推定すると決定する。

上記のように、区画領域Ｇｐに含まれる路面情報Ｉの個数が所定の数よりも少ない場合には表面形状を推定しないと決定する一方、区画領域Ｇｐに含まれる路面情報Ｉの個数が所定の数以上になったら表面形状を推定すると決定することにより、適切な表面形状を推定できる程度の数の路面情報Ｉが集まったタイミングにて、路面ＲＳの表面形状を適切に推定できる。

区画領域Ｇｐにおいて表面形状を推定すると決定後、路面形状判定部１５５は、さらに、区画領域Ｇｐに含まれる路面情報Ｉの数が、前回、表面形状関数Ｆを決定したときよりも増加しているか否かを判定する（ステップＳ１５）。
区画領域Ｇｐにおいて既に表面形状関数Ｆが表面形状として決定されているが、区画領域Ｇｐに含まれる路面情報Ｉの個数が当該表面形状関数Ｆを推定したときよりも増加していない場合（ステップＳ１５において「Ｎｏ」の場合）、路面形状判定部１５５は、既に決定されている表面形状関数Ｆを更新する必要がないと判定し、プロセスはステップＳ１８に進む。

一方、区画領域Ｇｐにおいて表面形状関数Ｆが決定されていない場合、又は、表面形状関数Ｆが既に決定されているが、区画領域Ｇｐに含まれる路面情報Ｉの個数が当該表面形状関数Ｆを推定したときよりも増加している場合（ステップＳ１５において「Ｙｅｓ」の場合）、路面形状判定部１５５は、表面形状関数Ｆを推定するか、又は、前回推定した表面形状関数Ｆを更新すると決定する（ステップＳ１６）。
既に決定されている表面形状関数Ｆを、当該表面形状関数Ｆを決定するために用いた路面情報Ｉの個数よりも多い個数の路面情報Ｉを用いて更新すると決定することにより、より適切に推定された表面形状を、路面ＲＳの真の表面形状とできる。

ステップＳ１６において、路面形状判定部１５５は、区画領域Ｇｐに含まれる路面情報Ｉに対して、上記の第１形状関数Ｆ１及び第２形状関数Ｆ２をフィッティングさせることにより、当該区画領域Ｇｐの路面ＲＳの形状を判定する。具体的には、図９のフローチャートに示す処理の流れにて、区画領域Ｇｐの路面ＲＳの表面形状を判定する。図９は、表面形状を判定する処理の流れを示すフローチャートである。

まず、路面形状判定部１５５は、区画領域Ｇｐに含まれる路面情報Ｉに対して、第１形状関数Ｆ１をフィッティングさせる（ステップＳ１６１）。例えば、面Ｚ＝ａ（−Ｘ１≦Ｘ≦Ｘ１、０≦Ｙ≦Ｙ１）、及び、面Ｚ＝ｂ（−Ｘ１≦Ｘ≦Ｘ１、−Ｙ１≦Ｙ≦０）については、最小二乗法などにより、区画領域Ｇｐに含まれる路面情報Ｉとの距離が最も小さくなるａ値及び／又はｂ値を算出できる。また、面Ｙ＝０（−Ｘ１≦Ｘ≦Ｘ１、ｂ≦Ｚ≦ａ）については、最小二乗法などにより、区画領域Ｇｐに含まれる路面情報Ｉとの距離が最も小さくなる、面Ｙ＝０をＹ軸方向にシフトした面Ｙ＝ｆ（ｆ：任意の数）を算出できる。
また、上記の３つの面を必要に応じて回転後、最小二乗法などにより、任意の３つの面を決定する。

最小二乗法を用いる以外にも、路面ＲＳ上の各点と表面形状との距離を路面情報Ｉに渡り合計した誤差関数を作成し、最急降下法などにより当該誤差関数を極小化する方法によりフィッティングを行うこともできる。この場合、上記のａ、ｂ、ｆ、回転量がパラメータとなる。

上記のようにして第１形状関数Ｆ１を決定後、路面形状判定部１５５は、決定された第１形状関数Ｆ１と区画領域Ｇｐに含まれる路面情報Ｉとの間の距離の合計を、３次元の面と点との間の距離を算出する公式などを用いて算出する（ステップＳ１６２）。

次に、路面形状判定部１５５は、区画領域Ｇｐに含まれる路面情報Ｉに対して、第２形状関数Ｆ２をフィッティングさせる（ステップＳ１６３）。例えば、第２形状関数Ｆ２を表す面ｃ＊（Ｘ−Ｘ０）＋ｄ＊（Ｙ−Ｙ０）＋ｅ＊（Ｚ−Ｚ０）＝０（−Ｘ１≦Ｘ≦Ｘ１，−Ｙ１≦Ｙ≦Ｙ１）に対して最小二乗法を適用することにより、区画領域Ｇｐに含まれる路面情報Ｉとの距離が最も小さくなる第２形状関数Ｆ２を算出できる。
上記の面ｃ＊（Ｘ−Ｘ０）＋ｄ＊（Ｙ−Ｙ０）＋ｅ＊（Ｚ−Ｚ０）＝０に対する最小二乗法の適用は、例えば、以下のようにして実行できる。

まず、上記の第２形状関数Ｆ２を表す面が、区画領域Ｇｐに含まれる路面情報Ｉの重心を通るとして、第２形状関数Ｆ２をｃ＊（Ｘ−Ｘ０’）＋ｄ＊（Ｙ−Ｙ０’）＋ｅ＊（Ｚ−Ｚ０’）＝０と決定する（座標（Ｘ０’，Ｙ０’，Ｚ０’）：区画領域Ｇｐに含まれる路面情報Ｉの重心）。
次に、上記の第２形状関数Ｆ２と、区画領域Ｇｐに含まれる各路面情報Ｉとの間の距離の二乗の合計が最小となる法線ベクトル（ｃ，ｄ，ｅ）の各要素の値を、ラグランジェの未定乗数法などを用いて算出することにより、最終的な第２形状関数Ｆ２を決定できる。なお、最小二乗法により第２形状関数Ｆ２を決定する際に、計算を簡単にするために、法線ベクトル（ｃ，ｄ，ｅ）を単位ベクトル（長さが１のベクトル）と仮定してもよい。

上記のようにして第２形状関数Ｆ２を決定後、路面形状判定部１５５は、決定された第２形状関数Ｆ２と区画領域Ｇｐに含まれる路面情報Ｉとの間の距離の合計を、３次元の面と点との間の距離を算出する公式などを用いて算出する（ステップＳ１６４）。

なお、上記のステップＳ１６１及びＳ１６３において最小二乗法により第１形状関数Ｆ１及び第２形状関数Ｆ２を決定する場合、区画領域Ｇｐに含まれる路面情報Ｉのうち、取得された時刻がより新しい路面情報Ｉに重点をおいて、第１形状関数Ｆ１及び第２形状関数Ｆ２を決定してもよい。
例えば、新しい路面情報Ｉの個数が、古い路面情報Ｉの個数よりも多くなるよう選択された複数の路面情報Ｉを用いて、第１形状関数Ｆ１及び第２形状関数Ｆ２を決定してもよい。これにより、物体検出センサ１４が区画領域Ｇｐにより接近して取得された信頼性の高い路面情報Ｉを用いて、より適切な区画領域Ｇｐの路面ＲＳの表面形状を推定できる。

その後、路面形状判定部１５５は、決定した第１形状関数と路面情報Ｉとの間の距離の合計、及び、第２形状関数と路面情報Ｉとの間の距離の合計のいずれもが、閾値よりも大きいか否かを判定する（ステップＳ１６５）。
既に決定された表面形状関数Ｆが存在せず、かつ、上記の２つの距離の合計がいずれも閾値より大きい場合（ステップＳ１６５において「Ｙｅｓ」の場合）、路面形状判定部１５５は、区画領域Ｇｐには第１形状関数Ｆ１にも第２形状関数Ｆ２にもフィッティングしない形状の障害物が存在すると判定し（ステップＳ１６６）、表面形状判定処理を終了する。

一方、上記の２つの距離の合計のいずれか又は両方が閾値以下である場合（ステップＳ１６５において「Ｎｏ」の場合）、路面形状判定部１５５は、第１形状関数Ｆ１の距離の合計が最小である場合（ステップＳ１６７において「第１形状関数」の場合）には第１形状関数Ｆ１を区画領域Ｇｐの表面形状関数Ｆと決定する（ステップＳ１６８）。
一方、第２形状関数Ｆ２の距離の合計が最小である場合（ステップＳ１６７において「第２形状関数」の場合）には、第２形状関数Ｆ２を区画領域Ｇｐの表面形状関数Ｆと決定する（ステップＳ１６９）。

なお、既に表面形状関数Ｆが存在する場合において、当該既に存在する表面形状関数Ｆと区画領域Ｇｐに含まれる路面情報Ｉとの間の距離の合計が最小である場合には、路面形状判定部１５５は、当該既に存在する表面形状関数Ｆを、区画領域Ｇｐの表面形状を表す関数としてそのまま維持する。

上記のステップＳ１６１〜Ｓ１６９を実行することにより、路面形状判定部１５５は、予め決められた第１形状関数Ｆ１又は第２形状関数のうち、区画領域Ｇｐに含まれる路面情報Ｉとより一致する方を、区画領域Ｇｐの路面ＲＳの表面形状を数学的モデルで表した表面形状関数Ｆとして、比較的簡単に決定できる。

上記のようにして表面形状（表面形状関数Ｆ）を決定後、路面形状判定部１５５は、当該決定した表面形状関数Ｆが、区画領域Ｇｐに隣接する区画領域にて決定された表面形状関数Ｆとずれているか否かを判定する（ステップＳ１７）。
例えば、区画領域Ｇｐと隣接する区画領域との境界線において、区画領域Ｇｐにおける表面形状関数Ｆが、当該隣接する区画領域における表面形状関数と大きく異なる場合に、区画領域Ｇｐにて決定された表面形状関数Ｆと、区画領域Ｇｐに隣接する区画領域にて決定された表面形状関数とが不連続であると判定する。

区画領域Ｇｐにて決定された表面形状関数Ｆと、区画領域Ｇｐに隣接する区画領域にて決定された表面形状関数とが不連続である場合（ステップＳ１７において「Ｙｅｓ」の場合）、路面形状判定部１５５は、区画領域Ｇｐにて決定された表面形状関数Ｆが、隣接する区画領域にて決定された表面形状関数と連続的に接続するよう、区画領域Ｇｐの表面形状関数Ｆを補正する（ステップＳ１８）。
具体的には、例えば、区画領域Ｇｐの境界線において、当該境界線を介して隣接している区画領域の表面形状関数が当該境界線上において取りうるいくつかの座標点を設定し、当該設定した座標点を必ず通るように、区画領域Ｇｐにおいて表面形状関数Ｆをフィッティングにより決定し直す。路面形状判定部１５５は、当該決定し直した表面形状関数Ｆを、区画領域Ｇｐの路面ＲＳの表面形状を表すものと決定し、記憶部１５１に記憶する。
上記のようにして区画領域Ｇｐにおける表面形状関数Ｆを補正することにより、互いに隣接し合った区画領域において推定された複数の表面形状の間に矛盾が生じた場合に、当該矛盾を解消して、各区画領域において適切な表面形状を推定できる。

一方、区画領域Ｇｐにて決定された表面形状関数Ｆが、区画領域Ｇｐに隣接する区画領域にて決定された表面形状関数に連続的に接続されている場合（ステップＳ１７において「Ｎｏ」の場合）、路面形状判定部１５５は、今回決定した表面形状関数Ｆを、区画領域Ｇｐの路面ＲＳの表面形状を表すものと決定し、記憶部１５１に記憶する。

区画領域Ｇｐの表面形状関数Ｆを決定後、表面形状関数Ｆを決定すべき区画領域がまだ存在する場合（ステップＳ１９において「Ｎｏ」の場合）には、路面形状判定プロセスは、ステップＳ１４に戻り、全ての区画領域Ｇ１〜Ｇ８７に対して上記のステップＳ１４〜Ｓ１７を実行し、可能な場合には表面形状関数Ｆを決定する。
一方、区画領域Ｇｐが表面形状関数Ｆを決定すべき最後の区画領域であり、全ての区画領域Ｇ１〜Ｇ８７において上記のステップＳ１４〜Ｓ１７を実行した場合（ステップＳ１９において「Ｙｅｓ」の場合）、路面形状判定プロセスは終了する。

このように、本実施形態に係る路面形状測定装置１００では、各区画領域Ｇ１〜Ｇ８７に含まれる路面情報Ｉを用いて、区画領域Ｇ１〜Ｇ８７ごとにその表面形状（すなわち、路面ＲＳの一部の形状）を推定している。すなわち、狭い領域に含まれる比較的少数の路面情報Ｉを用いて、当該狭い領域の路面ＲＳの表面形状を推定している。狭い領域の表面形状（表面形状関数Ｆ）を少数の路面情報Ｉを用いて推定することにより、基準平面ＳＦの全体といった広い範囲の表面形状を、多くの路面情報Ｉを用いて推定するのと比較して、特に、表面形状関数Ｆを決定するまでの計算量（計算回数）を大幅に減少できる。
すなわち、基準平面ＳＦ全体にて取得された全ての路面情報Ｉを用いて路面全体の形状を判定する場合と比較して、より少ない計算量により適切に路面形状を把握できる。

２．他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
例えば、図７を用いて説明した路面形状判定処理における各処理、及び／又は、図９を用いて説明した表面形状判定処理における各処理の順番や処理は、必要に応じて変更できる。

（Ａ）表面形状関数の補正についての他の実施形態
例えば、上記の第１実施形態の路面形状判定処理においては、ステップＳ１６にて一旦表面形状関数Ｆを決定後に、当該表面形状関数Ｆが隣接する区画領域における表面形状関数と不連続となる場合に、表面形状関数Ｆの補正がなされていた。
区画領域Ｇｐの表面形状関数Ｆが、区画領域Ｇｐと隣接している区画領域にて決定された表面形状関数と境界線において連続的に接続されればよいので、表面形状関数Ｆの補正は、第１実施形態にて説明した方法に限られない。

例えば、区画領域Ｇｐに含まれる路面情報Ｉを第１形状関数Ｆ１及び第２形状関数Ｆ２にフィッティングさせる際（第１実施形態のステップＳ１６１及びＳ１６３の実行時）に、区画領域Ｇｐに含まれる路面情報Ｉに加えて、区画領域Ｇｐに隣接する区画領域にて決定された表面形状関数が、区画領域Ｇｐと当該隣接する区画領域との境界線において取りうる座標値も含めて、第１形状関数Ｆ１及び第２形状関数Ｆ２とのフィッティングを実行することにより、隣接する区画領域にて決定された表面形状関数と連続的に接続する表面形状関数Ｆを一度のフィッティングにより算出できる。

（Ｂ）路面情報に対して第１形状関数及び第２形状関数をフィッティングさせる方法の他の実施形態
上記の第１実施形態においては、区画領域Ｇｐに含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２を算出する方法として、最小二乗法を用いていた。区画領域Ｇｐに含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２を算出する方法として、最小二乗法以外の他の方法を用いることもできる。
例えば、「ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）」アルゴリズムを用いて、区画領域Ｇｐに含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２を算出できる。

具体的には、以下のステップを実行することにより、区画領域Ｇｐに含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２を算出する。
（ｉ）区画領域Ｇｐに含まれる路面情報Ｉから、第１形状関数Ｆ１及び第２形状関数を決定するために必要な最低限の個数の路面情報Ｉを選択する。
（ｉｉ）（ｉ）にて選択した路面情報Ｉを用いて第１形状関数Ｆ１及び第２形状関数Ｆ２を算出する。
（ｉｉｉ）（ｉ）にて選択する路面情報Ｉを異ならせて（ｉ）及び（ｉｉ）を繰り返して実行して算出された複数の第１形状関数Ｆ１及び第２形状関数Ｆ２のうち、より多くの路面情報Ｉを通る（あるいは、より多くの路面情報Ｉが所定の距離の範囲内に存在する）第１形状関数Ｆ１及び第２形状関数Ｆ２を、最終的な第１形状関数Ｆ１及び第２形状関数Ｆ２とする。

「ＲＡＮＳＡＣ」アルゴリズムを用いる場合、上記の（ｉｉｉ）において第１形状関数Ｆ１及び第２形状関数Ｆ２の両方を決定するのではなく、上記の（ｉ）及び（ｉｉ）を繰り返して算出された第１形状関数Ｆ１及び第２形状関数Ｆ２のうち、最も多くの路面情報Ｉを通る（あるいは、最も多くの路面情報Ｉが所定の距離の範囲内に存在する）第１形状関数Ｆ１又は第２形状関数Ｆ２を、区画領域Ｇｐにおける表面形状関数Ｆと決定してもよい。

また、「ＲＡＮＳＡＣ」アルゴリズムを用いる場合、例えば、第１形状関数Ｆ１又は第２形状関数Ｆ２が１つの路面情報Ｉと一致する（または、１つの路面情報Ｉが所定の範囲内にある）ときに与えるスコアを当該１つの路面情報Ｉの新しさ（取得時刻）に応じて設定し、区画領域Ｇｐに含まれる各路面情報Ｉに対して算出される当該スコアの合計を路面情報Ｉと第１形状関数Ｆ１又は第２形状関数Ｆ２との一致度と定義してもよい。この場合、最大の一致度を算出した第１形状関数Ｆ１又は第２形状関数Ｆ２を、区画領域Ｇｐの表面形状関数Ｆと決定してもよい。

ここで、例えば、図１０Ａに示すように、古い路面情報（黒点にて示す路面情報）及び新しい路面情報（白点にて示す路面情報）が混在する区画領域Ｇｐを考える。このような区画領域Ｇｐにおいて、図１０Ｂの点線にて示した第２形状関数Ｆ２よりも、図１０Ｂの実線にて示した第１形状関数Ｆ１の方がより多くの新しい路面情報Ｉと一致する。すなわち、図１０Ｂの点線にて示した第２形状関数Ｆ２の一致度よりも、図１０Ｂの実線にて示した第１形状関数Ｆ１の一致度の方が大きくなる。図１０Ａ及び図１０Ｂは、新しい路面情報と古い路面情報が混在した場合の、ＲＡＮＳＡＣを用いた表面形状関数の決定方法の一例を示す図である。

上記のように、一致度が大きい方の第１形状関数Ｆ１又は第２形状関数Ｆ２を区画領域Ｇｐの表面形状関数Ｆと決定することにより、信頼性の高いより新しい路面情報Ｉと一致する（または、所定の範囲内にある新しい路面情報Ｉの個数が多い）表面形状関数Ｆを算出できる。

区画領域Ｇｐに含まれる路面情報Ｉと一致する第１形状関数Ｆ１及び第２形状関数Ｆ２を算出する方法として、その他、非線形最適化アルゴリズムを用いることもできる。

本発明は、起伏などを有する路面の表面形状を測定する路面形状測定装置に広く適用できる。

１００ 路面形状測定装置
１ 移動体システム
１１ 本体
１２ａ、１２ｂ、１２ｃ、１２ｄ 車輪
１３ 駆動部
１４ 物体検出センサ
１４１ 出力部
１４３−１〜１４３−ｎ 検出部
１４５ レンズ
ＤＳ 検出面
１４' 路面情報取得部
１５ 制御部
１５１ 記憶部
１５２ 距離画像取得部
１５３ 座標変換部
１５４ 位置決定部
１５５ 路面形状判定部
１５６ 路面状態判定部
１５７ 移動制御部
１６ 位置測定部
Ｄ１ 距離画像
Ｄ２ 路面情報データ
Ｆ 表面形状関数
Ｆ１ 第１形状関数
Ｆ２ 第２形状関数
Ｇ１〜Ｇ８７ 区画領域
ＧＩ 区画領域情報
Ｉ、Ｉ−１〜Ｉ−ｎ 路面情報
Ｌｍ 測定光
Ｌｒ 反射光
ＲＳ 路面
ＳＦ 基準平面
ｄ、ｄ１、ｄｎ距離

Claims (13)

1. 起伏を有する路面の形状を測定する路面形状測定装置であって、
   前記路面上の複数の点について、複数の区画領域に分けられた基準平面上における前記路面の路面位置と、前記路面位置における前記路面の前記基準平面からの高さを表す路面高さと、からなる路面情報を取得する路面情報取得部と、
   前記区画領域ごとに、当該区画領域に含まれる前記路面情報を用いて表面形状を推定することにより、前記路面の一部または全体の形状を判定する路面形状判定部と、
   を備える路面形状測定装置。
2. 前記路面情報取得部は、
   前記路面にて反射したエネルギーを測定するエネルギー測定部と、
   それぞれが前記エネルギーに基づいて測定される前記路面と前記路面情報取得部との間の距離を画素値として有する複数の画素で構成された距離画像を取得する距離画像取得部と、
   前記距離画像の各画素を前記基準平面の位置に対応付けて前記路面位置を算出し、当該路面位置における前記路面高さを当該各画素が有する画素値に基づいて算出することにより、前記路面情報を算出する座標変換部と、
   を有する請求項１の路面形状測定装置。
3. 前記区画領域に所定の数以上の前記路面情報が存在していれば、前記路面形状判定部は、当該区画領域の表面形状を推定することを決定し、
   前記区画領域に前記所定の数以上の前記路面情報が存在していなければ、前記路面形状判定部は、当該区画領域の表面形状を推定しないことを決定する、請求項１又は２に記載の路面形状測定装置。
4. 前記路面形状判定部は、前記表面形状を推定しようとしている前記区画領域に含まれる複数の前記路面情報の数が前回の前記表面形状の推定時よりも増加していれば、前回推定した前記表面形状を、当該数が増加した前記路面情報を用いて更新する、請求項１〜３のいずれかに記載の路面形状測定装置。
5. 前記路面形状判定部は、１つの区画領域に対して推定された前記表面形状と、当該１つの区画領域に隣接する区画領域に対して推定された前記表面形状とが、前記基準平面内で不連続となれば、当該１つの区画領域に対して推定された前記表面形状を、当該隣接する区画領域に対して推定された前記表面形状と連続的に接続するよう補正する、
   請求項１〜４のいずれかに記載の路面形状測定装置。
6. 前記区画領域の大きさは、各々、前記基準平面における前記路面情報取得部と当該区画領域との間の距離に基づいて決定される、請求項１〜５のいずれかに記載の路面形状測定装置。
7. 前記路面情報取得部は、前記基準平面上を移動し、移動中の異なる時刻において取得された前記路面情報を累積し、
   前記路面形状判定部は、当該累積された前記路面情報を用いて前記表面形状を推定する、請求項１〜６のいずれかに記載の路面形状測定装置。
8. 前記路面形状判定部は、前記区画領域に含まれる前記路面情報に対して所定の関数をフィッティングすることにより算出される表面形状関数を、当該区画領域における前記表面形状と推定する、
   請求項１〜７のいずれかに記載の路面形状測定装置。
9. 前記路面形状判定部は、取得された時刻がより新しい前記路面情報と一致する表面形状関数を、当該表面形状関数を算出中の区画領域の前記表面形状を表すものと推定する、請求項８に記載の路面形状測定装置。
10. 前記所定の関数は、前記路面の段差を表す第１形状関数と、前記路面における斜面を表す第２形状関数と、を含み、
    前記路面形状判定部は、前記第１形状関数と前記第２形状関数のうち、前記区画領域に含まれる前記路面情報とより一致する方を、前記表面形状関数と推定する、
    請求項８又は９に記載の路面形状測定装置。
11. 前記路面形状判定部にて推定された前記表面形状に基づき前記路面の路面状態を判定する路面状態判定部をさらに備える、請求項１〜１０のいずれかに記載の路面形状測定装置。
12. 起伏を有する路面の形状を測定する測定方法であって、
    前記路面上の複数の点について、複数の区画領域に分けられた基準平面上における前記路面の路面位置と、前記路面位置における前記路面の前記基準平面からの高さを表す路面高さと、からなる路面情報を取得するステップと、
    前記区画領域ごとに、当該区画領域に含まれる前記路面情報を用いて表面形状を推定することにより、前記路面の一部または全体の形状を判定するステップと、
    を含む測定方法。
13. 請求項１２に記載の前記測定方法を、コンピュータに実行させるプログラム。

Images