JP7067629B2 - 制御装置、制御方法、及びプログラム - Google Patents

Description

本発明は、飛翔体の飛行の制御に関する。

アームを備える飛翔体を利用して、様々な作業が行われている。そのような作業の一例として、ハンマーを備えるアームを利用した打音検査がある。打音検査は、橋梁、トンネル、又は配管などの種々の構造物の安全点検などで取り入れられている。

このように飛翔体を活用する技術を開示する先行技術文献として、特許文献１がある。特許文献１の発明は、飛行装置（自律飛行式の無人ヘリコプタ：いわゆるドローン）、地上側装置、及び管制装置から構成される。飛行装置には打音検査装置が取り付けられており、地上側装置で飛行装置を計測しながら、管制装置による自律した打音検査が行われる。

国際公開第２０１７／２０４０５０号

特許文献１では、飛翔体の飛行制御を行う際、飛翔体の位置が特定の一点で表されており、その特定の一点を目標の位置へ移動させるようにして、飛翔体の飛行制御が行われている。すなわち、飛翔体の特定の一点を他の位置よりも優先して、飛翔体の飛行制御が行われている。しかしながら本発明者は、アームを備える飛翔体の飛行制御において、飛翔体の飛行制御における優先位置を特定の一点に固定し続けることが好ましくないことを見出した。

本発明は、上記課題に鑑みてなされたものであり、その目的の一つは、アームを有する飛翔体の飛行制御を適切に行う技術を提供することである。

本発明の制御装置は、アームを有する飛翔体の飛行を制御する制御装置である。当該制御装置は、１）飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替部と、２）制御モードが第１モードである場合には飛翔体の第１位置を優先して飛翔体の飛行を制御し、制御モードが第２モードである場合には飛翔体の第２位置を優先して飛翔体の飛行を制御する制御部と、を有する。第１位置はアーム上の位置であり、第２位置はアーム上の位置ではない。

本発明の制御方法は、アームを有する飛翔体の飛行を制御するコンピュータによって実行される制御方法である。当該制御方法は、１）飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替ステップと、２）制御モードが第１モードである場合には飛翔体の第１位置を優先して飛翔体の飛行を制御し、制御モードが第２モードである場合には飛翔体の第２位置を優先して飛翔体の飛行を制御する制御ステップと、を有する。第１位置はアーム上の位置であり、第２位置はアーム上の位置ではない。

本発明のプログラムは、本発明の制御方法が有する各ステップをコンピュータに実行させる。

本発明によれば、アームを有する飛翔体の飛行制御を適切に行う技術が提供される。

実施形態１の制御装置の概要を例示する図である。 第１モードと第２モードで許容される制御誤差の違いを例示する図である。 制御装置の機能構成を例示するブロック図である。 制御装置を実現するための計算機を例示する図である。 実施形態１の制御装置によって実行される処理の流れを例示するフローチャートである。 実施形態２の制御装置によって実行される処理の流れを例示する図である。 実施形態２の制御装置によって実行される処理の流れを例示する図である。

以下、本発明の実施の形態について、図面を用いて説明する。尚、すべての図面において、同様な構成要素には同様の符号を付し、適宜説明を省略する。また各ブロック図において、特に説明がない限り、各ブロックは、ハードウエア単位の構成ではなく機能単位の構成を表している。

［実施形態１］
＜概要＞
図１は、実施形態１の制御装置２０００の概要を例示する図である。なお、図１は、制御装置２０００の理解を容易にするためにその概要の一例を示しているにすぎず、制御装置２０００の機能を何ら限定するものではない。

制御装置２０００は、飛翔体１０の飛行を制御する。飛翔体１０は、コンピュータによってその飛行が制御される任意の飛翔体である。例えば飛翔体１０はドローンである。

飛翔体１０はアーム２０を有する。例えばアーム２０は、打音検査で利用されるハンマーを有するアームや、所望の物体を把持するために利用される把持部材を有するアームなどである。

一般に、飛翔体の飛行を制御する場合、その機体中心（重心）の位置を優先した飛行制御（すなわち、機体中心が所望の位置になるように行う飛行制御）が行われることが多い。なぜなら、飛翔体の機体設計や制御設計が、機体中心を基準として行われていることが多いため、機体中心の位置を優先した飛行制御により、エネルギー効率が高くなる、アクチュエータの耐久性が高くなる、及び制御アルゴリズムの汎用性が高くなるといった種々のメリットが生じるためである。

しかしながら本発明者は、アーム２０を用いた作業のために飛翔体１０の飛行を制御する際には、アーム２０の位置を正確に制御することが重要であることを見いだした。アーム２０の位置を正確に制御することが重要である理由は、アーム２０の位置を正確に制御できないと、アーム２０を利用した作業を意図通りに実現することが難しいためである。例えばアーム２０を利用して打音検査をする場合、アーム２０の位置を正確に制御して、検査対象の所定の位置をアーム２０で正確に叩く必要がある。さもないと、点検したい箇所とは違う箇所を叩いてしまうことになり、意図した通りに打音検査を行うことができない。

特に飛翔体１０がマルチコプターである場合、飛翔体１０を水平方向に前進させるためには、飛翔体１０を前に傾ける必要がある。このように飛翔体１０の姿勢が傾くと、アーム２０の先端が大きく上下することになる。そのため、機体中心を優先して飛翔体１０を制御すると、アーム２０の位置が大きくぶれることになる。一方で、アーム２０の位置を優先して飛翔体１０の飛行制御を行うと、アーム２０の位置を正確に制御できるため、アーム２０を利用した作業を正確に行うことができる。

