JP4382266B2 - Method and apparatus for measuring position of moving object - Google Patents
Method and apparatus for measuring position of moving object Download PDFInfo
- Publication number
- JP4382266B2 JP4382266B2 JP2000225081A JP2000225081A JP4382266B2 JP 4382266 B2 JP4382266 B2 JP 4382266B2 JP 2000225081 A JP2000225081 A JP 2000225081A JP 2000225081 A JP2000225081 A JP 2000225081A JP 4382266 B2 JP4382266 B2 JP 4382266B2
- Authority
- JP
- Japan
- Prior art keywords
- steering wheel
- moving body
- movement amount
- amount
- turning
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims description 18
- 238000005259 measurement Methods 0.000 claims description 65
- 238000004364 calculation method Methods 0.000 claims description 25
- 238000010586 diagram Methods 0.000 description 7
- 238000000691 measurement method Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000006698 induction Effects 0.000 description 2
- 230000005674 electromagnetic induction Effects 0.000 description 1
- 238000001028 reflection method Methods 0.000 description 1
Images
Description
【０００１】
【発明の属する技術分野】
本発明は無人搬送車のような移動体の位置計測方法及び計測装置に関する。
【０００２】
【従来の技術】
ＡＧＶやＡＧＦにおける無人搬送車（以下、移動体と呼ぶ）は地上にレール等の固定設備を必要とせず、人や車両とスペースを共有できるために搬送システムの主要コンポーネントとして用いられてきた。
【０００３】
従来、移動体の誘導方式としては、電磁誘導方式、磁気誘導方式、光学反射方式が多く用いられている。しかし、これらの方式は、誘導線、磁気テープ、光学テープを走行路に設置するためレイアウト変更に容易に対応できない。また、床に貼り付けた磁気テープや光学反射テープの耐久性が悪い。
【０００４】
前記の誘導方式以外に、移動体に取り付けられた内界センサを使用して移動体の位置を計測する、いわゆる推測航法が知られている。
【０００５】
図７を参照して、代表的な推測航法を、前操舵輪６１と後操舵輪６２を有する移動体６０の場合について説明する。勿論、移動体６０には、前操舵輪６１と後操舵輪６２の他に、１個以上の固定輪（図示せず）を有する。ここでは、前後それぞれの操舵角は移動体６０に対して自由に変更できるものとする。移動体６０の位置を算出するために、前操舵輪６１の前操舵角及び移動距離、後操舵輪６２の後操舵角及び移動距離、移動体６０の向きを計測する。計測手段としては、前後の操舵角と前後の操舵輪の移動距離の計測は各操舵輪の駆動部に設けられたパルスエンコーダ、移動体６０の向きの計測はジャイロセンサなどの内界センサを使用するのが一般的である。
【０００６】
ここで、ある時刻の移動体６０の向きをθＬ、前操舵角をα、前操舵輪６１の移動速度をＶＳとすると、前操舵輪６１のＸ方向の移動速度ＶＸＳ、Ｙ方向の移動速度ＶＹＳは次式によって求めることができる。
【０００７】
ＶＸＳ＝ＶＳ×ｃｏｓ（θＬ＋α）
ＶＹＳ＝ＶＳ×ｓｉｎ（θＬ＋α）
前操舵輪６１の位置（移動量）は、これらの移動速度を積分することによって算出することができる。なお、後操舵輪６２の位置は、移動体６０の向きと前操舵輪６１の位置と移動体６０の寸法、例えばホイールベースＬを使用して幾何学関係から求めることができる。
【０００８】
このとき、移動体６０の向きθＬ、前操舵角α、前操舵輪６１の移動速度ＶＳを正しく計測できれば、移動体６０の位置の算出結果に誤差は発生しない。
【０００９】
【発明が解決しようとする課題】
しかし、一般に操舵角αの計測は、操舵機構に含まれる減速器のバックラッシュによる誤差や、操舵角の正確な零点調整が困難であることから、許容できない誤差を持つ場合が多い。このため、操舵角計測誤差に起因した移動体位置の計測誤差が発生し、実用上の問題点となっている。
【００１０】
そこで、本発明の課題は、移動体の位置を、極力操舵角を使用せずに、移動体の向きと操舵輪の移動距離から求めることのできる位置計測方法及び計測装置を提供することにある。
【００１１】
【課題を解決するための手段】
本発明は、前操舵輪、後操舵輪を有する移動体の位置計測方法において、前記移動体が旋回運動している間の位置計測を、微小時間での前記移動体の姿勢角変化量、前操舵輪移動量、後操舵輪移動量から前記前操舵輪、前記後操舵輪の旋回半径を算出するステップと、算出された前記前操舵輪の旋回半径、前記後操舵輪の旋回半径及び前記移動体のホイールベース長さからホイールベース上に設定した計測点の旋回半径を算出するステップと、算出された前記計測点の旋回半径、前記ホイールベース長さ、前記前操舵輪の旋回半径又は前記後操舵輪の旋回半径から、前記計測点と旋回中心とを結ぶ線分と前記ホイールベースの線分との間の角度Ｃを算出するステップと、前記計測点の旋回半径と前記微小時間での前記姿勢角変化量とから移動体上に設定された座標系における前記計測点のｘ方向移動量、ｙ方向移動量を算出するステップと、算出されたｘ方向移動量、ｙ方向移動量に対して、前記角度Ｃを使用して回転補正を行い、回転補正されたｘ方向移動量及びｙ方向移動量を算出するステップと、移動体の向きを計測し、計測された移動体の向きと前記回転補正されたｘ方向移動量及びｙ方向移動量とから地上に設定された座標系における前記計測点のＸ方向移動量、Ｙ方向移動量を算出するステップを実行して行なうことを特徴とする。
【００１２】
本位置計測方法においては、前記移動体が旋回運動しているかどうかの判定を行い、旋回運動していないと判定された場合には、前記前操舵輪移動量、前操舵輪操舵角、前記後操舵輪移動量、後操舵輪操舵角、前記移動体の姿勢角変化量及びホイールベース長さを用いて、推測航法により移動体の位置計測を行い、前記移動体が旋回運動していると判定された場合には、上記の移動体が旋回運動している間の位置計測を行うようにすることが好ましい。
【００１４】
本位置計測方法においては、前記移動体が旋回運動しているかどうかの判定は、前記姿勢角変化量をあらかじめ定められたしきい値と比較して行われる。
【００１５】
本発明によればまた、前操舵輪、後操舵輪を有する移動体の位置計測装置が提供される。本位置計測装置においては、移動体の向きを計測する向き計測手段からの移動体の姿勢角変化量を示す信号、前操舵輪走行距離計測手段からの前操舵輪移動量を示す信号及び前操舵輪操舵角計測手段からの前操舵輪操舵角を示す信号、後操舵輪走行距離計測手段からの後操舵輪移動量を示す信号及び後操舵輪操舵角計測手段からの後操舵輪操舵角を示す信号を受ける位置算出処理手段を備え、該位置算出処理手段は、移動体が旋回運動している間の位置計測を、微小時間での前記姿勢角変化量、前記前操舵輪移動量、前記後操舵輪移動量から前記前操舵輪、前記後操舵輪の旋回半径を算出するステップと、算出された前記前操舵輪の旋回半径、前記後操舵輪の旋回半径及び移動体のホイールベース長さからホイールベース上に設定した計測点の旋回半径を算出するステップと、算出された前記計測点の旋回半径、前記ホイールベース長さ、前記前操舵輪の旋回半径又は前記後操舵輪の旋回半径から、前記計測点と旋回中心とを結ぶ線分と前記ホイールベースの線分との間の角度Ｃを算出するステップと、前記計測点の旋回半径と前記微小時間での前記姿勢角変化量とから移動体上に設定された座標系における前記計測点のｘ方向移動量、ｙ方向移動量を算出するステップと、算出されたｘ方向移動量、ｙ方向移動量に対して、前記角度Ｃを使用して回転補正を行い、回転補正されたｘ方向移動量及びｙ方向移動量を算出するステップと、回転補正されたｘ方向移動量及びｙ方向移動量と計測された移動体の向きとから地上に設定された座標系における前記計測点のＸ方向移動量、Ｙ方向移動量を算出するステップを実行して行うことを特徴とする。
【００１６】
本位置計測装置においては、前記移動体が旋回運動しているかどうかの判定を行う判定手段を更に備え、前記位置算出処理手段は、前記判定手段により前記移動体が旋回運動していないと判定された場合には、前記前操舵輪移動量、前記前操舵輪操舵角、前記後操舵輪移動量、前記後操舵輪操舵角、前記姿勢角変化量及びホイールベース長さを用いて、推測航法により移動体の位置計測を行い、前記位置算出処理手段は、前記判定手段により前記移動体が旋回運動していると判定された場合には、上記の移動体が旋回運動している間の位置計測を行うことが望ましい。
【００１７】
前記判定手段は、前記姿勢角変化量をあらかじめ定められたしきい値と比較して判定を行う。
【００１９】
【発明の実施の形態】
図１を参照して、本発明の実施の形態について説明する。本発明による位置計測装置１０は判定部１０−１と位置算出処理部１０−２とを有し、位置算出処理部１０−２は判定部１０−１の判定結果に応じて異なった方法で位置計測の演算を行う。すなわち、判定部１０−１は、移動体が旋回運動中であるかどうかの判定を行い、判定結果を位置算出処理部１０−２に与える。判定結果が旋回運動中でない場合には、位置算出処理部１０−２は、以下の各種センサからの信号を使用して従来の技術において説明した推測航法と同じ計測方法で位置計測を行なう。一方、判定結果が旋回運動中である場合には、位置算出処理部１０−２は以下の各種センサからの信号のうち操舵角を示す信号は使用せずに、以下に述べる方法で位置計測を行なう。
【００２０】
移動体の向きを計測するために移動体にはジャイロセンサ２１が設けられている。移動体の前操舵輪には、前操舵輪の移動距離を計測する前操舵輪走行距離エンコーダ２２と前操舵輪の操舵角を計測する前操舵輪操舵角エンコーダ２３が取り付けられている。移動体の後操舵輪には、後操舵輪の移動距離を計測する後操舵輪走行距離エンコーダ２４と後操舵輪の操舵角を計測する後操舵輪操舵角エンコーダ２５が取り付けられている。
【００２１】
位置計測装置１０は上記の計測データ及び内部に有する移動体の車体データ（例えば、ホイールベースＬ）を使用して現在位置を演算し、移動体位置を出力する。
【００２２】
図２に示す移動体３０は前操舵輪３１の操舵角と後操舵輪３２の操舵角とが異なっている場合に旋回運動を行う。
【００２３】
判定部１０−１では、ジャイロセンサ２１からの信号により微少時間内の姿勢角の変化量Δθを計測し、計測した変化量Δθにより旋回運動を行なっているかどうかを判定する。特に、判定部１０−１は、変化量Δθ＞しきい値ｄの時、旋回運動中であると判定してその旨を位置算出処理部１０−２に出力する。この場合、位置算出処理部１０−２は、操舵角を使用せずに以下に説明する本発明による計測方法により位置計測を行なう。
【００２４】
一方、Δθ≦しきい値ｄの時には、判定部１０−１は、旋回運動を行なっていない（姿勢を変化させずに走行している状態）と判定し、その旨を位置算出処理部１０−２に出力する。この場合、位置算出処理部１０−２は、従来の技術において説明した推測航法と同じ計測方法で位置計測を行なう。
【００２５】
次に、移動体が旋回運動を行なっている時の位置計測演算方法について説明する。移動体の旋回運動は、前操舵輪３１の車軸の延長線と後操舵輪３２の車軸の延長線の交点を旋回中心Ｐｃとした旋回動作となる。ここでは、移動体のホイールベース中間位置を位置計測上の計測点（基準点）Ｐｓとし、以下に計測点Ｐｓの位置を求める方法を説明する。
【００２６】
ステップ１
微少時間での姿勢角の変化量Δθ（ラジアン）、前操舵輪３１の移動距離及び後操舵輪３２の移動距離から前操舵輪３１の旋回半径Ｒｆ、後操舵輪３２の旋回半径Ｒｂを算出する。
【００２７】
Ｒｆ＝（前操舵輪３１の移動距離ΔＬｆ／姿勢角変化量Δθ）
Ｒｂ＝（後操舵輪３２の移動距離ΔＬｒ／姿勢角変化量Δθ）
ステップ２
旋回中心Ｐｃから計測点Ｐｓまでの旋回半径Ｒｃを求める。
【００２８】
Ｒｃ＝［（Ｒｆ2 ＋Ｒｂ2 ）／２−Ｌ2 ／４］1/2
ステップ３
移動体の姿勢がΔθ変化しているので、計測点Ｐｓの移動量ΔＬｃは、
ΔＬｃ＝Ｒｃ×Δθ
で求めることが出来る。
【００２９】
ステップ４
ホイールベースの線分と、計測点Ｐｓと旋回中心Ｐｃとを結ぶ線分との間の角度Ｃを求める。
【００３０】
Ｃ＝ｃｏｓ-1［（Ｒｃ2 ＋（Ｌ／２）2 −Ｒｆ2 ）／（２×Ｒｃ×Ｌ／２）］
なお、Ｒｆに代えてＲｂを用いることもできる。
【００３１】
ステップ５
移動体上の座標系（図３）で計測点Ｐｓの移動量を求める。図４において、
ｘ移動量ΔＬｃｘ＝Ｒｃ×ｓｉｎ（Δθ）
ｙ移動量ΔＬｃｙ＝Ｒｃ×［１．０−ｃｏｓ（Δθ）］
なお、これらの式は旋回中心Ｐｃが移動体上の座標系におけるｙ軸上にある場合である。
【００３２】
ステップ６
旋回中心Ｐｃがｙ軸上に無い場合を考慮し、移動量の回転補正を行なう。
【００３３】
回転補正量γは角度Ｃから演算で求めることができる。
【００３４】
回転補正処理は、以下の数１で表される。
【００３５】
【数１】
図５の場合について言えば、
ｘ補正移動量ΔＬｃｘ´＝ΔＬｃｘ×ｃｏｓγ＋ΔＬｃｙ×ｓｉｎγ
ｙ補正移動量ΔＬｃｙ´＝−ΔＬｃｘ×ｓｉｎγ＋ΔＬｃｙ×ｃｏｓγ
ステップ７
地上に固定した座標系での計測点Ｐｓの位置を求める。
【００３７】
図６を参照して、いま、Ｘ方向に関してのある時刻の移動体の向き（図示しない固定輪の向きに同じ）をθＬとすると、地上座標系での移動量は次式により求まる。
【００３８】
Ｘ方向移動量ΔＸＬ＝ΔＬｃｘ´×ｃｏｓθＬ−ΔＬｃｙ´×ｓｉｎθＬ
Ｙ方向移動量ΔＹＬ＝ΔＬｃｘ´×ｓｉｎθＬ＋ΔＬｃｙ´×ｃｏｓθＬ
従って、計測点Ｐｓの位置（ＸＬ，ＹＬ）は、各移動量ΔＸＬとΔＹＬを積分することによって求められる。
【００３９】
本例ではホイールベースの中間点を位置の計測点Ｐｓとしたが、計測点Ｐｓを他の箇所に設定した場合でも同様の考え方で位置を算出することが出来る。また、本発明は、無人搬送車や無人フォークリフト、無人トラクタのための位置計測に適用することができる。
【００４０】
【発明の効果】
従来は、全ての位置計測時に操舵角を使用していたため、操舵角を正確に計測出来ない場合には常に位置計測誤差が発生していたが、本発明によれば、旋回運動中は移動体の位置演算処理で操舵角度を計測する必要が無いので、操舵角を正確に計測できない場合でも、移動体の位置を正しく推定することができる。
【００４１】
また、旋回運動中であるかどうかを自動で判断して計測方式を切り替えるため、操舵角による位置計測誤差を低減することが出来る。
【図面の簡単な説明】
【図１】本発明による位置計測装置と位置計測用の各種センサとの関係を示したブロック図である。
【図２】移動体が旋回運動中の移動体上での計測点の移動量算出を説明するための図である。
【図３】移動体に座標系を設定した場合の座標軸と計測点の関係を示した図である。
【図４】移動体の座標系での姿勢角変化量Δθに対する計測点の移動量算出を説明するための図である。
【図５】移動量の回転補正について説明するための図である。
【図６】座標系を地上に設定した場合の位置計測を説明するための図である。
【図７】従来の推測航法により位置計測を行う場合について説明するための図である。
【符号の説明】
１０ 位置計測装置
１０−１ 判定部
１０−２ 位置算出処理部
３０、６０ 移動体
３１、６１ 前操舵輪
３２、６２ 後操舵輪[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a position measuring method and a measuring apparatus for a moving body such as an automatic guided vehicle.
[0002]
[Prior art]
AGV and AGF automatic guided vehicles (hereinafter referred to as “moving bodies”) do not require fixed equipment such as rails on the ground, and can be used as a major component of a transport system because they can share space with people and vehicles.
[0003]
Conventionally, an electromagnetic induction method, a magnetic induction method, and an optical reflection method are often used as a moving body induction method. However, these systems cannot easily cope with layout changes because guide wires, magnetic tapes, and optical tapes are installed on the traveling path. Moreover, the durability of the magnetic tape or optical reflective tape affixed to the floor is poor.
[0004]
In addition to the above-described guidance method, so-called dead reckoning navigation is known in which the position of the moving body is measured using an internal sensor attached to the moving body.
[0005]
A typical dead reckoning method will be described with reference to FIG. 7 in the case of a moving
[0006]
Here, assuming that the direction of the moving
[0007]
VXS = VS × cos (θL + α)
VYS = VS × sin (θL + α)
The position (movement amount) of the front steering wheel 61 can be calculated by integrating these movement speeds. The position of the rear steering wheel 62 can be obtained from the geometric relationship using the orientation of the moving
[0008]
At this time, if the direction θL of the
[0009]
[Problems to be solved by the invention]
However, in general, the measurement of the steering angle α often has an unacceptable error because an error due to a backlash of a decelerator included in the steering mechanism and an accurate zero adjustment of the steering angle are difficult. For this reason, a measurement error of the moving body position due to the steering angle measurement error occurs, which is a practical problem.
[0010]
Accordingly, an object of the present invention is to provide a position measuring method and a measuring device that can determine the position of a moving body from the direction of the moving body and the moving distance of the steering wheel without using the steering angle as much as possible. .
[0011]
[Means for Solving the Problems]
The present invention relates to a position measuring method for a moving body having a front steering wheel and a rear steering wheel, wherein the position measurement while the moving body is turning is performed by measuring the amount of change in the posture angle of the moving body in a minute time, The step of calculating the turning radius of the front steering wheel and the rear steering wheel from the steering wheel movement amount and the rear steering wheel movement amount, the calculated turning radius of the front steering wheel, the turning radius of the rear steering wheel and the movement Calculating the turning radius of the measurement point set on the wheel base from the wheel base length of the body, the calculated turning radius of the measurement point, the wheel base length, the turning radius of the front steering wheel, or the rear A step of calculating an angle C between a line segment connecting the measurement point and the turning center and a line segment of the wheel base from the turning radius of the steered wheel; and the turning radius of the measurement point and the minute time Move from posture angle change amount The step of calculating the x-direction movement amount and the y-direction movement amount of the measurement point in the coordinate system set above, and using the angle C for the calculated x-direction movement amount and y-direction movement amount Performing rotation correction, calculating the rotation corrected x-direction movement amount and y-direction movement amount, measuring the direction of the moving body, measuring the direction of the moving body, the rotation-corrected x-direction movement amount, and The step of calculating the X-direction movement amount and the Y-direction movement amount of the measurement point in the coordinate system set on the ground from the y-direction movement amount is performed .
[0012]
In this position measurement method, it is determined whether or not the moving body is making a turning motion. If it is determined that the moving body is not making a turning motion, the front steering wheel movement amount, the front steering wheel steering angle, The position of the moving body is measured by dead reckoning using the moving amount of the steering wheel, the rear steering wheel steering angle, the attitude angle change amount of the moving body, and the wheelbase length, and it is determined that the moving body is making a turning motion. In such a case, it is preferable to perform position measurement while the above moving body is turning.
[0014]
In this position measurement method, the determination as to whether or not the moving body is turning is performed by comparing the amount of change in posture angle with a predetermined threshold value.
[0015]
According to the present invention, there is also provided a moving body position measuring device having a front steering wheel and a rear steering wheel. In this position measuring device, a signal indicating the amount of change in the attitude angle of the moving body from the direction measuring means for measuring the direction of the moving body, a signal indicating the amount of front steering wheel movement from the front steered wheel travel distance measuring means, and the front steering A signal indicating the front steering wheel steering angle from the wheel steering angle measuring means, a signal indicating the rear steering wheel movement amount from the rear steering wheel travel distance measuring means, and the rear steering wheel steering angle from the rear steering wheel steering angle measuring means Position calculation processing means for receiving a signal, the position calculation processing means for measuring the position while the moving body is turning, the posture angle change amount in a minute time, the front steering wheel movement amount, the rear From the step of calculating the turning radius of the front steering wheel and the rear steering wheel from the moving amount of the steering wheel, and from the calculated turning radius of the front steering wheel, the turning radius of the rear steering wheel, and the wheel base length of the moving body Measurement points set on the wheelbase A step of calculating a turning radius and connecting the measurement point and the turning center from the calculated turning radius of the measurement point, the wheelbase length, the turning radius of the front steering wheel or the turning radius of the rear steering wheel. In the coordinate system set on the moving body from the step of calculating the angle C between the line segment and the wheel base line segment, the turning radius of the measurement point, and the posture angle change amount in the minute time The step of calculating the x-direction movement amount and the y-direction movement amount of the measurement point, and the rotation correction is performed using the angle C for the calculated x-direction movement amount and y-direction movement amount. The measurement point in the coordinate system set on the ground from the step of calculating the x-direction movement amount and the y-direction movement amount, the rotation-corrected x-direction movement amount and y-direction movement amount, and the measured moving body orientation X direction movement amount, Y And performing by performing the step of calculating the direction movement amount.
[0016]
The position measuring apparatus further includes a determination unit that determines whether or not the moving body is making a turning motion, and the position calculation processing unit is determined by the determination unit that the moving body is not making a turning motion. In the case of the dead wheel, the front steering wheel movement angle, the front steering wheel steering angle, the rear steering wheel movement amount, the rear steering wheel steering angle, the attitude angle change amount, and the wheel base length, The position of the moving body is measured, and the position calculation processing means determines the position during the turning movement of the moving body when the determining means determines that the moving body is turning. It is desirable to do.
[0017]
The determination means performs the determination by comparing the posture angle change amount with a predetermined threshold value.
[0019]
DETAILED DESCRIPTION OF THE INVENTION
An embodiment of the present invention will be described with reference to FIG. The position measurement apparatus 10 according to the present invention includes a determination unit 10-1 and a position calculation processing unit 10-2, and the position calculation processing unit 10-2 is positioned in a different manner depending on the determination result of the determination unit 10-1. Perform measurement calculations. That is, the determination unit 10-1 determines whether the moving body is performing a turning motion, and gives the determination result to the position calculation processing unit 10-2. When the determination result is not during the turning motion, the position calculation processing unit 10-2 performs position measurement by the same measurement method as the dead reckoning navigation described in the related art using signals from the following various sensors. On the other hand, when the determination result is a turning motion, the position calculation processing unit 10-2 does not use a signal indicating the steering angle among signals from the following various sensors, and performs position measurement by the method described below. Do.
[0020]
A
[0021]
The position measuring device 10 calculates the current position using the above measurement data and the vehicle body data (for example, the wheel base L) of the moving body provided therein, and outputs the moving body position.
[0022]
The moving
[0023]
The determination unit 10-1 measures the change amount Δθ of the posture angle within a minute time based on the signal from the
[0024]
On the other hand, when Δθ ≦ threshold value d, the determination unit 10-1 determines that the turning motion is not performed (the state where the vehicle is traveling without changing its posture), and the position calculation processing unit 10- Output to 2. In this case, the position calculation processing unit 10-2 performs position measurement by the same measurement method as the dead reckoning navigation described in the related art.
[0025]
Next, a position measurement calculation method when the moving body performs a turning motion will be described. The turning motion of the moving body is a turning operation in which the intersection of the axle extension line of the front steering wheel 31 and the axle extension line of the rear steering wheel 32 is the turning center Pc. Here, the wheel base intermediate position of the moving body is set as a measurement point (reference point) Ps in position measurement, and a method for obtaining the position of the measurement point Ps will be described below.
[0026]
Step 1
The turning radius Rf of the front steering wheel 31 and the turning radius Rb of the rear steering wheel 32 are calculated from the change amount Δθ (radian) of the attitude angle in a very short time, the movement distance of the front steering wheel 31 and the movement distance of the rear steering wheel 32. .
[0027]
Rf = (movement distance ΔLf of front steering wheel 31 / posture angle change amount Δθ)
Rb = (movement distance ΔLr / posture angle change amount Δθ of the rear steering wheel 32)
A turning radius Rc from the turning center Pc to the measurement point Ps is obtained.
[0028]
Rc = [(Rf 2 + Rb 2) / 2-
Step 3
Since the posture of the moving body has changed by Δθ, the movement amount ΔLc of the measurement point Ps is
ΔLc = Rc × Δθ
Can be obtained.
[0029]
Step 4
An angle C between the wheel base line segment and the line segment connecting the measurement point Ps and the turning center Pc is obtained.
[0030]
C = cos −1 [(Rc 2 + (L / 2) 2 −Rf 2 ) / (2 × Rc × L / 2)]
Rb can be used instead of Rf.
[0031]
Step 5
The amount of movement of the measurement point Ps is obtained in the coordinate system (FIG. 3) on the moving body. In FIG.
x movement amount ΔLcx = Rc × sin (Δθ)
y movement amount ΔLcy = Rc × [1.0−cos (Δθ)]
These equations are for the case where the turning center Pc is on the y-axis in the coordinate system on the moving body.
[0032]
Step 6
Considering the case where the turning center Pc is not on the y-axis, the rotation of the movement amount is corrected.
[0033]
The rotation correction amount γ can be obtained from the angle C by calculation.
[0034]
The rotation correction process is expressed by the following formula 1.
[0035]
[Expression 1]
Speaking of the case of FIG.
x correction movement amount ΔLcx ′ = ΔLcx × cos γ + ΔLcy × sin γ
y correction movement amount ΔLcy ′ = − ΔLcx × sin γ + ΔLcy × cos γ
Step 7
The position of the measurement point Ps in the coordinate system fixed on the ground is obtained.
[0037]
Referring to FIG. 6, if the direction of the moving body at a certain time with respect to the X direction (same as the direction of a fixed wheel not shown) is θL, the amount of movement in the ground coordinate system is obtained by the following equation.
[0038]
X-direction movement amount ΔXL = ΔLcx ′ × cos θL−ΔLcy ′ × sin θL
Y-direction movement amount ΔYL = ΔLcx ′ × sin θL + ΔLcy ′ × cos θL
Therefore, the position (XL, YL) of the measurement point Ps is obtained by integrating the movement amounts ΔXL and ΔYL.
[0039]
In this example, the intermediate point of the wheel base is the position measurement point Ps. However, even when the measurement point Ps is set at another location, the position can be calculated in the same way. The present invention can also be applied to position measurement for automatic guided vehicles, automatic forklifts, and automatic tractors.
[0040]
【The invention's effect】
Conventionally, since the steering angle was used for all position measurements, a position measurement error always occurred when the steering angle could not be accurately measured. Since there is no need to measure the steering angle in this position calculation process, the position of the moving body can be correctly estimated even if the steering angle cannot be measured accurately.
[0041]
Further, since it is automatically determined whether or not a turning motion is being performed and the measurement method is switched, a position measurement error due to a steering angle can be reduced.
[Brief description of the drawings]
FIG. 1 is a block diagram showing the relationship between a position measuring device according to the present invention and various sensors for position measurement.
FIG. 2 is a diagram for explaining calculation of a moving amount of a measurement point on a moving body in which the moving body is making a turning motion.
FIG. 3 is a diagram showing a relationship between coordinate axes and measurement points when a coordinate system is set for a moving object.
FIG. 4 is a diagram for explaining calculation of a moving amount of a measurement point with respect to a posture angle change amount Δθ in a coordinate system of a moving body.
FIG. 5 is a diagram for explaining movement amount rotation correction;
FIG. 6 is a diagram for explaining position measurement when a coordinate system is set on the ground.
FIG. 7 is a diagram for explaining a case where position measurement is performed by conventional dead reckoning navigation.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 10 Position measuring apparatus 10-1 Determination part 10-2 Position
Claims (6)
前記移動体が旋回運動している間の位置計測を、
微小時間での前記移動体の姿勢角変化量、前操舵輪移動量、後操舵輪移動量から前記前操舵輪、前記後操舵輪の旋回半径を算出するステップと、
算出された前記前操舵輪の旋回半径、前記後操舵輪の旋回半径及び前記移動体のホイールベース長さからホイールベース上に設定した計測点の旋回半径を算出するステップと、
算出された前記計測点の旋回半径、前記ホイールベース長さ、前記前操舵輪の旋回半径又は前記後操舵輪の旋回半径から、前記計測点と旋回中心とを結ぶ線分と前記ホイールベースの線分との間の角度Ｃを算出するステップと、
前記計測点の旋回半径と前記微小時間での前記姿勢角変化量とから移動体上に設定された座標系における前記計測点のｘ方向移動量、ｙ方向移動量を算出するステップと、
算出されたｘ方向移動量、ｙ方向移動量に対して、前記角度Ｃを使用して回転補正を行い、回転補正されたｘ方向移動量及びｙ方向移動量を算出するステップと、
移動体の向きを計測し、計測された移動体の向きと前記回転補正されたｘ方向移動量及びｙ方向移動量とから地上に設定された座標系における前記計測点のＸ方向移動量、Ｙ方向移動量を算出するステップを実行して行なうことを特徴とする移動体の位置計測方法。 In the method of measuring the position of a moving body having a front steering wheel and a rear steering wheel,
Position measurement while the mobile body is turning,
Calculating the turning radius of the front steering wheel and the rear steering wheel from the amount of change in attitude angle of the moving body in a minute time, the amount of movement of the front steering wheel, and the amount of movement of the rear steering wheel;
Calculating the turning radius of the measurement point set on the wheel base from the calculated turning radius of the front steering wheel, the turning radius of the rear steering wheel, and the wheel base length of the movable body;
From the calculated turning radius of the measurement point, the wheelbase length, the turning radius of the front steering wheel or the turning radius of the rear steering wheel, a line segment connecting the measurement point and the turning center and the wheelbase line Calculating an angle C between minutes;
Calculating an x-direction movement amount and a y-direction movement amount of the measurement point in a coordinate system set on a moving body from the turning radius of the measurement point and the posture angle change amount in the minute time;
Performing rotation correction on the calculated x-direction movement amount and y-direction movement amount using the angle C, and calculating the rotation-corrected x-direction movement amount and y-direction movement amount;
The direction of the moving body is measured, and the X direction movement amount of the measurement point in the coordinate system set on the ground from the measured moving body direction and the rotation corrected x direction movement amount and y direction movement amount, Y A method for measuring a position of a moving body, comprising performing a step of calculating a direction moving amount .
前記移動体が旋回運動しているかどうかの判定を行い、It is determined whether or not the mobile body is swiveling,
旋回運動していないと判定された場合には、前記前操舵輪移動量、前操舵輪操舵角、前記後操舵輪移動量、後操舵輪操舵角、前記移動体の姿勢角変化量及びホイールベース長さを用いて、推測航法により移動体の位置計測を行い、When it is determined that the vehicle is not turning, the front steering wheel movement amount, the front steering wheel steering angle, the rear steering wheel movement amount, the rear steering wheel steering angle, the posture angle change amount of the moving body, and the wheel base Using the length, measure the position of the moving body by dead reckoning navigation,
前記移動体が旋回運動していると判定された場合には、前記移動体が旋回運動している間の位置計測を行うことを特徴とする移動体の位置計測方法。When it is determined that the moving body is making a turning motion, a position measurement is performed while the moving body is making a turning motion.
前記移動体が旋回運動しているかどうかの判定は、前記姿勢角変化量をあらかじめ定められたしきい値と比較して行われることを特徴とする移動体の位置計測方法。The determination as to whether or not the moving body is turning is performed by comparing the amount of change in posture angle with a predetermined threshold value.
移動体の向きを計測する向き計測手段からの移動体の姿勢角変化量を示す信号、前操舵輪走行距離計測手段からの前操舵輪移動量を示す信号及び前操舵輪操舵角計測手段からの前操舵輪操舵角を示す信号、後操舵輪走行距離計測手段からの後操舵輪移動量を示す信号及び後操舵輪操舵角計測手段からの後操舵輪操舵角を示す信号を受ける位置算出処理手段を備え、A signal indicating the amount of change in the attitude angle of the moving body from the direction measuring means for measuring the direction of the moving body, a signal indicating the amount of front steering wheel movement from the front steered wheel travel distance measuring means, and a signal from the front steering wheel steering angle measuring means Position calculation processing means for receiving a signal indicating the front steering wheel steering angle, a signal indicating the rear steering wheel movement amount from the rear steering wheel travel distance measurement means, and a signal indicating the rear steering wheel steering angle from the rear steering wheel steering angle measurement means With
該位置算出処理手段は、移動体が旋回運動している間の位置計測を、The position calculation processing means performs position measurement while the moving body is turning.
微小時間での前記姿勢角変化量、前記前操舵輪移動量、前記後操舵輪移動量から前記前操舵輪、前記後操舵輪の旋回半径を算出するステップと、Calculating a turning radius of the front steering wheel and the rear steering wheel from the amount of change in attitude angle in a minute time, the amount of movement of the front steering wheel, and the amount of movement of the rear steering wheel;
算出された前記前操舵輪の旋回半径、前記後操舵輪の旋回半径及び移動体のホイールベース長さからホイールベース上に設定した計測点の旋回半径を算出するステップと、Calculating the turning radius of the measurement point set on the wheel base from the calculated turning radius of the front steering wheel, the turning radius of the rear steering wheel, and the wheel base length of the moving body;
算出された前記計測点の旋回半径、前記ホイールベース長さ、前記前操舵輪の旋回半径又は前記後操舵輪の旋回半径から、前記計測点と旋回中心とを結ぶ線分と前記ホイールベースの線分との間の角度Ｃを算出するステップと、From the calculated turning radius of the measurement point, the wheelbase length, the turning radius of the front steering wheel or the turning radius of the rear steering wheel, a line segment connecting the measurement point and the turning center and the wheelbase line Calculating an angle C between minutes;
前記計測点の旋回半径と前記微小時間での前記姿勢角変化量とから移動体上に設定された座標系における前記計測点のｘ方向移動量、ｙ方向移動量を算出するステップと、Calculating an x-direction movement amount and a y-direction movement amount of the measurement point in a coordinate system set on a moving body from the turning radius of the measurement point and the posture angle change amount in the minute time;
算出されたｘ方向移動量、ｙ方向移動量に対して、前記角度Ｃを使用して回転補正を行い、回転補正されたｘ方向移動量及びｙ方向移動量を算出するステップと、Performing rotation correction on the calculated x-direction movement amount and y-direction movement amount using the angle C, and calculating the rotation-corrected x-direction movement amount and y-direction movement amount;
回転補正されたｘ方向移動量及びｙ方向移動量と計測された移動体の向きとから地上に設定された座標系における前記計測点のＸ方向移動量、Ｙ方向移動量を算出するステップを実行して行うことを特徴とする移動体の位置計測装置。A step of calculating the X-direction movement amount and the Y-direction movement amount of the measurement point in the coordinate system set on the ground from the rotation-corrected x-direction movement amount and y-direction movement amount and the measured direction of the moving body is executed. A position measurement apparatus for a moving body, which is performed as described above.
前記移動体が旋回運動しているかどうかの判定を行う判定手段を更に備え、A determination means for determining whether or not the movable body is turning;
前記位置算出処理手段は、前記判定手段により前記移動体が旋回運動していないと判定された場合には、前記前操舵輪移動量、前記前操舵輪操舵角、前記後操舵輪移動量、前記後操舵輪操舵角、前記姿勢角変化量及びホイールベース長さを用いて、推測航法により移動体の位置計測を行い、The position calculation processing unit, when the determination unit determines that the moving body is not turning, the front steering wheel movement amount, the front steering wheel steering angle, the rear steering wheel movement amount, Using the rear steering wheel steering angle, the attitude angle change amount and the wheelbase length, the position of the moving body is measured by dead reckoning navigation,
前記位置算出処理手段は、前記判定手段により前記移動体が旋回運動していると判定された場合には、前記移動体が旋回運動している間の位置計測を行うことを特徴とする移動体の位置計測装置。The position calculation processing means performs position measurement while the moving body is making a turning motion when the determining means determines that the moving body is making a turning movement. Position measuring device.
前記判定手段は、前記姿勢角変化量をあらかじめ定められたしきい値と比較して判定を行うことを特徴とする移動体の位置計測装置。The determination unit is configured to perform determination by comparing the posture angle change amount with a predetermined threshold value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000225081A JP4382266B2 (en) | 2000-07-26 | 2000-07-26 | Method and apparatus for measuring position of moving object |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000225081A JP4382266B2 (en) | 2000-07-26 | 2000-07-26 | Method and apparatus for measuring position of moving object |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2002039784A JP2002039784A (en) | 2002-02-06 |
| JP4382266B2 true JP4382266B2 (en) | 2009-12-09 |
Family
ID=18718916
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2000225081A Expired - Fee Related JP4382266B2 (en) | 2000-07-26 | 2000-07-26 | Method and apparatus for measuring position of moving object |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4382266B2 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110428603A (en) * | 2019-07-26 | 2019-11-08 | 北京主线科技有限公司 | Following Car travel control method and device in container truck formation |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10228701B2 (en) | 2013-12-27 | 2019-03-12 | Komatsu Ltd. | Mining machine management system and management method |
| CN109916403B (en) * | 2017-12-13 | 2023-12-05 | 华中科技大学 | Short-time accurate positioning device and method for AGV trolley |
| CN111854731B (en) * | 2020-07-22 | 2022-04-29 | 中国第一汽车股份有限公司 | Pose determination method and device, vehicle and storage medium |
| CN114111681B (en) * | 2021-11-24 | 2023-06-06 | 福建汉特云智能科技有限公司 | Wheelbase calibration method and system for robot chassis |
-
2000
- 2000-07-26 JP JP2000225081A patent/JP4382266B2/en not_active Expired - Fee Related
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110428603A (en) * | 2019-07-26 | 2019-11-08 | 北京主线科技有限公司 | Following Car travel control method and device in container truck formation |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2002039784A (en) | 2002-02-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0769735B1 (en) | Motion tracking apparatus for driverless vehicle | |
| US5345382A (en) | Calibration method for a relative heading sensor | |
| JPH09319430A (en) | Navigation steering control system for automatic guided vehicle | |
| US20140236412A1 (en) | Apparatus and method for automatically parking vehicle | |
| JP4382266B2 (en) | Method and apparatus for measuring position of moving object | |
| JP3863303B2 (en) | Moving body position measuring device | |
| JP2916625B1 (en) | Vehicle attitude detection device | |
| JP3334074B2 (en) | Moving object position measurement device | |
| JPH1195837A (en) | Method for determining initial truck position and attitude angle of gyro guide type automated guided vehicle, and method for improving travel stability at position correction | |
| JPH10253372A (en) | Recognition apparatus for position of moving body | |
| JP2676831B2 (en) | Automatic guided vehicle position detection device | |
| JP2983527B1 (en) | Vehicle measuring device | |
| JP2000132229A (en) | Travel controlling method for movable body | |
| JP3447480B2 (en) | Position measurement method and device | |
| CN109916403A (en) | A kind of device and method pinpoint in short-term for AGV trolley | |
| CN114394102B (en) | Calibration method of attitude sensor and running device | |
| JP3239992B2 (en) | Error Correction Method for Steering Zero Information in Gyro Guided Automated guided Vehicle | |
| JP2668249B2 (en) | Method and device for measuring relative direction changes in an in-vehicle navigation system | |
| JPH06332531A (en) | Unmanned trackless vehicle | |
| EP1102140B1 (en) | Motion tracking apparatus for driverless vehicle | |
| JPH1195835A (en) | Traveling distance measurement error correction method for gyro-guidance type automated guided vehicle | |
| JP4467467B2 (en) | Driving control method of automatic guided vehicle | |
| JPS62221707A (en) | Gyro-guide type unmanned carrier | |
| JPH05297938A (en) | Coordinate position estimating device for vehicle | |
| JPH04201B2 (en) |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20061115 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20081225 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20090128 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090325 |
|
| 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: 20090826 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20090917 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121002 Year of fee payment: 3 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 Ref document number: 4382266 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131002 Year of fee payment: 4 |
|
| S802 | Written request for registration of partial abandonment of right |
Free format text: JAPANESE INTERMEDIATE CODE: R311802 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |