移動体及び移動体の走行方法

【課題】制御性が良い走行軌道をオンボードで生成することによって、移動体の制御がスムーズになり高精度の走行を可能とすると共に、あらかじめ地図や軌道作成などの付帯作業の必要がなく、安価で応用性が高い移動体を提供する。
【解決手段】移動した移動軌跡を記憶する記憶手段と、前記記憶手段に記憶された移動軌跡を編集して自走行軌道を生成する走行軌道生成手段と、前記走行軌道生成手段により生成された自走行軌道に基づいて走行する走行手段と、前記走行手段により実際に走行した軌道である自走行軌跡と、前記走行軌道生成手段により生成された自走行軌道とに基づいて、前記自走行軌跡と前記自走行軌道との誤差が少なくなるように、前記走行手段に補正値を与える走行誤差補正手段とを備える。

【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、記憶した移動軌跡に基づいて自走行を行う移動体に関するものであり、特に農作業ロボットとして畝に沿って走行を行う移動体及びその走行方法に関するものである。
【背景技術】
【０００２】
近年、若者の３Ｋ離れや少子高齢化のため労働力不足が深刻化しつつある。とりわけ、１次産業などでは従事者の高齢化が進み、継続が困難になってきている。また日本農業の国際競争力は弱く、食料自給率は先進国のなかで最低の水準
にあり、機械化の努力がなされてきたものの事態は改善されていない。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
こうした状況のなかで、無人自律ロボットの研究開発が盛んになってきたが、いずれも高精度を得るために、制御性の良い走行軌道をあらかじめオフボードで作成する必要があり、付帯作業の煩わしさとコストアップが実用化の阻害要因であった。
【０００４】
本発明は、上述した課題を解決するためになされたものであり、制御性が良い走行軌道をオンボードで生成することによって、移動体の制御がスムーズになり高精度の走行を可能とすると共に、あらかじめ地図や軌道作成などの付帯作業の必要がなく、安価で応用性が高い移動体を提供することを目的としている。
【課題を解決するための手段】
【０００５】
上述した課題を解決するため、本発明に係る移動体は、移動した移動軌跡を記憶する記憶手段と、前記記憶手段に記憶された移動軌跡を編集して自走行軌道を生成する走行軌道生成手段と、前記走行軌道生成手段により生成された自走行軌道に基づいて走行する走行手段と、前記走行手段により実際に走行した軌道である自走行軌跡と、前記走行手段により実際に走行した軌道である自走行軌跡と、前記走行軌道生成手段により生成された自走行軌道とに基づいて、前記自走行軌跡と前記自走行軌道との誤差が少なくなるように、前記走行手段に補正値を与える走行誤差補正手段とを備えてなるものである。
【０００６】
また、本発明の移動体において、前記記憶手段は前記移動軌跡を座標点列として記憶し、前記走行軌道生成手段は、前記記憶手段に記憶された座標点列から、曲線近似によってスムージングした（Ｎ次）近似曲線を求め、該曲線を直線近似して走行軌道を生成する。
【０００７】
また、本発明の移動体において、前記走行誤差補正手段は、前記移動体の走行軌道からのズレ量の一定割合を補正値として補正する。
【０００８】
更に、本発明の移動体において、前記移動体は畝に沿って走行する農業ロボットであり、速度、操舵角、位置、畝を検出するセンサを備え、前記走行手段は、前記自走行軌道とこれらセンサによる検出値を用いて走行することを特徴とする。
【０００９】
また、本発明の移動体において、前記畝を検出するセンサは、畝の長さ方向の端部を検出する畝端センサと、畝の長さ方向に延びる斜面を検出する畝センサとを備えることを特徴とする。
【００１０】
また、本発明は、移動体の走行方法であって、移動した移動軌跡を記憶し、前記記憶された移動軌跡を編集して自走行軌道を生成し、前記生成された自走行軌道に基づいて走行するとともに、実際に走行した軌道である自走行軌跡と前記自走行軌道との誤差を補正する。
【発明の効果】
【００１１】
本発明によれば、移動体の制御がスムーズになり高精度の走行を可能とすると共に、あらかじめ地図や軌道作成などの付帯作業の必要がなく、安価で応用性が高い移動体を提供することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１２】
以下、本発明の実施の形態として、本発明の移動体を農業ロボットに適用した場合について図を用いて説明する。
【００１３】
図１は本発明の実施の形態における全体構成を示すブロック図である。この移動体１００は、各種センサ１〜６と、これらセンサ１〜６からの情報と予め取得したＧＰＳデータとに基づいて移動体を自走行させる制御部（ＥＣＵボックス）７と、制御部７からの指示に基づいて移動体を操舵する操舵部１１と、移動体を走行駆動させる駆動部１２と、操作／管理を行う操作部８と、農作業装置であるカッターモータ制御部９を備えている。
【００１４】
各種センサ１〜６としては、速度を検出する速度センサ１と、車輪の操舵角を検出する操舵角センサ２と、変動方位を検出するジャイロ３と、傾きを検出する傾斜センサ４と、畝の長さ方向に沿う斜面を検出することで畝の有無を検出する畝センサ５と、畝の長さ方向端部を検出する畝端センサ６を備える。
【００１５】
なお、速度センサ１と操舵角センサ２は、モータの単位時間当たりの回転速度を検出するロータリーエンコーダにより構成され、畝センサ５と畝端センサ６は、超音波の反射により畝の夫々の箇所を検出する超音波センサにより構成されている。
【００１６】
制御部７は、アンテナ１０を介してＧＰＳ位置信号を受信するＧＰＳ受信装置７６と、カッターモータ制御部９に駆動信号を与えるカッターモータドライバ７５と、メイン制御部７２(メイン処理部)と、Ｉ／Ｏ制御ボード７１と、駆動制御部（駆動モータ制御マイコン）７４と、操舵制御部（操舵モータ制御マイコン）７３とを備え、メイン処理部７２には各種データを記憶する記憶部７２ａを備えている。
【００１７】
駆動部１２は、左右前後の駆動輪それぞれを駆動する左前駆動モータドライバ１２ａ、左後駆動モータドライバ１２ｂ、右前駆動モータドライバ１２ｃおよび右後駆動モータドライバ１２ｂを備えている。
【００１８】
操舵部１１は、左前輪を操舵する左前操舵モータドライバ１１ａ、左後輪を操舵する左後操舵モータドライバ１１ｂ、右前輪を操舵する右前操舵モータドライバ１１ｃ、右後輪を操舵する右後操舵モータドライバ１１ｄを備えている。
【００１９】
図２、図３のそれぞれは本実施の形態の移動体(農業ロボット)１００を側面及び前方からみた図である。この移動体は四輪駆動による移動体であり、前後左右輪２１，２２を備え、電源ボックス１０１、農業機械取り付け部１３０が設けられ、前後左右夫々の駆動輪の車軸近傍における内側に畝センサ５が設けられ、外側に畝端センサ６が設けられている。この移動体１００は左右輪の間に作業対象となる畝２５を挟み適宜その畝を検出しつつ移動する。なお、移動体の前方正面には移動体の農業機械取り付け部１３０の照準を所定方向に定めるための照準１５１を設けた照準板１５０が設けられている。
【００２０】
以下、本実施の形態の動作について説明する。
まず、自走行を行う前提として、移動軌跡を取得する一例（移動軌跡取得処理）について説明する。本実施の形態の移動軌跡取得処理は、移動体（農業用ロボット）１００を人が操作し手動走行を行って、その走行軌跡を取得することで行われる。また、本移動体は畝に沿った農作業を行うものであり、その移動軌跡の一例を図１８に示す。
【００２１】
農業では、畝立、播種（又は移植）・施肥・除草・収穫など一連の作業が最初の工程で作られる畝および畝間が以後の基準になり、農業ロボットなどの移動体はこれを基準に走行や作業を行うことになる。したがって、畝立て工程は従事者の意図するように移動体を操作・走行させ、その軌跡をＧＰＳ（位置認識手段）によって検出し、記憶部７２ａに記憶させる。
【００２２】
記憶部７２ａに記憶される軌跡は点列となるが、位置認識手段は誤差をもっているのでこれらの点列を直線で結ぶと折れ線になる。位置認識手段は時として異常に大きな誤差を生む場合があるので、この場合単に結んでしまうと折れ線が大きく振れることになる。
【００２３】
一般にティーチングなどの手法ではこの折れ線を軌道とするので、走行の誤差が大きくなったり、軌道の方向が大きく変化する傾向になるので、反転時に移動体が軌道より膨らんでしまい、精度が悪くなる。そこで、各点をＮ次近似曲線（図参照）でスムージングすることによって、各点の異常ばらつきによる誤差を最小化し、且つ軌道が滑らかになり、膨らみを抑制できるので高精度な走行が可能となる。
【００２４】
曲線をそのまま軌道とすると制御速度が遅くなり、制御性が追いつかなくなるため、この曲線を更に直線近似（折れ線近似）することによって、制御時のリアルタイム解析の負担を低減できる。ＧＰＳなどによる位置認識手段は、その特性上時に異常な値を示すことがあり、この値と移動体の現位置との差を誤差補正値とすると、補正が大きくなり過ぎて適正な制御ができない。そこで、補正量に重みをつけて誤差補正することによって、異常な値による悪影響を最小限に抑える事ができる。以下、詳述する。
【００２５】
移動（走行）軌跡取得処理について図４のフローチャートを用いて説明する。走行軌道取得処理が開始されると、まず、初期化処理が行われて（ステップＳ１）計測ボタンが押下されて計測（手動走行）が開始される（ステップＳ２）。ＧＰＳおよび速度センサ、傾斜センサにより位置、速度、傾きを検出して走行情報を取得する（ステップＳ３）。ＧＰＳと傾きから３次元における現在地を計算し時間情報と共に記録する（ステップＳ４）。なお、このとき付帯情報としてロボットの作業内容（畝立、播種（又は移植）・施肥・除草・収穫など）を現在地情報に対応させて記憶するようにしてもよい。
【００２６】
次に一つの畝にわたる走行を終了した場合（ステップＳ６、Ｙｅｓ）、移動体を停止させて計測終了ボタンを押下し（ステップＳ７）、全畝を終了していない場合（ステップＳ８，Ｎｏ）は、マニュアル操作にて次の畝に移動し（ステップＳ９）、ステップＳ２に戻る。なお、ステップＳ８において、一つの畝の走行が終了していない場合（ステップＳ６、Ｎｏ）はステップＳ３に戻る。全畝が終了した場合（ステップＳ８，Ｙｅｓ）は移動（走行）軌跡取得処理を終了する。
【００２７】
次に移動軌跡から自走行を行う自走行軌道を生成する処理として走行軌道編集処理について図５のフローチャートを用いて説明する。
【００２８】
まず、近似式の次数（ｎ）を指定する初期情報の入力を行う（ステップＳ３１）。次に移動軌跡を記憶装置から読み取り（ステップＳ３２）、軌跡情報を点列データとしてｎ次近似曲線を求める（ステップＳ３３）。ｎ次曲線の求め方として、重みつきパラメータを採用する。
【００２９】
直線近似として、図１９（ａ）に重みつきパラメータが０．５の場合を例示する。この場合は点列を構成する二つの連続する点の中点を順次求め、これらを順に接続して求める。
【００３０】
２次曲線近似として、図１９（ｂ）に示すように、軌跡データの点列をある区間毎に区切って最小二乗法により各区間における曲線を決定してこれらをつなぎ曲線を求める。
【００３１】
３次曲線（スプライン）近似として、図１９（ｃ）に示すように、軌跡データの点列を全区間でスプライン近似を行う。近時はＮＵＲＢＳ曲線で行う。これにより、滑らかな線を構築することができる。
【００３２】
上述のようにｎ次近似曲線が求まると、次に、図１９（ｄ）に示すように、これを新たに等間隔の折れ線（走行軌道）に変換し（ステップＳ３４）、求まった自走行軌道を記憶装置に保存し（ステップＳ３５）、畝が終了するまで、上記動作を繰り返す（ステップＳ３６）。
【００３３】
次に、自走行処理について図６〜図８のフローチャートを用いて説明する。
【００３４】
上述した走行軌道編集処理で得られた自走行軌道データを記憶装置から読み取り（ステップＳ６１）、図示しないカウンタ（距離計）をリセットし（ステップＳ６２）、平均速度（目標値）を設定する（ステップＳ６３）。
【００３５】
ジャイロセンサ、距離計（カウンタ値）からデッドレコニングの現在位置を求める（ステップＳ６４）。デッドレコニングの現在位置とは、走行した計測データに基づいて算出される現在位置である。超音波センサにより移動体（ロボット）の方向角を求める（ステップＳ６５）。この方向角の算出については、図９、図１０に示すように行う。すなわち、前後左右輪に設けられた畝センサ５により、畝の両斜面からの距離Ｄｆｌ，Ｄｆｒ，Ｆｒｌ，Ｄｒｒを求め、図９に示す演算式より方向角θ（Δθ：ズレ角）を求める。
【００３６】
受信されるＧＰＳの値が更新された場合（ステップＳ６６，Ｙ）、ＧＰＳ、傾斜角センサからＧＰＳの現在位置を求める（ステップＳ６７−１）。畝センサ５により取得された畝情報よりＧＰＳの補正を行う（ステップＳ６７−２）。そして、補正されたＧＰＳの現在位置よりデッドレコニングの補正を行う（ステップＳ６７−３）。そして、デッドレコニングの現在位置に該当する軌道（折れ線）データを得る（ステップＳ６８）。
【００３７】
ここで、ステップＳ６７−２で行われる畝情報に基づくＧＰＳの補正について図２０を用いて説明する。この補正は４つの畝センサ５より得られる畝の両斜面と各畝センサとの距離（距離Ｄｆｌ，Ｄｆｒ，Ｆｒｌ，Ｄｒｒ）から畝の移動体の中心に対する畝中心線からのズレ量ΔＹを求める。次に畝（自走行軌道データ）を作成するときに得た、畝中心線からΔＹだけオフセットした線を作成する。移動体はこのオフセット線上のどこかにあるはずなので、ＧＰＳ座標がオフセット線上で最も近い点になるように補正する。
【００３８】
次にステップＳ６７−３におけるデッドレコニングの補正について説明する。まず、ＧＰＳ位置情報が補正されて取得されると、上記デッドレコニングの位置情報との差分を求める。この差分は移動体のドリフト、タイヤのスリップなどによってジャイロによる測定誤差が蓄積することにより発生するものである。そして、求められた差分を１／ｎ（例えばｎ＝２）だけデッドレコニング位置情報を補正されたＧＰＳの位置情報に近づけるようにする。図１１は、ｎを２とした場合に、その補正処理を示す概念図であり、ＧＰＳの変動（黒丸）に対して、デッドレコニングの変動（白丸）が少なくなっていることを示している。
【００３９】
このデッドレコニングの補正は、図１１より理解されるように、ＧＰＳにより重みをつけてデッドレコニングを修正することにより、移動平均のような効果が得られる。これによりＧＰＳの変動による影響を低減することができる。なお、ＧＰＳにより求めた位置とデッドレコニング位置とのズレ量が想定外の範囲にあるときは、デッドレコニングの補正は実施しない。
【００４０】
次に、図１２に示されるように、自走行している現在の折れ線データを求める（ステップＳ６８）。そして、図１３に示されるように、現在のデッドレコニング位置に最も近い軌道上の点（射影点）を求める（ステップＳ７１）。射影点での接線方向(目標とする方向角)を求め（ステップＳ７２）、現在のデッドレコニング位置と走行起動のズレ量を求める（ステップＳ７３）。
【００４１】
ズレ量が許容範囲にある場合は（ステップＳ７４）、駆動制御マイコンへ必要な速度を送信する（ステップＳ７５）とともに、操舵制御マイコンへ起動修正に必要な操舵角を送信する（ステップＳ７６）。センサエラーが無い場合において（ステップＳ７７，Ｎｏ）、畝センサ出力がゼロとなり（ステップＳ７８，Ｙｅｓ）、畝が検出されなくなった場合は、走行軌道（畝）が終了したか否か判断が行われ、畝が終了していない場合（ステップＳ８１，Ｎｏ）は、図１４に示すように、次の畝に移るため、移動体の長さ分ほど枕地部を直進し（ステップＳ８２）、移動体を走行軌道の方向に向けるため９０度回転させる（ステップＳ８３）。以上において、ステップＳ７１〜ステップＳ７６は本発明の走行誤差補正手段を構成している。
【００４２】
そして、図１５に示すように、畝端において、畝端を畝端センサ６で検出しつつ枕地部で軌道走行する（ステップＳ８４）。図１４に示すように目標のｎ番目の畝に到達した場合は（ステップＳ８５，Ｙｅｓ）、移動体を目標の畝の長さ方向に向けるため更に９０度回転させて（ステップＳ８５）、畝間の走行を開始する（ステップＳ８６）べく、ステップＳ６４に戻る。
【００４３】
なお、ステップＳ８１において走行軌道が全て終了した場合（ステップＳ８１，Ｙｅｓ）は処理を終了する。またステップＳ７８において畝センサによる畝が検出されている場合は（ステップＳ７８，Ｎｏ）、ステップＳ６４に戻る。また、ステップＳ７４においてズレ量が許容範囲内でない場合（ステップＳ７４，Ｎｏ）及びセンサエラーがある場合（ステップＳ７７，Ｙｅｓ）は緊急停止が行われる（ステップＳ７９）。
【００４４】
なお、図１６に移動体が旋回する様子を示している。
【００４５】
図１７は農業機械取り付け部１３０に取り付けられる農業機械の一例を示す側面図である。この農業機械はブロッコリーの刈り取り機２００を示し、刈り取り機２００全体がワイヤー２０１により吊下される。この刈り取り機２００は、取手２０２が設けられた支持棒２０３と、支持棒２０３の先に設けられる刈り取り部２０４を備え、刈り取り部２０４は、モータ２０５が取り付けられる断面半円状の外カバー２０６と、外カバー２０６の内側に設けられ、ブロッコリー３００を押さえる断面半円状のブロッコリーカバー２０７と、ブロッコリーカバー２０７と外カバー２０６の間に回転可能に設けられ、モータ２０５により回転駆動されてブロッコリー３００をカットする回転刃２０８とを備えている。取手２０２にはモータ駆動スイッチ２１０が設けられており、このスイッチ２１０の押下によりカッターモータ制御部９を介して回転刃２０８が回転し、ブロッコリーをカットする。
【図面の簡単な説明】
【００４６】
【図１】本発明の実施の形態における移動体を示すブロック図である。
【図２】本発明の実施の形態における移動体の側面図である。
【図３】本発明の実施の形態における移動体の正面図である。
【図４】移動軌跡（走行軌道）取得処理の一例を示すフローチャートである。
【図５】走行軌道編集処理の一例を示すフローチャートである。
【図６】自走行処理の一例を示すフローチャート（その１）である。
【図７】自走行処理の一例を示すフローチャート（その２）である。
【図８】自走行処理の一例を示すフローチャート（その３）である。
【図９】移動体の走行方向を示す式である。
【図１０】移動体の車輪と畝との関係を示す図である。
【図１１】デッドレコニングの重み付補正処理を示す概念図である。
【図１２】移動体の走行軌道を示す図である。
【図１３】走行軌道のズレを示す概念図である。
【図１４】畝と移動体の移動を示す図である。
【図１５】畝端センサの作用を示す図である。
【図１６】移動体の旋回を示す図である。
【図１７】農業機械の一例としてブロッコリーの刈取り機械を示す図である。
【図１８】移動体の走行軌道の取得例を示す概念図である。
【図１９】走行軌道の折れ線近似の一例を示す図である。
【図２０】ＧＰＳ位置情報の補正処理を示す概念図である。
【符号の説明】
【００４７】
１速度センサ、２操舵角センサ、３ジャイロ、４傾斜センサ、５畝センサ、６畝端センサ、７制御部、８操作パネル、９カッターモータ制御部、１０ＧＰＳ受信アンテナ、１１操舵駆動部、１２駆動部、７２メイン処理部、７２ａ記憶部。

【特許請求の範囲】
【請求項１】
移動した移動軌跡を記憶する記憶手段と、
前記記憶手段に記憶された移動軌跡を編集して自走行軌道を生成する走行軌道生成手段と、
前記走行軌道生成手段により生成された自走行軌道に基づいて走行する走行手段と、
前記走行手段により実際に走行した軌道である自走行軌跡と、前記走行軌道生成手段により生成された自走行軌道とに基づいて、前記自走行軌跡と前記自走行軌道との誤差が少なくなるように、前記走行手段に補正値を与える走行誤差補正手段と
を備えてなる移動体。
【請求項２】
前記記憶手段は前記移動軌跡を座標点列として記憶し、前記走行軌道生成手段は、前記記憶手段に記憶された座標点列から、曲線近似によってスムージングした（Ｎ次）近似曲線を求め、該曲線を直線近似して走行軌道を生成することを特徴とする請求項１に記載の移動体。
【請求項３】
前記走行誤差補正手段は、前記移動体の走行軌道からのズレ量の一定割合を補正値として補正することを特徴とする請求項１に記載の移動体。
【請求項４】
前記移動体は畝に沿って走行する農業ロボットであり、
速度、操舵角、位置、畝を検出するセンサを備え、前記走行手段は、前記自走行軌道とこれらセンサによる検出値を用いて走行することを特徴とする請求項１に記載の移動体。
【請求項５】
前記畝を検出するセンサは、畝の長さ方向の端部を検出する畝端センサと、畝の長さ方向に延びる斜面を検出する畝センサとを備えることを特徴とする請求項４に記載の移動体。
【請求項６】
移動体の走行方法であって、
移動した移動軌跡を記憶し、
前記記憶された移動軌跡を編集して自走行軌道を生成し、
前記生成された自走行軌道に基づいて走行するとともに、実際に走行した軌道である自走行軌跡と前記自走行軌道との誤差を補正する
移動体の走行方法。

【図１】