(57)【特許請求の範囲】
【請求項１】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを、前記位置情報に基づく割合であって、前記第１姿勢推定部による前記姿勢推定の結果が利用される重みＡと前記第２姿勢推定部による前記姿勢推定の結果が利用される重みＢとの比（ただし、重みＡ及び重みＢはそれぞれ０より大きい値）で表される割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項２】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報であって、前記航空機の高度を示す高度情報を含む位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記高度情報に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項３】
前記飛行制御部は、前記航空機が離陸してから所定の高度に達するまでは少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記所定の高度に達した後は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする請求項２に記載の制御装置。
【請求項４】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記位置情報に基づいて、前記航空機の離陸地点からの距離を算出する距離算出部と、
前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記距離に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項５】
前記飛行制御部は、前記航空機が離陸してから所定の距離に達するまでは少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記所定の距離に達した後は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする請求項４に記載の制御装置。
【請求項６】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項７】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記航空機の離陸前に基準方位を真値に校正する校正部と、
前記３軸センサに含まれる地磁気センサにより検出された基準方位を示す値と前記真値とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項８】
前記飛行制御部は、前記航空機が所定のエリア内にあるときは前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記航空機が前記所定のエリア外にあるときは前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする請求項１乃至７の何れか一項に記載の制御装置。
【請求項９】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、
前記光学センサにより検出された第１検出値を取得する第１取得部と、
前記３軸センサにより検出された第２検出値を取得する第２取得部と、
前記航空機の飛行位置を示す位置情報を取得する第３取得部と、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、
前記位置情報に基づいて、前記光学センサへの電力供給を制御する電力供給制御部と、
前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記位置情報に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、
を備えることを特徴とする制御装置。
【請求項１０】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記位置情報に基づく割合であって、前記第１姿勢推定ステップによる前記姿勢推定の結果が利用される重みＡと前記第２姿勢推定ステップによる前記姿勢推定の結果が利用される重みＢとの比（ただし、重みＡ及び重みＢはそれぞれ０より大きい値）で表される割合で利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【請求項１１】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報であって、前記航空機の高度を示す高度情報を含む位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記高度情報に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【請求項１２】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記位置情報に基づいて、前記航空機の離陸地点からの距離を算出するステップと、
前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記距離に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【請求項１３】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定ステップによる前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定ステップによる前記姿勢推定の結果を利用することなく前記第２姿勢推定ステップによる前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【請求項１４】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記航空機の離陸前に基準方位を真値に校正するステップと、
前記３軸センサに含まれる地磁気センサにより検出された基準方位を示す値と前記真値とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定ステップによる前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定ステップによる前記姿勢推定の結果を利用することなく前記第２姿勢推定ステップによる前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【請求項１５】
光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、
前記光学センサにより検出された第１検出値を取得するステップと、
前記３軸センサにより検出された第２検出値を取得するステップと、
前記航空機の飛行位置を示す位置情報を取得するステップと、
前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、
前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、
前記位置情報に基づいて、前記光学センサへの電力供給を制御するステップと、
前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記位置情報に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、
を含むことを特徴とする制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航空機の飛行制御を行うシステムにより実行される制御方法等の技術分野に関する。
【背景技術】
【０００２】
従来、地磁気センサ、及び慣性センサ（例えば、加速度センサや角速度センサ）は、姿勢を推定するために携帯電話やカーナビ、航空機など多くの装置で使用されている。例えば、特許文献１には、３軸のジャイロ装置（角速度センサ）により計測された方位情報を地磁気センサで計測された方位情報で補正することで飛翔体の姿勢を高精度に計測する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開2004-345435号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、地磁気センサや慣性センサにより検出された検出値（計測値）が信頼できない場合がある。例えば、付近に強い磁場や電場を発生させる物体が存在する場合には、地磁気センサや慣性センサが異常値を出力してしまい航空機の姿勢を正確に推定することができない場合がある。その結果、航空機の飛行が安定しなくなってしまうおそれがある。【０００５】
そこで、地磁気センサや慣性センサにより検出された検出値が信頼できない場合であっても、航空機を安定して飛行させることが可能な制御装置及び制御方法を提供する。  
【課題を解決するための手段】
【０００６】
上記課題を解決するために、請求項１に記載の発明は、  光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを、前記位置情報に基づく割合であって、前記第１姿勢推定部による前記姿勢推定の結果が利用される重みＡと前記第２姿勢推定部による前記姿勢推定の結果が利用される重みＢとの比（ただし、重みＡ及び重みＢはそれぞれ０より大きい値）で表される割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、地磁気センサや慣性センサにより検出された検出値が信頼できない場合であっても、航空機を安定して飛行させることができる。
【０００７】
請求項２に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報であって、前記航空機の高度を示す高度情報を含む位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記高度情報に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、第１姿勢推定部による姿勢推定の結果と第２姿勢推定部による姿勢推定の結果を航空機の飛行高度に応じて変化する割合で利用することができる。
【０００８】
請求項３に記載の発明は、請求項２に記載の制御装置において、前記飛行制御部は、前記航空機が離陸してから所定の高度に達するまでは少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記所定の高度に達した後は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする。これにより、第２姿勢推定部による姿勢推定の結果が信頼できる程度になることが予想される早い段階から負荷の小さい飛行制御に切り替えることができる。
【０００９】
請求項４に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記位置情報に基づいて、前記航空機の離陸地点からの距離を算出する距離算出部と、前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記距離に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、第１姿勢推定部による姿勢推定の結果と第２姿勢推定部による姿勢推定の結果を航空機の離陸地点からの距離に応じて変化する割合で利用することができる。
【００１０】
請求項５に記載の発明は、請求項４に記載の制御装置において、前記飛行制御部は、前記航空機が離陸してから所定の距離に達するまでは少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記所定の距離に達した後は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする。これにより、第２姿勢推定部による姿勢推定の結果が信頼できる程度になることが予想される早い段階から負荷の小さい飛行制御に切り替えることができる。
【００１１】
請求項６に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、第１姿勢推定部による姿勢推定の結果と第２姿勢推定部による姿勢推定の結果とのずれ量がある程度小さくなった早い段階から負荷の小さい飛行制御に切り替えることができる。
【００１２】
請求項７に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記航空機の離陸前に基準方位を真値に校正する校正部と、前記３軸センサに含まれる地磁気センサにより検出された基準方位を示す値と前記真値とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定部による前記姿勢推定の結果を利用することなく前記第２姿勢推定部による前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、基準方位を示す値と真値とのずれ量がある程度小さくなった早い段階から負荷の小さい飛行制御に切り替えることができる。
【００１３】
請求項８に記載の発明は、請求項１乃至７の何れか一項に記載の制御装置において、前記飛行制御部は、前記航空機が所定のエリア内にあるときは前記第１姿勢推定部による前記姿勢推定の結果を利用し、前記航空機が前記所定のエリア外にあるときは前記第２姿勢推定部による前記姿勢推定の結果を利用することを特徴とする。これにより、航空機の飛行中に当該航空機が所定のエリア内に入ったか否かにしたがって第１姿勢推定部による姿勢推定の結果と第２姿勢推定部による姿勢推定の結果を切り替えて利用することができる。【００１４】
請求項９に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行う制御装置であって、前記光学センサにより検出された第１検出値を取得する第１取得部と、前記３軸センサにより検出された第２検出値を取得する第２取得部と、前記航空機の飛行位置を示す位置情報を取得する第３取得部と、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定部と、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定部と、前記位置情報に基づいて、前記光学センサへの電力供給を制御する電力供給制御部と、前記第１姿勢推定部による前記姿勢推定の結果と、前記第２姿勢推定部による前記姿勢推定の結果とを前記位置情報に基づく割合で利用することにより前記航空機の飛行制御を行う飛行制御部と、を備えることを特徴とする。これにより、航空機における消費電力を抑えることができる。
【００１５】
請求項１０に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記位置情報に基づく割合であって、前記第１姿勢推定ステップによる前記姿勢推定の結果が利用される重みＡと前記第２姿勢推定ステップによる前記姿勢推定の結果が利用される重みＢとの比（ただし、重みＡ及び重みＢはそれぞれ０より大きい値）で表される割合で利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。請求項１１に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報であって、前記航空機の高度を示す高度情報を含む位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記高度情報に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。請求項１２に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記位置情報に基づいて、前記航空機の離陸地点からの距離を算出するステップと、前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記距離に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。請求項１３に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定ステップによる前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定ステップによる前記姿勢推定の結果を利用することなく前記第２姿勢推定ステップによる前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。請求項１４に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記航空機の離陸前に基準方位を真値に校正するステップと、前記３軸センサに含まれる地磁気センサにより検出された基準方位を示す値と前記真値とのずれ量が閾値以上である場合は少なくとも前記第１姿勢推定ステップによる前記姿勢推定の結果を利用し、前記ずれ量が前記閾値未満である場合は前記第１姿勢推定ステップによる前記姿勢推定の結果を利用することなく前記第２姿勢推定ステップによる前記姿勢推定の結果を利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。請求項１５に記載の発明は、光学センサと少なくとも１種類の３軸センサとを備える航空機の飛行制御を行うシステムにより実行される制御方法であって、前記光学センサにより検出された第１検出値を取得するステップと、前記３軸センサにより検出された第２検出値を取得するステップと、前記航空機の飛行位置を示す位置情報を取得するステップと、前記第１検出値に基づいて前記航空機の姿勢推定を行う第１姿勢推定ステップと、前記第２検出値に基づいて前記航空機の姿勢推定を行う第２姿勢推定ステップと、前記位置情報に基づいて、前記光学センサへの電力供給を制御するステップと、前記第１姿勢推定ステップによる前記姿勢推定の結果と、前記第２姿勢推定ステップによる前記姿勢推定の結果とを前記位置情報に基づく割合で利用することにより前記航空機の飛行制御を行うステップと、を含むことを特徴とする。
【発明の効果】
【００１６】
本発明によれば、地磁気センサや慣性センサにより検出された検出値が信頼できない場合であっても、航空機を安定して飛行させることができる。
【図面の簡単な説明】
【００１７】
【図１】飛行システムＳの概要構成例を示す図である。
【図２】UAV１の概要構成例を示す図である。
【図３】UAV１の制御部１８により実行される処理の一例を示すフローチャートである。
【図４】離陸前に設定されるUAV１の基準方位を示す図である。
【図５】UAV１が付近に特定物体が存在するポートから離陸し目的地点へ向かうまでの様子を示す概念図である。
【発明を実施するための形態】
【００１８】
以下、図面を参照して本発明の一実施形態について説明する。なお、以下の実施形態は、飛行システムに対して本発明を適用した場合の実施形態である。
【００１９】
［１．飛行システムＳの構成及び動作概要］
先ず、図１を参照して、無人で飛行可能な航空機を所定の目的のために飛行させる飛行システムＳの構成及び動作概要について説明する。所定の目的の例として、例えば、運搬、測量、撮影、点検、監視等が挙げられる。図１は、飛行システムＳの概要構成例を示す図である。図１に示すように、飛行システムＳは、大気中（空中）を飛行する無人航空機（以下、「UAV（Unmanned Aerial Vehicle）」と称する）１、運航管理システム（以下、「UTMS（UAV Traffic Management System）」と称する）２、及びポート管理システム（以下、「PMS（Port Management System）」と称する）３を含んで構成される。UAV１、UTMS２、及びPMS３は、通信ネットワークＮＷを介して互いに通信可能になっている。通信ネットワークＮＷは、例えば、インターネット、移動体通信ネットワーク及びその無線基地局等から構成される。なお、図１の例では、１つのUAV１を示すが実際には複数存在してよい。UTMS２とPMS３とは、１つの管理システムとして構成されてもよい。
【００２０】
UAV１は、地上からオペレータによる遠隔操縦に従って飛行、または自律的に飛行することが可能になっている。UAV１は、ドローン、またはマルチコプタとも呼ばれる。UAV１は、GCS（Ground Control Station）により管理される。GCSは、例えば、アプリケーションとして通信ネットワークＮＷに接続可能な操縦端末に搭載される。この場合、オペレータは、例えば、操縦端末を操作してUAV１を遠隔操縦する人である。或いは、GCSは、サーバ等により構成されてもよい。この場合、オペレータは、例えば、GCSの管理者、またはサーバが備えるコントローラである。なお、UAV１の自律的な飛行は、当該UAV１が飛行制御を行うことによる自律飛行に限定されるものではなく、当該UAV１の自律的な飛行には、例えば飛行システムＳ全体として飛行制御を行うことによる自律飛行も含まれる。
【００２１】
UTMS２は、制御サーバＣＳを含む１以上のサーバ等を備えて構成される。制御サーバＣＳは、制御装置の一例である。UTMS２は、UAV１の運航を管理する。UAV１の運航管理には、UAV１の運航計画の管理、UAV１の飛行状況の管理、及びUAV１の制御が含まれる。UAV１の運航計画とは、UAV１の出発地点（離陸地点）から目的地点までの飛行経路（予定経路）等を含む飛行計画である。飛行経路は、例えば、その経路上の緯度及び経度で表され、飛行高度を含んでもよい。UAV１の飛行状況の管理は、UAV１の航空機情報に基づいて行われる。UAV１の航空機情報には、少なくともUAV１の位置情報が含まれる。UAV１の位置情報は、UAV１の現在位置を示す。UAV１の現在位置とは、飛行中のUAV１の飛行位置である。UAV１の航空機情報には、UAV１の速度情報等が含まれてもよい。速度情報は、UAV１の飛行速度を示す。
【００２２】
PMS３は、１または複数のサーバ等により構成される。PMS３は、例えば、UAV１の出発地点（離陸地点）及び目的地点（または経由地点）に設置された離着陸施設（以下、「ポート」と称する）を管理する。なお、ポートは、例えば、地表（地面）または建造物の屋上等に設置される。このようなポート付近には強い磁場、電場または電磁場（以下、「磁場等」と称する）を発生させる特定の物体（以下、「特定物体」と称する）が存在する場合がある。特定物体の例として、永久磁石、変圧器、発電機等が挙げられ、予め位置が特定されている。また、ポートが設置される地表付近以外でも強い磁場等を発生させる特定物体（例えば、送電線、電波塔、基地局等の建造物等）は存在し得る。ポートの管理は、ポートの位置情報及びポートの予約情報等に基づいて行われる。ここで、ポートの位置情報は、ポートの設置位置を示す。ポートの予約情報には、ポートを予約したUAV１の機体ＩＤ、及び到着予定時刻の情報等が含まれる。UAV１の機体ＩＤは、UAV１を識別する識別情報である。なお、ポート付近に特定物体が存在する場合、当該特定物体の位置情報が当該ポートの位置情報とともにPMS３により管理されてもよい。
【００２３】
［２．UAV１の構成及び機能概要］
次に、図２を参照してUAV１の構成及び機能概要について説明する。図２は、UAV１の概要構成例を示す図である。図２に示すように、UAV１は、駆動部１１、測位部１２、無線通信部１３、撮像部１４、回転角検出部１５、第１姿勢推定部１６、第２姿勢推定部１７、及び制御部１８等を備える。ここで、第１姿勢推定部１６は、第１取得部としても機能し、第２姿勢推定部１７は、第２取得部としても機能する。一つの姿勢推定部（例えば一つのプロセッサ等で構成）が第１姿勢推定部と第２姿勢推定部の両方として機能してもよい。制御部１８は、制御装置、第３取得部、飛行制御部、電力供給制御部、校正部、及び距離算出部の一例である。なお、図示しないが、UAV１は、水平回転翼であるロータ（プロペラ）、及びUAV１の各部へ電力を供給するバッテリ等を備える。
【００２４】
駆動部１１は、モータ及び回転軸等を備える。駆動部１１は、制御部１８から出力された制御信号に従って駆動するモータ及び回転軸等により複数のロータを回転させる。測位部１２は、電波受信機及び高度センサ等を備える。測位部１２は、例えば、GNSS（Global Navigation Satellite System）の衛星から発信された電波を電波受信機により受信し、当該電波に基づいてUAV１の水平方向の現在位置（緯度及び経度）を検出する。UAV１の現在位置は、飛行中のUAV１の飛行位置である。なお、UAV１の水平方向の現在位置は、撮像部１４により撮像された画像や上記無線基地局から発信された電波に基づいて補正されてもよい。測位部１２により検出された現在位置を示す位置情報は、制御部１８へ出力される。さらに、測位部１２は、気圧センサ等の高度センサによりUAV１の垂直方向の現在位置（高度）を検出してもよい。この場合、位置情報には、UAV１の高度を示す高度情報が含まれる。高度情報を含む位置情報は、ワールド座標系における三次元位置座標（Ｘ,Ｙ,Ｚ）で表される。なお、UAV１の高度は、GNSSの衛星から受信された電波に基づいて検出されてもよい。この場合、GNSSの衛星から受信された電波に基づいて検出された高度は、高度センサにより検出された高度を補うために使用されてもよい。
【００２５】
無線通信部１３は、通信ネットワークＮＷを介して行われる通信の制御を担う。撮像部１４は、カメラ等を備える。カメラは、光学センサの一例である。カメラは、その画角に収まる範囲内の実空間の対象物を連続的に撮像（つまり、対象物から反射または放射される光を検出）し、画像（画素群）を表す検出値（第１検出値の一例）を第１姿勢推定部１６へ出力する。第１姿勢推定部１６に出力された検出値は、UAV１の姿勢推定に用いられる。ここで、UAV１の姿勢は、例えば、UAV１が地面と水平で且つUAV１の基準方位が例えば北に設定された状態からどのくらい回転したか（換言すると、どのような回転でその向きになったか）を示す。当該回転は、例えば、Ｘ軸まわりの回転角（ロール角）、Ｙ軸まわりの回転角（ピッチ角）、及びＺ軸（垂直方向）まわりの回転角（ヨー角）により表される（つまり、オイラー角で表される）。或いは、当該回転は、Ｘ軸まわりの回転行列、Ｙ軸まわりの回転行列、及びＺ軸まわりの回転行列で表されてもよい。望ましくは、当該回転は、いわゆるジンバルロックが発生せず、且つ補間が容易なクォータニオン（３つの回転軸（ベクトル）と１つの回転角（スカラ））で表されるとよい。具体的に、クォータニオンは、“ｉｘ＋ｊｙ＋ｋｚ＋ｗ”という式で表すことができる。ここで、ｉ，ｊ，ｋはそれぞれ虚数単位である。したがって、ｘ，ｙ，ｚはそれぞれ虚部となり、ｗは実部となる。また、ｘはＸ軸の値、ｙはＹ軸の値、ｚはＺ軸の値である。オイラー角、回転行列、及びクォータニオンは、公知の如く、相互に変換可能である。カメラは、その視野方向が少なくても水平方向をカバーするようにUAV１に取り付けられる。望ましくは、カメラは、広角レンズを備えるカメラ（例えば、全方位カメラ）であるとよい。また、カメラは、ＲＧＢカメラであることが望ましいが、赤外線カメラであってもよい。なお、UAV１の姿勢推定のために、これらのカメラ以外の光学センサ（例えば、距離センサ、レーザセンサ、またはLiDAR（Light Detection and Ranging、或いはLaser Imaging Detection and Ranging））がUAV１に備えられてもよい。
【００２６】
回転角検出部１５は、少なくとも１種類の３軸センサを備える。３軸センサの例として、３軸角速度センサ（ジャイロセンサともいう）、３軸加速度センサ、及び３軸地磁気センサ（磁気センサまたは方位センサともいう）が挙げられる。３軸角速度センサ及び３軸加速度センサは、それぞれ、慣性センサともいう。３軸角速度センサは、例えば圧電素子等を備え、Ｘ軸まわりの角速度（単位時間に回転する角度）、Ｙ軸まわりの角速度、及びＺ軸まわりの角速度をそれぞれ連続的に検出し、各角速度を表す検出値（第２検出値の一例）を第２姿勢推定部１７へ出力する。３軸加速度センサは、例えば圧電素子等を備え、重力加速度を連続的に検出し、重力加速度を３次元ベクトルで表す検出値（第２検出値の一例）を第２姿勢推定部１７へ出力する。３軸地磁気センサは、例えば電子コンパス等を備え、地磁気を連続的に検出し、北（磁北）を３次元ベクトルで表す検出値（第２検出値の一例）を第２姿勢推定部１７へ出力する。なお、２種類の３軸センサ（例えば、３軸角速度センサと３軸加速度センサ）を総称して６軸センサともいう。また、３種類の３軸センサ（すなわち、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサ）を総称して９軸センサともいう。
【００２７】
第１姿勢推定部１６は、プロセッサであるＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、及び不揮発性メモリ等を備える。第１姿勢推定部１６は、撮像部１４から検出値を連続的に取得し、当該検出値（画像）に基づいてVisual-SLAM（Simultaneous Localization and Mapping）を行うことでUAV１の姿勢（ワールド座標系における姿勢）推定を行う。例えば、第１姿勢推定部１６は、UAV１が着陸状態（例えば、地面に置かれ、基準方位が例えば北に設定された状態）にあるときに撮像された画像及びUAV１が飛行開始後に連続的に撮像された画像を含む複数の画像（フレーム）において、複数の特徴点のそれぞれを追跡し、UAV１（カメラ）の相対位置から特徴点までの３次元方向ベクトルを算出してマッピングする（例えば、デプスマップを生成する）ことでUAV１の姿勢を推定する。第１姿勢推定部１６による姿勢推定の結果（以下、「第１姿勢推定結果」と称する）は、オイラー角（ロール角、ピッチ角、及びヨー角の３パラメータからなる）、回転行列（Ｘ軸まわりの回転行列、Ｙ軸まわりの回転行列、及びＺ軸まわりの回転行列）、またはクォータニオン（３つの回転軸及び１つの回転角の４パラメータ（ｘ，ｙ，ｚ，ｗ）からなる）で表され（望ましくは、クォータニオンで表されるとよい）、制御部１８へ出力される。なお、特徴点を求めるために画像から得られるＲＧＢ値が用いられるとよい。また、特徴点の追跡の過程で発生した誤差の蓄積を低減するために、いわゆるバンドル調整が行われるとよい。また、第１姿勢推定部１６は、カメラ以外の光学センサにより検出された検出値に基づいてSLAMを行うことでUAV１の姿勢推定を行ってもよい。
【００２８】
第２姿勢推定部１７は、プロセッサであるＣＰＵ、ＲＯＭ、ＲＡＭ、及び不揮発性メモリ等を備える。第２姿勢推定部１７は、回転角検出部１５からの検出値を連続的に取得し、当該検出値に基づいてUAV１の姿勢（ワールド座標系における姿勢）推定を行う。第２姿勢推定部１７による姿勢推定の結果（以下、「第２姿勢推定結果」と称する）は、オイラー角、回転行列、またはクォータニオンで表され（望ましくは、クォータニオンで表されるとよい）、制御部１８へ出力される。例えば、回転角検出部１５が３軸角速度センサを備える場合、第２姿勢推定部１７は、UAV１が飛行開始後に３軸角速度センサにより連続的に検出された検出値（Ｘ軸まわりの角速度、Ｙ軸まわりの角速度、及びＺ軸まわりの角速度）を取得し、当該取得された検出値から３つのオイラー角（ロール角、ピッチ角、及びヨー角）を算出し、算出したオイラー角（ロール角、ピッチ角、及びヨー角）をUAV１の姿勢として推定する。ここで、ロール角は、Ｘ軸まわりの角速度に基づいて算出され、ピッチ角は、Ｙ軸まわりの角速度に基づいて算出され、ヨー角は、Ｚ軸まわりの角速度に基づいて算出される。或いは、第２姿勢推定部１７は、算出されたオイラー角に基づき所定の変換式により変換された回転行列をUAV１の姿勢として推定してもよい。望ましくは、第２姿勢推定部１７は、算出されたオイラー角に基づき所定の変換式により変換されたクォータニオンをUAV１の姿勢として推定するとよい。なお、第１姿勢推定結果の次元（つまり、パラメータ数）と、第２姿勢推定結果の次元は一致されることが望ましい。例えば、第１姿勢推定結果がクォータニオンで表される場合、第２姿勢推定結果もクォータニオンで表される。
【００２９】
また、回転角検出部１５が３軸角速度センサ及び３軸加速度センサを備える場合、第２姿勢推定部１７は、UAV１が飛行開始後に、３軸角速度センサにより連続的に検出された検出値（各軸まわりの各角速度）と、３軸加速度センサにより連続的に検出された検出値（重力加速度を表す３次元ベクトル）とを取得する。そして、第２姿勢推定部１７は、３軸加速度センサにより検出された重力加速度を表す３次元ベクトルからロール角及びピッチ角を所定の計算式により算出する。また、第２姿勢推定部１７は、３軸角速度センサにより検出されたＸ軸まわりの角速度、Ｙ軸まわりの角速度、及びＺ軸まわりの角速度から３つのオイラー角（ロール角、ピッチ角、及びヨー角）を算出する。そして、第２姿勢推定部１７は、重力加速度を表す３次元ベクトルから算出されたロール角とピッチ角、及び相補フィルタ（またはカルマンフィルタ）を用いて、Ｘ軸まわりの角速度及びＹ軸まわりの角速度から算出されたロール角及びピッチ角を補正する。これにより、ロール方向及びピッチ方向のドリフト誤差が打ち消される。そして、第２姿勢推定部１７は、補正されたロール角、補正されたピッチ角、及び算出されたヨー角をUAV１の姿勢として推定する。或いは、第２姿勢推定部１７は、算出または補正されたオイラー角に基づき所定の変換式により変換された回転行列をUAV１の姿勢として推定してもよい。望ましくは、第２姿勢推定部１７は、算出または補正されたオイラー角に基づき所定の変換式により変換されたクォータニオンをUAV１の姿勢として推定するとよい。
【００３０】
また、回転角検出部１５が３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサを備える場合、第２姿勢推定部１７は、UAV１が飛行開始後に、３軸角速度センサにより連続的に検出された検出値（各軸まわりの各角速度）と、３軸加速度センサにより連続的に検出された検出値（重力加速度の３次元ベクトル）と、３軸地磁気センサにより検出された検出値（北を表す３次元ベクトル）とを取得する。そして、第２姿勢推定部１７は、３軸加速度センサにより検出された重力加速度を表す３次元ベクトルからロール角及びピッチ角を所定の計算式により算出する。また、第２姿勢推定部１７は、３軸角速度センサにより検出されたＸ軸まわりの角速度、Ｙ軸まわりの角速度、及びＺ軸まわりの角速度から３つのオイラー角（ロール角、ピッチ角、及びヨー角）を算出する。そして、第２姿勢推定部１７は、重力加速度を表す３次元ベクトルから算出されたロール角とピッチ角、及び相補フィルタ（またはカルマンフィルタ）を用いて、Ｘ軸まわりの角速度及びＹ軸まわりの角速度から算出されたロール角及びピッチ角を補正する。さらに、第２姿勢推定部１７は、３軸地磁気センサにより検出された北を表す３次元ベクトルからヨー角を所定の計算式により算出し、当該算出されたヨー角、及び相補フィルタ（またはカルマンフィルタ）を用いて、Ｚ軸まわりの角速度から算出されたヨー角を補正する。これにより、ヨー方向の誤差が打ち消される。そして、第２姿勢推定部１７は、補正されたロール角、補正されたピッチ角、及び補正されたヨー角をUAV１の姿勢として推定する。或いは、第２姿勢推定部１７は、補正されたオイラー角に基づき所定の変換式により変換された回転行列をUAV１の姿勢として推定してもよい。望ましくは、第２姿勢推定部１７は、補正されたオイラー角に基づき所定の変換式により変換されたクォータニオンをUAV１の姿勢として推定するとよい。
【００３１】
なお、回転角検出部１５は、撮像部１４等に比べて磁場等の影響を受けやすく、UAV１の近くに上記特定物体が存在すると、その検出値が信頼できない場合がある。そのため、このような状況下では、第２姿勢推定部１７による姿勢推定の精度（つまり、第２姿勢推定結果の精度）は、第１姿勢推定部１６による姿勢推定の精度（つまり、第１姿勢推定結果の精度）より低くなるため、第１姿勢推定結果がUAV１の姿勢推定に利用されることが望ましい。一方、UAV１の近くに特定物体が存在しない状況下では、第２姿勢推定部１７による姿勢推定の精度は、第１姿勢推定部１６による姿勢推定の精度より高くなるため、第２姿勢推定結果がUAV１の姿勢推定に利用されることが望ましい。
【００３２】
制御部１８は、プロセッサであるＣＰＵ、ＲＯＭ、ＲＡＭ、及び不揮発性メモリ等を備える。制御部１８は、例えばＲＯＭまたは不揮発性メモリに記憶された制御プログラムに従ってUAV１の飛行制御（つまり、UAV１の離陸から着陸までの制御）を行う。飛行制御においては、測位部１２から取得された位置情報、第１姿勢推定部１６から取得された第１姿勢推定結果、第２姿勢推定部１７から取得された第２姿勢推定結果、及び飛行計画を示す飛行計画情報等が用いられて、ロータの回転数の制御、UAV１の位置、姿勢、及び進行方向の制御が行われる。
【００３３】
特に、制御部１８は、第１姿勢推定部１６から取得された第１姿勢推定結果と、第２姿勢推定部１７から取得された第２姿勢推定結果とを、測位部１２から取得された位置情報（つまり、現在位置を示す位置情報）に基づく割合で利用することによりUAV１の姿勢の制御を行う。ここで、位置情報に基づく割合とは、換言すると、飛行中のUAV１の現在位置に応じて変化しうる割合である。このような割合は、第１姿勢推定結果が利用される重み“Ａ”と第２姿勢推定結果が利用される重み“Ｂ”との比「Ａ：Ｂ（Ａ対Ｂ）」（Ａ及びＢは０以上の値）で表されるとよい（この例では、第１姿勢推定結果が利用される割合は“Ａ／（Ａ＋Ｂ）”となり、第２姿勢推定結果が利用される割合は“Ｂ／（Ａ＋Ｂ）”となる）。例えば、「Ａ：Ｂ＝１：０」である場合、制御部１８は、第２姿勢推定結果を利用することなく、第１姿勢推定結果だけを利用してUAV１の姿勢の制御（つまり、第１姿勢推定結果に基づく飛行制御）を行う。一方、「Ａ：Ｂ＝０：１」である場合、制御部１８は、第１姿勢推定結果を利用することなく、第２姿勢推定結果だけを利用してUAV１の姿勢の制御（つまり、第２姿勢推定結果に基づく飛行制御）を行う。なお、「Ａ：Ｂ＝１：０」または「Ａ：Ｂ＝０：１」のように、片方の姿勢推定結果が利用される場合、第１姿勢推定結果と第２姿勢推定結果との両方の姿勢推定結果が得られなくてもよい。つまり、この場合、利用される方の姿勢推定結果だけが得られて、利用されない方については計算されなくてもよい。
【００３４】
一方、「Ａ：Ｂ＝１：１」である場合、制御部１８は、第１姿勢推定結果と第２姿勢推定結果とを均等に利用してUAV１の姿勢の制御を行う。その他、「Ａ：Ｂ＝１：２」、または「Ａ：Ｂ＝３：１」などの場合も考えられる。Ａ及びＢがいずれも０でない場合、制御部１８は、第１姿勢推定結果と第２姿勢推定結果とをマージして利用するとよい。第１姿勢推定結果と第２姿勢推定結果とをマージする方法の一例として、下記（１）〜（４）式に示すように、第１姿勢推定結果と第２姿勢推定結果をα値により合成（αブレンディング）する方法が挙げられる。下記（１）〜（４）式は、クォータニオンによりUAV１の姿勢を表す場合における各パラメータ（ｘ，ｙ，ｚ，ｗ）の合成算出式の例であり、線形補間の式に相当する。
【００３５】
ｘ＝α×ｘ１＋（α−１）×ｘ２・・・（１）
【００３６】
ｙ＝α×ｙ１＋（α−１）×ｙ２・・・（２）
【００３７】
ｚ＝α×ｚ１＋（α−１）×ｚ２・・・（３）
【００３８】
ｗ＝α×ｗ１＋（α−１）×ｗ２・・・（４）
【００３９】
ここで、αは０以上１以下の値であり、Ａ／（Ａ＋Ｂ）である。ｘ１、ｙ１、ｚ１、及びｗ１は、それぞれ、第１姿勢推定結果におけるクォータニオンの４つのパラメータを示す。ｘ２、ｙ２、ｚ２、及びｗ２は、それぞれ、第２姿勢推定結果におけるクォータニオンの４つのパラメータを示す。ｘ、ｙ、ｚ、及びｗは、それぞれ、α値により合成されたクォータニオンの４つのパラメータを示し、これらはUAV１の姿勢の制御に用いられる。ここで、線形補間の式に代えて球面線形補間の式を用いて（つまり、（１）〜（４）に示すαに代えて“ｓｉｎαθ／ｓｉｎθ”を用い、（α−１）に代えて“ｓｉｎ（１−α）θ／ｓｉｎθ”を用いて）、クォータニオンの４つのパラメータがそれぞれα値により合成されてもよい。これにより、第１姿勢推定結果と第２姿勢推定結果のずれ量が大きい場合であっても、高精度の合成結果を得ることができる。なお、上記（１）〜（４）式に示すような線形補間の式を用いて、オイラー角または回転行列で表される第１及び第２姿勢推定結果がα値により合成されても構わない。
【００４０】
ところで、UAV１の位置情報に基づく割合は、UAV１と特定物体との位置関係によって決定される。例えば、UAV１と特定物体との間の距離が短いほど、Ａに対してＢが小さくなるように決定される。これにより、第１姿勢推定結果が利用される割合が大きくなる。一方、UAV１と特定物体との間の距離が長いほど、Ａに対してＢが大きくなるように決定される。これにより、第２姿勢推定結果が利用される割合が大きくなる。なお、第１姿勢推定結果が利用される割合はUAV１と特定物体との間の距離の２乗に反比例するように決定されるとよい。このように、UAV１と特定物体との間の距離が用いられる場合、UAV１の位置情報に基づく割合とは、UAV１と特定物体との間の距離に基づく割合ということができる。
【００４１】
例えば、特定物体が離陸地点（例えば、ポート）付近に存在する場合、UAV１の位置情報に基づく割合とは、UAV１の離陸地点からの距離に基づく割合ということができる。これにより、第１姿勢推定結果と第２姿勢推定結果をUAV１の離陸地点からの距離に応じて変化する割合で利用することができる。この場合、制御部１８は、UAV１の位置情報に基づいて、UAV１の離陸地点からの距離を算出する。例えば、UAV１の離陸地点の三次元位置座標とUAV１の現在位置の三次元位置座標とから当該距離が算出される。そして、制御部１８は、UAV１が離陸してから所定の距離（例えば、３０ｍ）に達するまでは少なくとも第１姿勢推定結果（つまり、第１姿勢推定結果、または第１姿勢推定結果と第２姿勢推定結果とがマージされた姿勢推定結果）を利用し、所定の距離に達した後は第２姿勢推定結果を利用する（つまり、「Ａ：Ｂ＝０：１」）ことによりUAV１の姿勢の制御を行ってもよい。つまり、第２姿勢推定結果が信頼できる程度になることが予想される（換言すると、UAV１の姿勢の制御に支障を与えないような）所定の距離までUAV１が特定物体から離れた際に第２姿勢推定結果に基づく飛行制御に切り替えられる。これにより、第２姿勢推定結果が信頼できる程度になることが予想される早い段階から負荷の小さい姿勢制御に切り替えることができる。
【００４２】
また、特定物体が離陸地点付近に存在する場合、UAV１が上昇するほど磁場等の影響が少なくなるので、UAV１の高度が低いほど、Ａに対してＢが小さくなるように決定される一方、UAV１の高度が高いほど、Ａに対してＢが大きくなるように決定されるとよい。このようにUAV１の高度が用いられる場合、UAV１の位置情報に基づく割合とは、UAV１の高度に基づく割合ということができる。これにより、第１姿勢推定結果と第２姿勢推定結果をUAV１の飛行高度に応じて変化する割合で利用することができる。この場合、制御部１８は、UAV１が離陸してから所定の高度に達するまでは少なくとも第１姿勢推定結果を利用し、所定の高度に達した後は第２姿勢推定結果を利用することによりUAV１の姿勢の制御を行ってもよい。つまり、第２姿勢推定結果が信頼できる程度になることが予想される（換言すると、UAV１の姿勢の制御に支障を与えないような）所定の高度までUAV１が上昇した際に第２姿勢推定結果に基づく飛行制御に切り替えられる。これにより、第２姿勢推定結果が信頼できる程度になることが予想される早い段階から負荷の小さい姿勢制御に切り替えることができる。
【００４３】
また、制御部１８は、第１姿勢推定結果と第２姿勢推定結果とのずれ量が閾値以上である場合は少なくとも第１姿勢推定結果を利用し、当該ずれ量が閾値未満である場合は第２姿勢推定結果を利用することによりUAV１の姿勢の制御を行ってもよい。これにより、第１姿勢推定結果と第２姿勢推定結果とのずれ量がある程度小さくなった（つまり、第２姿勢推定結果が信頼できる程度になった）早い段階から負荷の小さい姿勢制御に切り替えることができる。なお、制御部１８は、第１姿勢推定結果と第２姿勢推定結果とのずれ量が閾値未満になったときから所定時間における当該ずれ量の積分値を算出し、当該ずれ量の積分値が閾値未満である場合に第２姿勢推定結果を利用してもよい。また、当該ずれ量は、第１姿勢推定結果及び第２姿勢推定結果がオイラー角、回転行列、クォータニオンの何れで表される場合であっても算出することができるが、クォータニオンで表される第１姿勢推定結果及び第２姿勢推定結果を用いて当該ずれ量を算出すれば計算処理が複雑になることを抑制できる。
【００４４】
また、制御部１８は、例えば離陸地点にいるオペレータの指示にしたがってUAV１の離陸前にUAV１の基準方位を真値（例えば、北）に校正（設定）し、３軸地磁気センサにより検出された基準方位（北）を示す値と真値とのずれ量が閾値以上である場合は少なくとも第１姿勢推定結果を利用し、当該ずれ量が閾値未満である場合は第２姿勢推定結果を利用することによりUAV１の姿勢の制御を行ってもよい。これにより、基準方位を示す値と真値とのずれ量がある程度小さくなった早い段階から負荷の小さい姿勢制御に切り替えることができる。なお、制御部１８は、基準方位を示す値と真値とのずれ量が閾値未満になったときから所定時間における当該ずれ量の積分値を算出し、当該ずれ量の積分値が閾値未満である場合に第２姿勢推定結果を利用してもよい。
【００４５】
また、広い範囲で強い磁場等がある所定のエリア（例えば、送電線に沿ったエリア）を特定できる場合がある。この場合、制御部１８は、UAV１が所定のエリア内にあるときは第１姿勢推定結果を利用し、UAV１が所定のエリア外にあるときは第２姿勢推定結果を利用することによりUAV１の姿勢の制御を行ってもよい。これにより、UAV１の飛行中に強い磁場等がある所定のエリア内に入ったか否かにしたがって第１姿勢推定結果の利用と第２姿勢推定結果を切り替えて利用することができる。
【００４６】
そして、UAV１の姿勢の制御では、上述したようにUAV１の位置情報に基づく割合で得られる姿勢推定結果（つまり、第１姿勢推定結果、第２姿勢推定結果、または第１姿勢推定結果と第２姿勢推定結果とがマージされた姿勢推定結果）にしたがってUAV１が正常な姿勢を保つようにフィードバック制御が行われる。
【００４７】
［３．UAV１の動作］
次に、図３を参照して、UAV１の動作について説明する。図３は、UAV１の制御部１８により実行される処理の一例を示すフローチャートである。なお、図３に示す処理の全部または一部は制御サーバＣＳにより行われてもよい。図４は、離陸前に設定されるUAV１の基準方位を示す図である。図５は、UAV１が付近に特定物体が存在するポートから離陸し目的地点へ向かうまでの様子を示す概念図である。以下に説明する動作では、例えばUTMS２から飛行計画情報を取得したUAV１が、図５に示すように建物Ｂの屋上に設置されたポートＰであって付近に特定物体Ｏが存在するポートＰ（離陸地点）から離陸し目的地点へ向かう場合を例にとって説明する。かかる動作の前提として、UAV１のオペレータがUAV１の基準点（例えば、図４に示す基準点Ｒ）を北（Ｎ）に向けてポートＰ上に水平に置いた後に、操縦端末から飛行開始指示を行うと、当該操縦端末から飛行開始指令がUAV１に送信される。なお、オペレータによりUAV１の電源オンがなされると、UAV１のバッテリから各部に電力が供給される。制御部１８は、UAV１の飛行中にUAV１の位置情報に基づいて、撮像部１４及び第１姿勢推定部１６への電力供給を制御（オンオフ制御）してもよい。
【００４８】
図３に示す処理は、上記飛行開始指令が受信されたときに開始される。図３に示す処理が開始されると、制御部１８は、キャリブレーションを行う（ステップＳ１）ことでUAV１の基準方位を真値（UAV１の基準点が現在向いている方位であり、例えば、北）に校正する。これにより、制御部１８は、正しい北を認識することになる。
【００４９】
次いで、制御部１８は、UAV１の各部を起動させ、UAV１の飛行制御を開始する（ステップＳ２）。これにより、測位部１２は、UAV１の現在位置を示す位置情報を所定周期で制御部１８へ出力する。撮像部１４は、検出された検出値を所定周期で第１姿勢推定部１６（または、第１姿勢推定部１６及び制御部１８）へ出力する。第１姿勢推定部１６は、撮像部１４からの検出値に基づいてVisual-SLAMを行うことでUAV１の姿勢推定を行い、第１姿勢推定結果を所定周期で制御部１８へ出力する。回転角検出部１５は、検出された検出値を所定周期で第２姿勢推定部１７（または、第２姿勢推定部１７及び制御部１８）へ出力する。第２姿勢推定部１７は、回転角検出部１５からの検出値に基づいてUAV１の姿勢推定を行い、第２姿勢推定結果を所定周期で制御部１８へ出力する。
【００５０】
次いで、制御部１８は、撮像部１４からの位置情報を取得する（ステップＳ３）。次いで、制御部１８は、第１姿勢推定部１６からの第１姿勢推定結果を取得する（ステップＳ４）。次いで、制御部１８は、第２姿勢推定部１７からの第２姿勢推定結果を取得する（ステップＳ５）。次いで、制御部１８は、ステップＳ３で取得された位置情報に基づいて、上述したように、割合を決定する（ステップＳ６）。次いで、制御部１８は、UAV１の位置、姿勢、及び進行方向の制御を含む飛行制御を行う（ステップＳ７）。かかる飛行制御において、制御部１８は、ステップＳ４で取得された第１姿勢推定結果と、ステップＳ５で取得された第２姿勢推定結果とを、ステップＳ６で決定された割合で利用することによりUAV１の姿勢の制御（姿勢推定結果の割合に基づく飛行制御）を行う。
【００５１】
次いで、制御部１８は、UAV１の飛行計画情報及び位置情報に基づいて、UAV１が目的地点に到着したか否かを判定する（ステップＳ８）。制御部１８は、UAV１が目的地点に到着していないと判定した場合（ステップＳ８：ＮＯ）、ステップＳ９へ進む。一方、制御部１８は、UAV１が目的地点に到着したと判定した場合（ステップＳ８：ＹＥＳ）、図３に示す処理を終了する。
【００５２】
ステップＳ９では、制御部１８は、姿勢推定結果の割合に基づく飛行制御から、第２姿勢推定結果に基づく飛行制御に切り替えるか否かを判定する。例えば、図５に示すように、制御部１８は、UAV１の高度情報に基づいてUAV１がポートＰから離陸した後、UAV１の高度（飛行高度）を特定し、当該特定された高度が所定の高度に達した場合、姿勢推定結果の割合に基づく飛行制御から、第２姿勢推定結果に基づく飛行制御に切り替えると判定する。或いは、制御部１８は、UAV１の位置情報に基づいてUAV１のポートＰからの距離を算出し、当該算出された距離が所定の距離に達した場合、第２姿勢推定結果に基づく飛行制御に切り替えると判定してもよい。或いは、制御部１８は、直近に取得された第１姿勢推定結果と直近に取得された第２姿勢推定結果との間のずれ量を算出し、当該算出されたずれ量が閾値未満である場合、第２姿勢推定結果に基づく飛行制御に切り替えると判定してもよい。或いは、制御部１８は、回転角検出部１５の３軸地磁気センサにより検出された基準方位（北）を示す値と真値とのずれ量を算出し、当該算出された閾値未満である場合、第２姿勢推定結果に基づく飛行制御に切り替えると判定してもよい。
【００５３】
そして、制御部１８は、第２姿勢推定結果に基づく飛行制御に切り替えないと判定した場合（ステップＳ９：ＮＯ）、ステップＳ３に戻り、上記と同様の処理を繰り返す。一方、制御部１８は、第２姿勢推定結果に基づく飛行制御に切り替えると判定した場合（ステップＳ９：ＹＥＳ）、第１姿勢推定結果の取得を停止し、ステップＳ１０へ進む。ここで、制御部１８は、撮像部１４及び第１姿勢推定部１６への電力供給をオフにしてもよい。つまり、UAV１の位置情報に基づいて、撮像部１４及び第１姿勢推定部１６への電力供給が制御される。これにより、UAV１における消費電力を抑えることができる。
【００５４】
ステップＳ１０では、制御部１８は、撮像部１４からの位置情報を取得する。次いで、制御部１８は、第２姿勢推定部１７からの第２姿勢推定結果を取得する（ステップＳ１１）。次いで、制御部１８は、UAV１の位置、姿勢、及び進行方向の制御を含む飛行制御を行う（ステップＳ１２）。かかる飛行制御において、制御部１８は、ステップＳ１１で取得された第２姿勢推定結果を利用することによりUAV１の姿勢の制御を行う。
【００５５】
次いで、制御部１８は、UAV１の飛行計画情報及び位置情報に基づいて、UAV１が目的地点に到着したか否かを判定する（ステップＳ１３）。制御部１８は、UAV１が目的地点に到着していないと判定した場合（ステップＳ１３：ＮＯ）、ステップＳ１４へ進む。一方、制御部１８は、UAV１が目的地点に到着したと判定した場合（ステップＳ１３：ＹＥＳ）、図３に示す処理を終了する。
【００５６】
ステップＳ１４では、制御部１８は、UAV１の位置情報に基づいて、UAV１が強い磁場等がある所定のエリアAr（例えば事前に登録されたエリア）に入ったか否かを判定する。制御部１８は、UAV１がエリアArに入っていないと判定した場合（ステップＳ１４：ＮＯ）、ステップＳ１０に戻り、上記と同様の処理を繰り返す。一方、制御部１８は、UAV１がエリアArに入ったと判定した場合（ステップＳ１４：ＹＥＳ）、第２姿勢推定結果の取得を停止し、ステップＳ１５へ進む。これにより、図５に示すように、第２姿勢推定結果に基づく飛行制御から、第１姿勢推定結果に基づく飛行制御に切り替えられる。
【００５７】
なお、ステップＳ９において、撮像部１４及び第１姿勢推定部１６への電力供給がオフされた場合、制御部１８は、少なくともエリアArに進入する前（例えば、数秒前）に、撮像部１４及び第１姿勢推定部１６への電力供給をオンにして撮像部１４及び第１姿勢推定部１６を起動させる。これにより、第２姿勢推定結果に基づく飛行制御から第１姿勢推定結果に基づく飛行制御へスムーズに切り替えることができる。例えば、エリアArに進入する前に、第２姿勢推定結果と第１姿勢推定結果との相対関係を予め取得しておくことで、当該エリアArに進入し、第１姿勢推定結果に基づく飛行制御に切り替えられても安定した飛行を続けることができる。また、当該エリアArを何度も飛行したり、複数のUAV１が飛行したりした場合、当該エリアArの特徴点マップ情報を蓄積しておくことで第１姿勢推定部１６による姿勢推定の精度向上が期待できる。
【００５８】
ステップＳ１５では、制御部１８は、撮像部１４からの位置情報を取得する。次いで、制御部１８は、第１姿勢推定部１６からの第１姿勢推定結果を取得する（ステップＳ１６）。次いで、制御部１８は、UAV１の位置、姿勢、及び進行方向の制御を含む飛行制御を行う（ステップＳ１７）。かかる飛行制御において、制御部１８は、ステップＳ１６で取得された第１姿勢推定結果を利用することによりUAV１の姿勢の制御を行う。
【００５９】
次いで、制御部１８は、UAV１の飛行計画情報及び位置情報に基づいて、UAV１が目的地点に到着したか否かを判定する（ステップＳ１８）。制御部１８は、UAV１が目的地点に到着していないと判定した場合（ステップＳ１８：ＮＯ）、ステップＳ１９へ進む。一方、制御部１８は、UAV１が目的地点に到着したと判定した場合（ステップＳ１８：ＹＥＳ）、図３に示す処理を終了する。
【００６０】
ステップＳ１９では、制御部１８は、UAV１の位置情報に基づいて、UAV１が上記エリアAｒから出たか否かを判定する。制御部１８は、UAV１がエリアAｒから出ていないと判定した場合（ステップＳ１９：ＮＯ）、ステップＳ１５に戻り、上記と同様の処理を繰り返す。一方、制御部１８は、UAV１がエリアAｒから出たと判定した場合（ステップＳ１９：ＹＥＳ）、第１姿勢推定結果の取得を停止し、ステップＳ１０に戻る。これにより、図５に示すように、第１姿勢推定結果に基づく飛行制御から、第２姿勢推定結果に基づく飛行制御に切り替えられる。
【００６１】
以上説明したように、上記実施形態によれば、第１姿勢推定部１６が撮像部１４により検出された検出値に基づいてVisual-SLAMを行うことでUAV１の姿勢推定を行い、第２姿勢推定部１７が回転角検出部１５により検出された検出値に基づいてUAV１の姿勢推定を行う。そして、制御部１８が第１姿勢推定部１６からの第１姿勢推定結果と、第２姿勢推定部１７からの第２姿勢推定結果とをUAV１の位置情報に基づく割合で利用することによりUAV１の姿勢の制御を行うように構成したので、UAV１の離陸地点付近に特定物体が存在することで地磁気センサや慣性センサにより検出された検出値が信頼できない場合であっても、安定したUAV１の離陸が実現でき、UAV１を安定して飛行させることができる。
【００６２】
なお、上記実施形態は本発明の一実施形態であり、本発明は上記実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で上記実施形態から種々構成等に変更を加えてもよく、その場合も本発明の技術的範囲に含まれる。上記実施形態においては、UAV１の制御部１８が第１姿勢推定結果と第２姿勢推定結果とをUAV１の位置情報に基づく割合で利用することによりUAV１の姿勢の制御を行うように構成したが、制御サーバＣＳが本発明の制御装置としてUAV１の姿勢の制御を行うように構成してもよい。この場合、制御サーバＣＳが第１取得部、第２取得部、第３取得部、第１姿勢推定部、第２姿勢推定部、及び飛行制御部等として機能する。そして、制御サーバＣＳは、撮像部１４により検出された第１検出値をUAV１から取得し、回転角検出部１５により検出された第２検出値をUAV１から取得し、測位部１２により検出されたUAV１の現在位置を示す位置情報を取得する。そして、制御サーバＣＳは、取得した第１検出値に基づいて第１姿勢推定部によりUAV１の姿勢推定を行い、取得した第２検出値に基づいて第２姿勢推定部によりUAV１の姿勢推定を行い、第１姿勢推定部による第１姿勢推定結果と第２姿勢推定部による第２姿勢推定結果とを、取得した位置情報に基づく割合で利用することによりUAV１の姿勢を制御する。かかる制御は、制御サーバＣＳからUAV１へ制御指令が送信されることで行われる。上記実施形態においては、本発明をUAV１に対して適用した場合を例にとって説明したが、本発明は航空機内に操縦者（パイロット）が存在しなくても飛行することができる有人航空機に対しても適用可能である。
【符号の説明】
【００６３】
１  UAV
２  UTMS
３  PMS
１１  駆動部
１２  測位部
１３  無線通信部
１４  撮像部
１５  回転角検出部
１６  第１姿勢推定部
１７  第２姿勢推定部
１８  制御部
ＣＳ  制御サーバ
Ｓ  飛行システム
【要約】
第１姿勢推定部１６が撮像部１４により検出された検出値に基づいてVisual-SLAMを行うことでUAV１の姿勢推定を行い、第２姿勢推定部１７が回転角検出部１５により検出された検出値に基づいてUAV１の姿勢推定を行う。そして、制御部１８が第１姿勢推定部１６からの第１姿勢推定結果と、第２姿勢推定部１７からの第２姿勢推定結果とをUAV１の位置情報に基づく割合で利用することによりUAV１の姿勢の制御を行う。【図１】
0006934116.tif 000002
【図２】
0006934116.tif 000003
【図３】
0006934116.tif 000004
【図４】
0006934116.tif 000005
【図５】
0006934116.tif 000006