JP6095065B2 - ダンプトラック - Google Patents

Description

本発明は、鉱山や建設現場で移動するダンプトラックに関する。

鉱山や建設現場で利用されるダンプトラックの姿勢はロール角、ピッチ角、ヨー角で表わされる。重力方向と垂直に交わる面を水平面と定義すると、ダンプトラックの前後軸に直交する左右軸（側方軸）を中心に当該前後軸が回転して当該水平面と成す角度がピッチ角となり、当該前後軸を中心に当該左右軸が回転して当該水平面と成す角度がロール角となる。そして、ダンプトラックの前後軸と左右軸の双方に直交する上下軸の回転角であるヨー角は方位角となる。

ダンプトラックと関連して、建設機械の一種である油圧ショベルの姿勢を推定する公知の技術としては、特開２０１２−２３３３５３号公報に記載されているように、ＧＰＳ等の位置推定装置を２つ用いて、建設機械の方位（ヨー角）を計測し、ジャイロセンサや加速度センサを組み合わせた慣性計測装置を用いて建設機械のロール角およびピッチ角を推定するものがある。

また、姿勢推定の他の手段として、特開２０１０−１９０８０６号公報には、一般的な移動体に対して位置推定装置を３つ取り付けることにより姿勢を推定する方法が開示されている。

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

前者の特開２０１２−２３３３５３号公報の技術においては、慣性計測装置は、建設機械の動作中は角速度を積算して姿勢を推定しているため、ジャイロセンサや加速度センサのスケールファクタやバイアスの誤差によりロール角やピッチ角の推定値に大きな誤差が生じる。この誤差を補正するためには、建設機械の動作を一度停止するか、他の手段により姿勢を推定して補正することなる。

また、特開２０１０−１９０８０６号公報の技術に関して、一般的な移動体と比較してダンプトラックは過酷な環境下で動作するため、センサデバイスの故障が多く、位置推定装置というセンサの増加はメンテナンス機会の増加につながる。すなわち、センサの増加は導入コストだけでなく維持コストも増加する結果となるため、ダンプトラックでの利用を想定すると、センサ数を増加せずに常に高精度に姿勢が推定できる手法が望まれる。しかし、単純に２つの位置推定装置だけでダンプトラックの姿勢を推定しようとすると、２つの位置推定装置を結ぶ線分を中心とした回転角を算出できなくなるため、姿勢が一意に決定できない。

本発明の目的は、２つの位置推定装置を用いて、動作を停止することなく精度良く姿勢を推定できるダンプトラックを提供することにある。

ダンプトラックの姿勢を、重心を原点とした車体軸周りのロール角、ピッチ角、ヨー角で定義する。路面にカント角が付いていないことを前提とし、２つの位置推定手段を車体軸と平行にならないように、また、ダンプトラックの回転誤差値を最も小さくする接地点が唯一２つの位置推定手段の線分内に入るように設置する。路面の勾配を計算する勾配算出部を設け、路面の勾配を計算し、接地点の位置を算出する。接地点の位置と２つの位置推定手段から算出された位置から姿勢を算出する。

本発明によれば、２つの位置推定装置で精度良くダンプトラックの姿勢を推定できる。

本発明の第１の実施の形態に係るダンプトラックの概略構成図。 図１に示したダンプトラックの構造を模式的に示した上面図。 コンピュータ１１０の概略構成図。 記憶装置１０６に係るトポロジカル地図データベースのデータ構造。 コンピュータ１１０で実行されるダンプトラックの姿勢算出処理のフローチャート。 図１に示したダンプトラックのモデル図。 本発明の第１の実施の形態に係る線分ＤＫの移動可能範囲を示す図。 本発明の第２の実施の形態における記憶装置１０６のデータ構造のうち点Ｐ，Ｑの位置の時系列情報に関係する部分を示した図。 点Ｐ，Ｑの位置推定結果を上から見たモデル図

後述するように、本発明の各実施の形態では、鉱山などで利用されるダンプトラックに対して、フレームに起伏自在に取り付けられた荷台と、当該フレームに回転可能に取り付けられた複数の後輪と、車両前後方向に互いにずらして配置された２つの位置推定装置（例えば、ＧＰＳ受信機）とを備え、当該ダンプトラックに設定した車両座標系において前記複数の後輪が地面と接触している部分から任意に選択した点を基準点Ｄとし、前記２つの位置推定装置によって位置が算出される２点Ｐ，Ｑを結ぶ線分ＰＱから前記基準点Ｄに対して垂線が下ろせるように前記２つの位置推定装置を配置している。

本発明の創出に際し、まず、発明者らは、ダンプトラックの重心は、荷台の積載重量に応じて多少は前後移動するものの後輪の車軸近傍に位置することには変わりは無く、ダンプトラックの姿勢や積載重量が変化しても後輪は常に地面と接触している点に着目した。そして、ダンプトラックの姿勢の算出に際しては、後輪と地面の接触部（接触面）に基準点Ｄを設定し、当該基準点Ｄを固定端にして前記垂線（基準点Ｄから線分ＰＱに下ろした垂線）が揺動すると仮定できると考えた。なお、当該垂線は、その中心軸を中心に回転することも可能とする。

このような仮定の下では、基準点Ｄから前記２点Ｐ，Ｑまでの距離（ベクトルＰＤ，ＱＤのスカラー）はダンプトラックの姿勢が変化しても一定となるので、前記２つの位置推定装置を介して得られる前記２点の位置Ｐ，Ｑから前記基準点Ｄの位置を算出できる。これにより、前記２点Ｐ，Ｑの位置に加えて、基準点Ｄの位置が決まる。これら３点Ｐ，Ｑ，Ｄの位置が判明すれば、ダンプトラックの姿勢が特定できるので、２つの位置推定装置で精度良くダンプトラックの姿勢を推定できる。

また、ダンプトラックの姿勢をロール角φ、ピッチ角θ、ヨー角ψで定義する場合には、前記基準点Ｄが設定された後輪の位置における車高変化を検出するセンサ（例えば、サスペンション長の変化を検出するレベリングセンサや、荷台の積載量変化を算出するために利用されるサスペンション圧力を検出する圧力センサ）をさらに備え、前記２点Ｐ，Ｑのいずれか一方から地面までの距離Δｚを前記センサの検出値に基づいて算出し、当該算出距離Δｚと、前記２点Ｐ，Ｑおよび前記基準点Ｄの位置とに基づいてダンプトラックの姿勢をコンピュータ等の演算装置によって算出する。

これにより、前記基準点Ｄから地面の法線方向に距離Δｚだけ進んだ点Ｔを設定できる。２点Ｐ，Ｑのうち距離Δｚの基準とした点（点Ｐまたは点Ｑ）と点Ｔを結んだ線分は地面と平行となる。３点Ｐ，Ｑ，Ｔを通過する平面の法線ベクトルＵは、当該点Ｔから前記２点Ｐ，Ｑに向かうベクトルＴＱ，ＴＰの外積をとることで算出できる。そして、後述する式に、ベクトルＵと、前記２点Ｐ，Ｑおよび前記基準点Ｄの位置を入力することで、ダンプトラックのロール角φ、ピッチ角θ、ヨー角ψを算出できる。

なお、ロール角φ、ピッチ角θ、ヨー角ψは、ダンプトラックに設定した所定の原点（例えば、ダンプトラックの中心）を基準にして、互いに直交する３軸（ダンプトラックの前後軸、左右軸および上下軸）で座標系を設定したとき、当該座標系における各軸回りの回転角度として定義されるものとする。また、上記のように２つの位置推定装置を車両前後方向に互いにずらして配置すると、線分ＰＱが、上記３軸のいずれとも平行にならない。そのため、ダンプトラックの姿勢をロール角φ、ピッチ角θ、ヨー角ψで定義できる。

また、前記ダンプトラックが走行する道路の勾配情報が記憶された記憶装置をさらに備え、前記演算装置において、さらに、当該記憶装置の勾配情報と、前記２点Ｐ，Ｑおよび前記基準点Ｄの位置とに基づいてダンプトラックの姿勢を算出することが好ましい。これにより道路の勾配を考慮した姿勢が算出できる。

また、前記ダンプトラックでは、前記演算装置において、さらに、前記ダンプトラックの走行地点における勾配を前記ダンプトラックの過去の位置データに基づいて算出し、当該勾配情報と、前記２点および前記基準点の位置とに基づいてダンプトラックの姿勢を算出することが好ましい。これにより、ダンプトラックが走行する道路の勾配情報が記憶装置に記憶されていない場合にも、道路の勾配を考慮した姿勢が算出できる。

また、前記基準点Ｄは、前記ダンプトラックの重心に最も近い位置に設定することが好ましい。ダンプトラックは重心を中心にして姿勢が変化するので、このように基準点を設定すると、ダンプトラックの姿勢算出時に生じる誤差を小さくでき、算出されるダンプトラックの姿勢の精度が向上する。

また、前記基準点Ｄは、前記２点Ｐ，Ｑを結ぶ線分ＰＱから最も離れた位置にある後輪の接地面の中から抽出することが好ましい。これにより、基準点Ｄから線分ＰＱに対する垂線の長さが最大になり、ダンプトラックの姿勢算出時に生じる誤差を小さくできるので、算出されるダンプトラックの姿勢の精度が向上する。

また、２点Ｐ，Ｑと基準点Ｄは、各点Ｐ，Ｑ，Ｄを頂点とする三角形が正三角形になるように設定することが好ましい。このようにすると、ダンプトラックの姿勢算出時に生じる誤差をロール角φとピッチ角θに均等に分散できるので、算出されるダンプトラックの姿勢の精度が総合的に向上する。

また、２つの位置推定装置は、車両座標系における同じ高さに設置することが好ましい。このようにすると、２つの位置推定装置を異なる高さに設置した場合と比較してダンプトラックの姿勢算出が容易になる。

以下、図面を参照しながら、本発明に係るダンプトラックの効率的な姿勢推定を実現するための構成および処理について具体的に説明する。

＜第１の実施の形態＞
第１の実施の形態では、ダンプトラックの走行路を示す地図データとともに記憶装置（トポロジカル地図データベース）１０６に記憶された道路の勾配情報を参照しながら、ダンプトラックの姿勢を求める。なお、便宜上、本実施の形態でダンプトラックが走行する路面は縦断勾配を有するが、横断勾配を有しない（カント角がゼロ）とする。

本発明の第１の実施の形態に係るダンプトラックの概略構成を図１に示す。この図に示すダンプトラックは、フレーム１１と、フレームに起伏自在に取り付けられた荷台（ボディ）１２と、荷台１２を起伏するために車両の幅方向に所定の間隔を介して設置された２本の油圧シリンダ１３，１３と、フレーム１１の前方に回転自在に取り付けられた２つの前輪１４ａ，１４ｂと、フレームの後方に回転自在に取り付けられた４つの後輪１５ａ，１５ｂ，１５ｃ，１５ｄと、前輪１４ａ，１４ｂにフレーム１１を懸架するためのフロントサスペンション１６ａ，１６ｂと、後輪１５ａ，１５ｂ，１５ｃ，１５ｄにフレーム１１を懸架するためのリアサスペンション１７ａ，１７ｂと、フロントサスペンション１６ａ，１６ｂおよびリアサスペンション１７ａ，１７ｂの長さ変位をそれぞれ検出するための４つのレベリングセンサ１８と、直接又は間接的にフレーム１１に固定された２つのＧＰＳ受信機（位置推定装置）１０１，１０２と、ダンプトラックの加速度や角速度などを計測する慣性計測装置１０３と、ダンプトラックの姿勢算出処理を含む各種処理を実行するコンピュータ１１０とを備えている。なお、以下においては、同種の部分を区別して説明する必要が特に無い場合には、各符号に付した小文字のアルファベットは省略して表記することがある。また、各図において同じ部分または対応部分には同じ符号を付す。

図２は図１に示したダンプトラックの構造を模式的に示した上面図である。なお、図中に示すように、ダンプトラックの内部に原点を有し、前後軸ｙ、左右軸ｘ、上下軸ｚの３軸を有する直交座標系Ｂを設定する（図２では座標系Ｂの原点は、ダンプトラックの中心に設定している。）。以下では、この座標系を車両座標系Ｂと称することがある。また、ここでは、ダンプトラックが左右軸ｘを中心に回転したとき前後軸ｙが水平面と成す角度をピッチ角θとし、ダンプトラックが前後軸ｙを中心に回転したとき左右軸ｘが水平面と成す角度がロール角φとする。上下軸ｚの回転角であるヨー角ψは方位角となる。

図２において、２つのＧＰＳ受信機１０１，１０２は、車両左右方向（ｘ軸方向）にΔｘだけ間隔を空けて配置されており、さらに、車両前後方向（ｙ軸方向）にΔｙだけ互いにずらして配置されている。Δｘは車幅より大きく設定しても構わない。ここでは、２つのＧＰＳ受信機１０１，１０２によって位置が検出される点をそれぞれ点Ｐ，Ｑとする。このように２つのＧＰＳ受信機１０１，１０２を前後方向にずらして配置すると、２点Ｐ，Ｑを結ぶ線分ＰＱが、車両座標系Ｂに係る３軸ｘ，ｙ，ｚのいずれとも平行にならないので、点Ｐ，Ｑおよび後述する基準点Ｄの位置を取得することで、ダンプトラックのロール角φ、ピッチ角θおよびヨー角ψを特定できる。

ＧＰＳ受信機１０１，１０２が受信したＧＰＳ衛星からの航法信号は、コンピュータ１１０に定期的に出力されており、コンピュータ１１０では、ＧＰＳ受信機１０１，１０２から入力される航法信号に基づいて点Ｐ，Ｑの位置を推定する処理が行われる。

なお、ＧＰＳ受信機１０１，１０２の固定に関して、フレーム１１と相対移動不能に固定できればその固定方法について特に限定はない。例えば、ダンプトラックの外壁から略水平に突出させた梁状の部材の上にＧＰＳ受信機１０１，１０２を略垂直に立てて設置しても構わない。

ところで、車両座標系Ｂにおいて４つの後輪１５がそれぞれ地面と接触している部分（接地面）をＣとする。図２では接地面Ｃを矩形状の破線で囲まれた領域で表している。本実施の形態に係るダンプトラックは４つの後輪１５を備えているので接地面Ｃは４つ存在しており、その４つの接地面Ｃから任意に選択した点を基準点（接地点）Ｄとする。図の例では、車両の右端に位置する後輪１５ｄの接地面Ｃに後輪の車軸を投影した直線上に基準点Ｄを設定した。なお、図２では各接地面Ｃの輪郭を矩形で示したが、これは接地面Ｃを模式的に示したに過ぎず、接地面Ｃの輪郭を限定する趣旨ではない。基準点Ｄは、実際のタイヤと地面の接触面の中から任意に選択すれば良い。

