JP2008129813A - 柔軟アームを有する移動ロボット - Google Patents

Abstract

【課題】柔軟アームを有する移動ロボットにおいて、柔軟アームの振動および姿勢を、走行路面の環境に左右されることなく、安定的にかつ有効に制御すること。
【解決手段】走行機体およびブーム１０４にセンサ１１０〜１１６を取り付けて最小限の状態変数を計測し、計測できない状態変数はオブザーバによって推定して、ブーム１０４の姿勢および振動を状態フィードバック制御する。また、フィードバック制御に加えて、目標値変動補償用のフィードフォワード制御および外力軽減用のフィードフォワード制御を併用する。
【選択図】図２

Description

本発明は、柔軟アームを有する移動ロボットに関し、特に、移動ロボットに搭載された柔軟アームの振動および姿勢の制御に関する。

近時、農業規模の拡大に伴い、作業の効率化のために農業機械の大型化と高性能化が進んでいる。そのような農業機械の一例として、例えば、農薬散布用アーム（ブームスプレーヤ）を有する農業機械（トラクタなど）がある。

トラクタで牽引するブームスプレーヤ（以下単に「ブーム」という）は、全長１０〜２０ｍの柔軟な片持ちはり構造であり、農地走行時には先端が数十センチメールの振幅で複雑に振動し、精密な作業の妨げとなっている。また、例えば、農地の起伏によって、または旋回作業中に、トラクタ−ブーム系が一体となってローリングし、ブームの先端が接地する場合がある。このような挙動は、作物やブームに損傷をもたらすおそれがあるのみならず、トラクタの運転においても危険であるため、回避しなければならない。

従来、このような挙動を抑制するための技術として、例えば、特許文献１〜３に記載された技術が知られている。

特許文献１には、ブームに設けた一対の光センサによって作物を検出し、その検出結果に基づいてブームを昇降することにより、ブームの作物に対する高さを調整・保持する技術が記載されている。

特許文献２には、ブームに装着した測距用センサを用いてブームと作物や地面などとの距離を検出してブームの位置を自動修正する技術が記載されている。

特許文献３には、ブームの基端と先端との間に高さを保持するための補助車輪を取り付ける技術が記載されている。
実公昭５８−４５８０２号公報 実公平１−２３５７３号公報 特開昭６３−７２３７０号公報

しかしながら、特許文献１および特許文献２に記載の技術においては、ブームの作物などに対する高さを保持するために作物や地面を直接計測するため、作物の種類や農地の状況（起伏や傾斜など）によっては計測精度が低下したり走行機体（トラクタなど）が地面に完全に直接接地できなかったりする場合も想定される。このような場合には、適切な姿勢制御が十分に行われず、作物や地面などに対するブームの高さを保持することができないおそれがある。したがって、走行路面の環境（例えば、作物の種類や農地の状況など）に左右されることなく安定した姿勢制御を行うためには、対作物・対地面のセンサを使用しないことが望ましい。また、これらの技術においては、農地の起伏や旋回作業などに伴うブームの複雑な振動の抑制については、制御上何ら考慮されていない。

また、特許文献３に記載の技術においては、ブームの高さを保持するために、センサおよび制御回路を用いることなく、調整可能な補助車輪を利用する。そのため、姿勢制御に関する上記問題は生じない。しかし、やはり、農地の起伏や旋回作業などに伴うブームの複雑な振動の抑制については何ら考慮されていない。

本発明は、かかる点に鑑みてなされたものであり、柔軟アームを有する移動ロボットにおいて、柔軟アームの振動および姿勢を、走行路面の環境に左右されることなく、安定的にかつ有効に制御することができる移動ロボットを提供することを目的とする。

本発明の移動ロボットは、柔軟アームを有する移動ロボットであって、ロボット本体の姿勢を検出するロボット姿勢検出手段と、前記柔軟アームの姿勢を検出するアーム姿勢検出手段と、前記柔軟アームの振動を検出するアーム振動検出手段と、前記柔軟アームを複数の軸方向に回転可能に駆動する駆動手段と、前記ロボット姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力を状態変数として、状態フィードバック制御演算を行い、前記駆動手段に対する制御信号を生成する制御手段と、を有し、前記制御手段は、前記状態フィードバック制御演算に必要な状態変数のうち、前記姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力以外の状態変数を推定するオブザーバを含み、前記姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力、ならびに推定された状態変数を用いて、状態フィードバック制御演算を行う、構成を有する。

本発明によれば、柔軟アームを有する移動ロボットにおいて、柔軟アームの振動および姿勢を、走行路面の環境に左右されることなく、安定的にかつ有効に制御することができる。

以下、本発明の実施の形態について、図面を参照して詳細に説明する。

本発明は、走行機体（トラクタなど）および柔軟アーム（ブームスプレーヤなど）の振動と姿勢に関する情報のみを計測し、この計測結果に基づいて柔軟アームの振動・姿勢制御を行うものである。ここで、柔軟アームとは、例えば、ブームスプレーヤのような、弾性を有するアームを広く意味する。

図１は、本発明の一実施の形態に係る移動ロボットの構成を示す概略図である。図１（Ａ）は、移動ロボットを上方から見た平面図、図１（Ｂ）は、移動ロボットを後方から見た背面図である。また、図２は、図１に示す移動ロボットの制御系の構成を示すブロック図である。

