JP3952548B2

JP3952548B2 - Method and apparatus for controlling unmanned vehicle

Info

Publication number: JP3952548B2
Application number: JP21622997A
Authority: JP
Inventors: 直道藤永
Original assignee: 神鋼電機株式会社
Priority date: 1997-08-11
Filing date: 1997-08-11
Publication date: 2007-08-01
Anticipated expiration: 2017-08-11
Also published as: JPH1165662A

Description

【０００１】
【発明の属する技術分野】
本発明は，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御方法及びその装置に関するものである。
【０００２】
【従来の技術】
例えば，図６に示すような，それぞれ独立して速度及び操舵角の制御が可能な３つ以上の車輪Ｗ（図６ではＷ１〜Ｗ３の３つ）を有する無人走行車Ｂ０を，所定の走行経路に沿って走行させる場合，従来は，上記各車輪Ｗ１〜Ｗ３の操舵角α１〜α３及び速度Ｖ１〜Ｖ３に基づいて車体Ｂ０の位置・姿勢・進行方向を推定し，該推定値と，予め記憶されている上記所定の走行経路情報との偏差を小さくするように，各車輪Ｗ１〜Ｗ３の操舵角α１〜α３及び速度Ｖ１〜Ｖ３を制御していた。
その際，上記車体Ｂ０の位置・姿勢・進行方向の推定値は，車輪Ｗ１〜Ｗ３の軸線の交点として求められた車体の回転中心位置Ｃ，各車輪Ｗ１〜Ｗ３の速度Ｖ１〜Ｖ３，及び前回得られた車体の位置・姿勢・進行方向の値から求められていた。
また，各車輪の操舵角及び速度を制御する際には，各車輪の軸線が，車体の目標回転中心位置Ｃ′を通るように各車輪の操舵角α１〜α３が制御され，上記目標回転中心位置Ｃ′から各車輪Ｗ１〜Ｗ３までの距離に応じて各車輪の速度Ｖ１〜Ｖ３が制御されていた。
【０００３】
【発明が解決しようとする課題】
ところで，車体Ｂ０が進行方向を変えながら走行している場合には，車体Ｂ０は必ずある点を回転中心として走行するため，上述のように，上記各車輪Ｗ１〜Ｗ３は原則としてその軸線が一点（上記回転中心）で交差するようにそれぞれの操舵角α１〜α３が制御される。しかしながら，各車輪の操舵モータの応答性の違いや外乱などにより，上記各車輪Ｗ１〜Ｗ３の軸線が一点で交わらない場合には（図７参照），全車輪の交点の平均位置を車体の回転中心と推定して車体の位置・姿勢・進行方向が推定される。ところが，上記各車輪の操舵モータの応答性の違いや外乱などが大きく，各車輪の軸線の交点のズレが大きくなると，車輪同士の進行方向が大きく干渉し，各車輪のモータや車軸に過負荷がかかったり，車体が振動するなどの不具合が発生する場合があった。
本発明は上記事情に鑑みてなされたものであり，その目的とするところは，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車において，各車輪同士の干渉を最小限に抑えることが可能な無人走行車の制御方法及びその装置を提供することである。
【０００４】
【課題を解決するための手段】
上記目的を達成するために本発明の方法は，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御方法において，上記２つのマスタ輪の操舵角を検出する操舵角検出工程と，上記操舵角検出工程で検出された上記マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御工程と，上記２つのマスタ輪の速度を検出する速度検出工程と，上記操舵角検出工程及び上記速度検出工程で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定工程と，上記推定工程で推定された車体の走行軌跡と上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御工程とを具備し，上記スレーブ輪制御工程において，上記操舵角検出工程で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出工程及び上記速度検出工程で検出された上記マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御方法として構成されている。
また，上記スレーブ輪制御工程は，例えば上記操舵角検出工程で検出された上記両マスタ輪の操舵角より該両マスタ輪の軸線の交点を求め，上記速度検出工程で検出された上記両マスタ輪の速度と，上記交点から各車輪までの距離の比とを用いて，上記スレーブ輪の速度を制御するように構成できる。
更に，上記推定工程は，例えば上記両マスタ輪の軸線の交点より車体の回転中心位置を求め，上記回転中心位置と上記両マスタ輪との距離，及び上記両マスタ輪の速度を用いて，車体座標系における車体の並進速度，回転速度，及び進行方向を求め，上記車体座標系における車体の並進速度，回転速度，及び進行方向と，前回の制御時における車体の推定位置・姿勢・進行方向とに基づいて，現在の車体の推定位置・姿勢・進行方向を求めるように構成できる。
【０００５】
また，上記目的を達成するために本発明の装置は，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御装置において，上記所定の走行経路の情報が予め記憶された地図情報記憶手段と，上記２つのマスタ輪の操舵角を検出する操舵角検出手段と，上記操舵角検出手段で検出された上記両マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御手段と，上記２つのマスタ輪の速度を検出する速度検出手段と，上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定手段と，上記推定手段で推定された車体の走行軌跡と上記地図情報記憶手段に記憶された上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御手段とを具備し，上記スレーブ輪制御手段が，上記操舵角検出手段で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御装置として構成されている。
【０００６】
【作用】
本発明に係る無人走行車の制御装置では，無人走行車の走行中，まず操舵角検出手段及び速度検出手段により，両マスタ輪の操舵角及び移動速度が検出される。そして，スレーブ輪制御手段により，上記両マスタ輪の操舵角の実測値に基づいて，スレーブ輪の操舵角が制御される。その方法としては，例えば上記操舵角検出手段で検出された上記両マスタ輪の操舵角より該両マスタ輪の軸線の交点が求められ，スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角が制御される。また，スレーブ輪の移動速度は，上記両マスタ輪の操舵角及び移動速度の実測値に基づいて制御される。その方法としては，例えば上記操舵角検出手段で検出された両マスタ輪の操舵角より該両マスタ輪の軸線の交点が求められ，上記速度検出手段で検出された両マスタ輪の速度と，上記交点から各車輪までの距離の比とを用いて，スレーブ輪の速度が制御される。このように，スレーブ輪の操舵角及び移動速度が，両マスタ輪の操舵角及び移動速度の実測値に基づいて制御されるため，従来の制御装置のように各車輪の軸線の交点がズレて車輪同士の進行方向の干渉が大きくなり，各車輪のモータや車軸に過負荷がかかったり車体が振動するなどの不具合が防止される。
【０００７】
続いて，推定手段により，上記操舵角検出手段及び速度検出手段で検出された両マスタ輪の操舵角及び移動速度に基づいて，車体の走行軌跡，例えば現在の位置・姿勢・進行方向が推定される。その方法としては，例えば上記両マスタ輪の軸線の交点より車体の回転中心位置が求められ，上記回転中心位置と上記両マスタ輪との距離，及び上記両マスタ輪の速度を用いて，車体座標系における車体の並進速度，回転速度，及び進行方向が求められ，上記車体座標系における車体の並進速度，回転速度，及び進行方向と，前回の制御時における車体の推定位置・姿勢・進行方向とに基づいて，現在の車体の推定位置・姿勢・進行方向が求められる。
続いて，マスタ輪制御手段により，地図情報記憶手段に予め記憶された走行経路上での車体の位置・進行方向・姿勢と，上記推定手段で推定された車体の現在の位置・進行方向・姿勢との偏差を小さくするように両マスタ輪の速度及び操舵角が制御される。このように，２つのマスタ輪のみを用いて車体の現在の位置・姿勢・進行方向が推定され，更に所定の走行経路に近付ける制御についても上記２つのマスタ輪に対して行われるため，上記従来の制御装置のように各車輪の操舵モータの応答性の違いや外乱などが大きくなっても，車体の回転中心は必ず１点に決められ，正確な推定が行える。
【０００８】
【発明の実施の形態】
以下添付図面を参照して，本発明の実施の形態及び実施例につき説明し，本発明の理解に供する。尚，以下の実施の形態及び実施例は本発明を具体化した一例であって，本発明の技術的範囲を限定する性格のものではない。
ここに，図１は本発明の実施の形態に係る無人走行車の制御装置Ａ１の概略構成を示す模式図，図２は本発明の実施の形態に係る無人走行車の制御方法の概略処理手順を示すフローチャート，図３は上記制御装置Ａ１による無人走行車Ｂ１の制御手順の説明図，図４は上記制御装置Ａ１による無人走行車Ｂ１の制御手順の説明図，図５は無人走行車Ｂ１の推定位置の算出方法の説明図である。
本実施の形態において制御対象とする無人走行車Ｂ１は，図３に示すように，３つの車輪Ｗ１〜Ｗ３を有している。これら３つの車輪Ｗ１〜Ｗ３にはそれぞれ図示しない操舵モータと駆動モータとが接続されており，それぞれ独立して速度及び操舵角の制御が可能である。また，上記３つの車輪Ｗ１〜Ｗ３のうち，Ｗ１とＷ２をマスタ輪，Ｗ３をスレーブ輪とする。
上記無人走行車Ｂ１の走行制御を行う制御装置Ａ１は，図１に示すように，無人走行車Ｂ１のマスタ輪Ｗ１，Ｗ２の操舵角検出センサ１，３と，マスタ輪Ｗ１，Ｗ２の移動速度検出センサ２，４と，上記各検出センサ１〜４に接続されたスレーブ輪制御部５と，上記各検出センサ１〜４に接続された推定部６と，地図情報記憶部７と，上記推定部６及び上記地図情報記憶部７に接続されたマスタ輪制御部８と，上記マスタ輪制御部８に接続されたマスタ輪Ｗ１，Ｗ２の操舵モータコントローラ９，１１，及びマスタ輪Ｗ１，Ｗ２の駆動モータコントローラ１０，１２と，上記スレーブ輪制御部５に接続されたスレーブ輪Ｗ３の操舵モータコントローラ１３及び駆動モータコントローラ１４とで構成されている。
上記マスタ輪Ｗ１，Ｗ２の操舵角検出センサ１，３はそれぞれマスタ輪Ｗ１，Ｗ２の操舵角を検出し，マスタ輪Ｗ１，Ｗ２の移動速度検出センサ２，４はそれぞれマスタ輪Ｗ１，Ｗ２の移動速度を検出する。
上記スレーブ輪制御部５は，上記検出センサ１〜４で検出された両マスタ輪の操舵角及び移動速度に基づいて，スレーブ輪Ｗ３の操舵角及び移動速度を求め，上記スレーブ輪Ｗ３の操舵モータコントローラ１３及び駆動モータコントローラ１４に操舵角及び移動速度の変更を指示する。
上記推定部６は，上記検出センサ１〜４で検出された両マスタ輪の操舵角及び移動速度に基づいて，車体Ｂ１の現在の位置・姿勢・進行方向を推定する。
上記マスタ輪制御部８では，上記地図情報記憶部７に予め記憶されている車体Ｂ１の走行経路情報と，上記推定部６で推定された車体Ｂ１の現在の位置・姿勢・進行方向との偏差を小さくするような両マスタ輪Ｗ１，Ｗ２の速度及び操舵角を求め，上記マスタ輪Ｗ１，Ｗ２の操舵モータコントローラ９，１１及び駆動モータコントローラ１０，１２に操舵角及び移動速度の変更を指示する。
【０００９】
以下，上記制御装置Ａ１による無人走行車Ｂ１の制御動作について，図２に示すフローチャート，及び図３，図４に示す説明図を用いて詳述する。
無人走行車Ｂ１の走行中，まず上記両マスタ輪Ｗ１，Ｗ２の操舵角，移動速度検出センサ１〜４により，両マスタ輪の操舵角α₁，α₂及び移動速度Ｖ₁，Ｖ₂が検出される（ステップＳ１）。
続いて，スレーブ輪制御部５は，上記各検出センサ１〜４で検出された両マスタ輪の操舵角及び移動速度に基づいて，以下のようにスレーブ輪Ｗ３の操舵角及び移動速度を求める（ステップＳ２）。
図３に示すように，車体中心を原点とする車体座標系における上記両マスタ輪の操舵角α₁，α₂，及び座標値（Ｘ₁，Ｙ₁），（Ｘ₂，Ｙ₂）を用いて，車体の回転中心座標（Ｘ_p，Ｙ_p）を上記車体座標系で表すと次のようになる。
【数１】

