基于PID控制器的两轮自平衡小车设计.doc

本科毕业设计基于PID控制器的两轮自平衡小车设计摘 要两轮自平衡小车具有体积小、结构简单、运动灵活的特点，适用于狭小和危险的工作空间，在安防和军事上有广泛的应用前景。两轮自平衡小车是一种两轮左右平衡布置的，像传统倒立摆一样，本身是一种自然不稳定体，其动力学方程具有多变量、非线性、强耦合、时变、参数不确定性等特性，需要施加强有力的控制手段才能使其保持平衡。本文在总结和归纳国内外对两轮自平衡小车的研究现状，提出了自己的两轮自平衡小车软硬件设计方案，小车硬件采用陀螺仪和加速度传感器检测车身的重力方向的倾斜角度和车身轮轴方向上的旋转加速度，数据通过控制器处理后，控制电机调整小车状态，使小车保持平衡。由于陀螺仪存在温漂和积分误差，加速度传感器动态响应较慢，不能有效可靠的反应车身的状态，所以软件使用互补滤波算法将陀螺仪和加速度传感器数据融合，结合陀螺仪的快速的动态响应特性和加速度传感器的长时间稳定特性，得到一个优化的角度近似值。文中最后通过实验验证了自平衡小车软硬件控制方案的可行性。关键词：自平衡 互补滤波 数据融合 倒立摆Two-wheeled Self-balancing RobotMa Xuedong(College of Engineering, South China Agricultural University, Guangzhou 510642, China)Abstract:The two-wheeled self-balancing robot is small in mechanism, with simple structure and can make flexible motion, suitable for narrow and dangerous work space. So it has wide range of applications in security and military. The two-wheeled self-balancing robot is a natural unstable system. The device of this system is a parallel arrangement of two single wheels, like a traditional inverted pendulum. Its dynamics are multi-variable, non-linear, serious coupling and uncertain parameters etc. It must be exerted strong control to make it stable.In this paper, studies on two-wheel self-balancing vehicle at home and abroad are summarized. We designed the hardware and software of our two-wheel self-balancing vehicle. The car using rotational accelerometers, gyroscopes and acceleration sensors to detect body condition and the state in which the pitch change rate. The central processing unit calculate the appropriate data and instructions, and control the motor to achieve the body balancing. Because of gyro drift problems and Integral error with accelerometers and slow dynamic response of acceleration sensors. It cant provide effective or reliable information to reflect the real state of its body. So we using complementary filter to fuse the data of two sensors, so that the inclination of its body can be approximated better.Finally, we verified the feasibility of the systems hardware and software through experiment.Key Words: Self-Balancing complementary filter Data Fusion Inverted Pendui目 录1 前言11.1 研究意义11.2 国内外研究现状11.2.1 国外研究成果11.2.2 国内研究成果11.3 本文的研究内容22 两轮平衡车的平衡原理22.1 平衡车的机械结构22.2 两轮车倾倒原因的受力分析32.3 平衡的方法33 系统方案分析与选择论证43.1 系统方案设计43.1.1 主控芯片方案43.1.2 姿态检测传感器方案43.1.3 电机选择方案53.2 系统最终方案54 主要芯片介绍和系统模块硬件设计64.1 加速度传感器ADXL34564.2 陀螺仪传感器L3G4200D84.3 主控电路104.4 电机驱动电路114.5 供电电路115 系统软件设计125.1 系统初始化135.2 滤波器145.2.1 低通滤波器155.2.2 互补滤波器155.3 PID控制器175.3.1 PID概述175.3.2 数字PID算法175.3.3 PID控制器设计186 硬件电路196.1 硬件制作与调试196.2 硬件调试结果196.2.1 姿态感知系统测试结果196.2.2 PID控制器测试结果207 结论21参考文献23附录24致谢26华南农业大学本科生毕业设计成绩评定表I1 前言1.1 研究意义应用意义。自平衡车巧妙地利用地心引力使其自身保持平衡，并使得重力本身成为运动动能的提供者，载重越大，行驶动能也就越大，具有环保的特点(胡春亮等，2007)。驾驶者不必担心掌握平衡，车体自身的平衡稳定性，使得原本由于平衡能力障碍而无法骑自行车的人群也同样可以驾驭。车身小巧，转弯灵活，可以在狭窄、大转角的工作场合作业。自平衡车的种种优点使其可以作为一种快速、环保、安全、舒适、小巧灵活的绿色交通工具，是未来汽车和自行车的替代品，其市场的广阔性与经济效益不言而喻。理论研究意义。自平衡车，在重力作用下车体姿态本征不稳定，需要电机的控制来维持姿态的平衡，通过电机驱动转动车轮，传感器、软件、微处理器及车体机械装置整体协调控制电动车平衡，是集环境感知、动态决策与规划、行为控制与执行等多种功能于一体的综合复杂非线性系统，其控制难度大，控制算法复杂，给控制理论提出了很大的挑战，具有较强的理论研究价值。1.2 国内外研究现状美国、日本、瑞士等国家在研究自平衡车领域起步较早，目前已经达到了先进的水平。国内的一些高校以及科研机构也对其有所研究，并取得了一定的成绩。1.2.1 国外研究成果美国Lego公司Steve Hassenplug设计了两轮自平衡传感式机器人Legway。实现了电机差动驱动方式，遥控操作，可以向前，向后和转弯时保持平衡，可以实现U型回转和零半径转弯。Legway是第一个自平衡机器人。采用了模块化的结构设计，安装和拆卸都很方便。日本村田制作所的科学家研发了骑独轮车的机器人“村田顽童”和“村田婉童”。保持左右平衡通过转动机器人体内配备的惯性轮来实现。瑞士联邦工学院的工业电子实验室为模拟人类行走设计并制造了一个基于倒立摆理论的两轮小车，该小车使用DSP控制，车架上方附有重物模拟实际车中的驾驶员，该小车使用陀螺仪和电机编码器得到的信息来稳定系统。1.2.2 国内研究成果哈尔滨工业大学设计的HITBot两轮自平衡小车，采用Accodometry方法，通过融合码盘和加速度级数据对位置进行估计，有效解决了两轮自平衡小车在运行过程中遇到打滑、越障、碰撞等异常事件而导致的位置估计失败的问题，解决了非系统测程法误差对机器人位置估计的影响，降低了加速度级固有漂移的不利影响，提高了两轮自平衡车的定位精度。深圳职业技术学院等设计的两轮自平衡小车Opyanbot，应用最优控制与两轮差动等控制方法设计了控制器,提出了针对两轮自平衡机器人平衡和行进的新策略。为了提高两轮自平衡机器人的控制效果,利用基于DSP数字电路的全数字智能伺服驱动单元IPM100分别精确控制左右轮电机,并利用上位机实时控制机器人的运动状态,提高了控制精度、可靠度和集成度,得到了很好的控制效果。1.3 本文的研究内容本文研究内容有两轮自平衡小车的姿态检测算法，PID控制算法两方面。姿态检测算法通过互补滤波器融合姿态传感器(加速度传感器和陀螺仪传感器)数据，得到小车准确稳定的姿态信息，PID调节器则利用这些姿态信息，输出电机控制信号，控制电机的转动，使小车得以平衡。2 两轮平衡车的平衡原理2.1 平衡车的机械结构图1 平衡车机械结构现有的自平衡车结构种类繁多，但都归根于图1的基本结构，因此，本设计将使用图1的结构，车体由三层组成，从上到下依次是电池层，主控层，电机驱动层，电池层用于放置给整个系统供电的6V锂电池，主控层由主控芯片最小系统和传感器模块组成，电机驱动层接受单片机信号，并控制电机。每个层都是功能模块的电路路板之间用铜柱固定，电机外壳与电机驱动电路板固定，电机转轴与两只轮胎相连。2.2 两轮车倾倒原因的受力分析两轮车是一个高度不稳定系统，在重力作用下车体姿态本征不稳定，致使在没有外加调控下必然倾倒的现象(张三川，2011)。其受力如图2所示。 图2 平衡车受力分析图理想状态下，当M(车体重力)的方向与H(车轮支持力)的方向相差180时，系统此时受力平衡，可以达到稳定不倒的状态，角度为0。但自然界存在各式各样的干扰，角度总不为0，只要产生角，即使角度很小，M的方向与H的方向亦产生了角度，合力不为0，根据牛顿运动定律可知，角度将越来越大，直至车体倾倒在地上。2.3 平衡的方法从以上分析可得，导致车体倾倒的最大因素是角度的产生，因此，欲使小车平衡，需要消除或者将角度控制在一个足够小的范围内。其整体控制环路图3所示。 图3 小车平衡原理流程图消除角度的有效方法，是通过电机的转动，带动车体下部的移动，以保持与车体上部在一水平垂直线上。 3 系统方案分析与选择论证3.1 系统方案设计3.1.1 主控芯片方案方案一：采用意法半导体(ST)公司的STM32单片机作为主控芯片。此芯片是以ARM的Cortex-M系列为内核的单片机，相对其他单片机，外设丰富，主频高，价格便宜，有专门的软件库，操作简单，调试方便，低功耗。强型系列时钟频率达到72MHz，是同类产品中性能最高的产品；基本型时钟频率为36MHz，以16位产品的价格得到比16位产品大幅提升的性能，是16位产品用户的最佳选择。方案二：采用ATMEL公司的AVR单片机AVR单片机硬件结构采取8位机与16位机的折中策略，即采用局部寄存器存堆(32个寄存器文件)和单体高速输入/输出的方案(即输入捕获寄存器、输出比 较匹配寄存器及相应控制逻辑)。提高了指令执行速度(1Mips/MHz)，克服了瓶颈现象，增强了功能。其中的一款单片机型号为Atmega128，有64个引脚，最高可达到16M主频，IIC，UART，SPI接口都比较丰富，但价格高。方案三：采用宏晶科技有限公司的STC12C5A60S2增强型51单片机作为主控芯片。此芯片内置ADC(模数转换)和IIC总线接口，且内部时钟不分频，可达到1MPS。性价比低。考虑到此系统的复杂度，需要与传感器进行IIC通讯，输出灵活可控制的PWM信号，以及进行大量的数学运算。从性能和价格上综合考虑选择方案一，即用STM32作为本系统的主控芯片，由于外设比较简单，只需要IIC和PWM通道，因此具体型号定位为STM32RBT6。3.1.2 姿态检测传感器方案方案一：使用加速度传感器进行倾角。重力加速度传感器(g-sensor)能过输出以其芯片为中心的三轴加速度，通过这三个轴的重力加速度便可以计算出芯片的倾角，即车体的倾角。该方案的优点是重力加速度的静态性能很好，在车体静态下能测出准确稳定的倾角，而在动态下，三轴加速度各轴会受到其它加速度的影响，导致其数据并不稳定可靠。方案二：使用陀螺仪传感器进行测量。陀螺仪传感器能输出围绕以芯片为中心的三个轴的角速度，通过读角速度的积分，即可得出倾角。该方案的优点是陀螺仪的动态性能很好，在动态下测出的角速度没有太多的混杂成分，缺点是陀螺仪具有静态漂移，即静态下，陀螺仪仍然会输出数值，而积分却一直在进行，因此静态时，测出来的角度并不是0。方案三：加速度传感器与陀螺仪传感器结合，通过融合算法，提取出加速度传感器的静态效果和陀螺仪的动态效果。优点是能测出准确稳定的倾角，但融合算法比较复杂。综上考虑，由于准确稳定的倾角正是本文要讨论的话题，因此最终选择方案三，即加速度传感器与陀螺仪传感器数据融合测量倾角。并为了简化电路，最终选择了均为IIC接口的陀螺仪传感器L3G4200和加速度传感器ADXL345。3.1.3 电机选择方案方案一：步进电机。步进电机的选择角度正比于脉冲数，有较宽的调速范围，可以采用开环方式控制；步进电机有较大的输出转矩；有优秀的起制动性能；控制精度较高，误差不会累积。但是步进电机步距角固定，分辨率缺乏灵活性，而且步进驱动时容易造成车体震荡，不利于小车的稳定。步进电机虽然可以使用细分驱动方式克服上述缺点，但是细分驱动电路结构复杂，而且功耗增大不适合用于电池供电的应用上。方案二：直流无刷电机。直流无刷电机具有直流有刷电机机械特性好、调速范围宽等优点，而且无刷电机没有换向器和电刷，可靠性高，寿命长。但是无刷电机的驱动电路复杂，而且在本设计中小车为实验性质，车身较小，市面上很难找到大小合适的直流无刷电机。方案三：直流有刷电机。直流有刷电机具有机械特性硬，响应速度快，调速范围宽的特点，满足两轮自平衡小车对灵敏性、快速性等要求，虽然电机的电刷会是电机的寿命缩短，还会引发电磁干扰。但是由于本设计负载较轻，换向器和电刷的损耗较低。小车采用多层机械结构，电机驱动电路与其他电路分离，有效降低电磁干扰。综上所述，本设计使用两个6V带有减速齿轮的直流有刷电机驱动两轮自平衡小车。3.2 系统最终方案使用STM32RBT6为主控芯片，通过IIC接口读取陀螺仪传感器L3G4200和加速度传感器ADXL345的数据，再将两者数据融合测出小车的姿态，最终通过PID输出PWM电机控制信号，由电机驱动完成对电机的控制。此外，为了调试方便，除了设计了上述给模块外，还扩展了JLINK接口，使用的是SWD模式，用于仿真调试，同时扩展了串口电路，在系统运行时将需要观察的数据通过串口传输到电脑上，以记录数据和绘出数据波形，查看滤波和PID效果。系统方框图如图5所示。图5 系统方框图4 主要芯片介绍和系统模块硬件设计4.1 加速度传感器ADXL345MEMS(Micro-Electro-Mechanical Systems)微机电系统有一类传感器是加速度传感器。ADXL345是一款由ANALOG DEVICES 公司生产3轴加速度计，功耗低，包分辨率高(13位)，测量范围达 16g。数字输出数据为16位二进制补码格式，可通过SPI(3线或4线)或I2C数字接口访问。该加速度传感器电路的应用电路如图6所示。图6 加速度传感器电路ADXL345非常适合移动设备应用。它可以在倾斜检测应用中测量静态重力加速度，还可以测量运动或冲击导致的动态加速度(张志强，2011)。其高分辨率(3.9mg/LSB)，能够测量不到1.0的倾斜角度变化。其集成式存储器管理系统采用一个32级先进先出(FIFO)缓冲器，可用于存储数据，从而将主机处理器负荷降至最低，并降低整体系统功耗。低功耗模式支持基于运动的智能电源管理，从而以极低的功耗进行阈值感测和运动加速度测量。数字接口让加速度传感器的外围电路变得很简单，主控芯片通过IIC即可初始化该芯片并读取三轴的加速度值。表1 ADXL345 引脚功能管脚功能VDD电源1.9-3.6V输入GND地RESERVED厂家保留功能VS电源1.9-3.6V输入CS片选信号INT1中断引脚1INT2中断引脚2NC悬空SDAIIC数据线SCLIIC时钟线ADXL345的各引脚功能如表1所示。图6所示为ADXL345的应用电路，图中INT1(Interrupt1)1号中断线与INT2(Interrupt)2号中断线与STM32的外部中断引脚连接，SDA(synchronous Data Adapter)同步数据衔接器与SCL(Serial Communication Loop)串行通信环路引脚通过上拉电阻与主控芯片的硬件IIC接口连接，进行IIC通讯。当INT1或者INT2发生低电平时，表明传感器内部数据已经准备好，此时可通过IIC接口读出加速度计三个轴的数据。在读取三轴加速度数据之前，应对ADXL345进行必要的初始化。ADL345具有可设置的多个量程，即 2g， 4g， 8g， 16g这三个量程，在此应该设置为 16g满量程。可采用固定的4 mg/LSB 分辨率模式，该分辨率可测得0.25的倾角变化。将转换速率设置为最小的12.5微秒。将电源模式设置为低功耗模式，以减小对锂电池的消耗。使能引脚中断，单片机可以通过此外部中断，触发对三轴加速度数据的读取，用中断触发的方式可以较少系统计算周期，提高电机有效控制频率。 图7 加速度传感器测量角度原理图7是以传感器为基准简建立的三维坐标系，从传感器读出的数据通过一定的运行可以算出传感器的各类倾斜角，其中，正是平衡车要消除的角度，车身倾斜角度可以通过公式(1)计算出。式(1)中，A是各轴输出的加速度分量。 (1)4.2 陀螺仪传感器L3G4200D加速度计主要是用来测量静态加速度，而动态的加速度值对系统是一种干扰，用陀螺仪测量出车体的角速度信号能够有效的去除动态的干扰，两种装置并用可以在动态条件下实时的计算出自平衡车的倾角信息(冯智勇等，2011)。本方案采用的陀螺仪传感器型号是L3G4200。L3G4200D 是意法半导体(ST)近日推出一款业界独创、采用一个感应结构检测3 条正交轴向运动的3 轴数字陀螺仪。该3轴数字陀螺仪让用户可以设定全部量程，量程范围从 250 dps2000 dps，低量程数值用于高精度慢速运动测量，而高量程则用于测量超快速的手势和运动。这款器件提供一个16位数据输出，以及可配置的低通和高通滤波器等嵌入式数字功能。其应用引脚功能如表2所示。表2 L3G4200 引脚功能管脚功能VDD电源1.9-3.6V输入GND地RESERVED厂家保留功能VS电源1.9-3.6V输入CS片选信号INT中断引脚NC悬空SDAIIC数据线SCLIIC时钟线与加速度传感器的数字接口一致，也是通过用SDA和SCL与主控芯片的硬件IIC接口进行通讯，采用3.3V供电，其应用电路如图8所示。图8 陀螺仪传感器电路从图8可看出L3G4200的应用电路比较简单，不需外接模数转换器等负责的电路，使用极其方便。通过IIC接口即可读出三轴的角速度，再将读出的数据乘以所设置的量程对应的敏感度，即可得出每个周的角速度，单位为dps(degree per second)。4.3 主控电路本设计的两轮自平衡小车采用STM32F103R8单片机为主控芯片。STM32系列基于专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex-M3内核。STM32F103R8具有以下特点：采用ARM 32位Cortex-M3内核，最高时钟频率72MHz，1.25DMIPS/MHz,快速的指令执行速度使主控芯片能够运行复杂的滤波和控制算法。提高控制器的实时控制能力。片内高达64kB Flash和20kB SRAM，为复杂的算法程序提供足够的存储和运行空间。两个12位的16通道模拟/数字转换器(ADC),转换速度高达1Msample/s，ADC支持规则转换序列和注入转换序列两种转换模式，支持DMA模式，转换结果的搬运不需要CPU干预，提高程序运行效率。主控及其外围电路如图9所示图9 主控芯片及其外围电路图9中控制电路包括主控芯片、时钟电路、复位电路、模拟电路供电电路。时钟采用8MHz外部晶振作为时钟源，通过主控芯片内部PLL倍频后使主控芯片运行在72MHz。主控芯片为低电平复位，复位电路通过阻容电路构成上电复位电路。芯片采用3.3V供电。最小系统还引出了不需要使用的SPI，IIC，UART等接口，方便后续功能的扩张和调试。4.4 电机驱动电路本设计中使用减速直流有刷电机作为两轮自平衡车的驱动电机，电机采用H桥驱动方式，使用脉宽调制方式调节电机两端电压有效值，达到调速的目的7。电机驱动电路如图10所示。图10所示电路只给出了半桥驱动电路，H桥左右两个桥臂电路完全相同。图10 电机驱动电路本设计采用MOSFET作为驱动电路的开关器件，MOSFET型号为IRFR1205，开启电压最大值为VGS=4.0V，适合电池供电的应用；在VGS=10V时，RDS=0.027，低导通电阻一方面提高开关效率，另一方面降低开关器件的发热量，提高系统稳定性(康少华等，2011)。由于主控芯片输出信号高电平电压值VOH=3.3V，不能直接驱动MOSFET导通，所以电路设计了半桥电路，高边驱动电路由U1、D1和C1组成。U1使用International Rectifier公司的IR2302芯片，该芯片为专用的半桥驱动电路，IR2302支持输出520V的开启电压，支持3.3V、5V和15V逻辑输入。硬件集成100ns死区控制电路。4.5 供电电路本设计使用锂电池供电，供电电压6.4V，供电电路将电源电压转换成3.3V供主控芯片和传感器电路使用。由于电机的功耗较大，起制动时会引起电源电压的波动，所以电路设计滤波电路稳定电压输出。供电电路如图11所示。图11 供电电路C1、L1、C2构成型滤波器，对输入电压进行滤波。滤波器截止频率为，可以有效滤除来自电源的噪声。U1为低压差线性稳压源，将电源转化成3.3V后向传感器和主控制器供电。5 系统软件设计系统软件总体流程如图12所示。图12 系统软件总流程图系统上电复位后便开始初始化各个功能模块，并启动了1ms定时，每1ms进行一次姿态估算和PID控制，即1s内系统进行了1000次姿态调整。同时为了前期调试已经查看数据，使用了主控的串口将程序中产生的数据如估算出的最终角度等，上传到电脑，以观察数据的特性，上传周期为16ms。5.1 系统初始化系统初始化完成STM32单片机的初始化设置，初始化程序流程图如图13所示。图13 系统初始化流程图STM32单片机支持在RAM和Flash中运行，所以程序启动时必须配置中断向量表到对应的地址，RAM运行时中断向量表地址为0 x20000000；Flash运行时中断向量表地址为0 x08000000。STM32单片机的中断优先级有抢断优先级和从优先级两种。两种优先级的级数可以通过软件设置。本系统设置16级抢断优先级，0级从优先级。系统使用外部8MHz晶振作为外部时钟源，通过STM32内部的锁相环(PLL)倍频至72MHz。STM32有两条外设时钟总线，APB1总线时钟频率最高为36MHz，APB2总线时频率最高为72MHz。软件设置两个外设时钟至其最高频率，开启ADC、定时器、DMA、串口和GPIO的时钟源。本设计为了减少调试信号线对车体的影响，采用SWD调试模式，此模式下只需要3条线即可进行调试。I/O配置程序禁用JTAG调试模式，使能SWD调试模式；另外本设计中配置了4个LED，用于显示小车当前运行状态，引脚配置程序配置LED引脚为推挽输出模式。本设计中串口用于输出加速度传感器、陀螺仪采集到的倾角信号和Kalman滤波器的输出信号到上位机。串口配置波特率1152000、8位数据位、1位停止位、无奇偶检验位。定时器用于产生控制电机的PWM信号。STM32的1个定时器可以同时产生4路PWM信号。程序设定PWM信号的频率为20kHz，3600级占空比输出。间隔定时器(Systick)产生8kHz信号用于产生ADC和Kalman采样频率。间隔定时器为一个24位递减定时器，仅有3个寄存器，使用固件库仅需一个语句即可完成配置。5.2 滤波器单独使用加速度计或者陀螺仪，都无法提供一个有效而可靠的信息来保证车体的平衡(谢芳等，2010)。陀螺仪动态性能良好，不受加速度变化的影响，但是存在累积漂移误差，不适合长时间单独工作；加速度计静态效应好，但受到动态加速度影响较大，不适合跟踪动态角度运动。采用一种简易互补滤波方法来融合陀螺仪和加速度计的输出信号，补偿陀螺仪的漂移误差和加速度计的动态误差，便可以得到一个最优的倾角近似值。该滤波器的实现算法如图14所示。 图14 互补滤波器算法流程图5.2.1 低通滤波器 在使用互补滤波器前，先对加速度的值进行低通滤波。加速度的值里包含了一些短时性快速变化的信号，这些信号对计算角度有较大的干扰，因此应使用低通滤波器将其滤除掉，滤除后的加速度值具有长时性缓慢变化的平滑效果。5.2.2 互补滤波器由图可看出该滤波器的输入有两个数据，一是加速度计测得的角度，二是陀螺仪测得的角速度，两者经过一定的融合运算后得出一个稳定可靠的倾角。以下将介绍数据的融合过程。首先建立一个以主控板为基准的空间坐标系，如图15所示。 图15 传感器直角坐标系R代表电路板的加速度矢量，Rn(n可以是x，y，z)代表R在各个轴的分量，Ann(A代表角度angle，nn可以是xz，yz，如Axz)代表R与各个平面的夹角。因此，从加速度计测出来的RxAcc，RyAcc，RzAcc(acc代表accelerometer)便可合成矢量Racc，即Racc=Raccx，Raccy，Raccz。由Racc的分量即可计算出Axz，Ayz，但如前文讨论的，此时的Axz，Ayz并不稳定可靠。假设最后计算出的稳定可靠的矢量(实际上是单纯的重力加速度矢量)为Rest = RxEst,RyEst,RzEst(Est代表：estimate)。程序中，第一次计算Rest直接取值于加速度计的Racc，即： (2)此后的Rest(如Rest(1),Rest(2),Rest(3)Rest(n)则由陀螺仪参与调整。在计算Rest(n)时，可以通过RxEst(n-1) 和RzEst(n-1)计算出Axz(n-1),即： (3) 接着由Axz(n-1)和RateAxz(n)可计算出Axz(n)，即： (4)其中RateAxz(n)为绕y轴的陀螺仪角速度，T为计算Axz(n-1)到计算Axz(n)的时间差值。同样道理可得： (5)再由以下公式可得： (6)同理得： (7) (8)到此，计算出了两个单位为1的向量：Racc(n)，Rgyro(n)，通过一个加权平均公式便可得到Rest(n)，即： (9)令w2/w1 = wGyro，可得 (10)即： (11) (12) (13)而小车的倾角为： (14)5.3 PID控制器5.3.1 PID概述PID调节器是指按偏差的比例、积分和微分进行控制的调节器，其调节实质是根据输入的偏差值，按比例、积分、微分的函数关系进行运算，其运算结果用于输出控制。在实际应用中，在多数情况下，根据具体情况，可以灵活地改变PID的结构，取其一部分进行控制(杨德刚等，2010)。PID控制器的输入输出关系为： (15)PID控制有模拟和数字控制方式：模拟方式采用电子电路调节器，在调节器中，将被测信号与给定值比较，然后把比较出的差值经PID电路运算后送到执行 机构，改变给进量，达到调节之目的。数字方式用计算机进行PID运算，将计算结果转换成模拟量，输出去控制执行机构。比例控制对系统动态性能的影响：增大时，将使系统响应速度加快，偏大时，系统振荡次数增多，调节时间加长；偏小时，系统响应速度缓慢。选择以输出响应产生4:1衰减过程为宜。在系统稳定的前提下，加大Kp可以减少稳态误差，但不能消除稳态误差。积分时间对系统性能的影响：积分控制通常影响系统的稳定性。太小使，系统不稳定，且震荡次数较多；太大时，对系统的影响将削弱；合适时，系统的过度过程特性比较理性。积分控制有助于消除系统稳态误差。微分时间对系统性能的影响：微分作用的增强可以改善系统动态特性，如减少超调量，缩短调节时间等，适当加大比例控制，可以减少稳态误差，提高控制精度；另一方面微分作用会放大系统噪声，降低系统抗干扰能力。微分环节的加入可以在误差出现或变化瞬间，按偏差的变化的趋势进行控制。引进一个早期的修正作用，有助于提高系统稳定性。5.3.2 数字PID算法数字PID算法为用计算机实现，用数值逼近和连续信号离散化实现的PID控制规律。有两种实现方式：位置性数字PID、增量型数字PID。对于位置式PID算法，由于，位置性数字PID控制算法如公式(16)所示： (16)对于增量式PID算法，由公式()可知： (17)结合公式(3-12)和(3-13)，可以得到增量式数字PID控制算法如公式(3-14)所示： (18)增量式数字PID算法不需要做累加运算，计算误差和计算精度问题对控制量的计算影响较小；位置性数字PID算法要用到过去偏差的累加值，容易产生较大的累积误差(王效杰，2006)。5.3.3 PID控制器设计本设计中使用双环PID控制实现小车的平衡控制。信号流图如图16所示。角度环PID速度环PID电机小车转速测量测量倾角测量测量图16 PID控制流图本设计采用双环PID控制，内环为速度环，用于控制电机输出，使车轮转速与角度环输出值保持一致；外环为角度环，用于保持小车倾斜角度为0。图中为系统输入，即小车倾斜角度的目标值，该值一直为0。为系统角度环输出值，即小车电机转速目标值。为轮子转速，为小车实际的倾斜角度。为系统扰动输入由于本设计采用锂电池供电，电源的输出功率有限，而且调整小车倾角时电机需要频繁起制动，对电源功率要求较高，会引起电源较大幅度的扰动，所以在本系统中主要的扰动为电源电压的扰动。内环PID控制器控制流图如图17所示。微分系数比例系数积分系数图17 速度环PID控制器控制流图6 硬件电路6.1 硬件制作与调试 首先通过Altium Designer画图软件设计出电路PCB,然后发去PCB制造产打样，得到PCB空板。从元器件市场购买所需零器件，将其焊接于打样所得的PCB空板上。接着分别测试电源电路，主控芯片最系统，传感器模块，电机驱动模块是否能正常工作。至此，硬件调试完成。6.2 硬件调试结果6.2.1 姿态感知系统测试结果为了方便测试，将传感器数据通过串口输出到电脑，用串口波形软件将数据以波形的形式在电脑显示器上显示。如图18是由加速度计测出来的未经过滤波处理的角度波形图。图18 未经过滤波的倾角值波形从图18中看出，在第7秒之前静放系统，此时测出来的倾角为0，波形基本稳定不变，但在7秒后人为摇动电路板，其波形开始发生剧烈的变化。此波形说明了仅由加速度计计算出来的倾角存在太多干扰，没法使用。因此，需要一种有效的滤波器接入，滤除非重力加速度成分。而如图19所示，在加入了低通滤波器和互补滤波器后得到的波形与未经过滤波的波形进行比较，很明显地看出了互补滤波器估计出了一个低