WO2008041732A1 - 傾斜角推定機構を有する移動体 - Google Patents

傾斜角推定機構を有する移動体 Download PDF

Info

Publication number
WO2008041732A1
WO2008041732A1 PCT/JP2007/069404 JP2007069404W WO2008041732A1 WO 2008041732 A1 WO2008041732 A1 WO 2008041732A1 JP 2007069404 W JP2007069404 W JP 2007069404W WO 2008041732 A1 WO2008041732 A1 WO 2008041732A1
Authority
WO
WIPO (PCT)
Prior art keywords
tilt angle
acceleration
inclination angle
estimating
sensor
Prior art date
Application number
PCT/JP2007/069404
Other languages
English (en)
French (fr)
Other versions
WO2008041732A8 (ja
Inventor
Toshio Fuwa
Issei Nakashima
Original Assignee
Toyota Jidosha Kabushiki Kaisha
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Toyota Jidosha Kabushiki Kaisha filed Critical Toyota Jidosha Kabushiki Kaisha
Priority to EP07829143.2A priority Critical patent/EP2077432B1/en
Priority to CN2007800175536A priority patent/CN101443628B/zh
Priority to US12/297,323 priority patent/US8000925B2/en
Publication of WO2008041732A1 publication Critical patent/WO2008041732A1/ja
Publication of WO2008041732A8 publication Critical patent/WO2008041732A8/ja

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
    • G01B21/22—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring angles or tapers; for testing the alignment of axes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • G01C9/02—Details
    • G01C9/06—Electric or photoelectric indication or reading means

Definitions

  • the present invention relates to a mobile object having an inclination angle estimation mechanism, and more particularly to an inverted two-wheel vehicle having an inclination angle estimation mechanism.
  • a moving body such as an inverted two-wheeled vehicle
  • This is not limited to two-wheeled vehicles, and it is required to estimate the tilt angle with high accuracy even in a walking type moving body and a moving body driven by a chabilee.
  • the tilt angle can be obtained by integrating the output signal of the gyro sensor, but since the offset noise is inherently superimposed on the output signal of the gyro sensor, it is true. There was a problem when the deviation gradually increased with respect to the value.
  • the tilt angle is calculated using the geometrical relationship between two or more axes of acceleration sensors.
  • the tilt angle is calculated from the value of the biaxial gravitational acceleration, so that there is a problem that the estimation accuracy deteriorates unless it is in a static state.
  • the component generated by the acceleration / deceleration is superimposed on the biaxial acceleration sensor as noise.
  • Patent Document 1 Although a technique using a gyro sensor and an accelerometer by switching for each frequency band is disclosed in Patent Document 1, this is not a technique related to a moving body having a tilt angle estimation mechanism.
  • Patent Document 1 Japanese Unexamined Patent Publication No. 4-3 4 6 0 2 1
  • the present invention has been made to solve such a problem, and an object of the present invention is to provide a moving body having an inclination angle estimation mechanism capable of estimating a vehicle body inclination angle with high accuracy.
  • a moving body is a moving body including an acceleration sensor that detects biaxial acceleration, a gyro sensor that detects the angular velocity of the vehicle, and an inclination angle estimation mechanism that estimates the inclination angle of the vehicle body.
  • the tilt angle estimation mechanism is configured to estimate the first tilt angle based on the acceleration detected by the acceleration sensor, and based on the angular velocity detected by the gyro sensor according to the linear model equation of the moving body, Second slope Means for estimating an angle; and means for estimating an inclination angle by performing feedback control so that the second inclination angle follows the first inclination angle, using the first inclination angle as a reference value of an observer. It is what you have.
  • the tilt angle estimation mechanism has a setting unit that sets an observer gain K for adjusting the influence of the first tilt angle to an arbitrary value.
  • the setting means may set the observer gain K to a small value when the moving body is in the longitudinal acceleration state.
  • the setting means sets the observer gain K according to the failure state of the acceleration sensor and the gyro sensor.
  • a mouth-pass filter may be provided at the subsequent stage of the acceleration sensor.
  • the mobile body in a preferred embodiment is an inverted two-wheel vehicle. .
  • the invention's effect is an inverted two-wheel vehicle. .
  • the mobile body which has a tilt angle estimation mechanism which can estimate a vehicle body tilt angle with high precision can be provided.
  • FIG. 1 is a diagram for explaining a sensor coordinate system of a moving object according to an embodiment of the present invention.
  • FIG. 2 is a block diagram showing a method of estimating the tilt angle of the moving object according to the embodiment of the present invention.
  • FIG. 3 is a flowchart showing a method for estimating the tilt angle of a moving object according to an embodiment of the present invention.
  • FIG. 4 is a flowchart showing a method of estimating the tilt angle of the moving object according to the embodiment of the present invention.
  • FIG. 1 shows a sensor coordinate system of a moving object having an inclination angle estimation mechanism according to the present invention.
  • the moving body 10 has a vehicle body 1 and wheels 2 fixed to the vehicle body 1 via a drive mechanism (not shown).
  • An axis that is orthogonal to the front of the moving body 10 (that is, the moving direction and the pitch direction) and extends in a substantially horizontal direction (that is, the roll direction) is the X axis, and the front of the moving body 10
  • the angle between the line connecting the rotation center of the vehicle body 1 and the wheel 2 and the Z axis is the vehicle body inclination angle.
  • the moving body 10 includes an acceleration sensor that measures acceleration in the Y-axis direction and the Z-axis direction, and a low-pass filter (low-pass filter) having a low cutoff frequency at the subsequent stage. Based on the accelerations u (l) and u (2) in the Y-axis direction and the Z-axis direction detected by these two acceleration sensors, the vehicle body inclination angle (that is, the pitch angle) "can be obtained according to the following equation.
  • the moving body 10 includes a one-axis gyro sensor (angular velocity sensor) that measures an angular velocity in the X-axis direction. Furthermore, the moving body 10 implements a tilt angle estimating mechanism that estimates (calculates) the vehicle body tilt angle by inputting detection signals and control signals of the acceleration sensor and the gyro sensor.
  • a tilt angle estimating mechanism that estimates (calculates) the vehicle body tilt angle by inputting detection signals and control signals of the acceleration sensor and the gyro sensor.
  • the tilt angle estimation mechanism includes a means for estimating the tilt angle based on the acceleration detected by the acceleration sensor, a means for estimating the tilt angle based on the angular velocity detected by the gyro sensor, a tilt angle based on the acceleration sensor, And a means for estimating the tilt angle based on the tilt angle based on the gyro sensor using the linear model equation of the moving body as an observer.
  • the means for estimating the tilt angle uses the tilt angle based on the gyro sensor as the reference value of the observer, and estimates the estimated value so as to follow the feedback with respect to the tilt angle based on the gyro sensor.
  • FIG. 2 is a block diagram for explaining the tilt angle estimation processing according to the present invention.
  • the control signal the acceleration signal is I 2 of the detected Y-axis direction by the Y-axis acceleration sensor, acceleration signals of the detected Z-axis direction by the Z-axis acceleration sensor to I 3
  • the angular velocity signals detected by the 1-axis gyro sensor are input to I 4 respectively.
  • the vehicle body inclination angle is calculated geometrically according to the above equation (1).
  • the tilt angle calculated here is highly accurate for the direct current (DC) component to the low frequency range.
  • DC direct current
  • the mid frequency range to the high frequency range. Low accuracy for the area.
  • the control signal the vehicle body tilt angle is calculated, enter the angular velocity detected by the gyro sensor, the feedback processing by the addition external force observer, finally inclination angle, calculates an inclination angular velocity and the external force, O l 5 respectively From o 2 and o 3 I have power.
  • the body model (the equation of motion) of an inverted motorcycle can be expressed as follows.
  • Equation (5) is used to determine the tilt angular velocity. Summing the left side of Eq. (4) with ⁇ , the following Eq. (6) is obtained.
  • an external force observer is created.
  • the 0 term on the right-hand side of Eq. (6) works as an external force in the equation 7].
  • an external force observer is configured in consideration of the external force f as a state quantity.
  • the external force f is assumed to be a step with a constant value during the sample period, and the differential value during that period is set to zero.
  • the external force observer can be expressed by the following equation (10).
  • Observer gain (feedback gain) K in equation (10) is It is set as ⁇ 1, ⁇ 2, and ⁇ 3 for each attitude ⁇ , dot, and f.
  • the convergence speed of the estimation is determined by the size of the observer gain ⁇ in Eq. (10). In other words, the convergence speed of the estimation is determined by the pole size of the characteristic equation obtained from Eq. (10).
  • K (Kl, ⁇ 2, ⁇ 3)
  • K2 the estimated value by the gyro sensor
  • K is determined taking into account the trade-off between the following (A) and (B).
  • the estimated value gives priority to the observed quantity, and the influence of the basic model becomes small. For this reason, it is easily affected by sensor noise. In this case, the convergence speed of estimation is high.
  • the observed quantity which is the acceleration sensor input
  • the estimated value basically follows the basic model. For this reason, it is less susceptible to sensor noise. In this case, the convergence speed of estimation is low.
  • the observer gain K outputs the high-frequency component of the tilt angle estimated based on the gyro sensor as it is, and the low-frequency and DC components become the tilt angle estimated based on the acceleration sensor.
  • the tilt angle can be estimated with high accuracy in a wide frequency range up to.
  • the response may be slow.
  • a slow response indicates that the noise can be reduced accordingly, so a more accurate estimation is expected.
  • the pole is set relatively close to the real axis.
  • the equation of motion (model information) corresponding to the inclination angle is used for the external force observer.
  • the tilt angle is estimated with high accuracy in the middle to high frequency range.
  • feedback control is performed so that the estimated value follows the inclination angle estimated based on not only the single-axis gyro sensor but also the two-axis acceleration sensor as the reference value of the observer. This makes it possible to estimate the tilt angle with high accuracy in a wide frequency range from the direct current component to the high frequency range.
  • the noise of the acceleration sensor the effect of the noise can be reduced by setting the observer gain K appropriately.
  • the external force observer can be designed by discretizing the last designed continuous external force observer.
  • the observer gain K is controlled by the control mechanism.
  • Figure 3 shows a flowchart of this control.
  • the control mechanism determines whether the angular velocity detected by the gyro sensor or the like is within a predetermined range (S 1 0 1).
  • the angular velocity is used as a reference because it is possible to detect whether the moving body is in the longitudinal acceleration state. Therefore, in the example shown in FIG. 3, it is detected whether or not the moving body is in the longitudinal acceleration state based on the angular velocity.
  • the present invention is not limited to this.
  • the control mechanism does not execute control for changing the value of the observer gain K.
  • the control mechanism determines that the angular velocity is not higher than the predetermined range, that is, lower, the control is executed to increase the value of the observer gain K (S 1 0 4).
  • the control of the magnitude of the observer gain K select either the output value when estimation processing is performed with the observer gain K reduced or when estimation processing is performed with the observer gain K increased. You can do it.
  • the gyro sensor and the acceleration sensor are provided. However, if any of the sensors fails, the inclination angle estimation based on the failed sensor is not executed, that is, only to the sensor that does not fail. Based on this, control is performed so as to execute the inclination angle estimation.
  • the control mechanism determines whether a failure such as a disconnection or a sensor failure is detected by a watchdog or the like (S 2 0 1). If it is determined that a failure has been detected, the control mechanism determines whether or not the gyro sensor has failed (S 2 0 2). When it is determined that the gyro sensor is in failure, the control mechanism estimates the vehicle body inclination angle by using the acceleration sensor that has not failed (S 2 0 3). On the other hand, if it is determined that the gyro sensor is not in failure, the control mechanism determines whether or not the acceleration sensor is in failure (S 2 0 4).
  • the vehicle body inclination angle is estimated by a non-failed gyro sensor (S 2 0 5).
  • the control mechanism determines that the acceleration sensor is not faulty. In the case of a failure, the estimation process is stopped (S 2 0 6).
  • the estimation accuracy of the tilt angle can be increased using the model information of the moving object. For example, if the tilt angle does not exceed a predetermined value due to restrictions on the structure of the vehicle, if the estimated value exceeds the predetermined value, the true value is obtained by executing the estimation process after limiting to the predetermined value. Therefore, it is possible to prevent the estimation accuracy from deteriorating.
  • the moving body 10 is an inverted two-wheeled unstable vehicle.
  • the moving body 10 is not limited to this, and may be a walking-type moving body or a movable body driven by a flyer.
  • the mobile body 10 may be a boarding-type mobile body, and further applicable to a mobile body incorporated in the lower half of the robot or a mobile body that moves with a load. .
  • the movable body having the tilt angle estimation mechanism can be widely used particularly for an inverted two-wheeled vehicle having the tilt angle estimation mechanism.

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Motorcycle And Bicycle Frame (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Gyroscopes (AREA)

Abstract

　高精度に車体傾斜角度を推定することが可能な傾斜角推定機構を有する移動体を提供すること。本発明にかかる移動体は、２軸方向の加速度を検出する加速度センサと、車体の角速度を検出するジャイロセンサと、車体の傾斜角を推定する傾斜角推定機構とを備えた移動体である。この傾斜角推定機構は、加速度センサの検出した加速度に基づいて加速度センサに応じた傾斜角を推定する手段と、前記ジャイロセンサの検出した角速度に基づいて第２の傾斜角を推定する手段と、加速度センサに基づく傾斜角と、ジャイロセンサに基づく傾斜角に基づいて、移動体の線形モデル式をオブザーバとして傾斜角を推定する手段を有している。

Description

明 細 書 傾斜角推定機構を有する移動体 技術分野
[ 0 0 0 1 ]
本発明は、 傾斜角推定機構を有する移動体に関し、 特に傾斜角推定機構を有 する倒立 2輪型車両に関する。 背景技術
[ 0 0 0 2 ]
倒立 2輪型車両のような移動体においては、 倒立姿勢を維持するために傾斜 角度を高精度に推定する必要がある。 これは、 2輪型車両に限らず、 歩行型移 動体やキヤタビラ駆動の移動体においても、 傾斜角度を高精度に推定すること が求められている。
[ 0 0 0 3 ]
従来、 傾斜角を推定する機構としては、 ジャイロ （角速度） センサを用いる 方式や加速度センサを用いる方式がある。 ジャイロセンサを用いる方式におい ては、 ジャイロセンサの出力信号を積分することによって傾斜角度を求めるこ とができるが、 ジャイロセンサの出力信号には本来的にオフセットノイズが重 畳されているため、 真値に対して少しずつずれが大きくなつていくとレ、う問題 があった。
[ 0 0 0 4 ] 一
加速度センサを用いる方式では、 2軸以上の加速度センサによる幾何学的関 係を用いて傾斜角度を算出する。 この方式によれば、 2軸重力加速度の値から 傾斜角度を算出しているため、 静的な状態でなければ推定精度が悪化するとい う問題があった。 特に、 移動体が加減速した場合には、 加減速によって発生す る成分が 2軸の加速度センサにノイズとして重畳することになる。 [ 0 0 0 5 ]
また、 ジャイロセンサと 3軸の加速度センサを用いて自律慣性航法 （I N S ) により傾斜角度を推定する方式も提案されている。 しかしながら、 上述し たようなジャイロセンサにおけるオフセットノイズの問題や加速度センサにお ける加減速に基づくノイズの問題は解消されない。
[ 0 0 0 6 ]
なお、 ジャイロセンサ及び加速度計を周波数帯域ごとに切り替えて用いる技 術が特許文献 1に開示されているが、 これは傾斜角推定機構を有する移動体に 関する技術ではない。
[特許文献 1 ]特開平 4一 3 4 6 0 2 1号公報 明の開示
発明が解決しようとする課題
[ 0 0 0 7 ]
以上説明したように、 ジャイロセンサや加速度センサを用いることにより車 体傾斜角度を求めることは可能であるが、 精度に問題があった。
[ 0 0 0 8 ]
本発明は、 かかる課題を解決するためになされたものであり、 高精度に車体 傾斜角度を推定することが可能な傾斜角推定機構を有する移動体を提供するこ とを目的とする。 課題を解決するための手段
[ひ 0 0 9 ]
本発明にかかる移動体は、 2軸方向の加速度を検出する加速度センサと、 車 体の角速度を検出するジャイロセンサと、 車体の傾斜角を推定する傾斜角推定 機構とを備えた移動体であって、 当該傾斜角推定機構は、 前記加速度センサの 検出した加速度に基づいて第 1の傾斜角を推定する手段と、 前記移動体の線形 モデル式に応じて前記ジャイロセンサの検出した角速度に基づき、 第 2の傾斜 角を推定する手段と、 前記第 1の傾斜角をオブザーバの参照値として、 前記第 2の傾斜角が当該第 1の傾斜角に追従するようにフィードバック制御すること によって傾斜角を推定する手段を有するものである。
[0010]
ここで、 前記傾斜角推定機構は、 前記第 1の傾斜角の影響を調整するための オブザーバゲイン Kを任意の値に設定する設定手段を有することが望ましい。
[001 1]
また、 前記設定手段は、 移動体が前後加速状態にある場合に、 前記ォブザー バゲイン Kを小さい値に設定するとよい。
[0012]
さらに、 前記設定手段は、 加速度センサ及びジャイロセンサの故障状態に応 じて、 当該オブザーバゲイン Kを設定することが望ましい。
また、 前記加速度センサの後段に口一パスフィルタを設けるとよい。
好適な実施の形態における移動体は、 倒立 2輪型車両である。 . 発明の効果
[0013]
本発明によれば、 高精度に車体傾斜角度を推定することが可能な傾斜角推定 機構を有する移動体を提供することができる。 図面の簡単な説明
[0014]
[図 1：]本発明の実施の形態にかかる移動体のセンサ座標系を説明するため の図である。
[図 2]本発明の実施の形態にかかる移動体の傾斜角推定法を示すプロック 図である。
[図 3 ]本発明の実施の形態にかかる移動体の傾斜角推定方法を示すフロー チヤ一トである。 [図 4 ]本発明の実施の形態にかかる移動体の傾斜角推定方法を示すフ口一 チャートである。 符号の説明
[0015]
1 車体
2 車輪
10 移動体 発明を実施するための最良の形態
[0016]
図 1に、 本発明にかかる傾斜角推定機構を有する移動体のセンサ座標系を示 す。 図 1に示されるように、 当該移動体 10は、 車体 1と、 車体 1に対して図 示しない駆動機構を介して固定された車輪 2を有している。
[0017]
当該移動体 10の.前方 （即ち、 移動方向、 ピッチ方向） に対して直交し、 ほ ぼ水平方向 （即ち、 ロール方向） に延びる軸を X軸、 当該移動体 10の前方
(即ち、 移動方向、 ピッチ方向） を Y軸、 鉛直方向を Z軸としている。 本例で は、 車体 1と車輪 2の回転中心を結ぶ線と、 Z軸のなす角度を車体傾斜角とす る。
[0018] ' 移動体 10は、 Y軸方向及び Z軸方向の加速度を計測する加速度センサとそ の後段にカットオフ周波数の低いローパスフィルタ （低域通過フィルタ） を備 えている。 これら 2つの加速度センサによって検出された Y軸方向及び Z軸方 向の加速度 u(l),u(2)に基づき次式に従って車体傾斜角 （即ち、 ピッチ角） "を 求めることができる。
7] =tan"1 (u(l)/u(2)) · · · (1)
[0019] また、 当該移動体 1 0は、 X軸方向の角速度を計測する 1軸のジャイロセン サ （角速度センサ） を備えている。 さらに、 移動体 1 0では、 加速度センサ及 びジャイロセンサの検出信号や制御信号を入力し、 車体傾斜角を推定 （算出） する傾斜角推定機構をコンピュータにより実現している。
[ 0 0 2 0 ]
この傾斜角推定機構は、 加速度センサの検出した加速度に基づいて傾斜角を 推定する手段と、 ジャイロセンサの検出した角速度に基づいて傾斜角を推定す る手段と、 加速度センサに基づく傾斜角と、 ジャイロセンサに基づく傾斜角に 基づいて、 移動体の線形モデル式をオブザーバとして傾斜角を推定する手段と を備えている。 このとき、 傾斜角を推定する手段は、 ジャイロセンサに基づく 傾斜角をオブザーバの参照値とし、 ジャイロセンサに基づく傾斜角に対して推 定値がフィードバック追従するようにして推定する。
[ 0 0 2 1 ]
図 2は、 本発明にかかる傾斜角推定処理を説明するためのプロック図である。 図に示されるように、 制御信号が に、 Y軸加速度センサにより検出された Y 軸方向の加速度信号が I 2に、 Z軸加速度センサにより検出された Z軸方向の加 速度信号が I 3に、 1軸ジャイロセンサにより検出された角速度信号が I 4に対 してそれぞれ入力される。
[ 0 0 2 2 ]
まず、 I 2に入力された Y軸方向の加速度と、 I 3に Z軸方向の加速度に基づ いて、 上記式 （1 ) に従って、 幾何学的に車体傾斜角が算出される。 ここで算 出された傾斜角は、 直流 （D C) 成分〜低周波域に対して高精度であるが、 カロ 減速を伴う運動時には加速度センサに外乱が印加されるので、 中周波数域〜高 周波数域に対して低精度である。
[ 0 0 2 3 ]
次に、 制御信号、 算出された車体傾斜角、 ジャイロセンサにより検出された 角速度を入力し、 さらに外力オブザーバによるフィードバック処理によって、 最終的に傾斜角、 傾斜角速度及び外力を算出し、 それぞれ O l 5 o2， o3より出 力している。 なお、 図 2において、 外力オブザーバに関する式 y (n) =C x (n) +Du (n) を式 （2) とし、 x (n+ 1) =Ax (n) +B u (n) を式 （3) とする。
[00 24]
次に、 外力オブザーバ設計の詳細について説明する。
まず、 車体傾斜角の線型モデル化を実行する。 倒立二輪車の車体モデル （運 動方程式） は次式の通り表わすことができる。
[数 1]
{m l + J l + n'J m -nu (4) (OTJ- - n2J +
Figure imgf000008_0001
nu ( 5) ここで、 η ：車体の傾斜角、 Θ ：車輪の回転角、 mi ：車体の質量、 m2 ：車 輪の質量、 1 ：車体重心の車軸からの距離、 J ：車体の重心回りの慣性モーメ ント、 J m：モータロータの慣性モーメント、 J w：車輪の軸回りの慣性モー メント、 n ：ギア比 （モータ ：車輪 == 1 ： n) 、 r ：車輪半径、 f r ：駆動シ ステムの抵抗、 u ：モータのトルク値である。
[00 2 5]
上記式 （4) , (5) のうち、 本発明においては車体傾斜角 ηの推定を実行で きればよいため、 式 （4) のみを考慮する。 なお、 式 （5) は、 傾斜角速度を 求める場合に利用される。 式 （4) の左辺を ηでまとめると、 次の式 （6) の 通りとなる。
[数 2]
(m 2 + J1 + n2J m )η + f Γή - mxgl η = -nu - m^l - n ~J m )θ + f Γθ ( 6)
[00 2 6]
この式 （6) を基本モデルとして外力オブザーバを作成する。 ここで、 式 (6) の右辺の 0項は、 7]方程式の外力として働く。 本発明では、 当該 0項を 無視すると、 推定誤差が悪化するため、 これを防止するために、 状態量として 外力 f も考慮した外力オブザーバを構成した。 [0027]
次に、 外力オブザーバの基本モデルの導出について説明する。 今、 状態量を 次式 （7) とおく。
[数 3]
X = ( 7 )
f ここで、 f は外力である。
[0028]
式 7で示される状態量を用いて、 式 6を状態方程式表現に変換すると、 次の 式 （8) ， (9) のように表わすことができる。
[数 4]
X = AX + Bu (8) γ = CX (9)
A
Figure imgf000009_0002
M
Figure imgf000009_0001
ここで、 外力 f については、 サンプル周期の間は一定値をとるステップ状と し、 その間の微分値は 0とした。
[0029]
次に外力オブザーバのゲインの決定方法について説明する。
外力オブザーバは、 次式 （10) により表わすことができる。
[数 5]
X：推定値
X = AX + Bu + K[Y - CX ) (10)
Κ：オフ' -A'ケ
[0030]
式 （10) におけるオブザーバゲイン （フィードバックゲイン） Kは、 各状 態量 η、 ドット、 f 毎に Κ 1、 Κ2、 Κ 3のようにそれぞれ設定される。 式 (10) におけるオブザーバゲイン Κの大きさによって、 推定の収束速度が決 定される。 つまり、 式 （10) から求められる特性方程式の極の大きさによつ て、 推定の収束速度が決まる。
K= (Kl， Κ2， Κ3) としたとき、 Κ1が大きければ加速度センサによ る推定値、 K2が大きければジャイロセンサによる推定値に各々近づく。 低周 波では加速度センサの推定値が正しいと判断されるため、 K2を大きくし、 中 〜高周波では K 2を大きぐする。 動作周波数が予め推定できる場合は、 その周 波数に応じて Kを設定すればよレ、。
[0031]
具体的には、 次の （A) (B) 間のトレードオフに留意して Kを決定する。
(A) オブザーバゲイン Kが大きい （極が実軸から遠い） 場合
この場合には、 加速度センサ入力である観測量の影響を大きく受ける。 従つ て、 推定値は観測量を優先することになり、 基本モデルの影響は小さくなる。 このため、 センサノイズの影響を受けやすくなる。 また、 この場合には、 推定 の収束速度は高くなる。
[0032]
(B) オブザーバゲイン Kが小さい （極が実軸に近い） 場合
この場合には、 加速度センサ入力である観測量を大きくなますことになる。 換言すると、 観測量の影響を小さくすることになる。 従って、 基本的に推定値 は基本モデルに従うことになる。 このため、 センサノイズの影響は受けにくく なる。 また、 この場合には、 推定の収束速度は低くなる。
[0033]
オブザーバゲイン Kは、 ジャイロセンサに基づき推定された傾斜角の高周波 成分はそのまま出力し、 低域及び直流成分は加速度センサに基づき推定された 傾斜角となるようにすることで、 直流成分から高周波域までの広範な周波数域 で高精度に傾斜角を推定できる。
[0034] 本発明において用いられる外力オブザーバは、 傾斜角の直流成分を推定する のが目的であるため、 その応答は遅くてもよい。 応答が遅いということは、 そ れだけノイズを低減できることを示すので、 より精度の高い推定が期待される。 また、 応答の遅いオブザーバとするため、 極は比較的実軸の近くに設定する。
[ 0 0 3 5 ]
本発明においては、 上述のように、 外力オブザーバには、 傾斜角に対応する 運動方程式 （モデル情報） が利用されている。 このモデル情報と 1軸ジャイロ センサにより検出した角速度に基づき、 中周波域〜高周波域において高精度に 傾斜角を推定している。 さらに、 本発明では、 1軸ジャイロセンサのみならず、 2軸加速度センサに基づき推定された傾斜角をオブザーバの参照値として、 こ れに推定値が追従するようにフィードバック制御している。 これによつて、 直 流成分から高周波域までの広範な周波数域で高精度に傾斜角を推定できる。 ま た、 加速度センサのノイズについては、 オブザーバゲイン Kを適切に設定する ことによって、 そのノイズの影響を低減することができる。
[ 0 0 3 6 ]
最後に設計した連続系外力オブザーバを離散化することによって外力ォブザ ーバを設計することができる。
[ 0 0 3 7 ]
さらに、 本発明では、 オブザーバゲイン Kの制御を制御機構により実行して いる。 図 3に当該制御のフローチャートを示す。 まず、 制御機構は、 ジャイロ センサ等によって検出した角速度が予め定められた所定範囲内であるかを判定 する （S 1 0 1 ) 。 ここで、 角速度を基準としだのは、 移動体が前後加速状態 にあるかどうかを検出することができるからである。 したがって、 図 3の示す 例では、 角速度に基づき移動体が前後加速状態にあるかどうかを検出したが、 これに限らず、 他のパラメータに基づき、 当該前後加速状態にあるかを検出す るようにしてもよレ、。 制御機構は、 角速度が所定範囲内であると判定した場合 には、 オブザーバゲイン Kの値を変更する制御は実行しない。
[ 0 0 3 8 ] 次に制御機構が、 角速度が所定範囲内にない、 即ち所定範囲外であると判定 した場合には、 さらに、 角速度が所定範囲よりも高いかどうかを判定する （S 1 0 2 ) 。 判定の結果、 制御機構が、 角速度が所定範囲よりも高いと判定した 場合には、 オブザーバゲイン Kの値を小さくする制御を実行する （S 1 0 3 ) 。 これにより、 前後加速状態にあり加速度センサに外乱ノイズが発生しやすい状 態にあるため、 加速度センサの影響を低減することができる。
[ 0 0 3 9 ]
判定の結果、 制御機構が、 角速度が所定範囲よりも高くない、 即ち低いと判 定した場合には、 オブザーバゲイン Kの値を大きくする制御を実行する （S 1 0 4 ) 。 なお、 オブザーバゲイン Kの値の大きさの制御については、 ォブザー バゲイン Kを小さく して推定処理した場合と、 オブザーバゲイン Kを大きく し て推定処理した場合のそれぞれの出力値のいずれかを選択するようにしてもよ レ、。
[ 0 0 4 0 ]
続いて、 センサの故障制御について説明する。 本発明では、 ジャイロセンサ と加速度センサを備えているが、 いずれかのセンサが故障した場合には、 故障 したセンサに基づく傾斜角推定は実行しないように、 即ち、 故障していないセ ンサのみに基づいて傾斜角推定を実行するように制御する。
[ 0 0 4 1 ]
具体的には、 制御機構は、 ウォッチドッグ等によって、 断線やセンサ故障等 の故障を検出したかどうかを判定する （S 2 0 1 ) 。 制御機構は、 故障を検出 したものと判定した場合に、 ジャイロセンサの故障かどうかを判定する （S 2 0 2 ) 。 制御機構は、 ジャイロセンサの故障であると判定した場合には、 故障 していない加速度センサにより車体傾斜角を推定する （S 2 0 3 ) 。 他方で、 制御機構は、 ジャイロセンサの故障でないと判定した場合には、 加速度センサ の故障かどうかを判定する （S 2 0 4 ) 。 判定の結果、 制御機構が加速度セン サの故障と判定した場合には、 故障していないジャィ口センサにより車体傾斜 角を推定する （S 2 0 5 ) 。 制御機構が、 加速度センサの故障でないと判定し た場合には、 推定処理を停止する （S 2 0 6 ) 。
[ 0 0 4 2 ]
さらに、 本発明においては、 移動体のモデル情報を用いて、 傾斜角の推定精 度を高めることができる。 例えば、 車両の構造上の制約から、 傾斜角が所定値 以上にならない場合には、 推定値がその所定値を超える場合に、 その所定値に 制限した上で推定処理を実行することにより真値と推定値の偏差が限られるた め、 推定精度の悪化を防止することが可能となる。
[ 0 0 4 3 ]
なお、 上述の例では、 移動体 1 0は、 倒立 2輪型の不安定車両であつたが、 これに限らず、 歩行型移動体やキヤタビラ駆動の移動体であってもよい。 また、 移動体 1 0は、 搭乗型の移動体であってもよく、 さらには、 ロボットの下半身 に組み込まれた移動体や、 荷物を搭載して移動する移動体であっても適用可能 である。 産業上の利用可能性
[ 0 0 4 4 ]
傾斜角推定機構を有する移動体に関し、 特に傾斜角推定機構を有する倒立 2 輪型車両に対して、 広く利用可能である。

Claims

請 求 の 範 囲
1. 2軸方向の加速度を検出する加速度センサと、
車体の角速度を検出するジャイロセンサと、
車体の傾斜角を推定する傾斜角推定機構とを備えた移動体であって、 当該傾斜角推定機構は、
前記加速度センサの検出した加速度に基づいて第 1の傾斜角を推定する手段 と、
前記移動体の線形モデル式に応じて前記ジャィ口センサの検出した角速度に 基づき、 第 2の傾斜角を推定する手段と、
前記第 1の傾斜角をオブザーバの参照値として、 前記第 2の傾斜角が当該第 1の傾斜角に追従するようにフィードバック制御することによつて傾斜角を推 定する手段を有する移動体。
2. 前記傾斜角推定機構は、 前記第 1の傾斜角の影響を調整するためのォブザ ーバゲイン Kを任意の値に設定する設定手段を有することを特徴とする請求項
1記載の移動体。
3. 前記設定手段は、 移動体が前後加速状態にある場合に、 前記オブザーバゲ ィン Kを小さい値に設定することを特徴とする請求項 2記載の移動体。
4. 前記設定手段は、 加速度センサ及びジャイロセンサの故障状態に応じて、 当該オブザーバゲイン Kを設定することを特徴とする請求項 2記載の移動体。
5. 前記加速度センサの後段に口一パスフィルタを設けたことを特徴とする請 求項 1〜 4いずれか 1項に記載の移動体。
6. 前記移動体は、 倒立 2輪型車両であることを特徴とする請求項 1〜5いず れか 1項に記載の移動体。
PCT/JP2007/069404 2006-10-05 2007-09-27 傾斜角推定機構を有する移動体 WO2008041732A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
EP07829143.2A EP2077432B1 (en) 2006-10-05 2007-09-27 Moving object with tilt angle estimating mechanism
CN2007800175536A CN101443628B (zh) 2006-10-05 2007-09-27 具有倾斜角推定机构的移动体
US12/297,323 US8000925B2 (en) 2006-10-05 2007-09-27 Moving body with tilt angle estimating mechanism

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2006273622A JP4281777B2 (ja) 2006-10-05 2006-10-05 傾斜角推定機構を有する移動体
JP2006-273622 2006-10-05

