JPH0432710A - 姿勢の検出方法およびその装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、姿勢の検出方法とその装置に関し、詳しくは
３次元空間内で任意方向に連動する物体の変動角や姿勢
変化を検出する方法と装置に関するものである。

［従来の技術］ 従来３次元空間内を連動する物体の角度や位置は、予め
定めた基準となる方向や位置からの変位として計測され
る。すなわち物体に固定した座標系の中で変位置が定義
され、これらは例えばエン＝１−ダやポテンショメータ
によって高精度に計測することができる。

しかし、測定の基準となる方向が物体から物理的に切り
離された別空間に存在する場合、単体で物体と別空間と
関係づけることは困難であり、一般には磁気的、光学的
、電気的または音響的な空ｔバ１的手段を用いて双方が
関係つけられる。例えは、連動体の特定な方向が地球上
の東西南北に対してどの方向にあるかを検知する場合、
地磁気が利用される。また、磁気シールドされた空間内
においては地磁気を利用することができず、光や音波を
地磁気に置き換えて放射し、入力される方向を検知する
ことて連動体の方向を知ることかできる。

こうした方法は基準方法の入力を妨宵するものかない場
合適用てきるか、飛行機や潜水艦等物体か広域な領域を
連動する場合には外乱を受は易いため、連動体自身が３
次元空間に定めた方向の基準を記憶し、この記憶値から
のずれ量を計測して３次元姿勢を知る必要かある。

この３次元空間の方向基準を連動体自身に記憶させる方
法としてジャイロスコープかあり、機械的手段と電気的
手段とがある。＋？！械的手段としては一定速度て回転
する物体の回転軸か同じ方向を向き続ける性質、または
回転体に外力が加わる場合回転体の軸と外力に直角なコ
リオリカが仕しる性質を利用し、電気的手段としては光
路差を周波数差として検出するレー→ノ゛ジャイロや光
路差を位相差として検出するファイバオプティックジャ
イロ等かあり、特公昭！＋６−４７０３’）号公報に開
示されるように、このンヤイロ効果を利用して連動体の
姿勢制御を行なう方法か提案されている。

［発明か解決しようとする課題］ 」１記従来技術においては基本的にジャ・１口効果、あ
るいはサニヤック効果を利用した検出方法により、高い
精度や信頼性を得ることができるがコストが高く、空間
で何の拘束も与えない回転体を１７４成する茗・型性か
らセンサの’１４造か複雑になり、１・−ンヨ１ンバー
、フレクチャヒンジと呼はれる特殊な構成部品を必要と
する問題点か生しる。

またロボットの手足の制御においては、位置の他に方向
の制御か非常に重要な内容となっている。ところか、現
在ロボッ１−の手足等の方向検出に適したセンサかなく
、各関節の制御量を使った複雑な行列計算によって方向
を感覚している。この方法では、計算機に大きな負担を
かけ、高精度で高速な手足の制御は望めない問題点かあ
った。

そこで、本発明はこの問題を解決するしのとして、小型
で性能がよく、構造が簡単で安価な姿勢の検出方法およ
びその装置を提供することを目０勺とする。

［課題を解決するための手段］ 本発明はフライホイールを定速連続回転させ、フライホ
イール駆動用電動機の取り付は基板に一体ｆヒされたセ
ンサによって、フライホイール面、もしくはフライホイ
ールと機械的に連動して回転する円板面の特定方向の通
過を検出し、該特定方向を繰り返し検出するタイミング
の周期と姿勢変化のない静姿勢状態下における該特定方
向の繰り返し周期との差を求め、逐一計算される周期差
を累計することにより、３次元空間でフライホイール軸
周りに回転する基板の角度を測定可能にしたものであり
、３次元空間で互いに直角な２軸もしくは３軸の周りに
フライホイールを独立して定速連続回転させ、各フライ
ホイール駆動用電動機の取り付は基板に一体化されたセ
ンサによって、フライホイール面、もしくはフライホイ
ールと機械的に連動して回転する円板面の特定方向の通
過を検出し、該特定方向を繰り返し検出するタイミング
の周期と姿勢変化のない静姿勢状態下における該特定方
向の繰り返し周期との差をそれぞれ求め、逐一計算され
る周期差をそれぞれ累計することにより、３次元空間で
各フライホイール軸周りに回転する基板の角度を測定可
能にしたものであり、電動機と、フライホイールと、光
学的、磁気的、電気的手段のいずれかによる標的検出セ
ンサと、標的の現れる周期検出回路と静姿勢状悪検出回
路と定常周期読込回路と周期差ｈ１算回路と周期差累計
回路とて構成され、フライホイール軸周りに回転する角
度を測定可能にしたものである。

［作　用］ 本発明の姿勢検出方法によれば、回転体は−ａ的な方法
で支持され、ジンバルｔｆ４造はもとより、トーション
バーやフレクチャヒンジと呼ばれる特殊な機構部品を必
要としない。

また３次元空間のＩＩ意方向ｌ＼の姿勢変化を検出する
場合でも、互いに直交する軸方向にフライホイールと電
動機を取り付け、電動機を連続回転させたときのフライ
ホイールあるいはフライホイールに連動さぜな円板の特
定方向の通過を検出し、方向測定の基準を従来の信号処
理技術で記憶し、この基準からの変（ｉ角を出力て′き
る。すなわちフライホイールもしくはフライホイールと
機械的に連動して回転する円板の特定方向の通過を、光
学的、磁気的１電気的手段等のいずれかによって検出し
、繰り返し検出される特定方向の周期と、姿勢変化のな
い静姿勢状態下における該特定方向の繰りコヌし周期と
の差を求め、逐一計算される周期の差を累計することに
より、フライホイール軸周りに回転する基板の角度を測
定可能にする。さらに、電動機とフライホイールとセン
サは平面内の角変位検出時に２組、３次元空間内の角変
位検出時に３組用意するだけで基本的な処理かできる。

しかも、これらの和は、集中して設置される・ｇ・要か
°ないので組立や基板ｌ＼の絹込みか簡単になる。

［実施例］ 以下、本発明の実施例を添イ１図面を参照して説明する
。

第１図及び第２図は特定面内の姿勢変化を検出する第１
実ｈｌ！例の概略説明図であり、第１図においてｌ及び
２はフライホイール、３及び４はフライホイール１及び
２をそれぞれ回転させる電動機、５及び６はフライホイ
ール１及び２上に定めた特定方向の通過を検ｉ１するセ
ンサであり、電動機３及び４と共に基板７に取りイで１
け固定される。電動機３及び４に常時一定の電圧を印加
することによりフライホイール１及び２はそれぞれ矢印
の通り、互いに逆方向に回転する。このような状態では
、フライホイール】及び２は回転惰性力により速度を変
えずに３次元空間内で回り続けるなめ、センサ５または
６にて検出される信号は基板７の姿勢か変わらない限り
、一定周期で繰り３４ず。

第２図は基板７か矢印方向にある角度αだけ回転した場
合を示しており、この状態ではセンサ５がフライホイー
ル１士の特定点と再ひ出会うのは（１＋α／２π）回転
後てあり、またセンサ６ては（１−ｒｘ／’２π）回転
後である。基板７か３次元空間内において静止した静姿
勢状態にある場合には第３図（ｂ）に示すように、セン
サ５及び６の出力波形は共に一定周期で繰り遅ずか、第
２図のように基板７が回転し姿勢に変化を生じると、セ
ンサ５及び６の出力波形は第３図（ａ）及び第３図（ｃ
）に示すようにそれぞれ変化する。ずなわち、Ｔｂ　ｉ
　＜Ｔ”ｏ＜Ｔａ　ｉとなる。たたし、センサ５の出力
波形の信号周期を’Ｔ”　ａ　ｉ、センサ６の出力波形
の信号周期をＴｂｉとし、特に基板７か静姿勢状態にあ
るときのセンサ５または６の出力波形の信号周期をＴｏ
とする９ 以」二の現象に着１川すると、いずれか一方のセンサ（
以下、センサ５どする）から得られる信号の周期′Ｉ″
ａ１と定常周期′Ｉ゛０とを比較して増減する地金累計
することにより、基板７の変化方向を知ることかてきる
。すなわち３次元空間内にお（）る基板７の回転角度α
か次式で表される。

α＝Σｋ（Ｔｏ　−’ｌ″ａｉ） たたし、ｋは静姿勢状態の周期′Ｉ゛０を用いて２π／
　’ｆ’　Ｏと表される０本発明は上式に基づき、３次
元空間内における基板７の回転角度αを検出するもので
あるか、基板７が定速て回転変１ヒする場合にもＴａ（
ｉ　＋１）−’Ｔ’ａｉ′Ｉ″ｂ（ｉ　＋１）−′ｒ’
　ｂｉとして上式が適用できる。

第４図は本発明を実現する装置のフロック構成を示し、
１１はセンサ５の、また１２はセンサ６の出力信号であ
り、それぞれの周期検出回路１３ａ、　１３ｂにて周期
Ｔａｉ、　Ｔｂｉを検出する。１４は静姿勢状態検出回
路であり、上式における静姿勢状態時の定常周期＝ｒ’
ｏを定め、３次元空間内で基板７の方向に変化かないと
き、ずなわらＩ’ａｌ、　ｌ”ｌ）ｉか殆んど等しく双
方の差が零近傍のとき、後述する定常周期読込回路への
駆動信号１５を出力する。１６は定常周期コた巡回路で
あり、駆動信号１５にて起動し周期検出囲路１３１１に
より得られた周期を静姿勢状態時の定常周期１゛０とし
て読込み記憶する。

１７は周期差計算回路であり、周期検出回路１３ｒ１に
よって検出される周期′ｌ″ａｉと、定常周期読込回路
１６に記憶さｈている周期ＴＯとの差を計算する。また
１８は周期差累計回路であり周期差計算回路１７て求ま
る値を累計する。そして時間とｊｔ＋　＋Ｕの比かＴ　
ｏ　／　３６０　’　として与えられるので、周期差累
計回路１８の出力から得ｔ、れる周期差の累計値に一定
の値（前式のｋ）を東しることて姿勢ｊすａを容易に求
めることかできる。

第５図及び第（４図は静姿勢状態検出回路１４の詳細を
示しており、第５図は周波数−電圧変換器（以下＋ｒニ
アｖ変換器Ｊという）を用いて横１戊した回路のフロラ
フレ１て゛ある。２１ａ２１ｂはＩ＝”　／　Ｖ変換器
てあり、それぞれ出力信号４１．１２の信号周期を電圧
レベルに変換し、差動増幅器２２にて２つの信号の電圧
レベル差を検出する。後者の大小識別回路２３はレベル
差が所定値よりも低い場合、静姿勢状態にあるものとみ
なして、定常周期Ｅｖｅ込回路１６／＼の駆動信号１５
を出力する。

また、第（１図は通常の論理回路を用いてｈ子姿勢状態
検出回路１４の詳細を１”育成したしのである。２４．
２５．２Ｇは共にフリリブフロップ回路であり、パルス
か入力されることに出力信号の符号を反転さぜる機能を
ｆｌｉｉｉえている。２７は高周波パルス発生器であり
、センサ出力信号１１及び１２の周期の増減量を３４測
するための最小単位パルスを供給する。このパルスの周
期は後述するようにセンサ出力信号１１及び１２の周期
と比へて短いほど角用の検出精度を高めることかできる
。２８は論理ゲート回路て′あり、センサ出力信号１１
の周期′ｖ　ａ　ｉがセンサ出力信号１２の周期Ｔｂｉ
よりも長い場合、アップタウンカウンタ２９のア・ノブ
側端子に双方の周期差分に相当する数の最小単位パルス
を供給し、またセンサ出力信号１２の周期Ｔｂ＋かセン
サ出力信号１１の周期ｉ”　ａ　ｉ　Ｊ−りも長い場合
、・アップタウンカウンタ２９のダウン側端子に双方の
周期差分に相当する数の最小単位パルスを供給する。ま
た、’Ｉ’ａ　ｌ＝’Ｔ’ｂ　ｊの場合、パルスの供給
を市める。論理ケート回路２８はさらに、アップタウン
カウンタ２９にプリセット信号を、後述するサンプルホ
ールド回路にサンプル信号を供給する。アップタウンカ
ウンタ２９はアリセソ■・信号を入力するとカウント値
をクリアする。３０はサンプルホールド回路回路て′あ
り、アップタウンカウンタ２９から出力されたパルス数
をサンプル信号が供給されている間読み込む機能を有す
る。Ｉｉ（つで、もしアップダウンカウンタ２９かちの
カウント値が所定値よりも低い場合、大小識別回路２３
から定常周期読込回路１６／＼の１枢動信号１５を出力
する。

第７図は第４図に示す周期型計算回路１７と周期差累計
回路１８をパルスをカウントする方法により構成したも
のである。３１はカウンタてありフリップフロップ回路
３２にて出力されたパルス信号３３によって制御される
。具体的にはパルス信号３３のオン時間（正符号期間）
に入力ゲートをオープンし２、高周波パルス発生器２７
からの最小単位パルス数をカウントすることによりパル
ス信号３３の幅を定める．大小比較部３４てはカウンタ
３１の計測値Ｐ１と定常周期カウント値ｐｗとの大小が
逐一比較され、ＰＷ＜Ｐｉの場合アップタウンカウンタ
３５のアップケート３６を開き、ＰＷ＞Ｐｉの場合アッ
プタウンカウンタ３５のタウンゲート３７を開く信号を
出力する．ただし、ＰＷは定常周期読込回路１６に記憶
された数で、定常周期Ｔ　Ｏ期間内で引ａ！ｑされる最
少単位パルス数をさす。ずなわら第８図に示すように定
常周期ＴＯとフリップフロップ回路の出力信号のオン期
間′［ｉとが比較され、センサ５から得られる信号周期
１”　ａ　ｉか長くなるとき大きな値が、また信号周期
′Ｉ″ａｉが短くなるとき小さな値かアップタウンカウ
ンタ３５内に記録される。尚、アップタウンカウンタ３
５のケート制御信号かカウンタ３１の３１数値Ｐ　ｉ　
ｆ用いて作られているか、これはカウンタ３１か高周波
パルス発生器２７からの最小単位パルス数をカウントし
つつ、アップタウンカウンタ３５のグーｌ−操作を実時
間で行うためである．カウンタ３１はパルス信号３３の
立ち上かり部分を使って内容をクリアし、その後直ちに
カウントを開始し、パルス信号３３の立ら下かりてカウ
ント値を保持する．アップタウンカウンタ３５のリセッ
ト入力は姿勢検出の品切の段１苛て与えられ、３次元空
間での姿勢の方向基準を記憶させる機能をもつ。このリ
セット人力で以後記憶された方向を基準にした方向かア
ップタウンカウンタ３５から出力されるか、リセット入
力を与え直すことで方向の基準は変えられる。新しく記
憶される方向の基準はリセットを入力しな時点の基板７
の姿勢である。尚、３８は１〕／Ａ変換器であり検出し
た姿勢角ａをアナログ表示させるものである。

第０１乃至第１１図は３次元の姿勢を検出する第２実施
例を示し第１実施例と同一部分に同一符号を付し、同一
箇所の贋、明を省略して説明すると、第９図は装置のフ
ロック構成を示し、図示しない電動機は互いに直交する
軸周りにそれぞれ設け、３９，　４０．　４１はそれぞ
れ３次元？；！間をＸ，Ｙ，７．の直角座標で表現した
ときのＸ１１１＋，ｙ軸，Ｚ軸に平行な回転軸を持つ図
示しないフライホイール面」二の特定点を検出するセン
サである。周期検出回路１３。

定常周期読込回路１６，周期差計算回路１７，周期差累
計回路１８の内容及び具体的ｔｆ４成は第４図て示した
ものと同じであるか、静姿勢状態検出回路４２のみ異な
る。ずなわら３次元空間内て基板か姿勢を変えない師姿
勢状態は、ＳＸ，Ｓｙ，Ｓｚのずｌ＼ての周期に変化が
生じない場合に検出されることから、第１０図に示す回
路を用いて検出される９静姿勢状態検出回路４２は、第
１０図に示すようなＦ／Ｖ変換器２１ａ　、　２１ｂ　
、　２１ｃを使わない論理回路でも構成できることは第
６図と同じである。また図示しない３つの電動機に供給
する電圧が非當に安定しており、回転摩擦が姿勢変化に
対していずれも変わらない場合、図示しない３つのフラ
イホイールの回転はいずれも定速とみなすことかできる
ので第９図の構成は、第１１図のように簡単になる。

尚、本発明は上記実施例に限定されるものではなく本発
明の要旨の範囲内において種々の変形実施が可能である
。例えは基板の回転角度をディジタル表示させる場合、
第７図に示すＤ／Ａ変換器を取り除くことができる。

また電動機に供給する電圧を共通にし、しかも各電動機
の回転数を等しくｇＡ整することにより周期差計算回路
や静姿勢状態検出回路をより簡単な論理回路で構成てき
る。さらにセンサはフライホイール、またはこれと連動
して回転する円板上の特定方向に光学的、磁気的、電気
的手段のいずれかによる標的を設置し、該標的の通過を
非接触で検出する方法でもよい。また用途としては、産
業用ロボット、飛翔体移動体などの姿勢制御ｌ＼の工学
的応用のほかに、身体障害者の姿勢矯正装置などへの医
療福祉的応用も考えられる。

［発明の効果］ 本発明はフライホイールを回転させる電動機に一定の電
圧を印加しておきさえずれば、電動機を取り付ける基板
の姿勢が変化してもフライホイールは３次元空間内を一
定の速度で回転し続けるという慣性力効果を利用し、フ
ライホイールの特定方向に標的を設置し、基板上のセン
サてこの標的の通過時期を光学的、磁気的、あるいは電
気的に検出し、基板がフライホイールの軸周りに回転し
たときに標的を検出する周期が増減することに着目して
なさされたもので、姿勢か変化しないときに検出される
フライホイールの回転周期を定４゜ 雷同１１Ｊ１と定め、定雷同ＪｊＪｌとその都度検出さ
れる周７ｉＪｌとの差を逐一計算して累Ｊ１シ、定常周
期か角度３６０　”に対応する関係を使って累計値を角
度に変換し、その結果を変位角として出力する３次元姿
勢検出法とその装置に関したもので、ｊｊ！ｌｌ定基準
となる３次元空間の特定方向を累計値か零になる力面と
して配信保持させ、またその方向からずれる角を連続し
て出力て′き、特に磁気シールドされた環境や、−１−
下、左右１前後、東西南北τ？・の方向を識別１−る手
掛かりを得ることが困ｉｌｌなＩｎＩの中、水の中、管
の中、そしてカメラＭ％から方向感覚を得ることか困■
＃な閑ざされた視界の環境において、姿勢に係わるロボ
ットの自立制御。

走行制御２歩行の制御、あるいは飛翔体の動的に安定な
制御を高精度かつ高速に実現するうえに大きな効果を有
する。

【図面の簡単な説明】

第１図は本発明の第１実施例を示す正面側の概略説明図
、第２１図は第１実施例の平面０１すのＲ略説四層、第
３１λ１はセンサの出力信号１λｊ、第４図は特定面内
の角度検出のブロック図、第５し１はｌ・”　／’　Ｖ
変換器を使った静姿勢状那検出回路のフロック図、第６
図はＦ／Ｖ変換器を使わない静姿勢状態検出１ｆｉｌ　
！？１のフロック図、第７［Ｊは周期差計算回路及び周
期検出回路のフロック図、第８図はセンサ周期図であり
、第９図乃至第１１図は本発明の第２実施例を示し、第
９し１及び第１１し］は３次元姿勢検出のフロック図、
第１０図は３次元姿勢状態検出のフロック図である。 】、２　フライホイール ３．４・・電動機 ５、６．３９．４０　４１　　センサ ７・基板 １３ａ　、　１３ｂ　、　１３ｃ　＝−周期検出回路１
４．４２・静姿勢状態検出回路 １６・・・定常周期読込回路 １７・ 周期差計算回路 １８・・周期差累計回路 特 許 出 願 人 岡 徳 次 代 理 同 株式会社 遠藤製作所 人 Ｏｆ里士 牛 木 護 第３図 第５図 第９図 第１０図 出力 出力 出力 第１ 図

Claims (1)

1. 【特許請求の範囲】 （１）フライホィールを定速連続回転させ、フライホィ
   ール駆動用電動機の取り付け基板に一体化されたセンサ
   によつて、フライホィール面、もしくはフライホィール
   と機械的に連動して回転する円板面の特定方向の通過を
   検出し、該特定方向を繰り返し検出するタイミングの周
   期と姿勢変化のない静姿勢状態下における該特定方向の
   繰り返し周期との差を求め、逐一計算される周期差を累
   計することにより、３次元空間でフライホィール軸周り
   に回転する基板の角度を測定することを特徴とする姿勢
   の検出方法。 （２）３次元空間で互いに直角な２軸もしくは３軸の周
   りにフライホィールを独立して定速連続回転させ、各フ
   ライホィール駆動用電動機の取り付け基板に一体化され
   たセンサによつて、フライホイール面、もしくはフライ
   ホィールと機械的に連動して回転する円板面の特定方向
   の通過を検出し、該特定方向を繰り返し検出するタイミ
   ングの周期と姿勢変化のない静姿勢状態下における該特
   定方向の繰り返し周期との差をそれぞれ求め、逐一計算
   される周期差をそれぞれ累計することにより、３次元空
   間で各フライホィール軸周りに回転する基板の角度を測
   定にすることを特徴とする姿勢の検出方法。 ３）電動機と、フライホィールと、光学的、磁気的、電
   気的手段のいずれかによる標的検出センサと、標的の現
   れる周期検出回路と静姿勢状態検出回路と定常周期読込
   回路と周期差計算回路と周期差累計回路とで構成され、
   フライホィールを取り付ける基板が３次元空間でフライ
   ホィール軸周りに回転する角度を測定することを特徴と
   する姿勢の検出装置。 （４）前記静姿勢状態とは、フライホィールの回転軸と
   平行な回転軸の周りに別の電動機によって別のフライホ
   ィールを逆方向にほぼ等速で回転させ、フライホイール
   の特定方向を基板に一体化されたセンサで検出し、２つ
   のフライホィールの特定方向を繰り返し検出する周期の
   差が零近傍にある状態であると定めたことを特徴とする
   請求項（１）記載の姿勢の検出方法 （５）フライホィールをほぼ等速で回転させ、静姿勢状
   態を２軸もしくは３軸の周りに回転するフライホィール
   面、もしくはフライホィールと機械的に連動して回転す
   る円板面の特定方向の通過を検出するタイミングの周期
   間に変化がない状態であると定めたことを特徴とする請
   求項（２）記載の姿勢の検出方法。