1、2016 年第 1 期仪表技术与传感器Instrument Technique and Sensor2016No. 1基金项目 : 国家自然科学基金项目 ( 61170277) 。收稿日期 : 2015 04 12 收修改稿日期 : 2015 10 20基于三轴陀螺仪传感器的无线位置伺服算法应用巫华芳( 赣南师范学院数学与计算机科学学院 ， 江西赣州 341000)摘要 : 针对目前位置伺服控制系统存在一定程度跟随误差 ， 且在特殊作业环境下难以布线等特点 ， 文中提出一种基于三轴陀螺仪和加速度传感器无线位置伺服控制算法 。采用意法半导体微控制器 ( STM32F103BT6) 滤波处理三轴陀
2、螺仪( L3G4200D) 和加速度计 ( ADXL) 获取的位置信号 ， 并通过 24 G 无线射频技术进行上 、下位机数据传输 ， 利用 Fuzzy PID 超前校正复合算法对伺服系统进行全闭环控制 ， 在 Matlab/Simulation 中建立控制模型并做详细仿真分析 。实验仿真结果表明 ，该系统动态位置跟随误差低至 103， 动态响应调节时间小于 005 s， 控制精度和数据实时同步传输得到了有效改善 。关键词 : 陀螺仪 ; 位置伺服 ; 无线传输 ; 跟随误差中图分类号 : TP275 文献标识码 : A 文章编号 : 1002 1841( 2016) 01 0013 03Ap
3、plication of Algorithm for Wireless Position Servo Based on Triaxial Gyro SensorWU Hua-fang( College of mathematics and computer science， Gannan normal university， Ganzhou 341000， China)Abstract: In allusion to the following error of servo control system is higher， and its very difficult to cable in
4、 the special work-ing environment， algorithm for servo control system based on the triaxial gyro and accelerometer sensor was proposed in this paperMicro controller ( STM32F103BT6) was used to process the position signal from gyro ( L3G4200D) and ADXL， and data wastransmitted using 2 4G radio freque
5、ncy technology between upper and lower control system by radio frequency technology AndFuzzy-PID lead compensation composite algorithm was applied to system by full closed-loop， build model and the main error sourcein the Matlab/Simulation was analyzed Simulated results of experiment indicate that t
6、he dynamics following error can be low to103， the regulation time can be less than 005 s， the precision and real-time are improved greatlyKey words: gyro sensor; servo control; wireless transmission; following error0 引言位置伺服控制系统 ， 是指输出量以一定精度跟随输入量变化的自动控制系统 1。作为闭环伺服控制系统 ， 它在生产过程和运动对象的控制 、定位 、瞄准 、跟踪 、信号
7、传递和接收等装置中都有广泛的应用 2 3。国内外关于伺服控制的发展已经历交磁电机和扩大机控制 、磁放大器控制 、晶闸管控制 、集成电路控制 、计算机控制等历程 4 5。但就目前自动控制领域 ， 位置伺服控制系统尚存在一些进步的余地 ， 具体分析为 : ( 1) 当前大部分采用自整角机有线控制方法 ， 即被跟随物体数据输出端与跟随物体之间采取有线连接方式 ， 但有线数据传输限制了应用场合 ， 如远程控制和特种作业环境难以采用有线连接 ， 并且不易实现一对多控制 ;( 2) 多数伺服控制系统的执行机构使用的是直流电机或者步进电机 ， 其高速性 、稳定性和精度都不能够满足当下高端自动化 、集成化和智
8、能化产品需求 ; ( 3) 无线位置伺服控制大多数以指令方式传输为主 ， 虽然输出端能够按照给定的位置指令进行跟踪定位 ， 但其实时性和同步性存在欠缺 ; ( 4) 在传统控制算法的基础上进行优化 ， 如模糊控制 、神经网络 、自适应 、模糊与神经网络融合等 ， 但诸如神经网络等复杂控制不易在工程领域准确实现 6 7。文中为了弥补伺服控制精度不高和有线数据传输不便的问题 ， 提出了一种基于 AM 的无线高精度位置伺服控制系统设计及其算法的研究 。采用陀螺仪和加速度计传感器 ， 无线射频收发芯片实现数据传输 ， 使用模糊 PID 超前校正复合算法对伺服电机进行控制 ， 从而改善动态响应滞后 ，
9、提高位置伺服控制精度 。同时附加无线视频监控电机功能 ， 以便更好的进行人机交互 8。1 系统架构体系如图 1 所示 ， 上 、下位机无线伺服位置控制系统框图 ， 该系统主要由检测位置传感器 ( 三轴陀螺仪和加速度计 ) 、AM 控制器 ( 搭载 STM32F103BT6) 、无线数据收发和伺服电机组成 。上位机为发送数据端 ， 下位机为接收数据端 。同时搭建无线视频收发模块 ， 并通过串口连接至工控机 ， 实现人机智能界面 ， 实时监控下位机伺服电机工作状态 。首先通过三轴陀螺仪和加速度计分别获取当前位置速度值和加速度值 ， AM 控制器对采集到的位置信号经过复合控制 、滤波算法等处理后 ，
10、 由无线数据发送端传递给下位机控制器 ， 从而驱动伺服电机根据上位机位置信号跟踪运动 ， 以实现无线远程操控伺服电机 。同时无线视频模块实时监控电机运行状态 。2 复合控制和滤波算法如图 2 所示 ， 建立无线位置伺服控制算法模型 。为了最大14 Instrument Technique and Sensor Jan. 2016图 1 上 、下位机控制系统结构限度提高系统动态跟随特性 ， 减小输入输出响应滞后产生的误差 ， 在传统控制方法的基础上 ， 采用 Fuzzy-PID 复合控制 ， 即增加超前校正和前馈控制环节 ， 以提高位置跟随精度和系统稳定实时性 。具体控制算法分析如下 :( 1)
11、 获取陀螺仪和加速度计传感器位置信号 。为减小传感器输出信号噪声误差 ， 分别对输出角速度 和角加速度 进行滤波修正处理 。其离散状态和观测方程如下 :( t) =t0( t) dt， ( t) =t0( t) dt ( 1)( t) =( t 1) +( t 1) t ( 2)( t) =( t 1) +( t 1) t ( 3)取 x( T) =( t)( t )， 经处理后状态和观测方程如下 :X( t) =X( t 1) +W( t) ( 4)Y( t) = HX( t) ( 5)式 ( 1) 式 ( 3) ， 在 t 时刻 ， 陀螺仪采样角度值为 ( t) ， 加速度计采样加速度值
12、( t) ， ( t) 为 t 时刻角速度 。式 ( 4) 、式 ( 5)中 ， =1 t 0 1， =0 t， H = 1 0， W( t) =( t) 0， 将式 ( 4)和式 ( 5) 带入 Kalman 滤波方程对观测值进行滤波修正 ， 去除噪声干扰 。( 2) 超前校正和前馈控制 。传递函数和离散化方程如下 :G( s) =1 +sTs=1 +T1 11 +( )Ts( 6)( t) = ( 1 t/T) ( t 1) +t0( t) /T ( 7)( t) =( t) + Kp( t) ( t) ( 8)前馈和控制偏差为 : 其中 spd为角速度反馈值 ， fbk为位置反馈值 (
13、a， b 为常数 ) 。DAC=fbk+PIDspd( 9)err= fbk( 10)fbk= a + b ( 11)( 3) 确定模糊控制算法 。输入量为 err、cerr， 输出量为 KP、KI、KD， 利用二维模糊控制器对 err、cerr进行模糊化 ， 得到输入输出之间变化关系 ， 不断实验测试得到控制规律 。其对应的模糊关系为 = NBe NBec PBkp， 其中 NBe NBec= E1EC1， 根据最大隶属度法公式进行计算和去模糊化 。 = NBe NBec PBkp( 12) =0 0 0 0 0 0 0 0 0 0 0. 3 0. 8 1. 00 0 0 0 0 0 0 0
14、 0 0 0. 3 0. 7 0. 70 0 0 0 0 0 0 0 0 0 0. 3 0. 3 0. 3 0 0 0 0 0 0 0 0 0 0 0. 0 0. 0 0. 00 0 0 0 0 0 0 0 0 0 0. 0 0. 0 0. 00 0 0 0 0 0 0 0 0 0 0. 0 0. 0 0. 0( 13)又因 AM 是通过软件方式执行 Fuzzy-PID 算法 ， 故必须对其进行离散化处理 ， Fuzzy-PID 算法与调整公式如下 :u( k) = Kperr( k) + Ki i =0err( i) + Kd err( k) err( k 1) ( 14)KP= KP0+
15、k1 err， cerrPKI= KI0+ k2 err， cerrIKD= KD0+ k3 err， cerrD( 15)式中 : Kp0、KI0、KD0为预整定值 ; k1 err， cerrSP， k2 err，cerr5I， k3 err， cerrD为模糊控制器的输出 ， 即 PID 参数的校正量 。伺服系统控制调节 : 其中 T 为计算周期 ， Derr为微分角度偏差值 ， err为上一次偏差值 。I= KIerrTP= KPerrDerr= ( errerr) /T， D= KDDerr( 16)离散采样后 ， 需要转换成控制对象的基本论域 ， 再确定输出量比例因子 ， 即 :K
16、u=yul( 17)经过多次实验得到 ， 模糊 PID 输出的最大值 :Fuzzy( KP)max= KP010% ( 18)Fuzzy( KI)max= KI010% ( 19)Fuzzy( KD)max= KD010% ( 20)利用模糊 PID 控制算法对输出量进行实时在线调整 ， 将调整后的输出量参数传递给 AM 控制器 ， 改变伺服电机的输入第 1 期 巫华芳 : 基于三轴陀螺仪传感器的无线位置伺服算法应用 15图 2 复合控制算法模型电压值 ， 从而实现对伺服电机位置的实时精确控制 。3 仿真分析与验证系统选用 Faulhaber 伺服电机的主要参数为 : 额定电压 /电流 24
17、V/0 6 A， 转矩 21 mNm， 相间电阻 /电感 14 46 /1 337H， 转子惯量 192 gcm2。运用 Matlab/Simulink 对上述控制模型进行仿真分析 。如图 3， 输入阶跃信号时 Fuzzy-PID 与 PID 调节时间比较 ， 前者比后者动态响应调节时间快 0 03 s。如图 4为正弦输入输入输出跟踪曲线 ， 如图 5 为其输入跟随误差 trace曲线 ， 可以看出动态位置跟随误差低至 103。图 3 Fuzzy-PID 与 PID 算法比较图 4 正弦输入输出跟踪曲线图 5 正弦函数输入跟随误差 trace 曲线图 6 为视频监控界面 ， 可以实时在线监测伺
18、服电机工作状态 ， 可通过软件界面对伺服电机进行界面控制 。通过以上仿真研究表明 ， 无线位置伺服 Fuzzy-PID 复合控制系统具有较好的静态和动态特性 。图 6 下位机视频监控4 结束语文中设计的基于三轴陀螺仪无线位置伺服控制系统 ， 其动态位置跟随误差低至 103， 动态响应调节时间小于 0 05 s， 具有较好的动态跟随特性 ， 经过仿真和实验表明可以精确的控制伺服电机运动 ， 同时通过无线视频监控下位机实时工作状态 ，以便更好的进行人机交互 ， 弥补了传统伺服控制系统在跟随精度和数据传输上的缺陷 。参考文献 : 1 肖阳 基于 DSP 的伺服电机调速系统的控制设计 D 武汉 : 武
19、汉理工大学 ， 2009 2 ZHANG L， CAI K Y， CHEN G An improved robust fuzzy -PID con-troller with optimal fuzzy reasoning J IEEE Systems， Man， and Cy-bernetics Society， 2005， 35( 6) : 1283 1294 3 杨川 ， 赵强 ， 张志 智能控制在超精密定位中的应用研究 J 仪器仪表学报 ， 2009， 30( 6) : 1218 1223 4 PAKIN M， CZANECKI C A， SAFAIC ， et al A PID ser
20、vocontrol system experiment conducted remotely via internet J Mecha-tronics， 2002， 12( 6) : 833 843 5 何献忠 位置随动系统软件控制的研究 J 仪表技术与传感器 ，2012( 4) : 45 47 6 周婷 ， 王艳 ， 兰必丰 ， 等 圆感应同步器在高精度位置控制系统中的应用 J 传感器与微系统 ， 2005， 24( 3) : 73 75 7 KUANG C Y， YING Y T Fuzzy optimization techniques applied to thedesign of a digital PMSM servo drive J IEEE Transactions on PowerElectronics， 2004， 19( 4) : 1085 1099 8 钟斌 随动系统的模糊自适应整定 PID 变阻尼控制 J 系统仿真学报 ， 2006， 18( 6) : 3200 3233作者简介 : 巫华芳 ( 1968) ， 副教授 ， 硕士 ， 主要研究方向为计算机控制 、无线传感控制等 。E-mail: gnsywhf126 com
