JP6900341B2 - 位置算出装置及びダンプトラック - Google Patents

Description

本発明は、位置算出装置及びダンプトラックに係り、特に鉱山や建設現場で移動するダンプトラックに搭載された位置算出装置の位置測定誤差に関する。

自律的に走行するダンプトラックにおいてその位置を算出する機能は重要な機能の一つである。この位置算出の例として、慣性計測装置（ＩＭＵ）を用い、車輪の回転数から速度を計測する車輪速度計、車輪の車軸に対する傾きを計測する操舵角計等の計測装置の結果を用いて、ダンプトラックの位置を算出する。ＧＮＳＳ等のように直接位置を算出する機器から出力される位置に対して、速度、姿勢、加速度、角速度などの関連する運動パラメータから位置を順次更新する方法があり、デッドレコニングと呼ばれる。

従来よりデッドレコニングを用いて算出した位置を、カルマンフィルタなどに代表される確率フィルタを用いて、全地球航法衛星システム（ＧＮＳＳ）にて算出された位置へ徐々に修正する手法がある。この確率フィルタを用いた位置修正方法は、デッドレコニング、ＧＮＳＳのそれぞれから算出される誤差を考慮して位置を修正するため、確率的に最も存在する可能性の高い位置を算出できることが長所である。この際に、確率フィルタを用いて、自律走行ダンプトラックの位置の誤差を分散値として算出し、この分散値が存在する可能性のある範囲を誤差楕円として表わすことが可能である。

特許文献１には、誤差楕円を用いたダンプトラックの走行制御装置として、「自車両の予測位置を算出する位置算出装置と、予測位置を中心とし、運搬車両が予め定められた期待確率で存在する位置範囲を算出する誤差見積もり範囲算出部と、運搬車両の目標経路と位置範囲内に含まれる各地点との乖離量のうちの最大値からなる最大乖離量を算出する最大乖離量算出部と、最大乖離量が相対的に大きい場合に、運搬車両の目標車速を相対的に小さく定める目標車速決定部と、目標車速に従って、目標経路に沿って運搬車両を走行させるための制御を行う目標経路追従部（要約抜粋）」を備えた構成が開示されている。

また特許文献２には、「ナビゲーション装置は、ＧＰＳ位置算出部と、センサ情報取得部と、ＧＰＳ位置とセンサ情報とに基づいて移動体の位置および向き、並びに、これらの誤差共分散行列を算出する位置誤差算出部と、各センサ情報を累積して得られた位置と、ＧＰＳ位置との位置の差をオフセットとして算出するオフセット算出部と、算出されたオフセットに基づいて、位置誤差算出部によって算出された誤差共分散行列を補正する誤差共分散行列補正部と、位置誤差算出部によって算出された移動体の位置および向き、並びに、誤差共分散行列補正部によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に移動体の位置を推定するマップマッチ処理部とを備える（要約抜粋）」構成が開示されている。

米国特許出願公開第２０１７／０１７２３５号明細書 特開２０１４−１４２２７２号公報

従来の確率フィルタの手法ではデッドレコニングでの見積もり誤差を信用して、誤差分散値を算出する仕組みになっている。そのため、デッドレコニングでの見積もり誤差が大きく誤っている、もしくはスリップなどの考慮されていない車両運動が発生した場合には、現実にそぐわない誤差楕円を出力することがあるという課題がある。このような予め設定された見積もり誤差であるデッドレコニングでの見積もり誤差自体に意図せず含まれる誤差を、モデル化誤差と呼ぶことにする。特許文献１では誤差楕円を基づいて走行制御を行うため、誤差楕円に含まれるモデル化誤差が大きいと実際の走行制御に支障をきたす可能性があり、このモデル化誤差を含めた誤差楕円を出力することにより、目標車速を制御したいという要望がある。また、モデル化誤差の発生要因の一つとなる車両運動は、積載の有無等の車両状態の変化によっても生じるが、この車両状態の変化は、特定の車両に固有に生じる。これに対し、特許文献２では、過去の他の車両の動作を基に算出したオフセットを用いて誤差楕円の補正を行うので、特定の車両に固有の車両運動が生じた場合にはオフセットに反映されず、新たに生じた誤差の検出が遅れるという課題が残る。

本発明は上記課題に鑑みてなされたものであり、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することを目的とする。

上記課題を解決するために本発明は、グローバル座標系で表された移動体のＧＮＳＳ出力位置を第１時点及びそれよりも遅い第２時点の各時点において逐次計測するＧＮＳＳセンサと、前記移動体の運動量及び姿勢を検出する移動体センサの検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、前記計測始点からの前記移動体の相対位置を逐次更新するデッドレコニング装置と、前記移動体の速度、重量、前記移動体が走行する路面の勾配の少なくとも一つにより定義された移動体状態を検出する移動体状態センサと、の其々に接続された、前記移動体の位置算出装置であって、前記位置算出装置は、一時記憶装置を含む位置算出コントローラを備え、前記位置算出コントローラは、前記第１時点におけるＧＮＳＳ出力位置、及び前記第１時点において前記デッドレコニング装置が算出した相対位置を用いて前記第１時点における前記移動体の最尤位置を計算し、前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点における最尤位置に足し合わせて、前記第２時点における仮定デッドレコニング位置を算出し、前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点におけるＧＮＳＳ出力位置に足し合わせて、前記第２時点における適応デッドレコニング位置を算出し、前記第２時点におけるＧＮＳＳ出力位置と前記第２時点における適応デッドレコニング位置との差からなる適応デッドレコニング誤差を算出すると共に、前記第２時点における前記移動体状態センサからの検出値を基に移動体状態を決定し、予め一時記憶装置に記憶された移動体状態と適応デッドレコニング誤差の平均値及び分散値とを関係付けた誤差統計量データのうち、前記決定された移動体状態に応じた適応デッドレコニング誤差の平均値及び分散値は、新たに算出された適応デッドレコニング誤差を用いて再計算して更新し、前記第２時点における仮定デッドレコニング位置を中心とし仮定デッドレコニング誤差の分散値からなる誤差楕円に前記第２時点における仮定デッドレコニング位置から前記再計算された適応デッドレコニング誤差の平均値分ずれた位置を中心とし、前記再計算された適応デッドレコニング誤差の分散値からなる誤差楕円を加えて存在可能性範囲を算出し、前記存在可能性範囲内に前記第２時点におけるＧＮＳＳ出力位置が含まれている場合には、当該ＧＮＳＳ出力位置を用いて前記第２時点における最尤位置及び当該最尤位置を中心とする最尤誤差楕円を算出し、前記存在可能性範囲の外に前記第２時点におけるＧＮＳＳ出力位置が存在する場合には、前記第２時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、ことを特徴とする。

