WO2020031659A1 - 位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法 - Google Patents

位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法 Download PDF

Info

Publication number
WO2020031659A1
WO2020031659A1 PCT/JP2019/028458 JP2019028458W WO2020031659A1 WO 2020031659 A1 WO2020031659 A1 WO 2020031659A1 JP 2019028458 W JP2019028458 W JP 2019028458W WO 2020031659 A1 WO2020031659 A1 WO 2020031659A1
Authority
WO
WIPO (PCT)
Prior art keywords
image
orientation
measurement target
marker
information calculation
Prior art date
Application number
PCT/JP2019/028458
Other languages
English (en)
French (fr)
Inventor
邦裕 尾形
田中　秀幸
吉央 松本
Original Assignee
国立研究開発法人産業技術総合研究所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 国立研究開発法人産業技術総合研究所 filed Critical 国立研究開発法人産業技術総合研究所
Publication of WO2020031659A1 publication Critical patent/WO2020031659A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant

Definitions

  • the present invention relates to a technique for estimating the position and orientation of a measurement target.
  • Patent Literature 1 As described in Patent Literature 1 and Non-Patent Literature 1 below, techniques for measuring the position and orientation of a person using image information and inertia information have been developed.
  • Patent Document 1 since a mechanism for measuring the trajectory of a person's whole body movement and movement is complicated, it takes a great deal of time and cost to prepare in advance. There's a problem.
  • the present invention has been made to solve such a problem, and an object of the present invention is to provide a system, an apparatus, and a method that can easily and accurately measure the position and orientation of a measurement target.
  • the present invention is a position and orientation estimation system for estimating the position and orientation of a measurement target, wherein a marker and an inertial sensor for measuring the position and orientation of the measurement target are joined.
  • a device attached to the measurement object an imaging unit for imaging the measurement object, a determination unit for determining whether or not an image obtained by imaging includes a captured image of the device, the image If the determination unit determines that the captured image of the device is included in the marker information calculation unit that calculates at least one of the position and orientation of the measurement target from the captured image of the marker, and If the determination unit determines that the captured image of the device is not included in the image, the position and orientation of the measurement target are reduced based on the data acquired by the inertial sensor.
  • Providing the position and orientation estimation system having a position and orientation estimation means including an inertia information calculation unit for calculating the one and also.
  • the present invention provides a method in which a marker for measuring the position and orientation of a measurement target is joined to an inertial sensor, and a captured image of a device attached to the measurement target is included in the image.
  • Determining means for determining whether or not the captured image of the device is present in the image, and marker information calculating means for calculating at least one of the position and orientation of the measurement target from the captured image of the marker when the captured image of the device is present in the image.
  • a position and orientation estimator including an inertia information calculating unit that calculates at least one of a position and an orientation of a measurement target based on data acquired by the inertia sensor when a captured image of the device does not exist in the image; Provide equipment.
  • the present invention provides a method in which a marker for measuring the position and orientation of a measurement target is joined to an inertial sensor, and an image of a device attached to the measurement target exists in a captured image.
  • the second step if it is determined that the image of the device does not exist in the first step, at least one of the position and orientation of the measurement target is calculated based on data acquired by the inertial sensor.
  • a position and orientation estimation method including three steps is provided.
  • the position and orientation of a measurement target can be measured easily and with high accuracy.
  • FIG. 2 is a plan view illustrating a configuration example of a marker M illustrated in FIG. 1.
  • FIG. 2 is a block diagram illustrating a configuration of a position and orientation estimation device 10 that is a component of the position and orientation estimation system.
  • 5 is a flowchart illustrating a position and orientation estimation method according to the embodiment of the present invention. 5 is a flowchart showing a method for making a determination in step S1 of FIG.
  • FIG. 5 is a first diagram illustrating a method for estimating the position and orientation of a measurement target in step S3 of FIG. 4.
  • FIG. 5 is a second diagram for describing a method for estimating the position and orientation of the measurement target in step S3 of FIG. 4.
  • FIG. 5 is a third diagram for describing a method of estimating the position and orientation of the measurement target in step S3 of FIG. 4.
  • a marker disclosed in International Publication WO / 2016/024555 is attached to an object to be measured, and the position and orientation of the object to be measured are measured with high accuracy by imaging the marker with a camera. It has become possible.
  • the posture of the measurement target can be estimated with high accuracy, but a large error occurs because the position of the measurement target is integrated twice.
  • the position and orientation of the measurement object can be easily measured with high accuracy and robustness.
  • FIG. 1 is a diagram for explaining a method of estimating the position and orientation of a measurement target (not shown; the same applies hereinafter) using the position and orientation estimation system according to the embodiment of the present invention.
  • FIG. 2 is a plan view showing a configuration example of the marker M shown in FIG.
  • FIG. 3 is a block diagram showing a configuration of a position and orientation estimation device 10 which is a component of the position and orientation estimation system.
  • the marker M shown in FIG. 2 is a marker disclosed in the above-mentioned International Publication WO / 2016/024555, and includes a rectangular AR marker Mb including a white two-dimensional pattern code on a black background, and an AR marker Mb. And four moiré pattern forming portions VMP1x, VMP1y, VMP2x, VMP2y arranged in parallel with each side of the AR marker Mb on the same plane as the AR marker Mb, and arranged at the four corners of the marker M corresponding to the vertices of the AR marker Mb. It is provided with reference points RP1 to RP4 composed of four black circles.
  • a marker M for measuring the position and orientation of a measurement target as described above and an inertial sensor S are joined.
  • a marker unit configured to calculate at least one of the position and orientation of the measurement target from the captured image of the marker when the determination unit determines that the captured image of the device is included in the image;
  • the calculation unit 41 and the determination unit 3 determine that the captured image of the device 1 is not included in the image, the position and the shape of the measurement target based on the data acquired by the inertial sensor S Of and a position and orientation estimation apparatus 10 including the inertia information calculation section 42 for calculating at least one.
  • the position and orientation estimation device 10 includes a terminal 6 and a bus 7 connected to the terminal 6, and the determination unit 3 and a calculation unit 4 including a marker information calculation unit 41 and an inertia information calculation unit 42 include a bus 7 It is connected to the. Further, the position and orientation estimation device 10 includes a storage unit 5 connected to the bus 7.
  • FIG. 4 is a flowchart showing a position and orientation estimation method according to the embodiment of the present invention.
  • the position and orientation estimation method according to the embodiment of the present invention will be described in detail with reference to FIG. 4 and an example in which the position and orientation estimation system illustrated in FIGS. 1 to 3 is used.
  • step S ⁇ b> 1 the determination unit 3 joins the marker M for measuring the position and orientation of the measurement target and the inertial sensor S, and the image of the device 1 attached to the measurement target is captured by the camera 2. It is determined whether or not it exists. Note that whether or not the image of the device 1 exists in the image captured by the camera 2 means whether or not the device 1 exists within the imaging range A of the camera 2 shown in FIG.
  • step S10 the image captured by the camera 2 is binarized into two colors of white and black.
  • step S11 it is determined whether or not the two-dimensional black pattern, that is, the image of the AR marker Mb shown in FIG. 2 is included in the image binarized as described above. When it is determined that the image of the AR marker Mb is not included, it is determined that the captured image of the device 1 does not exist, while when it is determined that the image of the AR marker Mb is included, Proceed to step S12.
  • step S12 it is determined whether or not the four reference points RP1 to RP4 around the two-dimensional pattern (AR marker Mb) and the image of the two-dimensional pattern code in the AR marker Mb are included.
  • the captured image of the device 1 does not exist.
  • the captured image of the device 1 is not included. Is determined to exist.
  • step S2 shown in FIG. 4 when it is determined in step S1 that an image of the device 1 exists, at least one of the position and orientation of the measurement target is calculated from the image of the marker M.
  • the position and orientation of the measurement target can be calculated by using the images of the reference points RP1 to RP4 and the moire pattern forming units VMP1x, VMP1y, VMP2x, and VMP2y.
  • VMP1x, VMP1y, VMP2x, and VMP2y are disclosed in the above-mentioned International Publication WO / 2016/024555 and the like.
  • step S3 if it is determined in step S1 that no image of the device 1 exists, at least one of the position and orientation of the measurement target is calculated based on the data acquired by the inertial sensor S.
  • the position and orientation of the measurement target are measured using the image captured by the camera 2 as described above, while the device 1 is When the measurement object is outside the imaging range A of the camera 2, the position and orientation of the measurement target are measured using the inertial sensor S.
  • the method of calculating the orientation of the measurement target using the inertial sensor S has been widely put to practical use, but when calculating the position of the measurement target, a large error occurs. Therefore, this error is eliminated by using boundary conditions inside and outside the measurement range using the camera 2.
  • the device 1 attached to the measurement target goes out of the imaging range A across the boundary line BL, and crosses the boundary BL again into the imaging range A.
  • the position information obtained by imaging the marker M with the camera 2 is used immediately before going out of the imaging range A and immediately after returning to the inside of the imaging range A.
  • an error generated in the inertial sensor S is corrected by applying a time-series filtering technique.
  • the inertia information calculation unit 42 performs an integration calculation using a Kalman filter from the start point SP that is the position P of the device 1 calculated by the marker information calculation unit 41 immediately before the device 1 goes out of the imaging range A of the camera 2.
  • the Kalman filter updates the observation of the state vector x using the observation value y of the acceleration, and updates the prediction based on the estimated value obtained by the observation update. Then, the obtained prediction update result is used for calculating the next observation update.
  • the discrete time is t
  • the position is p t
  • the sampling time is dt
  • the system noise is w t
  • the observation noise is v t
  • the estimation error covariance matrix is P
  • the Kalman gain is K
  • the system noise is covariance Q t of the covariance of the observation noise and R t
  • the state vector x t at time t the observed value y at time t
  • the matrix F, H, G respectively represented by the following equations.
  • the linear probability model is expressed by the following equation (2).
  • the prediction update and the observation update in the Kalman filter are calculated by the following equations (3) and (4), respectively. That is, the estimated state vector value x hat t / t and the estimated error covariance matrix P t / t are stored in the storage unit 5 via the bus 7.
  • the estimated position of the measurement object between the start point SP and the end point EP is calculated by, for example, a smoothing operation according to the following equation (5), and the result is shown in a graph R1.
  • the estimated position of the measurement object close to the true value shown in the graph V can be calculated by eliminating the error.
  • the device 2 in which the marker M and the inertial sensor S are joined is simply imaged by the camera 2,
  • the position and orientation of the measurement target to which the device 1 is attached can be measured easily and with high accuracy.

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

