一种基于位置插值的轴角速度估计方法与流程

本发明涉及一种基于位置插值的轴角速度估计方法，属于光电探测技术领域。

背景技术：

机载光电稳定平台主要用来隔离外界扰动，并跟随指令角度，实现对目标的捕获和精确跟踪。然而在机载环境下，对指令角度的跟随受到载机姿态变化、目标机动、载机振动等因素的影响，视轴不能按照期望的精度跟踪目标运动，因此必须提升速度环的频响和跟随精度，从而提高光电稳定平台的抗扰能力和目标跟踪精度。

机载光电平台实现扰动隔离与指令跟随最主要的条件就是对轴角速度的快速、准确量测。然而，光电稳定平台因为体积限制，很难安装高精度的轴角测量传感器得到角速度。而t法、m法等，利用角位置差分，得到的角速率噪声非常大，且相位滞后严重，很难在较大的调速范围内使用。因此必须改进速度估计方法来消除角速度噪声、相位滞后等因素对机载光电平台扰动隔离与指令跟随性能的影响。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于位置插值的轴角速度估计方法。在测速定时器间隔内进行插值，准确捕获码盘最小分辨位变化的时刻，缩小采样时间与码盘分辨位之间的随机关系，使用增强速度估计的差分公式，得到轴角速度的方法。

技术方案

一种基于位置插值的轴角速度估计方法，其特征在于步骤如下：

步骤1：上电后，初始化速度估计值ω；

步骤2：启动fpga的计数器c和位置触发器tr；

步骤3：码盘位置变化时，位置触发器tr工作，计数器c清零重新计数，当前码盘位置存于寄存器θ(tm)中；

步骤4：重复步骤3直到控制器n-1采样时刻时，将最近一次码盘值变化时的计数值c(tm)和位置值θ(tm)，存储在寄存器c(n-1)和θ(n-1)中；

步骤5：码盘位置变化时，位置触发器tr工作，当前计数器值存于寄存器c(tn)中，码盘位置存于寄存器θ(tn)中；

步骤6：重复步骤5直到控制器n采样时刻时，将最近一次码盘值变化时的计数值c(tn)和位置值θ(tn)，存储在寄存器c(n)和θ(n)中；

步骤7：将寄存器c(n-1)、c(n)、θ(n-1)、θ(n)数据进行差分，ω(n)＝(θ(n)-θ(n-1))/(c(n)-c(n-1))，得到轴角速度估计值；

步骤8：进行赋值c(n-1)＝0，θ(n-1)＝θ(n)后，重复步骤5，开始下一采样周期轴角速度估计。

有益效果

本发明提出的基于位置插值的轴角速度估计方法，将在码盘t法测速的基础上，准确捕获码盘最小分辨位变化的时刻，使用增强速度估计公式得到轴角速度，减小了相位滞后，提高速度环增益，提升稳定平台扰动抑制能力和跟随精度，有非常明显的作战优势，具有广阔的应用前景。

附图说明

图1位置变化时间捕获原理图

图2基于位置插值的速度估计流程图

具体实施方式

基于位置插值的速度估计方法，通过在测速定时器间隔内进行插值，准确捕获码盘最小分辨位变化的时刻，使用增强速度估计的差分公式，得到较高速度分辨率和相位滞后的轴角速度，提升速度环的跟踪性能，从而提高光电稳定平台的视轴跟踪精度。主要涉及到三个方面：

1)位置变化时刻的捕获。采用fpga对位置变化的信号边沿进行触发，将fpga的计数器记为零，并开始计数，计数间隔可以按照待估计速度的范围进行设置，一般定为百ns级到μs级。

2)时间间隔计算。第二次位置变化的信号边沿出现时，将fpga的计数器数据发送至一个寄存器中，该数据即为两次位置变化的时间间隔，同时将计数器清零。

3)速度估计。根据控制器两次采样时码盘位置变化的准确时间和位置，采用fpga对两次存储的位置和位置变化时刻进行差分，并将差分后的速度值存到寄存器中，在控制器采样时由cpu读取。

以下结合附图1和附图2，对本发明进行详细说明。

定义控制器采样时刻为n-1、n，码盘位置变化时刻为tm、tn,码盘角度为θ，速度估计值为ω，fpga的计数器为c，位置触发器为tr，具体实施步骤如下：

第一步，上电后，初始化速度估计值ω；

第二步，启动计数器c和触发器tr；

第三步，码盘位置变化时，触发器tr工作，计数器c清零重新计数，当前码盘位置存于寄存器θ(tm)中；

第四步，重复第三步直到控制器n-1采样时刻时，将最近一次码盘值变化时的计数值c(tm)和位置值θ(tm)，存储在寄存器c(n-1)和θ(n-1)中；

第五步，码盘位置变化时，触发器tr工作，当前计数器值存于寄存器c(tn)中，码盘位置存于寄存器θ(tn)中；

第六步，重复第五步直到控制器n采样时刻时，将最近一次码盘值变化时的计数值c(tn)和位置值θ(tn)，存储在寄存器c(n)和θ(n)中；

第七步，将寄存器c(n-1)、c(n)、θ(n-1)、θ(n)数据进行差分，ω(n)＝(θ(n)-θ(n-1))/(c(n)-c(n-1))，得到轴角速度估计值。

第八步，进行赋值c(n-1)＝0，θ(n-1)＝θ(n)后,重复第五步，开始下一采样周期轴角速度估计。

技术特征：

技术总结
本发明提出的基于位置插值的轴角速度估计方法，将在码盘T法测速的基础上，准确捕获码盘最小分辨位变化的时刻，使用增强速度估计公式得到轴角速度，减小了相位滞后，提高速度环增益，提升稳定平台扰动抑制能力和跟随精度，具有广阔的应用前景。

技术研发人员：高军科;韩文斌;李莉;张红刚
受保护的技术使用者：中国航空工业集团公司洛阳电光设备研究所
技术研发日：2018.10.15
技术公布日：2019.01.25