CN1952996A - 两轮遥控小车倒立摆及其平衡控制方法 - Google Patents

Abstract

本发明两轮遥控小车倒立摆及其平衡控制方法，特征是将两直流电机(3)对称固定在车体(1)上，电机轴的一端与轮子(2)固接，轴的另一端与码盘(4)轴相连；在两轮轴线中部位置安装陀螺仪(7)并固接摆杆(5)；控制器(11)、遥控接收器(13)、电源转换模块(15)、电机驱动器(12)焊接在电路板(10)上；电路板(10)和直流电源(6)固定在车身(1)上；其平衡控制方法包括自动判断零位电压，校正其漂移，对测量噪声进行滤波。本发明避免了现有倒立摆因机械传动故障、或传动间隙可能引起的误差所造成的控制失败；可在地面及斜坡上前后左右行走和遥控，可用于教学和科研，更有效地检验各种控制算法的优劣。

Description

两轮遥控小车倒立摆及其平衡控制方法

技术领域：

本发明属于自动控制技术领域，特别涉及用于科研或教学的实验装置--两轮遥控小车倒立摆及其平衡控制方法。

背景技术：

倒立摆是一种复杂的、非线性的、不稳定的系统，它是现代控制理论及应用的典型科研或教学装置。现有最常使用的倒立摆有如《西南工学院学报》(2001，Vol.16，No.3，P.12-16)所介绍的在有限长度轨道上作直线运动的倒立摆(以下简称直线倒立摆)和如专利申请号02138164.x所介绍的以定轴作弧线运动的旋转倒立摆。它们的电机机身都是固定不动的。如直线倒立摆的电机固定在导轨的一端，电机轴通过包括皮带轮及皮带或齿轮及齿条的传动带驱动小车运动，使得小车上的摆杆稳定不倒，即通过传动装置变电机的旋转运动为小车在轨道上的直线运动；旋转倒立摆的电机固定在支架上，电机通过与其轴固接的旋臂带动摆杆运动，从而保持摆杆稳定不倒；即旋臂代替了传动带和小车，支撑摆杆的旋臂一端作的是弧线运动，旋臂另一端与电机轴固定连接，这样旋臂与电机轴联动，本质上说旋臂把电机轴的旋转运动放大了，它仍然是旋转运动，故称为旋转倒立摆。由于直线倒立摆中的控制器部分、功率驱动部分与小车摆杆等机械部分相分离，而旋转倒立摆虽然把控制器与功率驱动部分放到了一个箱体里，但它们仍然与摆杆部分相分离，因此这些倒立摆存在着共同问题：1)直线倒立摆最多在轨道范围内运动，旋转倒立摆在以电机轴为圆心的定点，最多做一百八十度范围内的弧线运动，这些倒立摆装置上不得不加装行程限位装置及保护电路，只能局限在水平导轨、或者水平平台上运动，或者固定点范围内运动，与实际生活和工程联系并不十分紧密；2)如果限位装置故障或保护电路装置失灵会导致设备损坏；3)由于上述的共同原因，现有倒立摆的运动范围都受到很大限制，控制量不能加得太大，否则就会超出其限制，使得一些算法在这些倒立摆系统上无法实现；4)现有倒立摆机械传动部分的故障、或传动间隙引起的误差会造成控制失败。

发明内容：

本发明提供一种能保持稳定、自由行走且可遥控的两轮遥控小车倒立摆及其平衡控制方法，以克服现有倒立摆的存在的上述问题。

本发明的两轮遥控小车倒立摆，包括：控制器、直流电机及电机驱动器、电源、小车及摆杆；其特征在于：两个直流电机(3)对称固定在车体(1)底盘左右两侧，每个直流电机(3)的轴分别与一个轮子(2)的轴相互固接，轴的另一端直接与一个码盘(4)同轴相连或通过传动部件(8)与一个码盘(4)的轴相连；该左右两个轮子(2)的轴线中部位置的车体底盘上固接一个支架(9)，支架(9)上端与摆杆(5)固接，使车体(1)及摆杆(5)可在与轴垂直的面内绕轴摆动；支架(9)内侧固定陀螺仪传感器(7)，使其测量轴与两个轮子轴处在一条直线上；直流电源(6)固定在车身(1)上；

控制器(11)、遥控接收器(13)、电源转换模块(15)、电机驱动器(12)按如下电气信号连接关系焊接在印制电路板(10)上：直流电源(6)经电源转换模块(15)接控制器、电机驱动器、遥控接收器、陀螺仪传感器和光电码盘的供电端，控制器(11)的输入端与码盘(4)、陀螺仪传感器(7)和遥控接收器(13)的输出端相连接，控制器的输出端与两个电机驱动器(12)的输入端相连接，电机驱动器(12)的输出端接两个电机输入端；该印制电路板(10)固定在车身(1)上。

所述控制器(11)可选用数字信号处理器DSP(Digital Signal Processors)、嵌入式计算机系统或单片机。

所述直流电机(3)可选用直流力矩电机、带码盘的直流电机或无刷直流电机。

所述直流电源(6)可与摆杆滑动连接，或与摆杆固定连接，或直接安装在车体上。

本发明的两轮遥控小车倒立摆的平衡控制方法，包括：由两个码盘(4)检测小车两个轮子的位移和速度信号后反馈给控制器(11)；由陀螺仪传感器(7)测量摆杆(5)的角速度信号后反馈给控制器(11)；控制器(11)将获得的上述反馈信号与由程序设定的参考信号比较得出误差信号；根据预先给定的控制算法，用该误差信号计算出控制量，经电机驱动器(12)输出给直流电机(3)，直流电机(3)带动小车运动使摆杆(5)保持平衡不倒；

其特征在于：

a)控制器(11)将获得的上述反馈信号与遥控接收器(13)的指令相比较，产生输出信号控制小车作前后左右运动；

b)控制器(11)按如下方法自动判断零位电压：

中断程序在每个5到^-20毫秒的采样周期调用一次自动判断零位电压程序模块(P8)；让摆杆在初始位置保持五到十五秒，采样模块(P6)和取平均值模块(P7)采样陀螺仪传感器的信号并取均值；接着，程序模块(P8)判断“零位电压标志”是否为结束：若是，则返回调用程序；若未结束，则进入判断零位电压程序：程序模块(P8)用一个存储单元来计数；每前后两个周期的采样均值是否在陀螺仪传感器的误差范围内？若不在误差范围内则为无效数据，存储单元计数值清零，返回调用程序；若在误差范围内则为有效数据，存储单元计数值加一；然后，判断计数值是否连续累积计数至五到十五秒，若否则返回调用程序；若是则置“零位电压标志”为结束，且取有效数据的平均值作为起始角度信号；起始位置是铅垂位置时，则有效数据的平均值即为零位电压，起始位置是摆杆倒在水平面上时，则程序里需加90度转换为铅垂位置；返回调用程序；

c)校正零位电压的漂移，按如下两种方法之一进行零漂的校正：

1).在用控制算法成功控制小车时，摆杆在平衡位置左右晃动一段时间后会在平衡位置短暂停顿，设此时摆杆的角度为零，称这种状态为停顿状态，设停顿时间间隔为[t₁，t₂]，(t₁＜t₂)，且在时刻t₁和t₂的测量角度值分别为θ(t₁)和θ(t₂)，它们的差值θ(t₁)-θ(t₂)即是测量角度值在时间间隔t₂-t₁内的漂移量，其对应的采样值是零位电压漂移；然后对其进行校正，经过多次校正后，零位电压的漂移逐渐逼近真实值；

2).对于陀螺仪传感器零位电压的漂移，利用降维状态观测器对摆杆角度进行观测，再用观测值对陀螺仪传感器零位电压进行实时校正；

d)按如下方法对测量噪声进行滤波处理：

采样模块(P6)在每个周期中对陀螺仪传感器(7)的输出端口进行多次采样，并将采样值转换成数字信号；取平均值模块(P7)先对这些信号进行初步筛选：剔除其中的最大和最小值；剩下的数据取均值；然后，数字滤波程序模块(P12)采用递归型三阶的巴特沃斯(butterworth)数字滤波器对均值信号滤波。

所述预先给定的控制算法，可选用线性状态反馈控制算法、自适应控制、非线性控制和智能控制算法。线性状态反馈控制算法包括闭环极点配置算法、线性二次型最优控制LQR(Linear Quadratic Regulator)算法和鲁棒方差控制算法；智能控制算法包括模糊控制算法、专家控制算法、模糊神经网路控制算法、拟人控制算法和遗传控制算法。

判断零位电压方法中所述初始位置可选择让摆杆在铅垂位置或倒在水平地面上的位置。

以本发明的上述两轮遥控小车倒立摆为基础，在控制电路板上还可焊接超声测距模块，由控制器(11)内部产生发射信号，并由控制器(11)的输入/输出口I/O经超声测距模块的发射器(17)发送超声波，在遇到物体后反射回来，通过接收器(18)将接收到的信号送入控制器的外部中断端口；控制器(11)将获得的小车的位移和速度信号与从超声测距模块获得的信息相比较，产生输出信号驱动电机，指挥小车避让前方障碍物。