Publications (2)

Publication Number Publication Date
WO2008041732A1 true WO2008041732A1 (ja) 2008-04-10
WO2008041732A8 WO2008041732A8 (ja) 2008-07-03

Family

ID=39268585

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2007/069404 WO2008041732A1 (ja) 2006-10-05 2007-09-27 傾斜角推定機構を有する移動体

Country Status (5)

Country Link
US (1) US8000925B2 (ja)
EP (1) EP2077432B1 (ja)
JP (1) JP4281777B2 (ja)
CN (1) CN101443628B (ja)
WO (1) WO2008041732A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2328055A1 (en) * 2008-09-17 2011-06-01 Murata Manufacturing Co. Ltd. Fall prevention controller and computer program

Families Citing this family (29)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5110009B2 (ja) * 2009-03-11 2012-12-26 株式会社エクォス・リサーチ 車両
JP5109990B2 (ja) * 2009-01-26 2012-12-26 株式会社エクォス・リサーチ 車両
US8589059B2 (en) 2009-01-26 2013-11-19 Equos Research Co., Ltd. Vehicle with inclination estimation
CN102574559B (zh) * 2009-09-18 2014-08-20 本田技研工业株式会社 倒立摆型移动体
JP5488095B2 (ja) * 2010-03-23 2014-05-14 トヨタ自動車株式会社 倒立型移動体及びその制御方法
US9218316B2 (en) 2011-01-05 2015-12-22 Sphero, Inc. Remotely controlling a self-propelled device in a virtualized environment
US8571781B2 (en) 2011-01-05 2013-10-29 Orbotix, Inc. Self-propelled device with actively engaged drive system
US9090214B2 (en) 2011-01-05 2015-07-28 Orbotix, Inc. Magnetically coupled accessory for a self-propelled device
US10281915B2 (en) 2011-01-05 2019-05-07 Sphero, Inc. Multi-purposed self-propelled device
US9429940B2 (en) 2011-01-05 2016-08-30 Sphero, Inc. Self propelled device with magnetic coupling
CN103370039B (zh) * 2011-02-23 2015-10-14 株式会社村田制作所 步行辅助车
CN102174884B (zh) * 2011-02-28 2013-08-21 重庆华渝电气仪表总厂 定向陀螺测斜仪点测装置
US8548722B2 (en) * 2011-08-12 2013-10-01 Deere & Company Tilt sensor and method for determining the tilt of a vehicle
US9280717B2 (en) 2012-05-14 2016-03-08 Sphero, Inc. Operating a computing device by detecting rounded objects in an image
US9827487B2 (en) 2012-05-14 2017-11-28 Sphero, Inc. Interactive augmented reality using a self-propelled device
US10056791B2 (en) * 2012-07-13 2018-08-21 Sphero, Inc. Self-optimizing power transfer
WO2014061057A1 (ja) * 2012-10-16 2014-04-24 トヨタ自動車株式会社 倒立型移動体及びその制御方法
JP5907037B2 (ja) * 2012-10-16 2016-04-20 株式会社豊田中央研究所 移動体
FR2999699B1 (fr) * 2012-12-19 2015-12-11 Commissariat Energie Atomique Procede de determination de l'inclinaison d'un objet
JP6123906B2 (ja) * 2013-10-10 2017-05-10 株式会社村田製作所 手押し車
US9829882B2 (en) 2013-12-20 2017-11-28 Sphero, Inc. Self-propelled device with center of mass drive system
WO2015098722A1 (ja) * 2013-12-25 2015-07-02 株式会社村田製作所 手押し車
CN104296722B (zh) * 2014-01-07 2017-11-14 郑州宇通客车股份有限公司 车辆侧倾状态检测方法
JP5962689B2 (ja) * 2014-02-14 2016-08-03 トヨタ自動車株式会社 自律移動体、及びその故障判定方法
JP6187310B2 (ja) * 2014-02-24 2017-08-30 トヨタ自動車株式会社 倒立型移動体
KR20170070944A (ko) * 2015-12-14 2017-06-23 현대오트론 주식회사 가속도 센서 및 자이로 센서를 이용한 차량 피치각 검출 장치 및 그 방법
CN105509693B (zh) * 2015-12-31 2018-04-06 山东科技大学 一种移动式中部溜槽检测装置及其应用
US10551407B2 (en) * 2016-07-29 2020-02-04 Blackberry Limited Determining an open/close status of a barrier
CN110514178B (zh) * 2019-09-03 2021-11-26 北京源清慧虹信息科技有限公司 基于单轴加速度传感器的倾角测量方法和装置

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH03186417A (ja) * 1989-12-18 1991-08-14 Nissan Motor Co Ltd 車両のローリング制御装置
JPH04346021A (ja) 1991-05-23 1992-12-01 Japan Aviation Electron Ind Ltd 静電浮揚起動装置
US20020128795A1 (en) 1998-11-19 2002-09-12 Schiffmann Jan K. Vehicle attitude angle estimator and method
JP2002318274A (ja) * 2001-02-13 2002-10-31 Mazda Motor Corp 車体運動計測装置、車体運動計測方法及びその方法を記録した記録媒体
EP1508408A1 (en) 2002-04-26 2005-02-23 Honda Giken Kogyo Kabushiki Kaisha System for estimating attitude of leg type moving robot itself
US20050149240A1 (en) 2004-01-07 2005-07-07 Tseng Hongtei E. Attitude sensing system for an automotive vehicle relative to the road
WO2005063536A1 (de) * 2003-12-23 2005-07-14 Daimlerchrysler Ag Verfahren und vorrichtung zur bestimmung eines fahrzeugzustandes
JP2006220490A (ja) * 2005-02-09 2006-08-24 Kubota Corp 傾斜角度計測装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6581714B1 (en) * 1993-02-24 2003-06-24 Deka Products Limited Partnership Steering control of a personal transporter
JP4346021B2 (ja) 2001-08-16 2009-10-14 独立行政法人理化学研究所 Ｖ−ｃａｄデータを用いたラピッドプロトタイピング方法と装置
JP4138546B2 (ja) 2003-03-26 2008-08-27 トヨタ自動車株式会社 移動台車及び移動台車の制御方法
JP4145741B2 (ja) * 2003-07-03 2008-09-03 三菱電機株式会社 車両のロールオーバ判別装置および車両のロールオーバ判別方法
EP1529556B1 (en) * 2003-11-04 2013-02-20 Toyota Jidosha Kabushiki Kaisha Travelling apparatus and method for controlling thereof

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH03186417A (ja) * 1989-12-18 1991-08-14 Nissan Motor Co Ltd 車両のローリング制御装置
JPH04346021A (ja) 1991-05-23 1992-12-01 Japan Aviation Electron Ind Ltd 静電浮揚起動装置
US20020128795A1 (en) 1998-11-19 2002-09-12 Schiffmann Jan K. Vehicle attitude angle estimator and method
JP2002318274A (ja) * 2001-02-13 2002-10-31 Mazda Motor Corp 車体運動計測装置、車体運動計測方法及びその方法を記録した記録媒体
EP1508408A1 (en) 2002-04-26 2005-02-23 Honda Giken Kogyo Kabushiki Kaisha System for estimating attitude of leg type moving robot itself
WO2005063536A1 (de) * 2003-12-23 2005-07-14 Daimlerchrysler Ag Verfahren und vorrichtung zur bestimmung eines fahrzeugzustandes
US20050149240A1 (en) 2004-01-07 2005-07-07 Tseng Hongtei E. Attitude sensing system for an automotive vehicle relative to the road
JP2006220490A (ja) * 2005-02-09 2006-08-24 Kubota Corp 傾斜角度計測装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP2077432A4

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2328055A1 (en) * 2008-09-17 2011-06-01 Murata Manufacturing Co. Ltd. Fall prevention controller and computer program
EP2328055A4 (en) * 2008-09-17 2014-07-16 Murata Manufacturing Co FALL PREVENTION CONTROLLER AND COMPUTER PROGRAM THEREFOR

