JP6491576B2 - 角度検出装置 - Google Patents

Description

本発明は、角度検出装置に関する。

土木建設工事等に使用される建設機械は、クローラを備えて走行可能な走行体と、走行体に旋回可能に取り付けた旋回体と、旋回体に上下方向に揺動可能に連結されたブームと、ブームに上下方向に揺動可能に連結されるアームと、アームの先端に上下方向に揺動可能に連結されたアタッチメントを備えているものが知られている。

このような建設機械にあっては、走行体上に取り付けられた旋回体、ブーム、アームおよびアタッチメントの姿勢を変えながら土木建設工事を行うため、無理な操作を行った場合に走行車両の重心が偏ってバランスを崩して転倒する恐れがある。

近年、転倒防止のため、ゼロモーメントポイント（以下、単に「ＺＭＰ」という）を活用する安全装置が開発されている。ＺＭＰは、物体に作用する慣性力、重力および外力によるモーメントがゼロになる地面上の点のことであり、ＺＭＰが平面視で見た走行体の中心付近に位置する場合、走行車両を転倒させるようなモーメントが生じない。そのため、ＺＭＰが平面視で見た走行体の中心付近に位置する場合、走行車両が転倒しにくくなり、ＺＭＰが平面視で見た走行体の端に寄れば寄るほど走行車両が転倒しやすくなる。したがって、ＺＭＰがどこに位置しているのかを求めれば、走行車両の転倒し易さを評価できるのである。

ＺＭＰを求めるには、旋回体、ブーム、アームおよびアタッチメントのワールド座標が必要である。これを求めるには、走行体に対する旋回体の姿勢、旋回体に対するブームの姿勢、ブームに対するアームの姿勢、およびアームに対するアタッチメントの姿勢をそれぞれ特定しなければならない。

従来、旋回体、ブーム、アームおよびアタッチメントの姿勢を特定する装置としては、旋回体の走行体に対する角度、旋回体に対するブームの角度、ブームに対するアームの角度、アームに対するアタッチメント角度を角度センサを利用して検出するものがある（たとえば、特許文献１参照）。

また、旋回体、ブーム、アームおよびアタッチメントの姿勢を特定する他の装置としては、カメラでブーム、アームに設けたマーカーを撮影してマーカーの座標を求め、求めたマーカーの座標と予め保有するマーカー位置データとを照合してこれらの姿勢を特定する装置も開発されている（たとえば、特許文献２参照）。

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

しかしながら、特許文献１に開示された装置では、旋回体等の角度の検出のために角度センサを利用しており、厳しい使用環境にも耐えうるよう角度センサの保護、配線の取り回しといった配慮が必要となる。

また、特許文献２に開示された装置では、角度センサが不要であり安全な場所（キャブ）へのカメラの設置により前記問題を解消できるが、ブーム等の姿勢の特定に際してマーカー位置データとブーム等の座標を照合する必要がある。そのため、特許文献２の装置では、膨大なマーカー位置データを保有する必要があり、大容量のメモリを必要としコスト高となる。

そこで、本願発明は、上記問題を解決するために創案されたものであって、その目的とするところは、センサが不要でセンサ保護の問題もなくコストも低減可能な角度検出装置の提供である。

上記目的を達成するために、本発明の建設機械の角度検出装置は、可動部材のうち末端のリンクに取付けられるカメラで他のリンクを撮影し、その撮影した画像から他のリンクの相対角度を求める。このように角度検出装置は、センサ類を用いないので、センサ類の故障の心配もなく、カメラの設置個所は自由度が高く安全な場所に設置できるので厳しい使用環境にも耐えられ、また、膨大なマップ位置データの保有が不要となり、大容量のメモリを必要としないので、コストも低減できる。また、角度検出装置は、角度検出部がカメラで撮影した前記画像を処理して、他のリンクのエッジを検出し、相対角度を求める。よって、このように構成された角度検出装置は、カメラで撮影した連続画像からではなく、一つの画像から相対角度を検出できるので、時間をかけずにタイムリーに角度検出できる。

また、請求項２の角度検出装置は、求めた相対角度から他のリンクの姿勢を特定する姿勢特定部を備えているので、角度検出装置が適用される建設機械等のＺＭＰの演算に必要な角度情報が得られる。

さらに、請求項３の角度検出装置は、可動部材が三つ以上のリンクを有し、角度検出部が末端のリンクから一番近いリンクの相対角度を求め、末端のリンクから二番目以降の近いリンクについては、姿勢特定部により特定された一つ手前のリンクの姿勢に基づいて相対角度を求める。よって、この角度検出装置にあっては、可動部材が多数のリンクを有する場合にあっても、全リンクの相対角度を簡単に検出できる。

さらに、請求項４の角度検出装置にあっては、可動部材の末端のリンクを建設機械の旋回体とし、他のリンクを建設機械の旋回体に回転自在に連結される作業装置とし、カメラが旋回体のキャビン内に設置されるようになっている。このように相対角度検出に必要な画像を撮影するカメラがキャビン内に収容されるので、カメラが建設機械の外部環境から保護される。よって、請求項４の角度検出装置にあっては、カメラが風雨に晒されず、土木工事作業中の石や土砂の飛来からも保護されるので、安定した相対角度検出が可能となる。

本発明の角度検出装置は、センサが不要でセンサ保護の問題もなくコストも低減可能である。

第一の実施の形態における角度検出装置のシステム構成の一例を示した図である。 第一の実施の形態における角度検出装置を建設機械に適用した図である。 第一の実施の形態における角度検出装置のカメラが撮影した画像を示す図である。 カメラが撮影した画像のエッジ画像処理後の画像を示す図である。 図４の画像を射影変換処理した画像を示す図である。 リンクの連結軸とエッジとの関係を示す図である。 エッジを抽出する処理を説明する図である。 エッジを抽出する処理を説明する図である。 カメラが撮影した画像のエッジから角度を求める処理を説明する図である。 カメラが撮影した画像のエッジから角度を求める処理を説明する図である。 第一の実施の形態における角度検出装置のハードウェア資源の構成例を示した図である。 第一の実施の形態における角度検出装置における角度検出の処理手順を示したフローチャートである。 第二の実施の形態における角度検出装置のシステム構成の一例を示した図である。 カメラが撮影した画像を射影変換処理した画像を示す図である。 第二の実施の形態における角度検出装置における角度検出の処理手順を示したフローチャートである。

＜第一の実施の形態＞
以下、図に示した実施の形態に基づき、本発明を説明する。図１に示すように、本実施の形態の角度検出装置１は、建設機械Ｃにおける作業装置Ｗの各部の角度検出を行うべく建設機械Ｃに適用されている。なお、角度検出装置１は、建設機械Ｃ以外の装置、機械におけるリンクの相対角度検出に利用可能である。

具体的には、角度検出装置１は、可動部材Ｍの末端のリンクである旋回体３に取り付けられて旋回体３以外の他のリンクである作業装置Ｗを撮影可能なカメラ４と、カメラ４が撮影した画像に基づいて隣り合うリンク同士の相対角度を検出する角度検出部５と、角度検出部５が検出した前記角度に基づいて、前記旋回体３に対する作業装置Ｗの姿勢を特定する姿勢特定部６とを備えて構成されている。

