位置算出装置及び運搬車両

本発明は、位置算出装置及び運搬車両に係り、特に鉱山用ダンプトラックの位置を慣性計測装置を用いた自律航法技術(Dead Reckoning)により算出する際の位置算出の精度向上に関する。

車両の位置算出をするための技術として、GPS(Global Positioning System)などのように地球上の実座標を測定する技術が用いられている。このGPSによる位置算出には、測位衛星から送波される測位電波の受信が不可欠である。

しかし、走行路周辺の構造物などにより測位電波が遮蔽されて、GPSによる位置算出が途絶えることがある。そこで、GPSによる位置算出を補間するための技術として、車両の運動状態を車輪速等から算出された速度及びジャイロセンサや加速度センサを含む慣性計測装置(IMU:Inertial Measurement Unit)からの出力を用いて地球上もしくは局所的な座標上の速度ベクトルとして表し、その速度ベクトルを積算することで位置を更新する自律航法技術が提案されている。

この自律航法技術では、車両に設置された加速度センサや角速度センサからの出力値を用いるので、これらの出力値の精度が位置算出精度に影響する。そこで、例えば特許文献1や特許文献2では、車両の傾きを加速度センサなどにより検知し、その傾きを用いて加速度センサやジャイロセンサの出力値を補正する技術が提案されている。

米国特許第6959240号明細書

米国特許第6895313号明細書

ところで、鉱山用の運搬車両として大型のダンプトラック(以下、「ダンプ」と略記する)が用いられている。鉱山用ダンプは、前後左右の車輪上にサスペンションを介して荷台を含む車体を搭載して構成され、荷台に重量が大きい鉱石や土砂などの積荷を積載して走行する。ここで荷台に搭載された積荷の重量バランスが不均衡な場合には、各サスペンションの伸縮量が独立して変化するので、積荷を積載した積載状態では車体が車輪に対して傾斜することがある。

この状況で慣性計測装置を車体に搭載して鉱山用ダンプの位置算出に自律航法技術を適用した場合に、ジャイロセンサや加速度センサからの出力に対し車体の傾斜が影響を与え、この影響が位置算出精度の低下をもたらすという課題がある。

この課題に対し、特許文献1及び特許文献2に記載されている技術は、積載状態における車体の傾斜を考慮していない補正技術であるので、上記課題を解決することができない。

本発明は上記に鑑みてなされたものであり、積載状態において車体に傾きが生じている場合でも、高精度に位置を算出することができる位置算出装置及び運搬車両を提供することを目的とする。

上記課題を解決するために、本発明は車輪及び当該車輪上にサスペンションを介して搭載された車体を含む運搬車両に搭載される位置算出装置であって、前記車輪に対する前記車体の姿勢を示す姿勢情報を出力する姿勢検出センサと、前記車輪の回転数を出力する車輪回転数センサと、前記車体に積荷が積載された積載状態であるか又は前記積荷が積載されていない空荷状態であるかを示す積載状態情報を取得する積載状態情報取得部と、前記姿勢情報を基に、前記積載状態において前記姿勢情報を出力する際に用いる検出軸を前記空荷状態において前記姿勢情報を出力する際に用いる検出軸に一致させるための補正量を前記積載状態情報と前記車輪回転数センサの出力とに基づき算出し、前記積載状態における前記運搬車両の速度ベクトルを算出するための補正量として設定する補正量設定部と、前記積載状態情報に基づいて前記車体が前記積載状態又は前記空荷状態であるかを判定し、前記車体が前記積載状態であると判定された場合に、前記車輪の回転数から得られる車輪速、前記姿勢情報から求まる前記運搬車両の運動方向、及び前記設定された補正量を用いて前記運搬車両の速度ベクトルを算出する速度ベクトル算出部と、前記速度ベクトルを用いて前記運搬車両の位置を算出する位置算出部と、を備えることを特徴とする。

これにより、積荷を積載して車体が車輪に対して傾斜した状態で出力された姿勢情報から求まる前記運搬車両の運動方向に対し、積載状態における検出軸を空荷状態における検出軸に一致させるための補正量を用いて補正を行うことで、傾斜による影響を取り除いた運動方向及び車輪速に基づく速度ベクトルが算出できる。よって、この速度ベクトルに基づいて算出された位置には、車両の傾斜による影響を取り除くことができ、積載状態においても運搬車両の位置算出精度を高精度に保つことができる。

また本発明は上記構成において、前記補正量設定部は、前記車輪回転数センサの出力に基づいて前記車両が移動中であると判定すると、前記算出された補正量を維持し、前記車両が停止中であり、前記積載状態情報に基づいて前記車体が前記空荷状態であると判定すると、前記算出された補正量を削除して単位行列からなる補正量を設定する、ことを特徴とする。

空荷状態では車輪に対して車体は傾斜していないので、姿勢情報から得られる運搬車両の運動方向には車体の傾斜の影響が含まれていない。そこで、空荷状態では車体の傾斜を補正するための補正量を削除して単位行列が補正量として適用することで、空荷状態で算出された運動方向を同方向の速度ベクトルを算出し、これを用いて積載状態における位置算出処理と同じ処理を適用して位置算出を行うことができる。

