一种应用于半捷联稳定平台的轴角速度估计方法
阅读说明：本技术 一种应用于半捷联稳定平台的轴角速度估计方法 (Axial angular velocity estimation method applied to semi-strapdown stabilized platform ) 是由 高军科 王建刚 潘旭辉 史鹏杰 于 2020-10-11 设计创作，主要内容包括：本发明涉及一种应用于半捷联稳定平台的轴角速度估计方法,对难以在万向节载荷上安装惯性测量传感器的小型稳定平台,通过对稳定平台各框架角在FPGA上并行高速采样、采用硬件语言实现跟踪微分算法,轴角估计速率同步输出,以此得到具备数据更新率高、通道独立、算法收敛速度快的轴角速度。通过将轴角估计速度与基座惯性角速度融合、重构得到视轴惯性角速度,并以此作为反馈信息实现视轴的惯性稳定。通过优化轴角估计算法的采样间隔、收敛因子和输出噪声,得到与惯性测量元件输出信号的频率、时延相匹配的轴角估计速度,增强稳定平台对基座扰动的抑制能力,提高光轴的指向准确性。(The invention relates to an axial angular velocity estimation method applied to a semi-strapdown stabilized platform, which is used for obtaining the axial angular velocity with high data updating rate, independent channel and high algorithm convergence rate by parallelly sampling all frame angles of the stabilized platform on an FPGA at a high speed and adopting a hardware language to realize a tracking differential algorithm on a small stabilized platform which is difficult to install an inertia measurement sensor on a universal joint load and synchronously outputting the axial angular velocity estimation rate. The inertial angular velocity of the visual axis is obtained by fusing and reconstructing the estimated velocity of the axis angle and the inertial angular velocity of the base, and the inertial angular velocity of the visual axis is used as feedback information to realize the inertial stability of the visual axis. By optimizing the sampling interval, the convergence factor and the output noise of the axial angle estimation algorithm, the axial angle estimation speed matched with the frequency and the time delay of the output signal of the inertia measurement element is obtained, the inhibition capability of the stable platform on the base disturbance is enhanced, and the pointing accuracy of the optical axis is improved.)
技术领域
本发明涉及一种应用于半捷联稳定平台的轴角速度估计方法，属于光电探测技术领域。
背景技术
机载光电稳定平台主要任务是隔离载体基座扰动，保证光轴在惯性空间中的稳定指向。然而，在小型化的光电稳定平台中，空间紧凑，万向节上的载荷轻巧，难以安装较高精度的惯性测量元件以直接测得视轴角速度。因为载荷尺寸和重量的限制，小型化的稳定平台难以在框架上安装精度足够的惯性测量元件，不能直接实现视轴惯性角速度的测量，因此需要将惯性测量元件安装在万向节以外的基座上，通过将基座惯性角速度与框架角速度信息合成，再进行坐标转换，然后重构得到视轴惯性角速度，形成半捷联稳定平台。因此，因万向节体积限制，无法安装轴角速度传感器，只能由轴角位置对角速度进行估计。轴角速度估计水平直接影响到稳定平台视轴的稳定性能。半捷联稳定的关键在于轴角速度的频率响应和精度。因光电稳定平台要实现对机动目标的指示、跟踪，速度范围很宽，通过传统位置差分得到的轴角速度难以同时满足精度和频率响应两方面要求。因此，必须有效地估计轴角速度，以满足较大范围内对速率信息频率响应和精度的需要，为半捷联稳定平台的视轴角速度重构提供关键信息，提高稳定平台控制性能。
发明内容
要解决的技术问题
为了解决传统位置差分得到的轴角速度难以同时满足精度和频率响应两方面要求的缺陷，本发明提出一种应用于半捷联稳定平台的轴角速度估计方法。将在现有稳定平台轴角传感器的基础上，通过跟踪微分算法估计得到轴角速度。
技术方案
一种应用于半捷联稳定平台的轴角速度估计方法，其特征在于步骤如下：
步骤1：确定执行频率、数据位宽度及积分步长；
步骤2：确定跟踪微分器的参数：速度因子r；
步骤3：执行非线性微分跟踪方法，具体如下：
3-1：初始化非线性跟踪微分器参数：跟踪位置x1(k)，估计速度x2(k)及中间变量e(k)、d、d0、y、a0均初始化为0；
3-2：判断是否接收到开始执行信号，若收到则发送同步时钟信号，读取寄存器中的编码器位置值，并将其转换成单精度浮点型，并进入步骤3-3，若未收到继续执行步骤3-2；
3-3：依次计算公式(1)中的变量e(k)、d、d0、y、a0值，并在寄存器中存储；
其中h0为滤波因子；
3-4：计算变量a的值：判断|y|是否大于d0，若是，则执行公式(2)计算，否则执行公式(3)计算；
3-5：计算变量fhan的值：判断|a|是否大于d，若是则执行公式(4)计算，否则执行公式(5)计算；
fhan＝-r·sign(a) (4)
3-6：计算跟踪位置x1(k+1)，估计速度x2(k+1)值：
3-7：将计算得到的跟踪位置信号x1(k+1)和估计速度信号x2(k+1)转换成整型数据，存入相应的寄存器；
3-8：周期执行3-2至3-7。
本发明技术方案更进一步的说：步骤1的确定方法如下：执行频率≤编码器的数据读取频率≤编码器更新频率；数据位宽度≥轴角编码器的数据位宽；积分步长h初始化设置为执行周期。
本发明技术方案更进一步的说：步骤2的速度因子r初始值选择为大于0，小于1/(2h)的任意值，其中为积分步长。
本发明技术方案更进一步的说：步骤3-3中滤波因子h0设置为积分步长值。
本发明技术方案更进一步的说：步骤3中所述方法在FPGA中并行运算。
一种对轴角速度进行性能调试的方法，其特征在于：执行轴角速度估计方法，采集估计的速度信号，并与标准速度估计器件采集的数据进行对比校核；如果发现相位滞后过大，则增大速度因子；如果发现测速噪声过大，则增大滤波因子；直至满足要求。
本发明技术方案更进一步的说：所述的标准速度估计器件为陀螺仪和测速仪。
有益效果
本发明提出的一种应用于半捷联稳定平台的轴角速度估计方法。该方法将跟踪微分器技术引入轴角速度估计中，以积分方法替代微分，克服传统角位置差分带来噪声放大的不足，可从带随机噪声离散信号中合理提取微分信号。对跟踪微分器输入一个角位置信号，将输出角位置跟踪信号和角速度估计信号，算法实现方式是角速度估计效果是否优异的关键。借助FPGA灵活的接口、强大的并行运算和高精度计数能力，通过对稳定平台各框架角在FPGA上并行高速采样、用硬件描述语言实现跟踪微分算法，轴角估计速率同步输出，以此得到具备数据更新率高、通道独立、算法收敛速度快的轴角速度估计方法，为半捷联稳定平台提供高质量的反馈信息，提高光轴指向的准确性，具有广阔的应用前景。
附图说明
图1本发明流程图
具体实施方式
一种应用于半捷联稳定平台的轴角速度估计方法，凭借FPGA灵活的接口、强大的并行运算和定时功能，设计实现了轴角位置的高速并行采集、用硬件描述语言实现跟踪微分算法，轴角估计速率同步输出，以此得到具备数据更新率高、通道独立、算法收敛速度快的轴角速度。此法得到的轴角速度在较宽范围内有比较高的精度，为半捷联稳定平台提供高质量的反馈信息，增强稳定平台的抗扰能力，提高光轴指向的准确性。主要涉及到两个方面：
一、轴角速度估计算法。主要步骤如下：
第一步，确定轴角估计算法的执行频率、数据宽度及积分步长，确定方法如下：轴角估计算法执行频率≤编码器的数据读取频率≤编码器更新频率；算法数据位宽度≥轴角编码器的数据位宽；积分步长h初始化设置为算法执行周期(单位为s)。
第二步，初步确定跟踪微分器的参数：速度因子r，速度因子初始值选择为大于0，小于1/(2h)的任意值，其中为积分步长，滤波因子h0设置为积分步长值。
第三步，执行非线性微分跟踪算法，详见附图1，具体如下：
1)初始化非线性跟踪微分器参数。跟踪位置x1(k)，估计速度x2(k)及中间变量e(k)、d、d0、y、a0均初始化为0；
2)判断是否接收到算法开始执行信号，若收到则发送同步时钟信号，读取寄存器中的编码器位置值，并将其转换成单精度浮点型，并进入步骤3)，若未收到继续执行步骤2)；
3)依次计算公式(1)中计算，并在寄存器中存储变量e(k)、d、d0、y、a0值；
4)计算变量a的值。判断|y|是否大于d0，若是，则执行公式(2)计算，否则执行公式(3)计算；
5)计算变量fhan的值。判断|a|是否大于d，若是则执行公式(4)计算，否则
执行公式(5)计算；
fhan＝-r·sign(a) (4)
6)计算跟踪位置x1(k+1)，估计速度x2(k+1)值。
7)将计算得到的跟踪位置信号x1(k+1)和估计速度信号x2(k+1)转换成整型数据，存入相应的寄存器；
8)周期执行第2)步至第7)步。
二、轴角速度估计方法性能调试方法。轴角速度估计方法性能调试方法，也是本发明的内容之一。周期循环执行轴角速度估计算法，将会得到估计角速度。改变步骤二中的速度因子r及滤波因子h0初值，可改变轴角速度估计方法的估计效果，逐步调整速度因子及滤波因子，可实现不同效果的速度估计性能。速度因子与滤波因子参数调试方法如下：增大速度因子r，可提升速度估计带宽，增大估计噪声，减小速度因子r，可减小估计噪声，但降低速度估计带宽。增大滤波因子h0可降低速度估计噪声，但也会降低估计带宽。
为了使本发明的技术方案及优点更加明晰，针对2路数据最大刷新频率为5k Hz、数据位宽为16位的编码器，实例化说明了一种应用于半捷联稳定平台的轴角速度估计方法。详细步骤如下：
一、轴角速度估计算法。主要步骤如下：
第一步，编码器最大数据刷新率为5k Hz，数据位宽为16位，依据轴角估计算法执行频率≤编码器的数据读取频率≤编码器更新频率方法，选择编码器的数据读取频率为4kHz，轴角估计算法执行频率为2k Hz；依据算法数据位宽度≥轴角编码器的数据位宽方法，选择FPGA估计算法中数据位宽为32位，积分步长h初始化设置为算法执行周期0.0005s。
第二步，速度因子初始值选择为500，滤波因子h0设置为积分步长值0.0005。
第三步，执行非线性微分跟踪算法，具体如下：
1)初始化非线性跟踪微分器参数。跟踪位置x1(k)，估计速度x2(k)，中间变量e(k)、d、d0、y、a0均初始化为0；
2)判断是否接收到算法开始执行信号，若收到则发送同步时钟信号，读取寄存器中的编码器位置值，并将其转换成单精度浮点型，并进入步骤3)，若未收到继续执行步骤2)；
3)依次计算公式(1)中计算，并在寄存器中存储变量e(k)、d、d0、y、a0值；
4)计算变量a的值，判断|y|是否大于d0，若是，则执行公式(2)，否则执行公式(3)；
5)判断|a|是否大于d，若是则执行公式(4)，否则执行公式(5)；
6)按照公式(6)计算跟踪位置x1(k+1)，估计速度x2(k+1)值。
7)将计算得到的位置信号x1(k+1)和估计速度信号x2(k+1)转换成整型数据，存入对应的寄存器；
8)周期循环执行第2)步至第7)步。
二、轴角速度估计方法性能调试。具体调试步骤如下：
1)执行轴角速度估计算法，采集估计的速度信号，并与标准速度估计器件(例如陀螺仪、测速仪等)采集的数据进行对比校核；
2)发现相位滞后过大，则根据需求增大步骤二中的速度因子r为1000；
3)重新执行上述1)中步骤，采集速度数据，对比标准速度估计器件发现测速噪声过大，增大滤波因子为0.0008；
4)重新执行1)中步骤，采集速度数据，对比标准速度估计器件，发现满足要求；
5)速度估计算法调试完成。
跟踪微分算法通过FPGA并行运算得以实现，速度快、更新频率高，且可以灵活调整跟踪微分算法的收敛速度和噪声。通过FPGA实现的轴角估计速度与惯性测量元件的带宽和延时相当，可以进行有效的信息融合，进而重构得到稳定平台的视轴角速度，增强光电稳定平台的扰动抑制能力，提高视轴指向准确性，具有广泛的应用前景。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种隧道内辅助车辆定位设施及方法