JPH01112183A - 対象物の位置推定装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 ［発明の目的］ （産業上の利用分野） この発明は、例えば移動体を対象物に接近させるために
、移動体に搭載して対象物の位置を推定する対象物の位
置推定装置に関する。

（従来の技術） 近時、無人宇宙機によって宇宙空間に放置された物体を
回収するシステムの開発が進められているが、このよう
に直接操縦することのできない宇宙機を目標（回収物体
）に誘導するには、目標の位置検出が不可欠である。従
来ては、目標位置検出手段として第７図に示すようなシ
ステムが考えられている。

第７図において、Ｔは目標、Ｃは宇宙機である宇宙機Ｃ
はレーダ１１及び自己の位置及び姿勢を検出する位置セ
ンサ１２及び姿勢センサ１３を搭載するレーダ１１の検
出情報はデータ処理装置１４に送られる。このデータ処
理装置１４は入力したレーダ検出情報から目標Ｔの測角
及び目標Ｔまでの相対距離を測定する。

各測定情報は中央処理装置１５に送られる。この中央処
理装置１５はセンサ１．２．１３で検出される自己の位
置及び姿勢情報を入力する。そして、自己の位置を原点
とし、自己の姿勢から進行方向を機軸とする座標系を形
成し、この座標系上に入力した測定情報に基づいて目標
Ｔの位置をプロットする。

これによって目標Ｔの位置を検出することができる。

しかしながら、上記のシステムでは、レーダ１１の寸法
が宇宙機Ｃ本体の外側に搭載する機器の制約となり、か
つレーダ１１の消費電力が宇宙機Ｃへの要求となり、し
かもその電力が宇宙機Ｃと目標Ｔとの相対距離に依存し
ているため、宇宙機Ｃは目標Ｔの位置に到達するまでに
相当な電力を発生しなければならない。

尚、宇宙機と目標との会合に限らず、規則的に運動する
対象物に移動体を誘導するシステムにおいても、レーダ
を使用する限り同様な問題を生じることになる。

（発明が解決しようとする問題点） 以上述べたように、従来の対象物の位置を検出する手段
では、レーダの使用によって、移動体に外部機器の搭載
を制約し、消費電力を増大するという問題を有している
ため、特に小型軽量化を要求される移動体には相当な負
担となる。

この発明は上記のような事情に鑑みなされたもので、レ
ーダのような測距機能がなくても対象物の位置を推定す
ることができ、これによって外部機器の搭載を制約せず
かつ極めて消費電力が少なく、被搭載移動体の小型軽量
化に供し得る対象物の位置推定装置を提供することを目
的とする。

［発明の構成］ （問題点を解決するための手段） 上記目的を達成するためにこの発明に係る対象物の位置
推定装置は、移動体に搭載され、規則性をもって運動す
る対象物の位置を推定するものであって、前記移動体の
位置情報及び姿勢情報を検出する移動体の状態検出手段
と、前記対象物を画像として捕える画像センサと、前記
対象物のもつ規則性を表わす運動方程式を予め登録し、
複数回の観測時刻で、それぞれ前記画像センサの出力デ
ータからセンサ上に捕えられた対象物の方位及び仰角の
２次元の角度情報を算出し、この２次元の角度情報を位
置情報及び姿勢情報を基準にして前記運動方程式に代入
し、各観測時刻の運動方程式によって方程式中のパラメ
ータを求め、該パラメータを代入した運動方程式に任意
の時刻を代入することによって任意の時刻の対象物の位
置を推定する演算装置とを具備して構成される。

（作用） 上記構成による対象物の位置推定装置は、測距機能を有
するレーダの代りに小型で低消費電力が期待される画像
センサを用いて対象物を捕らえ、複数の観測時刻に画像
センサ上の対象物の方位角及び仰角の２次元角度情報を
算出し、同時に状態検出手段によって自己の位置及び姿
勢を検出する。

そして、各観測点で自己の位置及び姿勢を基準とする座
標系に上記２次元角度情報を置き換え、予め登録された
対象物の運動方程式に代入し、各観副時刻の運動方程式
によって方程式中のパラメータを求め、該パラメータを
代入した運動方程式に任意の時刻を代入することによっ
て任意の時刻の対象物の位置を推定する。

（実施例） 以下、第１図乃至第６図を参照してこの発明の一実施例
を説明する。

第１図は第７図の場合と同様に、宇宙機Ｃを目標Ｔに会
合させる場合を想定し、宇宙機Ｃにこの発明に係る位置
推定装置を搭載させた場合の構成を示すもので、２１は
画像センサ、２２はデータ処理装置、２３は中央処理装
置、２４は位置センサ、２５は姿勢センサである。

画像センサ２１は目標Ｔを画像として捕らえるもので、
このセンサ２１から出力される画像データはデータ処理
装置２２に送られる。このデータ処理装置２２は入力デ
ータから第２図に示すセンサ２１の視野上に捕えられた
目標Ｔの方位及び仰角の２次元の角度情報を算出するも
ので、この角度情報は中央処理装置２３に送られる。

＝　６− 位置センサ２４は例えばＧＰＳ　（グローバル・ポジシ
ョニング・システム）受信機によって構成される。この
ＧＰＳ受信機は複数個の人口衛星から送られてくるＧＰ
Ｓ信号を受信し、予め用意されているＰＮコード情報と
受信ＰＮコード情報とのマツチングをとり、データを復
調することによって各衛星の位置情報及び各衛星からの
距離情報を算出し、画情報から幾何学的に自己の位置情
報を導出するものである。

姿勢センサ２５はスターセンサ、地球センサ、太陽セン
サ等によって自己の進行方向と地球や太陽との相対的な
関係を検出するものである。

位置センサ２４で検出された位置情報及び姿勢センサ２
５で検出された姿勢情報は中央処理装置２３に人力され
る。

中央処理装置２３はコマンド等による観測指令によって
データ処理装置２２から目標Ｔの２次元角度情報、位置
センサ２４からの位置情報、姿勢センサ２５からの姿勢
情報を取り入れる。そして、入力した位置情報及び姿勢
情報から自己の位置を原点とし、自己の姿勢から進行方
向を機軸とする座標系を形成し、この座標系上にデータ
処理装置２２からの２次元角度情報を置き換える。

この中央処理装置２３は、位置推定用アルゴリズムを実
行するもので、予めこのアルゴリズムで使用する目標Ｔ
の運動方程式が登録されており、この運動方程式に各観
測点の座標変換を行った２次元角度情報を代入して、式
中のパラメータを求め、このパラメータから目標Ｔの位
置を推定する。

上記構成において、以下その動作原理についで説明する
。

まず、画像センサ２１で目標Ｔを捕らえ、その出力デー
タによってセンサ２１上の目標Ｔの方位角及び仰角を測
定する。一方、位置センサ２４及び姿勢センサ２５の出
力データによって宇宙機Ｃの軌道や位置を検出し、この
検出情報によって宇宙機Ｃの姿勢を軌道座標系で表現す
る。そして、この軌道座標系に目標Ｔの方位角及び仰角
を変換し、結果として目標Ｔの各角度情報を宇宙機Ｃの
速度方向ベクトルを基準に表現する。ここで得られた情
報を以下測角情報と称する。

この測角情報が得られても距離を推定できるとは限らな
い。推定できない例を第３図（ａ）に示し、推定できる
場合を同図（ｂ）に示す。第３図（ａ）では、目標Ｔの
軌道と宇宙機Ｃの軌道が平行であり、目標Ｔ及び宇宙機
ＣがＸ輔に平行に正の方向へ等速運動している場合を想
定しており、時刻ｔ。で目標ＴがＸＴＯｓ宇宙機ＣがＸ
。ｏ１時時刻、で目標ＴがＸＴＩ、宇宙機ＣがＸｌｊの
位置にある。この場合、宇宙機Ｃの各点で目標Ｔの測角
情報を得ても目標Ｔの位置及び速度推定ができないため
、相対的な距離を推定することはできない。

これに対し、第３図（ｂ）では、宇宙機Ｃが時刻ｔ１に
おいてＺ軸方向に２．だけ平行位置から移動された場合
を示しており、その移動距離ｚ１が視差となる。したが
って、この視差２．と測角情報φ０．φ２によって目標
Ｔまでの位置及び速度を推定することができ、さらには
相対的な距離まで測定することができる。

そこで、この実施例では地球上を一定の円軌道で周回す
る目標Ｔに対し、宇宙機Ｃを以下のように運用する。

まず、宇宙機Ｃを目標Ｔが見える領域に誘導Ｌ、画像セ
ンサ２１で目標Ｔを捕らえた後、位置推定装置を動作さ
せ、目標Ｔの位置を求めて宇宙機Ｃを目標Ｔとの会合点
に誘導する。このとき、位置ｔｉｔ定を確実に行うため
に、宇宙機Ｃの軌道を１１標′１゛の軌道（図中ｙ軸方
向）に対して第４図に示すような楕円軌道とする。ここ
で、宇宙機Ｃがａ、　　ｃの範囲にあれば、宇宙機Ｃの
軌道が目標Ｔの軌道に平行でないので、目標Ｔの位置を
推定できるが、ｂの範囲にあるときには両者の軌道が略
平行になるので、上述したように良好な位置推定ができ
なくなる。そこで、ａの範囲で時間をパラメータする目
標Ｔの運動方程式を求め、以下この運動方程式に任意の
時刻を代入することによって目標Ｔの位置を推定する。

以下、第５図を参照して目標Ｔの位置推定手段について
説明する。

まず、宇宙機Ｃと目標Ｔが第５図に示すような位置関係
にあるとする。宇宙機Ｃは座標系ＸＣ＋Ｙｃを位置セン
サ２４によって設定する。ここでは例として、Ｘ、、Ｘ
ｏをそれぞれ地球中心と目標Ｔ１宇宙機Ｃを結ぶ方向に
設定し、Ｙｏ、Ｙｏをそれぞれ目標Ｔ１宇宙機Ｃの進行
方向でかつＸＴ。

Ｘｃと垂直に設定する。この座標系を設定したうえで宇
宙機Ｃから目標Ｔへの仰角φ□を計測する。

尚、Ｘｃ＋３’ｃは（ＸＴＩＹＴ）座標系で見た宇宙機
Ｃの座標、ＸＴＩ　　ｙＴは（Ｘｃ、Ｙｃ）座標系で見
た目標Ｔの座標、φｃ　　（＝φ□＋δ）は宇宙機Ｃ側
の仰角、φ０は目標Ｔ側の仰角、ＲはＴとＣの相対距離
、δはＴとＣの地球中心角、ωは地球を中心とする目標
Ｔの周回レート（角速度：ｒａｄ／秒）である。

目標Ｔは画像センサ２１によって方位角と仰角としてそ
の存在が認識される。尚、説明を簡単にするため、第５
図では方位角を省略している。

さて、画像センサ２１の出力データから求めた仰角は宇
宙機Ｃの本体を基準に計４（すしたものであるから、こ
のとき宇宙機Ｃが座標系Ｘｃ、Ｙｃに対−１１＝ してどのような姿勢であったかを知る必要がある。

これは姿勢センサ２５によって計測する。この過程を経
ることにより、画像センサ２１上の仰角は宇宙機Ｃの座
標系の仰角φ７に変換される。図示しないが方位角につ
いても同様である。

以上のことから、宇宙機Ｃを中心とする目標Ｔの位置は
、運動方程式により次のように表わすことができる。

ＸＴ　＝　Ｍ　（ｔ　、　　（Ｌｌ　）　ＸＴｏ　　　
　　−（１，）ここで、 ＸＴ　　＝（ＸＴ　＋　　Ｙ　Ｔ　＋　　交□、　シＴ
）’ＴＯ−（ｘＴＯ＋　　Ｙ　ＴＯ＋　　交ＴＯ＋　　
Ｙ　Ｔｏ）であり、灸、９はそれぞれｘ、ｙの時間微分
（速度）を表わす。ＸＴＯは目標Ｔの位置、速度の初期
値であり、Ｍ　（ｔ、ω）は４行４列の行列で、ｔはＸ
ＴＯからの経過時間を表わし、ωは地球を中心とする目
標Ｔの周回レート（角速度：　ｒａｄ／秒）である。す
なわち、（１）式は次式のように表わすことができる。

この運動方程式の未知のパラメータを求めれば、所望の
時刻を代入することによって任意の時刻の目標Ｔの位置
を推定することができる。この推定は以下に説明する位
置推定用アルゴリズムによって求めることができる。

まず、もし推定初期値χ。０が真の初期値χ工。′であ
ったとすると、 φＴ”　−ａｒｃｔａｎ　（ｘＴ”　／　ｙＴ’　）と
なり、これは観測値と一致するはずである。尚、ＸＴ’
−ＸＴ（ＸＴＯ”＋ｉ＋　ω） Ｖｔ’−ＹＴ　（ＸＴＯ”・　ｔ・　ω）である。しか
し、位置推定中はχＴｏ’は得られないので、これをχ
。０として推定する。

φＴ−ａｒｃｔａｎ　（ＸＴ　／　ｙＴ）は画像センサ
２１を用いずに中央処理装置２３によつて計算する。す
なわち、（１）式を用い、χ、　−Ｍ　（ｔ、ω）ＸＴ
。

を計算してχ１の要素ＸＴ、ｙＴを取り出す。

次に、 δφ−φＴ８−φ。　　　　　　　・・・（２）を計算
する。δφは観測値と推定値との差である。

ここで、 δχ−χ７０　＊Ｘ　７　。

とすると、　（２）式は次のように近似できる。

したがって、別々の時刻ｊｌ＋　　ｉ２＋　　ｉ３＋　
　ｉ４でφ０．φ２．φ３．φ４を観測することにより
、次式を求めることができる。

δφ　＝　　ｐ　（Ｘ′ｏ、　ｔ、、　ｔ２．ｔ３．　
ｔ４）　　δχ・・・（４） ここで、　（４〉式からδＸＯ＋　　δ’／　ｏ　＋　
　６女。。

δ夛。のうち未知のパラメータ（既知であればいずれも
Ｏ）だけについてδＸを構成し、行列Ｐの要素のうちδ
χに対応した要素を選択して構成しめるのに必要な要素
を取り出してδφとすると、・・・（５） のように表現できる。　（５）式の未知のパラメータは
δχであるので、 δχ、、、ｐ−１（χ。、ｔ工、　ｔ　２．　ｔ　３．
　ｔ　４　　）　　δφ・・・（６） とする。先に述べた宇宙機Ｃの運動はＰ″１が計算し得
る運動である必要がある。結局、目標Ｔの真の位置の値
χＴｏ”ゴ χ□。８　＝χ１ｏ十δＸ となる。実際は、χ、。８を求めるのに一回の計算で終
了するとは限らないので、 （χＴｏ）　Ｎ＋１−（χＴｏ）　Ｎ十δχ、′（但し
Ｎ−１，２，３，・・・）　　　・・・（７）として、
δφの絶対値１δφ１が十分小さくなるまで　（６）式
の計算を繰返して行ない、その結果の（χＴＯ）　Ｎ＋
１がχＴｏｘに等しいとして解とする。

上記位置推定用アルゴリズムを整理すると、以下のよう
に表される。

［ε、　Ｎｍｍｘ　　　　　　　　　　　　　設定］↓ ［ｉ＋、　　ｔ２．　　ｊ３．　　ｊ４　　　　　　　
　　　　　　　　　１錠コ↓ ［推定値ＸＴｏよ（Ｎ−１１設定］ ↓ ［φ、Ｎ−φ　（、Ｊ’ＴＯＮｌ　　　ｔ　＋　　）　
　　　　　　　　　　　計算］　ｊ　≦１〜４　；アル
ゴリズム↓ 上記位置推定用アルゴリズムは中央処理装置２３に格納
される。以下、第６図を参照して、第１図に示した装置
の動作を説明する。

まず、コマンド指令によって位置推定用アルゴリズムに
目標Ｔの運動方程式（（１）式）を登録する（ステップ
ａ）。次に観測時刻及びその回数Ｎを設定する（ステッ
プｂ）。

宇宙機Ｃが目標Ｔの可視領域に到達し、観測時刻になっ
たとき、画像センサ２１の出力データから目標Ｔの２次
元角度情報（方位角及び仰角）を求め、同時に宇宙機Ｃ
の位置及び姿勢を観測する（ステップＣ）。観測した位
置及び姿勢から宇宙機Ｃの座標系を作成し、この座標系
に目標Ｔの２次元角度情報を変換する（ステップｄ）。

さらに変換した２次元角度情報を登録した運動方程式に
代入する（ステップｅ）。

上記ステップｃ　−ｅをＮ回繰り返した後（ステップｆ
）、Ｎ個の運動方程式を位置推定用アルゴリズムにかけ
て未知のパラメータを求める（ステップｇ）。そして、
求めたパラメータを運動方程式に代入し、時刻と目標Ｔ
の推定位置との関係式を作成して、所望の観測時刻を代
入し、そのときの目標Ｔの位置を推定する（ステップｈ
）。

したがって、上記構成による位置推定装置を用いれば、
レーダのように特に４Ｉｌ］距機能を有する機器を用い
なくても、いったん未知のパラメータを求めてしまえば
所望の時刻の目標の位置を推定することができる。さら
に観ａｌ１１を継続してパラメータの算出値を更新すれ
ば、目標Ｔに近づくにつれ′Ｃ高精度の位置推定を行う
ことができる。このため、宇宙機Ｃには単に画像センサ
２１を外側に取り付けるだけなので、外部機器の搭載を
ほとんど制約しない。また、画像センサ２１を使用する
だけなので極めて消費電力が少なく、これによって被搭
載宇宙機Ｃの小型軽量化に供することができる。

尚、この発明は上記実施例に限定されるものではなく、
例えば電波を発して障害物を避ける自走ロボットや、自
動車の車線変更時の警告装置（自動車の斜め後方の自動
車が方向指示を行なったときにどこにいるかを推定して
、危険な位置にあれ−１９＝ ばブザーを発して運転者に警告する）等に利用すること
ができる。その他、この発明を逸脱しない範囲で種々変
更しても同様に実施可能であることはいうまでもない。

［発明の効果コ 以上のべたようにこの発明によれば、レーダのような測
距機能がなくても対象物の位置を推定することができ、
これによって外部機器の搭載を制約せずかつ極めて消費
電力が少なく、被搭載移動体の小型軽量化に供し得る対
象物の位置推定装置を提供することができる。

【図面の簡単な説明】

第１図はこの発明に係る対象物の位置推定装置の一実施
例を示すブロック構成図、第２図は同実施例の画像セン
サで目標を捕らえた様子を示す図、第３図はこの発明の
動作原理を説明するための図、第４図は同実施例の宇宙
機のとる軌道を示す図、第５図は同実施例の宇宙機と目
標との位置関係を示す図、第６図は同実施例の動作を説
明するためのフローチャート、第７図は従来の目標位置
検出装置の構成を示すブロック図である。 Ｃ・・・宇宙機、Ｔ・・・目標、１１・・・レーダ、１
２．２４・・・位置センサ、１３．２５・・・姿勢セン
サ、１４．２２・・・データ処理装置、１．５．２３・
・・中央処理装置。 出願人代理人　弁理士　鈴江武彦 ）−Ｕ　　　　　　　ｌ−（Ｊ

Claims (1)

【特許請求の範囲】
1. 移動体に搭載され、規則性をもって運動する対象物の位
   置を推定する対象物の位置推定装置において、前記移動
   体の位置情報及び姿勢情報を検出する移動体の状態検出
   手段と、前記対象物を画像として捕える画像センサと、
   前記対象物のもつ規則性を表わす運動方程式を予め登録
   し、複数回の観測時刻で、それぞれ前記画像センサの出
   力データからセンサ上に捕えられた対象物の方位及び仰
   角の２次元の角度情報を算出し、この２次元の角度情報
   を位置情報及び姿勢情報を基準にして前記運動方程式に
   代入し、各観測時刻の運動方程式によって方程式中のパ
   ラメータを求め、該パラメータを代入した運動方程式に
   任意の時刻を代入することによって、任意の時刻の対象
   物の位置を推定する演算装置とを具備する対象物の位置
   推定装置。