建設機械Ｃは、クローラ２ａを備えて走行可能な走行体２と、走行体２の上方に旋回可能に連結される可動部材Ｍとを備えている。可動部材Ｍは、走行体２に旋回可能に連結される旋回体３と、旋回体３に上下方向に揺動可能に連結される作業装置Ｗとを備えている。作業装置Ｗは、本例では、旋回体３に上下方向揺動可能に連結されるブーム７と、ブーム７に上下方向に揺動可能に連結されるアーム８と、アーム８に上下方向に揺動可能に連結されるアタッチメント９とを備えている。

そして、建設機械Ｃは、図示したところでは、アタッチメント９をバケットとしているが、バケットのほか、作業の用途に応じてブレーカー、カッター、グラップル等に変更できる。可動部材Ｍは、旋回体３、ブーム７、アーム８およびアタッチメント９のそれぞれをリンクとして、これらを相対回転可能に直列に連結した多関節のリンク機構として構成されている。そして、可動部材Ｍは、旋回体３を末端のリンクとし、それ以外の作業装置Ｗであるブーム７、アーム８およびアタッチメント９を他のリンクとしている。

なお、可動部材Ｍを構成するリンクの数は、２以上であればよく、可動部材Ｍを旋回体とし、作業装置Ｗをクレーンとして、建設機械Ｃを可動部材Ｍが二つのリンクで構成されるクレーン車としてもよい。また、本例では、角度検出装置１を建設機械Ｃにおける可動部材Ｍの相対角度検出に利用しているが、可動部材Ｍは建設機械Ｃの上部構造に限られず、直列に相対回転可能に連結される複数のリンクで構成されていればよい。

カメラ４は、ＣＣＤ（電荷結合素子、図示せず）とレンズ（図示せず）、焦点調節部（図示せず）を備えたＣＣＤカメラとして構成され、本例では、旋回体３のキャビン３ａ内に作業装置Ｗの全体を撮影できるように設置されている。具体的には、カメラ４は、作業装置Ｗを真後ろからではなく、図２に示すように、作業装置Ｗの側面も撮影できるように旋回体３に取り付けられている。なお、カメラ４は、可動部材Ｍの末端のリンクである旋回体３も部分的あるいは全体を撮影できるように設置されてもよく、旋回体３のキャビン３ａ外に取り付けてもよい。

また、カメラ４は、あらかじめ設定される撮影範囲をあらかじめ決められたフレームレートで常時撮影し続け、この撮影した画像を電気信号に変換して角度検出部５へ出力するようになっている。なお、カメラ４はＣＣＤカメラとして構成される以外にもＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ ＯｘｉｄｅＳｅｍｉｃｏｎｄｕｃｔｏｒ）を利用したカメラを使用できる。

角度検出部５は、カメラ４が撮影した画像をグレースケールの画像に変換後に二値化処理し、エッジ画像を取得するエッジ画像取得部５１と、エッジ画像取得部５１の処理によって得られたエッジ画像の座標系を作業装置Ｗを真横から見た画像の座標系へ変換する射影変換部５２と、射影変換部５２により変換された変換画像をハフ変換してから作業装置Ｗの各リンクであるブーム７、アーム８およびアタッチメント９のエッジを抽出するエッジ抽出部５３と、エッジ抽出部５３で抽出したエッジから旋回体３に対するブーム７の相対角度θ１（図７参照）、ブーム７に対するアーム８の相対角度θ２およびアーム８に対するアタッチメント９の相対角度θ３を求める角度演算部５４と備えて構成されている。

まず、角度検出部５は、カメラ４によって撮影され、カメラ４のレンズ歪等による歪をキャリブレーション処理によって較正された画像を取得して、エッジ画像取得部５１によって処理する。エッジ画像取得部５１は、カメラ４が撮影した画像をたとえば２５６階調の輝度値で表現されるグレースケールの画像に変換し、さらに、画像を二値化処理して二値化画像を得て、エッジ画像処理をしてエッジ画像を得る。カメラ４が撮影した図３に示した作業装置Ｗの画像をエッジ画像取得部５１によって処理すると、図４に示すエッジのみで表現される画像が得られる。

射影変換部５２は、エッジ画像取得部５１の処理によって得たエッジ画像の座標系を、作業装置Ｗを真横から見た側面画像の座標系へ変換する。この射影変換部５２によって図４に示すエッジ画像を処理すると、作業装置Ｗのブーム７、アーム８およびアタッチメント９の作動面、つまり、ブーム７、アーム８およびアタッチメント９が動作する平面を貫く方向となる真横から見た図５に示した画像が得られる。なお、エッジ画像を座標変換してもエッジが白線で示される画像が得られるが、図中への符号や引き出し線を記載する関係で、説明の便宜上、図５中では実際とは画像を白黒反転として記載しており、以下、図６、図７、図８、図９および図１４においても同様とする。

なお、射影変換部５２の射影変換処理では、実際に画像を得る必要はなく、変換後のエッジの座標情報を取得できればよい。なお、具体的には、射影変換部５２は、カメラ４の撮影した画像座標を射影変換行列を用いて処理するが、この座標変換行列は、予めカメラ４で作業装置Ｗを撮影して射影変換行列を予め求めておけばよい。

エッジ抽出部５３は、座標変換後のエッジ画像をハフ変換によりブーム７、アーム８およびアタッチメント９のエッジを抽出する。エッジ抽出部５３は、まず、ブーム７についてのみエッジを抽出する。つづいて、こうして得られたブーム７のエッジから角度演算部５４での処理で旋回体３に対するブーム７の相対角度θ１が得られ、さらに、姿勢特定部６がブーム７の姿勢を特定する。このブーム７の姿勢が特定された後、エッジ抽出部５３は、ブーム７の姿勢に基づいてアーム８のエッジを抽出する。そして、アーム８について前述のように姿勢が特定された後、エッジ抽出部５３は、アタッチメント９のエッジを抽出するようになっている。つまり、本例では、角度検出部５は、可動部材Ｍの末端のリンクである旋回体３に近いリンクから順に相対角度を求め、姿勢特定部６の処理によって姿勢が特定されると、次のリンクの相対角度を求めるようになっている。