図１に示す移動ロボット１００は、図３に示す農薬散布用アーム（ブームスプレーヤ、以下単に「ブーム」という）３を有する農業機械１を、柔軟アームを搭載した小型移動ロボットとしてモデル化したものである。この移動ロボット１００は、ロボット本体の運動によって生じる柔軟アームの振動制御および柔軟アームを一定に保持する姿勢制御を行う機能を有する。前述したように、移動ロボットに取り付けられた柔軟アームは、移動ロボットの加減速および旋回により上下・前後方向に外力を受けて振動する。また、移動ロボットが起伏した路面を走行する際には、ロボット本体に加速度が生じ、柔軟アームの先端が大きな変位を伴いながら振動する。この移動ロボット１００では、これらの挙動を、制御指令に基づくアクチュエータの駆動で抑制し、柔軟アームの先端を一定の位置に保つことができる。

なお、本実施の形態では、簡単化のため、ロボット本体の左右どちらか一方の側にのみ柔軟アームを取り付けた場合について説明するが、本発明はこれに限定されるわけではない。例えば、図１に示す農業機械１のように、左右対称に２つの柔軟アームを設置してもよい。

図１に示す移動ロボット１００は、走行機体（ロボット本体）１０２にブーム（柔軟アーム）１０４が取り付けられている。ブーム１０４は、２軸のアクチュエータ１０６を介して走行機体１０２に取り付けられている。２軸のアクチュエータ１０６は、水平方向のアクチュエータ１０６ａと、鉛直方向のアクチュエータ１０６ｂとで構成されている。各アクチュエータ１０６ａ、１０６ｂは、例えば、モータで構成されている（モータアクチュエータ）。また、各アクチュエータ１０６ａ、１０６ｂには、自己の回転位置を検出するための回転角センサ１０８ａ、１０８ｂがそれぞれ取り付けられている。回転角センサ１０８ａ、１０８ｂは、例えば、エンコーダで構成されている。ここでは、走行機体１０２上に固定された水平方向アクチュエータ１０６ａに、図示しない第１ハブを介して鉛直方向アクチュエータ１０６ｂが取り付けられている。そして、この鉛直方向アクチュエータ１０６ｂには、図示しない第２ハブを介してブーム１０４が取り付けられている。これにより、ブーム１０４は、２軸アクチュエータ１０６によって水平方向および鉛直方向に回転可能となっている。

走行機体１０２には、ロール方向（前後軸まわりの運動方向）の傾斜角を検出するための傾斜角センサ（以下「機体傾斜角センサ」ともいう）１１０およびヨー方向（上下軸まわりの運動方向）の角速度を検出するための１軸のジャイロセンサ（以下「機体ジャイロセンサ」ともいう）１１２が取り付けられている。この機体傾斜角センサ１１０および機体ジャイロセンサ１１２によって、走行機体１０２の姿勢に関する情報が取得される。具体的には、機体傾斜角センサ１１０および機体ジャイロセンサ１１２によって、走行機体１０２の姿勢角度（ロール方向の傾斜角つまりロール角）および回転運動（ヨー方向の角速度つまりヨー角速度）がそれぞれ検出される。

また、走行機体１０２には、２軸アクチュエータ１０６の近傍に、上下方向および前後方向の加速度を検出するための２軸の加速度センサ（以下「機体加速度センサ」ともいう）１１４が取り付けられている。この機体加速度センサ１１４によって、ブーム１０４に作用する外力を推定することができ、後述するフィードフォワード制御が可能となる。

また、ブーム１０４には、水平方向（前後方向）および鉛直方向（上下方向）の角速度を検出するための２軸のジャイロセンサ（以下「ブームジャイロセンサ」ともいう）１１６が取り付けられている。このブームジャイロセンサ１１６によって、ブーム１０４の水平方向および鉛直方向の角速度が検出される。ブーム１０４の振動制御は、ブームジャイロセンサ１１６の検出結果（ブーム１０４の上下・前後方向の角速度）を用いて行われる。

さらに、走行機体１０２には、コンピュータ（ＣＰＵ）１１８が搭載されている。コンピュータ１１８は、機体傾斜角センサ１１０、機体ジャイロセンサ１１２、およびブームジャイロセンサ１１６からの信号を入力処理する信号処理部１２０と、機体傾斜角センサ１１０、機体ジャイロセンサ１１２、およびブームジャイロセンサ１１６の検出結果に基づいて各アクチュエータ１０６ａ、１０６ｂに対する制御指令を生成する演算部１２２とで構成されている。後述するように、直接検出できない状態量は、演算部１２２にて、状態推定プログラムによって推定される。演算部１２２は、本発明者が開発したプログラムに従って所定の演算を行い、各アクチュエータ１０６ａ、１０６ｂに対する制御指令を生成する。なお、上記プログラムは、本発明者が設計した制御系に基づいている。そして、この制御系は、本発明者が導出した力学モデルに基づいて設計されている。この力学モデルにより、柔軟アームを有する移動ロボットの動的挙動解析が可能となっている。演算部１２２を具体的に構成するための理論解析、制御系設計、およびフィードバック係数決定については、後で詳細に説明する。

また、走行機体１０２には、アクチュエータ制御部１２４が搭載されている。アクチュエータ制御部１２４は、コンピュータ１１８の出力（制御指令）に基づいて、各アクチュエータ１０６ａ、１０６ｂの駆動を制御する。そのため、アクチュエータ制御部１２４には、各アクチュエータ１０６ａ、１０６ｂの回転角センサ１０８ａ、１０８ｂがそれぞれ接続されている。すなわち、アクチュエータ制御部１２４は、コンピュータ１２２の制御指令（目標値）と、対応する各回転角センサ１０８ａ、１０８ｂの検出値（現在値）とが一致するように、各アクチュエータ１０６ａ、１０６ｂの駆動を制御する。アクチュエータ制御部１２４は、例えば、モータドライバで構成されている。

本実施の形態では、走行機体１０２の傾斜角（ロール方向）と角速度（ヨー方向）を機体傾斜角センサ１１０と機体ジャイロセンサ１１２によって、また、ブーム１０４の角速度（水平方向と鉛直方向）をブームジャイロセンサ１１６によってそれぞれ検出し、この検出結果に基づいてブーム１０４展開用の２軸アクチュエータ１０６を駆動することにより、ブーム１０４の先端と作物や地面などとの高さを保持するよう、ブーム１０４に対する姿勢制御を行う。また、ブームジャイロセンサ１１６によってブーム１０４の振動を検出し、さらには、走行機体１０２の加速度（上下方向と前後方向）を機体加速度センサ１１４によって検出してブーム１０４に作用する外力を推定し、ブーム１０４展開用の２軸アクチュエータ１０６を利用したアクティブ制御系によって、ブーム１０４の振動を抑制するよう、ブーム１０４に対する振動制御を行う。

なお、本実施の形態では、走行機体１０２の姿勢に関する情報として、傾斜角センサの出力（傾斜角）を利用するようにしているが、これに限定されない。例えば、傾斜角センサの出力に代えて、１軸のジャイロセンサの出力（角速度）を利用することも可能である。さらには、傾斜角センサおよび１軸ジャイロセンサの組み合わせに代えて、２軸のジャイロセンサを利用することも可能である。

また、ブーム１０４の振動・姿勢に関する情報として、２軸ジャイロセンサの出力（角速度）を利用するようにしているが、これに限定されない。例えば、２軸ジャイロセンサの出力に代えて、２軸の歪みセンサや加速度センサの出力（変位や加速度）を利用することも可能である。

本実施の形態では、状態フィードバック制御によってブーム１０４の姿勢・振動制御を行う。状態フィードバック制御を実現するためには、制御系の状態変数がすべて計測可能である必要がある。基本的な姿勢と振動を制御するためには、走行機体１０２のロール角およびヨー角、各アクチュエータ（モータ）１０６ａ、１０６ｂの回転角、ならびにブーム１０４の振動変位が必要であり、さらに、これらの速度を直接検出できる場合は、すべての変数を直接計測する制御系を構築することができる。さらに、外力の影響を打ち消すフィードフォワードを実現するためには、外力（加速度）を計測する必要があり、走行機体１０２の上下方向加速度およびロール角加速度を計測できるセンサが必要となる。しかし、すべてを計測するために多くのセンサを配置することは現実的ではなかったり、計測可能なセンサが存在しない場合もある。そこで、本実施の形態では、最小限の状態変数を計測し、計測できない状態変数は、オブザーバを用いて、計算によって推定するようにしている。

例えば、ロール角加速度は、傾斜角センサの出力から演算によって求めることができる。また、ロール角加速度は、走行機体１０２の車幅方向の両側、より具体的には、両側のブーム支持部付近にそれぞれ取り付けられた２つの上下方向加速度センサから両側の上下方向の加速度を計測し、両者の差をとることによって、得ることができる。一般に、微分演算は信号ノイズが生じやすいため、後者の方法が好ましい。また、上下方向加速度センサと同じ位置に前後方向の加速度を検出するセンサを設置すれば、両側の前後方向の加速度に加えて、両者の差からヨー方向角加速度を得ることができる。

次いで、上記構成を有する移動ロボット１００の制御系について、さらに詳細に説明する。

図４は、シミュレーションモデルとして図１に示す移動ロボット１００に関する各種パラメータを説明するための図である。図４（Ａ）は、図１（Ａ）に対応する平面図であり、図４（Ｂ）は、図１（Ｂ）に対応する背面図である。

走行機体１０２は、質量Ｍ、ｙ軸（前後軸）まわりの慣性モーメントＪ_ｄとし、走行機体１０２上に固定された水平方向アクチュエータ（モータ）１０６ａには、質量ｍ_ｈ１、慣性モーメントＪ_ｈ１、および半径ｒ_ｈ１の第１ハブを介して、質量ｍ_ｍの鉛直方向アクチュエータ（モータ）１０６ｂが取り付けられている。鉛直方向アクチュエータ（モータ）１０６ｂには、質量ｍ_ｈ２、慣性モーメントＪ_ｈ２、および半径ｒ_ｈ２のハブ２を介して、密度ρ、断面積Ａ、長さＬ、および縦弾性係数Ｅの柔軟アーム（ブーム１０４）が取り付けられている。走行機体１０２は、ｙ軸方向にのみ運動するものとし、その変位をｙ_ｄとする。また、走行機体１０２のｙ軸まわりの回転角（ローリングに伴うバンク角）をφとする。

