JP3863303B2

JP3863303B2 - Moving body position measuring device

Info

Publication number: JP3863303B2
Application number: JP29672398A
Authority: JP
Inventors: 信義羽角; 圭三川崎
Original assignee: Sumitomo Heavy Industries Ltd
Current assignee: Sumitomo Heavy Industries Ltd
Priority date: 1998-10-19
Filing date: 1998-10-19
Publication date: 2006-12-27
Anticipated expiration: 2018-10-19
Also published as: JP2000121357A

Description

【０００１】
【発明の属する技術分野】
本発明は、無人搬送車などの移動体の位置計測装置に関する。
【０００２】
【従来の技術】
近年、製造業においては自動化が進んでおり、その生産ラインには無人搬送車（以下、移動体と呼ぶ）に組立て部品を搭載して自動走行させ、組立て部品を所望の目的地まで搬送する自動搬送システムが採用されている。このような自動搬送システムにおける移動体の誘導方式には、例えば電磁誘導方式や光学誘導方式などの固定の走行経路による誘導方式が知られている。
【０００３】
上記の方式とは別に、移動体の現在位置を計測する方法として、移動体に取り付けられた「内界センサ」を使った、いわゆる「推測航法」が知られている。
【０００４】
一般的な推測航法を、図２に示す操舵輪４１と固定輪４２とによる車輪配置を持つ移動体で説明する。ここでは、操舵輪４１の方向は移動体に対して自由に変更でき、固定輪４２の向きは移動体の向きと平行であるとする。なお、実際には、移動体の安定性を保つために、操舵輪４１、固定輪４２の他に、キャスタ輪が備えられる。移動体の現在位置を算出するために、移動体の「操舵輪の操舵角」、「車輪（操舵輪／固定輪）の移動距離あるいは移動速度」、「移動体の向き」を計測する。計測手段として、操舵角の計測には操舵角センサ、車輪の移動距離あるいは移動速度の計測には車輪の回転数、すなわち回転速度を検出するパルスエンコーダ、移動体の向きの計測にはジャイロセンサなどの内界センサを使用するのが一般的である。
【０００５】
一般的な位置算出は次の方法による。今、ある時刻の移動体の向きをθL 、操舵輪４１の操舵角をα、操舵輪４１の移動速度をＶｓとすると、２次元のＸ−Ｙ座標上での操舵輪４１のＸ方向の移動速度Ｘｓｖ、Ｙ方向の移動速度Ｙｓｖは次式によって求まる。
【０００６】
Ｘｓｖ＝Ｖｓ×ｃｏｓ（θL ＋α）
Ｙｓｖ＝Ｖｓ×ｓｉｎ（θL ＋α）
Ｘ−Ｙ座標系の原点をスタート地点とし、これを基準とする操舵輪４１の位置座標は、この移動速度算出値を積分することによって算出することができる。
【０００７】
同様に、固定輪４２の移動速度算出値Ｖｒを使用した一般的な位置算出は次の方法による。固定輪４２のＸ方向の移動速度Ｘｒｖ、Ｙ方向の移動速度Ｙｒｖは次式によって求まる。
【０００８】
Ｘｒｖ＝Ｖｒ×ｃｏｓ（θL ）
Ｙｒｖ＝Ｖｒ×ｓｉｎ（θL ）
固定輪４２の位置座標は、この移動速度算出値を積分することによって算出することができる。
【０００９】
移動体上の任意の点の位置は、移動体における各車輪の位置が分かれば、車輪の幾何学的配置条件から求めることができる。
【００１０】
ところで、上記でいう各車輪の移動速度（Ｖ）は、車輪径（Ｒ）と回転速度（θｖ）から次式によって算出される。
【００１１】
Ｖ＝Ｒ×θｖ
一般に、車輪の回転数計測は、車輪の回転軸に取り付けられたパルスエンコーダの単位時間あたりの出力パルス数を計測することなどによって容易かつ正確に計測が可能であり、回転数は回転速度として得られる。
【００１２】
【発明が解決しようとする課題】
しかし、車輪径は、その材質にゴムなどの弾性体を使用することが多い。特に、無人搬送車などの荷物の搬送を目的とした移動体においては、荷の有無によって積載荷重が変化することにより車輪径が変化し、車輪径の正確な把握が困難である。
【００１３】
一般的には、移動距離を計測する車輪は１つであることや、複数あった場合においても単純にその結果を平均処理している場合が多い。しかし、この方式では、上記の移動速度（Ｖ）の算出式から明らかなように、積載荷重の変化に伴う車輪径の変化によって速度算出誤差が生じる。この速度算出誤差は距離計測誤差の原因となり、これによって移動体位置の計測誤差が発生し、実用上の問題点となっている。
【００１４】
そこで、本発明の課題は、無人搬送車などの移動体に適用される位置計測装置であって、移動体の車輪径の変化に起因する距離計測誤差を抑制することによって、移動体の位置計測精度を向上させることのできる位置計測装置を提供することにある。
【００１５】
【課題を解決するための手段】
本発明によれば、操舵輪を含む複数の車輪を具備した移動体の位置計測装置において、２つ以上の車輪の回転数を検出する複数の回転数検出手段と、前記複数の回転数検出手段の検出結果に基づいて移動体の位置を演算する演算手段とを備え、前記演算手段は、前記複数の回転数検出手段の検出結果から車輪毎に移動速度を算出する第１のステップと、それらの移動速度の比から車輪毎に車輪径変化による距離計測誤差の割合を推定して車輪径補正係数を算出する第２のステップと、算出された車輪径補正係数に基づいて車輪毎の移動速度を修正し、修正された車輪毎の移動速度を平均処理することにより移動体の移動速度を算出する第３のステップと、算出した移動速度を積分することにより移動体の位置を算出する第４のステップとを実行し、更に、あらかじめ車輪毎に積載荷重の変化に対する車輪径変化量を測定することにより得られた、積載荷重と距離計測誤差関数との対応関係を各車輪毎に記憶している記憶部を備え、前記演算手段は、前記第２のステップにおいて、前記車輪毎に算出された複数の移動速度を比較することにより積載荷重を推定し、推定した積載荷重に基づいて参照された車輪毎の距離計測誤差関数を使用して、車輪径変化に起因する誤差を補償するための車輪径補正係数を車輪毎に算出することを特徴とする移動体の位置計測装置が提供される。
【００１６】
前記操舵輪の操舵角を計測する操舵角計測手段を更に備えることにより、前記演算手段は、前記第１のステップにおいて、前記操舵輪については前記操舵角計測手段の計測結果と対応する前記回転数検出手段の検出結果とから、それ以外の車輪については対応する前記回転数検出手段の検出結果からそれぞれ、移動体のあらかじめ定められた基準点における基準点移動速度を車輪毎の移動速度として算出する。
【００１８】
前記演算手段はまた、前記第３のステップにおいて、前記車輪毎に算出された複数の移動速度をそれぞれ、車輪毎に算出された前記車輪径補正係数によって修正し、その結果、各車輪毎に修正された複数の修正移動速度を重み付けしたうえで平均処理することによって、移動体の移動速度を算出する。
【００１９】
移動体の向きを検出する向き検出手段を更に備えることにより、前記演算手段は更に、前記第４のステップにおいて、前記算出された移動体の移動速度と前記操舵角計測手段の計測結果と前記向き検出手段の検出結果とから、２次元平面上におけるＸ軸方向、Ｙ軸方向の移動速度を算出し、算出されたＸ軸方向、Ｙ軸方向の移動速度を積分することにより２次元平面上における移動体の位置を算出する。
【００２１】
【発明の実施の形態】
以下、本発明の実施の形態を、図２に示すような操舵輪が１つ、固定輪が１つの場合の移動体モデルを例示して説明する。
【００２２】
本発明は、あらかじめ各車輪毎の積載荷重の変化に対する車輪径変化量を把握しておき、複数の車輪毎に算出した移動体の基準点の移動速度を比較することによって、各車輪径の変化量を推定し、それを補償することによって移動体の位置計測精度を向上させるものである。
【００２３】
本発明が適用される位置計測方式は、一般に推測航法と呼ばれるもので、移動体上の内界センサを使用するものである。すなわち、図１に示されるように、本形態における位置計測装置は、内界センサとして、操舵輪４１の操舵角を計測する操舵角センサ１０と、移動体の向き（姿勢角）を検出するジャイロセンサ１１と、操舵輪４１、固定輪４２のそれぞれの回転数（回転速度）を検出するパルスエンコーダ１２とを備えている。また、記憶部１３には、後で説明されるように、あらかじめ各車輪毎に積載荷重の変化に対する車輪径変化量を測定することにより得られた、積載荷重と距離計測誤差関数との対応関係が各車輪毎に記憶されている。
【００２４】
演算部１４は、以降に詳しく説明するように、パルスエンコーダ１２の検出結果から車輪毎に移動速度を算出する第１のステップと、それらの移動速度の比から車輪毎に車輪径変化による距離計測誤差の割合を推定して車輪径補正係数を算出する第２のステップと、算出された車輪径補正係数に基づいて車輪毎の移動速度を修正し、修正された車輪毎の移動速度を平均処理することにより移動体の移動速度を算出する第３のステップと、算出した移動速度を積分することにより移動体の位置を算出する第４のステップとを実行する。
【００２５】
以下、図２に示すような移動体モデルを例に位置計測手順を説明する。この移動体の運動は、操舵輪４１の車軸と固定輪４２の車軸を延長した直線の交点を中心とした旋回動作となる。ここでは、移動体にあらかじめ定められる代表点を基準点と呼び、その基準点を固定輪４２と同一点に定義する。
【００２６】
（１）基準点移動速度比の算出
積載荷重の変化に伴う車輪径の変化量は、車輪形状、材質や荷重により決まるが、これらの条件は各車輪によって異なるのが一般的である。その結果、例えば積載荷重がゼロの時に各車輪間で移動速度算出値が一致するように調整した場合、積載荷重が増加すると各車輪毎に算出される距離算出値に差が生じることになる。
【００２７】
ここでは、固定輪４２の移動速度算出値を基準とし、その比率を算出する。
【００２８】
まず、操舵輪４１の回転速度から移動速度算出値（Ｓｓ）を算出する。次に、操舵輪４１の移動速度算出値（Ｓｓ）から基準点の移動速度を算出する。操舵輪４１の移動速度算出値（Ｓｓ）から求まる基準点の移動速度（Ｖｓ）は、操舵角（α）から次式により求められる。
【００２９】
Ｖｓ＝Ｓｓ／ｃｏｓ（α）
上記と同様にして、固定輪４２の移動速度算出値（Ｓｒ）から求まる基準点の移動速度（Ｖｒ）は、次式により求められる。
【００３０】
Ｖｒ＝Ｓｒ
このとき、計測誤差が無いとすると、Ｖｓ＝Ｖｒ＝Ｖとなる。但し、Ｖは実際の基準点移動速度である。
【００３１】
いま、移動体の積載荷重の変化による距離計測誤差の割合を、操舵輪４１で（Ｅｓ）、固定輪４２で（Ｅｒ）とすると、それぞれの移動速度算出値（Ｓｓ），（Ｓｒ）から求めた基準点の移動速度算出値（Ｖｅｓ，Ｖｅｒ）は、次式により求められる。
【００３２】

このときの基準点の移動速度算出値の比は、距離計測誤差の割合になることは、次式により明らかである。
【００３３】

【００３４】
（２）車輪径補正係数の算出
積載荷重の変化に対する車輪径変化量の関係が既知であり、かつ各車輪毎にその特性が異なっている場合、結果として積載荷重と基準点移動速度算出値の比の関数が定義できる。したがって、その関数の逆関数を求めることにより、基準点移動速度算出値の比から積載荷重を推定することが可能となり、そこで推定した積載荷重から距離計測誤差の関数を記憶部１３から読み出す。
【００３５】
距離計測誤差の割合を関数｛ｆ（Ｗ）、ｇ（Ｗ）｝で表現し、さらにその比率｛ｈ（ｗ）｝を次式により定義する。
【００３６】
Ｅｓ＝ｆ（Ｗ）
Ｅｒ＝ｇ（Ｗ）
ｈ（Ｗ）＝ｆ（Ｗ）／ｇ（Ｗ）
このとき、基準点の移動速度算出値の比は、次式にて表現される。
【００３７】

ここで、ｆ（Ｗ）とｇ（Ｗ）は記憶部１３から読み出すことで既知であるので、各車輪の距離計測誤差の割合は次式により求められる。但し、ｈ^-1（Ｖ）はｈ
（Ｖ）の逆関数であるとする。
【００３８】

上記の式により算出されたＥｓおよびＥｒから、各車輪の「車輪径補正係数」（操舵輪：Ｋｓ、固定輪：Ｋｒ）を次式により算出する。
【００３９】
Ｋｓ＝１／Ｅｓ
Ｋｒ＝１／Ｅｒ
【００４０】
（３）移動体移動速度算出値の算出
各車輪毎に算出した「移動速度算出値」をそれぞれ、「車輪径補正係数」によって修正し、その結果として各車輪毎に修正される「修正基準点移動速度」を平均処理することによって、移動体の移動速度算出値を求める。
【００４１】
すなわち、各車輪毎に算出した「移動速度算出値」を「車輪径補正係数」によって修正した「修正基準点移動速度」をＶｍｓ（操舵輪４１），Ｖｍｒ（固定輪４２）とし、次式により定義する。
【００４２】
Ｖｍｓ＝Ｖｅｓ×Ｋｓ
Ｖｍｒ＝Ｖｅｒ×Ｋｒ
さらに、その結果を平均処理することによって、移動体の移動速度算出値（Ｖｍ）を求める。但し、ａｓ，ａｒはそれぞれ、平均化処理の重み係数で、この値は定数でも変数でもよい。
【００４３】
Ｖｍ＝｛（ａｓ×Ｖｍｓ）＋（ａｒ×Ｖｍｒ）｝／（ａｓ＋ａｒ）
車輪径補正係数の推定が正確に行われた場合、次式によりＶｍ＝Ｖとなることが明らかで、車輪径の変化が移動体移動速度の計測に影響を与えないことがわかる。
【００４４】

【００４５】
（４）移動体位置の算出
上記（３）により求めた移動体移動速度（Ｖｍ）と、操舵角センサ１０によって計測された操舵角（α）と、ジャイロセンサ１１によって計測された移動体の向き（θ）とから、２次元平面上での移動体の移動速度（Ｘｖ，Ｙｖ）は次式により求まる。
【００４６】
Ｘｖ＝Ｖｍ×ｃｏｓ（θ＋α）
Ｙｖ＝Ｖｍ×ｓｉｎ（θ＋α）
２次元平面上での移動体の位置は、これらの移動速度（Ｘｖ，Ｙｖ）を積分することによって算出することができる。
【００４７】
なお、本発明による位置計測装置は、ガイド線が床面に埋設されていることによりガイド線が物理的に設定されているガイド方式、ガイド線が仮想的に設定され、レーザ等による誘導方式を採用したガイドレス方式のいずれにも適用可能である。
【００４８】
また、上記（１）〜（４）において移動体の移動速度の代わりに移動距離を算出することで同様な位置計測を行うことができる。この場合、演算部１４は、複数のパルスエンコーダ１２の検出結果から車輪毎に移動距離を算出する第１のステップと、それらの移動距離の比から車輪毎に車輪径変化による距離計測誤差の割合を推定して車輪径補正係数を算出する第２のステップと、算出された車輪径補正係数に基づいて車輪毎の移動距離を修正し、修正された車輪毎の移動距離を平均処理することにより移動体の移動距離を算出する第３のステップと、算出した移動距離から移動体の位置を算出する第４のステップとを実行する。
【００４９】
上記の説明は、移動体の向きが様々に変化する走行経路に適用する場合を想定しているが、走行経路が直線のみである場合には、操舵角センサ１０、ジャイロセンサ１１は不要である。
【００５０】
以上、本発明を好ましい実施の形態について説明したが、内界センサは図１のものに限定されるものではない。また、本発明は、無人搬送車の他、無人フォークリフト、無人トラクタ、無人ダンプトラック等にも適用可能である。
【００５１】
【発明の効果】
以上説明してきたように、本発明によればあらかじめ移動体の積載荷重と車輪径変化量を記憶しておくことによって、複数の車輪の移動速度あるいは移動距離算出結果を比較することで、積載荷重を計測することなく車輪径変化量を正しく推定することができ、結果として正確な移動体位置計測が可能となる。
【図面の簡単な説明】
【図１】本発明による位置計測装置の実施の形態の構成を示したブロック図である。
【図２】本発明が適用される移動体モデルを示した図である。
【符号の説明】
１０操舵角センサ
１１ジャイロセンサ
１２パルスエンコーダ
１３記憶部
１４演算部
４１操舵輪
４２固定輪[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a position measuring device for a moving body such as an automatic guided vehicle.
[0002]
[Prior art]
In recent years, automation has been progressing in the manufacturing industry, and automatic production is carried out by mounting assembly parts on an automated guided vehicle (hereinafter referred to as a moving body) on the production line and transporting the assembly parts to a desired destination. A transport system is adopted. As a method for guiding a moving body in such an automatic conveyance system, for example, a guidance method using a fixed travel route such as an electromagnetic guidance method or an optical guidance method is known.
[0003]
In addition to the above method, as a method for measuring the current position of the moving body, so-called “dead reckoning navigation” using an “inner world sensor” attached to the moving body is known.
[0004]
A general dead reckoning navigation will be described with a moving body having a wheel arrangement of the steering wheel 41 and the fixed wheel 42 shown in FIG. Here, it is assumed that the direction of the steering wheel 41 can be freely changed with respect to the moving body, and the direction of the fixed wheel 42 is parallel to the direction of the moving body. In practice, caster wheels are provided in addition to the steering wheels 41 and the fixed wheels 42 in order to maintain the stability of the moving body. In order to calculate the current position of the moving body, the “steering angle of the steering wheel”, “the moving distance or moving speed of the wheel (steering wheel / fixed wheel)”, and “direction of the moving body” are measured. As a measuring means, a steering angle sensor for measuring the steering angle, a pulse encoder for detecting the rotational speed of the wheel, that is, a rotational speed for measuring the moving distance or moving speed of the wheel, a gyro sensor for measuring the direction of the moving body, etc. It is common to use an internal sensor.
[0005]
General position calculation is performed by the following method. Now, if the direction of the moving body at a certain time is θL, the steering angle of the steering wheel 41 is α, and the moving speed of the steering wheel 41 is Vs, the movement of the steering wheel 41 in the X direction on a two-dimensional XY coordinate. The speed Xsv and the moving speed Ysv in the Y direction are obtained by the following equations.
[0006]
Xsv = Vs × cos (θL + α)
Ysv = Vs × sin (θL + α)
Using the origin of the XY coordinate system as a starting point, the position coordinates of the steered wheels 41 based on this can be calculated by integrating the calculated moving speed values.
[0007]
Similarly, general position calculation using the moving speed calculation value Vr of the fixed wheel 42 is performed by the following method. The moving speed Xrv in the X direction and the moving speed Yrv in the Y direction of the fixed wheel 42 are obtained by the following equations.
[0008]
Xrv = Vr × cos (θL)
Yrv = Vr × sin (θL)
The position coordinates of the fixed wheel 42 can be calculated by integrating the calculated moving speed value.
[0009]
The position of an arbitrary point on the moving body can be obtained from the geometric arrangement condition of the wheel if the position of each wheel on the moving body is known.
[0010]
By the way, the moving speed (V) of each wheel mentioned above is calculated from the wheel diameter (R) and the rotational speed (θv) by the following equation.
[0011]
V = R × θv
In general, the number of rotations of a wheel can be measured easily and accurately by measuring the number of output pulses per unit time of a pulse encoder attached to the rotation axis of the wheel. It is done.
[0012]
[Problems to be solved by the invention]
However, the wheel diameter often uses an elastic body such as rubber as its material. In particular, in a mobile body for the purpose of transporting a load such as an automatic guided vehicle, the wheel diameter changes due to a change in the load depending on the presence or absence of the load, and it is difficult to accurately grasp the wheel diameter.
[0013]
In general, the number of wheels for measuring the moving distance is one, and even when there are a plurality of wheels, the result is often simply averaged. However, in this method, as apparent from the calculation formula for the moving speed (V), a speed calculation error occurs due to a change in the wheel diameter accompanying a change in the loaded load. This speed calculation error causes a distance measurement error, which causes a measurement error of the moving body position, which is a practical problem.
[0014]
Therefore, an object of the present invention is a position measurement device applied to a moving body such as an automatic guided vehicle, and the position measurement of the moving body is suppressed by suppressing a distance measurement error caused by a change in the wheel diameter of the moving body. An object of the present invention is to provide a position measuring apparatus capable of improving accuracy.
[0015]
[Means for Solving the Problems]
According to the present invention, in the position measuring apparatus for a moving body including a plurality of wheels including a steered wheel, a plurality of rotation speed detection means for detecting the rotation speeds of two or more wheels, and the plurality of rotation speed detection means. Calculating means for calculating the position of the moving body based on the detection results of the first, and calculating means for calculating the moving speed for each wheel from the detection results of the plurality of rotation speed detecting means; and A second step of calculating a wheel diameter correction coefficient by estimating a ratio of a distance measurement error due to a change in wheel diameter for each wheel from a ratio of the moving speeds of the wheels, and a moving speed for each wheel based on the calculated wheel diameter correction coefficient The third step of calculating the moving speed of the moving body by averaging the corrected moving speed for each wheel, and the fourth step of calculating the position of the moving body by integrating the calculated moving speed The steps and the real And, further, previously obtained by measuring the wheel diameter change amount with respect to changes in the live load on each wheel, the correspondence between the live load and the distance measurement error function includes a storage unit which stores for each wheel In the second step, the computing means estimates a load by comparing a plurality of movement speeds calculated for each wheel, and measures a distance for each wheel referred to based on the estimated load. There is provided a moving body position measuring device, wherein an error function is used to calculate a wheel diameter correction coefficient for each wheel to compensate for an error caused by a wheel diameter change .
[0016]
By further including a steering angle measuring means for measuring the steering angle of the steered wheel, the calculating means, in the first step, for the steered wheel, the rotation speed corresponding to the measurement result of the steering angle measuring means. Based on the detection result of the detection means, and for the other wheels, the reference point movement speed at a predetermined reference point of the moving body is calculated as the movement speed for each wheel from the detection result of the corresponding rotation speed detection means. .
[0018]
In the third step, the arithmetic means also corrects the plurality of movement speeds calculated for each wheel by the wheel diameter correction coefficient calculated for each wheel, and as a result, corrects for each wheel. The moving speed of the moving object is calculated by weighting the plurality of corrected moving speeds and averaging the weights.
[0019]
By further including a direction detecting means for detecting the direction of the moving body, the calculating means further includes, in the fourth step, the calculated moving speed of the moving body, the measurement result of the steering angle measuring means, and the direction. From the detection result of the detection means, the moving speed in the X-axis direction and the Y-axis direction on the two-dimensional plane is calculated, and the calculated moving speed in the X-axis direction and the Y-axis direction is integrated to thereby calculate on the two-dimensional plane. The position of the moving body is calculated.
[0021]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, an embodiment of the present invention will be described by exemplifying a moving body model in the case where there is one steering wheel and one fixed wheel as shown in FIG.
[0022]
The present invention grasps in advance the wheel diameter change amount with respect to the change in the loaded load for each wheel, and compares the moving speed of the reference point of the moving body calculated for each of the plurality of wheels, thereby changing the wheel diameter. By estimating the amount and compensating for it, the position measurement accuracy of the moving body is improved.
[0023]
The position measurement method to which the present invention is applied is generally called dead reckoning, and uses an internal sensor on a moving body. That is, as shown in FIG. 1, the position measuring device according to the present embodiment includes a steering angle sensor 10 that measures the steering angle of the steering wheel 41 and a gyro that detects the orientation (posture angle) of the moving body as an internal sensor. A sensor 11 and a pulse encoder 12 that detects the number of rotations (rotation speed) of each of the steered wheels 41 and the fixed wheels 42 are provided. In addition, as will be described later, in the storage unit 13, the correspondence relationship between the load load and the distance measurement error function obtained by measuring the wheel diameter change amount with respect to the load change for each wheel in advance. Is stored for each wheel.
[0024]
As will be described in detail later, the calculation unit 14 calculates the distance by the wheel diameter change for each wheel from the first step of calculating the movement speed for each wheel from the detection result of the pulse encoder 12 and the ratio of these movement speeds. A second step of estimating the error ratio and calculating the wheel diameter correction coefficient, and correcting the moving speed for each wheel based on the calculated wheel diameter correcting coefficient, and averaging the corrected moving speed for each wheel Thus, the third step of calculating the moving speed of the moving body and the fourth step of calculating the position of the moving body by integrating the calculated moving speed are executed.
[0025]
Hereinafter, the position measurement procedure will be described using a moving body model as shown in FIG. 2 as an example. This movement of the moving body is a turning operation centering on the intersection of a straight line obtained by extending the axle of the steering wheel 41 and the axle of the fixed wheel 42. Here, a representative point predetermined for the moving body is referred to as a reference point, and the reference point is defined as the same point as the fixed wheel 42.
[0026]
(1) Calculation of reference point moving speed ratio The amount of change in the wheel diameter accompanying the change in load capacity is determined by the wheel shape, material and load, but these conditions are generally different for each wheel. As a result, for example, when adjustment is made so that the movement speed calculation values coincide between the wheels when the loading load is zero, a difference occurs in the distance calculation values calculated for each wheel when the loading load increases.
[0027]
Here, the ratio is calculated based on the calculated moving speed of the fixed wheel 42.
[0028]
First, the movement speed calculation value (Ss) is calculated from the rotation speed of the steering wheel 41. Next, the movement speed of the reference point is calculated from the movement speed calculation value (Ss) of the steering wheel 41. The moving speed (Vs) of the reference point obtained from the moving speed calculation value (Ss) of the steering wheel 41 is obtained from the steering angle (α) by the following equation.
[0029]
Vs = Ss / cos (α)
Similarly to the above, the moving speed (Vr) of the reference point obtained from the moving speed calculation value (Sr) of the fixed wheel 42 is obtained by the following equation.
[0030]
Vr = Sr
At this time, if there is no measurement error, Vs = Vr = V. However, V is an actual reference point moving speed.
[0031]
Now, assuming that the ratio of the distance measurement error due to the change in the load of the moving body is (Es) for the steering wheel 41 and (Er) for the fixed wheel 42, it is obtained from the respective movement speed calculation values (Ss) and (Sr). The calculated moving speed (Ves, Ver) of the reference point is obtained by the following equation.
[0032]

It is clear from the following equation that the ratio of the calculated moving speed of the reference point at this time becomes the ratio of the distance measurement error.
[0033]

[0034]
(2) Calculation of wheel diameter correction coefficient When the relationship of the wheel diameter change amount to the change in the load capacity is known and the characteristics are different for each wheel, as a result, the load load and the reference point moving speed calculation value A function of the ratio can be defined. Therefore, by obtaining an inverse function of the function, it is possible to estimate the loaded load from the ratio of the reference point moving speed calculated values, and the distance measurement error function is read from the storage unit 13 from the estimated loaded load.
[0035]
The ratio of the distance measurement error is expressed by a function {f (W), g (W)}, and the ratio {h (w)} is defined by the following equation.
[0036]
Es = f (W)
Er = g (W)
h (W) = f (W) / g (W)
At this time, the ratio of the movement speed calculation values of the reference points is expressed by the following equation.
[0037]

Here, since f (W) and g (W) are known by reading from the storage unit 13, the ratio of the distance measurement error of each wheel is obtained by the following equation. However, h ^-1 (V) is h
It is assumed that it is an inverse function of (V).
[0038]

From “Es” and “Er” calculated by the above formula, “wheel diameter correction coefficient” (steering wheel: Ks, fixed wheel: Kr) of each wheel is calculated by the following formula.
[0039]
Ks = 1 / Es
Kr = 1 / Er
[0040]
(3) Calculation of moving body moving speed calculation value “Moving speed calculation value” calculated for each wheel is corrected by “wheel diameter correction coefficient”, and as a result, “correction reference point” is corrected for each wheel. A moving speed calculation value of the moving body is obtained by averaging the “moving speed”.
[0041]
That is, the “correction reference point movement speed” obtained by correcting the “movement speed calculation value” calculated for each wheel by the “wheel diameter correction coefficient” is Vms (steering wheel 41) and Vmr (fixed wheel 42), and Define.
[0042]
Vms = Ves × Ks
Vmr = Ver × Kr
Further, the moving speed calculation value (Vm) of the moving body is obtained by averaging the results. However, as and ar are weighting coefficients for the averaging process, and these values may be constants or variables.
[0043]
Vm = {(as × Vms) + (ar × Vmr)} / (as + ar)
When the wheel diameter correction coefficient is accurately estimated, it is clear that Vm = V by the following equation, and it can be seen that the change in the wheel diameter does not affect the measurement of the moving body moving speed.
[0044]

[0045]
(4) Calculation of moving body position The moving body moving speed (Vm) obtained by the above (3), the steering angle (α) measured by the steering angle sensor 10, and the direction of the moving body measured by the gyro sensor 11. From (θ), the moving speed (Xv, Yv) of the moving body on the two-dimensional plane is obtained by the following equation.
[0046]
Xv = Vm × cos (θ + α)
Yv = Vm × sin (θ + α)
The position of the moving body on the two-dimensional plane can be calculated by integrating these moving speeds (Xv, Yv).
[0047]
The position measuring device according to the present invention employs a guide method in which the guide line is physically set by the guide line being embedded in the floor surface, a guide method in which the guide line is virtually set, and a guide method using a laser or the like. It can be applied to any of the adopted guideless systems.
[0048]
Further, in the above (1) to (4), the same position measurement can be performed by calculating the moving distance instead of the moving speed of the moving body. In this case, the calculation unit 14 calculates the movement distance for each wheel from the detection results of the plurality of pulse encoders 12, and the ratio of the distance measurement error due to the wheel diameter change for each wheel from the ratio of the movement distances. The second step of estimating the wheel diameter correction coefficient and correcting the movement distance for each wheel based on the calculated wheel diameter correction coefficient, and averaging the corrected movement distance for each wheel A third step of calculating the moving distance of the moving body and a fourth step of calculating the position of the moving body from the calculated moving distance are executed.
[0049]
The above description assumes that the moving body is applied to a traveling route in which the direction of the moving body changes variously. However, when the traveling route is only a straight line, the steering angle sensor 10 and the gyro sensor 11 are not necessary. .
[0050]
Although the present invention has been described with reference to the preferred embodiment, the internal sensor is not limited to that shown in FIG. Further, the present invention can be applied to an unmanned forklift, an unmanned tractor, an unmanned dump truck, etc. in addition to an unmanned transport vehicle.
[0051]
【The invention's effect】
As described above, according to the present invention, the load load and the wheel diameter change amount of the moving body are stored in advance, and the load speed is calculated by comparing the movement speed or the distance calculation result of the plurality of wheels. The wheel diameter change amount can be correctly estimated without measuring the vehicle position, and as a result, accurate moving body position measurement can be performed.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of an embodiment of a position measuring apparatus according to the present invention.
FIG. 2 is a diagram showing a moving body model to which the present invention is applied.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 10 Steering angle sensor 11 Gyro sensor 12 Pulse encoder 13 Memory | storage part 14 Calculation part 41 Steering wheel 42 Fixed wheel

Claims

In a position measuring apparatus for a moving body having a plurality of wheels including steering wheels,
A plurality of rotation speed detection means for detecting rotation speeds of two or more wheels;
Calculating means for calculating the position of the moving body based on the detection results of the plurality of rotation speed detecting means;
The calculating means calculates a ratio of a distance measurement error due to a wheel diameter change for each wheel from a first step of calculating a moving speed for each wheel from the detection results of the plurality of rotation speed detecting means and a ratio of the moving speeds. The second step of estimating and calculating the wheel diameter correction coefficient, and correcting the movement speed for each wheel based on the calculated wheel diameter correction coefficient, and moving the corrected movement speed for each wheel by averaging Performing a third step of calculating the moving speed of the body and a fourth step of calculating the position of the moving body by integrating the calculated moving speed ;
Furthermore, a storage unit is provided for storing for each wheel the correspondence relationship between the load load and the distance measurement error function obtained by measuring the wheel diameter change amount with respect to the change of the load load for each wheel in advance.
In the second step, the computing means estimates a load by comparing a plurality of movement speeds calculated for each wheel, and distance measurement error for each wheel referred to based on the estimated load A position measuring apparatus for a moving body, wherein a wheel diameter correction coefficient for compensating for an error caused by a wheel diameter change is calculated for each wheel using a function .

2. The position measuring device according to claim 1, further comprising steering angle measuring means for measuring a steering angle of the steered wheel, wherein the calculating means is configured to perform the operation of the steering angle measuring means for the steered wheel in the first step. The reference point moving speed at a predetermined reference point of the moving object is determined from the measurement result and the detection result of the corresponding rotation speed detection means, and from the detection result of the corresponding rotation speed detection means for the other wheels. A moving body position measuring device, characterized in that it is calculated as a moving speed for each wheel.

3. The position measuring apparatus according to claim 2 , wherein in the third step, the calculation means corrects the plurality of movement speeds calculated for each wheel by the wheel diameter correction coefficient calculated for each wheel. As a result, a moving body position measuring apparatus is characterized in that the moving speed of the moving body is calculated by weighting a plurality of corrected moving speeds corrected for each wheel and then averaging.

4. The position measuring apparatus according to claim 3 , further comprising a direction detecting unit that detects a direction of the moving body, wherein the calculating unit measures the calculated moving speed of the moving body and the steering angle in the fourth step. The movement speed in the X-axis direction and the Y-axis direction on the two-dimensional plane is calculated from the measurement result of the means and the detection result of the direction detection means, and the calculated movement speeds in the X-axis direction and the Y-axis direction are integrated. Accordingly, a position measuring apparatus for a moving body, which calculates a position of the moving body on a two-dimensional plane.