JP6909752B2 - 作業機械の後退支援装置 - Google Patents

Description

本発明は、鉱山用ダンプトラック等の作業機械の後退走行を支援する後退支援技術に関する。特に、放土時の後退支援技術に関する。

鉱山用ダンプトラック等の作業車両（運搬車両）の運転者が、停止位置を把握できるよう支援する技術がある。例えば、特許文献１には、「運搬車両の車体後端または後輪タイヤ後端を路面に投影した位置あるいは該投影した位置から後方へ所定距離離れた路面上の位置に、ベッセルの積荷を排土する排土作業時に基準となる排土作業時距離目安線あるいは排土作業時距離目安点を設定し、該排土作業時距離目安線あるいは排土作業時距離目安点を、表示部に表示される画像に、重ね合わせて表示する（要約抜粋）」運搬車両の後方視界表示システムが開示されている。

特許第５３８０７３５号公報

しかしながら、特許文献１に開示の運搬車両の後方視界表示システムでは、車両を放土位置まで後退させる際に目安とする排土作業時距離目安線を、オペレータが画像上で設定する。このため、運搬車両が後退中に、この目安線を必ずしも正確に設定できるとは限らない。特に、鉱山のように走行路面や車止めの形状が頻繁に変化する環境では、排土作業時距離目安線を正確に設定することが難しく、その結果、適切な位置に誘導することが難しい。

本発明は、上記の状況に鑑みてなされたものであり、放土時に適切な放土位置に停車できるように、運搬車両を誘導可能な技術を提供することを目的とする。

本発明は、作業機械の後退走行を支援する作業機械の後退支援装置であって、レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定する第一距離計測装置及び第二距離計測装置を含み、前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面からなる第一スキャン面及び前記第二距離計測装置のレーザ照射面からなる第二スキャン面を直交させ、前記作業機械の車体の前後軸及び左右軸を含む車体水平面に対して前記第一スキャン面が垂直、かつ前記車体水平面に対して前記第二スキャン面が俯角を有して前記作業機械に配置された計測装置と、前記作業機械が動作を始めた時からの位置及び姿勢を自己位置として算出する自己位置推定装置と、前記第一距離計測装置で測定した第一距離を基に前記車体水平面に対する前記作業機械が走行する走行路面の傾きである傾斜角を算出する路面推定装置と、前記第一距離と前記傾斜角とに基づいて、前記作業機械の進行方向に位置する車止めまでの距離である車止め距離と前記車止めの傾斜角度である車止め傾斜角度とを算出する車止め推定装置と、前記傾斜角を用いて前記第二距離計測装置で測定した第二距離からピッチ振動による影響を取り除く補正処理を行うピッチ振動補正装置と、前記補正処理後の前記第二距離と、前記自己位置推定装置が算出した前記自己位置とに基づいて、前記作業機械の進行方向の地形を前記車止めの形状である車止め形状として推定する車止め形状推定装置と、前記車止め形状を蓄積する車止め形状記憶装置と、前記車止め距離と前記車止め傾斜角度と前記車止め形状記憶装置に蓄積された前記車止め形状とに基づいて、前記作業機械が前記車止めに接触するまでの距離を算出する車止め誘導装置と、を備えることを特徴とする。

本発明によれば、放土時に適切な放土位置に停車できるように、運搬車両を誘導できる。上記した以外の課題、構成および効果は、以下の実施形態の説明により明らかにされる。

本発明の実施形態に係るダンプトラックの放土時の周辺環境を示す説明図。 本発明の実施形態に係るダンプトラックの左側面図。 本発明の実施形態に係るダンプトラックの後方斜視図。 本発明の実施形態に係る後退支援装置の機能構成を示すブロック図。 本発明の実施形態に係る接触距離算出装置の機能構成を示すブロック図。 本発明の実施形態の映像表示装置に表示される映像例を示す図。 本発明の実施形態の映像表示装置に表示される他の映像例を示す図。 （ａ）は、本発明の実施形態に係る車止め検出処理を説明するための模式図、（ｂ）は、本発明の実施形態に係る第一距離計測装置の計測結果をプロットした模式図（ダンプトラック左側面視）。 本発明の実施形態に係る車止め位置推定処理のフローチャート。 （ａ）は、本発明の実施形態に係る車止め幅方向形状検出処理のフローチャート、（ｂ）は、本発明の実施形態に係る車止め幅方向形状検出処理におけるマッピングの説明図（ダンプトラックの上面視）。 本発明の実施形態に係る接触距離決定処理のフローチャート。 本発明の実施形態に係るダンプトラックにピッチ振動が発生している場合の車止め検出処理を説明するための模式図（ダンプトラックの左側面視）。 （ａ）は、本発明の実施形態に係る路面が平面である場合を示す図、（ｂ）は、路面が第一距離計測装置の鉛直方向真下の地面を中心として、傾斜角θ分回転した状態を示す図。 （ａ）は、本発明の実施形態に係る２本実施形態以上の直線近似の場合における傾斜角θを示す図、（ｂ）は、本発明の実施形態に係る２本以上の曲線近似の場合における傾斜角θを示す図。 本発明の実施形態に係るダンプトラックが傾斜を持つ走行路面上を走行している場合の後方検出処理を説明するための模式図（ダンプトラックの左側面視）。 本発明の実施形態に係るダンプトラックが傾斜に変化がある走行路面を走行している場合の後方検出処理を説明するための模式図（ダンプトラックの左側面視）。

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

図１から図３を参照してダンプトラックの概略構成について説明する。図１は、本実施形態に係るダンプトラック１の放土時の周辺環境を示す説明図である。図２は、ダンプトラック１の左側面図である。図３は、ダンプトラック１の後方斜視図である。

図１に示すように、ダンプトラック１は崖下にある放土場１００３に放土する際、崖際に配置された車止め（Ｂｕｎｄ）１００２に向かって走行路面１００１上を後退走行する。ダンプトラック１は、車体フレーム２（図２参照）の前下部に右前輪３ｒ及び左前輪３ｌ（図２参照）を備え、後下部に右後輪４ｒ及び左後輪４ｌを備える。更に車体フレーム２の後部にはリアアスクル７が備えられる。一方、車体フレーム２の前上部には運転席５（図２参照）が備えられ、後ろ上部にはベッセル６が備えられる。

更にダンプトラック１は、ダンプトラック１の後退走行を支援する後退支援装置１００を備える。後退支援装置１００は、ダンプトラック１の後方を撮影する撮影装置（カメラ）１０２と、車止めまでの距離（接触距離）を算出する接触距離算出装置１０１と、表示映像生成装置１０３（図４参照）と、映像表示装置（モニタ）１０４（図４参照）と、放土場所進入判定装置１０５（図４参照）と、放土場所進入報知装置（ブザー、ランプ）１０６（図４参照）と、を含む。

映像表示装置１０４及び放土場所進入報知装置１０６は、運転席５に配置される。

接触距離算出装置１０１は、ダンプトラック１の後方を走査して車止め形状を蓄積するとともに、ダンプトラック１が車止めに接触するまでの距離（接触距離）を算出する。接触距離算出装置１０１は、複数のレーザスキャナを含む計測装置（スキャナ）２００と、ダンプトラック１の自己位置を検出する自己位置推定装置３００と、計測装置２００からのセンサ出力及び自己位置推定装置３００からの自己位置を基に車止めを検出する車止め検出装置１０と、検出した車止めにダンプトラック１を誘導する情報を算出する車止め誘導装置６０（図５参照）とを含む。

以下の説明において、後退支援装置１００が動作を始めた時のダンプトラック１の位置を原点とし、後退支援装置１００が動作を始めた瞬間の進行方向をｙ軸とし、ダンプトラック１の車体高さ軸をｚ軸とし、ｙ軸及びｚ軸の２軸に直交する軸をｘ軸とする直交座標系４００を用いる。

計測装置２００は、第一距離計測装置２０１及び第二距離計測装置２０２を含んで構成される。計測装置２００は、ダンプトラック１の車体後方、より詳しくはリアアスクル７より上かつベッセル６より下で、左後輪４ｌ及び右後輪４ｒの間に設置される。第一距離計測装置２０１及び第二距離計測装置２０２は、同じ高さに配置される必要はなく、また左右対称でなくてもよい。

第一距離計測装置２０１及び第二距離計測装置２０２は、例えばレーザ光を扇状に照射し、被検出体からの反射光を受光して被検出体までの距離と方向を検出するレーザスキャナ（レーザレーダ）である。第一距離計測装置２０１及び第二距離計測装置２０２は、第二距離計測装置２０２のレーザ照射面（以下「第二スキャン面」という）２２と第一距離計測装置２０１のレーザ照射面（以下「第一スキャン面」という）２１とが直交するよう配置される。なおここでいう直交は、厳密に第一スキャン面２１及び第二スキャン面２２の法線ベクトルが垂直となる場合に限定されず、第一距離計測装置２０１が検出した被検出体と同じ被検出体の幅方向の形状が検出できる向きに第二スキャン面２２が向くように、第一距離計測装置２０１及び第二距離計測装置２０２が配置されている場合も含む。

より詳しくは、本実施形態では、第一スキャン面２１が車体フレーム２の前後軸及び左右軸を含む車体水平面Ｈに対して少なくとも垂直下向きから車体後方に向かって車体水平面Ｈと平行な方向までを覆うように第一距離計測装置２０１がダンプトラック１に設置される。

第二距離計測装置２０２は、走行路面１００１が平坦である時に、第二スキャン面２２と走行路面１００１とが交差する位置Ｐ１と、左後輪４ｌ及び右後輪４ｒの最後端を走行路面１００１に投影した位置Ｐ２との距離Ｄ１が、ダンプトラック１の最大後退速度の制動距離以上となるよう、車体水平面Ｈに対して設置俯角α（図２）を持つようにダンプトラック１に設置される。このように設置することにより、最大後退速度で後退しても車止めに接触することなく制動が可能である。

また第二距離計測装置２０２は、第二距離計測装置２０２の検出距離と第二距離計測装置２０２の車体水平面Ｈに対する設置俯角αと、第二スキャン面２２に対するダンプトラック１の遮蔽によって決まる、走行路面１００１と第二スキャン面２２との交線の長さＤ２（図３参照）がダンプトラック１の車輪幅（外幅）よりも長くなるような位置に設置される。このように設置することにより、ダンプトラック１の車輪が通過する領域を前もってスキャンすることが可能である。

なお第一距離計測装置２０１及び第二距離計測装置２０２は１つずつ備える構成に限定されず、各計測装置が複数のレーザスキャナを用いて構成されてもよい。また第一距離計測装置２０１に対して第一スキャン面２１は一つに限定されず複数あってもよい。同様に第二距離計測装置２０２に対して第二スキャン面２２は一つに限定されず複数あってもよい。

撮影装置１０２は、例えばダンプトラック１の後方の映像を取得するカメラである。撮影装置１０２は、ダンプトラック１の車体後方、より詳しくは計測装置２００より上かつベッセル６より下で、左後輪４ｌ及び右後輪４ｒの間に、ダンプトラック１の後方の、第一スキャン面２１及び第二スキャン面２２を視野範囲に含んで設置される。

ダンプトラック１の車両座標系における視野範囲の各位置と画素位置とは予め対応づけて表示映像生成装置１０３がアクセス可能なＲＯＭ等のメモリに記憶される。ダンプトラック１の車両座標系は、ダンプトラック１の重心を原点とし、車体水平面上の車体前後軸方向をｙ軸、左右軸方向をｘ軸、車体水平面に直交する方向をｚ軸方向とする座標系である。

図４は後退支援装置１００の、図５は接触距離算出装置１０１の、其々機能構成を示すブロック図である。

自己位置推定装置３００は、ダンプトラック１が備える車輪速計測装置（車輪速センサ）３０１及び操舵角計測装置（ヨーレートセンサ、旋回角速度センサ）３０２の其々に接続され、車輪速計測装置３０１から車輪回転速度を取得し、操舵角計測装置３０２からダンプトラック１の旋回角速度の計測結果を取得する。そして自己位置推定装置３００は、例えばデットレコニング処理を実行することにより、ダンプトラック１の速度、角速度、及び既述の直交座標系４００（図１参照）の原点からの位置及び姿勢を算出する。

なお、車体速度を計測する目的で、車輪速計測装置３０１に代わりＩＭＵ（慣性計測装置）を、旋回角速度を推定する目的で操舵角計測装置３０２として、ヨーレートセンサに代わり操舵角センサを用いてもよい。また、直交座標系４００の原点からの位置、姿勢を計測する目的で、自己位置推定装置３００に代わり、ＧＰＳ（全地球測位システム）と地磁気センサを用いてもよい。

車止め検出装置１０は、車止め位置を推定する第一車止め検出装置４０及び車止めの幅方向の形状を推定する第二車止め検出装置５０を含む。また車止め誘導装置６０は、車止め誘導判断装置６１及び車体形状記憶装置６２を含む。

第一車止め検出装置４０は、路面推定装置４１及び車止め推定装置４２を含む。既述の第一距離計測装置２０１の出力は、路面推定装置４１及び車止め推定装置４２の其々の入力に接続される。路面推定装置４１の出力は、車止め推定装置４２及び後述するピッチ振動補正装置５１の入力に接続される。車止め推定装置４２の入力には、第一距離計測装置２０１の出力が更に接続される。

第一距離計測装置２０１が出力する第一距離情報Ｒ１ｉ（ｉ：第一距離計測装置２０１におけるレーザ照射角度φを示し、例えば−４５≦ｉ≦２２５、後述する第二距離情報Ｒ２ｉも同様）には、測定点及び測定点までの距離が含まれる。

路面推定装置４１は、第一距離情報Ｒ１ｉの内、路面と推定される測定点及び距離を示す距離情報を抽出し、路面線を演算する。そして車体フレーム２の前後軸及び左右軸を含む車体水平面Ｈに対する路面線の傾斜角θ（図１２参照）を演算し、車体水平面Ｈに対する路面の傾きを算出する。傾斜角θをピッチ角θともいう。

車止め推定装置４２は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉの内、車止めと推定されるものの距離情報を抽出し、路面及び車止めの交点と、路面に対する車止めの傾斜を算出する。

第二車止め検出装置５０は、ピッチ振動補正装置５１、車止め形状推定装置５２、及び車止め形状記憶装置５３を含む。ピッチ振動補正装置５１の入力は、第二距離計測装置２０２及び路面推定装置４１の其々の出力に接続される。ピッチ振動補正装置５１の出力は車止め形状推定装置５２の入力に接続される。車止め形状推定装置５２の入力は、更に自己位置推定装置３００の出力に接続される。車止め形状記憶装置５３は、車止め形状推定装置５２及び後述する車止め誘導判断装置６１に接続される。

ピッチ振動補正装置５１は、路面推定装置４１が推定した車体水平面Ｈに対する路面の傾斜角θに基づいて、第二距離計測装置２０２が計測した第二距離情報Ｒ２ｉから、ピッチ振動による影響を取り除くための補正処理を実行する。

車止め形状推定装置５２は、ピッチ振動補正装置５１が補正した第二距離情報Ｒ２ｉと自己位置推定装置３００が推定したダンプトラック１の位置及び姿勢に基づいて、一般的なＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）等のマッピング方法によって、ダンプトラック１の進行方向の地形を車止め１００２の車幅方向の形状として演算する。

車止め形状記憶装置５３は、車止め形状推定装置５２が演算した車止め１００２の車幅方向の形状を示す幅方向形状情報を蓄積する。車止め形状推定装置５２はより高度に幅方向形状情報を算出するために、車止め形状記憶装置５３が蓄積した車止め形状情報を参照してもよい。蓄積された車止め形状情報は表示映像生成装置１０３も参照する。

車止め誘導装置６０は、車止め誘導判断装置６１及び車体形状記憶装置６２を含む。車止め誘導判断装置６１の入力は、車止め推定装置４２及び自己位置推定装置３００の其々の出力に接続されると共に、車止め形状記憶装置５３及び車体形状記憶装置６２の其々にも接続される。車止め誘導判断装置６１は、車止め形状記憶装置５３及び車体形状記憶装置６２に記憶された情報を必要に応じて読み出す。

車止め誘導判断装置６１は、車止め推定装置４２が推定した車止めまでの距離及び車止めの傾斜と、車止め形状記憶装置５３が記憶する車止めの幅方向形状と、自己位置推定装置３００を介して取得する操舵角計測装置３０２が計測する旋回角速度と、車体形状記憶装置６２が記憶する車体形状（車幅方向の大きさを含む）に基づいて、誘導の判断に必要な接触距離を算出する。車止め誘導判断装置６１は、算出した接触距離を、放土場所進入判定装置１０５に出力する。

表示映像生成装置１０３は、撮影装置１０２が取得した映像に、車止め形状記憶装置５３に蓄積された車止め形状を重畳し、表示映像を生成する。表示映像生成装置１０３の入力は、撮影装置１０２および接触距離算出装置１０１の其々の出力に接続され、表示映像生成装置１０３の出力は、映像表示装置１０４の入力に接続される。表示映像生成装置１０３は、表示映像上で車止め形状を識別可能な態様で重畳する。直交座標系４００の値で表される車止め形状は、自己位置推定装置３００が算出するダンプトラック１の自己位置及び姿勢を用い、画素位置に変換される。

映像表示装置１０４は、表示映像生成装置１０３が生成した表示映像を表示する。映像表示装置１０４に表示される映像例４１０を図６に示す。ベッセル６、左後輪４ｌ、右後輪４ｒを含むダンプトラック１の後方の映像に、車止め形状１００２ｂが識別可能な態様で表示される。

なお、撮影装置１０２によりダンプトラック１の周囲の映像を取得可能な場合は、表示映像生成装置１０３は、撮影装置１０２が取得した映像から俯瞰画像を生成し、生成した俯瞰画像に車止め形状を重畳して表示画像を生成してもよい。この場合の表示画像例４２０を図Ｘに示す。中心にダンプトラック１を表すアイコン画像１ａが表示される。取得した映像を俯瞰画像に変換する際に用いる変換テーブル、アイコン画像１ａは、後退支援装置１００が備えるＲＯＭ等の記憶装置に予め保持される。

放土場所進入判定装置１０５は、車止め誘導判断装置６１が算出した接触距離と車止めからの距離で予め定められた放土場所とから、ダンプトラック１が放土場所に進入したか否かを判定し、進入したと判定した場合、放土場所進入報知装置１０６に報知信号を出力する。放土場所進入判定装置１０５の入力は、接触距離算出装置１０１の出力に接続される。放土場所進入判定装置１０５の出力は、放土場所進入報知装置１０６の入力に接続される。

放土場所進入報知装置１０６は、例えばブザー、ランプを含む。放土場所進入報知装置１０６は、放土場所進入判定装置１０５から報知信号を受信するとブザーから音を出力したり、ランプを点灯させたりする報知出力を行う。

車止め検出装置１０と車止め誘導装置６０と自己位置推定装置３００と表示映像生成装置１０３と放土場所進入判定装置１０５とは、中央演算装置、記憶装置、入出力回路、及び通信回路を含むマイコン装置に実装され、ハードウェアと各装置の機能を実現するソフトウェアとの組み合わせにより構成してもよいし、各装置を演算回路により構成してもよい。

次に図８（ａ）及び図８（ｂ）を参照して計測装置２００を用いてダンプトラック１の後方にある車止めを検出する処理について説明する。図８（ａ）は車止め検出装置１０による車止め検出処理を説明するための模式図である。図８（ｂ）は第一距離計測装置２０１の計測結果をプロットした模式図である。

図８（ａ）の点線１０１１は、第一車止め検出装置４０が計測した被検出体（路面、車止めの両方を含む）までの距離を表す。また図８（ａ）の黒点１０１２は、第一距離計測装置２０１が計測した測定点を示す。図８（ｂ）の黒点１０１３は、直交座標上での第一距離計測装置２０１が第一距離情報Ｒ１ｉを出力した位置を示す。

ダンプトラック１が車止め１００２にむかって後退を始めると、計測装置２００であるスキャナが回転しながら−４５°〜−２２５°にレーザを照射してダンプトラック１の後輪最後端から車止め１００２の傾斜面１００２ａまでに沿って並ぶ各点までの距離を算出し、第一距離情報Ｒ１ｉとして出力する。ダンプトラック１が車止め１００２に接近するにつれて、第一スキャン面２１と走行路面１００１上との交線は短くなる。