このように接地面Ｃ上に基準点Ｄを設置するとき、２つのＧＰＳ受信機１０１，１０２は、基準点Ｄから線分ＰＱに対して垂線が１本だけ下ろせるように位置調整されて配置されている。ダンプトラックの姿勢算出に関してロール角またはピッチ角の精度低下を図る観点からは、基準点Ｄから線分ＰＱに対する垂線の足は線分ＰＱ上に位置するように基準点Ｄを位置調整することが好ましい。

また、車両座標系Ｂの前後軸ｙと左右軸ｘで張られる平面（すなわち図２の紙面上）において、点Ｐから点ＱへのベクトルＰＱが前後軸ｙと成す角をδとする。

また、図１に示すように、本実施の形態に係る２つのＧＰＳ受信機１０１，１０２は、点Ｐ，Ｑが同じ高さに位置するように設置されている。このように２つのＧＰＳ受信機１０１，１０２を設置すると、２つのＧＰＳ受信機を異なる高さに設置した場合と比較して後述するダンプトラックの姿勢算出が容易になる。

また、図２に示すように、本実施の形態に係るダンプトラックは、左側前輪１４ａおよび右側前輪１４ｂの２つの前輪を備えており、左側前輪１４ａに係るフロントサスペンション１６ａと、右側前輪１４ｂに係るフロントサスペンション１６ｂを備えている。また、後輪として合計４つの車輪１５ａ，１５ｂ，１５ｃ，１５ｄを備えており、このうち２つの後輪１５ａ，１５ｂは車両左側に配置されており、残りの２つの後輪１５ｃ，１５ｄは車両右側に配置されている。リアサスペンション１７ａは左側の後輪群１５ａ，１５ｂに係るものであり、リアサスペンション１７ｂは右側の後輪群１５ｃ，１５ｄに係るものである。

図２に示したシンボルＧはダンプトラックの重心を示している。重心Ｇは、荷台１２の積載量に応じてｙ軸に沿って図２中の上下方向に多少移動することはあるが、荷台１２の積載量が変化しても前輪１４よりも後輪１５に近い位置に常に存在する。すなわち、後輪１５は常に重心Ｇの近くに存在することになる。

図３はコンピュータ１１０の構成図である。この図に示すように、コンピュータ１１０は、各種プログラムを実行するための演算手段としての演算処理装置（例えば、ＣＰＵ）１０４と、当該プログラムをはじめ各種データを記憶するための記憶手段としての記憶装置（例えば、ＲＯＭ、ＲＡＭおよびフラッシュメモリ等の半導体メモリや、ハードディスクドライブ等の磁気記憶装置）１０６と、コンピュータ１１０内の各装置１０４，１０６およびコンピュータ１１０外の各装置へのデータ及び指示等の入出力制御を行うための入出力インターフェース１１１を備えている。なお、演算処理装置１０４の処理結果（例えば、ダンプトラックの姿勢の演算結果）等を表示するための表示装置（例えば、液晶モニタ等）をコンピュータ１１０に接続しても良い。

コンピュータ１１０には、入出力インターフェース１１１を介して、ＧＰＳ受信機１０１，１０２と、右側の後輪１５ｃ，１５ｄに係るリアサスペンション１７ｂの変位を検出するレベリングセンサ１８と、慣性計測装置１０３が接続されており、各装置１０１，１０２，１８，１０３からの出力値がコンピュータ１１０に入力されている。

記憶装置１０６には、ダンプトラックの走行路の形状および勾配を示す地図がデータ（地図データ）として記憶されている。走行路の形状は、点（以下、ノード）と、線（以下、リンク）で表わされており、記憶装置１０６内にはトポロジカル地図データベースが構成されている。

図４に記憶装置１０６に係るトポロジカル地図データベースのデータ構造を示す。この図に示すように、記憶装置１０６には、地図を構成する全てのリンク（リンク総数はｎ個とする）に個別に与えられ、各リンクを一意に表わすリンクＩＤ４０１と、対応するリンクＩＤを有するリンクの始点（ノード）の座標を示すリンク始点４０２、対応するリンクＩＤを有するリンクの終点（ノード）の座標を示すリンク終点４０３と、対応するリンクＩＤを有するリンクの始点が接続する他のリンクのＩＤを示す接続ＩＤ４０４と、対応するリンクＩＤを有するリンクの終点が接続する他のリンクのＩＤを示す接続ＩＤ４０５と、対応するリンクＩＤを有するリンクが示す道路の勾配を角度で示したリンク勾配４０６とが保存されている。

