JP6671152B2 - 自己位置推定装置の異常検知装置及び車両 - Google Patents

Description

本発明は、車両に搭載された自己位置推定装置の異常を検知する技術に関する。

ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）信号を用いた自己位置は、マルチパス等の外乱要因によって誤差が生じる場合がある。そこで、自己位置推定処理に発生した異常を検知するための技術の一例として、特許文献１には「ＧＰＳ信号を受信して車両位置を測位するＧＰＳ受信器と、車両の走行方向及び距離に基づいて車両位置及び方位を測位する自律航法演算器と、ＧＰＳ受信器及び自律航法演算器のそれぞれの測位結果に基づき現在位置及び方位を演算する位置計測部と、予め設定された走行経路と演算された現在位置及び方位との比較結果に基づき車両走行を制御する走行制御部とを備えた無人車両の走行制御装置において、車両から走行経路の脇に設けた路側帯までの距離を計測する路側帯距離計測器を設け、前記位置計測部は、路側帯距離計測器により計測した路側帯距離に基づき、ＧＰＳ受信器の測位位置、及び／又は、自律航法演算器の測位位置と方位との少なくとも一方を補正して現在位置と方位を求める（要約抜粋）」、「管制制御部から走行経路に沿った地形情報を入力し、入力した地形情報と路側帯距離計測器からの測位データとの差異情報に基づきデータ異常判定を行う（特許文献１の明細書段落００２１から抜粋）」ことが開示されている。

特開２００２−２１５２３６号公報

特許文献１は、走行経路に沿った地形情報を用いてＧＰＳによる測位データの異常検出を行うため、地形情報を事前に取得し記録しておく必要がある。従って、地形形状がわからない場所を走行する際には特許文献１記載発明ではＧＰＳによる測位データの異常検出が行えないという課題がある。特に鉱山では路肩を構成する土盛りの崩れや切羽などに起因して地形が変化しやすい。そこで鉱山用のダンプトラックにおいては、地形の変化に対して高い追従性で自己位置推定処理の異常が検知できる技術に対するニーズは高い。

本発明は上記課題に鑑みてなされたものであり、事前に地形情報を取得しなくとも、車両に搭載した自己位置推定装置の異常検知が行える技術を提供することを目的とする。

上記課題を解決するために、例えば特許請求の範囲に記載の構成を採用する。本願は上記課題を解決する手段を複数含んでいるが、その一例を挙げるならば、車両に搭載され、外部座標系で定義された自己位置を推定する自己位置推定装置の異常検知装置において、前記車両が走行する路面の周辺構造物上にある進行方向に沿って並ぶ特徴点群の内の各特徴点及び自車両の相対位置を計測する第一計測装置からの出力に基づいて、前記第一計測装置の座標系で表された前記各特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第一座標を算出する第一特徴点座標算出部と、前記第一座標を基に、前記特徴点群の前記外部座標系で定義された軌跡を生成する特徴点軌跡生成部と、前記周辺構造物の前記特徴点群の内、前記第一計測装置の計測時刻において前記第一計測装置が計測した特徴点よりも進行方向後方に位置する特徴点及び自車両の相対位置を計測する第二計測装置からの出力に基づいて、前記第二計測装置の座標系で表された前記進行方向後方に位置する特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第二座標を算出する第二特徴点座標算出部と、前記特徴点群の前記外部座標系で定義された軌跡及び前記第二座標の偏差が、前記軌跡上に前記第二座標があると見做すために定められた閾値を上回ると、前記自己位置推定装置に異常があると判定する異常判定部と、を備えることを特徴とする。

本発明により、事前に地形情報を取得しなくとも、車両に搭載した自己位置推定装置の異常検知が行える技術を提供することができる。上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本実施形態に係る異常検知装置を搭載したダンプトラックが稼働する鉱山用作業機械の自律走行システムの概略構成図 ダンプトラックの構成を示すブロック図 前方ライダー及び後方ライダーの設置位置を示す概略平面図であって、（a）はダンプトラックの前方に前方ライダー及び後方ライダーを設置した状態を示し、（ｂ）はダンプトラックの前方に前方ライダーを、ダンプトラックの後方に後方ライダーを設置した状態を示す。 ダンプトラックの走行環境と周辺構造物の特徴点検出例を示す説明図であり、（ａ）は走行環境を示し、（ｂ）は前方ライダーの出力から特徴点を検出する処理例を示す。 自己位置推定装置の異常検知処理の説明図 異常検知装置の動作の流れを示すフローチャート 路肩特徴点Ｐａ’の軌跡の算出例を示す説明図 路肩特徴点Ｐａ’の軌跡と路肩特徴点Ｐｂ’との比較処理を示す説明図 第２実施形態に係る前方ライダー及び後方ライダーの各スキャン面を示す概略斜視図 第２実施形態に係る前方ライダー及び後方ライダーの各スキャン面を示す概略平面図

