[发明专利]一种陀螺漂移量的自适应调节方法

权利要求书

1.一种陀螺漂移量的自适应调节方法，其特征在于，该方法在伺服系统的工作过程中进行陀螺漂移量的自适应调节，该方法在CPU中执行，CPU的时钟周期为t；在伺服系统的工作过程中，执行步骤1～4；

步骤1、构建如下变量，包括：

位置传感器反馈值opFeed.trueAng、陀螺采样值opFeed.speed以及陀螺调节值opData.tp；

计时变量i、陀螺采样累加和sum、漂移量PY、陀螺采样个数cnt、以及计时开始时的位置传感器反馈值angle；

步骤2、所述伺服系统中持续获取陀螺采样值opFeed.speed；设定陀螺调节周期为T；在当前陀螺调节周期内，判断伺服系统状态；

若伺服系统状态为搜索主令且速度为0，计时变量i针对CPU时钟周期进行计时，获取计时开始时的位置传感器反馈值angle；

若伺服系统状态为搜索主令且速度不为0或状态为非搜索主令，以下变量清零，包括：i、sum、cnt、angle以及opData.tp；

步骤3、判断i的大小；

当iT/t时，将陀螺采样值opFeed.speed进行累加赋值给sum，且每加一次，陀螺采样个数cnt加1；

当i＝T/t时，计算陀螺均值average、漂移量PY、陀螺调节值opData.tp，如下：

陀螺均值为average＝sum/cnt

漂移量为PY＝average–(opFeed.trueAng-angle)/T；

陀螺调节值opData.tp增加一个PY量并输出；

步骤4、将伺服系统获取的陀螺采样值opFeed.speed减去步骤3输出的陀螺调节值opData.tp并输出；

变量i、sum、cnt和angle清零，进入下陀螺调节周期，返回步骤2。