なお、リンク始点４０２およびリンク終点４０３に係るノードの座標は、例えば世界測地系等の地上（地球）に設定した地上座標系Ｏ（図６，７参照）における３次元座標で示す。また、本実施の形態におけるリンクの勾配（道路勾配）は、リンクが水平面となす角で定義されているが、これに代えて当該リンクの両端に位置する２つのノードの高さで定義しても良い。

次に上記のように構成されるダンプトラックのコンピュータ１１０で実行されるダンプトラックの姿勢算出処理について説明する。図５はコンピュータ１１０で実行されるダンプトラックの姿勢算出処理のフローチャートである。図６は図１に示したダンプトラックをモデル化した図（モデル図）であり、ここでは図６を参照しながら図５の姿勢算出処理について説明する。

図６において、座標系Ｏは地上に設定された３次元座標系（地上座標系）であり、ダンプトラックはリンク６０１を含む走行面Ｊを走行しているものとする。リンク６０１は、ダンプトラックの走行路の一部をなすものであり、個別のリンクＩＤと、始点Ｓ及び終点Ｅを有している。また、水平面に対するリンク６１０の傾斜角（すなわち、水平面と走行面Ｊのなす角）をαとする。

Δｚは、点Ｐ，Ｑから走行面Ｊまでの距離を示し、図６の例では基準点（接地点）Ｄを地面側の基準として設定している。Δｚは、基準点が設定された後輪１５ｄの位置における車高変化を検出することで算出可能である。本実施の形態では、後輪１５ｄに係るサスペンション１７ｂの長さをレベリングセンサ１８で検出し、これにサスペンション１７ｂの上端から点Ｐ，Ｑまでの高さ距離（当該距離は一定値となる）を加えることでΔｚを算出している。なお、２点Ｐ，Ｑの高さが異なる場合には、２点Ｐ，Ｑのうちいずれか一方から走行面までの距離をΔｚとすれば良い。また、サスペンション長の代わりに、サスペンション圧力を圧力センサで検出することで荷台１２の積載量を検出し、当該積載量の変化からΔｚを算出する等しても良い。

また、図６において、点Ｔは、基準点Ｄから走行面Ｊの法線方向にΔｚだけ進んだ点である。２点Ｐ，Ｑと点Ｔを含む平面は走行面Ｊと平行であり、当該平面の法線ベクトルＵは、当該点Ｔから前記２点Ｐ，Ｑに向かうベクトルＴＱ，ＴＰの外積をとることで次の式（１）のように算出できる。

図５に示す処理が開始されると、まず、コンピュータ１１０は、ＧＰＳ受信機１０１，１０２からの入力値に基づいて地上座標系Ｏにおける点Ｐ，Ｑの位置（ベクトルＯＰ，ＯＱ）を算出する（ステップ１００）。

次に、コンピュータ１１０は、ダンプトラックが現在走行中の走行路に係るリンク６０１を点Ｐ，Ｑの位置から特定し、当該リンクの始点、終点および勾配αを記憶装置１０６から取得する。本実施の形態では、点Ｐまたは点Ｑの位置を自車位置とみなし、自車位置から所定の範囲内に存在するリンク始点またはリンク終点を記憶装置１０６で検索し、そのリンク始点またはリンク終点に紐付けられたリンクＩＤを全て取得する。そして、取得したＩＤを有する全てのリンクに対して自車位置から垂線を引き、その垂線の長さが最も短いリンクを現在走行中の走行面を示すリンクとして選択している。さらに、当該選択したリンクの始点、終点および勾配αを記憶装置１０６から取得している（ステップ１１０）。

ステップ１１０で勾配を取得したら、コンピュータ１１０は、ステップ１００で取得した点Ｐ，Ｑの位置に基づいて基準点Ｄの位置を算出する。基準点Ｄの位置は次のようにして求めることができる。

まず、図６において、リンク６０１の始点Ｓと終点ＥからベクトルＳＥを定義し、当該ベクトルＳＥと走行面Ｊ上で直交する法線方向ベクトルをベクトルＮＬと定義する。地上座標系Ｏにおける基準点Ｄの位置を示すベクトルＯＤは、ベクトルＳＥとベクトルＮＬで張られる平面（走行面Ｊ）上に存在するため、未知変数ｓ，ｔを利用して次の式（２）で表すことができる。

一方、点Ｐから基準点Ｄまで距離（ベクトルＰＤのスカラー）は、ベクトルＯＤとベクトルＯＰの差のスカラーであり、上記式（２）を利用して下記式（３）のように表すことができる。同様に、点Ｑから基準点Ｄまでの距離（ベクトルＱＤのスカラー）は、ベクトルＯＤとベクトルＯＱの差のスカラーであり、上記式（２）を利用して下記式（４）のように表すことができる。

ところで、本発明の創出に際し、まず、発明者らは、ダンプトラックの重心Ｇ（図２参照）は、荷台１２の積載重量に応じて前後軸ｙに沿って多少移動するものの後輪１５の車軸近傍に位置することには変わりは無く、ダンプトラックの姿勢や積載重量が変化しても後輪１５は常に地面と接触している点に着目した。そして、ダンプトラックの姿勢の算出に際しては、後輪１５と地面の接触部（接地面Ｃ）に基準点Ｄを設定し、当該基準点Ｄを固定端にして基準点Ｄから線分ＰＱに下ろした垂線が揺動すると仮定できると考えた。つまり、基準点Ｄから線分ＰＱに下ろした垂線の足をＫとすると、図７に示すように、ダンプトラックの姿勢が変化しても、基準点Ｄを中心とし線分ＤＫの長さを半径とする球６１０の表面に常に点Ｋが位置すると仮定できる。なお、線分ＤＫは、その長軸を中心に回転することができ、線分ＰＱは水平面に対して角度を有することができる。このような仮定の下では、基準点Ｄから前記２点Ｐ，Ｑまでの距離（ベクトルＰＤ，ＱＤのスカラー）はダンプトラックの姿勢が変化しても一定であり、これらの距離は予め算出可能である。

このようにベクトルＰＤのスカラーと、ベクトルＱＤのスカラーは既知の値であり、また、ベクトルＯＰとベクトルＯＱはステップ１００で取得済みなので、上記２式（２），（３）から２組のｓ，ｔを求めることができる（ステップ１２０）。さらに、ダンプトラックは横転しないことを前提とすると、当該２組のｓ，ｔのうち下記の等式を満足するものが解となる。ただし、下記式においてsgnは符号を示すものとする。

上記２式（３），（４）から求めたs、tと、式（１）とからＯＤを算出する（ステップ１３０）。これにより３点Ｐ，Ｑ，Ｄの位置が決定するため、走行路に勾配がある場合にはαを適宜考慮して、ダンプトラックの姿勢を求めることができる。

そして、ダンプトラックの姿勢を、ロール角φ、ピッチ角θおよびヨー角ψで特定する場合には、さらに次の処理を行う。まず、コンピュータ１１０は、後輪１５ｄに係るサスペンション１７ｂの長さをレベリングセンサ１８で検出し、これにサスペンション１７ｂの上端から点Ｐ，Ｑまでの高さ距離を加えることでΔｚを算出し、点Ｔを決定する（ステップ１４０）。

そして、コンピュータ１１０は、上記式（１）で示したように、ステップ１４０で決定した点Ｔから２点Ｐ，Ｑに向かう２つのベクトルＴＱ，ＴＰの外積をとることで法線ベクトルＵを算出する（ステップ１５０）。ベクトルＵが算出できたら、コンピュータ１１０は、次の各式を利用することでロール角φ、ピッチ角θおよびヨー角ψを求める（ステップ１６０）。

すなわち、上記各式において、ヨー角ψはベクトルＰＱから算出でき、ピッチ角θはヨー角ψおよび法線ベクトルＵから算出でき、ロール角φはヨー角ψおよび法線ベクトルＵから算出できる。ロール角φ、ピッチ角θおよびヨー角ψを算出したら、ステップ１００に戻り、先述の処理を繰り返す。

なお、ステップ１６０からステップ１００に戻って新たな姿勢算出処理が開始されるタイミングが一定の間隔になるように、タイマでステップ１００の開始タイミングを制御しても良い。また、図５に示した各演算処理は、同じ演算結果が得られれば、各処理の順序を変更したり、複数の処理を同時に処理（並列処理）したりしても良い。

ところで、特に説明しなかったが、慣性計測装置１０３で計測したダンプトラックの加速度や角速度の変化を利用すれば、点Ｐ，Ｑの位置算出精度が向上するので、ダンプトラックの姿勢の算出精度を向上することができる。

以上のように、本実施の形態によれば、２つの位置推定装置（ＧＰＳ受信機１０１，１０２）だけで精度良くダンプトラックの姿勢を推定できる。

なお、上記の実施の形態では、道路勾配αがある場合を例に挙げて説明したが、勾配αがゼロ度である場合には図５のステップ１１０を実行することなくダンプトラックの姿勢を算出することができる。

＜第２の実施の形態＞
本発明の第２の実施の形態は、ＧＰＳ受信機１０１，１０２を介して得られる点Ｐ，Ｑの位置の時系列情報を記憶装置１０６に保存しておき、当該時系列情報に基づいて位置データの移動ベクトルを算出し、当該移動ベクトルに基づいてダンプトラックが走行する道路の勾配αを算出する点に特徴がある。本実施の形態と第１の実施の形態は、記憶装置１０６のデータ構造と、コンピュータ１１０によって勾配αが演算される点に相違点があるが、他の構成は姿勢算出処理も含めて第１の実施の形態と同じなので説明は省略する。

図８は、本発明の第２の実施の形態における記憶装置１０６のデータ構造のうち点Ｐ，Ｑの位置の時系列情報に関係する部分を示した図である。この図に示すように、本実施の形態に係る記憶装置１０６には、点Ｐ，Ｑの位置を検出した各時刻が時系列で記録される時刻列７０１と、時刻列７０１に係る各時刻における点Ｐの位置が記録される位置列７０２と、時刻列７１０に係る各時刻における点Ｑの位置が記録される位置列７０３とが保存されている。なお、ＧＰＳ受信機１０１，１０２から出力値の存在しない時刻も存在し、その場合には当該時刻には点Ｐまたは点Ｑの位置が算出できなかった旨を示す文字列ＦＦが入力される。

図５に示したステップ１１０に代替して本実施の形態に係るコンピュータ１１０が実行する勾配算出処理について説明する。勾配の算出に際して、コンピュータ１１０は、現時刻における２点Ｐ，Ｑの位置と、現時刻から所定のサンプル数前に係る２点Ｐ，Ｑの位置であって２点Ｐ，Ｑの出力が両方とも揃っているものの、合計４点の位置を記憶装置１０６から抽出する。例えば、当該４点の位置を図８の例で示すと、位置７０４，７０５，７０６，７０７がこれに該当する。

ところで、勾配算出のために抽出した４点は必ずしも同一平面上に存在している訳ではない。そこで、本実施の形態に係るコンピュータ１１０は、抽出した４点において同じ時刻に係る２点Ｐ，Ｑをそれぞれ結んで第１および第２の線分を描き、さらに当該第１の線分の中点と当該第２の線分の中点を結ぶことで第３の線分（位置データの移動ベクトル）を描き、当該第３の線分の水平面に対する傾斜角を道路勾配として算出している。なお、本実施の形態に係る勾配算出処理は、カント角が付いていないことを前提としている。

図９を用いて上記算出手順について説明する。図９は点Ｐ，Ｑの位置推定結果のモデル図である。図９には４点Ｐ（０）、Ｑ（０）、Ｐ（３）、Ｑ（３）が示されており、このうちＰ（０）とＱ（０）は現時刻における点Ｐ，Ｑの位置を示し、Ｐ（３）とＱ（３）は３サンプル数前の点Ｐ，Ｑの位置を示す。現時刻における２点Ｐ（０），Ｑ（０）から得られる線分ＰＱの中点をＲ０とし、３サンプル数前の２点Ｐ（０），Ｑ（０）から得られる線分ＰＱの中点をＲ１とする。このときの点Ｒ０、Ｒ１と勾配α[rad]は下記式により求めることができる。

このように、本実施の形態では、コンピュータ１１０において、ダンプトラックの走行地点における勾配αを点Ｐ，Ｑの現在および過去の位置データに基づいて算出できるので、当該勾配αと、３点Ｐ，Ｑ，Ｄの現在の位置に基づいてダンプトラックの姿勢を算出することができる。これにより、ダンプトラックの走行路の勾配情報が地図データに含まれていない場合（例えば、地図データの作成に際して勾配を測定しなかった場合）にも、道路の勾配を考慮したダンプトラックの姿勢が算出できる。

なお、上記ではΔｚ（車高）を算出する際に、リアサスペンション１７ｂの長さを検出するレベリングセンサ１８を利用したが、サスペンション１７ｂ内の流体（油圧）の圧力を圧力センサで検出することで荷台１２の積載重量を推定し、当該積載重量に基づいてΔｚを推定しても良い。

また、上記では、ダンプトラックの位置推定にＧＰＳ受信機１０１，１０２を利用したが、他の衛星からの航法信号を受信する受信装置をダンプトラックに同様に設置し、当該航法信号に基づいて測位を行う他の衛星測位システムを構成してダンプトラックの位置推定をしても良い。

また、微小な姿勢変化を検出する場合には、点Ｄから線分ＰＱへの垂線の長さをできるだけ長くした方が好ましい。そのため、基準点Ｄは線分ＰＱから最も離れた位置に設定することが好ましい。すなわち、図２の例では、後輪１５ｄの接地面Ｃにおける図中右下の頂点に基準点を設定することが好ましい。

また、上記では、右端の後輪１５ｄの接地面Ｃに基準点Ｄを設定したが、ダンプトラックの姿勢算出時に発生する誤差をできるだけ低減する観点からは、他の後輪よりもダンプトラックの重心Ｇ（図２参照）に近い位置にある２つの車輪１５ｂ，１５ｃの接地面Ｃ上において、重心Ｇに最も近い位置に基準点Ｄを設定することが好ましい。このように基準点Ｄを設定すると、ダンプトラックの揺動中心となる重心Ｇに基準点Ｄを近づけることができ、ダンプトラックの実際の動作に近い状態で計算できるので誤差を小さくできる。

また、２点Ｐ，Ｑと基準点Ｄは、各点Ｐ，Ｑ，Ｄを頂点とする三角形が正三角形になるように設定することが好ましい。すなわち、線分ＰＱ，ＱＤ，ＤＰの長さが均等になるように各点を設定することが好ましい。このようにすると、ダンプトラックの姿勢算出時に生じる誤差をロール角φとピッチ角θに均等に分散できるので、算出されるダンプトラックの姿勢の精度が総合的に向上する。

また、上記では、ダンプトラックに搭載したコンピュータ１１０でダンプトラックの姿勢を算出したが、自律走行する複数のダンプトラックの走行制御を行うコンピュータであって、管制局などの建屋に設置されたコンピュータにおいて無線通信装置等を介してダンプトラックから点Ｐ，Ｑの位置を入力し、当該コンピュータで姿勢を算出しても良い。

なお、本発明は、上記の実施の形態に限定されるものではなく、その要旨を逸脱しない範囲内の様々な変形例が含まれる。例えば、本発明は、上記の実施の形態で説明した全ての構成を備えるものに限定されず、その構成の一部を削除したものも含まれる。また、ある実施の形態に係る構成の一部を、他の実施の形態に係る構成に追加又は置換することが可能である。

また、上記のコンピュータに係る各構成や当該各構成の機能及び実行処理等は、それらの一部又は全部をハードウェア（例えば各機能を実行するロジックを集積回路で設計する等）で実現しても良い。また、上記のコンピュータに係る構成は、演算処理装置（例えばＣＰＵ）によって読み出し・実行されることで当該制御装置の構成に係る各機能が実現されるプログラム（ソフトウェア）としてもよい。当該プログラムに係る情報は、例えば、半導体メモリ（フラッシュメモリ、ＳＳＤ等）、磁気記憶装置（ハードディスクドライブ等）及び記録媒体（磁気ディスク、光ディスク等）等に記憶することができる。

また、上記の各実施の形態の説明では、制御線や情報線は、当該実施の形態の説明に必要であると解されるものを示したが、必ずしも製品に係る全ての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成が相互に接続されていると考えて良い。

１１…フレーム、１５…後輪、１７…リアサスペンション、１８…レベリングセンサ、１０１…ＧＰＳ受信機、１０２…ＧＰＳ受信機、１０３…慣性計測装置、１０６…記憶装置、１１０…コンピュータ、Ｐ…ＧＰＳ受信機１０１による位置算出点、Ｑ…ＧＰＳ受信機１０２による位置算出点、Ｃ…接地面、Ｄ…基準点、Δｚ…点Ｐ，Ｑから基準点Ｄまでの高さ距離、Ｇ…ダンプトラックの重心

Claims (2)

1. フレームに回転可能に取り付けられた複数の後輪と、
   車両前後方向に互いにずらして配置された２つの位置推定装置とを備え、
   ダンプトラックに設定した車両座標系において前記複数の後輪が地面と接触している部分から任意に選択した点を基準点としたとき、
   前記２つの位置推定装置は、当該２つの位置推定装置によって位置が算出される２点を結ぶ線分から前記基準点に対して垂線が下ろせるように配置されているダンプトラックにおいて、
   前記基準点が設定された後輪の位置における車高変化を検出するセンサと、
   前記ダンプトラックが走行する道路の勾配情報が記憶された記憶装置と、
   前記ダンプトラックが前記基準点を中心に回転して姿勢が変化すると仮定し、前記２つの位置推定装置によって算出される前記２点の位置から前記基準点の位置を算出し、前記２つの位置推定装置によって位置が算出される前記２点のうち少なくとも一方から地面までの距離を前記センサの検出値に基づいて算出し、前記２点のうち少なくとも一方から地面までの距離と、前記勾配情報と、前記２点および前記基準点の位置とに基づいて前記ダンプトラックの姿勢を算出する演算装置と
   を備えることを特徴とするダンプトラック。
2. フレームに回転可能に取り付けられた複数の後輪と、
   車両前後方向に互いにずらして配置された２つの位置推定装置とを備え、
   ダンプトラックに設定した車両座標系において前記複数の後輪が地面と接触している部分から任意に選択した点を基準点としたとき、
   前記２つの位置推定装置は、当該２つの位置推定装置によって位置が算出される２点を結ぶ線分から前記基準点に対して垂線が下ろせるように配置されているダンプトラックにおいて、
   前記基準点が設定された後輪の位置における車高変化を検出するセンサと、
   前記ダンプトラックが前記基準点を中心に回転して姿勢が変化すると仮定し、前記２つの位置推定装置によって算出される前記２点の位置から前記基準点の位置を算出し、前記２つの位置推定装置によって位置が算出される前記２点のうち少なくとも一方から地面までの距離を前記センサの検出値に基づいて算出し、前記ダンプトラックの走行地点における勾配を前記ダンプトラックの過去の位置データに基づいて算出し、その算出した勾配情報と、前記２点のうち少なくとも一方から地面までの距離と、前記２点および前記基準点の位置とに基づいて前記ダンプトラックの姿勢を算出する演算装置と
   を備えることを特徴とするダンプトラック。

Images