本発明によれば、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することができる。上記した以外の目的、構成、効果については以下の説明で明らかにされる。

ダンプトラックの外観を示す概略図 ダンプトラックのハードウェア構成図 位置算出装置、デッドレコニング装置、及び自律走行制御装置の機能構成を示すブロック図 位置算出装置の動作の概要を示すフローチャート 存在可能性範囲の概念図 誤差楕円を用いた位置計測安定性判断処理の流れを示すフローチャート 最尤位置及び最尤位置の誤差分散値の算出処理の流れを示すフローチャート 適応デッドレコニング誤差の概念図 適応デッドレコニング誤差更新処理の流れを示すフローチャート 一時記憶部のデータ構成例を示す図

以下、図面を参照して本発明の実施形態について説明する。全図を通じて同一の構成には同一符号を付け、重複説明を省略する。

以下、位置算出の対象となる移動体として、鉱山内を管制局からの指示に従って自律走行する自律走行ダンプトラック（以下「ダンプトラック」と略記する）を用いた例について説明する。

図１は、ダンプトラック１の外観を示す概略図である。図１に示すように、ダンプトラック１は、車体フレーム（ｖｅｈｉｃｌｅ ｆｒａｍｅ）２と、車体フレーム２上に起伏可能に設けられたベッセル３とを有する。更に車体フレーム２の前側上方に運転室４が設けられている。そして、車体フレーム２の下部には左右の前輪５及び後輪６が備えられる。

ダンプトラック１の前部にはＧＮＳＳアンテナ７を備える。図１ではＧＮＳＳアンテナ７を一つ図示しているが、実際には、ダンプトラック１の前部に車幅方向に間隔を空けて二つ以上のＧＮＳＳアンテナ７が設置されている。ＧＮＳＳアンテナ７は、グローバル座標系で表されたダンプトラック１の絶対位置を逐次計測するＧＮＳＳセンサ１０（図２参照）の構成要素である。

更にダンプトラック１の運動量を示す検出値及びダンプトラック１の姿勢を示す検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、計測始点からのダンプトラック１の相対位置を逐次更新するデッドレコニング（以下「デッドレコニング」を「ＤＲ」と略記する）装置２１０を備える。

ダンプトラック１は、ＧＮＳＳセンサ１０から出力される絶対座標系で表されたＧＮＳＳ出力位置、及びＤＲ装置２１０から出力される相対位置を基にダンプトラック１の自己位置を算出する位置算出装置１００と、加速装置や制動装置を含む走行駆動装置４００に対して位置算出装置１００から取得した自己位置データを用いて自律走行制御を行う自律走行制御装置３００と、を備える。

図２は、ダンプトラック１のハードウェア構成図である。位置算出装置１００は、ＣＰＵ１０１、ＲＯＭ１０２、ＲＡＭ１０３、ＨＤＤ１０４、入力インターフェース（Ｉ／Ｆ）１０５、出力Ｉ／Ｆ１０６を含みこれらがバス１０７を介して互いに接続されたコンピュータ（位置算出コントローラ）を用いて構成される。ＲＡＭ１０３又はＨＤＤ１０４は、一時記憶装置に相当する。

ＤＲ装置２１０は、コンピュータ（ＤＲコントローラ）を用いて構成される。ＤＲコントローラの入力段には、ダンプトラック１の運動量や姿勢を示すパラメータを計測するセンサ、操舵角センサ３０の其々が接続される。

運動量を検出するセンサは、具体的には、車輪回転数を基に速度を計測する速度センサ２０１、ダンプトラック１のヨー角、ロール角、ピッチ角などの各軸の角度（姿勢）を計測もしくは算出する姿勢センサ２０２と、ダンプトラック１の加速度を計測する加速度センサ２０３、ダンプトラック１の角速度を計測する角速度センサ２０４である。

積載状態センサ２０（重量センサに相当する）は、ベッセル３の積載状態を検出するセンサである。操舵角センサ３０は、ダンプトラック１の直進旋回状態を検出するセンサである。勾配演算器４０は、車輪速及び駆動輪に加わるトルクを基に、ダンプトラック１の走行路の勾配を演算する演算器である。

位置算出コントローラの入力Ｉ／Ｆ１０５はＤＲ装置２１０に接続され、出力Ｉ／Ｆ１０６は自律走行制御装置３００に接続される。自律走行制御装置３００は位置算出装置１００の位置算出結果の出力先となる外部装置に相当する。更に自律走行制御装置３００の出力段には、加速装置及び制動装置を含む走行駆動装置４００が接続される。ＤＲ装置２１０及び自律走行制御装置３００を構成するコンピュータの具体的なハードウェア構成は位置算出装置１００と同じである。

加速度センサ２０３に代えて速度センサ２０１からの出力に対して時間微分演算を行い、加速度を演算で求める加速度演算器を備えてもよい。

図３は、位置算出装置１００、ＤＲ装置２１０、及び自律走行制御装置３００の機能構成を示すブロック図である。

ＤＲ装置２１０は、速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、及び角速度センサ２０４、及び操舵角センサ３０からの出力を基に、ダンプトラック１の各時点の次の時刻における位置を求めて更新するＤＲ位置演算部２１１を含む。

ＧＮＳＳセンサ１０はＧＮＳＳ出力位置及び位置計測ステータスを含むＧＮＳＳセンサデータを位置算出装置１００へ出力し、ＤＲ装置２１０は、ＤＲ位置演算部２１１が算出した更新位置データを位置算出装置１００へ出力する。また、本実施形態では速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０の検出値をＤＲ装置２１０が位置算出装置１００にも出力するものとする。

