CN101443628A

CN101443628A - 具有倾斜角推定机构的移动体

Info

Publication number: CN101443628A
Application number: CNA2007800175536A
Authority: CN
Inventors: 不破稔夫; 中岛一诚
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2006-10-05
Filing date: 2007-09-27
Publication date: 2009-05-27
Anticipated expiration: 2027-09-27
Also published as: WO2008041732A8; US20090138232A1; EP2077432B1; US8000925B2; JP2008089531A; EP2077432A4; JP4281777B2; WO2008041732A1; EP2077432A1; CN101443628B

Abstract

本发明提供一种具有能够高精度地推定出车身倾斜角度的倾斜角推定机构的移动体。本发明的移动体包括：加速度传感器，检测二轴方向上的加速度；陀螺传感器，检测车身的角速度；以及倾斜角推定机构，推定车身的倾斜角。该倾斜角推定机构具有：根据加速度传感器检测出的加速度来推定与加速度传感器相应的倾斜角的单元；根据所述陀螺传感器检测出的角速度来推定第二倾斜角的单元；以及根据通过加速度传感器得到的倾斜角和通过陀螺传感器得到的倾斜角，将移动体的线性模型方程作为观察器来推定倾斜角的单元。

Description

具有倾斜角推定机构的移动体

技术领域

本发明涉及具有倾斜角推定机构的移动体，特别涉及具有倾斜角推定机构的倒立两轮型车辆。

背景技术

在倒立两轮型车辆这样的移动体中，为了维持倒立姿势而需要高精度地推定出倾斜角度。这不仅限于两轮型车辆，即使在步行型移动体或履带驱动的移动体中，也要求高精度地推定出倾斜角度。

以往，作为推定倾斜角的机构而有使用陀螺(角速度)传感器的方式或使用加速度传感器的方式。在使用陀螺传感器的方式中，可以通过对陀螺传感器的输出信号进行积分而求出倾斜角度，但是在陀螺传感器的输出信号上原本就重叠有偏移噪声(offset noise)，因此存在着相对于真值偏差逐渐变大的问题。

在使用加速度传感器的方式中，使用通过二轴以上的加速度传感器得到的几何学关系来计算倾斜角度。根据该方式，由于根据二轴重力加速度的值来计算倾斜角度，因此存在着如果不是静止的状态则推定精度会变差的问题。特别是在移动体进行了加减速的情况下，由于加减速而产生的成分会作为噪声而重叠在二轴加速度传感器上。

另外，还提出了使用陀螺传感器和三轴加速度传感器并通过自动惯性导航(INS，惯性导航系统)来推定倾斜角度的方式。但是，无法解决上述陀螺传感器中的偏移噪声的问题和加速度传感器中的由于加减速而产生的噪声的问题。

另外，专利文献1公开了按照每个频带切换使用陀螺传感器和加速度计的技术，但这不是与具有倾斜角推定机构的移动体相关的技术。

专利文献1：日本专利文献特开平4—346021号公报。

发明内容

发明所要解决的问题

如上所述，虽然能够通过使用陀螺传感器和加速度传感器求出车身倾斜角度，但是在精度上存在问题。

本发明是为了解决上述问题而完成的，其目的在于提供一种具有能够高精度地推定出车身倾斜角度的倾斜角推定机构的移动体。

用于解决问题的手段

本发明的移动体包括：加速度传感器，检测二轴方向上的加速度；陀螺传感器，检测车身的角速度；以及倾斜角推定机构，推定车身的倾斜角；其中，该倾斜角推定机构具有：根据所述加速度传感器检测出的加速度来推定第一倾斜角的单元；按照所述移动体的线性模型方程并根据所述陀螺传感器检测出的角速度来推定第二倾斜角的单元；以及通过将所述第一倾斜角作为观察器的参照值并进行反馈控制以使所述第二倾斜角追随所述第一倾斜角来推定倾斜角的单元。

这里，优选的是，所述倾斜角推定机构具有将用于调整所述第一倾斜角的影响的观察器增益K设定为任意值的设定单元。

另外，优选的是，所述设定单元在移动体处于前后加速状态的情况下将所述观察器增益K设定为小的值。

另外，优选的是，所述设定单元根据加速度传感器和陀螺传感器的故障状态来设定该观察器增益K。

另外，优选的是，在所述加速度传感器的后级设置有低通滤波器。

优选实施方式中的移动体是倒立两轮型车辆。

发明的效果

根据本发明，可以提供一种具有能够高精度地推定出车身倾斜角度的倾斜角推定机构的移动体。

附图说明

图1是用于说明本发明实施方式的移动体的传感器座标系的图；

图2是表示本发明实施方式的移动体的倾斜角推定方法的框图；

图3是表示本发明实施方式的移动体的倾斜角推定方法的流程图；

图4是表示本发明实施方式的移动体的倾斜角推定方法的流程图。

标号说明：

1 车身

2 车轮

10 移动体

具体实施方式

图1表示了本发明的具有倾斜角推定机构的移动体的传感器座标系。如图1所示，该移动体10具有车身1和经由未图示的驱动机构而固定在车身1上的车轮2。

将与该移动体10的前方(即，移动方向、倾斜(pitch)方向)正交并沿近似水平方向(即，侧倾(roll)方向)延伸的轴作为X轴，将该移动体10的前方(即，移动方向、倾斜方向)作为Y轴，将铅垂方向作为Z轴。在本例中，将连结车身1和车轮2的旋转中心的线与Z轴所成的角度作为车身倾斜角。

移动体10包括测量Y轴方向和Z轴方向上的加速度的加速度传感器和设置在其后级的截止频率低的低通滤波器。可以根据由这两个加速度传感器检测出的Y轴方向和Z轴方向上的加速度u(1)、u(2)，按照下式求出车身倾斜角(即，倾角(pitch angle))η。

η＝tan^-1(u(1)/u(2))…(1)

另外，该移动体10包括测量X轴方向上的角速度的单轴陀螺传感器(角速度传感器)。并且，在移动体10中，通过计算机来实现输入加速度传感器和陀螺传感器的检测信号和控制信号并推定(计算)出车身倾斜角的倾斜角推定机构。

该倾斜角推定机构包括：根据加速度传感器检测出的加速度来推定倾斜角的单元；根据陀螺传感器检测出的角速度来推定倾斜角的单元；以及根据通过加速度传感器得到的倾斜角和通过陀螺传感器得到的倾斜角并将移动体的线性模型方程(linear model equation)作为观察器来推定倾斜角的单元。此时，推定倾斜角的单元将通过陀螺传感器得到的倾斜角作为观察器的参照值，并按照使推定值反馈追随通过陀螺传感器得到的倾斜角的方式来进行推定。

图2是用于说明本发明的倾斜角推定处理的框图。如图所示，向I₁输入控制信号，向I₂输入由Y轴加速度传感器检测出的Y轴方向上的加速度信号，向I₃输入由Z轴加速度传感器检测出的Z轴方向上的加速度信号，向I₄输入由单轴陀螺传感器检测出的角速度信号。

首先，根据输入给I₂的Y轴方向上的加速度和输入给I₃的Z轴方向上的加速度，按照上述式(1)以几何学方式计算出车身倾斜角。这里计算出的倾斜角对于直流(DC)成分～低频带来说具有高精度，但是由于当伴随着加减速而运动时加速度传感器被施加了噪声，因此对于中频带～高频带来说精度低。

接着，输入控制信号、计算出的车身倾斜角、由陀螺传感器检测出的角速度，并且通过由外力观察器进行的反馈处理最终计算出倾斜角、倾斜角速度、以及外力，分别从O₁、O₂、O₃输出。另外，在图2中，将与外力观察器相关的式y(n)＝Cx(n)+Du(n)作为式(2)，将x(n+1)＝Ax(n)+Bu(n)作为式(3)。

接着，对外力观察器的设计的详细情况进行说明。

首先，对车身倾斜角进行线性模型化。倒立两轮车的车身模型(运动方程式)可以如下来表示。

[数1]

(m_{1} l^{2} + J_{1} + n^{2} J_{m}) \overset{\cdot \cdot}{η} + (m_{1} rl - n^{2} J_{m}) \overset{\cdot \cdot}{θ} - m_{1} glη + f_{r} (\overset{\cdot}{η} - \overset{\cdot}{θ}) = - nu - - - (4)

(m_{1} rl - n^{2} J_{m}) \overset{\cdot \cdot}{η} + {(m_{1} + m_{2}) r^{2} + J_{w} + n^{2} J_{m}} \overset{\cdot \cdot}{θ} + f_{r} (\overset{\cdot}{θ} - \overset{\cdot}{η}) = nu - - - (5)

这里，η：车身的倾斜角，θ：车轮的旋转角，m₁：车身的质量，m₂：车轮的质量，1：车身重心距车轴的距离，J₁：关于车身重心的惯性力矩，J_m：马达转子的惯性力矩，J_w：关于车轮轴的惯性力矩，n：齿轮比(马达：车轮＝1：n)，r：车轮半径，fr：驱动系统的阻力(resistance)，u：马达的转矩值。

在上述式(4)、(5)中，由于在本发明中能够推定出车身倾斜角η即可，因此仅考虑式(4)。另外，在求倾斜角速度的情况下利用式(5)。如果通过η来表示式(4)的左边，则成为式(6)。

[数2]

(m_{1} l^{2} + J_{1} + n^{2} J_{m}) \overset{\cdot \cdot}{η} + f_{r} \overset{\cdot}{η} - m_{1} glη = - nu - (m_{1} rl - n^{2} J_{m}) \overset{\cdot \cdot}{θ} + f_{r} \overset{\cdot}{θ} - - - (6)

将该式(6)作为基本模型来生成外力观察器。这里，式(6)右边的θ项作为η方程式的外力而发挥作用。在本发明中，如果忽视该θ项，则推定误差会变大，因此为了避免这种情况，构成了将外力f也作为状态量来进行考虑的外力观察器。

下面，对外力观察器的基本模型的导出进行说明。现在，通过下式(7)来表示状态量。

[数3]

X = [\begin{matrix} η \\ \overset{\cdot}{η} \\ f \end{matrix}] - - - (7)

这里，f为外力。

当使用式7表示的状态量将式6转换成状态方程式表示方式时，可以表示成下式(8)、(9)。

[数4]

\overset{\cdot}{X} = AX + Bu - - - (8)

Y＝CX (9)

A = [\begin{matrix} 0 & 1 & 0 \\ m_{1} gl / M & - f_{r} / M & 1 / M \\ 0 & 0 & 0 \end{matrix}]

B = [\begin{matrix} 0 \\ - n / M \\ 0 \end{matrix}]

C = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{matrix}]

M＝m₁l²+J₁+n²J_m

这里，对于外力f来说，在采样周期的期间内为取恒定值的阶跃(stepwise)状，该期间的微分值为0。

下面，对外力观察器的增益的确定方法进行说明。

外力观察器可以通过下式(10)来表示。

：推定值

\overset{\overset{\cdot}{^}}{X} = A \hat{X} + Bu + K (Y - C \hat{X}) - - - (10)

K：观测器增益

对于式(10)中的观察器增益(反馈增益)K来说，针对各个状态量η、η点、f而分别设定为K1、K2、K3。根据式(10)中的观察器增益K的大小来决定推定的收敛速度。即，根据由式(10)求出的特性方程式的极的大小来决定推定的收敛速度。

当K＝(K1，K2，K3)时，如果K1大，则接近于通过加速度传感器得到的推定值，如果K2大，则接近于通过陀螺传感器得到的推定值。由于在低频中判断加速度传感器的推定值正确，因此增大K2，在中～高频中增大K2。在能够预先推定出动作频率的情况下，根据该频率来设定K即可。

具体地说，考虑以下(A)、(B)之间的平衡来决定K。

(A)观察器增益K大(极距离实轴远)时

在该情况下，会大大地受到作为加速度传感器输入的观测量的影响。因此，对于推定值来说，使观测量优先，基本模型的影响变小。因此，容易受到传感器噪声的影响。另外，在该情况下，推定的收敛速度变高。

(B)观察器增益K小(极距离实轴近)时

在该情况下，大大地使作为加速度传感器输入的观测量平滑化。换言之，减小观测量的影响。因此，基本上推定值遵照基本模型。因此，难以受到传感器噪声的影响。另外，在该情况下，推定的收敛速度变低。

对于观察器增益K来说，原样输出根据陀螺传感器推定出的倾斜角的高频成分，使低频带和直流成分成为根据加速度传感器推定出的倾斜角，由此能够在从直流成分到高频带的宽的频带中高精度地推定出倾斜角。

在本发明中使用的外力观察器用于推定倾斜角的直流成分，因此其响应可以慢一些。所谓响应慢意味着能够相应地降低噪声，因此能够实现精度更高的推定。另外，由于使其成为响应慢的观察器，因此极设定得比较靠近实轴。

在本发明中，如上所述，与倾斜角对应的运动方程式(模型信息)被利用于外力观察器。根据该模型信息和由单轴陀螺传感器检测出的角速度在中频带～高频带中高精度地推定出倾斜角。并且，在本发明中，不但将根据单轴陀螺传感器推定出的倾斜角作为观察器的参照值，还将根据二轴加速度传感器推定出的倾斜角作为观察器的参照值，进行反馈控制以使推定值追随该参照值。由此，能够在从直流成分到高频带的宽的频带中高精度地推定出倾斜角。另外，对于加速度传感器的噪声来说，通过恰当地设定观察器增益K，能够降低该噪声的影响。

最后通过使设计出的连续系统外力观察器离散化，能够设计外力观察器。

另外，在本发明中，通过控制机构来控制观察器增益K。图3表示了该控制的流程图。首先，控制机构判断由陀螺传感器等检测出的角速度是否处于预先确定的预定范围内(S101)。这里，以角速度为基准的原因在于能够检测出移动体是否处于前后加速状态。因此，在图3所示的例子中根据角速度检测出移动体是否处于前后加速状态，但是不限于此，也可以根据其他参数来检测是否处于该前后加速状态。控制机构在判断出角速度处于预定范围内的情况下，不执行改变观察器增益K的值的控制。

接着，控制机构在判断出角速度不处于预定范围内、即处于预定范围外的情况下，还判断角速度是否比预定范围高(S102)。在判断的结果是控制机构判断出角速度比预定范围高的情况下，执行减小观察器增益K的值的控制(S103)。由此，由于处于前后加速状态而处于加速度传感器容易产生干扰噪声的状态，因此能够降低加速度传感器的影响。

在判断的结果是控制机构判断出角速度不比预定范围高、即比预定范围低的情况下，执行增大观察器K的值的控制(S104)。另外，对于观察器增益K的值的大小的控制来说，也可以选择减小观察器增益K而进行了推定处理的情况下和增大观察器增益K而进行了推定处理的情况下的各自的输出值中的某个输出值。

接下来，对传感器的故障控制进行说明。本发明包括陀螺传感器和加速度传感器，在某个传感器发生了故障的情况下，按照如下方式来进行控制：不执行基于发生了故障的传感器的倾斜角推定，而是仅根据没有发生故障的传感器来执行倾斜角推定。

具体地说，控制机构判断是否通过监视器等检测出了断线或传感器故障等故障(S201)。控制机构在判断为检测出了故障的情况下，判断是否是陀螺传感器的故障(S202)。控制机构在判断出是陀螺传感器的故障的情况下，通过没有发生故障的加速度传感器来推定车身倾斜角(S203)。另一方面，控制机构在判断出不是陀螺传感器的故障的情况下，判断是否是加速度传感器的故障(S204)。在判断的结果是控制机构判断出是加速度传感器的故障的情况下，通过没有发生故障的陀螺传感器来推定车身倾斜角(S205)。控制机构在判断为不是加速度传感器的故障的情况下停止推定处理(S206)。

另外，在本发明中使用了移动体的模型信息，能够提高倾斜角的推定精度。例如，在由于车辆的构造方面的限制而使得倾斜角不会大于等于预定值的情况下，如果推定值超过了该预定值，则在将该推定值限制为该预定值的基础上来进行推定处理，由此真值与推定值的偏差受到了限制，从而能够防止推定精度变差。

另外，在上述例子中移动体10是倒立两轮型的不稳定车辆，但是不限于此，也可以是步行型移动体或履带驱动的移动体。另外，移动体10也可以是搭乘型的移动体，还可以是组装到机器人的下半身的移动体或搭载货物而移动的移动体。

产业上的可利用性

本发明涉及具有倾斜角推定机构的移动体，特别是可以广泛地利用于具有倾斜角推定机构的倒立两轮型车辆。

Claims

1.一种移动体，包括：加速度传感器，检测二轴方向上的加速度；陀螺传感器，检测车身的角速度；以及倾斜角推定机构，推定车身的倾斜角；其中，

该倾斜角推定机构具有：

根据所述加速度传感器检测出的加速度来推定第一倾斜角的单元；

按照所述移动体的线性模型方程并根据所述陀螺传感器检测出的角速度来推定第二倾斜角的单元；以及

通过将所述第一倾斜角作为观察器的参照值并进行反馈控制以使所述第二倾斜角追随所述第一倾斜角来推定倾斜角的单元。

2.如权利要求1所述的移动体，其中，

所述倾斜角推定机构具有将用于调整所述第一倾斜角的影响的观察器增益K设定为任意值的设定单元。

3.如权利要求2所述的移动体，其中，

所述设定单元在移动体处于前后加速状态的情况下将所述观察器增益K设定为小的值。

4.如权利要求2所述的移动体，其中，

所述设定单元根据加速度传感器和陀螺传感器的故障状态来设定该观察器增益K。

5.如权利要求1至4中任一项所述的移动体，其中，

在所述加速度传感器的后级设置有低通滤波器。

6.如权利要求1至5中任一项所述的移动体，其中，

所述移动体是倒立两轮型车辆。