JP2016053553A - 位置算出装置及び運搬車両 - Google Patents

Abstract

【課題】 積載状態において車体に傾きが生じている場合でも、高精度に位置を算出する。【解決手段】車輪２１にサスペンション２３を介して搭載した車体２２に固定された姿勢検出センサ１０１、１０２と、車輪の回転数を出力する車輪回転数センサ１０３と、車体２１が積載状態又は空荷状態かを示す積載状態情報を取得する積載状態情報取得部１１１と、姿勢情報を基に、積載状態における車体姿勢の検出軸を前記空荷状態における車体姿勢の検出軸に一致させるための補正量を算出して設定する補正量設定部１１２と、積載状態において、車輪の回転数及び姿勢情報から求まる向きを用いて定義された仮速度ベクトルに対し、設定された補正量を用いた補正を行って速度ベクトルを算出する速度ベクトル算出部１１３と、速度ベクトルを積算して運搬車両の位置を算出する位置算出部１１４と、を備える。【選択図】図３

Description

本発明は、位置算出装置及び運搬車両に係り、特に鉱山用ダンプトラックの位置を慣性計測装置を用いた自律航法技術（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ）により算出する際の位置算出の精度向上に関する。

車両の位置算出をするための技術として、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）などのように地球上の実座標を測定する技術が用いられている。このＧＰＳによる位置算出には、測位衛星から送波される測位電波の受信が不可欠である。

しかし、走行路周辺の構造物などにより測位電波が遮蔽されて、ＧＰＳによる位置算出が途絶えることがある。そこで、ＧＰＳによる位置算出を補間するための技術として、車両の運動状態を車輪速等から算出された速度及びジャイロセンサや加速度センサを含む慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）からの出力を用いて地球上もしくは局所的な座標上の速度ベクトルとして表し、その速度ベクトルを積算することで位置を更新する自律航法技術が提案されている。

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

米国特許第６９５９２４０号明細書 米国特許第６８９５３１３号明細書

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

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

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

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

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

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

また本発明は上記構成において、前記補正量設定部は、前記積載状態情報に基づいて前記車体が前記空荷状態であると判定すると、前記算出された補正量を削除して単位行列からなる補正量を設定する、ことを特徴とする。

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

また本発明は上記構成において、予め算出された前記補正量を記憶する補正量記憶部を更に備え、前記補正量設定部は、前記積載状態情報に基づいて前記車体が積載状態であると判定すると、前記補正量の算出に代えて、前記補正量記憶部から前記補正量を読み出して設定する、ことを特徴とする。

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

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

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

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

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

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

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

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

鉱山内の概略構成を示す図 空荷状態及び積載状態の車体姿勢を示す図であり、（ａ）は空荷状態を示し、（ｂ）は積載状態を示す。 位置算出装置の内部構成を示す機能ブロック図 アプローチ地図の一例を示す図であって、（ａ）はリンク情報を示し、（ｂ）はノード情報を示す。 位置算出処理の全体の流れを示すフローチャート 第一実施形態に係る積載状態判定処理の流れを示すフローチャート 車両停止判定処理の流れを示すフローチャート 第一実施形態に係る補正量設定処理の流れを示すフローチャート 第二実施形態における補正量設定処理の流れを示すフローチャート 第三実施形態に係るダンプの内部構成を示す機能ブロック図 第三実施形態に係る積載状態判定処理の流れを示すフローチャート

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

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

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

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

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

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

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

ダンプ２０は、全地球航法衛星システム（ＧＮＳＳ：Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）の少なくとも３つの航法衛星５０−１、５０−２、５０−３から測位電波を受信して自車両の位置を取得するための位置算出装置（図１では図示を省略する）を備える。ＧＮＳＳとして、ＧＰＳの他、ＧＬＯＮＡＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）、ＧＡＬＩＬＥＯを用いてもよい。

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

図２の（ａ）、（ｂ）に示すように、ダンプ２０は、車輪と車輪上に搭載される車体２２とを含む。車輪及び車体（フレームともいう）２２との間には、サスペンションが配置される。説明の便宜のため、図２では、左前輪２１ＦＬ及び左後輪２１ＲＬと、左前輪２１ＦＬ及び左後輪２１ＲＬのそれぞれと車体２２とを接続する左前サスペンション２３ＦＬ及び左後サスペンション２３ＲＬのみを記載しているが、ダンプ２０は、右前輪、右後輪、及び右前輪、右後輪のそれぞれと車体２２とを接続する右前サスペンション及び右後サスペンションも含む。以下の説明において、前後左右の車輪、サスペンションをそれぞれ総称する場合には車輪２１、サスペンション２３と記載する。

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

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

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

図２の（ａ）に示すように、空荷状態では、車体２２は車輪２１に対して傾きを有しておらず、車体２２に取り付けられた加速度センサ１０１、ジャイロセンサ１０２の検出軸（ｘ軸、ｙ軸、ｚ軸）３０は車両の運動の前後方向軸及び左右方向軸と一致する。つまり、検出軸３０は、空荷状態において姿勢情報を出力する際に用いる検出軸である。

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

また、車体２２がｙ軸（図２の（ａ）参照）に対してロール角ψ分傾くと、ｙ軸がｙ’軸に変位する。ｚ’軸は、ｘ’軸及びｙ’軸の２軸に直交する軸であり、ｚ軸が変位した軸を示す。これら積載状態における検出軸３１は、積載状態において姿勢情報を出力する際に用いる検出軸であって、空荷状態の検出軸３０に対してピッチ角φ、ロール角ψ分、傾いている。

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

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

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

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

図４を参照してアプローチ地図の構成例について説明する。図４は、アプローチ地図の一例を示す図であって、（ａ）はリンク情報を示し、（ｂ）はノード情報を示す。

アプローチ地図は、車両が積載地点もしくは放土地点までに通るべき位置であるノードとそのつながりを示すリンクで表されている。図４の（ａ）に示すように、アプローチ地図にはリンクを識別するためのリンクＩＤ４０１とそのリンクが始まる座標にあるノードＩＤ４０２、リンクの終わる座標にあるノードＩＤ４０３、リンク始点に接続される別のリンクＩＤ４０４（複数存在する場合もある。）、及びリンク終点に接続される別のリンクＩＤ４０５（複数存在する場合もある。）を規定するテーブルがある。更に図４の（ｂ）に示すように、ノードＩＤ４０６に対応する座標値４０７、その属性４０８のテーブルが存在する。このノードＩＤのうち、いずれかを積載地点もしくは放土地点として属性４０８に設定されている。

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

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

積載判定部１１１ａがセンサの出力値を取得し、積載状態の変化の有無、及び変化がある場合には空荷から積荷に変化したか、又は積荷から空荷状態へ変化したかを判定する積載状態判定処理を行う（Ｓ５０３）。

次いで、停止判定部１１２ａは、ダンプ２０が移動中が停止中かを判定する車両停止判定処理を行う（Ｓ５０４）。

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

速度ベクトル算出部１１３は、ステップＳ５０５で設定された補正量を用いて、車両の速度ベクトルを算出する（Ｓ５０６）。

位置算出部１１４は算出された速度ベクトルを用いて位置算出処理を行う（Ｓ５０７）。その後、エンジンが停止するまでステップＳ５０１へ戻り、ステップＳ５０１からＳ５０７までの処理を繰り返す。

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

図６に示すように、積載判定部１１１ａは、現在の積載状態フラグを取得する（Ｓ６０１）。積載状態フラグは１を積載状態、０を空荷状態とする。

次に積載判定部１１１ａは、前サンプリングタイムに計算した車両位置を取得して、現在位置として設定する（Ｓ６０２）。この現在位置は、目的地までの距離Ｌの計算に用いられる。

積載判定部１１１ａは、現在走行中の経路の最終到達点を地図情報記憶部１１１ｂに格納されたアプローチ地図から検索し、目的地が積載地点であるか放土地点であるかを判定する。目的地が積載地点である場合（Ｓ６０３／目的地が積載地点）は、前回算出された位置（初回の場合は初期位置）から目的地、すなわちアプローチ地図おける積載地点までの距離Ｌを計算する（Ｓ６０４）。また、目的地が放土地点である場合（Ｓ６０３／目的地が放土地点）は、放土地点と現在位置の距離Ｌを計算する（Ｓ６０５）。

