JP3033363B2 - 自走台車の絶対位置検出方法 - Google Patents
自走台車の絶対位置検出方法Info
- Publication number
- JP3033363B2 JP3033363B2 JP4273740A JP27374092A JP3033363B2 JP 3033363 B2 JP3033363 B2 JP 3033363B2 JP 4273740 A JP4273740 A JP 4273740A JP 27374092 A JP27374092 A JP 27374092A JP 3033363 B2 JP3033363 B2 JP 3033363B2
- Authority
- JP
- Japan
- Prior art keywords
- self
- propelled vehicle
- accelerometer
- detecting
- slip
- 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
- 238000001514 detection method Methods 0.000 title claims description 10
- 230000001133 acceleration Effects 0.000 claims description 19
- 238000000034 method Methods 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 description 4
- 230000010354 integration Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 3
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 101100512254 Rattus norvegicus Magi3 gene Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
Landscapes
- Measurement Of Distances Traversed On The Ground (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
【０００１】
【産業上の利用分野】本発明は工場、オフィス等の中を
無人で物を搬送する自走台車の移動平面内の絶対位置検
出方法に関する。
無人で物を搬送する自走台車の移動平面内の絶対位置検
出方法に関する。
【０００２】
【従来の技術】従来、自由平面を移動する自走台車の自
律走行誘導装置として、自走台車の車輪に設けた回転セ
ンサにより計測した移動距離と、自走台車に設けたレー
トジャイロにより角速度を積分することにより得られた
姿勢角とにより自走台車の位置を検出し、自走台車の停
止ごとに、レートジャイロのオフセット値を検出・記憶
しておき、自走台車の走行中に得られるレートジャイロ
の出力をこのオフセット値で補正するものがある（例え
ば、特開昭５８−１６６４０６号公報）。
律走行誘導装置として、自走台車の車輪に設けた回転セ
ンサにより計測した移動距離と、自走台車に設けたレー
トジャイロにより角速度を積分することにより得られた
姿勢角とにより自走台車の位置を検出し、自走台車の停
止ごとに、レートジャイロのオフセット値を検出・記憶
しておき、自走台車の走行中に得られるレートジャイロ
の出力をこのオフセット値で補正するものがある（例え
ば、特開昭５８−１６６４０６号公報）。
【０００３】
【発明が解決しようとする課題】ところが、移動距離を
車輪に設けた回転センサにより検出しているため、走行
面が傾斜していて車体が横すべりを起こした場合や走行
面の凹凸が激しく車輪がスリップした場合は、累積誤差
が大きい。また、レートジャイロは高価である等の問題
があった。そこで、本発明は、車体の横すべりや車輪の
スリップを補正し、自由平面を移動する自走台車の移動
距離検出方法を精度よく、安価に提供することを目的と
する。
車輪に設けた回転センサにより検出しているため、走行
面が傾斜していて車体が横すべりを起こした場合や走行
面の凹凸が激しく車輪がスリップした場合は、累積誤差
が大きい。また、レートジャイロは高価である等の問題
があった。そこで、本発明は、車体の横すべりや車輪の
スリップを補正し、自由平面を移動する自走台車の移動
距離検出方法を精度よく、安価に提供することを目的と
する。
【０００４】
【課題を解決するための手段】自由平面を移動する自走
台車の移動距離を左右の車輪に設けた回転位置検出器に
より検出し、自走台車の現在位置を検出方法において、
自走台車の左右の駆動軸上に、取付けられた進行方向の
加速度を検出する２つの加速度計と、左右の駆動軸と車
軸の交点上に設けた進行方向と直交方向の加速度を検出
する加速度計を設け、各加速度計の出力をもとに、自走
台車の横滑りと駆動輪のスリップを検出し、おのおのの
状態に対応した演算ルーチンにより、微少サンプリング
時間内の移動距離を演算し、それまでの移動距離に逐次
加算し、自走台車の現在位置の絶対座標を検出する。ま
た、各加速度計と回転位置検出器のゲインとオフセット
を補正し、各検出器のマッチングをとる。
台車の移動距離を左右の車輪に設けた回転位置検出器に
より検出し、自走台車の現在位置を検出方法において、
自走台車の左右の駆動軸上に、取付けられた進行方向の
加速度を検出する２つの加速度計と、左右の駆動軸と車
軸の交点上に設けた進行方向と直交方向の加速度を検出
する加速度計を設け、各加速度計の出力をもとに、自走
台車の横滑りと駆動輪のスリップを検出し、おのおのの
状態に対応した演算ルーチンにより、微少サンプリング
時間内の移動距離を演算し、それまでの移動距離に逐次
加算し、自走台車の現在位置の絶対座標を検出する。ま
た、各加速度計と回転位置検出器のゲインとオフセット
を補正し、各検出器のマッチングをとる。
【０００５】
【作用】各微少サンプリング時間毎の移動距離が、横滑
りやスリップ等の状況に対応して補正される。微少サン
プリング時間毎に、自走台車の自由平面における絶対位
置の座標を正確に把握する。
りやスリップ等の状況に対応して補正される。微少サン
プリング時間毎に、自走台車の自由平面における絶対位
置の座標を正確に把握する。
【０００６】
【実施例】以下に、本発明の実施例を図面により説明す
る。図１は、本発明による自走台車の構成を示す平面図
である。左右の駆動輪４Ｒ、４Ｌは駆動軸４１上に一直
線に配置してある。駆動輪４Ｒ、４Ｌは、回転位置検出
器６Ｒ、６Ｌを備えたおのおのの駆動装置５Ｒ、５Ｌに
より回転駆動し、走行方向の制御は左右の駆動輪４Ｒ、
４Ｌに回転速度差を付けることにより行う。駆動軸４１
上の任意の位置には、検出方向を車軸８１方向（以下、
自走台車の進行方向に相当するｘ軸方向とする。）とし
た加速度と設置位置回りの角加速度を検出する加速度計
１Ｒ、１Ｌを設けてある。駆動軸４１と車軸８１の交点
Ｏ上には、検出方向を駆動軸４１方向（以下、自走台車
の進行方向と直角方向に相当するｙ軸方向とする。）と
する加速度計２を設けてある。車体の適当な位置に配置
された制御装置９には、移動距離の計算や駆動装置５
Ｒ、５Ｌへの駆動指令を与えるための演算回路が格納さ
れている。図２は駆動指令と演算回路のブロック図を示
したものである。駆動装置５Ｒ、５Ｌに設けた回転位置
検出器６Ｒ，６Ｌの回転量を示すパルス信号は、おのお
ののカウンタ１５Ｒ、１５Ｌに入力する。カウンタ１５
Ｒ、１５Ｌの出力は、おのおののラッチ１６Ｒ，１６Ｌ
でラッチされＣＰＵ１７の発する払出し指令により回転
量パルス数を渡し、ＣＰＵ１７内に設けたＲＡＭに回転
量パルス数を記憶する。各加速度計１Ｒ、１Ｌ、２の出
力は、増幅回路１０Ｒ、１０Ｌ、１１によって適当なＤ
Ｃレベルの信号に変換された後、マルチプレクサ１３で
選択されＡ／Ｄコンバータ１４でデジタル化される。Ｃ
ＰＵ１７は、リアルタイムクロック１８によって、微小
なサンプル時間ごとに割り込みがかけられ、内蔵した移
動距離計算ルーチンを呼び出す。移動距離計算ルーチン
は、各サンプル時間毎に、駆動輪４のスリップおよび自
走台車の横滑りを調べ、演算式を選択する。選択した演
算式に対応し、加速度計１Ｒ、１Ｌ、２により計測され
る加速度をＡ／Ｄコンバータ１４からサンプリングし、
おのおのの加速度を積分し、サンプル時間内の自走台車
の移動距離を計算する。このとき同時に、回転位置検出
器６Ｒ，６Ｌのカウンタ１５Ｒ，１５Ｌの値もサンプリ
ングする。各サンプル時間毎の移動距離の演算結果は、
それまでの移動距離に加算し、移動平面の原点（０，
０、０）からの累積移動距離および累積移動方向とし
て、現在位置の絶対座標（Ｘ，Ｙ，Θ）をメモリ１９に
蓄積する。なお、ＣＰＵ１７の内部にマップ情報をもっ
ている場合は、移動距離とこれを比較し、自走台車のマ
ップ上の位置を把握しながら駆動装置を制御することも
できる。ＣＰＵ１７には、移動距離計算ルーチンのほか
に、走行制御ルーチン（例えば、経路・操舵・速度・加
速度等）を内蔵しており、モータ制御器２０Ｒ、２０Ｌ
に指令を出す。
る。図１は、本発明による自走台車の構成を示す平面図
である。左右の駆動輪４Ｒ、４Ｌは駆動軸４１上に一直
線に配置してある。駆動輪４Ｒ、４Ｌは、回転位置検出
器６Ｒ、６Ｌを備えたおのおのの駆動装置５Ｒ、５Ｌに
より回転駆動し、走行方向の制御は左右の駆動輪４Ｒ、
４Ｌに回転速度差を付けることにより行う。駆動軸４１
上の任意の位置には、検出方向を車軸８１方向（以下、
自走台車の進行方向に相当するｘ軸方向とする。）とし
た加速度と設置位置回りの角加速度を検出する加速度計
１Ｒ、１Ｌを設けてある。駆動軸４１と車軸８１の交点
Ｏ上には、検出方向を駆動軸４１方向（以下、自走台車
の進行方向と直角方向に相当するｙ軸方向とする。）と
する加速度計２を設けてある。車体の適当な位置に配置
された制御装置９には、移動距離の計算や駆動装置５
Ｒ、５Ｌへの駆動指令を与えるための演算回路が格納さ
れている。図２は駆動指令と演算回路のブロック図を示
したものである。駆動装置５Ｒ、５Ｌに設けた回転位置
検出器６Ｒ，６Ｌの回転量を示すパルス信号は、おのお
ののカウンタ１５Ｒ、１５Ｌに入力する。カウンタ１５
Ｒ、１５Ｌの出力は、おのおののラッチ１６Ｒ，１６Ｌ
でラッチされＣＰＵ１７の発する払出し指令により回転
量パルス数を渡し、ＣＰＵ１７内に設けたＲＡＭに回転
量パルス数を記憶する。各加速度計１Ｒ、１Ｌ、２の出
力は、増幅回路１０Ｒ、１０Ｌ、１１によって適当なＤ
Ｃレベルの信号に変換された後、マルチプレクサ１３で
選択されＡ／Ｄコンバータ１４でデジタル化される。Ｃ
ＰＵ１７は、リアルタイムクロック１８によって、微小
なサンプル時間ごとに割り込みがかけられ、内蔵した移
動距離計算ルーチンを呼び出す。移動距離計算ルーチン
は、各サンプル時間毎に、駆動輪４のスリップおよび自
走台車の横滑りを調べ、演算式を選択する。選択した演
算式に対応し、加速度計１Ｒ、１Ｌ、２により計測され
る加速度をＡ／Ｄコンバータ１４からサンプリングし、
おのおのの加速度を積分し、サンプル時間内の自走台車
の移動距離を計算する。このとき同時に、回転位置検出
器６Ｒ，６Ｌのカウンタ１５Ｒ，１５Ｌの値もサンプリ
ングする。各サンプル時間毎の移動距離の演算結果は、
それまでの移動距離に加算し、移動平面の原点（０，
０、０）からの累積移動距離および累積移動方向とし
て、現在位置の絶対座標（Ｘ，Ｙ，Θ）をメモリ１９に
蓄積する。なお、ＣＰＵ１７の内部にマップ情報をもっ
ている場合は、移動距離とこれを比較し、自走台車のマ
ップ上の位置を把握しながら駆動装置を制御することも
できる。ＣＰＵ１７には、移動距離計算ルーチンのほか
に、走行制御ルーチン（例えば、経路・操舵・速度・加
速度等）を内蔵しており、モータ制御器２０Ｒ、２０Ｌ
に指令を出す。
【０００７】移動距離計算ルーチンの詳細を以下に述べ
る。まず最初に、車輪のスリップ検出方式について述べ
る。加速度計１Ｒ、１Ｌの出力加速度αSR，αSLをもと
に、下記の式によりＸ方向の加速度αSXとＯ点周りの角
加速度ｄωS ／ｄｔを計算する。 αSX＝（ｌSR＊αSL＋ｌSL＊αSR）／（ｌSR＋ｌSL） （１） ｄωS ／ｄｔ＝−（αSL−αSR）／（ｌSR＋ｌSL） （２） ただし、ｌSR、ｌSLはＯ点から加速度センサ１Ｒ、１Ｌ
までの距離である。つぎに、ｘ方向の速度ＶSXおよびＯ
点周りの角速度ωS を計算する。一般的には、（１）式
（２）式を積分して求めるが、サンプル時間ｔが小さい
場合は、下記の式でよい。 ＶSX(m) ＝ＶXS(m-1) ＋αSX(m) ＊ｔ （３） ωS(m) ＝ωS(m-1)＋（ｄωS ／ｄｔ）(m) ＊ｔ （４） ここで，サフィツクスｍはｍ番目のサンプル時間を示
す。従って、加速度計１Ｒ、１Ｌの出力加速度を基準と
して求めた、左右の駆動輪４Ｌ，４Ｒのｍ番目のサンプ
ル時間における、左右車輪位置の速度ＶSWL(m)，Ｖ
SWR(m)は下記の式で表される。 ＶSWL(m)＝ＶSX(m) −１WL＊ωS(m) （５） ＶSWR(m)＝ＶSX(m) ＋１WR＊ωS(m) （６） ただし、１WL、１WRはＯ点から駆動輪４Ｌ，４Ｒまでの
距離である。一方、回転位置検出器６Ｌ，６Ｒの検出す
る回転速度ωWL(m),ωWR(m) を基準として求めた、ｍ番
目のサンプル時間における、左右の駆動輪位置の速度を
ＶWL (m) 、ＶWR(m) とする。左右の駆動輪のスリップＶ
SLIPXL，ＶSRIPXRは下記の式で表される。 ＶSLIPXL＝ＶSWL(m)−ＶWL(m) （７） ＶSRIPXR＝ＶSWR(m)−ＶWR(m) （８）
る。まず最初に、車輪のスリップ検出方式について述べ
る。加速度計１Ｒ、１Ｌの出力加速度αSR，αSLをもと
に、下記の式によりＸ方向の加速度αSXとＯ点周りの角
加速度ｄωS ／ｄｔを計算する。 αSX＝（ｌSR＊αSL＋ｌSL＊αSR）／（ｌSR＋ｌSL） （１） ｄωS ／ｄｔ＝−（αSL−αSR）／（ｌSR＋ｌSL） （２） ただし、ｌSR、ｌSLはＯ点から加速度センサ１Ｒ、１Ｌ
までの距離である。つぎに、ｘ方向の速度ＶSXおよびＯ
点周りの角速度ωS を計算する。一般的には、（１）式
（２）式を積分して求めるが、サンプル時間ｔが小さい
場合は、下記の式でよい。 ＶSX(m) ＝ＶXS(m-1) ＋αSX(m) ＊ｔ （３） ωS(m) ＝ωS(m-1)＋（ｄωS ／ｄｔ）(m) ＊ｔ （４） ここで，サフィツクスｍはｍ番目のサンプル時間を示
す。従って、加速度計１Ｒ、１Ｌの出力加速度を基準と
して求めた、左右の駆動輪４Ｌ，４Ｒのｍ番目のサンプ
ル時間における、左右車輪位置の速度ＶSWL(m)，Ｖ
SWR(m)は下記の式で表される。 ＶSWL(m)＝ＶSX(m) −１WL＊ωS(m) （５） ＶSWR(m)＝ＶSX(m) ＋１WR＊ωS(m) （６） ただし、１WL、１WRはＯ点から駆動輪４Ｌ，４Ｒまでの
距離である。一方、回転位置検出器６Ｌ，６Ｒの検出す
る回転速度ωWL(m),ωWR(m) を基準として求めた、ｍ番
目のサンプル時間における、左右の駆動輪位置の速度を
ＶWL (m) 、ＶWR(m) とする。左右の駆動輪のスリップＶ
SLIPXL，ＶSRIPXRは下記の式で表される。 ＶSLIPXL＝ＶSWL(m)−ＶWL(m) （７） ＶSRIPXR＝ＶSWR(m)−ＶWR(m) （８）
【０００８】つぎに、Ｏ点に設けた加速度計２で検出す
るｙ方向の加速度αOyを基準として、横滑り加速度α
SLIPy は下記の式で表される。 αSLIPy(m)＝αOy(m) −ＶSX(m) ＊ωS(m) （９） 一般的には、車輪の横滑り速度ＶSLIPy(m)はαSLIPy(m)
を積分して求めるが、サンプル時間ｔが小さい場合は、
下記の式でよい。 ＶSLIPy(m)＝ＶSLIPy(m-1)＋αSLIPy(m)＊ｔ （１０） したがって、左右車輪４Ｌ，４Ｒのスリップと横滑りに
よる合成速度は、下記の式で表される。 ＶSLIPL(m)＝ＳＱＲＴ（ＶSLIPXL 2 ＋ＶSLIPyL 2 ） （１１） ＶSLIPR(m)＝ＳＱＲＴ（ＶSLIPXR 2 ＋ＶSLIPyR 2 ） （１２） （１０）式、（１１）式のいずれかの値が所定の値を越
えたとき、左右の車輪のいずれかにスリップまたは横滑
りが生じたと判断する。
るｙ方向の加速度αOyを基準として、横滑り加速度α
SLIPy は下記の式で表される。 αSLIPy(m)＝αOy(m) −ＶSX(m) ＊ωS(m) （９） 一般的には、車輪の横滑り速度ＶSLIPy(m)はαSLIPy(m)
を積分して求めるが、サンプル時間ｔが小さい場合は、
下記の式でよい。 ＶSLIPy(m)＝ＶSLIPy(m-1)＋αSLIPy(m)＊ｔ （１０） したがって、左右車輪４Ｌ，４Ｒのスリップと横滑りに
よる合成速度は、下記の式で表される。 ＶSLIPL(m)＝ＳＱＲＴ（ＶSLIPXL 2 ＋ＶSLIPyL 2 ） （１１） ＶSLIPR(m)＝ＳＱＲＴ（ＶSLIPXR 2 ＋ＶSLIPyR 2 ） （１２） （１０）式、（１１）式のいずれかの値が所定の値を越
えたとき、左右の車輪のいずれかにスリップまたは横滑
りが生じたと判断する。
【０００９】（１０）式、（１１）式の値で走行状況を
判断し、おのおののサンプリング時間において、自走台
車の距離計算式を使いわける。これは、距離計算の過程
の２階積分が含まれ計算誤差もある程度あると思われる
ので、加速度計１Ｒ、１Ｌおよび２の値のみを用いて自
走台車の移動距離を計算すると、積分誤差が大きくなる
からである。よって、車輪のスリップや横滑りが小さい
と思われるときは積分が一回少ない回転位置検出器６
Ｌ，６Ｒの値を使用する。（イ）スリップや横滑りが無
視できないと判断したとき。前記（３）、（４）式で求
めたＶSX(m) とωS(m)を積分して求めたθS(m)および
（１０）式で求めたＶSLIPy(m)から、ｍ番目のサンプル
時間における、自走台車の副座標（ｘ(m) ，ｙ(m) ，θ
(m) ）は下記の式で求める。 ｘ(m) ＝ｘ(m-1) ＋ＶSx(m) ＊ｔ＊ｃｏｓ（（θS(m)＋θ(m-1) ）／２） −ＶSLIPy(m)＊ｔ＊ｓｉｎ（（θS(m)＋θ(m-1) ）／２） （１３） ｙ(m) ＝ｙ(m-1) ＋ＶS(m)＊ｔ＊ｓｉｎ（（θS(m)＋θ(m-1) ）／２） ＋ＶSLIPY(m)＊ｔ＊ｃｏｓ（（θS(m)＋θ(m-1) ）／２） （１４） θ(m) ＝θ(m-1) ＋ωSm＊ｔ （１５） （ロ）スリップや横滑りが無視できると判断したとき。
回転位置検出器６Ｌ，６Ｒの検出する回転速度ωWL(m),
ωWR(m) を基準として求めた、ｍ番目のサンプル時間に
おける、左右車輪位置の速度をＶWL(m) ，ＶWRから、自
走台車のｘ方向の速度ＶWx、ωW は下記の式で表され
る。 ＶWX(m) ＝（１WR＊ＶWL(m) ＋１WL＊ＶWR(m) ）／（ｌWL＋ｌWR） （１６） ωW(m)＝−（ＶWL(m) −ＶWR(m) ）／（ｌWL＋ｌWR） （１７） ただし、１WR、１WLは、左車輪、右車輪のＯ点からの距
離である。 ｘ(m) ＝ｘ(m-1) ＋ＶWX(m) ＊ｔ＊ｃｏｓ（（θW(m)＋θ(m-1) ）／２） （１８） ｙ(m) ＝ｙ(m-1) ＋ＶWX(m) ＊ｔ＊ｓｉｎ（（θW(m)＋θ(m-1) ）／２） （１９） θ(m) ＝θ(m-1) ＋ωW(m)＊ｔ （２０） さて、自走台車の移動平面における絶対座標（Ｘ，Ｙ，
Θ）は前記（イ）の場合は（１３），（１４）および
（１５）式を用い、（ロ）の場合は（１８），（１９）
および（２０）式を用い、下記の式で求める。 Ｘ(m) ＝ΣＸ(m-1) ＋ｘ(m) （２１） Ｙ(m) ＝ΣＹ(m-1) ＋ｙ(m) （２２） Θ(m) ＝ΣΘ(m-1) ＋θ(m) （２３）
判断し、おのおののサンプリング時間において、自走台
車の距離計算式を使いわける。これは、距離計算の過程
の２階積分が含まれ計算誤差もある程度あると思われる
ので、加速度計１Ｒ、１Ｌおよび２の値のみを用いて自
走台車の移動距離を計算すると、積分誤差が大きくなる
からである。よって、車輪のスリップや横滑りが小さい
と思われるときは積分が一回少ない回転位置検出器６
Ｌ，６Ｒの値を使用する。（イ）スリップや横滑りが無
視できないと判断したとき。前記（３）、（４）式で求
めたＶSX(m) とωS(m)を積分して求めたθS(m)および
（１０）式で求めたＶSLIPy(m)から、ｍ番目のサンプル
時間における、自走台車の副座標（ｘ(m) ，ｙ(m) ，θ
(m) ）は下記の式で求める。 ｘ(m) ＝ｘ(m-1) ＋ＶSx(m) ＊ｔ＊ｃｏｓ（（θS(m)＋θ(m-1) ）／２） −ＶSLIPy(m)＊ｔ＊ｓｉｎ（（θS(m)＋θ(m-1) ）／２） （１３） ｙ(m) ＝ｙ(m-1) ＋ＶS(m)＊ｔ＊ｓｉｎ（（θS(m)＋θ(m-1) ）／２） ＋ＶSLIPY(m)＊ｔ＊ｃｏｓ（（θS(m)＋θ(m-1) ）／２） （１４） θ(m) ＝θ(m-1) ＋ωSm＊ｔ （１５） （ロ）スリップや横滑りが無視できると判断したとき。
回転位置検出器６Ｌ，６Ｒの検出する回転速度ωWL(m),
ωWR(m) を基準として求めた、ｍ番目のサンプル時間に
おける、左右車輪位置の速度をＶWL(m) ，ＶWRから、自
走台車のｘ方向の速度ＶWx、ωW は下記の式で表され
る。 ＶWX(m) ＝（１WR＊ＶWL(m) ＋１WL＊ＶWR(m) ）／（ｌWL＋ｌWR） （１６） ωW(m)＝−（ＶWL(m) −ＶWR(m) ）／（ｌWL＋ｌWR） （１７） ただし、１WR、１WLは、左車輪、右車輪のＯ点からの距
離である。 ｘ(m) ＝ｘ(m-1) ＋ＶWX(m) ＊ｔ＊ｃｏｓ（（θW(m)＋θ(m-1) ）／２） （１８） ｙ(m) ＝ｙ(m-1) ＋ＶWX(m) ＊ｔ＊ｓｉｎ（（θW(m)＋θ(m-1) ）／２） （１９） θ(m) ＝θ(m-1) ＋ωW(m)＊ｔ （２０） さて、自走台車の移動平面における絶対座標（Ｘ，Ｙ，
Θ）は前記（イ）の場合は（１３），（１４）および
（１５）式を用い、（ロ）の場合は（１８），（１９）
および（２０）式を用い、下記の式で求める。 Ｘ(m) ＝ΣＸ(m-1) ＋ｘ(m) （２１） Ｙ(m) ＝ΣＹ(m-1) ＋ｙ(m) （２２） Θ(m) ＝ΣΘ(m-1) ＋θ(m) （２３）
【０００９】各検出器が絶対的に同一の精度を維持でき
るときは、以上で求めた値を用いてよいが、通常、加速
度計１Ｒ、１Ｌ、２および回転位置検出器６Ｌ，６Ｒ等
の検出器はバラツキや温度ドリフト等による誤差の影響
を受けるので、オンラインで補正を行うのが好ましい。
オンラインで各検出器のゲインとオフセットの補正を行
う方法として、推定による同定方法を用いる。新しい推
定は次の一般式で表される。 新しい推定＝古い推定＋ゲインベクトル＊予測誤差 （２４） （２４）式のゲインベクトルと予測誤差をオンラインで
同定する方法の一例が、'ADAPTIVE FILTERING PREDICTI
ON AND CONTROL'(PRENTICE-HALL,INC,発行 3.3 節）に
記載されている。一例として、（２４）式を本発明の加
速度計１Ｌの検出値αSLに適用すると、下記の式とな
る。 αSL(m+1) ＝αSL(m) ＋ｋ(m) ＊Ｅ(m+1) （２５） ただし、ｋ(m) はサンプル時間ｍにおける加速度計１Ｌ
のゲイン、Ｅ(m+1) は予測誤差（オフセット）である。
（２５）式のｋ(m) およびＥ(m+1) は下記の式で表され
る。 ｋ(m) ＝１／Φ(m) T ＊Φ(m) （２６） Ｅ(m+1) ＝ｙ(m+1) −Φ(m) T ＊αSLest.(m) （２７） ただし、αSLest.(m) はサンプル時間ｍにおける推定
値、Φ(m) T は推定値と観測値の差の転置ベクトルを示
し下記の式で表される。 Φ(m) T ＝〔−ｙ(m) ，αSL(m) 〕 （２８） ただし、ｙ(m) は、サンプル時間ｍにおける、線形また
は非線形モデルによる推定値である。以下に、加速度計
の基準値をえるための初期補正方法について述べる。ま
ず、自走台車を横滑りやスリップが起きないような理想
平面上を走行させる。この状態では左右の加速度計と回
転位置検出器の出力に外乱が考えられ、左右の加速度計
と回転位置検出器のゲインとオフセットを補正する。次
に、ゲンイを固定と考えオフセットを（２７）式で補正
する。これを各サンプル時間毎に行なう。このような初
期補正を十分行なった後、自走台車の距離測定を始め
る。距離測定中も加速度計の補正を行なう。なお、距離
測定中の加速度計の補正は、（１１）式（１２）式の値
が所定の値の範囲内で車輪のスリップが無視できる程小
さいと判断される時、車輪の回転位置検出器のデータよ
り行なうようにする。
るときは、以上で求めた値を用いてよいが、通常、加速
度計１Ｒ、１Ｌ、２および回転位置検出器６Ｌ，６Ｒ等
の検出器はバラツキや温度ドリフト等による誤差の影響
を受けるので、オンラインで補正を行うのが好ましい。
オンラインで各検出器のゲインとオフセットの補正を行
う方法として、推定による同定方法を用いる。新しい推
定は次の一般式で表される。 新しい推定＝古い推定＋ゲインベクトル＊予測誤差 （２４） （２４）式のゲインベクトルと予測誤差をオンラインで
同定する方法の一例が、'ADAPTIVE FILTERING PREDICTI
ON AND CONTROL'(PRENTICE-HALL,INC,発行 3.3 節）に
記載されている。一例として、（２４）式を本発明の加
速度計１Ｌの検出値αSLに適用すると、下記の式とな
る。 αSL(m+1) ＝αSL(m) ＋ｋ(m) ＊Ｅ(m+1) （２５） ただし、ｋ(m) はサンプル時間ｍにおける加速度計１Ｌ
のゲイン、Ｅ(m+1) は予測誤差（オフセット）である。
（２５）式のｋ(m) およびＥ(m+1) は下記の式で表され
る。 ｋ(m) ＝１／Φ(m) T ＊Φ(m) （２６） Ｅ(m+1) ＝ｙ(m+1) −Φ(m) T ＊αSLest.(m) （２７） ただし、αSLest.(m) はサンプル時間ｍにおける推定
値、Φ(m) T は推定値と観測値の差の転置ベクトルを示
し下記の式で表される。 Φ(m) T ＝〔−ｙ(m) ，αSL(m) 〕 （２８） ただし、ｙ(m) は、サンプル時間ｍにおける、線形また
は非線形モデルによる推定値である。以下に、加速度計
の基準値をえるための初期補正方法について述べる。ま
ず、自走台車を横滑りやスリップが起きないような理想
平面上を走行させる。この状態では左右の加速度計と回
転位置検出器の出力に外乱が考えられ、左右の加速度計
と回転位置検出器のゲインとオフセットを補正する。次
に、ゲンイを固定と考えオフセットを（２７）式で補正
する。これを各サンプル時間毎に行なう。このような初
期補正を十分行なった後、自走台車の距離測定を始め
る。距離測定中も加速度計の補正を行なう。なお、距離
測定中の加速度計の補正は、（１１）式（１２）式の値
が所定の値の範囲内で車輪のスリップが無視できる程小
さいと判断される時、車輪の回転位置検出器のデータよ
り行なうようにする。
【００１０】
【発明の効果】以上のように、本発明によれば、加速度
計と駆動系の回転位置検出装置だけの組み合わせの比較
的簡単なシステム構成で、自走台車の移動距離と現在位
置の座標を検出でき、加速度計を用いているので、ジャ
イロスコープによるものよりも安価であるという効果が
ある。
計と駆動系の回転位置検出装置だけの組み合わせの比較
的簡単なシステム構成で、自走台車の移動距離と現在位
置の座標を検出でき、加速度計を用いているので、ジャ
イロスコープによるものよりも安価であるという効果が
ある。
【図面の簡単な説明】
【図１】本発明の実施例による自走台車の構成を示す平
面図。
面図。
【図２】本発明の実施例による制御装置のブロック図。
【図３】本発明の実施例による加速度検出器等の配置を
示す平面図。
示す平面図。
１Ｒ、１Ｌ、２ 加速度計 ４Ｒ、４Ｌ 駆動輪 ４１ 駆動軸 ５Ｒ、５Ｌ 駆動装置 ６Ｒ，６Ｌ 回転位置検出器 ８１ 車軸 ９ 制御装置 １０Ｒ、１０Ｌ、１１ 増幅回路 １３ マルチプレクサ １４ Ａ／Ｄコンバータ １５Ｒ、１５Ｌ カウンタ １６Ｒ，１６Ｌ ラッチ １７ ＣＰＵ １８ リアルタイムクロック １９ メモリ ２０Ｒ、２０Ｌ モータ制御器
───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.7，ＤＢ名) G05D 1/02 G01C 21/16
Claims (5)
- 【請求項１】 自由平面を移動する自走台車の移動距離
を車輪に設けた回転位置検出器により検出し、自走台車
の現在位置を検出方法において、自走台車の左右の駆動
軸上に、取付けられた進行方向の加速度を検出する２つ
の加速度計と、左右の駆動軸と車軸の交点上に設けた進
行方向と直交方向の加速度を検出する加速度計を設け、
各加速度計の出力をもとに、自走台車の横滑りと駆動輪
のスリップを検出し、おのおのの状態に対応した演算ル
ーチンにより、微少サンプリング時間内の移動距離を演
算し、それまでの移動距離に逐次加算し、自由平面内の
絶対座標系に対する自走台車の現在位置の絶対座標を検
出することを特徴とする自走台車の絶対位置検出方法。 - 【請求項２】 前記自走台車の横滑りの検出を、駆動軸
と車軸の交点に設けた加速度計の検出する車軸方向の加
速度を基準に演算する請求項１記載の自走台車の絶対位
置検出方法。 - 【請求項３】 前記駆動輪のスリップの検出を、駆動軸
上に設けた左右の加速度計の検出する加速度と回転位置
検出器の検出する回転速度の差から演算する請求項１記
載の自走台車の絶対位置検出方法。 - 【請求項４】 前記微少サンプリング時間内の移動距離
の演算を、自走台車の横滑りとスリップの状況に対応し
て、演算式を選択する請求項２または３記載の自走台車
の絶対位置検出方法。 - 【請求項５】 前記各加速度計と回転位置検出器のゲイ
ンとオフセットをオンラインで補正する請求項１ないし
４いずれか１項に記載の自走台車の絶対位置検出方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP4273740A JP3033363B2 (ja) | 1992-09-16 | 1992-09-16 | 自走台車の絶対位置検出方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP4273740A JP3033363B2 (ja) | 1992-09-16 | 1992-09-16 | 自走台車の絶対位置検出方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0695733A JPH0695733A (ja) | 1994-04-08 |
| JP3033363B2 true JP3033363B2 (ja) | 2000-04-17 |
Family
ID=17531913
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP4273740A Expired - Fee Related JP3033363B2 (ja) | 1992-09-16 | 1992-09-16 | 自走台車の絶対位置検出方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3033363B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| NL1031605C2 (nl) * | 2006-04-18 | 2007-10-19 | Maasland Nv | Onbemand autonoom voertuig voor het verplaatsen van voeder. |
| KR20110021191A (ko) | 2009-08-25 | 2011-03-04 | 삼성전자주식회사 | 로봇의 슬립 감지 장치 및 방법 |
| JP5504090B2 (ja) * | 2010-07-30 | 2014-05-28 | Ｄｍｇ森精機株式会社 | 移設検知方法及び移設検知ユニット |
| JP5962659B2 (ja) | 2011-09-13 | 2016-08-03 | 株式会社安川電機 | 自走ロボットおよび自走台車 |
| CN102819265A (zh) * | 2012-08-29 | 2012-12-12 | 上海富洋自动化工程设备有限公司 | 连续绝对地址系统的运载式机器人自动车 |
| JP6493143B2 (ja) * | 2015-10-15 | 2019-04-03 | 株式会社デンソー | 表示制御装置及び表示制御プログラム |
-
1992
- 1992-09-16 JP JP4273740A patent/JP3033363B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0695733A (ja) | 1994-04-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Ryu et al. | Estimation of vehicle roll and road bank angle | |
| JP6151428B2 (ja) | 慣性センサオフセット補正方法およびシステム | |
| US6522956B2 (en) | Method and device for estimating a transverse acceleration at an axle of a semitrailer or a trailer of a vehicle combination | |
| US20190368878A1 (en) | Method for determining an orientation of a vehicle | |
| CN106568449B (zh) | 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法 | |
| US11079237B2 (en) | Method for determining a relative position of a motor vehicle, position determination system for a motor vehicle and motor vehicle | |
| JPH06273187A (ja) | 車体重心スリップ角計測装置 | |
| US20020022924A1 (en) | Propagation of position with multiaxis accelerometer | |
| JP3033363B2 (ja) | 自走台車の絶対位置検出方法 | |
| JPH04113218A (ja) | 相対方位検出方式 | |
| JP2000074727A (ja) | 車両の質量推定装置 | |
| JP6594546B2 (ja) | 角度計測装置 | |
| Zunaidi et al. | Positioning system for 4-wheel mobile robot: encoder, gyro and accelerometer data fusion with error model method | |
| JP2878498B2 (ja) | 角加速度検出装置 | |
| JP2006189450A (ja) | ナビゲーション装置 | |
| JP3557508B2 (ja) | 方向検出装置 | |
| JP6632727B2 (ja) | 角度計測装置 | |
| JPH11115720A (ja) | 路面摩擦係数の推定装置 | |
| JP2020080743A (ja) | 車両の姿勢推定装置 | |
| JPH08268257A (ja) | 実車速推定装置 | |
| JPH08143273A (ja) | クレーンの振れ角検出方法およびクレーン用振れ角センサ | |
| JP6955315B2 (ja) | 自動運転における慣性航法 | |
| US20240043018A1 (en) | Method and electronic control system for ascertaining a distance travelled by a vehicle | |
| JP3147639B2 (ja) | 車両挙動検出装置 | |
| JP2002108447A (ja) | ジャイロ誘導式無人搬送装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20000118 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090218 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100218 Year of fee payment: 10 |
|
| LAPS | Cancellation because of no payment of annual fees |