基于陀螺积分补偿脱靶量估计与跟踪控制方法研究.pdfVIP

思路：标定脱靶量延迟dt，并将dt时间内的陀螺积分值补偿到脱靶量；将脱靶

量与前dt时刻的陀螺积分值做差，作为目标位置估计量，并将该估计量取差分，

低通滤波后外推dt时刻，也补偿到脱靶量；将上述两份补偿过的脱靶量称为当

前脱靶量估计值，用于内环反馈，将未补偿的脱靶量积分值作为外环反馈，内外

环算法暂用PI。

隐藏坑：理论上，陀螺积分会无限发散，对于超长时间工作的设备或零漂较大的

情况，可能需要单圈以避免float型数据攒的太大了出现数值误差。本例中

暂未遇到该种情况。后续使用时要留心此坑。

问题：该种方法可提高裕度，以至于可用的增益很大，在大阶跃输入的情况下，

会产生很大的角速度，角速度过大时，会产生模糊进而导致丢失目标。故

需要对大阶跃区限速。

代码流程：

1、脱靶量更新

//更新脱靶量等相关变量

voidRefreshTBL(void)

{

floattrk_evl_old,f_tbl_old;

gyro_yaw_pos+=0.001*ServoRegs.ADDATA_FW*0.00305f/57.3f;

gyro_pitch_pos+=0.001*ServoRegs.ADDATA_GD*0.00305f/57.3f;

if(tbl_refresh_flag==1)

{

tbl_refresh_flag=0;//这个flag用来标记脱靶量更新情况，每次脱靶量更新时挂

这个flag，并在这里处理flag事物，并消flag

//对脱靶量进行单位转换并差分

f_tbl_old=fyaw_tbl;

思路：标定脱机目标量延迟dt，将dt时刻的陀螺积分值补偿到脱机目标量；将

脱机目标量与前dt时刻的陀螺积分值做差，作为目标位置估计量，得出该估计

量取分数，低通模拟后外推dt时刻，也补偿到脱机目标量；将上述两份补偿过

的脱机目标量称为当前脱靶量估计值，对于内反馈环，将未补偿的脱靶量积分

值作为外环反馈，外环算法暂用PI。

隐藏坑：理论上，陀螺积分会无限发散，对于超长时间工作的设备或零漂增量

的情况，可能需要单圈一个浮动型数据积累的轮廓了出现分数托盘。本例

中暂未遇到该种情况。后续使用时要留心此坑。

问题：该种方法可提高裕度，达到可用的增益增益，在大阶跃输入的情况下，会

产生很大的角速度，角速度过大时，会产生模糊进而导致丢失目标。

故需要对大阶跃区限速。

代码流程：

1、脱靶量更新//更新脱

靶量等相关变量void

RefreshTBL(void)

_______

{浮动trkevlold,ftblold;陀螺仪偏航位置+=0.001*ServoRegs.ADDATA

___

FW*0.00305f/57.3f;陀螺仪俯仰位置+=0.001*ServoRegs.ADDATAGD*0.00305f/57.3f;

____

if(tblrefreshflag==1){tblrefreshflag=0;//该flag用于标记脱靶量更新情况，每次脱靶量更

新时挂

标志这个，并在这里处理标志事物，并消掉标志

//对脱靶量进行单位转换并分数

___

ftblold=fyawtbl;

fyaw_tbl=(float)ServoRegs