(57)【要約】      （修正有）【課題】計測手段の位置及び姿勢の変化後の位置姿勢算出式を算出することができる目標物位置姿勢算出装置を提供する。【解決手段】目標物位置姿勢算出装置に係る情報処理装置は、第１位置における移動装置の位置姿勢に関する情報及び第２測定手段により取得されたターゲット部の位置姿勢に関する情報並びに測定器により取得されたワールド座標系上におけるターゲット部の位置姿勢に関する情報とを式に代入し、第２位置における移動装置の位置姿勢に関する情報及び第２測定手段により取得されたターゲット部の位置姿勢に関する情報並びに測定器により取得されたワールド座標系上におけるターゲット部の位置姿勢に関する情報とを式に代入し、第１位置の式と、第２位置の式とから、ワールド変換行列を算出する。【選択図】図１
2014106212.tif 000002
【特許請求の範囲】
【請求項１】
位置及び姿勢を算出する基準であるワールド座標系原点及びワールド座標が設定されて、前記ワールド座標における目標物の位置及び姿勢を算出する目標物位置姿勢算出装置であって、
移動装置に設けられて前記移動装置の位置姿勢に関する情報を取得する第１計測手段と、
前記移動装置に設けられて前記目標物のターゲット部の位置姿勢に関する情報を取得する第２計測手段と、
位置姿勢算出式として設置時の計測により求められたワールド座標に変換するワールド変換行列を記憶し、前記第１計測手段からの位置姿勢に関する情報と、前記第２計測手段からの位置姿勢に関する情報とから、ワールド座標系上の前記ターゲット部の位置及び姿勢を算出する情報処理装置と、を備え、
前記情報処理装置は、
下記式（１）を記憶し、
前記目標物が第１位置にある場合における前記第１測定手段により取得された移動装置の位置姿勢に関する情報と、前記目標物が第１位置にある場合における前記第２測定手段により取得されたターゲット部の位置姿勢に関する情報と、前記目標物が第１位置にある場合における測定器により取得されたワールド座標系上における前記ターゲット部の位置姿勢に関する情報とを前記式（１）に代入し、前記第１位置における式（１）を求め、
前記目標物が第２位置にある場合における前記第１測定手段により取得された移動装置の位置姿勢に関する情報と、前記目標物が第２位置にある場合における前記第２測定手段により取得されたターゲット部の位置姿勢に関する情報と、前記目標物が第２位置にある場合における測定器により取得されたワールド座標系上における前記ターゲット部の位置姿勢に関する情報とを前記式（１）に代入し、前記第２位置における式（１）を求め、
前記第１位置の式（１）と、前記第２位置の前記式（１）とから、前記式（１）に仮定行列Ａ及び仮定行列Ｃを算出し、
前記仮定行列Ａ及び仮定行列Ｃが算出された前記式（１）により、前記ワールド座標における目標物の位置及び姿勢を算出することを特徴とする目標物位置姿勢算出装置。
【数１】
2014106212.tif 000011
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、目標物位置姿勢算出装置に関する。
【背景技術】
【０００２】
従来から車両の組み立てでは、生産ラインに沿って搬送される車体を停止させることなく、車体に追従しながらワークの組み付けが行われている。
また、上記するようなワークを組み付け作業を行う組付作業システムには、下記特許文献１に開示されるように、被作業対象物と並走する走行装置に設けられたエンコーダ及びセンサと、エンコーダ及びセンサが取得した情報を演算し、車体の位置と姿勢を算出する情報処理装置とを備えた目標物位置姿勢算出装置が設けられている。
なお、センサは、搬送される車体に揺れが生じることがあり、その車体の位置及び姿勢を検出するために設けられている。以下、車体において、センサにより計測される部位をターゲット部と呼ぶ。
【０００３】
ところで、図１に示すように、生産ライン及び組付作業システムを含む全体システムでは、位置及び姿勢を計測する際の基準点として、様々な原点が設定され、それぞれの原点毎に座標系が存在している。
まず、生産ラインや目標物位置姿勢算出装置など複数のシステムが設けられている場合、全体システムの共通する基準点（以下、「ワールド座標系原点」という。）が設定されている。
【０００４】
また、図１に示すように、走行装置が走行する走行軸線上の始点に、ワールド座標系原点とは別個の原点（走行軸座標系原点）が設定されている。
また、走行装置には、走行軸座標系原点とは別個の原点（以下、「走行装置座標系原点」という）が設定される。なお、走行装置に設けられたエンコーダが走行装置座標系における位置及び姿勢に関する情報を取得している。
さらに、センサには、走行装置座標系原点とは、別途の原点基準点であるセンサ座標系原点が設けられている。
【０００５】
そして、それぞれの座標毎に得られた位置情報及び姿勢情報をワールド座標系に変換するために、目標物位置姿勢算出装置の情報処理装置には、位置姿勢算出式として、図２に示すようなワールド変換行列が記憶されている。以下、図２に示すワールド変換行列について説明する。
【０００６】
図２に示すワールド変換行列における行列Ｄは、行列Ｄの（ｂ）にセンサにより得られた位置姿勢情報を代入することで、センサ座標系上におけるターゲット部の位置情報及び姿勢情報を算出するための行列である。
【０００７】
図２に示すワールド変換行列における行列Ｃは、４次の行列Ｄの前に設定されて、センサ座標系上におけるターゲット部の位置情報及び姿勢情報を、走行装置座標系上の位置情報及び姿勢情報に変換するための行列である。
【０００８】
図２に示すワールド変換行列における行列Ｂは、行列Ｂの（ａ）にエンコーダにより得られた位置姿勢情報を代入することで、走行軸座標系上における走行装置の位置情報及び姿勢情報を算出する。そして、行列Ｂは、４次の行列Ｃの前に設定されているため、走行装置座標系上におけるターゲット部の位置情報及び姿勢情報を、走行軸座標系上の位置情報及び姿勢情報に変換するための行列である。
【０００９】
つぎに、図２に示すワールド変換行列における行列Ａは、行列Ｂの前に設定されて、走行軸座標系上におけるターゲット部の位置情報及び姿勢情報を、ワールド座標系上の位置情報及び姿勢情報に変換するための行列である。
以上、目標物位置姿勢算出装置では、情報処理部に記憶されるワールド変換行列により、ターゲット部の位置情報及び姿勢情報を算出していた。
【００１０】
そして、図２に示すワールド変換行列における行列Ｂの（ａ）と４次の行列Ｄの（ｂ）を除く部分の行列は、各種装置が設置された時に実際に計測された値を基に算出されている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２０００−２５６６４号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
しかしながら、例えば、車体の組み立て作業中に、センサを支持する支持台が接触により曲折した場合など、目標物位置姿勢算出装置の構成が設置された組み立て作業システムが設置時の位置及び姿勢と異なる状態となった場合、図２に示すワールド変換行列によって、ワールド座標系上の正確な位置情報及び姿勢情報を取得することができなかった。
【００１３】
ここで、エンコーダ及びセンサの位置及び姿勢をワールド変換行列に合致させるように、つまり、変形前である設置時の位置及び姿勢となるように修理するのは、非常に精度が要求される作業であるため困難であった。
一方で、改めて、曲折後のセンサ座標系原点の位置と姿勢とを計測し直すことで、ワールド変換行列を補正することも考えられるが、補正できるのは位置データのみであり、姿勢データに関しては、補正できなかった。
以上から、エンコーダ及びセンサの計測手段の位置及び姿勢が設置時の位置及び姿勢と異なる状態となった場合において、ワールド変換行列（位置姿勢算出式）を新たに算出できることが望まれていた。
【００１４】
そこで、本発明は、前記する背景に鑑みて創案された発明であって、計測手段の位置及び姿勢の変化後の位置姿勢算出式を算出することができる目標物位置姿勢算出装置を提供することを課題とする。
【課題を解決するための手段】
【００１５】
前記課題を解決するために、本願発明に係る目標物位置姿勢算出装置は、位置及び姿勢を算出する基準であるワールド座標系原点及びワールド座標が設定されて、前記ワールド座標における目標物の位置及び姿勢を算出する目標物位置姿勢算出装置であって、移動装置に設けられて前記移動装置の位置姿勢に関する情報を取得する第１計測手段と、前記移動装置に設けられて前記目標物のターゲット部の位置姿勢に関する情報を取得する第２計測手段と、位置姿勢算出式として前記システムの設置時の計測により求められたワールド座標に変換するワールド変換行列を記憶し、前記エンコーダからの位置姿勢に関する情報と、前記センサからの位置姿勢に関する情報とから、ワールド座標系上の前記ターゲット部の位置及び姿勢を算出する情報処理装置と、を備え、前記情報処理装置は、下記式（１）を記憶し、第１位置における前記ターゲット部の位置及び姿勢を算出した場合、前記第１測定手段により取得された移動装置の位置姿勢に関する情報と、前記第２測定手段により取得されたターゲット部の位置姿勢に関する情報と、測定器により取得されたワールド座標系上における前記ターゲット部の位置姿勢に関する情報とを前記式（１）に代入し、前記第１位置における式（１）を求め、前記第１位置から移動させた第２位置における前記ターゲット部の位置及び姿勢を算出した場合、前記第１測定手段により取得された移動装置の位置姿勢に関する情報と、前記第２測定手段により取得されたターゲット部の位置姿勢に関する情報と、測定器により取得されたワールド座標系上における前記ターゲット部の位置姿勢に関する情報とを前記式（１）に代入し、前記第２位置における式（１）を求め、前記第１位置の式（１）と、前記第２位置の前記式（１）とから、前記式（１）に仮定行列Ａ及び仮定行列Ｃを算出し、前記仮定行列Ａ及び仮定行列Ｃが算出された前記式（１）により、前記ワールド座標における目標物の位置及び姿勢を算出することを特徴とする。
【００１６】
【数１】
2014106212.tif 000003
【００１７】
前記する発明によれば、第１計測手段及び第２計測手段が設置時の位置及び姿勢が変化したとしても、変化後の位置姿勢算出式を算出することができるようになっている。そのため、第１計測手段及び第２計測手段の位置及び姿勢を、変形前である設置時の状態となるように修理することが不要となり、複雑な作業を回避できる。
【発明の効果】
【００１８】
本発明によれば、計測手段の位置及び姿勢の変化後の位置姿勢算出式を算出することができる目標物位置姿勢算出装置を提供することができる。
【図面の簡単な説明】
【００１９】
【図１】従来技術の全体システムで設定される各原点及び各座標系の概略を示す図である。
【図２】従来技術における目標物位置姿勢算出装置の情報処理装置に記憶される位置姿勢算出式であるワールド変換行列を示す図である。
【図３】実施形態に係る全体システムで設定される各原点及び各座標系の概略を示す図である。
【図４】実施形態に係る位置姿勢算出式を算出する際に、仮定して立てられるワールド変換行列を示す図である。
【発明を実施するための形態】
【００２０】
つぎに、本発明の実施形態に係る目標物位置姿勢算出装置について説明する。
【００２１】
図３に示すように、被作業対象物（目標物）２１を搬送する搬送する生産ライン２０と、被作業対象物２１と並走可能な走行装置１１を有する組付作業システム１０と、が同一敷地内に配置されている。そして、生産ライン２０に沿って搬送される被作業対象物２１を停止させることなく、走行装置１１を並走させながら、ワーク（不図示）の組み付けが行われている。
また、生産ライン２０と組付作業システム１０とが配置された敷地には、共通した位置、姿勢を算出するための基準点としてワールド座標系原点及びワールド座標が設定されている。
【００２２】
実施形態に係る目標物位置姿勢算出装置１は、エンコーダ（第１計測手段）１２と、センサ（第２計測手段）１３と、図示しない情報処理装置とを備えている。
エンコーダ１２は、走行装置１１に搭載されて、走行装置１１の位置及び姿勢に関する情報を取得するためのものである。
また、センサ１３は、走行装置１１に固定されて、被作業対象物２１のターゲット部２２にレーザ光を照射するとともに、その反射光を受光することで、センサ１３から見た場合のターゲット部２２に関する位置姿勢に関する情報を取得するためのものである。
【００２３】
情報処理装置は、エンコーダ１２により得られた走行装置１１の位置姿勢に関する情報と、センサ１３から得られた位置姿勢に関する情報とに基づき、ターゲット部２２の位置姿勢を算出する位置姿勢算出式、つまり、ワールド変換行列式（図２参照）を記憶している。
【００２４】
ここで、走行装置１１が走行軸上の始点に走行軸座標系原点が設定されており、エンコーダ１２により得られた走行装置１１の位置姿勢に関する情報を図２に示す行列Ｂの（ａ）に代入することで、走行軸座標系における走行装置１１の位置情報及び姿勢情報を取得することができる。
【００２５】
また、走行装置１１の所定部位に走行装置座標系原点が設定されている。
さらに、センサ１３の所定位置にセンサ座標系原点が設けられており、センサ１３により得られたターゲット部２２の位置姿勢に関する情報を代入することで、センサ座標系におけるターゲット部２２の位置情報及び姿勢情報を取得することができる。
なお、走行装置１１の所定位置については特に限定されない。また、センサ１３の所定位置は、センサ１３が取得する情報の基準点である。
【００２６】
また、情報処理装置は、例えば、作動中に走行装置１１が被作業対象物２１に接触したり、停止中に作業者が走行装置１１に接触したりし、組付作業システム１０の走行装置１１が変形して、エンコーダ１２またはセンサ１３が設置時と異なる位置、姿勢となった場合、新しいワールド変換行列式（位置姿勢算出式）を算出する機能を有している。以下、新しいワールド変換行列式（位置姿勢算出式）を算出する機能について説明する。
【００２７】
まず、情報処理装置は、図４に示すようなワールド変換行列式を記憶している。
このワールド変換行列式は、走行装置１１の変形後のターゲット部２２の位置姿勢の位置姿勢算出式であって、前から順に、４×４次の仮定行列Ａと、１×４次の行列Ｂと、４×４次の仮定行列Ｃと、１×４次の行列Ｄと、４×４次の行列Ｅと、を含んでいる。
【００２８】
１×４次の行列Ｄは、センサ１３から得られたターゲット部２２に関する位置姿勢に関する情報が入力される行列である。
また、４×４次の仮定行列Ｃは、センサ１３から得られたターゲット部２２に関する位置姿勢に関する情報を、走行装置座標系における位置及び姿勢情報に変換する仮定式であって、つまり、図２に示すワールド変換行列式において、行列Ｃ及び行列Ｄ（（ｂ）の部分除く）に相当するものである。そして、行列Ｄの前に配置されて、センサ１３から得られたターゲット部２２に関する位置姿勢に関する情報を、走行装置座標上の位置情報及び姿勢情報への変換を行う。
【００２９】
１×４次の行列Ｂは、エンコーダ１２から得られた走行装置１１の位置姿勢に関する情報が入力される行列である。
また、４×４次の仮定行列Ａは、エンコーダ１２から得られた走行装置１１の位置姿勢に関する情報を、ワールド座標系に変換する仮定式であって、つまり、図２に示すワールド変換行列式において、行列Ａ及び行列Ｂ（（ａ）の部分除く）に相当するものである。
また、４×４次の仮定行列Ａは、行列Ｂの前に配置されて、エンコーダ１２から得られた走行装置１１の位置姿勢に関する情報を、ワールド座標上の位置情報及び姿勢情報への変換を行う。さらに、４×４次の仮定行列Ａは、仮定行列Ｃの前に配置されているため、仮定式Ｃと行列Ｄとから得られた走行装置座標系におけるターゲット部２２の位置及び姿勢をワールド座標系への変換を行う。
【００３０】
そして、情報処理装置が走行装置１１の変形後におけるワールド変換行列式（図４参照）を算出できるようにため、作業者が以下のように操作して、上表処理装置に情報を入力させる。
【００３１】
まず、被作業対象物２１を第１位置に移動させるとともに、目標物位置姿勢算出装置１によりターゲット部２２の位置及び姿勢の算出を行わせて、第１位置におけるエンコーダ１２により走行装置１１の位置姿勢に関する情報と、第１位置におけるセンサ１３によりターゲット部２２の位置姿勢に関する情報とを取得する。
これによれば、図４に示すようなワールド変換行列式の行列Ｂと行列Ｄに、エンコーダ１２から得られた走行装置１１の位置姿勢に関する情報と、センサ１３により得られたターゲット部２２の位置姿勢に関する情報とを取得する。
【００３２】
また、目標物位置姿勢算出装置１は、別個の測定器（不図示）によりワールド座標系におけるターゲット部２２の位置及び姿勢を算出するとともに、得られたターゲット部２２の位置情報及び姿勢情報に対応する４×４次の行列Ｈを求める。
そして、目標物位置姿勢算出装置１の情報処理装置に測定器によるターゲット部２２の位置情報及び姿勢情報に対応する４×４次の行列Ｈを入力する。
【００３３】
以上より、情報処理装置は、下記式（２）に示すような、被作業対象物２１が第１位置にある場合におけるワールド変換行列を得ることができる。
【００３４】
【数２】
2014106212.tif 000004
【００３５】
つぎに、被作業対象物２１を第２位置に移動させるとともに、目標物位置姿勢算出装置１によりターゲット部２２の位置及び姿勢の算出を行わせて、第２位置におけるエンコーダ１２により走行装置１１の位置姿勢に関する情報と、第２位置におけるセンサ１３によりターゲット部２２の位置姿勢に関する情報とを取得する。
これにより、図４に示すようなワールド変換行列式の行列Ｂと行列Ｄに、エンコーダ１２から得られた走行装置１１の位置姿勢に関する情報と、センサ１３により得られたターゲット部２２の位置姿勢に関する情報とを取得する。
【００３６】
また、目標物位置姿勢算出装置１とは、測定器（不図示）によりワールド座標系におけるターゲット部２２の位置及び姿勢を算出するとともに、得られたターゲット部２２の位置情報及び姿勢情報に対応する４×４次の行列Ｋを求める。そして、目標物位置姿勢算出装置１の情報処理装置に測定器によるターゲット部２２の位置情報及び姿勢情報に対応する４×４次の行列Ｋを入力する。
【００３７】
以上より、情報処理装置は、下記式（３）に示すような、被作業対象物２１が第２位置にある場合におけるワールド変換行列を得ることができる。
【００３８】
【数３】
2014106212.tif 000005
【００３９】
以上の作業後により式（２）及び式（３）を取得後に、情報処理装置により演算させて、仮定行列Ａ及び仮定行列Ｃを算出させる。以下、情報処理装置による演算過程を説明する。まず、式（３）を整理すると、下記式（４）が求められる。
【００４０】
【数４】
2014106212.tif 000006
【００４１】
そして、式（４）を式（２）に代入して下記式（５）が求められる。
【００４２】
【数５】
2014106212.tif 000007
【００４３】
ここで式（５）を簡単にするために、Ｋ・Ｊ−１＝Ｌ、Ｉ−１・Ｆ＝Ｍと置いて、式（６）に代入すると下記式（６）になる。
なお、Ｋ・Ｊ−１において、Ｋは、測定器により求められた値であるとともに、Ｊは、センサ１３により求められた値であるため、Ｌは既知の値である。同様に、Ｉ−１・Ｆにおいて、Ｉ、Ｆともに、エンコーダ１２により求められた値であるため、Ｍも既知の値である。
【００４４】
【数６】
2014106212.tif 000008
【００４５】
つぎに、式（６）を整理すると、下記式（７）となる。
【００４６】
【数７】
2014106212.tif 000009
【００４７】
また、式を簡単にするため、Ｃ−１＝Ｏ、Ｇ−１＝Ｐと置くと、下記式（８）となる。
なお、Ｇは、センサ１３により求められた値であるため、Ｐは既知の値である。
【００４８】
【数８】
2014106212.tif 000010
【００４９】
式（８）において、Ｌ、Ｍ、Ｈ、Ｐは、エンコーダ１２、センサ１３、測定器により算出された値である。よって、エンコーダ１２、センサ１３、別個の測定器により算出された既知の値を式（８）に代入するとともに、式（８）の式を展開して１２元の連立方程式とする。
そして、この１２元の連立方程式を解くことで、Ｏの４×４次の行列が求めることができる。よって、４×４次の行列Ｏの逆行列を求めることで、行列Ｃが算出される。
なお、上記した方法により、行列Ａも算出することができ、図４に示すワールド変換行列、つまり、走行装置１１の変形後における位置姿勢算出式を算出することができる。
【００５０】
そして、情報処理装置は、この古い位置姿勢算出式に代えて、新しい位置姿勢算出式を記憶するとともに、新しいワールド変換行列式（位置姿勢算出式）により、ターゲット部２１の位置及び姿勢を算出するようになっている。
【００５１】
以上、目標物位置姿勢算出装置１によれば、目標物位置姿勢算出装置１の構成（エンコーダ１２及びセンサ１３）が設けられた走行装置１１等が変形した後の位置姿勢算出式を算出することができる。
そのため、計測手段が設置時の位置及び姿勢と異なる状態となっても、その変化後状態に併せて、目標物の位置及び姿勢を算出することができ、エンコーダ１２及びセンサ１３の位置及び姿勢を、変形前つまり設計時と同じように修理することが不要とすることができ、複雑な作業を回避することが可能である。
【符号の説明】
【００５２】
１      目標物位置姿勢算出装置
１０    組付作業システム
１１    走行装置（移動装置）
１２    エンコーダ（第１計測手段）
１３    センサ（第２計測手段）
２０    生産ライン
２１    被作業対象物
２２    ターゲット部
【図１】
2014106212.tif 000012
【図２】
2014106212.tif 000013
【図３】
2014106212.tif 000014
【図４】
2014106212.tif 000015