図８（ｂ）に示すように第一距離情報Ｒ１ｉを横軸が水平方向位置、縦軸が垂直方向高さを示す座標系にプロットすると、水平方向にほぼ平行な直線１０２１と、垂直方向高さが変化する直線１０２２とが得られる。この二直線の交点１０２３が第一距離情報Ｒ１ｉの変曲点であり、路面（ｔ_ｒｏａｄ）と車止め（ｔ_ｂｅｒｍ）とが接する点と推定できる。

また図８（ａ）ではダンプトラック１が車止め１００２に十分接近しているので、第二スキャン面２２は、走行路面１００１ではなく傾斜面１００２ａ上の幅方向に並ぶ各点までの距離を算出して第二距離情報Ｒ２ｉとして出力する。以後、ダンプトラック１が車止め１００２に更に接近するにつれて、第二スキャン面２２は傾斜面１００２ａの上端に向かって移動する。

次に図９を参照して第一車止め検出装置４０の動作について説明する。図９は車止め位置推定処理を示すフローチャートである。

路面推定装置４１は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉのうち、路面と推定される第一距離情報を抽出する（Ｓ１０１）。具体的には、前回の計測で路面推定装置４１が推定した路面線ｔ_{ｒｏａｄ＿ｏｌｄ}との距離が第一閾値Ｋ１以下である第一距離情報Ｒ１ｉを抽出する。

路面推定装置４１は、路面として抽出した第一距離情報Ｒ１ｉの個数ｎを、予め定めた閾値（路面閾値Ｋｎ）と比較する。抽出した距離情報の個数ｎが路面閾値Ｋｎ未満である場合（Ｓ１０２／ＮＯ）、路面線ｔ_ｒｏａｄを初期値ｉｎｉ＿ｔ_ｒｏａｄに設定し（Ｓ１０８）、後述するＳ１０４へ進む。

抽出した第一距離情報Ｒ１ｉの個数ｎが路面閾値Ｋｎ以上の場合（Ｓ１０２／ＹＥＳ）、路面推定装置４１は、抽出された距離情報から路面線ｔ_ｒｏａｄを算出する（Ｓ１０３）。ここでは、抽出された第一距離情報Ｒ１ｉを線形回帰によって近似することにより、路面線ｔ_ｒｏａｄを算出する。線形回帰処理として、例えば、１次の最小二乗法により直線近似してもよいし、二本以上の直線近似、また曲線近似を用いてもよい。後述する車止め線の算出における線形回帰も同様である。路面推定装置４１は、算出した路面線ｔ_ｒｏａｄを車止め推定装置４２に出力する。

車止め推定装置４２は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉから、車止めと推定される第一距離情報Ｒ１ｉを抽出する（Ｓ１０４）。ここでは、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉのうち、路面線ｔ_ｒｏａｄからの距離が第二閾値Ｋ２以上であり、かつ、前回の計測で車止め推定装置４２が推定した車止め線ｔ_{ｂｅｒｍ＿ｏｌｄ}との距離が第三閾値Ｋ３以下である第一距離情報Ｒ１ｉを抽出する。

車止め推定装置４２は、車止めとして抽出した第一距離情報Ｒ１ｉの数ｍと、予め定めた閾値（車止め閾値Ｋｍ）と比較する。そして、車止めとして抽出した第一距離情報Ｒ１ｉの数ｍが車止め閾値Ｋｍ未満である場合（Ｓ１０５／Ｎｏ）、処理を終了する。

一方、車止め推定装置４２は、抽出した第一距離情報Ｒ１ｉの数ｍが車止め閾値Ｋｍ以上である場合（Ｓ１０５／ＹＥＳ）、抽出された第一距離情報Ｒ１ｉを線形回帰によって近似した車止め線ｔ_ｂｅｒｍを算出する（Ｓ１０６）。

そして車止め推定装置４２は、路面線ｔ_ｒｏａｄ及び車止め線ｔ_ｂｅｒｍの交点から車止めまでの距離Ｌｂ１を求めると共に、ｔ_ｒｏａｄ及びｔ_ｂｅｒｍの傾きから車止め傾斜角度Ａｂを算出する（Ｓ１０７）。

次に図１０（ａ）及び図１０（ｂ）を参照して、第二車止め検出装置５０の動作について説明する。図１０（ａ）は、第二車止め検出装置５０による、車止め幅方向形状検出処理を示すフローチャートである。図１０（ｂ）は、車止め幅方向形状検出処理におけるマッピングを説明するための模式図である。

ピッチ振動補正装置５１は、第二距離計測装置２０２の設置俯角α（図２参照）と、路面推定装置４１が推定した路面に対する車体水平面Ｈの傾斜角θ（図１２参照）とを次式（１）〜（３）に適用し、第二距離情報Ｒ２ｉに対してピッチ振動を補正しつつ３次元変換を行う（Ｓ２０１）。
ｘｉ＝Ｒ２ｉ×ｃｏｓ（φ） ・・・（１）
ｙｉ＝Ｒ２ｉ×ｓｉｎ（φ）×ｃｏｓ（α＋θ） ・・・（２）
ｚｉ＝Ｒ２ｉ×ｓｉｎ（φ）×ｓｉｎ（α＋θ） ・・・（３）

次に車止め形状推定装置５２は、自己位置推定装置３００が推定した自己位置に基づいて、ピッチ振動補正装置５１が補正した第二距離情報Ｒ２ｉのマッピングを行う（Ｓ２０２）。そして車止め形状推定装置５２は、車止め形状記憶装置５３にマッピングした補正後の第二距離情報Ｒ２ｉからなる幅方向形状情報を書込み、マッピングした距離情報を蓄積する（Ｓ２０３）。

図１０（ｂ）において点群Ｒ２＿ｃは現在の距離情報を示し、点群Ｒ２＿ｐｒｅ１、点群Ｒ２＿ｐｒｅ２は過去の幅方向形状情報を示す。なお、点群Ｒ２＿ｐｒｅ１よりも、点群Ｒ２＿ｐｒｅ２の方が更に前の幅方向形状情報である。

次に図１１を参照して車止め誘導装置６０の動作について説明する。図１１は、車止め誘導装置６０による接触距離決定処理のフローチャートである。

車止め誘導判断装置６１は、車止め推定装置４２が算出した車止めまでの距離Ｌｂ１及び車止め傾斜角度Ａｂを、それぞれ予め定めた第四閾値Ｋ４および第五閾値Ｋ５と比較する。

距離Ｌｂ１が第四閾値Ｋ４以下であり、かつ、車止め傾斜角度Ａｂが第五閾値Ｋ５以上である場合（Ｓ３０１／Ｙｅｓ）、車止め誘導判断装置６１は、予測される進行経路ｐを推定する（Ｓ３０２）。進行経路ｐは、操舵角計測装置３０２が計測し、自己位置推定装置３００を介して取得した旋回角速度（ステアリング角）ｓに基づいて推定される。

次に車止め誘導判断装置６１は、進行経路ｐと、第二車止め検出装置５０が車止め形状記憶装置５３に蓄積した幅方向形状情報Ｇと、車体形状記憶装置６２が記憶する車体形状Ｄとにより、車止め１００２までの距離Ｌｂ２を算出する（Ｓ３０３）。

算出した距離Ｌｂ２が第六閾値Ｋ６以下の場合（Ｓ３０４／Ｙｅｓ）、距離Ｌｂ２を接触距離Ｌｃとする（Ｓ３０５）。

算出した距離Ｌｂが第六閾値Ｋ６より大きい場合（Ｓ３０４／Ｎｏ）、距離Ｌｂ１を接触距離Ｌｃとする（Ｓ３０６）。

距離Ｌｂ１が第四閾値Ｋ４より大きいか、または、車止め傾斜角度Ａｂが第五閾値Ｋ５未満である場合（Ｓ３０１／Ｎｏ）、第四閾値Ｋ４を接触距離Ｌｃとする。

次にピッチ振動が発生している場合の車止め検出装置１０による車止め検出について説明する。図１２は、ダンプトラック１にピッチ振動が発生している場合の車止め検出処理を説明するための模式図である。図１３（ａ）及び図１３（ｂ）は図１２の状態において第一距離計測装置２０１が行った計測結果を示す図である。

図１３（ａ）は路面が平面である場合を示す図である。第一距離計測装置２０１の鉛直方向真下の地面を中心として、路面が傾斜角θ＝０、即ち路面が水平な場合を示す。第一距離計測装置２０１からの生データは、距離Ｌとレーザ照射角度φの極座標とであり、これを直交座標（ｘ、ｙ）に座標変換する。そして点群のうち、路面と推定されるものを抽出し、線形解析（最小二乗法）による直線近似を行い路面直線ｙ＝ａｘ＋ｂを算出する。このとき、傾斜角θはａｔａｎ（ａ）となる。

路面が理想的な平面な場合、路面直線はｙ＝ｂであり、傾斜角θ＝０となる。

図１３（ｂ）は、路面が第一距離計測装置２０１の鉛直方向真下の地面を中心として、傾斜角θ分回転した状態を示す。また、第二距離計測装置２０２が計測する場所は、ピッチ振動がない場合を基準として第二距離計測装置２０２を中心としてθ回転した場所となる。

傾斜角θが発生しているとき、レーザ照射角度φは変わらないが距離Ｌは短く（または長く）なる。第一距離計測装置２０１からの生データの距離Ｌとレーザ照射角度φの極座標とを直交座標（ｘ、ｙ）に変換すると、図１３（ｂ）のように直交座標上にプロットされるｘｙ座標に変換される。図１３（ａ）と同様に直線近似を行い、路面直線ｙ＝ａｘ＋ｂを算出することで、傾斜角θを算出することができる。

図１４（ａ）は、２本以上の直線近似の場合における傾斜角θを示す図である。２本以上の直線が求まった場合には、最初の直線、即ち、ダンプトラック１に近い側の直線の傾きａが傾斜角θとなる。

図１４（ｂ）は、２本以上曲線近似の場合における傾斜角θを示す図である。２本以上の曲線が求まった場合には、水平方向位置が０付近、すなわちダンプトラック１に近い位置での曲線の微分値の傾きが傾斜角θとなる。

図１４（ａ）、図１４（ｂ）のいずれの場合でも、直線近似、曲線近似のいずれも、近似路面線の水平方向位置が０付近（ダンプトラック１付近）における、水平方向位置軸と路面との傾きをθとしていることが共通している。

路面推定装置４１は、前回推定した路面線ｔ_{ｒｏａｄ＿ｏｌｄ}を用いて路面を計測した第一距離情報Ｒ１ｉの抽出を行い路面を推定するので、ピッチ振動が発生している状況でもピッチ振動の影響を緩和して路面の傾きを推定することができる。また、車止め推定装置４２も前回推定した車止め線ｔ_{ｂｅｒｍ＿ｏｌｄ}を用いて車止めを計測した第一距離情報Ｒ１ｉの抽出を行い、車止めを推定することで、ピッチ振動が発生している状況でもピッチ振動の影響を緩和して車止めの位置及び傾斜を推定することができる。

ピッチ振動補正装置５１は、式（１）〜式（３）の通り路面推定装置４１が算出した車体水平面Ｈと路面の傾斜角θを用いて補正を行っているので、第二距離計測装置２０２のピッチ振動による俯角ずれを補正することができる。

次に図１５を参照して走行路面１００１Ａに傾きがある場合の車止め検出装置１０の動作について説明する。図１５はダンプトラック１が傾斜を持つ走行路面１００１Ａ上を走行している場合の後方検出処理を説明するための模式図である。

図１５において、ダンプトラック１にピッチ振動がない場合、第一距離計測装置２０１及び第二距離計測装置２０２が計測する距離情報は、図８（ｂ）で示した傾きのない走行路面１００１上で計測したものと同じになる。従って、傾斜を持つ走行路面１００１Ａを走行していても正確に車止め１００２までの距離と車止め角度、及び車止め形状を推定することができる。

ピッチ振動を傾斜センサ等によって計測する場合、傾斜センサは重力によって傾きを判定するため、図１５で示したような傾斜を持つ走行路面１００１Ａ上を走行する場合は、ピッチ振動がない場合でもピッチ振動があると判定されてしまい、正確なピッチ振動補正が行えない。

一方で、ピッチ振動補正装置５１はダンプトラック１の車体水平面Ｈと走行路面１００１Ａとの相対的な傾斜角θを利用するため、路面の傾きによらずピッチ振動を補正することができる。従って、傾斜が変化する走行路面上を走行している場合は、ピッチ振動補正装置５１はピッチ振動だけでなく、路面の傾きの変化を補正することができる。

次に図１６を参照して走行路面１００１Ｂの傾斜に変化がある場合の車止め検出装置１０の動作について説明する。図１６はダンプトラック１が傾斜に変化のある走行路面１００１Ｂ上を走行している場合の後方検出処理を説明するための模式図である。

図１６では、走行路面１００１Ｂの傾きが途中から変化しているが、第一距離計測装置２０１は、進行方向の距離を計測しているため、現在のダンプトラック１と走行路面１００１Ｂの相対的な傾きではなく、今後走行する走行路面とダンプトラック１との相対的な傾きを計測している。

従って、傾斜の変化する走行路面１００１Ｂ上を走行している場合は、ピッチ振動補正装置５１は、ピッチ振動だけでなく、路面の傾きの変化を補正することができる。

以上説明したように、第一距離計測装置２０１及び第二距離計測装置２０２のスキャン面を交差させてダンプトラック１に設置することで、ダンプトラック１にピッチ振動が発生しているときや走行路面に傾斜がある場合でもピッチ振動による影響を緩和して車止めまでの距離と車止めの形状、向き（車止めの傾斜角や幅方向の形状）を精度よく計測することができる。本実施形態によれば、精度よく計測した車止めまでの距離と車止めの形状、向きとを用いてダンプトラック１を車止めまでの誘導できる。これにより、放土時に適切な放土位置に停車できるように誘導することができる。

上記実施形態は本発明を限定するものではなく、本発明の趣旨を逸脱しない範囲での変更態様は本発明に含まれる。例えば作業機械はダンプトラックに限らず、ドーザー、ホイールローダ、油圧ショベルでもよく、これらの作業機械の後方にスキャナを取り付けて後方障害物の形状検出を行ってもよい。また、後方障害物に限らず、スキャナを作業機械の前、左、右に設置し、前方障害物、左障害物、右障害物の形状検出を行ってもよい。

また、表示映像生成装置１０３および放土場所進入判定装置１０５は、接触距離算出装置１０１の出力先の一例に過ぎず、他の装置、例えば、ダンプトラック１が自律走行をする所謂無人ダンプトラックである場合には、自律走行を制御する自律走行制御装置に出力して、ダンプトラック１から車止めまでの距離や車止めに対するダンプトラック１の車体の向き（車体水平面の向き、車体前後軸、また車体左右軸の向きでもよい）等、停車に必要な演算を自律走行制御装置が実行するように構成されてもよい。この場合、撮影装置１０２、表示映像生成装置１０３、映像表示装置１０４、放土場所進入判定装置１０５、放土場所進入報知装置１０６は備えなくてもよい。

更に、ダンプトラック１に接触判定装置を備え、この接触判定装置を接触距離算出装置１０１の出力先としてもちいてもよい。そして、接触判定装置が車止めとの接触判定を行い、車止めとの接触（干渉）を回避するための制御を行ってもよい。

