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

摘要

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

说明书

技术领域：

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

背景技术：

倒立摆是一种复杂的、非线性的、不稳定的系统，它是现代控制理论及应用的典型科研或教学装置。现有最常使用的倒立摆有如《西南工学院学报》(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)

其中