以下、本発明の実施形態について図面を参照しながら説明する。図１は、本実施形態に係る異常検知装置を搭載したダンプトラックが稼働する鉱山用作業機械の自律走行システムの概略構成図である。

＜第１実施形態＞
図１に示す自律走行システム１は、自律走行する鉱山用作業機械の一例としてのダンプトラック１０と、採石場の近傍若しくは遠隔の管制センタに設置された管制サーバ２０とを無線通信回線４を介して通信接続して構成される。鉱山内には、ショベル３０が掘削作業を行いダンプトラック１０に掘削した土砂や鉱石を積込む積込場６１と、ダンプトラック１０が運搬した荷物を放土する放土場６２と、積込場６１及び放土場６２を連結する搬送路６０とが設置される。ダンプトラック１０は、無線通信回線４を介して管制サーバ２０から管制制御情報を受信し、それに従って搬送路６０上を予め定められた走行速度で自律走行する。

以下の説明ではダンプトラック１０に本発明に係る異常検知装置を搭載し、搬送路６０の周辺構造物として路肩を用いて異常検知を行う例について説明するが、車両例や周辺構造物例はダンプトラックや路肩に限定されない。本実施形態は、ＧＰＳ測位データや慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）等の慣性データを用いて外部座標系による自己位置を推定する自己位置推定装置を搭載する鉱山用作業機械であれば、その種類は問わない。例えばショベル３０やドーザ７１、散水車７２のように、ＧＰＳ受信機を搭載して自己位置情報を受信する作業機械に本発明を適用してもよい。この場合、積込場６１では積込場６１の周囲に存在する切羽や崖を周辺構造物として用いてもよい。

更に鉱山用作業機械に限らず、公道や私道などの一般道を通行する乗用車に本発明を適用してもよい。この場合、周辺構造物は道路沿いのカードレールや道路脇のビルでもよい。またトンネル内では、周辺構造物としてトンネル内の側壁を用いてもよい。

図２はダンプトラックの構成を示すブロック図である。図３は前方ライダー及び後方ライダーの取付位置を示す概略平面図であって、（a）は前方ライダー及び後方ライダーを共にダンプトラックの前方に設置した状態を示し、（ｂ）は前方ライダーがダンプトラックの前方に、後方ライダーはダンプトラックの後方に設置した状態を示す。

ダンプトラック１０は、外部座標系（グローバル座標系）による自己位置を推定する自己位置推定装置１１と、推定した自己位置を用いてダンプトラック１０に自律走行をさせるための車両駆動装置に対し制御指令を出力する車両制御装置１２と、ダンプトラック１０と管制サーバ２０との間の通信を行う通信装置１３とを備える。

自己位置推定装置１１は、ＧＰＳアンテナ１４から受信したＧＰＳ測位データを基に外部座標系（グローバル座標系）の自己位置を算出する。自己位置推定装置１１は、ＩＭＵ１５からの出力、前輪車軸の傾き（操舵角）を検知する操舵角センサ１６からの出力、及びダンプトラック１０の車輪（従動輪）の回転数に基づく車速を検知する車速センサ１７からの出力を用いて、ＧＰＳ由来の自己位置情報を補正し、より高精度な自己位置推定処理を行う。自己位置情報は車両制御装置１２に出力され自律走行制御に用いられたり、通信装置１３に出力され管制サーバ２０に送信されたりする。管制サーバ２０では、受信した自己位置情報を交通管制処理に用いる。

また、ダンプトラック１０の走行経路の周辺構造物とダンプトラック１０との相対位置を検出するセンサとして二つのライダーを備える。図３（ａ）の例では、ダンプトラック１０の進行方向前方を検出する前方ライダー２ａ（第一計測装置に相当する）が前方左側に設置される。前方ライダー２ａから照射するレーザ光が到達する路面Ａ上の計測点がなす直線、すなわち前方ライダー２ａのスキャン面と路面Ａとが交線Ｌ１ａ（第一路面走査線に相当する）である。