また，車体Ｂ１の並進速度Ｖ_c，回転速度ω_c，進行方向θ_c-bodyを上記車体座標系に対して算出すると次のようになる。
【数２】

上記車体の回転中心座標（Ｘ_p，Ｙ_p）と車輪Ｗ３（座標（Ｘ₃，Ｙ₃））とを結んだ直線に対して，車輪Ｗ３が垂直になるように，車輪Ｗ３の操舵角α₃を次式により求める。
α₃＝ｔａｎ^-1（（Ｘ₃−Ｘ_p）／（Ｙ_p−Ｙ₃）） …（６）
また，車輪Ｗ３の移動速度Ｖ₃は，
Ｖ_c／Ｒ₀＝Ｖ₃／Ｒ₃＝ω_c …（７）
の関係より，次のように求められる。
【数３】

以上のようにして車輪Ｗ３の操舵角α₃及び移動速度Ｖ₃が求められると，上記スレーブ輪制御部５は，車輪Ｗ３の操舵，駆動モータコントローラ１３，１４に対して，車輪Ｗ３の操舵角及び移動速度をそれぞれ上記α₃，Ｖ₃とするように指示する。
このように，スレーブ輪Ｗ３は，両マスタ輪Ｗ１，Ｗ２の操舵角及び移動速度の実測値に基づいて得られた車体Ｂ１の回転中心，並進速度，回転速度に合わせて操舵角及び駆動速度が制御されるため，従来の制御装置のように各車輪の軸線の交点がズレて車輪同士の進行方向の干渉が大きくなり，各車輪のモータや車軸に過負荷がかかったり車体が振動するなどの不具合が防止できる。
【００１０】
続いて，推定部６は，上記各検出センサ１〜４で検出された両マスタ輪の操舵角及び移動速度に基づいて，以下のように車体Ｂ１の現在の位置・姿勢・進行方向を推定する。（ステップＳ３）。
車体Ｂ１は，上式（１）〜（５）に示すように，回転中心を（Ｘ_p，Ｙ_p）として，並進速度Ｖ_c，回転速度ω_c，進行方向θ_c-body（全て車体座標系）で走行している。そこで，上式（１）〜（５）を用いて，経路座標系における車体Ｂ１の位置・進行方向・姿勢（Ｘ_n，Ｙ_n，θ_n，φ_n）を求める。
車体Ｂ１は回転速度ω_cで回転しているから，微小単位時間ΔＴの間にはω_cΔＴだけ車体姿勢が変化する。従って，車体姿勢φ_nは，前回の算出結果φ_n-1に変化分を加算することにより次のように求められる。
φ_n＝φ_n-1＋ω_cΔＴ …（９）
進行方向θ_nは，車体姿勢φ_nに車体座標系での進行方向θ_c-bodyを加算することにより次のように求められる。
θ_n＝θ_c-body＋φ_n …（１０）
車体の位置（Ｘ_n，Ｙ_n）は，微小時間内での走行軌跡を円弧と仮定し，前回の算出結果（Ｘ_n-1，Ｙ_n-1）に位置の変化分を加算することにより次のように求められる。
Ｘ_n＝Ｘ_n-1＋ｄ・ｃｏｓ（θ_n-1＋Δθ／２） …（１１）
Ｙ_n＝Ｙ_n-1＋ｄ・ｓｉｎ（θ_n-1＋Δθ／２） …（１２）
ここで，
Δθ≠０のとき（回転走行のとき），
Δｓ＝Ｒ₀Δθ，ｄ＝２Ｒ₀ｓｉｎ（Δθ／２）より（図５参照），
ｄ＝（ｓｉｎ（Δθ／２）／（Δθ／２））・Δｓ
Δθ＝０（直線走行のとき），
ｄ＝Δｓ
但し，Δθ＝θ_n−θ_n-1，
Δｓ＝（（Ｖ_n−Ｖ_n-1）／２）・ΔＴ
以上のように，車体Ｂ１の経路座標系における位置・進行方向・姿勢（Ｘ_n，Ｙ_n，θ_n，φ_n）が求められた。
【００１１】
続いて，マスタ輪制御部８では，地図情報記憶部７に予め記憶されている走行経路上での車体Ｂ１の位置・進行方向・姿勢（Ｘ_r，Ｙ_r，θ_r，φ_r）と，上記推定部６で推定された車体Ｂ１の現在の位置・進行方向・姿勢（Ｘ_n，Ｙ_n，θ_n，φ_n）との偏差（Ｘ_e，Ｙ_e，θ_e，φ_e）を小さくするような両マスタ輪Ｗ１，Ｗ２の速度Ｖ₁′，Ｖ₂′及び操舵角α₁′，α₂′を以下のように求める（ステップＳ４）。
まず，上記偏差（Ｘ_e，Ｙ_e，θ_e，φ_e）が小さくなるような車体Ｂ１の並進速度Ｖ₀，回転速度ω₀，進行方向θ₀（図４参照）を，車体座標系に対して算出する。ここで，基本的な考え方として，Ｘ方向の偏差は並進速度で，Ｙ方向の偏差は進行方向で，車体姿勢の偏差は角速度でそれぞれ補正する。
Ｖ₀＝Ｖ_r＋Ｋｘ・Ｘ_e …（１３）
ω₀＝ω_r＋Ｋφ・φ_e …（１４）
θ_0-body＝θ_r-body＋Ｋｙ・Ｙ_e …（１５）
但し，Ｖ_r：目標速度
ω_r：目標各速度
θ_r-body：車体座標系における目標進行方向（＝θ_r−φ_r）
Ｋｘ：Ｘ方向偏差の補正ゲイン（正の定数）
Ｋｙ：Ｙ方向偏差の補正ゲイン（正の定数）
Ｋφ：車体姿勢偏差の補正ゲイン（正の定数）
【００１２】
次に，上記（１３）〜（１５）式を用いて，次のようにマスタ輪Ｗ１，Ｗ２の速度Ｖ₁′，Ｖ₂′，及び操舵角α₁′，α₂′を求める（図４参照）。
まず，車体Ｂ１の回転半径Ｒ₀′は，
Ｒ₀′＝Ｖ₀／ω₀ …（１６）
であるため，車体Ｂ１の回転中心座標（Ｘ_p′，Ｙ_p′）は次式で表せる。
Ｘ_p′＝−Ｒ₀′・ｓｉｎθ_0-body …（１７）
Ｙ_p′＝Ｒ₀′・ｃｏｓθ_0-body …（１８）
そこで，マスタ輪Ｗ１，Ｗ２の速度Ｖ₁′，Ｖ₂′，及び操舵角α₁′，α₂′は次式により求められる。
【数４】

以上のようにして，マスタ輪Ｗ１，Ｗ２の速度及び操舵角が求められると，上記マスタ輪制御部８は，マスタ輪Ｗ１，Ｗ２の操舵，駆動モータコントローラ９〜１２に対して，マスタ輪Ｗ１，Ｗ２の操舵角及び駆動速度をそれぞれ上記Ｖ₁′，Ｖ₂′，α₁′，α₂′とするように指示する。
上記ステップＳ４が終了すると，上記ステップＳ１へ戻り，制御後の両マスタ輪の操舵角及び移動速度が検出され，ステップＳ２において上記検出値に基づいてスレーブ輪Ｗ３の制御が行われる。
【００１３】
以上のように，推定部６では，２つのマスタ輪Ｗ１，Ｗ２のみを用いて車体Ｂ１の現在の位置・姿勢・進行方向が推定され，更に所定の走行経路に近付ける制御についても上記２つのマスタ輪に対して行われるため，上記従来の制御装置のように各車輪の操舵モータの応答性の違いや外乱などが大きくなっても，車体Ｂ１の回転中心は必ず１点に決めることができ，正確な推定が行える。
以上説明したように，本実施の形態に係る制御装置Ａ１では，スレーブ輪Ｗ３の操舵角及び移動速度が，両マスタ輪Ｗ１，Ｗ２の操舵角及び移動速度の実測値に基づいて制御されるため，従来の制御装置のように各車輪の軸線の交点がズレて車輪同士の進行方向の干渉が大きくなり，各車輪のモータや車軸に過負荷がかかったり車体が振動するなどの不具合が防止できる。また，２つのマスタ輪Ｗ１，Ｗ２のみを用いて車体Ｂ１の現在の位置・姿勢・進行方向が推定され，更に所定の走行経路に近付ける制御についても上記２つのマスタ輪に対して行われるため，上記従来の制御装置のように各車輪の操舵モータの応答性の違いや外乱などが大きくなっても，車体Ｂ１の回転中心は必ず１点に決めることができ，正確な推定を行うことができる。
【００１４】
【実施例】
上記実施の形態では，スレーブ輪が１つの場合を取り上げたが，スレーブ輪が２つ以上の場合も全く同様に適用できる。その場合，各スレーブ輪の操舵角及び移動速度は，上記（６），（８）式内の座標値（Ｘ₃，Ｙ₃）を各スレーブ輪の座標値に入れ替えることにより求められる。
【００１５】
【発明の効果】
以上説明したように，本発明に係る無人走行車の制御方法は，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御方法において，上記２つのマスタ輪の操舵角を検出する操舵角検出工程と，上記操舵角検出工程で検出された上記マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御工程と，上記２つのマスタ輪の速度を検出する速度検出工程と，上記操舵角検出工程及び上記速度検出工程で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定工程と，上記推定工程で推定された車体の走行軌跡と上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御工程とを具備し，上記スレーブ輪制御工程において，上記操舵角検出工程で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出工程及び上記速度検出工程で検出された上記マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御方法として構成されているため，スレーブ輪の操舵角が両マスタ輪の操舵角の実測値に基づいて制御され，各車輪の軸線の交点がズレて車輪同士の進行方向の干渉が大きくなっても，各車輪のモータや車軸に過負荷がかかったり車体が振動するなどの不具合が防止できる。また，２つのマスタ輪のみを用いて車体の現在の位置・姿勢・進行方向が推定され，更に所定の走行経路に近付ける制御についても上記２つのマスタ輪に対して行われるため，各車輪の操舵モータの応答性の違いや外乱などが大きくなっても，車体の回転中心は必ず１点に決めることができ，正確な推定を行うことができる。
【００１６】
また，上記制御方法を適用可能な制御装置として，本発明の装置は，それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御装置において，上記所定の走行経路の情報が予め記憶された地図情報記憶手段と，上記２つのマスタ輪の操舵角を検出する操舵角検出手段と，上記操舵角検出手段で検出された上記両マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御手段と，上記２つのマスタ輪の速度を検出する速度検出手段と，上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定手段と，上記推定手段で推定された車体の走行軌跡と上記地図情報記憶手段に記憶された上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御手段とを具備し，上記スレーブ輪制御手段が，上記操舵角検出手段で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御装置として構成されているため，スレーブ輪の操舵角が両マスタ輪の操舵角の実測値に基づいて制御され，各車輪の軸線の交点がズレて車輪同士の進行方向の干渉が大きくなっても，各車輪のモータや車軸に過負荷がかかったり車体が振動するなどの不具合が防止できる。更に，２つのマスタ輪のみを用いて車体の走行軌跡が推定され，更に所定の走行経路に近付ける制御についても上記２つのマスタ輪に対して行われるため，各車輪の操舵モータの応答性の違いや外乱などが大きくなっても，車体の回転中心は必ず１点に決めることができ，正確な推定を行うことができる。
【図面の簡単な説明】
【図１】本発明の実施の形態に係る無人走行車の制御装置Ａ１の概略構成を示す模式図。
【図２】本発明の実施の形態に係る無人走行車の制御方法の概略処理手順を示すフローチャート。
【図３】上記制御装置Ａ１による無人走行車Ｂ１の制御手順の説明図。
【図４】上記制御装置Ａ１による無人走行車Ｂ１の制御手順の説明図。
【図５】無人走行車Ｂ１の推定位置の算出方法の説明図。
【図６】従来の無人走行車Ｂ０の走行制御方法の説明図。
【図７】従来の無人走行車Ｂ０の走行制御方法の説明図。
【符号の説明】
１…車輪Ｗ１の操舵角検出センサ
２…車輪Ｗ１の移動速度検出センサ
３…車輪Ｗ２の操舵角検出センサ
４…車輪Ｗ２の移動速度検出センサ
５…スレーブ輪制御部
６…推定部
７…地図情報記憶部
８…マスタ輪制御部
９…車輪Ｗ１の操舵モータコントローラ
１０…車輪Ｗ１の駆動モータコントローラ
１１…車輪Ｗ２の操舵モータコントローラ
１２…車輪Ｗ２の駆動モータコントローラ
１３…車輪Ｗ３の操舵モータコントローラ
１４…車輪Ｗ３の駆動モータコントローラ
Ｂ１…無人走行車
Ｗ１，Ｗ２…マスタ輪
Ｗ３…スレーブ輪[0001]
BACKGROUND OF THE INVENTION
The present invention relates to an unmanned traveling vehicle having two master wheels capable of independently controlling speed and steering angle and one or more slave wheels capable of independently controlling speed and steering angle. The present invention relates to a control method and an apparatus for an unmanned traveling vehicle that travels along a predetermined traveling route.
[0002]
[Prior art]
For example, as shown in FIG. 6, an unmanned vehicle B0 having three or more wheels W (three in FIG. 6, W1 to W3) capable of independently controlling the speed and the steering angle, In the case of traveling along a route, conventionally, the position / posture / traveling direction of the vehicle body B0 is estimated based on the steering angles α1 to α3 and the speeds V1 to V3 of the wheels W1 to W3. The steering angles α1 to α3 and the speeds V1 to V3 of the wheels W1 to W3 are controlled so as to reduce the deviation from the stored predetermined travel route information.
At that time, the estimated values of the position, posture, and traveling direction of the vehicle body B0 are the rotation center position C of the vehicle body determined as the intersection of the axes of the wheels W1 to W3, the speeds V1 to V3 of the wheels W1 to W3, and the previous time. It was obtained from the values of the obtained position, posture, and traveling direction of the vehicle body.
Further, when controlling the steering angle and speed of each wheel, the steering angles α1 to α3 of each wheel are controlled so that the axis of each wheel passes the target rotation center position C ′ of the vehicle body, and the target rotation center is controlled. The speeds V1 to V3 of the wheels are controlled according to the distance from the position C ′ to the wheels W1 to W3.
[0003]
[Problems to be solved by the invention]
By the way, when the vehicle body B0 is traveling while changing the traveling direction, the vehicle body B0 always travels around a certain point, so that each wheel W1 to W3 has one axis in principle as described above. The respective steering angles α <b> 1 to α <b> 3 are controlled so as to intersect at (the rotation center). However, if the axes of the wheels W1 to W3 do not intersect at a single point (see FIG. 7) due to responsiveness differences or disturbances of the steering motors of the wheels (see FIG. 7), the average position of the intersections of all the wheels is determined by the rotation of the vehicle body. The position / posture / traveling direction of the vehicle body is estimated by assuming the center. However, the difference in response and disturbance of the steering motor of each wheel is large, and if the deviation of the intersection of the axis of each wheel increases, the traveling direction of the wheels greatly interferes, and the motor and axle of each wheel are overloaded. In some cases, there were problems such as stagnation and vibration of the car body.
The present invention has been made in view of the above circumstances, and an object thereof is to provide two master wheels capable of independently controlling speed and steering angle, and independently controlling speed and steering angle. It is an object of the present invention to provide a control method and an apparatus for an unmanned traveling vehicle capable of minimizing interference between wheels in an unmanned traveling vehicle having one or more slave wheels capable of performing the above.
[0004]
[Means for Solving the Problems]
In order to achieve the above object, the method of the present invention includes two master wheels capable of independently controlling speed and steering angle, and one or more slaves capable of independently controlling speed and steering angle. In a method for controlling an unmanned traveling vehicle that causes an unmanned traveling vehicle having wheels to travel along a predetermined traveling route, a steering angle detecting step for detecting a steering angle of the two master wheels, and a detection by the steering angle detecting step. A slave wheel control step for controlling the steering angle of the slave wheel based on the steering angle of the master wheel, a speed detection step for detecting the speed of the two master wheels, the steering angle detection step, and the speed On the basis of the estimation step of estimating the travel locus of the vehicle body based on the speed and the steering angle of both master wheels detected in the detection step, the travel locus of the vehicle body estimated in the estimation step, and the predetermined travel route. Z And a travel control step for controlling the speed and steering angle of the master wheel, and in the slave wheel control step, based on the steering angle of the both master wheels detected in the steering angle detection step, And the control angle of the slave wheel so that the axis of the slave wheel passes through the intersection, and the speed of the master wheel detected in the steering angle detection step and the speed detection step, and It is configured as a control method for an unmanned traveling vehicle, wherein the speed of the slave wheel is controlled based on a steering angle.
In the slave wheel control step, for example, the intersection of the axes of the master wheels is obtained from the steering angle of the master wheels detected in the steering angle detection step, and the both master wheels detected in the speed detection step are obtained. And the ratio of the distance from the intersection point to each wheel can be used to control the speed of the slave wheel.
Further, in the estimation step, for example, the rotation center position of the vehicle body is obtained from the intersection of the axes of the two master wheels, the distance between the rotation center position and the two master wheels, and the speeds of the two master wheels are used. Obtain the translation speed, rotation speed, and travel direction of the vehicle body in the coordinate system, and the estimated position, posture, travel direction of the vehicle body in the previous control, and the translation position, rotation speed, and travel direction of the vehicle body in the previous control system. Based on the above, it can be configured to obtain the current estimated position / posture / traveling direction of the vehicle body.
[0005]
In order to achieve the above object, the apparatus of the present invention includes two master wheels capable of independently controlling speed and steering angle, and one or more capable of independently controlling speed and steering angle. In a control device for an unmanned traveling vehicle that travels an unmanned traveling vehicle having a plurality of slave wheels along a predetermined traveling route, map information storage means in which information on the predetermined traveling route is stored in advance, and the two masters Steering angle detection means for detecting the steering angle of the wheels, slave wheel control means for controlling the steering angle of the slave wheel based on the steering angles of the two master wheels detected by the steering angle detection means, and the 2 Speed detecting means for detecting the speed of one master wheel, estimating means for estimating the travel locus of the vehicle body based on the steering angle detecting means and the speed and steering angle of the two master wheels detected by the speed detecting means; Travel control means for controlling the speed and steering angle of the master wheel based on the travel locus of the vehicle body estimated by the estimation means and the predetermined travel route stored in the map information storage means; The slave wheel control means obtains the intersection of the axes of the two master wheels from the steering angle of the two master wheels detected by the steering angle detection means, and the slave wheel passes through the intersection so that the axis of the slave wheels passes through the intersection. An unmanned system that controls the steering angle of the wheels and controls the speed of the slave wheels based on the speed and steering angle of the master wheels detected by the steering angle detection means and the speed detection means. It is comprised as a control apparatus of a traveling vehicle.
[0006]
[Action]
In the control device for an unmanned traveling vehicle according to the present invention, the steering angle and the moving speed of both master wheels are first detected by the steering angle detecting means and the speed detecting means while the unmanned traveling vehicle is traveling. Then, the steering angle of the slave wheel is controlled by the slave wheel control means based on the measured value of the steering angle of both the master wheels. As the method, for example, the intersection of the axes of the two master wheels is obtained from the steering angle of the two master wheels detected by the steering angle detecting means, and the slave wheels are arranged so that the axis of the slave wheels passes through the intersection. The steering angle is controlled. The moving speed of the slave wheels is controlled based on the measured values of the steering angle and moving speed of both the master wheels. As the method, for example, the intersection of the axes of the two master wheels is obtained from the steering angle of the two master wheels detected by the steering angle detecting means, the speed of the two master wheels detected by the speed detecting means, The speed of the slave wheel is controlled using the ratio of the distance from the intersection to each wheel. In this way, the steering angle and moving speed of the slave wheels are controlled based on the actual measured values of the steering angle and moving speed of both master wheels, so that the intersections of the axes of the wheels are shifted as in the conventional control device. Interference between the traveling directions of the wheels increases, and problems such as overloading the motors and axles of each wheel and vibration of the vehicle body are prevented.
[0007]
Subsequently, the estimating means estimates the travel locus of the vehicle body, for example, the current position / posture / traveling direction, based on the steering angle and the moving speed of both master wheels detected by the steering angle detecting means and the speed detecting means. The As the method, for example, the rotation center position of the vehicle body is obtained from the intersection of the axes of the two master wheels, and the vehicle body coordinates are obtained using the distance between the rotation center position and the two master wheels and the speed of the two master wheels. The translation speed, rotation speed, and travel direction of the vehicle body in the system are determined. The translation speed, rotation speed, and travel direction of the vehicle body in the vehicle body coordinate system, and the estimated position / posture / travel direction of the vehicle body in the previous control Based on the above, the current estimated position / posture / traveling direction of the vehicle body is obtained.
Subsequently, the position, traveling direction, and posture of the vehicle body on the travel route stored in advance in the map information storage unit by the master wheel control unit, and the current position, traveling direction, and posture of the vehicle body estimated by the estimating unit. The speed and the steering angle of both master wheels are controlled so as to reduce the deviation from. In this way, the current position / posture / traveling direction of the vehicle body is estimated using only the two master wheels, and the control for approaching the predetermined travel route is also performed on the two master wheels. Even if the difference in response or disturbance of the steering motor of each wheel becomes large as in the control device, the rotation center of the vehicle body is always determined as one point, and accurate estimation can be performed.
[0008]
DETAILED DESCRIPTION OF THE INVENTION
DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments and examples of the present invention will be described below with reference to the accompanying drawings for understanding of the present invention. It should be noted that the following embodiments and examples are examples embodying the present invention and do not limit the technical scope of the present invention.
FIG. 1 is a schematic diagram showing a schematic configuration of a control device A1 for an unmanned traveling vehicle according to an embodiment of the present invention, and FIG. 2 is a schematic processing procedure of a control method for an unmanned traveling vehicle according to an embodiment of the present invention. FIG. 3 is an explanatory diagram of the control procedure of the unmanned traveling vehicle B1 by the control device A1, FIG. 4 is an explanatory diagram of the control procedure of the unmanned traveling vehicle B1 by the control device A1, and FIG. It is explanatory drawing of the calculation method of an estimated position.
As shown in FIG. 3, the unmanned traveling vehicle B1 to be controlled in the present embodiment has three wheels W1 to W3. A steering motor and a drive motor (not shown) are connected to these three wheels W1 to W3, respectively, and the speed and the steering angle can be controlled independently. Of the three wheels W1 to W3, W1 and W2 are master wheels, and W3 is a slave wheel.
As shown in FIG. 1, the control device A1 that performs the traveling control of the unmanned traveling vehicle B1 includes the steering