距離Ｌの計算が終了すると、積載判定部１１１ａが距離Ｌと、目的地に到達したと見做せる距離により定義された距離閾値とを比較する（Ｓ６０６）。そして、積載判定部１１１ａが、距離Ｌは距離閾値よりも小さいと判定すると（Ｓ６０６／Ｙｅｓ）、これから積載量が変化するとして、積載状態フラグをステップＳ６０１で取得した値と反転させる。つまり、現在の積載状態フラグの値が１であれば０に、０であれば１にすればよい（Ｓ６０７）。また、距離Ｌが距離閾値以上である場合は（Ｓ６０６／Ｎｏ）、積載量の変化は無いものとして、ステップＳ７０１で得られた積載状態フラグの値を維持する（Ｓ６０８）。積載状態フラグが設定されれば積載判定部１１１ａによる積載状態判定処理を終了する。

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

図７に示すように、停止判定部１１２ａは、ステップＳ５０２で各センサからの出力値を取得すると、入力された現在のサンプリングも含めて、Ｎ秒間の加速度センサ１０１、ジャイロセンサ１０２、車輪回転数センサ１０３の出力値を内部メモリに保存する（Ｓ７０１）。

次に停止判定部１１２ａは、Ｎ秒間の車輪速度出力が全て速度が無い状態であるかを判定する（Ｓ７０２）。車輪回転数出力値（車輪速）に速度が出ている状態であると判定すると（Ｓ７０２／Ｎｏ）、停止判定フラグに０を設定し（Ｓ６０３）、停止判定部１１２ａによる処理を終了する。

停止判定部１１２ａは、車輪回転数出力値（車輪速）が全て速度が出ていない状態であると判定すると（Ｓ６０２／Ｙｅｓ）、Ｎ秒間のＩＭＵ出力値の平均値、分散値、およびＮ秒間の全サンプルから最小二乗法により求められた傾き値（１階微分値）を計算する（Ｓ６０４）。

停止判定部１１２ａは、加速度センサ１０１、ジャイロセンサ１０２各々のＮ秒間出力値の平均値の絶対値が、停止していると判定するための加速度閾値以下及び角速度閾値以下であるかを判定し、加速度の平均値が加速度閾値よりも大きく、かつ角速度の平均値が角速度閾値よりも大きい場合（Ｓ６０５／Ｎｏ）は、ステップＳ６０３に移行し、停止判定フラグに０を設定して、停止判定部１１２ａの処理を終了する。一方、停止判定部１１２ａが、加速度の平均値は加速度閾値以下であり、角速度の平均値も角速度閾値以下であると判定すると（Ｓ６０５／Ｙｅｓ）ステップＳ６０６へ移行する。

停止判定部１１２ａは、加速度センサ１０１、ジャイロセンサ１０２各々のＮ秒間の全サンプル値の最小二乗法により求められた傾きの絶対値が、停止していると判定するための傾き閾値以下であるかを判定し、傾きの絶対値が傾き閾値よりも大きいと判定すると（Ｓ６０６／Ｎｏ）ステップＳ６０３に移行し、停止判定フラグに０を設定し、停止判定部１１２ａの処理を終了する。一方、停止判定部１１２ａが、傾きの絶対値は傾き閾値以下であると判定すると（Ｓ６０６／Ｙｅｓ）ステップＳ６０７へ移行する。

停止判定部１１２ａは、加速度センサ１０１、ジャイロセンサ１０２各々のＮ秒間の分散値の絶対値が停止していると判定するための分散値閾値以下であるかを判定し、分散値の絶対値が分散値閾値よりも大きいと判定すると（Ｓ６０７／Ｎｏ）、ステップＳ６０３に移行し、停止判定フラグに０を設定し、停止判定部１１２ａの処理を終了する。一方、停止判定部１１２ａが分散値の絶対値が分散値閾値以下であると判定すると（Ｓ６０７／Ｙｅｓ）、停止判定フラグに１を設定し（Ｓ６０８）、停止判定部１１２ａの処理を終了する。なお、ステップＳ６０５、Ｓ６０６、Ｓ６０７の順序は入れ替わっても良い。

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

図８に示すように、車体傾斜算出部１１２ｂは、そのサンプリング時の車体の傾斜角度（ロール角ψ、ピッチ角φ）を計算する（Ｓ８０１）。車体傾斜算出部１１２ｂにて算出された車体傾斜角度は、現在のサンプル時間含め、あるサンプリング時間分保存される。車体の傾斜は下式（１）、式（２）以下の方法で計算できる。
[数１]
（ロール角ψ）＝ａｒｃｓｉｎ［｛（加速度センサ横方向出力値）−（横加速度）／（重力加速度）｝・・・（１）
[数２]
（ピッチ角φ）＝ａｒｃｓｉｎ［｛（加速度センサ前後方向出力値）−（車両加速度）／（重力加速度）｝・・・（２）

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

車体２２の傾斜角度を算出すると、車体傾斜算出部１１２ｂによる車体傾斜算出処理（Ｓ８０１）を終了する。

車両２２の傾斜角度の算出が終了すると、補正量算出部１１２ｃは、ステップＳ７０３、Ｓ７０８で設定された停止判定フラグの値を参照し、車両が停止しているかどうかを確認する。車両が移動している場合（停止判定フラグの値０）（Ｓ８０２／移動中）ステップＳ８０３に移行し、前サンプリング時に算出した補正量を維持し（Ｓ８０３）、この維持した補正量を速度ベクトル算出のための補正量として設定して補正量設定処理を終了する。

補正量算出部１１２ｃは、車両が停止していると判定した場合（停止判定フラグの値１：Ｓ８０２／停止中）は、ステップＳ８０４へ移行し、積載判定部１１１ａから出力される積載状態フラグを用いて積荷状態が変化したかどうかを確認する。前サンプリングにおける積載状態フラグが今回のサンプリング時と同じであれば（Ｓ８０４／変化なし）ステップＳ８０５に移行する。

補正量算出部１１２ｃは、現在の積載状態フラグが積荷（積載状態フラグの値１）であれば（Ｓ８０５／Ｙｅｓ）ステップＳ８０６に移行し、逐次最小二乗法により補正量Ｍを算出する（Ｓ８０６）。以下、補正量算出部１１２ｃによる補正量Ｍの算出方法について説明する。

まず、補正量算出部１１２ｃは、加速度センサ１０１のｘ軸、ｙ軸、ｚ軸からそれぞれ（αｘ、αｙ、αｚ）の出力を取得する。車体傾斜算出部１１２ｂからの出力φ、ψを用いて、重力加速度ベクトル（０、０、ｇ）との関係を表すと、下式（４）のように表される。このことを利用して、比例係数行列Ａを逐次最小二乗法にて求める。そして、現在のサンプリング時における比例係数行列Ａの逆行列を下式（５）のように計算することで補正量Ｍを求めることができる。

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

補正量算出部１１２ｃは、現在の積載状態フラグが空荷である（積載状態フラグの値０）と判定すると（Ｓ８０５／Ｎｏ）ステップＳ８０７に移行し、現在設定されている補正量Ｍを消去するとともに新たな補正量として単位行列を設定し（Ｓ８０７）、補正量算出部１１２ｃの処理を終了する。

また、ステップＳ８０４において、前サンプリングにおける積載状態フラグが空荷から積荷に変化した場合は（Ｓ８０４／空荷→積荷）、ステップＳ８０６に移行し、逐次最小二乗法により補正量となる行列（補正量Ｍ）を算出し、最終結果を保存して補正量算出部１１２ｃの処理を終了する。

また、ステップＳ８０４において、前サンプリングにおける積載状態フラグが積荷から空荷に変化した場合は（Ｓ８０４／積荷→空荷）、ステップＳ８０７に移行し、設定中の補正量を削除して新たな補正量として単位行列を設定し、補正量算出部１１２ｃの処理を終了する。

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

補正後加速度出力値α’は補正量Ｍと元の加速度センサ出力値αより下式（６）のように表される。

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

これら補正後の加速度、角速度より、空荷状態の検出軸３０（図２の（ａ）参照）と一致するように積載状態の検出軸３１（図２の（ｂ）参照）を補正した後の補正後の回転角（補正ロール角ψ’、補正ピッチ角φ、補正後角速度を考慮したヨー角θ）は下式（８）、（９）、（１０）により求めることができる。なお、補正量Ｍとして単位行列が設定されている場合には、補正後加速度出力値α’は加速度センサ１０１からの出力αに一致し、補正後角速度出力値ω’はジャイロセンサ１０２からの出力ωに一致するので、以下の説明において補正後加速度を加速度センサの出力、補正後角速度をジャイロセンサの出力と読み替えればよい。
[数８]
（補正ロール角ψ’）＝ａｒｃｓｉｎ［｛（補正後加速度：横方向）−（車両横加速度）／（重力加速度）｝・・・（８）
[数９]
（補正ピッチ角φ’）＝ａｒｃｓｉｎ［｛（補正後加速度：前後方向）−（車両加速度）／（重力加速度）｝・・・（９）
[数１０]
（ヨー角θ）＝（前サンプリング時ヨー角）＋（補正後角速度：垂直軸）×（サンプリング時間）・・・（１０）

これら回転角より、速度方向の回転マトリクスＣは、下式（１１）により求まる。

速度方向の回転マトリクスＣが求まれば、車輪回転数センサ出力値ｓから、速度ベクトルＶは下式（１２）により求まる。

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

速度ベクトル算出部１１３にて速度ベクトルＶが算出されると、ステップＳ５０７において位置算出部１１４が下式（１３）により、位置を更新する。
[数１３]
（更新位置）＝（前サンプリング時の位置）＋（速度ベクトル）×（サンプリング時間）・・・（１３）

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

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

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

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

図９に示すように、第二実施形態に係る補正量設定処理では、補正量設定部１１２内の補正量算出部１１２ｃが積載状態の検出軸を空荷状態の検出軸に一致させるための補正量Ｍが記憶されているかを判定し、記憶されていない場合（Ｓ９０１／Ｎｏ）には、ダンプ２０に積荷を積載した状態で補正量Ｍを算出し、記憶する（Ｓ９０２）。ステップＳ９０２の補正量Ｍの算出方法は、第一実施形態と同様である。

補正量Ｍが記憶されている場合（Ｓ９０１／Ｙｅｓ）、及び補正量Ｍの算出が終了すると（Ｓ９０２）、補正量算出部１１２は車両が停止しているかどうかを判定（Ｓ８０２）する。以下、ステップＳ８０２からステップＳ８０５、ステップＳ８０７の処理は第一実施形態と同様であるので重複説明を省略する。

補正量算出部１１２ｃは、ダンプ２０が停止中（Ｓ８０２／Ｙｅｓ）で、積荷状態が空荷状態から積荷状態に変化した場合（Ｓ８０４／空荷から積荷）、又は積載状態に変化がなく（Ｓ８０４／変化なし）かつ現在の状態が積載状態の場合（Ｓ８０５／Ｙｅｓ）、ステップＳ９０３に移行する。

補正量算出部１１２ｃは、予め算出された補正量を記憶する補正量記憶部から補正量を読み出し、速度ベクトルを算出するための補正量として設定する（Ｓ９０３）。

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

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

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

図１１に示すように、積載判定部１１１ａは現在の積載状態フラグを取得する（Ｓ１１０１）。次いで積載判定部１１１ａは、前サンプリング時にサスペンション圧力センサにて計測されたサスペンション圧を記憶装置から読み出す（Ｓ１１０２）。次いで積載判定部１１１ａは、今回のサンプリング時に計測したサスペンション圧を取得する（Ｓ１１０３）。

積載判定部１１１ａは、ステップＳ１１０３で取得した現在のサスペンション圧とステップＳ１１０２で読み出した前サンプリング時のサスペンション圧の差の絶対値を計算し、その絶対値（サスペンション圧変化量）が圧変化量閾値より大きいかを判定する（Ｓ１１０３）。この圧変化量閾値は、積載状態から空荷状態へ、又は空荷状態から積載状態へと遷移したかを判定するための閾値であって、サスペンション圧の変化量を用いて定義される。サスペンション圧変化量が圧変化量閾値以下の場合（Ｓ１１０４／Ｎｏ）、積載判定部１１１ａは積載状態に変化なしと判定し、現在の積載状態フラグを維持して（Ｓ１１０５）、積載判定部１１１ａの処理を終了する。

サスペンション圧変化量が圧変化量閾値よりも大きい場合（Ｓ１１０４／Ｙｅｓ）、積載判定部１１１ａは現在のサスペンション圧が予め決められている圧閾値よりも小さいかを判定する（Ｓ１１０６）。この圧閾値は、空荷状態か積載状態かを判定するための閾値であって、サスペンション圧により定義される。圧閾値は、空荷におけるサスペンション圧の最大値を設定してもよい。

サスペンション圧の絶対値が圧閾値以上の場合（Ｓ１１０６／Ｎｏ）、積載判定部１１１ａは、積載状態フラグを積載に設定する（Ｓ１１０７）。圧閾値未満の場合（Ｓ１１０６／Ｙｅｓ）、積載判定部１１１ａは、積載状態フラグを空荷に設定する（Ｓ１１０８）。

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

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

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

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

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

２０ ダンプ
２１ 車輪
２２ 車体
２３ サスペンション
１０１ 加速度センサ
１０２ ジャイロセンサ
１０３ 車輪回転数センサ
１１０ 位置算出装置

Claims (6)

1. 車輪及び当該車輪上にサスペンションを介して搭載された車体を含む運搬車両に搭載される位置算出装置であって、
   前記車輪に対する前記車体の姿勢を示す姿勢情報を出力する姿勢検出センサと、
   前記車輪の回転数を出力する車輪回転数センサと、
   前記車体に積荷が積載された積載状態であるか又は前記積荷が積載されていない空荷状態であるかを示す積載状態情報を取得する積載状態情報取得部と、
   前記姿勢情報を基に、前記積載状態において前記姿勢情報を出力する際に用いる検出軸を前記空荷状態において前記姿勢情報を出力する際に用いる検出軸に一致させるための補正量を算出し、前記積載状態における前記運搬車両の速度ベクトルを算出するための補正量として設定する補正量設定部と、
   前記積載状態情報に基づいて前記車体が前記積載状態又は前記空荷状態であるかを判定し、前記車体が前記積載状態であると判定された場合に、前記車輪の回転数から得られる車輪速、前記姿勢情報から求まる前記運搬車両の運動方向、及び前記設定された補正量を用いて前記運搬車両の速度ベクトルを算出する速度ベクトル算出部と、
   前記速度ベクトルを用いて前記運搬車両の位置を算出する位置算出部と、
   を備えることを特徴とする位置算出装置。
2. 前記補正量設定部は、前記積載状態情報に基づいて前記車体が前記空荷状態であると判定すると、前記算出された補正量を削除して単位行列からなる補正量を設定する、
   ことを特徴とする請求項１に記載の位置算出装置。
3. 予め算出された前記補正量を記憶する補正量記憶部を更に備え、
   前記補正量設定部は、前記積載状態情報に基づいて前記車体が積載状態であると判定すると、前記補正量の算出に代えて、前記補正量記憶部から前記補正量を読み出して設定する、
   ことを特徴とする請求項１に記載の位置算出装置。
4. 前記積荷が積み込まれる積載地点、及び前記積荷を放土する放土地点を接続する走行経路を示す地図情報を記憶する地図情報記憶部を更に備え、
   前記積載状態情報取得部は、前記位置算出部が算出した前記運搬車両の位置、及び前記地図情報に基づき、前記積載地点又は前記放土地点に到達したかに基づいて前記積載状態又は前記空荷状態のいずれに相当するかを判定し、その判定結果を示す前記積載状態情報を生成する、
   ことを特徴とする請求項１に記載の位置算出装置。
5. 前記積載状態情報取得部は、前記運搬車両に備えられた前記サスペンションの圧力を検出するサスペンション圧力センサからの出力、及び前記空荷状態又は前記積載状態を区別するための閾値を比較し、その比較結果に基づいて前記積載状態情報を生成する、
   ことを特徴とする請求項１に記載の位置算出装置。
6. 車輪及び当該車輪上にサスペンションを介して搭載された車体を含む運搬車両であって、
   前記車輪に対する前記車体の傾斜姿勢を示す姿勢情報を出力する姿勢検出センサと、
   前記車輪の回転数を出力する車輪回転数センサと、
   前記車体に積荷が積載された積載状態であるか又は前記積荷が積載されていない空荷状態であるかを示す積載状態情報を取得する積載状態情報取得部と、
   前記姿勢情報を基に、前記積載状態において前記姿勢情報を出力する際に用いる検出軸を前記空荷状態において前記姿勢情報を出力する際に用いる検出軸に一致させるための補正量を算出し、前記積載状態における前記運搬車両の速度ベクトルを算出するための補正量として設定する補正量設定部と、
   前記積載状態情報に基づいて前記車体が前記積載状態又は前記空荷状態であるかを判定し、前記車体が前記積載状態であると判定された場合に、前記車輪の回転数から得られる車輪速、前記姿勢情報から求まる前記運搬車両の運動方向、及び前記設定された補正量を用いて前記運搬車両の速度ベクトルを算出する速度ベクトル算出部と、
   前記速度ベクトルを用いて前記運搬車両の位置を算出する位置算出部と、
   を備えることを特徴とする運搬車両。