簡易かつ高精度に計測対象物の位置と姿勢を計測することのできるシステムとして、計測対象物の位置と姿勢を計測するためのマーカＭと慣性センサＳが接合され、計測対象物に付設されたデバイス１と、計測対象物を撮像するカメラ２と、撮像された画像内にデバイス１の撮像画像が含まれているか否かを判定する判定部３、上記画像内にデバイス１の撮像画像が含まれていると判定部により判定された場合には、マーカＭの撮像画像から計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算部４１、及び、上記画像内にデバイス１の撮像画像が含まれていないと判定部により判定された場合には、慣性センサＳにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算部４２を含む位置姿勢推定装置１０を備えた位置姿勢推定システムを提供する。

Description

位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法
　本発明は、計測対象物の位置と姿勢を推定する技術に関するものである。
　これまで、以下の特許文献１や非特許文献１に示されるように、画像情報と慣性情報を利用して人の位置や姿勢を計測する技術が開発されている。
米国特許第９１４２０２４号公報
E. Foxlin et al., "VIS-Tracker: A Wearable Vision-Inertial Self-Tracker", IEEE VR2003, March 22-26, Los Angels
　しかし、上記の特許文献１や非特許文献１に示された技術においては、人の全身運動や移動の軌跡を計測するための仕組みが複雑なため、事前準備に多大な時間やコストを要するという問題がある。
　本発明は、このような問題を解決するためになされたもので、簡易かつ高精度に計測対象物の位置と姿勢を計測することのできるシステム及び装置と方法を提供することを目的とする。
　上記課題を解決するため、本発明は、計測対象物の位置と姿勢を推定するための位置姿勢推定システムであって、計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、計測対象物に付設されたデバイスと、計測対象物を撮像する撮像手段と、撮像により得られた画像内に、上記デバイスの撮像画像が含まれているか否かを判定する判定部、上記画像内に上記デバイスの撮像画像が含まれていると判定部により判定された場合には、上記マーカの撮像画像から計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算部、及び、上記画像内に上記デバイスの撮像画像が含まれていないと判定部により判定された場合には、上記慣性センサにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算部を含む位置姿勢推定手段を備えた位置姿勢推定システムを提供する。
　また、上記課題を解決するため、本発明は、計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、計測対象物に付設されたデバイスの撮像画像が画像内に含まれているか否かを判定する判定手段と、上記デバイスの撮像画像が画像内に存在する場合には、上記マーカの撮像画像から計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算手段と、上記デバイスの撮像画像が画像内に存在しない場合には、上記慣性センサにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算手段を備えた位置姿勢推定装置を提供する。
　また、上記課題を解決するため、本発明は、計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、計測対象物に付設されたデバイスの画像が撮像画像内に存在するか否かを判定する第一のステップと、第一のステップで上記デバイスの画像が存在すると判定された場合には、上記マーカの画像から計測対象物の位置及び姿勢の少なくとも一方を算出する第二のステップと、第一のステップで上記デバイスの画像が存在しないと判定された場合には、上記慣性センサにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する第三のステップを含む位置姿勢推定方法を提供する。
　本発明によれば、計測対象物の位置と姿勢を簡易かつ高精度に計測することができる。
本発明の実施の形態に係る位置姿勢推定システムを用いて計測対象物の位置や姿勢を推定する方法を説明するための図である。 図１に示されたマーカＭの構成例を示す平面図である。 上記位置姿勢推定システムの構成要素である位置姿勢推定装置１０の構成を示すブロック図である。 本発明の実施の形態に係る位置姿勢推定方法を示すフローチャートである。 図４のステップＳ１における判定を行う方法を示すフローチャートである。 図４のステップＳ３において計測対象物の位置や姿勢を推定する方法を説明するための第一の図である。 図４のステップＳ３において計測対象物の位置や姿勢を推定する方法を説明するための第二の図である。 図４のステップＳ３において計測対象物の位置や姿勢を推定する方法を説明するための第三の図である。
　以下において、本発明の実施の形態を図面を参照しつつ詳しく説明する。なお、図中同一符号は同一又は相当部分を示す。
　現在においては、国際公開ＷＯ／２０１６／０２４５５５号公報に開示されたマーカを計測対象物に付着させ、本マーカをカメラで撮像することにより、当該計測対象物の位置と姿勢を高精度に計測することが可能となっている。
　しかし、当該マーカが上記カメラの撮像範囲外に位置する場合や、上記カメラによる撮像画像へ正常に映り込まないという隠れが生じた場合には、上記の位置や姿勢を計測することが困難となる。
　一方、既存の慣性センサ（Inertial Measurement Unit: IMU）を用いれば高精度に計測対象物の姿勢を推定することができるものの、当該計測対象物の位置については２回積分を行うため大きな誤差が生じてしまう。
　そこで、上記マーカと慣性センサを以下のように併用して両者の弱点を相互補完することにより、高精度かつロバストに計測対象物の位置と姿勢を簡易に計測することができる。
　図１は、本発明の実施の形態に係る位置姿勢推定システムを用いて計測対象物（図示していない。以下同様。）の位置や姿勢を推定する方法を説明するための図である。また図２は、図１に示されたマーカＭの構成例を示す平面図である。また図３は、上記位置姿勢推定システムの構成要素である位置姿勢推定装置１０の構成を示すブロック図である。
　本発明の実施の形態に係る位置姿勢推定システムにおいては、図１に示されたマーカＭとして、計測対象物の位置と姿勢を高精度に計測できる種々の平面型マーカを利用することができる。従って、マーカＭの構成は、図２に示された構成例に限られないことは言うまでもない。
　図２に示されたマーカＭは、上記国際公開ＷＯ／２０１６／０２４５５５号公報に開示されたマーカであって、黒地に白抜きの二次元パターンコードを含む矩形のＡＲマーカＭｂと、ＡＲマーカＭｂと同一平面上でＡＲマーカＭｂの各辺に平行に配置された４つのモアレパターン形成部ＶＭＰ１ｘ，ＶＭＰ１ｙ，ＶＭＰ２ｘ，ＶＭＰ２ｙと、ＡＲマーカＭｂの各頂点に対応してマーカＭの四隅に配置された４つの黒丸からなる参照点ＲＰ１～ＲＰ４とを備えたものである。
　図１から図３に示されるように、本発明の実施の形態に係る位置姿勢推定システムは、上記のような計測対象物の位置と姿勢を計測するためのマーカＭと慣性センサＳが接合され、計測対象物に付設されたデバイス１と、計測対象物を撮像するカメラ２と、カメラ２による撮像により得られた画像内に、デバイス１の撮像画像が含まれているか否かを判定する判定部３、上記画像内にデバイス１の撮像画像が含まれていると判定部３により判定された場合には、マーカＭの撮像画像から計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算部４１、及び、上記画像内にデバイス１の撮像画像が含まれていないと判定部３により判定された場合には、慣性センサＳにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算部４２を含む位置姿勢推定装置１０を備えている。
　なお、位置姿勢推定装置１０は、端子６と端子６に接続されたバス７を含み、上記判定部３と、マーカ情報演算部４１及び慣性情報演算部４２を含む演算部４は、それぞれバス７に接続されている。さらに、位置姿勢推定装置１０はバス７に接続された記憶部５を含む。
　図４は、本発明の実施の形態に係る位置姿勢推定方法を示すフローチャートである。以下において、図４を参照しつつ、図１から図３に示された位置姿勢推定システムを用いた場合を例として、本発明の実施の形態に係る位置姿勢推定方法について詳しく説明する。
　ステップＳ１では、判定部３が、計測対象物の位置と姿勢を計測するためのマーカＭと慣性センサＳが接合され、計測対象物に付設されたデバイス１の画像がカメラ２により撮像された画像内に存在するか否かを判定する。なお、デバイス１の画像がカメラ２により撮像された画像内に存在するか否かは、図１に示されたカメラ２による撮像範囲Ａの中にデバイス１が存在するか否かを意味する。
　ここで、図５を参照しつつ、判定部３による本判定の方法を具体的に説明する。
　最初のステップＳ１０において、カメラ２により撮像された画像を白と黒の二色に２値化する。次に、ステップＳ１１では、上記のように２値化された画像内に黒地の二次元パターン、すなわち図２に示されたＡＲマーカＭｂの画像が含まれているか否かを判別する。そして、ＡＲマーカＭｂの画像が含まれていないと判別された場合には、デバイス１の撮像画像が存在しないと判定する一方、ＡＲマーカＭｂの画像が含まれていると判別された場合にはステップＳ１２へ進む。
　ステップＳ１２では、上記二次元パターン（ＡＲマーカＭｂ）周辺の４つの参照点ＲＰ１～ＲＰ４とＡＲマーカＭｂ内に存在する白抜きの二次元パターンコードの画像が含まれているか否かを判別する。そして、上記画像が含まれていないと判別された場合には、デバイス１の撮像画像が存在しないと判定する一方、上記画像が含まれていると判別された場合には、デバイス１の撮像画像が存在すると判定する。
　次に、図４に示されたステップＳ２では、ステップＳ１でデバイス１の画像が存在すると判定された場合には、マーカＭの画像から計測対象物の位置及び姿勢の少なくとも一方を算出する。なお、具体的には、参照点ＲＰ１～ＲＰ４やモアレパターン形成部ＶＭＰ１ｘ，ＶＭＰ１ｙ，ＶＭＰ２ｘ，ＶＭＰ２ｙの画像を利用することによって計測対象物の位置や姿勢を算出することができるが、この算出方法については上記の国際公開ＷＯ／２０１６／０２４５５５号公報などに開示されている。
　次に、ステップＳ３では、ステップＳ１でデバイス１の画像が存在しないと判定された場合には、慣性センサＳにより取得されたデータに基づいて計測対象物の位置及び姿勢の少なくとも一方を算出する。
　すなわち、デバイス１がカメラ２の撮像範囲Ａの中に存在する場合には、上記のようにカメラ２で撮像された画像を用いて計測対象物の位置及び姿勢を計測する一方で、デバイス１がカメラ２の撮像範囲Ａの外に存在する場合には、慣性センサＳを用いて計測対象物の位置及び姿勢を計測する。
　ここで、慣性センサＳを用いて計測対象物の姿勢を算出する方法については広く実用化されているが、計測対象物の位置を算出する場合には大きな誤差が生じてしまう。そこで、カメラ２を用いた計測範囲内外における境界条件を用いることにより、この誤差を解消する。
　すなわち、図１に示されるように、計測対象物に付設されたデバイス１が撮像範囲Ａの中から境界線ＢＬを越えて外へ出て、再び境界線ＢＬを跨いで撮像範囲Ａの中に戻ってきた場合を例に挙げると、撮像範囲Ａの外へ出る直前、及び撮像範囲Ａの中に戻ってきた直後において、それぞれカメラ２でマーカＭを撮像することにより得られた位置情報を利用し、時系列のフィルタリング技術を適用することによって、慣性センサＳに生じる誤差を補正する。
　以下において、慣性センサＳを用いた計測対象物の位置の推定方法、具体的には慣性情報演算部４２による演算内容について、図６から図８を参照しつつ詳しく説明する。
　なお、上記からも分かるように、慣性センサＳを用いた計測対象物の位置の推定においては、前提として、カメラ２は計測対象物を継続して撮像すると共に、マーカ情報演算部４１は計測対象物の位置を継続して算出することが必要とされる。
　慣性情報演算部４２は、デバイス１がカメラ２による撮像範囲Ａの外に出る直前の時点でマーカ情報演算部４１により算出されたデバイス１の位置Ｐである始点ＳＰからカルマンフィルタによる積分演算を行う。
　ここで、図６に示されるように、縦軸及び横軸がそれぞれ位置Ｐ、時間Ｔである座標系において、例えば真値がグラフＶで示されるとき、方向ｄ１（順方向）に上記カルマンフィルタによる演算を行って順次加速度から位置を計算した積分結果はグラフＣで示される。以下において、グラフＣを得る際に行う演算方法の具体例について説明する。
　上記カルマンフィルタは、加速度についての観測値ｙを用いて状態ベクトルｘの観測更新を行うものであるが、この観測更新により得られた推定値を基に予測更新を行う。そして、得られた予測更新の結果は、次の観測更新の算出に用いられる。
　具体的には、それぞれ、離散した時間をｔ、位置をｐｔ、サンプリング時間をｄｔ、システムノイズをｗｔ、観測ノイズをｖｔ、推定誤差共分散行列をＰ、カルマンゲインをＫ、システムノイズの共分散をＱｔ、観測ノイズの共分散をＲｔとすると、時間ｔにおける状態ベクトルｘｔ、時間ｔにおける観測値ｙ、行列Ｆ，Ｈ，Ｇはそれぞれ以下の式で示される。
Figure JPOXMLDOC01-appb-M000001
　このとき、デバイス１がカメラ２による撮像範囲Ａの中に戻った直後の時点をＮとすると、線形確率モデルは次式（２）で示される。
Figure JPOXMLDOC01-appb-M000002
　そして、推定値をハット（＾）で示すと、カルマンフィルタにおける上記予測更新及び上記観測更新は、それぞれ次式（３），（４）により計算され、式（４）による観測更新についての演算結果、すなわち状態ベクトル推定値ｘハットｔ／ｔ及び推定誤差共分散行列Ｐｔ／ｔは、バス７を介して記憶部５に保存される。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
　上記計算により得られたカルマンフィルタによる積分結果は、図６のグラフＣで示されるように、グラフＶで示される真値に対して時間Ｔと共に誤差が拡大することが分かる。そこで、デバイス１がカメラ２による撮像範囲Ａの中に戻った直後の時点Ｎにおいてマーカ情報演算部４１により算出されたデバイス１の位置を終点ＥＰとしたとき、図７に示されるように、境界条件である終点ＥＰから上記始点に向けて方向ｄ２（逆方向）へ、記憶部５に保存された上記演算結果を用いたスムージング演算を行う。なお、図７においては、時間（ｔ＋１）における上記誤差が矢印で示されるが、後述する図８（ｂ）においても同様である。
　具体的には、例えば次式（５）によるスムージング演算によって、始点ＳＰと終点ＥＰ間における計測対象物の推定位置が算出され、その結果がグラフＲ１で示される。
Figure JPOXMLDOC01-appb-M000005
　上記計算によれば、図７のグラフＲ１から分かるように、上記誤差を解消することにより、グラフＶで示される真値に近い計測対象物の推定位置を算出することができる。
　但し、図８（ｂ）に示されるように、上記順方向のカルマンフィルタによる計算結果に大きな誤差が生じると、上記のスムージング演算によっても始点ＳＰ付近での誤差が増大してしまう。そこで、そのような場合であっても計測対象物の位置を精度よく推定するというロバスト性を持たせるため、逆方向のフィルタリング演算における推定値への補正項として、図８（ａ）の減衰曲線ＤＣで示されるような減衰項を加えることで、始点ＳＰ付近での誤差の増大を低減させる。
　具体的には、例えば次式（６）による演算により、図８（ｂ）のグラフＲ２で示されるように、始点ＳＰ付近の誤差を抑えた計測対象物の推定位置の算出が可能となる。
Figure JPOXMLDOC01-appb-M000006
　以上より、本発明の実施の形態に係る位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法によれば、マーカＭと慣性センサＳが接合されたデバイス１をカメラ２で撮像するだけで、デバイス１が付設された計測対象物の位置や姿勢を簡易かつ高精度に計測することができる。
１　デバイス
２　カメラ
３　判定部
４　演算部
１０　位置姿勢推定装置
４１　マーカ情報演算部
４２　慣性情報演算部
Ｍ　マーカ
Ｓ　慣性センサ

 

Claims (9)

  1. 　計測対象物の位置と姿勢を推定するための位置姿勢推定システムであって、
    　前記計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、前記計測対象物に付設されたデバイスと、
    　前記計測対象物を撮像する撮像手段と、
    　前記撮像により得られた画像内に、前記デバイスの撮像画像が含まれているか否かを判定する判定部、前記画像内に前記デバイスの撮像画像が含まれていると前記判定部により判定された場合には、前記マーカの撮像画像から前記計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算部、及び、前記画像内に前記デバイスの撮像画像が含まれていないと前記判定部により判定された場合には、前記慣性センサにより取得されたデータに基づいて前記計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算部を含む位置姿勢推定手段を備えた位置姿勢推定システム。
  2. 　前記撮像手段は、前記計測対象物を継続して撮像すると共に、
    　前記マーカ情報演算部は、前記計測対象物の位置を継続して算出し、
    　前記慣性情報演算部は、前記デバイスが前記撮像手段による撮像範囲の外に出る直前の時点で前記マーカ情報演算部により算出された前記デバイスの位置を始点とし、前記デバイスが前記撮像手段による撮像範囲の中に戻った直後の時点で前記マーカ情報演算部により算出された前記デバイスの位置を終点としたとき、前記始点から第一のフィルタリング演算を行うと共に、前記終点から前記第一のフィルタリング演算の結果を用いて前記始点に向けた第二のフィルタリング演算を行うことにより、前記始点と前記終点間における前記計測対象物の位置を推定する請求項１に記載の位置姿勢推定システム。
  3. 　前記慣性情報演算部は、前記第二のフィルタリング演算において、前記始点に向けて演算結果を減衰させる請求項２に記載の位置姿勢推定システム。
  4. 　計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、前記計測対象物に付設されたデバイスの画像が撮像画像内に含まれているか否かを判定する判定手段と、
    　前記デバイスの画像が前記撮像画像内に存在する場合には、前記マーカの画像から前記計測対象物の位置及び姿勢の少なくとも一方を算出するマーカ情報演算手段と、
    　前記デバイスの画像が前記撮像画像内に存在しない場合には、前記慣性センサにより取得されたデータに基づいて前記計測対象物の位置及び姿勢の少なくとも一方を算出する慣性情報演算手段を備えた位置姿勢推定装置。
  5. 　前記撮像画像は継続して撮像されると共に、
    　前記マーカ情報演算手段は、前記計測対象物の位置を継続して算出し、
    　前記慣性情報演算手段は、前記デバイスが前記撮像の範囲外に出る直前の時点で前記マーカ情報演算手段により算出された前記デバイスの位置を始点とし、前記デバイスが前記範囲の中に戻った直後の時点で前記マーカ情報演算手段により算出された前記デバイスの位置を終点としたとき、前記始点から第一のフィルタリング演算を行うと共に、前記終点から前記第一のフィルタリング演算の結果を用いて前記始点に向けた第二のフィルタリング演算を行うことにより、前記始点と前記終点間における前記計測対象物の位置を推定する請求項４に記載の位置姿勢推定装置。
  6. 　前記慣性情報演算手段は、前記第二のフィルタリング演算において、前記始点に向けて演算結果を減衰させる請求項５に記載の位置姿勢推定装置。
  7. 　計測対象物の位置と姿勢を計測するためのマーカと慣性センサが接合され、前記計測対象物に付設されたデバイスの画像が撮像画像内に存在するか否かを判定する第一のステップと、
    　前記第一のステップで前記デバイスの画像が存在すると判定された場合には、前記マーカの画像から前記計測対象物の位置及び姿勢の少なくとも一方を算出する第二のステップと、
    　前記第一のステップで前記デバイスの画像が存在しないと判定された場合には、前記慣性センサにより取得されたデータに基づいて前記計測対象物の位置及び姿勢の少なくとも一方を算出する第三のステップを含む位置姿勢推定方法。
  8. 　前記撮像画像を継続して撮像すると共に、
    　前記第二のステップでは、前記計測対象物の位置を継続して算出し、
    　前記第三のステップでは、前記デバイスが前記撮像の範囲外に出る直前の時点で前記第二のステップにおいて算出された前記デバイスの位置を始点とし、前記デバイスが前記範囲の中に戻った直後の時点で前記第二のステップにおいて算出された前記デバイスの位置を終点としたとき、前記始点から第一のフィルタリング演算を行うと共に、前記終点から前記第一のフィルタリング演算の結果を用いて前記始点に向けた第二のフィルタリング演算を行うことにより、前記始点と前記終点間における前記計測対象物の位置を推定する請求項７に記載の位置姿勢推定方法。
  9. 　前記第三のステップでは、前記第二のフィルタリング演算において、前記始点に向けて演算結果を減衰させる請求項８に記載の位置姿勢推定方法。

     
