(57)【特許請求の範囲】
【請求項１】
車輪及び当該車輪上にサスペンションを介して車体を搭載し、当該車体に取り付けられ車体の加速度及び角速度を出力する車体慣性計測装置、前記車体に取り付けられた前記車輪の車輪接地点を含む路面までの距離を計測する路面距離計測装置、前記車輪の回転数に基づく車輪速度を出力する車輪速度計測装置、及び前記車輪の操舵角を計測する操舵角計測装置を備えたダンプトラックの車輪の滑り角推定装置であって、
前記路面距離計測装置が計測した前記路面上の計測点までの距離の系列を用いて、前記車体の前後軸、左右軸、及び前後軸と左右軸とに直交する上下軸からなる直交３軸系の車体座標系から、前記前後軸及び前記左右軸を前記路面に投影した直交２軸及びこの直交２軸に垂直な路面垂直軸からなる直交３軸系の路面座標系に変換する車体―路面座標変換情報を推定する路車間姿勢推定部と、
前記車体座標系で定義された前記車体慣性計測装置が計測した車体の加速度から重力加速度成分を取り除いて前記車体の運動に起因する加速度及び角速度を求め、当該車体の運動に起因する加速度及び角速度を前記車体―路面座標変換情報を用いて前記路面座標系に変換する路面上慣性量計算部と、
前記操舵角計測装置からの操舵角及び前記車輪速度計測装置が計測した車輪速度に基づいて前記車輪の傾き方向に沿った前記車輪速度計測装置に由来する車輪加速度ベクトルを求め、前記路面座標系に変換された車体の運動に起因する加速度及び角速度を用いて前記車体慣性計測装置に由来する前記車輪接地点における加速度ベクトルを求め、当該加速度ベクトルと前記車輪速度計測装置に由来する車輪接地点における車輪加速度ベクトルとの差分を基に前記車輪の横滑り角度を推定する車輪滑り角推定部と、
を備えることを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項２】
請求項１に記載のダンプトラックの車輪の滑り角推定装置であって、
前記路車間姿勢推定部は、前記路面上の計測点までの距離の系列に基づいて、前記車体
座標系で定義された前記路面の法線ベクトルを求め、前記路面垂直軸及び前記法線ベクト
ルの傾き角を算出し、当該傾き角を用いて前記車体―路面座標変換情報を推定する、
ことを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項３】
請求項１に記載のダンプトラックの車輪の滑り角推定装置であって、
前記車体のグローバル座標系からのずれ量を計測する車体姿勢計測装置を更に備え、
前記路面上慣性量計算部は、前記車体姿勢計測装置の計測値を基に前記グローバル座標
系から前記車体座標系に変換するグローバル―車体座標変換情報を推定し、当該グローバ
ル―車体座標変換情報を用いて重力加速度を前記車体座標系に変換し、変換後の重力加速
度を前記車体慣性計測装置が計測した前記車体の加速度から取り除く、
ことを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項４】
請求項１に記載のダンプトラックの車輪の滑り角推定装置であって、
前記車輪滑り角推定部は、前記車輪速度計測装置に由来する車輪加速度ベクトル及び前
記車体慣性計測装置に由来する加速度ベクトルの差分から前記路面座標系での向心加速度
ベクトルを求め、前記路面上において前記車輪速度計測装置に由来する車輪加速度ベクト
ルに垂直に交わるベクトル及び前記向心加速度ベクトルのなす角を車輪の横滑り角度とし
て推定する、
ことを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項５】
請求項１に記載のダンプトラックの車輪の滑り角推定装置において、
前記ダンプトラックは、左前輪、右前輪、左後輪、及び右後輪を備え、
前記車輪の滑り角推定部は、左前輪、右前輪、左後輪、及び右後輪のそれぞれについて
、各車輪が前記路面に接地する各車輪接地点での前記車輪速度計測装置に由来する車輪加
速度ベクトル、及び前記各車輪接地点での前記車体慣性計測装置に由来する加速度ベクト
ルを求め、これらを基に前記各車輪の横滑り角度を推定する、
ことを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項６】
請求項１に記載のダンプトラックの車輪の滑り角推定装置において、
前記路面距離計測装置は、前記車体の進行方向と交わる１本の直線で連結もしくは近似
することができる前記路面上の２点以上を同サンプル時に計測する、
ことを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項７】
車輪及び当該車輪上にサスペンションを介して車体を搭載し、前記車輪及び前記車体と
のストローク距離を計測するストロークセンサ、前記車体の速度を計測する車体速度計測
装置、及び前記車体のヨーレートを計測するヨーレートセンサ、及び前記車輪の操舵角を
計測する操舵角計測装置を備えたダンプトラックの車輪の滑り角推定装置であって、
前記ストローク距離を用いて、前記車体の前後軸、左右軸、及び前後軸と左右軸とに直
交する上下軸からなる直交３軸系の車体座標系から、前記車輪を連結する車輪軸位置にお
ける前記車輪が接地する路面に平行な平面に前記前後軸及び前記左右軸を投影した直交２
軸及びこの直交２軸に垂直な路面垂直軸からなる直交３軸系の路面座標系に変換する車体
―路面座標変換情報を推定する路車間姿勢推定部と、
前記ヨーレートセンサが出力する前記車体の角速度及び前記車体速度計測装置が計測し
た車体速度ベクトルを前記車体―路面座標変換情報に基づき前記路面座標系に変換し、こ
れら路面座標系に変換された前記車体の角速度及び前記車体速度ベクトルを基に前記平面
上の前記車輪軸位置における前記路面座標系で表されたける車体速度ベクトルを推定する
路面上慣性量計算部と、
前記車輪軸位置における車体速度ベクトルの横方向成分を前記車体速度ベクトルの大き
さで除算した値、及び前記操舵角計測装置から取得した操舵角に基づいて前記車輪の横滑
り角度を推定する車輪滑り角推定部と、
を備えることを特徴とするダンプトラックの車輪の滑り角推定装置。
【請求項８】
車輪及び当該車輪上にサスペンションを介して車体を搭載したダンプトラックにおける車輪の滑り角推定方法であって、
前記車体に取り付けられた前記車体及び前記車輪が接地する路面までの距離を測定する路面距離計測装置が計測した前記路面上の計測点までの距離の系列を用いて、前記車体の前後軸、左右軸、及び前後軸と左右軸とに直交する上下軸からなる直交３軸系の車体座標系から、前記前後軸及び前記左右軸を前記路面に投影した直交２軸及びこの直交２軸に垂直な路面垂直軸からなる直交３軸系の路面座標系に変換する車体―路面座標変換情報を推定するステップと、
前記車体に取り付けられた車体慣性計測装置が計測した加速度及び角速度を取得し、当該加速度から重力加速度成分を取り除いて前記車体の運動に起因する加速度及び角速度を求め、当該車体の運動に起因する加速度及び角速度を前記車体―路面座標変換情報を用いて前記路面座標系に変換するステップと、
前記車輪の操舵角を計測する操舵角計測装置から操舵角を取得し、前記車輪の回転数に基づく車輪速度を計測する車輪速度計測装置から車輪速度を取得し、前記車輪の傾き方向に沿った前記車輪速度計測装置に由来する車輪加速度ベクトルを求め、前記路面座標系に変換された車体の運動に起因する加速度及び角速度を用いて前記車体慣性計測装置に由来する車輪接地点における加速度ベクトルを求め、当該加速度ベクトルと前記車輪速度計測装置に由来する前記車輪接地点における車輪加速度ベクトルとの差分を基に前記車輪の横滑り角度を推定するステップと、
を含むことを特徴とするダンプトラックの車輪の滑り角推定方法。
【請求項９】
車輪及び当該車輪上にサスペンションを介して車体を搭載したダンプトラックにおける車輪の滑り角推定方法であって、
前記車輪及び前記車体とのストローク距離を計測するストロークセンサから取得した前記ストローク距離を用いて、前記車体の前後軸、左右軸、及び前後軸と左右軸とに直交する上下軸からなる直交３軸系の車体座標系から、前記車輪を連結する車輪軸位置における前記車輪が接地する路面に平行な平面に前記前後軸及び前記左右軸を投影した直交２軸及びこの直交２軸に垂直な路面垂直軸からなる直交３軸系の路面座標系に変換する車体―路面座標変換情報を推定するステップと、
前記車体のヨーレートを計測するヨーレートセンサから前記車体の角速度を取得し、前記車体の速度を計測する車体速度計測装置から車体速度ベクトルを取得し、前記取得した角速度及び車体速度ベクトルを前記車体―路面座標変換情報に基づき前記路面座標系に変換し、これら路面座標系に変換された前記角速度及び前記車体速度ベクトルを基に前記平面上の前記車輪軸位置における前記路面座標系で表された車体速度ベクトルを推定するステップと、
前記車輪軸位置における車体速度ベクトルの横方向成分を前記車体速度ベクトルの大きさで除算した値及び前記車輪の操舵角を計測する操舵角計測装置から取得した操舵角に基づいて前記車輪の横滑り角度を推定するステップと、
を含むことを特徴とするダンプトラックの車輪の滑り角推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、鉱山や建設現場で移動するダンプトラックに関し、特に車輪の滑り角推定装置及びその方法に関する。
【背景技術】
【０００２】
ダンプトラックにおいて車輪の滑り角は車体の安定化を図るためにも重要なパラメータである。車輪の滑り角を推定する技術として、特許文献１には、車輪を有する車体に取り付けられた慣性計測装置と操舵角を計測し、車輪の滑り角を推定する構成が開示されている。特許文献１における車輪の滑り角の推定技術は、４つの車輪が接地する平坦な路面が車体と平行であることを仮定している。この仮定によって、路面上での速度を出力する車輪と、ばね上の車体の加速度や角速度といった慣性計測装置の出力軸が合うため、車輪の滑り角を正しく求めることが出来る。そして、多くの乗用車ではこの仮定は大きくずれることは無い。これは、該文献の００３８段落に記載のある「ロール角が発生すると（中記省略）タイヤ接地点が左右に移動するため、これと車速Vとの関係でタイヤスリップ角が発生する。」の文言からも判る通り、車体が傾くとタイヤも呼応して傾くことを用いて滑り角を求めていることに相当する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−０７６６１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
一般に車両は車輪上にばねを介して車体を搭載して構成される。ダンプトラックのようにばね上の車体に対して積荷により大きな荷重が加えられる場合、ばねが収縮し、車輪及び車輪軸に対して車体が荷重で傾く。車輪及び車輪軸が路面と平行であっても、ばねの収縮によってばね上の車体は傾くので、車体と路面が平行であると仮定をおくことが出来なくなる。このため、車体に取り付けられた慣性計測装置で計測される値は、路面とは違った方向の加速度を計測することになり、正確な車輪滑り角を算出することが難しい。そのため、特許文献１に記載の技術を、乗用車と比べて車体に大きな重量がかかるダンプトラックに適用しても、車体の滑り角を正確に測定することは難しいという課題が残る。
【０００５】
本発明は上記課題に鑑みてなされたものであり、積載状態によって車体重量が大きく変化するダンプトラックにおいて、車体に取り付けられた慣性計測装置からの出力値を用いて車輪の滑り角を精度よく推定することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するために、本発明の一態様は、車輪及び当該車輪上にサスペンションを介して車体を搭載し、当該車体に取り付けられ車体の加速度及び角速度を出力する車体慣性計測装置、前記車体に取り付けられた前記車輪の車輪接地点を含む路面までの距離を計測する路面距離計測装置、前記車輪の回転数に基づく車輪速度を出力する車輪速度計測装置、及び前記車輪の操舵角を計測する操舵角計測装置を備えたダンプトラックの車輪の滑り角推定装置であって、前記路面距離計測装置が計測した前記路面上の計測点までの距離の系列を用いて、前記車体の前後軸、左右軸、及び前後軸と左右軸とに直交する上下軸からなる直交３軸系の車体座標系から、前記前後軸及び前記左右軸を前記路面に投影した直交２軸及びこの直交２軸に垂直な路面垂直軸からなる直交３軸系の路面座標系に変換する車体―路面座標変換情報を推定する路車間姿勢推定部と、前記車体座標系で定義された前記車体慣性計測装置が計測した車体の加速度から重力加速度成分を取り除いて前記車体の運動に起因する加速度及び角速度を求め、当該車体の運動に起因する加速度及び角速度を前記車体―路面座標変換情報を用いて前記路面座標系に変換する路面上慣性量計算部と、前記操舵角計測装置からの操舵角及び前記車輪速度計測装置が計測した車輪速度に基づいて前記車輪の傾き方向に沿った前記車輪速度計測装置に由来する車輪加速度ベクトルを求め、前記路面座標系に変換された車体の運動に起因する加速度及び角速度を用いて前記車体慣性計測装置に由来する前記車輪接地点における加速度ベクトルを求め、当該加速度ベクトルと前記車輪速度計測装置に由来する車輪接地点における車輪加速度ベクトルとの差分を基に前記車輪の横滑り角度を推定する車輪滑り角推定部と、を備えることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、車体重量が大きく変化するダンプトラックにおいて、車体に取り付けられた慣性計測装置からの出力値を用いて車輪の滑り角を精度よく推定することができる。なお、上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【０００８】
【図１】ダンプトラックの概略構成を示す図
【図２】４つの座標系を示す説明図であって、（ａ）はグローバル座標系、（ｂ）は路面座標系、車体座標系、及び路面距離計測装置座標系Ｌ１、Ｌ２を示す。
【図３】車体座標系を示す図
【図４】路面座標系を示す説明図
【図５】ダンプトラックの機能構成を示すブロック図
【図６】路車間姿勢推定部の処理フローを示した図
【図７】Lidarの座標系および計測点を示す図
【図８】車体に取り付けたLidarの計測点列のモデル図
【図９】路面上慣性量計算部の処理フローを示した図
【図１０】車輪滑り角推定部の処理フローを示した図
【図１１】車輪における速度・加速度・舵角・滑り角の関係を示す図
【図１２】車両を上面からみた２次元速度ベクトルを示した図
【図１３】第二実施形態におけるダンプトラックの機能構成を示すブロック図
【図１４】第二実施形態に係る路車間姿勢推定部のおける路車間姿勢推定部の処理フローを示した図
【図１５】第二実施形態における路面上慣性量計算部の処理フローを示した図
【図１６】第二実施形態における実施例２における車輪滑り角推定手段の処理フローを示した図
【図１７】第二実施形態における車輪における速度・滑り角の関係を示すモデル図
【発明を実施するための形態】
【０００９】
以下、路面上の各車輪位置における滑り角を精度よく算出するための処理について説明する。
【００１０】
＜第一実施形態＞
まず、センサからの計測値を主に使用し、推定値が可能な限り少ない構成での車輪滑り角推定方法について説明する。図１は、ダンプトラックの概略構成を示す図である。ダンプトラック１００は、車輪１０１と左右の車輪を繋いだ車輪軸１０２と、車輪軸とばね等（例えば不図示のサスペンション）を介してつながった頑丈なフレームである車体１０３とを含んで構成される。
【００１１】
ここで説明に使用する４つの座標系を定義する。図２は、４つの座標系を示す説明図であって、（ａ）はグローバル座標系、（ｂ）は路面座標系、車体座標系、及び路面距離計測装置座標系Ｌ１、Ｌ２を示す。図３は車体座標系を示す図である。図４は路面座標系を示す説明図である。
【００１２】
第一に、車両の現在地から遠くない、地球上のある点を原点とし、重力方向に対して垂直に交わる平面上にXe軸、Ye軸を設定し、Xe軸とYe軸から右手系を成す向きにZe軸をとった3軸直交座標系を、グローバル座標系eと呼ぶ（図２（ａ）参照）。
【００１３】
第二に、車体前後方向にXb軸、車体左右方向Yb軸、上方向にZb軸を取った3軸直交座標系を車体座標系ｂと呼ぶ（図２（ｂ）参照）。図３に示すに車体座標系ｂの原点は、車体内に固定された任意の点とする。
【００１４】
第三に、複数個の車輪１０１が接地する点により構成される面上を路面と呼び、ダンプトラック１００が動作する路面は、どの瞬間を見てもおおむね平坦な平面であると仮定する。この路面上に車体座標系bのXb軸、Yb軸を上記平面に射影した軸をそれぞれ路面座標系rのXr軸、Yr軸とし、上記Xr軸とYr軸から右手系を成す向きにZr軸をとった３軸直交座標系を路面座標系ｒと呼ぶ（図４参照）。この定義から、グローバル座標系eからみると、路面座標系ｒは、車体座標系ｂとともに逐次移動しているように見える。また、Xr軸とYr軸の方向は、車体座標系ｂにおけるXb軸、Yb軸の方向と一致しているが、Zr軸は上記複数個の車輪が接地する点により構成される面に対して垂直となる方向とはなるが、車体１０３が必ずしも上記路面に対し平行とはならないので、車体座標系bのZb軸と必ずしも一致するとは限らない。
【００１５】
第四に、車体１０３の任意の点に固定された路面までの距離を計測する構成（路面距離計測装置という。詳細は後述する。）において、距離を計測する始点を原点とし、距離を計測する方向にXL軸、XL軸と垂直な方向にYL軸、XL軸とYL軸から右手系を成す向きにZL軸をとった３軸直交座標系を路面距離計測装置座標系Lと呼ぶ。この定義から、グローバル座標系eからみると、路面距離計測装置座標系Lは、車体座標系ｂとともに逐次移動しているように見える。車体１０３に複数の路面距離計測装置が固定されていた場合、それぞれに対して座標系が定義され、n個の路面距離計測装置中のi番目の路面距離計測装置によって定義される座標系を路面距離計測装置座標系Liと呼ぶ。
【００１６】
以下の説明において、変数や値の右側についたe、b、r、Liの添え字は、添え字ついた変数や値が、それぞれ、グローバル座標系e、車体座標系ｂ、路面座標系ｒ、i番目の路面距離計測装置座標系Liで表わされた変数や値であることを示している。
【００１７】
車輪の滑り角とは、上記路面座標系ｒ上にて、車輪１０１の向いている向きと車輪速度ベクトルが成す角のことをいうものとする。
【００１８】
図５は、ダンプトラック１００の機能構成を示すブロック図である。ダンプトラック１００の車体１０３には、図５に示すように、重力加速度を含んだ加速度や角速度などを車体座標系ｂで計測する車体慣性計測装置１０４と、車体の進行方向と交わる1本の直線で連結もしくは近似することができる路面上の２点以上を同サンプル時に計測可能な路面距離計測装置１０５と、車輪の車体座標系ｂのXb軸からの傾きを計測する操舵角計測装置１０６、車輪の回転数に基づく車輪速度を計測する車輪速度計測装置１０７と、グローバル座標系eのXe軸とYe軸によって張られる水平面と車体座標系ｂのXb軸およびYb軸が成す傾斜角度およびグローバル座標系eのXe軸から車体座標系ｂのXb軸までの回転角度で表わされる車体姿勢を計測する車体姿勢計測装置１０８とが取り付けられている。
【００１９】
またダンプトラック１００は、車体滑り角推定装置１２０を更に備え、上記車体慣性計測装置１０４、路面距離計測装置１０５、操舵角計測装置１０６、車輪速度計測装置１０７、車体姿勢計測装置１０８の其々は車体滑り角推定装置１２０に各装置の計測結果を出力する。車体滑り角推定装置１２０は、路車間姿勢推定部１２１、路面上慣性量計算部１２２、及び車輪滑り角推定部１２３を含む。車体滑り角推定装置１２０は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）等の演算・制御装置の他、車体滑り角推定装置１２０で実行されるプログラムを格納するＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）やＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）等の記憶装置、また、ＣＰＵがプログラムを実行する際の作業領域となるＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）を含むハードウェアと、路車間姿勢推定部１２１、路面上慣性量計算部１２２、及び車輪滑り角推定部１２３の各機能を実現するソフトウェアとが協働することにより構成される。
【００２０】
路車間姿勢推定部１２１は、路面距離計測装置１０５にて計測した路面との距離の系列を用いて車体１０３と路面との相対的な傾きを車体座標系ｂから路面座標系rへの座標変換行列Crbとして推定する。
【００２１】
路面上慣性量計算部１２２は、路車間姿勢推定部１２１と車体慣性計測装置１０４と車体姿勢計測装置１０８から、重力加速度成分が取り除き車体座標系ｂで表わされた車両の慣性量を路面座標系ｒで表わされた車両の慣性量に変換する。
【００２２】
車輪滑り角推定部１２３は、路面上慣性量計算部１２２と操舵角計測装置１０６および車輪速度計測装置１０７を用いて、各車輪の滑り角を高精度に推定する。
【００２３】
以下に路車間姿勢推定部１２１での、路面と車体との相対姿勢を推定する方法を図６〜
図８を参照して説明する。図６は、路車間姿勢推定部の処理フローを示した図である。図
７は、Lidarの座標系および計測点を示す図である。図８は、車体に取り付けたLidarの計
測点列のモデル図である。
【００２４】
まず、ステップ６０１にて、路面距離計測装置１０５によって車体１０３に固定された
路面距離計測装置１０５から路面までの距離を計測する。第一実施形態では距離計測装置
として、レーザーにより線走査を行うLidarを２つ用いた場合を例に説明する。Lidarは図
７に示すように、ある点までの距離を連続的に線状に走査する。また、Lidarから計測さ
れる距離は、走査した角度と計測点Pまでの距離ｌであるとする。上記、路面距離計測装
置座標系Lの定義に従い、Lidarの走査レーザー射出面方向にXL軸、それと右手系を成す側
面方向にYL軸、それら２つの軸によって形成される法線方向にZL軸を規定する。点Pまで
の距離ｌは、計測した際のXL軸と成す角度θによって、３次元座標（pxL、pyL、pzL）で
表わすことが可能である。Lidarに固定された路面距離計測装置座標系Lから車体座標系b
への座標変換行列ＣｂＬはLidarの取り付け角度を計測することで事前に与えられる。２
つのLidar８０１、８０２は図８に示すように互いのレーザーが交差するように（交点８
０５）、路面に向けて車体側面に設置されているものとする。
【００２５】
次にステップ６０２にて、路面上の点として計測された点座標系列をレーザーの走査線
分だけ作る。本実施形態では２つのLidar８０１、８０２で合計２本のレーザー走査を行
ったとして記す。路面点座標系列は、P11＝(p11xL、p11yL、p11zL)、P12＝(p12xL、p12yL
、p12zL)、…、P1i＝(p1ixL、p1iyL、p1izL)のように作成される。
【００２６】
次にステップ６０３にて、ステップ６０２で生成した路面点座標系列を車体座標系で表
わす。Lidar８０１によってn個の計測点列が計測され、それぞれの値が路面距離計測装置
座標系LでP1i=（p1ixL、p1iyL、p1izL）（0<i<=n）であったとする。このとき、各点を車
体座標系ｂで表すには、Lidar８０１の取り付けられている位置を車体座標系ｂで表わし
た点（P1xb、P1yb、P1zb）により、次式（１）のように求めることができる。
【数１】
0006473684.tif 000002
（車体座標系ｂで表わされた路面上の計測点座標）＝（路面距離計測装置座標系Lから車
体座標系ｂへの変換行列）×（路面距離計測装置座標系Lで表わされた計測点座標）＋（L
idar８０１の取り付けられている位置を車体座標系ｂで表わした点）
【００２７】
同様にLidar８０２でもｍ個の計測点列が計測され、それぞれの値が路面距離計測装置
座標系LでP2i=（p2ixL、p2iyL、p2izL）（0<i<=m）であったとする。このとき、各点を車
体座標系bで表すには、Lidar８０２の取り付けられている位置を車体座標系ｂで表わした
点（P2xb、P2yb、P2zb）により、次式（２）のように求めることができる。
【数２】
0006473684.tif 000003
（車体座標系ｂで表わされた路面上の計測点座標）＝（路面距離計測装置座標系Lから車
体座標系ｂへの変換行列）×（路面距離計測装置座標系Lで表わされた計測点座標）＋（L
idar８０２の取り付けられている位置を車体座標系ｂで表わした点）
【００２８】
次にステップ６０４にて路面平面の法線ベクトルを求める。この法線ベクトルは路面座標系ｒのＺｒ軸上の単位ベクトルを車体座標系ｂで表わしていることになる。
【００２９】
２つの距離計測装置（本実施形態ではLidar８０１、８０２）は計測の同期がとれてい
るものとすると、1回の走査によって、各距離計測装置から得られた２つの点列合わせて
ｎ＋ｍ個を得ることが出来る。これらの点が成す平面を路面として、最小二乗法により路
面の法線ベクトルＵｂ求める。法線ベクトルＵｂは路面を表わす平面方程式Ａｘ＋Ｂｙ＋
Ｃｚ＋Ｄ＝０の法線単位ベクトルであり、次式（３）のように表される。
【数３】
0006473684.tif 000004
【００３０】
ステップ６０３で求めた、２つの路面距離計測装置１０５によって計測された車体座標系ｂで表わされた路面上の計測点座標であるP1i=（p1ixL、p1iyL、p1izL）（0<i<=n）およびP2i=（p2ixL、p2iyL、p2izL）（0<i<=m）から、最小二乗法により法線ベクトルUbは次式（４）で求めることができる。
【数４】
0006473684.tif 000005
【００３１】
次にステップ６０５にて、車体座標系ｂから路面座標系ｒへの座標変換行列を計算する。路面座標系ｒは上述の定義に従い、車体座標系ｂのＸｂ軸、Ｙｂ軸を路面に射影した２つの直交ベクトルexｒ、eyｒとその法線ezrベクトルがそれぞれＸｒ軸、Ｙｒ軸、Ｚｒ軸と合致することで形成される。ステップ６０４で求めた車体座標系ｂで表わされた法線ベクトルＵｂは路面座標系ｒのＺｒ軸上に存在するため、車体座標系ｂのＺｂ軸と同じく車体座標系で表された路面の平面方程式から得られた法線ベクトルＵｂ＝（Ａ、Ｂ、Ｃ）との回転行列Ｒｂは四元数（q0、q1、q2、q3）により次式（５）のように求めることが出来る。
【数５】
0006473684.tif 000006
【００３２】
上述のように、車体座標系ｂで表わされた路面の法線ベクトルＵｂは路面座標系のＺｒ軸と重なるため、車体座標系ｂから路面座標系ｒへの座標変換行列Ｃｒｂ＝Ｒｔｂと求めることができる。以上の処理フローを経て路車間姿勢推定部１２１の処理を終了する。
【００３３】
次に路面上慣性量計算部１２２の処理を図９に示すフローに従って説明する。図９は路面上慣性量計算部の処理フローを示した図である。
【００３４】
まず、ステップ９０１にて、車体姿勢計測装置１０８により車体の姿勢を計測する。本実施形態では姿勢は重力加速度の向きに垂直に交わる平面上に設定されたグローバル座標系eに対する車体座標系ｂへの変換を求めるパラメータである。一例として、位置推定装置を３つ取り付けることにより姿勢を推定する方法が特開2010-190806号公報に示されており、これを姿勢計測手段に適用することで姿勢を計測する。上記方法により計測される姿勢はグローバル座標系eから車体座標系ｂへの３つの変換パラメータであり、ロール角ψ、ピッチ角φ、ヨー角θと呼ぶ。 
【００３５】
次にステップ９０２にて、グローバル座標系eから車体座標系ｂへの座標変換行列Ｃｅｂを求める。ステップ９０１で計測された角度は、グローバル座標系eの各軸をXe軸、Ye軸、Ze軸の回転角度であり、本実施形態では、Ze軸、Ye軸、Xe軸の順番にそれぞれの角度分だけ回転させることを考える。このとき、座標変換行列Ｃｅｂは次式（６）のように求まる。
【数６】
0006473684.tif 000007
【００３６】
次にステップ９０３にて、車体座標系ｂにおける慣性量を計測する。ここで慣性量とは加速度や角速度をいう。車体慣性計測装置１０４は車体座標系ｂの各軸に対する重力加速度を含めた加速度と角速度を計測する。本実施形態では車体慣性計測装置１０４として、車体座標系ｂのＸｂ軸、Ｙｂ軸、Ｚｂ軸それぞれに備えられた３軸加速度センサ、３軸ジャイロセンサを考える。
【００３７】
次にステップ９０４にて、車体１０３の運動による加速度および角速度を計算する。ステップ９０２にて算出したグローバル座標系eから車体座標系bへの座標変換行列を用いて、ステップ９０３で得られた加速度センサの出力値ベクトルαｂより車体座標系ｂにおける重力項を取り除き、車体１０３が運動することによって生じる加速度ベクトルａｂを算出する。
【数７】
0006473684.tif 000008
（車体座標系ｂの車体加速度）＝（車体座標系ｂで計測された加速度センサ出力値）―（グローバル座標系eから車体座標系ｂへの座標変換行列）×（グローバル座標系eで表わされた重力加速度ベクトル）
【００３８】
車体角速度はグローバル座標系eで表わされた誤差入力がない限り、ステップ９０３で得られたジャイロセンサの出力値ベクトルωｂ（次式（８）参照）をそのまま用いる。
【数８】
0006473684.tif 000009
【００３９】
次にステップ９０５にて、車輪の接地点における、車体運動によって生じる加速度および角速度を路面座標系ｒで表わす。ステップ９０４にて求めた車体座標系ｂで表わされた車体に加わる加速度ベクトルａｂと車体座標系各軸の角速度であるジャイロセンサの出力値ベクトルωｂを路車間姿勢推定部１２１にて求めた座標変換行列Ｃｒｂを用いて路面座標系ｒで表わす。車体座標系ｂから路面座標系ｒへの座標変換行列Ｃｒｂにより、路面座標系ｒで表わされる車体慣性計測装置１０４上の加速度ベクトルａｒおよび角速度ベクトルωｒは次式（９）のように表わされる。
【数９】
0006473684.tif 000010
【００４０】
よって、車輪接地点の路面座標系ｒで表わされた加速度ベクトルａｔｒは次式（１０）のように表される。
【数１０】
0006473684.tif 000011
【００４１】
以上の処理フローを経て路面上慣性量計算部１２２の処理を終了する。
【００４２】
次に車輪滑り角推定部１２３の処理を図１０及び図１１を参照して説明する。図１０は、車輪滑り角推定部の処理フローを示した図である。図１１は、車輪における速度・加速度・舵角・滑り角の関係を示す図である。路面上の加速度・角速度が求まれば、車輪滑り角推定部１２３において以下のように各車輪１０１における滑り角を求める。
【００４３】
まず、ステップ１００１にて、操舵角計測装置１０６にて計測した車輪の車体座標系ｂのＸｂ軸に対する傾きδbを取得する。
【００４４】
次に、ステップ１００２にて、車輪の傾きδbを路面座標系ｒのＸｒ軸に対する傾きδr１１０１に変換する。ここでは、車体座標系ｂと路面座標系ｒの定義から、Ｘｂ軸とＸｒ軸の向きは合致しているため、δb=δr１１０１としてもよいことが明らかである。
【００４５】
次に、ステップ１００３にて、車輪速度計測装置１０７にて車輪接地点における速度Ｖを取得する。前述のように車輪接地点における車輪速度は路面上の進行速度（スカラー値）である。
【００４６】
次に、ステップ１００４にて、路面座標系ｒで表わした車輪接地点での速度ベクトルＶｒ１１０２を求める。車輪回転量が車輪の傾き方向の回転を表わしているとすると、ステップ１００３で得られた車輪速度Ｖは路面座標系ｒの車輪接地点での速度ベクトルＶｒ１１０２の大きさであると言えるので、路面座標系ｒの車輪接地点での速度ベクトルＶｒ１１０２は次式（１１）のように表わすことができる。
【数１１】
0006473684.tif 000012
（路面座標系ｒにおける車輪接地点での速度ベクトル）＝（路面座標系ｒにおける車輪速度の大きさ）×（車輪傾きによる路面座標系分配成分）
ただし、Ｖは車輪速度計測装置１０７から得られた速度であり、δｒは、ステップ１００２にて得られた路面座標系ｒのＸｒ軸に対する傾きである。
【００４７】
次にステップ１００５にて、路面座標系ｒで表わした車輪接地点での加速度ベクトル（ｄＶｒ／ｄｔ）１１０３を求める。加速度ベクトルはステップ１００４にて計算された速度ベクトルＶｒ１１０２を時間微分した値とすればよいので以下のように表わすことができる。
【数１２】
0006473684.tif 000013
【００４８】
次にステップ１００６にて、路面座標系ｒでの車輪接地点における向心加速度ベクトルαtr１１０４を計算する。車体慣性計測装置１０４由来の路面座標系ｒでの車輪接地点での加速度ベクトルａtr１１１２と、 車輪速度計測装置１０７由来の路面座標系ｒで表わした車輪接地点での加速度ベクトル（ｄＶｒ／ｄｔ）１１０３との差から、路面座標系ｒでの車輪接地点における向心加速度ベクトルαtr１１０４を次式（１３）のように計算する。 
【数１３】
0006473684.tif 000014
【００４９】
次にステップ１００７にて、車輪滑り角を求める。
ステップ１００６にて計算した、路面座標系ｒでの車輪接地点における向心加速度ベクトルαtr１１０４と垂直な方向に旋回速度成分、ベクトルｖtr１１０５が生じるので、車輪の横滑り角度β１１０６は路面座標系ｒでの車輪接地点における向心加速度ベクトルαtr１１０４と車輪進行方向と垂直に交わるベクトルとの成す角β１１０６として次式（１４）に従って求めることができる。
【数１４】
0006473684.tif 000015
（横滑り角度）＝arccos{(路面座標系ｒでの車輪接地点における向心加速度ベクトルと車輪横方向の単位ベクトルとの内積)/(路面座標系ｒでの車輪接地点における向心加速度ベクトルの大きさ)}
【００５０】
以上の処理フローを経て車輪滑り角推定部１２３の処理を終了する。
【００５１】
図１２は車両を上面からみた２次元速度ベクトルを示した図である。車輪滑り角推定部１２３は、上述の方法で、左前輪１２０２の滑り角、右前輪１２０３の滑り角、左後輪１２０４の滑り角、右後輪１２０５の滑り角を求めることができる。ただし、後輪１２０４、１２０５のように複数の車輪が連なって構成されている場合は、車輪と車輪との間をとって滑り角を求めてもよい。
【００５２】
以上の処理により、車輪滑り角推定部１２３では車体に取り付けられた車体慣性計測装置１０４の出力値を用いて高精度に滑り角を推定することができる。
【００５３】
第一実施形態によれば、車体重量が大きく変化するダンプトラックにおいて、車輪滑り角推定部１２３では車体に取り付けられた車体慣性計測装置の出力値を用いて車体と路面が平行になっておらずとも高精度に各車輪における滑り角を推定することができる。
【００５４】
＜第二実施形態＞
次に、第一実施形態と異なった少ない計測装置の構成で車輪滑り角推定方法について説明する。第二実施形態でもダンプトラックが動作する路面は、どの瞬間を見てもおおむね平坦な平面であると仮定する。また、各座標系の定義は第一実施形態と同じである。
【００５５】
図１３は第二実施形態におけるダンプトラックの機能構成を示すブロック図である。ダンプトラック１００ａは、第一実施形態と同様、車輪１０１と左右の車輪を繋いだ車輪軸１０２と、車輪軸がばね等によりつながった車体１０３とで構成される。
【００５６】
ダンプトラック１００ａの車体１０３に取り付けられた車体慣性計測装置１０４として、図１３に示すように、車体座標系ｂのＺｂ軸周りの角速度を計測するヨーレートセンサ１３０１を備えた場合を考える。また、路面距離計測装置１０５として、車輪軸１０２に取り付けられたストロークを計測するストロークセンサ１３０２を用いる。車輪の傾きを計測する操舵角計測装置１０６を有し、車輪速度計測装置１０７に代わり車体１０３の路面に対する速度を車体座標系ｂで計測する車体速度計測装置１３０３が取り付けられている。車体速度計測装置１３０３は、例えば車体に取り付けたGNSSアンテナで計測されるドップラー周波数を用いて推定したグローバル座標系eの速度を第一実施形態で用いたような車体姿勢計測装置等により車体座標系ｂに変換したり、直接車体に取り付けたミリ波レーダーで車体と路面との相対速度を計測するなどが当たる。
【００５７】
第二実施形態における車輪滑り角の推定方法について述べる。まず、路車間姿勢推定部１３２１はストロークセンサ１３０２の計測値を用いて、路面と車体座標系ｂのＸｂ軸Ｙｂ軸で張られる平面との傾きを推定し、車体座標系ｂから路面座標系rへの座標変換行列Crbとして算出する。
【００５８】
次に、路面上慣性量計算部１３２２はヨーレートセンサ１３０１で計測される車体１０３のＺｂ軸周りの角速度と車体速度計測装置１３０３で計測される車体の速度を計測し、路車間姿勢推定部１３２１にて推定された車体座標系ｂから路面座標系rへの座標変換行列Crbから、路面座標系ｒで表わされた車輪接地点の速度および角速度に変換する。そして、各車輪の滑り角を推定する車輪滑り角推定部１３２３は操舵角計測装置１０６と路面上慣性量計算部１３２２で得られた路面座標系ｒで表わされた車輪接地点の速度および角速度を用いて、車輪接地点の横速度を求めることで、車輪滑り角を高精度に推定する。
【００５９】
上述の構成で車輪滑り角を推定する一つの方法を例示する。
【００６０】
初めに、路車間姿勢推定部１３２１の処理について図１４のフローに従い説明する。図１４は第二実施形態に係る路車間姿勢推定部のおける路車間姿勢推定部の処理フローを示した図である。
【００６１】
ステップ１４０１にて、各車輪に対して取り付けられたストロークセンサ１３０２によって車体１０３に取り付けられたストロークセンサ１３０２の始点から車輪軸１０２までの距離を計測する。ストロークセンサ１３０２は車輪軸１０２に垂直もしくは一定の角度を保ったまま車輪軸１０２と車体１０３のストロークセンサ１３０２が取り付けられた始点の位置との距離を計測するものとする。最終的に推定したい値は路面座標系ｒと車体座標系ｂとの傾きであるので、車輪１０１のひずみは無視できると考え、ばね下の車輪軸は路面平面と平行であると仮定すれば直接路面上の点を計測する必要はない。
【００６２】
次に、ステップ１４０２にて、車体座標系ｂで表わされた車輪軸１０２の位置を求める。合計n個のストロークセンサ１３０２に固定された路面距離計測装置座標系Lから車体座標系ｂへの座標変換行列ＣｂＬは取り付け位置や方向を計測することで事前に与えられているとすれば、路面距離計測装置座標系Lで表わされるi番目のストロークセンサi(0<i<n)からの出力ｌｉＬによって、計測される車輪軸１０２の位置（pixb、piyb、pizb）を車体座標系ｂで表すと、ストロークセンサの取り付け位置を車体座標系ｂで表わした点Pi（Pixb、Piyb、Pizb）により、以下のように求めることができる。
【数１５】
0006473684.tif 000016
【００６３】
次にステップ１４０３にて、路面平面の法線ベクトルを求める。合計n個の点を計測した場合、これらに最も近い平面を路面と平行な平面として求めることができ、路面の法線ベクトルUbを平行移動したベクトルを求めることもできる。後段での傾きを求める際にはこの平面の法線ベクトルの始点は重要でないため、路面と平行な平面の法線ベクトルを路面の法線ベクトルＵbとしてもよい。最小二乗法により路面の法線ベクトルUbを求めると、次式（１６）以下のように表される。
【数１６】
0006473684.tif 000017
【００６４】
ステップ１４０２で求めた、ストロークセンサ１３０２によって計測された車体座標系ｂで表わされた車輪軸１０２の点座標（pixb、piyb、pizb）（3<i）から、最小二乗法によりUbベクトルは以下で求めることができる。
【数１７】
0006473684.tif 000018
【００６５】
次にステップ１４０４にて、車体座標系ｂから路面座標系ｒへの座標変換行列を計算する。路面座標系ｒは上述の定義に従い車体座標系ｂのＸｂ軸、Ｙｂ軸を路面に射影した２つの直交ベクトルを考えexr、eyrとその法線ezrベクトルがそれぞれＸｒ軸、Ｙｒ軸、Ｚｒ軸と合致することで形成される。ステップ１４０３で求めた車体座標系ｂのＺｂ軸と同じく車体座標系で表された路面に平行なの平面方程式から得られたベクトルUb＝（Ａ、Ｂ、Ｃ）との回転行列Ｒｂから車体座標系ｂから路面座標系ｒへの座標変換行列Ｃｒｂは第一実施形態のステップ６０５と同様にして求めることが出来る。
【００６６】
以上で路車間姿勢推定部１３２１の処理を終了する。
【００６７】
次に路面上慣性量計算部１３２２の処理を図１５のフローに従って説明する。図１５は、第二実施形態における路面上慣性量計算部の処理フローを示した図である。
【００６８】
まずステップ１５０１にて、車体速度計測装置１３０３から車体速度ベクトルＶｂを取得する。車体速度計測装置１３０３で計測できるのは、車体速度計測装置１３０３が取り付けられた位置における車体１０３の路面に対する車体座標系ｂの各軸方向の速度である。
【００６９】
次にステップ１５０２にて、ヨーレートセンサ１３０１により、車体座標系ｂのＺｂ軸周りの車体座標系ｂで表わされた角速度を計測する。
【００７０】
次にステップ１５０３にて、路車間姿勢推定部１３２１にて、求めた座標変換行列Ｃｒｂからステップ１５０１で取得した車体速度ベクトルＶｂと、ステップ１５０２で取得した車体座標系Ｚｂ軸の角速度であるジャイロセンサの出力値ωｚｂを路面座標系ｒで表わす。車体座標系ｂから路面座標系ｒへの座標変換行列Ｃｒｂにより、路面と平行な平面上の速度ベクトルＶｒおよび角速度ベクトルωｒは次式（１８）のように表わされる。
【数１８】
0006473684.tif 000019
【００７１】
よって、路面と平行な平面上の路面座標系ｒで表わされた各車輪軸位置での速度ベクトルＶｔｒは次式（１９）のように表される。
【数１９】
0006473684.tif 000020
【００７２】
以上の処理フローを経て路面上慣性量計算部１３２２の処理を終了する。
【００７３】
次に、車輪滑り角推定部１３２３において図１６及び図１７を参照した第二実施形態における車輪滑り角推定部の処理について説明する。図１６は、第二実施形態における実施例２における車輪滑り角推定手段の処理フローを示した図である。図１７は、第二実施形態における車輪における速度・滑り角の関係を示すモデル図である。
【００７４】
まず、ステップ１６０１にて、操舵角計測装置１０６にて計測した車輪の車体座標系ｂのＸｂ軸に対する傾きδbを取得する。
【００７５】
次に、ステップ１６０２にて、車輪の傾きδbを路面座標系ｒのＸｒ軸に対する傾きδr１７０１に変換する。ここでは、車体座標系ｂと路面座標系ｒの定義から、Ｘｂ軸とＸｒ軸の向きは合致しているため、δb=δr１７０１としてもよいことが明らかである。
【００７６】
次に、ステップ１６０３にて、路面上慣性量計算部１３２２にて計算した各車輪軸１０２における路面と平行な平面上の路面座標系ｒで表わされた各車輪軸位置での速度ベクトルＶｔｒを取得する。
【００７７】
次に、ステップ１６０４にて、ステップ１６０２にて取得した車輪１０１の路面座標系ｒに対する傾きδ１７０１と、ステップ１６０３で求めた路面と平行な平面上の各車輪軸位置での速度ベクトルＶｔｒから、車輪滑り角を計算する。車輪の横滑り角β１７０２は次式（２０）により求めることができる。
【数２０】
0006473684.tif 000021
【００７８】
第二実施形態によれば、第一実施形態と同様、車体重量が大きく変化するダンプトラックにおいて、車輪滑り角推定部１２３では車体に取り付けられた車体慣性計測装置の出力値を用いて車体と路面が平行になっておらずとも高精度に各車輪における滑り角を推定することができる。それに加えて、第一実施形態よりも少ない計測装置の構成で車輪滑り角を推定することができる。
【００７９】
上記実施形態は、本発明を限定するものではなく本発明の趣旨を逸脱しない範囲での様々な変更態様がありうる。例えば、本発明の構成として、第一実施形態の一部と第二実施形態の一部を入れ替えもしくは組み合わせたとしても同様の効果が得られる。
【符号の説明】
【００８０】
１００、１００ａ  ダンプトラック
１０１  車輪
１０２  車輪軸
１０３  車体
１０４  車体慣性計測装置
１０５  路面距離計測装置
１０６  操舵角計測装置
１０７  車輪速度計測装置
１２０、１２０ａ  車輪滑り角算出装置
１２１  路車間姿勢推定部
１２２  路面上慣性量計算部
１２３  車輪滑り角推定部
【図１】
0006473684.tif 000022
【図２】
0006473684.tif 000023
【図３】
0006473684.tif 000024
【図４】
0006473684.tif 000025
【図５】
0006473684.tif 000026
【図６】
0006473684.tif 000027
【図７】
0006473684.tif 000028
【図８】
0006473684.tif 000029
【図９】
0006473684.tif 000030
【図１０】
0006473684.tif 000031
【図１１】
0006473684.tif 000032
【図１２】
0006473684.tif 000033
【図１３】
0006473684.tif 000034
【図１４】
0006473684.tif 000035
【図１５】
0006473684.tif 000036
【図１６】
0006473684.tif 000037
【図１７】
0006473684.tif 000038