１：ダンプトラック、１ａ：アイコン画像、２：車体フレーム、３ｌ：左前輪、３ｒ：右前輪、４ｌ：左後輪、４ｒ：右後輪、５：運転席、６：ベッセル、７：リアアスクル、
１００：後退支援装置、１０１：接触距離算出装置、１０２撮影装置、１０３：表示映像生成装置、１０４：映像表示装置、１０５：放土場所進入判定装置、１０６：放土場所進入報知装置、
１０：車止め検出装置、４０：第一車止め検出装置、４１：路面推定装置、４２：車止め推定装置、５０：第二車止め検出装置、５１：ピッチ振動補正装置、５２：車止め形状推定装置、５３：車止め形状記憶装置、６０：車止め誘導装置、６１：車止め誘導判断装置、６２：車体形状記憶装置、
２００：計測装置、２０１：第一距離計測装置、２０２：第二距離計測装置、２１：第一スキャン面、２２：第二スキャン面、
３００：自己位置推定装置、３０１：車輪速計測装置、３０２：操舵角計測装置、４００：直交座標系、
１００１：走行路面、１００１Ａ：走行路面、１００１Ｂ：走行路面、１００２：車止め、１００２ａ：傾斜面、１００２ｂ：車止め形状、１００３：放土場、１０１１：点線、１０１２：黒点、１０１３：黒点、１０２３：交点

Claims (7)

1. 作業機械の後退走行を支援する作業機械の後退支援装置であって、
   レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定する第一距離計測装置及び第二距離計測装置を含み、前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面からなる第一スキャン面及び前記第二距離計測装置のレーザ照射面からなる第二スキャン面を直交させ、前記作業機械の車体の前後軸及び左右軸を含む車体水平面に対して前記第一スキャン面が垂直、かつ前記車体水平面に対して前記第二スキャン面が俯角を有して前記作業機械に配置された計測装置と、
   前記作業機械が動作を始めた時からの位置及び姿勢を自己位置として算出する自己位置推定装置と、
   前記第一距離計測装置で測定した第一距離を基に前記車体水平面に対する前記作業機械が走行する走行路面の傾きである傾斜角を算出する路面推定装置と、
   前記第一距離と前記傾斜角とに基づいて、前記作業機械の進行方向に位置する車止めまでの距離である車止め距離と前記車止めの傾斜角度である車止め傾斜角度とを算出する車止め推定装置と、
   前記傾斜角を用いて前記第二距離計測装置で測定した第二距離からピッチ振動による影響を取り除く補正処理を行うピッチ振動補正装置と、
   前記補正処理後の前記第二距離と、前記自己位置推定装置が算出した前記自己位置とに基づいて、前記作業機械の進行方向の地形を前記車止めの形状である車止め形状として推定する車止め形状推定装置と、
   前記車止め形状を蓄積する車止め形状記憶装置と、
   前記車止め距離と前記車止め傾斜角度と前記車止め形状記憶装置に蓄積された前記車止め形状とに基づいて、前記作業機械が前記車止めに接触するまでの距離を算出する車止め誘導装置と、を備えること
   を特徴とする作業機械の後退支援装置。
2. 請求項１記載の作業機械の後退支援装置であって、
   前記ピッチ振動補正装置は、前記俯角をαとし、前記傾斜角をθとし、極座標で表される前記第二距離（Ｒ２ｉ、φ）から下記の式に従って直交座標（ｘｉ、ｙｉ、ｚｉ）を算出し、前記補正処理後の前記第二距離とすること
   ｘｉ＝Ｒ２ｉ×ｃｏｓ（φ）
   ｙｉ＝Ｒ２ｉ×ｓｉｎ（φ）×ｃｏｓ（α＋θ）
   ｚｉ＝Ｒ２ｉ×ｓｉｎ（φ）×ｓｉｎ（α＋θ）
   を特徴とする作業機械の後退支援装置。
3. 請求項１に記載の作業機械の後退支援装置であって、
   前記第一距離計測装置及び前記第二距離計測装置は、それぞれ、照射面が１つまたは複数のレーザレーダであることを特徴とする作業機械の後退支援装置。
4. 請求項１に記載の作業機械の後退支援装置であって、
   前記第二距離計測装置の俯角は、前記作業機械が走行する走行路面と前記第二スキャン面とが交わる交線と、前記作業機械の後輪の最後端を前記走行路面に投影した点との距離が、前記作業機械の最大後退速度の制動距離以上になるよう設定されることを特徴とする作業機械の後退支援装置。
5. 請求項１に記載の作業機械の後退支援装置であって、
   前記第二距離計測装置は、前記第二スキャン面と前記作業機械が走行する走行路面との交線の長さが前記作業機械の車輪幅以上となるよう設置されることを特徴とする作業機械の後退支援装置。
6. 請求項１に記載の作業機械の後退支援装置であって、
   前記作業機械の後方を撮影とする撮影装置と、
   前記撮影装置が取得した映像から表示映像を生成する表示映像生成装置と、
   生成された前記表示映像を表示する映像表示装置と、をさらに備え、
   前記表示映像生成装置は、前記撮影装置が取得した映像に検出された前記車止めの形状を識別可能な態様で重畳して前記表示映像を生成することを特徴とする作業機械の後退支援装置。
7. 請求項１に記載の作業機械の後退支援装置であって、
   前記作業機械が放土場所に進入した場合、報知信号を出力する放土場所進入判定装置と、
   前記報知信号を受信すると報知出力を行う放土場所進入報知装置と、をさらに備えることを特徴とする作業機械の後退支援装置。

Images