angle detection sensors

1, 3 of the master wheels W1, W2 of the unmanned traveling vehicle B1, and the moving speed of the master wheels W1, W2. Detection sensors 2 and 4, a slave wheel control unit 5 connected to the detection sensors 1 to 4, an estimation unit 6 connected to the detection sensors 1 to 4, a map information storage unit 7, and the estimation Master wheel control unit 8 connected to unit 6 and map information storage unit 7, steering motor controllers 9 and 11 of master wheels W1 and W2 connected to master wheel control unit 8, and master wheels W1 and W2. The drive motor controllers 10 and 12 and the steering motor controller 13 and the drive motor controller 14 of the slave wheel W3 connected to the slave wheel control unit 5 are configured.
The steering angles

detection sensors

1 and 3 of the master wheels W1 and W2 detect the steering angles of the master wheels W1 and W2, respectively. The moving speed detection sensors 2 and 4 of the master wheels W1 and W2 respectively move the master wheels W1 and W2. Detect speed.
The slave wheel control unit 5 obtains the steering angle and moving speed of the slave wheel W3 based on the steering angle and moving speed of both master wheels detected by the detection sensors 1 to 4, and the steering motor of the slave wheel W3. The controller 13 and the drive motor controller 14 are instructed to change the steering angle and the moving speed.
The estimation unit 6 estimates the current position / posture / traveling direction of the vehicle body B <b> 1 based on the steering angle and moving speed of both master wheels detected by the detection sensors 1 to 4.
In the master wheel control unit 8, the deviation between the travel route information of the vehicle body B 1 stored in advance in the map information storage unit 7 and the current position / posture / traveling direction of the vehicle body B 1 estimated by the estimation unit 6. The speeds and steering angles of both master wheels W1, W2 are calculated so as to reduce the steering wheel, and the steering motor controllers 9, 11 and the drive motor controllers 10, 12 of the master wheels W1, W2 are instructed to change the steering angle and moving speed. .
[0009]
Hereinafter, the control operation of the unmanned vehicle B1 by the control device A1 will be described in detail with reference to the flowchart shown in FIG. 2 and the explanatory diagrams shown in FIGS.
While the unmanned vehicle B1 is traveling, first, the steering angles α ₁ and α ₂ and the moving speeds V ₁ and V _{2 of the} master wheels are detected by the steering angles and moving speed detection sensors 1 to 4 of the master wheels W1 and W2. (Step S1).
Subsequently, the slave wheel control unit 5 obtains the steering angle and movement speed of the slave wheel W3 as follows based on the steering angle and movement speed of both master wheels detected by the detection sensors 1 to 4 ( Step S2).
As shown in FIG. 3, the steering angles α ₁ and α ₂ and the coordinate values (X ₁ , Y ₁ ) and (X ₂ , Y ₂ ) of both master wheels in the vehicle body coordinate system with the vehicle body center as the origin are used. The rotation center coordinates (X _p , Y _p ) of the vehicle body are expressed in the vehicle body coordinate system as follows.
[Expression 1]

The translational velocity V _c , rotational velocity ω _c , and traveling direction θ _c-body of the vehicle body B1 are calculated with respect to the vehicle body coordinate system as follows.
[Expression 2]

The steering angle α of the wheel W3 so that the wheel W3 is perpendicular to the straight line connecting the rotation center coordinates (X _p , Y _p ) of the vehicle body and the wheel W3 (coordinates (X ₃ , Y ₃ )). _{3 is obtained} by the following equation.
α ₃ = tan ⁻¹ ((X ₃ −X _p ) / (Y _p −Y ₃ )) (6)
In addition, the moving speed V ₃ of the wheels W3 is,
V _c / R ₀ = V ₃ / R ₃ = ω _c (7)
From the relationship, the following is obtained.
[Equation 3]

When the steering angle α ₃ and the moving speed V _{3 of} the wheel W3 are obtained as described above, the slave wheel control unit 5 determines the steering angle of the wheel W3 with respect to the steering and drive motor controllers 13 and 14 of the wheel W3. And the moving speeds are instructed to be α ₃ and V ₃ , respectively.
As described above, the slave wheel W3 has a steering angle and a driving speed in accordance with the rotation center, translational speed, and rotational speed of the vehicle body B1 obtained based on the actual measured values of the steering angle and the moving speed of the master wheels W1 and W2. Because of this control, the intersections of the axes of the wheels are shifted as in the case of the conventional control device, and the interference in the traveling direction between the wheels increases, and the motors and axles of the wheels are overloaded and the vehicle body vibrates. Trouble can be prevented.
[0010]
Subsequently, the estimation unit 6 estimates the current position / posture / traveling direction of the vehicle body B1 as follows based on the steering angle and the moving speed of both master wheels detected by the detection sensors 1 to 4. . (Step S3).
As shown in the above formulas (1) to (5), the vehicle body B1 has the rotation center as (X _p , Y _p ), the translation speed V _c , the rotation speed ω _c , the traveling direction θ _c-body (all vehicle body coordinates System). Therefore, the position, traveling direction, and posture (X _n , Y _n , θ _n , φ _n ) of the vehicle body B1 in the path coordinate system are obtained using the above equations (1) to (5).
Since the vehicle body B1 rotates at the rotational speed ω _c , the vehicle body posture changes by ω _c ΔT during the minute unit time ΔT. Accordingly, the vehicle body posture φ _n is obtained as follows by adding the change to the previous calculation result φ _n−1 .
φ _n = φ _n-1 + ω _c ΔT (9)
The traveling direction θ _n is obtained as follows by adding the traveling direction θ _c-body in the vehicle body coordinate system to the vehicle body posture φ _n .
_{_{θ n = θ c-body +}} φ n ... (10)
The position of the vehicle body (X _n , Y _n ) is calculated by adding the change in position to the previous calculation result (X _n−1 , Y _n−1 ), assuming that the traveling locus within a minute time is an arc. It is required as follows.
_Xn = _Xn-1 + d.cos ([theta] _n-1 + [Delta] [theta] / 2) (11)
Y _n = Y _n-1 + d · sin (θ _n-1 + Δθ / 2) (12)
here,
When Δθ ≠ 0 (when rotating),
From Δs = R ₀ Δθ, d = 2R ₀ sin (Δθ / 2) (see FIG. 5),
d = (sin (Δθ / 2) / (Δθ / 2)) · Δs
Δθ = 0 (when running straight),
d = Δs
_{_{However, Δθ = θ n -θ n-}} 1,
Δs = ((V _n −V _n−1 ) / 2) · ΔT
As described above, the position, the traveling direction, and the posture (X _n , Y _n , θ _n , φ _n ) of the vehicle body B1 in the route coordinate system are obtained.
[0011]
Subsequently, in the master wheel control unit 8, the position / traveling direction / posture (X _r , Y _r , θ _r , φ _r ) of the vehicle body B1 on the travel route stored in advance in the map information storage unit 7, The deviation (X _e , Y _e , θ _e , φ _e ) from the current position / traveling direction / posture (X _n , Y _n , θ _n , φ _n ) estimated by the estimation unit 6 is reduced. The speeds V ₁ ′, V ₂ ′ and steering angles α ₁ ′, α ₂ ′ of both master wheels W1, W2 are determined as follows (step S4).
First, the translational velocity V ₀ , rotational velocity ω ₀ , and traveling direction θ ₀ (see FIG. 4) of the vehicle body B1 such that the deviation (X _e , Y _e , θ _e , φ _e ) becomes small are expressed in the vehicle coordinate system. To calculate. Here, as a basic idea, the deviation in the X direction is corrected by the translational velocity, the deviation in the Y direction is corrected by the traveling direction, and the deviation of the vehicle body posture is corrected by angular velocity.
V ₀ = V _r + Kx · X _e (13)
ω ₀ = ω _r + Kφ · φ _e (14)
θ _0-body = θ _r-body + Ky · Y _e (15)
However, V _r: target speed omega _r: target each speed θ _r-body: target traveling direction of the vehicle body coordinate system (= θ _{_r} -φ _r)
Kx: X direction deviation correction gain (positive constant)
Ky: Y direction deviation correction gain (positive constant)
Kφ: Body position deviation correction gain (positive constant)
[0012]
Next, the speeds V ₁ ′ and V ₂ ′ and the steering angles α ₁ ′ and α ₂ ′ of the master wheels W1 and W2 are obtained as follows using the above equations (13) to (15) (FIG. 4). reference).
First, the turning radius R ₀ ′ of the vehicle body B1 is
R ₀ ′ = V ₀ / ω ₀ (16)
Therefore, the rotation center coordinates (X _p ′, Y _p ′) of the vehicle body B1 can be expressed by the following equation.
X _p ′ = −R ₀ ′ · sin θ _0-body (17)
Y _p ′ = R ₀ ′ · cos θ _0-body (18)
Therefore, the speeds V ₁ ′, V ₂ ′ and steering angles α ₁ ′, α ₂ ′ of the master wheels W1, W2 are obtained by the following equations.
[Expression 4]