ただし前述したように、エネルギー効率等の面では、機体中心などを優先した制御が好ましい。そのため、常にアーム２０を優先して飛翔体１０の飛行制御を行うことも、好ましくないと言える。

そこで制御装置２０００は、アーム２０の位置を正確に制御すべき状況とそれ以外の状況とで、飛翔体１０の飛行制御における飛翔体１０の優先制御位置を切り替える。具体的には、制御装置２０００は、アーム２０の位置を正確に制御すべき状況では、アーム２０の位置を優先して飛翔体１０の飛行制御を行い、それ以外の状況では、アーム２０の位置以外の基準位置（例えば前述した機体中心）を優先して飛翔体１０の飛行制御を行う。こうすることで、アーム２０を用いた作業を正確に実現しつつ、できる限り飛翔体１０を効率よく動作させることができる。

より具体的には、制御装置２０００では、以下のような制御が行われる。まず制御装置２０００では、飛翔体の制御に関する制御モードが、少なくとも２つ存在する。以下、これら２つのモードを、第１モードと第２モードと呼ぶ。第１モードは、アーム２０の位置を正確に制御するための制御モードである。そのため、制御装置２０００は、飛翔体１０の制御モードが第１モードである場合、アーム２０上の位置である第１位置３０を優先して、飛翔体１０の飛行制御を行う。

一方、第２モードは、第１モードとは異なるモードである。制御装置２０００は、飛翔体１０の制御モードが第２モードである場合、アーム２０上の位置以外の基準位置である第２位置４０を優先して、飛翔体１０の飛行制御を行う。例えば第２位置は機体中心である。

図１には、第１モードにおける飛翔体１０の飛行制御と、第２モードにおける飛翔体１０の飛行制御のそれぞれが例示されている。第１モードでは、アーム２０上の第１位置３０を優先して飛翔体１０の飛行制御が行われている。具体的には、アーム２０上の第１位置３０が安定して目標位置に向かうように、飛翔体１０の飛行が制御されている。例えば目標位置は、打音検査においてアーム２０で叩くべき箇所である。アーム２０が安定して目標位置に向かっているため、アーム２０で正確に目標位置を叩くことができる。一方で第２位置４０（この例では機体中心）については、上下に大きくぶれてしまっている。

これに対し、第２モードでは、第２位置４０を優先して飛翔体１０の飛行制御が行われている。図１の例において、第２位置４０は機体中心である。よって、機体中心が安定して目標位置に向かって移動するように、飛翔体１０の飛行が制御されている。一方で、第１位置３０については、上下に大きくぶれてしまっている。

図２は、第１モードと第２モードで許容される制御誤差の違いを例示する図である。第１モードでは、アーム２０上の第１位置３０について許容される制御誤差が小さい一方で、アーム２０以外の基準位置である第２位置４０について許容される制御誤差が大きくなっている。これは、第１位置３０を優先した飛行制御が行われることを表している。これに対し、第２モードでは、第２位置４０について許容される制御誤差が小さい一方で、第１位置３０について許容される制御誤差が大きくなっている。これは、第２位置４０を優先した飛行制御が行われることを表している。

＜作用・効果＞
本実施形態の制御装置２０００によれば、アーム２０の位置を正確に制御すべき状況とそれ以外の状況とで、飛翔体１０の飛行制御における飛翔体１０の優先制御位置が切り替わる。具体的には、アーム２０の位置を正確に制御すべき状況に適した制御モードである第１モードでは、アーム２０上の位置である第１位置３０を優先した飛行制御が行われる。一方、それ以外の状況に適した制御モードである第２モードでは、アーム２０上の位置以外の基準位置である第２位置４０を優先した飛行制御が行われる。こうすることで、アーム２０を用いた作業を正確に実現しつつ、「エネルギー効率が高くなる、アクチュエータの耐久性が高くなる、及び制御アルゴリズムの汎用性が高くなる」などといった機体中心等を優先した飛行制御のメリットも享受できる。

特に、劣駆動系に分類されるロボットでは、制御における制限が大きい。劣駆動系ロボットとは、そのロボットに搭載されたアクチュエータによって直接制御可能な運動自由度の数が、そのロボットの実際の運動自由度の数よりも少ないロボットのことである。例えば４つのローターを有するマルチコプター（クアッドコプター）は、制御可能な運動自由度の数が４である一方で、実際の運動自由度の数６（並進３自由度と回転３自由度）であるため、劣駆動系ロボットである。そのため、機体を傾けないと水平方向の推力を得られなかったり、傾いた状態では一点に滞空し続けることができないなどのように、制御における制限が大きい。そのため、飛翔体１０の機体中心とアーム２０の位置の双方を正確に制御することは難しいため、本実施形態の制御装置２０００が行うような、飛翔体１０の機体中心を優先した制御と、アーム２０の位置を優先した制御との切り替えが重要となる。

以下、本実施形態についてさらに詳細を述べる。

＜機能構成の例＞
図３は、制御装置２０００の機能構成を例示するブロック図である。制御装置２０００は、アーム２０を有する飛翔体１０の飛行を制御する。そのために、制御装置２０００は、切替部２０２０及び制御部２０４０を有する。切替部２０２０は、飛翔体１０の飛行制御に関する制御モードを、第１モードと第２モードのいずれか一方に切り替える。制御部２０４０は、制御モードが第１モードである場合、飛翔体１０の第１位置３０を優先して、飛翔体１０の飛行制御を行う。一方、制御モードが第２モードである場合、制御部２０４０は、飛翔体１０の第２位置４０を優先して、飛翔体１０の飛行制御を行う。第１位置３０は、アーム２０上の位置である。一方、第２位置４０は、アーム２０上の位置以外の位置である。

＜制御装置２０００のハードウエア構成の例＞
制御装置２０００の各機能構成部は、各機能構成部を実現するハードウエア（例：ハードワイヤードされた電子回路など）で実現されてもよいし、ハードウエアとソフトウエアとの組み合わせ（例：電子回路とそれを制御するプログラムの組み合わせなど）で実現されてもよい。以下、制御装置２０００の各機能構成部がハードウエアとソフトウエアとの組み合わせで実現される場合について、さらに説明する。

図４は、制御装置２０００を実現するための計算機１０００を例示する図である。計算機１０００は、任意の種類の計算機である。例えば計算機１０００は、飛翔体１０に内蔵される SoC（System on a Chip）などの制御チップとして実現される。ただし、制御装置２０００は、飛翔体１０を外部から制御する装置であってもよい。この場合、制御装置２０００は、PC（Personal Computer）などの据え置き型のコンピュータや、スマートフォンなどの可搬型のコンピュータとして実現されてもよい。計算機１０００は、制御装置２０００を実現するために設計された専用の計算機であってもよいし、汎用の計算機であってもよい。

計算機１０００は、バス１０２０、プロセッサ１０４０、メモリ１０６０、ストレージデバイス１０８０、入出力インタフェース１１００、及びネットワークインタフェース１１２０を有する。バス１０２０は、プロセッサ１０４０、メモリ１０６０、ストレージデバイス１０８０、入出力インタフェース１１００、及びネットワークインタフェース１１２０が、相互にデータを送受信するためのデータ伝送路である。ただし、プロセッサ１０４０などを互いに接続する方法は、バス接続に限定されない。

プロセッサ１０４０は、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、FPGA（Field－Programmable Gate Array）などの種々のプロセッサである。メモリ１０６０は、RAM（Random Access Memory）などを用いて実現される主記憶装置である。ストレージデバイス１０８０は、RAM や ROM（Read Only Memory）などを用いて実現される補助記憶装置である。

入出力インタフェース１１００は、計算機１０００と入出力デバイスとを接続するためのインタフェースである。例えば入出力インタフェース１１００には、キーボードなどの入力装置や、ディスプレイ装置などの出力装置が接続される。

また、計算機１０００が飛翔体１０に内蔵される制御チップである場合、入出力インタフェース１１００には、飛翔体１０のアクチュエータが接続される。計算機１０００は、入出力インタフェース１１００を介してアクチュエータへ制御信号を送信することにより、飛翔体１０の飛行を制御する。また、入出力インタフェースには、飛翔体１０の状態を把握するための種々のセンサ（GPS（Global Positioning System）センサや加速度センサなど）が接続される。計算機１０００は、入出力インタフェースを介してセンサから検出値を得ることにより、飛翔体１０に関する観測値を得る。

ネットワークインタフェース１１２０は、計算機１０００を通信網に接続するためのインタフェースである。この通信網は、例えば LAN（Local Area Network）や WAN（Wide Area Network）である。ネットワークインタフェース１１２０が通信網に接続する方法は、無線接続であってもよいし、有線接続であってもよい。

また、計算機１０００が飛翔体１０の外部に設けられているコンピュータである場合、ネットワークインタフェース１１２０には、飛翔体１０に内蔵されている制御チップが接続される。計算機１０００は、ネットワークを介して、飛翔体１０内の制御チップに対し、飛翔体１０の飛行を制御する指示を送信する。制御チップは、計算機１０００から受信した指示に従ってアクチュエータに制御信号を送信することにより、飛翔体１０の飛行を制御する。また、計算機１０００は、ネットワークを介して、飛翔体１０に設けられている種々のセンサから、飛翔体１０の状態を把握するための検出値を取得する。これらの検出値は、例えば、制御チップを介して送信される。

ストレージデバイス１０８０は、制御装置２０００の各機能構成部を実現するプログラムモジュールを記憶している。プロセッサ１０４０は、これら各プログラムモジュールをメモリ１０６０に読み出して実行することで、各プログラムモジュールに対応する機能を実現する。

＜処理の流れ＞
図５は、実施形態１の制御装置２０００によって実行される処理の流れを例示するフローチャートである。制御部２０４０は、制御モードが第１モードと第２モードのどちらであるかを判定する（Ｓ１０２）。制御モードが第１モードである場合（Ｓ１０２：第１モード）、制御部２０４０は、第１位置３０を優先して飛翔体１０の飛行を制御する（Ｓ１０４）。制御モードが第２モードである場合（Ｓ１０２：第２モード）、制御部２０４０は、第２位置４０を優先して飛翔体１０の飛行を制御する（Ｓ１０６）。なお、図５に示す一連の処理は、例えば所定の周期（0.1sec に１回など）で実行される。

＜第１モードと第２モードの切り替え＞
切替部２０２０は、飛翔体１０の制御モードを、第１モード又は第２モードに設定する。飛翔体１０の制御モードは、例えば、ユーザの入力操作に基づいて設定される。具体的には、切替部２０２０は、第１モードを指定するユーザ入力を受け付けたら、飛翔体１０の制御モードを第１モードに設定し、第２モードを指定するユーザ入力を受け付けたら、飛翔体１０の制御モードを第２モードに設定する。