また本発明は上記構成において、予め算出された前記補正量を記憶する補正量記憶部を更に備え、前記補正量設定部は、前記車輪回転数センサの出力に基づいて前記車両が移動中であると判定すると、前記算出された補正量を維持し、前記車両が停止中であり、前記積載状態情報に基づいて前記車体が積載状態であると判定すると、前記補正量の算出に代えて、前記補正量記憶部から前記補正量を読み出して設定する、ことを特徴とする。

これにより、積載状態と判定された度に補正量を算出する場合と比べて、位置算出装置の処理負荷を下げることができる。

また本発明は上記構成において、前記積荷が積み込まれる積載地点、及び前記積荷を放土する放土地点を接続する走行経路を示す地図情報を記憶する地図情報記憶部を更に備え、前記積載状態情報取得部は、前記位置算出部が算出した前記運搬車両の位置、及び前記地図情報に基づき、前記積載地点又は前記放土地点に到達したかに基づいて前記積載状態又は前記空荷状態のいずれに相当するかを判定し、その判定結果を示す前記積載状態情報を生成する、ことを特徴とする。

これにより、位置算出部が算出した運搬車両の位置が積載地点に向かって移動している場合には空荷状態であり、放土地点に向かって移動している場合には積載状態であると判定することができる。

また本発明は上記構成において、前記積載状態情報取得部は、前記運搬車両に備えられた前記サスペンションの圧力を検出するサスペンション圧力センサからの出力、及び前記空荷状態又は前記積載状態を区別するための閾値を比較し、その比較結果に基づいて前記積載状態情報を生成する、ことを特徴とする。

これにより、空荷状態と積載状態とでサスペンションの圧力が異なるので、サスペンション圧力の異同を利用して積載状態であるか空荷状態であるかを判定することができる。

また本発明は、車輪及び当該車輪上にサスペンションを介して搭載された車体を含む運搬車両であって、前記車輪に対する前記車体の傾斜姿勢を示す姿勢情報を出力する姿勢検出センサと、前記車輪の回転数を出力する車輪回転数センサと、前記車体に積荷が積載された積載状態であるか又は前記積荷が積載されていない空荷状態であるかを示す積載状態情報を取得する積載状態情報取得部と、前記姿勢情報を基に、前記積載状態において前記姿勢情報を出力する際に用いる検出軸を前記空荷状態において前記姿勢情報を出力する際に用いる検出軸に一致させるための補正量を前記積載状態情報と前記車輪回転数センサの出力とに基づき算出し、前記積載状態における前記運搬車両の速度ベクトルを算出するための補正量として設定する補正量設定部と、前記積載状態情報に基づいて前記車体が前記積載状態又は前記空荷状態であるかを判定し、前記車体が前記積載状態であると判定された場合に、前記車輪の回転数から得られる車輪速、前記姿勢情報から求まる前記運搬車両の運動方向、及び前記設定された補正量を用いて前記運搬車両の速度ベクトルを算出する速度ベクトル算出部と、前記速度ベクトルを用いて前記運搬車両の位置を算出する位置算出部と、を備えることを特徴とする。

これにより、積荷を積載して車体が車輪に対して傾斜した状態で出力された姿勢情報から求まる前記運搬車両の運動方向に対し、積載状態における車体姿勢の検出軸を空荷状態における車体姿勢の検出軸に一致させるための補正量を用いて補正を行うことで、傾斜による影響を取り除いた向き及び車輪速に基づく速度ベクトルが算出できる。よって、この速度ベクトルに基づいて算出された位置には、車両の傾斜による影響を取り除くことができ、積載状態においても運搬車両の位置算出精度を高精度に保つことができる。

積載状態において車体に傾きが生じている場合でも、高精度に位置を算出することができる位置算出装置及び運搬車両を提供することができる。上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

鉱山内の概略構成を示す図

空荷状態及び積載状態の車体姿勢を示す図であり、(a)は空荷状態を示し、(b)は積載状態を示す。

位置算出装置の内部構成を示す機能ブロック図

アプローチ地図の一例を示す図であって、(a)はリンク情報を示し、(b)はノード情報を示す。

位置算出処理の全体の流れを示すフローチャート

第一実施形態に係る積載状態判定処理の流れを示すフローチャート

車両停止判定処理の流れを示すフローチャート

第一実施形態に係る補正量設定処理の流れを示すフローチャート

第二実施形態における補正量設定処理の流れを示すフローチャート

第三実施形態に係るダンプの内部構成を示す機能ブロック図

第三実施形態に係る積載状態判定処理の流れを示すフローチャート

以下、図面を参照して本発明の実施形態について説明する。以下の実施の形態においては、便宜上その必要があるときは、複数のセクションまたは実施の形態に分割して説明する。以下の実施の形態において、要素の数等(個数、数値、量、範囲等を含む)に言及する場合、特に明示した場合および原理的に明らかに特定の数に限定される場合等を除き、その特定の数に限定されるものではなく、特定の数以上でも以下でもよい。なお、以下の実施の形態において、その構成要素(処理ステップ等も含む)は、特に明示した場合および原理的に明らかに必須であると考えられる場合等を除き、必ずしも必須ではない。

また、以下の実施の形態における各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路その他のハードウェアとして実現しても良い。また、後述する各構成、機能、処理部、処理手段等は、コンピュータ上で実行されるプログラムとして実現しても良い。すなわち、ソフトウェアとして実現しても良い。各構成、機能、処理部、処理手段等を実現するプログラム、テーブル、ファイル等の情報は、メモリやハードディスク、SSD(Solid State Drive)等の記憶装置、ICカード、SDカード、DVD等の記憶媒体に格納することができる。

以下、本発明の実施の形態を図面に基づいて詳細に説明する。なお、実施の形態を説明するための全図において、同一の機能を有する部材には同一または関連する符号を付し、その繰り返しの説明は省略する。また、以下の実施の形態では、特に必要なとき以外は同一または同様な部分の説明を原則として繰り返さない。

<第一実施形態> 図1を参照して、本実施形態に係る位置算出装置を搭載した運搬車両としての鉱山用ダンプ(以下「ダンプ」と略記する)が走行する鉱山内の概略構成について説明する。図1は、鉱山内の概略構成を示す図である。

図1に示すように、鉱山内では、積込場61(積載地点に相当)及び放土場62(放土地点に相当)を接続する走行経路60が設けられる。積込場61では土砂や鉱石の積込作業を行うショベル10が掘削作業を行う。そして、ダンプ20−1、20−2は、積込場61においてショベル10から土砂や鉱石等の積荷を積込まれて積載状態となり、この積載状態で走行経路60に沿って放土場62に向かって走行する。ダンプ20−1、20−2は、放土場62に到着すると積荷を放土して空荷状態(非積載状態)になる。そしてダンプ20−1、20−2は空荷状態で積込場61に向けて走行する。

ダンプ20−1、20−2が、運転手が搭乗することなく自律走行する無人ダンプである場合には、各ダンプ20−1、20−2は管制センタ30に設置された車両管制サーバ31に無線通信回線40を介して互いに通信接続される。そして、ダンプ20−1、20−2は車両管制サーバ31から管制制御に従って走行する。図1の符号32は、車両管制サーバ31に接続される無線アンテナであり、符号41−1、41−2、41−3は無線移動局を示す。

ダンプ20−1、20−2が、運転手が搭乗し、運転手の操舵に従って走行する有人ダンプである場合は、車両管制サーバ31による管制制御は必須ではないが、管制制御の下で走行することでより安全性を確保するようにしてもよい。以下の説明では、ダンプ20−1、20−2を区別しない場合は、ダンプ20と記載する。

ダンプ20は、全地球航法衛星システム(GNSS:Global Navigation System)の少なくとも3つの航法衛星50−1、50−2、50−3から測位電波を受信して自車両の位置を取得するための位置算出装置(図1では図示を省略する)を備える。GNSSとして、GPSの他、GLONASS(Global Navigation Satellite System)、GALILEOを用いてもよい。

更に本実施形態に係るダンプ20の位置算出装置は、GNSSを補間して位置算出の精度を向上させるために、自律航法技術を併用する。本実施形態は自律航法技術による位置算出に際して車体姿勢の影響を低減する構成に特徴がある。以下、図2を参照して、空荷状態及び積載状態の車体姿勢について説明する。図2は、空荷状態及び積載状態の車体姿勢を示す図であり、(a)は空荷状態を示し、(b)は積載状態を示す。

図2の(a)、(b)に示すように、ダンプ20は、車輪と車輪上に搭載される車体22とを含む。車輪及び車体(フレームともいう)22との間には、サスペンションが配置される。説明の便宜のため、図2では、左前輪21FL及び左後輪21RLと、左前輪21FL及び左後輪21RLのそれぞれと車体22とを接続する左前サスペンション23FL及び左後サスペンション23RLのみを記載しているが、ダンプ20は、右前輪、右後輪、及び右前輪、右後輪のそれぞれと車体22とを接続する右前サスペンション及び右後サスペンションも含む。以下の説明において、前後左右の車輪、サスペンションをそれぞれ総称する場合には車輪21、サスペンション23と記載する。

に車体22の上には、積荷を積載する荷台24が搭載される。以下の説明において、車輪21を含めたダンプ20の全構成要素を車両と呼ぶことにする。

車体22には、運動を司る3軸(以下「検出軸」という)の角度(または角速度)と加速度を検出する慣性計測装置(IMU)100を搭載する。IMU100は、車体22にかかる重力方向を含めた加速度を計測する加速度センサ101と、車体22の回転角速度を計測するジャイロセンサ102とを含み、これらが車体22の重心付近に備えられる。以下の説明において加速度センサ101からの出力(加速度)とジャイロセンサ102からの出力(角速度)とを総称してIMU出力値という。上記IMUは姿勢検出センサに相当し、IMU出力値が姿勢情報に相当する。

また、車輪21には、車輪の回転数を計測する車輪回転数センサ103が取り付けられる。

