CN102405449B

CN102405449B - 倒立型移动体的控制装置

Info

Publication number: CN102405449B
Application number: CN2010800176070A
Authority: CN
Inventors: 安藤玄
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2009-04-28
Filing date: 2010-02-26
Publication date: 2013-08-21
Anticipated expiration: 2030-02-26
Also published as: US8612127B2; EP2426566A1; JP5093185B2; CN102405449A; EP2426566A4; WO2010125727A1; EP2426566B1; US20110071728A1; JP2010257399A

Abstract

当负荷角度θ₁接近负荷角度指令θ₁ ^*时，进行控制以向移动体主体(141)仅施加衰减。即，控制装置(110)包括情况区分线性转矩计算器(123)以及控制切换器(124)。情况区分线性转矩计算器(123)计算衰减转矩和线性反馈转矩，所述衰减转矩是向负荷速度和衰减参数的乘积值添上负号而得到的，所述线性反馈转矩是对位置偏差、速度偏差以及加速度偏差中的一个以上偏差乘以预定增益而得到的。控制切换器(124)切换并输出所述衰减转矩和所述线性反馈转矩。控制切换器(124)在负荷角度θ₁接近负荷角度指令θ₁ ^*时，输出衰减转矩，否则输出所述线性反馈转矩。由此，倒立型移动体能够不振动而稳定地以期望的速度行驶。

Description

倒立型移动体的控制装置

技术领域

本发明涉及倒立型移动体的控制装置(control portion，控制部)。具体涉及包括车轮驱动单元和杆状的负荷体、并在进行使杆状的负荷体倒立的平衡控制的状态下移动的倒立型移动体的控制装置。

背景技术

公知有包括同轴设置的左右一对车轮、并在维持倒立状态的情况下行驶的移动装置。例如，专利文献1(日本专利文献特开2006-123014号公报)公开了在维持倒立状态的情况下自动行驶的倒立二轮行驶机器人。另外，专利文献2(日本专利文献特开2006-315666号公报)公开了在人以站立姿态搭乘在踏板上的状态下保持平衡并行驶的同轴二轮车。

图10是示出专利文献1所公开的倒立二轮行驶机器人的控制器的构成的图。

在图10中，附图标记1001是摩擦估计器(friction observer)，1002是目标状态生成器(target state generator)，1003是状态反馈增益(statefeedback gains)，1004是倒立机器人(inverted robot)。

角速度指令值(angular speed reference input)被输入给摩擦估计器1001，摩擦估计器1001计算并输出马达的摩擦、以及车轮与路面的摩擦，作为摩擦估计值(estimated friction)。

所述加速度指令值和所述摩擦估计值被输入给目标状态生成器1002输入，目标状态生成器1002计算并输出作为控制对象(plant)的倒立机器人1004的目标状态(target state)。

从所述目标状态减去倒立机器人1004的状态变量的信号被输入给状态反馈增益1003，状态反馈增益1003基于该输入信号来计算使倒立机器人1004进行期望的(desired)动作的状态反馈信号(state feedbacksignals)并输出该状态反馈信号。

倒立机器人1004通过所述摩擦估计值和所述状态反馈信号的相加值而被驱动。

这样，在现有技术(conventional method)的倒立二轮行驶机器人的控制中，基于将作为控制对象的倒立机器人1004在期望姿态的附近(inthe vicinity of)线性化了的线性化模型来控制倒立机器人1004的动作。

现有技术文献

专利文献

专利文献1：日本专利文献特开2006-123014号公报(图4)。

发明内容

发明要解决的问题

如上所述，在现有的倒立控制中进行简单的线性反馈控制，但倒立机器人和同轴二轮车具有在同轴车轮上设置有长杆的构造，因此有作为负荷的杆部分不能以目标姿态保持稳定而容易晃动的特点。

对于这样的构成，如果如以往那样持续施加与相对于目标姿态的偏差相应的反馈，则会导致不能稳定地保持目标姿态而在目标附近发生摆动的问题。

以往，针对上述的在目标附近的摆动，调整了反馈增益以抑制振动，但仅通过简单地调整反馈增益，难以进行下述控制：一方面在大幅度偏离目标时迅速恢复到目标姿态以免跌倒，而另一方面在目标附近防止振动。

本发明的目的在于，提供一种能够进行不导致振动而稳定的倒立平衡控制、并且使倒立型移动体以期望的水平速度行驶的倒立型移动体的控制装置。

用于解决问题的手段

为了解决上述问题，本发明如下构成。

即，本发明的倒立型移动体的控制装置在使移动体主体(invertedvehicle)倒立的状态下控制该移动体主体的行驶，所述移动体主体包括具有车轮的驱动单元、以及经由连杆在所述车轮上被倒立控制的负荷，所述倒立型移动体的控制装置的特征在于，将连结所述负荷的重心和所述车轮的重心(center of gravity)的直线与竖直线所形成的角定义为负荷角度，如果所述负荷角度接近负荷角度指令(load angular position referenceinput)，则所述控制装置进行控制以向所述移动体主体仅施加衰减(damping)，其中所述负荷角度指令是期望的所述负荷角度。

在本发明中，优选通过对所述负荷角度指令的绝对值乘以预定系数来计算作为所述负荷角度指令的附近的宽度的衰减范围。

在本发明中，优选将所述衰减定义为粘性摩擦(viscous friction)。

在本发明中，优选将作为所述粘性摩擦的衰减参数定义为负荷角度跟踪偏差(tracking error)和所述负荷角度指令的函数来计算，所述负荷角度跟踪偏差是从所述负荷角度指令减去所述负荷角度而得的值。

在本发明中，优选通过从所述负荷角度跟踪偏差减去将所述负荷角度指令的绝对值除以2而得的值，并且将该差的绝对值除以所述负荷角度指令的绝对值并乘以常数，来计算所述衰减参数。

在本发明中，优选将作为所述粘性摩擦的衰减参数定义为恒定值。

在本发明中，优选包括：情况区分(switching)线性转矩计算器，其计算衰减转矩和线性反馈转矩，所述衰减转矩是向所述负荷速度和所述衰减参数的乘积值添上负号而得到的，所述线性反馈转矩是对位置偏差(position tracking error)、速度偏差(speed tracking error)以及加速度偏差(acceleration tracking error)中的一个以上偏差乘以预定增益而得到的；以及控制切换器，其切换并输出通过所述情况区分线性转矩计算器算出的所述衰减转矩和所述线性反馈转矩。

在本发明中优选如下：所述控制切换器

如果0≤sgn(θ₁ ^*)·e＜h，则输出所述衰减转矩，

否则输出所述线性反馈转矩，

其中，e＝θ₁ ^*-θ₁；θ₁ ^*是负荷角度指令；θ₁是负荷角度；sgn(·)是·为正数时取+1、·为负数时取-1、·为零时取0值的符号函数；h是通过向所述负荷角度指令的绝对值乘以预定系数而算出的衰减范围。

发明效果

根据上述的本发明，能够使得移动体的负荷角度不在期望值的附近振动。并且，能够使移动体的负荷角度不振动地快速收敛于期望值，使移动体以期望的速度安全行驶。

附图说明

图1是示出本发明倒立型移动体涉及的第一实施方式的图；

图2是将移动体主体模型化了的图；

图3是示出负荷角度的仿真结果的图；

图4是示出车轮水平速度的仿真结果的图；

图5是示出变形例1的图；

图6是示出变形例2的图；

图7是示出作为倒立型移动体的同轴二轮车的图；

图8是示出作为倒立型移动体的倒立型自动行驶机器人的图；

图9是示出在四轮的车轮驱动单元上可摆动地设置有杆机构的倒立型移动体的图；

图10是示出现有的倒立二轮行驶机器人的控制器的构成的图。

具体实施方式

以下，参照附图来说明本发明的实施方式。

(第一实施方式)

图1是示出本发明倒立型移动体涉及的第一实施方式的图。

倒立型移动体包括：作为控制对象的移动体主体141；状态传感器(sensors)142，其检测移动体主体141的状态；指令部(referenceportion)100，其指示期望的目标状态；以及控制装置110，其基于状态传感器142检测的检测信号(measurement signal)和来自指令部100的指令值来执行移动体主体141的控制。

作为移动体主体141，可举出同轴二轮车(图7)、倒立型自动行驶机器人(图8)等一般的例子。

不限于此，只要是包括基于车轮的驱动单元以及杆状的负荷体、并进行使杆状的负荷体倒立的平衡控制的移动体即可。

例如，也可以是图9所示的构成。

图9是在四轮的车轮驱动单元901上可摆动地设置有杆机构902的构成。

例如，也可以将杆机构902的上部做成篮筐状903，以装载物品来运送。

此外在以下的说明中，将上述的移动体主体141如图2那样模型化。

这里，在图2中，附图标记201是负荷，202是车轮，203是路面。

如图2所示，移动体主体141倒立行驶。

负荷201是机器人的机身、搭乘在移动体141上的人或货物。

车轮202通过承载负荷201并旋转，从而利用与路面203的摩擦力而移动。

状态传感器142检测负荷201的角度(θ₁)以及车轮202的角度(θ₂)。

指令部100包括车轮水平速度指令生成器(wheel horizontal speedreference input generator)101以及负荷角度指令计算器(load anglereference input unit)102。

车轮水平速度指令生成器101生成并输出车轮水平速度指令，该车轮水平速度指令是移动体主体141的车轮202的期望的水平移动速度。

负荷角度指令计算器102将车轮水平速度指令作为输入，计算并输出负荷角度指令，该负荷角度指令是在移动体主体141所行驶的路面203水平时使得车轮水平速度跟踪(track)车轮水平速度指令的负荷角度。

控制装置110包括：情况区分线性控制部(switching linear controlportion)120、非线性控制部(nonlinear control portion)130、以及转矩指令(torque reference input)计算器111。

情况区分线性控制部120包括：衰减范围计算器(damping rangeunit)121、衰减参数计算器(damping parameter unit)122、情况区分线性转矩计算器(switching linear torque unit)123、以及控制切换器(controlswitching unit)124。

来自负荷角度指令计算器102的负荷角度指令、以及作为状态传感器142的检测信号的负荷角度(θ₁)和车轮角度(θ₂)被输入给衰减范围计算器121。

衰减范围计算器121基于输入信号来计算并输出在移动体主体141的动作控制中仅施加利用了粘性摩擦的衰减的负荷角度的范围，作为衰减范围。

来自负荷角度指令计算器102的负荷角度指令(θ₁ ^*)、以及作为状态传感器142的检测信号的负荷角度(θ₁)和车轮角度(θ₂)被输入给衰减参数计算器122。

衰减参数计算器122基于输入信号来计算并输出在所述衰减范围中的控制中使用的衰减参数。

来自负荷角度指令计算器102的负荷角度指令(θ₁ ^*)、来自衰减参数计算器122的衰减参数、以及作为状态传感器142的检测信号的负荷角度(θ₁)和车轮角度(θ₂)被输入给情况区分线性转矩计算器123。

情况区分线性转矩计算器123计算并输出衰减转矩以及线性反馈转矩，该衰减转矩是向负荷速度和衰减参数的乘积值添加负号而得到的，该线性反馈转矩是对位置偏差、速度偏差以及加速度偏差中的一个以上偏差乘以预定增益而得到的。

通过衰减范围计算器121算出的衰减范围、状态传感器142检测到的检测信号、以及通过情况区分线性转矩计算器123算出的情况区分线性转矩被输入给控制切换器124。

控制切换器124切换并输出通过情况区分线性转矩计算器123算出的情况区分线性转矩。

非线性控制部130包括：车轮垂直加速度估计器(wheel verticalacceleration observer)131、车轮水平速度估计器(wheel horizontal speedobserver)132、非线性转矩计算器(nonlinear torque unit)133。

来自状态传感器142的检测信号被输入给车轮垂直加速度估计器131，车轮垂直加速度估计器131基于该输入信号来估计车轮202的垂直加速度，并将其作为车轮垂直加速度估计值而输出。

来自状态传感器142的检测信号被输入给车轮水平速度估计器132，车轮水平速度估计器132基于该输入信号来估计车轮202的水平速度，并将其作为车轮水平速度估计值而输出。

所述车轮垂直加速度估计值和所述车轮水平速度估计值被输入给非线性转矩计算器133，非线性转矩计算器133计算并输出非线性转矩，该非线性转矩表示移动体主体141的非线性动态。

通过控制切换器124被切换输出的所述情况区分线性转矩以及从非线性转矩计算器133输出的所述非线性转矩被输入给转矩指令计算器(torque reference input unit)111，转矩指令计算器111输出将这些输入信号的相加值除以所述车轮202的半径而得到的转矩指令。

移动体主体141根据所述转矩指令而被驱动。

以下，对由本第一实施方式涉及的控制装置110控制移动体主体141的动作的详细的控制机制进行说明。

在图2中，如下设定参数。

m₁为负荷质量，

J₁为负荷惯性矩，

m₂为车轮质量，

J₂为车轮惯性矩，

I为作为负荷与车轮的重心间距离的负荷车轮重心间距离，

r为车轮半径，

θ₁为负荷角度，

θ₂为车轮角度，

T_ref为转矩指令。

并且，当将车轮水平位置作为x₂、将车轮垂直位置作为y₂时，负荷水平位置x₁和负荷垂直位置y₁如下分别通过式(1)和式(2)表示。

[式1]

x₁＝lsinθ₁+x₂ (1)

[式2]

y₁＝lcosθ₁+y₂ (2)

移动体主体141的动能(kinetic energy)T和势能(potential energy)V使用式(1)和式(2)如下分别通过式(3)和式(4)表示。

[式3]

T = \frac{1}{2} m_{1} ({\overset{\cdot}{x}}_{1}^{2} + {\overset{\cdot}{y}}_{1}^{2}) + \frac{1}{2} J_{1} {\overset{\cdot}{θ}}_{1}^{2}

+ \frac{1}{2} m_{2} ({\overset{\cdot}{x}}_{2}^{2} + {\overset{\cdot}{y}}_{2}^{2}) + \frac{1}{2} J_{2} {\overset{\cdot}{θ}}_{2}^{2}

= \frac{1}{2} m_{1} ({\overset{\cdot}{x}}_{2}^{2} + {\overset{\cdot}{y}}_{2}^{2} + l^{2} {\overset{\cdot}{θ}}_{1}^{2}

+ 2 l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \cos θ_{1} + 2 r {\overset{\cdot}{θ}}_{2} {\overset{\cdot}{x}}_{2}

- 2 l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{y}}_{2} \sin θ_{1})

+ \frac{1}{2} J_{1} {\overset{\cdot}{θ}}_{1}^{2} + \frac{1}{2} m_{2} ({\overset{\cdot}{x}}_{2}^{2} + {\overset{\cdot}{y}}_{2}^{2})

+ \frac{1}{2} J_{2} {\overset{\cdot}{θ}}_{2}^{2} - - - (3)

[式4]

V＝m₁gy₁+m₂gy₂ (4)

于是，移动体主体141的运动方程式使用欧拉-拉格朗日方程式被求得为式(5)至式(8)。

[式5]

(m_{1} l^{2} + J_{1}) {\overset{\cdot \cdot}{θ}}_{1} + m_{1} l {\overset{\cdot \cdot}{x}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot \cdot}{y}}_{2} \sin θ_{1} - m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{y}}_{2} \cos θ_{1} + m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot \cdot}{x}}_{2} \sin θ_{1}

+ m_{1} l {\overset{\cdot \cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1} + m_{1} l {\overset{\cdot}{θ}}_{1}^{2} {\overset{\cdot}{x}}_{2} \cos θ_{1}

+ m_{1} l {\overset{\cdot \cdot}{θ}}_{1} {\overset{\cdot}{y}}_{2} \cos θ_{1} + m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot \cdot}{y}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1}^{2} {\overset{\cdot}{y}}_{2} \sin θ_{1} - - - (5)

= 0

[式6]

J_{2} {\overset{\cdot \cdot}{θ}}_{2} = T_{ref} - - - (6)

[式7]

(m_{1} + m_{2}) {\overset{\cdot \cdot}{x}}_{2} + m_{1} l {\overset{\cdot \cdot}{x}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1}

= 0 - - - (7)

[式8]

(m_{1} + m_{2}) {\overset{\cdot \cdot}{y}}_{2} - m_{1} l {\overset{\cdot \cdot}{θ}}_{1} \sin θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1}^{2} \cos θ_{1} + m_{2} g

= 0 - - - (8)

其中，g为重力加速度。

并且，当考虑车轮202和路面203之间的粘性摩擦时，式(6)和式(7)可改写为式(9)和式(10)。

这里，D为粘性摩擦系数。

[式9]

J_{2} {\overset{\cdot \cdot}{θ}}_{2} + D ({\overset{\cdot}{θ}}_{2} - \frac{{\overset{\cdot}{x}}_{2}}{r}) = T_{ref} - - - (9)

[式10]

(m_{1} + m_{2}) {\overset{\cdot \cdot}{x}}_{2} + m_{1} l {\overset{\cdot \cdot}{x}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1} + D (r {\overset{\cdot}{θ}}_{2} - {\overset{\cdot}{x}}_{2})

= 0 - - - (10)

使用式(9)和式(10)可得到式(11)。

[式11]

(m_{1} + m_{2}) {\overset{\cdot \cdot}{x}}_{2} {+ m}_{1} l {\overset{\cdot \cdot}{x}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1} - r J_{2} {\overset{\cdot \cdot}{θ}}_{2} - r T_{ref} - - - (11)

式(5)减去式(11)，可得到式(12)。

[式12]

(m_{1} l^{2} + J_{1}) {\overset{\cdot \cdot}{θ}}_{1} + N_{x} + N_{y} = r T_{ref}

N_{x} = - (m_{1} + m_{2}) {\overset{\cdot \cdot}{x}}_{2}

+ m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot \cdot}{x}}_{2} \sin θ_{1}

+ m_{1} l {\overset{\cdot \cdot}{θ}}_{1} {\overset{\cdot}{x}}_{2} \sin θ_{1}

+ m_{1} l {\overset{\cdot}{θ}}_{1}^{2} {\overset{\cdot}{x}}_{2} \cos θ_{1}

N_{y} = r J_{2} {\overset{\cdot \cdot}{θ}}_{2} - m_{1} l {\overset{\cdot \cdot}{y}}_{2} \sin θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot}{y}}_{2} \cos θ_{1}

+ m_{1} l {\overset{\cdot \cdot}{θ}}_{1} {\overset{\cdot}{y}}_{2} \cos θ_{1}

+ m_{1} l {\overset{\cdot}{θ}}_{1} {\overset{\cdot \cdot}{y}}_{2} \cos θ_{1}

- m_{1} l {\overset{\cdot}{θ}}_{1}^{2} {\overset{\cdot}{y}}_{2} \sin θ_{1} - - - (12)

其中，N_x是将车轮水平位置x₂作为参数的非线性项，Ny是将车轮垂直位置y₂作为参数的非线性项。

如果假定负荷角度θ₁比车轮水平位置x₂足够缓慢地变化，则式(11)可改写为式(13)。

[式13]

c_{1} {\overset{\cdot \cdot}{x}}_{2} + c_{2} {\overset{\cdot}{x}}_{2} = r J_{2} {\overset{\cdot \cdot}{θ}}_{2} - r T_{ref}

c₁＝m₁+m₂+m₁lcosθ₁

c_{2} = - m_{1} l {\overset{\cdot}{θ}}_{1} \sin θ_{1} - - - (13)

其中，将比车轮水平位置x₂足够缓慢地变化的部分设为常数c1、c2、c3。

根据式(13)，车轮水平速度dx₂/dt通过式(14)来表示。

[式14]

{\overset{\cdot}{x}}_{2} = L^{- 1} {\frac{1}{c_{1} s + c_{2}} (s^{2} Θ_{2} - r T_{ref})} - - - (14)

其中，s²·θ₂是车轮加速度(d²θ₂/dt²)的拉普拉斯变换，L是拉普拉斯算子。

车轮水平速度估计器132使用式(14)来计算所述车轮水平速度估计值。

另一方面，当关于车轮垂直加速度(d2y2/dt2)求解式(8)时，得到式(15)。

[式15]

{\overset{\cdot \cdot}{y}}_{2} = \frac{m_{1} l {\overset{\cdot \cdot}{θ}}_{1} \sin θ_{1} + m_{1} l {\overset{\cdot}{θ}}_{1}^{2} \cos θ_{1} - m_{2} g}{(m_{1} + m_{2})} - - - (15)

车轮垂直加速度估计器131使用式(15)来计算所述车轮垂直加速度估计值。

如果将作为期望的车轮水平速度的车轮水平速度指令设为v₂ ^*(＝x₂ ^*的一阶微分)，负荷角度指令θ₁ ^*通过式(16)来表示，该负荷角度指令θ₁ ^*是在路面203水平平坦时车轮水平速度dx₂/dt等于所述车轮水平速度指令v₂ ^*的负荷角度θ₁。即，负荷角度指令是车轮水平加速度指令除以重力加速度而得的值的反正切。

[式16]

θ_{1}^{*} = \tan^{- 1} \frac{{\overset{\cdot \cdot}{x}}_{2}^{*}}{g} - - - (16)

车轮水平速度指令生成器101输出车轮水平速度指令v₂ ^*(＝x₂ ^*的一阶微分)，负荷角度指令计算器102使用式(16)来计算负荷角度指令θ₁ ^*并输出。

如果将式(14)和式(15)代入到式(12)，则可得到式(17)。

[式17]

(m_{1} l^{2} + J_{1}) {\overset{\cdot \cdot}{θ}}_{1} + N_{x} + N_{y}

= r T_{ref} - - - (17)

如果改写式(17)，则可表示为式(18)。

[式18]

(m_{1} l^{2} + J_{1}) {\overset{\cdot \cdot}{θ}}_{1} = u

u＝rT_ref-N_x-N_y (18)

其中，u为情况区分线性转矩。

并且，考虑使得负荷角度θ₁收敛到负荷角度指令θ₁ ^*的式(19)所示的情况区分线性转矩u。

[式19]

u = \{\begin{matrix} β \overset{\cdot}{e} + κe & sgn (θ_{1}^{*}) e < 0 \\ - γ {\overset{\cdot}{θ}}_{1} & 0 \leq sgn (θ_{1}^{*}) e < h \\ β \overset{\cdot}{e} + κe & sgn (θ_{1}^{*}) e &GreaterEqual; h \end{matrix} - - - (19)

其中，e＝θ₁ ^*-θ₁为负荷角度跟踪偏差，

β为速度比例控制增益，

κ为位置比例控制增益，

γ为衰减参数，

sgn(·)是·为正数时取+1、·为负数时取-1、·为零时取0值的符号函数。

另外，h＝c|θ₁ ^*|是衰减范围，该衰减范围是为抑制由反馈控制引起的抖动(chattering)而设置的负荷角度范围，c是衰减范围h的参数。

这里，式(19)所表示的情况区分线性转矩u表示如下含义：当负荷角度θ₁和负荷角度指令θ₁ ^*的偏差小时，具体地说，当在负荷角度θ₁的绝对值减少的方向上负荷角度θ₁和负荷角度指令θ₁ ^*的角度偏差处于衰减范围h的范围内时，通过使负荷201进行粘性摩擦为衰减参数γ的动作，能够使负荷角度θ₁稳定地收敛于负荷角度指令θ₁ ^*。

并且上述情况区分线性转矩u还表示如下含义：当负荷角度θ₁和负荷角度指令θ₁ ^*的偏差处于上述衰减范围h之外时，通过对负荷201施加刚度为位置比例控制增益κ、粘性摩擦为速度比例控制增益β的反馈控制，能够使负荷角度θ₁收敛于负荷角度指令θ₁ ^*。

这里，优选将衰减参数γ设定为负荷角度指令θ₁ ^*和负荷角度跟踪偏差e的函数，例如，如式(20)所示。

[式20]

γ = \frac{2 β}{| θ_{1}^{*} |} | e - \frac{| θ_{1}^{*} |}{2} | - - - (20)

即，一旦情况区分线性控制部120从负荷角度指令计算器102接收到负荷角度指令θ₁ ^*，则负荷角度指令θ₁ ^*被输入给衰减范围计算器121、衰减参数计算器122以及情况区分线性转矩计算器123。

并且，衰减范围计算器121使用负荷角度指令θ₁ ^*和参数c来计算h＝c|θ₁ ^*|，即衰减范围。

如此求出的衰减范围h被输出给控制切换器124。

另外，衰减参数计算器122按照上述式(20)计算衰减参数γ，并将该衰减参数γ输出给情况区分线性转矩计算器123。

情况区分线性转矩计算器123使用从衰减参数计算器122得到的衰减参数γ以及预先设定的速度比例控制增益β、位置比例控制增益κ，计算式(19)的情况区分线性转矩u。

算出的情况区分线性转矩u被输出给控制切换器124。

控制切换器124参照负荷角度跟踪偏差e和衰减范围h来切换选择由情况区分线性转矩计算器123求出的情况区分线性转矩u。

通过控制切换器124选出的情况区分线性转矩被输出给转矩指令计算器111。

另外，非线性转矩计算器133基于使用式(15)算出的车轮垂直加速度估计值以及使用式(14)算出的车轮水平速度估计值，来计算并输出非线性转矩，该非线性转矩是式(12)的Nx+Ny。

转矩指令计算器111使用情况区分线性转矩u和非线性转矩Nx+Ny通过式(21)来计算并输出转矩指令Tref。

[式21]

T_{ref} = \frac{u + N_{x} + N_{y}}{r} - - - (21)

其中，r为车轮半径。

移动体主体141根据转矩指令Tref被驱动控制。

根据具有这样的构成的本第一实施方式，可获得以下的效果。

(1)在本第一实施方式中，如式(19)所示，将负荷角度θ₁分为三个范围，并在各范围内，计算最优的转矩指令。

并且，控制切换器124根据衰减范围内和衰减范围外来切换控制。

由此，能够使得负荷角度θ₁不在负荷角度指令θ₁ ^*的附近振动而平滑地收敛于负荷角度指令θ₁ ^*。

其结果，能够实现稳定的水平行驶。

(2)由于具有衰减参数计算器122，并通过式(20)来设定衰减参数γ，因此与将衰减参数γ固定为恒定值的情况相比，能够使负荷角度θ₁平滑快速地收敛于负荷角度指令θ₁ ^*。

(3)由于使用式(14)的车轮水平速度估计值来控制移动体主体141，因此在车轮202与路面203相对打滑的情况下也能够使移动体主体141的车轮水平速度v₂(＝x₂的一阶微分)与车轮水平速度指令v₂ ^*(x₂ ^*的一阶微分)一致。

(4)由于使用式(15)的车轮垂直加速度估计值来控制移动体主体141，因此即使路面203凹凸不平，也能够在车轮202与路面203接触的期间稳定地控制负荷角度θ₁。

(实验例)

接着，示出证实本发明效果的实验例。

作为实验例，示出第一实施方式的仿真结果。

这里，用于仿真的数值如下。

m₁＝70[kg]、

J₁＝25.2[kg·m²]、

m₂＝15[kg]、

J₂＝0.075[kg·m²]、

I＝0.9[m]、

r＝0.1[m]、

D＝0.1[N·s/m]、

g＝9.8[m/s²]、

T＝1×10^-3[s]、

κ＝40[s^-1]、

J₁₀＝m₁×I²+J₁[kg·m²]、

β₀＝2πκ[s^-1]、

β＝β₀×J₁₀[N·m·s/rad]、

γ＝0.1[N·m·s/rad]、

pcl＝[-49.9、-201.4][rad/s]、

td＝0.5[s]

其中，

m₁是负荷质量，

J₁是负荷惯性矩，

m₂是车轮质量，

J₂是车轮惯性矩，

I是负荷车轮重心间距离，

r是车轮半径，

D是车轮路面间粘性摩擦，

g是重力加速度，

T是控制周期(sampling time)，

κ是本发明中的位置比例控制增益，

J₁₀是公称(nominal)惯性矩，

β₀是本发明中的标准化速度比例控制增益，

β是本发明中的速度比例控制增益，

γ是本发明中的摩擦参数，

pcl是现有技术的闭环极点，

td是脉冲扰动时间。

另外，车轮路面间粘性摩擦D是在图2的车轮202与路面203之间起作用的粘性摩擦，

公称惯性矩J₁₀是将本发明中的速度环(speed control loop)标准化的参数。

pcl是在现有技术的状态反馈控制中配置的闭环的极点。

另外，假定在脉冲扰动时间td内，在车轮202的竖直上方方向上有脉冲状的加速度扰动输入的情况。

图3、图4是示出仿真结果的图。

图3示出了负荷角度的变化。

在图3中，实线L₁₀表示本发明中的负荷角度(Load angular positionwith proposed control)，虚线L₁₁表示负荷角度指令(Load angular positionreference input)，单点划线L₁₂表示现有技术的负荷角度。

在输入加速度扰动前的0.5[s]内，现有技术和本发明均大致相同程度地跟踪负荷角度指令，但在0.5[s]及其以后，现有技术中的负荷角度振动，相对于此，本发明在输入所述加速度扰动后也不振动而一直跟踪所述负荷角度指令。

并且，根据本发明的负荷角度随时间而发生折线状的变化是由于在式(19)的所述衰减范围内实施了仅施加衰减的控制，表明了通过具有所述衰减范围，所述负荷位置在所述负荷位置指令的附近变得难以振动。

另外，通过在车轮垂直加速度估计器131中计算式(15)的车轮垂直加速度估计值，并在非线性转矩计算器133中计算非线性转矩Ny，能够补偿由于路面203凹凸不平而施加到车轮202上的垂直方向的所述加速度扰动，因此表明了本发明即使在有所述加速度扰动的情况下也使所述负荷角度稳定。

图4示出了车轮的水平速度变化。

在图4中，实线L₂₀是本发明的车轮水平速度，虚线L₂₁是期望的车轮水平速度，单点划线L₂₂是现有技术的车轮水平速度。

在输入加速度扰动前的0.5[s]内，现有技术和本发明均跟踪所述期望的车轮水平速度，但在0.5[s]及其以后，现有技术发生振动，相对于此，本发明不振动而一直跟踪所述期望的车轮水平速度。

(变形例1)

在上述第一实施方式中，衰减参数γ采用了由衰减参数计算器122按照上述式(20)算出的值，但衰减参数γ也可以设为预先设定的固定值。

即，如图5所示，也可以将预先设定的衰减参数设定存储在衰减参数存储部(damping parameter memory)125中，并从衰减参数存储部125向情况区分线性转矩计算器123提供衰减参数γ的值。

(变形例2)

另外，在上述第一实施方式中，说明了作为最优选实施方式的具有非线性控制部130的情况，但当实现使负荷角度θ₁不在负荷角度指令θ₁ ^*的附近振动而平滑地收敛于负荷角度指令θ₁ ^*的稳定的控制时，也可以没有非线性控制部130。

即如图6所示，也可以是省略了非线性控制部130的控制装置610。

这里，当省略非线性控制部130而仅具有情况区分线性控制部120时，优选通过调整由情况区分线性转矩计算器123计算的情况区分线性转矩u的线性反馈转矩的增益来设定尽可能地抑制作为扰动的非线性因素的增益。

当然，为了应对在不是水平的凹凸路面上的行驶、与障碍物的碰撞或车轮空转等非线性因素，优选兼有非线性控制部130。

本发明不限定于上述实施方式，本发明也包含在能够达到本发明目的的范围内进行的变形、改进等。

例如，在式(19)中，位置P/速度P控制(position P/speed Pcontrol)当然也可以被替换成位置P/速度PI控制、位置P/速度I-P控制、位置PID控制(position PID control)等任意的控制规则。

该申请要求基于2009年4月28日提出的日本专利申请2009-109591的优先权，其全部公开内容合并于此。

产业上的可利用性

本发明无论是在路面凹凸不平的情况还是与人或物体碰撞的情况下均能够使二轮倒立的移动体不跌倒也不振动地以期望的水平速度行驶，因此能够广泛地应用于以二轮倒立状态行驶的机器人、电动轮椅、自动运送装置、用于发生灾害时救援生命等的在狭小空间内作业的机器人、抗振动能力差的电子部件的组装装置等。

附图标记说明

100…指令部，101…车轮水平速度指令生成器，102…负荷角度指令计算器，110、610…控制装置，111…转矩指令计算器，120…情况区分线性控制部，121…衰减范围计算器，122…衰减参数计算器，123…情况区分线性转矩计算器，124…控制切换器，125…衰减参数存储部，130…非线性控制部，131…车轮垂直加速度估计器，132…车轮水平速度估计器，133…非线性转矩计算器，141…移动体主体，142…状态传感器，201…负荷，202…车轮，203…路面，1001…摩擦估计器，1002…目标状态生成器，1003…状态反馈增益，1004…倒立机器人。

Claims

1.一种倒立型移动体的控制装置，该控制装置在使移动体主体倒立的状态下控制该移动体主体的行驶，所述移动体主体包括具有车轮的驱动单元、以及经由连杆在所述车轮上被倒立控制的负荷，所述倒立型移动体的控制装置的特征在于，

将连结所述负荷的重心和所述车轮的重心的直线与竖直线所形成的角定义为负荷角度，

所述控制装置将基于车轮水平速度指令求出的负荷角度指令作为输入，并进行控制以向所述移动体主体施加基于负荷角度跟踪偏差的线性反馈转矩，所述负荷角度跟踪偏差是从所述负荷角度指令减去所述负荷角度而得的值，

如果所述负荷角度在作为所述负荷角度指令的附近的宽度的衰减范围内，则所述控制装置进行控制以向所述移动体主体施加衰减转矩而不施加所述线性反馈转矩。

2.如权利要求1所述的倒立型移动体的控制装置，其特征在于，

通过对所述负荷角度指令的绝对值乘以预定系数来计算所述衰减范围。

3.如权利要求1或2所述的倒立型移动体的控制装置，其特征在于，

将所述衰减转矩定义为粘性摩擦。

4.如权利要求3所述的倒立型移动体的控制装置，其特征在于，

将作为所述粘性摩擦的衰减参数作为所述负荷角度跟踪偏差和所述负荷角度指令的函数来计算。

5.如权利要求4所述的倒立型移动体的控制装置，其特征在于，

通过从所述负荷角度跟踪偏差减去将所述负荷角度指令的绝对值除以2而得的值，并且将该差的绝对值除以所述负荷角度指令的绝对值并乘以常数，来计算所述衰减参数。

6.如权利要求3所述的倒立型移动体的控制装置，其特征在于，

将作为所述粘性摩擦的衰减参数定义为恒定值。

7.如权利要求4至6中任一项所述的倒立型移动体的控制装置，其特征在于，包括：

情况区分线性转矩计算器，其计算衰减转矩和基于所述负荷角度跟踪偏差的线性反馈转矩，所述衰减转矩是向负荷速度与所述衰减参数的乘积值添上负号而得到的；以及

控制切换器，其切换并输出通过所述情况区分线性转矩计算器算出的所述衰减转矩和所述线性反馈转矩。

8.如权利要求7所述的倒立型移动体的控制装置，其特征在于，

所述控制切换器

如果0≤sgn(θ1*)·e＜h，则输出所述衰减转矩，

否则输出所述线性反馈转矩，

其中，e＝θ₁ ^*-θ₁；θ₁ ^*是负荷角度指令；θ₁是负荷角度；sgn(·)是·为正数时取+1、·为负数时取-1、·为零时取0值的符号函数；h是通过对所述负荷角度指令的绝对值乘以预定系数而算出的衰减范围。