位置計測ステータスは、その時刻においてＧＮＳＳセンサ１０からどのような精度で位置が算出されているかを示すデータである。例えば「４」なら公称誤差０．０２ｍのＲＴＫ測位、「３」なら公称誤差０．５ｍ、「１」なら公称誤差５ｍの単独測位、「０」なら非測位（測位データ無し）などである。ただし、位置計測ステータスは、上記に限らず位置の計測有無が判断できるデータであれば、ユーザーが任意に定義してよい。

位置算出装置１００は、速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０、及び勾配演算器４０の出力を基に車両状態を決定する車両状態決定部１３２、ＧＮＳＳセンサ１０の計測結果の安定性を判断する位置計測安定性判断部１１４、確率フィルタを用いて自車位置を推定する最尤位置演算部１１５、過去のある時点から最新のサンプリング時点までに算出された推定位置の誤差分散値を算出する仮定ＤＲ誤差算出部１１２、一時記憶部１３１、適応ＤＲ誤差算出部１４２、存在可能性範囲算出部１１３、仮定ＤＲ位置算出部１１１、及び適応ＤＲ誤差パラメータ算出部１４１を含む。

最尤位置演算部１１５は、ＧＮＳＳセンサ１０及びＤＲ装置２１０の出力を用いて確率フィルタ処理等を実行することにより、ダンプトラック１の現在の確率的な位置の分布を推定する。

最尤位置演算部１１５が推定する確率的な位置の分布は、最も存在可能性が高い位置の絶対座標を中心とし、ＧＮＳＳセンサ１０とＤＲ装置２１０の演算結果に含まれる仮定ＤＲ誤差算出部１１２により計算される誤差分散である。

適応ＤＲ誤差算出部１４２は、ＧＮＳＳセンサ１０の位置の第１時点と第２時点での位置の差分とその間のＤＲ装置２１０からの出力を比較することによりＤＲによって生じるモデル化誤差を適応ＤＲ誤差の平均値と誤差分散値として計算する。平均値及び誤差分散値は統計量の一例である。

一時記憶部１３１は、車両状態や運動の検出値等を一時的に記憶する。

存在可能性範囲算出部１１３は、仮定ＤＲ誤差算出部１１２の出力するＤＲによる誤差分散値（仮定ＤＲ位置の誤差分散）および適応ＤＲ誤差算出部１４２が出力する適応ＤＲ誤差の誤差分散値からダンプトラック１の存在可能性範囲である、仮定ＤＲ位置での誤差楕円と適応ＤＲ誤差の誤差楕円を算出する。

自律走行制御装置３００は、予め定められた目標軌道と最尤位置演算部１１５から取得した誤差楕円の最外縁との距離に応じて、目標軌道への復帰速度を算出する目標速度算出部３０１と、走行駆動装置４００に対して目標速度に従って目標軌道に復帰させるための制御を実行する速度制御部３０２と、を備える。

上記各機能ブロックは、図２で示すハードウェアと上記各機能ブロックの機能を実現するソフトウェアとが協働して構成してもよいし、各機能ブロックを実現する回路を用いて構成してもよい。

次に図４を参照してダンプトラック１における位置算出装置１００の動作について説明する。図４は、ダンプトラック１の動作の概要を示すフローチャートである。以下の処理は、位置算出装置１００が第２時点において各センサから出力された検出値を用いて第２時点におけるダンプトラック１の存在可能性範囲を算出し、その算出過程において第２時点よりも前の第１時点で算出した位置との比較を行い、ＤＲでの見積もり誤差が適正な範囲内にあるかを評価する例を示す。

位置算出装置１００はＧＮＳＳセンサ１０からＸ（ｔ）時点（第１時点）の出力を取得する（Ｓ１００）。

ＤＲ装置２１０と位置算出装置１００は、Ｘ（ｔ）時点以降から車体センサ、より具体的には、速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、角速度センサ２０４及び操舵角センサ３０から車体センサ１サンプリング周期後の検出値を取得する（Ｓ１０１）。

また速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、角速度センサ２０４、及び操舵角センサ３０は時刻同期がとれており、少なくともＧＮＳＳセンサ１０の計測間隔と同じか、短い計測間隔で同期をとりながら各センサが検出結果を出力する。

ＤＲ位置演算部２１１は、ステップＳ１０１で取得した各センサの出力を用いて、第１時点での位置からの位置変位量Δｘを算出する（Ｓ１０２）。

ＤＲ位置演算部２１１は、各センサの検出値に基づいて１サンプル時間（Δｔ）の更新量δｘを算出し、第１時点からの位置変位量Δｘを計算する。

更新量δｘは車体座標系から絶対座標系への変換行列Ｒθが決まった場合は、ＤＲ位置演算部２１１は下式（１）により算出する。

また、加速度センサ２０３が存在しない場合には、ＤＲ位置演算部２１１は下式（２）により更新量δｘを算出する。

第１時点からの位置変位量Δｘは下式（３）のように算出する。

次に仮定ＤＲ位置算出部１１１が第１時点での絶対位置ｘ（ｔ）に、ステップ１０２で得た位置変位量Δｘ（ｔ＋１）を足し合わせて仮定ＤＲ位置ｘ（ｔ+１）ｔｉｌｄｅを下式（４）により算出する（Ｓ１０３）。

車両状態決定部１３２は、速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０、及び勾配演算器４０からの出力を基に車両状態を決定し、決定した車両状態に応じて、１サンプル時間当たりの適応ＤＲ誤差の平均値と分散値を一時記憶部１３１から取得する（Ｓ１０４）。

車両状態は、積荷もしくは空荷を示す積載状態と、加速中もしくは減速中もしくは一定速度走行中を示す速度状態と、上り勾配走行中もしくは下り勾配走行中もしくは平坦地走行中を示す勾配状態と、直進走行中もしくは右旋回走行中もしくは左旋回走行中を示す直進旋回状態と、車輪滑り有りもしくは車輪滑り無しを示す車輪すべり状態と、のうちの少なくとも一つ又は少なくとも二つ以上の任意の組み合わせを用いて定義される。