When the speeds and steering angles of the master wheels W1 and W2 are obtained as described above, the master wheel control unit 8 controls the master wheels W1 with respect to the steering and drive motor controllers 9 to 12 of the master wheels W1 and W2. , W2 is instructed to set the steering angle and the driving speed to V ₁ ′, V ₂ ′, α ₁ ′, and α ₂ ′, respectively.
When the step S4 is completed, the process returns to the step S1, and the steering angle and the moving speed of both master wheels after the control are detected. In step S2, the slave wheel W3 is controlled based on the detected value.
[0013]
As described above, the estimation unit 6 estimates the current position / posture / traveling direction of the vehicle body B1 using only the two master wheels W1 and W2, and further controls the two masters to approach the predetermined travel route. Since the operation is performed on the wheels, the rotation center of the vehicle body B1 can always be determined as one point even if the difference in the response of the steering motor of each wheel or the disturbance increases as in the conventional control device. Accurate estimation can be performed.
As described above, in the control device A1 according to the present embodiment, the steering angle and moving speed of the slave wheel W3 are controlled based on the actual measured values of the steering angle and moving speed of the master wheels W1 and W2. , As with conventional control devices, the intersections of the axes of the wheels are misaligned, causing interference in the direction of travel between the wheels, thereby preventing problems such as overload on the motors and axles of the wheels and vibration of the vehicle body. . In addition, since the current position / posture / traveling direction of the vehicle body B1 is estimated using only the two master wheels W1, W2, and control for approaching a predetermined travel route is also performed on the two master wheels, Even if the difference in response or disturbance of the steering motor of each wheel increases as in the conventional control device, the rotation center of the vehicle body B1 can always be determined as one point, and accurate estimation can be performed. .
[0014]
【Example】
In the above embodiment, the case where there is one slave wheel has been taken up, but the present invention can be applied to the case where there are two or more slave wheels. In this case, the steering angle and moving speed of each slave wheel can be obtained by replacing the coordinate values (X ₃ , Y ₃ ) in the above equations (6) and (8) with the coordinate values of each slave wheel.
[0015]
【The invention's effect】
As described above, the control method of the unmanned traveling vehicle according to the present invention can control the speed and the steering angle independently of the two master wheels capable of independently controlling the speed and the steering angle. In a control method of an unmanned traveling vehicle that causes an unmanned traveling vehicle having one or more slave wheels to travel along a predetermined traveling route, a steering angle detecting step of detecting a steering angle of the two master wheels, and the steering A slave wheel control step for controlling the steering angle of the slave wheel based on the steering angle of the master wheel detected in the angle detection step, a speed detection step for detecting the speed of the two master wheels, and the steering angle An estimation step of estimating a travel locus of the vehicle body based on a speed and a steering angle of both the master wheels detected in the detection step and the speed detection step; a travel locus of the vehicle body estimated in the estimation step; And a travel control step of controlling the speed and steering angle of the master wheel based on the travel route, and in the slave wheel control step, based on the steering angle of the master wheels detected in the steering angle detection step. The intersection of the axes of the master wheels is obtained, the steering angle of the slave wheel is controlled so that the axis of the slave wheel passes through the intersection, and the steering angle detection step and the speed detection step detect the above Since it is configured as a control method for an unmanned vehicle that controls the speed of the slave wheel based on the speed and the steering angle of the master wheel, the steering angle of the slave wheel is equal to the steering angle of both master wheels. Control is based on the measured values, and even if the intersections of the axes of the wheels are misaligned and the interference in the direction of travel between the wheels increases, the motors and axles of the wheels are overloaded and the vehicle body vibrates. If it is possible to prevent. In addition, since the current position / posture / traveling direction of the vehicle body is estimated using only two master wheels, and control for approaching a predetermined travel route is also performed on the two master wheels, the steering of each wheel is performed. Even if the difference in motor responsiveness or disturbance increases, the center of rotation of the vehicle body can always be determined as one point, and accurate estimation can be performed.
[0016]
In addition, as a control device to which the above control method can be applied, the device of the present invention is capable of independently controlling the speed and the steering angle with two master wheels capable of independently controlling the speed and the steering angle. In a control device for an unmanned traveling vehicle that travels an unmanned traveling vehicle having one or more slave wheels along a predetermined traveling route, map information storage means in which information on the predetermined traveling route is stored in advance, Steering angle detection means for detecting the steering angle of the two master wheels, and slave wheel control means for controlling the steering angle of the slave wheel based on the steering angles of the two master wheels detected by the steering angle detection means. And a speed detection means for detecting the speeds of the two master wheels, and a travel locus of the vehicle body is estimated based on the steering angle detection means and the speeds and steering angles of the both master wheels detected by the speed detection means. And a travel control means for controlling the speed and steering angle of the master wheel based on the travel locus of the vehicle body estimated by the estimation means and the predetermined travel route stored in the map information storage means. The slave wheel control means obtains the intersection of the axes of the master wheels from the steering angle of the master wheels detected by the steering angle detection means, and the axis of the slave wheels determines the intersection. Controlling the steering angle of the slave wheel so as to pass, and controlling the speed of the slave wheel based on the speed and steering angle of the master wheels detected by the steering angle detecting means and the speed detecting means. Therefore, the steering angle of the slave wheel is controlled based on the measured value of the steering angle of both master wheels, and the intersections of the axes of the wheels are misaligned. Also increases the traveling direction of the interference, a problem such as an overload on each wheel of the motor and the axle body it takes to vibration can be prevented. Furthermore, since the travel locus of the vehicle body is estimated using only two master wheels, and the control for approaching the predetermined travel route is also performed on the two master wheels, the difference in the response of the steering motor of each wheel. Even if a disturbance or disturbance increases, the center of rotation of the vehicle body can always be determined as one point, and accurate estimation can be performed.
[Brief description of the drawings]
FIG. 1 is a schematic diagram showing a schematic configuration of a control device A1 for an unmanned traveling vehicle according to an embodiment of the present invention.
FIG. 2 is a flowchart showing a schematic processing procedure of a control method for an unmanned traveling vehicle according to an embodiment of the present invention.
FIG. 3 is an explanatory diagram of a control procedure of the unmanned traveling vehicle B1 by the control device A1.
FIG. 4 is an explanatory diagram of a control procedure of the unmanned traveling vehicle B1 by the control device A1.
FIG. 5 is an explanatory diagram of a method for calculating an estimated position of the unmanned traveling vehicle B1.
FIG. 6 is an explanatory diagram of a traveling control method for a conventional unmanned traveling vehicle B0.
FIG. 7 is an explanatory diagram of a traveling control method for a conventional unmanned traveling vehicle B0.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 ... Steering angle detection sensor 2 of wheel W1 ... Movement speed detection sensor 3 of wheel W1 ... Steering angle detection sensor 4 of wheel W2 ... Movement speed detection sensor 5 of wheel W2 ... Slave wheel control part 6 ... Estimation part 7 ... Map information Storage unit 8 ... Master wheel control unit 9 ... Wheel W1 steering motor controller 10 ... Wheel W1 drive motor controller 11 ... Wheel W2 steering motor controller 12 ... Wheel W2 drive motor controller 13 ... Wheel W3 steering motor controller 14 ... Drive motor controller B1 for the wheel W3 ... unmanned vehicles W1, W2 ... master wheel W3 ... slave wheel

Claims

それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御方法において，
上記２つのマスタ輪の操舵角を検出する操舵角検出工程と，
上記操舵角検出工程で検出された上記マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御工程と，
上記２つのマスタ輪の速度を検出する速度検出工程と，
上記操舵角検出工程及び上記速度検出工程で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定工程と，
上記推定工程で推定された車体の走行軌跡と上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御工程とを具備し，
上記スレーブ輪制御工程において，上記操舵角検出工程で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出工程及び上記速度検出工程で検出された上記マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御方法。An unmanned vehicle having two master wheels that can independently control the speed and steering angle and one or more slave wheels that can independently control the speed and steering angle, In a control method of an unmanned vehicle that travels along
A steering angle detection step of detecting the steering angle of the two master wheels;
A slave wheel control step of controlling the steering angle of the slave wheel based on the steering angle of the master wheel detected in the steering angle detection step;
A speed detection step for detecting the speed of the two master wheels;
An estimation step of estimating a travel locus of the vehicle body based on the speed and steering angle of both master wheels detected in the steering angle detection step and the speed detection step;
A travel control step for controlling the speed and steering angle of the master wheel based on the travel locus of the vehicle body estimated in the estimation step and the predetermined travel route;
In the slave wheel control step, the intersection of the axes of the master wheels is obtained from the steering angle of the master wheels detected in the steering angle detection step, and the slave wheel passes through the intersection so that the axis of the slave wheels passes through the intersection. The unmanned travel characterized by controlling the steering angle of the wheel and controlling the speed of the slave wheel based on the speed and steering angle of the master wheel detected in the steering angle detection step and the speed detection step. Car control method.

上記スレーブ輪制御工程が，
上記操舵角検出工程で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，
上記速度検出工程で検出された上記両マスタ輪の速度と，上記交点から各車輪までの距離の比とを用いて，上記スレーブ輪の速度を制御するものである請求項１記載の無人走行車の制御方法。The slave wheel control process
From the steering angle of the two master wheels detected in the steering angle detection step, the intersection of the axes of the two master wheels is obtained,
2. The unmanned traveling vehicle according to claim 1, wherein the speed of the slave wheel is controlled using the speed of the two master wheels detected in the speed detecting step and the ratio of the distance from the intersection to each wheel. Control method.

上記推定工程が，
上記両マスタ輪の軸線の交点より車体の回転中心位置を求め，
上記回転中心位置と上記両マスタ輪との距離，及び上記両マスタ輪の速度を用いて，車体座標系における車体の並進速度，回転速度，及び進行方向を求め，
上記車体座標系における車体の並進速度，回転速度，及び進行方向と，前回の制御時における車体の推定位置・姿勢・進行方向とに基づいて，現在の車体の推定位置・姿勢・進行方向を求めるものである請求項１又は２記載の無人走行車の制御方法。The estimation process is
Find the center of rotation of the car body from the intersection of the axes of the two master wheels.
Using the distance between the center of rotation and the two master wheels and the speed of the two master wheels, the translation speed, rotational speed, and traveling direction of the vehicle body in the vehicle coordinate system are obtained,
Based on the translation speed, rotation speed, and travel direction of the vehicle body in the vehicle body coordinate system, and the estimated position, posture, and travel direction of the vehicle body at the previous control, the current estimated position / posture / travel direction of the vehicle body is obtained. The method for controlling an unmanned traveling vehicle according to claim 1 or 2, wherein

それぞれ独立して速度及び操舵角の制御が可能な２つのマスタ輪と，それぞれ独立して速度及び操舵角の制御が可能な１つ以上のスレーブ輪とを有する無人走行車を，所定の走行経路に沿って走行させる無人走行車の制御装置において，
上記所定の走行経路の情報が予め記憶された地図情報記憶手段と，
上記２つのマスタ輪の操舵角を検出する操舵角検出手段と，
上記操舵角検出手段で検出された上記両マスタ輪の操舵角に基づいて，上記スレーブ輪の操舵角を制御するスレーブ輪制御手段と，
上記２つのマスタ輪の速度を検出する速度検出手段と，
上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，車体の走行軌跡を推定する推定手段と，
上記推定手段で推定された車体の走行軌跡と上記地図情報記憶手段に記憶された上記所定の走行経路とに基づいて，上記マスタ輪の速度及び操舵角を制御する走行制御手段とを具備し，
上記スレーブ輪制御手段が，上記操舵角検出手段で検出された上記両マスタ輪の操舵角より，該両マスタ輪の軸線の交点を求め，上記スレーブ輪の軸線が上記交点を通るように上記スレーブ輪の操舵角を制御すると共に，上記操舵角検出手段及び上記速度検出手段で検出された上記両マスタ輪の速度及び操舵角に基づいて，上記スレーブ輪の速度を制御することを特徴とする無人走行車の制御装置。An unmanned vehicle having two master wheels that can independently control the speed and steering angle and one or more slave wheels that can independently control the speed and steering angle, In a control device for an unmanned vehicle that travels along
Map information storage means in which information on the predetermined travel route is stored in advance;
Steering angle detection means for detecting the steering angle of the two master wheels;
Slave wheel control means for controlling the steering angle of the slave wheel based on the steering angles of the master wheels detected by the steering angle detection means;
Speed detecting means for detecting the speed of the two master wheels;
Estimating means for estimating the travel locus of the vehicle body based on the speed and steering angle of the two master wheels detected by the steering angle detecting means and the speed detecting means;
Travel control means for controlling the speed and steering angle of the master wheel based on the travel locus of the vehicle body estimated by the estimation means and the predetermined travel route stored in the map information storage means;
The slave wheel control means obtains the intersection of the axes of the two master wheels from the steering angle of the two master wheels detected by the steering angle detection means, and the slave wheel passes through the intersection so that the axis of the slave wheels passes through the intersection. An unmanned system that controls the steering angle of the wheels and controls the speed of the slave wheels based on the speed and steering angle of the master wheels detected by the steering angle detection means and the speed detection means. Control device for traveling vehicles.