JP2014526627A - バケットを備えた作業機械のアタック姿勢を選択する方法 - Google Patents

Abstract

本発明はバケットを備えた作業機械の、パイル材料を積み込むときのアタック姿勢を選択する方法であって、該方法は、三次元パイルデータを取得する段階と、１組のアタック姿勢を生成する段階と、各アタック姿勢に関して前記パイル内のバケット軌道を生成する段階とを含み、前記１組のアタック姿勢の各アタック姿勢に関して、バケット幅と前記バケット軌道の垂直投影とによって区切られた、前記パイル表面の領域に関して、前記パイル表面の凸性の大きさを計算する段階と、前記凸性の大きさに基づいてアタック姿勢を選択する段階とを含む。
【選択図】図８ｂ

Description

本発明は、ホイールローダなどのバケットを備えた作業機械のアタック姿勢を選択する方法に関する。詳細には、本発明は、バケットの可能充満率に対して適切な負荷状態を推定するために１組の可能なアタック姿勢を評価する方法に関する。本発明は、また、アタック姿勢及び対応するバケット軌道を選択する前記方法を使用してバケットに積載する方法に関する。

異種混合パイル材料の自動処理は、多くの建築及び採鉱用途で中心となることである。そのような用途において作業する自律ホイールローダの典型的な作業サイクルは、積載、運搬及び放下の３つの繰り返し作業から成る。積載点と放下点の間の運搬は、ＧＰＳウェイポイント追随又は移動ロボットナビゲーションに関する豊富な文献からの幾つかの柔軟な手法によって、幾つかの方法で処理することができる。放下は、比較的簡単であり、基本的には、予めプログラムされた動きで行うことができる。効率的な積載は、放下手順より難しい問題であり、今日、完全自律車両のための実際的な解決方法は存在しない。経済的及び環境上の理由のために、バケットが、各積載サイクルで最高に充満され、機械への力学的応力が最小にさることが重要である。自動ホイールローダが、小石パイルに近づくときには、可能なアタック姿勢、即ちパイルに効率的に近づける位置と向きを評価しなければならない。また、オペレータ支援機能として使用されるとき、同じ機能は、可能なアタック姿勢を評価し、選択品質に関してユーザに知らせるために重要である。

掘削戦略を決定するための先行技術の方法の一例は、特許文献１に示されている。この方法によれば、バケット内の容積を全バケット容積で割った割当て量を求めることによって、くぼみ量が決定される。

この大きさは、バケットの前方角部がパイルに当たったときのバケット内の容積に基づいて決定される。したがって、このくぼみの大きさの決定には、パイルのごく限られた情報しか考慮されない。バケットの充満が、パイル内を通るバケットの全軌道に沿ったパイルの形状によって決定されるので、この大きさを使用して、実際のパイル内の悪い軌道と良い軌道を分離できないことは明らかである。更に、この手法は、特に少しだけ凸状の場所で、パイルの点抽出法にかなり左右されやすい。前方角部が縁に触れたときバケットモデルの小さい部分だけがパイルに入るので、バケットがパイルに垂直に入ったときでも高い値が得られることが起こり得る。

アタック姿勢の品質を評価する方法の別の例は、非特許文献１に提供されている。

この論文では、パイル内のバケットの軌道でのパイルの形状が考慮される。この方法は、バケットの中心のまわりのトルクと関連した値が計算され、アタック姿勢の品質を決定するパラメータとして使用される手法を使用する。求めたトルクの大きさから荷重分布が得られないので、提案された方法によって重要な情報が失われることは明らかである。

本発明の目的は、バケットの高い充満率を実現するアタック姿勢を選択するための改善された方法を提供することである。

米国特許第６，１６７，３３６号明細書

"Planning of scooping position and approach path for loading operation by wheel loader", Shigeru Sarata, et. al., 22nd International Symposium on Automation and Robotics in Construction ISARC 2005 - September 11-14, 2005, Ferrara (Italy)

目的は、請求項１に定義されたような方法を提供することにより達成される。方法は、以下の段階を含む。

三次元パイルデータを取得する段階、１組のアタック姿勢を生成する段階、及び各アタック姿勢ごとにパイル内のバケット軌道を生成する段階。パイルデータの取得は、三次元距離センサを使用して行うことができる。

１組の生成されたアタック姿勢と対応するバケット軌道が、可能なアタックである。各積載作業で、１組のアタック姿勢とバケット軌道の中から１つが選択される。積載作業が行なわれた後で、新しいパイルデータが取得され、実際のバケット軌道及び対応するアタック姿勢の選択を実行できる新しい組のアタック姿勢とバケット軌道が決定される。

この方法は、更に、前記１組のアタック姿勢の各アタック姿勢ごとに、バケット幅とバケット軌道の垂直投影とによって区切られたパイルの領域のパイル表面の凸性の大きさを計算し、前記凸性の大きさに基づいてアタック姿勢を選択する段階とを含む。

アタック姿勢は、バケットのアタック角度とアタックの位置から成る。アタック角度は、負の表面法線を基準にしたバケットの角度であり、負の表面法線は、パイルの内方に向く法線であり、したがって、アタックの位置におけるパイル内へのバケットのアタックと同じ方向である。アタックの位置は、バケットの横方向中央がパイルと接触するパイルの点と見なされる。

バケット幅と軌道の垂直投影とによって区切られたパイルの表面の領域に関してパイルの領域の凸性を検討することによって、バケットの充満率に大きい影響を及ぼす大きさが推定される。凸形状を有するバケット軌道を横切るパイルの断面が、凸形状を有するバケット積載の提供を支援することは明らかである。これは、バケット軌道に沿ったパイルの形状を考慮しておらず、したがって関連領域内のパイルの凸性を決定するために使用できない既知の先行技術とは異なる。

本発明の一実施形態では、前記バケットの区分の収集体積（sweep volume）を決定することによって凸性の大きさが決定される。収集体積は、バケットの区分の幅とバケット軌道の長さ範囲（即ち、バケット軌道の垂直投影）とによって画定された、パイルの表面における収集領域内のパイルの体積である。バケット軌道は、実行するバケット軌道が選択された場合、バケットがパイル内で拡がるようにされた軌道である。方法は、更に、前記バケットの区分の前記収集体積に基づいて前記凸性の大きさを計算する段階を含む。

具体的には、区分は、中央区分、右翼区分、及び左翼区分を含んでもよい。凸性の大きさは、左翼区分、右翼区分、及び中央区分の収集体積の比較に基づいて計算されてもよい。

一実施形態では、バケットの幅は、等しい幅の３つの区分に分割される。ここで、凸性の大きさは、Ｃ_c＝（Ｖ_c／ｍａｘ（Ｖ_r，Ｖ_l））−１として計算されてもよい。

必要に応じて、側面荷重の大きさは、更に、各アタック姿勢に関して決定されてもよい。アタック姿勢は、凸性の大きさと側面荷重の大きさに応じて選択されてもよい。側面荷重の大きさは、左翼区分と右翼区分の体積の比較に基づいてもよい。側面荷重の大きさは、Ｃ_s＝（ａｂｓ（Ｖ_r−Ｖ_l））／（Ｖ_r＋Ｖ_l）として計算されてもよい。

パイルデータは、センサから三次元点クラウドを取得し、前記三次元点クラウドをグランドデータと前記三次元パイルデータに分離することによって取得されてもよい。

必要に応じて、グランド平面は、グランドデータにフィッティングされてもよい。

必要に応じて、可能なアタック姿勢が、パイルの近くと好ましくはその境界の走査点の中から選択されてもよく、アタック姿勢の向きは、境界法線から２０°未満のずれの範囲内で選択される。

必要に応じて、グランド平面からパイル表面までの収集体積が計算される。必要に応じて、軌道からパイル表面までの収集体積が計算される。

代替実施形態では、表面は、バケットの幅と前記バケット軌道によって画定された領域内の三次元パイルデータにフィッティングされてもよく、凸性の大きさは、前記表面から計算されてもよい。

そのような代替実施形態は、
−二次多項式：ｚ_r＝ａｘ_r ²＋ｂｘ_rｙ_r＋ｃｙ_r ²＋ｄｘ_r＋ｅｙ_r
を、バケットの幅と前記バケット軌道とによって画定された領域内の前記三次元パイルデータにフィッティングする段階と、
−前記因子ａから前記凸性の大きさを決定する段階を含むことがある。

方法は、必要に応じて、
−平面を、バケットの幅と前記バケット軌道とによって画定された領域内の三次元パイルデータにフィッティングする段階と、
−前記平面の表面法線のまわりに回転された地面フレームから回転主フレームを構成する段階と、
−三次元パイルデータを回転主フレームにマッピングする段階とを含んでもよい。

必要に応じて、側面荷重の大きさは、前記因子ｄから決定されてもよい。

本発明は、また、バケットと、三次元パイルデータを検出するためのセンサシステムと、パイル内のバケット軌道に沿ったバケットの進行を可能にするための自動操縦システムとを有する建設機械の使用によって、バケットに積載するための方法に関し、前記方法は、前述のようなアタック姿勢を選択するための方法を使用することによって、アタック姿勢及び対応するバケット軌道を選択し、建設機械を制御し、前記選択されたアタック姿勢におけるバケットを位置決めし、前記バケットを前記選択されたバケット軌道に沿って移動させて前記バケットの充満を最適化する段階を含む。この方法は、自律ホイールローダによる異種混合のパイル材料の自動処理に適する。

本発明は、添付図面を参照して、以下に更に詳しく説明される。

本発明が使用されることがあるホイールローダを示す図である。 作業機械の動きを自動制御するシステムを示す図である。 本発明による方法のフローチャートである。 作動中のレーザ距離計によって生成された、グラベルパイルを示す例示的な点クラウドを示す図である。 局所表面形状にしたがって分類された、図１と同じ点クラウドを示す図である。 図１及び図２と同じ点クラウドを示し、最も近いパイル表面クラスタが三角形分割されている（三角形面の縁がパイル点を繋げていることに注意されたい）。 二次フィッティングを使用する凸性評価の例を示す図である。表面点は、十字マークで示されている。この場合、ａ＝１．０であり、したがって、きわめて凹状の面である。 パイル内を通るバケット軌道を示し、バケット幅とバケット軌道の垂直投影とによって区切られたパイル表面の領域を示す図である。 第１の実施形態の前記バケットの区分の収集体積を示す図である。 第２の実施形態の前記バケットの区分の収集体積を示す図である。 側面荷重Ｃ_sの大きさを水平軸に、凹性Ｃ_cの大きさを垂直軸に有する平面図である。

図１は、ホイールローダの形態の作業機械１を示す。ホイールローダ１は、機械の操縦並びにバケットの位置を制御する油圧システムを備えた作業機械の例と見なされる。

ホイールローダは、前方機械部分３８と後方機械部分３９を有する。これらの機械部分はそれぞれ、フレームと、車軸に配置されたホイール４２とを有する。後方機械部分３９は、ホイールローダ３１のオペレータのための運転室４０を有する。機械部分３８，３９は、機械部分３８，３９の間に配置され取り付けられた２本の油圧シリンダ（ステアリングシリンダ）３７ａ，３７ｂによって、垂直軸のまわりに互いに対して旋回できるように互いに接続される。したがって、油圧シリンダ３７ａ，３７ｂは、油圧シリンダによってホイールローダを回転又は操縦するために、作業機械３１の縦方向に延在する中心線のそれぞれの側に１本ずつ配置される。換言すると、ホイールローダ１３は、いわゆるフレーム操縦式作業機械である。

ホイールローダは、物体や材料などの様々な積荷を処理するための積載アームアセンブリ３２を有する。積載アームアセンブリ３２は、リフトアームユニット３４と、リフトアームユニット３４に取り付けられたバケットの形状の道具３３とを有する。示した例では、バケット３３は、材料４１で満たされる。積載アームユニット３４の第１端は、バケット３３の持ち上げ運動を行うために、前方機械部分３８に枢動可能に接続される。バケット３３は、バケットの傾斜運動を行うために、リフトアームユニット３４の第２端に枢動可能に接続される。リフトアームユニット３４は、２本の油圧シリンダ（リフトシリンダ）３５ａ，３５ｂによって、車両の前方機械部分３８に対して上昇下降することができる。油圧シリンダはそれぞれ、その第１端が前方機械部分３８に結合され、第２端がリフトアームユニット３４に接続される。バケット３３は、更に他の油圧シリンダ（傾斜シリンダ）３６によってリフトアームユニット３４に対して傾けることができ、油圧シリンダ３６は、その第１端が前方機械部分３８に結合され、その第２端がリンクアームシステムを介してバケット３３に結合される。

ホイールローダは、内燃機関などのエンジン、トルクコンバータ、ギアボックスを含む動力伝達経路（図示せず）も有する。エンジンは、動力伝達経路に動力を供給して、ホイール４２を駆動し、またエンジンとトルクコンバータの間の動力伝達経路に配置されることがある動力取出装置（ＰＴＯ）によって油圧系統内の油圧機械（ポンプ）を駆動するように構成され得る。

ホイールローダは、更に、三次元パイルデータを検出するためのセンサシステムと、パイル内のバケット軌道に沿ったバケットの進行を可能にする自動操縦システムを有する。

図２では、作業機械の動きを自動制御するシステム４５が示される。システムは、電子制御装置４７に接続されたセンサシステム４３を含む。電子制御装置は、センサ４３から三次元パイルデータを取得する機能ブロック４９を含む。更に、第２の制御ブロック５１で、バケット軌道とアタック姿勢が生成される。アタック姿勢、アタック軌道及び三次元パイルデータから、第３の機能ブロック５３で、実際のアタック姿勢とアタック軌道が選択される。第３の機能ブロック５３は、機械座標系決定装置５５と通信して、作業機械の位置、特にパイルに対するバケットの位置を決定してもよい。機械座標系決定装置５５は、ＧＰＳユニット５７を有してもよい。更に、機械座標系決定装置５７は、作業機械及びパイルの相対位置を決定する情報をセンサシステム４３から取り出し、作業機械に対するバケットの位置を決定する情報を作動シリンダ３５ａ、３５ｂ及び３６から取り出すことができる。第４の機能ブロックで、バケットの動きが、ホイールローダのホイール４２並びに作業機械に対するバケットの位置を決定する作動シリンダ３５ａ、３５ｂ及び３６の制御によって、特定のバケット軌道５９に従うように制御される。

図３では、本発明による方法のフローチャートが示される。本発明によるアタック姿勢を選択する方法には、以下の段階が含まれる。

第１の方法段階Ｓ１０で、三次元パイルデータが、三次元スキャナによって取得される。

第２の方法段階Ｓ２０で、１組のアタック姿勢が生成される。

第３の方法段階Ｓ３０で、各アタック姿勢の前記パイル内のバケット軌道が生成される。

第４の方法段階Ｓ４０で、バケット幅とバケット軌道の垂直投影とによって区切られたパイル表面の領域のパイル表面の凸量が、前記アタック姿勢セットの各アタック姿勢に関して計算される。第５の方法段階Ｓ５０で、前記凸量に基づいてアタック姿勢が選択される。一実施形態では、第４の方法段階Ｓ４０は、前記バケットの区分の収集体積を決定する段階を含み、収集体積は、区分の幅と軌道の長さによって定義された収集領域内のパイルの量であり、
前記バケットの区分の前記収集体積に基づいて前記凸量を計算する段階を含む。

別の実施形態では、第４の方法段階Ｓ４０は、バケットの幅と前記バケット軌道とによって画定された領域内の三次元パイルデータに平面をフィッティングする段階と、前記平面の表面法線のまわりに回転されたグランドフレームから回転主フレームを構成する段階と、三次元パイルデータを回転主フレームにマッピングする段階と、回転基本二次多項式
ｚ_r＝ａｘ_r ²＋ｂｘ_rｙ_r＋ｃｙ_r ²＋ｄｘ_r＋ｅｙ_r
を、バケットの幅と前記バケット軌道とによって画定された領域内の前記三次元パイルデータにフィッティングする段階と、
前記因子ａから前記凸量を決定する段階とを含む。

必要に応じて、方法は、スキャナ４３からの点クラウドデータをパイルデータとグランドデータに分離する段階を含んでもよい。

更に任意選択の方法段階は、パイル点をクラスタ化し三角形分割する段階を含む。

以下に、例示として、詳細な方法を述べる。

次に、提示された方法を実行するとき実行されることがある操作のフローチャートを説明する。プロセスは、以下の段階を含む。
・三次元データの取得
・三次元データのグランド点とパイル点への分類
・パイル点のクラスタ化
・パイル点の三角形分割
・掘削用のアタック点の選択
・アタック点の基準面、グランド面又は軌道面の計算
・体積の計算
・凸性と側面荷重によるアタック点の評価

方法段階Ｓ１０
三次元データの取得
最初に、システムは、三次元距離センサを使用して周囲の形状の三次元データを取得する。例示的なセンサ形式は、レーザ距離計（例えば、アクチュエータに取り付けられて三次元カバレッジを取得するＶｅｌｏｄｙｎｅ ＨＤＬ−６４Ｅ又はＳＩＣＫ ＬＭＳ２９１）、ステレオビジョン（例えば、Ｐｏｉｎｔ Ｇｒｅｙ Ｂｕｍｂｌｅｂｅｅ ＸＢ３）、及び構造光センサ（例えば、Ｍｉｃｒｏｓｏｆｔ Ｋｉｎｅｃｔ）である。三次元距離センサの出力は、クラウド点、即ち、周囲表面からの１組の測定点であり、それぞれ三次元（ｘ，ｙ，ｚ）位置を有する。図４の例示的な点クラウドを参照されたい。

任意選択の方法段階
グランドとクラッタに対するパイル点の分類
三次元点クラウドの場合、システムは、現在ビューのどの部分がパイルに属するかを確実に検出する方法を必要とする。点クラウドデータのパイルに対応する部分をセグメント化する１つの方法を以下に述べる。

１．各点ｐに関して、局所近傍半径ｒ内の全ての包囲点ｑ_ｉを見つける。ここで、ｒは、適切には０．５ｍに選択されることがある。高速の最近傍検索を可能にするために、点は、ｋｄツリーなどの高効率のデータ構造で記憶されなければならない。

２．ｒ内のｎ個の点の位置の平均ベクトルｍと分散行列Ｃを計算する。

３．（例えば、ヤコビの方法を使用して）Ｃの固有値（λ₁，λ₂，λ₃）及び対応する固有ベクトル（ｅ₁，ｅ₂，ｅ₃）を計算し、固有値と固有ベクトルがソートされλ₁≦λ₂≦λ₃になるようにする。

４．ソートされた固有値λ₁≦λ₂≦λ₃は、点ｐのまわりの局所表面形状を表す。十分な平面（λ₁＜＜λ₂＜＜λ₃、即ち、線状でも球状でもない分布）と右傾斜（対応する固有ベクトルｅ₁と水平面との角度が、２つの角度しきい値α₁及びα₂の範囲内になるような）を有する点が、「パイル」として分類される。α₁より小さい角度を有する平面点が、「地面」として分類される。

５．次に、同じクラスを有する近傍点をクラスタ化しセグメント化する（半径方向に有界の最近傍クラスタ化を使用する［１］）。

６．特定のしきい値サイズを超えるバウンディングボックスを有するパイルクラスタを、次のパイル分析に使用する。

分類後の例示的な点クラウドを図５に示す。

任意選択の方法段階
三角形分割
パイルの体積又はパイルの区分を推定するために、「パイル」として分類されたクラスタ内の点を、二次元マニホールド面を構成する三角形に分割する。パイルに対応する三角形分割面を図６に示す。

レーザ距離計からの三次元データの場合、三次元点をグリッドパターンで配列し三角形分割を容易にする。理想的状態（欠測データがないとき）で、これは、隣り合った点を結ぶのと同じくらい単純である。

より希薄で非構造的な三次元データの場合（例えば、ステレオビジョンシステムによって作成されたもの、又は異なる視点による複数の三次元走査が共通座標フレームに登録されたとき、又はパイル材料の不十分な反射率によって生じた欠測点があるとき）には、より細密な三角形分割アルゴリズムを適用する必要がある。例えば、Ｄｅｌａｕｎｅｙ三角形分割、マッチングキューブ、又はボールピボッティング。

方法段階Ｓ２０
アタック姿勢の選択
点のクラスタがパイル表面からサンプリングされた場合、次の段階は、パイルをアタックする姿勢（即ち、位置と向き）を選択することである。

オペレータは、典型的には、バケットができるだけ低く地面に接触する状態でパイルに侵入する。可能なアタック姿勢を選択する１つの方法は、グランド平面に近い走査点の位置を選択し、その点における表面法線に沿って（又は、それに近い）向きを選択することである。表面法線は、パイル分類段階で全ての点に関して行われる方法と類似の方法で計算することができる（前述の番号付きリストの項目１乃至４を参照）。局所近傍内の点の位置の共分散が計算され、法線は、最小固有値を有する分散行列の固有ベクトルによって提供される。

任意選択の方法段階
グランド平面の評価
パイルの体積を計算するには、パイルの下の地面を評価しなければならない。これは、平面を、「グランド」として分類された点にフィッティングすることによって行うことができる。異常値点が平面推定を歪ませるのを防ぐため、グランド平面は、ＭＬＥＳＡＣ［２］などのロバストフィッティング法を使用して評価されなければならない。

仮想正常値（hypothetical inlier）として「グランド」点のランダムサブセットが選択され、平面モデルが、これらの点だけにフィッティングされる。次に、フィットモデルに対して全ての他の点が試験され、モデルと一致する点が、仮想正常値として含まれる。これらの２つの段階を繰り返し、潜在的異常値の影響のない改良平面モデルが得られる。

次に、グランド平面が、局所座標フレーム内の平面ｚ＝０と一致するように、走査が、回転され平行移動される。

方法段階Ｓ３０
軌道の生成
バケットを満たす軌道は、多くの前提条件に依存し、ここでは、最も重要なもののうちの幾つかについて述べる。掘削が、バケットを完全に満たすことであり、また特定の値のバケットの終了高さが必要とされないと想定し、また、材料をすくい取りたいときにできるだけ多くの平面状のパイル表面内を移動することが重要でないと仮定する。この掘削に関して、軌道は、通常、以下のような４段階で計画される。

１．バケットが地面に平らな状態でパイルに突入する。様々な機械、パイルの材料、及びバケットの様々な形状とサイズによって異なる典型的な距離だけ進行し続ける。３．２ｍ²のバケットを備えた２０トンホイールローダの典型的な長さは、０．５ｍである。これは、持ち上げ始める前にバケットをパイル内に十分に入れることである。

２．次の段階は、前輪に十分な圧力をかけることである。したがって、軌道の典型的な次の段階は、水平姿勢のままバケットを持ち上げ、即ちリフトアクチュエータだけを使用することである。この部分の長さは、様々な連結機構システムと油圧を有する様々な機械で異なる。失速トルクは、どれだけ持ち上げることができるかの上限である。高い持ち上げトルクを有する連結システムは、持ち上げトルクが少ない設計のシステムより長く続く。また、パイルと相関関係にあるバケット先端の絶対軌道は、パイルの勾配に依存する。急な勾配は、より急な軌道を必要とし、より平らなパイルは、より平らな軌道を必要とする。典型的には、パイルを繰り返し掘削するときに一般的なように、約３０度より高い平均値勾配を有するパイルには固定勾配の軌道を使用する。前述の内容と同じように、軌道のこの部分の通常長さはパイル内に約１ｍ、バケット先端が高さ０．５ｍである。

３．その後、通常軌道は、バケットを勾配面と平行に上昇させることであり、即ち、急勾配のパイルに掘削するとき、軌道は同様に急勾配である。平坦でないパイルを掘削するとき、これは、平均値軌道によって達成される。

４．軌道の最終段階は、バケットが、十分な体積のパイルを移動させることである。この場合、オペレータは、パイルの外でバケットを傾け、掘削が完了する。バケットの主な移動は、垂直方向であり、水平移動は、機械と材料に依存する。通常、垂直の平行移動は、水平の平行移動よりも大きい。

当然ながら、バケットが収容する全体積は、多少の超過を有するバケットの容積より大きくなければならず、ここでは、バケット容積とほぼ同じ体積を提供する軌道を仮定する。

軌道を評価する１つの明らかな方法は、前述のデフォルト値を仮定し、選択された掘削点におけるパイル勾配の評価に基づいて、出口座標と共にパイル内の軌道高さを計算することである。

次に、この軌道は、選択された掘削点の評価を行うときの体積評価の基準面として働く。

方法段階Ｓ４０
体積の評価
グランド平面又は基準平面を、例えばパイル内のバケット移動の軌道として提供する場合、端点ｉ＝［ｉ₁，ｉ₂，ｉ₃］，ｊ＝［ｊ₁，ｊ₂，ｊ₃］，ｋ＝［ｋ₁，ｋ₂，ｋ₃］を有する三角面ｆより下の体積Ｖ_fは、
Ｖ_ｆ＝（ｉ₃＋ｊ₃＋ｋ₃）／３＊｜（ｉ₁（ｊ₂−ｋ₂）＋ｊ₁（ｋ₂−ｉ₂）＋ｋ₁（ｉ₂−ｊ₂））／２｜
によって示される。
走査は、グランド平面がｚ＝０になるように位置合わせされると仮定する。

パイルの全体積は、三角形分割点が存在する場合、これらの全ての三角面にわたって合算することによって推定することができる。

また、アタック点の場合、同じように、基準面より上のバケット幅又はバケット幅の一部によって区切られたパイルの区分の体積が計算されてもよい。

凸性と側面荷重の推定
アタック姿勢の場合の、局所パイル凸性と側面荷重を推定する２つの方法について述べる。

方法段階Ｓ４０
三部品バケット
三角形分割されたパイル表面と生成された軌道がアタック姿勢で始まる場合、積載作業による影響を受けるパイル区分の凸性を、バケットの３つの区分によって追跡される部分容積を比較することにより推定することができる。

Ｖ_cをバケットの中央の３分の１の容積、Ｖ_lとＶ_rを左と右の３分の一の容積とする。次に、次のように凸性をモデル化することができる。

凸領域はＣ_c＞０を有し、凹領域はＣ_c＜０を有する。

側面荷重は、次のように測定される。

Ｃ_s＝｜Ｖ_l−Ｖ_r｜／（Ｖ_l＋Ｖ_r）

荷重が斜めになる軌道は、大きいＣ_s値を有し、適切な軌道のＣ_s値は、ゼロに近い。

代替の方法段階Ｓ４０
二次フィッティング
三角形分割面の下の体積から凸性と側面荷重を推定する代わりに、二次曲面を可能なアタック姿勢の局所近傍内の点にフィッティングすることによって、それらの量を推定することができる。局所近傍は、バケット軌道が辿る容積の面に落ちる全ての点と見なされる。

平滑面上の点ｐに関して、主方向は、最小曲率と最大曲率を有する接平面内の方向である。主座標フレームＸ_p＝［ｘ_p ｙ_p ｚ_p］^Tは、主方向と位置合わせされたｘ_p及びｙ_pと、表面法線ｎと位置合わされたｚ_pとを有する正規直交フレームである。主フレームでは、主二次曲面は、面の二次記述である。

アルゴリズム１ 表面点ｐでの局所曲率の推定。
・Ｐ＝{ｐⁱ}ⁿ _i=1を、グローバル座標フレームで表されたｐの近傍内の１組の点とする。
１）全最小二乗法を使用して、平面をＸの点にフィッティングする。平面の法線をｐにおける表面法線ｎの推定として使用する。
２）回転主フレームＸ_rを構成する。（７）
３）ｐの点をＸ_rにマッピングする。（６）
ｐⁱ _r＝［ｘⁱ _r ｙⁱ _r ｚⁱ _r］^Ｔ＝Ｒ_r（ｐⁱ−ｐ） （１０）
４）回転主二次曲面
ｚ_r＝ａｘ² _r＋ｂｘ_rｙ_r＋ｃｙ² _r＋ｄｘ_r＋ｅｙ_r （１１）
を、写像点にフィッティングする。
・水平曲率は、ａに対応する。側面荷重は、水平勾配項ｄによって推定することができる。

二次フィッティングの一般的手法（ＭｃｌｖｏｒとＶａｌｋｅｎｂｕｒｇから改良［３］）をアルゴリズム１に示す。回転主フレームＸ_rを利用する。パイル凸性推定に関して、対象は、主に水平曲率である。水平曲率は、主曲率に対応しないことがある。したがって、代わりに、回転主フレーム（１つの軸が表面法線と合わせられ、１つの軸が地面と平行になるように定義された）を使用しなければならない。

ｘ_wとｙ_wがグランド平面にあり、ｚ_wが空を指す「ワールド」座標フレームＸ_w＝［ｘ_w ｙ_w ｚ_w］^Ｔを検討する。ワールド座標フレームから点ｐ_wの回転主フレームへの点ｐⁱ _ｗの変換は、次の通りである。

ｐ^ｉ _ｒ＝Ｒ_ｒ（ｐ^ｉ _ｗ−ｐ_ｗ） （６）

回転主フレームは、次の式を用いて定義することができる。

Ｒ_r＝［ｒ₁ ｒ₂ ｒ₃］^T （７）、ｒ₃＝ｎ，ｒ₂＝（ｚ_w×ｎ）／（ｚ_w×ｎ），ｒ₁＝ｒ₃×ｒ₂

回転主フレームは、ｎのまわりの回転によって主フレームＸ_pと関連付けられる。

回転された主二次曲面のパラメータをフィッティングするために（アルゴリズム１の段階４）、我々の実施態様ではＢＦＧＳ準ニュートンアルゴリズムを使用する。

アタック姿勢の推定に使用される量は、次の凸性と側面荷重と共に、（１１）から得られる。
Ｄ_c＝−ａ
Ｄ_s＝｜ｄ｜

凸領域は、Ｄ_c＞０を有し、凹領域は、Ｄ_ｃ＜０を有する。

１組の点にフィッティングされた二次曲面の実例は、図７を参照されたい。

フィッティング誤差（即ち、フィット面までの全ての点の平均二乗距離）は、Ｄ_ｃとＤ_sの信頼性評価として使用することができる。

曲面フィッティングの目的が、正確なパラメータ化された表面記述を作成することである場合、二次曲面の代わりにより高次の曲面をフィッティングすることができる。しかしながら、我々の目的は、表面パッチの全体的凸性を評価することなので、二次パラメータ化は十分である。また、高次多項式を面にフィッティングするときの数値問題がある。面を分析的にモデル化する適切な手法は、例えば、ガウス過程である。

方法段階Ｓ５０
この方法によれば、計算された凸量に応じて、対応するバケット軌道を有するアタック姿勢が選択され、あるいは重み付き品質値が使用されてもよい。推定された凸性と側面荷重の量が、所定のアタック姿勢での積載作業によるものである場合、そのアタック姿勢の品質を、凸性と側面荷重の重み付けした組み合わせによって測定することができ、その場合、重み付けを、記録データに対する実際の掘削する試験によって設定し評価することができる。

図８ａは、グランド平面３に配置されたパイル１を示す。パイルは、グランド平面３からパイル１を分離するパイル境界５によって区切られる。アタック姿勢は、アタック位置９とアタック角度αを有するベクトル７によって示される。アタック角度αは、負の表面法線から１０°未満のずれを有する範囲で選択されることが好ましいことがある。バケット１１の位置が示される。実際的な理由のため、アタック位置９は、バケットの横方向中間に設定される。この方法で、１組のアタック姿勢が生成される。１組のアタック姿勢は、様々な可能なアタック位置とアタック角度を含むことがある。アタック位置は、パイル境界に沿って選択されるか、必要に応じて境界から高い距離で選択される。アタックが、グランド平面から特定の高さで選択された場合、アタック角度は、選択された高さの平面に関して、パイルの表面法線に対して決定される。バケット軌道１３は、アタック姿勢が選択された場合にパイル内を通るバケットの意図された軌道であり、点線によって示される。点線１３_L及び１３_Rは、バケットの左側の少し前方角の軌道を示す。パイル表面の凸性の大きさは、バケット幅１９とバケット軌道１３の垂直投影２１とによって区切られるパイル表面１７の領域１５に関して計算される。この領域は、パイルの表面にあり、前方端１５ａ、後方端１５ｂ、及び左側及び右側に１５ｃ，１５ｄによって区分され、これらはそれぞれ、バケットの幅とバケット軌道の垂直投影とによって構成された領域の表面投影である。ここで、表面投影は、バケットの幅とパイルの表面領域上のバケット軌道の垂直投影とによって構成された領域の投影のことである。

パイル表面領域１５の凸性は、一実施形態では、前記バケットの区分の収集体積を決定することによって決定され、収集体積は、区分の幅と軌道の長さ範囲とによって画定された収集領域内のパイルの体積である。図８ｂで、３つの区分、即ち、左側区分２３、中央区分２５及び右側区分２７の収集体積が示される。図７ｂで、体積Ｖ_L、Ｖ_C及びＶ_Rは、バケットの幅とバケット軌道によって画定された面２９と、パイル表面の領域１５との間の体積である。

図８ｃに示したような代替実施形態では、体積Ｖ_L、Ｖ_C及びＶ_Rは、グランド平面とパイル表面の領域１５との間の体積である。

図９は、水平軸に側面荷重Ｃ_sの大きさと、垂直軸に凹部Ｃ_cの大きさとを有する平面図を示す。左側と右側の間の平衡荷重に近くかつ比較的高い凹性を有する関心分野が示される。この領域を有するアタック姿勢は、バケットに適切な負荷状態を提供するアタック姿勢で選択されてもよい。

参考文献
[1] K. Klasing, D. Wollherr, and M. Buss. A clustering method for efficient segmentation of 3D laser data. In ICRA, pages 4043-4048, Pasadena, USA, May 2008.
[2] P.H.S. Torr and A. Zisserman. MLESAC: a new robust estimator with application to estimating image geometry. Computer Vision and Image Understanding, 78(1): 138-156, Apr.2000.
[3] A. M. Mclvor and R. J. Valkenburg. A comparison of local surface geometry estimation methods. Machine Vision and Applications, 10:17-26,1997.

１ パイル
１３ バケット軌道
１７ パイル表面
２１ 垂直投影
３１ 作業機械
３３ バケット
９，α アタック姿勢
Ｃ_c 凸性の大きさ
Ｐ 三次元パイルデータ

Claims (16)

1. バケット（３３）を備えた作業機械（３１）の、パイル材料を積み込むときのアタック姿勢（９，α）を選択する方法であって、
   三次元パイルデータ（Ｐ）を取得する段階（Ｓ１０）と、
   １組のアタック姿勢（９，α）を生成する段階（Ｓ２０）と、
   各アタック姿勢（９，α）に関して前記パイル（１）内のバケット軌道（１３）を生成する段階（Ｓ３０）とを含む方法であって、
   前記１組のアタック姿勢（９，α）の各アタック姿勢（９，α）に関して、バケット幅（１９）と前記バケット軌道（１３）の垂直投影（２１）とによって区切られた、前記パイル表面（１７）の領域（１５）に関して、前記パイル表面の凸性の大きさ（Ｃ_c）を計算する段階（Ｓ４０）と、
   前記凸性の大きさ（Ｃ_c）に基づいてアタック姿勢（９，α）を選択する段階とを含む方法。
2. 前記バケット（３３）の区分の収集体積（Ｖ_L，Ｖ_C，Ｖ_R）を決定する段階であって、収集体積が、区分の幅と前記軌道（１３）の長さ範囲（２１）とよって画定された収集領域内の前記パイル（１）の体積である段階と、
   前記バケット（３３）の区分の前記収集体積（Ｖ_L，Ｖ_C，Ｖ_R）に基づいて前記凸性の大きさ（Ｃ_c）を計算する段階を特徴とする、請求項１に記載の方法。
3. 更に、各アタック姿勢（９，α）に関して側面荷重の大きさ（Ｃ_L）を決定し、前記側面荷重の大きさ（Ｃ_s）と前記凸性の大きさ（Ｃ_c）とに基づいてアタック姿勢（９，α）を選択する段階を特徴とする、請求項１又は２に記載の方法。
4. 前記区分が、中央区分、右翼区分及び左翼区分を含み、
   前記左翼区分（Ｖ_L）、前記右翼区分（Ｖ_R）及び前記中央区分（Ｖ_C）の前記収集体積の比較に基づいて前記凸性の大きさ（Ｃ_c）を計算する段階を特徴とする、請求項１乃至３のいずれかに記載の方法。
5. 前記側面荷重の大きさ（Ｃ_s）が、前記左翼区分と右翼区分（Ｖ_L，Ｖ_R）の体積の比較に基づく、請求項３又は４に記載の方法。
6. 前記バケット（１９）の前記幅が、等しい幅の３つの区分に分離され、前記凸性の大きさ（Ｃ_c）が、Ｃ_c＝（Ｖ_c／ｍａｘ（Ｖ_r，Ｖ_l））−１として計算され、前記側面荷重の大きさは、Ｃ_s＝（ａｂｓ（Ｖ_r−Ｖ_l））／（Ｖ_r＋Ｖ_l）として計算される、請求項１乃至５のいずれかに記載の方法。
7. センサ（４３）から三次元点クラウドを取得し、前記三次元点クラウドをグランドデータと前記三次元パイルデータに分離する段階を特徴とする、請求項１乃至６のいずれかに記載の方法。
8. グランド平面を前記グランドデータにフィッティングする段階を特徴とする、請求項７に記載の方法。
9. 前記パイル（１）の境界（５）の近く及び好ましくは前記境界（５）で走査点の間で可能なアタック姿勢（９，α）を選択する段階を特徴とし、前記アタック姿勢（９，α）の向きが、境界法線から２０°未満のずれの範囲内で選択される、請求項１乃至８のいずれかに記載の方法。
10. 前記収集体積が、グランド平面からパイル表面まで計算される、請求項１乃至９のいずれかに記載の方法。
11. 前記収集体積が、前記バケット軌道（１３）からパイル表面まで計算される、請求項１乃至１０のいずれかに記載の方法。
12. 前記バケット（１９）の幅と前記バケット軌道（１３）とによって画定された、前記パイルの表面の領域内の三次元パイルデータに面をフィッティングする段階と、
    前記面から前記凸性の大きさ（Ｃ_c）を計算する段階とを特徴とする、請求項１、又は７乃至１１のいずれかに記載の方法。
13. 二次多項式：
    ｚ_r＝ａｘ_r ²＋ｂｘ_rｙ_r＋ｃｙ_r ²＋ｄｘ_r＋ｅｙ_r
    を、前記バケット（１９）の幅と前記バケット軌道（１３）とによって画定された領域内の前記三次元パイルデータにフィッティングする段階と、
    前記二次多項式内の因子ａから前記凸性の大きさ（Ｃ_c）を決定する段階とを含む、請求項１２に記載の方法。
14. 前記バケット（１９）の幅と前記バケット軌道（１３）とによって画定された領域内の前記三次元パイルデータに面をフィッティングする段階と、
    前記面の前記表面法線のまわりに回転された地面フレームから回転主フレームを構成する段階と、
    前記三次元パイルデータを前記回転主フレームにマッピングする段階とを含む、請求項１３に記載の方法。
15. 側面荷重の大きさを前記因子ｄから決定する段階を特徴とする、請求項１３又は１４に記載の方法。
16. バケット（３３）と、三次元パイルデータを検出するセンサシステム（４３）と、パイル内のバケット軌道（１３）に沿った前記バケット（３３）の進行を可能にする自動操縦システム（４５）とを備えた作業機械（３１）を使用することによって、前記バケット（３３）に積載する方法であって、請求項１乃至１４のいずれかに記載の方法を使用してアタック姿勢（９，α）及び対応するバケット軌道（１３）を選択する段階を含み、前記作業機械（３１）を制御して、前記バケット（３３）を前記選択されたアタック姿勢（９，α）に位置決めし、前記バケット（３３）を前記選択されたバケット軌道（１３）に沿って移動させて前記バケット（３３）の充満を最適化する方法。

Images