JP4157397B2

JP4157397B2 - 移動体の速度計算回路および移動体

Info

Publication number: JP4157397B2
Application number: JP2003062434A
Authority: JP
Inventors: 彰佐藤
Original assignee: Yamaha Motor Co Ltd
Current assignee: Yamaha Motor Co Ltd
Priority date: 2003-03-07
Filing date: 2003-03-07
Publication date: 2008-10-01
Anticipated expiration: 2023-03-07
Also published as: JP2004271339A

Description

【０００１】
【発明の属する技術分野】
本発明は、ＧＰＳを用いて速度制御を行う移動体の速度計算回路に関する。
【０００２】
【従来の技術】
無人ヘリコプタの速度制御及び位置制御を行うために、ＧＰＳセンサからの速度信号及びジャイロセンサからの加速度信号とをハイブリッドフィルタ回路を通し無人ヘリコプタの現在位置を算出する速度計算回路が特許文献１に記載されている。
【０００３】
しかしながら、このＧＰＳセンサで得られるＧＰＳ信号のデータ受信間隔とジャイロセンサで得られる加速度信号のデータ受信間隔は異なり、ジャイロセンサからの受信回数が毎秒５０回程度の加速度データの受信間隔に対し、ＧＰＳセンサからの受信回数が毎秒２回程度のＧＰＳ速度データの受信間隔は長くなる。
【０００４】
このため速度計算を行う回路に入力される加速度データと同時点でのＧＰＳ速度データが得られない場合がある。このように加速度データとＧＰＳ速度データの受信間隔の相違によるＧＰＳ速度データが同時に得られない場合に対し上記特許文献１の回路使用構成では対処できない。
【０００５】
また、ＧＰＳ速度データは、ＧＰＳセンサがＧＰＳ信号を受信してから速度データとして発信するまでに時間遅れを生じる。したがって、加速度データとＧＰＳ速度データが計算回路側で同時に得られた場合であっても、実際にはＧＰＳ速度データの時間遅れのため、ＧＰＳ速度データと加速度データは時間的にずれた時点でのデータであって、これらのデータをそのまま用いたのでは正確な速度計算ができない。
【０００６】
【特許文献１】
特開２００１−３２５２４５号公報
【０００７】
【発明が解決しようとする課題】
本発明は上記従来技術を考慮したものであって、ＧＰＳ速度データ及び加速度データに基づいて速度を計算する場合に、受信データの時間間隔が異なる場合を考慮し、ＧＰＳ速度データの時間遅れに対処して正確な速度を計算可能な移動体の速度計算回路の提供を目的とする。
【０００８】
【課題を解決するための手段】
前記目的を達成するため、本発明では、計算サイクルごとに同時にサンプリングした移動体の加速度データ及びＧＰＳ速度データに基づいて、この移動体の速度を算出する移動体の速度計算回路において、サンプリング時点でのＧＰＳ速度データの有無を判別し、加速度データと同時にＧＰＳ速度データがサンプリングできない場合に、加速度データのみから移動体の速度を算出することを特徴とする移動体の速度計算回路を提供する。
【０００９】
この構成によれば、各計算サイクルでのデータのサンプリング時点でＧＰＳ速度データの有無を判別し、判別結果に応じて計算処理をするため、ＧＰＳ速度データがないときには加速度データのみに基づいて速度計算を行うことにより、ＧＰＳ速度データと加速度データの時間間隔の相違によりＧＰＳ速度データがない場合に対処して速度を計算することができる。
【００１０】
好ましい構成例では、サンプリング時点でＧＰＳ速度データがある場合に、このＧＰＳ速度データ及びＧＰＳ速度データの時間遅れに対応したサイクルだけ前の計算サイクルでサンプリングした加速度データに基づいて、前の計算サイクル時の移動体の速度を算出することを特徴としている。
【００１１】
この構成によれば、ＧＰＳ速度データの時間遅れに対応してその分前の時点での加速度データを用いて速度を計算するため、同一時点でのＧＰＳ速度データと加速度データに基づき正確な速度計算ができる。
【００１２】
好ましい構成例では、前の計算サイクル時点から現時点までの加速度データの積分値を、前の計算サイクル時点での速度に加えることを特徴としている。
【００１３】
この構成によれば、ＧＰＳ速度データの時間遅れに合せた分だけ前の計算サイクル時の加速度データを用いて算出した前の計算サイクル時の速度に対し、前の計算サイクルから現時点までの加速度データの積分値を現時点での速度の補正値として加えることにより現時点での正確な速度が計算できる。
【００１４】
【発明の実施の形態】
図１は、本発明の実施形態に係る無人ヘリコプタの構成図である。
【００１５】
機体１は、メインロータ２及びラダーロータ３を有し、エンジン４及びその点火系５を搭載する。ラダーロータ３の近傍に受信アンテナ６が備わり、機体内の受信ボックス７内のプリント基板上に形成した受信機８に接続される。受信ボックス７内にはさらに別のプリント基板上に形成したコントローラ９が備わる。機体１の後部テール２７の下側にはＧＰＳセンサ１０が備わる。ＧＰＳセンサ１０は、ＧＰＳアンテナ１３で受信したＧＰＳ信号から速度を測定する。ＧＰＳセンサ１０の後端部に飛行中のＧＰＳの作動状態や受信状態を表示するＧＰＳ表示灯１１及び機体の異常を表示する警告灯２６が備わる。ＧＰＳ表示灯１１及び警告灯２６は、実際には左右に並列して配設されている。
【００１６】
機体後部のテール２７の上側の機体１には飛行前の機体の初期状態を表示するパネル表示部２５が備わる。機体１の重心付近にジャイロからなる姿勢センサ１２が備わる。１４は地磁気を検出する方位センサ、１５はエンジン回転センサである。
【００１７】
機体１内には、コントローラ９により駆動制御される５つのサーボモータ１６〜２０が備わる。１６は左エルロンサーボモータ、１７は右エルロンサーボモータ、１８はエレベータサーボモータ、１９はスロットルサーボモータ、２０はラダーサーボモータである。
【００１８】
２１は地上側の送信機を示す。送信機２１は、スティック形状の第１操作子２２と第２操作子２３とを備える。
【００１９】
第１操作子２２はエレベータ操作及びラダー操作用である。この第１操作子２２をａｂ方向に操作することによりエレベータサーボモータ１８が制御され、機首を下げて前進飛行（ａ方向操作）又は機首を上げて後進飛行（ｂ方向操作）する。第１操作子２２をｃｄ方向に操作することにより、ラダーサーボモータ２０を制御して、機体１前方に向かって左右方向の向きを調整し、機首を左（ｃ方向操作）又は右に振る（ｄ方向操作）。
【００２０】
第２操作子２３は、スロットル操作及びエルロン操作用であって、エンジン回転数及びメインロータ負荷を同時に調整するためのエンコン操作子である。この第２操作子２３をｅｆ方向に操作することにより、機体が水平姿勢のまま上昇（ｅ方向操作）又は下降（ｆ方向操作）する。すなわち、第２操作子２３のｅｆ方向の操作により、スロットルサーボモータ１９が制御され、エンジンスロットル開度が調整されるとともに、左右のエルロンサーボモータ１６，１７及びエレベータサーボモータ１８が同時に駆動される。これにより、機体が水平姿勢のまま上昇又は下降する。
【００２１】
第２操作子２３をｇｈ方向に操作することにより、左右のエルロンサーボモータ１６，１７が制御され、機体１を左に傾けて左移動させ（ｇ方向操作）又は右に傾けて右移動させる（ｈ方向操作）。
【００２２】
送信機２１には第２操作子（エンコン操作子）２３のｅｆ方向のエンコン操作位置を検出するためのエンコン位置センサ２４が備わる。このエンコン位置センサ２４は、エンジンスロットル開度に対応するメインロータ負荷に応じて目標エンジン回転数を制御するためのものである。
【００２３】
図２は、本発明に係る無人ヘリコプタの制御系ブロック図である。また、図３は、コントローラによる制御演算処理のフローチャートである。
【００２４】
地上側の送信機２１の操作による操縦指令信号が機体側の受信機８で受信されコントローラ９に送られて信号処理される。コントローラ９は、内部の制御回路２８内で予め設定された制御プログラムにしたがって、図３に示すフローの演算処理を行う。
【００２５】
まず、運転状態のフラグ等を初期値にセットする（ステップＡ１）。続いて入力信号処理部２９で入力信号処理を行う（ステップＡ２）。これは、指令信号や各種センサの検出信号等の入力信号に基づいて受信状態が正常かどうか及び各種センサ類が正常かどうかをチェックするものである。
【００２６】
次にエンジン回転制御計算を行う（ステップＡ３）。これは、エンジン回転制御計算部３０で、送信機２１のエンコン操作子２３（図１）によるエンコンサーボ指令に基づいてスロットル開度を制御し所定のエンジン回転数で飛行するように制御するものである。
【００２７】
次に姿勢制御計算を行う（ステップＡ４）。これは姿勢制御計算部３１で、姿勢センサ１２からの信号に基づいて機体の前後及び左右方向の傾斜を制御するものである。
【００２８】
次にＧＰＳ制御計算を行う（ステップＡ５）。これはＧＰＳ制御計算部３２で、ＧＰＳセンサ１０からの信号に基づいて飛行位置及び飛行速度を制御するものである。
【００２９】
次にこれらの入力信号処理及び各制御計算処理を行った後、出力信号処理部３３から処理結果を出力する（ステップＡ６）。この出力信号により、エンジンの点火系を駆動して指令されたエンジン回転数に基づいてエンジンを駆動するとともに、各サーボモータ１６〜２０を駆動して方向や姿勢を制御する。
【００３０】
これらのステップＡ１〜Ａ６のルーチンは、飛行中例えば２０ｍｓ程度ごとに繰り返されてデータを更新しながら制御される。
【００３１】
図４は、本発明に係る速度計算プログラムからなる速度計算回路の計算ルーチンのフローチャートである。このプログラムルーチンの計算サイクルは例えば数十ｍｓ程度で繰り返される。
【００３２】
ステップＰ１：
計算サイクル番号ｋを前の計算サイクルから１繰上げる。
【００３３】
ステップＰ２：
計算サイクル番号ｋがｍ以上か否かを判別する。ｍは、ＧＰＳ速度データの時間遅れに対応する計算サイクル数である。本発明では、ｍサイクル前の加速度データを用いて現時点の速度を算出するため、計算プロセス開始後、計算サイクルがｍサイクル以上繰り返されたか否か、すなわちｍサイクル前のデータが取得可能か否かを判別するためである。ｋがｍ以上であればステップＰ４に進み、ｍ未満であればステップＰ３に進む。
【００３４】
ステップＰ３：
ｋがｍ未満でｍサイクル前のデータが得られない場合、速度Ｖ（ｋ）の初期値としてＶ（ｋ）＝０とする。
【００３５】
ステップＰ４：
ＧＰＳセンサからの出力の有無を判別する。すなわち、ＧＰＳセンサからＧＰＳ速度データがこの計算回路に入力されたか否かを判別する。これは、本発明では、ジャイロセンサから出力される加速度データとこれより出力回数が少ないＧＰＳ速度データを用いて速度を算出するため、加速度データとともにこれと同時にＧＰＳ速度データが得られたか否かを判別するためである。ＧＰＳ速度データがあればステップＰ６に進み、なければステップＰ５に進む。
【００３６】
ステップＰ５：
ＧＰＳ速度データがない場合、加速度データのみを用いて現時点の速度Ｖ（ｋ）を算出する。これは、前回の計算サイクル（ｋ−１）で算出した速度Ｖ（ｋ−１）に加速度データの積分値を加えるものである（後述）。すなわち、
Ｖ（ｋ）＝Ｖ（ｋ−１）＋ΔＴ・ａ_gyro（ｋ）
とする。
【００３７】
ステップＰ６：
ＧＰＳ速度データがある場合、ｍサイクル前の計算サイクル時での速度Ｖ（ｋ−ｍ）を算出する。これは、ＧＰＳ速度データは時間遅れがあるため、現時点で取得したデータは、実際にはｍサイクル前の時点でのＧＰＳ信号に基づく速度データであり、時系列的に加速度データをこれに合せるために、ｍサイクル前の加速度データを用いてｍサイクル前の時点での速度Ｖ（ｋ−ｍ）を算出するものである。
【００３８】
ステップＰ７：
ｍ−１サイクル前から現時点までの加速度データの積分値をｍサイクル前の速度に付加する。これにより、現時点での速度Ｖ（ｋ）を算出する。
【００３９】
図５〜図８は、本発明の速度計算回路で用いる計算ロジックの説明図である。
【００４０】
図５は、速度計算の基本原理を示す。
ジャイロセンサからの加速度データａ_gyro（ｔ）を積分回路５０を通して速度データＶ_gyroに変換する。これをハイパスフィルタ５１を通して高周波成分のみ取り出しノイズを除去する。ＧＰＳセンサからのＧＰＳ速度データＶ_GPS（ｔ）をローパスフィルタ５２を通して低周波成分のみ取り出してノイズを除去する。各フィルタ５１，５２を通したこれら２つの加速度信号及びＧＰＳ速度信号を加算器５３でミキシングして推定速度Ｖ（ｔ）を算出する。
【００４１】
この場合、微分写像を用いたｚ変換によりアナログフィルタを離散化する。すなわち、ラプラス変換子をＳ＝（１−ｚ^-1）／ΔＴ、ｚをｚ変換素子、ΔＴをサンプリングタイム、Ｔ₁をフィルタの時定数として、以下のようにハイパスフィルタ５１及びローパスフィルタ５２の演算回路を作成する。
【００４２】
ハイパスフィルタ５１は、
Ｈ₁（Ｓ）＝Ｔ₁Ｓ／（Ｔ₁Ｓ＋１）であり、ｚ変換すると、
【数１】

となる。
【００４３】
ローパスフィルタ５２は、
Ｈ₂（Ｓ）＝１／（Ｔ₁Ｓ＋１）であり、ｚ変換すると、
【数２】

となる。
【００４４】
また、積分回路５０は、
Ｈ₀（Ｓ）＝１／Ｓであり、ｚ変換すると、
Ｈ₀（ｚ）＝ΔＴ／（１−ｚ^-1）
となる。
【００４５】
図６は、ｚ変換後の計算回路の構成を示す。図７は図６の積分回路による加速度データから速度データへの変換ロジックを示す。
【００４６】
図７のＶ（ｋ）が、図４のフローチャートのステップＰ３のＶ（ｋ）あるいはステップＰ５のｖ（ｋ）である。
【００４７】
図８は、加算器でミキシングして速度計算する場合の計算回路の構成を示す。ＧＰＳの出力にｍサイクルの遅れがあるとすると、ｍサイクル前の加速度データとのミキシングを行い、ｍサイクル前の推定速度を算出する。
ローパスフィルタ５２についてみると、
【数３】

となる。
【００４８】
積分回路５０及びハイパスフィルタ５１を通した速度計算データはについてみると、
【数４】

となる。
【００４９】
加算器５３でＶ_high（ｋ−ｍ）とＶ_low（ｋ−ｍ）を加算する。したがって、上記（数３）の式と（数４）の式を加算して、
【数５】

となる。この（数５）の値が前述の図４のフローチャートのステップＰ６の式である。これは、ｍサイクル前の時点での速度であり、現時点での速度は、図４のステップＰ７で説明したように、ｍサイクル前の時点から現時点までの加速度データの積分値を加算したものである。
【００５０】
なお、本発明は無人ヘリコプタに限らず、他の飛行体、船舶、陸上乗物等の移動体に対して適用可能である。
【００５１】
【発明の効果】
以上説明したように、本発明では、各計算サイクルでのデータのサンプリング時点でＧＰＳ速度データの有無を判別し、判別結果に応じて計算処理をするため、ＧＰＳ速度データがないときには加速度データのみに基づいて速度計算を行うことにより、ＧＰＳ速度データと加速度データの時間間隔の相違によりＧＰＳ速度データがない場合に対処して速度を計算することができる。
【００５２】
また、サンプリング時点でＧＰＳ速度データがある場合に、このＧＰＳ速度データ及びＧＰＳ速度データの時間遅れに対応したサイクルだけ前の計算サイクルでサンプリングした加速度データに基づいて、前の計算サイクル時の移動体の速度を算出する構成によれば、ＧＰＳ速度データの時間遅れに対応してその分前の時点での加速度データを用いて速度を計算するため、同一時点でのＧＰＳ速度データと加速度データに基づき正確な速度計算ができる。
【００５３】
また、前の計算サイクル時点から現時点までの加速度データの積分値を、前の計算サイクル時点での速度に加える構成によれば、ＧＰＳ速度データの時間遅れに合せた分だけ前の計算サイクル時の加速度データを用いて算出した前の計算サイクル時の速度に対し、前の計算サイクルから現時点までの加速度データの積分値を現時点での速度の補正値として加えることにより現時点での正確な速度が計算できる。
【図面の簡単な説明】
【図１】本発明に係る無人ヘリコプタの構成図。
【図２】本発明に係る無人ヘリコプタの制御系ブロック図。
【図３】コントローラによる制御演算処理のフローチャート。
【図４】本発明に係る速度計算回路のフローチャート。
【図５】本発明に係る速度計算回路の基本原理説明図。
【図６】本発明に係る図５の計算回路をｚ変換した場合の構成説明図。
【図７】本発明に係る速度計算回路でのジャイロセンサからの加速度データ処理の説明図。
【図８】本発明に係る速度計算回路での加算器のミキシングによるデータ処理の説明図。
【符号の説明】
１：機体、２：メインロータ、３：ラダー、４：エンジン、５：点火系、
６：受信アンテナ、７：受信ボックス、８：受信機、９：コントローラ、
１０：ＧＰＳセンサ、１１：ＧＰＳ表示灯、１２：姿勢センサ、
１３：ＧＰＳアンテナ、１４：方位センサ、１５：エンジン回転センサ、
１６：右エルロンサーボモータ、１７：左エルロンサーボモータ、
１８：エレベーションサーボモータ、１９：エンコンサーボモータ、
２０：ラダーサーボモータ、２１：送信機、２２：第１操作子、
２３：第２操作子、２４：操作位置センサ、２５：パネル表示部、
２６：警告灯、２７：テール、２８：制御回路、５０：積分回路、
５１：ハイパスフィルタ、５２：ローパスフィルタ、５３：加算器。

Claims

計算サイクルごとに同時にサンプリングした移動体の加速度データ及びＧＰＳ速度データに基づいて、この移動体の速度を算出する移動体の速度計算回路において、
サンプリング時点でのＧＰＳ速度データの有無を判別し、加速度データと同時にＧＰＳ速度データがサンプリングできない場合に、加速度データのみから移動体の速度を算出し、
サンプリング時点でＧＰＳ速度データがある場合に、このＧＰＳ速度データ及びＧＰＳ速度データの時間遅れに対応したサイクルだけ前の計算サイクルでサンプリングした加速度データに基づいて、前の計算サイクル時の移動体の速度を算出する
ことを特徴とする移動体の速度計算回路。
前の計算サイクル時点から現時点までの加速度データの積分値を、前の計算サイクル時点での速度に加えることを特徴とする請求項１に記載の移動体の速度計算回路。
ＧＰＳ信号に基づいて移動体の速度を算出し、これをＧＰＳ速度データとして出力するＧＰＳセンサと、
前記移動体の加速度を測定し、これを加速度データとして出力するジャイロセンサと、
前記ＧＰＳセンサから出力される前記ＧＰＳ速度データと、前記ジャイロセンサから出力される前記加速度データとに基づいて前記移動体の速度を算出する速度計算回路と
を備えた移動体であって、
前記速度計算回路は、請求項１または請求項２に記載された移動体の速度計算回路からなる
ことを特徴とする移動体。