JP3025604B2

JP3025604B2 - 無人作業車の操舵制御方法

Info

Publication number: JP3025604B2
Application number: JP5169713A
Authority: JP
Inventors: 重裕山本; 敏弘鈴木; 一正古倉
Original assignee: 日本輸送機株式会社
Priority date: 1993-06-15
Filing date: 1993-06-15
Publication date: 2000-03-27
Anticipated expiration: 2015-03-27
Also published as: JPH075922A

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、床面掃除等を行う無人
作業車の操舵制御方法に関する。

【０００２】

【従来の技術】この種の無人作業車の操舵制御方法にお
いては、特に、車体左右に存在する障害物に対して、車
体と障害物との間隔を少なくとも車体の方向転換がいつ
でも可能な状態に保つよう操舵制御することが望まれ
る。これを実現すべく、車体の左右に向けて超音波距離
センサや光センサ等の非接触式距離センサを設置し、左
右の障害物との距離を検出し、この検出値と所定の間隔
値との差に応じて車体の走行経路を平行移動させて障害
物との間隔を保とうとした操舵方法が提案されている。

【０００３】この操舵方法を図１０、図１１を用いて説
明する。図１１に示すように、車体１が進行方向Ａから
進行方向Ｂに方向転換する時、車体１のコーナーの軌跡
は円弧を描くので、車体１と障害物２との間には所定間
隔Ｄが必要となる。なお、３は距離センサ、４は車輪で
ある。図１０は所定の走行経路５の側方に障害物２が存
在した場合の操舵状況を示す。車体１と障害物２との間
隔のセンサ検出値ｄ１が所定間隔Ｄより小さい時、（Ｄ
−ｄ１）だけ走行経路を移動させる。新たな走行経路と
障害物２との間隔の検出値ｄ２が、再び所定間隔Ｄより
小さくなれば更に走行経路を移動させる。検出値ｄ３が
所定間隔Ｄ以上であれば、そのまま走行する。以下、同
様な動作を繰り返す。障害物２がなくなって検出値ｄ４
が（Ｄ＋α）より大きくなれば（αは横変位の発生量よ
り若干大きな値に選ぶ）、走行距離Ｌの計測を開始し、
ＬがＬａ（ほぼ車体長さ）より大きくなれば、走行経路
をそれ以前の横移動量（Ｄ−ｄ１）＋（Ｄ−ｄ２）だけ
元に戻し、車体１を所定の走行経路５に戻す。

【０００４】

【発明が解決しようとする課題】ところが、この操舵方
法は、走行経路を平行移動させるため、斜めの障害物２
に対しては、図１２に示すように、すぐに障害物に接近
してしまい、不安定な動作となり、方向転換が可能な間
隔を保って走行するという効果がなかった。本発明は、
上記問題を解消するもので、車体左右の障害物に対して
常に所望の間隔を保って走行することができる無人作業
車の操舵制御方法を提供することを目的とする。

【０００５】

【課題を解決するための手段】上記目的を達成するため
に本発明は、走行経路上での車体位置を演算するステッ
プと、上記の演算により得られた車体位置に応じて操舵
制御を行うための操舵機構における信号のフィードバッ
ク量を演算するステップと、障害物と車体との距離の検
出値と所望の間隔値との大きさを比較するステップと、
上記の比較結果に応じてフィードバック量のオフセット
値を演算するステップと、上記の各演算により得られた
フィードバック量及びそのオフセット値を用いて操舵量
を演算するステップとを有し、無人作業車の車体左右に
存在する障害物との距離を検出して、車体と障害物との
間隔を所望値に保つように操舵する操舵制御方法であっ
て、上記フィードバック量のオフセット値を演算するス
テップにおいて、下記の処理を行うものである。Ｆ＝Ｇ
_l・ｌⁱ＋Ｇψ・ψⁱ 車体左のセンサ検出値については、Ｆ≧Ｇ_l・（Ｄ−ｄ）であれば、Ｓ₀＝０Ｆ＜Ｇ_l・（Ｄ−ｄ）であれば、Ｓ₀＝Ｆ−Ｇ_l・（Ｄ−ｄ）車体右のセンサ検出値については、Ｆ≦−Ｇ_l・（Ｄ−ｄ）であれば、Ｓ₀＝０Ｆ＞Ｇ_l・（Ｄ−ｄ）であれば、Ｓ₀＝Ｆ＋Ｇ_l・（Ｄ−ｄ）ただし、Ｆ＝操舵量Ｇ_l＝横変位フィードバックゲインｌⁱ＝横変位フィードバック量Ｇψ＝姿勢角フィードバックゲイン ψⁱ＝姿勢角フィードバック量Ｄ＝車体と障害物との間の必要とする間隔ｄ＝センサ検出値Ｓ₀＝フィードバック量オフセット値