ダンプトラック１０の前方右側に、後方ライダー２ｂ（第二計測装置に相当する）が備えられる。後方ライダー２ｂから照射するレーザ光が到達する路面Ａ上の計測点がなす直線、すなわち後方ライダー２ｂのスキャン面と路面Ａとが交線Ｌ１ｂ（第二路面走査線）である。前方ライダー２ａ及び後方ライダー２ｂは交線Ｌ１ａ、Ｌ１ｂが互いに平行となる位置及び取付角度でダンプトラック１０に設置される。

前方ライダー２ａ及び後方ライダー２ｂのそれぞれは、レーザ光の照射方向を予め定めた所定の角度、例えば０．２５度毎に徐々に変化させて路面Ａ上の計測点を走査していき、これら前方ライダー２ａ及び後方ライダー２ｂの各スキャン面において、所定の角度毎の路面Ａまでの距離を計測する。前方ライダー２ａ及び後方ライダー２ｂのそれぞれは、例えば０．２５度の角度分解能を有し、３０ｍ離れた地点での計測点間の分解能が１ｍである。

また、図３（ｂ）に示すように、前方ライダー２ａはダンプトラック１０の前方、後方ライダー２ｂはダンプトラック１０の後方に取り付け、前方ライダー２ａのスキャン面が前方、後方ライダー２ｂのスキャン面が後方を向く取付角度でダンプトラック１０に設置してもよい。このように、前方ライダー２ａ及び後方ライダー２ｂの設置位置は二つのライダーのスキャン面がダンプトラック１０の進行方向の前後方向にずれていればよく、図３の例には限定されない。

車両制御装置１２は、例えば制動装置、駆動輪に対する回転トルク指令値を制限するための駆動トルク制限装置、及びダンプトラック１０の操舵角を変更する操舵制御装置（いずれも不図示）を含む車両駆動装置に対する制御指令信号を出力する。

更にダンプトラック１０は、前方ライダー２ａ、後方ライダー２ｂの出力を用いて自己位置推定装置１１が算出した自己位置に異常がないかを検知する異常検知装置１８を備える。

異常検知装置１８は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等の演算・制御装置の他、異常検知装置１８で実行されるプログラムを格納するＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）やＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の記憶装置、また、ＣＰＵがプログラムを実行する際の作業領域となるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を含むハードウェアと、第一路肩検出部１８１、路肩軌跡生成部１８２、第二路肩検出部１８３、及び異常判定部１８４の各機能を実現するソフトウェアとが協働することにより構成される。また異常検知装置１８の記憶装置の一部領域は、第一路肩検出部１８１が検出した外部座標系で定義された路肩点情報を格納する第一路肩記憶部１８５、路肩軌跡生成部１８２が生成した路肩軌跡情報を格納する路肩軌跡記憶部１８６を構成する。上記第一路肩検出部１８１、路肩軌跡生成部１８２、第二路肩検出部１８３、及び異常判定部１８４が実行する処理の詳細は後述する。

図４は、ダンプトラックの走行環境と周辺構造物の特徴点検出例を示す説明図であり、（ａ）は走行環境を示し、（ｂ）は前方ライダーの出力から特徴点を検出する処理例を示す。

ダンプトラック１０は図４（ａ）に示すように、鉱山に予め設けられた走行経路等の路面Ａを自律運転で走行する。鉱山の路面Ａの側部には、路面Ａに沿って路肩Ｂが設けられている。路肩Ｂは、少なくともダンプトラック１０が走行する側、例えば進行方向左側に設けられ、所定の高さ寸法および幅寸法を有する構造の盛土であり、ダンプトラック１０の走行位置から例えば３０ｍほど離れている。本実施形態では路肩Ｂをダンプトラック１０の走行経路の周辺に位置する周辺構造物として用いる。そして路肩Ｂのうち、路面Ａと路肩Ｂとの接合部を周辺構造物の特徴点Ｐとして用いる。

第一路肩検出部１８１は、第一路面走査線Ｌ１ａ、及び路肩Ｂのダンプトラック１０に対向する斜面からの反射点を結ぶ線、すなわちスキャン面４０ａと斜面との交線からなる第一路肩走査線Ｌ２ａを算出する。

更に、第一路肩検出部１８１は、第一路面走査線Ｌ１ａと第一路肩走査線Ｌ２ａとの交点を路面Ａと路肩Ｂとの接合部からなる路肩特徴点Ｐａとして検出する（図４（ｂ）参照）。なお、ここでの路肩特徴点Ｐａは前方ライダー２ａの相対位置を示すセンサ座標系で定義される。

