轮式倒立摆的数学建模及状态反馈控制

石 祥，许 哲，何青义，田 卡

（上海海洋大学 工程学院，上海 201306）

0 引言

倒立摆[1～3]作为一类非线性控制系统的典型特例，能有效地反映诸如可镇定性、鲁棒性、随动性以及跟踪等许多控制中的关键问题，是检验各种控制理论的理想模型，目前已有多种控制算法[4～9]应用其中，但是在研究其控制算法之前都存在着建模问题。目前关于各种倒立摆的研究论文中所给出的数学模型中，通常没有系统微分方程的推理过程，或者模型比较单一，尤其是轮式倒立摆。

轮式倒立摆又称 “两轮自平衡机器人[10]”，其模型是建立在由两个车轮左右平行布置、可移动的小车车体上，每个车轮都与电机相连。当车体运动时，通过控制电机的转速来控制车轮，使摆杆尽快地达到一个平衡位置，并且没有大的振荡。当摆杆到达期望的位置后，系统能克服随机扰动而保持稳定的位置。本文将对轮式倒立摆进行数学建模，从而得出其状态方程，并基于Matlab及ADAMS的联合仿真对轮式倒立摆的状态反馈控制加以验证。

1 数学建模

1.1 直流电机的数学模型

轮式倒立摆由两个直流电机驱动，本节主要推导直流电机的数学模型，图1展示了机械载荷和直流电源的驱动，从直流电源得到的电能转换为机械能，机械能再转换为作用在直流电机上的载荷。

图1 直流电机图解Fig.1 Diagram of DC motor

因为轴上产生的所有扭矩与轴的加速度呈比例关系，而轴上的加速度又是通过电枢的转动惯量IR产生，可推出如下关系：

为了简化直流电机模型，电机的电感和摩擦忽略不计，电流导数规定为零，因此式（1）可分别被等效成如下：

一旦输入电压改变，输出轴的转动速率也会改变，而此时电流将会达到一个瞬间恒定状态，整理式（2）得：

电机的输入变量是外加电压，电机的摩擦忽略不计，则电机扭矩方程如下：

1.2 轮式倒立摆的数学模型

轮式倒立摆分为车体和摆杆两部分，将摆杆约束在二维平面，这样更易于对其建模和控制，同时假设车轮总是与地面相接触并且没有产生滑动，这样就没有关于z轴方向的移动和绕x轴的转动。

如图2所示，轮式倒立摆三维坐标的定义。本文通过车体的位移量、车体的速度、摆杆的角度φ和摆杆的角速度φ˙作为状态变量，直流电机的端电压值作为输入变量，以此描述轮式倒立摆的运动，得状态方程。

（1）车轮的动态分析。根据牛顿运动定理，得到车轮左右两轮的运动方程，右和左轮动态方程：

图2 三维坐标的定义Fig.2 The definition of three-dimensional coordinates

（2）摆杆的动态分析。摆杆受力图如图3所示，其动态方程：

令φ=π+φ，使上述两公式线性化，其中φ代表竖直方向上的小角度变化。 假设cosφ=-1， sinφ=-φ and则整理式（8）得：

图3 摆杆的受力图Fig.3 Pendulum diagram

整理式（9）得状态方程如下：

其中 a22， a23， a42， a43和 b2， b4根据如下系统的参数定义：

轮式倒立摆中参数的意义：V—作用在电机上的端电压（V）；Tm—电机产生的扭矩（N·m）；Km—电机的扭矩系数，是电机产生的扭矩与电机内电流的比例系数（N·m/A）Ve—电机内部的反电动势电压（V）；ω—电机轴的角速度（rad/sec）； θ—电机轴的转动角度（rad）； Ke—反电动势电压系数，是电机的反电动势电压与电机轴角速度的比例系数（V·sec/rad）；R—电机内的电阻（Ω）； Li—电机的电感（H）； IR—电机的转动惯量（kg·m2）； Kf—电机轴的摩擦系数（N·m·sec/rad）； Tα—作用在电机轴上的负载扭矩（N·m）；φ—摆杆绕Z轴的转动角度（rad）；xr—轮式倒立摆车体的位移（m）；Mw—车轮质量（kg）； HfR，HfL—车轮与地面的摩擦力（N）HL，HR—摆杆与车轮之间的作用力（N）；Iw—车轮的转动惯量 （kg·m2）；θRw，θLw—右轮和左轮的转动惯量（rad）；TR，TL—右轮和左轮的负载扭矩（N·m）；L—车体中心和摆杆重心之间的距离（m）；Mp—摆杆质量（kg）；r—车轮半径（m）；Ip—摆杆的转动惯量（kg·m2）；δ—摆杆绕 Y 轴的转动角度 （rad）， 其值为：g=9.81m/s2，r=0.051m，Mw=0.1272kg，Mp=0.86kg，Iw=0.000069kg·m2，Ip=0.0013kg·m2，L=0.07m，Km=0.006123Nm/A，Ke=0.006087V·sec/rad，R=3Ω。将以上参数带入式（10）得：

2 Matlab 控制仿真

2.1 状态反馈控制律设计

状态反馈控制律为u=ξ-Kx，其中ξ—参考输入。因为系统是能控的，则可以任意配置系统极点。经过调试，本文配置的系统极点为 [-10-11-20-25]。

Matlab 控制箱的place 函数， 能够实现单输入/多输入系统极点配置，通过Matlab的place 函数，可求出反馈矩阵 K：K=[-0.0327-0.0092 1.0803 0.0692]·105。

综上所述，K 和ξ确定之后，控制律u 便能确定。

2.2 仿真结果及分析

闭环系统的响应指标为：摆杆稳定时间小于5s，稳态时摆杆与垂直方向夹角小于0.1 弧度，程序采样时间0.02s。仿真结果如图4所示，表示状态反馈控制律及系统响应，系统在初始位置不稳定，通过调节直流电机的端电压，从而改变车体的位移和速度、摆杆的角度和角速度，系统在1 秒内，达到稳定状态。仿真结果验证了状态方程、状态反馈控制律和Matlab 仿真过程的正确性。

图4 状态反馈控制律及系统响应Fig.4 Control law and system response of state feedback control

3 Matlab与ADAMS的联合仿真

3.1 联合仿真简介

为了更真实反映轮式倒立摆的实际动态过程，论文在ADAMS 中建立轮式倒立摆的虚拟样机模型，在Matlab 中建立控制系统，其中控制系统将参考上述第二节，从而结合Matlab与ADAMS 进行联合仿真，目的是更准确地显示轮式倒立摆状态反馈控制的特点。本章通过确定ADAMS的输入和输出变量，可以在ADAMS 和Matlab 之间形成一个闭合回路。ADAMS 模块将车体的位移和速度、摆杆角度和角速度输出给Matlab 控制系统，并根据这些变量计算出控制律u，将其输入进ADAMS 模块中，控制ADAMS 模块中轮式倒立摆车轮上的转矩，如此，二者之间形成循环反馈。

图5 联合仿真原理Fig.5 The principle of collaborative simulation

3.2 控制律的设计

在联合仿真中，轮式倒立摆系统虚拟样机模型，参数与上述一致，有关参数无需再次计算，只需作简要调试。因此本章状态反馈控制律，其反馈矩阵Γ调整为[-0.0032-0.0075 3.9823 1.9070]·104。

此外，上述第一节中提及轮式倒立摆的输入变量为直流电机的端电压值，而本章中虚拟样机模型的输入变量是车轮上的转矩，两者不一致，因此Matlab 控制系统在输出控制律u 前，还需将电压值转为转矩值。

3.3 仿真结果及分析

程序作图采样时间为0.005 秒，仿真结果如图6所示。图中表示轮式倒立摆的状态反馈控制在初始位置不稳定时，通过调节车轮力矩的大小，在4 秒内，使车体位移、车体速度、摆杆角度和摆杆角速度收敛至0 附近，即轮式倒立摆平衡稳定。仿真结果表明状态反馈控制能够完成预期的控制目标，同时验证了状态反馈控制律和联合仿真的正确性。

图6 状态反馈控制律和系统响应Fig.6 Control law and system response of State feedback control

4 总结

首先基于牛顿力学分析的方法，依次对直流电机、车体和摆杆进行动力学分析，列出其动力学方程，其次采用小角度线性化得出轮式倒立摆的近似线性模型。

由于轮式倒立摆的数学模型通常是线性化处理后得出，而倒立摆本身是高阶非线性系统，那么二者之间必然存在偏差。所以本文不仅对轮式倒立摆进行Matlab 仿真，还对其进行Matlab与ADAMS的联合仿真，目的是更为准确地表明轮式倒立摆状态反馈控制的特点，最终，仿真结果不仅表明状态反馈控制能够使轮式倒立摆保持平衡，而且验证了状态方程、状态反馈控制律、联合仿真过程的正确性。

本文之所以在联合仿真前，先进行Matlab 仿真，因为联合仿真所涉及到的控制律，可以参考Matlab 仿真中设计出的控制律，这样能极大地降低联合仿真难度，其次，也可先行验证轮式倒立摆状态反馈控制的可行性。

[1]于占东，王刚，王显峰.基于BVP 算法的旋转倒立摆自动起摆动控制[J].控制工程，2013，1.

[2]王家军，刘栋良，王宝军.X-Z 倒立摆的一种饱和非线性稳定控制方法的研究[J].自动化学报，2013，1.

[3]但远宏，李祖枢.二级倒立摆DU2UD的非线性控制研究[J].测控技术，2013，3.

[4]段学超，袁俊，满曰钢.两轮自平衡机器人的动力学建模与模糊进化极点配置控制[J].信息与控制，2013，2.

[5]Z.Li,J.Luo(2009),“Adaptive robust dynamic balance and motion controls of mobile wheeled inverted pendulums”,IEEE Transactions on Control Systems Technology,17（1）.

[6]阮晓刚，蔡建羡，陈静.基于强化学习规则的两轮机器人自平衡控制[J].计算机测量与控制，2009，2.

[7]C.W.Tao,J.S.Taur,T.W.Hsieh and C.L.Tsai（2008）,“Design of a fuzzy controller with fuzzy swing-up and parallel distributed pole assignment schemes for an inverted pendulum and cart system”,IEEE Control Systems Technology,16（6）.

[8]D.S.Nasrallah,H.Michalska and J.Angleles(2007),“Controllability and posture control of a wheeled pendulum moving on an inclined plane”,IEEE Transactions on Robotics,23(3).

[9]A.Casavola,E.Mosca and M.Papini（2004）, “Control under constraints:An application of the command governor approach to an inverted pendulum”,IEEE Control Systems Technology,12（1）.

[10]TIRMANT H，BALOH M，VERRNEIREN L.B2， “An alternative two wheeled vehicle for an automated urban transportation system”,IEEE.2002，39（4）.