よって、まず、エッジ抽出部５３は、ブーム７のエッジを抽出する。ブーム７は、旋回体３に対して上下方向へ揺動可能に連結されており、ブーム７は旋回体３に連結軸７ａを中心として揺動可能とされている。そして、ブーム７の連結軸７ａ側端の下端のエッジＥ１は、図６中太線に示すように、ブーム７の旋回体３に対する連結軸７ａを中心とした回転によって常に連結軸７ａから一定の半径Ｒ１だけ離れて位置するはずである。つまり、エッジＥ１を延長した仮想線に連結軸７ａの中心から垂らした垂線の長さは、必ず、Ｒ１となる。また、カメラ４の撮影範囲は固定されているので、連結軸７ａの位置は、撮影範囲外であって撮影されずとも定位置であり、射影変換後の座標系にあってもその座標は不動である。カメラ４で撮影した画像から連結軸７ａが撮影されていなくとも、ブーム７と旋回体３の位置関係からブーム７の回転中心である連結軸７ａの位置を予め知ることができ、連結軸７ａの射影変換後の座標は既知である。図７に示すように、ブーム７の作動面上にｘ軸とｙ軸を採ると、連結軸７ａから地面へ垂らした垂線の地面との交点を原点Ｏとすると、連結軸７ａの高さは設計値であり既知であるので、高さをＬ０とすると、連結軸７ａの中心座標は（Ｌ０，０）となる。そして、射影変換後のエッジ画像をハフ変換によってブーム７の直線のエッジが複数得られても、これらのエッジ或いはこれらエッジを延長して得られた直線と連結軸７ａの中心座標からの距離が半径Ｒ１に等しいエッジを選択すれば、ブーム７のエッジを抽出できる。

前述したように、半径Ｒ１は、設計値であり、この設計値を利用してエッジを抽出できるが、以下の手順によって、連結軸７ａの中心座標とエッジまでの半径Ｒ１を設定してもよい。まず、ブーム７と旋回体３の相対角度を変えて、ブーム７をカメラ４で撮影した画像を少なくとも三つ得て、これら三つの画像から座標変換後のブーム７のエッジを得る。これら三つのエッジの内接円を求め、この内接円の半径をエッジ抽出のための半径Ｒ１とし、内接円の中心座標を連結軸７ａの中心座標として設定してもよい。このように、カメラ４で撮影した画像を処理して連結軸７ａの中心座標とエッジ抽出のための半径Ｒ１を設定すると、より、精緻にブーム７のエッジを抽出可能である。

また、本例では、建設機械Ｃが走行中或いは旋回体３や作業装置Ｗが駆動中であると、建設機械Ｃが振動してカメラ４に振動が伝達される。すると、無振動でカメラ４がブーム７を撮影した画像に対して振動中のカメラ４がブーム７を撮影した場合の画像中のブーム７の位置がオフセットされる場合がある。そこで、半径Ｒ１に対して閾値Δｒ１を設定して、図８に示すように、連結軸７ａの中心座標からエッジ或いはエッジの延長線までの距離が半径Ｒ１を中心として閾値Δｒ１で設定される範囲Ｒ１±Δｒ１内にあるエッジを選択する。そして、抽出したエッジのうち、一番長いエッジをブーム７のエッジＥ１として抽出する。このように、本例では、連結軸７ａの中心座標からエッジまで距離が半径Ｒ１を中心として閾値Δｒ１で設定される範囲Ｒ１±Δｒ１内にあるエッジをブーム７のエッジとして抽出する。そのため、閾値Δｒ１で振動によるオフセット分を吸収してカメラ４が振動してもブーム７のエッジを正確に抽出できる。さらに、エッジ抽出部５３は、エッジまで距離が半径Ｒ１を中心として閾値Δｒ１で設定される範囲Ｒ１±Δｒ１内にあるエッジが複数ある場合には、一番長いものをブーム７のエッジとして抽出する。一番長いエッジをブーム７のエッジとして抽出すると、ブーム７のエッジの誤抽出を防止でき、より確実にブーム７のエッジの抽出が可能となる。つまり、前述のエッジ抽出部５３の処理を行うと、角度検出に利用するブーム７のエッジＥ１を精度よく抽出できるのである。

つづいて、角度演算部５４は、エッジ抽出部５３により選択されたエッジＥ１から旋回体３に対するブーム７の相対角度θ１を検出する。具体的には、角度演算部５４は、図９に示すように、画像の水平軸Ｈから時計回りに見てエッジＥ１までの間の角度θ１’を求める。図９中でエッジを延長すると延長されたエッジと画像の縁との交点によって、延長されたエッジの画像内での高さｙと幅ｘが求まるので、この高さｙと幅ｘからθ１’＝ｔａｎ^−１（ｙ／ｘ）を演算してθ１’が求まる。角度θ１’は、旋回体３に対してエッジが画像内で水平となる場合を０度として求められる。ブーム７のエッジＥ１は旋回体３側端の下端のエッジであり、ブーム７とアーム８とを回転可能に連結する連結軸８ａと連結軸７ａとを結ぶ直線とエッジＥ１とが平行ではない。ブーム７の旋回体３に対する相対角度θ１は前記直線と旋回体３とのなす角度であるため、角度θ１’を求めた後、角度θ１’を相対角度θ１へ変換する。角度θ１’と相対角度θ１は、所定の角度差だけずれを持っており、設計値であるため、角度差は既知であり、角度θ１’が分かれば一義的に相対角度θ１を求め得る。本例では、角度θ１’と相対角度θ１との関係をテーブル化しており、角度演算部５４は、角度θ１’を求めた後、テーブルを参照して相対角度θ１を求めるが、角度差が既知であるので角度θ１’に角度差を加算して相対角度θ１を求めてもよい。なお、相対角度θ１は、地面に対して連結軸７ａと連結軸８ａを結ぶ直線が水平となる状態となる角度を０度として扱っている。

姿勢特定部６は、前述の角度検出部５によって検出されたブーム７の旋回体３に対する相対角度θ１から連結軸８ａの中心座標を求めて、ブーム７の姿勢を特定する。ブーム７の連結軸７ａから連結軸８ａまでの距離Ｌ１は、設計値であるので既知である。そして、角度検出部５によって、相対角度θ１が検出されているので、ブーム７の作動面上において、連結軸８ａの中心座標を（ｘ２，ｙ２）とし、既知である連結軸７ａの中心座標を（Ｌ０，０）とする。すると、ｘ２＝Ｌ０＋Ｌ１・ｃｏｓθ１、ｙ２＝Ｌ１・ｓｉｎθ１となる。よって、姿勢特定部６は、相対角度θ１と距離Ｌ１を利用してブーム７の姿勢を特定して連結軸８ａの中心座標を求め得る。このようにして連結軸８ａの中心座標が求まると、角度検出部５のエッジ抽出部５３は、連結軸８ａの中心座標を用いてブーム７のエッジを抽出する処理と同様の処理を行ってアーム８のエッジを検出する。なお、姿勢特定部６は、連結軸８ａの中心座標だけではなく、ブーム７の重心座標を求めてもよい。ブーム７の重心から連結軸７ａまでの距離は設計値であり既知であるから連結軸８ａの中心座標と同様に求められる。

エッジ抽出部５３は、連結軸８ａの中心座標を利用して、アーム８のエッジを抽出する。アーム８は、ブーム７に対して上下方向へ揺動可能に連結されており、アーム８はブーム７に連結軸８ａを中心として揺動可能とされている。そして、アーム８の連結軸８ａ側端の下端のエッジＥ２は、図６中太線で示すように、アーム８のブーム７に対する連結軸８ａを中心とした回転によって常に連結軸８ａから一定の半径Ｒ２だけ離れて位置する。つまり、エッジＥ２を延長した仮想線に連結軸８ａの中心から垂らした垂線の長さは、必ず、Ｒ２となる。したがって、エッジ抽出部５３は、ブーム７のエッジを検出する処理と同様に、ハフ変換後の前記エッジ画像をからアーム８の直線のエッジが複数得られても、これらのエッジ或いはこれらエッジを延長して得られた直線と連結軸８ａの中心座標からの距離が半径Ｒ２に等しいエッジを選択すれば、アーム８のエッジを抽出できる。なお、この処理において使用する連結軸８ａの中心座標と半径Ｒ２は、前述と同様に、アーム８とブーム７の相対角度を変えて撮影した画像を三つ以上取得し、アーム８のエッジの内接円を求めて設定してもよい。また、本例では、先程と同様に、カメラ４の振動によるエッジが画像中でオフセットされてもエッジＥ２の抽出が可能なように、半径Ｒ２に対して閾値Δｒ２を設定してある。よって、連結軸８ａの中心座標からエッジ或いはエッジの延長線までの距離が半径Ｒ２を中心として閾値Δｒ２で設定される範囲Ｒ２±Δｒ２内にあるエッジを選択する。

このように選択されたエッジが複数ある場合、エッジ長、エッジ或いはエッジの延長線と連結軸８ａの中心座標までの距離と半径Ｒ２とのずれ量、エッジ端と連結軸８ａの中心座標までの距離の三つの尺度で評価してエッジＥ２を抽出する。具体的には、それぞれのエッジについて、エッジ長が長い程良い点数をつけ、前記ずれ量が少ない程良い点数をつけ、さらに、エッジ端までの距離が短い程良い点数をつけ、これら点数の総合点が最も高い点数のエッジをエッジＥ２として抽出する。このようにスコアリングによる評価を行ってアーム８のエッジＥ２を抽出すると高精度で真のアーム８のエッジＥ２を抽出できる。なお、スコアリングによる評価を行わず、ブーム７のエッジ抽出と同様の処理によってアーム８のエッジＥ２を抽出してもよい。また、エッジ抽出部５３は、連結軸８ａの至近のブーム７の下端のエッジＥ１_２を抽出する。

つづいて、角度演算部５４は、アーム８のエッジＥ２とブーム７のエッジＥ１_２からブーム７に対するアーム８の相対角度θ２を検出する。具体的には、角度演算部５４は、図９に示すように、エッジＥ２とエッジＥ１_２は直線であり、両者の交点における両者のなす角度であって、エッジＥ１_２から時計回りに見て両者のなす角度θ２’を求める。図９中でエッジＥ２とエッジＥ１_２の座標と傾きのデータが得られるので、これらの情報からθ２’が求まる。角度θ２’は、ブーム７のエッジＥ１_２とアーム８のエッジＥ２のなす角度であり、本例では、エッジＥ２は、アーム８とアタッチメント９とを回転可能に連結する連結軸９ａと連結軸８ａとを結ぶ直線と平行ではなく、エッジＥ１_２は、連結軸８ａと連結軸７ａとを結ぶ直線も平行ではない。したがって、アーム８のブーム７に対する相対角度θ２は、連結軸９ａと連結軸８ａとを結ぶ直線と、連結軸８ａと連結軸７ａとを結ぶ直線とのなす角度であるため、角度θ２’を求めた後、角度θ２’を相対角度θ２へ変換する。角度θ２’と相対角度θ２は、所定の角度差だけずれを持っており、設計値であるため、角度差は既知であり、角度θ２’が分かれば一義的に相対角度θ２を求め得る。本例では、角度θ２’と相対角度θ２との関係をテーブル化しており、角度演算部５４は、角度θ２’を求めた後、テーブルを参照して相対角度θ２を求めるが、角度差が既知であるので角度θ２’に角度差を加算して相対角度θ２を求めてもよい。

このように相対角度θ２が求まると、姿勢特定部６は、相対角度θ２から連結軸９ａの中心座標を求めて、アーム８の姿勢を特定する。アーム８の連結軸９ａから連結軸８ａまでの距離Ｌ２は、設計値であるので既知である。そして、角度検出部５によって、相対角度θ２が検出されているので、アーム８の作動面上において、連結軸９ａの中心座標を（ｘ３，ｙ３）とすると、既知である連結軸８ａの中心座標（ｘ２，ｙ２）とする。すると、ｘ３＝ｘ２＋Ｌ２・ｃｏｓ（θ１＋θ２）、ｙ２＝ｙ２−Ｌ２・ｓｉｎ（θ１＋θ２）となる。よって、姿勢特定部６は、相対角度θ２と距離Ｌ２を利用してアーム８の姿勢を特定して連結軸９ａの中心座標を求め得る。また、姿勢特定部６は、アーム８に対してアタッチメント９を駆動するためのレバー１０とアーム８とを回転可能に連結する連結軸１０ａの中心座標を求める。なお、姿勢特定部６は、連結軸９ａの中心座標だけではなく、アーム８の重心座標を求めてもよい。アーム８の重心から連結軸８ａまでの距離は設計値であり既知であるから連結軸９ａの中心座標と同様に求められる。

連結軸１０ａの中心座標が求まると、エッジ抽出部５３は、連結軸１０ａの中心座標を利用して、アタッチメント９とアーム８の相対角度θ３を求めるために、アタッチメント９のエッジを抽出する。本例では、相対角度θ３を求める際には、射影変換する前のエッジ画像を利用する。アタッチメント９は、この場合、バケットであり、エッジ抽出部５３は、図１０中で太線で示すように、アタッチメント９の上端と下端のエッジＥ３_１，Ｅ３_２を抽出する。

つづいて、角度演算部５４は、まず、連結軸１０ａから各エッジＥ３_１，Ｅ３_２までの上下方向の距離Ｈ２，Ｈ３を求めて、アーム８に対するアタッチメント９の相対角度θ３を検出する。角度演算部５４は、連結軸１０ａとエッジＥ３_１との距離Ｈ２と、連結軸１０ａとエッジＥ３_２との距離Ｈ３の値の組み合わせに対して、相対角度θ３は一対一の関係にあり、これら距離Ｈ２，Ｈ３から相対角度θ３を求める。アタッチメント９の長さは既知であり、連結軸１０ａに対して上下方向の距離Ｈ２，Ｈ３が得られれば、アタッチメント９のアーム８の相対角度θ３を知りえる。また、本例ではアタッチメント９がバケットであり、上端外形が側方から見て円形をしているために、アタッチメント９がアーム８に対して相対角度θ３が変化するように姿勢を変えても連結軸１０ａとアタッチメント９の上端のエッジＥ３_１の距離Ｈ２が変化しない場合もある。他方、アタッチメント９がアーム８に対して相対角度θ３が変化するように姿勢を変えると連結軸１０ａとアタッチメント９の下端のエッジＥ３_２の距離Ｈ３は、必ず変化する。しかし、アタッチメント９が、たとえば、鉛直下方から旋回体側に３０度傾いた状態と、鉛直下方から反旋回体側へ３０度傾いた状態とで、下端のエッジＥ３_２は同じ高さ位置ある。そのため、アタッチメント９の下端のエッジＥ３_２の距離Ｈ３だけで相対角度θ３を検出すると、アタッチメント９が旋回体側へ傾いているのか反旋回体側へ傾いているのか分からない。そこで、本例では、連結軸１０ａから各エッジＥ３_１，Ｅ３_２までの上下方向の二つの距離Ｈ２，Ｈ３から相対角度θ３を求めるので、アタッチメント９が姿勢を変えて相対角度θ３が変化するとこれを正確に検出できる。なお、アタッチメント９がバケットではなく、アーム８とブーム７の相対角度θ２の検出処理と同様に、アタッチメント９とアーム８の双方のエッジの抽出によって相対角度θ３を検出できる場合には、アーム８とブーム７の相対角度θ２の検出処理と同様の処理を行ってもよい。したがって、図６中でアタッチメント９のバケットの側方上端の直線のエッジＥ３_３と、アーム８のエッジＥ２_２を抽出してこれらのなす角度を求めて相対角度θ３を求めるようにしてもよい。