第二路肩検出部１８３は、後方ライダー２ｂからの出力に基づいて上記と同様の処理を行い、センサ座標系により定義された路肩特徴点Ｐｂ（図７参照）を検出する。

図５を参照して、本実施形態に係る自己位置推定装置１１の異常検知処理の概要について説明する。図５は、自己位置推定装置の異常検知処理の説明図である。

ダンプトラック１０が山壁のそばを走行するときなど、マルチパスの影響でＧＰＳ測位データに大きな誤差が含まれる場合がある。このとき、自己位置推定装置１１は、ＩＭＵ由来の自己位置とＧＰＳ測位位置とを比較し、その差が小さい場合には２つのデータを融合し（ケース１）、差が大きい場合には、ＧＰＳ測位位置を採用しない（ケース２）。これにより、マルチパスの影響で突発的に生じたＧＰＳ測位位置の外れ値を除外する。

一方、図５のケース３のように、ＧＰＳ測位誤差が少しずつ大きくなり、オドメトリで求めた推定位置とＧＰＳ測位位置との偏差が閾値より小さい場合には、自己位置推定装置１１は、誤差を含んだＧＰＳ測位位置による補正を行うこととなり、自己位置推定装置１１から出力される自己位置の真値に対する誤差が大きくなることがある。本実施形態は、ケース３のような場合にも自己位置推定装置１１の異常を検知することに特徴がある。

図６を参照して、本実施形態に係る自己位置推定装置の異常検知装置を用いた異常検知処理について説明する。図６は、異常検知装置の動作の流れを示すフローチャートである。

ダンプトラック１０が起動すると、自己位置推定装置１１はＧＰＳアンテナ１４から受信したＧＰＳ測位データに基づく自己位置を算出し、それに対してＩＭＵ１５からの出力、操舵角センサ１６、及び車速センサ１７を用いて補正を行い、外部座標系で定義された自己位置を算出する（Ｓ０１）。

ダンプトラック１０の前方ライダー２ａにて路面Ａ及び路肩Ｂを測定し、測定データを第一路肩検出部１８１に出力する（Ｓ１１）。

第一路肩検出部１８１は、前方ライダー２ａによる測定データ基づき、路肩特徴点Ｐａを検出する。更に第一路肩検出部１８１は、前方ライダー２ａによる測定データに基づき路肩特徴点Ｐａまでの距離を検出し、ステップ０１で算出したＧＰＳ測位データに基づく自己位置（外部座標系）を用いて下式（１）によりセンサ座標系の路肩特徴点Ｐａの座標値を外部座標系の座標値（路肩特徴点Ｐａ’、第一座標に相当する）に変換する。従って、第一路肩検出部１８１は、第一特徴点座標算出部に相当する。

第一路肩検出部１８１は、路肩特徴点Ｐａ’の座標を第一路肩記憶部１８５に記憶させる（Ｓ１２）。

（外部座標系の路肩特徴点Ｐａ’の座標）＝（前方ライダー２ａのセンサ座標系から車体中心座標系への座標変換行列）（前方ライダー２ａのセンサ座標系による路肩特徴点Ｐａの座標ベクトル）＋（車体中心座標系における前方ライダー２ａの車体への設置位置から自己位置推定装置１１の基準点の位置までの偏差）＋（自己位置推定装置１１が算出した外部座標系で定義された自己位置）・・・（１）
但し
車体中心座標系：車体前後軸と、前後軸に垂直な車体幅方向の左右軸と、前後軸及び左右軸に右手座標系において垂直な垂直軸からなる直交３軸系
前方ライダー２ａのセンサ座標系：レーダの照射角度をｘａ軸と、レーダスキャン面内においてｘａ軸に直交するｙａ軸と、ｘａ軸及びｙａ軸に右手座標系において垂直なｚａ軸

式（１）では説明の便宜のため、自己位置推定装置１１の基準点は車体中心座標系の原点と一致するものとする。自己位置推定装置１１の基準点は車体中心座標系の原点と一致しない場合はその偏差を上記の式（１）に対して更に加減する。

路肩軌跡生成部１８２は、第一路肩記憶部１８５に記憶された路肩特徴点Ｐａ’を読み出し、路肩特徴点Ｐａ’の第一座標を計測した時系列に沿って並べ、第一座標の軌跡を生成し、路肩軌跡記憶部に記憶させる（Ｓ１３）。この軌跡は、外部座標系で定義された路肩特徴点Ｐａ’の点列を示す。

路肩特徴点Ｐａ’の座標は、前方ライダー２ａのサンプリングレート＜ＧＰＳのサンプリングレートの場合、少なくともＧＰＳのサンプリングレート以上の時間間隔を空けて検出された点の座標である。そこで路肩軌跡生成部１８２は、これら路肩特徴点Ｐａ’の点群を時系列によって読込み、各点間を例えばスプライン補間をすることで、外部座標系で定義された路肩特徴点Ｐａ’の軌跡を求める。

図７は、路肩特徴点Ｐａ’の軌跡の算出例を示す説明図である。ダンプトラック１０が進行方向（図７の右から左）に走行しながら、自己位置推定装置１１により自己位置を推定する。その自己位置の軌跡を符号Ｚ＿ｔｒで示す。路肩特徴点Ｐａ’は上記自己位置にダンプトラック１０及び路肩特徴点Ｐａの相対位置を加算したものなので、路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒの形状と自己位置の軌跡を符号Ｚ＿ｔｒの形状とは類似する。

ステップ１１〜ステップ１３と並行して、後方ライダー２ｂにて路面及び路肩の測定を行う（Ｓ２１）。

第二路肩検出部１８３は、後方ライダー２ｂによる測定データ基づき、路肩特徴点Ｐｂを検出し、ステップ０１で算出したＧＰＳ測位データに基づく自己位置（外部座標系）を用いて下式（２）により路肩特徴点Ｐｂの外部座標系における位置を算出する（Ｓ２２）。

（路肩Ｂの外部座標系での位置）＝（後方ライダー２ｂのセンサ座標系による測定結果）×（後方ライダー２ｂのセンサ座標系から車体中心座標系への座標変換行列）＋（後方ライダー２ｂの車体への設置位置からＧＰＳに由来する外部座標系で定義された自車両の算出位置の基準点までの差分）＋（ＧＰＳに由来する外部座標系で定義された自己位置）・・・（２）
但し
後方ライダー２ｂのセンサ座標系：レーダの照射角度をｘｂ軸と、レーダスキャン面内においてｘｂ軸に直交するｙｂ軸と、ｘｂ軸及びｙｂ軸に右手座標系において垂直なｚｂ軸

なお、式（１）と同様、自己位置推定位置の基準点は車体中心座標系の原点と一致するものとする。

第二路肩検出部１８３は、後方ライダー２ｂが計測した後方ライダー２ｂのセンサ座標系で定義された路肩特徴点Ｐｂの座標を外部座標系に変換して路肩特徴点Ｐｂ’（第二座標）を算出するので、第二特徴点座標算出部に相当する。

図７において、路肩特徴点Ｐｂ’も自己位置を使って外部座標系に変換されているので、自己位置が正常に推定されていれば路肩特徴点Ｐｂ’は路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒ上に位置する。そこで次のステップでこの検証を行う。

異常判定部１８４は、路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒと路肩特徴点Ｐｂ’とを比較し、それらの偏差Ｄ（図８参照）が、閾値Ｄ_ｔｈを上回ったと判断すると（Ｓ３１／Ｙｅｓ）、自己位置推定装置１１に異常が発生したとして、車両制御装置１２に対してダンプトラック１０の停止動作を行わせるための停止制御指示信号を出力する（Ｓ３２）。閾値Ｄ_ｔｈは、路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒ上に路肩特徴点Ｐｂ’がのっていると判定するための距離又は座標のズレ量により定義された閾値である。車両制御装置１２は、不図示の制動装置及び駆動トルク制限装置を制御してダンプトラック１０を停止させる。自己位置推定装置１１の異常要因には、ＧＰＳ測位データに対するマルチパスの影響の他、ＩＭＵ１５、操舵角センサ１６、又は車速センサ１７の故障などが考えられる。

異常判定部１８４は、路肩特徴点Ｐａの軌跡と路肩特徴点Ｐｂとの偏差が閾値Ｄ_ｔｈを以下であると判断すると（Ｓ３１／Ｎｏ）、自己位置推定装置１１は正常であると判断し、ステップＳ０１へ戻る。

図８を参照して路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒと路肩特徴点Ｐｂ’との比較処理について説明する。図８は、路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒと路肩特徴点Ｐｂ’との比較処理を示す説明図である。

時刻ｔ_ｎ−３における路肩特徴点Ｐａ’（ｔ＝ｔ_ｎ−３）（図中丸印で図示）は、同時刻における後方の路肩特徴点Ｐｂ’（ｔ＝ｔ_ｎ−３）（図中三角形で図示）よりも前に位置する。自己位置推定装置１１が時刻ｔ_ｎ−３、ｔ_ｎ−２では正常なので路肩特徴点Ｐａ’（ｔ＝ｔ_ｎ−３）、Ｐａ’（ｔ＝ｔ_ｎ−２）を結ぶ軌跡上に後方の路肩特徴点Ｐｂ’（ｔ＝ｔ_ｎ−３）、Ｐｂ’（ｔ＝ｔ_ｎ−２）が位置する。

ところが時刻ｔ_ｎ−１において自己位置推定装置１１から出力される自己位置情報に異常（真値からのズレ）が発生すると、第二路肩検出部１８３は、時刻ｔ_ｎ−１における真値からのズレを含む自己位置を使って後方の路肩特徴点Ｐｂ’を算出する。すなわち、既述の式（２）の最終項において、真値からのズレを含む自己位置をセンサ座標系から外部座標系への変換に用いてしまう。

ここで、路肩特徴点Ｐｂ’（ｔ＝ｔ_ｎ−１）と比較される路肩特徴点Ｐａ’の軌跡Ｐａ’＿ｔｒ上の特徴点は、路肩特徴点Ｐｂ’の計測時刻（ｔ＝ｔ_ｎ−１）よりも前の時刻で計測されている。この前の時刻では自己位置情報が正しいとすると、計測時刻（ｔ＝ｔ_ｎ−１）において自己位置の異常量と同等の位置ズレが、路肩特徴点Ｐｂ’（ｔ＝ｔ_ｎ−１）の軌跡Ｐａ’＿ｔｒからの偏差として観測できる。

また前方ライダー２ａによる前方の路肩特徴点Ｐａ（ｔ＝ｔ_ｎ−１）の外部座標系に変換後の路肩特徴点Ｐａ’（ｔ＝ｔ_ｎ−１）の座標も上記と同様の位置ズレが観測できる。

そこで、異常判定部１８４は、路肩特徴点Ｐｂ’（ｔ＝ｔ_ｎ）の軌跡Ｐａ’＿ｔｒからの偏差Ｄを求め、それが上述の閾値Ｄ_ｔｈを上回ると、自己位置推定装置１１に異常が発生したと判定する。

本実施形態によれば、前方ライダーで周辺構造物の特徴点を検出してセンサ座標系で定義された特徴点座標を算出する。これに対して外部座標系で算出された自己位置を用いて外部座標系への座標変換を行い、軌跡を生成する。そして、軌跡をティーチング座標系列とし、これと後方ライダーが検出した特徴点座標とを比較することで、その偏差が大きければ（正しいと見做さない程度の偏差があれば）、後方ライダーで検出した特徴点を外部座標系に変換する際に用いた自己位置に異常（真値とのズレ）が生じていることがわかる。これにより、事前に走行経路の周辺構造物の形状を計測していなくても、走行中に得られる前方ライダーの計測点を用いて自己位置推定装置の異常発生を検知できる。すなわち、本実施形態によれば、ある時点において式（２）で算出した特徴点の座標と、少し前の時点での式（１）で算出した特徴点の座標（正確には軌跡）とを比較するので、ＧＰＳ測位データに誤差がのりはじめる少し前の時点の比較を行うこととなり、既述の図５のケース３の場合でも誤差の有無を判定することができる。

＜第２実施形態＞
図９は、第２実施形態に係る前方ライダー及び後方ライダーの各スキャン面を示す概略斜視図である。図１０は、第２実施形態に係る前方ライダー及び後方ライダーの各スキャン面を示す概略平面図である。

第２実施形態が第１実施形態と異なる点は、第１実施形態は前方ライダー２ａの第一路面走査線Ｌ１ａ、及び後方ライダー２ｂの第二路面走査線Ｌ１ｂが互いに平行となるようにダンプトラック１０に対して配置されているのに対し、第２実施形態は、第一路面走査線Ｌ１ａ及び第二路面走査線Ｌ１ｂを車両前方、かつ車幅内で交差させてダンプトラックに各ライダー２ａ、２ｂが設置される点である。なお、第２実施形態において、第１実施形態と同一又は対応する部分には同一符号を付している。