その他にも例えば、第１モードと第２モードの切り替えは、切替部２０２０によって自動的に行われてもよい。例えば、飛翔体１０が所定の作業位置で作業を行うために、その作業位置へ向かっているとする。この場合、切替部２０２０は、飛翔体１０の現在位置と作業位置との距離に基づいて、モードの切り替えを行う。より具体的には、切替部２０２０は、飛翔体１０の現在位置と作業所定との距離が閾値以上である場合には、飛翔体１０の制御モードを第２モードに設定し、その距離が閾値未満である場合には、飛翔体１０の制御モードを第１モードに設定する。こうすることで、作業位置から或る程度離れている段階では、飛翔体１０の機体中心などを優先した制御によってエネルギー効率等を重視することができる。また、作業位置に或る程度近づいた場合には、アーム２０を優先した制御によって、正確な作業を行うための準備をすることができる。なお、モードの切り替えに利用する上記閾値は、切替部２０２０に予め設定しておいてもよいし、切替部２０２０からアクセス可能な記憶装置に記憶させておいてもよい。

＜飛翔体１０の飛行制御：Ｓ１０４とＳ１０６＞
飛翔体１０の飛行制御は、飛翔体１０の現在の状態と飛翔体１０の目標の状態とに基づいて飛翔体１０に対する制御入力を決定し、その制御入力を制御対象（飛翔体１０のアクチュエータなど）に出力することで実現される。ここで、決定した制御入力を制御対象へ出力する技術には、既存の技術を利用することができる。そこで、ここでは、飛翔体１０の現在の状態と飛翔体１０の目標の状態とを把握する方法、及びそれらに基づいて飛翔体１０に対する制御入力を決定する方法について説明する。

＜＜現在の状態を把握する方法＞＞
飛翔体１０の現在の状態を表す状態モデルと、飛翔体１０の観測を表す観測モデルは、例えば以下の様に表される。

ここで、X(t) は時刻 t における飛翔体１０の状態を表す。X'(t) は、時刻 t における状態の変化を表す。U(t) は飛翔体１０に対する制御入力を表す。D は外乱を表す。A と B は状態モデルの係数である。Y(t) は、時刻 t における飛翔体１０の観測を表す。

なおここでは、状態を線形変換することで観測が得られるものとしてモデル化されている。しかしながら、観測と状態の関係は、必ずしもこの関係に限定されない。例えば、観測誤差（ガウス分布に従う確率的な誤差など）を表す誤差項を観測モデルに導入してもよい。

制御部２０４０は、飛翔体１０の現在の状態を推定する。具体的には、制御部２０４０は、センサ等を利用して得られた現在時刻 t における飛翔体１０に関する観測値と、前述した状態モデル及び観測モデルとを用いて、飛翔体１０の現在の（時刻 t の）状態を推定する。なお、観測値、状態モデル、及び観測モデルを用いて状態を推定する技術には、例えばカルマンフィルタなどの既存のアルゴリズムを利用することができる。また、これらのアルゴリズムを利用して、前述した観測モデルや状態モデルにおけるパラメータが、観測値を利用して更新されていく。

飛翔体１０の状態を定める要素、すなわち、ベクトル X の要素には、飛翔体１０の種類ごとに、様々なものを採用することができる。飛翔体１０がマルチコプターである場合、例えば状態ベクトル X は、以下の様に定められる。

ここで、x、y、及び z は、飛翔体１０の第２位置４０（例えば、飛翔体１０の機体中心）の x 座標、y 座標、及び z 座標を表す。xarm、yarm、及び zarm は、第１位置３０（例えば、アーム２０の先端位置）の x 座標、y 座標、及び z 座標を表す。記号 ' は、時刻 t における変化（すなわち微分）を意味する。例えば x' は、時刻 t における飛翔体１０の第２位置４０の x 座標の変化を表す。状態ベクトル X には、飛翔体１０に関するその他の状態（例えば、飛翔体１０の姿勢を表すロール角、ピッチ角、及びヨー角など）が含まれてもよい。

これら飛翔体１０の状態を定める種々の要素の値は、例えば、種々のセンサの検出値で定まる観測モデルの出力から推定することができる。ここで、センサを利用して、飛翔体の位置などの状態を定めるための観測を行う技術には、既存の様々な技術を利用することができる。

例えば、飛翔体１０に IMU（inertial measurement unit）センサなどの内界センサを設けておく。IMU センサを利用することにより、飛翔体１０の位置を定める x, y, z それぞれの加速度 x'', y'', 及び z'' を観測することができる。そして、観測された加速度 x'', y'', z'' を用いて拡張カルマンフィルタなどのアルゴリズムを実行することにより、飛翔体１０の位置 (x,y,z) を推定することができる。

また、観測の精度を高めるために、内界センサと併用して、外界センサが用いられてもよい。外界センサとしては、例えば、トータルステーションを利用することができる。その他にも例えば、レーダ、ライダ、又はカメラなどを用いてもよい。

アームの位置 (xarm, yarm, zarm) については、飛翔体１０の位置 (x,y,z) との相対位置を予め定めておく。こうすることで、センサの検出値を利用して算出した飛翔体１０の位置から、アームの位置を算出することができる。

＜＜制御入力を決定する方法＞＞
制御部２０４０は、飛翔体１０の現在の状態を目標状態へ変化させるように、飛翔体１０を制御する。具体的には、制御部２０４０は、飛翔体１０の現在の状態を目標状態に変化させるために必要な、飛翔体１０に対する制御入力 U を決定する。なお、目標状態の定め方については後述する。

制御入力 U は、例えば、評価関数 J(X, Xref, U) に基づく以下の目的関数を満たすように決定される。