【０００６】

【作用】上記の方法によれば、走行経路上での車体位置
を演算し、演算して得られた車体位置に応じて操舵制御
を行うためのフィードバック量を演算し、障害物と車体
との距離の検出値と所望の間隔値との大きさを比較し、
センサ検出値とその時の走行経路からの偏差によるフィ
ードバック量の他、姿勢角によるフィードバック量から
もオフセット値を演算し、上記の各演算により得られた
フィードバック量及びそのオフセット値を用いて操舵量
を演算し、これにより操舵制御を行う。

【０００７】

【実施例】以下，本発明の一実施例について図面を参照
して説明する。図１は操舵輪により操舵を行う場合の無
人作業車の操舵構成を示し、車体１は、（ａ）において
は駆動兼操舵輪と従動輪を備え、（ｂ）においては操舵
輪と駆動輪を備える。図２は左右駆動輪により操舵を行
う場合の無人作業車の操舵構成を示し、キャスタ輪と駆
動輪を備える。図３は、本実施例による左右の障害物を
回避するための操舵方法の手順を示す。この手順の各種
演算等を順に以下説明する。まず、下記（１）式を用い
た車体位置の演算（♯１）について説明する。下記
（１）式及び下記（２）式は、図１（ａ）、（ｂ）、図
２の車に共通する式である。図４は所定の走行経路を座
標軸ｘとした時の座標軸ｘ−ｙ上での車体位置を示す。
図４において、位置演算は下記（１）より得られる。

【０００８】

【数１】 Δｌ_L ⁱ ＝（ｐ_L ⁱ −ｐ_L ^i-1 ）・Ｄ_L ／Ｐ_L Δｌ_R ⁱ ＝（ｐ_R ⁱ −ｐ_R ^i-1 ）・Ｄ_R ／Ｐ_R Δθⁱ ＝（Δｌ_R ⁱ −Δｌ_L ⁱ ）／Ｗ Δｌⁱ ＝（Δｌ_L ⁱ ＋Δｌ_R ⁱ ）／２ｘⁱ ＝ｘ^i-1 ＋Δｌⁱ ・ｃｏｓ（θ^i-1 ＋Δθⁱ ／２）ｙⁱ ＝ｙ^i-1 ＋Δｌⁱ ・ｓｉｎ（θ^i-1 ＋Δθⁱ ／２） θⁱ ＝θ^i-1 ＋Δθⁱ …（１）

【０００９】|Ｐ_L ⁱまたはＰ_R ⁱ|≧Ｐ_limとなればエンコ
ーダのパルスカウンタを左右ともリセットし、次の演算
に用いるＰ_L ^i-1＝Ｐ_R ^i-1＝０とする。なお、上記（１）
式において、ｉ＝演算周期毎の値を示す添字、Ｐ_L，Ｐ_R＝左右各エンコーダの１回転当たりのパルス
数、Ｐ_L ⁱ，Ｐ_R ⁱ＝左右各エンコーダのパルス数積算値（前進
時加算、後進時加算）Ｐ_lim＝エンコーダパルスカウンタをリセットする条件Ｄ_L，Ｄ_R＝左右各車輪のエンコーダ１回転当たりのころ
がり距離（ｍｍ） Δｌ_L，Δｌ_R＝演算周期間の左右各車輪のころがり距離
（ｍｍ）Ｗ＝車輪のトレッド（ｍｍ） Δθⁱ＝演算周期間の車体の方向変化量（ｒａｄ） Δｌⁱ＝演算周期間の車体の移動距離（ｍｍ）ｘⁱ，ｙⁱ＝車体位置（ｍｍ） θⁱ＝車体方向（ｒａｄ） −π≦θⁱ≦πである。なお、角度値については全て反時計周りを正とする。

【００１０】次に、下記（２）式を用いた操舵制御にお
けるフィードバック量演算（＃２）について説明する。
図５は所定の走行経路を座標軸ｘとした時の座標軸ｘ−
ｙ上での車体位置を示し、直線経路走行中におけるフィ
ードバック量演算は次式（２）により得られる。

【００１１】

【数２】ｌⁱ＝ｙ_l ⁱ ψⁱ＝θ_l ⁱ・１８０／π（前進時） ψⁱ＝−θ_l ⁱ・１８０／π（後進時） φⁱ＝バーφⁱ−φ₀ （操舵輪による操舵を行う場合のみ） ωⁱ＝（Δθⁱ／Δｔⁱ）・（１８０／π）（前進時）＝−（Δθⁱ／Δｔⁱ）・（１８０／π）（後進時）…（２）

【００１２】なお、上記（２）式において、ｉ＝演算周期毎の値を示す添字、ｙ_l ⁱ＝車体位置（ｍｍ） θ_l ⁱ＝車体の方向（ｒａｄ） Δθⁱ＝演算周期間の車体の方向変化量（ｒａｄ） Δｔⁱ＝演算周期（ｓｅｃ）ｌⁱ＝横変位フィードバック量（ｍｍ） ψⁱ＝姿勢角フィードバック量（ｄｅｇ） φⁱ＝操舵角フィードバック量（ｄｅｇ）バーφⁱ＝操舵角検出値（ｄｅｇ） φ₀＝操舵角検出オフセット値（ｄｅｇ） ωⁱ＝角速度フィードバック量（ｄｅｇ／ｓｅｃ）なお、角度値については全て反時計周りを正とする。

【００１３】次に、センサ検出値ｄと所望間隔Ｄとを比
較し（♯１３）、その比較結果に応じて、オフセット値
Ｓｏを決め（♯４，♯７，♯１１）、さらに、下記
（３）又は（４）式を用いて操舵指令値を演算し（♯
５）、得られた操舵指令値に基づいて操舵出力する（♯
６）。オフセット値Ｓｏを決める手順は後述することと
し、先に、操舵指令値の演算につき説明する。まず、操
舵輪による操舵を行う場合を説明する。即ち、図１の
（ａ）及び（ｂ）の車の場合の操舵指令値は次の（３）
式で求める。この場合の、車体位置に応じて操舵制御を
行うための操舵機構における信号のフィードバック回路
構成を図６に示している。

【００１４】

【数３】バーＳⁱ ＝Ｇ_S ・（Ｇ_l ・ｌⁱ ＋Ｇψ・ψⁱ ＋Ｇφ・φⁱ ＋Ｇω・ωⁱ −Ｓ₀ ）Ｓⁱ ＝バーＳⁱ （バーＳⁱ ＜Ｓ_limの場合）Ｓⁱ ＝（バーＳⁱ ／｜バーＳⁱ ｜）・Ｓ_lim （バーＳⁱ ≧Ｓ_lim の場合） …（３）

【００１５】なお、上記（３）式において、ｉ＝演算周期毎の値を示す添字、Ｇ_s ＝総合フィードバックゲイン、Ｇ_l ＝横変位フィードバックゲイン、Ｇψ＝姿勢角フィードバックゲイン、Ｇφ＝操舵角フィードバックゲイン、Ｇω＝角速度フィードバックゲイン、ｌⁱ ＝横変位フィードバック量（ｍｍ）、 ψⁱ ＝姿勢角フィードバック量（ｄｅｇ）、 φⁱ ＝操舵角フィードバック量（ｄｅｇ）、 ωⁱ ＝角速度フィードバック量（ｄｅｇ／ｓｅｃ）、Ｓ₀ ＝フィードバック量オフセット値、Ｓⁱ ＝操舵指令値（操舵速度指令）、Ｓ_lim ＝操舵指令リミット値である。

【００１６】次に、左右駆動輪による操舵を行う場合を
下記に説明する。即ち、図２の車の場合の操舵指令値は
次の（４）式で求める。この場合の、車体位置に応じて
操舵制御を行うための操舵機構における信号のフィード
バック回路構成を図７に示している。

【００１７】

【数４】１／Ｒⁱ ＝（１／Ｇ_R ）・（Ｇ_l ・ｌⁱ ＋Ｇψ・ψⁱ ＋Ｇω・ωⁱ −Ｓ₀） ΔＴⁱ ＝Ｃ_v ・（Ｗ／２）・（１／Ｒⁱ ）・（Δｌⁱ ／Δｔⁱ ）Ｔ_L ⁱ ＝Ｔⁱ ＋ΔＴⁱ Ｔ_R ⁱ ＝Ｔⁱ −ΔＴⁱ …（４）

【００１８】なお、上記（４）式において、ｉ＝演算周期毎の値を示す添字、Ｇ_R ＝旋回半径の逆数フィードバックゲイン、Ｇ_l ＝横変位フィードバックゲイン、Ｇψ＝姿勢角フィードバックゲイン、Ｇω＝角速度フィードバックゲイン、ｌⁱ ＝横変位フィードバック量（ｍｍ）、 ψⁱ ＝姿勢角フィードバック量（ｄｅｇ）、 ωⁱ ＝角速度フィードバック量（ｄｅｇ／ｓｅｃ）、Ｓ₀ ＝フィードバック量オフセット値、１／Ｒ＝旋回半径の逆数目標値（１／ｍｍ）、Ｗ＝左右車輪のトレッド（ｍｍ）、 Δｌⁱ ＝演算周期間の車体の移動量（ｍｍ）…数式
（１）の演算値、 Δｔⁱ ＝演算周期（ｓｅｃ）、 Δｌⁱ ／Δｔⁱ で走行
速度、Ｃ_v ＝モータ回転速度指令値への変換定数、 ΔＴⁱ ＝操舵指令値（駆動モータ回転速度の増減値）、Ｔ_L ⁱ ，Ｔ_R ⁱ ＝左、右駆動モータ回転速度指令値であ
る。

【００１９】上記（３）又は（４）式により得られた操
舵量に基づいて操舵出力されるが、ここで、（３）又は
（４）式におけるフィードバックオフセット値Ｓ₀の決
定処理（♯３，♯４，♯７〜♯１２）を説明する。

【００２０】

【数５】Ｆ＝Ｇ_l ・ｌⁱ ＋Ｇψ・ψⁱ を求める。車体左のセンサ検出値については、Ｆ≧Ｇ_l ・（Ｄ−ｄ）であれば、Ｓ₀ ＝０Ｆ＜Ｇ_l ・（Ｄ−ｄ）であれば、Ｓ₀ ＝Ｆ−Ｇ_l ・（Ｄ−ｄ）とする。車体右のセンサ検出値については、Ｆ≦−Ｇ_l ・（Ｄ−ｄ）であれば、Ｓ₀ ＝０Ｆ＞−Ｇ_l ・（Ｄ−ｄ）であれば、Ｓ₀ ＝Ｆ＋Ｇ_l ・（Ｄ−ｄ）とする。 …（５）

【００２１】上記の繰り返し処理中に、センサ検出値ｄ
が、ｄ＞Ｄとなれば、オフセット値Ｓ₀はｄ＞Ｄとなる
前の値を維持する（♯７）。また、ｄ≧Ｄ＋αとなれ
ば、その時より走行距離Ｌの計測を開始し（♯１０）、
センサ検出値ｄが、ｄ≧Ｄ＋αを維持した状態での走行
距離が、Ｌ＞Ｌａとなれば（♯１１でＹＥＳ）、Ｓ₀＝
０とする（♯１２）。なお、ｄ≧Ｄ＋αでなくなれば、
走行距離Ｌをクリアする（♯９）。

【００２２】本実施例の操舵方法によれば、走行経路を
移動する必要がない。走行経路からの偏差等によるフィ
ードバック量（ｌⁱ，ψⁱ）がゼロであれば、必要な間隔
Ｄを得るために必要な操舵量Ｆは、必要な間隔Ｄとセン
サ検出値ｄとの差に横変位ゲインＧ_lを乗じ、Ｇ_l・（Ｄ
−ｄ）で定まる。しかし、実際には、走行経路からの偏
差等によるフィードバック量が発生するため、上記
（５）式では、これを考慮して必要な間隔Ｄを得るため
の操舵量Ｆを定めている。（５）式でオフセット値Ｓ₀
＝０となっている場合があるのは、走行経路５からのフ
ィードバック量のみで、必要な操舵量Ｆを発生し得る場
合であって、図９では、車体左のセンサで障害物がなく
なり、車体が所定の走行経路５に戻れる状態になった場
合である。

【００２３】図８、図９は、上記による操舵制御を実施
した場合の状況を示す。所定の走行経路（ｘ軸）からの
車体１の偏差がどのような形態で発生しても、必要な所
望間隔Ｄを得るための操舵量Ｆが定められているので、
図９に示すように、斜めの障害物２に対しても、安定し
て間隔Ｄを保つことができる。図９では、車体左のセン
サを用いた例であるが、車体右のセンサを用いても略同
様である。

【００２４】

【発明の効果】以上のように本発明によれば、フィード
バック操舵制御における操舵量を求める時に用いられる
オフセット値を、センサ検出値とその時の走行経路から
の偏差によるフィードバック量の他、姿勢角によるフィ
ードバック量からも設定するようにしているので、斜め
の障害物に対しても、車体左右の障害物との間隔をいつ
でも方向転換等が可能なような所望の値を保つことがで
き、従来のように走行経路を移動させる必要がなく、し
かも走行の安定が図れる。

【図面の簡単な説明】

【図１】本発明の一実施例による操舵輪により操舵を行
う場合の無人作業車の操舵構成を示す図である。

【図２】左右駆動輪により操舵を行う場合の無人作業車
の操舵構成を示す図である。

【図３】本実施例による障害物を回避するための操舵方
法の手順を示すフローチャートである。

【図４】所定の走行経路を座標軸とした時の車体位置を
示す図である。

【図５】所定の走行経路を座標軸とした時の車体位置を
示す図である。

【図６】操舵輪による操舵を行う場合の操舵機構におけ
る信号のフィードバック回路構成図である。

【図７】左右駆動輪による操舵を行う場合の操舵機構に
おける信号のフィードバック回路構成図である。

【図８】本実施例による操舵制御の状況を示す図であ
る。

【図９】本実施例による操舵制御の状況を示す図であ
る。

【図１０】従来方法による操舵制御の状況を示す図であ
る。

【図１１】車体が方向転換する時の状況を示す図であ
る。

【図１２】傾斜した障害物に対して従来方法により操舵
制御したときの状況を示す図である。

【符号の説明】

１車体２障害物３センサ

───────────────────────────────────────────────────── フロントページの続き (56)参考文献特開平３−174607（ＪＰ，Ａ) 特開平４−324505（ＪＰ，Ａ) 特開平４−260905（ＪＰ，Ａ) 特開平１−219907（ＪＰ，Ａ) 特開平４−160507（ＪＰ，Ａ) 実開昭63−151008（ＪＰ，Ｕ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05D 1/02

Claims

(57)【特許請求の範囲】

【請求項１】走行経路上での車体位置を演算するステ
ップと、上記の演算により得られた車体位置に応じて操舵制御を
行うための操舵機構における信号のフィードバック量を
演算するステップと、障害物と車体との距離の検出値と所望の間隔値との大き
さを比較するステップと、上記の比較結果に応じてフィードバック量のオフセット
値を演算するステップと、上記の各演算により得られたフィードバック量及びその
オフセット値を用いて操舵量を演算するステップとを有
し、無人作業車の車体左右に存在する障害物との距離を検出
して、車体と障害物との間隔を所望値に保つように操舵
する操舵制御方法であって、上記フィードバック量のオフセット値を演算するステッ
プにおいて、下記の処理を行うことを特徴とする無人作
業車の操舵制御方法。Ｆ＝Ｇ _l ・ｌ ⁱ ＋Ｇψ・ψ ⁱ 車体左のセンサ検出値については、Ｆ≧Ｇ _l ・（Ｄ−ｄ）であれば、Ｓ ₀ ＝０Ｆ＜Ｇ _l ・（Ｄ−ｄ）であれば、Ｓ ₀ ＝Ｆ−Ｇ _l ・（Ｄ−ｄ）車体右のセンサ検出値については、Ｆ≦−Ｇ _l ・（Ｄ−ｄ）であれば、Ｓ ₀ ＝０Ｆ＞Ｇ _l ・（Ｄ−ｄ）であれば、Ｓ ₀ ＝Ｆ＋Ｇ _l ・（Ｄ−ｄ）ただし、Ｆ＝操舵量Ｇ _l ＝横変位フィードバックゲインｌ ⁱ ＝横変位フィードバック量Ｇψ＝姿勢角フィードバックゲイン ψ ⁱ ＝姿勢角フィードバック量Ｄ＝車体と障害物との間の必要とする間隔ｄ＝センサ検出値Ｓ ₀ ＝フィードバック量オフセット値