図2の(a)に示すように、空荷状態では、車体22は車輪21に対して傾きを有しておらず、車体22に取り付けられた加速度センサ101、ジャイロセンサ102の検出軸(x軸、y軸、z軸)30は車両の運動の前後方向軸及び左右方向軸と一致する。つまり、検出軸30は、空荷状態において姿勢情報を出力する際に用いる検出軸である。

これに対し、図2の(b)に示すように、積載状態では、荷台24に積載された積荷の重量が車体22に対して均一にかからず、車体22の前後左右方向で重量バランスが不均衡となることがある。重量バランスの不均衡が生じた場合、4つのサスペンション23がそれぞれ独立して伸縮することで車体バランスの調整を行う結果、車輪21に対して車体22が傾斜する。以下、重力方向と垂直に交わる面を水平面と定義して水平面と車両前方軸と成す角度をピッチ角φ、水平面と車両側方軸との成す角度をロール角ψと定義する。図2の(b)は車体22の前部よりも後部に重量がかかる、いわゆるリアヘビーな状態を示す。この場合、左後サスペンション23RL、及び右後サスペンション(不図示)が収縮し、左前サスペンション23FL及び右前サスペンション(不図示)が伸張する。その結果、車体22がx軸(図2の(a)参照)に対してピッチ角φ分傾き、x軸がx’軸に変位する。

また、車体22がy軸(図2の(a)参照)に対してロール角ψ分傾くと、y軸がy’軸に変位する。z’軸は、x’軸及びy’軸の2軸に直交する軸であり、z軸が変位した軸を示す。これら積載状態における検出軸31は、積載状態において姿勢情報を出力する際に用いる検出軸であって、空荷状態の検出軸30に対してピッチ角φ、ロール角ψ分、傾いている。

本実施形態では、検出軸31からIMU出力値が示す車体傾斜の影響を取り除き、ダンプ20の実際の運動を示す速度ベクトルを算出する位置算出装置を備える。以下、図3を参照して位置算出装置の内部構成について説明する。図3は、位置算出装置の内部構成を示す機能ブロック図である。

図3に示すように位置算出装置110は、加速度センサ101及びジャイロセンサ102を含むIMU100と、車輪21の回転数を計測する車輪回転数センサ103と、車体22が積載状態又は空荷状態か示す積載状態情報を生成又は取得する積載状態情報取得部111と、IMU出力値を基に、積載状態における車体姿勢の検出軸を空荷状態における車体姿勢の検出軸に一致させるための補正量を算出し、積載状態における自車両の速度ベクトルを算出するための補正量として設定する補正量設定部112と、積載状態情報に基づいて車体22が積載状態又は空荷状態であるかを判定し、車体22が積載状態であると判定された場合に、車輪21の回転数から得られる車輪速及びIMU出力値から求まる向き、及び、設定された補正量を用いてダンプ20の速度ベクトルを算出する速度ベクトル算出部113と、速度ベクトルを用いてダンプ20の位置を算出する位置算出部114と、を含む。

また第一実施形態では、積載状態情報取得部111はIMU出力値を用いて積載状態又は空荷状態を判定する積載判定部111aと、積荷が積み込まれる積載地点、及び積荷を放土する放土地点を接続する走行経路を示す地図情報(アプローチ地図)を記憶する地図情報記憶部111bと、を含む。アプローチ地図は、車両の積載、非積載が変化する地点(積載地点及び放土地点)までの経路の繋がりを点(以下、「ノード」と記載する)と線(以下、「リンク」と記載する)で表したトポロジカル地図である。

また補正量設定部112は、ダンプ20が停止中又は移動中かを判定する停止判定部112a、IMU出力値を用いて空荷状態の車体姿勢を基準としたときの車体22の傾斜角度を随時計算する車体傾斜算出部112bと、積載量の変化による車体の傾きの変化量を補正するための補正量を逐次計算する補正量算出部112cと、を含む。 なお、位置算出装置110には、GPSからの測位電波を受信して位置を算出するユニットを含み、本実施形態に係る自律航法技術による位置算出と併用して最終的なダンプ20の位置を決定してもよいが、GPSユニットについては図示を省略する。

図4を参照してアプローチ地図の構成例について説明する。図4は、アプローチ地図の一例を示す図であって、(a)はリンク情報を示し、(b)はノード情報を示す。

アプローチ地図は、車両が積載地点もしくは放土地点までに通るべき位置であるノードとそのつながりを示すリンクで表されている。図4の(a)に示すように、アプローチ地図にはリンクを識別するためのリンクID401とそのリンクが始まる座標にあるノードID402、リンクの終わる座標にあるノードID403、リンク始点に接続される別のリンクID404(複数存在する場合もある。)、及びリンク終点に接続される別のリンクID405(複数存在する場合もある。)を規定するテーブルがある。更に図4の(b)に示すように、ノードID406に対応する座標値407、その属性408のテーブルが存在する。このノードIDのうち、いずれかを積載地点もしくは放土地点として属性408に設定されている。

次に図5を参照し、第一実施形態に係る位置の算出処理の概略について説明する。図5は、位置算出処理の全体の流れを示すフローチャートである。