第２実施形態では、第一路面走査線Ｌ１ａ及び第二路面走査線Ｌ１ｂを前方位置の交点Ｇにおいて交差させている。この結果、図９に示すように、ダンプトラック１０と路肩Ｂとの間の距離Ｄｓが大きくなるに連れて、路肩特徴点Ｐａと路肩特徴点Ｐｂとの間の距離Ｗが大きくなる。

通常、路幅が狭く、崖に落ちないように設置された路肩近辺を走行しなければならない場合には走行速度が低く設定される。一方、路幅が広く路肩から離れて走行することが可能な場合には走行速度が速く設定される。

ここで、第１実施形態では、車両速度が速くなるにつれて、前方ライダー２ａ及び後方ライダー２ｂが路肩を計測する時間間隔が小さくなるため、閾値Ｄ_ｔｈが相対的に大きく設定されたことと同様の効果が生じる。その結果、高速走行になるにつれ、自己位置推定装置１１の異常検知感度が低下する。また逆に、低速走行時には、自己位置推定装置１１の異常検知感度が高くなりすぎ、必要以上に頻繁に異常と判断されてしまう可能性がある。

これに対し第２実施形態では、車両が路肩から離れるにつれて路肩特徴点Ｐａと路肩特徴点Ｐｂとの距離Ｗが長くなるので、高速走行時における自己位置推定装置１１の異常検知感度の低下を防ぐことができる。

また第２実施形態によれば、走行速度に応じて前方ライダー２ａ及び後方ライダー２ｂのレーザ照射角度を変えることで、走行速度によらず前方ライダー２ａ及び後方ライダー２ｂが路肩特徴点を計測する時間間隔を一定にすることができる。すなわち、低速走行時は交点Ｇがダンプトラック１０により近い位置に、高速になるにつれて各ライダーの路面に対するレーザ照射角度を小さくし、交点Ｇがダンプトラック１０から遠い位置になるように制御する。これにより、自己位置推定装置１１の異常検知を一定時間間隔で行うことができる。

上記各実施形態は本発明を限定するものではなく、本発明の趣旨を逸脱しない様々な変更態様も本発明の技術的範囲に属する。

例えば、上記ステップＳ３２において、停車制御に代わり若しくは停車制御と共に、通信装置１３を介して管制サーバ２０に自己位置推定装置１１の異常検出結果を送信してもよい。管制サーバ２０は、異常検出結果の受信履歴を記憶する。そして、搬送路上の同一・近傍区間で異なるダンプトラック１０から異常検出結果を受信している場合は、異常発生原因が各ダンプトラック１０の自己位置推定装置１１や車両に搭載されたセンサではなく、ＧＰＳ測位データのマルチパスが原因であると推定できる。この場合、その区間ではＧＰＳ由来の自己位置を外れ値として用いＩＭＵの出力値で自己位置推定を行うことで、次回以降のダンプトラック１０の停止制御を回避することができる。

また上記では前方ライダー２ａ及び後方ライダー２ｂは別体に構成された複数のセンサであったが、マルチレイヤーレーザスキャナや、ステレオカメラなどを用いて、路肩を計測する第一計測装置と同じ箇所を後から計測する第二計測装置とを１台のセンサで構成してもよい。

１ 自律走行システム
２ａ 前方ライダー
２ｂ 後方ライダー
１０ ダンプトラック
１１ 自己位置推定装置
１８ 異常検知装置

Claims (7)

1. 車両に搭載され、外部座標系で定義された自己位置を推定する自己位置推定装置の異常検知装置において、
   前記車両が走行する路面の周辺構造物上にある進行方向に沿って並ぶ特徴点群の内の各特徴点及び自車両の相対位置を計測する第一計測装置からの出力に基づいて、前記第一計測装置の座標系で表された前記各特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第一座標を算出する第一特徴点座標算出部と、
   前記第一座標を基に、前記特徴点群の前記外部座標系で定義された軌跡を生成する特徴点軌跡生成部と、
   前記周辺構造物の前記特徴点群の内、前記第一計測装置の計測時刻において前記第一計測装置が計測した特徴点よりも進行方向後方に位置する特徴点及び自車両の相対位置を計測する第二計測装置からの出力に基づいて、前記第二計測装置の座標系で表された前記進行方向後方に位置する特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第二座標を算出する第二特徴点座標算出部と、
   前記特徴点群の前記外部座標系で定義された軌跡及び前記第二座標の偏差が、前記軌跡上に前記第二座標があると見做すために定められた閾値を上回ると、前記自己位置推定装置に異常があると判定する異常判定部と、
   を備えることを特徴とする自己位置推定装置の異常検知装置。
