JP7434627B2 - Movement control method of moving object - Google Patents
Movement control method of moving object Download PDFInfo
- Publication number
- JP7434627B2 JP7434627B2 JP2023023985A JP2023023985A JP7434627B2 JP 7434627 B2 JP7434627 B2 JP 7434627B2 JP 2023023985 A JP2023023985 A JP 2023023985A JP 2023023985 A JP2023023985 A JP 2023023985A JP 7434627 B2 JP7434627 B2 JP 7434627B2
- Authority
- JP
- Japan
- Prior art keywords
- movement
- straight
- moving
- schedule information
- route
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 75
- 238000005259 measurement Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 description 38
- 238000012545 processing Methods 0.000 description 30
- 238000005096 rolling process Methods 0.000 description 24
- 238000010586 diagram Methods 0.000 description 12
- 238000012937 correction Methods 0.000 description 11
- 230000003028 elevating effect Effects 0.000 description 8
- 230000003014 reinforcing effect Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 125000004122 cyclic group Chemical group 0.000 description 4
- 230000001788 irregular Effects 0.000 description 3
- 230000005856 abnormality Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000008602 contraction Effects 0.000 description 2
- 230000010365 information processing Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000003825 pressing Methods 0.000 description 1
- 230000002250 progressing effect Effects 0.000 description 1
- 238000007789 sealing Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
本発明は、移動体に設定した移動予定情報に基づいて移動体を移動させる移動体の移動制御方法に関する。 The present invention relates to a method for controlling the movement of a moving object, which moves the moving object based on movement schedule information set for the moving object.
本体に、障害物検知手段(センサ)、方向検知手段(センサ)、距離認識手段(センサ)、位置認識手段、緩衝手段(センサ)等を搭載し、これら各手段からの出力信号に基づいて移動体(自律走行装置)の走行を制御する移動体の移動制御方法が知られている(特許文献1参照)。 The main body is equipped with obstacle detection means (sensor), direction detection means (sensor), distance recognition means (sensor), position recognition means, buffer means (sensor), etc., and moves based on the output signals from each of these means. 2. Description of the Related Art A movement control method for a mobile body (autonomous mobile device) is known (see Patent Document 1).
上述した移動体の移動制御方法によれば、移動体に、多数のセンサと、当該多数のセンサからの情報に基づく制御プログラムとを搭載することにより、障害物との距離を保ちながら移動体を移動させることができるが、予め定めた移動予定経路に沿って移動体を移動させることはできないという課題があった。
本発明は、予め定めた移動予定経路に沿って移動体を精度よく移動させることができる移動体の移動制御方法を提供するものである。
According to the above-mentioned method for controlling the movement of a moving object, the moving object is equipped with a large number of sensors and a control program based on information from the large number of sensors, so that the moving object can be moved while maintaining a distance from obstacles. Although the mobile object can be moved, there is a problem in that the mobile object cannot be moved along a predetermined planned movement route.
The present invention provides a method for controlling the movement of a moving object, which allows the moving object to move accurately along a predetermined planned movement route.
本発明に係る移動体の移動制御方法は、移動対象面上を移動させる移動体の移動予定情報を設定する移動予定情報設定処理と、移動体を移動予定情報に基づいて移動させるとともに、移動情報取得手段により取得された移動体の実際の移動情報と移動予定情報とを比較して移動体の移動を制御する移動制御処理と、を備え、移動予定情報設定処理では、移動対象面上において直線経路の折り返しが繰り返されるジグザグ経路を辿って移動体を移動させるための移動予定情報を設定する移動体の移動制御方法であって、移動予定情報設定処理では、移動対象面上のジグザグ経路を設定するために、移動体の移動開始地点のXY座標値と、最初の直線経路の終点のXY座標値と、移動対象面のXY座標上における移動開始地点の対角地点として測定される対角測定点のXY座標値と、隣り合う直線経路間の距離の初期値とを設定することにより、ジグザグ経路における複数の直線経路が並ぶ方向の幅と直線経路間の距離の初期値とに基づいて直線経路の数の最適値を算出して設定することを特徴とする。
以上の移動体の移動制御方法によれば、移動予定情報に基づいて予め定めたジグザグ経路に沿って移動体を精度よく移動させることができるようになるとともに、移動体を移動させる予定のジグザグ経路を容易に設定できるようになる。
The movement control method of a moving object according to the present invention includes a movement schedule information setting process of setting movement schedule information of a moving object to be moved on a moving target surface, moving the moving object based on the movement plan information, and moving the moving object. a movement control process that controls the movement of the moving object by comparing the actual movement information of the moving object acquired by the information acquisition means with the movement schedule information, and the movement schedule information setting process includes: A moving object movement control method that sets movement schedule information for moving a moving object along a zigzag path in which a straight path is repeatedly turned back and forth. In order to set, the XY coordinate values of the movement start point of the moving object, the XY coordinate values of the end point of the first straight path, and the diagonal measured as the diagonal point of the movement start point on the XY coordinate of the movement target surface. By setting the XY coordinate values of the measurement point and the initial value of the distance between adjacent straight paths, the width of the zigzag path in the direction in which multiple straight paths are lined up and the initial value of the distance between the straight paths It is characterized by calculating and setting the optimal value of the number of straight paths.
According to the above-described movement control method for a moving object, it becomes possible to move the moving object with high precision along a predetermined zigzag route based on the movement schedule information, and the zigzag path along which the moving object is planned to move. can be easily set.
実施形態1
図1に示すように、実施形態1に係る移動体の移動制御方法は、移動対象面としての例えば建物内の床面F上を移動させる移動体1に移動予定情報を設定する移動予定情報設定処理と、移動体1を移動予定情報に基づいて移動させるとともに、移動情報取得手段としての自動追尾型のトータルステーション(以下、TSと言う)2により取得された移動体1の実際の移動情報(移動体の逐次位置情報)と移動予定情報とを比較して移動体1の移動を制御する移動制御処理と、を備えた方法である。
尚、本明細書においては、前、後、上、下、左、右は、図1,図2に示した方向と定義して説明する。
As shown in FIG. 1, the movement control method for a moving object according to the first embodiment includes movement schedule information setting in which movement schedule information is set for a moving
In this specification, front, back, top, bottom, left, and right will be defined as the directions shown in FIGS. 1 and 2.
移動予定情報設定処理においては、移動予定情報として、例えば、床面F上において複数の直線経路(レーン)L(L1,L2,L3…)の折返しが繰り返されるジグザグ経路Z(図7参照)を辿って移動体1を移動させるための移動予定情報を設定する。
In the movement schedule information setting process, for example, a zigzag route Z (see FIG. 7) in which multiple straight routes (lanes) L (L1, L2, L3...) are repeatedly turned back and forth on the floor F is set as the movement schedule information. The moving schedule information for moving the moving
尚、移動予定情報に基づいて予め設定されるジグザグ経路Wとは、例えば、床面F上における移動体1の移動開始地点S1(S1x,S1y)から移動体1を前進させる方向に沿って終点G1(G1x,G1y)まで延長する最初の直線経路L1と、最初の直線経路L1のG1(G1x,G1y)から移動体1を前進させる方向と直交する方向に沿って2番目の直線経路L2の開始地点S2まで延長する直線間経路M1と、2番目の直線経路L2の開始地点S2から移動体1を前進させる方向に沿って2番目の直線経路L2の終点G2まで延長する直線経路L2と、2番目の直線経路L2の終点G2から移動体1を前進させる方向と直交する方向に沿って3番目の直線経路L3の開始地点S3まで延長する直線間経路M2とからなる1サイクルの折返し経路が複数サイクル繰り返されて構成される移動予定経路のことである。
Note that the zigzag route W preset based on the movement schedule information is, for example, a path starting from a movement start point S1 (S1x, S1y) of the moving
当該移動体の移動制御方法を実現する移動体制御システムは、図1に示すように、床面F上を移動可能に構成された移動体1と、移動体1を移動させる床面F上での移動予定情報を設定するための移動予定情報設定手段としての例えばパーソナルコンピュータ等のコンピュータ3と、移動体1の実際の移動情報を取得して移動体1に送信するTS2とを備える。
As shown in FIG. 1, a mobile body control system that realizes the movement control method for a mobile body includes a
移動体1は、基体10と、基体10の下側に設けられた移動手段20と、基体10の表面側に設けられたTS2の視準となるプリズム等のターゲットT2と、基体10に設けられて移動体1の前側を昇降させる昇降装置40と、制御手段50とを備えている。
The moving
図2に示すように、移動手段20は、例えば、基体10の前側下部に設けられた左右の前側車輪21L,21Rと、基体10の後側下部に設けられた左右の後側車輪22L,22Rと、後側車輪22L,22Rの駆動源としてのモータ23L,23Rと、図外の駆動制御回路とを備える。
As shown in FIG. 2, the moving
尚、モータ23L,23Rの各モータ軸には、それぞれ、後側車輪22L,22Rの回転量に基づいて移動体1の移動距離(移動量)を検出するための移動量検出手段としてのエンコーダ25L,25Rが取付けられている。
Incidentally, each motor shaft of the motors 23L and 23R is provided with an encoder 25L as a movement amount detection means for detecting the movement distance (movement amount) of the moving
ターゲットT2は、TS2から発射される光を反射させる反射プリズム等で構成される。当該ターゲットT2は、例えば、図1,図2に示すように、基体10の上面の前側における左右間の中央位置に設置される。
The target T2 is composed of a reflecting prism or the like that reflects the light emitted from the TS2. For example, as shown in FIGS. 1 and 2, the target T2 is installed at the center position between the left and right sides on the front side of the upper surface of the
昇降装置40は、例えば、図3に示すように、基体10の前側の内側に設けられた取付部41に取付けられたリニアアクチュエータ42と、リニアアクチュエータ42のロッド43の下端(先端)に連結された補強体44と、補強体44の下端(先端)に設けられた転動体45と、補強体44を上下方向にガイドするガイド部46とを備えて構成される。
即ち、基体10の前側に設けられて移動体1の前側を昇降可能に構成された昇降装置40は、リニアアクチュエータ42のロッド43とロッド43の下端(先端)に連結された補強体44とにより構成されて上下方向に伸縮する伸縮手段と、伸縮手段の下端(先端)に設けられた転動体45とを備える。
換言すれば、昇降装置40は、リニアアクチュエータ42のロッド43を伸縮させることにより、取付部41から転動体45までの長さが変更するように構成された伸縮手段と、当該伸縮手段の下端(先端)に設けられた転動体45とを備えた構成である。
ガイド部46は、例えば、補強体44が上下移動するための通路となるガイド壁47と、補強体44の外周面に設けられてガイド壁47を摺動するシール部材48とにより構成される。
当該ガイド部46は、伸縮手段を、後述する移動体1の回転中心線10Cと平行を維持しながら当該回転中心線10Cに沿った上下方向に移動させるためのガイドとして機能するものである。
For example, as shown in FIG. 3, the
That is, the
In other words, the
The
The
制御手段50は、移動予定情報、TS2からの移動体1の実際の移動情報、エンコーダ25L,25Rからの情報に基づいて、後側車輪22L,22Rのモータ23L,23Rの駆動制御回路、及び、リニアアクチュエータ42のロッド43を伸縮させる図外の伸縮駆動源の駆動制御回路を制御することにより移動体1の移動を制御する移動制御プログラムと、当該移動制御プログラムによる情報処理を実現するコンピュータ等のハードウエア資源により構成される。
The control means 50 controls the drive control circuits of the motors 23L, 23R of the
制御手段50は、移動体1の移動方向(進行方向)を変更する際、図4(a)に示すように、昇降装置40のリニアアクチュエータ42のロッド43を縮退状態から伸長させて転動体45を床面Fに押し付けるとともに基体10の前側を上方に移動させて移動体1の前側車輪21L,21Rを床面Fから浮かせた状態で、図4(b)に示すように、左右の後側車輪22L,22Rのモータ23L,23Rを制御して床面Fに接触している左右の後側車輪22L,22Rを互いに反対方向に回転させる。
この場合、移動体1を前進させる回転方向に一方の後側車輪を回転させるとともに、移動体1を後進させる回転方向に他方の後側車輪を回転させることによって、移動体1の回転中心線10Cを回転中心として、左右の後側車輪22L,22Rと転動体45とが床面F上を転動するので、床面F上において、移動体1が回転中心線10Cを回転中心として左方向又は右方向にスムーズに回転する。よって、移動体1の水平方向の向きがスムーズに変更されるようになる。尚、回転中心線10Cは、左右の後側車輪22L,22Rにおける各車輪の中心を結ぶ直線の中間位置と直交して上下方向に延長するとともにロッド43の中心軸と平行な線である。
即ち、この場合、図4(d)に示すように、左の後側車輪22Lの床面Fとの接触面の中心点22Cと、右の後側車輪22Rの床面Fとの接触面の中心点22Cと、転動体45の床面Fとの接触面の中心点45Cとが、移動体1の回転中心線10Cを回転中心とする1つの円弧10R上に位置するように構成されるので、移動体1は、回転中心線10Cを回転中心として左方向又は右方向にスムーズに回転する。
移動体1の水平方向の向きが変更された後、制御手段50は、図4(c)に示すように、昇降装置40のリニアアクチュエータ42のロッド43を伸長状態から縮退させることによって、基体10の前側を下方に移動させて移動体1の左右の前側車輪21L,21Rを床面Fに接触させることにより、移動体1の移動方向を変更できる。
以上により、移動体1の移動方向を変更でき、その後、変更された移動方向に移動体1を移動させることができる。
即ち、昇降装置40と、昇降装置40及び左右の後側車輪22L,22Rの回転方向を制御する制御手段50とによって、移動体1の移動方向変更手段が構成される。
When changing the moving direction (progressing direction) of the
In this case, by rotating one rear wheel in the direction of rotation that moves the moving
That is, in this case, as shown in FIG. 4(d), the
After the horizontal direction of the
As described above, the moving direction of the moving
That is, the moving direction changing means of the
以下、図5乃至図19に基づいて、移動体1の移動制御方法の具体例を説明する。
移動体1の移動制御方法は、まず、移動体1を移動させる床面F上での移動予定情報を設定する移動予定情報設定処理を行った後、移動予定情報設定処理で設定された移動予定情報を移動体1の制御手段50に読み込ませて、制御手段50は読み込んだ移動予定情報とTS2からの逐次送信されてくる移動体1の実際の移動情報とに基づいて移動体1の移動制御処理を行う。
Hereinafter, a specific example of the movement control method for the
The movement control method for the moving
図5に基づいて移動予定情報設定処理を説明する。
・ステップS1
移動予定情報設定手段としての例えばコンピュータ3の設定画面31(図6参照)にキーボード等の入力手段を用いて、図7に示すような、床面F上に移動予定経路としてのジグザグ経路Zを設定するために、移動体1の移動開始地点S1のXY座標値(S1x,S1y)と、最初の直線経路(レーン)L1の終点G1のXY座標値(G1x,G1y)と、隣り合う直線経路L,L間の距離wと、直線経路Lの数Nとを設定する。
尚、当該XY座標値は、床面F上の所定の位置に固定されたTS2の固定位置を基準としたTS2から見たXY座標値であり、移動体1の制御手段50は、床面F上の位置を、当該TS2から見たXY座標値に基づいて管理する。
即ち、床面F上において、予め定めた、移動体1の移動開始地点S1と、最初の直線経路L1の終点G1とに、図外のターゲットを設置し、当該移動開始地点S1のXY座標値(S1x,S1y)と当該最初の直線経路L1の終点G1のXY座標値(G1x,G1y)とを、床面F上の所定の位置に固定されたTS2を使用して取得し、このTS2により取得されたXY座標値を設定者が設定画面31を介して設定する。
そして、移動体1は、移動開始地点S1の真上にターゲットT2を位置させるとともに、前進方向(移動方向)が最初の直線経路L1の終点G1に向かう方向となるように床面F上に設置された後、移動制御されることになる。
・ステップS2
設定画面31のスキャン開始ボタン32をクリックする。
・ステップS3
コンピュータ3が設定された移動予定情報をファイルにしてコンピュータ3の記憶装置に書き込む
・ステップS4
コンピュータ3が移動体1の制御手段50に移動予定情報を送信して制御手段50の記憶装置に読み込ませた後、コンピュータ3から移動体1の制御手段50に移動制御プログラムの実行コマンドを送信する。
The movement schedule information setting process will be explained based on FIG.
・Step S1
Using an input means such as a keyboard on the setting screen 31 (see FIG. 6) of the
Note that the XY coordinate values are the XY coordinate values seen from the TS2 with reference to the fixed position of the TS2 fixed at a predetermined position on the floor surface F, and the control means 50 of the moving
That is, on the floor F, a target (not shown) is installed at a predetermined movement start point S1 of the moving
The moving
・Step S2
Click the
・Step S3
After the
移動体1の制御手段50は、移動予定情報設定手段としてのコンピュータ3によって設定された移動予定情報、TS2からの移動体1の実際の移動情報、エンコーダ25L,25Rからの移動量情報に基づいて、移動体1の移動を制御する図8に示すような移動制御処理を行う。
移動制御処理においては、通常走行処理A、移動方向修正処理B、折返し処理C、イレギュラー発生時処理Dを行う。
The control means 50 of the
In the movement control process, normal travel process A, movement direction correction process B, return process C, and irregular occurrence process D are performed.
図9,図10を参照し、通常走行処理A(ステップS10~ステップS18)について説明する。
・ステップS10
制御手段50は、移動予定情報設定処理により設定された移動予定情報を記憶装置のファイルから読み込み、移動制御プログラムに基づいて、移動体1の移動制御処理を開始する。
・ステップS11
制御手段50が、移動体1の最新の位置情報PnのXY座標値(Pnx,Pny)をTS2から取得して、移動体1の位置情報を更新する。
即ち、制御手段50は、TS2から送信されてくる移動体1の最新の位置情報Pnを、例えば、100ms毎に受信して、移動体1の位置情報更新処理を行う。
・ステップS12(イレギュラー発生時処理Dに移行するか否かの判定)
移動体1の位置情報更新でエラーが発生したか否かを判定する。即ち、制御手段50が、TS2から移動体1の最新の位置情報Pnを取得できたか否かを判定する。移動体1の位置情報更新でエラーが発生したと判定した場合(ステップS12でYesの場合)、イレギュラー発生時処理Dに移行する。
・ステップS13
位置情報更新でエラーが発生していないと判定された場合(ステップS12でNoの場合)、即ち、制御手段50が、TS2から移動体1の最新の位置情報Pnを取得できた場合、制御手段50は、移動制御プログラムに基づいて、移動体1の軌跡ベクトルT、直線経路ベクトルC、ゴール判定用ベクトルJを計算する。
即ち、制御手段50は、図10に示すように、今回の直線経路(レーン)Lの開始地点Snから今回の直線経路Lの終点Gn(Gnx,Gny)までの有向線分であるベクトル、即ち、直線経路ベクトルCを計算するとともに、移動体1の最新位置(現在のターゲット(T2の)座標)Pnを取得する毎に、移動体1の1フェーズ前のターゲット(T2の)座標Pn-1(Pn-1x,Pn-1y)から移動体1の最新位置Pnまでの有向線分であるベクトル、即ち、軌跡ベクトルTを計算し、さらに、移動体1の最新位置Pnから今回の直線経路Lの終点Gnまでの有向線分であるベクトル、即ち、ゴール判定用ベクトルJを計算する。
・ステップS14
制御手段50は、移動制御プログラムに基づいて、移動体1の回転中心座標Pr、ゴール判定角θg、直線経路Lからのずれ距離E(移動予定情報と移動情報との位置ずれ量)、必要旋回角θrを計算する。
即ち、移動体1の回転中心座標Pr(Prx,Pry)は、図15に示すように、位置座標Pn(Pnx,Pny)と、位置座標Pn-1(Pn-1x,Pn-1y)と、式(3),(4)で示される軌跡ベクトルTとを用いて、式(1),(2)に基づいて求める。
また、ゴール判定角θg、及び、必要旋回角θrは、図16に示すように、2本のベクトルa,bがなす角度を求める方法により求まられる。
即ち、直線経路ベクトルCとゴール判定用ベクトルJとがなす角度であるゴール判定角θgを、図16の式(1)に基づいて求める。
また、移動体1の回転中心座標Prから今回の直線経路の終点Gnまでの有向線分であるベクトル、即ち、必要旋回角度判定用ベクトルMと軌跡ベクトルTとがなす角度である必要旋回角θrを、図16の式(1)に基づいて求める。
直線経路Lからのずれ距離Eは、図17に示すように、式(2)で示される直線経路ベクトルCと、点Pn(Pnx,Pny)とを用いて、式(1)に基づいて求める。即ち、式(1)のd=Eである。但し、式(1)のa,b,cは、式(3)に示されるとおりである。
・ステップS15(折返し処理Cに移行するか否かの判定)
移動体1が今回の直線経路Lの終点Gnにゴールしたか否かを判定する。即ち、今回の直線経路Lに沿った移動体1の移動制御処理が終了したか否かを判定する。
つまり、制御手段50は、移動制御プログラムに基づいて、ゴール判定角θg>90°であれば、移動体1が今回の直線経路Lの終点Gnにゴールしたと判定し、ゴール判定角θg≦90°であれば、移動体1が終点Gnに未だゴールしていないと判定する。移動体1が終点Gnにゴールしたと判定した場合(ステップS15でYesの場合)、折返し処理Cに移行する。
・ステップS16(移動方向修正処理Bに移行するか否かの判定)
移動体1が終点Gnに未だゴールしていないと判定された場合(ステップS15でNoの場合)、移動体1の直線経路Lからのずれ距離Eが所定値(例えば移動体1の左右幅寸法)以上で、かつ、移動体1の移動方向の向きが今回の直線経路ベクトルCから離れる方向に向いているか否かを判定する。そして、ずれ距離Eが所定値以上で、かつ、移動体1の移動方向の向きが今回の直線経路ベクトルCから離れる方向に向いていると判定された場合(ステップS16でYesの場合)、移動方向修正処理Bに移行する。
The normal driving process A (steps S10 to S18) will be explained with reference to FIGS. 9 and 10.
・Step S10
The control means 50 reads the movement schedule information set by the movement schedule information setting process from the file in the storage device, and starts the movement control process for the
・Step S11
The control means 50 acquires the XY coordinate values (Pnx, Pny) of the latest position information Pn of the
That is, the control means 50 receives the latest position information Pn of the
・Step S12 (determining whether to proceed to processing D when irregularity occurs)
It is determined whether an error has occurred in updating the location information of the
・Step S13
If it is determined that no error has occurred in the location information update (No in step S12), that is, if the control means 50 is able to obtain the latest location information Pn of the
That is, as shown in FIG. 10, the control means 50 generates a vector that is a directed line segment from the starting point Sn of the current straight line route (lane) L to the end point Gn (Gnx, Gny) of the current straight line route L; That is, each time the linear path vector C is calculated and the latest position (current target (T2) coordinate) Pn of the moving
・Step S14
Based on the movement control program, the control means 50 determines the rotation center coordinates Pr of the moving
That is, as shown in FIG. 15, the rotation center coordinates Pr (Prx, Pry) of the moving
Further, the goal determination angle θg and the required turning angle θr are determined by a method of determining the angle formed by two vectors a and b, as shown in FIG.
That is, the goal determination angle θg, which is the angle formed by the straight path vector C and the goal determination vector J, is determined based on equation (1) in FIG.
Also, a vector that is a directed line segment from the rotation center coordinate Pr of the moving
As shown in FIG. 17, the deviation distance E from the straight path L is determined based on the equation (1) using the straight path vector C shown by the equation (2) and the point Pn (Pnx, Pny). . That is, d=E in equation (1). However, a, b, and c in formula (1) are as shown in formula (3).
-Step S15 (determining whether to proceed to return process C)
It is determined whether the moving
That is, based on the movement control program, the control means 50 determines that the moving
-Step S16 (determining whether to proceed to movement direction correction processing B)
If it is determined that the moving
通常走行処理Aにおける移動体1の直進制御処理について説明する。
・ステップS17
ずれ距離Eが所定値よりも小さく、かつ、移動体1の移動方向の向きが直線経路ベクトルCから離れる方向に向いていないと判定された場合(ステップS16でNoの場合)、ゴール判定用ベクトルJの大きさに基づいて、左右の後側車輪22L,22Rを駆動するモータ23L,23Rへの駆動信号を調整する。例えば、ゴール判定用ベクトルJの大きさが1m以上であれば、モータ23L,23Rの出力を最大とし、ゴール判定用ベクトルJの大きさが1m未満でかつ0.5m以上であれば、モータ23L,23Rの出力を70%とし、ゴール判定用ベクトルJの大きさが0.5m未満であれば、モータ23L,23Rの出力を50%とするよう調整する。
・ステップS18
左右の後側車輪22L,22Rのエンコーダ25L,25Rの値の差が0になるように左右のモータ23L,23Rへの駆動信号を制御しつつ、移動体1を一定距離だけ前進させる。即ち、直進性を保ちながら、移動体1を一定距離だけ前進させる。
尚、上述したステップ17では、移動体1と直線経路Lの終点Gnとの距離が所定値になる毎に移動体1の移動速度を段階的に減速させる段階的減速制御を行う例を示したが、移動体1と直線経路Lの終点Gnとの距離が短くなるに従って移動体1の移動速度を徐々に減速させる連続的減速制御を行うようにしてもよい。また、移動体1と直線経路Lの終点Gnとの距離が所定値以下になった場合に所定の速度に減速する1回だけの減速制御を行うようにしてもよい。即ち、移動体1が直線経路Lの終点Gnに近づいた場合に移動体1の移動速度を1回以上減速する制御を行えばよい。
The straight-line control process of the moving
・Step S17
If the deviation distance E is smaller than a predetermined value and it is determined that the moving direction of the moving
・Step S18
The moving
Note that in step 17 described above, an example is shown in which stepwise deceleration control is performed in which the moving speed of the moving
図12,図10を参照し、移動方向修正処理B(ステップS20~ステップS22)について説明する。
・ステップS20
ステップS16でYesの場合、即ち、今回の直線経路Lからのずれ距離E(移動予定情報と移動情報との位置ずれ量)が所定値(例えば移動体1の左右幅寸法)以上で、かつ、移動体1の向きが今回の直線経路ベクトルCから離れる方向に向いていると判定された場合、昇降装置40のロッド43を伸長させて転動体45で床面Fを押圧することによって移動体1の左右の前側車輪21L,21Rを床面Fから浮かす。
・ステップS21
左右の後側車輪22L,22Rを互いに逆方向に回転させて移動体1の方向を軌跡ベクトルTの方向から必要旋回角θrだけ回転させることで、移動体1の移動方向を修正する(図10参照)。
・ステップS22
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かして、移動体1の左右の前側車輪21L,21Rを床面Fに接触(接面)させた後、移動体1を前進させる。
The movement direction correction process B (steps S20 to S22) will be described with reference to FIGS. 12 and 10.
・Step S20
If Yes in step S16, that is, the deviation distance E (positional deviation amount between movement schedule information and movement information) from the current straight path L is greater than or equal to a predetermined value (for example, the horizontal width dimension of the moving body 1), and If it is determined that the direction of the moving
・Step S21
The moving direction of the moving
・Step S22
After retracting the
・図14,図11を参照し、折返し処理C(ステップS30~ステップS50)について説明する。
・ステップS30
ステップS15でYesの場合、即ち、移動体1が終点Gnにゴールしたと判定された場合(今回の直線経路Lに沿った移動体1の移動制御処理が終了したと判定された場合)、設定された本数Nの直線経路Lに沿った移動体1の移動制御処理が全て終了したか否かを判定する。
ステップS30でNoの場合、即ち、移動制御処理が全て終了していないと判定された場合、次の直線経路Lの沿った移動体1の移動制御処理に移行するための折返し処理Cを行う。
・ステップS31
移動体1をジグザグ経路Zに沿って移動させるに当たって、隣り合う折返し地点(前の直線経路の終点Gn-1と次の直線経路Lの開始地点Snとのラインを揃えるため、移動体1が終点Gnにゴールしたと判定された時点から、移動体1の全長の2倍の距離だけ離れた方向変換点Pnr(Pnrx,Pnry)まで移動体1をそのまま前進させる。
・ステップS32
ステップS31での前進により移動体1の位置が変化しているため、TS2から移動体1の位置情報を最新の位置情報Pnrに更新する。
・ステップS33
位置情報更新の際にエラーが発生したか否かを判定する。
・ステップS34
位置情報更新でエラーが発生していない場合(ステップS33でNoの場合)、軌跡ベクトルTを再計算する。
・ステップS35
図11に示すように、次の直線経路Lまでの距離EX、旋回補正角θcを計算する。
次の直線経路Lまでの距離EXは、図17に示すように、式(2)で示される次の直線経路ベクトルCと、点P(=点Pnr)とを用いて、式(1)に基づいて求める。即ち、式(1)のd=EXである。
旋回補正角θc、即ち、前の直線経路ベクトルCと軌跡ベクトルTとがなす角度である旋回補正角θcは、図16の式(1)に基づいて求める。
・ステップS36
図18に示すように、前の直線経路(レーン)の開始地点Sn-1(Sn-1x,Sn-1y)、前の直線経路の終点Gn-1(Gn-1x,Gn-1y)、直線経路間の距離wを用いて、次の直線経路の開始地点Sn(Snx,Sny)、次の直線経路の終点Gn(Gnx,Gny)を求める。
次の直線経路の開始地点Sn(Snx,Sny)、次の直線経路の終点Gn(Gnx,Gny)は、図18の式(1)又は式(2)に基づいて求める。尚、図18に示すように、各直線経路は、基準となるXY座標のX軸からθだけ傾いているとする。そして、次の直線経路が前回の直線経路の進行方向に向かって右側にある場合は、次の直線経路の開始地点Sn(Snx,Sny)、次の直線経路の終点Gn(Gnx,Gny)は、図18の式(1)に基づいて求める。また、次の直線経路が前回の直線経路の進行方向に向かって左側にある場合は、次の直線経路の開始地点Sn(Snx,Sny)、次の直線経路の終点Gn(Gnx,Gny)は、図18の式(2)に基づいて求める。
・ステップS37
昇降装置40のロッド43を伸長させて転動体45で床面Fを押圧することによって、移動体1の左右の前側車輪21L,21Rを床面Fから浮かす。
-Return processing C (steps S30 to S50) will be explained with reference to FIGS. 14 and 11.
・Step S30
If Yes in step S15, that is, if it is determined that the moving
If No in step S30, that is, if it is determined that all movement control processing has not been completed, turnaround processing C is performed to move to movement control processing for the
・Step S31
When moving the moving
・Step S32
Since the position of the
・Step S33
Determine whether an error has occurred when updating location information.
・Step S34
If no error occurs in updating the position information (No in step S33), the trajectory vector T is recalculated.
・Step S35
As shown in FIG. 11, the distance EX to the next straight route L and the turning correction angle θc are calculated.
As shown in FIG. 17, the distance EX to the next straight route L can be calculated using the next straight route vector C shown in formula (2) and point P (=point Pnr) using formula (1). Find based on. That is, d=EX in equation (1).
The turning correction angle θc, that is, the turning correction angle θc, which is the angle formed by the previous straight path vector C and the trajectory vector T, is determined based on equation (1) in FIG.
・Step S36
As shown in FIG. 18, the starting point Sn-1 (Sn-1x, Sn-1y) of the previous straight route (lane), the end point Gn-1 (Gn-1x, Gn-1y) of the previous straight route, the straight line Using the distance w between the routes, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are determined.
The starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are determined based on equation (1) or equation (2) in FIG. As shown in FIG. 18, it is assumed that each straight line path is inclined by θ from the X axis of the reference XY coordinates. If the next straight route is on the right side in the direction of travel of the previous straight route, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are , is determined based on equation (1) in FIG. Also, if the next straight route is on the left side in the direction of travel of the previous straight route, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are , is determined based on equation (2) in FIG.
・Step S37
By extending the
・ステップS38
巡回パターンと直近の直線経路(レーン)の番号の組み合わせによる条件分岐
・条件1:右巡回パターン、かつ、直近の直線経路の番号が奇数の場合、もしくは、左巡回パターン、かつ、直近の直線経路の番号が偶数の場合。
・条件2:右巡回パターン、かつ、直近の直線経路の番号が偶数の場合、もしくは、左巡回パターン、かつ、直近の直線経路の番号が奇数の場合。
尚、右巡回パターンとは、1番目(最初)の直線経路から見て以降の直線経路が1番目の直線経路の右側に設定されるジグザグ経路Zに沿って移動体1を移動させるパターンを言い、左巡回パターンとは、1番目(最初)の直線経路から見て以降の直線経路が1番目の直線経路の左側に設定されるジグザグ経路Zに沿って移動体1を移動させるパターンを言う。
この左右の巡回パターンは、図6の設定画面上に設けられている図外の巡回パターン決定ボタンをクリックすることにより決定でき、コンピュータ3は、決定された巡回パターンを移動予定情報として移動体1の制御手段50に送信する。
・Step S38
Conditional branching based on the combination of the patrol pattern and the most recent straight route (lane) number ・Condition 1: If it is a right circuit pattern and the number of the most recent straight route is an odd number, or if it is a left circuit pattern and the most recent straight route If the number is even.
・Condition 2: When the number of the right circular route and the nearest straight route is an even number, or when the left circular pattern and the number of the nearest straight route is an odd number.
Note that the right-hand patrol pattern refers to a pattern in which the
This left and right patrol pattern can be determined by clicking a patrol pattern determination button (not shown) provided on the setting screen of FIG. The information is transmitted to the control means 50 of.
ステップS38での判定で条件1の場合には、移動体1を時計回りに旋回させる処理を行う。
・ステップS39
移動体1を、時計回りに、旋回補正角θc+90°だけ旋回させることにより、移動体1の軌道を修正する。
・ステップS40
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かして、移動体1の左右の前側車輪21L,21Rを床面Fに接触させる。
・ステップS41
次の直線経路Lまでの距離EX分だけ、移動体1を前進させる。
・ステップS42
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かす。
・ステップS43
移動体1を、時計回りに、90°だけ旋回させることにより、移動体1の軌道を修正する。
・ステップS44
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かして、移動体1の左右の前側車輪21L,21Rを床面Fに接触させ、移動体1を次の直線経路Lに沿って移動させる。
If the
・Step S39
The trajectory of the moving
・Step S40
The
・Step S41
The moving
・Step S42
The
・Step S43
The trajectory of the moving
・Step S44
The
ステップS38での判定で条件2の場合には、移動体1を反時計回りに旋回させる処理を行う。
・ステップS45
移動体1を、反時計回りに、旋回補正角θc+90°だけ旋回させることにより、移動体1の軌道を修正する。
・ステップS46
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かして、移動体1の左右の前側車輪21L,21Rを床面Fに接触させる。
・ステップS47
次の直線経路Lまでの距離EX分だけ、移動体1を前進させる。
・ステップS48
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かす。
・ステップS49
移動体1を、反時計回りに、90°だけ旋回させることにより、移動体1の軌道を修正する。
・ステップS50
昇降装置40のロッド43を縮退させて転動体45を床面Fから浮かして、移動体1の左右の前側車輪21L,21Rを床面Fに接触させ、移動体1を次の直線経路Lに沿って移動させる。
In the case of
・Step S45
The trajectory of the moving
・Step S46
The
・Step S47
The moving
・Step S48
The
・Step S49
The trajectory of the moving
・Step S50
The
図13を参照し、イレギュラー発生時処理D(ステップS60~ステップS65)について説明する。
・ステップS60
ステップS12でYesの場合、即ち、位置情報更新でエラーが発生したと判定された場合、位置情報が得られているときの軌跡ベクトルTとエンコーダ25L,25Rからの検出値とを用いて移動体1の自己位置を推定する。
即ち、図19に示すように、自己位置を見失う直前の位置座標Pn-1(Pn-1x,Pn-1y)と、位置座標Pn-1より1フェーズ前の位置座標Pn-2(Pn-2x,Pn-2y)とで、軌跡ベクトルTを定義し、直前の位置座標Pn-1を通過してから自己位置を見失うまでに増加したエンコーダ25L,25Rの値を用いて、前進した距離Dを推定し、移動体1の推定自己位置Pc(Pcx,Pcy)を,式(1),(2)に基づいて計算する。
・ステップS61
現在、移動体1が、直線経路Lの終点(ゴール)付近のいると推測されるか否かを判定する。
・ステップS62
現在、移動体1が、終点付近のいると推測されなかった場合、一定距離前進させる。
・ステップS63
現在、移動体1が、終点付近のいると推測された場合、停止して待つ。
・ステップS64
連続エラー回数が所定値(例えば10回)を超えたか、又は、連続エラー中に進んだ距離が所定値(例えば1m)を超えたか否かを判定する。連続エラー回数が所定値を超えたか、又は、連続エラー中に進んだ距離が所定値を超えた場合、終了する。また、連続エラー回数が所定値を超えておらず、かつ、連続エラー中に進んだ距離が所定値を超えていない場合は、ステップS11に戻る。
・ステップS65
ステップS33で、エラー発生と判断された後に、連続エラー回数が所定値(例えば10回)を超えたか否かを判定する。連続エラー回数が所定値を超えている場合、終了する。連続エラー回数が所定値を超えていない場合は、ステップS32に戻る。
With reference to FIG. 13, the irregular occurrence processing D (steps S60 to S65) will be described.
・Step S60
If Yes in step S12, that is, if it is determined that an error has occurred in updating the position information, the moving object is 1's self-position is estimated.
That is, as shown in FIG. 19, the position coordinates Pn-1 (Pn-1x, Pn-1y) immediately before losing the self-position, and the position coordinates Pn-2 (Pn-2x, Pn-2x) one phase before the position coordinate Pn-1. , Pn-2y), define the trajectory vector T, and use the values of the encoders 25L and 25R that increased from passing the previous position coordinate Pn-1 until losing track of the self-position to calculate the distance D traveled forward. The estimated self-position Pc (Pcx, Pcy) of the
・Step S61
It is determined whether the moving
・Step S62
If it is not estimated that the moving
・Step S63
Currently, if the moving
・Step S64
It is determined whether the number of consecutive errors exceeds a predetermined value (for example, 10 times) or the distance traveled during the continuous errors exceeds a predetermined value (for example, 1 m). If the number of consecutive errors exceeds a predetermined value or the distance traveled during consecutive errors exceeds a predetermined value, the process ends. Further, if the number of consecutive errors does not exceed the predetermined value and the distance traveled during the continuous errors does not exceed the predetermined value, the process returns to step S11.
・Step S65
In step S33, after it is determined that an error has occurred, it is determined whether the number of consecutive errors has exceeded a predetermined value (for example, 10 times). If the number of consecutive errors exceeds a predetermined value, the process ends. If the number of consecutive errors does not exceed the predetermined value, the process returns to step S32.
実施形態1によれば、TS2により測定した床面F上のXY座標値で定められた移動予定情報によって設定された移動予定経路としてのジグザグ経路Z、即ち、移動体1の移動開始地点S1のXY座標値(S1x,S1y)と、最初の直線経路Lの終点G1のXY座標値(G1x,G1y)と、隣り合う直線経路L,L間の距離Wと、直線経路Lの数Nとによって設定されたジグザグ経路Zを辿るように移動体1を移動させることができるとともに、TS2により取得される床面F上を移動中の移動体1の床面F上のXY座標値とジグザグ経路ZのXY座標値とを比較して移動体1の移動を制御することにより、移動予定情報に基づいて予め定めたジグザグ経路Zに沿って移動体1を精度良くを移動させることができるようになる。
According to the first embodiment, the zigzag route Z is the planned movement route set based on the movement plan information determined by the XY coordinate values on the floor F measured by TS2, that is, the movement start point S1 of the moving
また、移動体1が直線経路Lの終点に到達したと判定された場合に、移動体1に搭載された移動方向変更手段により、移動体1の進行方向を反転させる折返し処理Cを行うので、ジグザグ経路Zに沿って移動体1を精度よく移動させることができる。
Furthermore, when it is determined that the moving
また、移動制御処理では、移動体1が直線経路Lの終点Gnに近づいた場合に移動体1の移動速度を1回以上減速する制御を行うようにしたので、ジグザグ経路Zに沿って移動体1を移動させる場合の移動体1の折返し処理Cの精度を向上させることが可能となる。
In addition, in the movement control process, when the moving
また、移動制御処理では、TS2から移動体1の移動情報を得られなかった場合に、移動体1に搭載された移動量検出手段としてのエンコーダから得られる移動量情報に基づいて移動体1の位置を推定するようにしているので、TS2から移動体1の移動情報を得られなかった場合でも、移動予定情報に基づいて予め定めたジグザグ経路Zに沿って移動体1を精度よく移動させることができる。
In addition, in the movement control process, when the movement information of the
また、移動予定情報設定処理では、床面F上のジグザグ経路Zを設定するために、移動体1の移動開始地点S1のXY座標値と、最初の直線経路L1の終点G1のXY座標値と、直線経路Lの数と、隣り合う直線経路L,L間の距離wとを設定するだけで、移動体1を移動させる予定のジグザグ経路Zを容易に設定できるようになる。
In the movement schedule information setting process, in order to set the zigzag route Z on the floor F, the XY coordinate values of the movement start point S1 of the moving
また、移動体1は、制御手段50が、昇降装置40の伸縮手段を伸長させて転動体45を床面Fに押し付けるとともに前側車輪21L,21Rを床面Fから浮かせた状態で、床面Fに接触している後側車輪22L,22Rを互いに反対方向に回転させることによって、水平方向の向きを変更するように構成されているので、移動方向を変更する際、水平方向の向きをスムーズに変更できる。
即ち、基体10を床面Fから浮かせて転動体45を床面Fに接触させた状態で、左右の後側車輪22L,22Rを互いに反対方向に回転させることにより、回転中心線10Cを中心とした床面F上の1つの円弧10R上を、左右の後側車輪22L,22R及び転動体45が転動することになるので、移動体1が回転中心線10Cを回転中心として右回り又は左回りにスムーズに回転し、移動体1の水平方向の向きをスムーズに変更できるようになる。
当該移動体1によれば、移動体1の重量が重い場合でも、移動体1の水平方向の向きをスムーズに変更できるようになるので、当該構成は、重量が重い移動体に好適である。
また、昇降装置40の伸縮手段を、移動体1の回転中心線10Cと平行を維持しながら当該回転中心線10Cに沿った上下方向に移動させるためのガイド部46を備えたので、昇降装置40の伸縮手段が正確に上下方向に移動して、回転中心線10Cを正確に形成できるようになるので、移動体1の水平方向の向きをスムーズに変更できるようになる。
The moving
That is, with the
According to the
Further, since the elevating
尚、転動体45としては、例えば、図4(d)に示すように、移動体1の回転中心線10Cを回転中心とする1つの円弧10R上を転動可能なように構成された車輪、又は、球輪等の転動体を用いればよい。
Note that the rolling
実施形態2
図21乃至図23、及び、図20に基づいて、実施形態2に係る移動体の移動制御方法における移動予定情報設定処理について説明する。
実施形態2に係る移動予定情報設定処理は、図23に示すように、床面F上の移動予定経路としてのジグザグ経路Zを設定するために、移動体1の移動開始地点S1のXY座標値(S1x,S1y)と、最初の直線経路L1の終点G1のXY座標値(G1x,G1y)と、床面FのXY座標上における移動開始地点S1の対角地点として測定される対角測定点RdのXY座標値(Rdx,Rdy)とを、床面F上の所定の位置に固定されたTS2を用いて取得し、取得したこれらXY座標値と、隣り合う直線経路(レーン)L,L間の距離wの初期値w’とを、設定者が設定画面31を介して設定することにより、移動予定情報設定手段としての例えばコンピュータ3が、移動予定情報設定プログラムに基づいて、最適な直線経路Lの数Nを算出するとともに、巡回パターンを決める処理を行う。
The movement schedule information setting process in the movement control method for a moving body according to the second embodiment will be described based on FIGS. 21 to 23 and FIG. 20.
As shown in FIG. 23, in the movement schedule information setting process according to the second embodiment, in order to set a zigzag route Z as a planned movement route on the floor F, the XY coordinate values of the movement start point S1 of the moving
・移動予定情報設定処理
・ステップS100
コンピュータ3の設定画面31(図22参照)にキーボード等の入力手段を用いて、図23に示すような、床面F上のジグザグ経路Zを設定するために、移動体1の移動開始地点S1(S1x,S1y)と、最初の直線経路L1の終点G1(G1x,G1y)と、対角測定点Rd(Rdx,Rdy)と、隣り合う直線経路間の距離wの初期値w’とを設定する。
即ち、床面F上において、移動体1の移動開始地点Sと、最初の直線経路L1の終点Gと、対角測定点Rdとに、ターゲットを設置し、移動開始地点S(S1x,S1y)と、最初の直線経路L1の終点G1(G1x,G1y)と、対角測定点Rd(Rdx,Rdy)とを、床面F上の所定の位置に固定されたTS2を用いて取得し、取得した各XY座標値と、隣り合う直線経路間の距離wの初期値w’とを、設定者が設定画面31を介して設定する。
・ステップS101
設定画面31の自動計算ボタン33(図22参照)をクリックする。
・ステップS102
コンピュータ3は、移動予定情報設定プログラムに基づいて、対角測定点Rdのズレtを補正するためのθを計算する。即ち、図23に示すように、終点G1から対角測定点Rdまでの有向線分であるベクトルと、終点G1から対角設定点Rまでのベクトルとを計算し、これらベクトル間の角度θを、図16の式(1)に基づいて求める。
・ステップS103
θが、入力データミス、又は、対角測定点の設置ミスが疑われるほどに大きいか否かを判定する。
・ステップS104
ステップS103において、θが、入力データミス、又は、対角測定点Rdの設定ミスが疑われるほどに大きくないと判定された場合は、当該ステップS104中の計算式に基づいて、エリア幅Lwを計算する。尚、エリア幅Lwは、図23に示すように、ジグザグ経路Zにおける複数の直線経路L,L…が並ぶ方向の幅である。
・ステップS105
ステップS103において、θが、入力データミス、又は、対角測定点Rdの設置ミス(測定点ミス)が疑われるほどに大きいと判定された場合は、パラメータ異常であることを発報して終了する。
・ステップS106
ステップS104で計算したエリア幅Lwと直線経路間の距離wの初期値w’とを用いて、当該ステップS105中の計算式に基づいて、ブロック数の初期値であるブロック数B’を計算する(余りrも計算する)。尚、ブロック数とは、図23に示すように、エリア幅Lw内に形成される、隣り合う直線経路L,Lと直線間経路Mとで囲まれた矩形領域bの数のことをいう。
・ステップS107
余りが0か否かを判定する。
・ステップS108
ステップS107において、余りが0ならば、初期値w’を直線経路(レーン)間の距離wに決定する(w=w’)。
・ステップS109
ブロック数B’に+1して、直線経路(レーン)数Nを確定(N=B’+1)。つまり、最適な直線経路Lの数Nが決まる。
・ステップS110
ステップS107において、余りが0でないならば、ブロック数B’に+1して、ブロック数Bとする(B=B’+1)。
・ステップS1101
直線経路(レーン)間の距離wの再計算(w=Lw/B)。尚、当該再計算において余りが出た場合は、例えば有効数字4桁で切り捨てる。
・ステップS1102
ブロック数Bに+1して、直線経路(レーン)数Nを確定(N=B+1)。つまり、最適な直線経路Lの数Nが決まる。
尚、図23から明らかなように、直線経路(レーン)数Nは、必ずブロック数+1となる。
・ステップS111
次に、移動体1の移動開始地点S1から終点G1までの有向線分であるベクトルと、移動開始地点S1から対角設定点Rまでの有向線分であるベクトルとを計算し、これらベクトルの外積を求める。
・ステップS112
ステップS111で計算した外積のZ軸成分(垂直成分)が、正か負かを判定する。
・ステップS113
ステップS111で計算した外積のZ軸成分が、図20の式(1)に示すように、正であれば、左巡回パターンのジグザグ経路Zに決定される。
・ステップS114
ステップS111で計算した外積のZ軸成分が、図20の式(2)に示すように、負であれば、右巡回パターンのジグザグ経路Zに決定される。
・ステップS115
ステップS111で計算した外積のZ軸成分が、図20の式(3)に示すように、0であれば、パラメータ異常であることを発報して終了する。
・ステップS116
決定されたそれぞれの値を移動予定情報として設定する。
・ステップS117
設定画面31のスキャン開始ボタン32(図22参照)をクリックする。
・ステップS118
コンピュータ3が設定された移動予定情報をファイルにしてコンピュータ3の記憶装置に書き込む。
・ステップS119
コンピュータ3が移動体1の制御手段50に移動予定情報を送信して制御手段50の記憶装置に読み込ませた後、コンピュータ3から移動体1の制御手段50に移動制御プログラムの実行コマンドを送信する。
・Movement schedule information setting process ・Step S100
Using an input means such as a keyboard on the
That is, on the floor F, targets are installed at the movement start point S of the moving
・Step S101
Click the automatic calculation button 33 (see FIG. 22) on the
・Step S102
The
・Step S103
It is determined whether θ is large enough to suspect an error in input data or an error in setting the diagonal measurement points.
・Step S104
If it is determined in step S103 that θ is not so large that an input data error or a setting error in the diagonal measurement point Rd is suspected, the area width Lw is calculated based on the calculation formula in step S104. calculate. Note that, as shown in FIG. 23, the area width Lw is the width in the direction in which the plurality of straight paths L, L, . . . in the zigzag path Z are lined up.
・Step S105
In step S103, if it is determined that θ is so large that an error in input data or an error in setting the diagonal measurement point Rd (measurement point error) is suspected, a parameter abnormality is reported and the process ends. do.
・Step S106
Using the area width Lw calculated in step S104 and the initial value w' of the distance w between straight paths, the number of blocks B', which is the initial value of the number of blocks, is calculated based on the calculation formula in step S105. (Also calculate the remainder r). Note that the number of blocks refers to the number of rectangular areas b defined within the area width Lw and surrounded by the adjacent straight-line paths L, L and the straight-line path M, as shown in FIG.
・Step S107
Determine whether the remainder is 0 or not.
・Step S108
In step S107, if the remainder is 0, the initial value w' is determined to be the distance w between the straight routes (lanes) (w=w').
・Step S109
Add 1 to the number of blocks B' to determine the number N of straight routes (lanes) (N=B'+1). In other words, the optimal number N of straight-line routes L is determined.
・Step S110
In step S107, if the remainder is not 0, the number of blocks B' is incremented by 1 to set the number of blocks B (B=B'+1).
・Step S1101
Recalculate the distance w between straight paths (lanes) (w=Lw/B). If there is a remainder in the recalculation, it is rounded down to four significant digits, for example.
・Step S1102
Add 1 to the number of blocks B to determine the number N of straight routes (lanes) (N=B+1). In other words, the optimal number N of straight-line routes L is determined.
Note that, as is clear from FIG. 23, the number N of straight routes (lanes) is always the number of blocks +1.
・Step S111
Next, a vector that is a directed line segment from the movement start point S1 to the end point G1 of the moving
・Step S112
It is determined whether the Z-axis component (vertical component) of the cross product calculated in step S111 is positive or negative.
・Step S113
If the Z-axis component of the cross product calculated in step S111 is positive, as shown in equation (1) in FIG. 20, the zigzag path Z of the left cyclic pattern is determined.
・Step S114
If the Z-axis component of the cross product calculated in step S111 is negative, as shown in equation (2) in FIG. 20, the zigzag path Z of the right cyclic pattern is determined.
・Step S115
If the Z-axis component of the cross product calculated in step S111 is 0, as shown in equation (3) in FIG. 20, a parameter abnormality is reported and the process ends.
・Step S116
Each determined value is set as movement schedule information.
・Step S117
Click the scan start button 32 (see FIG. 22) on the
・Step S118
The
・Step S119
After the
実施形態2に係る移動予定情報設定処理によれば、移動開始地点S(S1x,S1y)と、最初の直線経路L1の終点G1(G1x,G1y)と、対角測定点Rd(Rdx,Rdy)と、隣り合う直線経路間の距離wの初期値w’とを移動予定情報として設定することにより、移動予定情報設定プログラムが、エリア幅Lwを計算した後、当該エリア幅Lwと初期値w’とに基づいて、最適な直線経路Lの数を算出するとともに、巡回パターンを決める処理も行うようにしたので、移動予定情報設定処理がより容易となる。
尚、ステップS111~ステップS115で計算して求められる巡回パターンは、実施形態1で説明した図6の設定画面上に設けられている図外の巡回パターン決定ボタンをクリックすることによっても決定できるが、実施形態2によれば、当該巡回パターン決定ボタンをクリックし忘れた場合、あるいは、当該巡回パターン決定ボタンを設けない場合であっても、ステップS111~ステップS115の移動予定情報設定処理により巡回パターンが自動的に決定されるので、移動予定情報設定処理に係る作業手間を削減できるようになる。
According to the movement schedule information setting process according to the second embodiment, the movement start point S (S1x, S1y), the end point G1 (G1x, G1y) of the first straight route L1, and the diagonal measurement point Rd (Rdx, Rdy) By setting the initial value w' of the distance w between adjacent straight routes as the movement schedule information, the movement schedule information setting program calculates the area width Lw, and then calculates the area width Lw and the initial value w'. Based on this, the optimal number of straight-line routes L is calculated, and the process of determining the tour pattern is also performed, making the travel schedule information setting process easier.
Note that the cyclic pattern calculated in steps S111 to S115 can also be determined by clicking the cyclic pattern determination button (not shown) provided on the setting screen of FIG. 6 described in the first embodiment. According to the second embodiment, even if the user forgets to click the tour pattern determination button, or even if the tour pattern determination button is not provided, the travel schedule information setting process of steps S111 to S115 determines the travel pattern. Since this is automatically determined, it becomes possible to reduce the work involved in the movement schedule information setting process.
尚、各実施形態では、後側車輪を駆動輪とした構成の移動体を例示したが、前側車輪を駆動輪とした構成の移動体を用いても良い。この場合、制御手段は、昇降装置のロッドを伸長させて転動体を床面に押し付けるとともに移動体の後側車輪を床面から浮かせた状態となるように移動体を設定して、床面に接触している前側車輪の左右の車輪を互いに反対方向に回転させることにより、移動体の向きを変更するように構成すればよい。
即ち、この場合、移動体は、昇降装置が、基体の後側に設けられて移動体の後側を昇降可能に構成され、左の前側車輪の移動対象面との接触面の中心点と、右の前側車輪の移動対象面との接触面の中心点と、転動体の移動対象面との接触面の中心点とが、移動体の水平方向の向きを変更する際の移動体の回転中心線を中心とする1つの円弧上に位置するように構成される。
In addition, in each embodiment, although the moving body of the structure which used the rear side wheel as the driving wheel was illustrated, you may use the moving body of the structure which used the front side wheel as the driving wheel. In this case, the control means sets the moving body so that the rod of the lifting device is extended to press the rolling elements against the floor and the rear wheels of the moving body are lifted off the floor. The configuration may be such that the direction of the moving body is changed by rotating the left and right wheels of the front wheels that are in contact with each other in opposite directions.
That is, in this case, the movable body is configured such that the lifting device is provided on the rear side of the base body so as to be able to raise and lower the rear side of the movable body, and the center point of the contact surface of the left front wheel with the surface to be moved, The center point of the contact surface of the right front wheel with the moving target surface and the center point of the contact surface of the rolling element with the moving target surface are the center of rotation of the moving body when changing the horizontal direction of the moving body It is configured to be located on one circular arc centered on the line.
また、各実施形態では、移動予定経路としてジグザグ経路Zを設定したが、移動予定経路は、TSにより測定した床面上のXY座標値で定められた移動予定情報によって設定される経路であれば、どのような経路であっても構わない。 Furthermore, in each embodiment, the zigzag route Z is set as the planned movement route, but the planned movement route may be any route set based on the movement schedule information determined by the XY coordinate values on the floor measured by the TS. , any route is acceptable.
また、各実施形態では、移動対象面として、建物内の床面を移動させる例を示したが、移動対象面は、建物外の道路や空き地等の面であってもよい。 Furthermore, in each of the embodiments, an example is shown in which a floor surface inside a building is moved as the surface to be moved, but the surface to be moved may be a surface such as a road or a vacant lot outside the building.
また、各実施形態では、移動体の実際の移動情報を取得して移動体に送信する移動情報取得手段として、TS(トータルステーション)を用いた例を示したが、移動情報取得手段として、TS以外の手段、例えば、GPSを用いても構わない。
即ち、この場合、GPSによる移動対象面上のXY座標値で定められた移動予定情報によって移動予定経路を設定し、GPSにより取得される移動対象面上を移動中の移動体の移動対象面上のXY座標値と移動予定経路のXY座標値とを比較して移動体の移動を制御することにより、移動予定経路を精度良く辿るように移動体を移動させることができるようになる。
Furthermore, in each of the embodiments, an example has been shown in which a TS (total station) is used as a movement information acquisition means for acquiring actual movement information of a moving object and transmitting it to the moving object. For example, GPS may be used.
That is, in this case, the planned movement route is set based on the movement plan information determined by the XY coordinate values on the movement target plane by GPS, and the movement target plane of the moving object that is moving on the movement target plane acquired by GPS is set. By controlling the movement of the moving object by comparing the XY coordinate values of the object and the XY coordinate values of the planned movement route, the moving object can be moved so as to accurately follow the planned movement route.
1 移動体、2 トータルステーション(移動情報取得手段)、
3 コンピュータ(移動予定情報設定手段)、10 基体、20 移動手段、
21L,21R 左右の前側車輪、22L,22R 左右の後側車輪、
23L,23R モータ、25L,25R エンコーダ(移動量検出手段)、
40 昇降装置、43 ロッド、45 転動体、50 制御手段、
F 床面(移動対象面)、L 直線経路(レーン)、Z ジグザグ経路(移動予定経路)。
1 mobile object, 2 total station (movement information acquisition means),
3 computer (movement schedule information setting means), 10 base, 20 movement means,
21L, 21R Left and right front wheels, 22L, 22R Left and right rear wheels,
23L, 23R motor, 25L, 25R encoder (movement amount detection means),
40 lifting device, 43 rod, 45 rolling element, 50 control means,
F floor surface (movement target surface), L straight path (lane), Z zigzag path (planned movement path).
Claims (1)
移動体を移動予定情報に基づいて移動させるとともに、移動情報取得手段により取得された移動体の実際の移動情報と移動予定情報とを比較して移動体の移動を制御する移動制御処理と、
を備え、
移動予定情報設定処理では、移動対象面上において直線経路の折り返しが繰り返されるジグザグ経路を辿って移動体を移動させるための移動予定情報を設定する移動体の移動制御方法であって、
移動予定情報設定処理では、移動対象面上のジグザグ経路を設定するために、移動体の移動開始地点のXY座標値と、最初の直線経路の終点のXY座標値と、移動対象面のXY座標上における移動開始地点の対角地点として測定される対角測定点のXY座標値と、隣り合う直線経路間の距離の初期値とを設定することにより、ジグザグ経路における複数の直線経路が並ぶ方向の幅と直線経路間の距離の初期値とに基づいて直線経路の数の最適値を算出して設定することを特徴とする移動体の移動制御方法。 a movement schedule information setting process for setting movement schedule information of a moving object to be moved on a movement target surface;
a movement control process of moving the mobile body based on the movement schedule information and controlling the movement of the mobile body by comparing the actual movement information of the mobile body acquired by the movement information acquisition means with the movement schedule information;
Equipped with
The movement schedule information setting process is a movement control method for a movable body that sets movement schedule information for moving the movable body along a zigzag path in which a straight path is repeatedly turned back and forth on a movement target surface, the method comprising:
In the movement schedule information setting process, in order to set a zigzag route on the movement target surface, By setting the XY coordinate values of the diagonal measurement point measured as the diagonal point of the movement start point above and the initial value of the distance between adjacent straight routes, you can determine the direction in which multiple straight routes in the zigzag route are lined up. 1. A method for controlling the movement of a moving object, comprising calculating and setting an optimal value for the number of straight paths based on the width of the straight path and an initial value of the distance between the straight paths.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2023023985A JP7434627B2 (en) | 2019-03-18 | 2023-02-20 | Movement control method of moving object |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019049786A JP7325978B2 (en) | 2019-03-18 | 2019-03-18 | Mobile body movement control method |
JP2023023985A JP7434627B2 (en) | 2019-03-18 | 2023-02-20 | Movement control method of moving object |
Related Parent Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2019049786A Division JP7325978B2 (en) | 2019-03-18 | 2019-03-18 | Mobile body movement control method |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2023054122A JP2023054122A (en) | 2023-04-13 |
JP7434627B2 true JP7434627B2 (en) | 2024-02-20 |
Family
ID=72558973
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2019049786A Active JP7325978B2 (en) | 2019-03-18 | 2019-03-18 | Mobile body movement control method |
JP2023023985A Active JP7434627B2 (en) | 2019-03-18 | 2023-02-20 | Movement control method of moving object |
Family Applications Before (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2019049786A Active JP7325978B2 (en) | 2019-03-18 | 2019-03-18 | Mobile body movement control method |
Country Status (1)
Country | Link |
---|---|
JP (2) | JP7325978B2 (en) |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008067617A (en) | 2006-09-12 | 2008-03-27 | Yanmar Co Ltd | Agricultural working vehicle |
JP2012120449A (en) | 2010-12-06 | 2012-06-28 | National Agriculture & Food Research Organization | Fertilizing apparatus |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6421069U (en) * | 1987-07-30 | 1989-02-02 | ||
JPH06214639A (en) * | 1993-01-18 | 1994-08-05 | Fujita Corp | Travel controller for mobile object |
JPH0994006A (en) * | 1995-09-29 | 1997-04-08 | Matsushita Electric Ind Co Ltd | Unmanned apparatus for farm working |
JPH11152017A (en) * | 1997-11-21 | 1999-06-08 | Technical Earth System Kk | Automobile |
DE10019011A1 (en) * | 2000-04-17 | 2001-10-25 | Geo Tec Electronics Gmbh | Automatic controls for guidance of agricultural machine along defined path uses signals from global positioning system and makes continuous comparison between actual and reference positions |
JP4353628B2 (en) * | 2000-11-28 | 2009-10-28 | 東京エレクトロン株式会社 | Coating device |
JP6552420B2 (en) * | 2016-01-13 | 2019-07-31 | 株式会社クボタ | Work vehicle |
-
2019
- 2019-03-18 JP JP2019049786A patent/JP7325978B2/en active Active
-
2023
- 2023-02-20 JP JP2023023985A patent/JP7434627B2/en active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008067617A (en) | 2006-09-12 | 2008-03-27 | Yanmar Co Ltd | Agricultural working vehicle |
JP2012120449A (en) | 2010-12-06 | 2012-06-28 | National Agriculture & Food Research Organization | Fertilizing apparatus |
Also Published As
Publication number | Publication date |
---|---|
JP7325978B2 (en) | 2023-08-15 |
JP2020154400A (en) | 2020-09-24 |
JP2023054122A (en) | 2023-04-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US8388263B2 (en) | Self-propelled civil engineering machine and method of controlling a self-propelled civil engineering machine | |
US5155684A (en) | Guiding an unmanned vehicle by reference to overhead features | |
CN109958036B (en) | Self-propelled construction machine and method for controlling a self-propelled construction machine | |
JPH07205085A (en) | Position detection and control device of mobile robot | |
CN106959697B (en) | Automatic indoor map construction system for long straight corridor environment | |
JP7434627B2 (en) | Movement control method of moving object | |
JP7389655B2 (en) | Movement control method for moving objects | |
JP7324800B2 (en) | Travel control method for passenger boarding bridge | |
JP7139018B2 (en) | Autonomous Inking Robot | |
JP6587734B1 (en) | Crane control system and control method | |
JP2691485B2 (en) | Position control device for refractory injection nozzle | |
JP2021087359A (en) | Working vehicle | |
KR102633311B1 (en) | automatic driven robot enabling precise position control based on tracking by a total station | |
JP7333488B1 (en) | welding system | |
CN115031705B (en) | Intelligent navigation robot measurement system and measurement method | |
JP7090053B2 (en) | Crane control system and control method | |
CN114018246B (en) | Positioning navigation method and positioning navigation device | |
JP7273560B2 (en) | mobile | |
CN116136688A (en) | Robot motion trail continuous processing method | |
CN117697741A (en) | Path planning control method and system based on self-rotation concrete vibrating robot | |
JPH0392277A (en) | Robot controlling section | |
JPH0379156B2 (en) | ||
JP2023113294A (en) | Movable carriage | |
CN116838058A (en) | Control method, control system and processor for wall construction robot | |
CN112627567A (en) | Grouting apparatus, control method of grouting apparatus, and computer-readable storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20230224 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20231226 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20240117 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20240206 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20240207 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7434627 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |