JP2011105072A - Control device of inverted-pendulum type vehicle - Google Patents
Control device of inverted-pendulum type vehicle Download PDFInfo
- Publication number
- JP2011105072A JP2011105072A JP2009260086A JP2009260086A JP2011105072A JP 2011105072 A JP2011105072 A JP 2011105072A JP 2009260086 A JP2009260086 A JP 2009260086A JP 2009260086 A JP2009260086 A JP 2009260086A JP 2011105072 A JP2011105072 A JP 2011105072A
- Authority
- JP
- Japan
- Prior art keywords
- value
- speed
- vehicle
- idling
- center
- 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.)
- Granted
Links
- 230000001133 acceleration Effects 0.000 claims description 52
- 230000005540 biological transmission Effects 0.000 claims description 8
- 230000005484 gravity Effects 0.000 abstract description 64
- 238000005259 measurement Methods 0.000 abstract description 29
- 238000001514 detection method Methods 0.000 abstract description 16
- 238000012545 processing Methods 0.000 description 151
- 230000001629 suppression Effects 0.000 description 94
- 238000000034 method Methods 0.000 description 70
- 230000008569 process Effects 0.000 description 59
- 230000033001 locomotion Effects 0.000 description 51
- 230000036544 posture Effects 0.000 description 49
- 230000014509 gene expression Effects 0.000 description 33
- 238000006243 chemical reaction Methods 0.000 description 17
- 230000006870 function Effects 0.000 description 17
- 230000008859 change Effects 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 10
- 230000002093 peripheral effect Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 238000012937 correction Methods 0.000 description 8
- 239000000758 substrate Substances 0.000 description 7
- 238000001914 filtration Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 4
- 235000012489 doughnuts Nutrition 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000035945 sensitivity Effects 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 239000003638 chemical reducing agent Substances 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 239000003990 capacitor Substances 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000005489 elastic deformation Effects 0.000 description 1
- 239000013013 elastic material Substances 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 230000002123 temporal effect 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
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Motorcycle And Bicycle Frame (AREA)
- Electric Propulsion And Braking For Vehicles (AREA)
Abstract
Description
本発明は、床面上を移動可能な倒立振子型車両の制御装置に関する。 The present invention relates to a control device for an inverted pendulum type vehicle that can move on a floor surface.
倒立振子型車両は、床面上を移動する移動動作部と、この移動動作部を駆動するアクチュエータ装置とが組付けられた基体に、鉛直方向に対して傾動自在な乗員の搭乗部が組付けられた車両であり、搭乗部の傾斜角度をある目標角度に保つために(搭乗部が傾倒しないようにするために)、倒立振子の支点を動かすような形態で、移動動作部を移動させる必要がある車両である。 In an inverted pendulum type vehicle, a passenger's riding section that can be tilted in the vertical direction is assembled to a base body on which a moving operation section that moves on a floor surface and an actuator device that drives the movement operation section are assembled. In order to maintain the inclination angle of the riding section at a certain target angle (in order to prevent the riding section from tilting), it is necessary to move the moving operation section in such a manner that the fulcrum of the inverted pendulum is moved. There is a vehicle.
そして、この種の倒立振子型車両の制御技術としては、例えば、特許文献1に見られるものが本願出願人により提案されている。
As a control technique for this type of inverted pendulum type vehicle, for example, the one shown in
この特許文献1には、乗員の搭乗部が組付けられた車両の基体が球体状の移動動作部に対して前後方向の軸周りと左右方向の軸周りとの2軸周りに傾動自在に設けられた倒立振子型車両の制御技術が記載されている。この技術では、搭乗部の傾斜角度(前後方向及び左右方向の2軸周りの傾斜角度)の計測値を所要の目標傾斜角度に収束させ、また、車両の前後方向及び左右方向の移動速度の計測値を目標速度に収束させるように、アクチュエータとしての電動モータのトルク指令値を決定し、このトルク指令値に応じて電動モータの運転制御を行なうことで、車両を移動させるようにしている。
In this
なお、倒立振子型車両として機能し得る車両としては、特許文献2、3に見られるものも本願出願人により提案されている。
In addition, as a vehicle that can function as an inverted pendulum type vehicle, those shown in
また、特許文献4には、車輪と路面との間におけるスリップの発生を検知する車両が記載されている。この車両は、座席を備え前後方向に揺動可能な車体の下部に左右一対の車輪を備えている。そして、左右の車輪の回転角速度の実測値から算出した移動体の旋回角速度の予測値と、移動体の旋回角速度の実測値とを比較して、スリップの発生を検知している。 Patent Document 4 describes a vehicle that detects the occurrence of slip between a wheel and a road surface. This vehicle includes a pair of left and right wheels at a lower portion of a vehicle body that includes a seat and can swing in the front-rear direction. Then, the predicted value of the turning angular velocity of the moving body calculated from the actual measurement values of the rotational angular velocities of the left and right wheels is compared with the actual measurement value of the turning angular velocity of the moving body to detect the occurrence of slip.
しかしながら、特許文献4に記載されたスリップ検知(空転検知)では、走行方向に対して非平行となるように複数の車輪が配置されている車両のスリップしか検知できず、スリップ検知可能な移動動作部の構成が限定される。また、旋回時のスリップしか検知できず、例えば直進時のスリップなどは検知できず、検知可能な空転が限定される。 However, in the slip detection (idling detection) described in Patent Document 4, only a slip of a vehicle in which a plurality of wheels are arranged so as to be non-parallel to the traveling direction can be detected, and a moving operation capable of slip detection is possible. The configuration of the part is limited. Moreover, only slip at the time of turning can be detected, for example, slip at the time of going straight cannot be detected, and the idling that can be detected is limited.
本発明はかかる背景に鑑みてなされたものであり、空転検知可能な移動動作部の構成の限定が少なく、検知可能な空転の限定が少ない空転検知を行なうことが可能な倒立振子型車両の制御装置を提供することを目的とする。 The present invention has been made in view of such a background, and the control of an inverted pendulum type vehicle capable of performing idling detection with few limitations on the configuration of the moving operation unit capable of detecting idling and having few limitations on idling that can be detected. An object is to provide an apparatus.
本発明の倒立振子型車両の制御装置は、かかる目的を達成するために、床面上を移動可能な移動動作部と、該移動動作部を駆動するアクチュエータ装置と、該移動動作部及びアクチュエータ装置が組付けられた基体と、鉛直方向に対して傾動自在に前記基体に組付けられた運搬対象物体の搭載部とを備えた倒立振子型車両の制御装置であって、前記搭載部又は該搭載部と一体に傾斜可能な部位の加速度の観測値に基づいて、前記車両の所定の代表点の速度を推定する第1の速度推定手段と、少なくとも、前記搭載部又は該搭載部と一体に傾斜可能な部位の傾斜角速度の観測値に基づいて、前記代表点の速度を推定する第2の速度推定手段と、前記第1の速度推定手段が推定した推定値と、前記第2の速度推定手段が推定した推定値とに少なくとも基づいて、前記移動動作部の空転発生を検知する空転検知手段とを備えることを特徴とする。 In order to achieve such an object, a control device for an inverted pendulum type vehicle according to the present invention includes a moving operation unit that can move on a floor surface, an actuator device that drives the moving operation unit, and the moving operation unit and actuator device. Is a control device for an inverted pendulum type vehicle, comprising: a base body to which the object is to be transported; and a mounting portion for the object to be transported that is attached to the base body so as to be tiltable with respect to a vertical direction. First speed estimation means for estimating a speed of a predetermined representative point of the vehicle based on an observed value of acceleration of a part that can be tilted integrally with the part, and at least the mounting part or the slope integrally with the mounting part A second speed estimating means for estimating the speed of the representative point based on an observed value of the tilt angle speed of the possible part; an estimated value estimated by the first speed estimating means; and the second speed estimating means. Less than the estimated value estimated by Be based, characterized in that it comprises a idling detection means for detecting the idling generation of the traveling motion unit.
なお、本発明において、「床」は、通常的な意味での床(屋内の床など)だけを意味するものではなく、屋外の地面もしくは路面をも含むものとして使用する。 In the present invention, “floor” does not mean a floor in a normal sense (such as an indoor floor) but also includes an outdoor ground or road surface.
本発明によれば、第1の速度推定手段は、搭載部又は該搭載部と一体に傾斜可能な部位の加速度の観測値に基づいて、車両の所定の代表点の速度を推定する。一方、第2の速度推定手段は、少なくとも、搭載部又は該搭載部と一体に傾斜可能な部位の傾斜角速度の観測値に基づいて、前記代表点の速度を推定する。そして、空転検知手段は、第1及び第2の速度推定手段が推定した推定値に基づいて、移動動作部の空転発生を検知する。 According to the present invention, the first speed estimating means estimates the speed of the predetermined representative point of the vehicle based on the observed acceleration value of the mounting portion or a portion that can be tilted integrally with the mounting portion. On the other hand, the second speed estimation means estimates the speed of the representative point based at least on the observed value of the tilt angular speed of the mounting portion or a portion that can tilt integrally with the mounting portion. Then, the idling detection means detects the occurrence of idling in the moving operation unit based on the estimated values estimated by the first and second speed estimation means.
移動動作部で空転が発生していなければ、2つの速度推定手段が推定した推定値の間に差は生じない。しかし、移動動作部に空転が発生した場合には、2つの推定値の間に差が生じる。そこで、2つの推定値に基づいて、移動動作部に空転が発生したおそれを判断することが可能となる。 If no idling occurs in the moving operation unit, there is no difference between the estimated values estimated by the two speed estimating means. However, when idling occurs in the moving operation unit, a difference is generated between the two estimated values. Therefore, based on the two estimated values, it is possible to determine the possibility that idling has occurred in the moving operation unit.
上記特許文献4に記載されたスリップ検知とは異なり、移動動作部の構成に拘わらず、空転が発生したおそれを判断することが可能である。さらに、搭載部又は該搭載部と一体に傾斜可能な部位の傾斜に影響する移動動作部で空転が発生したおそれを検知することが可能であるので、旋回時の他、直進時などの空転も検知することができる。 Unlike the slip detection described in Patent Document 4, it is possible to determine the possibility of idling regardless of the configuration of the moving operation unit. Furthermore, since it is possible to detect the possibility that idling has occurred in the moving part that affects the inclination of the mounting part or the part that can incline integrally with the mounting part, it is possible to detect idling during straight traveling as well as during turning. Can be detected.
また、本発明において、前記第2の速度推定手段は、推定した前記代表点の速度の値を、前記アクチュエータ装置と前記移動動作部との間の駆動力伝達経路におけるすべりを考慮して補正することが好ましい。 In the present invention, the second speed estimating means corrects the estimated speed value of the representative point in consideration of a slip in a driving force transmission path between the actuator device and the moving operation unit. It is preferable.
この場合、第2の速度推定手段は、アクチュエータ装置と移動動作部との間の駆動力伝達経路におけるすべりを考慮して、車両の代表点の速度の推定値を補正するので、より正確に代表点の速度を推定することが可能となる。よって、移動動作部に空転が発生したおそれを高精度に判断することが可能となる。 In this case, since the second speed estimating means corrects the estimated value of the speed of the representative point of the vehicle in consideration of the slip in the driving force transmission path between the actuator device and the moving operation unit, the representative speed can be represented more accurately. It becomes possible to estimate the velocity of the point. Therefore, it is possible to determine with high accuracy the risk of idling in the moving operation unit.
本発明の一実施形態を以下に説明する。まず、図1〜図6を参照して、本実施形態における倒立振子型車両の構造を説明する。 One embodiment of the present invention will be described below. First, the structure of an inverted pendulum type vehicle in the present embodiment will be described with reference to FIGS.
図1及び図2に示すように、本実施形態における倒立振子型車両1(以降、単に車両1という)は、該車両1の運搬対象物体としての乗員(運転者)の搭乗部3と、床面に接地しながら該床面上を全方向(第1の方向である前後方向及び第2の方向である左右方向を含む2次元的な全方向)に移動可能な移動動作部5と、この移動動作部5を駆動する動力を該移動動作部5に付与するアクチュエータ装置7と、これらの搭乗部3、移動動作部5及びアクチュエータ装置7が組付けられた基体9とを備える。
As shown in FIGS. 1 and 2, an inverted pendulum type vehicle 1 (hereinafter simply referred to as a vehicle 1) in the present embodiment includes a passenger (driver)
ここで、本実施形態の説明では、「前後方向」、「左右方向」は、それぞれ、搭乗部3に標準的な姿勢で搭乗した乗員の上体の前後方向、左右方向に一致もしくはほぼ一致する方向を意味する。なお、「標準的な姿勢」は、搭乗部3に関して設計的に想定されている姿勢であり、乗員の上体の体幹軸を概ね上下方向に向け、且つ、上体を捻ったりしていない姿勢である。
Here, in the description of the present embodiment, “front-rear direction” and “left-right direction” respectively match or substantially coincide with the front-rear direction and the left-right direction of the upper body of the occupant who has boarded the
この場合、図1においては、「前後方向」、「左右方向」はそれぞれ、紙面に垂直な方向、紙面の左右方向であり、図2においては、「前後方向」、「左右方向」はそれぞれ、紙面の左右方向、紙面に垂直な方向である。また、本実施形態の説明では、参照符号に付する添え字「R」,「L」は、それぞれ車両1の右側、左側に対応するものという意味で使用する。
In this case, in FIG. 1, the “front-rear direction” and the “left-right direction” are the direction perpendicular to the paper surface and the left-right direction of the paper surface, respectively. In FIG. It is the left-right direction of the paper surface and the direction perpendicular to the paper surface. In the description of the present embodiment, the suffixes “R” and “L” attached to the reference numerals are used to mean the right side and the left side of the
基体9は、移動動作部5及びアクチュエータ装置7とが組付けられた下部フレーム11と、この下部フレーム11の上端から上方に延設された支柱フレーム13とを備える。
The
支柱フレーム13の上部には、該支柱フレーム13から前方側に張り出したシートフレーム15が固定されている。そして、このシートフレーム15上に、乗員が着座するシート3が装着されている。本実施形態では、このシート3が乗員の搭乗部(運搬対象物体の搭載部)となっている。従って、本実施形態における倒立振子型車両1は、乗員がシート3に着座した状態で、床面上を移動するものである。
A
また、シート3の左右には、シート3に着座した乗員が必要に応じて把持するためのグリップ17R,17Lが配置され、これらのグリップ17R,17Lがそれぞれ、支柱フレーム13(又はシートフレーム15)から延設されたブラケット19R,19Lの先端部に固定されている。
Further, on the left and right sides of the
下部フレーム11は、左右方向に間隔を存して二股状に対向するように配置された一対のカバー部材21R,21Lを備える。これらのカバー部材21R,21Lの上端部(二股の分岐部分)は、前後方向の軸心を有するヒンジ軸23を介して連結され、カバー部材21R,21Lの一方が他方に対して相対的にヒンジ軸23の周りに揺動可能となっている。この場合、カバー部材21R,21Lは、図示を省略するバネによって、カバー部材21R,21Lの下端部側(二股の先端側)が狭まる方向に付勢されている。
The
また、カバー部材21R,21Lのそれぞれの外面部には、前記シート3に着座した乗員の右足を載せるステップ25Rと左足を載せるステップ25Lとが各々、右向き、左向きに張り出すように突設されている。
Further, on each outer surface portion of the
移動動作部5及びアクチュエータ装置7は、下部フレーム11のカバー部材21R,21Lの間に配置されている。これらの移動動作部5及びアクチュエータ装置7の構造を図3〜図6を参照して説明する。
The moving
なお、本実施形態で例示する移動動作部5及びアクチュエータ装置7は、例えば前記特許文献2の図1に開示されているものと同じ構造のものである。従って、本実施形態の説明においては、移動動作部5及びアクチュエータ装置7の構成に関して、前記特許文献2に記載された事項については、簡略的な説明に留める。
The moving
本実施形態では、移動動作部5は、ゴム状弾性材により円環状に形成された車輪体であり、ほぼ円形の横断面形状を有する。この移動動作部5(以降、車輪体5という)は、その弾性変形によって、図5及び図6の矢印Y1で示す如く、円形の横断面の中心C1(より詳しくは、円形の横断面中心C1を通って、車輪体5の軸心と同心となる円周線)の周りに回転可能となっている。
In the present embodiment, the moving
この車輪体5は、その軸心C2(車輪体5全体の直径方向に直交する軸心C2)を左右方向に向けた状態で、カバー部材21R,21Lの間に配置され、該車輪体5の外周面の下端部にて床面に接地する。
The
そして、車輪体5は、アクチュエータ装置7による駆動(詳細は後述する)によって、図5の矢印Y2で示す如く車輪体5の軸心C2の周りに回転する動作(床面上を輪転する動作)と、車輪体5の横断面中心C1の周りに回転する動作とを行なうことが可能である。その結果、車輪体5は、それらの回転動作の複合動作によって、床面上を全方向に移動することが可能となっている。
The
アクチュエータ装置7は、車輪体5と右側のカバー部材21Rとの間に介装される回転部材27R及びフリーローラ29Rと、車輪体5と左側のカバー部材17Lとの間に介装される回転部材27L及びフリーローラ29Lと、回転部材27R及びフリーローラ29Rの上方に配置されたアクチュエータとしての電動モータ31Rと、回転部材27L及びフリーローラ29Lの上方に配置されたアクチュエータとしての電動モータ31Lとを備える。
The
電動モータ31R,31Lは、それぞれのハウジングがカバー部材21R,21Lに各々取付けられている。なお、図示は省略するが、電動モータ31R,31Lの電源(蓄電器)は、支柱フレーム13等、基体9の適所に搭載されている。
The
回転部材27Rは、左右方向の軸心を有する支軸33Rを介してカバー部材21Rに回転可能に支持されている。同様に、回転部材27Lは、左右方向の軸心を有する支軸33Lを介してカバー部材21Lに回転可能に支持されている。この場合、回転部材27Rの回転軸心(支軸33Rの軸心)と、回転部材27Lの回転軸心(支軸33Lの軸心)とは同軸心である。
The rotating
回転部材27R,27Lは、それぞれ電動モータ31R,31Lの出力軸に、減速機としての機能を含む動力伝達機構を介して接続されており、電動モータ31R,31Lからそれぞれ伝達される動力(トルク)によって回転駆動される。各動力伝達機構は、例えばプーリ・ベルト式のものである。すなわち、図3に示す如く、回転部材27Rは、プーリ35Rとベルト37Rとを介して電動モータ31Rの出力軸に接続されている。同様に、回転部材27Lは、プーリ35Lとベルト37Lとを介して電動モータ31Lの出力軸に接続されている。
The rotating
なお、上記動力伝達機構は、例えば、スプロケットとリンクチェーンとにより構成されるもの、あるいは、複数のギヤにより構成されるものであってもよい。また、例えば、電動モータ31R,31Lを、それぞれの出力軸が各回転部材27R,27Lと同軸心になるように各回転部材27R,27Lに対向させて配置し、電動モータ31R,31Lのそれぞれの出力軸を回転部材27R,27Lに各々、減速機(遊星歯車装置等)を介して連結するようにしてもよい。
The power transmission mechanism may be constituted by, for example, a sprocket and a link chain, or may be constituted by a plurality of gears. In addition, for example, the
各回転部材27R,27Lは、車輪体5側に向かって縮径する円錐台と同様の形状に形成されており、その外周面がテーパ外周面39R,39Lとなっている。
Each rotating
回転部材27Rのテーパ外周面39Rの周囲には、回転部材27Rと同心の円周上に等間隔で並ぶようにして、複数のフリーローラ29Rが配列されている。そして、これらのフリーローラ29Rは、それぞれ、ブラケット41Rを介してにテーパ外周面39Rに取付けられ、該ブラケット41Rに回転自在に支承されている。
A plurality of
同様に、回転部材27Lのテーパ外周面39Lの周囲には、回転部材27Lと同心の円周上に等間隔で並ぶようにして、複数(フリーローラ29Rと同数)のフリーローラ29Lが配列されている。そして、これらのフリーローラ29Lは、それぞれ、ブラケット41Lを介してにテーパ外周面39Lに取付けられ、該ブラケット41Lに回転自在に支承されている。
Similarly, a plurality (the same number as the
前記車輪体5は、回転部材27R側のフリーローラ29Rと、回転部材27L側のフリーローラ29Lとの間に挟まれるようにして、回転部材27R,27Lと同軸心に配置されている。
The
この場合、図1及び図6に示すように、各フリーローラ29R,29Lは、その軸心C3が車輪体5の軸心C2に対して傾斜すると共に、車輪体5の直径方向(車輪体5をその軸心C2の方向で見たときに、該軸心C2と各フリーローラ29R,29Lとを結ぶ径方向)に対して傾斜する姿勢で配置されている。そして、このような姿勢で、各フリーローラ29R,29Lのそれぞれの外周面が車輪体5の内周面に斜め方向に圧接されている。
In this case, as shown in FIGS. 1 and 6, each of the free rollers 29 </ b> R and 29 </ b> L has the axis C <b> 3 inclined with respect to the axis C <b> 2 of the
より一般的に言えば、右側のフリーローラ29Rは、回転部材27Rが軸心C2の周りに回転駆動されたときに、車輪体5との接触面で、軸心C2周りの方向の摩擦力成分(車輪体5の内周の接線方向の摩擦力成分)と、車輪体5の前記横断面中心C1の周り方向の摩擦力成分(円形の横断面の接線方向の摩擦力成分)とを車輪体5に作用させ得るような姿勢で、車輪体5の内周面に圧接されている。左側のフリーローラ29Lについても同様である。
More generally speaking, the
この場合、前記したように、カバー部材21R,21Lは、図示しないバネによって、カバー部材21R,21Lの下端部側(二股の先端側)が狭まる方向に付勢されている。このため、この付勢力によって、右側のフリーローラ29Rと左側のフリーローラ29Lとの間に車輪体5が挟持されると共に、車輪体5に対する各フリーローラ29R,29Lの圧接状態(より詳しくはフリーローラ29R,29Lと車輪体5との間で摩擦力が作用し得る圧接状態)が維持される。
In this case, as described above, the
以上説明した構造を有する車両1においては、電動モータ31R,31Lによりそれぞれ、回転部材27R,27Lを同方向に等速度で回転駆動した場合には、車輪体5が回転部材27R,27Lと同方向に軸心C2の周りに回転することとなる。これにより、車輪体5が床面上を前後方向に輪転して、車両1の全体が前後方向に移動することとなる。なお、この場合は、車輪体5は、その横断面中心C1の周りには回転しない。
In the
また、例えば、回転部材27R,27Lを互いに逆方向に同じ大きさの速度で回転駆動した場合には、車輪体5は、その横断面中心C1の周りに回転することとなる。これにより、車輪体4がその軸心C2の方向(すなわち左右方向)に移動し、ひいては、車両1の全体が左右方向に移動することとなる。なお、この場合は、車輪体5は、その軸心C2の周りには回転しない。
Further, for example, when the
さらに、回転部材27R,27Lを、互いに異なる速度(方向を含めた速度)で、同方向又は逆方向に回転駆動した場合には、車輪体5は、その軸心C2の周りに回転すると同時に、その横断面中心C1の周りに回転することとなる。
Furthermore, when the
この時、これらの回転動作の複合動作(合成動作)によって、前後方向及び左右方向に対して傾斜した方向に車輪体5が移動し、ひいては、車両1の全体が車輪体5と同方向に移動することとなる。この場合の車輪体5の移動方向は、回転部材27R,27Lの回転方向を含めた回転速度(回転方向に応じて極性が定義された回転速度ベクトル)の差に依存して変化するものとなる。
At this time, the
以上のように車輪体5の移動動作が行なわれるので、電動モータ31R,31Lのそれぞれの回転速度(回転方向を含む)を制御し、ひいては回転部材27R,27Lの回転速度を制御することによって、車両1の移動速度及び移動方向を制御できることとなる。
Since the moving operation of the
なお、シート(搭乗部)3及び基体9は、車輪体5の軸心C2を支点として、左右方向の軸心C2周りに傾動自在となっていると共に、車輪体5の接地面(下端面)を支点として、前後方向の軸周りに該車輪体5と共に傾動自在となっている。
The seat (boarding portion) 3 and the
次に、本実施形態の車両1の動作制御のための構成を説明する。なお、以降の説明では、図1及び図2に示すように、前後方向の水平軸をX軸、左右方向の水平軸をY軸、鉛直方向をZ軸とするXYZ座標系を想定し、前後方向、左右方向をそれぞれX軸方向、Y軸方向と言うことがある。
Next, the structure for operation control of the
まず、車両1の概略的な動作制御を説明すると、本実施形態では、基本的には、シート3に着座した乗員がその上体を傾けた場合(詳しくは、乗員と車両1とを合わせた全体の重心点の位置(水平面に投影した位置)を動かすように上体を傾けた場合)に、該上体を傾けた側に基体9がシート3と共に傾動する。そして、この時、基体9が傾いた側に車両1が移動するように、車輪体5の移動動作が制御される。例えば、乗員が上体を前傾させ、ひいては、基体9をシート3と共に前傾させると、車両1が前方に移動するように、車輪体5の移動動作が制御される。
First, schematic operation control of the
すなわち、本実施形態では、乗員が上体を動かし、ひいては、シート3と共に基体9を傾動させるという動作が、車両1に対する1つの基本的な操縦操作(車両1の動作要求)とされ、その操縦操作に応じて車輪体5の移動動作がアクチュエータ装置7を介して制御される。
That is, in the present embodiment, the operation of the occupant moving the upper body and thus tilting the
ここで、本実施形態の車両1は、その全体の接地面としての車輪体5の接地面が、車両1とこれに搭乗する乗員との全体を床面に投影した領域に比して面積が小さい単一の局所領域となり、その単一の局所領域だけに床反力が作用する。このため、基体9が傾倒しないようにするためには、乗員及び車両1の全体の重心点が車輪体5の接地面のほぼ真上に位置するように、車輪体5を動かす必要がある。
Here, in the
そこで、本実施形態では、乗員及び車両1の全体の重心点が、車輪体5の中心点(軸心C2上の中心点)のほぼ真上に位置する状態(より正確には当該重心点が車輪体5の接地面のほぼ真上に位置する状態)での基体9の姿勢を目標姿勢とし、基本的には、基体9の実際の姿勢を目標姿勢に収束させるように、車輪体5の移動動作が制御される。
Therefore, in the present embodiment, the center of gravity of the entire occupant and
さらに、車両1に乗員が搭乗していない状態では、車両1の単体の重心点が、車輪体5の中心点(軸心C2上の中心点)のほぼ真上に位置する状態(より正確には当該重心点が車輪体5の接地面のほぼ真上に位置する状態)での基体9の姿勢を目標姿勢とし、該基体9の実際の姿勢を目標姿勢に収束させ、ひいては、基体9が傾倒することなく車両1が自立するように、車輪体5の移動動作が制御される。
Further, in a state where no occupant is on board the
また、車両1に乗員が搭乗している状態と搭乗していない状態とのいずれの状態においても、基体9の実際の姿勢の目標姿勢からずれが大きいほど、車両1の移動速度が速くなると共に、基体9の実際の姿勢の目標姿勢に一致する状態では、車両1の移動が停止するように車輪体5の移動動作が制御される。
In addition, in both the state where the occupant is on board the
補足すると、「姿勢」は空間的な向きを意味する。本実施形態では、基体9がシート3と共に傾動することで、基体9やシート3の姿勢が変化する。また、本実施形態では、基体9とシート3とは一体的に傾動するので、基体9の姿勢をその目標姿勢に収束させるということは、シート3の姿勢を該シート3に対応する目標姿勢(基体9の姿勢が基体9の目標姿勢に一致する状態でのシート3の姿勢)に収束させるということと等価である。
Supplementally, “posture” means a spatial orientation. In the present embodiment, the
本実施形態では、以上の如き車両1の動作制御を行なうために、図1及び図2に示すように、マイクロコンピュータや電動モータ31R,31Lのドライブ回路ユニットなどを含む電子回路ユニットにより構成された制御ユニット50と、基体9の所定の部位の鉛直方向(重力方向)に対する傾斜角度θb及びその変化速度(=dθb/dt)を計測するための傾斜センサ52と、車両1に乗員が搭乗しているか否かを検知するための荷重センサ54と、電動モータ31R,31Lのそれぞれの出力軸の回転角度及び回転角速度を検出するための角度センサとしてのロータリエンコーダ56R,56Lとがそれぞれ、車両1の適所に搭載されている。
In the present embodiment, in order to control the operation of the
この場合、制御ユニット50及び傾斜センサ52は、例えば、基体9の支柱フレーム13の内部に収容された状態で該支柱フレーム13に取付けられている。また、荷重センサ54は、シート3に内蔵されている。また、ロータリエンコーダ56R,56Lは、それぞれ、電動モータ31R,31Lと一体に設けられている。なお、ロータリエンコーダ56R,56Lは、それぞれ、回転部材27R,27Lに装着してもよい。
In this case, the
上記傾斜センサ52は、より詳しくは、加速度センサとジャイロセンサ等のレートセンサ(角速度センサ)とから構成され、これらのセンサの検出信号を制御ユニット50に出力する。そして、制御ユニット50が、傾斜センサ52の加速度センサ及びレートセンサの出力を基に、所定の計測演算処理(これは公知の演算処理でよい)を実行することによって、傾斜センサ52を搭載した部位(本実施形態では支柱フレーム13)の、鉛直方向に対する傾斜角度θbの計測値とその変化速度(微分値)である傾斜角速度θbdotの計測値とを算出する。
More specifically, the
この場合、計測する傾斜角度θb(以降、基体傾斜角度θbということがある)は、より詳しくは、それぞれ、Y軸周り方向(ピッチ方向)の成分θb_xと、X軸周り方向(ロール方向)の成分θb_yとから成る。同様に、計測する傾斜角速度θbdot(以降、基体傾斜角速度θbdotということがある)も、Y軸周り方向(ピッチ方向)の成分θbdot_x(=dθb_x/dt)と、X軸周り方向(ロール方向)の成分θbdot_y(=dθb_y/dt)とから成る。 In this case, the tilt angle θb to be measured (hereinafter also referred to as the base body tilt angle θb) is more specifically, the component θb_x in the Y axis direction (pitch direction) and the X axis direction (roll direction), respectively. It consists of component θb_y. Similarly, the measured tilt angular velocity θbdot (hereinafter also referred to as the base tilt angular velocity θbdot) is also measured in the Y-axis direction (pitch direction) component θbdot_x (= dθb_x / dt) and the X-axis direction (roll direction). Component θbdot_y (= dθb_y / dt).
補足すると、本実施形態では、基体9の支柱フレーム13と一体にシート3が傾動するので、基体傾斜角度θbは、搭乗部3の傾斜角度としての意味も持つ。
Supplementally, in the present embodiment, since the
なお、本実施形態の説明では、上記基体傾斜角度θbなど、X軸及びY軸の各方向(又は各軸周り方向)の成分を有する運動状態量等の変数、あるいは、該運動状態量に関連する係数等の変数に関しては、その各成分を区別して表記する場合に、該変数の参照符号に、添え字“_x”又は“_y”を付加する。 In the description of the present embodiment, a variable such as a motion state quantity having a component in each direction of the X axis and the Y axis (or a direction around each axis) such as the base body inclination angle θb, or a relation to the motion state quantity. For a variable such as a coefficient to be processed, a suffix “_x” or “_y” is added to the reference symbol of the variable when each component is expressed separately.
この場合において、並進速度等の並進運動に係わる変数については、そのX軸方向の成分に添え字“_x”を付加し、Y軸方向の成分に添え字“_y”を付加する。 In this case, for a variable related to translational motion such as translational speed, a subscript “_x” is added to the component in the X-axis direction, and a subscript “_y” is added to the component in the Y-axis direction.
一方、角度、回転速度(角速度)、角加速度など、回転運動に係わる変数については、並進運動に係わる変数と添え字を揃えるために、便宜上、Y軸周り方向の成分に添え字“_x”を付加し、X軸周り方向の成分に添え字“_y”を付加する。 On the other hand, for variables related to rotational motion, such as angle, rotational speed (angular velocity), angular acceleration, etc., the subscript “_x” is added to the component around the Y axis for convenience in order to align the subscript with the variable related to translational motion. In addition, the subscript “_y” is added to the component around the X axis.
さらに、X軸方向の成分(又はY軸周り方向の成分)と、Y軸方向の成分(又はX軸周り方向の成分)との組として変数を表記する場合には、該変数の参照符号に添え字“_xy”を付加する。例えば、上記基体傾斜角度θbを、Y軸周り方向の成分θb_xとX軸周り方向の成分θb_yの組として表現する場合には、「基体傾斜角度θb_xy」というように表記する。 Further, when a variable is expressed as a set of a component in the X-axis direction (or a component around the Y-axis) and a component in the Y-axis direction (or a component around the X-axis), the reference numeral of the variable The subscript “_xy” is added. For example, when the base body tilt angle θb is expressed as a set of a component θb_x around the Y axis and a component θb_y around the X axis, it is expressed as “base body tilt angle θb_xy”.
前記荷重センサ54は、乗員がシート3に着座した場合に該乗員の重量による荷重を受けるようにシート3に内蔵され、その荷重に応じた検出信号を制御ユニット50に出力する。そして、制御ユニット50が、この荷重センサ54の出力により示される荷重の計測値に基づいて、車両1に乗員が搭乗しているか否かを判断する。
The
なお、荷重センサ54の代わりに、例えば、乗員がシート3に着座したときにONとなるようなスイッチ式のセンサを用いてもよい。
Instead of the
ロータリエンコーダ56Rは、電動モータ31Rの出力軸が所定角度回転する毎にパルス信号を発生し、このパルス信号を制御ユニット50に出力する。そして、制御ユニット50が、そのパルス信号を基に、電動モータ53Rの出力軸の回転角度を計測し、さらにその回転角度の計測値の時間的変化率(微分値)を電動モータ53Rの回転角速度として計測する。電動モータ31L側のロータリエンコーダ56Lについても同様である。
The rotary encoder 56 </ b> R generates a pulse signal each time the output shaft of the electric motor 31 </ b> R rotates by a predetermined angle, and outputs this pulse signal to the
制御ユニット50は、上記の各計測値を用いて所定の演算処理を実行することによって、電動モータ31R,31Lのそれぞれの回転角速度の目標値である速度指令を決定し、その速度指令に従って、電動モータ31R,31Lのそれぞれの回転角速度をフィードバック制御する。
The
なお、電動モータ31Rの出力軸の回転角速度と、回転部材27Rの回転角速度との間の関係は、該出力軸と回転部材27Rとの間の一定値の減速比に応じた比例関係になるので、本実施形態の説明では、便宜上、電動モータ31Rの回転角速度は、回転部材27Rの回転角速度を意味するものとする。同様に、電動モータ31Lの回転角速度は、回転部材27Lの回転角速度を意味するものとする。
The relationship between the rotational angular velocity of the output shaft of the
以下に、制御ユニット50の制御処理をさらに詳細に説明する。
Hereinafter, the control process of the
制御ユニット50は、所定の制御処理周期で図7のフローチャートに示す処理(メインルーチン処理)を実行する。
The
まず、STEP1において、制御ユニット50は、傾斜センサ52の出力を取得する。
First, in
次いで、STEP2に進んで、制御ユニット50は、取得した傾斜センサ52の出力を基に、基体傾斜角度θbの計測値θb_xy_sと、基体傾斜角速度θbdotの計測値θbdot_xy_sとを算出する。
Next, proceeding to
なお、以降の説明では、上記計測値θb_xy_sなど、変数(状態量)の実際の値の観測値(計測値)を参照符号により表記する場合に、該変数の参照符号に、添え字“_s”を付加する。 In the following description, when an observed value (measured value) of an actual value of a variable (state quantity) such as the measured value θb_xy_s is represented by a reference symbol, the subscript “_s” is added to the reference symbol of the variable. Is added.
次いで、制御ユニット50は、STEP3において、荷重センサ54の出力を取得した後、STEP4の判断処理を実行する。この判断処理においては、制御ユニット50は、取得した荷重センサ54の出力が示す荷重計測値が予め設定された所定値よりも大きいか否かによって、車両1に乗員が搭乗しているか否か(シート3に乗員が着座しているか否か)を判断する。
Next, after acquiring the output of the
そして、制御ユニット50は、STEP4の判断結果が肯定的である場合には、基体傾斜角度θbの目標値θb_xy_objを設定する処理と、車両1の動作制御用の定数パラメータ(各種ゲインの基本値など)の値を設定する処理とを、それぞれSTEP5、6で実行する。
If the determination result in STEP 4 is affirmative, the
STEP5においては、制御ユニット50は、基体傾斜角度θbの目標値θb_xy_objとして、予め定められた搭乗モード用の目標値を設定する。
In
ここで、「搭乗モード」は、車両1に乗員が搭乗している場合での車両1の動作モードを意味する。この搭乗モード用の目標値θb_xy_objは、車両1とシート3に着座した乗員との全体の重心点(以降、車両・乗員全体重心点という)が車輪体5の接地面のほぼ真上に位置する状態となる基体9の姿勢において、傾斜センサ52の出力に基づき計測される基体傾斜角度θbの計測値θb_xy_sに一致又はほぼ一致するように予め設定されている。
Here, the “boarding mode” means an operation mode of the
また、STEP6においては、制御ユニット50は、車両1の動作制御用の定数パラメータの値として、予め定められた搭乗モード用の値を設定する。なお、定数パラメータは、後述するhx,hy,Ki_a_x,Ki_b_x,Ki_a_y,Ki_b_y(i=1,2,3)等である。
In STEP 6, the
一方、STEP4の判断結果が否定的である場合には、制御ユニット50は、基体傾斜角度θb_xyの目標値θb_xy_objを設定する処理と、車両1の動作制御用の定数パラメータの値を設定する処理とを、STEP7、8で実行する。
On the other hand, if the determination result in STEP 4 is negative, the
STEP7においては、制御ユニット50は、傾斜角度θbの目標値θb_xy_objとして、予め定められた自立モード用の目標値を設定する。
In
ここで、「自立モード」は、車両1に乗員が搭乗していない場合での車両1の動作モードを意味する。この自立モード用の目標値θb_xy_objは、車両1単体の重心点(以降、車両単体重心点という)が車輪体5の接地面のほぼ真上に位置する状態となる基体9の姿勢において、傾斜センサ52の出力に基づき計測される基体傾斜角度θbの計測値θb_xy_sに一致又はほぼ一致するように予め設定されている。この自立モード用の目標値θb_xy_objは、搭乗モード用の目標値θb_xy_objと一般的には異なる。
Here, the “self-supporting mode” means an operation mode of the
また、STEP8においては、制御ユニット50は、車両1の動作制御用の定数パラメータの値として、予め定められた自立モード用の値を設定する。この自立モード用の定数パラメータの値は、搭乗モード用の定数パラメータの値と異なる。
In STEP 8, the
搭乗モードと自立モードとで、上記定数パラメータの値を異ならせるのは、それぞれのモードで上記重心点の高さや、全体質量等が異なることに起因して、制御入力に対する車両1の動作の応答特性が互いに異なるからである。
The difference in the value of the constant parameter between the boarding mode and the independent mode is due to the difference in the height of the center of gravity, the overall mass, etc. in each mode, and the response of the operation of the
以上のSTEP4〜8の処理によって、搭乗モード及び自立モードの各動作モード毎に各別に、基体傾斜角度θb_xyの目標値θb_xy_objと定数パラメータの値とが設定される。 Through the processing in STEPs 4 to 8 described above, the target value θb_xy_obj of the base body tilt angle θb_xy and the value of the constant parameter are set separately for each operation mode of the boarding mode and the self-supporting mode.
なお、STEP5,6の処理、又はSTEP7,8の処理は、制御処理周期毎に実行することは必須ではなく、STEP4の判断結果が変化した場合にだけ実行するようにしてもよい。
Note that the processing in
補足すると、搭乗モード及び自立モードのいずれにおいても、基体傾斜角速度θbdotのY軸周り方向の成分θbdot_xの目標値とX軸周り方向の成分θbdot_yの目標値とは、いずれも“0”である。このため、基体傾斜角速度θbdot_xyの目標値を設定する処理は不要である。 Supplementally, in both the boarding mode and the self-supporting mode, the target value of the component θbdot_x around the Y axis and the target value of the component θbdot_y around the X axis of the base body tilt angular velocity θbdot are both “0”. For this reason, the process which sets the target value of base | substrate inclination angular velocity (theta) bdot_xy is unnecessary.
以上の如くSTEP5,6の処理、又はSTEP7,8の処理を実行した後、制御ユニット50は、次にSTEP9において、車両制御演算処理を実行することによって、電動モータ31R,31Lのそれぞれの速度指令を決定する。この車両制御演算処理の詳細は後述する。
After executing the processing of
次いで、STEP10に進んで、制御ユニット50は、STEP9で決定した速度指令に応じて電動モータ31R,31Lの動作制御処理を実行する。この動作制御処理では、制御ユニット50は、STEP9で決定した電動モータ31Rの速度指令と、ロータリエンコーダ56Rの出力に基づき計測した電動モータ31Rの回転速度の計測値との偏差に応じて、該偏差を“0”に収束させるように電動モータ31Rの出力トルクの目標値(目標トルク)を決定する。そして、制御ユニット50は、その目標トルクの出力トルクを電動モータ31Rに出力させるように該電動モータ31Rの通電電流を制御する。左側の電動モータ31Lの動作制御についても同様である。
Next, proceeding to STEP 10, the
以上が、制御ユニット50が実行する全体的な制御処理である。
The above is the overall control process executed by the
次に、上記STEP9の車両制御演算処理の詳細を説明する。
Next, the details of the vehicle control calculation process in
なお、以降の説明においては、前記搭乗モードにおける車両・乗員全体重心点と、前記自立モードにおける車両単体重心点とを総称的に、車両系重心点という。該車両系重心点は、車両1の動作モードが搭乗モードである場合には、車両・乗員全体重心点を意味し、自立モードである場合には、車両単体重心点を意味する。
In the following description, the vehicle / occupant overall center-of-gravity point in the boarding mode and the vehicle single body center-of-gravity point in the autonomous mode are collectively referred to as a vehicle system center-of-gravity point. When the operation mode of the
また、以降の説明では、制御ユニット50が各制御処理周期で決定する値(更新する値)に関し、現在の(最新の)制御処理周期で決定する値を今回値、その1つ前の制御処理周期で決定した値を前回値ということがある。そして、今回値、前回値を特にことわらない値は、今回値を意味する。
In the following description, regarding the value (value to be updated) determined by the
また、X軸方向の速度及び加速度に関しては、前方向きを正の向きとし、Y軸方向の速度及び加速度に関しては、左向きを正の向きとする。 Further, regarding the speed and acceleration in the X-axis direction, the forward direction is a positive direction, and regarding the speed and acceleration in the Y-axis direction, the left direction is a positive direction.
本実施形態では、前記車両系重心点の動力学的な挙動(詳しくは、Y軸方向からこれに直交する面(XZ平面)に投影して見た挙動と、X軸方向からこれに直交する面(YZ平面)に投影して見た挙動)が、近似的に、図8に示すような、倒立振子モデルの挙動(倒立振子の動力学的挙動)によって表現されるものとして、STEP9の車両制御演算処理が行なわれる。 In the present embodiment, the dynamic behavior of the center of gravity of the vehicle system (specifically, the behavior seen by projecting from the Y-axis direction onto a plane (XZ plane) orthogonal thereto, and orthogonal to the X-axis direction) The vehicle of STEP9 is assumed that the behavior (projected and projected on the plane (YZ plane)) is approximately expressed by the behavior of the inverted pendulum model (the dynamic behavior of the inverted pendulum) as shown in FIG. Control arithmetic processing is performed.
なお、図8において、括弧を付していない参照符号は、Y軸方向から見た倒立振子モデルに対応する参照符号であり、括弧付きの参照符号は、X軸方向から見た倒立振子モデルに対応する参照符号である。 In FIG. 8, reference numerals without parentheses are reference numerals corresponding to the inverted pendulum model viewed from the Y-axis direction, and reference numerals with parentheses refer to the inverted pendulum model viewed from the X-axis direction. Corresponding reference sign.
この場合、Y軸方向から見た挙動を表現する倒立振子モデルは、車両系重心点に位置する質点60_xと、Y軸方向に平行な回転軸62a_xを有して床面上を輪転自在な仮想的な車輪62_x(以降、仮想車輪62_xという)とを備える。そして、質点60_xが、仮想車輪62_xの回転軸62a_xに直線状のロッド64_xを介して支持され、該回転軸62a_xを支点として該回転軸62a_xの周りに揺動自在とされている。 In this case, the inverted pendulum model expressing the behavior seen from the Y-axis direction has a mass point 60_x located at the center of gravity of the vehicle system and a rotation axis 62a_x parallel to the Y-axis direction. Wheel 62_x (hereinafter referred to as virtual wheel 62_x). The mass point 60_x is supported by the rotation shaft 62a_x of the virtual wheel 62_x via the linear rod 64_x, and can swing around the rotation shaft 62a_x with the rotation shaft 62a_x as a fulcrum.
この倒立振子モデルでは、質点60_xの運動が、Y軸方向から見た車両系重心点の運動に相当する。また、鉛直方向に対するロッド64_xの傾斜角度θbe_xがY軸周り方向での基体傾斜角度計測値θb_x_sと基体傾斜角度目標値θb_x_objとの偏差θbe_x_s(=θb_x_s−θb_x_obj)に一致するものとされる。また、ロッド64_xの傾斜角度θbe_xの変化速度(=dθbe_x/dt)がY軸周り方向の基体傾斜角速度計測値θbdot_x_sに一致するものとされる。また、仮想車輪62_xの移動速度Vw_x(X軸方向の並進移動速度)は、車両1の車輪体5のX軸方向の移動速度に一致するものとされる。
In this inverted pendulum model, the motion of the mass point 60_x corresponds to the motion of the vehicle system center of gravity as viewed from the Y-axis direction. In addition, the inclination angle θbe_x of the rod 64_x with respect to the vertical direction coincides with the deviation θbe_x_s (= θb_x_s−θb_x_obj) between the measured base body tilt angle value θb_x_s and the base body tilt angle target value θb_x_obj in the direction around the Y axis. Further, the changing speed (= dθbe_x / dt) of the inclination angle θbe_x of the rod 64_x is set to coincide with the measured body inclination angular velocity θbdot_x_s in the direction around the Y axis. Further, the movement speed Vw_x (translation movement speed in the X-axis direction) of the virtual wheel 62_x is the same as the movement speed in the X-axis direction of the
同様に、X軸方向から見た挙動を表現する倒立振子モデル(図8の括弧付きの符号を参照)は、車両系重心点に位置する質点60_yと、X軸方向に平行な回転軸62a_yを有して床面上を輪転自在な仮想的な車輪62_y(以降、仮想車輪62_yという)とを備える。そして、質点60_yが、仮想車輪62_yの回転軸62a_yに直線状のロッド64_yを介して支持され、該回転軸62a_yを支点として該回転軸62a_yの周りに揺動自在とされている。 Similarly, an inverted pendulum model (refer to the reference numerals in parentheses in FIG. 8) expressing the behavior viewed from the X-axis direction includes a mass point 60_y located at the vehicle system center of gravity and a rotation axis 62a_y parallel to the X-axis direction. And virtual wheels 62_y (hereinafter referred to as virtual wheels 62_y) that can rotate on the floor surface. The mass point 60_y is supported by the rotation shaft 62a_y of the virtual wheel 62_y via a linear rod 64_y, and can swing around the rotation shaft 62a_y with the rotation shaft 62a_y as a fulcrum.
この倒立振子モデルでは、質点60_yの運動が、X軸方向から見た車両系重心点の運動に相当する。また、鉛直方向に対するロッド64_yの傾斜角度θbe_yがX軸周り方向での基体傾斜角度計測値θb_y_sと基体傾斜角度目標値θb_y_objとの偏差θbe_y_s(=θb_y_s−θb_y_obj)に一致するものとされる。また、ロッド64_yの傾斜角度θbe_yの変化速度(=dθbe_y/dt)がX軸周り方向の基体傾斜角速度計測値θbdot_y_sに一致するものとされる。また、仮想車輪62_yの移動速度Vw_y(Y軸方向の並進移動速度)は、車両1の車輪体5のY軸方向の移動速度に一致するものとされる。
In this inverted pendulum model, the motion of the mass point 60_y corresponds to the motion of the vehicle system center-of-gravity point viewed from the X-axis direction. Further, the inclination angle θbe_y of the rod 64_y with respect to the vertical direction coincides with the deviation θbe_y_s (= θb_y_s−θb_y_obj) between the measured base body tilt angle value θb_y_s and the base body tilt angle target value θb_y_obj in the direction around the X axis. In addition, the change speed (= dθbe_y / dt) of the inclination angle θbe_y of the rod 64_y coincides with the measured base body inclination angular velocity θbdot_y_s in the direction around the X axis. Further, the moving speed Vw_y (translational moving speed in the Y-axis direction) of the virtual wheel 62_y is set to coincide with the moving speed in the Y-axis direction of the
なお、仮想車輪62_x,62_yは、それぞれ、予め定められた所定値Rw_x,Rw_yの半径を有するものとされる。 The virtual wheels 62_x and 62_y are assumed to have predetermined radii of predetermined values Rw_x and Rw_y, respectively.
また、仮想車輪62_x,62_yのそれぞれの回転角速度ωw_x,ωw_yと、電動モータ31R,31Lのそれぞれの回転角速度ω_R,ω_L(より正確には、回転部材27R,27Lのそれぞれの回転角速度ω_R,ω_L)との間には、次式01a,01bの関係が成立するものとされる。
Further, the rotational angular velocities ωw_x and ωw_y of the virtual wheels 62_x and 62_y and the rotational angular velocities ω_R and ω_L of the
ωw_x=(ω_R+ω_L)/2 ……式01a
ωw_y=C・(ω_R−ω_L)/2 ……式01b
なお、式01bにおける“C”は、前記フリーローラ29R,29Lと車輪体5との間の機構的な関係や滑りに依存する所定値の係数である。また、ωw_x,ω_R,ω_Lの正の向きは、仮想車輪62_xが前方に向かって輪転する場合の該仮想車輪62_xの回転方向、ωw_yの正の向きは、仮想車輪62_yが左向きに輪転する場合の該仮想車輪62_yの回転方向である。
ωw_x = (ω_R + ω_L) / 2 Equation 01a
ωw_y = C · (ω_R−ω_L) / 2 …… Formula 01b
Note that “C” in the expression 01b is a coefficient of a predetermined value depending on the mechanical relationship between the
ここで、図8に示す倒立振子モデルの動力学は、次式03x,03yにより表現される。なお、式03xは、Y軸方向から見た倒立振子モデルの動力学を表現する式、式03yは、X軸方向から見た倒立振子モデルの動力学を表現する式である。 Here, the dynamics of the inverted pendulum model shown in FIG. 8 is expressed by the following equations 03x and 03y. The expression 03x is an expression expressing the dynamics of the inverted pendulum model viewed from the Y-axis direction, and the expression 03y is an expression expressing the dynamics of the inverted pendulum model viewed from the X-axis direction.
d2θbe_x/dt2=α_x・θbe_x+β_x・ωwdot_x ……式03x
d2θbe_y/dt2=α_y・θbe_y+β_y・ωwdot_y ……式03y
式03xにおけるωwdot_xは仮想車輪62_xの回転角加速度(回転角速度ωw_xの1階微分値)、α_xは、質点60_xの質量や高さh_xに依存する係数、β_xは、仮想車輪62_xのイナーシャ(慣性モーメント)や半径Rw_xに依存する係数である。式03yにおけるωwdot_y、α_y、β_yについても上記と同様である。
d 2 θbe_x / dt 2 = α_x · θbe_x + β_x · ωwdot_x ...... Formula 03x
d 2 θbe_y / dt 2 = α_y · θbe_y + β_y · ωwdot_y ...... Formula 03y
In equation 03x, ωwdot_x is the rotational angular acceleration of the virtual wheel 62_x (first-order differential value of the rotational angular velocity ωw_x), α_x is a coefficient that depends on the mass and height h_x of the mass 60_x, and β_x is the inertia (moment of inertia of the virtual wheel 62_x ) And the radius Rw_x. The same applies to ωwdot_y, α_y, and β_y in Expression 03y.
これらの式03x,03yから判るように、倒立振子の質点60_x,60_yの運動(ひいては車両系重心点の運動)は、それぞれ、仮想車輪62_xの回転角加速度ωwdot_x、仮想車輪62_yの回転角加速度ωwdot_yに依存して規定される。 As can be seen from these equations 03x and 03y, the motions of the mass points 60_x and 60_y of the inverted pendulum (and hence the motion of the center of gravity of the vehicle system) are the rotational angular acceleration ωwdot_x of the virtual wheel 62_x and the rotational angular acceleration ωwdot_y of the virtual wheel 62_y, respectively. It is defined depending on.
そこで、本実施形態では、Y軸方向から見た車両系重心点の運動を制御するための操作量(制御入力)として、仮想車輪62_xの回転角加速度ωwdot_xを用いると共に、X軸方向から見た車両系重心点の運動を制御するための操作量(制御入力)として、仮想車輪62_yの回転角加速度ωwdot_yを用いる。 Therefore, in the present embodiment, the rotational angular acceleration ωwdot_x of the virtual wheel 62_x is used as an operation amount (control input) for controlling the motion of the vehicle system center of gravity point viewed from the Y-axis direction, and viewed from the X-axis direction. The rotational angular acceleration ωwdot_y of the virtual wheel 62_y is used as an operation amount (control input) for controlling the motion of the vehicle system center of gravity.
そして、STEP9の車両制御演算処理を概略的に説明すると、制御ユニット50は、X軸方向で見た質点60_xの運動と、Y軸方向で見た質点60_yの運動とが、車両系重心点の所望の運動に対応する運動となるように、操作量としての上記回転角加速度ωwdot_x,ωwdot_yの指令値(目標値)である仮想車輪回転角加速度指令ωwdot_x_cmd,ωwdot_y_cmdを決定する。さらに、制御ユニット50は、仮想車輪回転角加速度指令ωwdot_x_cmd,ωwdot_y_cmdをそれぞれ積分してなる値を、仮想車輪62_x,62_yのそれぞれの回転角速度ωw_x,ωw_yの指令値(目標値)である仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdとして決定する。
Then, the vehicle control arithmetic processing in
そして、制御ユニット50は、仮想車輪回転角速度指令ωw_x_cmdに対応する仮想車輪62_xの移動速度(=Rw_x・ωw_x_cmd)と、仮想車輪回転角速度指令ωw_y_cmdに対応する仮想車輪62_yの移動速度(=Rw_y・ωw_y_cmd)とを、それぞれ、車両1の車輪体5のX軸方向の目標移動速度、Y軸方向の目標移動速度とし、それらの目標移動速度を実現するように、電動モータ31R,31Lのそれぞれの速度指令ω_R_cmd,ωL_cmdを決定する。
Then, the
なお、本実施形態では、操作量(制御入力)としての上記仮想車輪回転角加速度指令ωwdot_x_cmd,ωwdot_y_cmdは、それぞれ、後述する式07x,07yに示す如く、3個の操作量成分を加え合わせることによって決定される。 In this embodiment, the virtual wheel rotation angular acceleration commands ωwdot_x_cmd and ωwdot_y_cmd as the operation amount (control input) are obtained by adding three operation amount components as shown in equations 07x and 07y described later, respectively. It is determined.
補足すると、本実施形態における操作量(制御入力)としての上記仮想車輪回転角加速度指令ωwdot_x_cmd,ωwdot_y_cmdのうち、ωwdot_x_cmdは、X軸方向に移動する仮想車輪62_xの回転角速度であるから、車輪体5をX軸方向に移動させるために該車輪体5に付与すべき駆動力を規定する操作量として機能するものとなる。また、ωwdot_y_cmdは、Y軸方向に移動する仮想車輪62_yの回転角速度であるから、車輪体5をY軸方向に移動させるために該車輪体5に付与すべき駆動力を規定する操作量として機能するものとなる。
Supplementally, among the virtual wheel rotation angular acceleration commands ωwdot_x_cmd and ωwdot_y_cmd as the operation amount (control input) in the present embodiment, ωwdot_x_cmd is the rotation angular velocity of the virtual wheel 62_x moving in the X-axis direction. This functions as an operation amount that defines the driving force to be applied to the
制御ユニット50は、上記の如き、STEP9の車両制御演算処理を実行するための機能として、図9のブロック図で示す機能を備えている。
The
すなわち、制御ユニット50は、基体傾斜角度計測値θb_xy_sと基体傾斜角度目標値θb_xy_objとの偏差である基体傾斜角度偏差計測値θbe_xy_sを算出する偏差演算部70と、前記車両系重心点の移動速度である重心速度Vb_xyの観測値としての重心速度推定値Vb_xy_sを算出する重心速度算出部72と、電動モータ31R,31Lの回転角速度の許容範囲に応じた制限を加味して、重心速度Vb_xyの目標値としての制御用目標重心速度Vb_xy_mdfdを決定する重心速度制限部76と、後述する式07x,07yのゲイン係数の値を調整するためのゲイン調整パラメータKr_xyを決定するゲイン調整部78とを備える。
That is, the
制御ユニット50は、さらに、前記仮想車輪回転角速度指令ωw_xy_cmdの基本値を算出する姿勢制御演算部80と、車輪体5と床面との間に空転が発生している場合に、この仮想車輪回転角速度指令ωw_xy_cmdを抑制する抑制係数λを決定する空転抑制部81と、この抑制係数λを仮想車輪回転角速度指令ωw_xy_cmdに乗じて修正された仮想車輪回転角速度指令ωw_xy_cmdの値を算出する演算部83と、この演算部83が出力した仮想車輪回転角速度指令ωw_xy_cmdを、右側の電動モータ31Rの速度指令ω_R_cmd(回転角速度の指令値)と左側の電動モータ31Lの速度指令ω_L_cmd(回転角速度の指令値)との組に変換するモータ指令演算部82とを備える。
The
なお、図9中の参照符号84を付したものは、姿勢制御演算部70が制御処理周期毎に算出する仮想車輪回転角速度指令ωw_xy_cmdを入力する遅延要素を示している。該遅延要素84は、各制御処理周期において、仮想車輪回転角速度指令ωw_xy_cmdの前回値ωw_xy_cmd_pを出力する。
Note that the
前記STEP9の車両制御演算処理では、これらの上記の各処理部の処理が以下に説明するように実行される。
In the vehicle control calculation process in
すなわち、制御ユニット50は、まず、偏差演算部70の処理と重心速度算出部72の処理とを実行する。
That is, the
偏差演算部70には、前記STEP2で算出された基体傾斜角度計測値θb_xy_s(θb_x_s及びθb_y_s)と、前記STEP5又はSTEP7で設定された目標値θb_xy_obj(θb_x_obj及びθb_y_obj)とが入力される。そして、偏差演算部70は、θb_x_sからθb_x_objを減算することによって、Y軸周り方向の基体傾斜角度偏差計測値θbe_x_s(=θb_x_s−θb_x_obj)を算出すると共に、θb_y_sからθb_y_objを減算することによって、X軸周り方向の基体傾斜角度偏差計測値θbe_y_s(=θb_y_s−θb_y_obj)を算出する。
The
なお、偏差演算部70の処理は、STEP9の車両制御演算処理の前に行なうようにしてもよい。例えば、前記STEP5又は7の処理の中で、偏差演算部70の処理を実行してもよい。
The process of the
前記重心速度算出部72には、前記STEP2で算出された基体傾斜角速度計測値θbdot_xy_s(θbdot_x_s及びθbdot_y_s)の今回値が入力されると共に、仮想車輪速度指令ωw_xy_cmdの前回値ωw_xy_cmd_p(ωw_x_cmd_p及びωw_y_cmd_p)が遅延要素84から入力される。そして、重心速度算出部72は、これらの入力値から、前記倒立振子モデルに基づく所定の演算式によって、重心速度推定値Vb_xy_s(Vb_x_s及びVb_y_s)を算出する。
The center-of-gravity
具体的には、重心速度算出部72は、次式05x,05yにより、Vb_x_s及びVb_y_sをそれぞれ算出する。
Specifically, the center-of-gravity
Vb_x_s=Rw_x・ωw_x_cmd_p+h_x・θbdot_x_s ……05x
Vb_y_s=Rw_y・ωw_y_cmd_p+h_y・θbdot_y_s ……05y
これらの式05x,05yにおいて、Rw_x,Rw_yは、前記したように、仮想車輪62_x,62_yのそれぞれの半径であり、これらの値は、予め設定された所定値である。また、h_x,h_yは、それぞれ倒立振子モデルの質点60_x,60_yの高さである。この場合、本実施形態では、車両系重心点の高さは、ほぼ一定に維持されるものとされる。そこで、h_x,h_yの値としては、それぞれ、予め設定された所定値が用いられる。補足すると、高さh_x,h_yは、前記STEP6又は8において値を設定する定数パラメータに含まれるものである。
Vb_x_s = Rw_x · ωw_x_cmd_p + h_x · θbdot_x_s ...... 05x
Vb_y_s = Rw_y · ωw_y_cmd_p + h_y · θbdot_y_s …… 05y
In these expressions 05x and 05y, as described above, Rw_x and Rw_y are the radii of the virtual wheels 62_x and 62_y, and these values are predetermined values set in advance. H_x and h_y are the heights of the mass points 60_x and 60_y of the inverted pendulum model, respectively. In this case, in the present embodiment, the height of the vehicle system center-of-gravity point is maintained substantially constant. Therefore, predetermined values set in advance are used as the values of h_x and h_y, respectively. Supplementally, the heights h_x and h_y are included in the constant parameters whose values are set in STEP 6 or 8.
上記式05xの右辺の第1項は、仮想車輪62_xの速度指令の前回値ωw_x_cmd_pに対応する該仮想車輪62_xのX軸方向の移動速度であり、この移動速度は、車輪体5のX軸方向の実際の移動速度の現在値に相当するものである。また、式05xの右辺の第2項は、基体9がY軸周り方向にθbdot_x_sの傾斜角速度で傾動することに起因して生じる車両系重心点のX軸方向の移動速度(車輪体5に対する相対的な移動速度)の現在値に相当するものである。これらのことは、式05yについても同様である。
The first term on the right side of the formula 05x is the moving speed in the X-axis direction of the virtual wheel 62_x corresponding to the previous value ωw_x_cmd_p of the speed command of the virtual wheel 62_x, and this moving speed is the X-axis direction of the
なお、前記ロータリエンコーダ56R,56Lの出力を基に計測される電動モータ31R,31Lのそれぞれの回転角速度の計測値(今回値)の組を、仮想車輪62_x,62_yのそれぞれの回転角速度の組に変換し、それらの回転角速度を、式05x、05yのωw_x_cmd_p、ωw_y_cmd_pの代わりに用いてもよい。ただし、回転角速度の計測値に含まれるノイズの影響を排除する上では、目標値であるωw_x_cmd_p、ωw_y_cmd_pを使用することが有利である。
Note that a set of measured values (current values) of the respective rotational angular velocities of the
次に、制御ユニット50は、重心速度制限部76の処理とゲイン調整部78の処理とを実行する。この場合、重心速度制限部76及びゲイン調整部78には、それぞれ、重心速度算出部72で上記の如く算出された重心速度推定値Vb_xy_s(Vb_x_s及びVb_y_s)が入力される。
Next, the
そして、ゲイン調整部78は、入力された重心速度推定値Vb_xy_s(Vb_x_s及びVb_y_s)を基に、前記ゲイン調整パラメータKr_xy(Kr_x及びKr_y)を決定する。
Then, the
このゲイン調整部78の処理を図10及び図11を参照して以下に説明する。
The processing of the
図10に示すように、ゲイン調整部78は、入力された重心速度推定値Vb_x_s,Vb_y_sをリミット処理部86に入力する。このリミット処理部86では、重心速度推定値Vb_x_s,Vb_y_sに、電動モータ31R,31Lのそれぞれの回転角速度の許容範囲に応じた制限を適宜、加えることによって、出力値Vw_x_lim1,Vw_y_lim1を生成する。出力値Vw_x_lim1は、前記仮想車輪62_xのX軸方向の移動速度Vw_xの制限後の値、出力値Vw_y_lim1は、前記仮想車輪62_yのY軸方向の移動速度Vw_yの制限後の値としての意味を持つ。
As shown in FIG. 10, the
このリミット処理部86の処理を、図11を参照してさらに詳細に説明する。なお、図11中の括弧付きの参照符号は、後述する重心速度制限部76のリミット処理部104の処理を示すものであり、リミット処理部86の処理に関する説明では無視してよい。
The processing of the
リミット処理部86は、まず、重心速度推定値Vb_x_s,Vb_y_sをそれぞれ処理部86a_x,86a_yに入力する。処理部86a_xは、Vb_x_sを仮想車輪62_xの半径Rw_xで除算することによって、仮想車輪62_xのX軸方向の移動速度をVb_x_sに一致させたと仮定した場合の該仮想車輪62_xの回転角速度ωw_x_sを算出する。同様に、処理部86a_yは、仮想車輪62_yのY軸方向の移動速度をVb_y_sに一致させたと仮定した場合の該仮想車輪62_yの回転角速度ωw_y_s(=Vb_y_s/Rw_y)を算出する。
First, the
次いで、リミット処理部86は、ωw_x_s,ωw_y_sの組を、XY−RL変換部86bにより、電動モータ31Rの回転角速度ω_R_sと電動モータ31Lの回転角速度ω_L_sとの組に変換する。
Next, the
この変換は、本実施形態では、前記式01a,01bのωw_x,ωw_y,ω_R,ω_Lをそれぞれ、ωw_x_s,ωw_y_s,ω_R_s,ω_L_sに置き換えて得られる連立方程式を、ω_R_s,ω_L_sを未知数として解くことにより行なわれる。 In this embodiment, in this embodiment, simultaneous equations obtained by substituting ωw_x, ωw_y, ω_R, and ω_L in equations 01a and 01b with ωw_x_s, ωw_y_s, ω_R_s, and ω_L_s, respectively, are solved by using ω_R_s and ω_L_s as unknowns. Done.
次いで、リミット処理部86は、XY−RL変換部86bの出力値ω_R_s,ω_L_sをそれぞれ、リミッタ86c_R,86c_Lに入力する。このとき、リミッタ86c_Rは、ω_R_sが、予め設定された所定値の上限値(>0)と下限値(<0)とを有する右モータ用許容範囲内に収まっている場合には、ω_R_sをそのまま出力値ω_R_lim1として出力する。また、リミッタ86c_Rは、ω_R_sが、右モータ用許容範囲から逸脱している場合には、該右モータ用許容範囲の上限値と下限値とのうちのω_R_sに近い方の境界値を出力値ω_R_lim1として出力する。これにより、リミッタ86c_Rの出力値ω_R_lim1は、右モータ用許容範囲内の値に制限される。
Next, the
同様に、リミッタ86c_Lは、ω_L_sが、予め設定された所定値の上限値(>0)と下限値(<0)とを有する左モータ用許容範囲内に収まっている場合には、ω_L_sをそのまま出力値ω_L_lim1として出力する。また、リミッタ86c_Lは、ω_L_sが、左モータ用許容範囲から逸脱している場合には、該左モータ用許容範囲の上限値と下限値とのうちのω_L_sに近い方の境界値を出力値ω_L_lim1として出力する。これにより、リミッタ86c_Lの出力値ω_L_lim1は、左モータ用許容範囲内の値に制限される。 Similarly, when the limiter 86c_L is within the allowable range for the left motor having a predetermined upper limit value (> 0) and lower limit value (<0), the limiter 86c_L keeps ω_L_s as it is. Output as output value ω_L_lim1. Further, when ω_L_s deviates from the left motor allowable range, the limiter 86c_L outputs the boundary value closer to ω_L_s between the upper limit value and the lower limit value of the left motor allowable range as the output value ω_L_lim1. Output as. As a result, the output value ω_L_lim1 of the limiter 86c_L is limited to a value within the left motor allowable range.
上記右モータ用許容範囲は右側の電動モータ31Rの回転角速度(絶対値)が高くなり過ぎないようにし、ひいては、電動モータ31Rが出力可能なトルクの最大値が低下するのを防止するために設定された許容範囲である。このことは、左モータ用許容範囲についても同様である。
The allowable range for the right motor is set so that the rotational angular velocity (absolute value) of the right
次いで、リミット処理部86は、リミッタ86c_R,86c_Lのそれぞれの出力値ω_R_lim1,ω_L_lim1の組を、RL−XY変換部86dにより、仮想車輪62_x,62_yのそれぞれの回転角速度ωw_x_lim1,ωw_y_lim1の組に変換する。
Next, the
この変換は、前記XY−RL変換部86bの変換処理の逆変換の処理である。この処理は、前記式01a,01bのωw_x,ωw_y,ω_R,ω_Lをそれぞれ、ωw_x_lim1,ωw_y_lim1,ω_R_lim1,ω_L_lim1に置き換えて得られる連立方程式を、ωw_x_lim1,ωw_y_lim1を未知数として解くことにより行なわれる。
This conversion is a reverse conversion process of the conversion process of the XY-
次いで、リミット処理部86は、RL−XY変換部86dの出力値ωw_x_lim1,ωw_y_lim1をそれぞれ処理部86e_x,86e_yに入力する。処理部86e_xは、ωw_x_lim1に仮想車輪62_xの半径Rw_xを乗じることによって、ωw_x_lim1を仮想車輪62_xの移動速度Vw_x_lim1に変換する。同様に、処理部86e_yは、ωw_y_lim1を仮想車輪62_yの移動速度Vw_y_lim1(=ωw_y_lim1・Rw_y)に変換する。
Next, the
以上のリミット処理部86の処理によって、仮想車輪62_xのX軸方向の移動速度Vw_xと、仮想車輪62_yのY軸方向の移動速度Vw_yとをそれぞれ重心速度推定値Vb_x_s,Vb_y_sに一致させたと仮定した場合(換言すれば、車輪体5のX軸方向の移動速度とY軸方向の移動速度とをそれぞれ、Vb_x_s,Vb_y_sに一致させたと仮定した場合)に、それらの移動速度を実現するために必要な電動モータ31R,31Lのそれぞれの回転角速度ω_R_s,ω_L_sが、両方とも、許容範囲内に収まっている場合には、Vb_x_s,Vb_y_sにそれぞれ一致する出力値Vw_x_lim1,Vw_y_lim1の組がリミット処理部86から出力される。
It is assumed that the movement speed Vw_x of the virtual wheel 62_x in the X-axis direction and the movement speed Vw_y of the virtual wheel 62_y in the Y-axis direction are made to coincide with the center-of-gravity speed estimated values Vb_x_s and Vb_y_s, respectively, by the above processing of the
一方、電動モータ31R,31Lのそれぞれの回転角速度ω_R_s,ω_L_sの両方又は一方が許容範囲から逸脱している場合には、その両方又は一方の回転角速度が強制的に許容範囲内に制限された上で、その制限後の電動モータ31R,31Lのそれぞれの回転角速度ω_R_lim1,ω_L_lim1の組に対応する、X軸方向及びY軸方向の移動速度Vw_x_lim1,Vw_y_lim1の組がリミット処理部86から出力される。
On the other hand, when both or one of the rotational angular velocities ω_R_s and ω_L_s of the
従って、リミット処理部86は、その出力値Vw_x_lim1,Vw_y_lim1の組に対応する電動モータ31R,31Lのそれぞれの回転角速度が許容範囲を逸脱しないことを必須の必要条件として、その必要条件下で可能な限り、出力値Vw_x_lim1,Vw_y_lim1をそれぞれVb_x_s,Vb_y_sに一致させるように、出力値Vw_x_lim1,Vw_y_lim1の組を生成する。
Therefore, the
図10の説明に戻って、ゲイン調整部78は、次に、演算部88_x,88_yの処理を実行する。演算部88_xには、X軸方向の重心速度推定値Vb_x_sと、リミット処理部86の出力値Vw_x_lim1とが入力される。そして、演算部88_xは、Vw_x_lim1からVb_x_sを減算してなる値Vover_xを算出して出力する。また、演算部88_yには、Y軸方向の重心速度推定値Vb_y_sと、リミット処理部86の出力値Vw_y_lim1とが入力される。そして、演算部88_yは、Vw_y_lim1からVb_y_sを減算してなる値Vover_yを算出して出力する。
Returning to the description of FIG. 10, the
この場合、リミット処理部86での出力値Vw_x_lim1,Vw_y_lim1の強制的な制限が行なわれなかった場合には、Vw_x_lim1=Vb_x_s、Vw_y_lim1=Vb_y_sとなるので、演算部88_x,88_yのそれぞれの出力値Vover_x,Vover_yはいずれも“0”となる。
In this case, if the output values Vw_x_lim1 and Vw_y_lim1 are not forcibly limited by the
一方、リミット処理部86の出力値Vw_x_lim1,Vw_y_lim1が、入力値Vb_x_s,Vb_y_sに対して強制的な制限を施して生成された場合には、Vw_x_lim1のVb_x_sからの修正量(=Vw_x_lim1−Vb_x_s)と、Vw_y_lim1のVb_y_sからの修正量(=Vw_y_lim1−Vb_y_s)とがそれぞれ、演算部88_x,88_yから出力される。
On the other hand, when the output values Vw_x_lim1 and Vw_y_lim1 of the
次いで、ゲイン調整部78は、演算部88_xの出力値Vover_xを処理部90_x,92_xに順番に通すことによって、ゲイン調整パラメータKr_xを決定する。また、ゲイン調整部78は、演算部88_yの出力値Vover_yを処理部90_y,92_yに順番に通すことによって、ゲイン調整パラメータKr_yを決定する。なお、ゲイン調整パラメータKr_x,Kr_yは、いずれも“0”から“1”までの範囲内の値である。
Next, the
上記処理部90_xは、入力されるVover_xの絶対値を算出して出力する。また、処理部92_xは、その出力値Kr_xが入力値|Vover_x|に対して単調に増加し、且つ、飽和特性を有するようにKr_xを生成する。該飽和特性は、入力値がある程度大きくなると、入力値の増加に対する出力値の変化量が“0”になるか、もしくは、“0”に近づく特性である。 The processing unit 90_x calculates and outputs the absolute value of the input Vover_x. Further, the processing unit 92_x generates Kr_x so that the output value Kr_x monotonously increases with respect to the input value | Vover_x | and has a saturation characteristic. The saturation characteristic is a characteristic in which the change amount of the output value with respect to the increase of the input value becomes “0” or approaches “0” when the input value increases to some extent.
この場合、本実施形態では、処理部92_xは、入力値|Vover_x|が予め設定された所定値以下である場合には、該入力値|Vover_x|に所定値の比例係数を乗じてなる値をKr_xとして出力する。また、処理部92_xは、入力値|Vover_x|が所定値よりも大きい場合には、“1”をKr_xとして出力する。なお、上記比例係数は、|Vover_x|が所定値に一致するときに、|Vover_x|と比例係数との積が“1”になるように設定されている。 In this case, in this embodiment, when the input value | Vover_x | is equal to or less than a predetermined value, the processing unit 92_x sets a value obtained by multiplying the input value | Vover_x | by a proportional coefficient of the predetermined value. Output as Kr_x. In addition, when the input value | Vover_x | is larger than a predetermined value, the processing unit 92_x outputs “1” as Kr_x. The proportional coefficient is set so that the product of | Vover_x | and the proportional coefficient is “1” when | Vover_x | matches a predetermined value.
また、処理部90_y,92_yの処理は、それぞれ上記した処理部90_x,92_xの処理と同様である。 The processing of the processing units 90_y and 92_y is the same as the processing of the above-described processing units 90_x and 92_x, respectively.
以上説明したゲイン調整部78の処理によって、リミット処理部86での出力値Vw_x_lim1,Vw_y_lim1の強制的な制限が行なわれなかった場合、すなわち、車輪体5のX軸方向及びY軸方向のそれぞれの移動速度Vw_x,Vw_yを、それぞれ、重心速度推定値Vb_x_s,Vb_y_sに一致させるように電動モータ31R,31Lを動作させても、電動モータ31R,31Lのそれぞれの回転角速度が許容範囲内に収まるような場合には、ゲイン調整パラメータKr_x,Kr_yはいずれも“0”に決定される。従って、通常は、Kr_x=Kr_y=0である。
When the output values Vw_x_lim1 and Vw_y_lim1 are not forcibly limited by the
一方、リミット処理部86の出力値Vw_x_lim1,Vw_y_lim1が、入力値Vb_x_s,Vb_y_sに対して強制的な制限を施して生成された場合、すなわち、車輪体5のX軸方向及びY軸方向のそれぞれの移動速度Vw_x,Vw_yを、それぞれ、重心速度推定値Vb_x_s,Vb_y_sに一致させるように電動モータ31R,31Lを動作させると、電動モータ31R,31Lのいずれかの回転角速度が許容範囲を逸脱してしまう場合(いずれかの回転角速度の絶対値が高くなり過ぎる場合)には、前記修正量Vover_x,Vover_yのそれぞれの絶対値に応じて、ゲイン調整パラメータKr_x,Kr_yの値がそれぞれ決定される。この場合、Kr_xは、“1”を上限値して、修正量Vx_overの絶対値が大きいほど、大きな値になるように決定される。このことは、Kr_yについても同様である。
On the other hand, when the output values Vw_x_lim1 and Vw_y_lim1 of the
また、前記重心速度制限部76は、入力された重心速度推定値Vb_xy_s(Vb_x_s及びVb_y_s)を使用して、図12のブロック図で示す処理を実行することによって、制御用目標重心速度Vb_xy_mdfd(Vb_x_mdfd及びVb_y_mdfd)を決定する。
Further, the center-of-
具体的には、重心速度制限部76は、まず、定常偏差算出部94_x,94_yの処理を実行する。
Specifically, the center-of-gravity
この場合、定常偏差算出部94_xには、X軸方向の重心速度推定値Vb_x_sが入力されると共に、X軸方向の制御用目標重心速度Vb_x_mdfdの前回値Vb_x_mdfd_pが遅延要素96_xを介して入力される。そして、定常偏差算出部94_xは、まず、入力されるVb_x_sが比例・微分補償要素(PD補償要素)94a_xに入力する。この比例・微分補償要素94_xは、その伝達関数が1+Kd・Sにより表される補償要素であり、入力されるVb_x_sと、その微分値(時間的変化率)に所定値の係数Kdを乗じてなる値とを加算し、その加算結果の値を出力する。 In this case, the steady-state deviation calculating unit 94_x receives the estimated center-of-gravity velocity value Vb_x_s in the X-axis direction and the previous value Vb_x_mdfd_p of the control target center-of-gravity velocity Vb_x_mdfd in the X-axis direction via the delay element 96_x. . The steady deviation calculating unit 94_x first inputs the input Vb_x_s to the proportional / differential compensation element (PD compensation element) 94a_x. The proportional / differential compensation element 94_x is a compensation element whose transfer function is represented by 1 + Kd · S, and is obtained by multiplying the input Vb_x_s and its differential value (time change rate) by a predetermined coefficient Kd. Add the value and output the result of the addition.
次いで、定常偏差算出部94_xは、入力されるVb_x_mdfd_pを、比例・微分補償要素94_xの出力値から減算してなる値を演算部94b_xにより算出した後、この演算部94b_xの出力値を、位相補償機能を有するローパスフィルタ94c_xに入力する。このローパスフィルタ94c_xは、伝達関数が(1+T2・S)/(1+T1・S)により表されるフィルタである。そして、定常偏差算出部94_xは、このローパスフィルタ94c_xの出力値Vb_x_prdを出力する。 Next, the steady deviation calculating unit 94_x calculates a value obtained by subtracting the input Vb_x_mdfd_p from the output value of the proportional / differential compensation element 94_x by the calculating unit 94b_x, and then outputs the output value of the calculating unit 94b_x to the phase compensation It inputs into the low-pass filter 94c_x which has a function. The low-pass filter 94c_x is a filter whose transfer function is represented by (1 + T2 · S) / (1 + T1 · S). The steady deviation calculating unit 94_x outputs the output value Vb_x_prd of the low-pass filter 94c_x.
また、定常偏差算出部94_yには、Y軸方向の重心速度推定値Vb_y_sが入力されると共に、Y軸方向の制御用目標重心速度Vb_y_mdfdの前回値Vb_y_mdfd_pが遅延要素96_yを介して入力される。 In addition, the steady-state deviation calculating unit 94_y receives the Y-axis centroid speed estimated value Vb_y_s and the previous value Vb_y_mdfd_p of the Y-axis control target centroid speed Vb_y_mdfd via the delay element 96_y.
そして、定常偏差算出部94_yは、上記した定常偏差算出部94_xと同様に、比例・微分補償要素94a_y、演算部94b_y及びローパスフィルタ94c_yの処理を順次実行し、ローパスフィルタ94c_yの出力値Vb_y_prdを出力する。 Then, similarly to the above-described steady deviation calculation unit 94_x, the steady deviation calculation unit 94_y sequentially executes the processing of the proportional / differential compensation element 94a_y, the calculation unit 94b_y, and the low-pass filter 94c_y, and outputs the output value Vb_y_prd of the low-pass filter 94c_y. To do.
ここで、定常偏差算出部94_xの出力値Vb_x_prdは、Y軸方向から見た車両系重心点の現在の運動状態(換言すればY軸方向から見た倒立振子モデルの質点60_xの運動状態)から推測される、将来のX軸方向の重心速度推定値の収束予測値の制御用目標重心速度Vb_x_mdfdに対する定常偏差としての意味を持つものである。同様に、定常偏差算出部94_y出力値Vb_y_prdは、X軸方向から見た車両系重心点の現在の運動状態(換言すればX軸方向から見た倒立振子モデルの質点60_yの運動状態)から推測される、将来のY軸方向の重心速度推定値の収束予測値の制御用目標重心速度Vb_y_mdfdに対する定常偏差としての意味を持つものである。以降、定常偏差算出部94_x,94_yのそれぞれの出力値Vb_x_prd,Vb_y_prdを重心速度定常偏差予測値という。 Here, the output value Vb_x_prd of the steady deviation calculating unit 94_x is based on the current motion state of the vehicle system center of gravity as viewed from the Y-axis direction (in other words, the motion state of the mass point 60_x of the inverted pendulum model as viewed from the Y-axis direction). It has a meaning as a steady deviation with respect to the control target center-of-gravity speed Vb_x_mdfd of the estimated predicted center-of-gravity speed estimated value in the X-axis direction. Similarly, the steady deviation calculating unit 94_y output value Vb_y_prd is estimated from the current motion state of the vehicle system center of gravity as viewed from the X-axis direction (in other words, the motion state of the mass point 60_y of the inverted pendulum model as viewed from the X-axis direction). The convergence predicted value of the estimated center-of-gravity speed value in the future Y-axis direction has a meaning as a steady deviation with respect to the control target center-of-gravity speed Vb_y_mdfd. Hereinafter, the respective output values Vb_x_prd and Vb_y_prd of the steady deviation calculation units 94_x and 94_y are referred to as center-of-gravity velocity steady deviation prediction values.
重心速度制限部76は、上記の如く定常偏差算出部94_x,94_yの処理を実行した後、上記重心速度定常偏差予測値Vb_x_prd,Vb_y_prdをリミット処理部100に入力する。このリミット処理部100の処理は、前記したゲイン調整部78のリミット処理部86の処理と同じである。この場合、図11に括弧付きに参照符号で示す如く、リミット処理部100の各処理部の入力値及び出力値だけがリミット処理部86と相違する。
The center-of-
具体的には、リミット処理部100では、前記仮想車輪62_x,62_yのそれぞれの移動速度Vw_x,Vw_yを、Vb_x_prd,Vb_y_prdにそれぞれ一致させたと仮定した場合の各仮想車輪62_x,62_yの回転角速度ωw_x_t,ωw_y_tがそれぞれ処理部86a_x,86a_yにより算出される。そして、この回転角速度ωw_x_t,ωw_y_tの組が、XY−RL変換部86bにより、電動モータ31R,31Lの回転角速度ω_R_t,ω_L_tの組に変換される。
Specifically, in the
さらに、これらの回転角速度ω_R_t,ω_L_tが、リミッタ86c_R,86c_Lによって、それぞれ、右モータ用許容範囲内の値と左モータ用許容範囲内の値とに制限される。そして、この制限処理後の値ω_R_lim2,ω_L_lim2が、RL−XY変換部86dによって、仮想車輪62_x,62_yの回転角速度ωw_x_lim2,ωw_y_lim2に変換される。
Further, these rotational angular velocities ω_R_t and ω_L_t are limited by the limiters 86c_R and 86c_L to values within the allowable range for the right motor and values within the allowable range for the left motor, respectively. Then, the values ω_R_lim2 and ω_L_lim2 after the restriction process are converted into the rotational angular velocities ωw_x_lim2 and ωw_y_lim2 of the virtual wheels 62_x and 62_y by the RL-
次いで、この各回転角速度ωw_x_lim2,ωw_y_lim2に対応する各仮想車輪62_x,62_yの移動速度Vw_x_lim2,Vw_y_lim2がそれぞれ処理部86e_x,86e_yによって算出され、これらの移動速度Vw_x_lim2,Vw_y_lim2がリミット処理部100から出力される。
Next, the moving speeds Vw_x_lim2 and Vw_y_lim2 of the virtual wheels 62_x and 62_y corresponding to the rotational angular velocities ωw_x_lim2 and ωw_y_lim2 are calculated by the processing units 86e_x and 86e_y, respectively, and the moving speeds Vw_x_lim2 and Vw_y_lim2 are output from the
以上のリミット処理部100の処理によって、リミット処理部100は、リミット処理部86と同様に、その出力値Vw_x_lim2,Vw_y_lim2の組に対応する電動モータ31R,31Lのそれぞれの回転角速度が許容範囲を逸脱しないことを必須の必要条件として、その必要条件下で可能な限り、出力値Vw_x_lim2,Vw_y_lim2をそれぞれVb_x_t,Vb_y_tに一致させるように、出力値Vw_x_lim2,Vw_y_lim2の組を生成する。
By the above processing of the
なお、リミット処理部100における右モータ用及び左モータ用の各許容範囲は、リミット処理部86における各許容範囲と同一である必要はなく、互いに異なる許容範囲に設定されていてもよい。
Note that the permissible ranges for the right motor and the left motor in the
図12の説明に戻って、重心速度制限部76は、次に、演算部102_x,102_yの処理を実行することによって、それぞれ制御用目標重心速度Vb_x_mdfd,Vb_y_mdfdを算出する。この場合、演算部102_xは、リミット処理部100の出力値Vw_x_lim2から、X軸方向の重心速度定常偏差予測値Vb_x_prdを減算してなる値をX軸方向の制御用目標重心速度Vb_x_mdfdとして算出する。同様に、演算部102_yは、リミット処理部100の出力値Vw_y_lim2から、Y軸方向の重心速度定常偏差予測値Vb_y_prdを減算してなる値をY軸方向の制御用目標重心速度Vb_y_mdfdとして算出する。
Returning to the description of FIG. 12, the center-of-gravity
以上のようにして決定される制御用目標重心速度Vb_x_mdfd,Vb_y_mdfdは、リミット処理部100での出力値V_x_lim2,V_y_lim2の強制的な制限が行なわれなかった場合、すなわち、車輪体5のX軸方向及びY軸方向のそれぞれの移動速度を、それぞれ、重心速度定常偏差予測値Vb_x_prd,Vb_y_prdに一致させるように電動モータ31R,31Lを動作させても、電動モータ31R,31Lのそれぞれの回転角速度が許容範囲内に収まるような場合には、制御用目標重心速度Vb_x_mdfd,Vb_y_mdfdはいずれも“0”に決定される。従って、通常は、Vb_x_mdfd=Vb_y_mdfd=0である。
The control target center-of-gravity velocities Vb_x_mdfd and Vb_y_mdfd determined as described above are obtained when the output values V_x_lim2 and V_y_lim2 in the
一方、リミット処理部100の出力値Vw_x_lim2,Vw_y_lim2が、入力値Vb_x_t,Vb_y_tに対して強制的な制限を施して生成された場合、すなわち、車輪体5のX軸方向及びY軸方向のそれぞれの移動速度を、それぞれ、重心速度定常偏差予測値Vb_x_prd,Vb_y_prdに一致させるように電動モータ31R,31Lを動作させると、電動モータ31R,31Lのいずれかの回転角速度が許容範囲を逸脱してしまう場合(いずれかの回転角速度の絶対値が高くなり過ぎる場合)には、X軸方向については、リミット処理部100の出力値Vw_x_lim2の入力値Vb_x_prdからの修正量(=Vw_x_lim2−Vb_x_prd)が、X軸方向の制御用目標重心速度Vb_x_mdfdとして決定される。
On the other hand, when the output values Vw_x_lim2 and Vw_y_lim2 of the
また、Y軸方向については、リミット処理部100の出力値Vw_y_lim2の入力値Vb_y_prdからの修正量(=Vw_y_lim2−Vb_y_prd)が、Y軸方向の制御用目標重心速度Vb_y_mdfdとして決定される。
For the Y-axis direction, a correction amount (= Vw_y_lim2-Vb_y_prd) from the input value Vb_y_prd of the output value Vw_y_lim2 of the
この場合において、例えばX軸方向の速度に関し、制御用目標重心速度Vb_x_mdfdは、定常偏差算出部94_xが出力するX軸方向の重心速度定常偏差予測値Vb_x_prdと逆向きの速度となる。このことは、Y軸方向の速度に関しても同様である。 In this case, for example, regarding the speed in the X-axis direction, the control target center-of-gravity speed Vb_x_mdfd is a speed opposite to the X-axis direction center-of-gravity speed steady deviation predicted value Vb_x_prd output by the steady deviation calculating unit 94_x. The same applies to the speed in the Y-axis direction.
以上が、重心速度制限部76の処理である。
The above is the process of the gravity center
図9の説明に戻って、制御ユニット50は、以上の如く重心速度制限部76、ゲイン調整部78、及び偏差演算部70の処理を実行した後、次に、姿勢制御演算部の処理を実行する。
Returning to the description of FIG. 9, the
この姿勢制御演算部80の処理を、以下に図13を参照して説明する。なお、図13において、括弧を付していない参照符号は、X軸方向に輪転する仮想車輪62_xの回転角速度の目標値である前記仮想車輪回転角速度指令ωw_x_cmdを決定する処理に係わる参照符号であり、括弧付きの参照符合は、Y軸方向に輪転する仮想車輪62_yの回転角速度の目標値である前記仮想車輪回転角速度指令ωw_y_cmdを決定する処理に係わる参照符号である。
The processing of the attitude
姿勢制御演算部80には、偏差演算部70で算出された基体傾斜角度偏差計測値θbe_xy_sと、前記STEP2で算出された基体傾斜角速度計測値θbdot_xy_sと、重心速度算出部72で算出された重心速度推定値Vb_xy_sと、重心速度制限部76で算出された制御用目標重心速度Vb_xy_mdfdと、ゲイン調整部78で算出されたゲイン調整パラメータKr_xyとが入力される。
The posture
そして、姿勢制御演算部80は、まず、これらの入力値を用いて、次式07x,07yにより、仮想車輪回転角加速度指令ωdotw_xy_cmdを算出する。
The attitude
ωwdot_x_cmd=K1_x・θbe_x_s+K2_x・θbdot_x_s
+K3_x・(Vb_x_s−Vb_x_mdfd) ……式07x
ωwdot_y_cmd=K1_y・θbe_y_s+K2_y・θbdot_y_s
+K3_y・(Vb_y_s−Vb_y_mdfd) ……式07y
従って、本実施形態では、Y軸方向から見た倒立振子モデルの質点60_xの運動(ひいては、Y軸方向から見た車両系重心点の運動)を制御するための操作量(制御入力)としての仮想車輪回転角加速度指令ωdotw_x_cmdと、X軸方向から見た倒立振子モデルの質点60_yの運動(ひいては、X軸方向から見た車両系重心点の運動)を制御するための操作量(制御入力)としての仮想車輪回転角加速度指令ωdotw_y_cmdとは、それぞれ、3つの操作量成分(式07x,07yの右辺の3つの項)を加え合わせることによって決定される。
ωwdot_x_cmd = K1_x ・ θbe_x_s + K2_x ・ θbdot_x_s
+ K3_x · (Vb_x_s−Vb_x_mdfd) ...... Formula 07x
ωwdot_y_cmd = K1_y ・ θbe_y_s + K2_y ・ θbdot_y_s
+ K3_y · (Vb_y_s−Vb_y_mdfd) …… Formula 07y
Therefore, in the present embodiment, as an operation amount (control input) for controlling the motion of the mass point 60_x of the inverted pendulum model viewed from the Y-axis direction (and hence the motion of the vehicle system center-of-gravity point viewed from the Y-axis direction). Virtual wheel rotation angular acceleration command ωdotw_x_cmd and operation amount (control input) for controlling the motion of the mass point 60_y of the inverted pendulum model viewed from the X-axis direction (and hence the motion of the center of gravity of the vehicle system viewed from the X-axis direction) The virtual wheel rotation angular acceleration command ωdotw_y_cmd is determined by adding three manipulated variable components (three terms on the right side of equations 07x and 07y).
これらの式07x,07yにおけるゲイン係数K1_x,K1_yは、基体9(又はシート3)の傾斜角度に関するフィードバックゲイン、ゲイン係数K2_x,K2_yは、基体9(又はシート3)の傾斜角速度(傾斜角度の時間的変化率)に関するフィードバックゲイン、ゲイン係数K3_x,K3_yは、車両系重心点(車両1の所定の代表点)の移動速度に関するフィードバックゲインとしての意味を持つものである。 In these equations 07x and 07y, the gain coefficients K1_x and K1_y are feedback gains related to the inclination angle of the base 9 (or the sheet 3), and the gain coefficients K2_x and K2_y are the inclination angular velocities (time of the inclination angle) of the base 9 (or the sheet 3). The feedback gain and gain coefficients K3_x and K3_y related to the dynamic change rate have a meaning as a feedback gain related to the moving speed of the vehicle system center-of-gravity point (a predetermined representative point of the vehicle 1).
この場合、式07xにおける各操作量成分に係わるゲイン係数K1_x,K2_x,K3_xは、ゲイン調整パラメータKr_xに応じて可変的に設定され、式07yにおける各操作量成分に係わるゲイン係数K1_y,K2_y,K3_yは、ゲイン調整パラメータKr_yに応じて可変的に設定される。以降、式07xにおけるゲイン係数K1_x,K2_x,K3_xのそれぞれを第1ゲイン係数K1_x、第2ゲイン係数K2_x、第3ゲイン係数K3_xということがある。このことは、式07yにおけるゲイン係数K1_y,K2_y,K3_yについても同様とする。 In this case, the gain coefficients K1_x, K2_x, K3_x related to each manipulated variable component in the expression 07x are variably set according to the gain adjustment parameter Kr_x, and the gain coefficients K1_y, K2_y, K3_y related to each manipulated variable component in the expression 07y. Is variably set according to the gain adjustment parameter Kr_y. Hereinafter, the gain coefficients K1_x, K2_x, and K3_x in Expression 07x may be referred to as a first gain coefficient K1_x, a second gain coefficient K2_x, and a third gain coefficient K3_x, respectively. The same applies to the gain coefficients K1_y, K2_y, and K3_y in Expression 07y.
式07xにおける第iゲイン係数Ki_x(i=1,2,3)と、式07yにおける第iゲイン係数Ki_y(i=1,2,3)とは、図13中にただし書きで示した如く、次式09x、09yにより、ゲイン調整パラメータKr_x,Kr_yに応じて決定される。 The i-th gain coefficient Ki_x (i = 1, 2, 3) in Expression 07x and the i-th gain coefficient Ki_y (i = 1, 2, 3) in Expression 07y are as follows. It is determined according to the gain adjustment parameters Kr_x and Kr_y by the equations 09x and 09y.
Ki_x=(1−Kr_x)・Ki_a_x+Kr_x・Ki_b_x ……式09x
Ki_y=(1−Kr_y)・Ki_a_y+Kr_y・Ki_b_y ……式09y
(i=1,2,3)
ここで、式09xにおけるKi_a_x、Ki_b_xは、それぞれ、第iゲイン係数Ki_xの最小側(“0”に近い側)のゲイン係数値、最大側(“0”から離れる側)のゲイン係数値として予め設定された定数値である。このことは、式09yにおけるKi_a_y、Ki_b_yについても同様である。
Ki_x = (1−Kr_x) · Ki_a_x + Kr_x · Ki_b_x ...... Expression 09x
Ki_y = (1−Kr_y) · Ki_a_y + Kr_y · Ki_b_y ...... Equation 09y
(I = 1, 2, 3)
Here, Ki_a_x and Ki_b_x in Expression 09x are preliminarily set as the gain coefficient value on the minimum side (side closer to “0”) and the gain coefficient value on the maximum side (side away from “0”) of the i-th gain coefficient Ki_x, respectively. It is a set constant value. The same applies to Ki_a_y and Ki_b_y in Expression 09y.
従って、式07xの演算に用いる各第iゲイン係数Ki_x(i=1,2,3)は、それぞれに対応する定数値Ki_a_x、Ki_b_xの重み付き平均値として決定される。そして、この場合、Ki_a_x、Ki_b_xにそれぞれ掛かる重みが、ゲイン調整パラメータKr_xに応じて変化させられる。このため、Kr_x=0である場合には、Ki_x=Ki_a_xとなり、Kr_x=1である場合には、Ki_x=Ki_b_xとなる。そして、Kr_xが“0”から“1”に近づくに伴い、第iゲイン係数Ki_xはKi_a_xからKi_b_x近づいていく。 Accordingly, each i-th gain coefficient Ki_x (i = 1, 2, 3) used in the calculation of Expression 07x is determined as a weighted average value of the corresponding constant values Ki_a_x and Ki_b_x. In this case, the weights applied to Ki_a_x and Ki_b_x are changed according to the gain adjustment parameter Kr_x. Therefore, when Kr_x = 0, Ki_x = Ki_a_x, and when Kr_x = 1, Ki_x = Ki_b_x. As Kr_x approaches “1” from “0”, the i-th gain coefficient Ki_x approaches Ki_b_x from Ki_a_x.
同様に、式07yの演算に用いる各第iゲイン係数Ki_y(i=1,2,3)は、それぞれに対応する定数値Ki_a_y、Ki_b_yの重み付き平均値として決定される。そして、この場合、Ki_a_y、Ki_b_yにそれぞれ掛かる重みが、ゲイン調整パラメータKr_yに応じて変化させられる。このため、Ki_xの場合と同様に、Kr_yの値が“0”から“1”の間で変化するに伴い、第iゲイン係数Ki_yの値が、Ki_a_yとKi_b_yとの間で変化する。 Similarly, each i-th gain coefficient Ki_y (i = 1, 2, 3) used in the calculation of Expression 07y is determined as a weighted average value of the corresponding constant values Ki_a_y and Ki_b_y. In this case, the weights applied to Ki_a_y and Ki_b_y are changed according to the gain adjustment parameter Kr_y. Therefore, as in the case of Ki_x, as the value of Kr_y changes between “0” and “1”, the value of the i-th gain coefficient Ki_y changes between Ki_a_y and Ki_b_y.
なお、前記したように、Kr_x,Kr_yは、通常は(詳しくはゲイン調整部78のリミット処理部86での出力値Vw_x_lim1,Vw_y_lim1の強制的な制限が行なわれなかった場合)、“0”である。従って、第iゲイン係数Ki_x,Ki_y(i=1,2,3)は、通常は、それぞれ、Ki_x=Ki_a_x,Ki_y=Ki_a_yとなる。
As described above, Kr_x and Kr_y are normally “0” (specifically, when the output values Vw_x_lim1 and Vw_y_lim1 in the
補足すると、上記定数値Ki_a_x、Ki_b_x及びKi_a_y,Ki_b_y(i=1,2,3)は、前記STEP6又は8において値が設定される定数パラメータに含まれるものである。 Supplementally, the constant values Ki_a_x, Ki_b_x and Ki_a_y, Ki_b_y (i = 1, 2, 3) are included in the constant parameters whose values are set in STEP 6 or 8.
また、本実施形態では、Kr_x=0、Kr_y=0となる状況で、式07xに係わる第3ゲイン係数K3_xの絶対値に対する第1ゲイン係数K1_xに対する割合いと、式07yに係わる第3ゲイン係数K3_yの絶対値に対する第1ゲイン係数K1_yに対する割合いとが前記搭乗モードと自立モードとで異なるものとなるが、これについては後述する。 In the present embodiment, in the situation where Kr_x = 0 and Kr_y = 0, the ratio of the third gain coefficient K3_x related to the expression 07x to the first gain coefficient K1_x relative to the absolute value of the third gain coefficient K3_x and the third gain coefficient K3_y related to the expression 07y The ratio of the absolute value to the first gain coefficient K1_y differs between the boarding mode and the independent mode, which will be described later.
姿勢制御演算部80は、上記の如く決定した第1〜第3ゲイン係数K1_x,K2_x,K3_xを用いて前記式07xの演算を行なうことで、X軸方向に輪転する仮想車輪62_xに係わる仮想車輪回転角加速度指令ωwdot_x_cmdを算出する。
The attitude
さらに詳細には、図13を参照して、姿勢制御演算部80は、基体傾斜角度偏差計測値θbe_x_sに第1ゲイン係数K1_xを乗じてなる操作量成分u1_xと、基体傾斜角速度計測値θbdot_x_sに第2ゲイン係数K2_xを乗じてなる操作量成分u2_xとをそれぞれ、処理部80a,80bで算出する。さらに、姿勢制御演算部80は、重心速度推定値Vb_x_sと制御用目標重心速度Vb_x_mdfdとの偏差(=Vb_x_s−Vb_x_mdfd)を演算部80dで算出し、この偏差に第3ゲイン係数K3_xを乗じてなる操作量成u3_xを処理部80cで算出する。そして、姿勢制御演算部80は、これらの操作量成分u1_x,u2_x,u3_xを演算部80eにて加え合わせることにより、仮想車輪回転角加速度指令ωwdot_x_cmdを算出する。
In more detail, with reference to FIG. 13, the posture
同様に、姿勢制御演算部80は、上記の如く決定した第1〜第3ゲイン係数K1_y,K2_y,K3_yを用いて前記式07yの演算を行なうことで、Y軸方向に輪転する仮想車輪62_yに係わる仮想車輪回転角加速度指令ωwdot_y_cmdを算出する。
Similarly, the posture
この場合には、姿勢制御演算部80は、基体傾斜角度偏差計測値θbe_y_sに第1ゲイン係数K1_yを乗じてなる操作量成分u1_yと、基体傾斜角速度計測値θbdot_y_sに第2ゲイン係数K2_yを乗じてなる操作量成分u2_yとをそれぞれ、処理部80a,80bで算出する。さらに、姿勢制御演算部80は、重心速度推定値Vb_y_sと制御用目標重心速度Vb_y_mdfdとの偏差(=Vb_y_s−Vb_y_mdfd)を演算部80dで算出し、この偏差に第3ゲイン係数K3_yを乗じてなる操作量成u3_yを処理部80cで算出する。そして、姿勢制御演算部80は、これらの操作量成分u1_y,u2_y,u3_yを演算部80eにて加え合わせることにより、仮想車輪回転角加速度指令ωwdot_x_cmdを算出する。
In this case, the posture
ここで、式07xの右辺の第1項(=第1操作量成分u1_x)及び第2項(=第2操作量成分u2_x)は、X軸周り方向での基体傾斜角度偏差計測値θbe_x_sを、フィードバック制御則としてのPD則(比例・微分則)により“0”に収束させる(基体傾斜角度計測値θb_x_sを目標値θb_x_objに収束させる)ためのフィードバック操作量成分としての意味を持つ。 Here, the first term (= the first manipulated variable component u1_x) and the second term (= the second manipulated variable component u2_x) on the right side of the expression 07x are the body tilt angle deviation measured values θbe_x_s in the direction around the X axis, It has a meaning as a feedback manipulated variable component for converging to “0” (converging the base body tilt angle measurement value θb_x_s to the target value θb_x_obj) by the PD law (proportional / differential law) as a feedback control law.
また、式07xの右辺の第3項(=第3操作量成分u3_x)は、重心速度推定値Vb_x_sと制御用目標重心速度Vb_x_mdfdとの偏差をフィードバック制御則としての比例則により“0”に収束させる(Vb_x_sをVb_x_mdfdに収束させる)ためのフィードバック操作量成分としての意味を持つ。 In addition, the third term (= third manipulated variable component u3_x) on the right side of Expression 07x converges to “0” by the proportional law as the feedback control law for the deviation between the center of gravity speed estimated value Vb_x_s and the control target center of gravity speed Vb_x_mdfd. It has a meaning as a feedback manipulated variable component for causing Vb_x_s to converge to Vb_x_mdfd.
これらのことは、式07yの右辺の第1〜第3項(第1〜第3操作量成分u1_y,u2_y,u3_y)についても同様である。 The same applies to the first to third terms (first to third manipulated variable components u1_y, u2_y, u3_y) on the right side of Expression 07y.
なお、前記したように通常は(より詳しくは、前記重心速度制限部76のリミット処理部100での出力値V_x_lim2,V_y_lim2の強制的な制限が行なわれなかった場合)、制御用目標重心速度Vb_x_mdfd,Vb_y_mdfdは“0”である。そして、Vb_x_mdfd=Vb_y_mdfd=0となる通常の場合は、第3操作量成分u3_x,u3_yは、それぞれ、重心速度推定値Vb_x_s,Vb_y_sに第3ゲイン係数K3_x,K3_yを乗じた値に一致する。
Normally, as described above (more specifically, when the output values V_x_lim2 and V_y_lim2 are not forcibly limited by the
姿勢制御演算部80は、上記の如く、仮想車輪回転角加速度指令ωwdot_x_cmd,ωwdot_y_cmdを算出した後、次に、これらのωwdot_x_cmd,ωwdot_y_cmdをそれぞれ積分器80fにより積分することによって、前記仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdを決定する。
The attitude
補足すると、式07xの右辺の第3項を、Vb_x_sに応じた操作量成分(=K3_x・Vb_x_s)と、Vb_x_mdfdに応じた操作量成分(=−K3_x・Vb_x_mdfd)とに分離した式によって、仮想車輪回転角加速度指令ωdotw_x_cmdを算出するようにしてよい。同様に、式07yの右辺の第3項を、Vb_y_sに応じた操作量成分(=K3_y・Vb_y_s)と、Vb_y_mdfdに応じた操作量成分(=−K3_y・Vb_y_mdfd)とに分離した式によって、仮想車輪回転角加速度指令ωdotw_y_cmdを算出するようにしてよい。 Supplementally, the third term on the right side of Expression 07x is hypothesized by an expression that is separated into an operation amount component (= K3_x · Vb_x_s) according to Vb_x_s and an operation amount component (= −K3_x · Vb_x_mdfd) according to Vb_x_mdfd. The wheel rotation angular acceleration command ωdotw_x_cmd may be calculated. Similarly, the third term on the right side of the expression 07y is virtually divided into an operation amount component (= K3_y · Vb_y_s) corresponding to Vb_y_s and an operation amount component (= −K3_y · Vb_y_mdfd) corresponding to Vb_y_mdfd. The wheel rotation angular acceleration command ωdotw_y_cmd may be calculated.
また、本実施形態では、車両系重心点の挙動を制御するための操作量(制御入力)として、仮想車輪62_x,62_yの回転角速度指令ωw_x_cmd,ωw_y_cmdを用いるようにしたが、例えば、仮想車輪62_x,62_yの駆動トルク、あるいは、この駆動トルクを各仮想車輪62_x,62_yの半径Rw_x,Rw_yで除算してなる並進力(すなわち仮想車輪62_x,62_yと床面との間の摩擦力)を操作量として用いるようにしてもよい。 In the present embodiment, the rotational angular velocity commands ωw_x_cmd and ωw_y_cmd of the virtual wheels 62_x and 62_y are used as the operation amount (control input) for controlling the behavior of the vehicle system center of gravity. However, for example, the virtual wheels 62_x 62_y or a translational force obtained by dividing the driving torque by the radii Rw_x and Rw_y of the virtual wheels 62_x and 62_y (that is, the frictional force between the virtual wheels 62_x and 62_y and the floor surface). You may make it use as.
以上が姿勢制御演算部80の処理の詳細である。
The above is the details of the processing of the attitude
次に、空転抑制部81の処理の詳細を、図14〜図18を参照して説明する。
Next, details of the processing of the idling
空転抑制部81は、本実施形態では、車輪体5の空転発生を検知し、空転が発生した場合には、基体9(及び基体9と一体に傾斜する搭乗部3)の傾斜角度に応じて車輪体5の移動動作を抑制するよう、抑制係数λを決定する。
In this embodiment, the idling
空転抑制部81は、まず、STEP21の処理を実行する。この処理では、空転抑制部81は、基体9の加速度ab_xyzの観測値に基づいて第1の重心速度予想値Vb_xy_exp1を算出し、基体9の傾斜角速度θbdot_xyzの観測値と仮想車輪回転角速度指令ωw_xy_cmdの前回値ωw_xy_cmd_pに基づいて第2の重心速度予想値Vb_xy_exp2を算出する。
First, the idling
なお、添え字”_xyz”は、Y軸回り成分、X軸回り成分及びZ軸回り成分、又は、X方向成分、Y方向成分及びZ方向成分をまとめて表記する場合に付加する。添え字“_exp”は、変数(状態量)の予想値(予測値)を参照符号により表記する場合に付加する。 The subscript “_xyz” is added when the Y-axis component, the X-axis component, and the Z-axis component, or the X-direction component, the Y-direction component, and the Z-direction component are described together. The subscript “_exp” is added when an expected value (predicted value) of a variable (state quantity) is expressed by a reference sign.
以下、STEP21における第1の重心速度予想値Vb_xy_exp1及び第2の重心速度予想値Vb_xy_exp2の設定方法について説明する。これらの設定は、ストラップダウン方式により、(1)世界座標系における基体傾斜角度θbの推定値θb_xyz_sと、(2)世界座標系における基体加速度abの推定値ab_xyz_sとが算出される。なお、以下に詳細に説明する以外のストラップダウン方式による方法やストラップダウン方式以外の方法により算出してもよい。 Hereinafter, a method for setting the first predicted center-of-gravity speed value Vb_xy_exp1 and the second predicted center-of-gravity speed value Vb_xy_exp2 in STEP 21 will be described. For these settings, (1) an estimated value θb_xyz_s of the base body tilt angle θb in the world coordinate system and (2) an estimated value ab_xyz_s of the base body acceleration ab in the world coordinate system are calculated by the strap-down method. The calculation may be performed by a method using a strap-down method or a method other than the strap-down method other than those described in detail below.
車両座標系は、例えば車両系重心点60_xyのいずれか一方を原点とし、この原点と、仮想車輪62_xの回転軸62a_x及び仮想車輪62_yの回転軸62a_yの交点とを結ぶ線に平行な線をZ軸とし、仮想車輪62_xの回転軸62a_xに平行な線をY軸として定義される。なお、車両1に対して位置が固定され、搭乗部3の傾動に応じて姿勢が変化するような任意の3次元直交座標系が車両座標系として定義されうる。
The vehicle coordinate system has, for example, one of the vehicle system center-of-gravity points 60_xy as an origin, and a line parallel to a line connecting the origin and the intersection of the rotation axis 62a_x of the virtual wheel 62_x and the rotation axis 62a_y of the virtual wheel 62_y is Z. A line parallel to the rotation axis 62a_x of the virtual wheel 62_x is defined as the Y axis. An arbitrary three-dimensional orthogonal coordinate system whose position is fixed with respect to the
添え字”_world”は、変数(状態量)が世界座標系における値である場合に付与され、この添え字がない場合には車両座標系における値であることを意味する。 The subscript “_world” is assigned when the variable (state quantity) is a value in the world coordinate system, and when there is no subscript, it means a value in the vehicle coordinate system.
まず、世界座標系における基体傾斜角度θbの推定値θb_xyz_s_worldの算出方法について、図15を参照して説明する。基体傾斜角度θbの世界座標系における推定値θb_xyz_s_worldの算出に際して、車両座標系におけるY軸、X軸及びZ軸のそれぞれ回りの基体傾斜角速度θbdot_xyz_sと、車両座標系におけるX方向、Y方向及びZ方向のそれぞれについての基体加速度ab_xyz_sとが用いられる。 First, a method of calculating the estimated value θb_xyz_s_world of the base body tilt angle θb in the world coordinate system will be described with reference to FIG. When calculating the estimated value θb_xyz_s_world of the base body tilt angle θb in the world coordinate system, the base body tilt angular velocity θbdot_xyz_s around the Y axis, the X axis, and the Z axis in the vehicle coordinate system, and the X direction, Y direction, and Z direction in the vehicle coordinate system The base acceleration ab_xyz_s for each of the above is used.
なお、車両座標系におけるX軸回りの基体傾斜角速度θbdot_x_s、Y軸回りの基体傾斜角速度θbdot_y_s及びZ軸回りの基体回動角速度(便宜上「基体傾斜角速度」という。)θbdot_z_sのそれぞれは、傾斜センサ52(正確には傾斜センサ52を構成するレートセンサ)の出力信号に基づいて算出される。車両座標系におけるX方向、Y方向及びZ方向のそれぞれについての基体加速度ab_xyz_sは、傾斜センサ52(正確には傾斜センサ52を構成する加速度センサ)の出力信号に基づいて算出される。 In the vehicle coordinate system, the substrate tilt angular velocity θbdot_x_s around the X axis, the substrate tilt angular velocity θbdot_y_s around the Y axis, and the substrate rotation angular velocity around the Z axis (referred to as “substrate tilt angular velocity” for convenience) θbdot_z_s, respectively. It is calculated based on the output signal of the rate sensor (precisely, the rate sensor constituting the tilt sensor 52). The base body acceleration ab_xyz_s in each of the X direction, the Y direction, and the Z direction in the vehicle coordinate system is calculated based on the output signal of the tilt sensor 52 (more precisely, the acceleration sensor that constitutes the tilt sensor 52).
傾斜センサ52、又は、傾斜センサ52を構成する加速度センサ及び角速度センサの車両座標系における並進ベクトルがメモリに保存され、車両座標系におけるY軸、X軸及びZ軸のそれぞれ回りの基体傾斜角速度θbdot_xyz_sと、車両座標系におけるX方向、Y方向及びZ方向のそれぞれについての基体加速度ab_xyz_sとの推定に際して当該並進ベクトルが用いられてもよい。
The translation sensor in the vehicle coordinate system of the
車両−世界座標変換部211により、車両座標系における基体傾斜角速度θbdot_xyz_sに基づき、世界座標系における基体傾斜角速度θbdot_xyz_s_worldが算出される。車両−世界座標変換部221において用いられる、車両座標系から世界座標系への座標変換行列(又はこれと等価なクォータニオン)は、世界座標系における基体傾斜角度θb_xyz_s_worldを遅延要素210に通して得られる結果をオイラー角とする回転行列の逆行列として算出される。
The vehicle-world coordinate
例えば、車両座標系における基体傾斜角速度θbdot_xyz_sを表わすベクトルt(θbdot_x_s,θbdot_y_s,θbdot_z_s)が当該回転行列により変換された結果が、世界座標系における基体傾斜角速度θbdot_xyz_s_worldの各成分を要素とするベクトルt(θbdot_x_s_world,θbdot_y_s_world,θbdot_z_s_world)として算出される。車両−世界座標変換部211において用いられる、車両座標系から世界座標系への座標変換行列は、車両−世界座標変換部221において用いられる回転行列と同じであり、世界座標系における基体傾斜角度θb_xyz_s_worldを遅延要素220に通して得られる結果をオイラー角とする回転行列の逆行列として算出される。。
For example, a vector t (θbdot_x_s, θbdot_y_s, θbdot_z_s) representing the base body tilt angular velocity θbdot_xyz_s in the vehicle coordinate system is converted by the rotation matrix, and the result is a vector t with each component of the base body tilt angular velocity θbdot_xyz_s_world in the world coordinate system as an element ( θbdot_x_s_world, θbdot_y_s_world, θbdot_z_s_world). The coordinate transformation matrix from the vehicle coordinate system to the world coordinate system used in the vehicle-world coordinate
車両−世界座標変換部221により、車両座標系における基体加速度ab_xyz_sに基づき、世界座標系における基体加速度ab_xyz_s_worldが算出される。
The vehicle-world coordinate
例えば、車両座標系における基体加速度ab_xyz_sを表わすベクトルt(ab_x_s,ab_y_s,ab_z_s)が当該回転行列の逆行列により変換された結果が、世界座標系における基体加速度ab_xyz_s_worldの各成分を要素とするベクトルt(ab_x_s_world,ab_y_s_world,ab_z_s_world)として算出される。 For example, a vector t (ab_x_s, ab_y_s, ab_z_s) representing the base acceleration ab_xyz_s in the vehicle coordinate system is transformed by the inverse matrix of the rotation matrix, and a vector t having each component of the base acceleration ab_xyz_s_world in the world coordinate system as an element It is calculated as (ab_x_s_world, ab_y_s_world, ab_z_s_world).
また、演算部222において、世界座標系における基体加速度ab_xyz_s_worldに重力加速度定数の逆数(1/g)が乗じられる。さらに、演算部223において、演算部222による演算結果がarctan関数に入力された結果arctan(ab_xy_s_world /g)が、加速度正接角度θacc_xyzとして算出される。ただし、演算部223により加速度正接角度のZ成分θacc_zとして「0」が出力される。基体加速度ab_xy_s_worldが大きくなるほど、加速度正接角度θacc_xyは大きくなる。演算部224において、世界座標系における基体傾斜角度θb_xyz_s_worldと、加速度正接角度θacc_xyzとの差が算出される。
Further, the
演算部215において、演算部224による演算結果(θb_xyz_s_world−θacc_xyz)と係数Ksd2とが乗算される。演算部212において、世界座標系における基体傾斜角速度θbdot_xyz_s_worldから、演算部215による演算結果Ksd2(θb_xyz_s_world−θacc_xyz)が積分要素216に通された結果(Ksd2/s)・(θb_xyz_s_world−θacc_xyz)が減算される。
In the
演算部217において、演算部224による演算結果(θb_xyz_s_world−θacc_xyz)と、係数Ksd1とが乗算される。演算部213において、演算部212による演算結果θbdot_xyz_s_world−(Ksd2/s)・(θb_xyz_s_world−θacc_xyz)から、演算部217による演算結果Ksd1(θb_xyz_s_world−θacc_xyz)が減算される。そして、演算部213による演算結果が積分要素214に通されることにより、世界座標系における基体傾斜角度推定値θb_xyz_s_worldが算出される。そして、この世界座標系における基体傾斜角度推定値θb_xyz_s_worldが、前述したように、遅延要素210,220に入力される。
In the
世界座標系における基体傾斜角度θb_xy_s_worldと、加速度正接角度θacc_xyとの差に応じて、基体傾斜角速度θbdot_xy_s_worldが補正された結果が積分されることによって基体傾斜角度推定値θb_xy_s_worldが算出される。これは、世界座標系における基体加速度ab_xy_worldに応じて、基体傾斜角度θb_xy_worldの変化態様が変化するためである。 The base body tilt angle estimated value θb_xy_s_world is calculated by integrating the result of correcting the base body tilt angular velocity θbdot_xy_s_world according to the difference between the base body tilt angle θb_xy_s_world and the acceleration tangent angle θacc_xy in the world coordinate system. This is because the change mode of the base body tilt angle θb_xy_world changes according to the base body acceleration ab_xy_world in the world coordinate system.
次に、世界座標系における基体加速度ab_xy_s_worldを用いて第1の重心速度予想値Vb_xy_exp1を算出する方法について、図16(a)を参照して説明する。世界座標系における基体加速度ab_xy_s_worldは、積分要素230を通された後、ローカットフィルタ231において、設定したカットオフ以下の周波数帯域をカットされ、その結果、第1の重心速度予想値Vb_xy_exp1が算出される。
Next, a method for calculating the first predicted center-of-gravity velocity value Vb_xy_exp1 using the base body acceleration ab_xy_s_world in the world coordinate system will be described with reference to FIG. Substrate acceleration ab_xy_s_world in the world coordinate system is passed through
次に、世界座標系における基体傾斜角速度θbdot_xyz_s_worldを用いて第2の重心速度予想値Vb_xy_exp2を算出する方法について、図16(b)を参照して説明する。 Next, a method for calculating the second predicted center-of-gravity velocity value Vb_xy_exp2 using the base body tilt angular velocity θbdot_xyz_s_world in the world coordinate system will be described with reference to FIG.
まず、演算部240において、世界座標系における基体傾斜角速度θbdot_xy_s_worldに質点60_xyの高さh_xy(図8参照)が乗算される。演算部240における演算結果θbdot_xy_s_world・h_xyは、世界座標系における車両1の速度推定値に相当する。
First, the
また、演算部242において、電動モータ31R,31Lの駆動電流の計測値Imot_RL_sに係数Ktが乗算される。ここで、添え字”_RL”は左右成分をまとめて表記すする場合に付与される。演算部242による演算結果Kt・Imot_R_s, Kt・Imot_L_sは、車両座標系における車輪体5のX軸回り及びY軸回りのそれぞれのトルク推定値Trq_R_s,Trq_L_sに相当する。
Further, in the
そして、RL−XY変換部243により、演算部242による演算結果Trq_R_s, Trq_L_sが、次式11a,11bに従って、仮想車輪62_xのトルクTrqw_x_s及び仮想車輪62_yのトルクTrqw_y_sに変換される。
Then, the calculation results Trq_R_s and Trq_L_s by the
Trqw_x=(Trq_R+Trq_L)/2 ……式11a
Trqw_y=C・(Trq_R−Trq_L)/2 ……式11b
ところで、車輪体5とアクチュエータ装置7との間における駆動伝達機構の特性から、X軸方向(前後方向)はトラクションの影響をほとんど受けないので、X軸方向におけるすべりを考慮する必要はない。一方、Y軸方向(左右方向)はそのすべり角θtwistに応じてトラクションの影響を大きく受けるので、補正を行なう必要がある。Y軸方向のおけるトラクションが大きいほど、すべり角θtwistは大きくなり、これは、事実上減速比が大きくなったことに相当する。
Trqw_x = (Trq_R + Trq_L) / 2 …… Equation 11a
Trqw_y = C · (Trq_R−Trq_L) / 2 …… Equation 11b
By the way, because of the characteristics of the drive transmission mechanism between the
そこで、前記式03aを参照して、仮想車輪回転角速度指令値ωw_y_cmdと、電動モータ31R,31Lの回転角速度ω_R_com,ω_L_comとの間には、前記フリーローラ29R,29Lの車輪径Rcontを用いて、次式13の関係が成立する。
Therefore, referring to the equation 03a, the wheel diameter Rcont of the
また、仮想車輪回転角速度ωw_yの補正後実測値ωw_y_cmpnと、仮想車輪回転角速度ωw_xyの実測値ωw_RL_actとの間には、仮係数γ_yを用いて、次式15の関係が成立する。
Further, the relationship of the following
ここで、仮想車輪回転角速度ωw_xyの実測値ωw_RL_actと指令値の前回値ωw_y_cmd_pとは近似しているので、式13と式15から次式17が導出される。
Here, since the actual measurement value ωw_RL_act of the virtual wheel rotation angular velocity ωw_xy and the previous value ωw_y_cmd_p of the command value are approximate, the following equation 17 is derived from the
そこで、演算部244において、式17を用いて求めた係数γ_xyに、RL−XY変換部243で算出した仮想車輪62_xyのトルクTrqw_xy_s2_yを乗じた値(=γ_xy・Trqw_xy_s)が算出される。ただし、γ_xは0である。そして、演算部245において、演算部244で算出された値と、仮想車輪回転角速度指令値ωw_xy_cmdを遅延要素241に通した仮想車輪回転角速度指令の前回値ωw_xy_cmd_pとが加算され、仮想車輪回転角速度ωw_xyの補正後実測値ωw_xy_cmpnが算出される。なお、γ_xは0であるので、仮想車輪回転角速度ωw_xの補正後実測値ωw_x_cmpnは前回値ωw_x_cmd_pと等しくなる。
Therefore, the
そして、演算部246において、演算部245で算出された仮想車輪回転角速度ωw_xyの補正後実測値ωw_xy_cmpnと仮想車輪62_x,62_yの半径Rw_xyとが乗算される。演算部245による演算結果は、トラクションによるすべりを考慮した車両1の速度推定値に対する補正値に相当する。
Then, the
最後に、演算部247において、演算部240での演算結果θbdot_xy_s_world・h_xyと、演算部246での演算結果を加算した結果、第2の重心速度予想値Vb_xy_exp2が算出される。
Finally, the
次いで、STEP22に進んで、空転抑制部81は、STEP21にて算出した第1の重心速度予想値Vb_xy_exp1と第2の重心速度予想値Vb_xy_exp2との差である重心速度偏差Vb_xy_dif(=Vb_xy_exp1−Vb_xy_exp2)を算出する。
Next, proceeding to STEP 22, the idling
次いで、STEP23に進んで、空転抑制部81は、現在の処理モードが非空転モードか空転モードのいずれのモードであるかを判断する。
Next, proceeding to STEP 23, the idling
非空転モードは、車輪体5が空転していない場合のモードであり、抑制係数λは“1”であり、仮想車輪回転角速度指令ωw_xy_cmdを抑制しない。一方、空転モードは、車輪体5が空転している場合のモードであり、抑制係数λを“0”から“1”の範囲内で決定し、この抑制係数λを乗じて仮想車輪回転角速度指令ωw_xy_cmdを抑制する。なお、抑制係数λは、制御用操作量を基本目標値から“0”に近づける割合に相当する。
The non-idling mode is a mode when the
なお、制御ユニット50の起動時等に該制御ユニット50が初期化された状態での処理モード(初期処理モード)は、非空転モードである。
Note that the processing mode (initial processing mode) in a state in which the
空転抑制部81は、上記STEP22において、現在の処理モードが非空転モードである場合と、空転モードである場合とで、それぞれ、次に、STEP24の処理、STEP25の処理を実行する。
The idling
STEP24における非空転モードの処理は、図17のフローチャートに示す如く実行される。具体的には、空転抑制部81は、まず、前記STEP22で算出した重心速度偏差Vb_x_difに関して|Vb_x_dif|>DV1_xという条件が成立し、且つ、STEP21で遅延要素81bから出力された仮想車輪回転角速度指令の前回値ωw_x_cmd_pに関して|ωw_x_cmd_p|>DV2_xという条件が成立するか否かをSTEP24−1で判断する。
The processing in the non-idling mode in STEP 24 is executed as shown in the flowchart of FIG. Specifically, the idling
ここで、上記DV1_xは、前後方向における予め設定された正の値の閾値(>0)である。そして、|Vb_x_dif|>DV1_xであるということは、車両1のX軸方向における重心速度Vb_xが、実測値から算出された第1の重心速度予想値Vb_x_exp1と、制御ユニット50によるアクチュエータ装置7を介した仮想車輪62_xの駆動制御も考慮した第2の重心速度予想値Vb_x_exp2との差の絶対値が閾値DV1_xより大きい状況を意味する。この状況は、X軸方向における重心速度Vbが制御ユニット50による制御に反したものであり、その原因として、仮想車輪62_xの空転が含まれ得る。
Here, the DV1_x is a preset positive value threshold (> 0) in the front-rear direction. And | Vb_x_dif |> DV1_x means that the center-of-gravity velocity Vb_x in the X-axis direction of the
また、上記DV2_xは、予め設定された正の値の閾値(>0)である。そして、|ωw_x_cmd_p|>DV2_xであるということは、制御ユニット50によるアクチュエータ装置7を介しての仮想車輪62_xの仮想車輪回転角速度指令の前回値ωw_x_cmd_pの絶対値が閾値DV2_xより大きい状況を意味する。この状況では、制御ユニット50による駆動制御が仮想車輪62_xを所定の回転角速度を超えて駆動させるものとなっている。例えば、静止又は静止に近い状態にある車両1にX軸方向に外力が加えられたとき、この外力によって仮想車輪62_xが回転した状態になるので、仮想車輪62_xが空転していない状態とみなすことが可能であっても、前記|Vb_x_dif|>DV1_xの条件を満たす状況がある。そこで、前記|ωw_x_cmd_p|>DV2_xの条件を追加することで、仮想車輪62_xが空転しているとみなされるおそれ(以下、みなし空転のおそれという)があることを確実に判断することが可能となる。
The DV2_x is a preset positive value threshold (> 0). And | ωw_x_cmd_p |> DV2_x means that the absolute value of the previous value ωw_x_cmd_p of the virtual wheel rotation angular velocity command of the virtual wheel 62_x by the
従って、STEP24−1の判断結果が肯定的になる状況は、仮想車輪62_xにみなし空転のおそれがある状況である。 Therefore, the situation in which the determination result in STEP 24-1 is affirmative is a situation in which the virtual wheel 62_x is considered to run idle.
STEP24−1の判断結果が否定的となる場合、すなわち、仮想車輪62_xにみなし空転のおそれがないと判断した場合には、空転抑制部81は、次に、STEP24−2の判断処理を実行する。空転抑制部81は、STEP24−2において、具体的には、まず、前記STEP22で算出した重心速度偏差Vb_y_difに関して|Vb_y_dif|>DV1_yという条件が成立し、且つ、STEP21で遅延要素81bから出力された仮想車輪回転角速度指令の前回値ωw_y_cmd_pに関して|ωw_y_cmd_p|>DV2_yという条件が成立するか否かを判断する。
When the determination result of STEP 24-1 is negative, that is, when it is determined that the virtual wheel 62_x is deemed not to cause idling, the idling
ここで、上記DV1_yは、左右方向における予め設定された正の値の閾値(>0)である。そして、|Vb_y_dif|>DV1_yであるということは、車両1のY軸方向における重心速度Vb_yが、実測値から算出された第1の重心速度予想値Vb_y_exp1と、制御ユニット50によるアクチュエータ装置7を介した仮想車輪62_yの駆動制御も考慮した第2の重心速度予想値Vb_y_exp2との差の絶対値が閾値DV1_yより大きい状況を意味する。この状況は、Y軸方向における重心速度Vbが制御ユニット50による制御に反したものであり、その原因として、仮想車輪62_yの空転が含まれ得る。
Here, DV1_y is a preset positive value threshold (> 0) in the left-right direction. And | Vb_y_dif |> DV1_y means that the center-of-gravity velocity Vb_y in the Y-axis direction of the
また、上記DV2_yは、予め設定された正の値の閾値(>0)である。そして、|ωw_y_cmd_p|>DV2_yであるということは、制御ユニット50によるアクチュエータ装置7を介しての仮想車輪62_yの仮想車輪回転角速度指令の前回値ωw_y_cmd_pの絶対値が閾値DV2_yより大きい状況を意味する。この状況では、制御ユニット50による駆動制御が仮想車輪62_yを所定の回転角速度を超えて駆動させるものとなっている。例えば、静止又は静止に近い状態にある車両1にY軸方向に外力が加えられたとき、この外力によって仮想車輪62_yが回転した状態になるので、仮想車輪62_yにみなし空転がなくとも、前記|Vb_y_dif|>DV1_yの条件を満たす状況がある。そこで、前記|ωw_y_cmd_p|>DV2_yの条件を追加することで、仮想車輪62_yにみなし空転のおそれがあることを確実に判断することが可能となる。
The DV2_y is a preset positive value threshold (> 0). And | ωw_y_cmd_p |> DV2_y means that the absolute value of the previous value ωw_y_cmd_p of the virtual wheel rotation angular velocity command of the virtual wheel 62_y by the
従って、STEP24−2の判断結果が肯定的になる状況は、仮想車輪62_yにみなし空転のおそれがある状況である。 Therefore, the situation in which the determination result in STEP 24-2 is affirmative is a situation in which the virtual wheel 62_y is considered to be idle.
なお、閾値DV1_y,DV2_yは、それぞれ閾値DV1_x,DV2_xよりも小さい値に設定することが好ましい。これにより、ドーナツ状である車輪体5の車輪径が小さいY軸方向の空転検知の感度がX軸方向よりも高まり、車両1の安定性を向上させることが可能となる。
The threshold values DV1_y and DV2_y are preferably set to values smaller than the threshold values DV1_x and DV2_x, respectively. Thereby, the sensitivity of the idling detection in the Y-axis direction in which the wheel diameter of the
STEP24−1又はSTEP24−2の判断結果が肯定的な場合、すなわち、仮想車輪62_x,62_yの少なくもいずれかにみなし空転が発生しているおそれがあると判断した場合には、空転抑制部81は、STEP24−3において、インクリメントカウンタのカウンタ値CNTに1を加える。このインクリメントカウントは、空転が発生しているおそれを検知した後の制御処理周期の経過回数を計数するカウンタである。
When the determination result of STEP 24-1 or STEP 24-2 is affirmative, that is, when it is determined that at least one of the virtual wheels 62_x and 62_y may cause idling, the idling
次いで、空転抑制部81は、インクリメントカウンタのカウント値CNTが閾値DV1_CNTよりも大きいか否かをSTEP24−4にて判断する。
Next, the idling
ここで、上記DV1_CNTは、予め設定された正の値の閾値(>0)であり、空転判定カウント閾値である。そして、CNT>DV1_CNTであるということは、空転が発生しているおそれがあると判断された状態が予め定められた所定時間(=Δt・DV1_CNT)を越えて連続したことを意味し、空転が発生したことが確実的となる。 Here, the DV1_CNT is a positive threshold value (> 0) set in advance and is an idling determination count threshold value. CNT> DV1_CNT means that the state where it is determined that there is a possibility of idling has continued for a predetermined time (= Δt · DV1_CNT), and idling It is certain that it has occurred.
そこで、空転抑制部81は、STEP23−4の判断結果が肯定的である場合には、STEP24−5において、インクリメントカウンタのカウント値CNTを“0”にリセットし、STEP24−6において、処理モードを非空転モードから空転モードに変更して、図17の処理を終了する。
Therefore, if the determination result in STEP23-4 is affirmative, the idling
一方、STEP24−1及びSTEP24−2の判断結果が否定的な場合には、STEP24−7において、インクリメントカウンタのカウント値CNTを“0”にリセットして、図17の処理を終了する。なお、STEP24−2の判断結果が否定的となる場合には、処理モードは変更されないので、次回の制御処理周期においても、処理モードは、非空転モードに維持されることとなる。 On the other hand, if the determination results in STEP 24-1 and STEP 24-2 are negative, the count value CNT of the increment counter is reset to “0” in STEP 24-7, and the processing in FIG. If the determination result in STEP 24-2 is negative, the processing mode is not changed, so that the processing mode is maintained in the non-idling mode even in the next control processing cycle.
以上が、STEP24の非空転モードの処理の詳細である。 The above is the details of the processing in STEP 24 in the non-idling mode.
次に、STEP25における空転モードの処理は、図18のフローチャートに示す如く実行される。具体的には、空転抑制部81は、まず、前記STEP22で算出した重心速度Vb_x_difに関して|Vb_x_dif|<DV1_xという条件が成立するか否かをSTEP24−1で判断する。
Next, the idling mode process in STEP 25 is executed as shown in the flowchart of FIG. Specifically, the idling
|Vb_x_dif|<DV1_xであるということは、重心速度Vb_xが、制御ユニット50によるアクチュエータ装置7を介しての仮想車輪62_xの駆動制御を考慮した第2の予想値Vb_x_exp2と実測値から求めた第1の予想値Vb_x_exp1との偏差であるVb_x_difの絶対値が閾値DV1_xより小さい状況を意味する。この場合、2つの予想値Vb_x_exp1,Vb_x_exp2は一致又はほぼ一致しており、仮想車輪62_xにみなし空転のおそれは少ない状況である。
| Vb_x_dif | <DV1_x means that the center-of-gravity velocity Vb_x is calculated based on the first predicted value Vb_x_exp2 and the first measured value obtained by taking into account the drive control of the virtual wheel 62_x via the
従って、STEP25−1の判断結果が肯定的になる状況は、仮想車輪62_xにみなし空転のおそれが少ない状況である。 Therefore, the situation in which the determination result in STEP 25-1 is positive is a situation in which the virtual wheel 62_x is regarded as being less likely to run idle.
STEP25−1の判断結果が肯定的となる場合、すなわち、仮想車輪62_xにみなし空転のおそれが少ないと判断した場合には、空転抑制部81は、次に、STEP25−2の判断処理を実行する。具体的には、空転抑制部81は、STEP25−2において、まず、前記STEP22で算出した重心速度偏差Vb_y_difに関して|Vb_y_dif|<DV1_yという条件が成立するか否かを判断する。
If the determination result in STEP 25-1 is affirmative, that is, if it is determined that the virtual wheel 62_x is considered to be less likely to cause idling, the idling
|Vb_y_dif|<DV1_yであるということは、重心速度Vb_yが、制御ユニット50によるアクチュエータ装置7を介しての仮想車輪62_yの駆動制御を考慮した第2の予想値Vb_y_exp2と実測値から求めた第1の予想値Vb_y_exp1との偏差であるVb_y_difの絶対値が閾値DV1_yより小さい状況を意味する。この場合、2つの予想値Vb_y_exp1,Vb_y_exp2は一致又はほぼ一致しており、仮想車輪62_yにみなし空転のおそれは少ない状況である。
| Vb_y_dif | <DV1_y means that the center-of-gravity velocity Vb_y is calculated based on the first predicted value Vb_y_exp2 and the first measured value obtained by taking into account the drive control of the virtual wheel 62_y via the
STEP25−2の判断結果が肯定的な場合、すなわち、仮想車輪62_x,62_yのいずれもみなし空転のおそれが少ないと判断した場合には、空転抑制部81は、STEP25−3において、インクリメントカウンタのカウンタ値CNTに1を加える。このインクリメントカウントは、空転の解消の可能性を検知した後の制御処理周期の経過回数を計数するカウンタである。
If the determination result in STEP 25-2 is affirmative, that is, if it is determined that both the virtual wheels 62_x and 62_y are considered to be less likely to cause idling, the idling
次いで、空転抑制部81は、インクリメントカウンタのカウント値CNTが閾値DV2_CNTよりも大きいか否かをSTEP24−4にて判断する。
Next, the idling
ここで、上記DV2_CNTは、予め設定された正の値の閾値(>0)であり、非空転(接地)判定カウント閾値である。そして、CNT|>DV2_CNTであるということは、仮想車輪62_x,62_yにみなし空転のおそれが少ないと判断された状態が予め定められた所定時間(=Δt・DV2_CNT)を越えて連続したことを意味し、空転が解消したことが確実的となる。 Here, DV2_CNT is a preset positive value threshold (> 0), and is a non-idling (grounding) determination count threshold. And, CNT |> DV2_CNT means that a state in which the virtual wheels 62_x and 62_y are considered to be less likely to run idle continues for a predetermined time (= Δt · DV2_CNT). However, it is certain that idling has been resolved.
そこで、空転抑制部81は、STEP23−4の判断結果が肯定的である場合には、STEP25−5において、インクリメントカウンタのカウント値CNTを“0”にリセットし、STEP25−6において、処理モードを空転モードから非空転モードに変更して、図18の処理を終了する。
Therefore, if the determination result in STEP23-4 is affirmative, the idling
一方、STEP25−1又はSTEP25−2の判断結果が否定的な場合には、STEP25−7において、インクリメントカウンタのカウント値CNTを“0”にリセットして、図18の処理を終了する。なお、STEP25−1又はSTEP25−2の判断結果が否定的となる場合には、処理モードは変更されないので、次回の制御処理周期においても、処理モードは、空転モードに維持されることとなる。 On the other hand, if the determination result in STEP 25-1 or STEP 25-2 is negative, the count value CNT of the increment counter is reset to “0” in STEP 25-7, and the processing in FIG. If the determination result in STEP 25-1 or STEP 25-2 is negative, the processing mode is not changed, and therefore the processing mode is maintained in the idling mode in the next control processing cycle.
以上が、STEP25の空転モードの処理の詳細である。 The above is the details of the idling mode processing of STEP25.
図14の説明に戻って、空転抑制部81は、上記の如くSTEP24の非空転モードの処理を実行した後、抑制係数λを“1”に決定すると共に、STEP27のフィルタリング処理におけるフィルタ時定数Tを決定する処理をSTEP26にて実行する。この場合、フィルタ時定数Tを比較的短い時定数Ta、例えば0.01秒に決定する。
Returning to the description of FIG. 14, the idling
一方、空転抑制部81は、上記の如くSTEP25の空転モードの処理を実行した後、次に、抑制係数λを“0”から“1”の範囲内にて決定すると共に、後述するSTEP28のフィルタリング処理におけるフィルタ時定数Tを決定する処理をSTEP27にて実行する。
On the other hand, the idling
まず、空転抑制部81は、抑制係数λの候補値である暫定抑制係数λ_xy_tempを決定する処理をSTEP27−1にて行なう。具体的には、空転抑制部81は、次式19x,19yにより、暫定抑制係数λ_xy_tempを算出する。
First, the idling
λ_x_temp=A_x・|θb_x_s|+B_x ……式19x
λ_y_temp=A_y・|θb_y_s|+B_y ……式19y
ここで、式19x,19yにおけるA_x,A_yは、それぞれ、予め設定された負の定数値(<0)であり、B_x,B_yは、それぞれ、予め設定された正の定数値(>0)である。従って、基体傾斜角度計測値θb_xy_sの絶対値が大きいほど、すなわち基体9の傾斜が大きいほど、暫定抑制係数λ_xy_tempは小さくなり、基体傾斜角度計測値θb_xy_sが0である場合、すなわち基体9が傾斜していない場合、暫定抑制係数λ_xy_tempがB_xyで最大となる。
λ_x_temp = A_x · | θb_x_s | + B_x Equation 19x
λ_y_temp = A_y · | θb_y_s | + B_y Equation 19y
Here, A_x and A_y in the expressions 19x and 19y are respectively negative constant values (<0) set in advance, and B_x and B_y are respectively positive constant values (> 0) set in advance. is there. Therefore, the larger the absolute value of the base body tilt angle measurement value θb_xy_s, that is, the greater the tilt of the
なお、A_xy,B_xyは、基体傾斜角度計測値θb_xy_sの絶対値が所定角度θth1_xy、例えばθb_x_sが10度、θb_y_sが5度のとき、暫定抑制係数λ_xy_tempが“0”となるように設定されている。すなわち、基体傾斜角度計測値θb_xy_sの絶対値が所定角度θth1_xyを超えると、暫定抑制係数λ_xy_tempは負の値(<0)になる。このように、暫定抑制係数λ_xy_tempが負の値になる境界における基体傾斜角度計測値θb_xy_sの絶対値が、X軸方向よりもY軸方向のほうが大きくなっている。これにより、ドーナツ状である車輪体5の車輪径が小さいY軸方向の空転検知の感度がX軸方向よりも高まり、車両1の安定性を向上させることが可能となる。
A_xy and B_xy are set so that the temporary suppression coefficient λ_xy_temp is “0” when the absolute value of the measured base body tilt angle θb_xy_s is a predetermined angle θth1_xy, for example, θb_x_s is 10 degrees and θb_y_s is 5 degrees. . That is, when the absolute value of the base body tilt angle measurement value θb_xy_s exceeds the predetermined angle θth1_xy, the temporary suppression coefficient λ_xy_temp becomes a negative value (<0). Thus, the absolute value of the base body tilt angle measurement value θb_xy_s at the boundary where the temporary suppression coefficient λ_xy_temp becomes a negative value is larger in the Y-axis direction than in the X-axis direction. Thereby, the sensitivity of the idling detection in the Y-axis direction in which the wheel diameter of the
また、A_xy,B_xyは、基体傾斜角度計測値θb_xy_sの絶対値が所定角度θth2_xy、例えばθb_x_sが2度、θb_y_sが1度のとき、暫定抑制係数λ_xy_tempが“1”となるように設定されている。すなわち、基体傾斜角度計測値θb_xy_sの絶対値が所定角度θth1_xy未満であると、暫定抑制係数λ_xy_tempは1を超える値(>1)になる。このように、暫定抑制係数λ_xy_tempが“1”となる境界における基体傾斜角度計測値θb_xy_sの絶対値が、X軸方向よりもY軸方向のほうが小さくなっている。これにより、ドーナツ状である車輪体5の車輪径が小さいY軸方向の空転検知の感度がX軸方向よりも高まり、車両1の安定性を向上させることが可能となる。ただし、基体傾斜角度計測値θb_x_s,θb_y_sの絶対値が所定の同じ角度、例えば0度のとき、暫定抑制係数λ_xy_tempが“1”となるように、A_xy,B_xyを設定してもよい。
Further, A_xy and B_xy are set so that the temporary suppression coefficient λ_xy_temp is “1” when the absolute value of the base body tilt angle measurement value θb_xy_s is a predetermined angle θth2_xy, for example, θb_x_s is 2 degrees and θb_y_s is 1 degree. . That is, when the absolute value of the measured base body tilt angle θb_xy_s is less than the predetermined angle θth1_xy, the temporary suppression coefficient λ_xy_temp becomes a value exceeding 1 (> 1). Thus, the absolute value of the base body tilt angle measurement value θb_xy_s at the boundary where the temporary suppression coefficient λ_xy_temp is “1” is smaller in the Y-axis direction than in the X-axis direction. Thereby, the sensitivity of the idling detection in the Y-axis direction in which the wheel diameter of the
次に、空転抑制部81は、STEP27−1にて算出した暫定抑制係数λ_xy_tempを用いて、抑制係数λを決定する処理をSTEP27−2にて行なう。具体的には、空転抑制部81は、暫定抑制係数λ_x_temp,λ_y_tempのうちの小さいほうの値min(λ_x_temp,λ_y_temp)を、抑制係数λとして仮決定する。そして、この仮決定した抑制係数λの値が“0”から“1”の範囲になるように(0<=λ<=1)リミッタを通してなる値を、抑制係数λの今回値として決定する。この場合、リミッタは、抑制係数λが過大や過小になるのを防止するためのものであり、仮決定した抑制係数λの値が“0”から“1”の範囲である場合には、その抑制係数λの値をそのまま今回値として出力する。しかし、リミッタは、仮決定した抑制係数λの値が“0”未満である場合には、“0”を抑制係数λの今回値として出力し、仮決定した抑制係数λの値が“1”を超える場合には、“1”を抑制係数λの今回値として出力する。
Next, the idling
このようにして、基体傾斜角度計測値θb_x_sの絶対値が所定角度θth1_x以上である場合、又は基体傾斜角度計測値θb_y_sの絶対値が所定角度θth1_y以上である場合、抑制係数λの今回値は“0”に決定される。また、基体傾斜角度計測値θb_x_sの絶対値が所定角度θth2_x以下である場合、又は基体傾斜角度計測値θb_y_sの絶対値が所定角度θth21_y以下である場合、抑制係数λの今回値は“1”に決定される。 Thus, when the absolute value of the base body tilt angle measurement value θb_x_s is greater than or equal to the predetermined angle θth1_x, or when the absolute value of the base body tilt angle measurement value θb_y_s is greater than or equal to the predetermined angle θth1_y, the current value of the suppression coefficient λ is “ 0 "is determined. When the absolute value of the base body tilt angle measurement value θb_x_s is equal to or smaller than the predetermined angle θth2_x, or when the absolute value of the base body tilt angle measurement value θb_y_s is equal to or smaller than the predetermined angle θth21_y, the current value of the suppression coefficient λ is “1”. It is determined.
次いで、空転抑制部81は、後述するSTEP28のフィルタリング処理におけるフィルタ時定数Tを決定する処理をSTEP27−3にて実行する。この場合、フィルタ時定数Tを比較的長い時定数Tb、例えば0.1秒に決定する。
Next, the idling
そして、空転抑制部81は、STEP26又はSTEP27の処理を実行した後、STEP26又はSTEP27で決定した抑制係数λをフィルタに入力する処理(フィルタリング処理)をSTEP28にて実行する。
And the idling
ここで、抑制係数λを入力するフィルタは、特に、処理モードが空転モードと非空転モードと間で変更された直後に、抑制係数λの大きさがステップ状に急変するのを防止するための一次遅れ特性のローパスフィルタである。そして、処理モードが空転モードから非空転従モードに変更された場合、STEP26で決定されたフィルタ時定数Tは比較的短い時定数Taに設定されているので、抑制係数λが急変する空転モードから非空転モードに変更された直後以外の状況ではフィルタの出力値が抑制係数λ、即ち“1”に一致又はほぼ一致するようになっている。一方、処理モードが非空転モードから空転モードに変更された場合、STEP27−3で決定されたフィルタ時定数Tは比較的長い時定数Tbに設定されているので、非空転モードから空転モードに変更され抑制係数λが急変しても、フィルタの出力値が急変しないようになっている。 Here, the filter for inputting the suppression coefficient λ is to prevent the magnitude of the suppression coefficient λ from changing suddenly stepwise immediately after the processing mode is changed between the idle mode and the non-idle mode. This is a low-pass filter having a first-order lag characteristic. When the processing mode is changed from the idling mode to the non-idling mode, since the filter time constant T determined in STEP 26 is set to a relatively short time constant Ta, the idling mode in which the suppression coefficient λ changes abruptly. In situations other than immediately after changing to the non-idling mode, the output value of the filter matches or substantially matches the suppression coefficient λ, that is, “1”. On the other hand, when the processing mode is changed from the non-idling mode to the idling mode, the filter time constant T determined in STEP 27-3 is set to a relatively long time constant Tb, so the non-idling mode is changed to the idling mode. Even if the suppression coefficient λ changes suddenly, the output value of the filter does not change suddenly.
STEP28の処理を実行して、図14の処理を終了する。 The process of STEP28 is executed and the process of FIG.
以上が空転抑制部81の処理の詳細である。
The above is the details of the processing of the idling
以上説明した空転抑制部81の処理によって、抑制係数λは、以下のような態様で決定されることとなる。
By the processing of the idling
例えば、現在の処理モードが非空転モードであるとすると、前記STEP24−1又はSTEP24−2の判断結果が否定的となると、処理モードは非空転モードに維持され、抑制係数λは“1”から変更されない(STEP26)。そのため、前記姿勢制御演算部80から出力された仮想車輪回転角速度指令ωw_xy_cmdの基本値がそのまま、前記モータ指令演算部82に入力される。
For example, if the current processing mode is the non-idle mode, if the determination result in STEP 24-1 or STEP 24-2 is negative, the processing mode is maintained in the non-idle mode, and the suppression coefficient λ is “1”. It is not changed (STEP 26). Therefore, the basic value of the virtual wheel rotation angular velocity command ωw_xy_cmd output from the attitude
一方、前記STEP24−1且つSTEP24−2の判断結果が否定的であり、その状態が予め定められた所定時間(=Δt・DV1_CNT)を越えて連続すると(STEP24−4:YES)、処理モードが空転モードに変更される。そして、STEP27で抑制係数λの変更が行なわれ、STEP28でフィルタリング処置された後、抑制係数λは空転抑制部81から出力される。このとき出力される抑制係数λは“0”から“1”の範囲であり、この抑制係数λの値に応じて、前記姿勢制御演算部80からの出力された仮想車輪回転角速度指令ωw_xy_cmdの基本値が前記演算部83で抑制されて、前記モータ指令演算部82に入力される。
On the other hand, when the determination result of STEP24-1 and STEP24-2 is negative and the state continues for a predetermined time (= Δt · DV1_CNT) (STEP24-4: YES), the processing mode is changed. Change to idle mode. Then, the suppression coefficient λ is changed in STEP 27, and after being filtered in STEP 28, the suppression coefficient λ is output from the idling
STEP27で決定される抑制係数λの値は、基体傾斜角度θb_xyの絶対値が大きいほど小さくなるように設定される。そのため、基体9の傾斜が大きく車両1が転倒するおそれが生じると、より早急に空転の解消を図ることができる。さらに、STEP27で決定される抑制係数λの値は、X軸方向における基体傾斜角度θb_xの絶対値に対応して変化する抑制係数λの変化率|A_x|よりも、Y軸方向における基体斜角度θb_yの絶対値に対応して変化する抑制係数λの変化率|A_y|の方が大きい。そのため、車両1が転倒し易いY軸方向の基体9の傾斜が大きくなった場合、車両1が転倒し難いX軸方向の基体9の傾斜が大きくなった場合に比べて、より早急に空転の解消を図ることができる。
The value of the suppression coefficient λ determined in STEP 27 is set to be smaller as the absolute value of the base body tilt angle θb_xy is larger. For this reason, when the inclination of the
そして、処理モードが空転モードであるとき、前記STEP25−1且つSTEP25−2の判断結果が肯定的であり、その状態が予め定められた所定時間(=Δt・DV2_CNT)を越えて連続すると(STEP25−4:YES)、処理モードが非空転モードに変更され、抑制係数λは“1”に戻る(STEP26)。 When the processing mode is the idling mode, the determination result of STEP25-1 and STEP25-2 is affirmative, and the state continues for a predetermined time (= Δt · DV2_CNT) (STEP25). -4: YES), the processing mode is changed to the non-idling mode, and the suppression coefficient λ returns to “1” (STEP 26).
以上が空転抑制部81の演算処理の詳細である。
The above is the details of the calculation processing of the idling
図9の説明に戻って、制御ユニット50は、次に、前記姿勢制御演算部80で決定した仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdの基本値に空転抑制部81で決定した抑制係数λを乗じた値を前記演算部83で算出する。そして、制御ユニット50は、空転発生の有無に応じて演算部83で抑制された仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdをモータ指令演算部82に入力し、該モータ指令演算部82の処理を実行することによって、電動モータ31Rの速度指令ω_R_cmdと電動モータ31Lの速度指令ω_L_cmdとを決定する。このモータ指令演算部82の処理は、前記リミット処理部86(図11参照)のXY−RL変換部86bの処理と同じである。
Returning to the description of FIG. 9, the
具体的には、モータ指令演算部82は、前記式01a,01bのωw_x,ωw_y,ω_R,ω_Lをそれぞれ、ωw_x_cmd,ωw_y_cmd,ω_R_cmd,ω_L_cmdに置き換えて得られる連立方程式を、ω_R_cmd,ω_L_cmdを未知数として解くことによって、電動モータ31R,31Lのそれぞれの速度指令ω_R_cmd,ω_L_cmdを決定する。
Specifically, the motor
以上により前記STEP9の車両制御演算処理が完了する。
Thus, the vehicle control calculation process of
以上説明した如く制御ユニット50が制御演算処理を実行することによって、前記搭乗モード及び自立モードのいずれの動作モードにおいても、基本的には、シート3及び基体9の姿勢が、前記基体傾斜角度偏差計測値θbe_x_s,θbe_y_sの両方が“0”となる姿勢(以下、この姿勢を基本姿勢という)に保たれている状態では、車両系重心点が静止するように操作量(制御入力)としての仮想車輪回転角加速度指令ωdotw_xy_cmdが決定される。そして、シート3及び基体9の姿勢を前記基本姿勢に対して傾けると、換言すれば、車両系重心点(車両・乗員全体重心点又は車両単体重心点)の水平方向位置を、車輪体5の接地面のほぼ真上に位置する状態から変位させると、シート3及び基体9の姿勢を基本姿勢に復元させるように(θbe_x_s,θbe_y_sを“0”に近づけるか、もしくは“0”に保持するように)、仮想車輪回転角加速度指令ωdotw_xy_cmdが決定される。
As described above, the
そして、ωdotw_xy_cmdの各成分を積分してなる仮想車輪回転角速度指令ωw_xy_cmdを変換してなる電動モータ31R,31Lのそれぞれの回転角速度が、電動モータ31R,31Lの速度指令ω_R_cmd,ω_L_cmdとして決定される。さらに、その速度指令ω_R_cmd,ω_L_cmdに従って、各電動モータ31R,31Lの回転速度が制御される。ひいては車輪体5のX軸方向及びY軸方向のそれぞれの移動速度が、ωw_x_cmdに対応する仮想車輪62_xの移動速度と、ωw_y_cmdに対応する仮想車輪62_yの移動速度とに各々一致するように制御される。
Then, the rotational angular velocities of the
このため、例えば、Y軸周り方向で、実際の基体傾斜角度θb_xが目標値θb_x_objから前傾側にずれると、そのずれを解消すべく(θbe_x_sを“0”に収束させるべく)、車輪体5が前方に向かって移動する。同様に、実際のθb_xが目標値θb_x_objから後傾側にずれると、そのずれを解消すべく(θbe_x_sを“0”に収束させるべく)、車輪体5が後方に向かって移動する。
For this reason, for example, if the actual base body tilt angle θb_x shifts forward from the target value θb_x_obj in the direction around the Y axis, the
また、例えば、X軸周り方向で、実際の基体傾斜角度θb_yが目標値θb_y_objから右傾側にずれると、そのずれを解消すべく(θbe_y_sを“0”に収束させるべく)、車輪体5が右向きに移動する。同様に、実際のθb_yが目標値θb_y_objから左傾側にずれると、そのずれを解消すべく(θbe_y_sを“0”に収束させるべく)、車輪体5が左向きに移動する。
Further, for example, when the actual base body tilt angle θb_y shifts to the right tilt side from the target value θb_y_obj in the direction around the X axis, the
さらに、実際の基体傾斜角度θb_x,θb_yの両方が、それぞれ目標値θb_x_obj,θb_y_objからずれると、θb_xのずれを解消するための車輪体5の前後方向の移動動作と、θb_yのずれを解消するための車輪体5の左右方向の移動動作とが合成され、車輪体5がX軸方向及びY軸方向の合成方向(X軸方向及びY軸方向の両方向に対して傾斜した方向)に移動することとなる。
Further, when both the actual base body tilt angles θb_x and θb_y deviate from the target values θb_x_obj and θb_y_obj, respectively, in order to eliminate the shift operation of the
このようにして、シート3及び基体9の姿勢が前記基本姿勢から傾くと、その傾いた側に向かって、車輪体5が移動することとなる。従って、例えば前記搭乗モードにおいて、乗員が意図的にその上体をシート3及び基体9と共に傾けると、その傾けた側に、車輪体5が移動することとなる。なお、本実施形態では、後述する理由によって、シート3及び基体9を基本姿勢から傾けた場合における車両系重心点の水平面内の移動方向(Z軸に直交する方向での移動方向)と、車輪体5の移動方向とは必ずしも一致しない。
In this way, when the postures of the
そして、車輪体5の移動時(車両1全体の移動時)において、シート3及び基体9の姿勢を、基本姿勢から傾けた一定の姿勢(基体傾斜角度偏差計測値θbe_xy_sが一定となる姿勢)に保持すると、車両系重心点の移動速度(ひいては車輪体5の移動速度)は、制御用目標重心速度Vb_xy_mdfdと一定の偏差を有し、且つ、その偏差が基体傾斜角度偏差計測値θbe_xy_sに依存するものとなる移動速度に収束する。
When the
Vb_x_mdfd,Vb_x_mdfdが“0”に保たれる状況において、シート3及び基体9の姿勢を、基本姿勢から傾けた一定の姿勢に保持した場合には、前記したように、定常状態において、車両系重心点の移動速度(ひいては車輪体5の移動速度)は、基体傾斜角度偏差計測値θbe_xy_sに依存する大きさ及び向きを有する移動速度に収束することとなる。
In a situation where Vb_x_mdfd and Vb_x_mdfd are kept at “0”, if the posture of the
そして、制御ユニット50は、基体9(搭載部3と一体に傾動可能な部位)の傾斜角速度θbdot_xyzの計測値(観測値)θbdot_xyz_sに基づいて、第1の重心速度推定値Vb_xy_exp1を算出している。また、制御ユニット50は、基体9の傾斜角速度θbdot_xyzに基づいて、第2の重心速度推定値Vb_xy_exp2を算出している。さらに、この第2の重心速度推定値Vb_xy_exp2は、アクチュエータ装置7と車輪体5との間の駆動力伝達経路におけるすべりを考慮して補正されている。
Then, the
制御ユニット50で意図した通りに車輪体5が移動動作していれば、2つの推定値Vb_xy_exp1,Vb_xy_exp2の間に差は生じず、重心速度偏差Vb_xy _difの絶対値は“0”又は閾値DV1_xyより小さい微小な値となる。しかし、車輪体5に空転が発生した場合には、制御ユニット5で意図した通りに車輪体5が移動動作せず、2つの推定値Vb_xy_exp1,Vb_xy_exp2の間に差が生じ、重心速度偏差Vb_xy_difの絶対値は閾値DV1_xyより大きな値となる。そこで、重心速度偏差Vb_xy_difに基づいて、車輪体5に空転が発生したおそれを判断することが可能となる。従って、空転抑制部81は、重心速度偏差Vb_xy_difに少なくとも基づいて、車輪体5の空転発生を検知することができる。
If the
さらに、上記特許文献4に記載されたスリップ検知とは異なり、移動動作部の構成に拘わらず、空転が発生したおそれを判断することが可能である。さらに、床反力Fに影響を及ぼす車輪体5で空転が発生したおそれを検知することが可能であるので、旋回時の他、直進時などの空転も検知することができる。
Furthermore, unlike the slip detection described in the above-mentioned Patent Document 4, it is possible to determine the possibility of idling regardless of the configuration of the moving operation unit. Furthermore, since it is possible to detect the possibility that idling has occurred in the
そして、例えば、静止又は静止に近い状態にある車両1に外力が加えられ、この外力によって車輪体5が移動したとき、車輪体5に空転が発生していなくとも、重心速度偏差Vb_xy_difが閾値DV1_xyより大きな値となることがある。そこで、仮想車輪回転角速度指令の前回値ωw_xy_cmd_pの絶対値が閾値DV2_xyより小さい場合には、車輪体5に空転が発生していないと判断する。これにより、車輪体5に空転が発生したことを確実に検知することが可能となる。
For example, when an external force is applied to the
ここで、本実施形態の車両1と本発明との対応関係を補足しておく。
Here, the correspondence between the
本実施形態では、シート(搭乗部)3が、本発明における搭載部(運搬対象物体としての乗員の搭載部)に相当する。 In the present embodiment, the seat (boarding portion) 3 corresponds to the mounting portion (the mounting portion of the occupant as the object to be transported) in the present invention.
また、前記空転抑制部81により本発明における第1及び第2の速度推定手段、並びに空転検知手段が実現される。該空転抑制部81が実行する前記STEP21の処理を行なう手段が本発明における第1及び第2の速度推定手段に相当し、STEP22の処理を行なう手段が本発明における空転検知手段に相当する。そして、第1の重心速度推定値V b_xy_exp1が本発明における第1の速度推定手段が推定する第1の推定値に相当し、第2の重心速度推定値Vb_xy_exp2が本発明における第2の速度推定手段が推定する第2の推定値に相当し、重心速度偏差Vb_xy_difが本発明における空転検知手段が算出する差に相当する。
The idling
次に、以上説明した実施形態に係わる変形態様に関していくつか説明しておく。 Next, some modifications relating to the embodiment described above will be described.
前記各実施形態では、車両系重心点(詳しくは車両・乗員全体重心点)を車両1の所定の代表点としたが、該代表点を例えば、車輪体5の中心点や、基体9の所定の部位(例えば支持フレーム13)の点等に設定してもよい。
In each of the above-described embodiments, the vehicle system center of gravity (specifically, the vehicle / occupant overall center of gravity) is set as a predetermined representative point of the
前記実施形態では、仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdに対する抑制係数λを共通なものとしたが、これに限られるものではない。例えば、暫定抑制係数λ_x_temp,λ_y_tempにリミッタ処理及びフィルタリング処理を行った値を、それぞれ仮想車輪回転角速度指令ωw_x_cmd,ωw_y_cmdに対する抑制係数としてもよい。 In the above embodiment, the suppression coefficient λ for the virtual wheel rotation angular velocity commands ωw_x_cmd and ωw_y_cmd is the same, but the present invention is not limited to this. For example, values obtained by performing limiter processing and filtering processing on the temporary suppression coefficients λ_x_temp and λ_y_temp may be used as the suppression coefficients for the virtual wheel rotation angular velocity commands ωw_x_cmd and ωw_y_cmd, respectively.
また、前記実施形態では、仮想車輪62_x,62_yのいずかにおいてのみに空転が発生したことを検知した場合、仮想車輪62_x,62_yに関する仮想車輪回転角速度指令ωw_xy_cmdを共に抑制するものとしたが、これに限られるものではない。例えば、空転が発生した仮想車輪62_x,62_yに関する仮想車輪回転角速度指令ωw_xy_cmdのみを抑制してもよい。 In the embodiment, when it is detected that idling occurs only in one of the virtual wheels 62_x and 62_y, the virtual wheel rotation angular velocity command ωw_xy_cmd related to the virtual wheels 62_x and 62_y is suppressed together. It is not limited to this. For example, only the virtual wheel rotation angular velocity command ωw_xy_cmd related to the virtual wheels 62_x and 62_y in which idling has occurred may be suppressed.
また、前記実施形態では、STEP22で算出した重心速度偏差Vb_xy _difと閾値DV1_xyと比較して、仮想車輪62_x,62_yに空転が発生したおそれを検知したものとしたが、これに限られるものではない。例えば、第1の重心速度推定値Vb_xy_exp1を第2の重心速度推定値Vb_xy_exp2 で除算した値(=Vb_xy_exp1/Vb_xy_exp2)を閾値と比較して、仮想車輪62_x,62_yに空転が発生したおそれを検知してもよい。 In the above embodiment, the center of gravity speed deviation Vb_xy_dif calculated in STEP 22 is compared with the threshold value DV1_xy to detect the possibility of idling in the virtual wheels 62_x and 62_y. However, the present invention is not limited to this. . For example, a value obtained by dividing the first center of gravity speed estimated value Vb_xy_exp1 by the second center of gravity speed estimated value Vb_xy_exp2 (= Vb_xy_exp1 / Vb_xy_exp2) is compared with a threshold value to detect the possibility of idling in the virtual wheels 62_x and 62_y. May be.
また、前記実施形態では、図1及び図2に示した構造の車両1を例示したが、本発明における倒立振子型車両1は、本実施形態で例示した車両に限られるものではない。
Moreover, in the said embodiment, although the
具体的には、本実施形態の車両1の移動動作部としての車輪体5は一体構造のものであるが、例えば、前記特許文献3の図10に記載されているような構造のものであってもよい。すなわち、剛性を有する円環状の軸体に、複数のローラをその軸心が該軸体の接線方向に向くようにして回転自在に外挿し、これらの複数のローラを軸体に沿って円周方向に配列させることによって、車輪体を構成してもよい。
Specifically, the
さらに移動動作部は、例えば、特許文献2の図3に記載されているようなクローラ状の構造のものであってもよい。 Further, the moving operation unit may have a crawler-like structure as described in FIG.
あるいは、例えば、前記特許文献2の図5、特許文献3の図7、もしくは特許文献1の図1に記載されているように、移動動作部を球体により構成し、この球体を、アクチュエータ装置(例えば前記車輪体5を有するアクチュエータ装置)によりX軸周り方向及びY軸周り方向に回転駆動するように車両を構成してもよい。
Alternatively, for example, as described in FIG. 5 of
また、本実施形態では、乗員の搭乗部としてシート3を備えた車両1を例示したが、本発明における倒立振子型車両は、例えば特許文献3の図8に見られるように、乗員が両足を載せるステップと、そのステップ上で起立した乗員が把持する部分とを基体に組付けた構造の車両であってもよい。
Moreover, in this embodiment, although the
このように本発明は、前記特許文献1〜3等に見られる如き、各種の構造の倒立振子型車両に適用することが可能である。
As described above, the present invention can be applied to inverted pendulum type vehicles having various structures as seen in
さらには、本発明における倒立振子型車両は、床面上を全方向に移動可能な移動動作部を複数(例えば、左右方向に2つ、あるいは、前後方向に2つ、あるいは、3つ以上)備えていてもよい。 Furthermore, the inverted pendulum type vehicle according to the present invention includes a plurality of moving operation units that can move in all directions on the floor surface (for example, two in the left-right direction, two in the front-back direction, or three or more). You may have.
また、本発明における倒立振子型車両は、物体搭載部を1軸周り(例えば車両に搭乗した乗員の左右方向の軸周り)だけ、傾動自在とし、その傾動に応じて車両を乗員の前後方向に移動させるような形態の倒立振子型車両であってもよい。 Further, in the inverted pendulum type vehicle according to the present invention, the object mounting portion can be tilted only around one axis (for example, around the left and right axis of the occupant riding in the vehicle), and the vehicle is moved in the front-rear direction of the occupant according to the tilt. It may be an inverted pendulum type vehicle that is moved.
また、前記実施形態の倒立振子型車両では、搭載部として、乗員の搭乗部(シート)3を備えた車両であるが、乗員の搭乗部3の代わりに、荷物等の運搬対象物体を搭載する搭載部を備えたものであってもよい。この場合には、搭載部に運搬対象物を搭載した状態で、前記搭乗モードと同様の制御処理を実行し、該運搬対象物を搭載していない状態で、前記自立モードと同様の制御処理を実行するようにすればよい。
Moreover, in the inverted pendulum type vehicle of the above-described embodiment, the vehicle is provided with the occupant's riding part (seat) 3 as the mounting part. However, instead of the occupant's
1…倒立振子型車両、3…シート(物体搭載部)、5…車輪体(移動動作部)、7…アクチュエータ装置、9…基体、50…制御ユニット(移動動作部制御手段)、81…空転抑制部(第1の速度推定手段、第2の速度推定手段、空転検知手段)、STEP21…第の速度推定手段、第2の速度推定手段、STEP22−28…空転検知手段。
DESCRIPTION OF
Claims (2)
前記搭載部又は該搭載部と一体に傾斜可能な部位の加速度の観測値に基づいて、前記車両の所定の代表点の速度を推定する第1の速度推定手段と、
少なくとも、前記搭載部又は該搭載部と一体に傾斜可能な部位の傾斜角速度の観測値に基づいて、前記代表点の速度を推定する第2の速度推定手段と、
前記第1の速度推定手段が推定した推定値と、前記第2の速度推定手段が推定した推定値とに少なくとも基づいて、前記移動動作部の空転発生を検知する空転検知手段とを備えることを特徴とする倒立振子型車両の制御装置。 A moving operation unit movable on the floor, an actuator device for driving the moving operation unit, a base body on which the moving operation unit and the actuator device are assembled, and a base member on which tilting with respect to the vertical direction is possible. A control device for an inverted pendulum type vehicle including a mounting portion for the object to be transported,
First speed estimation means for estimating a speed of a predetermined representative point of the vehicle based on an observed value of acceleration of the mounting part or a part that can be tilted integrally with the mounting part;
A second speed estimating means for estimating a speed of the representative point based on at least an observed value of an inclination angular velocity of the mounting portion or a portion that can tilt integrally with the mounting portion;
Comprising: an idling detecting means for detecting the occurrence of idling of the moving operation unit based at least on the estimated value estimated by the first speed estimating means and the estimated value estimated by the second speed estimating means. A control device for an inverted pendulum type vehicle.
前記第2の速度推定手段は、推定した前記代表点の速度の値を、前記アクチュエータ装置と前記移動動作部との間の駆動力伝達経路におけるすべりを考慮して補正することを特徴とする倒立振子型車両の制御装置。 In the control apparatus for an inverted pendulum type vehicle according to claim 1,
The second speed estimation means corrects the estimated speed value of the representative point in consideration of a slip in a driving force transmission path between the actuator device and the moving operation unit. A control device for a pendulum type vehicle.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009260086A JP5330200B2 (en) | 2009-11-13 | 2009-11-13 | Control device for inverted pendulum type vehicle |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009260086A JP5330200B2 (en) | 2009-11-13 | 2009-11-13 | Control device for inverted pendulum type vehicle |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2011105072A true JP2011105072A (en) | 2011-06-02 |
JP5330200B2 JP5330200B2 (en) | 2013-10-30 |
Family
ID=44229093
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009260086A Active JP5330200B2 (en) | 2009-11-13 | 2009-11-13 | Control device for inverted pendulum type vehicle |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5330200B2 (en) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102642584A (en) * | 2012-04-11 | 2012-08-22 | 浙江易力车业有限公司 | Self-balancing electric manned monocycle |
CN102923204A (en) * | 2012-11-19 | 2013-02-13 | 北京理工大学 | Multi-degree-of-freedom two-wheeled robot with variable gravity center |
WO2013084354A1 (en) * | 2011-12-09 | 2013-06-13 | 株式会社日立製作所 | Inverted pendulum type mobile device and control method therefor |
JP2013237324A (en) * | 2012-05-14 | 2013-11-28 | Honda Motor Co Ltd | Inverted pendulum type vehicle |
CN103786601A (en) * | 2014-02-20 | 2014-05-14 | 深圳乐行天下科技有限公司 | Smooth transition method for two-wheeled balance car |
WO2014196405A1 (en) * | 2013-06-03 | 2014-12-11 | Ntn株式会社 | Slip control device for electric vehicle |
WO2015005215A1 (en) * | 2013-07-08 | 2015-01-15 | Ntn株式会社 | Slip control device for electric vehicle |
US10981617B2 (en) | 2017-04-24 | 2021-04-20 | Honda Motor Co., Ltd. | Inverted pendulum type vehicle |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004345608A (en) * | 2003-05-26 | 2004-12-09 | Sony Corp | Vehicle steered by movement of center of gravity |
JP2008189089A (en) * | 2007-02-02 | 2008-08-21 | Toyota Motor Corp | Moving body and control method therefor |
JP2008247138A (en) * | 2007-03-29 | 2008-10-16 | Equos Research Co Ltd | Vehicle |
WO2008132779A1 (en) * | 2007-04-20 | 2008-11-06 | Honda Motor Co., Ltd. | Omnidirectional driver and omnidirectional vehicle employing it |
WO2008139740A1 (en) * | 2007-05-16 | 2008-11-20 | Honda Motor Co., Ltd. | Vehicle movable in all directions |
JP2009073271A (en) * | 2007-09-19 | 2009-04-09 | Equos Research Co Ltd | Vehicle |
JP2009104360A (en) * | 2007-10-23 | 2009-05-14 | Toyota Motor Corp | Inverted vehicle and method of controlling inverted vehicle |
JP2009142127A (en) * | 2007-12-10 | 2009-06-25 | Equos Research Co Ltd | Vehicle control device |
-
2009
- 2009-11-13 JP JP2009260086A patent/JP5330200B2/en active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004345608A (en) * | 2003-05-26 | 2004-12-09 | Sony Corp | Vehicle steered by movement of center of gravity |
JP2008189089A (en) * | 2007-02-02 | 2008-08-21 | Toyota Motor Corp | Moving body and control method therefor |
JP2008247138A (en) * | 2007-03-29 | 2008-10-16 | Equos Research Co Ltd | Vehicle |
WO2008132779A1 (en) * | 2007-04-20 | 2008-11-06 | Honda Motor Co., Ltd. | Omnidirectional driver and omnidirectional vehicle employing it |
WO2008139740A1 (en) * | 2007-05-16 | 2008-11-20 | Honda Motor Co., Ltd. | Vehicle movable in all directions |
JP2009073271A (en) * | 2007-09-19 | 2009-04-09 | Equos Research Co Ltd | Vehicle |
JP2009104360A (en) * | 2007-10-23 | 2009-05-14 | Toyota Motor Corp | Inverted vehicle and method of controlling inverted vehicle |
JP2009142127A (en) * | 2007-12-10 | 2009-06-25 | Equos Research Co Ltd | Vehicle control device |
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9290109B2 (en) | 2011-12-09 | 2016-03-22 | Hitachi, Ltd. | Inverted pendulum moving apparatus and control method therefor |
WO2013084354A1 (en) * | 2011-12-09 | 2013-06-13 | 株式会社日立製作所 | Inverted pendulum type mobile device and control method therefor |
JPWO2013084354A1 (en) * | 2011-12-09 | 2015-04-27 | 株式会社日立製作所 | Inverted pendulum type moving device and control method thereof |
CN102642584B (en) * | 2012-04-11 | 2014-06-04 | 浙江易力车业有限公司 | Self-balancing electric manned monocycle |
CN102642584A (en) * | 2012-04-11 | 2012-08-22 | 浙江易力车业有限公司 | Self-balancing electric manned monocycle |
JP2013237324A (en) * | 2012-05-14 | 2013-11-28 | Honda Motor Co Ltd | Inverted pendulum type vehicle |
CN102923204A (en) * | 2012-11-19 | 2013-02-13 | 北京理工大学 | Multi-degree-of-freedom two-wheeled robot with variable gravity center |
CN102923204B (en) * | 2012-11-19 | 2015-06-03 | 北京理工大学 | Multi-degree-of-freedom two-wheeled robot with variable gravity center |
US9475396B2 (en) | 2013-06-03 | 2016-10-25 | Ntn Corporation | Slip control device for electric vehicle |
WO2014196405A1 (en) * | 2013-06-03 | 2014-12-11 | Ntn株式会社 | Slip control device for electric vehicle |
JP2014236590A (en) * | 2013-06-03 | 2014-12-15 | Ntn株式会社 | Slip control device for electric vehicle |
CN105263745A (en) * | 2013-06-03 | 2016-01-20 | Ntn株式会社 | Slip control device for electric vehicle |
WO2015005215A1 (en) * | 2013-07-08 | 2015-01-15 | Ntn株式会社 | Slip control device for electric vehicle |
US9688161B2 (en) | 2013-07-08 | 2017-06-27 | Ntn Corporation | Slip control device for electric vehicle |
CN103786601A (en) * | 2014-02-20 | 2014-05-14 | 深圳乐行天下科技有限公司 | Smooth transition method for two-wheeled balance car |
US10981617B2 (en) | 2017-04-24 | 2021-04-20 | Honda Motor Co., Ltd. | Inverted pendulum type vehicle |
Also Published As
Publication number | Publication date |
---|---|
JP5330200B2 (en) | 2013-10-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5208906B2 (en) | Inverted pendulum type vehicle | |
JP5306474B2 (en) | Control device for inverted pendulum type vehicle | |
JP5504272B2 (en) | Control device for inverted pendulum type vehicle | |
JP5436564B2 (en) | Control device for inverted pendulum type vehicle | |
JP5330200B2 (en) | Control device for inverted pendulum type vehicle | |
JP5318216B2 (en) | Inverted pendulum type vehicle | |
JP5404800B2 (en) | Inverted pendulum type vehicle | |
JP5414801B2 (en) | Control device for inverted pendulum type vehicle | |
JP5306472B2 (en) | Control device for inverted pendulum type vehicle | |
JP5414804B2 (en) | Control device for inverted pendulum type vehicle | |
JP5414800B2 (en) | Control device for inverted pendulum type vehicle | |
JP5355700B2 (en) | Inverted pendulum type moving body control device | |
JP5306470B2 (en) | Control device for omnidirectional vehicle | |
JP5355348B2 (en) | Control device for omnidirectional vehicle | |
JP5306471B2 (en) | Control device for inverted pendulum type vehicle | |
JP2011068167A (en) | Control device of inverted pendulum type vehicle | |
JP5386283B2 (en) | Inverted pendulum type moving body, control device, and control method | |
JP5330199B2 (en) | Control device for inverted pendulum type vehicle | |
JP5330198B2 (en) | Control device for inverted pendulum type vehicle | |
JP5306951B2 (en) | Inverted pendulum type vehicle | |
JP2011073628A (en) | Control device for inverted pendulum type moving body |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20111124 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20120518 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130625 |
|
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: 20130702 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130725 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5330200 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 |