このように相対角度θ３が求まると、姿勢特定部６は、相対角度θ３からアタッチメント９の姿勢を特定でき、たとえば、アタッチメント９の重心位置や先端位置を把握できる。たとえば、アタッチメント９の先端座標を求めたい場合、アタッチメント９の先端からアーム８の連結軸９ａまでの距離Ｌ３は、設計値であるので既知である。そして、角度検出部５によって、相対角度θ３が検出されているので、アタッチメント９の作動面上において、アタッチメント９の先端座標を（ｘ４，ｙ４）とすると、既知である連結軸９ａの中心座標（ｘ３，ｙ３）とする。すると、ｘ４＝ｘ３＋Ｌ３・ｃｏｓ（θ１＋θ２＋θ３）、ｙ４＝ｙ３−Ｌ３・ｓｉｎ（θ１＋θ２＋θ３）となる。よって、姿勢特定部６は、相対角度θ３と距離Ｌ３を利用してアタッチメント９の姿勢を特定して先端座標を求め得る。アタッチメント９の重心座標についても連結軸９ａから重心までの距離は既知であるから、先端座標と同様に求め得る。

このように、角度検出部５は、可動部材Ｍにおける末端のリンクである旋回体３に一番近い作業装置Ｗのリンクであるブーム７の旋回体３に対する相対角度θ１を求める。そして、姿勢特定部６がブーム７の姿勢を特定すると、角度検出部５は、次に、旋回体３に二番目に近いアーム８のブーム７に対する相対角度θ２を求める。さらに、姿勢特定部６がアーム８の姿勢を特定すると、角度検出部５は、次に、旋回体３から一番遠いアタッチメント９のアーム８に対する相対角度θ３を求める。つまり、角度検出部５は、可動部材Ｍにおける末端のリンクである旋回体３に一番近い作業装置Ｗのリンクから順番に相対角度を検出する。

本実施の形態における角度検出装置１のハードウェア資源の構成について説明すると、この角度検出装置１は、図１１に示すように、ハードウェアとしてはカメラ４と、カメラ４から出力される信号を増幅する増幅器とアナログ／デジタル変換器を含んで構成される画像インターフェース６０と、角度検出装置１における処理を実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｓｓｅｓｉｎｇ Ｕｎｉｔ）６１と、前述の角度検出装置１における処理を行うためＣＰＵ６１が実行するアプリケーションやオペレーティングシステム等のプログラムを格納するＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）６２と、上記ＣＰＵに記憶領域を提供するＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）６３とを備えて構成されればよい。なお、角度検出装置１の各部における構成は、ＣＰＵ６１の角度検出装置１の処理を行うためアプリケーションプログラムの実行により実現できる。

以上、角度検出装置１の構成について説明したが、以下、角度検出装置１の角度検出処理手順を図１２のフローチャートの一例に基づいて説明する。

まず、角度検出装置１は、角度検出のため、まず、カメラ４で撮影範囲を撮影する（ステップＳ１）。次に、カメラ４で撮影した画像に対して前述のエッジ画像処理と射影変換を行ってエッジ画像を得る（ステップＳ２）。

つづいて、エッジ画像からブーム７のエッジを抽出し（ステップＳ３）、旋回体３に対するブーム７の相対角度θ１を求める（ステップＳ４）。さらに、相対角度θ１から連結軸８ａの中心座標を演算する（ステップＳ５）。

さらに、エッジ画像からアーム８とブーム７のエッジを抽出し（ステップＳ６）、ブーム７に対するアーム８の相対角度θ２を求める（ステップＳ７）。そして、相対角度θ２から連結軸９ａの中心座標を演算する（ステップＳ８）。

つづいて、エッジ画像からアタッチメント９の上端と下端のエッジを抽出し（ステップＳ９）、アーム８に対するアタッチメント９の相対角度θ３を求める（ステップＳ１０）。さらに、相対角度θ３からアタッチメント９の先端の座標を演算する（ステップＳ１１）。以上、ステップＳ１からステップＳ１１までの処理を所定周期で繰り返し行って、相対角度θ１，θ２，θ３を連続して検出可能である。

このように構成された角度検出装置１は、可動部材Ｍのうち末端のリンク（旋回体３）に取付けられるカメラ４で他のリンク（ブーム７、アーム８およびアタッチメント９）を撮影し、その撮影した画像から他のリンク７，８，９の相対角度θ１，θ２，θ３を求める。このように角度検出装置１は、センサ類を用いないので、センサ類の故障の心配もなく、カメラ４の設置個所は自由度が高く安全な場所に設置できるので厳しい使用環境にも耐えられる。また、角度検出装置１では、ブーム等の姿勢の特定に際してマーカー位置データとブーム等の座標を照合する必要がないので、膨大なマップ位置データの保有が不要となり、大容量のメモリを必要としない。よって、角度検出装置１のコストも低減できる。なお、本例では、各リンク３，７，８，９が揺動可能に連結されているが、首振り可能に連結されていても、同じ画像処理によって相対角度を検出できる。

また、本例では、角度検出装置１は、求めた相対角度θ１，θ２，θ３から他のリンクの姿勢を特定できるので、角度検出装置１が適用される建設機械Ｃのリンクの重心位置が求められ、ＺＭＰの演算に必要な角度情報が得られる。

さらに、本例では、可動部材Ｍが三つ以上のリンク３，７，８，９を有し、角度検出部５は、末端のリンク３から一番近いリンク７の相対角度θ１を求め、末端のリンク３から二番目以降の近いリンク８，９については、姿勢特定部６により特定された一つ手前のリンク７，８の姿勢に基づいて相対角度θ２，θ３を求めるようになっている。

このように角度検出装置１が構成されると、相対角度検出対象のリンク８より末端のリンク３側へ一つ手前の姿勢を特定して、末端のリンク３に近いリンク７から順に相対角度θ２を求められる。このように、他のリンク７，８，９について順にリンク８，９の相対角度θ２，θ３の検出と姿勢の特定を行えば、可動部材Ｍが多数のリンクを有する場合にあっても、全リンクの相対角度θ１，θ２，θ３を簡単に検出できるのである。

また、本例の角度検出装置１にあっては、角度検出部５がカメラ４で撮影した前記画像を処理して、他のリンク７，８，９のエッジを検出し、相対角度θ１，θ２，θ３を求める。よって、このように構成された角度検出装置１は、カメラ４で撮影した連続画像からではなく、一つの画像から相対角度θ１，θ２，θ３を検出できるので、時間をかけずにタイムリーに角度検出できる。

さらに、本例の角度検出装置１にあっては、可動部材Ｍが末端のリンクを建設機械Ｃの旋回体３とし、他のリンクを建設機械Ｃの旋回体３に回転自在に連結される作業装置Ｗとし、カメラ４が旋回体３のキャビン３ａ内に設置されるようになっている。このように相対角度検出に必要な画像を撮影するカメラ４がキャビン３ａ内に収容されるので、カメラ４が建設機械Ｃの外部環境から保護される。よって、本例の角度検出装置１にあっては、カメラ４が風雨に晒されず、土木工事作業中の石や土砂の飛来からも保護されるので、安定した相対角度検出が可能となる。

＜第二の実施の形態＞
第二の実施の形態の角度検出装置８０にあっては、図１３に示すように、角度検出部８１の構造および処理が第一の実施の形態の角度検出装置１の角度検出部５の構造及び処理と異なり、そのほかの構成及び処理は同一である。よって、以下では、構成の異なる角度検出部８１について詳しく説明し、同一の構成については詳しい説明を省略する。また、この角度検出部８１にあっても、建設機械Ｃに適用されており、カメラ４は旋回体３のキャビン３ａ内に設置されている。

角度検出部８１は、カメラ４が撮影した画像をグレースケールの画像に変換後に二値化処理して二値化画像を得る二値化処理部８２と、二値化画像の座標系を作業装置Ｗを真横から見た画像の座標系へ変換する射影変換部８３と、射影変換後の二値化画像から連結軸７ａ，８ａ，９ａが撮影されている領域を抽出する領域抽出部８４と、領域抽出部８４で抽出した連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂから旋回体３に対するブーム７の相対角度θ１、ブーム７に対するアーム８の相対角度θ２およびアーム８に対するアタッチメント９の相対角度θ３を求める角度演算部８５と備えて構成されている。なお、連結軸１０ａは、アタッチメント９をアーム８に対して回転駆動するレバー１０とアーム８の連結軸であり、連結軸１０ｂは、アタッチメント９とレバー１０の連結軸である。

まず、角度検出部８１は、カメラ４が撮影し、カメラ４のレンズ歪等による歪をキャリブレーション処理によって較正された画像は、エッジ画像取得部５１によって処理する。二値化処理部８２は、カメラ４が撮影した画像をたとえば２５６階調の輝度値で表現されるグレースケールの画像に変換し、さらに、画像を二値化処理して二値化画像を得る。

射影変換部８３は、二値化処理部８２の処理によって得た二値化画像の座標系を、作業装置Ｗを真横から見た側面画像の座標系へ変換する。この射影変換部８３によって処理すると、作業装置Ｗのブーム７、アーム８およびアタッチメント９の作動面を貫く方向となる真横から見た図１４に示した画像が得られる。

なお、射影変換部８３の射影変換処理では、実際に画像を得る必要はなく、変換後のエッジの座標情報を取得できればよい。なお、具体的には、射影変換部５２は、カメラ４の撮影した画像座標を射影変換行列を用いて処理するが、この座標変換行列は、予めカメラ４で作業装置Ｗを撮影して射影変換行列を予め求めておけばよい。

領域抽出部８４は、本例では連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂを予め決められた箇所としてこれらが撮影されている領域をたとえばＭＳＥＲ法（Ｍａｘｉｍａｌｌｙ Ｓｔａｂｌｅ Ｅｘｔｅｒｍａｌ Ｒｅｇｉｏｎｓ）などの特徴ベースマッチングによる領域抽出処理を行って抽出する。連結軸７ａ，８ａは、旋回体３とブーム７、ブーム７とアーム８の結合ピンであり、これらの特徴とマッチングする領域を抽出する。なお、ＭＳＥＲ法以外の領域抽出法を用いて、連結軸７ａ，８ａの領域を抽出してもよい。また、連結軸７ａは、旋回体３とブーム７との連結軸７ａであり、射影変換後における座標は既知であるので、カメラ４で連結軸７ａを撮影せずともよい。

連結軸８ａと連結軸７ａの距離Ｌ１は、図１４に示すように、連結軸７ａの座標はカメラ４の画像を射影変換すると定点であり不動であるから、ブーム７がどのような姿勢をとっても一定であり、設計値で有るから既知である。よって、連結軸８ａは、連結軸７ａの中心座標から距離Ｌ１を半径として円周上に存在するので、領域抽出の際に連結軸８ａの特徴に一致する領域であって、連結軸７ａからの距離がＬ１である領域は連結軸８ａが撮影されている領域となる。また、本例では、建設機械Ｃが走行中或いは旋回体３や作業装置Ｗが駆動中であると、建設機械Ｃが振動してカメラ４に振動が伝達される。すると、無振動でカメラ４が連結軸８ａを撮影した画像に対して振動中のカメラ４が連結軸８ａを撮影した場合の画像中の連結軸８ａの位置がオフセットされる場合がある。そこで、半径Ｌ１に対して閾値ΔＬ１を設定して、連結軸７ａの中心座標から距離が範囲Ｌ１±ΔＬ１内にあって連結軸８ａと一致する特徴を持つ画像領域を抽出する。そのため、閾値ΔＬ１で振動によるオフセット分を吸収してカメラ４が振動しても画像において連結軸８ａが写っている領域を正確に抽出できる。

連結軸８ａの中心座標から連結軸９ａまでの距離Ｌ２は既知である。連結軸９ａは、連結軸８ａから距離Ｌ２だけ離れた位置に存在するため、連結軸９ａの特徴が一致する領域のうち連結軸８ａを中心として半径Ｌ２の円周上にある領域が連結軸９ａの撮影されている領域となる。そこで、領域抽出部８４は、連結軸８ａの領域が抽出されると、連結軸８ａの領域の中心座標を求め、この中心座標から距離Ｌ２だけ離れた位置にあって連結軸９ａと一致する特徴を持つ領域を選択すれば連結軸９ａが撮影されている領域を抽出できる。より詳細には、半径Ｌ２に対して閾値ΔＬ２を設定して、連結軸８ａの中心座標から距離が範囲Ｌ２±ΔＬ２内にあって連結軸９ａと一致する特徴を持つ画像領域を抽出する。そのため、閾値ΔＬ２でカメラ４の振動によるオフセット分を吸収してカメラ４が振動しても画像において連結軸９ａが写っている領域を正確に抽出できる。さらに、領域抽出部８４は、連結軸９ａの領域抽出が済むと、続いて、連結軸１０ａ，１０ｂの撮影されている領域を抽出する。アーム８に対してアタッチメント９が姿勢を変えても、連結軸９ａの中心座標に対して連結軸１０ａ，１０ｂは、一定距離を保ったままである。よって、領域抽出部８４は、連結軸９ａの連結軸８ａに対する領域抽出と連結軸８ａの連結軸７ａに対する領域抽出と同様の手法で連結軸１０ａ，１０ｂの領域を抽出すればよい。この連結軸１０ａ，１０ｂの領域抽出にあっても、距離に対して閾値を設定してカメラ４の振動による連結軸１０ａ，１０ｂの位置のオフセットを吸収するようにすればよい。