車両状態決定部１３２は、各センサからの検出値を変数に持つ関数から、車両状態Λｉ（ｉ＝１、２、・・・、ｋ）である確率Ｐ（Λｉ）を、下式（５）を用いて決定する。

関数ｆ_ｉ（＊）は事前に決められているものとし、単一の式でなく複数の式で構成されていても良い。また、入力値と検出値から機械学習を用いて車両状態を学習してもよい。また、Ｍは自重であるが、積荷状態と空荷状態を区別するだけのステータス値でも良い。車両状態決定部１３２は、算出された確率が最も高くなる状態を現在の車両状態として決定する。車両状態決定部１３２は、決定した車両状態を仮定ＤＲ誤差算出部１１２、及び適応ＤＲ誤差パラメータ算出部１４１の両方に出力する。

次に仮定ＤＲ誤差算出部１１２では、ステップ１０３で算出した仮定ＤＲ位置における誤差分散値を算出する（Ｓ１０５）。仮定ＤＲ位置の誤差分散値Σ（ｔ＋１）は、１サンプル時間前の仮定ＤＲ位置の誤差分散値Σ（ｔ）に、予め設定されている１サンプル時間での更新量の誤差分散値Ｗ、および車両状態決定部１３２が決定する車両状態に応じた１サンプリング時間当たりの適応ＤＲ誤差の分散値Ｅ［ε^２（ｔ）］を足した値となる。仮定ＤＲ誤差算出部１１２は仮定ＤＲ位置の誤差分散値を以下の式（６）を用いて算出する。仮定ＤＲ誤差算出部１１２は、算出した仮定ＤＲ位置の誤差分散値Σ（ｔ＋１）を存在可能性範囲算出部１１３に出力し、ステップＳ１０７へ進む。

一方、適応ＤＲ誤差算出部１４２は、Ｓ１０３で算出した仮定ＤＲ位置における適応ＤＲ誤差の平均値の蓄積量とその誤差分散値を算出する（Ｓ１０６）。この適応ＤＲ誤差の蓄積量や誤差分散値はＳ１０４で得られる適応ＤＲ誤差の平均値Ｅ［ε（ｔ）］と分散値Ｅ［ε^２（ｔ）］から算出する。この処理を適応ＤＲ誤差算出処理という。この処理内容の詳細は後述する。適応ＤＲ誤差算出部１４２は、算出した適応ＤＲ誤差を存在可能性範囲算出部１１３に出力し、ステップＳ１０７へ進む。

存在可能性範囲算出部１１３は、仮定ＤＲ位置とその誤差分散値、および適応ＤＲ誤差の平均値と誤差分散値から、ＧＮＳＳセンサ１０の出力する位置が存在する可能性のある位置を算出する（Ｓ１０７）。本ステップの処理を存在可能性範囲算出処理という。この処理内容の詳細は後述する。存在可能性範囲算出部１１３は、上記処理の結果、次の位置計測安定性判断部１１４で用いる、仮定ＤＲの誤差楕円と適応ＤＲの誤差楕円を算出する。

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０からの出力の安定性を判断する（Ｓ１０８）。本ステップの処理を位置計測安定性判断処理という。この処理内容の詳述は後述する。位置計測安定性判断部１１４は、上記処理の結果、ＧＮＳＳセンサ１０からの出力が、予め定めた安定性を満たすと判断した場合には、ステップＳ１０１においてＧＮＳＳセンサ１０から取得したＧＮＳＳセンサデータに値「１」からなる安定性フラグを付加し、上記安定性を満たさないと判断した場合には、ＧＮＳＳセンサ１０から取得したＧＮＳＳセンサデータに値「０」からなる安定性フラグを付加する。その後、ステップＳ１０９へと続く。

最尤位置演算部１１５は、確率フィルタを用いてダンプトラック１の最尤位置及び誤差分散値を演算し、出力Ｉ／Ｆ１０６を通して、自律走行制御装置３００へ自車位置データを出力する（Ｓ１０９）。

確率フィルタの代表的な方法としてカルマンフィルタや、パーティクルフィルタなどを挙げることができる。本実施形態では、ＧＮＳＳセンサ１０及びＤＲ装置２１０においてそれぞれ算出した誤差分散から、カルマンフィルタにより最尤位置及び誤差分散値を求める。最尤位置演算部１１５が実行する最尤位置及びその誤差分散値の算出処理は後述する。

次に、適応ＤＲ誤差パラメータ算出部１４１は、ＧＮＳＳセンサデータに付加された安定性フラグを参照し、安定性フラグの値が「１」であれば（Ｓ１１０／Ｙｅｓ）、適応ＤＲ誤差更新処理を実行する（Ｓ１１１）。適応ＤＲ誤差更新処理の詳細は後述する。

一方、適応ＤＲ誤差算出部１４２は、ＧＮＳＳセンサデータに付加された安定性フラグの値が「０」であれば（Ｓ１１０／Ｎｏ）、適応ＤＲ誤差更新処理を実行することなく、一時記憶部１３１に適応ＤＲ誤差の平均値及び誤差分散値を書きこむ、所謂データ保持処理だけを実行する（Ｓ１１２）。

次に、適応ＤＲ誤差パラメータ算出部１４１は、１ステップ前の車両状態（第１移動体状態に相当する）と現在の車両状態（第２移動体状態に相当する）の遷移確率Ｐを更新し、一時記憶部１３１へ保存する（Ｓ１１３）。この際、複数の車両状態の遷移確率が一定以上の場合、それらの車両状態は同一の車両状態であるとみなし、適応ＤＲ誤差の平均値および誤差分散値を融合する。この処理を車両状態更新処理といい、詳細は後述する。

位置算出装置１００がＧＮＳＳセンサ１０からＸ（ｔ＋１）時点の出力を受け取らなければ（Ｓ１１４／Ｎｏ）、第１時点Ｘ（ｔ）を計測始点として維持したまま、Ｓ１０１〜Ｓ１１４の処理をＧＮＳＳサンプリング１周期分ループして処理し続ける（Ｓ１１５）。