2. 請求項１に記載の自己位置推定装置の異常検知装置において、
   前記車両は鉱山内を走行するダンプトラックであり、
   前記第一計測装置及び前記第二計測装置は、前記ダンプトラックが走行する路面及び前記ダンプトラックの進行方向に対して側方に位置する路肩を走査し、前記路面及び前記路肩の各計測点及び自車両の相対位置を計測する、
   ことを特徴とする自己位置推定装置の異常検知装置。
3. 請求項２に記載の自己位置推定装置の異常検知装置において、
   前記第一特徴点座標算出部は、前記第一計測装置からの出力に基づいて、前記第一計測装置の走査面及び前記路面の交線となる第一路面走査線と、前記第一計測装置の走査面及び前記路肩の斜面の交線となる第一路肩走査線と、の交点を路肩特徴点として検出して前記第一座標を算出し、
   前記第二特徴点座標算出部は、前記第二計測装置からの出力に基づいて、前記第二計測装置の走査面及び前記路面の交線となる第二路面走査線と、前記第二計測装置の走査面及び前記路肩の斜面の交線となる第二路肩走査線と、の交点を路肩特徴点として検出して前記第二座標を算出する、
   ことを特徴とする自己位置推定装置の異常検知装置。
4. 請求項２に記載の自己位置推定装置の異常検知装置において、
   前記第一計測装置及び前記第二計測装置は前記ダンプトラックの前部に設置され、前記第一計測装置及び前記第二計測装置の其々の前記ダンプトラックに対する取付位置及び取付角度は、前記第一計測装置の走査面及び前記路面の交線となる第一路面走査線と前記第二計測装置の走査面及び前記路面の交線となる第二路面走査線との交点が前記ダンプトラックの前方かつ前記ダンプトラックの車幅内に収まる、取付位置及び取付角度である、
   ことを特徴とする自己位置推定装置の異常検知装置。
5. 請求項１に記載の自己位置推定装置の異常検知装置において、
   前記第一計測装置及び前記第二計測装置は前記車両の前面に設置され、前記第二計測装置の走査面は前記第一計測装置の走査面よりも後方に位置し、かつ前記第一計測装置の走査面及び前記第二計測装置の走査面が平行になる位置及び取付角度で設置される、
   ことを特徴とする自己位置推定装置の異常検知装置。
6. 請求項１に記載の自己位置推定装置の異常検知装置において、
   前記第一計測装置は当該第一計測装置の走査面が前記車両の進行方向前方に向かう向きに設置され、
   前記第二計測装置は当該第二計測装置の走査面が前記車両の進行方向後方に向かう向きに設置される、
   ことを特徴とする自己位置推定装置の異常検知装置。
7. 自己位置推定装置の異常検知装置を搭載した車両であって、
   車両が走行する路面の周辺構造物上にある進行方向に沿って並ぶ特徴点群の内の各特徴点及び自車両の相対位置を計測する第一計測装置と、
   前記周辺構造物の前記特徴点群の内、前記第一計測装置の計測時刻において前記第一計測装置が計測した特徴点よりも進行方向後方に位置する特徴点及び自車両の相対位置を計測する第二計測装置と、
   外部座標系で定義された自己位置を推定する自己位置推定装置と、
   前記自己位置推定装置の異常を検知する異常検知装置と、を備え、
   前記異常検知装置は、
   前記第一計測装置からの出力に基づいて、前記第一計測装置の座標系で表された前記各特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第一座標を算出する第一特徴点座標算出部と、
   前記第一座標を基に、前記特徴点群の前記外部座標系で定義された軌跡を生成する特徴点軌跡生成部と、
   前記第二計測装置からの出力に基づいて、前記第二計測装置の座標系で表された前記進行方向後方に位置する特徴点の座標を求め、その座標を前記自己位置を用いて外部座標系に変換して第二座標を算出する第二特徴点座標算出部と、
   前記特徴点群の前記外部座標系で定義された軌跡及び前記第二座標の偏差が、前記軌跡上に前記第二座標があると見做すために定められた閾値を上回ると、前記自己位置推定装置に異常があると判定する異常判定部と、を備える、
   ことを特徴とする車両。

Images