以下図5の各ステップ順に沿って説明するが、位置の算出処理の開始にあたり、ダンプ20の初期位置は与えられているものとする。この状態で、ダンプ20のエンジンが始動すると(S501)、加速度センサ101、ジャイロセンサ102、車輪回転数センサ103はある決められた周期で車体の重力を含めた加速度、回転角速度および車輪の回転数の出力が行われる(S502)。各センサからの出力は、エンジンが停止するまで継続的に行われる。

積載判定部111aがセンサの出力値を取得し、積載状態の変化の有無、及び変化がある場合には空荷から積荷に変化したか、又は積荷から空荷状態へ変化したかを判定する積載状態判定処理を行う(S503)。

次いで、停止判定部112aは、ダンプ20が移動中が停止中かを判定する車両停止判定処理を行う(S504)。

補正量設定部112は、停止判定部112aによる停止中又は移動中の判定結果、及び積載状態の変化の有無及び状態変化がある場合に状態変化の内容に応じて、速度ベクトルを算出するための補正量設定処理を行う(S505)。補正量の設定に際し、新たな補正量を算出する必要がある場合、車体傾斜算出部112bは、加速度センサ101及びジャイロセンサ102から出力された加速度及び回転速度を用いて車体傾斜角度を算出し、これを用いて補正量算出部112cが新たな補正量を計算して設定する。

速度ベクトル算出部113は、ステップS505で設定された補正量を用いて、車両の速度ベクトルを算出する(S506)。

位置算出部114は算出された速度ベクトルを用いて位置算出処理を行う(S507)。その後、エンジンが停止するまでステップS501へ戻り、ステップS501からS507までの処理を繰り返す。

図6を参照して積載状態判定処理について説明する。図6は、第一実施形態に係る積載状態判定処理の流れを示すフローチャートである。

図6に示すように、積載判定部111aは、現在の積載状態フラグを取得する(S601)。積載状態フラグは1を積載状態、0を空荷状態とする。

次に積載判定部111aは、前サンプリングタイムに計算した車両位置を取得して、現在位置として設定する(S602)。この現在位置は、目的地までの距離Lの計算に用いられる。

積載判定部111aは、現在走行中の経路の最終到達点を地図情報記憶部111bに格納されたアプローチ地図から検索し、目的地が積載地点であるか放土地点であるかを判定する。目的地が積載地点である場合(S603/目的地が積載地点)は、前回算出された位置(初回の場合は初期位置)から目的地、すなわちアプローチ地図おける積載地点までの距離Lを計算する(S604)。また、目的地が放土地点である場合(S603/目的地が放土地点)は、放土地点と現在位置の距離Lを計算する(S605)。

距離Lの計算が終了すると、積載判定部111aが距離Lと、目的地に到達したと見做せる距離により定義された距離閾値とを比較する(S606)。そして、積載判定部111aが、距離Lは距離閾値よりも小さいと判定すると(S606/Yes)、これから積載量が変化するとして、積載状態フラグをステップS601で取得した値と反転させる。つまり、現在の積載状態フラグの値が1であれば0に、0であれば1にすればよい(S607)。また、距離Lが距離閾値以上である場合は(S606/No)、積載量の変化は無いものとして、ステップS701で得られた積載状態フラグの値を維持する(S608)。積載状態フラグが設定されれば積載判定部111aによる積載状態判定処理を終了する。

次に図7を参照して車両停止判定処理について説明する。図7は、車両停止判定処理の流れを示すフローチャートである。

図7に示すように、停止判定部112aは、ステップS502で各センサからの出力値を取得すると、入力された現在のサンプリングも含めて、N秒間の加速度センサ101、ジャイロセンサ102、車輪回転数センサ103の出力値を内部メモリに保存する(S701)。

次に停止判定部112aは、N秒間の車輪速度出力が全て速度が無い状態であるかを判定する(S702)。車輪回転数出力値(車輪速)に速度が出ている状態であると判定すると(S702/No)、停止判定フラグに0を設定し(S703)、停止判定部112aによる処理を終了する。

停止判定部112aは、車輪回転数出力値(車輪速)が全て速度が出ていない状態であると判定すると(S702/Yes)、N秒間のIMU出力値の平均値、分散値、およびN秒間の全サンプルから最小二乗法により求められた傾き値(1階微分値)を計算する(S704)。

停止判定部112aは、加速度センサ101、ジャイロセンサ102各々のN秒間出力値の平均値の絶対値が、停止していると判定するための加速度閾値以下及び角速度閾値以下であるかを判定し、加速度の平均値が加速度閾値よりも大きく、かつ角速度の平均値が角速度閾値よりも大きい場合(S705/No)は、ステップS703に移行し、停止判定フラグに0を設定して、停止判定部112aの処理を終了する。一方、停止判定部112aが、加速度の平均値は加速度閾値以下であり、角速度の平均値も角速度閾値以下であると判定すると(S705/Yes)ステップS706へ移行する。

停止判定部112aは、加速度センサ101、ジャイロセンサ102各々のN秒間の全サンプル値の最小二乗法により求められた傾きの絶対値が、停止していると判定するための傾き閾値以下であるかを判定し、傾きの絶対値が傾き閾値よりも大きいと判定すると(S706/No)ステップS703に移行し、停止判定フラグに0を設定し、停止判定部112aの処理を終了する。一方、停止判定部112aが、傾きの絶対値は傾き閾値以下であると判定すると(S706/Yes)ステップS707へ移行する。

停止判定部112aは、加速度センサ101、ジャイロセンサ102各々のN秒間の分散値の絶対値が停止していると判定するための分散値閾値以下であるかを判定し、分散値の絶対値が分散値閾値よりも大きいと判定すると(S707/No)、ステップS603に移行し、停止判定フラグに0を設定し、停止判定部112aの処理を終了する。一方、停止判定部112aが分散値の絶対値が分散値閾値以下であると判定すると(S707/Yes)、停止判定フラグに1を設定し(S708)、停止判定部112aの処理を終了する。なお、ステップS705、S706、S707の順序は入れ替わっても良い。

積載状態判定処理が終了すると、補正量設定処理(S505)へ移行する。次に図8を参照して補正量設定処理について説明する。図8は、第一実施形態に係る補正量設定処理の流れを示すフローチャートである。

図8に示すように、車体傾斜算出部112bは、そのサンプリング時の車体の傾斜角度(ロール角ψ、ピッチ角φ)を計算する(S801)。車体傾斜算出部112bにて算出された車体傾斜角度は、現在のサンプル時間含め、あるサンプリング時間分保存される。車体の傾斜は下式(1)、式(2)以下の方法で計算できる。 [数1] (ロール角ψ)=arcsin[{(加速度センサ横方向出力値)−(横加速度)/(重力加速度)}・・・(1) [数2] (ピッチ角φ)=arcsin[{(加速度センサ前後方向出力値)−(車両加速度)/(重力加速度)}・・・(2)

ただし、停止判定部112aの結果により車両が停止している場合はロール角ψ、ピッチ角φ共に前サンプリング時の値をそのまま引き継ぎ、車両が運動していると判定された場合には車輪回転数センサ103の出力から計算した車輪速の微分値を加速度として用いて計算する。また、式(1)の横加速度は車両(ダンプ20)の旋回半径とジャイロセンサ102から得られるヨー角速度から、下式(3)により求めることができる。 [数3] (横加速度)=(旋回半径)×(ヨー角速度)²・・・(3)

車体22の傾斜角度を算出すると、車体傾斜算出部112bによる車体傾斜算出処理(S801)を終了する。

車体22の傾斜角度の算出が終了すると、補正量算出部112cは、ステップS703、S708で設定された停止判定フラグの値を参照し、車両が停止しているかどうかを確認する。車両が移動している場合(停止判定フラグの値0)(すでに/移動中)ステップS803に移行し、前サンプリング時に算出した補正量を維持し(S803)、この維持した補正量を速度ベクトル算出のための補正量として設定して補正量設定処理を終了する。

補正量算出部112cは、車両が停止していると判定した場合(停止判定フラグの値1:S802/停止中)は、ステップS804へ移行し、積載判定部111aから出力される積載状態フラグを用いて積荷状態が変化したかどうかを確認する。前サンプリングにおける積載状態フラグが今回のサンプリング時と同じであれば(S804/変化なし)ステップS805に移行する。

補正量算出部112cは、現在の積載状態フラグが積荷(積載状態フラグの値1)であれば(S805/Yes)ステップS806に移行し、逐次最小二乗法により補正量Mを算出する(S806)。以下、補正量算出部112cによる補正量Mの算出方法について説明する。

まず、補正量算出部112cは、加速度センサ101のx軸、y軸、z軸からそれぞれ(αx、αy、αz)の出力を取得する。車体傾斜算出部112bからの出力φ、ψを用いて、重力加速度ベクトル(0、0、g)との関係を表すと、下式(4)のように表される。このことを利用して、比例係数行列Aを逐次最小二乗法にて求める。そして、現在のサンプリング時における比例係数行列Aの逆行列を下式(5)のように計算することで補正量Mを求めることができる。

補正量算出部112cは、最終結果として得られた補正量Mを保存して処理を終了する。

補正量算出部112cは、現在の積載状態フラグが空荷である(積載状態フラグの値0)と判定すると(S805/No)ステップS807に移行し、現在設定されている補正量Mを消去するとともに新たな補正量として単位行列を設定し(S807)、補正量算出部112cの処理を終了する。

また、ステップS804において、前サンプリングにおける積載状態フラグが空荷から積荷に変化した場合は(S804/空荷→積荷)、ステップS806に移行し、逐次最小二乗法により補正量となる行列(補正量M)を算出し、最終結果を保存して補正量算出部112cの処理を終了する。

また、ステップS804において、前サンプリングにおける積載状態フラグが積荷から空荷に変化した場合は(S804/積荷→空荷)、ステップS807に移行し、設定中の補正量を削除して新たな補正量として単位行列を設定し、補正量算出部112cの処理を終了する。

補正量設定部112における補正量設定処理が終了すると、速度ベクトル計算処理(S506)に移行する。速度ベクトル算出部113は、ジャイロセンサ102の出力ω、および補正量設定部112にて設定された補正量M又は単位行列、車体傾斜算出部112bにて算出されたロール角ψ、ピッチ角φおよび、前サンプリング時に算出されているヨー角θから、以下のように速度ベクトルを算出する。(ただし、ヨー角の初期値は与えられているものとする。)

補正後加速度出力値α’は補正量Mと元の加速度センサ出力値αより下式(6)のように表される。

また、補正後角速度出力値ω’は補正量Mと元のジャイロセンサ出力値ωより下式(7)のように表される。

これら補正後の加速度、角速度より、空荷状態の検出軸30(図2の(a)参照)と一致するように積載状態の検出軸31(図2の(b)参照)を補正した後の補正後の回転角(補正ロール角ψ’、補正ピッチ角φ、補正後角速度を考慮したヨー角θ)は下式(8)、(9)、(10)により求めることができる。なお、補正量Mとして単位行列が設定されている場合には、補正後加速度出力値α’は加速度センサ101からの出力αに一致し、補正後角速度出力値ω’はジャイロセンサ102からの出力ωに一致するので、以下の説明において補正後加速度を加速度センサの出力、補正後角速度をジャイロセンサの出力と読み替えればよい。 [数8] (補正ロール角ψ’)=arcsin[{(補正後加速度:横方向)−(車両横加速度)/(重力加速度)}・・・(8) [数9] (補正ピッチ角φ’)=arcsin[{(補正後加速度:前後方向)−(車両加速度)/(重力加速度)}・・・(9) [数10] (ヨー角θ)=(前サンプリング時ヨー角)+(補正後角速度:垂直軸)×(サンプリング時間)・・・(10)

これら回転角より、速度方向の回転マトリクスCは、下式(11)により求まる。

速度方向の回転マトリクスCが求まれば、車輪回転数センサ出力値sから、速度ベクトルVは下式(12)により求まる。

上記仮速度ベクトルは、車輪回転数センサ103から出力される車輪回転数から算出された速さs(スカラー量)、及び積載状態で検出された加速度センサ101及びジャイロセンサ102の出力を用いて算出したIMU出力値を基に算出した車両の運動方向の向きを有する補正前の速度ベクトルである。

速度ベクトル算出部113にて速度ベクトルVが算出されると、ステップS507において位置算出部114が下式(13)により、位置を更新する。 [数13] (更新位置)=(前サンプリング時の位置)+(速度ベクトル)×(サンプリング時間)・・・(13)

本実施形態によれば、空荷状態の車体姿勢の検出軸に対し、荷物を積載することにより車体が傾斜し、積載状態の車体姿勢の検出軸にずれが生じた場合にもこれらを一致させるための補正量を計算し、この補正量を用いて車両の速度ベクトルを算出することができるので、車輪速及び車体姿勢(車体の向き)を用いた位置算出を高精度化することができる。また、空荷時は補正量として単位行列を用いることにより、積載状態と同じ速度ベクトルの算出処理を用いつつ、両検出軸が一致させた状態で位置算出を行うことができる。

また、オペレータが搭乗しないで自律走行をする無人ダンプでは、自律走行制御のために走行経路の地図情報を搭載していることが多いので、この地図情報を本実施形態に係る位置算出装置に流用することで、既存のリソースを用いて位置算出精度の向上を図ることができる。

<第二実施形態> 次に、第二実施形態について図9を参照して説明する。第一実施形態は、空荷状態から積載状態に遷移した場合に毎回逐次最小二乗法で補正量を算出したが(図8のステップS806参照)、第二実施形態では、空荷状態から積載状態に遷移した場合の車体の傾斜角度が毎回殆ど変わらないという前提で、最初に車体傾斜を補正するための補正量Mを一意に決める実施形態である。ダンプ20の最大積載容量は決まっているので、ダンプ20が満載か空荷かのいずれかの状態で稼働し、満載時の積載状態がほぼ一定で荷台24の左右又は前後の一方側に極端に偏ることなく積載される場合に、本実施形態は好適である。

第二実施形態の処理の流れは補正量設定部112での処理以外は第一実施形態と同じであるため、補正量設定処理についてのみ説明する。図9は第二実施形態における補正量設定処理の流れを示すフローチャートである。

図9に示すように、第二実施形態に係る補正量設定処理では、補正量設定部112内の補正量算出部112cが積載状態の検出軸を空荷状態の検出軸に一致させるための補正量Mが記憶されているかを判定し、記憶されていない場合(S901/No)には、ダンプ20に積荷を積載した状態で補正量Mを算出し、記憶する(S902)。ステップS902の補正量Mの算出方法は、第一実施形態と同様である。

補正量Mが記憶されている場合(S901/Yes)、及び補正量Mの算出が終了すると(S902)、補正量算出部112cは車両が停止しているかどうかを判定(S802)する。以下、ステップS802からステップS805、ステップS807の処理は第一実施形態と同様であるので重複説明を省略する。

補正量算出部112cは、ダンプ20が停止中(S802/Yes)で、積荷状態が空荷状態から積荷状態に変化した場合(S804/空荷から積荷)、又は積載状態に変化がなく(S804/変化なし)かつ現在の状態が積載状態の場合(S805/Yes)、ステップS903に移行する。

補正量算出部112cは、予め算出された補正量を記憶する補正量記憶部から補正量を読み出し、速度ベクトルを算出するための補正量として設定する(S903)。

本実施形態によれば、車体の傾斜角度を補正するために毎回の補正量設定処理を行うことなく、一意に定めた補正量Mを用いることで、位置算出装置の処理負荷を軽減することができる。

<第三実施形態> 第三実施形態は、積載状態判定処理においてアプローチ地図を使う代わりに、車両のサスペンション圧を用いる実施形態である。その他の構成・処理内容は第一実施形態及び第二実施形態と同様であるので説明を省略する。以下、図10及び図11を参照して第三実施形態について説明する。図10は、第三実施形態に係るダンプの内部構成を示す機能ブロック図である。図11は、第三実施形態に係る積載状態判定処理の流れを示すフローチャートである。

図10に示すように、第三実施形態に係るダンプ20には、左前サスペンション、右前サスペンション、左後サスペンション、右後サスペンションには、各サスペンション圧力を検知するサスペンション圧力センサ23sが備えられる。そして、位置算出装置内の積載判定部111aとサスペンション圧力センサとは電気的に接続され、サスペンション圧力センサの出力が積載判定部111aに入力される。

図11に示すように、積載判定部111aは現在の積載状態フラグを取得する(S1101)。次いで積載判定部111aは、前サンプリング時にサスペンション圧力センサにて計測されたサスペンション圧を記憶装置から読み出す(S1102)。次いで積載判定部111aは、今回のサンプリング時に計測したサスペンション圧を取得する(S1103)。

積載判定部111aは、ステップS1103で取得した現在のサスペンション圧とステップS1102で読み出した前サンプリング時のサスペンション圧の差の絶対値を計算し、その絶対値(サスペンション圧変化量)が圧変化量閾値より大きいかを判定する(S1103)。この圧変化量閾値は、積載状態から空荷状態へ、又は空荷状態から積載状態へと遷移したかを判定するための閾値であって、サスペンション圧の変化量を用いて定義される。サスペンション圧変化量が圧変化量閾値以下の場合(S1104/No)、積載判定部111aは積載状態に変化なしと判定し、現在の積載状態フラグを維持して(S1105)、積載判定部111aの処理を終了する。

サスペンション圧変化量が圧変化量閾値よりも大きい場合(S1104/Yes)、積載判定部111aは現在のサスペンション圧が予め決められている圧閾値よりも小さいかを判定する(S1106)。この圧閾値は、空荷状態か積載状態かを判定するための閾値であって、サスペンション圧により定義される。圧閾値は、空荷におけるサスペンション圧の最大値を設定してもよい。

サスペンション圧の絶対値が圧閾値以上の場合(S1106/No)、積載判定部111aは、積載状態フラグを積載に設定する(S1107)。圧閾値未満の場合(S1106/Yes)、積載判定部111aは、積載状態フラグを空荷に設定する(S1108)。

本実施形態によれば、アプローチ地図を用いずに積載状態の判定処理が行えるので、例えば、管制制御が不要な有人ダンプであってアプローチ地図を搭載していない場合にも積載判定を行い、その結果を用いて本発明に係る位置算出処理が適用できる。

また、運搬車両の異常検出のためにサスペンション圧力センサが搭載されている場合には、この出力を本実施形態に係る位置算出装置に流用することで、既存のリソースを用いて位置算出精度の向上を図ることができる。

積載判定部111aによる処理の他例として、現在位置と積載地点・放土地点との関係を管制局にて監視し、管制局からの通信にて積載の有無を判定する方法が挙げられる。この場合、車両側ではアプローチ地図の変わりに管制局との通信装置を有し、管制局から積載状態フラグを受け取ることになる。

また、有人ダンプでは、積載、空荷を示す積載状態を位置算出装置に接続された入力装置を有人ダンプの運転席に設け、運転手が入力装置から積載又は空荷の状態信号を入力し、積載判定部111aが入力信号に従って積載状態フラグの値を設定するように構成してもよい。

以上、本発明を実施するための実施の形態について説明したが、本発明の具体的な構成は上記各実施の形態のみに限定されるものではなく、発明の要旨を逸脱しない範囲の設計変更等があっても本発明に含まれる。例えば、上記では姿勢検出センサとして角速度センサ及びジャイロスコープを用い、これらの出力を姿勢検出パラメータとして用いて車体の傾斜角度を算出したが、姿勢検出センサは角速度センサ及びジャイロスコープに限らず傾斜計を用いて傾斜角度を求めてもよい。この場合、ピッチ角を検出するY軸(車両側方軸)を回転中心とする傾斜計と、ロール角を検出するX軸(車両前方軸)を回転中心とする傾斜計とを備えてもよい。また、ヨー角は、方位センサからの出力により求めてもよい。

20 ダンプ 21 車輪 22 車体 23 サスペンション 101 加速度センサ 102 ジャイロセンサ 103 車輪回転数センサ 110 位置算出装置