評価関数 J は、現実の状態 X と目標状態 Xref の誤差、及び制御入力 U を評価している。i はステップ番号を表す。ステップ番号とは、離散時間に割り当てた番号である。H は予測ホライズン長を表す。すなわち、評価関数 J は、H ステップ先までの予測について評価を行う。Q は、時点 i における現実の状態と目標状態の誤差の重みを表す行列である。R は、制御入力の重みを表す行列である。ここでは、飛翔体１０がクアッドコプターであり、飛翔体１０に対し、各プロペラの回転速度を制御する制御入力を与えることができることを仮定している。そして、重み行列 R の各要素は、各プロペラについて与える制御入力の重みを表している。

ここで、上述した目的関数を満たす制御入力 U の具体的な算出方法、すなわち、評価関数 J を最小化する制御入力 U の具体的な算出方法には、既存の手法を利用することができる。

ここで、第１モードでは第１位置３０の制御を優先する一方で、第２モードでは第２位置４０の制御を優先する。このような制御の違いは、上述した評価関数における重み行列 Q の内容を変化させることで実現できる。具体的には、第１モードでは、第１位置３０を表す要素である xarm、yarm、及び zarm に対する重み（q7、q8、及び q9）を、第２位置４０を表す要素である x、y、及び z に対する重み（q1、q3、及び q5）よりも大きくする。こうすることで、第１位置３０の誤差が評価関数に与える影響が、第２位置４０の誤差が評価関数に与える影響よりも大きくなるため、第１位置３０の制御が優先されることになる。

一方、第２モードでは、第２位置４０を表す要素である x、y、及び z に対する重み（q1、q3、及び q5）を、第１位置３０を表す要素である xarm、yarm、及び zarm に対する重み（q7、q8、及び q9）よりも大きくする。こうすることで、第２位置４０の誤差が評価関数に与える影響が、第１位置３０の誤差が評価関数に与える影響よりも大きくなるため、第２位置４０の制御が優先されることになる。

なお、重み行列 R の設定については、実施形態２で説明する。第１モードと第２モードの切り替えのみを考慮する場合には、R の値は固定でよい。

第１モードと第２モードにおける制御の切り替えを実現するためには、前述したように、重み行列 Q を変更する必要がある。そこで例えば、第１モード用の重み行列 Q1 と、第２モード用の重み行列 Q2 を、予め制御部２０４０からアクセス可能な記憶装置に記憶させておく。制御モードが第１モードであると判定した場合（Ｓ１０２：第１モード）、制御部２０４０は、上記記憶装置から Q1 を読み出して上述の目的関数に設定し、その目的関数を満たす制御入力 U を決定する。こうすることで、第１位置３０を優先した制御を実現する。一方、制御モードが第２モードであると判定した場合（Ｓ１０２：第２モード）、制御部２０４０は、上記記憶装置から Q2 を読み出して上述の目的関数に設定し、その目的関数を満たす制御入力 U を決定する。こうすることで、第２位置４０を優先した制御を実現する。

＜＜目標状態を設定する方法＞＞
制御部２０４０は、飛翔体１０の現在の状態 X を目標状態 Xref に近づけるように、制御入力を決定する。ここで、制御対象の目標状態を定める方法には、既存の技術を利用することができる。ここでは、その方法の一例を記載する。

例えば、飛翔体１０について、予め飛行計画を定めておく。飛行計画は、飛翔体１０の理想状態の時系列データとして定められる。ここで、外乱の影響などにより、飛翔体１０の実際の状態は理想状態とは異なるものになりうる。そして、飛翔体１０の実際の状態と理想状態との差異が大きくなると、飛翔体１０の状態を一度の制御で理想状態に戻すことは難しい。そこで制御部２０４０は、飛翔体１０の状態を理想状態に戻すための飛行計画を動的に生成する。この飛行計画が、飛翔体１０の参照状態の時系列データを表す。

＜結果の出力＞
制御装置２０００は、制御部２０４０によって決定された制御入力を表す制御信号を、飛翔体１０に対して出力する。こうすることで、飛翔体１０の飛行の制御を実現する。なお、モデル予測制御によって決定した制御信号に基づいて制御対象を制御する技術には、既存の技術を利用することができる。

［実施形態２］
＜概要＞
実施形態２の制御装置２０００では、飛翔体１０の制御モードとして、第３制御モードが設けられている。第３制御モードは、飛翔体１０において外乱の影響が大きい場合に利用され、外乱の抑制を優先して、飛翔体１０の制御が行われる。

＜作用効果＞
風などの外乱の影響が大きい場合、衝突や墜落などの事態を避けるため、外乱を抑制することが重要となる。本実施形態の制御装置２０００によれば、外乱の影響が大きい場合に、外乱の抑制を優先して飛翔体１０の飛行制御を行うことにより、飛翔体１０の安全な飛行を実現することができる。

なお、外乱の大きさを把握する技術には、既存の技術を利用することができる。例えば外乱の大きさは、外乱センサを用いて把握することができる。その他にも例えば、外乱の大きさは、数式（１）における D のノルムとして推定することができる。

＜機能構成の例＞
実施形態２の制御装置２０００の機能構成は、実施形態１の制御装置２０００と同様に、例えば図３で表される。ただし、実施形態２の制御部２０４０は、飛翔体１０に対する外乱の大きさが所定の大きさ以上である場合に、外乱の抑制を優先して飛翔体１０の飛行制御を行う。

＜ハードウエア構成の例＞
実施形態２の制御装置２０００を実現する計算機のハードウエア構成は、実施形態１と同様に、例えば図４によって表される。ただし、本実施形態の制御装置２０００を実現する計算機１０００のストレージデバイス１０８０には、本実施形態の制御装置２０００の機能を実現するプログラムモジュールがさらに記憶される。

＜処理の流れ＞
図６は、実施形態２の制御装置２０００によって実行される処理の流れを例示する図である。制御部２０４０は、飛翔体１０の状態を推定する（Ｓ２０２）。制御部２０４０は、飛翔体１０に対する外乱の大きさが閾値以上であるか否かを判定する（Ｓ２０４）。外乱の大きさが閾値以上である場合（Ｓ２０４：ＹＥＳ）、制御部２０４０は、外乱の抑制を優先して飛翔体１０の飛行を制御する（Ｓ２０６）。外乱の大きさが閾値未満である場合（Ｓ２０６：ＮＯ）、制御部２０４０は、制御モードが第１モードと第２モードのどちらであるかを判定する（Ｓ２０８）。ここで、Ｓ２０８以降の処理は、図５で説明した処理と同様である。なお、図６に示す一連の処理は、図５の処理と同様、例えば所定の周期（0.1sec に１回など）で実行される。

＜外乱を抑制する方法＞
外乱の抑制は、飛翔体１０の飛行を制御する評価関数において、制御入力に対する評価の重みを小さくすることで実現することができる。こうすることで、外乱に対する抗力を増すことができる。

例えば数式（３）の評価関数 J を利用する場合において、外乱の抑制を優先するとき、制御部２０４０は、重み行列 R の各要素の値を基準値より小さくする。一方、外乱の抑制を優先しないとき、制御部２０４０は、重み行列 R の各要素の値を基準値以上にする。基準値は、重み行列 R の要素ごとに異なる値であってもよいし、全ての要素で共通の値であってもよい。

より具体的な方法の例としては、外乱の抑制を優先するときに利用する重み行列 R1 と、外乱の抑制を優先しないときに利用する重み行列 R2 を予め定めておく。そして制御部２０４０は、飛翔体１０に対する外乱の大きさが閾値以上である場合、評価関数 J における重み行列 R として R1 を利用する。一方、飛翔体１０に対する外乱の大きさが閾値未満である場合、制御部２０４０は、評価関数 J における重み行列 R として R2 を利用する。なお、R1 と R2 は、制御部２０４０からアクセス可能な記憶装置に記憶させておく。

［実施形態３］
＜概要＞
実施形態３の制御装置２０００は、実施形態２の制御装置２０００と同様に、飛翔体１０において外乱の影響が大きい場合に外乱の抑制を行う。ただし、実施形態３の制御装置２０００は、外乱の影響が大きい場合、外乱の抑制を行いつつ、第１制御モード又は第２制御モードのいずれか一方で動作する。こうすることで、飛翔体１０を用いた正確な作業を実現するための制御と、エネルギー効率等を重視するための制御とを適切に切り替えつつ、外乱の抑制による飛翔体１０の安全な飛行を実現することができる。

＜機能構成の例＞
実施形態２の制御装置２０００の機能構成は、実施形態１の制御装置２０００と同様に、例えば図３で表される。ただし、実施形態３の制御部２０４０は、飛翔体１０に対する外乱の大きさが所定の大きさ以上である場合に、外乱の抑制を優先した制御行う。さらに、外乱の抑制を行うか否かにかかわらず、第１モードと第２モードのいずれか一方における制御を行う。

＜ハードウエア構成の例＞
実施形態２の制御装置２０００を実現する計算機のハードウエア構成は、実施形態１と同様に、例えば図４によって表される。ただし、本実施形態の制御装置２０００を実現する計算機１０００のストレージデバイス１０８０には、本実施形態の制御装置２０００の機能を実現するプログラムモジュールがさらに記憶される。

＜処理の流れ＞
図７は、実施形態２の制御装置２０００によって実行される処理の流れを例示する図である。制御部２０４０は、飛翔体１０の状態を推定する（Ｓ３０２）。制御部２０４０は、飛翔体１０に対する外乱の大きさが閾値以上であるか否かを判定する（Ｓ３０４）。さらに制御部２０４０は、外乱の大きさが閾値以上である場合（Ｓ３０４：ＹＥＳ）と外乱の大きさが閾値未満である場合（Ｓ３０４：ＮＯ）の双方で、飛翔体１０の制御モードが第１モードと第２モードのどちらであるかを判定する（Ｓ３０６、Ｓ３０８）。

外乱の大きさが閾値以上であり、なおかつ飛翔体１０の制御モードが第１モードである場合（Ｓ３０６：第１モード）、制御部２０４０は、外乱を抑制しつつ第１位置３０を優先した飛翔体１０の制御を行う（Ｓ３１０）。一方、外乱の大きさが閾値以上であり、なおかつ飛翔体１０の制御モードが第２モードである場合（Ｓ３０６：第２モード）、制御部２０４０は、外乱を抑制しつつ第２位置４０を優先した飛翔体１０の制御を行う（Ｓ３１２）。

外乱の大きさが閾値未満であり、なおかつ飛翔体１０の制御モードが第１モードである場合（Ｓ３０８：第１モード）、制御部２０４０は、外乱の抑制は行わずに、第１位置３０を優先した飛翔体１０の制御を行う（Ｓ３１４）。一方、外乱の大きさが閾値未満であり、なおかつ飛翔体１０の制御モードが第２モードである場合（Ｓ３０８：第２モード）、制御部２０４０は、外乱の抑制は行わずに、第２位置４０を優先した飛翔体１０の制御を行う（Ｓ３１６）。

＜外乱を抑制しつつ第１位置３０を優先して制御する方法：Ｓ３１０＞
外乱の大きさが閾値以上であり、なおかつ飛翔体１０の制御モードが第１モードである場合（Ｓ３０６：第１モード）、制御部２０４０は、外乱を抑制しつつ第１位置３０を優先した飛翔体１０の制御を行う（Ｓ３１０）。実施形態２で説明したように、外乱の抑制は、飛翔体１０の飛行を制御する評価関数において、制御入力に対する評価の重み（数式（４）における重み行列 R）を小さくすることで実現することができる。また、第１位置３０を第２位置４０よりも優先する制御は、評価関数において、第１位置３０を表す要素に対する重みを、第２位置４０を表す要素に対する重みよりも大きくすることで実現できる。例えば数式（４）において、第１位置３０を表す要素は xarm、yarm、及び zarm であり、これらに対する重みは q7、q8、及び q9 である。また、第２位置４０を表す要素は x、y、及び z であり、これらに対する重みは q1、q3、及び q5 である。

そこで、外乱を抑制しつつ第１位置３０を優先した飛翔体１０の制御を行う場合、制御部２０４０は、飛翔体１０の飛行を制御する評価関数において、１）制御入力に対する評価の重みを小さくし、なおかつ２）第１位置３０を表す要素に対する重みを、第２位置４０を表す要素に対する重みよりも大きくする。例えば数式（４）の評価関数を用いる場合、制御部２０４０は、重み行列 R として前述した重み行列 R1 を利用し、なおかつ重み行列 Q として前述した重み行列 Q1 を利用する。

＜外乱の抑制をしつつ第２位置４０を優先して制御する方法：Ｓ３１２＞
外乱の大きさが閾値以上であり、なおかつ飛翔体１０の制御モードが第２モードである場合（Ｓ３０６：第２モード）、制御部２０４０は、外乱を抑制しつつ第２位置４０を優先した飛翔体１０の制御を行う（Ｓ３１２）。第２位置４０を第１位置３０よりも優先する制御は、評価関数において、第２位置４０を表す要素に対する重みを、第１位置３０を表す要素に対する重みよりも大きくすることで実現できる。

そこで、外乱を抑制しつつ第２位置４０を優先した飛翔体１０の制御を行う場合、制御部２０４０は、飛翔体１０の飛行を制御する評価関数において、１）制御入力に対する評価の重みを小さくし、なおかつ２）第２位置４０を表す要素に対する重みを、第１位置３０を表す要素に対する重みよりも大きくする。例えば数式（４）の評価関数を用いる場合、制御部２０４０は、重み行列 R として前述した重み行列 R1 を利用し、なおかつ重み行列 Q として前述した重み行列 Q2 を利用する。

＜外乱を抑制せずに第１位置３０を優先して制御する方法：Ｓ３１４＞
外乱の大きさが閾値未満であり、なおかつ飛翔体１０の制御モードが第１モードである場合、制御部２０４０は、外乱の抑制は行わずに、第１位置３０を優先した飛翔体１０の制御を行う（Ｓ３１４）。実施形態２で説明したように、外乱の抑制を行わない場合には、外乱の抑制を行う場合と比較し、飛翔体１０の飛行を制御する評価関数において、制御入力に対する評価の重みを大きくする。

そこで、外乱を抑制せずに第１位置３０を優先した飛翔体１０の制御を行う場合、制御部２０４０は、飛翔体１０の飛行を制御する評価関数において、１）制御入力に対する評価の重みを大きくし、なおかつ２）第１位置３０を表す要素に対する重みを、第２位置４０を表す要素に対する重みよりも大きくする。例えば数式（４）の評価関数を用いる場合、制御部２０４０は、重み行列 R として前述した重み行列 R2 を利用し、なおかつ重み行列 Q として前述した重み行列 Q1 を利用する。

＜外乱を抑制せずに第２位置４０を優先して制御する方法：Ｓ３１６＞
外乱の大きさが閾値未満であり、なおかつ飛翔体１０の制御モードが第２モードである場合、制御部２０４０は、外乱の抑制は行わずに、第２位置４０を優先した飛翔体１０の制御を行う（Ｓ３１６）。具体的には、制御部２０４０は、飛翔体１０の飛行を制御する評価関数において、１）制御入力に対する評価の重みを大きくし、なおかつ２）第２位置４０を表す要素に対する重みを、第１位置３０を表す要素に対する重みよりも大きくする。より具体的には、数式（４）の評価関数を用いる場合、制御部２０４０は、重み行列 R として前述した重み行列 R2 を利用し、なおかつ重み行列 Q として前述した重み行列 Q2 を利用する。

以上、図面を参照して本発明の実施形態について述べたが、これらは本発明の例示であり、上記各実施形態の組み合わせ、又は上記以外の様々な構成を採用することもできる。

上記の実施形態の一部又は全部は、以下の付記のようにも記載されうるが、以下には限られない。
１． アームを有する飛翔体の飛行を制御する制御装置であって、
前記飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替部と、
前記制御モードが前記第１モードである場合には前記飛翔体の第１位置を優先して前記飛翔体の飛行を制御し、前記制御モードが前記第２モードである場合には前記飛翔体の第２位置を優先して前記飛翔体の飛行を制御する制御部と、を有し、
前記第１位置は前記アーム上の位置であり、
前記第２位置は前記アーム上の位置ではない、制御装置。
２． 前記制御部は、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
前記制御部は、
前記制御モードが前記第１モードである場合、前記評価関数において、前記飛翔体の第１位置が前記評価値に与える影響の大きさを表す第１の重みを、前記評価関数において前記飛翔体の第２位置が前記評価値に与える影響の大きさを表す第２の重みよりも大きくし、
前記制御モードが前記第２モードである場合、前記第２の重みを前記第１の重みよりも大きくする、１．に記載の制御装置。
３． 前記制御部は、前記飛翔体に対する外乱の大きさが閾値以上であるか否かを判定し、その大きさが閾値以上である場合、前記外乱の抑制を優先した前記飛翔体の飛行制御を行う、１．又は２．に記載の制御装置。
４． 前記制御部は、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
前記制御部は、前記外乱の大きさが閾値以上である場合、前記外乱の大きさが閾値未満である場合と比較し、前記評価関数において前記制御入力が前記評価値に与える影響の大きさを表す重みを小さくする、３．に記載の制御装置。
５． 前記第１位置は前記アームの先端位置であり、
前記第２位置は前記飛翔体の中心位置である、１．乃至４．いずれか一つに記載の制御装置。

６． アームを有する飛翔体の飛行を制御するコンピュータによって実行される制御方法であって、
前記飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替ステップと、
前記制御モードが前記第１モードである場合には前記飛翔体の第１位置を優先して前記飛翔体の飛行を制御し、前記制御モードが前記第２モードである場合には前記飛翔体の第２位置を優先して前記飛翔体の飛行を制御する制御ステップと、を有し、
前記第１位置は前記アーム上の位置であり、
前記第２位置は前記アーム上の位置ではない、制御方法。
７． 前記制御ステップにおいて、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
前記制御ステップにおいて、
前記制御モードが前記第１モードである場合、前記評価関数において、前記飛翔体の第１位置が前記評価値に与える影響の大きさを表す第１の重みを、前記評価関数において前記飛翔体の第２位置が前記評価値に与える影響の大きさを表す第２の重みよりも大きくし、
前記制御モードが前記第２モードである場合、前記第２の重みを前記第１の重みよりも大きくする、６．に記載の制御方法。
８． 前記制御ステップにおいて、前記飛翔体に対する外乱の大きさが閾値以上であるか否かを判定し、その大きさが閾値以上である場合、前記外乱の抑制を優先した前記飛翔体の飛行制御を行う、６．又は７．に記載の制御方法。
９． 前記制御ステップにおいて、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
前記制御ステップにおいて、前記外乱の大きさが閾値以上である場合、前記外乱の大きさが閾値未満である場合と比較し、前記評価関数において前記制御入力が前記評価値に与える影響の大きさを表す重みを小さくする、８．に記載の制御方法。
１０． 前記第１位置は前記アームの先端位置であり、
前記第２位置は前記飛翔体の中心位置である、６．乃至９．いずれか一つに記載の制御方法。

１１． ６．乃至１０．いずれか一つに記載の制御方法の各ステップをコンピュータに実行させるプログラム。

この出願は、２０１８年９月２６日に出願された日本出願特願２０１８－１８０５５２号を基礎とする優先権を主張し、その開示の全てをここに取り込む。

１０ 飛翔体
２０ アーム
３０ 第１位置
４０ 第２位置
１０００ 計算機
１０２０ バス
１０４０ プロセッサ
１０６０ メモリ
１０８０ ストレージデバイス
１１００ 入出力インタフェース
１１２０ ネットワークインタフェース
２０００ 制御装置
２０２０ 切替部
２０４０ 制御部

Claims (7)

1. アームを有する飛翔体の飛行を制御する制御装置であって、
   前記飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替部と、
   前記制御モードが前記第１モードである場合には前記飛翔体の第１位置を優先して前記飛翔体の飛行を制御し、前記制御モードが前記第２モードである場合には前記飛翔体の第２位置を優先して前記飛翔体の飛行を制御する制御部と、を有し、
   前記第１位置は前記アーム上の位置であり、
   前記第２位置は前記アーム上の位置ではない、制御装置。
2. 前記制御部は、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
   前記制御部は、
   前記制御モードが前記第１モードである場合、前記評価関数において、前記飛翔体の第１位置が前記評価値に与える影響の大きさを表す第１の重みを、前記評価関数において前記飛翔体の第２位置が前記評価値に与える影響の大きさを表す第２の重みよりも大きくし、
   前記制御モードが前記第２モードである場合、前記第２の重みを前記第１の重みよりも大きくする、請求項１に記載の制御装置。
3. 前記制御部は、前記飛翔体に対する外乱の大きさが閾値以上であるか否かを判定し、その大きさが閾値以上である場合、前記外乱の抑制を優先した前記飛翔体の飛行制御を行う、請求項１又は２に記載の制御装置。
4. 前記制御部は、前記飛翔体の現在の状態と目標の状態との誤差及び制御入力に基づいて評価値を算出する評価関数を小さくするように、前記制御入力を決定し、前記決定した制御入力を前記飛翔体に出力することで、前記飛翔体の飛行を制御し、
   前記制御部は、前記外乱の大きさが閾値以上である場合、前記外乱の大きさが閾値未満である場合と比較し、前記評価関数において前記制御入力が前記評価値に与える影響の大きさを表す重みを小さくする、請求項３に記載の制御装置。
5. 前記第１位置は前記アームの先端位置であり、
   前記第２位置は前記飛翔体の中心位置である、請求項１乃至４いずれか一項に記載の制御装置。
6. アームを有する飛翔体の飛行を制御するコンピュータによって実行される制御方法であって、
   前記飛翔体の飛行制御に関する制御モードを第１モードと第２モードのいずれか一方に切り替える切替ステップと、
   前記制御モードが前記第１モードである場合には前記飛翔体の第１位置を優先して前記飛翔体の飛行を制御し、前記制御モードが前記第２モードである場合には前記飛翔体の第２位置を優先して前記飛翔体の飛行を制御する制御ステップと、を有し、
   前記第１位置は前記アーム上の位置であり、
   前記第２位置は前記アーム上の位置ではない、制御方法。
7. 請求項６に記載の制御方法の各ステップをコンピュータに実行させるプログラム。

Images