JPS63128405A - 位置・姿勢同定方式 - Google Patents
位置・姿勢同定方式Info
- Publication number
- JPS63128405A JPS63128405A JP61274169A JP27416986A JPS63128405A JP S63128405 A JPS63128405 A JP S63128405A JP 61274169 A JP61274169 A JP 61274169A JP 27416986 A JP27416986 A JP 27416986A JP S63128405 A JPS63128405 A JP S63128405A
- Authority
- JP
- Japan
- Prior art keywords
- register
- image
- underwater
- calculates
- equation
- Prior art date
- Legal status (The legal status 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 status listed.)
- Granted
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。
め要約のデータは記録されません。
Description
【発明の詳細な説明】
（産業上の利用分野）
本発明は航走体の位置・姿勢の同定方式に関し、特に水
中音響映像を用いて、慣性航法装置や音９航法装置等か
ら得られるデータを補正して位置・姿勢の同定を行う方
式に関するものである。
中音響映像を用いて、慣性航法装置や音９航法装置等か
ら得られるデータを補正して位置・姿勢の同定を行う方
式に関するものである。
（従来の技術）
ロボットの視覚情報に基づきその姿勢及び位置の同定を
行うことは高度な自律性と機能性を持たせるために不可
欠な要素である。そしてその同定に光学映像を用いる方
法については従来より種々の方法が提案、開発されてい
る。しかし水中が作業環境となる航走体では、視覚距離
の短い光学映像を用いて位置・姿勢の同定を行うことは
困難であり、慣性航法装置や音響航法装置によって位置
・姿勢の同定が行われていた。
行うことは高度な自律性と機能性を持たせるために不可
欠な要素である。そしてその同定に光学映像を用いる方
法については従来より種々の方法が提案、開発されてい
る。しかし水中が作業環境となる航走体では、視覚距離
の短い光学映像を用いて位置・姿勢の同定を行うことは
困難であり、慣性航法装置や音響航法装置によって位置
・姿勢の同定が行われていた。
（発明が解決しようとする問題点）
しかしながら、これらの航法装置の位置・姿勢データの
精度では、ロボットが作業を行うには不十分であり、何
らかの補正手段が求められていた。
精度では、ロボットが作業を行うには不十分であり、何
らかの補正手段が求められていた。
本発明は、上記従来技術の問題点を解決するためになさ
れたものであって、視覚情報として水中音響映像を用い
て慣性航法装置や音響航法装置の測位データを補正する
ことにより、精度の高い位置・姿勢の同定を行うことの
できる位置・姿勢同定方式を提供することを目的とする
。
れたものであって、視覚情報として水中音響映像を用い
て慣性航法装置や音響航法装置の測位データを補正する
ことにより、精度の高い位置・姿勢の同定を行うことの
できる位置・姿勢同定方式を提供することを目的とする
。
（問題点を解決するための手段）
本発明は、慣性航法装置、音響航法装置等の航法装置か
ら得られる測位データに基づき、水中を作業環境とする
航走体の位置及び姿勢の同定を行う位置・姿勢同定方式
に関し、前記従来技術の問題点を解決するため、円柱状
物体を観測対象としてその水中音響映像を求め、該水中
音響映像から得られる２つの直交断面像の特徴情報を用
いて前記測位データを補正するようにしたものである。
ら得られる測位データに基づき、水中を作業環境とする
航走体の位置及び姿勢の同定を行う位置・姿勢同定方式
に関し、前記従来技術の問題点を解決するため、円柱状
物体を観測対象としてその水中音響映像を求め、該水中
音響映像から得られる２つの直交断面像の特徴情報を用
いて前記測位データを補正するようにしたものである。
上記観測対象としては例えば海底石油生産用ジャケット
型プラットフォームの脚部等が考えられる。
型プラットフォームの脚部等が考えられる。
（作 用）
本発明において使用する水中音響映像は、分解能の高い
ものでありしかも光学映像が利用できない水中等の作業
環境でも利用できるものである。
ものでありしかも光学映像が利用できない水中等の作業
環境でも利用できるものである。
そしてこの分解能の高い水中音響映像を利用して観測対
象である円柱状物体の２つの直交断面像より得られる楕
円像の長袖半径、短軸半径、楕円の傾き、中心位置、長
袖の延長線に対する航走体からの最短点情報のごとき特
徴情報（パラメータ）を用い、航法装置の測位データの
補正を行うので、航走体の位置・姿勢が高い精度で同定
できるようになる。従って前記従来技術の問題点が解決
される。
象である円柱状物体の２つの直交断面像より得られる楕
円像の長袖半径、短軸半径、楕円の傾き、中心位置、長
袖の延長線に対する航走体からの最短点情報のごとき特
徴情報（パラメータ）を用い、航法装置の測位データの
補正を行うので、航走体の位置・姿勢が高い精度で同定
できるようになる。従って前記従来技術の問題点が解決
される。
（実施例）
本発明では、円柱状物体を観測対象としてその水中音響
映像を求め、該水中音響映像から得られる２つの直交断
面像の特徴情報を用いて航法装置の測位データを補正す
ることにより、航走体の位置・姿勢を高い精度で同定す
るようにしている。
映像を求め、該水中音響映像から得られる２つの直交断
面像の特徴情報を用いて航法装置の測位データを補正す
ることにより、航走体の位置・姿勢を高い精度で同定す
るようにしている。
まず、測位データの補正に用いる２つの直交断面像につ
いて説明する。
いて説明する。
本発明に係る航走体は水中音響映像装置を備えている。
この音響映像装置としては例えば海法宇治、似鳥−彦他
［ファンビーム走査による多機能超音波水中映像装置Ｊ
５ＡＮＥ８２−１２．ｐ９−１６．電子通信学会（１
９８２年６月）に示されている装置を用いることができ
る。音響映像装置は３次元の情報を持っているので、３
次元情報の任意の２次元断面を映像として切り出すこと
ができる。そこで−例としてここでは第２図（ａ）及び
（ｂ）に示すような２つのモードの映像を得ることにす
る。Ｃモード（正面像モード）では、音波伝搬方向に垂
直な平面の映像が得られ、Ｂモード（断面モード）では
、音波伝搬方向に平行な映像が得られる。即ち、２つの
映像モードによる断面像は互いに直交する直交断面像を
形成する。
［ファンビーム走査による多機能超音波水中映像装置Ｊ
５ＡＮＥ８２−１２．ｐ９−１６．電子通信学会（１
９８２年６月）に示されている装置を用いることができ
る。音響映像装置は３次元の情報を持っているので、３
次元情報の任意の２次元断面を映像として切り出すこと
ができる。そこで−例としてここでは第２図（ａ）及び
（ｂ）に示すような２つのモードの映像を得ることにす
る。Ｃモード（正面像モード）では、音波伝搬方向に垂
直な平面の映像が得られ、Ｂモード（断面モード）では
、音波伝搬方向に平行な映像が得られる。即ち、２つの
映像モードによる断面像は互いに直交する直交断面像を
形成する。
次に、この２つの直交断面像から、航走体の３次元位置
および姿勢を同定する原理について説明する。
および姿勢を同定する原理について説明する。
第３図はここで用いる座標系および観測対象とする円柱
モデルを示す図である。対象とする円柱は、軸方向単位
ベクトルＷ、中心位置Ｃ９半径ｒおよび、Ｃを基点とし
た上端位置ｚ２と下端位置２、によって記述する。
モデルを示す図である。対象とする円柱は、軸方向単位
ベクトルＷ、中心位置Ｃ９半径ｒおよび、Ｃを基点とし
た上端位置ｚ２と下端位置２、によって記述する。
既知の円柱モデルに対して、航走体の位置ベクトルｐ、
視線方向単位ベクトルＶ、航走体の乗っている仮想平面
単位法線ベクトルｎを同定対象とする。第４図はこれら
パラメータ間の関係と、Ｂモード映像によって得られた
楕円像とを表わしている。ただし、円柱の両端は十分長
いものと仮定する。この楕円像に関して以下の諸量を定
義する（第５図参照）。
視線方向単位ベクトルＶ、航走体の乗っている仮想平面
単位法線ベクトルｎを同定対象とする。第４図はこれら
パラメータ間の関係と、Ｂモード映像によって得られた
楕円像とを表わしている。ただし、円柱の両端は十分長
いものと仮定する。この楕円像に関して以下の諸量を定
義する（第５図参照）。
（ｉ） 長軸半径 ａ
（ｉ ｉ） 短軸半径 ｂ
（ｉｉｉ） 長軸とＸ座標軸とのなす角 α（ｉＶ）
中心位置 （ｘ（Ｂ、ｙｃ）（Ｖ） 航走体か
ら楕円の長袖の延長線に致る最短点 Ｎ（Ｎ、、Ｎ、） また、ｐからＶの方向に距離１だけ離れた点りを通るＣ
モード断面（第６図参照）から得られる楕円像に関して
は、上記（ｉ）〜（Ｖ）のパラメータについてプライム
（１をつけて表わすこととする。
中心位置 （ｘ（Ｂ、ｙｃ）（Ｖ） 航走体か
ら楕円の長袖の延長線に致る最短点 Ｎ（Ｎ、、Ｎ、） また、ｐからＶの方向に距離１だけ離れた点りを通るＣ
モード断面（第６図参照）から得られる楕円像に関して
は、上記（ｉ）〜（Ｖ）のパラメータについてプライム
（１をつけて表わすこととする。
さてこれらの諸量が観測されたとき、
と定義する。ただしδは０〈δく１なる実数である。さ
らに、２を ２会ｃ−ｐ ・・・（２） ただし“×”はベクトルの外積を、“・”は内積を表わ
す。
らに、２を ２会ｃ−ｐ ・・・（２） ただし“×”はベクトルの外積を、“・”は内積を表わ
す。
上ａ己直交断面像においては、円柱の軸方向の平行移動
と、軸のまわりの回転に関する情報は含まれていない。
と、軸のまわりの回転に関する情報は含まれていない。
従って、上式（３）のうち独立なものは７つであり、ｎ
、Ｖ、Ｅ （ｐ）を−意に定めることはできないが、こ
れらを慣性航法装置や音響航法装置等で得られた情報（
ｎ ｇ、Ｖ Ｏ＋ Ｚ ｏ）の補正に用いることは可能
である。
、Ｖ、Ｅ （ｐ）を−意に定めることはできないが、こ
れらを慣性航法装置や音響航法装置等で得られた情報（
ｎ ｇ、Ｖ Ｏ＋ Ｚ ｏ）の補正に用いることは可能
である。
そこで、（ｆ’Ｉ Ｏ＊ Ｖ Ｏ＋ Ｚ Ｏ）と真値と
の差が小さいと仮定して、式（３）に線形近似を施す。
の差が小さいと仮定して、式（３）に線形近似を施す。
すなわち（１，’％／、 Ｚを
と表わしてこれらを式（３）に代入し、Δｎ、ΔＶ。
Δ２に関する２次以上の項を省略すれば、次の方程式が
得られる。なお（Ｌ′は転置を表わす。
得られる。なお（Ｌ′は転置を表わす。
ＴＸ＝ｕ ＝（５）
（以下余白）
ただし、
Ｔのランクが７の方程式であるので、この方程式を解く
ためには、−膜化逆行列を用いなければならない。この
場合、Δｎ、Δ■、Δ２の各ノルムの２乗和を最小にす
るという条件のもとに、最小２乗誤差解を求めることに
より、解を一意に決定できる。
ためには、−膜化逆行列を用いなければならない。この
場合、Δｎ、Δ■、Δ２の各ノルムの２乗和を最小にす
るという条件のもとに、最小２乗誤差解を求めることに
より、解を一意に決定できる。
したがって上述のように、慣性航法装置や音響航法装置
から得られるデータに対し、音響映像を用いることによ
り、補正量が求められ、精度の高い位置・姿勢の同定が
可能となる。
から得られるデータに対し、音響映像を用いることによ
り、補正量が求められ、精度の高い位置・姿勢の同定が
可能となる。
次に本発明の一実施例について説明する。第１図は本実
施例の位置・姿勢同定システムの構成を示すブロック図
である。図において、１は慣性航法装置または音響航法
装置からの測位データ入力端子、２はこれらの人力デー
タを格納するレジスタ、３は水中音響映像装置からの断
面映像データ入力端子、４は断面映像データから楕円像
のパラメータを抽出するパラメータ抽出手段、５は前記
パラメータ抽出手段４により抽出されたバラメ一方程式
の右辺式（８）を格納するレジスタ、８は行列方程式の
係数行列式（６）の■を格納するレジスタ、９はこの係
数行列より一般化逆行列を算出する一般化逆行列生成手
段、１０は求められた一般化逆行列を格納するレジスタ
、１１は行列とベクトルの乗算を行う乗算器、１２は求
められた補正量を格納するレジスタ、１３はレジスタ２
に格納された測位データとレジスタ１２に格納された補
正量を加算する加算器、Ｉ４は結果の出力端子である。
施例の位置・姿勢同定システムの構成を示すブロック図
である。図において、１は慣性航法装置または音響航法
装置からの測位データ入力端子、２はこれらの人力デー
タを格納するレジスタ、３は水中音響映像装置からの断
面映像データ入力端子、４は断面映像データから楕円像
のパラメータを抽出するパラメータ抽出手段、５は前記
パラメータ抽出手段４により抽出されたバラメ一方程式
の右辺式（８）を格納するレジスタ、８は行列方程式の
係数行列式（６）の■を格納するレジスタ、９はこの係
数行列より一般化逆行列を算出する一般化逆行列生成手
段、１０は求められた一般化逆行列を格納するレジスタ
、１１は行列とベクトルの乗算を行う乗算器、１２は求
められた補正量を格納するレジスタ、１３はレジスタ２
に格納された測位データとレジスタ１２に格納された補
正量を加算する加算器、Ｉ４は結果の出力端子である。
次に上記実施例の動作について説明する。測位データ入
力端子１から人力された航走体の測位データである位置
・姿勢データＢＯ，ｖ、、ｚ６はレジスタ２に格納され
る。一方、パラメータ抽出手段４は、断面映像データ入
力端子３から人力された直交する２つの断面像から、楕
円像の長袖半径、短軸半径、長袖の傾き角、中心位置、
長袖延長線への最短点のパラメータをそれぞれの断面像
について抽出し、レジスタ５へ転送する。線形化方程式
形成手段６は、レジスタ２およびレジスタ５に格納され
たデータに基づき式（５）の方程式を一般化逆行列１’
ｔを算出し、レジスタＩＯへ転送する。乗算器１１はレ
ジスタ１０に格納された前記丁ｔとレジスタ７に格納さ
れたＵより Ｘ＝丁ｔｕ ・−（９） を計算し、結果の補正量、式（９）［（７）］をレジス
タ】２に格納する。そしてレジスタ２の測位データとレ
ジスタ１２の補正量を式（４）に従い加算器１３を用い
て加算する。加算結果は出力端子１４から出力される。
力端子１から人力された航走体の測位データである位置
・姿勢データＢＯ，ｖ、、ｚ６はレジスタ２に格納され
る。一方、パラメータ抽出手段４は、断面映像データ入
力端子３から人力された直交する２つの断面像から、楕
円像の長袖半径、短軸半径、長袖の傾き角、中心位置、
長袖延長線への最短点のパラメータをそれぞれの断面像
について抽出し、レジスタ５へ転送する。線形化方程式
形成手段６は、レジスタ２およびレジスタ５に格納され
たデータに基づき式（５）の方程式を一般化逆行列１’
ｔを算出し、レジスタＩＯへ転送する。乗算器１１はレ
ジスタ１０に格納された前記丁ｔとレジスタ７に格納さ
れたＵより Ｘ＝丁ｔｕ ・−（９） を計算し、結果の補正量、式（９）［（７）］をレジス
タ】２に格納する。そしてレジスタ２の測位データとレ
ジスタ１２の補正量を式（４）に従い加算器１３を用い
て加算する。加算結果は出力端子１４から出力される。
（発明の効果）
以上説明したように、本発明は観測対象である円柱状物
体に対する水中音響映像の２つの直交する断面像から得
られる特徴情報（パラメータ）を用いて、慣性航法装置
や音響航法装置の測位データを補正する方式であるため
、水中のように光学映像が利用できない作業環境におい
ても航走体の位置・姿勢を高１ｎ度で同定することがで
きるようになる。
体に対する水中音響映像の２つの直交する断面像から得
られる特徴情報（パラメータ）を用いて、慣性航法装置
や音響航法装置の測位データを補正する方式であるため
、水中のように光学映像が利用できない作業環境におい
ても航走体の位置・姿勢を高１ｎ度で同定することがで
きるようになる。
第１図は本発明の一実施例の位置・姿勢同定シの関係と
Ｂモード映像によって得られた楕円像を示す図、第５図
は楕円像に関する諸パラメータの定義図、第６図はＢモ
ード断面とＣモード断面の関係を示す説明図である。 ｌ・・・測位データ入力端子 ２・・・レジスタ ３・・・断面映像データ入力端子 ４・・・パラメータ抽出手段 ５・・・レジスタ ６・・・線形化方程式形成手段 ７・・・レジスタ ８・・・レジスタ ９−−−−一般化逆行列生成手段 １０−・・レジスタ １１・・・乗算器 １２−・・レジスタ １３−・・加算器 １４−・・出力端子
Ｂモード映像によって得られた楕円像を示す図、第５図
は楕円像に関する諸パラメータの定義図、第６図はＢモ
ード断面とＣモード断面の関係を示す説明図である。 ｌ・・・測位データ入力端子 ２・・・レジスタ ３・・・断面映像データ入力端子 ４・・・パラメータ抽出手段 ５・・・レジスタ ６・・・線形化方程式形成手段 ７・・・レジスタ ８・・・レジスタ ９−−−−一般化逆行列生成手段 １０−・・レジスタ １１・・・乗算器 １２−・・レジスタ １３−・・加算器 １４−・・出力端子
Claims (1)
- 【特許請求の範囲】 慣性航法装置、音響航法装置等の航法装置から得られる
測位データに基づき、水中を作業環境とする航走体の位
置及び姿勢の同定を行う位置・姿勢同定方式において、 円柱状物体を観測対象としてその水中音響映像を求め、
該水中音響映像から得られる２つの直交断面像の特徴情
報を用いて前記測位データを補正することを特徴とする
位置・姿勢同定方式。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61274169A JPS63128405A (ja) | 1986-11-19 | 1986-11-19 | 位置・姿勢同定方式 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61274169A JPS63128405A (ja) | 1986-11-19 | 1986-11-19 | 位置・姿勢同定方式 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS63128405A true JPS63128405A (ja) | 1988-06-01 |
| JPH0562765B2 JPH0562765B2 (ja) | 1993-09-09 |
Family
ID=17537996
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP61274169A Granted JPS63128405A (ja) | 1986-11-19 | 1986-11-19 | 位置・姿勢同定方式 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPS63128405A (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009295107A (ja) * | 2008-06-09 | 2009-12-17 | Kajima Corp | 誘導システム及び誘導方法 |
| US7797851B2 (en) | 2006-05-18 | 2010-09-21 | Panasonic Corporation | Shape measurement device probe and shape measurement device |
-
1986
- 1986-11-19 JP JP61274169A patent/JPS63128405A/ja active Granted
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7797851B2 (en) | 2006-05-18 | 2010-09-21 | Panasonic Corporation | Shape measurement device probe and shape measurement device |
| JP2009295107A (ja) * | 2008-06-09 | 2009-12-17 | Kajima Corp | 誘導システム及び誘導方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0562765B2 (ja) | 1993-09-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9378559B2 (en) | System and method for motion estimation | |
| CN108170297B (zh) | 实时六自由度vr/ar/mr设备定位方法 | |
| JPH03505924A (ja) | ３次元空間内の物体の位置及び向きを自動的に決定する方法及びシステム | |
| US6175648B1 (en) | Process for producing cartographic data by stereo vision | |
| JP3412973B2 (ja) | Ｉｓａｒ画像目標識別処理装置 | |
| CN111415375A (zh) | 一种基于多鱼眼摄像机和双针孔投影模型的slam方法 | |
| CN111899303B (zh) | 一种新的考虑空间逆投影约束的特征匹配与相对定位方法 | |
| AU774749B2 (en) | Method and apparatus for calibrating positions of a plurality of first light sources on a first part | |
| JPS63128405A (ja) | 位置・姿勢同定方式 | |
| CN111623775B (zh) | 运载体姿态测量系统、方法以及存储介质 | |
| JPH07218251A (ja) | ステレオ画像計測方法およびステレオ画像計測装置 | |
| CN118691647B (zh) | 基于轮廓的空间目标位姿跟踪方法 | |
| JP3512894B2 (ja) | 相対的移動量算出装置及び相対的移動量算出方法 | |
| US20240134033A1 (en) | Method for determining a movement state of a rigid body | |
| CN116883517B (zh) | 基于平面镜的无重叠视场的相机参数标定方法 | |
| JPS59228180A (ja) | レ−ダ画像模擬方式 | |
| Zou | Calibrating non-overlapping cameras with a laser ray | |
| Sturm et al. | Visual navigation for flying robots | |
| JPH06194138A (ja) | 物体の姿勢推定方法およびその装置 | |
| JP3453769B2 (ja) | ３次元情報復元装置 | |
| JPH0648173B2 (ja) | 円筒計測方法 | |
| JPH0454409A (ja) | 物体の姿勢推定方法およびその装置 | |
| Konov et al. | Algorithm of correction of error caused by perspective distortions of measuring mark images | |
| Araguás et al. | Visual homography-based pose estimation of a quadrotor using spectral features | |
| Zhang et al. | Using concurrent lines in central catadioptric camera calibration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| EXPY | Cancellation because of completion of term |