以本发明的上述两轮遥控小车倒立摆为基础，还可以做成二级、三级或更多级形式的倒立摆，做法如下：

二级倒立摆制作方法之一：在与车体固接的原摆杆(5)上端再铰接另一根摆杆(16)，该摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；在铰接处与旋转轴同轴的位置增装一个测量两摆杆之间形成的夹角(θ1-θ2)的角度测量装置；这样，车体和与其固接的原摆杆构成第一级摆(下摆)，与第一级摆杆上端铰接的另一根摆杆构成第二级摆(上摆)。

二级倒立摆制作方法之二：摆杆(5)与车体(1)上端铰接，使摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；并在铰接处与旋转轴同轴的位置增装一个测量摆杆与车体之间形成的夹角(θ1-θ2)的角度测量装置；这样，车体构成一级摆(下摆)，与车体铰接的摆杆构成第二级摆(上摆)。

在上面介绍的两种二级倒立摆之任一种的基础上，即在第二级摆杆的上端以同样方法继续铰接另一根杆子，使该摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；并在铰接处增装一个角度测量装置，即成三级倒立摆；以此类推，可做成四级或更多级的倒立摆。

由于本发明将控制器、直流电机及电机驱动器、直流电源、小车及摆杆、码盘和陀螺仪、遥控模块安装在小车体底盘上，形成一个整体，不需要导轨、支架，因而克服了现有倒立摆只能局限在水平导轨、或者水平平台上、或者在固定的支架上定点范围内运动的不足，使得倒立摆装置可以在地面上随意行走；并且，一旦把程序下载或固化到控制器后，可以完全脱离主计算机工作。现实生活中有着很多类似倒立摆的重心在上，支点在下的平衡控制问题，如机器人行走、火箭垂直发射和卫星飞行姿态等。这些控制对象都是运动轨迹不受限制的。两轮遥控小车倒立摆可以在左右电机的驱动下在水平面内自由行走，因此要比那些只能运行在固定轨迹上的直线倒立摆和定点弧线运行的旋转倒立摆更接近实际，两轮遥控小车倒立摆更能反映实际控制中的一些问题，控制难度增加，可进行的控制手段也多，能有效地验证各种控制算法。对两轮遥控小车倒立摆的控制更具有实际应用价值。

由于本发明将摆杆的一端安装在与电机机身固接的小车底盘上，电机轴上装小车车轮，电机轴与轮子固接的结构形式，因而两轮遥控小车倒立摆的摆杆与电机轴间没有任何传动部件或悬臂，从而避免了现有倒立摆由于机械传动部分的故障、或传动间隙可能引起的误差所造成的控制失败。

由于本发明将控制器、遥控模块、电源转换模块、电机驱动器、以及存储器均焊接在一块印制电路板(10)上，使得所述控制器可采用数字信号处理器或嵌入式计算机系统或单片机，从而省掉了另需配置的运动控制卡或模/数、数/模、脉宽调制PWM(Pulse-WidthModulation)等模块。

由于本发明安装了遥控模块，使得倒立摆装置能进行遥控；本发明可安装超声测距模块，从而使得倒立摆装置能进行避障运动和运动轨迹规划与控制。

由于本发明的两轮遥控小车倒立摆由左右两个电机分别驱动，在保持摆杆竖直的同时，可以在水平面自由行走，具有更多的运动自由度；加上本发明安装了陀螺仪传感器，使两轮遥控小车倒立摆可以在斜坡上自由行走。

由于本发明将各部件安装在一起，省去了部件之间的外部连线，从而使得结构更简洁紧凑、工作更可靠。

现有倒立摆的运动行程是有限制的，摆杆与电机轴间存在传动机构或悬臂，因而增加了控制的约束条件，使得一些算法在这些倒立摆系统上无法实现。而本发明两轮小车倒立摆的运动约束放宽了，使得这一系统的变量、非线性程度及不稳定性成倍地增加。两轮遥控小车倒立摆系统控制的实现困难得多。而控制难度越大，越会吸引控制专家的兴趣，越能体现控制方法的效果。它除了保留以往倒立摆有关的稳定性、非线性和不确定性方面的研究内容外，还能增加许多控制难度，以便研究和展示更多的控制方案及控制效果。

与一般倒立摆相比较，本发明的两轮遥控小车倒立摆在非线性、不确定性、稳定性、耦合特性、控制目标和被控量、控制量、计算机等方面具有以下不同点及其优点：

1)非线性：本发明的两轮遥控小车倒立摆装置属于典型的非线性系统。若不考虑左右方向运动，两轮小车倒立摆的非线性程度与一般型式倒立摆的非线性程度是一样的。若考虑左右方向运动，则两轮小车倒立摆的非线性程度显然更高，因此与实际结合更紧密。这为非线性控制提供了更为实际、更真实的研究对象(平台)。

2)不确定性：两轮小车倒立摆的不确定性主要是模型的参数误差以及机械运动过程中的摩擦所导致；直线倒立摆的小车轮子与导轨间摩擦基本上是已知(确定)的，两轮小车倒立摆与直线型倒立摆装置相比，虽然由于没有导轨和机械传动机构(皮带轮及皮带或齿轮及齿条)而不存在机械传动间隙，但两轮小车倒立摆的两个轮子与地面的摩擦是不可预知的，且还具有其它的不确定性，这就为研究控制性能的鲁棒性、自适应性提供了对象，也使在其装置上使用智能控制方案有了用武之地。

3)稳定性：垂直向上的摆杆与铅垂线的角度偏移(θ＝0)是不稳定的平衡点，任何微小的扰动，都将使得摆杆偏离平衡点，从而丧失其“倒立”特性。两轮小车倒立摆装置也属于固有不稳定的系统，保留了以往倒立摆的研究内容，且由于两轮小车倒立摆的运动更自由，它比一般倒立摆更不稳定，也就能进行更好的稳定性研究。

4)耦合特性：两轮小车倒立摆的车体及摆杆绕车轴前后摆动与两轮小车左右运动之间具有强耦合，是进行解耦控制、鲁棒控制等研究方案的较好对象。而直线倒立摆是直线运动，旋转倒立摆是定点弧线运动，不存在上述耦合。

5)控制目标和被控量：一般型式倒立摆的控制目标主要是使摆杆在其不稳定的平衡点内稳定平衡；其被控量是摆杆与铅垂线的角度偏移θ，另一个被控量是小车沿轨道方向的位移X，或悬臂的转角。直线倒立摆导轨不长，通常目标位移取为零，不作控制要求；旋转倒立摆弧线运动范围更有限，角位移也不作为控制要求；以往倒立摆只是一维运动。两轮小车倒立摆的控制目标是：除了使小车倒立摆装置稳定在其不稳定的平衡点外，还必需使小车在地面上自由运动。所以，两轮小车倒立摆装置的被控量除了有摆杆与铅垂线的角度θ外，还有小车左右方向的水平位移X、y。由于而两轮小车倒立摆装置是在地面上行走的二维运动；因此在该装置上装上超声波、红外等传感器就可进行避障运动研究和运动轨迹规划与控制。而一般型式的倒立摆是无法进行上述控制研究的。另外如果要使角度θ有一个不为零的偏移，小车必需以一个恒定的速度运动，一般型式的倒立摆装置由于运动距离的限制而做不到，而本发明的两轮小车倒立摆则能办到。

6)控制量：本发明两轮小车倒立摆的控制器给出的控制量是两个，即小车左右两个轮子的控制电压，故可在地面自由行走并可遥控；一般型式倒立摆只需一个控制量，只能在垂直面内做直线运动或弧线运动，不能在地平面上运动和遥控。

7)计算机：一般型式倒立摆是采用微处理计算机加上运动控制器给出控制信号来对摆杆进行控制的；本发明两轮小车倒立摆的控制器可以是数字信号处理器DSP，也可以是嵌入式计算机系统，不仅省掉了另需配置的运动控制卡，或模/数、数/模、脉宽调制PWM等模块，且可以脱离微处理计算机到处运动。

附图说明：

图1为两轮遥控小车倒立摆的结构示意图。

图2为两轮遥控小车倒立摆的俯视结构示意图。

图3为两轮遥控小车倒立摆的侧视结构及摆动角度示意图。

图4为两轮遥控小车倒立摆的摆杆与车底盘安装的“几”型支架示意图。

图5为两轮遥控小车倒立摆的控制系统框图。

图6为两轮遥控小车倒立摆的控制电路图。

图7为两轮遥控小车倒立摆工作原理示意图。

图8为两轮遥控小车倒立摆的程序流程示意图。

图9为两轮遥控小车倒立摆带降维观测器的反馈控制系统的动态响应曲线。

图10为两轮遥控小车倒立摆数字滤波器的频率特性曲线。

图11为两轮遥控小车倒立摆的摆杆静止情况下加巴特沃斯(butterworth)数字滤波前后的角速度记录曲线。

