JP4097844B2 - Moving body guidance device - Google Patents

Description

さらに座標軸ＸとＹは、座標軸ｘとｙとを座標変換したものであり、下記（２１）式で表される。
【０２４０】

また、上記（２１）式のθ2の方向ベクトルｃｏｓθ2、ｓｉｎθ2を求めるには、まず、移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）と目標地点Ｐ2（ｘ2、ｙ2）から下記（２２）式、（２３）式を用いてｃｏｓφt、ｓｉｎφtを求める。
【０２４１】
ｃｏｓφt＝（ｘ2−ｘ1）／｛（ｘ2−ｘ1）²＋（ｙ2−ｙ1）² ｝ … （２２）
ｓｉｎφt＝（ｙ2−ｙ1）／｛（ｘ2−ｘ1）²＋（ｙ2−ｙ1）² ｝ … （２３）
そして、これらｃｏｓφt、ｓｉｎφtから下記（２４）式、（２５）式を用いてθ2の方向ベクトルｃｏｓθ2、ｓｉｎθ2が求められる。
【０２４２】
ｃｏｓθ2＝（ｃｏｓ²φt−ｓｉｎ²φt）・ｃｏｓθt＋２ｓｉｎφt・ｃｏｓφt・ｓｉｎθt … （２４）
ｓｉｎθ2＝＋２ｓｉｎφt・ｃｏｓφt・ｃｏｓθt−（ｃｏｓ²φt−ｓｉｎ²φt）・ｓｉｎθt… （２５）
ここで、上記（１８）式、（１９）式を用いて求められた横方向偏移ηと、端点ＰR2（ＸR2、ＹR2）、ＰL2（ＸL2、ＹL2）間の距離ｄとの関係が−ｄ／２＜η＜ｄ／２である場合、走行条件選択手段４は移動車両Ｓが任意の幾何学図形Ｍ2内の端点ＰR2（ｘR2、ｙR2）、ＰL2（ｘL2、ｙL2）間を通るものと判断する。
【０２４３】
この走行条件選択手段４の判断結果に基づいて、操舵指令算出手段５は移動車両Ｓに対して操舵をそのまま維持するように操舵指令を出力する（ステップ１０７）。
【０２４４】
次に旋回走行している移動車両Ｓを操舵して任意の幾何学図形Ｍ2に向けて進行させる処理について説明する。
【０２４５】
移動車両Ｓが任意の幾何学図形Ｍ2を通過できない場合（ステップ１０５の判断ＮＯ）、走行条件選択手段４は、移動車両Ｓを操舵するためのデータを操舵指令算出手段５に対して出力する。
【０２４６】
走行条件選択手段４で行われる処理について図１４を参照して説明する。
【０２４７】
上述したように横方向偏移ηが、移動車両Ｓが現在の任意の幾何学図形Ｍ1を最初に検出してから所定の距離だけ進行し、かつ任意の幾何学図形Ｍ1内の所定の地点Ｐ1に到達した時点以降、上記（１８）、（１９）式（下記に示す）を用いて求められる。
【０２４８】
η＝Ｙ−ｒtｃｏｓΘt＋√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１８）
η＝Ｙ−ｒtｃｏｓΘt−√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１９）
ここで、上記（１８）式、（１９）式を用いて求められた横方向偏移ηが端点ＰR2（ＸR2、ＹR2）、ＰL2（ＸL2、ＹL2）間ｄに含まれない場合、横方向偏移ηと端点ＰR2、ＰL2間の距離ｄとの関係はη＜−ｄ／２、ｄ／２＜ηとなる。
【０２４９】
これにより走行条件選択手段４は移動車両Ｓが任意の幾何学図形Ｍ2内の端点ＰR2（ｘR2、ｙR2）、ＰL2（ｘL2、ｙL2）間を通ることができないと判断する。（ステップ１０４の判断ＮＯ）。
【０２５０】
そこで、走行条件選択手段４は、上述したように予定走行経路記憶手段１から取得した目標地点Ｐ2（ｘ2、ｙ2）のデータと、現在運動計測手段２から取得した移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向Θtのデータとを操舵指令算出手段５に出力する。
【０２５１】
操舵指令算出手段５は移動車両Ｓを操舵するための演算を行う（ステップ１０６）。
【０２５２】
ここで、図６、図１３を示して上述したように、操舵指令算出手段５は、取得した目標地点Ｐ2（ｘ2、ｙ2）、移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向Θtの各データより、移動車両Ｓを目標地点Ｐ2（ｘ2、ｙ2）に向かわせるための目標旋回半径ｒgを上記（８）式（下記に示す）を用いて求める。
【０２５３】
ｒg＝｛（ｘ2−ｘ1）²＋（ｙ2−ｙ1）²｝／｛（ｘ2−ｘ1）・ｓｉｎΘt−（ｙ2−ｙ1）・ｃｏｓΘt｝／２ … （８）
そして、上記（１０）式（下記に示す）より目標旋回半径ｒgのときの目標操舵角度ζを求める。
【０２５４】
ζ＝ｔａｎ^-1（Ｌ／ｒg） … （１０）
こうして求めた目標操舵角度ζを、操舵指令算出手段５は移動車両Ｓ（車体７）に対して操舵指令として出力して移動車両Ｓの操舵角の制御を行う。
【０２５５】
これにより、旋回走行している移動車両Ｓを次の任意の幾何学図形Ｍ2に向かわせることができる。
【０２５６】
このように、上述した実施形態では、移動体Ｓが任意の幾何学図形Ｍ1を最初に検出してから所定の距離だけ進行し、かつ任意の幾何学図形Ｍ1内の所定の地点Ｐ1に到達した時点で、次の任意の幾何学図形Ｍ2に移動体Ｓを向かわせるようにしているので、移動体Ｓの予定走行経路Ｋ上の目標が一対の端点でなく任意の幾何学図形であっても、一対の端点の場合と同様に小回り現象を発生させることなく移動体Ｓを誘導することができる。
【０２５７】
また、この実施形態では、一対の端点の場合と同様に移動車両Ｓの前進時の操舵角ζと後退時の操舵角ζとを同一にすることができる。
【０２５８】
図１５（ａ）は任意の幾何学図形Ｍnの目標地点Ｐnに向けて移動車両Ｓ0を前進させる場合を示す図であり、同図９（ｂ）は任意の幾何学図形Ｍnの目標地点Ｐnに向けて移動車両Ｓ0を後退させる場合を示す図である。
【０２５９】
同図１５（ａ）、（ｂ）に示すように、移動車両Ｓ0の操舵角ζは前進時と後退時で同一にされている。
【０２６０】
従って、この実施形態では、一対の端点の場合と同様に移動車両Ｓの前進時と後退時の制御特性を同一にすることができる。また、操舵角ζの制御を簡易にすることができる。
【０２６１】
さて、上述した実施形態では、予定走行経路Ｋ上の目標データは任意の幾何学図形として説明したが、当然目標データは円形であってもよい。
【０２６２】
そこで、予定走行経路Ｋ上の目標データを円形とした場合の、図２の移動車両の誘導装置の動作について図３、図１６を参照して説明する。
【０２６３】
図１６は予定走行経路Ｋの目標データを円Ｃ1とした場合の実施形態を示す図である。
【０２６４】
同図１６では円形Ｃ1が予定走行経路Ｋ上の目標とされている。
【０２６５】
なお、図３のフローチャートのステップ１０１〜１０３で行われる処理は、上述した予定走行経路Ｋ上の目標データが一対の端点あるいは任意の幾何学図形である場合の処理と同様なので説明を省略する。
【０２６６】
まず直進している移動車両Ｓを操舵せずにそのまま円Ｃ2に向けて進行させる処理について説明する。
【０２６７】
ステップ１０４では、直進している移動車両Ｓが現在の円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の地点Ｐ1に到達した時点以降、次の円Ｃ2内を通過できるか否かを判定する。
【０２６８】
このときステップ１０４では、上述したように図２の走行条件選択手段４が移動車両Ｓの操舵を行うか否かを判断する。
【０２６９】
ここで移動車両Ｓが操舵しなくてもそのまま円Ｃ2を通過できる場合（ステップ１０４の判断ＹＥＳ）、走行条件選択手段４は、移動車両Ｓをそのまま進行させるようにするデータを操舵指令算出手段５に対して出力する（ステップ１０７）。
【０２７０】
直進している移動車両Ｓを操舵せずにそのまま円Ｃ2内に向けて進行させる走行条件選択手段４の処理について図１７を参照して説明する。
【０２７１】
図１７において、移動車両Ｓが直進する際の軌跡となる直線の方程式は、上記（４）式（下記に示す）で表される。
【０２７２】
（ｘ−ｘ1）・ｔａｎθt−（ｙ−ｙ1）＝０ … （４）
また、中心点Ｐ2（ｘ2、ｙ2）、半径ｄ／２の円Ｃ2の方程式は、下記（２６）式で表される。
【０２７３】
（ｘ−ｘ2）²＋（ｙ−ｙ2）²＝（ｄ／２）² … （２６）
そこで、上記（４）式と（２６）式とを連立方程式とみて、この連立方程式の解が求められる。
【０２７４】
従って、この連立方程式の解が実数解であれば、上記（４）式の直線と円Ｃ2とが交差することがわかる。
【０２７５】
これにより、走行条件選択手段４は、移動車両Ｓは操舵しなくてもそのまま円Ｃ2内を通過できると判断する（ステップ１０４の判断ＹＥＳ）。
【０２７６】
この走行条件選択手段４の判断結果に基づいて、操舵指令算出手段５は移動車両Ｓに対して操舵をそのまま維持するように操舵指令を出力する（ステップ１０７）。
【０２７７】
次に直進している移動車両Ｓを操舵して円Ｃ2に向けて進行させる処理について説明する。
【０２７８】
移動車両Ｓが円Ｃ2内を通過できない場合（ステップ１０４の判断ＮＯ）、走行条件選択手段４は、移動車両Ｓを操舵するためのデータを操舵指令算出手段５に対して出力する。
【０２７９】
走行条件選択手段４で行われる処理について図１７を参照して説明する。
【０２８０】
上述した図１７に示すように走行条件選択手段４は、移動車両Ｓが現在の円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の地点Ｐ1に到達したか否かを確認する。
【０２８１】
ここで移動車両Ｓを操舵して円Ｃ2に向かわせる場合（ステップ１０４の判断ＮＯ）、走行条件選択手段４は、円Ｃ2に対する現在の移動車両Ｓの相対位置を操舵指令算出手段５に対して出力する（ステップ１０７）。
【０２８２】
直進している移動車両Ｓを操舵して円Ｃ2内に向けて進行させる走行条件選択手段４の処理について図１７を参照して説明する。
【０２８３】
図１７において、上記（４）式と（２６）式（下記に示す）との連立方程式の解が求められる。
【０２８４】
（ｘ−ｘ1）・ｔａｎθt−（ｙ−ｙ1）＝０ … （４）
（ｘ−ｘ2）²＋（ｙ−ｙ2）²＝（ｄ／２）² … （２６）
ここで、この連立方程式の解が実数解でなければ、上記（４）式の直線と上記（２６）式の円Ｃ2とが交差しないことがわかる。
【０２８５】
これにより、走行条件選択手段４は、移動車両Ｓはこの状態では円Ｃ2内を通過できないと判断する（ステップ１０４の判断ＮＯ）。
【０２８６】
そこで、走行条件選択手段４は、上述したように予定走行経路記憶手段１から取得した目標地点Ｐ2（ｘ2、ｙ2）のデータと、現在運動計測手段２から取得した移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向θtのデータとを操舵指令算出手段５に出力する。
【０２８７】
操舵指令算出手段５は移動車両Ｓを操舵するための演算を行う（ステップ１０６）。
【０２８８】
ここで、図６、図１３を示して上述したように、操舵指令算出手段５は、取得した目標地点Ｐ2（ｘ2、ｙ2）、移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向θtの各データより、移動車両Ｓを目標地点Ｐ2（ｘ2、ｙ2）に向かわせるための目標旋回半径ｒgを上記（８）式（下記に示す）を用いて求める。
【０２８９】
ｒg＝｛（ｘ2−ｘ1）²＋（ｙ2−ｙ1）²｝／｛（ｘ2−ｘ1）・ｓｉｎθt−（ｙ2−ｙ1）・ｃｏｓθt｝／２ … （８）
そして、上記（１０）式（下記に示す）より目標旋回半径ｒgのときの目標操舵角度ζを求める。
【０２９０】
ζ＝ｔａｎ^-1（Ｌ／ｒg） … （１０）
こうして求めた目標操舵角度ζを、操舵指令算出手段５は移動車両Ｓ（車体７）に対して操舵指令として出力して移動車両Ｓの操舵角の制御を行う。
【０２９１】
これにより、直進している移動車両Ｓを次の円Ｃ2に向かわせることができる。
【０２９２】
次に旋回走行している移動車両Ｓを操舵せずにそのまま円Ｃ2に向けて進行させる処理について説明する。
【０２９３】
ステップ１０５では、旋回走行している移動車両Ｓが現在の円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の地点Ｐ1に到達したか否かを判定する。
【０２９４】
つまりステップ１０５では、図２の走行条件選択手段４が移動車両Ｓの操舵を行うか否かを判断する。
【０２９５】
ここで移動車両Ｓが操舵しなくてもそのまま円Ｃ2に向かわせることができる場合（ステップ１０５の判断ＹＥＳ）、走行条件選択手段４は、移動車両Ｓをそのまま進行させるようにするデータを操舵指令算出手段５に対して出力する（ステップ１０７）。
【０２９６】
走行条件選択手段４で行われる処理について図１８を参照して説明する。
【０２９７】
図１８において移動車両Ｓは進行方向Θt、旋回半径ｒtで旋回走行して、中心点Ｐ2（Ｘ2、Ｙ2）、半径ｄ／２の円Ｃ2内に向かわされているものとする。
さらに、移動車両Ｓが円Ｃ2の直径ｄを通過する時点での中心点Ｐ2からの距離を横方向偏移ηとする。
【０２９８】
この横方向偏移ηは、移動車両Ｓが現在の円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の目標地点Ｐ1に到達した時点以降、上記（１８）、（１９）式（下記に示す）を用いて求められる。
【０２９９】
つまり、現在運動計測手段２によって求められた旋回半径ｒtと、移動車両Ｓの進行方向Θtとから、旋回半径ｒtが０より大きい場合には、
η＝Ｙ−ｒtｃｏｓΘt＋√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１８）
と求められ、また旋回半径ｒtが０より小さい場合には、
η＝Ｙ−ｒtｃｏｓΘt−√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１９）
と求められる。
【０３００】
ここで、上記（１８）式、（１９）式を用いて求められた横方向偏移ηと、円Ｃ2の半径ｄ／２との関係が−ｄ／２＜η＜ｄ／２である場合、走行条件選択手段４は移動車両Ｓが円Ｃ2内を通るものと判断する。
【０３０１】
この走行条件選択手段４の判断結果に基づいて、操舵指令算出手段５は移動車両Ｓに対して操舵をそのまま維持するように操舵指令を出力する（ステップ１０７）。
【０３０２】
次に旋回走行している移動車両Ｓを操舵して円Ｃ2に向けて進行させる処理について説明する。
【０３０３】
移動車両Ｓが円Ｍ2を通過できない場合（ステップ１０５の判断ＮＯ）、走行条件選択手段４は、移動車両Ｓを操舵するためのデータを操舵指令算出手段５に対して出力する。
【０３０４】
走行条件選択手段４で行われる処理について図１４を参照して説明する。
【０３０５】
上述したように横方向偏移ηが、移動車両Ｓが現在の円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の地点Ｐ1に到達した時点以降、上記（１８）、（１９）（下記に示す）を用いて求められる。
【０３０６】
η＝Ｙ−ｒtｃｏｓΘt＋√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１８）
η＝Ｙ−ｒtｃｏｓΘt−√｛ｒt²−（Ｘ＋ｒtｓｉｎΘt））²｝ … （１９）
ここで、上記（１８）式、（１９）式を用いて求められた横方向偏移ηが円Ｃ2の直径ｄに含まれない場合、横方向偏移ηと円Ｃ2の半径ｄ／２との関係はη＜−ｄ／２、ｄ／２＜ηとなる。
【０３０７】
これにより走行条件選択手段４は移動車両Ｓが円Ｃ2内を通ることができないと判断する。（ステップ１０４の判断ＮＯ）。
【０３０８】
そこで、走行条件選択手段４は、上述したように予定走行経路記憶手段１から取得した目標地点Ｐ2（ｘ2、ｙ2）のデータと、現在運動計測手段２から取得した移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向Θtのデータとを操舵指令算出手段５に出力する。
【０３０９】
操舵指令算出手段５は移動車両Ｓを操舵するための演算を行う（ステップ１０６）。
【０３１０】
ここで、図６、図１７を示して上述したように、操舵指令算出手段５は、取得した目標地点Ｐ2（ｘ2、ｙ2）、移動車両Ｓの現在位置Ｐ1（ｘ1、ｙ1）、進行方向Θtの各データより、移動車両Ｓを目標地点Ｐ2（ｘ2、ｙ2）に向かわせるための目標旋回半径ｒgを上記（８）式（下記に示す）を用いて求める。
【０３１１】
ｒg＝｛（ｘ2−ｘ1）²＋（ｙ2−ｙ1）²｝／｛（ｘ2−ｘ1）・ｓｉｎΘt−（ｙ2−ｙ1）・ｃｏｓΘt｝／２ … （８）
そして、上記（１０）式（下記に示す）より目標旋回半径ｒgのときの目標操舵角度ζを求める。
【０３１２】
ζ＝ｔａｎ^-1（Ｌ／ｒg） … （１０）
こうして求めた目標操舵角度ζを、操舵指令算出手段５は移動車両Ｓ（車体７）に対して操舵指令として出力して移動車両Ｓの操舵角の制御を行う。
【０３１３】
これにより、旋回走行している移動車両Ｓを円Ｃ2に向かわせることができる。
【０３１４】
このように、上述した実施形態では、移動体Ｓが円Ｃ1を最初に検出してから所定の距離だけ進行し、かつ円Ｃ1内の所定の地点Ｐ1に到達した時点で、次の円Ｃ2に移動体Ｓを向かわせるようにしているので、移動体Ｓの予定走行経路Ｋ上の目標が一対の端点が円であっても、一対の端点の場合と同様に小回り現象を発生させることなく移動体Ｓを誘導することができる。
【０３１５】
また、この実施形態では、一対の端点の場合と同様に移動車両Ｓの前進時の操舵角ζと後退時の操舵角ζとを同一にすることができる。
【０３１６】
図１９（ａ）は円Ｃn内の目標地点（円Ｃnの中心点）Ｐnに向けて移動車両Ｓ0を前進させる場合を示す図であり、同図１９（ｂ）は円Ｃnの目標地点Ｐnに向けて移動車両Ｓ0を後退させる場合を示す図である。
【０３１７】
同図１９（ａ）、（ｂ）に示すように、移動車両Ｓ0の操舵角ζは前進時と後退時で同一にされている。
【０３１８】
従って、この実施形態では、一対の端点の場合と同様に移動車両Ｓ0の前進時と後退時の制御特性を同一にすることができる。また、操舵角ζの制御を簡易にすることができる。
【０３１９】
次に予定走行経路Ｋ上の目標データの生成について説明する。
【０３２０】
例えば、点Ｐn（ｘn、ｙn）、及びこの点Ｐn（ｘn、ｙn）を中心とする半径ｄn／２の円から成る点列データから一対の端点の目標データを生成するものとする。
【０３２１】
なお、ここでは一対の端点をゲートとして説明する。
【０３２２】
また、点列データは上述したようにＣＡＤ（Computer Automation Design）やティーチングプレイバックによって作成される。
【０３２３】
そして、点列データからゲートに変換されたデータは目標データとして予定走行経路記憶手段１に記憶されている。
【０３２４】
図２０（ａ）は、点列データの密度が予定経路Ｋの曲率に比べて十分密である場合を示す図であり、同図（ｂ）は、点列データの密度が予定経路Ｋの曲率に比べて疎となる場合を示す図である。
【０３２５】
同図２０（ｂ）の走行経路Ｋa2に示すように、点列データの密度が予定経路Ｋの曲率に比べて疎になると、予定走行経路Ｋの曲線が再現できなくなる。
【０３２６】
従って、予定走行経路Ｋ上の目標データの生成について説明するにあたっては、同図２０（ａ）に示すように、目標データが生成される点列データの密度が予定経路Ｋの曲率に比べて十分密であるものとする。
【０３２７】
また、目標データが生成される点列データの密度は、隣接する３つの目標地点を円弧で結んだときに、走行経路Ｋa1に示すように予定走行経路Ｋの曲線を十分再現できる程度とする。
【０３２８】
そこで、隣接する３つの目標地点が一直線上に並んでいる点列データを想定し、移動車両Ｓが初めの目標地点Ｐ1の周辺を通過して次の目標地点Ｐ2に向かう際に、目標地点Ｐ2から目標ゲートＧ2を生成する場合について図２１を参照して説明する。
【０３２９】
図２１は３つの目標地点が一直線上に並んでいる点列データを示す図である。
【０３３０】
同図２１に示すように、３点の目標地点が一直線上に並んでいる場合、目標地点Ｐ1からＰ2への方向ベクトル、およびＰ1からＰ3への方向ベクトルは同一になる。
【０３３１】
よって、これらの方向ベクトルの関係は下記（２７）式で表される。
【０３３２】
（ｘ3−ｘ1）／（ｙ3−ｙ1）＝（ｘ2−ｘ1）／（ｙ2−ｙ1） … （２７）
さらに、上記（２７）式を変形すると、下記（２８）式が得られる。
【０３３３】
（ｘ3−ｘ1）・（ｙ2−ｙ1）−（ｘ2−ｘ1）・（ｙ3−ｙ1）＝０ … （２８）
ここで、上記（２８）式が成立している場合、目標地点Ｐ1、Ｐ2、Ｐ3の３点は一直線上に存在する。
【０３３４】
また、目標地点Ｐ2から生成される目標ゲートＧ2の方向ベクトルは目標地点Ｐ1（ｘ1、ｙ1）からＰ2（ｘ2、ｙ2）への方向ベクトルと直交する。よって目標ゲートＧ2の傾きθ2は下記（２９）式で表される。
【０３３５】

従って、予定走行経路Ｋが直線の場合に目標地点Ｐ2から生成される目標ゲートＧ2は、中心点Ｐ2（ｘ2、ｙ2）、幅ｄ2（目標地点Ｐ2（ｘ2、ｙ2）を中心とする円の直径）、及び上記（２９）式で表される方向ベクトル（ｃｏｓθ2、ｓｉｎθ2）によって設定することができる。
【０３３６】
ところで、上記（２８）式（下記に示す））が成立しない場合、目標地点Ｐ1、Ｐ2、Ｐ3の３点は直線上にないから、これら目標地点Ｐ1、Ｐ2、Ｐ3を通過するために移動車両Ｓは例えば円弧を描きながら旋回動作を行わねばならない。
【０３３７】
（ｘ3−ｘ1）・（ｙ2−ｙ1）−（ｘ2−ｘ1）・（ｙ3−ｙ1）＝０ … （２８）
図２２は３つの目標地点Ｐ1、Ｐ2、Ｐ3が、移動車両Ｓの旋回円の円周上に並んでいる点列データを示す図である。
【０３３８】
この場合、同図２２に示すように予定走行経路Ｋは曲線となる。
【０３３９】
そこで、次に隣接する３つの目標地点が移動車両Ｓの旋回円の円周上に並んでいる点列データを想定し、移動車両Ｓが初めの目標地点Ｐ1の周辺を通過して次の目標地点Ｐ2に向かう際に、目標地点Ｐ2から目標ゲートＧ2を生成する場合について図２３、図２４を参照して説明する。
【０３４０】
図２３は３つの目標地点Ｐ1、Ｐ2、Ｐ3を通る円と目標ゲートの傾きを示す図である。
【０３４１】
ここで、目標地点Ｐ2から生成される目標ゲートＧ2の方向ベクトル（ｃｏｓθ2、ｓｉｎθ2）は、同図２３の円の中心点Ｐ0（ｘ0、ｙ0）から目標ゲートＧ2の中心点Ｐ2（ｘ2、ｙ2）（目標地点Ｐ2）に向いたベクトルと等しい。
【０３４２】
よって、目標ゲートＧ2の方向ベクトル（ｃｏｓθ2、ｓｉｎθ2）は、下記（３０）式で表される。
【０３４３】

また、目標ゲートＧ2の中心点Ｐ2（ｘ2、ｙ2）を通り、中心点Ｐ0（ｘ0、ｙ0）の円の半径ｒ0は下記（３１）式で表される。
【０３４４】
ｒ0＝√｛（ｘ2−ｘ0）²＋（ｙ2−ｙ0）² ｝ … （３１）
ここで、円の中心点Ｐ0（ｘ0、ｙ0）の設定について説明する。
【０３４５】
図２４は３つの目標地点Ｐ1、Ｐ2、Ｐ3を通る円の中心点Ｐ0（ｘ0、ｙ0）の設定について説明する図である。
【０３４６】
中心点Ｐ0（ｘ0、ｙ0）は、目標地点Ｐ1、Ｐ2を結ぶ線を垂直に２等分する直線
Ｔと、目標地点Ｐ2、Ｐ3を結ぶ線を垂直に２等分する直線Ｓとの交点である。
【０３４７】
ここで、直線Ｔは下記（３２）式、一方、直線Ｓは下記（３３）式で表される。
【０３４８】

従って、これら直線Ｔ、Ｓの交点となる中心点Ｐ0（ｘ0、ｙ0）の座標は下記（３４）式で表される。
【０３４９】

但し、
ｔ0＝｛（−ｘb＋ｘd）・ｙc−（−ｙb＋ｙd）・ｘc｝／（ｘa・ｙc−ｙa・ｘc）
… （３５）

従って、隣接する３つの目標地点Ｐ1、Ｐ2、Ｐ3が円周上に存在する場合に目標地点Ｐ2から生成される目標ゲートＧ2は、中心点Ｐ2（ｘ2、ｙ2）、幅ｄ2（目標地点Ｐ2（ｘ2、ｙ2）を中心とする円の直径）、及び上記（３０）式で表される方向ベクトル（ｃｏｓθ2、ｓｉｎθ2）によって設定することができる。また、目標ゲートＧ2の中心点Ｐ2（ｘ2、ｙ2）を通り、中心点Ｐ0（ｘ0、ｙ0）の円の半径ｒ0は上記（３１）式で表することができる。
【０３５０】
以上説明したように、本実施形態では、移動車両Ｓの前方の予定走行経路Ｋが曲線の場合について、点列データから目標ゲートを生成するようにしているので、点列データの全てを目標ゲートとして生成して記憶しておく場合と比べてデータ量を減らすことができる。この結果、点列データから目標ゲートを生成する演算が、点列データの全てを目標ゲートに生成する演算を行う場合と比べて容易になる。
【０３５１】
また、本実施形態では、移動車両Ｓの前方の間もなく通過する予定の複数の目標点列について、点列データから目標ゲートを生成するようにしているが、本発明では移動車両Ｓの前方の間もなく通過する予定の一つの目標点について、点列データから目標ゲートを生成してもよい。
【０３５２】
なお、本実施形態では、移動車両Ｓの前方の予定走行経路Ｋが曲線の場合に、移動車両Ｓの前方の間もなく通過する予定の複数の点列について、点列データから目標ゲートを生成するようにしているが、移動車両Ｓの前方の予定走行経路Ｋが直線であっても、移動車両Ｓの前方の間もなく通過する予定の一つないし複数の点列について、点列データから目標ゲートを生成してもよい。
【０３５３】
また、移動体Ｓの予定走行経路Ｋが鋭角なカーブであっても、小回り現象を発生させることなく移動体Ｓを誘導することができる。
【０３５４】
ところで隣接する３つの目標地点Ｐ1、Ｐ2、Ｐ3、を通り、中心点Ｐ0（ｘ0、ｙ0）、半径ｒ0の円の円周上を移動車両Ｓが走行しているとき、下記の例（イ）、（ロ）、（ハ）の場合には、車速指令算出手段６から移動車両Ｓに対して許容速度以下になるように適宜減速の走行指令を出力する必要がある。
【０３５５】
例（イ）：移動車両Ｓは小さな旋回半径ｒ0のカーブを高速で走行すると遠心力が生じ、車体が横滑りしてカーブを曲がりきれない場合がある。この場合、予め自動的に移動車両を旋回可能速度まで減速させる必要がある。
【０３５６】
例（ロ）：移動車両Ｓが直線走行から小さな旋回半径ｒ0のカーブに突入して、目標ゲートＧ1を通過し、次の目標ゲートＧ2に向かう瞬間、移動車両Ｓがステアリング車であれば操舵を直進から急旋回になるようにしなければならない。このとき、移動車両Ｓが高速で走行していれば、操舵を急旋回してもカーブを曲がりきれない場合がある。この場合、予め自動的に移動車両を操舵可能速度まで減速させる必要がある。
【０３５７】
例（ハ）：上記例（イ）、（ロ）の場合、目標ゲートＧ1を通過した時点で目標ゲートＧ2に向かわせるように大幅に移動車両Ｓを減速しても、移動車両Ｓが大幅に高速で走行しているため目標ゲートＧ2の周辺のカーブを曲がりきれない場合がある。
【０３５８】
そこで、点列データから目標ゲートを生成する場合には、目標ゲートＧ1を通過する時点ではなく、移動車両が安全に減速あるいは停止できる十分早い時点で点列データから目標ゲートを生成する必要がある。
【０３５９】
さて、予定走行経路上に予定外の障害物が存在する場合には、移動車両を障害物から回避させるか、あるいは移動車両を障害物の直前で減速、停止させるなどの措置が必要になる。
【０３６０】
そこで、移動車両を障害物から回避させたり、移動車両を障害物の直前で減速、停止させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態について図２、図２５を参照して説明する。
【０３６１】
図２５は移動車両を障害物から回避させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【０３６２】
なお、この実施形態では目標データを点列データとし、目標データと異なるデータも点列データとしている。
【０３６３】
図２５では、移動車両Ｓが予定走行経路Ｋ上の目標地点Ｐ1、Ｐ2、Ｐ3、Ｐ4、Ｐ5、Ｐ6、Ｐ7を順次通過するように進行する際に、移動車両Ｓの前方のαｍの距離（予定走行経路Ｋ上の目標地点Ｐ3とＰ4との間）に障害物ＳＢが存在する場合を想定している。また、同図２５においては、移動車両Ｓの右前方から対向車ＴＫが走行してくるものとする。
【０３６４】
ここで、予定走行経路Ｋ上に障害物ＳＢが存在すると、図２の障害物検知手段８が下記（ニ）、（ホ）、（ヘ）、（ト）に示す障害物に関する情報を検知する。
【０３６５】
（ニ）障害物ＳＢが移動車両Ｓの前方の距離αｍ、進路正面を中央にして左右方向に±β度の範囲に存在する。
【０３６６】
（ホ）移動車両Ｓの右前方から対向車が走行してくるので右側に回避したら危険である。
【０３６７】
（ヘ）移動車両Ｓの左前方には障害物がない。
【０３６８】
（ト）移動車両Ｓの左後方からは他の車両が迫ってきていない。
【０３６９】
障害物検知手段８は、上記（ニ）、（ホ）、（ヘ）、（ト）に示す障害物に関する情報を予定走行経路変更手段３に出力する。
【０３７０】
予定走行経路変更手段３は、障害物検知手段８から障害物の情報を受け取るとともに、現在運動計測手段２から移動車両Ｓの現在の運動状態の情報を取得する。そして、これら障害物の情報と移動車両Ｓの現在の運動状態の情報とに基づいて、走行条件選択手段４に対して「予定走行経路Ｋ上において移動車両Ｓから距離αの地点に障害物ＳＢが存在するので、進行方向に対して左側に予定走行経路Ｋを変更せよ。障害物ＳＢを回避して元の予定走行経路Ｋに復帰する際の経路に、もし新たな障害物が発見された場合にはそのまま減速停止せよ。もし元の予定走行経路Ｓに復帰できればそのまま元の予定走行経路Ｋに復帰して走行せよ。」という予定走行経路Ｋの変更に関する情報を出力する。
【０３７１】
走行条件選択手段４は、予定走行経路変更手段３から「予定走行経路Ｋ上において移動車両Ｓから距離αの地点に障害物ＳＢが存在するので、進行方向に対して左β度の方向に回避して停車準備せよ。」との情報を取得する。
【０３７２】
そして、走行条件選択手段４はこの情報に基づき、直ちに安全性が確保できる範囲で可能な限り減速させるよう車速指令算出手段６に走行条件データを出力する。そして車速指令算出手段６は移動車両Ｓに対して減速させるように走行指令を出力する。さらに走行条件選択手段４は、移動車両Ｓから距離αｍの地点から、進行方向に対して左β度の方向に移動車両Ｓの幅の分だけ空けて移動車両Ｓが走行できるように、予定走行経路Ｋ上の目標地点Ｐ2、Ｐ3、Ｐ4、Ｐ5、Ｐ6、Ｐ7と異なる地点ＶＰ2、ＶＰ3、ＶＰ4（障害回避点）、ＶＰ5（停止可能点）、ＶＰ6（安全停止点）を生成する。
【０３７３】
ＶＰ４（障害回避点）は目標地点Ｐ4を左β度の方向に移動させた地点である。
【０３７４】
ＶＰ５（停止可能点）は路面状況が悪くても移動車両Ｓが停止可能な地点である。
【０３７５】
ＶＰ６（安全停止点）はＶＰ５よりも離れた次の地点である。
【０３７６】
地点ＶＰ2、ＶＰ3、ＶＰ4、ＶＰ5、ＶＰ6を生成した後、走行条件選択手段４は、地点ＶＰ2、ＶＰ3、ＶＰ4、ＶＰ5、ＶＰ6からさらにゲートを生成していく。
【０３７７】
そして、走行条件選択手段４は、移動車両Ｓの現在の運動状態の情報と上述した予定走行経路Ｋの変更に関する情報とに基づいて操舵指令算出手段５にはゲートに対する現在の移動車両の相対位置情報、一方、車速指令算出手段６に走行条件を出力する。
【０３７８】
さらに、移動車両Ｓに対して操舵指令算出手段５からは操舵指令（ゲートを通過するために必要な操舵角度）が出力され、車速指令算出手段６からは走行指令（ゲートを通過するのに必要な速度）が出力される。
【０３７９】
これにより、地点ＶＰ2、ＶＰ3、ＶＰ4、ＶＰ5、ＶＰ6から生成されたゲートを通過するように移動車両Ｓが誘導される。
【０３８０】
但し、移動車両Ｓの回避動作時の走行指令は地点ＶＰ5で停止可能な速度である。
【０３８１】
次に移動車両Ｓを障害物ＳＢから回避させて予定走行経路Ｋに復帰させる場合について図２６を参照して説明する。
【０３８２】
図２６は移動車両Ｓを障害物ＳＢから回避させて予定走行経路Ｋに復帰させる場合を説明する図である。
【０３８３】
図２６では、地点ＶＰ4と目標地点Ｐ7との間の復帰用走行経路Ｋ1に復帰用のゲートＴＧ6、ＴＧ7を生成している。
【０３８４】
ここでは、まず障害物検知手段８が復帰用走行経路Ｋ1を形成する予定の場所に障害物がないことを確認する。
【０３８５】
そして、予定走行経路変更手段３が「障害物センサの視野範囲に障害物なし」という情報を走行条件選択手段４に対して出力する。
【０３８６】
走行条件選択手段４はこの情報に基づいて、移動車両Ｓを現在の回避用走行経路Ｋ0から予定走行経路Ｋ上に復帰させるための復帰用ゲートＴＧ6、ＴＧ7を、復帰用走行経路Ｋ1上に生成するとともに、移動車両Ｓの速度を元の速度に戻す。
【０３８７】
これにより移動車両Ｓは、復帰用ゲートＴＧ6、ＴＧ7を通過して予定走行経路Ｋ上に復帰し、障害物ＳＢが存在する地点Ｐ4を走り去る。
【０３８８】
このように、上述した実施形態では、障害物のある危険な地点で移動車両を停止させる必要がないので、移動車両と後続車両による追突事故を未然に防止することができる。
【０３８９】
なお、予定走行経路Ｋ上で障害物ＳＢを発見したら移動車両Ｓを減速させる必要があることは上述した。
【０３９０】
ここで、移動車両Ｓの現在位置である地点Ｐ1から回避完了の地点ＶＰ4までの間は、さらにブレーキをかけて移動車両Ｓを減速させながら、急速にステアリングをきる動作が必要になる。
【０３９１】
そこで、移動車両Ｓの現在位置である地点Ｐ1から回避直前まで地点ＶＰ2、ＶＰ3を生成するが、これら地点ＶＰ2、ＶＰ3を通る移動車両Ｓの旋回円の半径ｒ0は現在の速度で安全に操舵するのに十分大きな旋回半径とする。
【０３９２】
また、この実施形態では移動車両Ｓの現在位置は地点Ｐ1、障害物ＳＢの位置は目標地点Ｐ3とＰ4との間に存在しているものとして説明しているが、理想的に移動車両Ｓは常に停止、操舵が可能な速度で走行させて、速度が高ければ地点Ｐ1よりも手前で減速させることが望ましい。
【０３９３】
次に上述した実施形態以外に、移動車両を障害物から回避させたり、移動車両を障害物の直前で減速、停止させる場合の実施形態について説明する。
【０３９４】
図２７は移動車両を車庫に入れる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【０３９５】
同図２７において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ1、Ｇ2を通過した移動車両Ｓが目標ゲートＧ3に到達した時点で目標ゲートＧ1、Ｇ2、Ｇ3と異なるゲートＴＧ1、ＴＧ2を生成する。
【０３９６】
そこで、移動車両Ｓを目標ゲートＧ3からゲートＴＧ1、ＴＧ2に向かわせる。
【０３９７】
そして、ゲートＴＧ2に到達した時点で移動車両Ｓを後退させて車庫ＳＫ内に生成したゲートＰＧ1、ＰＧ2に向かわせる。
【０３９８】
このように、この実施形態では予定走行経路Ｋを走行している移動車両Ｓを後退させて車庫ＳＫに入れることができる。
【０３９９】
図２８は予定走行経路を走行している移動車両を予定走行経路と異なる走行経路に合流させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【０４００】
図２８において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ1に到達した時点で、目標ゲートＧ1と異なるゲートＴＧ1を生成する。
【０４０１】
そこで、移動車両ＳをゲートＴＧ1に向かわせる。
【０４０２】
そして、ゲートＴＧ1に到達した時点で移動車両Ｓを予定走行経路Ｋb上の目標ゲートＧ2′、Ｇ3′に向かわせる。
【０４０３】
このように、この実施形態では予定走行経路Ｋを走行している移動車両Ｓを予定走行経路Ｋと異なる走行経路Ｋbに合流させることができる。
【０４０４】
図２９は予定走行経路を走行している移動車両の前方に障害物が存在する場合、移動車両を障害物の手前で減速させる実施形態を示す図である。
【０４０５】
図２９において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ2に到達した時点で、目標ゲートＧ1、Ｇ2と異なるゲートＴＧ1、ＴＧ2を障害物ＳＢ′の手前に生成する。
【０４０６】
そこで、移動車両ＳをゲートＴＧ1、ＴＧ2で順次減速させる。
【０４０７】
このように、この実施形態では移動車両Ｓの前方に障害物ＳＢ′が存在する場合、移動車両Ｓを障害物の手前で減速させることができる。
【０４０８】
図３０は移動車両の予定走行経路上に障害物が存在する場合に予定走行経路上の目標データと異なるデータを生成して障害物を回避する実施形態を示す図である。
【０４０９】
図３０において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ1に到達した時点で、目標ゲートＧ1と異なるゲートＴＧ1、ＴＧ2、ＴＧ3を移動車両Ｓが障害物ＳＢ′を回避できるように進行方向の右側に生成する。
【０４１０】
そこで、移動車両ＳをゲートＴＧ1、ＴＧ2、ＴＧ3に向かわせる。
【０４１１】
そして、ゲートＴＧ3に到達した時点で移動車両Ｓを元の予定走行経路Ｋ上の目標ゲートＧ2に向かわせる。
【０４１２】
このように、この実施形態では移動車両Ｓの予定走行経路Ｋ上に障害物ＳＢ′が存在する場合、障害物ＳＢ′を回避することができる。
【０４１３】
図３１は予定走行経路を走行している移動車両の前方に障害物が存在する場合、移動車両を障害物の手前で停止させる実施形態を示す図である。
【０４１４】
図３１において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ2に到達した時点で、目標ゲートＧ1、Ｇ2と異なるゲートＴＧ1、ＴＧ2を障害物ＳＢ′の手前に生成する。
【０４１５】
そこで、移動車両ＳをゲートＴＧ1で急減速させて、ＴＧ2またはＴＧ3で停止させる。
【０４１６】
このように、この実施形態では移動車両Ｓの前方に障害物ＳＢ′が存在する場合、移動車両Ｓを障害物の手前で停止させることができる。
【０４１７】
図３２は予定走行経路を走行している移動車両を停止点に横付けして停止させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【０４１８】
図３２において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ2に到達した時点で、目標ゲートＧ1、Ｇ2と異なるゲートＴＧ1、ＴＧ2を目標ゲートＧ2と停止点Ｐsとの間に生成し、停止点ＰsにＴＧ3を生成する。
【０４１９】
そこで、移動車両ＳをゲートＴＧ1、ＴＧ2に向かわせながら減速させ、ゲートＴＧ3で停止させる。
【０４２０】
このように、この実施形態では予定走行経路Ｋを走行している移動車両Ｓを停止点Ｐsに横付けして停止させることができる。
【０４２１】
図３３は停止点に横付けされて停止している移動車両を、発進させて徐々に加速させ、予定走行経路に復帰させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【０４２２】
図３３において、走行条件選択手段４は停止点Ｐsから予定走行経路Ｋ上の目標ゲートＧ1との間に目標ゲートＧ1、Ｇ2と異なるゲートＴＧ1、ＴＧ2、ＴＧ3を生成する。
【０４２３】
そこで、移動車両ＳをゲートＴＧ1、ＴＧ2、ＴＧ3に向かわせながら徐々に加速させ、予定走行経路Ｋ上の目標Ｇ1、Ｇ2に向かわせる。
【０４２４】
このように、この実施形態では停止点Ｐsに横付けされて停止している移動車両Ｓを発進させて徐々に加速させ、予定走行経路Ｋに復帰させることができる。
【０４２５】
図３４は移動車両の隣を走行している車両が移動車両に対して幅寄せしてきた場合に、この幅寄せしてくる車両から移動車両を回避させる実施形態を示す図である。
【０４２６】
図３４において、走行条件選択手段４は予定走行経路Ｋ上の目標ゲートＧ1に到達した時点で、目標ゲートＧ1、Ｇ2と異なるゲートＴＧ1、ＴＧ2、ＴＧ3を移動車両Ｓが車両Ｓxを回避できるように進行方向の右側に生成する。
【０４２７】
そこで、移動車両ＳをゲートＴＧ1、ＴＧ2、ＴＧ3に向かわせる。
【０４２８】
そして、ゲートＴＧ3に到達した時点で移動車両Ｓを元の予定走行経路Ｋ上の目標ゲートＧ2に向かわせる。
【０４２９】
このように、この実施形態では移動車両Ｓの隣を走行している車両Ｓxが移動車両Ｓに対して幅寄せしてきた場合に、この幅寄せしてくる車両Ｓxから移動車両Ｓを回避させることができる。
【０４３０】
図３５は移動車両の予定走行経路が急カーブの場合に目標ゲートと目標ゲートとの間隔を狭くして急カーブでの移動車両の速度を下げる実施形態を示す図である。
【０４３１】
図３５において、急カーブＫcに生成されている目標ゲートＧ3とＧ4との間隔ＷTおよび目標ゲートＧ3とＧ4との間隔ＷTは、目標ゲートＧ1とＧ2との間隔Ｗおよび目標ゲートＧ6とＧ7との間隔Ｗに比べて狭くされている。
【０４３２】
ここで、各目標ゲートでは移動車両Ｓが各目標ゲートに到達した時点で操舵指令算出手段５が操舵の演算を行うので、急カーブＫcでは目標ゲートの間隔が狭くなった分だけ演算が連続して行われるようになる。この結果、急カーブＫcでは演算時間が長くなるので移動車両Ｓが急カーブＫcを通過する速度ｖが下がる。このように目標ゲート間の通過時間が一定になるように調整することができる。
【０４３３】
従って、この実施形態では予定走行経路Ｋが急カーブの場合には、移動車両Ｓの速度ｖを下げて脱輪を防ぎ、移動車両Ｓを安全に走行させることができる。
【０４３４】
図３６は急カーブの予定走行経路を走行している移動車両に及ぼす遠心力を減らすために速度を下げる実施形態を示す図である。
【０４３５】
同図３６において、急カーブの予定走行経路Ｋを走行している移動車両Ｓに及ぼす遠心力は、旋回半径ｒ0に反比例し、移動車両Ｓの速度ｖに比例する。
【０４３６】
また、移動車両Ｓが急カーブを走行すると移動車両Ｓの旋回半径ｒ0は小さくなる。
【０４３７】
このため、急カーブの予定走行経路Ｋを走行している移動車両Ｓに及ぼす遠心力は増加し、移動車両Ｓが横転事故やスリップ事故を起こす虞がある。
【０４３８】
従って移動車両Ｓの速度ｖは下げる必要がある。
【０４３９】
そこで、現在運動計測手段２（車速センサで速度ｖ、角速度センサでωを各々検出）によって移動車両Ｓの旋回半径ｒ0を求め（ｒ0＝ｖ／ω）、車体７（移動車両Ｓ）において旋回半径ｒ0が小さければ速度ｖを下げるように調整する。
【０４４０】
これにより急カーブの予定走行経路Ｋを走行している移動車両Ｓの横転事故やスリップ事故を防ぐことができる。
【０４４１】
なお、上述した実施形態では、上記（１）式及び（４）式に示す直線の方程式の傾きにｔａｎθを用いているが、θが９０度の整数倍のとき直線はＸ軸ないしＹ軸に平行になる。この場合、直線と点の座標との位置関係を判定するには、適宜、Ｘ座標ないしＹ座標だけを比較すればよい。
【０４４２】
また、上述した実施形態では、目標点列に予定走行経路記憶手段１に記憶された点列データを用いているが、この記憶された点列データに対応して実際の道路に磁石などの目印を埋設したり、道路に沿って移動車両Ｓの通過位置を示す発信機を設置すれば、現在運動計測手段２によって移動車両Ｓの現在位置を精度良く検出することができる。
【図面の簡単な説明】
【図１】図１は、本発明に係わる移動車両の誘導装置の実施形態を示す図である。
【図２】図２は、本発明に係わる移動車両の誘導装置の構成を示すブロック図である。
【図３】図３は、本発明に係わる移動車両の誘導装置が行う処理を説明するフローチャートである。
【図４】図４は、移動車両が一対の端点間を通過したことを確認する処理を説明する図である。
【図５】図５は、直進している移動車両を一対の端点間に向けて進行させる処理を説明する図である。
【図６】図６は、移動車両を等価２輪車に置き換えて移動車両の操舵を説明する図である。
【図７】図７は、スキッドステア方式の移動車両の操舵を説明する図である。
【図８】図８は、旋回走行している移動車両を一対の端点間に向けて進行させる処理を説明する図である。
【図９】図９（ａ）は目標ゲートに向けて移動車両を前進させる場合を示す図であり、同図９（ｂ）は目標ゲートに向けて移動車両を後退させる場合を示す図である。
【図１０】図１０は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図１１】図１１は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図１２】図１２は、本発明に係わる移動車両の誘導装置の実施形態を示す図である。
【図１３】図１３は、直進している移動車両を任意の幾何学図形に向けて進行させる処理を説明する図である。
【図１４】図１４は、旋回走行している移動車両を任意の幾何学図形に向けて進行させる処理を説明する図である。
【図１５】図１５（ａ）は任意の幾何学図形に向けて移動車両を前進させる場合を示す図であり、同図１５（ｂ）は任意の幾何学図形に向けて移動車両を後退させる場合を示す図である。
【図１６】図１６は、本発明に係わる移動車両の誘導装置の実施形態を示す図である。
【図１７】図１７は、直進している移動車両を円に向けて進行させる処理を説明する図である。
【図１８】図１８は、旋回走行している移動車両を円に向けて進行させる処理を説明する図である。
【図１９】図１９（ａ）は円に向けて移動車両を前進させる場合を示す図であり、同図１９（ｂ）は円に向けて移動車両を後退させる場合を示す図である。
【図２０】図２０（ａ）は、点列データの密度が予定経路の曲率に比べて十分密である場合を示す図であり、同図（ｂ）は、点列データの密度が予定経路Ｋの曲率に比べて疎となる場合を示す図である。
【図２１】図２１は、３つの目標地点が一直線上に並んでいる点列データを示す図である。
【図２２】図２２は、３つの目標地点が移動車両の旋回円の円周上に並んでいる点列データを示す図である。
【図２３】図２３は、３つの目標地点を通る円と目標ゲートの傾きを示す図である。
【図２４】図２４は、３つの目標地点を通る円の中心点の設定を説明する図である。
【図２５】図２５は、移動車両を障害物から回避させる場合に予定走行経路上の目標データと異なるデータを生成する実施形態を示す図である。
【図２６】図２６は、移動車両を障害物から回避させて予定走行経路に復帰させる場合を説明する図である。
【図２７】図２７は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図２８】図２８は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図２９】図２９は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３０】図３０は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３１】図３１は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３２】図３２は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３３】図３３は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３４】図３４は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３５】図３５は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３６】図３６は、本発明に係わる移動車両の誘導装置の実施形態の変形例を示す図である。
【図３７】図３７は、従来技術を示す図である。
【図３８】図３８（ａ）は等価２輪車を前進させて目標地点に誘導する場合を示す図であり、同図（ｂ）は等価２輪車を後退させて目標地点に誘導する場合を示す図である。
【符号の説明】
Ｓ…移動車両
Ｋ…予定走行経路
ＰRn、ＰLn…一対の端点[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a moving body guiding apparatus, and more particularly to a moving body guiding apparatus suitable for a case where the moving body bends a sharp curve with a narrow road width, or when the moving body moves forward or backward.
[0002]
[Background Art and Problems to be Solved by the Invention]
As a conventional technique for guiding a moving vehicle, there is a technique called a “point tracking method”.
[0003]
FIG. 37 is a diagram showing a technique for guiding a moving vehicle using the “point tracking method”.
[0004]
In the technique shown in FIG. 37, a curve that is a planned travel route of the moving vehicle S is taught as a point sequence of the target points P1, P2, P3, P4, P5, P6, and P7 of the moving vehicle S. A region where the moving vehicle S travels is expressed in a coordinate system, and target points P1, P2, P3, P4, P5, P6, and P7 of the moving vehicle S traveling in this coordinate system are expressed in coordinate data.
[0005]
Therefore, in the technique shown in FIG. 37, the coordinate data and the radii r1, r2, r3, r4, r5, r6, r7 of the circles centered on the target points P1, P2, P3, P4, P5, P6, P7, and Is used to guide the moving vehicle S to sequentially follow the target points P1, P2, P3, P4, P5, P6, and P7.
[0006]
The technique shown in FIG. 37 is suitable for guiding a large construction machine traveling on a vast rough terrain.
[0007]
On the other hand, small mobile vehicles can be used indoors, on narrow premises, or on narrow roads, such as using automated guided vehicles and automated forklifts in automated factories, or using automated vehicles with ITS (Intelligent Transportation System). May be induced.
[0008]
This case will be described with reference to FIG.
[0009]
For example, the moving vehicle S is guided to bend a curve while sequentially following each target point P1, P2, P3, P4, P5, P6, P7.
[0010]
At this time, since the curve is steep, the moving vehicle S is greatly deviated from the target points P1, P2, P3, P4, P5, P6, and P7, and a small turning phenomenon occurs through the inside K ′ of the curve.
[0011]
It is considered that this is because the moving vehicle S is guided toward the next target when it enters the circle corresponding to the current target point.
[0012]
That is, it is considered that the cause is that the moving vehicle S is guided to the next target before the target point.
[0013]
For this reason, if there is an obstacle at the point Kp inside the curve, there is a possibility that the obstacle and the moving vehicle S collide to cause an accident.
[0014]
The present invention has been made in view of such a situation, and even if the planned traveling route of the moving body is an acute curve, the first problem to be solved is to guide the moving body without generating a small turning phenomenon. .
[0015]
In the field of automotive engineering, a steering type four-wheeled vehicle may be virtually replaced with a two-wheeled vehicle. The two-wheeled vehicle in this case is called an equivalent two-wheeled vehicle.
[0016]
FIG. 38A is a diagram showing a case where the equivalent two-wheeled vehicle is moved forward and guided to the target point, and FIG. 38B is a diagram showing a case where the equivalent two-wheeled vehicle is moved backward and guided to the target point. is there.
[0017]
38 (a) and 38 (b), the equivalent two-wheeled vehicle S0 includes two wheels, a steering wheel S1 and a fixed wheel S2.
[0018]
In FIGS. 38 (a) and 38 (b), the steering angle ζ of the steered wheel S1 is directed to the target point Pn (Xn, Yn) so that the equivalent two-wheeled vehicle reaches the target point Pn (Xn, Yn). I have to.
[0019]
As shown in FIG. 38 (a), when the equivalent two-wheeled vehicle S0 is advanced to reach the target point Pn (Xn, Yn), the fixed wheel S2 advances according to the steering angle ζ of the steered wheel S1.
[0020]
Therefore, when the equivalent two-wheeled vehicle S0 is moved forward, the equivalent two-wheeled vehicle S0 can reach the target point Pn (Xn, Yn).
[0021]
However, as shown in FIG. 38B, when the equivalent two-wheeled vehicle S0 is moved backward to reach the target point Pn (Xn, Yn), the steering angle ζ is set to the target point Pn (Xn, Yn), the traveling direction of the steered wheel S1 and the fixed wheel S2 is different.
[0022]
Therefore, when the equivalent two-wheeled vehicle S0 is moved backward, the equivalent two-wheeled vehicle S0 cannot reach the target point Pn (Xn, Yn).
[0023]
That is, in FIGS. 38 (a) and 38 (b), the steering angle ζ when the equivalent two-wheeled vehicle moves forward cannot be made the same as the steering angle ζ when the vehicle reverses.
[0024]
For this reason, in the conventional technique for guiding the moving vehicle S to the target point Pn (Xn, Yn), for example, as shown in Japanese Patent Laid-Open No. 63-19011, a control algorithm is used when the equivalent two-wheeled vehicle moves forward and backward. And switched.
[0025]
As a result, the above-described prior art causes a problem that the control characteristics change when the moving vehicle S moves forward and backward. Further, there arises a problem that the control of the steering angle ζ becomes complicated.
[0026]
The present invention has been made in view of such a situation, and a second problem to be solved is to make the control characteristics of the moving vehicle forward and backward the same and to simplify the control of the steering angle ζ.
[0027]
[Means for solving the problems and effects]
  Therefore, in the first invention of the present invention, in order to achieve the first problem to be solved,
  In the guidance device for a moving body that guides the moving body toward the next target when the current target is reached among a plurality of targets on the planned traveling route of the moving body,
  Scheduled travel routeTop target pointA pair of end points separated by a predetermined distance acrossArea betweenIs the goal,
  When the moving body reaches between the current pair of end points,When traveling with the current steering angle maintained, it is determined whether or not the next target area between the pair of end points is reached,
If it is determined that the vehicle has reached the current steering angle, the current steering angle is maintained. If it is determined that the vehicle has not reached, the target steering angle for directing the moving body to the target point between the pair of target end points is calculated. The steering angle is changed based on the target steering angle, and then between the target pair of end points.The moving body is made to face.
[0028]
The first invention will be described with reference to FIG.
[0029]
According to the first aspect of the invention, the pair of end points PRn and PLn separated by a predetermined distance with the planned travel route K interposed therebetween are set as the targets of the moving body S.
[0030]
Therefore, for example, when the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is directed between the next pair of end points PR2 and PL2.
[0031]
Therefore, according to the first aspect of the invention, when the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is made to face between the next pair of end points PR2 and PL2. Even if the planned traveling route K of the moving body S is an acute curve, the moving body S can be guided without causing a small turning phenomenon.
[0032]
  In the second invention of the present invention, in order to achieve the first solution,
  In the moving body guidance device for guiding the moving body toward the next passing area when the current passing area is reached among the plurality of passing areas on the planned traveling route of the moving body,
  When the mobile body reaches a predetermined point in the passing area, it is determined whether or not the next passing area is reached when traveling while maintaining the current steering angle;
If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle does not reach, the target steering angle for directing the moving body to the predetermined point in the next passing area is calculated and this is calculated. The steering angle is changed based on the target steering angle,The moving body is directed to the next passage area.
[0033]
The second invention will be described with reference to FIG.
[0034]
According to the second aspect of the present invention, when the mobile object S travels a predetermined distance after first detecting the current passing area M1 and reaches the predetermined point P1 in the passing area M1, the next pass The moving body S is directed to the area M2.
[0035]
Therefore, according to the second aspect of the present invention, when the moving body S first travels a predetermined distance after detecting the current passing area M1, and reaches a predetermined point P1 in the passing area M1, Since the moving body S is directed to the passing area M2 of the moving body S, even if the planned traveling route K of the moving body S is an acute curve as in the first aspect of the invention, the moving body S does not cause a small turning phenomenon. Can be induced. Furthermore, according to the second invention, the effect of the first invention can be obtained regardless of the geometric shape of the moving object S such as a circle or a polygon.
[0041]
  In addition, the present inventionThird inventionThen, in order to achieve the first solution problem and the second solution problem,
  Guidance of the moving body that guides the moving body toward the next target when the current target is reached among a plurality of targets on the planned traveling route of the moving body having the steering wheel and the fixed wheel In the device
  A region between a pair of end points separated by a predetermined distance across the target point on the planned travel route is the target,
  Using the wheel base of the steered wheel and the fixed wheel as a denominator, the steering angle of the steered wheel is obtained from an arc tangent function with the radius of the turning circle when the moving body turns to the next target as a numerator,
  When the mobile body has reached between the current pair of end points, it is determined whether or not to reach the region between the next target pair of end points when traveling while maintaining the current steering angle,
  If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle will not reach, the target steering angle for moving the moving body to the target point between the next pair of target end points is calculated. The steering angle is changed based on the target steering angle, and the moving body is made to face between a pair of target end points.
[0042]
  the aboveThird inventionWill be described with reference to FIG. 1A, FIG. 2, FIG. 6, and FIG.
[0043]
  the aboveThird inventionAccording to the above, the pair of end points PRn and PLn separated by a predetermined distance across the planned travel route K is set as the target of the moving body S.
[0044]
Therefore, the steering command calculation means 5 determines the steering angle ζ of the steering wheel S1 from the distance L between the central axis of the steering wheel S1 and the central axis of the fixed wheel S2 and the turning radius rg.
[0045]
When the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is steered based on the steering angle ζ, and the moving body S moves toward the next pair of end points PR2 and PL2. Is done.
[0046]
  Therefore, the aboveThird inventionThe steering command calculation means 5 determines the steering angle ζ of the steering wheel S1 from the distance L between the central axis of the steering wheel S1 and the central axis of the fixed wheel S2 and the turning radius rg. When the moving body S arrives between the end points PR1 and PL1, the moving body S is steered based on the steering angle ζ, and the moving body S is directed toward the next pair of end points PR2 and PL2.The same effect as the first invention can be obtained. In addition, the control characteristics when the moving vehicle moves forward and backward can be made the same. Further, the control of the steering angle ζ can be simplified.
[0047]
  In addition, the present inventionFourth inventionThen, in order to achieve the first solution problem and the second solution problem,
  Movement that guides the moving body toward the next passing area when it reaches the current passing area among a plurality of passing areas on the planned traveling route of the moving body having the steering wheel and the fixed wheel. In the body guidance device,
  Using the wheel base of the steered wheel and the fixed wheel as a denominator, the steering angle of the steered wheel is obtained from an arc tangent function with the radius of the turning circle when the moving body turns to the next target as a numerator,
  When the mobile body reaches a predetermined point in the passing area, it is determined whether or not the next passing area is reached when traveling while maintaining the current steering angle;
  If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle does not reach, the target steering angle for directing the moving body to the predetermined point in the next passing area is calculated and this is calculated. The steering angle is changed based on the target steering angle, and the moving body is directed toward the next passing area.
[0048]
  the aboveFourth inventionWill be described with reference to FIG. 2, FIG. 6, and FIG.
[0049]
  the aboveFourth inventionThe steering command calculation means 5 steers the steered wheel S1 from the distance L between the center axis of the steered wheel S1 and the center axis of the fixed wheel S2 and the turning radius rg when turning to the next target. An angle ζ is determined.
[0050]
Then, when the moving body S first travels a predetermined distance after detecting the current passing area M1 and reaches a predetermined point P1 in the passing area M1, the moving body S is based on the steering angle ζ. Is steered and the moving body S is directed toward the next passing area M2.
[0051]
  Therefore, the aboveFourth inventionAccordingly, the steering angle ζ of the steered wheel S1 is obtained from the distance L between the center axis of the steered wheel S1 and the center axis of the fixed wheel S2 and the turning radius rg by the steering command calculation means 5, and the moving body S The vehicle S is steered based on the steering angle ζ when the vehicle travels a predetermined distance after first detecting the current passing region M1 and reaches a predetermined point P1 in the passing region M1. Since the moving body S is directed toward the passing area M2,The same effect as the second invention can be obtained. In addition, the control characteristics when the moving vehicle moves forward and backward can be made the same. Further, the control of the steering angle ζ can be simplified.
[0052]
  In addition, the present invention5th inventionThen, in the first invention,
  Generating a pair of end points for a point sequence corresponding to a curve among a plurality of target points on the planned travel route;
  When the moving body reaches between a pair of end points centered on the target point, the moving body is made to face between the pair of end points centered on the next target point.
[0053]
  the above5th inventionWill be described with reference to FIG.
[0054]
  the above5th inventionAccording to the above, the point sequences P1, P1, P2, P3, P4, P5, P6, and P7 on the planned travel route K are set as a plurality of targets.
[0055]
Therefore, a pair of end points PR1 and PL1 are generated from the point P1 in the sequence of points P1, P1, P2, P3, P4, P5, P6, and P7 in the planned traveling route K of the curve.
[0056]
The moving body S is then moved between the generated pair of end points PR2 and PL2 when it reaches the current point P1 among the point sequences P1, P1, P2, P3, P4, P5, P6, and P7.
[0057]
  That is, the above5th inventionAccording to the above, when the planned travel route K is a curve, a pair of end points PR2 and PL2 are generated from the point P2.
[0058]
  Therefore, the above5th inventionAccording to the above, since the pair of end points PR2 and PL2 are generated when the planned travel route K is a curve, the calculation for generating the pair of end points from the point sequence becomes the pair of end points. Compared to the case of performing the operation to be generated. further5th inventionAccording to this, the same effect as the first invention can be obtained.
[0059]
DETAILED DESCRIPTION OF THE INVENTION
DESCRIPTION OF EMBODIMENTS Embodiments of a moving body guiding apparatus according to the present invention will be described below with reference to the drawings. The moving body of this embodiment is assumed to be a steering four-wheel vehicle.
[0060]
FIG. 2 is a block diagram showing the configuration of the moving vehicle guidance apparatus according to the present invention.
[0061]
The moving vehicle guidance apparatus shown in FIG. 2 includes a current motion measuring means 2, a planned travel route storage means 1, a planned travel route change means 3, a travel condition selection means 4, a steering command calculation means 5, a vehicle speed. The command calculation means 6, the obstacle detection means 8, and the vehicle body 7 (moving vehicle S) are comprised.
[0062]
Next, each said structure is demonstrated.
[0063]
The current motion measuring means 2 uses GPS (Global Positioning System), dead reckoning, etc. for information on the current motion state of the moving vehicle, such as the current position P1, moving direction θt, speed, and turning angular velocity ω of the moving vehicle S. Measure using
[0064]
The planned traveling route storage means 1 uses data converted from point data created by CAD (Computer Automation Design) or teaching playback into a pair of end points or arbitrary geometric figures as a plurality of target data of the planned traveling route K. Remember. The scheduled travel route storage unit 1 acquires information on the current motion state of the moving vehicle S from the current motion measurement unit 2. Furthermore, the planned travel route storage unit 1 outputs a plurality of stored target data of the planned travel route K to the travel condition selection unit 4.
[0065]
The obstacle detection means 8 detects an obstacle present on the planned travel route K.
[0066]
The planned traveling route changing means 3 stores data of a pair of end points or arbitrary geometric figures different from the target data of the planned traveling route K. Further, the planned traveling route changing unit 3 acquires information on the obstacle on the planned traveling route K from the obstacle detecting unit 8 and also acquires information on the current motion state of the moving vehicle S from the current motion measuring unit 2. Further, the planned travel route changing unit 3 outputs a pair of end points or arbitrary geometric figure data (planned travel route change information) different from the target data of the planned travel route K to the travel condition selecting unit 4.
[0067]
The travel condition selection means 4 acquires a plurality of target data of the planned travel route K from the planned travel route storage means 1 in the normal case (when there is no obstacle on the planned travel route K) and measures the current motion. Information on the current motion state of the moving vehicle S is acquired from the means 2. On the other hand, in the case of an emergency (when there is an obstacle on the planned travel route K), the planned travel route changing means 3 sets a pair of end points different from the target data of the planned travel route K or data of any geometric figure (planned Travel route change information). The traveling condition selection means 4 outputs the current relative position of the moving vehicle to the target data or a pair of end points or data of an arbitrary geometric figure different from the target data to the steering command calculation means 5 and the vehicle speed command calculation means. The driving condition is output to 6.
[0068]
The steering command calculation means 5 calculates the steering angle ζ and outputs a steering command to the vehicle body 7 (moving vehicle S) to control the steering angle ζ of the moving vehicle S. As a result, the steering command calculation means 5 causes the moving vehicle S to go straight, turn right, and turn left.
[0069]
The vehicle speed command calculation means 6 calculates the speed of the vehicle body 7 and outputs a travel command to the vehicle body 7 to control the speed of the vehicle body 7. As a result, the vehicle speed command calculation means 6 causes the vehicle body 7 to accelerate, decelerate, advance, and reverse.
[0070]
The vehicle body 7 (moving vehicle S) adjusts the vehicle body speed in response to the steering command output from the steering command calculation means 5 and the travel command output from the vehicle speed command calculation means 6.
[0071]
Obstacle detection means 8 uses an ultrasonic sensor or a laser range finder, or uses a plurality of video cameras to measure the distance of the object from the parallax of the video imaged on the video camera. Detection of the presence or absence of obstacles in front of or around S, recognition of the movement status of the obstacles, measurement of the distance to the obstacles, and the like.
[0072]
The scheduled travel route changing unit 3 receives the information on the obstacle from the obstacle detecting unit 8 and acquires information on the current motion state of the moving vehicle S from the current motion measuring unit 2. Then, based on the information on the obstacles and the information on the current motion state of the moving vehicle S, information related to the change of the planned travel route K is output to the travel condition selection unit 4.
[0073]
Next, the operation of the moving vehicle guidance device shown in FIG. 2 will be described.
[0074]
Here, a case where a plurality of target data of the planned travel route K are set as a plurality of a pair of end points will be described with reference to FIGS. 1 and 3.
[0075]
FIG. 1A is a diagram showing an embodiment in which a plurality of target data of the planned travel route K is a plurality of a pair of end points PRn and PLn (n = 1 to 7), and FIG. It is a figure which shows the example which described data as width d, inclination (theta) a with respect to a reference direction, and line segment Ga of center point Pa. FIG.
[0076]
The pair of end points PRn and PLn are separated so as to sandwich the planned travel route K.
[0077]
Each target point Pn (n = 1 to 7) is included between each pair of end points PRn and PLn.
[0078]
Further, the allowable amount of passage of the moving vehicle S is determined by the width d of the line segment Ga in FIG.
[0079]
Each pair of end points PRn and PLn and the line segment Ga is also called a gate.
[0080]
FIG. 3 is a flowchart for explaining processing performed by the moving vehicle guidance apparatus shown in FIG.
[0081]
In the flowchart shown in FIG. 3, first, in step 101, it is determined whether or not the moving vehicle S in FIG. 1 is traveling straight (turning).
[0082]
In step 101, the current motion measuring means 2 in FIG. 2 measures the turning angular velocity ωt and the velocity vt of the moving vehicle S at time t, and obtains the turning radius rt of the moving vehicle S. As a result, if the turning radius rt is so large that the trajectory traveled by the moving vehicle S can be regarded as a straight line, it is determined that the moving vehicle S is traveling straight (determination YES in step 101). On the other hand, it is determined that the moving vehicle S is turning except when the turning radius rt is extremely large (determination NO in step 101). If the turning radius rt of the moving vehicle S is sufficiently larger than the vehicle body dimension (for example, the turning radius rt is 10 times the vehicle body dimension), the moving vehicle S can be regarded as traveling straight.
[0083]
If “YES” is determined in the step 101, the process is subsequently performed on the assumption that the moving vehicle S is traveling straight (step 102). On the other hand, if NO is determined in step 101, the process is performed assuming that the moving vehicle S is turning thereafter (step 103).
[0084]
First, a process for causing the moving vehicle S traveling straight ahead to proceed between the end points PR2 and PL2 without steering will be described.
[0085]
In step 104, it is determined whether or not the moving vehicle S traveling straight ahead can pass between the next end points PR2 and PL2 after reaching the end point PR1 and PL1.
[0086]
That is, in step 104, it is determined whether or not the traveling condition selection means 4 in FIG.
[0087]
Here, if the moving vehicle S can pass between the end points PR2 and PL2 without being steered (YES in step 104), the traveling condition selection means 4 provides the steering command with the data for causing the moving vehicle S to proceed as it is. It outputs to the calculation means 5 (step 107).
[0088]
Processing performed by the traveling condition selection means 4 will be described with reference to FIGS.
[0089]
First, a process for confirming the passage between the end points PR2 and PL2 will be described with reference to FIG.
[0090]
In FIG. 4, the pair of end points PR1 and PL1 are expressed as a width d, an inclination θ1 with respect to the reference direction, and a line segment G1 of the center point P1 (x1, y1). The representative point of the moving vehicle S at time t is Pt (xt, yt), and the representative point of the moving vehicle S at time t + 1 is Pt + 1 (xt + 1, yt + 1). Therefore, the equation of the line segment G1 is expressed by the following equation (1).
[0091]
(X−x1) · tan θ1− (y−y1) = 0 (1)
Here, the point Pt (xt, yt) exists below the line segment G1 as shown in FIG. Therefore, when the point Pt (xt, yt) is substituted into the above equation (1), the equation of a straight line passing through the point Pt (xt, yt) is expressed by the following equation (2).
[0092]
μt = (xt−x1) .tan θ1− (yt−y1)> 0 (2)
On the other hand, the point Pt + 1 (xt + 1, yt + 1) exists above the line segment G1, as shown in FIG. Therefore, when the point Pt + 1 (xt + 1, yt + 1) is substituted into the above equation (1), the equation of the straight line passing through the point Pt + 1 (xt + 1, yt + 1) is expressed by the following equation (3). expressed.
[0093]
μt + 1 = (xt + 1−x1) · tan θ1− (yt + 1−y1) <0 (3)
When the representative point Pt (xt, yt) or Pt + 1 (xt + 1, yt + 1) of the moving vehicle S is present on the line segment G1, the above equation (1) is established. Alternatively, the value of μt + 1 is 0.
[0094]
When the moving vehicle S passes through the line segment G1, the signs of the above-mentioned μt and μt + 1 are opposite to each other, or each value is zero.
[0095]
That is, the traveling condition selection means 4 uses the equation (1) of the line G1 that is the current target for the representative point Pt (xt, yt) of the moving vehicle S at the time t that changes every moment as the moving vehicle S advances. To obtain μt, and further substitute the representative point Pt + 1 (xt + 1, yt + 1) of the moving vehicle S at time t + 1, which is the next moment of time t, into equation (1) of the line segment G1. To obtain μt + 1.
[0096]
When the product of μt and μt + 1 obtained in this way becomes 0 or the sign of this product becomes negative (when μt · μt + 1 ≦ 0), the moving vehicle S is a line. It turns out that the minute G1 has been passed.
[0097]
Thus, the traveling condition selection means 4 confirms whether or not the moving vehicle S has reached the line segment G1.
[0098]
Here, a process of moving the moving vehicle S toward the pair of end points PR2 and PL2 will be described with reference to FIG.
[0099]
In FIG. 5, a representative point of the moving vehicle S (a point on the line segment G1) is P1 (x1, y1). The traveling direction of the moving vehicle S is θt. Therefore, the equation of the straight line that becomes the trajectory when the moving vehicle S goes straight can be expressed by the following equation (4).
[0100]
(X−x1) · tan θt− (y−y1) = 0 (4)
Here, when the straight line represented by the equation (4) is used as a boundary line, the end point PR2 (xR2, yR2) shown in FIG. 5 is negative because it exists below the straight line of the equation (4). It is included in the area. On the other hand, the end point PL2 (xL2, yL2) is included in the positive region because it exists above the straight line of the equation (4).
[0101]
By the way, the equation of the straight line through which the point P1 (x1, y1) and the end point PR2 (xR2, yR2) pass is
λR = (xR2−x1) · tan θt− (yR2−y1) (5)
It is represented by
[0102]
Further, the equation of a straight line passing through the point P1 (x1, y1) and the end point PL2 (xL2, yL2) is
λL = (xL2−x1) tan θt− (yL2−y1) (6)
It is represented by
[0103]
Here, since the straight line of the equation (5) passes through the negative region, the sign of λR becomes negative. On the other hand, since the straight line in the equation (6) passes through the positive region, the sign of λL becomes positive.
[0104]
Therefore, the product of the above equation (5) and equation (6),
λ = λR · λL (7)
, The sign of λ is negative.
[0105]
Further, if the straight line of the equation (4) passes through one of the end points PR2 (xR2, yR2) and PL2 (xL2, yL2), λ is 0.
[0106]
Therefore, it can be seen that the straight line of the equation (4) passes between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) when λ ≦ 0.
[0107]
Thus, the traveling condition selection means 4 determines that the moving vehicle S can pass between the end points PR2 and PL2 without being steered (determination YES in step 104).
[0108]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0109]
Next, a process of steering the moving vehicle S that is traveling straight and proceeding between the pair of end points PR2 and PL2 will be described.
[0110]
When the moving vehicle S cannot pass between the end points PR2 and PL2 (NO in Step 104), the traveling condition selection unit 4 outputs data for steering the moving vehicle S to the steering command calculation unit 5.
[0111]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0112]
As shown in FIG. 4 described above, the traveling condition selecting means 4 confirms whether or not the moving vehicle S has reached the upper side between the end points PR1 and PL1 (on the line segment G1).
[0113]
Here, the above formula (4) (shown below) is
(X−x1) · tan θt− (y−y1) = 0 (4)
In the case where they exist above the end points PR2 and PL2 shown in FIG. 5, these end points PR2 and PL2 are included in the negative region.
[0114]
That is, the signs of λR and λL are negative.
[0115]
Therefore, when λ is obtained using the above equation (7) (shown below),
λ = λR · λL (7)
The sign of λ is positive.
[0116]
Further, when the above expression (4) exists below the end points PR2 and PL2 shown in FIG. 5, these end points PR2 and PL2 are included in the positive region.
[0117]
That is, the signs of λR and λL are positive.
[0118]
Therefore, when λ is obtained using the above equation (7), the sign of λ becomes positive.
[0119]
That is, when λ> 0, the straight line in the formula (4) does not pass between the end points PR2 and PL2.
[0120]
Thereby, the traveling condition selection means 4 determines that the moving vehicle S cannot pass between the end points PR2 and PL2 in this state (determination NO in step 104).
[0121]
Therefore, the travel condition selection means 4 includes the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 and the current position P1 (x1, y1) of the moving vehicle S acquired from the current motion measurement means 2. Then, the data of the traveling direction θt is output to the steering command calculation means 5.
[0122]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0123]
Processing performed by the steering command calculation means 5 will be described with reference to FIGS.
[0124]
The steering command calculation means 5 moves the moving vehicle S to the target point P2 (x2, y2) from the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction θt. ) To obtain the target turning radius rg for use in the following equation (8).
[0125]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sin θt− (y2−y1) · cos θt} / 2 (8)
When rg> 0, the vehicle turns right, and when rg <0, the vehicle turns left.
[0126]
The moving vehicle S described above is assumed to be a steering four-wheel vehicle.
[0127]
Here, the moving vehicle S is replaced with an equivalent two-wheeled vehicle S0, and the steering of the moving vehicle S will be described with reference to FIG.
[0128]
In FIG. 6, the equivalent two-wheeled vehicle S0 includes a steering wheel S1 and a fixed wheel S2. The distance (wheel base) between the center axis A of the steered wheel S1 and the center axis B of the fixed wheel S2 is L. The turning radius rt of the turning circle and the tangential direction of the turning circle at the time when the equivalent two-wheeled vehicle S0 turns is always at a right angle R. The steering angle θt is equal to APcB around the turning center point Pc of the turning circle.
[0129]
Therefore, the current turning radius rt of the equivalent two-wheeled vehicle S0 is expressed by the following equation (9).
rt = L / tan θt (9)
It is represented by
[0130]
Therefore, the target steering angle ζ at the target turning radius rg from the above equation (9) is
ζ = tan^-1(L / rg) (10)
Is required.
[0131]
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7) to control the steering angle of the equivalent two-wheeled vehicle S0.
[0132]
As a result, the moving vehicle S can be directed between the next end points PR2 and PL2.
[0133]
However, the processing from step 101 to step 107 shown in FIG. 3 is repeated about 10 times per second. As a result, the traveling direction of the moving vehicle S is constantly corrected, so that the moving vehicle S can be directed in the direction between the next end points PR2 and PL2.
[0134]
By the way, the moving vehicle S can be steered using the turning radius even if it is not a steering system.
[0135]
FIG. 7 is a diagram illustrating a case where the skid steer type moving vehicle S is steered. The skid steer method is also called a “differential drive method”.
[0136]
The skid steer type moving vehicle S0 ′ shown in FIG. 7 includes two drive wheels SR and SL that can have different rotational speeds on the left and right. And it becomes possible to turn by making the rotational speeds vR and vL of the drive wheels SR and SL different from each other.
[0137]
Here, the turning radius rc of the moving vehicle S0 'is expressed by the following equation (11) from the vehicle width W and the speeds vR and vL of the drive wheels SR and SL.
[0138]
rc = W (vL + vR) / 2 (vL-vR) (11)
That is, when the moving vehicle S0 'goes straight in order to pass the target point P2 (x2, y2), the steering command calculation means 5 gives vR = vL as a steering command to the moving vehicle S0' (vehicle body 7). Is output.
[0139]
Similarly, when turning the moving vehicle S0 ′ in order to pass the target point P2 (x2, y2), the above equation (8) (shown below) is used.
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sin θt− (y2−y1) · cos θt} / 2 (8)
The steering command calculation means 5 obtains a turning radius rc with rg = rc and outputs the turning radius rc as a steering command to the moving vehicle S0 '(vehicle body 7).
[0140]
Further, the vehicle speed command calculating means 6 calculates the vehicle center speed vc using the following equation (12).
[0141]
vc = (vR + vL) / 2 (12)
Then, the vehicle center speed vc is output as a travel command from the vehicle speed command calculation means 6 to the moving vehicle S0 '(vehicle body 7).
[0142]
The moving vehicle S0 ′ (vehicle body 7) receives these steering command and travel command and back-calculates the speeds vR and vL from the turning radius rc and the vehicle center speed vc obtained by the above equations (11) and (12). , Adjust the speed.
[0143]
As a result, even the skid steer type moving vehicle S0 ′ can be moved between the next end points PR2 and PL2.
[0144]
When the moving vehicle S0 'is steered from a state where the vehicle travels straight at the maximum speed vmax, the drive wheels S1 or S2 need to be set to the maximum speed vmax or more. Since the maximum speed vmax cannot be exceeded, only the drive wheel to be driven at a high speed out of the drive wheels S1 or S2 is driven at the maximum speed vmax. For the other drive wheel, the speed of the moving vehicle S0 'is adjusted by calculating backward the speed using the above equation (11) in the moving vehicle S0' so as to maintain the desired turning radius rc.
[0145]
Next, a description will be given of a process of causing the moving vehicle S that is turning to travel without being steered toward the end points PR2 and PL2.
[0146]
In step 105, it is determined whether or not the moving vehicle S traveling in a turn can pass between the next end points PR2 and PL2 after reaching the end point PR1 and PL1.
[0147]
That is, in step 105, it is determined whether or not the traveling condition selection means 4 in FIG.
[0148]
Here, if the moving vehicle S can pass between the end points PR2 and PL2 without being steered (YES at step 105), the traveling condition selection means 4 provides the steering command with the data for causing the moving vehicle S to proceed as it is. It outputs to the calculation means 5 (step 107).
[0149]
The process performed by the traveling condition selection means 4 will be described with reference to FIG.
[0150]
In FIG. 8, the moving vehicle S is turning with a traveling direction θt and a turning radius rt.
[0151]
The traveling condition selecting means 4 confirms whether or not the moving vehicle S has reached the upper side between the end points PR1 and PL1 (on the line segment G1) as shown in FIG. Then, after the time when the moving vehicle S reaches between the end points PR1 and PL1 (on the line segment G1), the distance rR from the turning center point Pc (xc, yc) to the end point PR2 (xR2, yR2), and the turning center point A distance rL from Pc (xc, yc) to the end point PL2 (xL2, yL2) is obtained.
[0152]
The distance rR is
rR = √ {(xR2−xc)²+ (YR2-yc)²} (13)
It is represented by
[0153]
On the other hand, the distance rL is
rL = √ {(xL2−xc)²+ (YL2-yc)²} (14)
It is represented by
[0154]
Further, the current motion measuring means 2 measures the speed vt and the turning angular speed ωt of the moving vehicle S to obtain the turning radius rt.
[0155]
Here, if the turning radius rt is longer than the distance rR, the sign of the difference λrR between the distance rR and the turning radius rt shown in the following equation (15) becomes negative.
[0156]
λrR = rR− | rt | (15)
(Here, the turning radius rt is an absolute value because the sign changes between turning right and turning left.)
On the other hand, if the turning radius rt is shorter than the distance rL, the sign of the difference λrL between the distance rL and the turning radius rt shown in the following equation (16) is positive.
[0157]
λrL = rL− | rt | (16)
If the turning radius rt passes through one of the end points PR2 (xR2, yR2) and PL2 (xL2, yL2), either λrR or one of λrL becomes zero.
[0158]
Therefore, the product of the above formula (15) and the formula (16) shown in the following formula (17)
λ = λrR · λrL (17)
Thus, the traveling condition selection means 4 determines that the turning radius rt passes between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) when λ ≦ 0.
[0159]
Thereby, the traveling condition selection means 4 determines that the moving vehicle S can pass between the end points PR2 and PL2 without being steered (determination YES in step 105).
[0160]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0161]
Next, a process of steering the moving vehicle S that is turning and advancing it between the pair of end points PR2 and PL2 will be described.
[0162]
When the moving vehicle S cannot pass between the end points PR2 and PL2 (NO in Step 105), the traveling condition selection unit 4 outputs data for steering the moving vehicle S to the steering command calculation unit 5.
[0163]
The process performed by the traveling condition selection means 4 will be described with reference to FIG.
[0164]
In FIG. 8, as described above, the distance rR from the turning center point Pc (xc, yc) to the end point PR2 (xR2, yR2) and the turning center point Pc (xc, yc) to the end point PL2 (xL2, yL2). Distance rL and the turning radius rt of the moving vehicle S are respectively obtained.
[0165]
In addition, as shown in FIG. 4 described above, the traveling condition selection means 4 confirms whether or not the moving vehicle S has reached the upper side between the end points PR1 and PL1 (on the line segment G1).
[0166]
Here, if the turning radius rt is shorter than the distance rR, the sign of λrR in the equation (15) (shown below) becomes positive.
[0167]
λrR = rR− | rt | (15)
On the other hand, when the turning radius rt is shorter than the distance rR, the turning radius rt is naturally shorter than the distance rL. Therefore, the sign of λrL in the above equation (16) (shown below) is positive.
[0168]
λrL = rL− | rt | (16)
Therefore, when the turning radius rt is shorter than the distances rR and rL, λ in the above equation (17) (shown below) is
λ = λrR · λrL (17)
Become positive. (Λ> 0)
If the turning radius rt is longer than the distance rL, the sign of λrL in the above equation (16) becomes negative.
[0169]
On the other hand, when the turning radius rt is longer than the distance rL, the turning radius rt is naturally longer than the distance rR. Therefore, the sign of λrR in the above equation (15) is negative.
[0170]
Therefore, when the turning radius rt is longer than the distances rR and rL, λ in the above equation (17) is positive. (Λ> 0)
Thereby, the traveling condition selection means 4 determines that the turning radius rt cannot pass between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) when λ> 0. (Decision NO in step 104).
[0171]
Therefore, the travel condition selection means 4 is the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 as described above, and the current position P1 of the moving vehicle S acquired from the current motion measurement means 2 ( x1, y1) and the data of the traveling direction θt are output to the steering command calculation means 5.
[0172]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0173]
Here, as described above with reference to FIGS. 5 and 6, the steering command calculation means 5 obtains the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction θt. From these data, a target turning radius rg for moving the moving vehicle S toward the target point P2 (x2, y2) is obtained using the above equation (8) (shown below).
[0174]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sin θt− (y2−y1) · cos θt} / 2 (8)
Then, the target steering angle ζ at the target turning radius rg is obtained from the above equation (10) (shown below).
[0175]
ζ = tan^-1(L / rg) (10)
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7), and controls the steering angle of the moving vehicle S.
[0176]
Thereby, the moving vehicle S that is turning can be directed between the next end points PR2 and PL2.
[0177]
As described above, in the above-described embodiment, when the moving body S reaches between the pair of end points PR1 and PL1, the moving body S is directed between the next pair of end points PR2 and PL2. Even if the planned traveling route K of the body S is an acute curve, the moving body S can be guided without causing a small turning phenomenon.
[0178]
In the above-described embodiment, the steering angle ζ when the moving vehicle S0 moves forward can be made the same as the steering angle ζ when the vehicle moves backward.
[0179]
FIG. 9A is a view showing a case where the moving vehicle S0 is moved forward toward the center point Pn of the target gate Gn, and FIG. 9B is a view showing the moving vehicle S0 toward the center point Pn of the target gate Gn. It is a figure which shows the case where it makes it retreat.
[0180]
As shown in FIGS. 9A and 9B, the steering angle ζ of the moving vehicle S0 is the same when moving forward and when moving backward.
[0181]
Therefore, in the above-described embodiment, the control characteristics when the moving vehicle moves forward and backward can be made the same. Further, the control of the steering angle ζ can be simplified.
[0182]
By the way, the road surface of the planned travel route K of the moving vehicle S is uneven, or there are obstacles on the road and the vehicle cannot pass between the target pair of end points PLn and PRn. It may come off.
[0183]
Accordingly, when the moving vehicle S cannot pass between the target pair of end points PLn and PRn, an embodiment in which the moving vehicle S is directed between the next pair of end points PLn and PRn will be described with reference to FIGS. 10 and 11. explain. For the sake of explanation, a pair of target end points PLn and PRn are expressed by gates.
[0184]
In FIG. 10, since the moving vehicle S has deviated from the current target gate Gn, it reaches the extension line Gb of the current target gate Gn.
[0185]
Therefore, when the moving vehicle S reaches the extension line of the current target gate Gn, the moving vehicle S is directed to the next target gate Gn + 1.
[0186]
As a result, even when the road surface of the planned travel route K of the moving vehicle S is uneven or an obstacle exists on the road and cannot pass through the target gate Gn, the mobile vehicle S can be returned to the planned travel route K. it can. However, when the moving vehicle S deviates from the current target gate Gn as described above, it is desirable to reduce the speed of the moving vehicle S.
[0187]
In FIG. 11, the representative point of the moving vehicle S changes to Pt-1, Pt, Pt + 1, and the distance between these points Pt-1, Pt, Pt + 1 and the current end point of the target gate Gn. When the moving vehicle S moves such that changes to rt-1, rt, rt + 1, the moving vehicle S is directed to the next target gate Gn + 1.
[0188]
As a result, even when the road surface of the planned travel route K of the moving vehicle S is uneven or an obstacle exists on the road and cannot pass through the target gate Gn, the mobile vehicle S can be returned to the planned travel route K. it can. However, when the moving vehicle S deviates from the current target gate Gn as described above, it is desirable to reduce the speed of the moving vehicle S.
[0189]
In the above-described embodiment, the case where the target data on the planned travel route K is a pair of end points has been described, but the target data on the planned travel route K may be an arbitrary geometric figure.
[0190]
2 will be described with reference to FIGS. 3 and 12 when the target data on the planned travel route K is an arbitrary geometric figure.
[0191]
FIG. 12 is a diagram showing an embodiment in which a plurality of target data of the planned travel route K is a plurality of arbitrary geometric figures Mn (n = 1 to 3).
[0192]
In FIG. 12, an arbitrary geometric figure Mn is set as a target on the planned travel route K.
[0193]
Note that the processing performed in steps 101 to 103 in the flowchart of FIG. 3 is the same as the processing in the case where the target data on the planned travel route K is a pair of end points, and thus description thereof is omitted.
[0194]
First, a description will be given of a process for causing the moving vehicle S traveling straight ahead to proceed to an arbitrary geometric figure M2 without steering.
[0195]
In step 104, the moving vehicle S traveling straight advances for a predetermined distance after first detecting an arbitrary geometric figure M1, and reaches a predetermined point P1 in the arbitrary geometric figure M1. After the time, it is determined whether or not it can pass through the next arbitrary geometric figure M2.
[0196]
At this time, in step 104, it is determined whether or not the traveling condition selection means 4 of FIG.
[0197]
Here, when the moving vehicle S can pass through the arbitrary geometric figure M2 as it is without being steered (determination YES in step 104), the traveling condition selection means 4 steers data for allowing the moving vehicle S to proceed as it is. It outputs to the command calculation means 5 (step 107).
[0198]
The processing of the traveling condition selection means 4 for causing the moving vehicle S traveling straight ahead to proceed into an arbitrary geometric figure M2 without steering will be described with reference to FIG.
[0199]
This processing is performed with reference to FIG. 5 described above except that a pair of end points PRn and PLn are placed at a position perpendicular to the traveling direction θt of the moving vehicle S in an arbitrary geometric figure M2 with the target point P2 interposed therebetween. This is the same as the processing described.
[0200]
Also in FIG. 13, the equation of a straight line that becomes a trajectory when the moving vehicle S goes straight is expressed by the above equation (4) (shown below).
[0201]
(X−x1) · tan θt− (y−y1) = 0 (4)
Therefore, when the straight line represented by the above equation (4) is used as the boundary line, the end point PR2 (xR2, yR2) in the arbitrary geometric figure M2 shown in FIG. 13 is more than the straight line of the equation (4). Since it exists on the lower side, it is included in the negative region. On the other hand, the end point PL2 (xL2, yL2) is included in the positive region because it exists above the straight line of the equation (4).
[0202]
Further, the equation of a straight line passing through the point P1 (x1, y1) and the end point PR2 (xR2, yR2) is expressed by the above equation (5) (shown below).
[0203]
λR = (xR2−x1) · tan θt− (yR2−y1) (5)
Further, the equation of a straight line passing through the point P1 (x1, y1) and the end point PL2 (xL2, yL2) is expressed by the above equation (6) (shown below).
[0204]
λL = (xL2−x1) tan θt− (yL2−y1) (6)
Here, since the straight line in the equation (5) passes through the negative region, the sign of λR becomes negative. On the other hand, since the straight line in the equation (6) passes through the positive region, the sign of λL becomes positive.
[0205]
Therefore, when the above equation (7) (shown below) is obtained,
λ = λR · λL (7)
The sign of λ is negative.
[0206]
Further, if the straight line of the above equation (4) passes through one of the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) in an arbitrary geometric figure M2, λ becomes zero.
[0207]
Therefore, it can be seen that when λ ≦ 0, the straight line of the above equation (4) passes between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) in an arbitrary geometric figure M2.
[0208]
Thereby, the traveling condition selection means 4 determines that the moving vehicle S can pass through the arbitrary geometric figure M2 as it is without steering (determination YES in step 104).
[0209]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0210]
Next, a process of steering the moving vehicle S traveling straight and moving it toward the end points PR2 and PL2 in an arbitrary geometric figure M2 will be described.
[0211]
When the moving vehicle S cannot pass between the end points PR2 and PL2 in the arbitrary geometric figure M2 (determination NO in step 104), the traveling condition selection means 4 uses the steering command calculation means as data for steering the moving vehicle S. 5 is output.
[0212]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0213]
As shown in FIG. 12 described above, the traveling condition selection means 4 advances a predetermined distance after the moving vehicle S first detects the present arbitrary geometric figure M1, and the traveling condition selection means 4 within the arbitrary geometric figure M1. It is confirmed whether or not a predetermined point P1 has been reached.
[0214]
Here, the above formula (4) (shown below) is
(X−x1) · tan θt− (y−y1) = 0 (4)
When existing above the end points PR2 and PL2 shown in FIG. 13, the end points PR2 and PL2 in these arbitrary geometric figures M1 are included in the negative region.
[0215]
That is, the signs of λR and λL are negative.
[0216]
Therefore, when λ is obtained using the above equation (7) (shown below),
λ = λR · λL (7)
The sign of λ is positive.
[0217]
Further, when the above equation (4) exists below the end points PR2 and PL2 shown in FIG. 13, these end points PR2 and PL2 are included in the positive region.
[0218]
That is, the signs of λR and λL are positive.
[0219]
Therefore, when λ is obtained using the above equation (7), the sign of λ becomes positive.
[0220]
That is, when λ> 0, the straight line in the formula (4) does not pass between the end points PR2 and PL2.
[0221]
Thereby, the traveling condition selection means 4 determines that the moving vehicle S cannot pass between the end points PR2 and PL2 in the arbitrary geometric figure M1 in this state (determination NO in step 104).
[0222]
Therefore, the travel condition selection means 4 is the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 as described above, and the current position P1 of the moving vehicle S acquired from the current motion measurement means 2 ( x1, y1) and the data of the traveling direction θt are output to the steering command calculation means 5.
[0223]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0224]
Here, as described above with reference to FIGS. 6 and 13, the steering command calculation means 5 obtains the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction θt. From these data, a target turning radius rg for moving the moving vehicle S toward the target point P2 (x2, y2) is obtained using the above equation (8) (shown below).
[0225]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sin θt− (y2−y1) · cos θt} / 2 (8)
Then, the target steering angle ζ at the target turning radius rg is obtained from the above equation (10) (shown below).
[0226]
ζ = tan^-1(L / rg) (10)
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7), and controls the steering angle of the moving vehicle S.
[0227]
As a result, the moving vehicle S traveling straight can be directed to the next arbitrary geometric figure M2.
[0228]
Next, a description will be given of a process of causing the moving vehicle S that is turning to travel to an arbitrary geometric figure M2 without steering.
[0229]
In step 105, the moving vehicle S that is turning travels for a predetermined distance after first detecting an arbitrary geometric figure M1, and reaches a predetermined point P1 in the arbitrary geometric figure M1. Determine whether or not.
[0230]
That is, in step 105, it is determined whether or not the traveling condition selection means 4 in FIG.
[0231]
Here, when the moving vehicle S can be directed to the arbitrary geometric figure M2 as it is without being steered (YES in Step 105), the traveling condition selection means 4 causes the moving vehicle S to proceed as it is. Data is output to the steering command calculation means 5 (step 107).
[0232]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0233]
In FIG. 14, the moving vehicle S is turning in a traveling direction Θt and a turning radius rt.
[0234]
Further, it is assumed that a pair of end points PR2 (XR2, YR2) and PL2 (XL2, YL2) exist at positions perpendicular to the traveling direction θt of the moving vehicle S in an arbitrary geometric figure M2.
Here, the distance between the end points PR2 (XR2, YR2) and PL2 (XL2, YL2) is defined as d.
[0235]
The center point of these end points PR2 and PL2 is P2 (X2, Y2). Further, the distance from the center point P2 (X2, Y2) when the moving vehicle S passes between the end points PR2, PL2 is defined as a lateral shift η.
[0236]
This lateral deviation η travels a predetermined distance after the moving vehicle S first detects the present arbitrary geometric figure M1, and reaches a predetermined point P1 in the arbitrary geometric figure M1. After the time, it is obtained using the following formulas (18) and (19).
[0237]
That is, when the turning radius rt is larger than 0 from the turning radius rt obtained by the current motion measuring means 2 and the traveling direction Θt of the moving vehicle S,
η = Y−rt cos Θt + √ {rt²-(X + rtsinΘt))²} (18)
And when the turning radius rt is smaller than 0,
η = Y−rt cos Θ t−√ {rt²-(X + rtsinΘt))²} (19)
Is required.
[0238]
Further, the direction vector of Θt in the equations (18) and (19) indicates the current traveling direction θt of the moving vehicle S at the target point P2 (X2, Y2) in the next arbitrary geometric figure M2. The coordinate is converted according to the traveling direction, and is expressed by the following equation (20).
[0239]

Further, the coordinate axes X and Y are obtained by coordinate conversion of the coordinate axes x and y and are expressed by the following equation (21).
[0240]

In order to obtain the direction vectors cos θ2 and sin θ2 of θ2 in the above equation (21), first, from the current position P1 (x1, y1) and the target point P2 (x2, y2) of the moving vehicle S, the following equation (22): Cosφt and sinφt are obtained using equation (23).
[0241]
cosφt = (x2−x1) / {(x2−x1)²+ (Y2−y1)² } (22)
sin φt = (y2−y1) / {(x2−x1)²+ (Y2−y1)² } (23)
Then, the direction vectors cos θ2 and sin θ2 of θ2 are obtained from these cos φt and sin φt using the following equations (24) and (25).
[0242]
cos θ2 = (cos²φt-sin²φt) · cos θt + 2 sin φt · cos φt · sin θt (24)
sin θ2 = + 2 sin φt · cos φt · cos θt− (cos²φt-sin²φt) ・ sinθt ... (25)
Here, the relationship between the lateral shift η obtained using the above equations (18) and (19) and the distance d between the end points PR2 (XR2, YR2) and PL2 (XL2, YL2) is −d. When / 2 <η <d / 2, the traveling condition selection means 4 determines that the moving vehicle S passes between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) in an arbitrary geometric figure M2. To do.
[0243]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0244]
Next, a process of steering the moving vehicle S that is turning and moving it toward an arbitrary geometric figure M2 will be described.
[0245]
When the moving vehicle S cannot pass through the arbitrary geometric figure M 2 (NO in Step 105), the traveling condition selection unit 4 outputs data for steering the moving vehicle S to the steering command calculation unit 5.
[0246]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0247]
As described above, the lateral deviation η travels a predetermined distance after the moving vehicle S first detects the present arbitrary geometric figure M1, and the predetermined point P1 in the arbitrary geometric figure M1. From the point of time when the value is reached, it is obtained using the above formulas (18) and (19) (shown below).
[0248]
η = Y−rt cos Θt + √ {rt²-(X + rtsinΘt))²} (18)
η = Y−rt cos Θ t−√ {rt²-(X + rtsinΘt))²} (19)
Here, when the lateral shift η obtained using the above formulas (18) and (19) is not included in d between the end points PR2 (XR2, YR2) and PL2 (XL2, YL2), the lateral shift The relationship between the shift η and the distance d between the end points PR2 and PL2 is η <−d / 2 and d / 2 <η.
[0249]
Thus, the traveling condition selection means 4 determines that the moving vehicle S cannot pass between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2) in the arbitrary geometric figure M2. (Decision NO in step 104).
[0250]
Therefore, the travel condition selection means 4 is the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 as described above, and the current position P1 of the moving vehicle S acquired from the current motion measurement means 2 ( x1, y1) and the data of the traveling direction Θt are output to the steering command calculation means 5.
[0251]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0252]
Here, as described above with reference to FIGS. 6 and 13, the steering command calculation means 5 obtains the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction Θt. From these data, a target turning radius rg for moving the moving vehicle S toward the target point P2 (x2, y2) is obtained using the above equation (8) (shown below).
[0253]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sinΘt− (y2−y1) · cosΘt} / 2 (8)
Then, the target steering angle ζ at the target turning radius rg is obtained from the above equation (10) (shown below).
[0254]
ζ = tan^-1(L / rg) (10)
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7), and controls the steering angle of the moving vehicle S.
[0255]
Thereby, the moving vehicle S that is turning can be directed to the next arbitrary geometric figure M2.
[0256]
As described above, in the above-described embodiment, the moving body S has traveled a predetermined distance after first detecting the arbitrary geometric figure M1, and has reached the predetermined point P1 in the arbitrary geometric figure M1. At this point, the moving object S is directed to the next arbitrary geometric figure M2, so even if the target on the planned travel route K of the moving object S is not a pair of end points but an arbitrary geometric figure. Similarly to the case of the pair of end points, the moving body S can be guided without causing the small turning phenomenon.
[0257]
Further, in this embodiment, similarly to the case of the pair of end points, the steering angle ζ when the moving vehicle S moves forward and the steering angle ζ when the vehicle moves backward can be made the same.
[0258]
FIG. 15 (a) is a diagram showing a case where the moving vehicle S0 is advanced toward the target point Pn of an arbitrary geometric figure Mn, and FIG. It is a figure which shows the case where the moving vehicle S0 is moved backwards.
[0259]
As shown in FIGS. 15 (a) and 15 (b), the steering angle ζ of the moving vehicle S0 is the same during forward movement and backward movement.
[0260]
Therefore, in this embodiment, the control characteristics when the moving vehicle S moves forward and backward can be made the same as in the case of the pair of end points. Further, the control of the steering angle ζ can be simplified.
[0261]
In the embodiment described above, the target data on the planned travel route K has been described as an arbitrary geometric figure, but the target data may naturally be circular.
[0262]
2 will be described with reference to FIGS. 3 and 16 when the target data on the planned travel route K is circular.
[0263]
FIG. 16 is a diagram showing an embodiment in which the target data of the planned travel route K is a circle C1.
[0264]
In FIG. 16, the circle C1 is set as a target on the planned travel route K.
[0265]
Note that the processing performed in steps 101 to 103 in the flowchart of FIG. 3 is the same as the processing in the case where the target data on the planned travel route K is a pair of end points or an arbitrary geometric figure, and thus description thereof is omitted.
[0266]
First, a process for causing the moving vehicle S traveling straight ahead to proceed toward the circle C2 without steering will be described.
[0267]
In step 104, after the time when the moving vehicle S traveling straight advances for a predetermined distance after the current circle C1 is first detected and reaches a predetermined point P1 in the circle C1, it is within the next circle C2. It is determined whether it can pass through.
[0268]
At this time, in step 104, it is determined whether or not the traveling condition selection means 4 of FIG.
[0269]
Here, if the moving vehicle S can pass through the circle C2 as it is without being steered (YES in step 104), the traveling condition selection means 4 uses the steering command calculation means 5 to make the data for making the moving vehicle S travel as it is. (Step 107).
[0270]
The processing of the traveling condition selection means 4 for causing the moving vehicle S traveling straight ahead to proceed into the circle C2 without steering will be described with reference to FIG.
[0271]
In FIG. 17, the equation of a straight line that becomes a trajectory when the moving vehicle S travels straight is expressed by the above-described equation (4) (shown below).
[0272]
(X−x1) · tan θt− (y−y1) = 0 (4)
The equation of the circle C2 having the center point P2 (x2, y2) and the radius d / 2 is expressed by the following equation (26).
[0273]
(X-x2)²+ (Y−y2)²= (D / 2)²... (26)
Therefore, the equations (4) and (26) are regarded as simultaneous equations, and a solution of the simultaneous equations is obtained.
[0274]
Therefore, if the solution of the simultaneous equations is a real number solution, it can be seen that the straight line of the above equation (4) intersects with the circle C2.
[0275]
As a result, the traveling condition selection means 4 determines that the moving vehicle S can pass through the circle C2 without being steered (determination YES in step 104).
[0276]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0277]
Next, a process of steering the moving vehicle S traveling straight and moving toward the circle C2 will be described.
[0278]
When the moving vehicle S cannot pass through the circle C2 (NO in Step 104), the traveling condition selection unit 4 outputs data for steering the moving vehicle S to the steering command calculation unit 5.
[0279]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0280]
As shown in FIG. 17 described above, the traveling condition selection means 4 determines whether the moving vehicle S has traveled a predetermined distance after first detecting the current circle C1 and has reached a predetermined point P1 in the circle C1. Confirm whether or not.
[0281]
Here, when the moving vehicle S is steered toward the circle C2 (NO in Step 104), the traveling condition selection means 4 determines the current relative position of the moving vehicle S with respect to the circle C2 with respect to the steering command calculation means 5. Output (step 107).
[0282]
The processing of the traveling condition selection means 4 that steers the traveling vehicle S that is traveling straight and proceeds toward the circle C2 will be described with reference to FIG.
[0283]
In FIG. 17, the solution of simultaneous equations of the above equations (4) and (26) (shown below) is obtained.
[0284]
(X−x1) · tan θt− (y−y1) = 0 (4)
(X-x2)²+ (Y−y2)²= (D / 2)²... (26)
Here, if the solution of the simultaneous equations is not a real number solution, it can be seen that the straight line of the above equation (4) and the circle C2 of the above equation (26) do not intersect.
[0285]
Thereby, the traveling condition selection means 4 determines that the moving vehicle S cannot pass through the circle C2 in this state (determination NO in step 104).
[0286]
Therefore, the travel condition selection means 4 is the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 as described above, and the current position P1 of the moving vehicle S acquired from the current motion measurement means 2 ( x1, y1) and the data of the traveling direction θt are output to the steering command calculation means 5.
[0287]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0288]
Here, as described above with reference to FIGS. 6 and 13, the steering command calculation means 5 obtains the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction θt. From these data, a target turning radius rg for moving the moving vehicle S toward the target point P2 (x2, y2) is obtained using the above equation (8) (shown below).
[0289]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sin θt− (y2−y1) · cos θt} / 2 (8)
Then, the target steering angle ζ at the target turning radius rg is obtained from the above equation (10) (shown below).
[0290]
ζ = tan^-1(L / rg) (10)
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7), and controls the steering angle of the moving vehicle S.
[0291]
As a result, the moving vehicle S traveling straight can be directed to the next circle C2.
[0292]
Next, a description will be given of a process of causing the moving vehicle S that is turning to travel as it is toward the circle C2 without being steered.
[0293]
In step 105, it is determined whether or not the moving vehicle S that is turning travels a predetermined distance after the current circle C1 is first detected and reaches a predetermined point P1 in the circle C1.
[0294]
That is, in step 105, it is determined whether or not the traveling condition selection means 4 in FIG.
[0295]
Here, if the moving vehicle S can be directed to the circle C2 without being steered (YES at Step 105), the traveling condition selection means 4 provides the steering command with the data for causing the moving vehicle S to proceed as it is. It outputs to the calculation means 5 (step 107).
[0296]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0297]
In FIG. 18, it is assumed that the moving vehicle S turns in a traveling direction Θt and a turning radius rt and is directed into a circle C2 having a center point P2 (X2, Y2) and a radius d / 2.
Further, the distance from the center point P2 when the moving vehicle S passes the diameter d of the circle C2 is defined as a lateral shift η.
[0298]
This lateral shift η is equal to or greater than (18) after the time point when the moving vehicle S travels a predetermined distance after the current circle C1 is first detected and reaches a predetermined target point P1 within the circle C1. , (19) (shown below).
[0299]
That is, when the turning radius rt is larger than 0 from the turning radius rt obtained by the current motion measuring means 2 and the traveling direction Θt of the moving vehicle S,
η = Y−rt cos Θt + √ {rt²-(X + rtsinΘt))²} (18)
And when the turning radius rt is smaller than 0,
η = Y−rt cos Θ t−√ {rt²-(X + rtsinΘt))²} (19)
Is required.
[0300]
Here, when the relationship between the lateral deviation η obtained using the above equations (18) and (19) and the radius d / 2 of the circle C2 is −d / 2 <η <d / 2 The traveling condition selection means 4 determines that the moving vehicle S passes through the circle C2.
[0301]
Based on the determination result of the traveling condition selection means 4, the steering command calculation means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).
[0302]
Next, a description will be given of a process of steering the moving vehicle S that is turning to advance toward the circle C2.
[0303]
When the moving vehicle S cannot pass through the circle M2 (NO in Step 105), the traveling condition selection unit 4 outputs data for steering the moving vehicle S to the steering command calculation unit 5.
[0304]
Processing performed by the traveling condition selection unit 4 will be described with reference to FIG.
[0305]
As described above, after the time point when the lateral deviation η travels a predetermined distance after the moving vehicle S first detects the current circle C1 and reaches the predetermined point P1 in the circle C1, the above ( 18), (19) (shown below).
[0306]
η = Y−rt cos Θt + √ {rt²-(X + rtsinΘt))²} (18)
η = Y−rt cos Θ t−√ {rt²-(X + rtsinΘt))²} (19)
Here, when the lateral shift η obtained using the above equations (18) and (19) is not included in the diameter d of the circle C2, the lateral shift η and the radius d / 2 of the circle C2 Of η <−d / 2 and d / 2 <η.
[0307]
As a result, the traveling condition selection means 4 determines that the moving vehicle S cannot pass through the circle C2. (Decision NO in step 104).
[0308]
Therefore, the travel condition selection means 4 is the data of the target point P2 (x2, y2) acquired from the planned travel route storage means 1 as described above, and the current position P1 of the moving vehicle S acquired from the current motion measurement means 2 ( x1, y1) and the data of the traveling direction Θt are output to the steering command calculation means 5.
[0309]
The steering command calculation means 5 performs a calculation for steering the moving vehicle S (step 106).
[0310]
Here, as described above with reference to FIGS. 6 and 17, the steering command calculation means 5 obtains the acquired target point P2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction Θt. From these data, a target turning radius rg for moving the moving vehicle S toward the target point P2 (x2, y2) is obtained using the above equation (8) (shown below).
[0311]
rg = {(x2-x1)²+ (Y2−y1)²} / {(X2−x1) · sinΘt− (y2−y1) · cosΘt} / 2 (8)
Then, the target steering angle ζ at the target turning radius rg is obtained from the above equation (10) (shown below).
[0312]
ζ = tan^-1(L / rg) (10)
The steering command calculation means 5 outputs the target steering angle ζ thus determined as a steering command to the moving vehicle S (vehicle body 7), and controls the steering angle of the moving vehicle S.
[0313]
Accordingly, the moving vehicle S that is turning can be directed to the circle C2.
[0314]
As described above, in the above-described embodiment, when the moving body S detects the circle C1 for the first time and travels a predetermined distance and reaches the predetermined point P1 in the circle C1, the next circle C2 is reached. Since the moving body S is made to face, even if the target on the planned travel route K of the moving body S is a pair of end points, it moves without causing a small turning phenomenon as in the case of the pair of end points. The body S can be induced.
[0315]
Further, in this embodiment, similarly to the case of the pair of end points, the steering angle ζ when the moving vehicle S moves forward and the steering angle ζ when the vehicle moves backward can be made the same.
[0316]
FIG. 19 (a) is a diagram showing a case where the moving vehicle S0 is moved forward toward the target point (center point of the circle Cn) Pn in the circle Cn, and FIG. 19 (b) shows the target point Pn of the circle Cn. It is a figure which shows the case where the moving vehicle S0 is moved backwards.
[0317]
As shown in FIGS. 19 (a) and 19 (b), the steering angle ζ of the moving vehicle S0 is the same during forward movement and backward movement.
[0318]
Therefore, in this embodiment, as in the case of the pair of end points, the control characteristics when the moving vehicle S0 moves forward and backward can be made the same. Further, the control of the steering angle ζ can be simplified.
[0319]
Next, generation of target data on the planned travel route K will be described.
[0320]
For example, target data of a pair of end points is generated from point sequence data consisting of a point Pn (xn, yn) and a circle with a radius dn / 2 centered on the point Pn (xn, yn).
[0321]
Here, a pair of end points will be described as a gate.
[0322]
Further, the point sequence data is created by CAD (Computer Automation Design) or teaching playback as described above.
[0323]
The data converted from the point sequence data to the gate is stored in the planned traveling route storage means 1 as target data.
[0324]
FIG. 20A is a diagram showing a case where the density of the point sequence data is sufficiently dense compared with the curvature of the planned route K, and FIG. It is a figure which shows the case where it becomes sparse compared with.
[0325]
As shown in the travel route Ka2 in FIG. 20B, when the density of the point sequence data is sparse compared to the curvature of the planned route K, the curve of the planned travel route K cannot be reproduced.
[0326]
Therefore, in describing the generation of the target data on the planned travel route K, the density of the point sequence data for generating the target data is sufficiently higher than the curvature of the planned route K, as shown in FIG. It shall be dense.
[0327]
The density of the point sequence data for generating the target data is such that the curve of the planned travel route K can be sufficiently reproduced as shown by the travel route Ka1 when the three adjacent target points are connected by an arc.
[0328]
Therefore, assuming point sequence data in which three adjacent target points are arranged in a straight line, when the moving vehicle S passes around the first target point P1 and heads for the next target point P2, the target point P2 A case where the target gate G2 is generated from will be described with reference to FIG.
[0329]
FIG. 21 is a diagram showing point sequence data in which three target points are arranged in a straight line.
[0330]
As shown in FIG. 21, when the three target points are aligned, the direction vector from the target point P1 to P2 and the direction vector from P1 to P3 are the same.
[0331]
Therefore, the relationship between these direction vectors is expressed by the following equation (27).
[0332]
(X3−x1) / (y3−y1) = (x2−x1) / (y2−y1) (27)
Further, when the equation (27) is modified, the following equation (28) is obtained.
[0333]
(X3-x1). (Y2-y1)-(x2-x1). (Y3-y1) = 0 (28)
Here, when the above equation (28) is established, the three target points P1, P2, and P3 exist on a straight line.
[0334]
Further, the direction vector of the target gate G2 generated from the target point P2 is orthogonal to the direction vector from the target point P1 (x1, y1) to P2 (x2, y2). Therefore, the inclination θ2 of the target gate G2 is expressed by the following equation (29).
[0335]

Therefore, when the planned travel route K is a straight line, the target gate G2 generated from the target point P2 has a center point P2 (x2, y2) and a width d2 (the diameter of a circle centered on the target point P2 (x2, y2)). ) And the direction vector (cos θ2, sin θ2) represented by the above equation (29).
[0336]
By the way, if the above expression (28) (shown below) is not established, the three points of the target points P1, P2, and P3 are not on a straight line, so that the moving vehicle passes through these target points P1, P2, and P3. For example, S must turn while drawing an arc.
[0337]
(X3-x1). (Y2-y1)-(x2-x1). (Y3-y1) = 0 (28)
FIG. 22 is a diagram showing point sequence data in which three target points P1, P2, and P3 are arranged on the circumference of a turning circle of the moving vehicle S.
[0338]
In this case, as shown in FIG. 22, the planned travel route K is a curve.
[0339]
Therefore, assuming point sequence data in which the next three target points are lined up on the circumference of the turning circle of the moving vehicle S, the moving vehicle S passes around the first target point P1 and the next target point. A case where the target gate G2 is generated from the target point P2 when going to the point P2 will be described with reference to FIGS.
[0340]
FIG. 23 is a diagram showing a circle passing through the three target points P1, P2, and P3 and the inclination of the target gate.
[0341]
Here, the direction vector (cos θ2, sin θ2) of the target gate G2 generated from the target point P2 is from the center point P0 (x0, y0) of the circle in FIG. 23 to the center point P2 (x2, y2) of the target gate G2. It is equal to the vector facing (target point P2).
[0342]
Therefore, the direction vector (cos θ2, sin θ2) of the target gate G2 is expressed by the following equation (30).
[0343]

Further, the radius r0 of the circle of the center point P0 (x0, y0) passing through the center point P2 (x2, y2) of the target gate G2 is expressed by the following equation (31).
[0344]
r0 = √ {(x2−x0)²+ (Y2−y0)²  } (31)
Here, the setting of the center point P0 (x0, y0) of the circle will be described.
[0345]
FIG. 24 is a diagram for explaining the setting of the center point P0 (x0, y0) of a circle passing through the three target points P1, P2, P3.
[0346]
The center point P0 (x0, y0) is a straight line that bisects the line connecting the target points P1 and P2 vertically.
This is the intersection of T and a straight line S that bisects the line connecting the target points P2 and P3 vertically.
[0347]
Here, the straight line T is represented by the following equation (32), while the straight line S is represented by the following equation (33).
[0348]

Therefore, the coordinates of the center point P0 (x0, y0) that is the intersection of the straight lines T and S are expressed by the following equation (34).
[0349]

However,
t0 = {(-xb + xd) .yc-(-yb + yd) .xc} / (xa.yc-ya.xc)
... (35)

Therefore, when three adjacent target points P1, P2, and P3 exist on the circumference, the target gate G2 generated from the target point P2 has a center point P2 (x2, y2) and a width d2 (target point P2 ( x2 and y2), and a direction vector (cos θ2, sin θ2) expressed by the above equation (30). Further, the radius r0 of the circle of the center point P0 (x0, y0) passing through the center point P2 (x2, y2) of the target gate G2 can be expressed by the above equation (31).
[0350]
As described above, in the present embodiment, when the planned traveling route K ahead of the moving vehicle S is a curve, the target gate is generated from the point sequence data. The amount of data can be reduced compared to the case where the data is generated and stored. As a result, the calculation for generating the target gate from the point sequence data is easier than the case of performing the calculation for generating all of the point sequence data in the target gate.
[0351]
Further, in the present embodiment, the target gate is generated from the point sequence data for a plurality of target point sequences scheduled to pass immediately before the moving vehicle S. For one target point scheduled to pass, a target gate may be generated from the point sequence data.
[0352]
In the present embodiment, when the planned traveling route K ahead of the moving vehicle S is a curve, the target gate is generated from the point sequence data for a plurality of point sequences scheduled to pass immediately before the moving vehicle S. However, even if the planned traveling route K ahead of the moving vehicle S is a straight line, the target gate is generated from the point sequence data for one or more point sequences scheduled to pass immediately before the moving vehicle S. May be.
[0353]
Further, even if the planned traveling route K of the moving body S is an acute curve, the moving body S can be guided without causing a small turning phenomenon.
[0354]
By the way, when the moving vehicle S travels on the circumference of a circle having a center point P0 (x0, y0) and a radius r0 through three adjacent target points P1, P2, P3, the following example (A) In the cases of (b) and (c), it is necessary to appropriately output a deceleration command from the vehicle speed command calculation means 6 so that the moving vehicle S is less than the allowable speed.
[0355]
Example (A): When the moving vehicle S travels at a high speed on a curve with a small turning radius r0, there is a case where centrifugal force is generated and the vehicle body slides sideways and cannot bend the curve. In this case, it is necessary to automatically decelerate the moving vehicle to a turnable speed in advance.
[0356]
Example (b): If the moving vehicle S is a steering vehicle at the moment when the moving vehicle S enters a curve with a small turning radius r0 from a straight run, passes through the target gate G1, and moves toward the next target gate G2, steering is performed. You must make a sharp turn from going straight. At this time, if the moving vehicle S is traveling at a high speed, the curve may not be able to bend even if the steering is suddenly turned. In this case, it is necessary to automatically decelerate the moving vehicle to a steerable speed in advance.
[0357]
Example (c): In the cases (a) and (b) above, even if the moving vehicle S is decelerated greatly so as to go to the target gate G2 when it passes through the target gate G1, the moving vehicle S is greatly reduced. Since the vehicle is traveling at high speed, the curve around the target gate G2 may not be able to bend.
[0358]
Therefore, when generating the target gate from the point sequence data, it is necessary to generate the target gate from the point sequence data not at the time of passing through the target gate G1 but at a sufficiently early time when the moving vehicle can be decelerated or stopped safely. .
[0359]
Now, when there is an unscheduled obstacle on the planned travel route, it is necessary to take measures such as avoiding the moving vehicle from the obstacle or decelerating and stopping the moving vehicle immediately before the obstacle.
[0360]
Therefore, an embodiment in which data different from the target data on the planned travel route is generated when the moving vehicle is avoided from an obstacle or the moving vehicle is decelerated and stopped immediately before the obstacle is described with reference to FIGS. I will explain.
[0361]
FIG. 25 is a diagram illustrating an embodiment in which data different from target data on a planned travel route is generated when a moving vehicle is avoided from an obstacle.
[0362]
In this embodiment, the target data is point sequence data, and data different from the target data is also point sequence data.
[0363]
In FIG. 25, when the moving vehicle S travels sequentially through the target points P1, P2, P3, P4, P5, P6, and P7 on the scheduled travel route K, the distance αm in front of the moving vehicle S ( It is assumed that an obstacle SB exists between the target points P3 and P4 on the planned travel route K). In FIG. 25, it is assumed that the oncoming vehicle TK travels from the right front of the moving vehicle S.
[0364]
Here, when there is an obstacle SB on the planned travel route K, the obstacle detection means 8 in FIG. 2 detects information on the obstacles shown in the following (d), (e), (f), and (g). .
[0365]
(D) The obstacle SB exists in the range of ± β degrees in the left-right direction with the distance αm in front of the moving vehicle S and the front of the route in the center.
[0366]
(E) Since an oncoming vehicle runs from the right front of the moving vehicle S, it is dangerous to avoid it on the right side.
[0367]
(F) There is no obstacle in the left front of the moving vehicle S.
[0368]
(G) No other vehicles are approaching from the left rear of the moving vehicle S.
[0369]
The obstacle detection means 8 outputs information related to the obstacles shown in (d), (e), (f), and (g) to the scheduled travel route changing means 3.
[0370]
The scheduled travel route changing unit 3 receives the information on the obstacle from the obstacle detecting unit 8 and acquires information on the current motion state of the moving vehicle S from the current motion measuring unit 2. Then, based on the information on the obstacles and the information on the current motion state of the moving vehicle S, the traveling condition selection means 4 is notified to the obstacle SB at a point a distance α from the moving vehicle S on the planned traveling route K. Therefore, change the planned travel route K to the left with respect to the direction of travel.A new obstacle is found in the route when returning to the original planned travel route K while avoiding the obstacle SB. If it is possible to return to the original planned travel route S, return to the original planned travel route K as it is. ”Information regarding the change of the planned travel route K is output.
[0371]
The travel condition selecting means 4 is notified by the planned travel route changing means 3 that “the obstacle SB exists on the planned travel route K at a distance α from the moving vehicle S, so that it is avoided in the direction of left β degrees with respect to the traveling direction. And get ready to stop. "
[0372]
Based on this information, the traveling condition selection means 4 outputs traveling condition data to the vehicle speed command calculation means 6 so as to decelerate as much as possible within a range where safety can be secured immediately. The vehicle speed command calculation means 6 outputs a travel command so as to decelerate the moving vehicle S. Further, the traveling condition selection means 4 is configured to perform the planned traveling so that the moving vehicle S can travel from the point of the distance αm from the moving vehicle S in the direction of the left β degrees with respect to the traveling direction by the width of the moving vehicle S. Points VP2, VP3, VP4 (failure avoidance points), VP5 (stoppable points), and VP6 (safe stop points) different from the target points P2, P3, P4, P5, P6, and P7 on the route K are generated.
[0373]
VP4 (failure avoidance point) is a point where the target point P4 is moved in the direction of left β degrees.
[0374]
VP5 (stoppable point) is a point where the moving vehicle S can stop even if the road surface condition is bad.
[0375]
VP6 (safety stop point) is the next point away from VP5.
[0376]
After generating the points VP2, VP3, VP4, VP5, and VP6, the traveling condition selecting means 4 generates further gates from the points VP2, VP3, VP4, VP5, and VP6.
[0377]
Then, the traveling condition selection unit 4 determines the relative position of the current moving vehicle with respect to the gate based on the information on the current motion state of the moving vehicle S and the information on the change of the planned traveling route K described above. On the other hand, the traveling condition is output to the vehicle speed command calculating means 6.
[0378]
Further, a steering command (a steering angle necessary for passing through the gate) is output from the steering command calculation means 5 to the moving vehicle S, and a travel command (necessary for passing through the gate) is output from the vehicle speed command calculation means 6. Output).
[0379]
As a result, the moving vehicle S is guided to pass through the gates generated from the points VP2, VP3, VP4, VP5, and VP6.
[0380]
However, the travel command during the avoiding operation of the moving vehicle S is a speed that can be stopped at the point VP5.
[0381]
Next, a case where the moving vehicle S is returned from the obstacle SB and returned to the planned travel route K will be described with reference to FIG.
[0382]
FIG. 26 is a diagram illustrating a case where the moving vehicle S is avoided from the obstacle SB and returned to the planned travel route K.
[0383]
In FIG. 26, return gates TG6 and TG7 are generated on the return travel route K1 between the point VP4 and the target point P7.
[0384]
Here, first, the obstacle detection means 8 confirms that there is no obstacle at the place where the return travel route K1 is to be formed.
[0385]
Then, the planned traveling route changing unit 3 outputs information “no obstacle in the visual field range of the obstacle sensor” to the traveling condition selecting unit 4.
[0386]
Based on this information, the travel condition selection means 4 generates return gates TG6 and TG7 on the return travel route K1 for returning the moving vehicle S from the current avoidance travel route K0 to the planned travel route K. At the same time, the speed of the moving vehicle S is returned to the original speed.
[0387]
As a result, the moving vehicle S passes through the return gates TG6 and TG7, returns to the planned travel route K, and runs away from the point P4 where the obstacle SB exists.
[0388]
In this way, in the above-described embodiment, it is not necessary to stop the moving vehicle at a dangerous point where there is an obstacle, so it is possible to prevent a rear-end collision caused by the moving vehicle and the following vehicle.
[0389]
As described above, when the obstacle SB is found on the planned travel route K, the moving vehicle S needs to be decelerated.
[0390]
Here, between the point P1 which is the current position of the moving vehicle S and the point VP4 where the avoidance is completed, an operation of rapidly turning the steering wheel while decelerating the moving vehicle S by applying a brake is necessary.
[0390]
Therefore, points VP2 and VP3 are generated from the point P1 which is the current position of the moving vehicle S to immediately before avoidance, and the radius r0 of the turning circle of the moving vehicle S passing through these points VP2 and VP3 is safely steered at the current speed. The turning radius is sufficiently large.
[0392]
In this embodiment, the current position of the moving vehicle S is described as being located at the point P1, and the position of the obstacle SB is present between the target points P3 and P4. It is desirable to always run at a speed at which the vehicle can be stopped and steered, and decelerate before point P1 if the speed is high.
[0393]
Next, in addition to the above-described embodiment, an embodiment in which the moving vehicle is avoided from the obstacle, or the moving vehicle is decelerated and stopped immediately before the obstacle will be described.
[0394]
FIG. 27 is a diagram illustrating an embodiment in which data different from target data on a planned travel route is generated when a moving vehicle is put in a garage.
[0395]
In FIG. 27, the travel condition selection means 4 is configured to have gates TG1, TG2 different from the target gates G1, G2, G3 when the moving vehicle S that has passed the target gates G1, G2 on the planned travel route K reaches the target gate G3. Is generated.
[0396]
Therefore, the moving vehicle S is directed from the target gate G3 to the gates TG1 and TG2.
[0397]
When the vehicle reaches the gate TG2, the moving vehicle S is moved backward to the gates PG1 and PG2 generated in the garage SK.
[0398]
Thus, in this embodiment, the moving vehicle S traveling on the planned travel route K can be moved backward and placed in the garage SK.
[0399]
FIG. 28 is a diagram showing an embodiment in which data different from target data on a planned travel route is generated when a moving vehicle traveling on the planned travel route is merged with a travel route different from the planned travel route.
[0400]
In FIG. 28, the travel condition selection means 4 generates a gate TG1 different from the target gate G1 when it reaches the target gate G1 on the planned travel route K.
[0401]
Therefore, the moving vehicle S is directed to the gate TG1.
[0402]
When the vehicle reaches the gate TG1, the moving vehicle S is directed toward the target gates G2 'and G3' on the planned travel route Kb.
[0403]
Thus, in this embodiment, the moving vehicle S traveling on the planned travel route K can be joined to a travel route Kb different from the planned travel route K.
[0404]
FIG. 29 is a diagram illustrating an embodiment in which when an obstacle is present in front of a moving vehicle traveling on a planned traveling route, the moving vehicle is decelerated in front of the obstacle.
[0405]
In FIG. 29, when the travel condition selection means 4 reaches the target gate G2 on the planned travel route K, the travel condition selection means 4 generates gates TG1, TG2 different from the target gates G1, G2 before the obstacle SB '.
[0406]
Therefore, the moving vehicle S is sequentially decelerated at the gates TG1 and TG2.
[0407]
Thus, in this embodiment, when the obstacle SB ′ exists in front of the moving vehicle S, the moving vehicle S can be decelerated before the obstacle.
[0408]
FIG. 30 is a diagram showing an embodiment for avoiding an obstacle by generating data different from the target data on the planned traveling route when an obstacle exists on the planned traveling route of the moving vehicle.
[0409]
In FIG. 30, when the traveling condition selection means 4 reaches the target gate G1 on the planned traveling route K, the traveling vehicle S can avoid the obstacle SB 'through the gates TG1, TG2, and TG3 different from the target gate G1. Generate to the right of the direction of travel.
[0410]
Therefore, the moving vehicle S is directed to the gates TG1, TG2, and TG3.
[0411]
When the vehicle reaches the gate TG3, the moving vehicle S is directed to the target gate G2 on the original scheduled travel route K.
[0412]
Thus, in this embodiment, when the obstacle SB ′ exists on the planned travel route K of the moving vehicle S, the obstacle SB ′ can be avoided.
[0413]
FIG. 31 is a diagram illustrating an embodiment in which a moving vehicle is stopped in front of an obstacle when an obstacle is present in front of the moving vehicle traveling on the planned traveling route.
[0414]
In FIG. 31, when the travel condition selection means 4 reaches the target gate G2 on the planned travel route K, the travel condition selection means 4 generates gates TG1 and TG2 different from the target gates G1 and G2 before the obstacle SB '.
[0415]
Therefore, the moving vehicle S is rapidly decelerated at the gate TG1 and stopped at TG2 or TG3.
[0416]
Thus, in this embodiment, when the obstacle SB ′ is present in front of the moving vehicle S, the moving vehicle S can be stopped before the obstacle.
[0417]
FIG. 32 is a diagram showing an embodiment in which data different from the target data on the planned travel route is generated when a moving vehicle traveling on the planned travel route is stopped at the stop point.
[0418]
In FIG. 32, when the travel condition selection means 4 reaches the target gate G2 on the planned travel route K, the travel condition selection means 4 generates gates TG1, TG2 different from the target gates G1, G2 between the target gate G2 and the stop point Ps. TG3 is generated at the stop point Ps.
[0419]
Therefore, the moving vehicle S is decelerated while moving toward the gates TG1 and TG2, and stopped at the gate TG3.
[0420]
As described above, in this embodiment, the moving vehicle S traveling on the planned travel route K can be placed sideways on the stop point Ps and stopped.
[0421]
FIG. 33 shows an embodiment in which data that is different from the target data on the planned travel route is generated when the moving vehicle that has been stopped at the stop point is started, gradually accelerated, and returned to the planned travel route. FIG.
[0422]
In FIG. 33, the travel condition selection means 4 generates gates TG1, TG2, TG3 different from the target gates G1, G2 between the stop point Ps and the target gate G1 on the planned travel route K.
[0423]
Therefore, the moving vehicle S is gradually accelerated while moving toward the gates TG1, TG2, and TG3, and is directed toward the targets G1 and G2 on the scheduled travel route K.
[0424]
As described above, in this embodiment, the moving vehicle S that is stopped at the stop point Ps can be started, gradually accelerated, and returned to the planned travel route K.
[0425]
FIG. 34 is a diagram showing an embodiment in which when a vehicle traveling next to a moving vehicle approaches the moving vehicle, the moving vehicle is avoided from the approaching vehicle.
[0426]
In FIG. 34, when the traveling condition selecting means 4 reaches the target gate G1 on the planned traveling route K, the traveling vehicle S can avoid the vehicle Sx by using the gates TG1, TG2, TG3 different from the target gates G1, G2. Generate to the right of the direction of travel.
[0427]
Therefore, the moving vehicle S is directed to the gates TG1, TG2, and TG3.
[0428]
When the vehicle reaches the gate TG3, the moving vehicle S is directed to the target gate G2 on the original scheduled travel route K.
[0429]
As described above, in this embodiment, when the vehicle Sx traveling next to the moving vehicle S has approached the moving vehicle S, the moving vehicle S is avoided from the approaching vehicle Sx. Can do.
[0430]
FIG. 35 is a diagram showing an embodiment in which when the planned travel route of a moving vehicle is a sharp curve, the distance between the target gate and the target gate is narrowed to reduce the speed of the moving vehicle on the sharp curve.
[0431]
In FIG. 35, the distance WT between the target gates G3 and G4 and the distance WT between the target gates G3 and G4 generated on the sharp curve Kc are the distance W between the target gates G1 and G2 and the target gates G6 and G7. It is narrower than the interval W.
[0432]
Here, at each target gate, the steering command calculation means 5 performs the steering calculation when the moving vehicle S reaches each target gate. Therefore, the calculation is continued for the sharp curve Kc corresponding to the narrowing of the target gate interval. Will be done. As a result, since the calculation time becomes longer at the sharp curve Kc, the speed v at which the moving vehicle S passes the sharp curve Kc decreases. Thus, it can adjust so that the passage time between target gates may become constant.
[0433]
Therefore, in this embodiment, when the planned travel route K is a sharp curve, the speed v of the moving vehicle S can be reduced to prevent the wheel from being removed, and the moving vehicle S can travel safely.
[0434]
FIG. 36 is a diagram showing an embodiment in which the speed is lowered in order to reduce the centrifugal force exerted on the moving vehicle traveling along the planned traveling route with a sharp curve.
[0435]
In FIG. 36, the centrifugal force exerted on the moving vehicle S traveling along the sharply planned traveling route K is inversely proportional to the turning radius r0 and proportional to the speed v of the moving vehicle S.
[0436]
Further, when the moving vehicle S travels on a sharp curve, the turning radius r0 of the moving vehicle S decreases.
[0437]
For this reason, the centrifugal force exerted on the moving vehicle S traveling on the planned traveling route K having a sharp curve increases, and the moving vehicle S may cause a rollover accident or a slip accident.
[0438]
Therefore, it is necessary to reduce the speed v of the moving vehicle S.
[0439]
Therefore, the turning radius r0 of the moving vehicle S is obtained by the current motion measuring means 2 (speed v is detected by the vehicle speed sensor and ω is detected by the angular velocity sensor) (r0 = v / ω), and the turning radius is determined in the vehicle body 7 (moving vehicle S). If r0 is small, the speed v is adjusted to decrease.
[0440]
As a result, it is possible to prevent a rollover accident or a slip accident of the moving vehicle S traveling on the planned traveling route K having a sharp curve.
[0441]
In the above-described embodiment, tan θ is used for the slope of the linear equation shown in the above equations (1) and (4). However, when θ is an integral multiple of 90 degrees, the straight line is on the X axis or the Y axis. Become parallel. In this case, in order to determine the positional relationship between the straight line and the coordinates of the points, it is only necessary to compare only the X or Y coordinates.
[0442]
In the above-described embodiment, the point sequence data stored in the planned travel route storage unit 1 is used as the target point sequence. However, a mark such as a magnet on the actual road corresponding to the stored point sequence data. If a transmitter indicating the passing position of the moving vehicle S along the road is installed, the current position of the moving vehicle S can be accurately detected by the current motion measuring means 2.
[Brief description of the drawings]
FIG. 1 is a diagram showing an embodiment of a moving vehicle guidance device according to the present invention.
FIG. 2 is a block diagram showing a configuration of a moving vehicle guidance device according to the present invention.
FIG. 3 is a flowchart for explaining processing performed by the moving vehicle guidance device according to the present invention;
FIG. 4 is a diagram illustrating a process for confirming that a moving vehicle has passed between a pair of end points.
FIG. 5 is a diagram illustrating a process of causing a moving vehicle traveling straight toward a pair of end points.
FIG. 6 is a diagram illustrating steering of a moving vehicle by replacing the moving vehicle with an equivalent two-wheeled vehicle.
FIG. 7 is a diagram illustrating steering of a skid steer type moving vehicle.
FIG. 8 is a diagram illustrating a process of causing a moving vehicle that is turning to travel toward a pair of end points.
FIG. 9A is a diagram showing a case where the moving vehicle is moved forward toward the target gate, and FIG. 9B is a diagram showing a case where the moving vehicle is moved backward toward the target gate. .
FIG. 10 is a view showing a modification of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 11 is a view showing a modification of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 12 is a diagram showing an embodiment of a moving vehicle guidance device according to the present invention.
FIG. 13 is a diagram illustrating a process of causing a moving vehicle traveling straight toward an arbitrary geometric figure.
FIG. 14 is a diagram illustrating a process of causing a moving vehicle that is turning to travel toward an arbitrary geometric figure.
FIG. 15A is a diagram showing a case in which a moving vehicle is advanced toward an arbitrary geometric figure, and FIG. 15B is a diagram in which the moving vehicle is moved backward toward an arbitrary geometric figure. It is a figure which shows a case.
FIG. 16 is a diagram showing an embodiment of a moving vehicle guidance device according to the present invention.
FIG. 17 is a diagram for explaining a process of causing a moving vehicle traveling straight toward a circle.
FIG. 18 is a diagram illustrating a process of causing a moving vehicle that is turning to travel toward a circle.
FIG. 19A is a diagram showing a case where a moving vehicle is moved forward toward a circle, and FIG. 19B is a diagram showing a case where the moving vehicle is moved backward toward a circle.
FIG. 20A is a diagram showing a case where the density of the point sequence data is sufficiently dense compared to the curvature of the planned route, and FIG. 20B is a diagram where the density of the point sequence data is the planned route. It is a figure which shows the case where it becomes sparse compared with the curvature of K.
FIG. 21 is a diagram showing point sequence data in which three target points are arranged in a straight line.
FIG. 22 is a diagram showing point sequence data in which three target points are arranged on the circumference of a turning circle of a moving vehicle.
FIG. 23 is a diagram showing a circle passing through three target points and the inclination of the target gate.
FIG. 24 is a diagram illustrating the setting of the center point of a circle passing through three target points.
FIG. 25 is a diagram illustrating an embodiment in which data different from target data on a planned travel route is generated when a moving vehicle is avoided from an obstacle.
FIG. 26 is a diagram illustrating a case where a moving vehicle is avoided from an obstacle and is returned to a planned travel route.
FIG. 27 is a diagram showing a modification of the embodiment of the moving vehicle guidance device according to the present invention.
FIG. 28 is a diagram showing a modification of the embodiment of the guide device for a moving vehicle according to the present invention.
FIG. 29 is a diagram showing a modification of the embodiment of the guide device for a moving vehicle according to the present invention.
FIG. 30 is a diagram showing a modification of the embodiment of the guide device for a moving vehicle according to the present invention.
FIG. 31 is a diagram showing a modification of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 32 is a view showing a modified example of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 33 is a diagram showing a modified example of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 34 is a diagram showing a modified example of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 35 is a diagram showing a modification of the embodiment of the moving vehicle guidance device according to the present invention.
FIG. 36 is a diagram showing a modified example of the embodiment of the guidance device for a moving vehicle according to the present invention.
FIG. 37 is a diagram illustrating a conventional technique.
FIG. 38 (a) is a diagram showing a case where the equivalent two-wheeled vehicle is moved forward and guided to the target point, and FIG. 38 (b) is a case where the equivalent two-wheeled vehicle is moved backward and guided to the target point. FIG.
[Explanation of symbols]
S ... Mobile vehicle
K ... Planned route
PRn, PLn ... A pair of end points

Claims (5)

In the guidance device for a moving body that guides the moving body toward the next target when the current target is reached among a plurality of targets on the planned traveling route of the moving body,
The region between a pair of end points separated by a predetermined distance across the target point on the planned travel route is the target,
When the mobile body has reached between the current pair of end points, it is determined whether or not to reach the region between the next target pair of end points when traveling while maintaining the current steering angle,
If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle does not reach, the target steering angle for directing the moving body to the target point between the pair of target end points is calculated. An apparatus for guiding a moving body, wherein the steering angle is changed based on the target steering angle, and the moving body is then directed between a pair of target end points. In the moving body guidance device for guiding the moving body toward the next passing area when the current passing area is reached among the plurality of passing areas on the scheduled traveling route of the moving body,
When the mobile body reaches a predetermined point in the passing area, it is determined whether or not the next passing area is reached when traveling while maintaining the current steering angle;
If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle does not reach, the target steering angle for directing the moving body to the predetermined point in the next passing area is calculated and this is calculated. An apparatus for guiding a moving body, wherein the steering angle is changed based on a target steering angle, and the moving body is directed to the next passage region. Guidance of the moving body that guides the moving body toward the next target when the current target is reached among a plurality of targets on the planned traveling route of the moving body having the steering wheel and the fixed wheel In the device
The region between a pair of end points separated by a predetermined distance across the target point on the planned travel route is the target,
Using the wheel base of the steered wheel and the fixed wheel as a denominator, the steering angle of the steered wheel is obtained from an arc tangent function with the radius of the turning circle when the moving body turns to the next target as a numerator,
When the mobile body has reached between the current pair of end points, it is determined whether or not to reach the region between the next target pair of end points when traveling while maintaining the current steering angle,
If it is determined that the vehicle will reach the current steering angle, the current steering angle will be maintained. An apparatus for guiding a moving body, wherein the steering angle is changed based on the target steering angle, and the moving body is then directed between a pair of target end points. Movement that guides the moving body toward the next passing area when it reaches the current passing area among a plurality of passing areas on the planned traveling route of the moving body having the steering wheel and the fixed wheel. In the body guidance device,
Using the wheel base of the steered wheel and the fixed wheel as a denominator, the steering angle of the steered wheel is obtained from an arc tangent function with the radius of the turning circle when the moving body turns to the next target as a numerator,
When the mobile body reaches a predetermined point in the passing area, it is determined whether or not the next passing area is reached when traveling while maintaining the current steering angle;
If it is determined that the vehicle will reach the current steering angle, the current steering angle is maintained. If it is determined that the vehicle does not reach, the target steering angle for directing the moving body to the predetermined point in the next passing area is calculated and this is calculated. An apparatus for guiding a moving body, wherein the steering angle is changed based on a target steering angle, and the moving body is directed toward the next passing region. Generating a pair of end points for a point sequence corresponding to a curve among a plurality of target points on the planned travel route;
2. The moving body guide according to claim 1, wherein when the moving body reaches between a pair of end points centered on a target point, the moving body is directed between a pair of end points centered on a next target point. apparatus.

Images