また、空間に固定された座標系をＸ−Ｙ−Ｚとし、走行機体１０２の中心を原点とし走行機体１０２と共に移動・回転する座標系をｘ_０−ｙ_０−ｚ_０とし、回転するハブに沿った座標系をｘ−ｙ−ｚとする。座標系ｘ−ｙ−ｚの原点において、水平方向アクチュエータ（モータ）１０６ａはｚ軸まわりに、鉛直方向アクチュエータ（モータ）１０６ｂはｙ軸まわりに、それぞれτ_１、τ_２のトルクを柔軟アーム（ブーム１０４）に作用させる。また、各アクチュエータ（モータ）１０６ａ、１０６ｂの回転角を、それぞれθ_１、θ_２とする。また、ｘ軸に対する柔軟アーム（ブーム１０４）の先端のｙ軸方向およびｚ軸方向の変位を、それぞれｖ（ｘ,ｔ）、ｗ（ｘ,ｔ）とする。Ｌ_１は、２軸アクチュエータ１０６の中心とｙ_０軸との距離である。

なお、実際には、図３に示す農業機械１のように、Ｙ軸に関して対称に２つの柔軟アームが設置されるが、本実施の形態では、走行機体１０２の慣性が大きく、柔軟アームを駆動する際の反トルクによって走行機体１０２の挙動が変化しないものと仮定して、上記のように、片側のみに柔軟アーム（ブーム１０４）を設けた場合について考えることとした。

次いで、演算部１２２を具体的に構成するための理論解析、制御系設計、およびフィードバック係数決定の流れについて説明する。

まず、流れの概要を説明する。

本発明者は、まず、制御対象である柔軟アーム（ブーム１０４）の力学モデルを構築し、運動方程式をハミルトン（Hamilton）の原理を用いて導出した。柔軟アームに関しては、片持ちはりの固有関数を適用したモード展開を行い、直交条件式を適用することで、有限次元の線形運動方程式に近似した。固有関数は、有限要素法を用いて導出することができ、変断面の場合でも解析可能である（運動方程式の導出、振動解析）。そして、得られた運動方程式から離散時間系状態方程式を導出し（状態方程式の導出）、最適ディジタルサーボ系を構成した（制御系設計）。制御入力として、最適レギュレータ理論によるフィードバック制御入力、目標値変動を補償するフィードフォワード入力、および外力を軽減する外力フィードフォワード入力を考えた。本実施の形態で扱う柔軟アームに関して、直接測定可能な情報は、関節の回転角（つまり、各アクチュエータの回転角センサの出力）およびジャイロセンサにより測定される柔軟アーム上のセンサ位置の角速度であり、その他の状態変数は直接測定することができない。そこで、本発明者は、最小次元オブザーバを用いて、直接測定できない状態変数を推定し、状態フィードバックによる制御を実現した。一方、走行機体１０２に取り付けた２軸加速度センサ１１４によって、走行機体１０２に作用する加速度を測定し、これを柔軟アームに働く既知外力として考えれば、それを打ち消すようなフィードフォワード入力を求めることができる。さらに、目標値と外乱の変動に対するフィードフォワード補償を考えた。そして、得られた制御系を用いて数値シミュレーションを行い、フィードバック係数を決定した。なお、上記のように、走行機体１０２に取り付けた傾斜角センサ１１０および１軸ジャイロセンサ１１２から得られる情報から、走行機体１０２に作用する加速度を算出することも可能である。

図５は、本実施の形態における制御系の基本的な構成を示すブロック線図である。

図５のブロック線図において、アーム２００は、アクチュエータ制御部１２４、アクチュエータ１０６、ブーム１０４、およびブームジャイロセンサ１１６を含む構成を有する。目標値は、アーム２００（特にブーム１０４、以下同様）の振動変位＝ゼロに加えて、アーム２００の目標姿勢角を含む。アーム２００の目標姿勢角を決める際には、アクチュエータ１０６の回転角センサ１０８ａ、１０８ｂからの出力が、走行機体１０２の傾斜角とアーム２００の傾斜角との差となるため、走行機体１０２の初期状態（水平状態）からのロール角およびヨー角の変化量を用いて、走行機体１０２が傾斜してもアーム２００が水平となるようにアーム２００の目標姿勢角を決定するようにしている。

次に、流れの各要素を詳細に説明する。

運動方程式の導出
ここでは、ハミルトンの原理を適用して変分計算を行い、非線形項を無視することにより、次の式（１）〜式（５）に示す運動方程式と境界条件式を得る。ここで、式（１）と式（２）は、関節まわりのモーメントのつりあい方程式であり、式（３）と式（４）は、たわみ振動の方程式である。また、式（５）は、境界条件式である。

ただし、変数に対するドットの記号は、時間ｔに関する微分を表し、ダッシュの記号は、ｘに関する微分を表す。また、関節まわりのつりあい式には、関節角の速度比例型の減衰を考慮し、たわみ振動の式にはたわみのひずみ速度比例型の減衰を考慮する。Ｃ_θｉ（ｉ＝１,２）、Ｃは、それぞれ関節とアームの減衰係数である。

実際のブームスプレーヤは、ノズルや関節を有する複雑な形状を有するが、本実施の形態では、これを変断面はりでモデル化し、その支配方程式を有限要素法によって導くこととし、たわみ振動の振動モード関数を、有限要素法を用いて導出する。

図６は、はりおよびはり要素を示す図である。

ここでは、長さＬのはりを一要素長ｄＬの要素にｎ等分し、要素における座標系をｕ−ｗとする。厚さｈは、長さＬ、幅ｂよりも十分小さいものとする。有限要素法の詳細は省略するが、全体系の剛性、質量マトリックスをそれぞれ［Ｋ］、［Ｍ］とすると、ｚ方向の振動に関する運動方程式は、次の式（６）となる。

ただし、｛Ｗ｝は、全体系の節点変位ベクトルであり、次の式（７）で表される。

さらに、定常振動を仮定して、｛Ｗ｝を次の式（８）で表す。

ただし、ω_ｉは、第ｉ次の円固有振動数であり、｛Ａ｝は、振幅ベクトルである。

式（８）を式（６）に代入して、次の式（９）を得る。

この式（９）が恒等的に成り立つ条件から、次の式（１０）で表されるはり要素の振動数方程式が求まる。

この式（１０）の固有値計算を行うことで、たわみ振動の固有振動数と振動モードを求めることができる。

たわみ関数を第ｉ次モードの振動モード関数F_iと時間関数q_ziを用いて、次の式（１１）で表す。

この式（１１）を式（２）と式（４）に代入し、境界条件式（式（５）参照）および直交条件式を考慮すると、次の式（１２）で表す状態変数、

を用いて、次の式（１３）で表される運動方程式を得る。

ただし、上記直交条件式は、次の式（１４）で与えられる。この直交条件式は、式（１１）で用いられている振動モード関数に関して成立する関係式である。

なお、行列の詳細は省略するが、［Ｍ］は質量行列、［Ｃ］は減衰行列、［Ｋ］は剛性行列、｛ｆ｝はロボット本体（走行機体１０２）からの外力、［Ｄ］は入力行列である。また、ｙ方向の変位成分についても同様の手順で解析することができる。

状態方程式の導出
本実施の形態では、ｖおよびｗの両方向について、最も支配的である１次モードのみを考慮するものとする。これにより、状態変数は、次の式（１５）で表すことができる。

ここで、ｑ_ｙ１＝ｑ_ｙ、ｑ_ｚ１＝ｑ_ｚとおいた。

また、状態変数を新たに次の式（１６）で定義する。

この状態変数を用いて、式（１３）の運動方程式を状態方程式表現に書き直すと、次の式（１７）を得る。

ただし、［Ａ_ｐ］、［Ｂ_ｐ］、［Ｆ_ｐ］は、次の式（１８）で表される。

さらに、出力ベクトルを次の式（１９）で表す。

ただし、α_ｙ、α_ｚは、アーム２００に含まれるジャイロセンサ１１６の出力信号であり、［Ｃ_ｄ］は、出力係数行列である。

なお、出力係数行列を変更することによって、変位センサや速度センサに対応することができる。

また、式（１７）および式（１９）を離散時間表現にそれぞれ変換すると、次の式（２０）、式（２１）を得る。

制御系の設計
ここでは、式（２０）のシステムを安定にするような制御入力｛ｕ（ｋ）｝を求めるため、上記で得られた状態方程式をもとに、ディジタルサーボ系を設計する。

まず、目標値信号と出力信号との差をとった誤差信号を、次の式（２２）で定義する。

さらに、各変数の１階差分値を、次の式（２３）で表すと、

式（２０）より、次の式（２４）となる。

したがって、誤差信号の１階差分値は、次の式（２５）で表すことができる。

ここで、誤差信号と状態変数の１階差分値を新たな状態変数として、次の式（２６）の拡大系状態方程式を得る。

そして、状態変数とそれぞれの係数行列を新しく置き換えれば、式（２６）は、次の式（２７）となる。

この系を制御入力｛Δｕ（ｋ）｝により安定化することによって、ｋ→∞で｛Ｘ（ｋ）｝⇒０、つまり、｛ｅ（ｋ）｝⇒０にすることができる。このような制御入力を求めるために、最適レギュレータ理論を用いる。

すなわち、ΔＲ（ｋ＋１）＝０、Δｆ（ｋ）＝０とし、まず、次の式（２８）の評価関数を定義する。

ただし、［Ｑ］は重み行列であり、［Ｒ］は重み係数である。

この評価関数を最小にするような制御入力は、次の式（２９）を定義すると、

次の式（３０）となる。

ただし、［Ｐ］は、次の式（３１）で表される離散時間定常リッカティ（Riccati）方程式の正定対称解である。

ここで、制御入力｛Δｕ（ｋ）｝を次の式（３２）で表す。

この式を｛ｕ（ｋ）｝について解いて、次の式（３３）を得る。

以上の式から、最適ディジタルサーボ系の状態変数線図を、図７のように表すことができる。図７は、以上の式から得られるディジタルサーボ系の構成を示すブロック線図であって、図５に対応するものである。図７のブロック線図において、制御対象３００は図５のアーム２００であり、Ｒ（ｋ）は図５の目標値であり、ｙ（ｋ）は図５のセンサ出力である。

上記では、エラーシステムの評価関数を評価する際、目標値信号の差分値ΔＲ（ｋ＋１）＝０としてフィードバック入力を算出した。しかし、実際には、走行機体１０２が傾いた場合には、アーム２００（ブーム１０４）が地面に対して水平を保つように目標値を設定する必要がある。このとき、目標値信号の差分値ΔＲ（ｋ＋１）は０以外の値を持つことになり、その影響により過渡状態でエラーシステムの状態が望ましくない方向に動かされることになる。そこで、ΔＲ（ｋ＋１）の情報を利用して、これらの悪影響を軽減するフィードフォワード入力を制御入力に加えることにする。さらに、外力を何らかのセンサ情報から取得できるとすれば、外力項を打ち消すような入力を加えることで、制御性能の向上が期待される。かかる観点から、本実施の形態では、走行機体１０２に取り付けた２軸加速度センサ１１４によって測定された上下・前後方向加速度をもとにしたフィードフォワード補償を考える。なお、上記のように、走行機体１０２に取り付けた２つのセンサ（傾斜角センサ１１０、１軸ジャイロセンサ１１２）から得られた走行機体１０２の傾斜角および角速度をもとにしたフィードフォワード補償も可能である。

図８は、図７の制御系に目標値および外力に関するフィードフォワード入力をそれぞれ加えた制御系の構成を示すブロック線図である。以下、便宜上、図８に示す制御系を「併合制御系」と呼ぶことにする。上記のように、走行機体１０２において２軸アクチュエータ１０６の近傍に２軸加速度センサ１１４を取り付けることによって、ブーム１０４に作用する外力を推定することができ、フィードフォワード制御が可能となる

また、状態フィードバックによる制御では、状態フィードバックに必要なすべての状態変数を直接測定できるという前提がある。しかし、実際にはすべての状態変数を直接測定できることは少なく、状態フィードバックを実現するためには、制御対象の直接測定可能な出力から状態変数を推定することが必要となる。

本実施の形態で扱うシステムでは、直接測定可能な出力は、第１関節の回転角（水平方向アクチュエータ１０６ａの回転角センサ１０８ａの出力）、第２関節の回転角（鉛直方向のアクチュエータ１０６ｂの回転角センサ１０８ｂの出力）、および２軸ジャイロセンサ１１６によって測定される柔軟アーム（ブーム１０４）上のセンサ位置の角速度であり、その他の状態変数は直接測定することができない。そこで、本実施の形態では、最小次元オブザーバを用いて、直接測定できない状態変数を推定し、状態フィードバックによる制御を実現するようにしている。

図９は、オブザーバを導入した制御系の構成を示すブロック線図である。図９に示す制御系は、図８に示す併合制御系にオブザーバ４００を加えた構成をしている。

最小次元オブザーバは、例えば、次の手順で導出される。

式（２０）の外力項は、フィードフォワード制御によってほぼ相殺されるため、ここでは、次の式（３４）で表す状態方程式を考える。

測定できる状態変数は、式（１９）の｛ｙ｝であり、測定できない状態変数を次の式（３５）で表し、ｙを状態変数に組み込むため、次の式（３６）〜式（３９）に示す変数変換を行う。

ここで、Ｔは、正方行列ではないため、式（３６）の両辺に左側からＴ^Ｔを掛けて次の式（４０）とし、さらに（Ｔ^ＴＴ）^−１を左から掛けると、次の式（４１）を得る。

これを式（３４）の状態方程式に代入すると、次の式（４２）となる。

ここで、測定可能な状態変数がｙ、測定できない状態変数がｗであるため、次の式（４３）を定義すると、状態方程式を次の式（４４）および式（４５）に示すように分割して表すことができる。

測定できない状態変数の推定値をハットの記号を付けて表すと、式（４４）は、次の式（４６）となる。

この式（４６）に、出力Ａ_１２ｗ（ｋ）とその推定値Ａ_１２ｗ（ｋ）ハットとの差にゲインＫを掛けたものを修正項として加えると、次の式（４７）が得られる。

次の式（４８）で示す誤差ベクトルに関する部分を計算してみると、次の式（４９）となり、Ｋを選んで（Ａ_２２−ＫＡ_１２）の固有値を任意に配置することができる。

（Ａ_２２−ＫＡ_１２）を安定に選んだとき、ｗ（ｋ）ハットはｗ（ｋ）の推定値となり、これが最小次元オブザーバである。しかし、式（４７）の右辺にはｙ（ｋ＋１）を含んでいるため、このままでは使いにくい。ｙ（ｋ＋１）の計算を不要とするためには、ｗ（ｋ）を直接推定するのではなく、次の式（５０）で表すベクトルを推定すればよい。

式（５０）を用いると、式（４７）は、次の式（５１）となる。

したがって、ｚ（ｋ）の推定が得られると、式（５０）から、推定値は、次の式（５２）で与えられる。

シミュレーション
本発明者は、設計した制御系をシミュレーションするため、実験装置を製作した。

図１０は、実験装置の主要機器の構成を示す概略図である。

図１０に示す実験装置５００は、柔軟アームを有する移動ロボットとして、走行機体１０２に対応する台車５０２と、ブーム１０４に対応する柔軟アーム５０４とを有する。柔軟アーム５０４は、２つの回転角センサ付きモータ５０６ａ、５０６ｂによって水平方向および鉛直方向にそれぞれ回転可能に構成されている。台車５０２には、ロール方向の傾斜角を検出する傾斜角センサ５０８およびヨー方向の加速度を検出する加速度センサ５１０が取り付けられている。また、柔軟アーム５０４には、水平方向および鉛直方向の角速度を検出する２軸のジャイロセンサ５１２が取り付けられている。傾斜角センサ５０８、加速度センサ５１０、およびジャイロセンサ５１２の出力信号は、ＡＤ変換用のＰＣカード５１４を介してコンピュータ（ここでは、ノートパソコン）５１６に入力される。各モータ５０６ａ、５０６ｂは、ハブ基盤５１８に接続されている。ハブ基盤５１８は、ＵＳＢケーブル５２０を介してコンピュータ５１６に接続される。

ここで、柔軟アーム５０４は、長さ１ｍ、高さ４ｍｍ、厚さ２ｍｍのアルミ製はりとした。実験条件は、台車５０２をｙ軸方向にｔ＝１［ｓ］から３秒間加速し、２秒間等速運動させた後、２秒間で減速して停止させるものとした。また、図１１（Ａ）に示すように、右側の車輪がｔ＝３［ｓ］から１秒間で高さ６ｃｍの突起に乗り上げ、２秒間突起上を走行した後、１秒間で元の高さに戻るものとする。このとき、ｙ軸まわりの回転角φは、図１１（Ｂ）のように変化する。数値シミュレーションでは、これらの外力によって生じる柔軟アーム５０４の振動が制御入力によって抑制されるか否かを確認した。ただし、状態変数の初期値はすべて０とした。

ここで、フィードバック係数決定の一例について説明する。

最適１型ディジタルサーボ系では、式（２８）の評価関数の重み行列において、どのパラメータに重きをおくかによって制御性能が変化する。これらの重みは、システムごとに試行錯誤を繰り返し、システムが安定して動作し、さらに十分な振動抑制性能と目標値追従性能を有する値を求めることによって決定する。本実験では、重み行列を次の式（５３）で示すように設定した。

このとき、式（３２）のフィードバック係数Ｆｓは、リッカティ方程式を解くことにより、次の式（５４）と求められる。

台車５０２の加速度および回転角加速度をもとにしたフィードフォワード補償を考えるとき、フィードフォワード入力係数を次の式（５５）とする。

Ｋ_ｆｆの値を大きくすれば、フィードフォワードによる制御性能は向上するものの、ノイズの影響を受けやすくなるため制御は不安定になりやすい。逆に、Ｋ_ｆｆの値を小さくすれば、ノイズの影響は緩和することができるものの、フィードフォワードによる制御の効果は小さくなる（つまり、制御性能は劣化する）。制御系を設計する場合は、センサノイズの大きさや要求する制御性能に応じて適切にＫ_ｆｆの値を設定する必要がある。ここでは、Ｋ_ｆｆ１＝Ｋ_ｆｆ２＝０.７とした。

次に、シミュレーションの内容と結果について説明する。

まず、最適１型ディジタルサーボ系の応答を調べるため、サーボ系の目標値を変動させない場合についてシミュレーションを行った。図１２は、そのシミュレーション結果を示す図であり、空間固定座標から見た柔軟アーム先端座標（フィードフォワードなし）のシミュレーション結果を示している。図１２（Ａ）は、柔軟アーム先端の水平方向（Ｙ方向）変位であり、図１２（Ｂ）は、柔軟アーム先端の鉛直方向（Ｚ方向）変位である。

図１２（Ａ）から、柔軟アーム５０４の先端は、非制御の場合には、加速時に後方に大きく変位しているのに対し、サーボ系によるフィードバック制御を行った場合には、より小さな変位に抑えられていることがわかる。

しかし、図１２（Ｂ）における非制御とレギュレータの場合には、台車５０２の回転角により先端のＺ座標が大きく変位しており、柔軟アーム５０４は、水平を保つことができていない。これに対し、サーボ系を用いた場合には、図１２（Ｂ）に示すように、目標位置からの偏差を抑制できていることがわかる。

図１３は、フィードフォワード補償を加えた併合制御系の応答（シミュレーション結果）を示す図である。ここでは、比較のため、非制御の場合の応答および目標値変動を考慮した最適１型ディジタルサーボ系の応答を併せて示している。

図１３（Ａ）から、サーボ系は、非制御の場合に比べて振動低減効果を示しているが、フィードフォワード補償を加えた併合制御では、サーボ系よりもさらに制御性能が向上し、振幅を効果的に抑制していることがわかる。また、図１３（Ｂ）からは、目標値変動に対してもフィードフォワード補償は有用であることがわかる。したがって、併合制御系は、サーボ系に比べて、より良い制御性能を有することがわかる。

このように、上記の数値シミュレーションの結果、目標値変動がない場合は、サーボ系を用いると目標位置からの偏差を抑制できることがわかった。また、目標値変動がある場合は、フィードバック制御に加えてフィードフォワード制御を併用した併合制御が最も有効であることが確認できた。

このように、本実施の形態によれば、走行機体１０２およびブーム１０４（柔軟アーム）にセンサを取り付けて最小限の状態変数を計測し、計測できない状態変数はオブザーバによって推定して、ブーム１０４の姿勢および振動を状態フィードバック制御するため、柔軟アームを有する移動ロボットにおいて、柔軟アームの振動および姿勢を、走行路面の環境に左右されることなく、安定的にかつ有効に制御することができる。

また、本実施の形態によれば、フィードバック制御に加えて、目標値変動補償用のフィードフォワード制御および外力軽減用のフィードフォワード制御を併用するため、柔軟アームの振動および姿勢に対する制御性能をより一層向上することができる。

これにより、例えば、本発明をブームスプレーヤを有する農業機械に適用することによって、ブームの振動を抑制し、走行機体の傾斜によるブームの回転を補正して、ブーム先端が接地しないように制御することが可能となる。

本発明に係る移動ロボットは、柔軟アームを有する移動ロボットにおいて、柔軟アームの振動および姿勢を、走行路面の環境に左右されることなく、安定的にかつ有効に制御することができる移動ロボットとして有用である。

本発明の一実施の形態に係る移動ロボットの構成を示す概略図であり、（Ａ）は、移動ロボットを上方から見た平面図、（Ｂ）は、移動ロボットを後方から見た背面図 図１に示す移動ロボットの制御系の構成を示すブロック図 ブームスプレーヤを有する農業機械の一例を示す概略図 シミュレーションモデルとして図１に示す移動ロボットに関する各種パラメータを説明するための図であり、（Ａ）は、移動ロボットを上方から見た平面図、（Ｂ）は、移動ロボットを後方から見た背面図 本実施の形態における制御系の基本的な構成を示すブロック線図 はりおよびはり要素を示す図 最適ディジタルサーボ系の状態変数を示すブロック線図 併合制御系の構成を示すブロック線図 オブザーバを加えた併合制御系の構成を示すブロック線図 シミュレーションに使用した実験装置の主要機器の構成を示す概略図 実験条件を示す図であり、（Ａ）は、右側の車輪のｚ座標の変位を示す図、（Ｂ）は、ｙ軸まわりの回転角の変位を示す図 フィードフォワード補償を加えない１型最適ディジタルサーボ系の応答（シミュレーション結果）を示す図であり、（Ａ）は、柔軟アーム先端のＹ座標の変位を示す図、（Ｂ）は、柔軟アーム先端のＺ座標の変位を示す図 フィードフォワード補償を加えた併合制御系の応答（シミュレーション結果）を示す図であり、（Ａ）は、柔軟アーム先端のＹ座標の変位を示す図、（Ｂ）は、柔軟アーム先端のＺ座標の変位を示す図

符号の説明

１００ 移動ロボット
１０２ 走行機体
１０４ ブーム
１０６ ２軸アクチュエータ
１０６ａ 水平方向アクチュエータ
１０６ｂ 鉛直方向アクチュエータ
１０８ａ、１０８ｂ 回転角センサ
１１０ 機体傾斜角センサ
１１２ 機体ジャイロセンサ
１１４ 機体加速度センサ
１１６ ブームジャイロセンサ
１１８ コンピュータ
１２０ 信号処理部
１２２ 演算部
１２４ アクチュエータ制御部

Claims (7)

1. 柔軟アームを有する移動ロボットであって、
   ロボット本体の姿勢を検出するロボット姿勢検出手段と、
   前記柔軟アームの姿勢を検出するアーム姿勢検出手段と、
   前記柔軟アームの振動を検出するアーム振動検出手段と、
   前記柔軟アームを複数の軸方向に回転可能に駆動する駆動手段と、
   前記ロボット姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力を状態変数として、状態フィードバック制御演算を行い、前記駆動手段に対する制御信号を生成する制御手段と、を有し、
   前記制御手段は、
   前記状態フィードバック制御演算に必要な状態変数のうち、前記姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力以外の状態変数を推定するオブザーバを含み、
   前記姿勢検出手段、前記アーム姿勢検出手段、および前記振動検出手段の出力、ならびに推定された状態変数を用いて、状態フィードバック制御演算を行う、
   移動ロボット。
2. 前記ロボット姿勢検出手段は、傾斜角センサ、角速度センサ、またはこれらの組み合わせである、請求項１記載の移動ロボット。
3. 前記アーム姿勢検出手段は、前記駆動手段に軸ごとに設けられた回転角センサである、請求項１記載の移動ロボット。
4. 前記アーム振動検出手段は、角速度センサ、変位センサ、加速度センサ、またはこれらの組み合わせである、請求項１記載の移動ロボット。
5. 前記制御手段は、
   目標値の変動を補償するためのフィードフォワード入力を生成する手段を含み、
   前記状態フィードバック制御演算の結果および生成されたフィードフォワード入力を用いて、前記駆動手段に対する制御信号を生成する、
   請求項１記載の移動ロボット。
6. 前記ロボット本体に作用する加速度を検出する加速度検出手段、をさらに有し、
   前記制御手段は、
   目標値の変動を補償するためのフィードフォワード入力を生成する手段と、
   前記加速度検出手段の出力に基づいて、前記柔軟アームに作用する外力を推定する手段と、
   推定された外力を軽減するためのフィードフォワード入力を生成する手段と、を含み、
   前記状態フィードバック制御演算の結果、生成された目標値変動補償用のフィードフォワード入力、および生成された外力軽減用のフィードフォワード入力を用いて、前記駆動手段に対する制御信号を生成する、
   請求項１記載の移動ロボット。
7. 前記加速度検出手段は、前記柔軟アームの取付部近傍に設けられた加速度センサである、請求項６記載の移動ロボット。
   

Images