PCT/JP2019/028458 2018-08-06 2019-07-19 位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法 WO2020031659A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018147666 2018-08-06
JP2018-147666 2018-08-06

Publications (1)

Publication Number Publication Date
WO2020031659A1 true WO2020031659A1 (ja) 2020-02-13

Family

ID=69414073

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/028458 WO2020031659A1 (ja) 2018-08-06 2019-07-19 位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法

Country Status (1)

Country Link
WO (1) WO2020031659A1 (ja)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010038707A (ja) * 2008-08-05 2010-02-18 Shimadzu Corp モーショントラッカ装置
JP2013124972A (ja) * 2011-12-15 2013-06-24 Samsung Yokohama Research Institute Co Ltd 位置推定装置及び方法、並びにテレビジョン受信機
JP2016006415A (ja) * 2014-05-29 2016-01-14 アニマ株式会社 光学式モーションキャプチャにおける光学式マーカーの位置の推定方法及び装置
WO2016024555A1 (ja) * 2014-08-12 2016-02-18 国立研究開発法人産業技術総合研究所 マーカとマーカを用いた姿勢推定方法
WO2017043181A1 (ja) * 2015-09-09 2017-03-16 ソニー株式会社 センサ装置、センサシステム及び情報処理装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010038707A (ja) * 2008-08-05 2010-02-18 Shimadzu Corp モーショントラッカ装置
JP2013124972A (ja) * 2011-12-15 2013-06-24 Samsung Yokohama Research Institute Co Ltd 位置推定装置及び方法、並びにテレビジョン受信機
JP2016006415A (ja) * 2014-05-29 2016-01-14 アニマ株式会社 光学式モーションキャプチャにおける光学式マーカーの位置の推定方法及び装置
WO2016024555A1 (ja) * 2014-08-12 2016-02-18 国立研究開発法人産業技術総合研究所 マーカとマーカを用いた姿勢推定方法
WO2017043181A1 (ja) * 2015-09-09 2017-03-16 ソニー株式会社 センサ装置、センサシステム及び情報処理装置

Similar Documents

Publication Publication Date Title
JP3859574B2 (ja) ３次元視覚センサ
JP6415066B2 (ja) 情報処理装置、情報処理方法、位置姿勢推定装置、ロボットシステム
EP1596330B1 (en) Estimating position and orientation of markers in digital images
EP2360638B1 (en) Method, system and computer program product for obtaining a point spread function using motion information
JP2008506953A5 (ja)
JP5035467B2 (ja) 立体性認証方法、立体性認証装置および立体性認証プログラム
US20020041282A1 (en) Shape measurement system
JP2005326282A (ja) 位置姿勢計測方法および装置
CN112880687A (zh) 一种室内定位方法、装置、设备和计算机可读存储介质
US10110822B2 (en) Method for tracking at least one object and method for replacing at least one object by a virtual object in a moving image signal recorded by a camera
KR20190059639A (ko) 콘크리트 표면 균열 측정 장치 및 방법
JP6282377B2 (ja) ３次元形状計測システムおよびその計測方法
JP6135972B2 (ja) 標定方法、標定プログラム、及び標定装置
JP2006350897A (ja) 運動計測装置
JPWO2008032375A1 (ja) 画像補正装置及び方法、並びにコンピュータプログラム
JP2007025863A (ja) 撮影システム、撮影方法及び画像処理プログラム
JP4935769B2 (ja) 平面領域推定装置及びプログラム
WO2020031659A1 (ja) 位置姿勢推定システム及び位置姿勢推定装置と位置姿勢推定方法
WO2020175621A1 (ja) カメラ校正情報取得装置、画像処理装置、カメラ校正情報取得方法および記録媒体
KR101575934B1 (ko) 관성 센서 및 광학 센서를 이용한 모션 캡쳐 장치 및 방법
JP5230354B2 (ja) 位置特定装置及び異動建物検出装置
WO2008032376A1 (fr) Dispositif et procédé de mesure de distance, et programme informatique
JP5409451B2 (ja) ３次元変化検出装置
JP2010038753A (ja) 運動推定装置及びプログラム
JP4958992B2 (ja) 運動算出装置及び運動算出方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19847578

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19847578

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP