JPH07234722A - 飛行体の姿勢制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 角速度中の誤差を差し引きジャイロを用い自
動姿勢制御を行う。 【構成】 角速度センサ12，14，16の出力から得た角度
（θ1） に加速度センサ11，13、地磁気方位センサ15の
角度｛φ（X，Y，Z）｝ をある割合（１−α）で加算し
角度（θ2）を求める混成現在姿勢角演算手段29を備え
る。θ2が０度付近のときから時間（ｔ1）経過後の角度
差（Δθ1）に対する時間（ｔ2） での角度差（Δθ2）
の増加分（δ）から角速度中の誤差（ωd）を求める角
速度基準値演算手段26を備えた。真の角速度を積分して
機体の回転角度を求め、この回転角度から機体の姿勢を
検出して正確に姿勢制御が行われる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、無線操縦される無人ヘ
リコプタの姿勢制御を自動で行う飛行体の姿勢制御装置
に関するものである。

【０００２】

【従来の技術】従来、例えば農薬散布用の無線操縦式無
人ヘリコプタは、操縦者からのパイロット指令信号によ
って機体の方位、傾斜角度等を遠隔操作によって制御す
る構造になっていた。そして、この種の無人ヘリコプタ
では、操縦を容易に行うことができるように、パイロッ
ト指令信号によって定められた方位、傾斜角度等に対し
て機体の実際の方位、傾斜角度等が風等の外乱によって
変化したとしても機体の姿勢を自動的に修正する姿勢制
御装置を設けることが望まれていた。

【０００３】この姿勢制御装置としては、機体の地球に
対する実際の傾斜角度をセンサによって検出し、この検
出値と、パイロット指令信号によって設定された目標値
との偏差が零となるようにヘリコプタの各種サーボモー
タを制御するように構成すればよい。詳述すると、機体
の左右、前後の主方位となる軸線が鉛直方向に対してど
れだけ傾斜しているかを加速度センサからなる傾斜計に
よって検出すると共に、機体が鉛直軸回りに地球の基準
方位に対してどれだけ回っているかを地磁気方位センサ
によって検出し、これらの検出値を前記目標値に近づけ
る制御を行えばよい。

【０００４】

【発明が解決しようとする課題】しかるに、加速度セン
サを用いて機体の左右、前後の鉛直方向に対する傾斜角
を検出するのでは、加速度センサはヘリコプタが飛行し
ているときには飛行に伴って生じる加速度の影響を受け
るために正確な傾斜角を測定することができない。この
ため、上述した自動姿勢制御を行うと精度が低くなって
しまうという問題があった。

【０００５】また、加速度センサに代えてジャイロ等の
角速度センサを採用し、この角速度センサから出力され
る角速度を積分して角度を求めることも考えられるが、
角速度センサでは初期値に対する角度の変化分しか求め
ることができないので、鉛直方向に対する傾斜角度は求
めることができない。その上、ジャイロの温度が増減す
ること等に起因して角速度値の零点が変化してしまうと
いう問題もあった。この零点変化に基づく誤差は、例え
零点変化が微小であっても長期間積分を行うことにより
非常に大きくなってしまう。

【０００６】本発明はこのような問題点を解消するため
になされたもので、ジャイロ等の角速度センサの零点が
変化したとしてもそれを修正し、ジャイロを用いて検出
した角度を利用して自動姿勢制御を正確に行うことがで
きるようにすることを目的とする。

【０００７】

【課題を解決するための手段】第１の発明に係る飛行体
の姿勢制御装置は、機体が前後、左右、上下方向の主方
位の基準軸に対して回るときの角速度を検出する角速度
センサと、地球に対する機体の主方位の傾斜角および方
位角を検出する姿勢角センサと、前記角速度センサが検
出した角速度を積分して角度（θ1） を求める角速度セ
ンサ用角度算出手段と、前記姿勢角センサが検出した角
度｛φ（X，Y，Z）｝ を予め定めた割合（１−α）をも
って加算して混成姿勢角（θ2） を演算する混成姿勢角
演算手段と、前記混成姿勢角（θ2） が０度付近である
ときに計時を開始して予め定めた時間（ｔ1）だけ経過
した後の角度差（Δθ1＝θ1−θ2）に対するその一定
時間経過後の時間（ｔ2）での角度差（Δθ2＝θ1−θ
2）の増加分（δ＝Δθ2 −Δθ1）から前記角速度セン
サが検出する角速度中に含まれる誤差（ωd）を、N2＝
ｔ2／Δｔ、N1＝ｔ1／Δｔ、Δｔを演算サイクル、αを
混成姿勢角（θ2） 中の角度（θ1）の割合とし、 ωd（X，Y，Z）＝δ／｛（ｔ2−ｔ1）＋（α^N2−α^N1）
αΔｔ／（１−α）｝ なる数式に基づいて算出する誤差演算手段と、前記角速
度センサが検出した角速度から前記誤差（ωd） を減算
してなる真の角速度を積分して機体の主方位に対する回
転角度を求める姿勢角演算手段と、この姿勢角演算手段
が求めた回転角度に基づいて機体の姿勢を制御する機体
制御手段とを備えたものである。

【０００８】第２の発明に係る飛行体の姿勢制御装置
は、第１の発明に係る飛行体の姿勢制御装置において、
姿勢角演算手段を、離陸以前に地球に対する機体の主方
位の傾斜角および方位角を姿勢角センサによって検出
し、この傾斜角および方位角に、真の角速度から求めた
機体の回転角度を加算して機体の姿勢を検出する構成と
したものである。

【０００９】

【作用】第１の発明では、角速度センサによって検出さ
れた角速度中に含まれる誤差（ωd） が誤差演算手段に
より算出され、前記角速度からこの誤差（ωd） を差し
引いてなる真の角速度を積分して機体の回転角度を求
め、この回転角度から機体の姿勢を検出して姿勢制御が
行われる。

【００１０】第２の発明では、離陸以前に求めた地球に
対する傾斜角および方位角に、前記真の角速度から求め
た回転角度を加算して機体の姿勢を検出し、姿勢制御が
行われる。

【００１１】

【実施例】以下、本発明の一実施例を図１ないし図５に
よって詳細に説明する。図１は本発明に係る飛行体の姿
勢制御装置を搭載した無人ヘリコプタの概略構成図、図
２は本発明に係る飛行体の姿勢制御装置の全体構成を示
すブロック図、図３は要部の構成を示すブロック図、図
４は本発明に係る飛行体の姿勢制御装置の誤差検出手法
を説明するためのグラフ、図５は同じく誤差検出動作を
説明するためのグラフである。

【００１２】これらの図において、１は無人ヘリコプタ
の機体、２はメインロータ、３はテールロータ、４は前
記メインロータ２およびテールロータ３を回転駆動する
エンジンである。５はこのエンジン４の回転数を制御す
るエンジンコントローラサーボモータ、６は前記メイン
ロータ２の傾斜角やピッチ角を制御するコレクティブサ
ーボモータ、７は前記テールロータ３のピッチ角を制御
するラダーサーボモータで、これらのサーボモータ５〜
７は本発明に係る姿勢制御装置としてのコントローラ８
によって制御される構造になっている。

【００１３】９は機体１に搭載された受信機で、この受
信機９は送信機１０が発信したパイロット指令信号を受
信部９ａが受信してコントローラ８に出力すると共に、
コントローラ８からの制御信号を前記サーボモータ５〜
７に伝えるアンプ９ｂを内蔵している。なお、この機体
１には、図示してはいないが農薬を空中から散布するた
めの農薬散布装置が装着されている。

【００１４】前記コントローラ８は、機体１の互いに直
交する３つの主方位（左右、前後および上下方向）に対
する角度や機体の高度、鉛直方向に対する加速度等を後
述する各種センサを使用して検出し、送信機１０から送
られたパイロット指令信号によって設定された目標飛行
状態となるように制御する構造になっている。ここで、
前記センサとしては、機体１の左右方向の軸線（Ｘ軸）
回りの角度を検出する傾斜計としての加速度センサ１１
および角速度センサ１２と、機体１の前後方向の軸線
（Ｙ軸）回りの角度を検出する傾斜計としての加速度セ
ンサ１３および角速度センサ１４と、機体１の鉛直方向
の軸線（Ｚ軸）回りの角度を検出する地磁気方位センサ
１５および角速度センサ１６と、機体１の鉛直方向に対
する加速度を検出するための昇降センサ１７と、機体１
の高度を検出するための高度センサ１８と、エンジン４
の回転数を検出するエンジン回転数検出センサ１９（図
２）である。前記加速度センサ１１，１３および地磁気
方位センサ１５が本発明に係る変位検出用センサを構成
している。

【００１５】これらのセンサのうちＸ軸の加速度センサ
１１は機体１のＹ軸が鉛直方向に対して何度傾斜してい
るかを検出し、Ｘ軸の角速度センサ１２は機体１がＸ軸
回りに回転するときの角速度を検出するように構成され
ている。また、Ｙ軸の加速度センサ１３は機体１のＸ軸
が鉛直方向に対して何度傾斜しているかを検出し、Ｙ軸
の角速度センサ１４は機体１がＹ軸回りに回転するとき
の角速度を検出するように構成されている。

【００１６】さらに、地磁気方位センサ１５は例えば機
体のＹ軸が北方位に対して何度回っているかを検出し、
Ｚ軸の角速度センサ１６は機体１がＺ軸回りに回転する
ときの角速度を検出するように構成されている。加え
て、昇降センサ１７は機体１のＺ軸方向に対する移動方
向および移動速度を検出するように構成され、高度セン
サ１８は機体１と地表との距離を光学的に検出するよう
に構成されている。さらにまた、エンジン回転数検出セ
ンサ１９は、エンジン４のクランク軸（図示せず）の回
転を検出するように構成されている。

【００１７】なお、前記各角速度センサ１２，１４，１
６としては、本実施例では光ファイバジャイロが採用さ
れている。

【００１８】コントローラ８は、図２に示すように、機
体１の実際の姿勢角を前記各種センサの出力値から演算
する姿勢角演算装置２１と、この姿勢角演算装置２１に
よって求められた機体１の実際の姿勢角を目標姿勢角に
近づけると共に、飛行方向、エンジン回転数および高度
を目標値に近づけるように前記サーボモータ５〜７を制
御する飛行状態制御用ＣＰＵ２２等から構成されてい
る。この飛行状態制御用ＣＰＵ２２は、送信機１０から
送られたパイロット指令信号に基づいて目標姿勢角、目
標飛行方向および目標飛行速度等の目標値を演算する目
標値演算処理手段と、上述したように前記サーボモータ
５〜７を制御する各アクチュエータ制御量演算処理手段
を備えている。なお、この飛行状態制御用ＣＰＵ２２が
本発明に係る機体制御手段を構成している。

【００１９】前記姿勢角演算装置２１は、図３に示すよ
うに、各種演算を行うＣＰＵ２３と、このＣＰＵ２３に
前記各センサおよび前記飛行状態制御用ＣＰＵ２２を接
続するインターフェイスとから形成されている。この姿
勢角演算装置２１のＣＰＵ２３は、前記加速度センサ１
１，１３および地磁気方位センサ１５から機体１の地球
に対する傾斜角および方位を求める傾斜角・方位角演算
手段２４と、前記角速度センサ１２，１４，１６の出力
から求めた角度および前記傾斜角・方位角演算手段２４
が求めた角度から後述する演算を行うドリフト・混成現
在姿勢角演算処理部２５と、角速度センサ１２，１４，
１６によって検出された角速度中に含まれるオフセット
誤差ωd を演算によって求めてこのオフセット誤差ωd
を角速度基準値として設定する角速度基準値演算手段２
６と、前記傾斜角・方位角演算手段２４が求めた角度並
びに各角速度センサ１２，１４，１６の出力値、前記角
速度基準値演算手段２６が求めた角速度基準値ωd から
機体１の現在の姿勢角を求める現在姿勢角演算手段２７
とから構成されている。前記角速度基準値演算手段２６
が本発明に係る誤差演算手段を構成し、前記傾斜角・方
位角演算手段２４と前記現在姿勢角演算手段２７とで本
発明に係る姿勢角演算手段が構成されている。

【００２０】ここで、前記角速度基準値演算手段２６が
求めるオフセット誤差ωd とは、角速度センサ１２，１
４，１６の温度が増減すること等に起因して角速度値の
零点が変化するときの零点変化分のことである。なお、
本明細書中では、角速度センサの出力値を積分して求め
た角度が前記オフセット誤差ωd の影響により次第に大
きくなることをドリフトと称して説明する。

【００２１】前記傾斜角・方位角演算手段２４は、前記
加速度センサ１１，１３および地磁気方位センサ１５の
出力値に基づいて機体１の地球に対する姿勢を演算し、
機体１のＸ，Ｙ軸の鉛直方向に対する傾斜角度φX，φY
と、例えばＹ軸の北方位に対する角度φZ とを検出する
ように構成されている。詳述すると、この傾斜角・方位
角演算手段２４は、機体１が離陸以前であって静止した
状態にあるときに前記角度の離陸前初期値としてφX，
φY，φZ を検出し、この初期値を不図示のメモリに記
憶させる。機体１が静止した状態であることを検出する
には、角速度センサ１２，１４，１６が一定時間の間に
予め定めた値より小さい値を継続して出力したことを検
出することによって行う。また、この傾斜角・方位角演
算手段２４は、機体静止状態以外のときには常に角度φ
X，φY，φZ をドリフト・混成現在姿勢角演算処理部２
５に出力する構造になっている。

【００２２】前記ドリフト・混成現在姿勢角演算処理部
２５は、図３に示すように、本発明に係る角速度センサ
用角度算出手段としてのドリフト現在姿勢角演算手段２
８と、本発明に係る混成姿勢角演算手段としての混成現
在姿勢角演算手段２９とを備えている。

【００２３】ドリフト現在姿勢角演算手段２８は、角速
度センサ１２，１４，１６によって検出された角速度ω
X，ωY，ωZ をそれぞれ積分してその時々の角度θ1X，
θ1Y，θ1Zを演算するように構成されている。この角度
のことを以下においてドリフト現在姿勢角という。これ
らの３軸に対するドリフト現在姿勢角のうち一つを代表
させて仮にθとすると、ｎサンプリング時刻でのドリフ
ト現在姿勢角θn は下記の（１）式のようになる。 θn＝θn-1＋Δθn・・・（１） ここで、Δθn は角速度の積分による角度増分で、ωｎ
Δｔ（ω：角速度、Δｔ：単位積分時間）のことであ
る。ドリフト現在姿勢角演算手段２８は、前記（１）式
に基づいてＸ，Ｙ，Ｚ軸毎にドリフト現在姿勢角を求め
るように構成されている。

【００２４】混成現在姿勢角演算手段２９は、前記ドリ
フト現在姿勢角演算手段２８が求めたドリフト現在姿勢
角θ1X，θ1Y，θ1Zに、前記傾斜角・方位角演算手段２
４が求めた角度φX，φY，φZ を予め定めた割合をもっ
て加算してその時々の混成現在姿勢角θ2X，θ2Y，θ2Z
を演算するように構成されている。これらの３軸に対す
る混成現在姿勢角のうち一つを代表させて仮にθとする
と、ｎサンプリング時刻での混成現在姿勢角θn は下記
の（２）式のようになる。なお、角度φX，φY，φZ も
その一つを代表させてφn で示し、予め定めた割合をα
で示す。 θn（X，Y，Z）＝α（θn-1＋Δθn）＋（１−α）φn・・・（２）

【００２５】前記角速度基準値演算手段２６は、前記
（１）式と（２）式から下記に説明する手法によってオ
フセット誤差ωd を求めるように構成されている。ここ
で、オフセット誤差ωd を求める手法について説明す
る。

【００２６】前記（１）式で求められたドリフト現在姿
勢角にオフセット誤差ωd が含まれているとすると下記
の（３）式のようになる。 θn＝θn-1＋Δθn＋Δｔωd・・・（３） この（３）式を時刻０からｎサンプリング時刻の間で展
開すると、下記の（４）式のようになる。 θn＝θ0＋（ωn＋・・・＋ω1）Δｔ＋ｎΔｔωd・・・（４） なお、ωは角速度、ωd はΔｔ時間内のオフセット誤差
である。この式（４）をグラフ化すると図４中の実線の
ようになる。なお、図４では角速度ω、角度θ0，φn、
オフセット誤差ωd を一定として示してある。同図から
も分かるように、オフセット誤差ωd があると角度θは
その分、経過時間と共に増大する。

【００２７】一方、前記（２）式で求められた混成現在
姿勢角にもオフセット誤差ωd が含まれているとすると
下記の（５）式のようになる。 θn＝α（θn-1＋Δθn＋Δｔωd）＋（１−α）φn・・・（５） そして、この（５）式を時刻０からｎサンプリング時刻
の間で展開すると下記の（６）式のようになる。

【００２８】

【数１】 この（６）式から分かるように、角速度ωや角度θが０
であったとしてもオフセット誤差ωd があると第４項目
が残り、角度θn は０にはならない。ｎが大きくなると
αn→０に収束するため、ある定数のα／（１−α）Δ
ｔωdの値に収束していく。この式（６）をグラフ化す
ると図４中の一点鎖線のようになる。

【００２９】ここで、前記（４）式で求められる角度を
θ1 とし、（６）式で求められる角度をθ2 とすると、
図４中に示す時刻ｔ2での（θ1−θ2）の値は時刻ｔ1で
の（θ1−θ2）の値より増大している。この増大してい
る分がオフセット誤差ωd によるものである。（θ1−
θ2）の値は前記（４）式および（６）式から下記の
（７）式のようになる。

【００３０】

【数２】 そして、時刻ｔ2と時刻ｔ1でのθ1−θ2の差（角度差増
加分δ）は、下記の（８）式のようになる。 δ＝（θ1−θ2）ｔ₂−（θ1−θ2）ｔ₁・・・（８） この（８）式のθ1に前記（４）式を代入すると共に、
θ2の前記（６）式を代入すると、下記の（９）式のよ
うになる。

【００３１】

【数３】 この（９）式において真値部分のΣωiΔtと｛Σαⁱω
（_n-i+1）Δt＋Σα^i-1φ（_n-i+1）（１−α）｝ とが
略等しいとすると、上述した差δは下記の（１０）式の
ようになる。

【００３２】

【数４】 なお、この（１０）式においてはt1＝N1Δt，t2＝N2Δt
の関係がある。この（１０）式よりオフセット誤差ωd
を求めると下記の（１１）式のようになる。

【００３３】

【数５】 ここで、θ0 はオフセットの計算を開始するときの姿勢
角であり、姿勢角が０付近になることは常にあるため、
θ0 ＝０近辺のときから計算を始めると（１１）式はそ
の分子の第２項が無視できるので、下記の（１２）式の
ように変形することができる。

【００３４】

【数６】 すなわち、角速度基準値演算手段２６は、３個の角速度
センサ１２，１４，１６によって検出された角速度中に
それぞれ含まれるオフセット誤差ωd を前記（１２）式
に基づいて算出する。なお、実際ではθ1およびθ2は細
かく変動しているため、ローパスフィルタにより滑らか
にしたθ1′、θ2′を求め、角度差増加分δを下記の
（１３）式のようにして算出する。 δ＝｛（θ1′−θ2′）ｔ₂−（θ1′−θ2′）ｔ₁｝・・・（１３）

【００３５】前記現在姿勢角演算手段２７は、機体１の
現在の姿勢角θX，θY，θZ を前記（２）式に基づいて
算出する。

【００３６】すなわち、角速度センサ１２，１４，１６
によってそれぞれ検出された角速度ωX，ωY，ωZか
ら、前記角速度基準値演算手段が求めた角速度基準値ω
dを差し引いて真の角速度を求めると共にこの真の角速
度を積分し、この積分値に、傾斜角・方位角演算手段２
４が離陸以前に予め求めてメモリに記憶させておいた傾
斜角度φX，φY，および角度φZ をそれぞれ加算して算
出する。

【００３７】このように現在姿勢角演算手段２７によっ
て求められた現在の姿勢角θX，θY，θZ は前記飛行状
態制御用ＣＰＵ２２に入力される。そして、この飛行状
態制御用ＣＰＵ２２は前記姿勢角θX，θY，θZ が目標
姿勢角に一致するように各サーボモータ５〜７を制御す
る。

【００３８】次に、上述したように構成されたコントロ
ーラ８の動作を図５によって説明する。図５は、角速度
センサ１２，１４，１６によって検出された角速度を積
分して求めた角度、言い換えれば、前記（１）式によっ
て求めた角度をθ1 とし、前記（２）式によって求めた
角度をθ2 としてこれらの角度θ1，θ2が時間と共に変
化する様子を示している。

【００３９】コントローラ８の角速度基準値演算手段２
６は、θ2 が０付近であるときに計時を開始し、この計
時開始時（図５中にｔ0で示す）のθ1，θ2をそれぞれ
θ0に設定する。そして、ｔ1時間経過したときに図中符
号Δθ1で示す角度差Δθ1 ＝（θ1−θ2）を計算す
る。さらに、計時開始から時間ｔ2 だけ経過したときに
図中符号Δθ2で示す角度差Δθ2＝（θ1−θ2）を計算
する。しかる後、前記角度差Δθ2から角度差Δθ1を減
算して角度差増加分δ＝Δθ2−Δθ1を計算し、この角
度差増加分δを前記（１２）式に代入してこのときのオ
フセット誤差ωd を求める。

【００４０】（１２）式に代入する変数αは、前記
（６）式の値がある数値に収束するまでの時間と密接に
関連しているので、この収束時間が最適な時間になるよ
うに設定する。

【００４１】このようにオフセット誤差ωd を求めた後
は、コントローラ８の現在姿勢角演算手段２７が前記
（２）式に基づいて現在の機体１の姿勢角θX ，θY，
θZを算出する。

【００４２】したがって、このコントローラ８を使用す
ると、角速度センサ１２，１４，１６によって検出され
た角速度中に含まれるオフセット誤差ωd が角速度基準
値演算手段２６により算出され、前記角速度からこのオ
フセット誤差ωd を差し引いてなる真の角速度を積分し
て機体１の回転角度を求め、この回転角度から機体１の
姿勢を検出して姿勢制御が行われる。このため、姿勢制
御を自動で行うに当たって角速度センサに存するオフセ
ット誤差ωd を除いて精度を高めることが可能になる。
特に、このコントローラ８は、機体１が静止状態であろ
うと飛行状態であろうと常に前記θ2 が０付近にあると
きからオフセット誤差検出動作を開始するから、きわめ
て高精度に現在の機体１の姿勢角を検出することができ
る。

【００４３】また、離陸以前に求めた地球に対する傾斜
角度φX，φY，φZ に、前記真の角速度から求めた回転
角度を加算して機体１の姿勢を検出し、姿勢制御を行う
から、機体１に風等の外乱が加わったとしても、地球を
基準として現在の機体１の姿勢を目標姿勢に一致させる
ことができるので、きわめて正確な姿勢制御を行うこと
ができる。

【００４４】

【発明の効果】以上説明したように第１の発明に係る飛
行体の姿勢制御装置は、機体が前後、左右、上下方向の
主方位の基準軸に対して回るときの角速度を検出する角
速度センサと、地球に対する機体の主方位の傾斜角およ
び方位角を検出する姿勢角センサと、前記角速度センサ
が検出した角速度を積分して角度（θ1） を求める角速
度センサ用角度算出手段と、前記姿勢角センサが検出し
た角度｛φ（X，Y，Z）｝ を予め定めた割合（１−α）
をもって加算して混成姿勢角（θ2）を演算する混成姿
勢角演算手段と、前記混成姿勢角（θ2） が０度付近で
あるときに計時を開始して予め定めた時間（ｔ1）だけ
経過した後の角度差（Δθ1＝θ1−θ2）に対するその
一定時間経過後の時間（ｔ2）での角度差（Δθ2＝θ1
−θ2）の増加分（δ＝Δθ2−Δθ1）から前記角速度
センサが検出する角速度中に含まれる誤差（ωd） を、
N2＝ｔ2／Δｔ、N1＝ｔ1／Δｔ、Δｔを演算サイクル、
αを混成姿勢角（θ2）中の角度（θ1）の割合とし、ω
d（X，Y，Z）＝δ／｛（ｔ2−ｔ1）＋（α^N2−α^N1）α
Δｔ／（１−α）｝なる数式に基づいて算出する誤差演
算手段と、前記角速度センサが検出した角速度から前記
誤差（ωd） を減算してなる真の角速度を積分して機体
の主方位に対する回転角度を求める姿勢角演算手段と、
この姿勢角演算手段が求めた回転角度に基づいて機体の
姿勢を制御する機体制御手段とを備えたため、角速度セ
ンサによって検出された角速度中に含まれる誤差（ω
d） が誤差演算手段により算出され、前記角速度からこ
の誤差（ωd） を差し引いてなる真の角速度を積分して
機体の回転角度を求め、この回転角度から機体の姿勢を
検出して姿勢制御が行われる。

【００４５】このため、離陸前に機体が静止していると
きから離陸して着陸するまでの間に飛行状態如何に係わ
らず前記誤差を検出することになる。したがって、常に
高精度に現在の機体の姿勢角を検出することができる。

【００４６】第２の発明に係る飛行体の姿勢制御装置
は、第１の発明に係る飛行体の姿勢制御装置において、
姿勢角演算手段を、離陸以前に地球に対する機体の主方
位の傾斜角および方位角を姿勢角センサによって検出
し、この傾斜角および方位角に、真の角速度から求めた
機体の回転角度を加算して機体の姿勢を検出する構成と
したため、離陸以前に求めた地球に対する傾斜角および
方位角に、前記真の角速度から求めた回転角度を加算し
て機体の姿勢を検出し、姿勢制御が行われる。

【００４７】このため、機体に風等の外乱が加わったと
しても、地球を基準として現在の機体の姿勢を目標姿勢
に一致させることができるので、きわめて正確な姿勢制
御を行うことができる。

【図面の簡単な説明】

【図１】本発明に係る飛行体の姿勢制御装置を搭載した
無人ヘリコプタの概略構成図である。

【図２】本発明に係る飛行体の姿勢制御装置の全体構成
を示すブロック図である。

【図３】要部の構成を示すブロック図である。

【図４】本発明に係る飛行体の姿勢制御装置の誤差検出
手法を説明するためのグラフである。

【図５】本発明に係る飛行体の姿勢制御装置の誤差検出
動作を説明するためのグラフである。

【符号の説明】

１ 機体 ４ エンジン ５ エンジンコントローラサーボモータ ６ コレクティブサーボモータ ７ ラダーサーボモータ ８ コントローラ ９ 受信機 １０ 送信機 １１ 加速度センサ １２ 角速度センサ １３ 加速度センサ １４ 角速度センサ １５ 地磁気方位センサ １６ 角速度センサ ２１ 姿勢角演算装置 ２２ 飛行状態制御用ＣＰＵ ２４ 傾斜角・方位角演算手段 ２６ 角速度基準値演算手段 ２７ 現在姿勢角演算手段 ２８ ドリフト現在姿勢角演算手段 ２９ 混成現在姿勢角演算手段

Claims (2)

【特許請求の範囲】
1. 【請求項１】 機体が前後、左右、上下方向の主方位の
   基準軸に対して回るときの角速度を検出する角速度セン
   サと、地球に対する機体の主方位の傾斜角および方位角
   を検出する姿勢角センサと、前記角速度センサが検出し
   た角速度を積分して角度（θ1） を求める角速度センサ
   用角度算出手段と、前記姿勢角センサが検出した角度
   ｛φ（X，Y，Z）｝ を予め定めた割合（１−α）をもっ
   て加算して混成姿勢角（θ2）を演算する混成姿勢角演
   算手段と、前記混成姿勢角（θ2）が０度付近であると
   きに計時を開始して予め定めた時間（ｔ1） だけ経過し
   た後の角度差（Δθ1＝θ1−θ2）に対するその一定時
   間経過後の時間（ｔ2）での角度差（Δθ2＝θ1−θ2）
   の増加分（δ＝Δθ2−Δθ1）から前記角速度センサ
   が検出する角速度中に含まれる誤差（ωd） を、N2＝ｔ
   2／Δｔ、N1＝ｔ1／Δｔ、Δｔを演算サイクル、αを混
   成姿勢角（θ2）中の角度（θ1）の割合とし、 ωd（X，Y，Z）＝δ／｛（ｔ2−ｔ1）＋（α^N2−α^N1）
   αΔｔ／（１−α）｝ なる数式に基づいて算出する誤差演算手段と、前記角速
   度センサが検出した角速度から前記誤差（ωd） を減算
   してなる真の角速度を積分して機体の主方位に対する回
   転角度を求める姿勢角演算手段と、この姿勢角演算手段
   が求めた回転角度に基づいて機体の姿勢を制御する機体
   制御手段とを備えたことを特徴とする飛行体の姿勢制御
   装置。
2. 【請求項２】 請求項１記載の飛行体の姿勢制御装置に
   おいて、姿勢角演算手段を、離陸以前に地球に対する機
   体の主方位の傾斜角および方位角を姿勢角センサによっ
   て検出し、この傾斜角および方位角に、真の角速度から
   求めた機体の回転角度を加算して機体の姿勢を検出する
   構成としたことを特徴とする飛行体の姿勢制御装置。