つづいて、角度演算部８５は、ブーム７の回転中心である連結軸７ａと領域抽出部８４により抽出された箇所である連結軸８ａの中心座標を通る直線を求め、この直線の画像に対する傾きから旋回体３とブーム７の相対角度θ１を検出する。さらに、角度演算部８５は、アーム８の回転中心である連結軸８ａと領域抽出部８４により抽出された箇所である連結軸９ａの中心座標を通る直線を求め、この直線と連結軸７ａ，８ａの中心座標を通る直線とのなす角度を求めてアーム８とブーム７の相対角度θ２を検出する。そして、角度演算部８５は、アタッチメント９の回転中心である連結軸１０ａと領域抽出部８４により抽出された連結軸１０ｂの中心座標を通る直線を求め、この直線と連結軸８ａ，９ａの中心座標を通る直線とのなす角度θ３”を求める。この角度θ３”とアーム８とアタッチメント９の相対角度θ３は、一対一の関係にあり、予めこの関係をテーブル化しておき、角度θ３”からテーブルを利用すれば相対角度θ３を求められる。

なお、本例では、領域抽出部８４が、旋回体３とブーム７の回転軸である連結部７ａ、ブーム７とアーム８の回転軸である連結軸８ａ、アーム８とアタッチメント９の回転軸である連結軸９ａ、アーム８とレバー１０の回転軸である連結軸１０ａ、アタッチメント９とレバー１０の回転軸である連結軸１０ｂの領域を予め決められた箇所として抽出するので、一度に領域抽出可能である。

領域抽出部８４が連結軸８ａ，９ａ，１０ａ，１０ｂでなく、他所を予め決められた箇所として抽出する場合は、第一の実施の形態と同様に処理するようにすればよい。つまり、領域抽出部８４による領域抽出により連結軸７ａと連結軸７ａから一定距離はなれたブーム７の連結軸８ａ以外の部位の領域を抽出する。次に、連結軸７ａと連結軸７ａから一定距離はなれたブーム７の連結軸８ａ以外の部位の領域の座標から角度演算部８５が旋回体３とブーム７の相対角度θ１を求めて、姿勢特定部６でブーム７における連結軸８ａの中心座標を求める。連結軸７ａとブーム７の連結軸８ａの任意の部位を通る直線と画像の水平軸とのなす角度と旋回体３のブーム７の相対角度θ１とは一対一の関係を有しているので、この関係をテーブル化しておけば簡単に相対角度θ１を演算できる。連結軸８ａの中心座標が求めると、領域抽出部８４は、連結軸８ａとアーム８の任意の部位までの既知の距離とから前記部位の領域抽出を行えば、角度演算部８５で連結軸８ａと前記部位との直線を得てアーム８とブーム７のなす相対角度θ２を求められる。連結軸８ａとアーム８の任意の部位を通る直線と連結軸７ａとブーム７の任意の部位を通る直線のなす角度と、ブーム７とアーム８の相対角度θ２とは一対一の関係を有しているので、この関係をテーブル化しておけば簡単に相対角度θ２を演算できる。さらに、相対角度θ２が求まれば、姿勢特定部６が連結軸９ａ，１０ａの中心座標を求められ、領域抽出部８４は、連結軸１０ａから任意の距離はなれたレバー１０の任意の部位の領域を抽出でき、角度演算部８５は、連結軸１０ａとレバー１０の任意の部位とからアタッチメント９とアーム８の相対角度θ３を求められる。連結軸８ａとアーム８の任意の部位を通る直線とレバー１０の連結軸１０ａとレバー１０の任意の部位を通る直線のなす角度と、アーム８とアタッチメント９の相対角度θ３とは一対一の関係を有しているので、この関係をテーブル化しておけば簡単に相対角度θ３を演算できる。このようにすれば、領域抽出する部位を予め決めておけば、連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂ以外の部位を領域抽出する部位として設定しても相対角度θ１，θ２，θ３を求められ、各部の姿勢も特定できる。領域抽出する部位を連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂとする場合には、角度演算部８５と姿勢特定部６の処理を交えず、領域抽出部８４は単独の処理で連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂの領域抽出が可能である。

また、本例では、射影変換部８３により、画像の座標系を作業装置Ｗを真横から見た座標系へ変換しているが、射影変換部８３による座標変換を省略してもよい。射影変換部８３による座標変換を行わずとも、領域抽出部８４で抽出したいブーム７の任意の箇所のカメラ４によって撮影した画像内での連結軸７ａに対する軌跡は、カメラ４でブーム７を旋回体３に対して回転させて撮影すれば予め把握できる。よって、射影変換部８３によって座標変換しなくとも、ブーム７の任意の箇所の軌跡上に特徴点が一致する部分を領域抽出部８４で抽出すれば、得たい領域を抽出できる。この領域と連結軸７ａとを結ぶ直線が画像の水平軸となす角度は、ブーム７と旋回体３との相対角度θ１と一対一の関係を有しているので、角度演算部８５は、前記直線と水平軸とのなす角度から相対角度θ１を求められる。相対角度θ１が得られれば、カメラ４が撮影する画像中での連結軸８ａの座標が求められ、連結軸８ａの座標が分かれば、アーム８がブーム７に対して回転する場合においてアーム８の領域抽出部８４にて抽出したい箇所が画像中でどのような軌跡を描くのかが分かる。よって、領域抽出部８４は、アーム８の任意の箇所の領域を抽出可能である。このように、射影変換部８３による座標変換を省略しても、ブーム７と旋回体３との相対角度θ１を検出でき、相対角度θ１が検出できれば、アーム８とブーム７との相対角度θ２を検出できる。つまり、順を追って領域抽出と角度検出を順を追って行えば、相対角度θ１，θ２，θ３が求まるのである。

このように、角度検出部８１によって相対角度θ１，θ２，θ３が求まると、姿勢特定部６は、第一の実施の形態と同様に、ブーム７、アーム８およびアタッチメント９の姿勢の特定が可能である。

第二の実施の形態における角度検出装置８０のハードウェア資源の構成についても第一の実施の形態の角度検出装置１と同様に構成されており、角度検出装置８０の各部における構成は、ＣＰＵ６１の角度検出装置８０の処理を行うためアプリケーションプログラムの実行により実現できる。

以上、角度検出装置８０の構成について説明したが、以下、角度検出装置１の角度検出処理手順を図１５のフローチャートの一例に基づいて説明する。

まず、角度検出装置８０は、角度検出のため、まず、カメラ４で撮影範囲を撮影する（ステップＳ２１）。次に、カメラ４で撮影した画像に対して前述の二値化処理を行って二値化画像を得る（ステップＳ２２）。さらに、二値化画像の座標を射影変換を行う（ステップＳ２３）。

つづいて、射影変換後の二値化画像から予め決められた箇所である連結軸８ａ，９ａ，１０ａ，１０ｂの領域抽出を行って（ステップＳ２４）、相対角度θ１，θ２，θ３を求める（ステップＳ２５）。

最後に、得られた相対角度θ１，θ２，θ３からブーム７、アーム８およびアタッチメント９の姿勢を特定する（ステップＳ２６）。

以上、ステップＳ２１からステップＳ２６までの処理を所定周期で繰り返し行って、相対角度θ１，θ２，θ３の角度検出とブーム７、アーム８およびアタッチメント９の姿勢の特定を連続して行う。

なお、領域抽出する箇所が連結軸８ａ，９ａ，１０ａ，１０ｂでない場合、図１５に示すように、ステップＳ２２で二値化画像を得たのち、第一の実施の形態のフローチャートと同様に、ブーム７の予め決められた箇所を抽出して（ステップＳ３１）、旋回体３に対するブーム７の相対角度θ１を求めて（ステップＳ３２）、相対角度θ１から連結軸８ａの中心座標を求める（ステップＳ３３）。続いて、連結軸８ａの座標からアーム８の予め決められた箇所の領域を抽出して（ステップＳ３４）、ブーム７に対するアーム８の相対角度θ２を求める（ステップＳ３５）。そして、相対角度θ２から連結軸１０ａの中心座標を求めて（ステップＳ３６）、アタッチメント９の予め決められた箇所の領域を抽出して（ステップＳ３７）、アーム８に対するアタッチメント９の相対角度θ３を求め（ステップＳ３８）、相対角度θ３からアタッチメント９の先端の座標を求める（ステップＳ３９）。このようにすれば、領域抽出する箇所が連結軸８ａ，９ａ，１０ａ，１０ｂでない場合にも相対角度θ１，θ２，θ３の角度検出とブーム７、アーム８およびアタッチメント９の姿勢の特定を連続して行える。

このように構成された角度検出装置８０は、可動部材Ｍのうち末端のリンク（旋回体３）に取付けられるカメラ４で他のリンク（ブーム７、アーム８およびアタッチメント９）を撮影し、その撮影した画像から他のリンク７，８，９の相対角度θ１，θ２，θ３を求める。このように角度検出装置８０は、センサ類を用いないので、センサ類の故障の心配もなく、カメラ４の設置個所は自由度が高く安全な場所に設置できるので厳しい使用環境にも耐えられる。また、角度検出装置８０では、ブーム等の姿勢の特定に際してマーカー位置データとブーム等の座標を照合する必要がないので、膨大なマップ位置データの保有が不要となり、大容量のメモリを必要としない。よって、角度検出装置８０のコストも低減できる。なお、本例では、各リンク３，７，８，９が揺動可能に連結されているが、首振り可能に連結されていても、同じ画像処理によって相対角度を検出できる。

また、本例では、角度検出装置８０は、求めた相対角度θ１，θ２，θ３から他のリンクの姿勢を特定できるので、角度検出装置１が適用される建設機械Ｃのリンクの重心位置が求められ、ＺＭＰの演算に必要な角度情報が得られる。

角度検出装置８０は、各リンク７，８，９の回転軸である連結軸７ａ，８ａ，９ａ，１０ａ，１０ｂを予め決められた箇所として領域抽出すれば、各リンク７，８，９の姿勢を特定せずとも、相対角度θ１，θ２，θ３を求められる。なお、本例にあっても、可動部材Ｍが三つ以上のリンク３，７，８，９を有し、角度検出部５は、末端のリンク３から一番近いリンク７の相対角度θ１を求め、末端のリンク３から二番目以降の近いリンク８，９については、姿勢特定部６により特定された一つ手前のリンク７，８の姿勢に基づいて相対角度θ２，θ３を求められる。よって、この角度検出装置８０にあっても、第一の実施の形態の角度検出装置１と同様に、可動部材Ｍが多数のリンクを有する場合にあっても、全リンクの相対角度θ１，θ２，θ３を簡単に検出できる。

また、本例の角度検出装置８０にあっては、角度検出部８１がカメラ４で撮影した前記画像を処理して、カメラ４が撮影した画像から他のリンク７，８，９の予め決められた箇所を抽出し、箇所と他のリンク７，８，９における回転中心７ａ，８ａ，１０ａとを結ぶ直線を求め、この直線に基づいて相対角度θ１，θ２，θ３を求める。よって、このように構成された角度検出装置８０は、カメラ４で撮影した連続画像からではなく、一つの画像から相対角度θ１，θ２，θ３を検出できるので、時間をかけずにタイムリーに角度検出できる。

さらに、本例の角度検出装置８０にあっても、可動部材Ｍが末端のリンクを建設機械Ｃの旋回体３とし、他のリンクを建設機械Ｃの旋回体３に回転自在に連結される作業装置Ｗとし、カメラ４が旋回体３のキャビン３ａ内に設置されるようになっている。このように相対角度検出に必要な画像を撮影するカメラ４がキャビン３ａ内に収容されるので、カメラ４が建設機械Ｃの外部環境から保護される。よって、本例の角度検出装置８０にあっては、カメラ４が風雨に晒されず、土木工事作業中の石や土砂の飛来からも保護されるので、安定した相対角度検出が可能となる。

以上で、本発明の実施の形態についての説明を終えるが、本発明の範囲は図示されまたは説明された詳細そのものには限定されないのは勿論である。

１，８０・・・角度検出装置、３・・・旋回体（末端のリンク）、３ａ・・・キャビン、４・・・カメラ、５，８１・・・角度検出部、６・・・姿勢特定部、７・・・ブーム（他のリンク）、８・・・アーム（他のリンク）、９・・・アタッチメント（他のリンク）、Ｃ・・・建設機械、Ｍ・・・可動部材、Ｗ・・・作業装置、

Claims (4)

1. 相対回転可能に直列に連結される複数のリンクを有する可動部材のうち末端のリンクに取り付けられて前記末端のリンク以外の他のリンク或いは全リンクを撮影可能なカメラと、
   前記カメラが撮影した画像に基づいて隣り合う前記リンク同士の相対角度を検出する角度検出部とを備え、
   前記角度検出部は、前記カメラで撮影した前記画像を処理して、前記他のリンクのエッジを検出し、前記相対角度を求める
   ことを特徴とする角度検出装置。
2. 前記角度検出部が検出した前記相対角度に基づいて、前記末端のリンクに対する前記他のリンクの姿勢を特定する姿勢特定部を備えた
   ことを特徴とする請求項１に記載の角度検出装置。
3. 前記可動部材は、三つ以上のリンクを有し、
   前記角度検出部は、
   前記末端のリンクから一番近いリンクの前記相対角度を求めるとともに、
   前記末端のリンクから二番目以降の近いリンクについては、前記姿勢特定部により特定された一つ手前のリンクの前記姿勢に基づいて前記相対角度を求める
   ことを特徴とする請求項２に記載の角度検出装置。
4. 前記可動部材は、前記末端のリンクを建設機械の旋回体とし、前記他のリンクを前記建設機械の前記旋回体に回転自在に連結される作業装置とし、
   前記カメラは、前記旋回体のキャビン内に設置される
   ことを特徴とする請求項１から３のいずれか一項に記載の角度検出装置。

Images