Also Published As

Publication number Publication date
EP2077432A1 (en) 2009-07-08
JP2008089531A (ja) 2008-04-17
WO2008041732A8 (ja) 2008-07-03
CN101443628B (zh) 2010-12-08
US8000925B2 (en) 2011-08-16
US20090138232A1 (en) 2009-05-28
EP2077432B1 (en) 2014-03-19
EP2077432A4 (en) 2012-11-14
CN101443628A (zh) 2009-05-27
JP4281777B2 (ja) 2009-06-17

Similar Documents

Publication Publication Date Title
WO2008041732A1 (ja) 傾斜角推定機構を有する移動体
EP2517941B1 (en) Roll angle estimation device and transport equipment
JP4760162B2 (ja) 移動台車の制御方法及び移動台車
KR101156822B1 (ko) 이동체 및 이동체의 제어 방법
EP3441718B1 (en) Orientation estimation device and transport equipment
WO2007148818A1 (ja) 姿勢角検出装置と姿勢角検出方法
JP2007182209A (ja) 車両物理量推定装置及びプロブラム
JP2015209106A (ja) ロール角推定装置および輸送機器
JP5919889B2 (ja) 車両姿勢制御装置
CN110109354B (zh) 一种反作用轮平衡自行车机器人自适应滑模控制方法
US10919358B2 (en) Attitude estimation apparatus and transportation machine
KR101117040B1 (ko) 도립 진자형 이동 기구
EP3666634A1 (en) Leaning vehicle
JP6604175B2 (ja) ピッチ角速度補正値算出装置、姿勢角算出装置およびピッチ角速度補正値算出方法
JP2019501055A (ja) 車両の挙動検出装置
JP5907037B2 (ja) 移動体
JP7391616B2 (ja) 移動体の姿勢推定装置
JP6454857B2 (ja) 姿勢検出装置及び姿勢検出方法
JP2014070939A (ja) 車両の操舵角演算装置
US20230314462A1 (en) Inertial sensor
JP2022104270A (ja) 車両状態推定装置、車両状態推定方法、及び、車両状態推定プログラム
Ďuriš et al. A Two Wheel Self-Balancing Vehicle
CN114763994A (zh) 一种应用于扫地机器人的惯性姿态导航系统
CN107193208A (zh) 一种智能车单边行驶的控制方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 07829143

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 12297323

Country of ref document: US

WWE Wipo information: entry into national phase

Ref document number: 200780017553.6

Country of ref document: CN

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2007829143

Country of ref document: EP