一方、位置算出装置１００がＧＮＳＳセンサからＸ（ｔ＋１）時点の出力を受け取ると（Ｓ１１４／Ｙｅｓ）と、計測始点をＸ（ｔ＋１）に更新してＳ１０１からＳ１１４までの処理を繰り返す（Ｓ１１６）。

＜適応ＤＲ誤差算出処理＞
次に仮定ＤＲ位置における適応ＤＲ誤差の蓄積量とその誤差分散値の算出について説明する。

適応ＤＲ誤差算出部１４２は、Ｓ１０４にて決定した現在の車両状態における適応ＤＲ誤差の平均値と分散値を取得する。

次に適応ＤＲ誤差算出部１４２は、第１時点からのＤＲ維持時間を算出する。ＤＲ維持時間は仮定ＤＲ位置の更新サンプル数ｎとし、適応ＤＲ誤差算出部１４２が仮定ＤＲ位置算出部１１１が起動するたびに起動しているとすれば、単純に前回値ｎ＿に１を足せばよい。よって、更新サンプル数ｎは下式（７）で表せる。

また、更新サンプル数ｎは車両状態によって変わるものとする。つまり、前サンプル時刻における車両状態と今回の車両状態が違った場合、ｎ＿＝０として上記の算出を実行するものとする。

適応ＤＲ誤差算出部１４２は、ｎサンプル時間後に仮定ＤＲ位置の更新毎にＤＲにより生じる誤差の平均値を下式（８）、分散値を下式（９）により逐次算出する。

これらの処理が終了すれば、適応ＤＲ誤差算出部１４２の適応ＤＲ誤差算出処理を終了する。

＜存在可能性範囲算出処理＞
存在可能性範囲算出部１１３は、仮定ＤＲ誤差と適応ＤＲ誤差を考慮した車両位置の存在する可能性がある範囲を算出する（Ｓ１０７）。存在可能性範囲は仮定ＤＲ位置算出部１１１にて算出される仮定ＤＲ位置（暫定的な位置である）及びその誤差分散と、適応ＤＲ誤差算出部１４２にて算出する適応ＤＲ誤差の分散値及び平均値を用いて算出することができる。

具体的には、仮定ＤＲ位置算出部１１１にて算出される仮定ＤＲ位置を（ｘｅ、ｙｅ）ｔ、仮定ＤＲ位置での誤差分散をΣ、事前に決定されているマハラノビス距離をｄとすると、仮定ＤＲ位置の存在範囲は下式（１０）に示す楕円方程式範囲内である。

同様に、適応ＤＲ誤差の平均値をＥ［ε（ｔ）］＝（ｘｄ、ｙｄ）ｔ、分散値をＥ［ε^２（ｔ）］として、適応ＤＲ誤差の存在範囲は下式（１１）の楕円方程式範囲内となる。

よって、存在可能性範囲算出部１１３は、式（１０）から得られる仮定ＤＲ位置の存在範囲（誤差楕円）に、式（１１）から得られる適応ＤＲ誤差の存在範囲（誤差楕円）を加えた範囲を存在可能範囲として算出する。すなわち、存在可能性範囲は、これら二つの楕円の範囲となる。

図５に存在可能性範囲の概念図を示す。点１００１が仮定ＤＲ位置、誤差楕円１００２が式（１０）により求められる仮定ＤＲ誤差の誤差楕円である。誤差楕円１００２は仮定ＤＲ位置１００１を中心としている。この誤差楕円１００２は楕円内部に、マハラノビス距離ｄで規定された確率で位置が存在することを示している。また、式（１１）で表わされる適応ＤＲ誤差の誤差楕円は、適応ＤＲ誤差の平均値［ε（ｔ）］をベクトル１００３で表せば、仮定ＤＲ位置を始点として、適応ＤＲ誤差の平均値［ε（ｔ）］＝（ｘｄ、ｙｄ）ｔで表わされるベクトルの終点としたベクトルとなり、この終点が適応ＤＲ誤差範囲を示す誤差楕円１００５の中心１００４であることを示している。つまり、適応ＤＲ誤差の誤差楕円１００５の中心１００４は仮定ＤＲ位置から適応ＤＲ誤差の平均値分だけずらした点を意味する。また、適応ＤＲ誤差の誤差分散Ｅ［ε^２（ｔ）］とマハラノビス距離ｄにより、適応ＤＲ誤差の誤差楕円の中心１００４からの適応ＤＲ誤差の広がりを持つ楕円が式（１１）により求まり、これが適応ＤＲ誤差の誤差楕円１００５で表わされる。ダンプトラック１の位置の存在範囲は、仮定ＤＲ誤差の誤差楕円１００２及び適応ＤＲ誤差の誤差楕円１００５のいずれかもしくは両方の内側にあると考えればよい。

＜位置計測安定性判断処理＞
次に、ステップＳ１０８の位置計測安定性判断処理について図６を参照して説明する。本実施形態では位置計測安定性の判断を存在可能性範囲算出処理によって求めた誤差楕円により実施する。この安定性判断は他にも測位ステータスのみで行ってもよく、またχ２乗検定による棄却判断でも良い。図６は誤差楕円を用いた位置計測安定性判断処理の流れを示すフローチャートである。

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から取得した位置計測ステータスをチェックし、ＧＮＳＳセンサ１０からの出力が存在するかを判断する。ＧＮＳＳセンサ１０からの出力がある場合は（Ｓ２０１／Ｙｅｓ）、ステップＳ２０２に移行し、無い場合は（Ｓ２０１／Ｎｏ）、安定性フラグを「０」に設定する（Ｓ２０７）。

位置計測安定性判断部１１４は、Ｓ１０７で求めた仮定ＤＲ誤差の誤差楕円を取得する（Ｓ２０２）。

次に位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から出力される検出位置が仮定ＤＲ誤差の誤差楕円の範囲内であるかを判断する（Ｓ２０３）。

この楕円の範囲内であるかの判断は式（６）の右辺（ｘ、ｙ）にＧＮＳＳセンサ１０から出力される検出位置（Ｘ、Ｙ）を代入し計算した結果が、マハラノビス距離ｄの２乗以下の場合は、仮定ＤＲ誤差の誤差楕円の範囲内であると判断（Ｓ２０３／Ｙｅｓ）し、安定性フラグを「１」に設定する（Ｓ２０６）。マハラノビス距離ｄの２乗より大きい場合は仮定ＤＲ誤差の誤差楕円の範囲外であると判断（Ｓ２０３／Ｎｏ）し、ステップＳ２０４へ移行する。

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０の検出値が仮定ＤＲ誤差の誤差楕円範囲外であった場合、Ｓ１０７で求めた適応ＤＲ誤差の誤差楕円を取得する（Ｓ２０４）。

次に位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から出力される検出位置が適応ＤＲ誤差の誤差楕円の範囲内であるかを判断する（Ｓ２０５）。

この楕円の範囲内であるかの判断は式（７）の右辺に（ｘ、ｙ）にＧＮＳＳセンサ１０から出力されるＧＮＳＳ出力位置（Ｘ、Ｙ）を代入し計算した結果が、マハラノビス距離ｄの２乗以下の場合は、適応ＤＲ誤差の誤差楕円の範囲内であると判断（Ｓ２０５／Ｙｅｓ）し、安定性フラグを「１」に設定する（Ｓ２０６）。マハラノビス距離ｄの２乗より大きい場合は適応ＤＲ誤差の誤差楕円の範囲外であると判断（Ｓ２０５／Ｎｏ）し、安定性フラグを「０」に設定する（Ｓ２０７）。そして位置計測安定性判断部１１４の処理を終了する。

＜推定位置及び推定位置の誤差分散値の算出処理＞
次に、ステップＳ１０９の最尤位置及び最尤位置の誤差分散値の算出処理について図７を参照して説明する。図７は最尤位置演算部１１５が実行する最尤位置及び最尤位置の誤差分散値の算出処理の流れを示すフローチャートである。

まず最尤位置演算部１１５は、位置計測安定性判断部１１４が設定した安定性フラグの値が「１」かどうかを判断する。安定性フラグの値が「１」の場合は（Ｓ３０１／Ｙｅｓ）、ＧＮＳＳセンサ１０からの検出位置が存在し、かつ外れ値でもないため、検出位置の誤差分散値を設定する（Ｓ３０２）。

一方、最尤位置演算部１１５は、安定性フラグの値が「０」の場合は（Ｓ３０１／Ｎｏ）、ＧＮＳＳセンサ１０からの検出値が存在しない、もしくは外れ値であったため、ＧＮＳＳセンサ１０の検出位置の誤差分散値に予め決められた極大値を設定する。また、ＧＮＳＳセンサ１０の検出位置を仮定ＤＲ位置算出部１１１が算出した仮定ＤＲ位置と同じ値に設定する（Ｓ３０３）。

次に最尤位置演算部１１５は、カルマンフィルタにおけるイノベーションベクトルνを算出する（Ｓ３０４）。イノベーションベクトルνはＧＮＳＳセンサ１０の検出位置Ｘ及び仮定ＤＲ位置ｘｔｉｌｄｅから下式（１２）のように求まる。

最尤位置演算部１１５は、カルマンゲインＫを算出する（Ｓ３０５）。カルマンゲインＫは、仮定ＤＲ位置の誤差分散値が適応ＤＲ誤差の誤差分散値を含むことで、モデル化誤差を考慮でき、下式（１３）により求められる。

次に最尤位置演算部１１５は、最尤位置をダンプトラック１の推定位置として算出する（Ｓ３０６）。最尤位置は、下式（１４）により算出できる。

次に最尤位置演算部１１５は、最尤位置における誤差分散値Ｐ（ｔ）を算出する（Ｓ３０７）。誤差分散値は下式（１５）のように算出できる。

最後に最尤位置演算部１１５は、算出した最尤位置ｘ（ｔ）、誤差分散値Ｐ（ｔ）から誤差楕円（「最尤誤差楕円」という）を計算し、それらを自律走行制御装置３００へ出力する（Ｓ３０８）。

最尤位置ｘ（ｔ）と誤差分散値Ｐ（ｔ）から算出される最尤誤差楕円は下式（１６）のように求めることが出来る。

最尤誤差楕円は、最尤位置ｘ（ｔ）を中心として、誤差分散値Ｐ（ｔ）で規定される確率分布においてマハラノビス距離がｄとなる等確率線を表わしている。最尤誤差楕円内にマハラノビス距離ｄで規定される確率で正しい位置（ダンプトラック１が実際に存在する位置）が存在することを表わしている。

自律走行制御装置３００の目標速度算出部３０１は、最尤誤差楕円の最外縁部と目標軌道との乖離に応じて目標軌道に復帰する際の目標速度を決定し、速度制御部３０２が走行駆動装置４００に対して目標速度で走行するための制御信号を出力する。また、速度制御部３０２は必要に応じて制動装置に対する制動信号や操舵モータに対する操舵角信号も出力する。

また、上記の方法以外に、適応ＤＲ誤差の平均値および誤差楕円も出力することで、自律走行制御装置３００は、適応ＤＲ誤差の平均値が最尤誤差楕円の内側にある場合は、最尤誤差楕円を基に速度を算出し、適応ＤＲ誤差の平均値が最尤誤差楕円の外側にある場合は、最尤位置に適応ＤＲ誤差の平均値を足した点を中心とした適応ＤＲ誤差の誤差楕円を基に速度を算出してもよい。

上記の方法以外に、「２つの存在範囲を内包する楕円を生成して目標速度算出部３０１へ出力する」、「２つの位置の存在範囲を表わす分散を重ね合わせて一つの正規分布にフィッティングして目標速度算出部３０１へ出力する」、「２つの位置の存在範囲を両方とも目標速度算出部３０１へ出力する」などの方法でも同じ効果が得られる。

位置算出装置１００に接続される外部装置は自律走行制御装置３００に限らない。例えばダンプトラック１の管制制御を行う管制サーバに対して、最尤誤差楕円を通知してもよい。

＜適応ＤＲ誤差更新処理＞
ステップＳ１１１における適応ＤＲ誤差パラメータ算出部１４１による適応ＤＲ誤差ε（ｔ）およびその平均値、分散値の算出処理について図９に従って説明する。図９は、適応ＤＲ誤差更新の流れを示すフローチャートである。

まず適応ＤＲ誤差ε（ｔ）を、ＧＮＳＳセンサ１０の検出値とＤＲによって積算した移動量による値とを比較することにより算出する（Ｓ４０１）。図８に適応ＤＲ誤差の概念図を示す。

ＤＲはそれ単体では絶対的な位置を表わすことができない。そこで、適応ＤＲ誤差パラメータ算出部１４１は、計測始点となる第１時点でのＧＮＳＳセンサ１０の検出値７０１（Ｘ（ｔ−１））にＤＲ装置２１０によってステップＳ１０２にて算出された第１時点からの位置変位量Δｘ（ｔ）により表わされるベクトル７０２を足し合わせた適応ＤＲ位置と、次の計測タイミングでＧＮＳＳセンサ１０にて計測された位置７０３（Ｘ（ｔ））との差分７０４によって求めることができる。時刻ｔにおける差分７０４を適応ＤＲ誤差ε（ｔ）で表す。適応ＤＲ誤差ε（ｔ）は、下式（１７）により算出する。

次に適応ＤＲ誤差パラメータ算出部１４１は、ステップＳ１０４にて決定した現在の車両状態における、適応ＤＲ誤差の平均値を更新する（Ｓ４０２）。適応ＤＲ誤差パラメータ算出部１４１は、適応ＤＲ誤差ε（ｔ）の平均値を一時記憶部１３１から取得した現在の車両状態の平均値Ｅ［ε（ｔ−１）］を基に下式（１８）で求める。

ここで、一時記憶部１３１のデータ構成を図１０に示す。一時記憶部１３１には、車両状態と、それぞれに対応する適応ＤＲ誤差の平均値、分散値、及びそれらを算出したサンプル数が記録されている。

車両状態は、例えば、「積荷ｏｒ空荷」「加速ｏｒ減速ｏｒ一定速度」「上り勾配走行ｏｒ下り勾配走行ｏｒ平坦地走行」「直進走行ｏｒ右旋回走行ｏｒ左旋回走行」「グリップｏｒスリップ」などの状態の組み合わせで表現される。それぞれは関数ｆ（＊）内で表わされる。例えば「空荷ｏｒ積荷」であれば、「Ｍ＜０．１トン」を空荷、「Ｍ＞＝０．１トン」を積荷とするなどである。

また、適応ＤＲ誤差パラメータ算出部１４１は、適応ＤＲ誤差ε（ｔ）の分散値Ｅ［ε^２（ｔ）］も更新する。適応ＤＲ誤差パラメータ算出部１４１は適応ＤＲ誤差ε（ｔ）の分散値Ｅ［ε^２（ｔ）］を下式（１９）により逐次求める。

次に適応ＤＲ誤差パラメータ算出部１４１は、算出した適応ＤＲ誤差の平均値及び分散値を一時記憶部１３１の該当する部分へ上書き更新する（Ｓ４０３）。これらの処理が終了すれば、適応ＤＲ誤差更新処理を終了する。

＜車両状態更新処理＞
適応ＤＲ誤差パラメータ算出部１４１は、車両状態の遷移確率Ｐ（Λｔ｜Λｔ−１）を更新する。ある時刻における車両状態Λｍ（ｔ−１）から次のサンプル時における車両状態Λｎ（ｔ）への各車両状態への遷移確率Ｐ（Λｍｎ（ｔ｜ｔ−１））を下式（２０）により算出する。

これらを集めた遷移確率Ｐ（Λｔ｜Λｔ−１）は下記行列（２１）で表わされる。

車両状態の遷移確率は、上式をそのまま用いた場合は、値が安定しない場合も多いので、確率勾配法などを用いて、徐々に収束させるような手法をとってもよい。

ステップＳ４０３において、ある車両状態Λｓからある車両状態Λｕ（ｔ）までの遷移確率が両方向とも、閾値よりも高い場合は、車両状態に安定性が無いため、一つの車両状態へと融合する。その場合は、各車両状態に設定された適応ＤＲ誤差平均値と適応ＤＲ誤差分散値とをそれぞれのサンプル数を用いて、下式（２２）、（２３）のように更新する。

これらの処理が終了すれば、適応ＤＲ誤差パラメータ算出部１４１による車両状態更新処理を終了する。

本実施形態では、仮定ＤＲ位置の誤差分散値が適応ＤＲ誤差の誤差分散値を含むことで、カルマンゲインＫがモデル化誤差を考慮し、このカルマンゲインＫを用いて最尤誤差楕円を求める。そして最尤誤差楕円の最外縁と目標軌道との差を基に走行制御を行うので、スリップなどの考慮されていなかった現象が生じた場合でも適切な走行制御が可能となる。

また特許文献２に記載発明では、近々の過去の車両の動作しか考慮されていないため、有る特定の車両状態の場合にオフセットが生じるとその検出が遅れる。これに対して、本実施形態によれば、車両状態毎に適応ＤＲ誤差を記憶しているので、いち早く誤差の検出が可能となる。

更に本実施形態によれば、適応ＤＲ誤差を算出する際に、適応ＤＲ誤差の平均分誤差楕円の中心をずらし、誤差楕円そのものの大きさ（面積）は不変とする。これにより、誤差楕円そのものの大きさを大きくする（長軸、短軸の長さを変える）特許文献２の手法の比べて、より適切に誤差楕円を出力することができる。

上記各実施形態は本発明を限定する趣旨ではなく、本発明の要旨を変更しない範囲の様々な態様は、本発明に含まれる。例えば、移動体として自律走行するダンプトラック１を例に挙げたが、異なる種類の作業機械であってもよい。また、各処理の実行順序は上記実施形態に限定されず、後に続く処理を妨げない範囲で処理の前後を入れ替えてもよい。

１ ：ダンプトラック
２ ：車体フレーム
３ ：ベッセル
４ ：運転室
５ ：前輪
６ ：後輪
７ ：ＧＮＳＳアンテナ
１０ ：ＧＮＳＳセンサ
２０ ：積載状態センサ
３０ ：操舵角センサ
４０ ：勾配演算器
１００ ：位置算出装置
２１０ ：デッドレコニング装置
３００ ：自律走行制御装置
４００ ：走行駆動装置

Claims (6)

1. グローバル座標系で表された移動体のＧＮＳＳ出力位置を第１時点及びそれよりも遅い第２時点の各時点において逐次計測するＧＮＳＳセンサと、
   前記移動体の運動量及び姿勢を検出する移動体センサの検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、前記計測始点からの前記移動体の相対位置を逐次更新するデッドレコニング装置と、
   前記移動体の速度、重量、前記移動体が走行する路面の勾配の少なくとも一つにより定義された移動体状態を検出する移動体状態センサと、
   の其々に接続された、前記移動体の位置算出装置であって、
   前記位置算出装置は、一時記憶装置を含む位置算出コントローラを備え、
   前記位置算出コントローラは、
   前記第１時点におけるＧＮＳＳ出力位置、及び前記第１時点において前記デッドレコニング装置が算出した相対位置を用いて前記第１時点における前記移動体の最尤位置を計算し、
   前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点における最尤位置に足し合わせて、前記第２時点における仮定デッドレコニング位置を算出し、
   前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点におけるＧＮＳＳ出力位置に足し合わせて、前記第２時点における適応デッドレコニング位置を算出し、
   前記第２時点におけるＧＮＳＳ出力位置と前記第２時点における適応デッドレコニング位置との差からなる適応デッドレコニング誤差を算出すると共に、前記第２時点における前記移動体状態センサからの検出値を基に移動体状態を決定し、
   予め一時記憶装置に記憶された移動体状態と適応デッドレコニング誤差の平均値及び分散値とを関係付けた誤差統計量データのうち、前記決定された移動体状態に応じた適応デッドレコニング誤差の平均値及び分散値は、新たに算出された適応デッドレコニング誤差を用いて再計算して更新し、
   前記第２時点における仮定デッドレコニング位置を中心とし仮定デッドレコニング誤差の分散値からなる誤差楕円に前記第２時点における仮定デッドレコニング位置から前記再計算された適応デッドレコニング誤差の平均値分ずれた位置を中心とし、前記再計算された適応デッドレコニング誤差の分散値からなる誤差楕円を加えて存在可能性範囲を算出し、
   前記存在可能性範囲内に前記第２時点におけるＧＮＳＳ出力位置が含まれている場合には、当該ＧＮＳＳ出力位置を用いて前記第２時点における最尤位置及び当該最尤位置を中心とする最尤誤差楕円を算出し、前記存在可能性範囲の外に前記第２時点におけるＧＮＳＳ出力位置が存在する場合には、前記第２時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、
   前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、
   ことを特徴とする位置算出装置。
2. 請求項１に記載の位置算出装置において、
   前記位置算出コントローラは、前記移動体状態センサの前記第１時点における検出値を基に前記第１時点における第１移動体状態を算出し、前記移動体状態センサの前記第２時点における検出値を基に前記第２時点における第２移動体状態を算出し、前記第１移動体状態から前記第２移動体状態への遷移確率を算出し、当該遷移確率が予め定められた閾値よりも高い場合は、前記第１移動体状態において算出された適応デッドレコニング誤差の平均値及び分散値と、前記第２移動体状態において算出された適応デッドレコニング誤差の平均値及び分散値と、を前記第２時点で算出された適応デッドレコニング誤差を加えて更新する、
   ことを特徴とする位置算出装置。
3. 請求項１に記載の位置算出装置において、
   前記位置算出コントローラは、前記第２時点における前記ＧＮＳＳ出力位置と前記第２時点における仮定デッドレコニングとの乖離、又は前記第２時点における前記ＧＮＳＳ出力位置と前記第２時点における適応デッドレコニングとの乖離に基づいて、前記第２時点における前記ＧＮＳＳ出力位置の位置計測安定性を判断し、位置計測安定性はあると判断した場合にのみ、前記第２時点に出力された適応デッドレコニング誤差を用いて前記誤差統計量データを更新する、
   ことを特徴とする位置算出装置。
4. 請求項１に記載の位置算出装置と、
   前記位置算出装置が算出した位置を基に自律走行制御を行う自律走行制御装置と、前記自律走行制御装置からの制御信号に基づいて動作する走行駆動装置と、を備えたダンプトラックであって、
   前記移動体は前記ダンプトラックであり、
   前記位置算出装置は、前記最尤誤差楕円を前記位置範囲として前記自律走行制御装置に出力する、
   ことを特徴とするダンプトラック。
5. 請求項４に記載のダンプトラックにおいて、
   前記ダンプトラックは、
   速度センサと、
   加速度センサ又は速度センサの出力を基に前記ダンプトラックの加速度を演算する加速度演算器と、
   積荷の積載状態を検出する積載状態センサと、
   角速度センサと、
   前記ダンプトラックの直進旋回状態を検出する操舵角センサと、
   前記ダンプトラックの走行路の勾配を演算する勾配演算器と、のうちの少なくとも一つ以上を備え、
   前記位置算出コントローラは、前記ダンプトラックの速度、加速度、積載状態、角速度、操舵角、又は勾配の少なくとも一つ以上を用いて車両状態を決定し、前記ダンプトラックの車両状態毎に生じる適応デッドレコニング誤差の大きさを学習する、
   ことを特徴とするダンプトラック。
6. 請求項５に記載のダンプトラックにおいて、
   前記位置算出コントローラは、逐次変化する車両状態に応じて学習された適応デッドレコニング誤差の分散値を選択し、
   前記選択された適応デッドレコニング誤差の分散値が相対的に大きければ相対的に大きな最尤誤差楕円を生成し、前記選択された適応デッドレコニング誤差の分散値が相対的に小さければ相対的に小さな最尤誤差楕円を生成する、
   ことを特徴とするダンプトラック。
   

Images