基于姿态传感器的自平衡系统设计

高亮

摘 要：针对现有装置在运动过程由于颠簸、摇晃产生抖动、晃动等问题，设计了自平衡控制系统。该系统利用四元数和欧拉角转换关系，从姿态传感器输出数据中得到横滚角和俯仰角，并在常规PID基础上加入了模糊控制方法控制相应舵机进行补偿运动。实验结果表明，该系统姿态数据误差为0.1度；在阶跃冲击作为输入时，经过0.1秒的调整进入稳态，有较快的响应速度。

关键词：自平衡控制；四元数和欧拉角；模糊PID

0 引言

自平衡控制系统是动力学理论和自动控制理论技术相结合的研究课题，可以认为是计算机控制下，通过对系统状态参数的实时分析，使系统在水平方向或垂直方向的位移和角度的偏移量控制在允许的范围以内，从而使系统保持平衡[1]。自平衡控制系统关键是解决如何计算和补偿系统在运动过程中自平衡装置跟随其载体运动而发生的倾斜与晃动。

针对以上问题，本文提出利用四元数和欧拉位移定理求出横滚角与俯仰角，再通过模糊PID算法输出两路PWM驱动两个方向的相应舵机进行补偿运动达到自平衡效果。

1 自平衡控制平台的动态平衡原理

假设系统的前进方向为X，并以此建立右手坐标系，当自平衡发生倾斜，其倾斜姿态如图1所示，其中绕Y轴运动的角度称之为俯仰角θ，绕前进方向运动X轴的运动角度称为横滚角φ。

当自平衡控制系统载体发生倾斜时，与系统固定在同一支架的姿态传感器会跟随倾斜相同的角度，通过传感数据进行解算可以得到当前系统的俯仰角度和横滚角度，再经过PID等算法输出两路PWM驱动两个方向的相应舵机进行补偿，这就是本自平衡控制系统的基本原理。

2 四元数与欧拉角

通过四元数和欧拉角解算可以快速得到当前自平衡系统倾斜状态下的横滚角φ与俯仰角θ[2] 。

使用自平衡控制系统载体坐标系XYZ与世界坐标系依次按照Z-Y-X的顺序进行旋转得到俯仰角θ和横滚角为φ表达式[3]。

其中A表示姿态矩阵。

在单位时间Δt内，假定刚体的角速度是，根据欧拉位移定理那么该转动轴的转动方向、环绕该轴转动的角度分别是：

四元数表达式为：

并满足约束条件q20+q21+q22+q23=1。

用超复数形式，可以表达为。利用三角公式：ij=-ji=k

可以把四元数转变为姿态矩阵：

将姿态矩阵表达式带入表达式（1）可得：

3 参数自定的模糊PID控制原理

由于本系统结构复杂，很难得到被控对象的精确数学模型，所以采用模糊PID控制控制相应舵机进行补偿运动。

模糊PID控制器主要由两部分组成，分别为常规PID控制器和模糊推理。其原理图如图2[4]所示。

常规PID控制器比例、积分、微分控制的简称，由于其控制算法简单、鲁棒性好和可靠性高，而成为众多自动控制方法中应用最为普遍的控制方法[5]。控制原理图如图3所示。

模糊推理是通过模糊条件语句来实现模糊控制器的控制规则。经过量化后的模糊态变量为{NB，NM，NS，O，PS，PM，PB}，子集中的元素从左到右依次表示当前控制误差值的状态，从负大到正大。精确态到模糊态是通过隶属函数实现的。当前普遍采用的隶属函数有梯形分布、正态分布、三角形分布等。

4 实验测试

4.1 姿态传感器数据修正

在自平衡控制平台完成组装后，由于存在安装误差，不能保证姿态传感器的在安装后全与自平衡系统姿态保持绝对一致，所以需要对传感器进行数据修正。使用角位移测量平台对整个控制平台进行倾斜测试，同时读出传感器的姿态输出数据，通过数据分析可以得到安装误差以及系统误差。把角位移测量平台的数据与传感器输出数据进行对比分析，得出以下修正公式：

x'=0.9*x-0.6

y'=0.9*y+3.0 （2）

其中，x代表姿态解算后的横滚角，x代表修正后的横滚角，y代表姿态解算后的俯仰角，y代表修正后的横滚角。

4.2 模糊PID控制算法MatLab仿真

在MatLab中使用模糊PID算法进行仿真实验。经过调试，设定PID控制参数初值为kp=0.9，ki=0，kd=1时，系统能够迅速进入稳态。阶跃响应的变化如下图4所示：

实验仿真中，模糊PID控制器在经过0.1秒左右的调整时间，系统进入稳态，响应速度快，控制结果稳定，是比较理想的控制器。

5 结语

实验表明安装误差横滚方向为负0.6度，俯仰方向安装误差正3度，修正系数为0.9，经过公式（2）修正后，得到实测值与修正值的对比关系，姿态误差约为0.1度，符合设计要求；在MatLab仿真测试中，在阶跃冲击作为输入，系统经过0.1秒的调整进入稳态，有较快的响应速度。因此，本自平衡控制系统能够有效的解决现有装置在颠簸、摇晃中产生抖动、晃动等问题。

参考文献：

[1]张华宾等.基于DSP的两轮式倒立摆自平衡控制系统[J].测控技术，2007，26（11）：23-26.

[2]张帆.一种新的全角度四元数与欧拉角的转换算法[J].南京理工大学学报（自然科学版），2002，26（04）：376-380.

[3]章仁为.卫星轨道姿态动力学控制[M]. 北京：北京航空航天大学出版社，1998.

[4]黄忠森.控制系统MatLab计算及仿真[M].北京：国防工业出版社，2001.

[5]刘金坤.先进PID控制及其MatLab仿真[M].北京：电子工业出版社，2003.endprint