JP5336955B2 - 倒立振子型移動体及びその制御方法 - Google Patents
倒立振子型移動体及びその制御方法 Download PDFInfo
- Publication number
- JP5336955B2 JP5336955B2 JP2009160133A JP2009160133A JP5336955B2 JP 5336955 B2 JP5336955 B2 JP 5336955B2 JP 2009160133 A JP2009160133 A JP 2009160133A JP 2009160133 A JP2009160133 A JP 2009160133A JP 5336955 B2 JP5336955 B2 JP 5336955B2
- Authority
- JP
- Japan
- Prior art keywords
- target
- moving body
- calculated
- command value
- control command
- 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 22
- 238000001514 detection method Methods 0.000 claims description 18
- 230000001133 acceleration Effects 0.000 description 32
- 230000004044 response Effects 0.000 description 21
- 238000012545 processing Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 10
- 238000012937 correction Methods 0.000 description 8
- 238000005259 measurement Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000005484 gravity Effects 0.000 description 4
- 230000015572 biosynthetic process Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000003786 synthesis reaction Methods 0.000 description 2
- 230000004913 activation Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000001678 irradiating effect Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Electric Propulsion And Braking For Vehicles (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Motorcycle And Bicycle Frame (AREA)
Description
本明細書中、「移動体の進行方向」とは、移動体が移動する平面において、その平面に投影した車輪の回転軸に対して直交する方向をいう。「移動体の旋回方向」とは、移動体の鉛直軸回りの方向をいう。また、「目標軌道」とは、移動体の経時的な目標位置(すなわち、移動体の目標位置(目標経路)を時間の関数として表したもの)をいう。
この移動体では、第1制御指令値算出手段が進行方向の位置を制御するための第1制御指令値を算出し、第2制御指令値算出手段が旋回方向の角度を制御するための第2制御指令値を算出し、これらの制御指令値に基づいて車輪が駆動される。ここで、第2制御指令値算出手段は、目標軌道と移動体の進行方向の位置に関する情報を用いて第2制御指令値を算出する。このため、移動体の進行方向の応答速度の遅れによって移動体が目標軌道からずれた場合は、そのずれを修正して旋回方向の第2制御指令値を算出することができる。これによって、移動体は目標経路上を精度よく走行することができる。
ここで、「移動体の進行方向の位置に関する情報」とは、移動体の進行方向の位置(移動距離)を特定することができる情報であり、例えば、移動体の進行方向の加速度、速度等の物理量が相当する。
この構成によると、所定周期毎に制御指令値が算出され、目標経路からのずれが所定周期毎に修正される。このため、移動体は目標経路上を精度よく走行することができる。
この構成によると、測定された進行方向の位置に関する情報を利用して旋回方向の第2制御指令値が算出される。実測された位置に関する情報を利用することで、目標経路に沿った(目標経路に近い又は目標経路の形状に沿った)動きが実現でき、移動体は目標経路上を精度よく移動することができる。
この構成によると、第1制御指令値から推定される移動体の進行方向の位置に関する情報を利用して第2制御指令値が算出される。第1制御指令値から推定される進行方向の位置に関する情報を利用することで、移動体の目標経路からのずれを見込んで旋回方向の制御指令値を算出することができる。この構成によっても、移動体は目標経路上を精度よく移動することができる。
この構成によると、センサの検出結果から移動体の位置及び旋回方向の角度(すなわち、向き(姿勢))が分かり、これらを用いて目標軌道からの偏差が特定され、この特定された偏差を用いて移動体の進行方向及び旋回方向の各目標値が算出される。そして、これらの目標値に基づいて、第1制御指令値と第2制御指令値を算出する。その際、第1制御指令値を利用して第2制御指令値を算出するため、進行方向の遅れを考慮して旋回方向の第2制御指令値が算出される。この構成によっても、移動体は目標経路上を精度よく移動することができる。
この制御方法では、(1)移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第1制御指令値を算出する工程と、(2)目標軌道と実測又は推定される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第2制御指令値を算出する工程と、(3)第1制御指令値と第2制御指令値に基づいて車輪を駆動する工程と、を有している。そして、上記各工程を繰返し実行することで移動体を制御する。
この制御方法によっても、移動体の進行方向の位置に関する情報を利用して移動体の旋回方向の制御指令値を算出するため、目標経路との誤差を小さくすることができる。
(形態1)移動体は、同一の車軸上に配された2つの車輪と、車輪によって支持される車体によって構成される。
(形態2)車体は、各車輪を駆動するモータと、車体の傾斜角速度を検出するジャイロセンサと、各モータの回転角速度を検出するエンコーダと、これらのセンサの検出結果に応じてモータを駆動する制御装置を備えている。
(形態3)制御装置は、目標軌道(x(t),y(t))を用いて、移動体の進行方向の位置を制御するための制御指令値を算出する。
(形態4)制御装置は、車体の進行方向の位置と速度と加速度の少なくとも一つを用いて、目標軌道(x(t),y(t))を修正する。そして、修正した目標軌道(x’,y’)を用いて、移動体の旋回方向の角度を制御する制御指令値を算出する。
(形態5)制御装置は、エンコーダにより検出されたモータの回転角速度を用いて、直前の制御周期から移動体が移動した距離を算出する。次に、算出された移動距離と目標軌道(x(t),y(t))を用いて、その移動距離を実現すべきだった時刻t’を算出する。そして、時刻t’における目標軌道(x(t’),y(t’))を用いて、移動体の旋回方向の角度を制御するための制御指令値を算出する。
(形態6)制御装置は、今回の制御周期で算出された「移動体の進行方向の位置を制御するための制御指令値」を用いて、今回の制御周期において移動体が移動するであろう距離を算出する。次に、算出された移動距離と目標軌道(x(t),y(t))を用いて、その移動距離を実現すべき時刻t’を算出する。そして、時刻t’における目標軌道(x(t’),y(t’))を用いて、移動体の旋回方向の角度を制御するための制御指令値を算出する。
(形態7)移動体は、移動体の位置と姿勢(x,y,φ)を測定するセンサを備える。この種のセンサとしては、GPSセンサ、レーザレンジセンサ、モーションキャプチャ等を用いることができる。
(形態8)制御装置は、測定した移動体の位置と姿勢(x,y,φ)と、目標軌道(x(t),y(t))とから、(1)移動体の進行方向の位置偏差と(2)移動体の進行方向に直交する方向の位置偏差と(3)移動体の姿勢偏差を算出する。次に、算出されたこれらの偏差を用いて、移動体の進行方向の位置に関する目標値(例えば、進行方向の速度)と、移動体の旋回方向の角度に関する目標値(例えば、旋回方向の角速度)を算出する。そして、移動体の進行方向の位置を制御するための制御指令値については、算出された進行方向の位置に関する目標値を用いて算出する。移動体の旋回方向の角度を制御するための制御指令値については、算出された進行方向の位置に関する目標値と、算出された移動体の旋回方向の角度に関する目標値と、算出された移動体の進行方向の位置を制御するための制御指令値を用いて算出する。
(形態9)形態8において、移動体の旋回方向の角度を制御するための制御指令値は、「移動体の旋回方向の角度に関する目標値」に、「移動体の進行方向の位置に関する目標値」と「移動体の進行方向の位置を制御するための制御指令値」の比(すなわち、「移動体の進行方向の位置を制御するための制御指令値」/「移動体の進行方向の位置に関する目標値」)を掛けて算出された「移動体の旋回方向の角度に関する新しい目標値」に基づいて算出される。
モータ22にはエンコーダ22aが設けられている。エンコーダ22aは、制御装置30と電気的に接続されている。エンコーダ22aは、モータ22の回転角速度(すなわち、車輪12の回転角速度)を検出する。エンコーダ22aが検出した車輪12の回転角速度は、制御装置30に入力される。また、モータ24にもエンコーダ24aが設けられている。エンコーダ24aは、車輪14の回転角速度を検出する。エンコーダ24aが検出した車輪14の回転角速度も、制御装置30に入力される。
図3は制御装置30の機能を示すブロック図である。図3に示すように、制御装置30は、目標軌道入力部38、進行方向目標値算出部40、倒立制御コントローラ32、旋回方向目標値算出部50、旋回制御コントローラ34、指令値合成部36によって構成されている。なお、図3においては、図の見易さを考慮してモータ及びエンコーダを1つだけ図示したが、実際には2つのモータ22、24にそれぞれ独立して制御指令値が出力される。
目標速度算出部42は、目標軌道入力部38から入力された目標軌道(x(t),y(t))から移動体10の目標速度を算出する。具体的には、目標軌道(x(t),y(t))を微分することによって、目標速度(x’(t),y’(t))を算出する。算出された目標速度(x’(t),y’(t))は目標接線速度算出部44に入力される。
目標接線速度算出部44は、入力された目標速度(x’(t),y’(t))から目標接線速度を算出する。目標接線速度とは、ベクトルで与えられた目標速度(x’(t),y’(t))の絶対値(スカラー)である。したがって、目標接線速度s’(t)は、(x’(t)2+y’(t)2)1/2によって算出される。算出された目標接線速度s’(t)は、倒立制御コントローラ32と目標経路長算出部46に入力される。
目標経路長算出部46は、入力された目標接線速度(x’(t)2+y’(t)2)1/2を積分することによって移動体10の目標経路長s(t)を算出する。算出された目標経路長s(t)は、倒立制御コントローラ32と目標経路長算出部46に入力される。
上述したことから明らかなように、移動体10の進行方向の目標値(目標接線速度s’、目標経路長s)は目標軌道(x(t),y(t))のみに基づいて算出される。
すなわち、計測接線速度算出部59は、エンコーダ22a,24aで検出された車輪12,14の回転角速度から、移動体10の接線速度sr’を算出する。すなわち、車輪12,14の回転角速度と、車輪12,14の幾何学的関係(車輪12,14の直径,車輪12,14間の距離)とから、移動体10の接線速度sr’を算出する。算出された接線速度sr’は、目標角度算出部56と計測経路長算出部58に入力される。
計測経路長算出部58は、計測接線速度算出部59で算出された接線速度sr’を積分することによって、移動体10の経路長srを算出する。算出された経路長srは、修正時刻算出部57に入力される。
修正時刻算出部57は、算出された経路長srと目標軌道(x(t),y(t))とから、その算出された経路長srを実現すべきであった時刻t’を算出する。すなわち、目標軌道(x(t),y(t))から時間tと目標経路長s(t)の関係が得られる。次いで、時間tと目標経路長s(t)の関係に基づいて、計測経路長算出部58で算出された経路長srとなるときの時刻t’を算出する。算出された時刻t’は新目標位置算出部51に入力される。
新目標位置算出部51は、修正時刻算出部57で算出した時刻t’を用いて、新しい目標位置(x(t’),y(t’))を算出する。したがって、進行方向の速度応答性の遅れから、現実の経路長srが目標軌道から得られる目標経路長s(t)とならない場合には、経路長srを実現すべきであった時刻t’が求められ、その時刻t’における目標位置が新しい目標位置(x(t’),y(t’))として採用される。
すなわち、新目標速度算出部52は、新しく算出された新目標位置(x(t’),y(t’))を用いて、移動体10の新目標速度を算出する。具体的には、新目標位置(x(t’),y(t’))を微分することによって、新目標速度(x’(t’),y’(t’))を算出する。算出された新目標速度(x’(t’),y’(t’))は目標姿勢算出部53と新目標加速度算出部44と新目標曲率算出部55に入力される。
目標姿勢算出部53は、算出された新目標速度(x’(t’),y’(t’))を用いて、移動体10の目標姿勢(すなわち、旋回方向の目標角度φ)を算出する。すなわち、tanφ=[y’(t’)]/[x’(t’)]の関係が成立するため、この式に算出された新目標速度(x’(t’),y’(t’))を代入することで、目標角度φが算出される。算出された目標角度φは、旋回制御コントローラ34に入力される。
新目標加速度算出部54は、算出された新目標速度(x’(t’),y’(t’))を用いて、移動体10の新目標加速度を算出する。具体的には、新目標速度(x’(t’),y’(t’))を微分することによって、新目標加速度(x’’(t’),y’’(t’))を算出する。算出された新目標加速度(x’’(t’),y’’(t’))は、新目標曲率算出部55に入力される。
新目標曲率算出部55は、新目標速度(x’(t’),y’(t’))と、新目標加速度(x’’(t’),y’’(t’))を用いて、移動体10の新目標曲率1/Rを算出する。具体的には、新目標速度(x’(t’),y’(t’))から移動体の目標接線速度v’を算出し、新目標加速度(x’’(t’),y’’(t’))から移動体の法線方向の目標加速度anを算出する。ここで、目標接線速度v’と法線方向の加速度anと曲率半径Rの間には、an=v’2/Rの関係が成立する。このため、目標接線速度v’と法線方向の加速度anが分かると、曲率半径Rを算出することができる(すなわち、曲率1/Rを算出することができる)。算出された曲率1/Rは目標角度算出部56に入力される。
目標角速度算出部56は、計測接線速度算出部59で算出された現実の接線速度sr’と、算出された曲率1/Rを用いて、移動体10の旋回方向の目標角速度φ’を算出する。具体的には、接線速度sr’に曲率(1/R)を掛けることで目標角速度φ’を算出する。算出された目標角速度φ’は旋回制御コントローラ34に入力される。
図6に示すように、制御装置30は、まず、制御装置30が起動されてからの時間t(すなわち、移動体10の制御開始時からの経過時間)を読み込む(S2)。時間tは、例えば、制御装置30にタイマを装備し、このタイマによって制御開始時からの経過時間を計測することで得られる。
次に、各モータ22,24のエンコーダ22a,24aの値(すなわち、車輪12,14の回転角速度)を読込む(S4)。
次に、ステップS4で読込んだエンコーダ22a,24aの値から、移動体10の現在の接線速度sr’と、経路長srと、旋回方向の角度(姿勢)φrと、旋回方向の角速度φr’を算出する(S6)。すなわち、エンコーダ22a,24aの値から車輪12,14の回転速度を算出し、この算出された回転速度と車輪12,14の幾何学的関係から移動体10のx方向、y方向、φ方向の速度を算出する(例えば、特許文献1に開示されたヤコビ行列を利用して算出することができる)。次いで、算出された各速度成分から、移動体10の現在の接線速度sr’と、経路長srと、旋回方向の角度(姿勢)φrと、旋回方向の角速度φr’を算出する。
ステップS8では、ジャイロセンサ28で検出された傾斜角速度θ’を読込む。
ステップS10では、ステップS2で読み込んだ時間tと、目標軌道入力部38に記憶されている目標軌道データ(x(t),y(t))とから、目標接線速度s’(t)と目標経路長s(t)を算出する(進行方向目標値算出部40の説明参照)。
次に、ステップS6で算出された現在の接線速度sr’及び経路長srと、ステップS8で読み込まれた傾斜角速度θ’と、ステップS10で算出された目標接線速度s’(t)と目標経路長s(t)とを用いて、移動体10の進行方向に関するモータ22,24のトルク指令値をそれぞれ算出する(S12)。
次に、ステップS6で算出した現在の経路長srを用いて、その経路長srを実現すべきであった時刻t’を求め(S14)、その求めた修正時刻t’から移動体10の旋回方向の目標角度(目標姿勢)φと目標角速度φ’を算出する(s16)。(旋回方向目標値算出部50の説明を参照)。
次に、ステップS6で算出された現在の旋回方向の角度(姿勢)φr及び角速度φr’と、ステップS14で算出された旋回方向の目標角度(目標姿勢)φ及び目標角速度φ’とを用いて、移動体10の旋回方向に関するモータ22,24のトルク指令値をそれぞれ算出する(S18)。
ステップS20では、ステップS12で算出されたトルク指令値と、ステップS18で算出されたトルク指令値を加算し、これらの値をモータ22,24(詳細には、モータ22,24を駆動するモータ駆動回路)に出力する。これによって、各車輪12,14が駆動されることとなる。ステップS20が終わるとステップ2に戻り、次の制御タイミングにおける処理が開始される。
なお、ステップS2〜ステップS20までの処理は、所定の時間間隔(例えば、10ms)で行われる。これによって、移動体10は、制御周期毎に、目標軌道からのずれが補正されることとなる。
次に、移動体は、目標位置P2から目標位置P3に移動するための制御指令値を算出し、この制御指令値に基づいて車輪12,14を駆動する。この際も、進行方向については速度応答性の遅れから、移動体は目標位置P2から目標位置P3に到達するために必要な距離(経路長)を移動することはできないものとする。一方、旋回方向については速度応答性の遅れがなく、目標軌道で設定された通りに制御される。そのため、移動体は、時刻t1+2Δtにおいて目標位置P3からさらにずれた位置P3’’に移動する。
以下、同様に目標位置P3から目標位置P4に移動するための制御指令値を算出し、この制御指令値に基づいて車輪12,14を駆動する。この場合も、進行方向の速度応答性の遅れから、移動体は目標軌道からさらにずれた位置P4’’に移動する。
このように、従来技術の移動体では、進行方向の速度応答性の遅れから移動体が目標軌道からずれると、そのずれが修正されることなく制御指令値が算出される。このため、目標軌道からのずれが大きくなっていく。
次に、移動体10は、進行方向については、目標位置P2(t1+Δt)から目標位置P3(t1+2Δt)に移動するための制御指令値を算出する。一方、旋回方向については、点P1から点P2’間の距離に対応した目標軌道上の点P2nを基準として制御目標値を算出する。ここで、目標軌道どおりに移動体が移動した場合に、目標軌道上の点P2nを移動体10が時刻t’で通過するものとする。旋回方向の制御目標値は、点P(t’)(時刻t’における目標軌道上の点)から点P(t’+Δt)(時刻t’+Δt)における目標軌道上の点)に移動するための制御指令値を算出する。次に、これらの制御目標値に基づいて車輪12,14が駆動される。このため、移動体は点P2’から点P3’に移動する。これによって、従来技術と比較して、目標軌道との誤差が小さくなる。
以下、同様に、進行方向については目標位置P3(t1+2Δt)から目標位置P4(t1+3Δt)に移動するための制御指令値を算出する。旋回方向については、移動体10の現在の位置P3’を基準として制御指令値が算出される。このため、移動体10は点P3’から点P4’に移動することとなる。
具体的には、旋回方向制御指令値算出部70は、まず、目標接線速度算出部66で算出された新目標接線速度を用いて、移動体の推定経路長を算出する(新経路長算出部76)。次に、移動体の推定される経路長から、その経路長を実現すべき時刻t’を算出する(修正時刻算出部74)。次に、算出した時刻t’と目標軌道(x(t),y(t))から新しい目標位置(x(t’),y(t’))を算出する(新目標位置算出部72)。新しい目標位置(x(t’),y(t’))が算出されると、その目標位置(x(t’),y(t’))から新目標速度を算出する(新目標速度算出部78)。次いで、その算出された新目標速度から新目標加速度を算出し(新目標加速度算出部80)、その新目標加速度と新目標速度から曲率1/Rを算出する(目標曲率算出部82)。最後に、新目標速度算出部78で算出された新目標速度と、目標曲率算出部82で算出された曲率1/Rを掛けて、目標角速度φ’を算出する(目標角速度算出部84)。この算出された目標角速度φ’は車輪角速度制御器68に入力される。
図9に示すように、制御装置60は、まず、制御装置60が起動されてからの時間t(制御開始時からの経過時間)を読み込む(S22)。次に、各モータ22,24のエンコーダ22a,24aの値(すなわち、車輪12,13の回転角速度)と、ジャイロセンサ28の値(車体の傾斜角速度)を読込む(S23)。また、読み込んだエンコーダ22a,24aの値から、移動体10の現在の接線速度sr’と、経路長srを算出する。
次に、ステップS22で読み込んだ時刻tより移動体の目標接線速度及び目標経路長を算出し(S24)、この算出した目標接線速度及び目標経路長と、エンコーダ22a,24aの値から得られた現在の接線速度及び経路長と、ジャイロセンサ28の傾斜角速度とから、倒立制御用の制御指令値(目標接線加速度)を算出する(S26)。
ステップS28では、ステップS26で算出した制御指令値(加速度)を積分して新目標接線速度を算出する。次いで、ステップS28で算出した新目標接線速度より新経路長を算出し(S30)、この新経路長より修正時刻t’を算出する(S32)。次いで、ステップS32で求めた修正時刻t’から移動体10の旋回方向の目標角速度φ’を算出する(S34)。
次に、ステップS28で算出した新目標接線速度と、ステップS34で算出した目標角速度φ’を合成し、その合成したものを各モータ12,14に分配し、各モータ12,14の目標角速度を算出する(S36)。
次いで、ステップS36で算出された目標角速度とエンコーダ22a,24aで測定された車輪回転速度の偏差が0となるように、モータ22,24を制御する(S38)。これによって、各車輪12,14が駆動されることとなる。ステップS38が終わるとステップ22に戻り、次の制御タイミングにおける処理が開始される。
また、第2実施例では、エンコーダ22a,24aの検出結果が車輪角速度制御器68にフィードバックされる構成とされている。このため、フィードバックループが小さくなり、モータ22,24を所望の角度に精度良く制御できる。これによって、移動体は、目標軌道によって規定される目標経路上を精度良く走行することができる。
位置・姿勢センサ86は、移動体の位置(x,y)と姿勢(φ)を検出する。位置・姿勢センサ86には、例えば、レーザレンジセンサを用いることができる。レーザレンジセンサは、レーザを照射することによって、レーザが照射された物体までの距離を検出する。このため、移動体が走行する部屋の壁の形状を記憶したマップと、レーザを全方位(360度)に照射したときの壁の形状とを照合することで、移動体の位置と姿勢(x,y,φ)を得ることができる。位置・姿勢センサ86の検出結果(x,y,φ)は、制御装置88に入力される。なお、位置・姿勢センサ86には、GPSセンサやモーションキャプチャ等を用いることもできる。
具体的には、制御装置88は、目標軌道入力部38に記憶されている目標軌道(x(t),y(t))を微分することで、移動体の目標速度(x’(t),y’(t))を算出する。そして、φ=Arctan(y’(t)/x’(t))の式にx’(t)とy’(t)を代入し、移動体の目標姿勢φを算出する(目標姿勢算出部90)。
目標姿勢が算出されると、その目標姿勢と位置・姿勢センサで検出された移動体の姿勢(φ)から姿勢偏差Δφを算出し(偏差算出部92)、その姿勢偏差Δφを軌道トラッキングコントローラ100に入力する。
また、目標軌道(x(t),y(t))から算出される移動体の位置と、位置・姿勢センサ86の検出結果から算出される現在の移動体の位置とを用いて、移動体の進行方向の位置偏差を算出し(偏差算出部94)、その位置偏差を軌道トラッキングコントローラ100に入力する。
また、目標軌道(x(t),y(t))から算出される移動体の位置と、位置・姿勢センサ86の検出結果から算出される現在の移動体の位置とを用いて、移動体の進行方向に直交する方向の位置偏差を算出し(偏差算出部96)、その偏差を軌道トラッキングコントローラ100に入力する。
次に、倒立制御用コントローラ110で算出された新接線加速度を積分して新接線速度を算出する(新接線速度算出部108)。算出された新接線速度と軌道トラッキングコントローラ100によって算出された目標接線速度の比(新接線速度/目標接線速度)を算出する(速度比算出部104)。すなわち、倒立制御用コントローラ110で処理された新接線速度は、移動体の進行方向の応答遅れが考慮されたものとなっている。一方、軌道トラッキングコントローラ100で算出された目標接線速度は、移動体の進行方向の応答遅れが考慮されていない。このため、新接線速度と目標接線速度の比を求めることで、移動体の進行方向の速度応答の遅れの程度が算出される。
次に、軌道トラッキングコントローラ100で算出された目標角速度に上記の比(新接線速度/目標接線速度)を掛けて、移動体の旋回方向の制御指令値(目標角速度)を算出する(旋回方向の新目標算出部106)。
車輪角速度制御器112は、新接線速度算出部108で算出された制御指令値(新接線速度)と、目標角度算出部106で算出された制御指令値(目標角速度φ’)を合成して、各モータ22,24に分配することで、各モータ22,24(各車輪12,14)の目標角速度を算出する。そして、算出された目標角速度とエンコーダ22a,24aで検出される目標角速度との偏差が0となるようにモータ22,24を駆動する。
図11に示すように、制御装置88は、まず、制御装置88が起動されてからの時間t(すなわち、制御開始時からの経過時間)を読み込む(S42)。次に、位置・姿勢センサ86の出力を読み込む(S43)。また、ジャイロセンサ28の出力、エンコーダ22a,24aの出力を読み込む(S44)。
次に、目標軌道から目標接線速度を算出し、この目標接線速度から目標姿勢(φ)を算出する(S46)。そして、位置・姿勢センサ86の出力と目標軌道から、各偏差(Δx,Δy,Δφ)を算出する(S48)。次いで、ステップS48で算出した偏差を用いて、軌道トラッキングコントローラ110が移動体の目標接線速度と目標角速度(旋回方向の目標角速度)を算出する(S50)。ステップS50で目標接線速度が算出されると、その目標接線速度を積分して目標経路長が算出される(S52)。次に、ステップS44で検出された検出結果と、ステップS50で算出された目標接線速度と、ステップS52で算出された目標経路長とを用いて、倒立制御用の新接線加速度を算出する(S54)。
ステップS52で算出された新接線加速度は積分されて新接線速度が算出される(S56)。次に、ステップS50で算出した目標接線速度とステップS56で算出した新接線速度の速度比(新接線速度/目標接線速度)を算出し(S58)、この速度比(新目標接線速度/目標接線速度)をステップS50で算出された目標角速度に掛け、旋回方向の目標角速度を修正する(S60)。
次に、ステップS56で算出した新接線速度と、ステップS60で算出した目標角速度φ’を合成し、その合成したものを各モータ12,14に分配し、各モータ12,14の目標角速度を算出する(S62)。
次いで、ステップS62で算出された目標加速度とエンコーダ22a,24aで測定された車輪回転速度の偏差が0となるように、モータ22,24を制御する(S64)。これによって、各車輪12,14が駆動されることとなる。ステップS64が終わるとステップ42に戻り、次の制御タイミングにおける処理が開始される。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
12、14:車輪
16:車軸
20:車体
22、24:モータ
22a、24a:エンコーダ
28:ジャイロセンサ
30,60,88:制御装置
Claims (2)
- 車輪と、車輪に支持される車体を備えており、車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する倒立振子型の移動体であって、
移動体の進行方向の位置に関する情報を測定するためのセンサと、
移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第1制御指令値を算出する手段と、
前記目標軌道とセンサで実測される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第2制御指令値を算出する手段と、
第1制御指令値と第2制御指令値に基づいて車輪を駆動する手段と、を備えており、
第1制御指令値算出手段と第2制御指令値算出手段と駆動手段とを所定周期毎に繰り返し実行することで、移動体が路面上を走行し、
第2制御指令値算出手段は、センサの検出結果から移動体の経路長さを算出し、その算出された経路長さに対応する前記目標軌道上の位置を算出し、その算出された位置を用いて移動体の旋回方向の角度を制御するための第2制御指令値を算出することを特徴とする倒立振子型移動体。 - 車輪と、車輪に支持される車体と、進行方向の位置に関する情報を測定するためのセンサと、車輪を回転駆動する駆動装置と、駆動装置を制御する制御装置を備えており、駆動装置によって車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する倒立振子型移動体の制御方法であって、
移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第1制御指令値を算出する工程と、
前記目標軌道とセンサで実測される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第2制御指令値を算出する工程と、
第1制御指令値と第2制御指令値に基づいて車輪を駆動する工程と、を有しており、
上記各工程を所定周期毎に繰返し実行することで移動体を制御し、
前記第2制御指令値を算出する工程は、センサの検出結果から移動体の経路長さを算出し、その算出された経路長さに対応する前記目標軌道上の位置を算出し、その算出された位置を用いて移動体の旋回方向の角度を制御するための第2制御指令値を算出することを特徴とする倒立振子型移動体の制御方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009160133A JP5336955B2 (ja) | 2009-07-06 | 2009-07-06 | 倒立振子型移動体及びその制御方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009160133A JP5336955B2 (ja) | 2009-07-06 | 2009-07-06 | 倒立振子型移動体及びその制御方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2011014100A JP2011014100A (ja) | 2011-01-20 |
JP5336955B2 true JP5336955B2 (ja) | 2013-11-06 |
Family
ID=43592885
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009160133A Expired - Fee Related JP5336955B2 (ja) | 2009-07-06 | 2009-07-06 | 倒立振子型移動体及びその制御方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5336955B2 (ja) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5736952B2 (ja) * | 2011-05-17 | 2015-06-17 | トヨタ自動車株式会社 | 移動体の制御方法及びプログラム |
CN105867371B (zh) * | 2016-04-06 | 2019-05-21 | 北京小米移动软件有限公司 | 控制平衡车移动的方法及装置 |
JP7078120B2 (ja) * | 2018-09-05 | 2022-05-31 | 日本電気株式会社 | モーション制御装置、モーション制御方法、モーション制御プログラム、及びモーション制御システム |
CN111736471B (zh) * | 2020-07-14 | 2021-06-15 | 江南大学 | 一种旋转倒立摆的迭代反馈整定控制及其鲁棒优化方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4138546B2 (ja) * | 2003-03-26 | 2008-08-27 | トヨタ自動車株式会社 | 移動台車及び移動台車の制御方法 |
-
2009
- 2009-07-06 JP JP2009160133A patent/JP5336955B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JP2011014100A (ja) | 2011-01-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4760162B2 (ja) | 移動台車の制御方法及び移動台車 | |
JP3993883B2 (ja) | 倒立二輪走行型ロボット及びその制御方法 | |
JP7093504B2 (ja) | ドライバトルク推定装置およびそれを備えた操舵装置 | |
JP5321681B2 (ja) | 移動方向制御装置及びコンピュータプログラム | |
JP2007219986A (ja) | 倒立移動装置及びその制御方法 | |
US8935050B2 (en) | Moving body control system, moving body control method, and non-transitory computer readable medium storing control program | |
JP2019014468A (ja) | 操舵装置 | |
US9321498B2 (en) | Method of estimating mounting angle error of gyroscopes by using a turning device, and corresponding turning device | |
JP2008059497A (ja) | 移動体及び移動体の制御方法 | |
JP4138546B2 (ja) | 移動台車及び移動台車の制御方法 | |
JPWO2007063665A1 (ja) | 転倒防止制御装置 | |
JP6281594B2 (ja) | 走行装置、走行装置の制御方法および走行装置の制御プログラム | |
JP5336955B2 (ja) | 倒立振子型移動体及びその制御方法 | |
CN109144078A (zh) | 一种基于二维码导航共线双舵轮agv路线纠偏方法及*** | |
Kim et al. | Stable control of the bicycle robot on a curved path by using a reaction wheel | |
JP2702370B2 (ja) | 慣性を利用したナビゲーションシステムを取り付けた移動ロボット | |
JP5907037B2 (ja) | 移動体 | |
JP4577247B2 (ja) | 移動体及びその制御方法 | |
JP5240365B2 (ja) | 移動方向制御装置及びコンピュータプログラム | |
JP2006205839A (ja) | 移動台車及び移動台車の制御方法 | |
JP2005080902A (ja) | 駆動体及び走行車両の姿勢制御方法及装置 | |
JP7391616B2 (ja) | 移動体の姿勢推定装置 | |
JP2006334729A (ja) | 倒立振り子型台車ロボットとその制御方法 | |
JP2014156216A (ja) | 車輪型移動体 | |
JP2733924B2 (ja) | 移動体の走行制御装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20111125 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20121218 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20121225 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20130225 |
|
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: 20130723 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130802 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5336955 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
LAPS | Cancellation because of no payment of annual fees |