图12为两轮遥控小车倒立摆摆杆运动时加巴特沃斯数字滤波前后角速度记录曲线。

图13为两轮遥控小车倒立摆的闭环极点配置算法控制仿真曲线。

图14为两轮遥控小车倒立摆的实时控制响应曲线。

图15为两轮遥控小车倒立摆的线性二次型最优控制算法仿真曲线。

图16为两轮遥控小车二级倒立摆的结构示意图之一。

图17为两轮遥控小车二级倒立摆的结构示意图之二。

图18为两轮遥控小车倒立摆加超声测距模块后的控制系统框图。

图19为两轮遥控小车倒立摆加超声测距模块后的工作原理示意图。

具体实施方式：

实施例1：两轮遥控小车倒立摆及控制方法

一、两轮遥控小车倒立摆的硬件：

本实施例两轮遥控小车倒立摆的车身(1)(长×宽×高230×80×90mm)的底盘(230×80mm)和“几”型支架(9)(长×宽×高64×30×60mm)采用不锈钢板(厚2mm)制作以确保强度，轮子(2)(φ120mm)、摆杆(5)(φ10*500mm)和其它机械部件均采用铝合金板(厚2mm～3mm)制作以减轻小车自重。元器件安装应尽量分布均匀，使装置重量对称，特别要使与车轴垂直方向的车体重量对称。

1.各功能模块的选取：

本实施例中的控制器(11)采用型号为TMS320LF2407的数字信号处理器DSP；用到5V、3.3V和3.300V电压；该芯片片内资源主要包括：544字×16位的双端口数据/程序DRAM，2K字×16位单端口数据/程序SARAM，片内32K×16位的flash程序存储器，256字×16位的片内Boot ROM。该芯片主频可以达到40M，可以满足系统实时性要求；该芯片还自带一些功能模块，使用起来非常方便。在系统中使用到的DSP资源还包括输入输出IO口、定时计数器、模数转换ADC模块、脉宽调制PWM模块以及正交编码计数模块，这样可省掉另需配置的运动控制卡，或模/数、脉宽调制PWM等模块。

存储器芯片IS61LV6416为一种64K×16的高速CMOS静态RAM，供电电压为5V，用于扩充DSP片内存储空间，保存下载的程序和运行的数据。本实施例的系统中用了两片：一片用作外部程序存储器，另一片用作外部数据存储器。

电源转换模块(15)：控制电路板上各元器件需要24V，5V，3.3V，3.300V四种电源。电路板的外接电源是22.5～^-24V直流电源。通过各种电压转换芯片可得到5V、3.3V和3.300V的电源：其中宽直流电压转换模块SF5W芯片将18V～36V的直流电压转换为5V直流电压；电压转换芯片AS1117芯片将5V直流电压转换为3.3V的直流电压；小封装精密电压基准源MAX6043芯片将5V直流电压转换为3.300V的直流电压，精度可以达到0.1％，该芯片专门为DSP控制器中模/数AD采样提供基准电压。

本实施例中直流电机(3)采用两个永磁直流力矩电机60LYX031-Z，最大输入电压27V，最大电流2A，最大输出力矩1.1Nm，最大空载转速470r/min，重量1.2Kg。该种电机输出转矩大，不需要减速装置。若选用带码盘的直流电机，则可省去另配码盘；若选用无刷直流电机，则除电机驱动器不同外，其它没有什么区别。

电机驱动器(12)采用两块脉冲宽度调制式电机驱动模块A3952SW，用于放大从控制器出来的控制信号并驱动直流电机。电机驱动器(12)用到24V、5V和3.3V电压。

用于检测电机(3)的旋转状态--即测量小车位移和速度的码盘(4)可选用光电码盘、电磁式码盘或霍尔元件式编码盘；本实施例采用两个增量式光电码盘OVW02-06-2MD，供电电压为5V，分辨率600脉冲/转。

施密特反向触发器74HC14D芯片为光电码输出脉冲整形也为DSP控制器的PWM输出信号作电平变换。触发器74HC14D芯片供电电压为5V。

在测量方面，两轮遥控小车倒立摆与通常的倒立摆的最大的区别就在于车身及摆杆与铅垂线夹角的测量。一般的倒立摆都有水平导轨或支架，这个导轨或支架就提供了一个水平基准，有了水平基准就可以直接用码盘或电位计来测量摆杆的角度，码盘不存在噪声和零漂问题。两轮遥控小车倒立摆没有这样一个水平基准，只能选择不需要水平基准的传感器件来测量摆杆的角度和角速度，同时还要兼顾测量精度高和测量响应时间短以及价格方面的要求；陀螺仪就是这样一种传感器，但陀螺仪的引入，带来了角速度测量噪声，角度漂移等问题，针对陀螺仪噪声必须进行的数字滤波处理。本实施例陀螺仪传感器(7)采用一种微型惯性器件陀螺仪CRS02，先测出角速度然后积分得到角度；测量范围0～±150°/sec，线性度％F.S.＜0.5％，漂移±10°/s；该传感器的输出可用下式表示：

                    V₀＝Vdd/2+(ω×SF×Vdd/5)

其中，V₀(V)是陀螺仪输出电压，Vdd(V)是陀螺仪的供电电压，ω(rad/s)是测量的角速度，SF(V/rad/s)是比例系数。在本实施例中陀螺仪供电电压Vdd为5V。

遥控模块包括遥控发射器(14)和接收器(13)；供电电压为5V；通过遥控器指挥小车前进、后退、右转，左转；本实施例中遥控发射器为市场购得的四键遥控手柄，为推拉盖式，发射功率为10mW，发射电流为6mA，STM工艺为PT2262编码，其发射有效距离为20米；手柄中的发射器包括一个编码芯片和一个发射模块；当给出一个遥控指令时，编码芯片将指令变成串行信号，传输给发射模块，发射模块再将串行信号转换成315MHZ调频信号发射出去；遥控接收器(13)为射频接收模块J04P，该模块用来接收遥控手柄发送的调频信号，将其解调成串行信号发送给解码器PT2272解码，最终在解码器数据端得到与编码芯片数据端相对应的高低电平，由于芯片内部带有锁存器，因此电平只有在下一个指令状态才会改变；数据输出端口直接与DSP的IO口连接，输入遥控指令信号。

2.机械结构：

本实施例两轮遥控小车倒立摆的机械结构如图1、2、3、4所示：控制电路板(10)由一个控制器(11)DSP和一些外围电路组成，固定在小车车身前部或后部。控制器(11)、电机驱动器(12)、和遥控接收器(13)均焊接在一块印制电路板(10)上。

两个直流电机(3)的机身一端用螺丝对称固定在车体(1)底盘的左右两侧，光电码盘(4)的外壳经一块硬铝合金板与直流电机(3)机身的另一端固定，直流电机(3)轴的一端与轮子(2)固定连接成为轮子的轴，轮子与电机轴用键槽配合，轴顶端由螺丝压紧，使车体及摆杆可在与两轮子轴线相垂直的面内绕两轮子的轴摆动；直流电机(3)轴的另一端通过传动部件(8)如齿轮及齿轮带，与光电码盘(4)的轴相连接，以便测量电机的转速和小车的位移。直流电机(3)的该轴端也可直接与光电码盘(4)同轴相连，从而可省去传动部件(8)，但这样连接会使车体(1)加宽，小车运动不如短车体灵活；有些电机自带编码器，不必另配码盘，但这种电机通常比较长，因而车身较宽，影响小车运动的灵活性；若要使制成的小车车身短，这种电机轴与轮子轴间就必须经传动机构连接。

为保证测量的准确性，必须使陀螺仪(7)测量轴与两个轮子的轴处在一条直线上，由于两轮子转速通常不一致，所以陀螺仪须装在两轮子的中间；为了使两轮遥控小车倒立摆装置以垂线为中心各方向对称，摆杆(5)也必须安装在该位置，即在两轮轴线中心的同一位置既安装陀螺仪(7)又安装摆杆(5)；因此在该位置固接一个“几”型支架(9)，摆杆(5)的一端开丝扣，与支架(9)上端丝扣连接，使车体(1)及摆杆(5)可在与该轴垂直的面内绕轴摆动；因支架(9)内侧处在两车轮轴线中部，因此支架内侧用螺丝固定陀螺仪(7)，使陀螺仪传感器(7)的测量轴与两个轮子轴处在一条直线上；摆杆(5)也可固接(如机械焊接)在底盘中部的支架(9)上，但采用丝扣连接便于装卸、包装和搬运；摆杆(5)也可固接在车体(1)中心的顶部上，省去“几”型支架；但因摆杆经常处于摆动状态，不完全是刚性的，有一些绕性，在底部安装比在车身顶部安装牢固。

采用尼龙材料制作的圆柱状电池盒(φ75*100mm)，内装十八节五号充电电池组成22.5伏直流电源或十六节五号普通电池组成24伏直流电源(6)；为电机和控制电路板上各元器件提供直流电源；为了使车体及摆杆对称，电池盒(6)做成圆柱型，中心有孔套在摆杆上用螺丝固定，可上下滑动调节以改变摆杆重心，从而可改变控制对象的参数；电池盒(6)也可与摆杆(5)固接，或者直接固定在车体(1)上，但固定连接后不易改变被控对象参数；为了更便于更改被控对象的参数，在摆杆上端还可安装一个小托盘，这样在倒立摆稳定运行过程中不必停机的情况下就能在小托盘上放置一个物体(如装有水的杯子)，从而改变摆杆长度、重量和重心，检验控制算法的控制效果。电池盒(6)也可用其它材料制作。

将光电码盘、陀螺仪传感器、直流电机、小车及摆杆、直流电源、控制器及电路板等元器件安装成一个整体，这样省去了各元器件之间的许多外部连线，且工作更可靠。

3.电气连接：

图5给出了本实施例两轮遥控小车倒立摆的电气原理示意图：控制器(11)、电机驱动器(12)、电源转换模块(15)和遥控接收器(13)均焊接在一块印制电路板(10)上；

直流电源(6)经电源转换模块(15)转换成控制器、电机驱动器、遥控接收器、陀螺仪传感器和光电码盘所需电压，接至各元器件的供电端；控制器(11)的输入端与码盘(4)、陀螺仪传感器(7)和遥控接收器(13)的输出端相连接，控制器的输出端与两个电机驱动器(12)的输入端相连接，电机驱动器(12)的输出端接两个电机输入端。

图6给出了两轮遥控小车倒立摆控制电路图即本实施例电路板(10)的具体控制电路图：两个光电码盘(4)的输出信号经74HC14D整形后与DSP控制器(11)的正交编码脉冲计数器T2、T4的输入端相连接，即与DSP控制器(11)的QEP1/QEP2和QEP3/QEP4引脚相接，利用两个定时器T2和T4对两个码盘产生的正交编码进行计数，在每个控制周期的中断程序中读取定时器T2和T4的计数值，经过换算得到车轮位移，进而对其微分得到车轮速度；DSP控制器内的事件管理器模块(EV)中有一个专门用于检测的正交编码脉冲电路。码盘的旋转方向可以通过检测两个脉冲序列哪一列先到达来确定，旋转角位置和转速可通过检测脉冲数和脉冲频率来确定。DSP内部提供了两对输入正交编码脉冲的引脚，QEP1/QEP2和QEP3/QEP4；这两对引脚分别与EVA模块中的定时器T2，和EVB模块中的定时器T4相对应。以EVA模块为例，将定时器T2设置成定向增减计数模式，并以从QEP1/QEP2输入正交编码脉冲作为时钟源，对该口输入的正交编码脉冲进行计数；EVA模块中的正交编码脉冲电路的方向检测逻辑决定了两个序列中的哪一个是先导序列，以决定定时器T2是增计数还是减计数。两列正交输入脉冲的两个边沿都会被正交编码电路计数，因此定时器T2内的计数值是实际脉冲数的4倍。对于EVB模块与EVA模块基本类似；

陀螺仪(7)的输出端接控制器(11)的一个模/数A/D端口ADCIN11引脚相连；控制器(11)的PWM信号输出端PWM7/IOPB0和PWM9/IOPB3经74HC14D作电平变换后分别与A3952SW两块电机驱动器(12)的输入端BRAKE/PHASE相连。驱动器(12)的输出接电机。

遥控接收器(13)的数据端D0.D1.D2.D3与DSP控制器(11)的四个IO端口IOE6，IOE5，IOE4，IOE2相连。

电路图6中：“编程接口插座”是DSP控制器与PC机的编程电缆连接插座；“跳线端子”用不同的跳线来选择下载程序到DSP控制器片内还是到片外存储器、选择确定运行的是片内还是片外程序存储器的程序；“电机驱动器电源跳线”用来选择在调试时电机驱动器是否接24V电源；“电源指示灯”用来显示各电源是否正常。

4.工作原理：

倒立摆的平衡稳定是最重要的，控制的首要目标是使摆杆树立在与铅垂线之间的夹角为零的位置上。然后才是遥控、避障和运动轨迹规划与控制。因为车体及摆杆主要是以小车前后运动的方向偏离铅垂线位置的，与小车左右方向运动的影响很小，为了简化问题，在倒立摆的平衡控制时，主要考虑小车前后方向的运动，因此控制器得出控制量后先给左右电机相同的控制量，使小车前后运动，在满足车体及摆杆平衡稳定后，再给左右电机不同的控制量进行小车左右方向的运动控制。

图7给出了两轮遥控小车倒立摆工作原理示意图：首先，DSP控制器(11)定时启动DSP片内各路数据采集电路；遥控接收器(13)收到遥控发射器(14)的信号后送入DSP控制器；两个光电码盘(4)检测两个电机(3)的旋转状态即小车的位移信号后反馈给DSP控制器(11)，DSP控制器根据该信号和采样周期计算速度信号；陀螺仪(7)测量车身(1)及摆杆(5)绕车轮轴摆动的角速度信号并反馈给DSP控制器(11)，DSP控制器根据该信号积分后得到角度信号；DSP控制器获得以上位移、速度、角度和角速度信号之后与给定的参考信号比较得出误差信号，参考信号由内部程序或遥控接收器(13)设定；最后，用该误差信号根据给定的控制算法计算出使摆杆保持稳定左右电机所需输出的控制力矩，也就是说算出与力矩所对应的控制量即输出两路脉宽调制PWM信号，经电机驱动模块(12)放大后输出给直流电机(3)，控制两个电机的输出转矩，直流电机带动小车运动使摆杆保持平衡不倒。为了简化问题，平衡控制时，上述控制器给出的控制量，加在两个电机驱动器上是同样大小的，在满足了控制的首要目标即车体及摆杆平衡稳定后，控制器(11)才将获得的以上位移、速度、角度和角速度信号与遥控接收器(13)的指令相比较，产生两个不同的控制量输出给电机驱动器(12)，控制小车(1)作前后左右运动；

二.两轮遥控小车倒立摆的平衡控制方法：

两轮小车倒立摆装置控制器的软件是基于C语言设计的，采用美国德州仪器TI(TexasInstruments)公司提供的CCS语言开发系统，使用循环周期中断触发机制对系统进行实时的控制，每个周期中完成了对输入模拟信号的采集、滤波，控制律(控制算法)的实时计算，输出控制量，实现倒立摆的反馈控制。控制和采样周期定为五至二十毫秒；周期长了来不及控制，摆杆会倒；周期短又受完成任务的多少、程序计算时间以及微处理器CPU和其它硬件速度的限制；图8给出了其程序流程图：其中(P1)主程序先进行必要的初始化如完成系统中所使用的变量的初始化和对DSP工作状态的设置，其中DSP工作状态的设置主要包括中断端口的设置，遥控信号等的输入输出端口的设置，通用时间基准T1，T2，T3，T4的设置，T1为中断周期的设置，T2、T4为码盘计数的设置，T3为PWM信号输出的设置。程序模块(P2)为中断入口，当响应INT2中断时，转入中断子程序c_int2。进入中断子程序后，程序模块(P3)先根据外设中断矢量寄存器判断是否为通用定时器T1的周期中断？若不是，程序模块(P4)使能定时器T1的周期中断，为下一次定时器T1中断做准备，并返回主程序；若是，则执行(P5)至(P18)各程序模块：模块(P5)采样左右两个码盘脉冲数的通用定时器T2、T4的计数器数值，因为每个码盘送来的信号为两个正交编码脉冲输入，数字信号处理器DSP里的正交编码脉冲电路对这两个正交脉冲输入信号的两个边沿(上沿和下沿)进行计数，因此产生的时钟频率是每个输入序列的四倍，所以，在采样计数器数值之后，先要把数值除以四再赋给两个记录码盘输送脉冲数的单元Encode_l_1和Encode_r_1。为了消除模/数AD采样和陀螺仪输出信号可能存在的偶然因素，需要在一个周期内多次采样取均值；如采样模块(P6)对陀螺仪的输出端口连续采样9次，将9次的采样值转换成数字信号，依次送入单元pendulum[0]～pendulum[8]，取平均值模块(P7)对这些数据进行初步筛选，剔除单元pendulum[0]～pendulum[8]中的最大的两个数据和最小的两个数据后，剩下的数据取均值并放入数据单元pendulum_midl。于是，进入计算陀螺仪自动判断零位电压的模块(P8)，(具体方法见如下b)“自动判断零位电压”)，得到零位电压值并放入单元pendulum_zero，且此零位电压的计算部分只在程序开始时执行一次，得到零位电压以后下一个中断响应时不执行程序模块(P8)。然后，模块(P9)判断是否满足摆杆稳定且能进行零位漂移校正？若能，则执行模块(P10)进行零位电压漂移的校正，(具体方法见如下c)“校正零位电压的漂移”)接着执行模块(P11)；若不满足零位漂移校正条件，则直接执行模块(P11)，根据处理过的原始采样数据由公式ω＝(V₀-V_zero)/SF计算摆杆角速度；其中ω为摆杆角速度，V₀为均值pendulum-midl，V_zero为零位电压(pendulum_zero)，SF为转换的比例系数，这个比例系数，是事先通过保持小车不动，摆动摆杆时比较陀螺仪信号与码盘输出信号测得比例系数为0.8而选定的。由于引入陀螺仪的信号而带来的高频噪声，模块P(12)紧接着采用递归型数字滤波器将摆杆角速度信号进行滤波。(滤波方法见d)“对测量噪声进行滤波处理”)，然后，模块(P13)由所得滤波后的角速度(adjusted_speed)和定时器T1的周期时间积分计算摆杆的角度(pendulum_angle)。至此，程序已经处理完陀螺仪产生的信号，得到了摆杆的角速度和角度。接着，模块(P14)根据上述的码盘计数单元(Encode_l_1)、和(Encode_r_1)计算小车的位移(本实施例所选码盘为每转600个脉冲)，并由小车前后两次采样的位移差和定时器T1的周期时间计算出小车的速度，因为两轮小车倒立摆的采样周期为5～20ms，非常短，这样小车速度的计算可以由位移序列的后向差商来近似。程序的下一步为小车遥控功能模块，调用程序模块(P15)(见如下a))；程序模块(P16)根据小车倒立摆装置的输出量(小车的位移、速度，摆杆的角度和角速度)和设定的控制算法(见如下e)“控制算法”)计算出所需要的脉宽调制PWM控制电压的占空比；接着，程序模块(P17)将计算出的占空比数据，编程转换为脉宽调制PWM信号输出给电机驱动器，驱动直流电机的转动，使得两轮小车倒立摆在不稳定的平衡点处保持平衡。最后，模块(P18)使能定时器T1的周期中断，为下一次定时器T1中断做准备，返回主程序。

a)程序模块(P15)首先判断有无新的遥控接收器信号？没有则返回调用点执行程序模块(P16)、(P17)和(P18)；有新的信号，则根据遥控接收器接收到的信号分别设置小车运动状态参数，如前进、后退、左右拐弯；然后，控制器(11)再对摆杆是否平衡稳定作判断即判断摆杆的角度、角速度是否连续在一百个周期内都很小如角度在[-0.2，0.2]内，角速度在[-0.1，0.1]内？若摆杆稳定平衡，则控制器(11)根据遥控接收器(13)的指令与现有小车状态相比较，用设定的控制算法(见如下e)“控制算法”)计算出所需要的脉宽调制PWM控制电压的占空比；返回执行程序模块(P17)，产生输出信号控制小车前后左右自由运动；若摆杆还没稳定平衡，则返回执行程序模块(P16)、(P17)和(P18)；直到摆杆稳定平衡时才执行遥控指令。

b)自动判断零位电压：

程序中模块(P8)按如下方法对起始角度信号做自动判断处理：

运行程序模块(P8)，让摆杆处在初始位置，使其在该位置保持五到十五秒不动，程序采样陀螺仪信号，采样周期时间为5到^-20毫秒，每前后两个周期的采样值在陀螺仪误差范围内如-0.3～0.3V为有效数据，采集到的数据有效后用一个存储单元计数开始计数，累积计数至五到十五秒为止，取有效数据的平均值作为起始角度信号，即找到与初始位置相对应的采样值，再经简单处理该采样值转换为铅垂位值所对应的零位，零位电压自动判断完后接下的程序模块会使倒立摆进入下一步工作；若前后两次采样值不在误差范围内，则重新计数，直到满足连续五到十五秒钟采样数据有效后，才停止初始位置的判断；例如：采样周期时间为15毫秒，存储单元累积计数一千次就为15秒，每前后两个周期的采样值是否在陀螺仪传感器的误差范围内？若在误差范围内为有效数据，存储单元进行计数；若不在误差范围内为无效数据，存储单元计数值清零；连续累积计数至一千为止，取有效数据的平均值作为起始角度信号；起始位置是铅垂位置时，则有效数据的平均值即为零位电压；起始位置是摆杆倒在水平面上时，则程序里需加90度转变铅垂位置；零位电压自动判断完后倒立摆进入下一步工作。

零位电压的判断只运行一次，而且是必须的一次，找到之后程序跳过此段程序；没找到需继续寻找，程序不会进行下一步工作就中断返回。

每一次程序复位，或者小车装置启动，程序最开始的一段时间都不对小车实施控制，即控制器计算出控制量后不输出给电机驱动器，在中断程序中只采样陀螺仪输出电压，对起始角度信号做判断，如在采样到满足要求的一千个电压值后，对采样值求均值，得到零位电压。采样周期为0.015秒，每次复位后都要等待20秒左右程序检测零位电压，在这段时间内，用手扶住摆杆，使之在竖直位置保持静止。可在小车上安装一个气泡水平仪，以此来保证小车处于竖直位置。如果采样过程中手抖动比较大，或者摆杆位置有一定的移动，就重新再采样一千个数，判断出起始角度信号后才对小车实施控制；

c)校正零位电压的漂移

对于陀螺仪零位电压漂移即初始角度漂移主要是指摆杆的测量角度与真实角度之间存在误差，并且误差会随着时间的推移逐步累积。校正陀螺仪零位电压的目的是为了抑制测量摆杆角度的漂移，如果已知测量角度在某一时间内的漂移量，就可以反推零位电压的误差，从而对零位电压进行校正。该方法是否可行的关键在于能否准确地确定角度的漂移量，而这必须建立在系统已经控制稳定的基础上。程序模块(P9)用测量的摆杆角速度在连续的50个采样周期内保持在[-0.1 0.1]范围内来作为满足摆杆稳定且能进行零位漂移校正的条件，摆杆角速度在连续的多少个采样周期内保持在什么测量值范围内的这个条件可根据实际情况和多次实验来确定。在程序中按如下两种方法之一或两种方法结合对陀螺仪零位电压进行零漂的校正：

1).在用控制算法成功控制小车时，摆杆在平衡位置左右晃动一段时间后会在平衡位置短暂停顿，此时摆杆的实际角度为零，假如称这种状态为停顿状态，并假定停顿时间间隔为[t₁，t₂]，(t₁，t₂)，且在时刻t₁和t₂的测量角度值分别为θ(t₁)和θ(t₂)，它们的差值θ(t₁)-θ(t₂)便是测量角度值在时间间隔t₂-t₁内的漂移量；如果测量角度漂移完全是由陀螺仪的零位电压所引起，由此便可以计算出零位电压的误差，然后对其进行校正，经过多次校正后，零位电压的误差就会逐渐逼近真实值，这样可以很大程度上抑制测量角度漂移。这种方法的关键所在就是如何判断小车正处于停顿状态，要准确地检测出摆杆的停顿状态。

2)．对于陀螺仪零位电压即角度漂移，可利用如《计算机控制基础》(中国科学技术大学出版社2001.9 P258-264)或《离散时间控制系统》(西安交通大学出版社1990.5 487-514)所介绍的降维状态观测器对摆杆角度进行观测，再用观测值对陀螺仪零位电压进行实时校正：系统中的小车位移和速度、摆杆角度和角速度，有三个是可以准确测量的，只有摆杆角度值存在漂移。可以设计一个降维状态观测器，利用位移、速度和角速度以及系统的输入(控制器的输出控制量)来估计角度值，然后和另外三个状态：位移、速度和角速度一起对系统构成反馈控制。

将系统的离散状态空间方程(c-1)：

x(k+1)=GX(k)+Hc_θ(k)    (c-1)

其中 $G=\begin{array}{c}\left[\begin{array}{cccc}1.002& 0& 1.015& 0\\ -0.0002& 1& 0& 0.015\\ 0.2685& 0& 1.002& 0\\ -0.0237& 0& -0.0002& 1\end{array}\right]\text{,H=}\left[\begin{array}{c}-0.0012\\ 0.0005\\ -0.1646\\ 0.0673\end{array}\right]\end{array}$

写成如下形式：

$\left[\begin{array}{c}{X}_1(k+1)\\ X_2(k+1)\end{array}\right]=\left[\begin{array}{cc}{A}_{11}& A_{12}\\ A_{21}& A_{22}\end{array}\right]\left[\begin{array}{c}{X}_1\left(k\right)\\ X_2\left(k\right)\end{array}\right]+\left[\begin{array}{c}{B}_1\\ B_2\end{array}\right]c_{\mathrm{\θ}}\left(k\right)---(c-2)$

Y(k)＝CX(k)

其中，X₁＝[ω x_m v_m]^T，X₂＝θ， $A_{11}=\left[\begin{array}{ccc}1.002& 0& 0\\ 0& 1& 0.015\\ -0.0002& 0& 1\end{array}\right]$ ，A₂₁＝[0.015 0 0]，A₂₂＝1.002，A₁₂＝[0.2675 -0.0002 -0.0231]^T，B₁＝[-0.1551 0.0005 0.0638]^T，B₂＝-0.0012，C＝[I₃ 0]。

在离散系统(c-2)中X₁(k)是可以直接测量的，目的是估计状态X₂(k)。

降维观测器的设计主要分以下几个步骤：

第一步，证明(A₂₂，A₁₂)是完全能观测的。由于A₂₂为一非零常数，所以(A₂₂，A₁₂)的能观矩阵M₀＝A₁₂，rank(M₀)＝1，显然是完全能观测的。

第二步，计算常值矩阵G，使得A₂₂+GA₁₂的特征值在复平面的单位园内。

第三步，得到状态X₂(k)的降维观测器：

${\tilde{X}}_2\left(k\right)=Z_e\left(k\right)-\mathrm{GY}\left(k\right)$

$Z_e(k+1)=(A_{22}+G{A}_{12})Z_e\left(k\right)+(B_2+G{B}_1)c_{\mathrm{\θ}}\left(k\right)+$

$[(A_{21}+G{A}_{11})-(A_{22}+G{A}_{12})G]Y\left(k\right)---(c-3)$

其中

为X₂(k)的估计值。

最后再用如《(离散时问控制系统)》(西安交通大学出版社1990.5 582-591)或《(计算机控制基础)》(中国科学技术大学出版社2001.9 269-278)所介绍的线性状态反馈控制中的线性二次型最优控制的方法设计状态反馈控制系数K，并用估计值 代替X₂(k)进行反馈，此时系统输入为：

$c_{\mathrm{\θ}}\left(k\right)=K{\left[\begin{array}{cc}{X_1}^T& {\tilde{X}}_2\end{array}\right]}^T---(c-4)$

仿真效果：取G=[-3.3475 0.0022 0.2887]，此时A₂₂+GA₁₂的特征值为0.1。用LQR的方法得到式(c-4)中的状态反馈系数K=[1.7979 0.6351 1.2224 6.7037]。假定系统初始状态X=[0 0 0 0.3]^T，此时带降维观测器的反馈控制系统的动态相应曲线如图9所示。从图9可以看出，这种带降维观测器的反馈控制系统是闭环稳定的，并且由降维观测器(c-3)所估计的角度值确实能够很好地逼近真实角度值。由于状态观测器是基于精确模型的，如果得到了系统的精确模型，便可以采用本方法。

d)对测量噪声进行滤波处理即对陀螺仪传感器输出噪声信号进行数字滤波：

理想状况下，当摆杆静止不动时，检测到的摆杆角速度应该为零。现实中，由于角速度存在噪声，并通过角速度反馈给控制器，将噪声带入控制变量，这会导致在稳定状态下，控制变量仍然会正负波动，从而引起其他系统状态的波动，表现在实际系统中就是小车会在平衡位置不停地抖动。本实施例中，摆杆静止不动的情况下检测到的摆杆角速度噪声的幅值在[-0.1，+0.1]之间变化，单位是rad/s，频率在200Hz以上。角速度的有用信号的变化范围一般在[-2，+2]之间。由此可见信噪比非常大，有必要进行滤波处理。

选用递归型数字滤波器，其Z传递函数形式为：

$H\left(z\right)=\frac{B\left(z\right)}{A\left(z\right)}=\frac{b\left(1\right)+b\left(2\right)z^{-1}+\…+b(n+1)z^{-n}}{a\left(1\right)+a\left(2\right)z^{-1}\text{+\…+a}(n+1)z^{-n}}---(d-1)$

写成输入输出关系可表示为：

$y\left(N\right)=\frac{1}{a\left(1\right)}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}a(i+1)y(N-i)+\frac{1}{a\left(1\right)}\underset{j=0}{\overset{n}{\mathrm{\Σ}}}b(j+1)x(N-j)---(d-2)$

设计递归型数字滤波器实际上就是根据系统实际要求，确定上式中的滤波系数a(1)…a(n+1)和b(1)…b(n+1)。一种比较常用的设计方法就是先根据系统要求设计出模拟滤波器，然后将其转换为数字滤波器。

程序模块(P10)按如下方法对陀螺仪输出噪声信号进行数字滤波处理：

对陀螺仪的采样值，为了消除模/数AD采样和陀螺仪输出信号可能存在的偶然因素，需在一周期内多次采样取均值：运行程序模块(P6)和(P7)进行初步筛选、剩下的数据取均值并放入了数据单元pendulum_midl后，采用数字滤波器对采样均值信号进行滤波；滤波方法用的是递归型三阶的巴特沃斯(butterworth)模拟低通滤波器原型，然后将其转换成数字滤波器；选择巴特沃斯滤波器主要是因为这种滤波器的通带内的幅值响应比较平稳，并且相比较于其他类型的模拟滤波器，巴特沃斯滤波器转化为数字滤波器后，它的滤波特性不会有太大的改变。

利用软件“矩阵实验室”MATLAB语言(MATrix LABoratory)提供的信号处理工具箱，工具箱提供了一个butter的函数，可以非常便捷地按照系统实际要求设计butterworth数字滤波器。

函数格式：[b，a]＝butter(n，ω)

其中，n为滤波器阶数，ω为截止频率，在该函数中ω取值在[0，1]之间，其中1对应于0.5f_s(采样频率)。得到两个n+1维的向量b，a，这两个向量中的元素对应(d-1)式中的滤波系数a(1)…a(n+1)和b(1)…b(n+1)。

实验发现，当数字滤波器的截止频率选择在10Hz时滤波效果比较好。并且由于实际采样时间为0.015s时，所以函数butter中的参数 $\mathrm{\ω}=\frac{10}{0.5f_s}=0.3。$

在MATLAB命令窗口输入：

[b，a]＝butter(3，0.3)；

freqz(b，a，100，66.7)；

可以得到数字滤波器参数以及该滤波器的频率输出特性如图10。

滤波前后的角速度信号之间的差异，分摆杆静止和摆杆运动两种情况：

在摆杆静止情况下如图11所示，陀螺仪输出即为噪声。在没滤波前，噪声的均方差为0.0023，滤波后噪声的均方差为0.0002，少了一个数量级。可看出噪声的频率明显降低。在摆杆运动的情况下如图12所示：滤波前，所测得的角速度信由于噪声的影响，有很多毛刺，尤其是在摆杆运动幅度不大的时候，噪声的影响愈为严重。在经过滤波处理后，记录的输出曲线明显平滑了很多。

由此可见，用这种方法设计出来的数字滤波器无论对摆杆静止时的噪声还是摆杆运动时的噪声均有很好的抑制作用。控制程序在没有加滤波模块时直流电机会因频繁换向而震动；加了滤波模块后，这个问题马上得到了解决。

e)控制算法：

所述的给定控制算法，可选用线性状态反馈控制、自适应控制、非线性控制和智能控制等算法；如《离散时间控制系统》(西安交通大学出版社1990.5 466-487)或《计算机控制基础》(中国科学技术大学出版社2001.9 176-180，243-255)所介绍的线性状态反馈控制中的闭环极点配置；有如《离散时间控制系统》(西安交通大学出版社1990.5582-591)或《计算机控制基础》(中国科学技术大学出版社2001.9 269-278)所介绍的线性状态反馈控制中的线性二次型最优控制；有如《自动化学报》(2000，26(4)：509-514)和《南京航天航空大学学报》(2002，34(5)：461-465.)所介绍的鲁棒方差控制算法；有如《航空学报》(1995，16(6)：654-661)所介绍的智能控制中的拟人控制算法；有如《遗传算法及其应用》(人民邮电出版社1999.5 356-358)所介绍的遗传控制算法；有如《智能控制理论和方法》(西安电子科技大学出版社1999.10 193-197)所介绍的智能控制中的模糊控制与遗传算法；有如《模糊控制原理与应用》(机械工业出版社1998.4 575-577)所介绍的智能控制中的模糊与神经网路控制；均可利用。利用软件“矩阵实验室”MATLAB语言提供的许多工具箱，可以很方便地对以上算法进行求解、设计和仿真。例如对本实施例的其中两种方法如下：

在本实施例中根据已制成的装置硬件可以得到两轮遥控小车倒立摆的状态空间方程，并能够实时、准确地检测系统状态后，接下来的工作就是设计一个合适的反馈控制控制器，使系统闭环稳定。在各种反馈控制算法中，状态反馈是最基本，也是最容易实现的，对于两轮遥控小车倒立摆的状态空间方程，在平衡控制时，先假设控制器输出给左右两个电机的控制量是相同的，因此控制器输出的控制量只有一个：

                        X＝AX+Bc_θ                            (e-1)

其中 $X={[\mathrm{\θ},x_m,\mathrm{\ω},v_m]}^T,A=\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 17.819& 0& 0& 0\\ -1.5365& 0& 0& 0\end{array}\right],B=\begin{array}{}\left[\begin{array}{c}0\\ 0\\ -10.3318\\ 4.2519\end{array}\right]\end{array}$

鉴于该系统状态空间方程是一个连续的时不变的4阶系统，因此该系统能控的一个充要条件就是：能控判别矩阵M_c＝[B，AB，A²B，A³B]，为满秩，即rank(M_c)＝4。

在MATLAB中键入命令：

＞＞A＝[0，0，1，0；0，0，0，1；17..819，0，0，0；-1.5365，0，0，0]；

B＝[0，0，-10.3318，4.2519]′；

rank([B，A*B，A^2*B，A^3*B])

得到：ans＝4

可见，系统的能控判别矩阵M_c为满秩的。因此该系统时能控的。

在离散的情况下：在实际系统中，是定时对各个控制状态进行采样，然后给出控制量，因此控制状态与控制量均为离散值，因此还须对系统(e.1)进行离散化处理。

由于控制量在采样周期内是不变的，等效于零阶保持器作用于控制量，可以按照零阶保持器的情况对连续系统(e.1)进行离散化。

在MATLAB中键入命令：[G，H]＝c2d(A，B，Ts)

其中Ts为采样时间，系统中的采样时间定为0.015秒；G和H分别为离散系统的状态转移矩阵和控制矩阵。由此可以得到系统(e-1)离散化后的状态方程：

                   X(k+1)＝GX(k)+Hc_θ(k)             (e-2)

其中 $G=\left[\begin{array}{cccc}1.002& 0& 0.015& 0\\ -0.0002& 1& 0& 0.015\\ 0.2685& 0& 1.002& 0\\ -0.0237& 0& -0.0002& 1\end{array}\right],H=\begin{array}{}\left[\begin{array}{c}-0.0012\\ 0.0005\\ -0.1646\\ 0.0673\end{array}\right]\end{array}$

对于离散系统(e-2)能控的充要条件为：其能控矩阵M_d＝[H，GH，G²H，G³H]为满秩。

同样，用MATLAB可以很方便地求得rank(M_d)＝4。因此状态空间方程(e-2)也是能控的。

综上所述，无论是连续状态空间方程(e-1)，还是离散状态空间方程(e-2)，均可用状态反馈c_θ＝K*X将闭环系统极点配置在稳定区域。

闭环极点配置算法的设计方法：

对于系统(e-1)，希望引入一个反馈控制器c_θ＝K*X，使得闭环系统的极点可以移到指定的位置，用MATLAB控制系统工具箱提供的place函数可以很方便地解决极点配置问题。

该函数调用格式：K＝place(A，B，P)

其中(A，B)为系统(e-1)的状态空间方程模型，向量P为期望极点位置。返回状态反馈向量。对于闭环极点位置的选择主要遵循以下原则：

所有的闭环极点都位于左半s平面；要突出一对闭环主导极点，两对极点的实部比值最好超过5；主导极点远离jω轴，闭环系统时间常数变大；主导极点远离σ轴，闭环系统阻尼系数变小。本例闭环极点选择为：P＝[-8±j -1.5±j]

调用place函数，得到状态反馈系数K＝[13.66 3.196 3.261 3.726]。将该反馈系数带入离散系统(e-2)中，并假定初始状态X₀＝[0.2 0 0 0]^T，系统的零输入响应曲线如图13所示，从中看出系统三秒后就可进入平衡稳定状态。实控曲线如图14所示。

线性二次型最优控制(LQR)算法的设计方法：

考虑离散系统(e-2)，引入最优控制的性能指标：

$J=\underset{k=1}{\overset{+\∞}{\mathrm{\Σ}}}[X{\left(k\right)}^T\mathrm{QX}\left(k\right)+R{c}_{\mathrm{\θ}}{\left(k\right)}^2]---(e-3)$

设计反馈控制率c_θ(k)＝K*X(k)，使得性能指标J最小，其中Q和R分别为状态变量和输入变量的加权矩阵。MATLAB控制工具箱提供了dlqr函数来求解这类问题。

该函数调用格式：K＝dlqr(G，H，Q，R)

其中(G，H)为系统(e-2)的状态空间方程模型。返回状态反馈状态。

关于加权矩阵的选取，考虑到实际系统中控制电机的最大输出转矩有限，因此应该选择加权矩阵Q和R，使得闭环系统在满足动态性能的前提下，输入量要尽可能小。

选择R＝1，Q＝3*ones(4)，得到状态反馈控制系数：K＝[6.5337 1.5331 2.1776 2]，将该反馈系数带入系统中得到的系统闭环仿真曲线如图15所示，条件和前面一样，初始状态X₀＝[0.2 0 0 0]^T，从中看出系统四秒后就可进入平衡稳定状态。

三.效果：

倒立摆装置组装好后，按两轮遥控小车倒立摆的平衡控制方法编好程序，通过电缆给DSP控制器下载程序，下载完毕后断开下载电缆，接通装置的电源，运行程序进行周期循环采样、，用手扶直摆杆，尽量使其在铅垂位置并保持十五秒左右不动，这时程序会自动判断此位置是摆杆的初始位置，然后放开手，控制器进行周期循环采样、计算、输出：即采集倒立摆装置的运动状态、按一定的控制算法计算并输出控制量，驱动直流电机使电机运动，以保持摆杆不倒，从找到零位到摆杆稳定平衡只需五六秒时间，稳定后，可用遥控手柄发出左、右、前、后运动的遥控指令，倒立摆装置收到遥控指令后，会按遥控指令在地面上转弯行走。用闭环极点配置或线性二次型最优控制方法设计的状态反馈系数来控制倒立摆平衡，控制效果非常好。另两例是用具有圆盘极点、方差约束的鲁棒控制算法和拟人智能控制算法，运行表明两轮遥控小车倒立摆不仅能按照遥控指令在地平面上稳定行走，而且在其摆杆上斜挂一公斤多重的水杯(改变了被控对象的重量和中心)和在摆杆上端顶一公斤多重的水杯(改变了被控对象的重量、重心和相当于改变了摆杆的长度)时，它都可以照样稳定不倒，挂水杯和顶水杯前后系统的模型会有很大的差异，使系统的重量，重心位置以及摆杆绕重心旋转的转动惯量都发生较大的变化，然而即便是在这样的情况下，小车仍然可以很好地保持平衡；为了进一步检验控制算法的抗干扰和自适应能力，把两轮遥控小车倒立摆装置放入一个具有15度左右上坡和下坡的地面上，它能在斜坡上行走仍能行动自如。将倒立摆挂上一公斤多重的水杯、并在摆杆上端顶一公斤多重的水杯、走斜坡、遥控操作等已在中国科技大学信息学院给学生们做过演示；这些演示充分说明了在两轮遥控小车倒立摆上能有效验证各种控制算法性能。经使用证实：两轮遥控小车倒立摆结构紧凑、工作可靠、产生了自由无约束运动、抗干扰能力强、鲁棒性好的积极效果。

实施例2：两轮遥控小车二级倒立摆及多级形式的倒立摆

以本发明的上述两轮遥控小车倒立摆的实施例为基础，还可以做成二级、三级或更多级形式的倒立摆，做法如下：

二级倒立摆制作方法之一，如图16所示：车体(1)及摆杆(5)绕轮子(2)轴旋转；在与车体固接的原摆杆(5)上端再铰接另一根摆杆(16)，该摆杆(16)可绕铰接点旋转，它们的旋转面均为与旋转轴垂直的面；在铰接处与旋转轴同轴的位置增装一个角度测量装置，使其能测量两摆杆之间形成的夹角(θ1-θ2)；这样车体(1)和与其固接的原摆杆(5)构成第一级摆(下摆)，与第一级摆杆(5)上端铰接的另一根摆杆(16)构成第二级摆(上摆)；车体(1)及摆杆(5)绕轮子的轴旋转，旋转面也为与旋转轴垂直的面，由于测量没有水平基准，第一级摆(下摆)的旋转角θ1需用惯性测量元件，本例中用陀螺仪，陀螺仪先测出角速度然后通过积分得到角度；第一级摆(下摆)和第二级摆(上摆)之间形成的夹角(θ1-θ2)可用比陀螺仪价廉的码盘或价格更廉的电阻电位计来测量。

二级倒立摆制作方法之二，如图17所示：车体(1)绕轮子(2)轴旋转；摆杆(5)在车体(1)的顶部中心与车体(1)铰接，使摆杆(5)可绕铰接点旋转，它们的旋转面均为与旋转轴垂直的面；并在铰接处与旋转轴同轴的位置增装一个角度测量装置，使其能测量摆杆(5)与车体(1)之间形成的夹角(θ1-θ2)；这样车体(1)构成一级摆(下摆)，与车体铰接的摆杆(5)构成第二级摆(上摆)；由于车体绕轮子的轴旋转，测量没有水平基准，车体(1)绕轮子(2)轴旋转的角度即第一级摆(下摆)的旋转角θ1需用惯性测量元件，本例中用陀螺仪，陀螺仪先测出角速度然后通过积分得到角度；车体(下摆)和第二级摆(上摆)之间形成的夹角(θ1-θ2)可用比陀螺仪价廉的码盘或价格更廉的电阻电位计来测量。

在上面介绍的两种二级倒立摆之任一种的基础上，即在第二级摆杆的上端以同样方法继续铰接另一根杆子，并在铰接处增装一个角度测量装置，就可做成三级倒立摆；类似地可以做成四级或更多级形式的倒立摆。

两轮遥控小车二级倒立摆的平衡控制与两轮遥控小车一级倒立摆的平衡控制原理相同，程序设计方法也一样，只是状态变量多了两个，计算机程序多了几次运算。

实施例3：可以避障的两轮遥控小车二级倒立摆

以本发明的上述两轮遥控小车倒立摆的实施例1为基础，还可以使该倒立摆具有避让前方障碍物的功能，做法如下：

超声测距模块包括一个发射器(17)和两个接收器(18)，超声模块与遥控模块相似，由发射和接收两部分组成。发射部分由一个38KHZ频率发生器和一个超声波发射探头组成；接收部分则由一个接收探头，和一个传感器放大电路组成。它们的供电电压都为5V；38KHZ的频率发生器由DSP控制器内部程序实现，并通过DSP控制器的IOPF6口直接驱动发射探头；发射探头将脉冲信号转换为超声波发射出去，声波在遇到障碍物后会反射回来，两个接收探头在接收到反射波后，再将其复原为脉冲，脉冲经过放大电路放大后分别接到DSP控制器的两个外部中断口。在DSP控制器产生38KHZ脉冲的同时打开定时器，在接收到脉冲时，引起外部中断，在中断程序中记下此时定时器的值，这就是声波来回所用的时间t；声波在空气中的速度为340m/s；所以距离l＝t*340/2。由此便可得到障碍物的距离信息，由于两个接收器收到的距离不同，由此得到障碍物的方向信息。

图18给出了本实施例两轮遥控小车倒立摆加了超声波模块后的电气原理示意图：把超声测距模块的发射器(17)和接收器(18)按原理示意图焊接在电路板上，电路板安装于车体前部。由控制器DSP内部产生38KHz的发射信号，并由DSP的IOPF6口经超声测距模块的发射探头(17)发送超声波，在遇到物体后反射回来，通过超声测距模块的接收器(18)即红外接收电路将接收到的信号送入控制器DSP的外部中断INT1端口。

图19给出了两轮遥控小车倒立摆加超声波模块后工作原理示意图，图6给出了具体电路图：左右两个超声波模块接收器(18)将接收到的信号接到控制器DSP的外部中断XINT1和XINT2端口，它的中断级别低于主中断INT2；控制器(11)根据接收到的信号与现有小车状态相比较，确定是否减速、停止或拐弯，并产生相应输出信号给电机驱动器驱动电机，指挥小车避让前方障碍物。

Claims (9)

1、一种两轮遥控小车倒立摆，包括：控制器、直流电机及电机驱动器、电源、小车及摆杆；其特征在于：两个直流电机(3)对称固定在车体(1)底盘左右两侧，每个直流电机(3)的轴分别与一个轮子(2)的轴相互固接，轴的另一端直接与一个码盘(4)同轴相连或通过传动部件(8)与一个码盘(4)的轴相连；该左右两个轮子(2)的轴线中部位置的车体底盘上固接一个支架(9)，支架(9)上端与摆杆(5)固接，使车体(1)及摆杆(5)可在与轴垂直的面内绕轴摆动；支架(9)内侧固定陀螺仪传感器(7)，使其测量轴与两个轮子轴处在一条直线上；直流电源(6)固定在车身(1)上；

控制器(11)、遥控接收器(13)、电源转换模块(15)、电机驱动器(12)按如下电气信号连接关系焊接在印制电路板(10)上：直流电源(6)经电源转换模块(15)接控制器、电机驱动器、遥控接收器、陀螺仪传感器和光电码盘的供电端，控制器(11)的输入端与码盘(4)、陀螺仪传感器(7)和遥控接收器(13)的输出端相连接，控制器的输出端与两个电机驱动器(12)的输入端相连接，电机驱动器(12)的输出端接两个电机输入端；该印制电路板(10)固定在车身(1)上。

2、如权利要求1所述的两轮遥控小车倒立摆，特征在于所述控制器(11)选用数字信号处理器、嵌入式计算机系统或单片机。

3、如权利要求1所述的两轮遥控小车倒立摆，特征在于所述直流电机(3)选用直流力矩电机、带码盘的直流电机或无刷直流电机。

4、如权利要求1所述的两轮遥控小车倒立摆，特征在于所述直流电源(6)与摆杆滑动连接，或与摆杆固定连接，或直接安装在车体上。

5、如权利要求1所述的两轮遥控小车倒立摆，特征在于在与所述车体固接的原摆杆(5)上端再铰接另一根摆杆(16)，该摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；在铰接处与旋转轴同轴的位置增装一测量两摆杆之间形成的夹角(θ1-θ2)的角度测量装置；这样，车体和与其固接的原摆杆构成第一级摆(下摆)，与第一级摆杆上端铰接的另一根摆杆构成第二级摆(上摆)。

6、如权利要求1所述的两轮遥控小车倒立摆，特征在于所述摆杆(5)与车体(1)上端铰接，使摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；并在铰接处与旋转轴同轴的位置增装一个测量摆杆与车体之间形成的夹角(θ1-θ2)的角度测量装置；这样，车体构成一级摆(下摆)，与车体铰接的摆杆构成第二级摆(上摆)。

7、如权利要求5或6所述的两轮遥控小车倒立摆，特征在于在所述第二级摆杆的上端以同样方法继续铰接另一根杆子，使该摆杆可绕铰接点旋转，旋转面为与该旋转轴垂直的面；并在铰接处增装一个角度测量装置，即成三级倒立摆。

8、权利要求1所述两轮遥控小车倒立摆的平衡控制方法，包括：由两个码盘(4)检测小车两个轮子的位移和速度信号后反馈给控制器(11)；由陀螺仪传感器(7)测量摆杆(5)的角速度信号后反馈给控制器(11)；控制器(11)将获得的上述反馈信号与由程序设定的参考信号比较得出误差信号；根据预先给定的控制算法，用该误差信号计算出控制量，经电机驱动器(12)输出给直流电机(3)，直流电机(3)带动小车运动使摆杆(5)保持平衡不倒；

其特征在于：

a)控制器(11)将获得的上述反馈信号与遥控接收器(13)的指令相比较，产生输出信号控制小车作前后左右运动；

b)控制器(11)按如下方法自动判断零位电压：

中断程序在每个5到^-20毫秒的采样周期调用一次自动判断零位电压程序模块(P8)；让摆杆在初始位置保持五到十五秒，采样模块(P6)和取平均值模块(P7)采样陀螺仪传感器的信号并取均值；接着，程序模块(P8)判断“零位电压标志”是否为结束：若是，则返回调用程序；若未结束，则进入判断零位电压程序：程序模块(P8)用一个存储单元来计数；每前后两个周期的采样均值是否在陀螺仪传感器的误差范围内？若不在误差范围内则为无效数据，存储单元计数值清零，返回调用程序；若在误差范围内则为有效数据，存储单元计数值加一；然后，判断计数值是否连续累积计数至五到十五秒，若否则返回调用程序；若是则置“零位电压标志”为结束，且取有效数据的平均值作为起始角度信号；起始位置是铅垂位置时，则有效数据的平均值即为零位电压，起始位置是摆杆倒在水平面上时，则程序里需加90度转换为铅垂位置；返回调用程序；

c)校正零位电压的漂移，按如下两种方法之一进行零漂的校正：

1).在用控制算法成功控制小车时，摆杆在平衡位置左右晃动一段时间后会在平衡位置短暂停顿，设此时摆杆的角度为零，称这种状态为停顿状态，设停顿时间间隔为[t₁，t₂]，(t₁＜t₂)，且在时刻t₁和t₂的测量角度值分别为θ(t₁)和θ(t₂)，它们的差值θ(t₁)-θ(t₂)即是测量角度值在时间间隔t₂-t₁内的漂移量，其对应的采样值是零位电压漂移；然后对其进行校正，经过多次校正后，零位电压的漂移逐渐逼近真实值；

2).对于陀螺仪传感器零位电压的漂移，利用降维状态观测器对摆杆角度进行观测，再用观测值对陀螺仪传感器零位电压进行实时校正；

d)按如下方法对测量噪声进行滤波处理：

采样模块(P6)在每个周期中对陀螺仪传感器(7)的输出端口进行多次采样，并将采样值转换成数字信号；取平均值模块(P7)先对这些信号进行初步筛选：剔除其中的最大和最小值；剩下的数据取均值；然后，数字滤波程序模块(P12)采用递归型三阶的巴特沃斯数字滤波器对均值信号滤波。

9、如权利要求8所述两轮遥控小车倒立摆的平衡控制方法，特征在于：所述预先给定的控制算法，包括线性状态反馈控制算法、自适应控制、非线性控制和智能控制算法；线性状态反馈控制算法包括闭环极点配置算法、线性二次型最优控制LQR算法和鲁棒方差控制算法；智能控制算法包括模糊控制算法、专家控制算法、模糊神经网路控制算法、拟人控制算法和遗传控制算法。

Images