阅读说明：本技术 一种应用于半捷联稳定平台的轴角速度估计方法 (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. An axial angular velocity estimation method applied to a semi-strapdown stabilized platform is characterized by comprising the following steps:

step 1: determining an execution frequency, a data bit width and an integration step size;

step 2: determining parameters of the tracking differentiator: a speed factor r;

and step 3: the non-linear differential tracking method is executed, which specifically comprises the following steps:

3-1: initializing non-linear tracking differentiator parameters: tracking position x₁(k) Estimate the velocity x₂(k) And intermediate variables e (k), d₀、y、a₀Are all initialized to 0;

3-2: judging whether an execution starting signal is received, if so, sending a synchronous clock signal, reading the position value of the encoder in the register, converting the position value into a single-precision floating point type, and entering the step 3-3, and if not, continuing to execute the step 3-2;

3-3: the variables e (k), d in formula (1) are calculated in sequence₀、y、a₀The value is stored in a register;

wherein h is₀Is a filter factor;

3-4: calculating the value of variable a: determine if y is greater than d₀If yes, executing the formula (2) for calculation, otherwise executing the formula (3) for calculation;

3-5: calculating the value of variable fhan: judging whether | a | is larger than d, if so, executing the calculation of a formula (4), and otherwise, executing the calculation of a formula (5);

fhan＝-r·sign(a) (4)

3-6: calculating tracking position x₁(k +1), estimating the velocity x₂(k +1) value:

x₁(k+1)＝x₁(k)+h·x₂(k)

x₂(k+1)＝x₂(k)+h·fhan(e(k),x₂(k),r,h₀) (6)

3-7: the calculated tracking position signal x₁(k +1) and the estimated velocity signal x₂(k +1) converting the data into integer data and storing the integer data into a corresponding register;

3-8: the cycle is performed 3-2 to 3-7.

2. The method for estimating the axial angular velocity applied to the semi-strapdown stabilized platform according to claim 1, wherein the determination method in the step 1 is as follows: the data reading frequency of the execution frequency is less than or equal to the updating frequency of the encoder; the data bit width is more than or equal to the data bit width of the shaft-position encoder; the integration step h is initially set to the execution period.

3. The method for estimating axial angular velocity applied to a semi-strapdown stabilized platform of claim 1, wherein the initial value of the velocity factor r in step 2 is selected to be any value greater than 0 and less than 1/(2h), where h is an integration step.

4. The method for estimating axial angular velocity of a semi-strapdown stabilized platform according to claim 1, wherein the filtering factor h is selected in step 3-3₀Set to the integration step size value.

5. The method for estimating the shaft angular velocity applied to the semi-strapdown stabilized platform according to claim 1, wherein the method in step 3 is operated in parallel in an FPGA.

6. A method of tuning the performance of the shaft angular velocity obtained in claim 1, characterized by: executing an axial angular velocity estimation method, collecting an estimated velocity signal, and comparing and checking the estimated velocity signal with data collected by a standard velocity estimation device; if the phase lag is found to be too large, increasing the speed factor; if the speed measurement noise is found to be too large, the filtering factor is increased; until the requirements are met.

7. The method of claim 6, wherein the standard speed estimation devices are gyroscopes and velocimeters.

Technical Field

The invention relates to an axial angular velocity estimation method applied to a semi-strapdown stabilized platform, and belongs to the technical field of photoelectric detection.

Background

The main task of the airborne photoelectric stabilized platform is to isolate disturbance of a carrier base and ensure stable pointing of an optical axis in an inertial space. However, in a miniaturized electro-optically stabilized platform, the space is compact, the load on the gimbal is light, and it is difficult to mount a higher precision inertial measurement unit to directly measure the apparent angular velocity. Because of the limitation of load size and weight, it is difficult for a miniaturized stabilized platform to install an inertial measurement element with sufficient accuracy on a frame, and the measurement of the inertial angular velocity of the visual axis cannot be directly realized, so that the inertial measurement element needs to be installed on a base outside a universal joint, the inertial angular velocity of the base and the angular velocity of the frame are synthesized, then coordinate conversion is performed, and then reconstruction is performed to obtain the inertial angular velocity of the visual axis, so that a semi-strapdown stabilized platform is formed. Therefore, due to the limitation of the volume of the universal joint, the shaft angular velocity sensor cannot be mounted, and the angular velocity can be estimated only from the shaft angular position. The shaft angular velocity estimation level directly affects the stability performance of the stable platform visual axis. The key to semi-strapdown stabilization is the frequency response and accuracy of the shaft angular velocity. The photoelectric stabilization platform is wide in speed range to realize indication and tracking of a maneuvering target, and the shaft angular speed obtained through traditional position difference cannot meet the requirements of precision and frequency response at the same time. Therefore, the axial angular velocity must be effectively estimated to meet the requirements of frequency response and precision of velocity information in a larger range, key information is provided for the reconstruction of the visual axial angular velocity of the semi-strapdown stabilized platform, and the control performance of the stabilized platform is improved.

Disclosure of Invention

Technical problem to be solved

In order to overcome the defect that the axial angular velocity obtained by traditional position difference is difficult to meet the requirements of two aspects of precision and frequency response, the invention provides an axial angular velocity estimation method applied to a semi-strapdown stable platform. The axial angular velocity is estimated by tracking differential algorithm on the basis of the existing stable platform axial angle sensor.

Technical scheme

An axial angular velocity estimation method applied to a semi-strapdown stabilized platform is characterized by comprising the following steps:

step 1: determining an execution frequency, a data bit width and an integration step size;

step 2: determining parameters of the tracking differentiator: a speed factor r;

and step 3: the non-linear differential tracking method is executed, which specifically comprises the following steps:

3-1: initializing non-linear tracking differentiator parameters: tracking position x₁(k) Estimate the velocity x₂(k) And intermediate variables e (k), d₀、y、a₀Are all initialized to 0;

3-2: judging whether an execution starting signal is received, if so, sending a synchronous clock signal, reading the position value of the encoder in the register, converting the position value into a single-precision floating point type, and entering the step 3-3, and if not, continuing to execute the step 3-2;

3-3: the variables e (k), d in formula (1) are calculated in sequence₀、y、a₀The value is stored in a register;

wherein h is₀Is a filter factor;

3-4: calculating the value of variable a: determine if y is greater than d₀If yes, executing the formula (2) for calculation, otherwise executing the formula (3) for calculation;

3-5: calculating the value of variable fhan: judging whether | a | is larger than d, if so, executing the calculation of a formula (4), and otherwise, executing the calculation of a formula (5);

fhan＝-r·sign(a) (4)

3-6: calculating tracking position x₁(k +1), estimating the velocity x₂(k +1) value:

3-7: the calculated tracking position signal x₁(k +1) and the estimated velocity signal x₂(k +1) converting the data into integer data and storing the integer data into a corresponding register;

3-8: the cycle is performed 3-2 to 3-7.

The technical scheme of the invention is further that: the determination method of the step 1 is as follows: the data reading frequency of the execution frequency is less than or equal to the updating frequency of the encoder; the data bit width is more than or equal to the data bit width of the shaft-position encoder; the integration step h is initially set to the execution period.

The technical scheme of the invention is further that: the initial value of the speed factor r in step 2 is selected to be any value which is greater than 0 and less than 1/(2h), wherein the value is an integration step.

The technical scheme of the invention is further that: filter factor h in step 3-3₀Set to the integration step size value.

The technical scheme of the invention is further that: and 3, performing parallel operation in the FPGA by using the method.

A method for performing performance debugging on an angular velocity, characterized by: executing an axial angular velocity estimation method, collecting an estimated velocity signal, and comparing and checking the estimated velocity signal with data collected by a standard velocity estimation device; if the phase lag is found to be too large, increasing the speed factor; if the speed measurement noise is found to be too large, the filtering factor is increased; until the requirements are met.

The technical scheme of the invention is further that: the standard speed estimation device is a gyroscope and a velocimeter.

Advantageous effects

The invention provides an axial angular velocity estimation method applied to a semi-strapdown stabilized platform. The method introduces the tracking differentiator technology into the shaft angular velocity estimation, replaces differentiation with an integral method, overcomes the defect of noise amplification caused by the traditional angular position difference, and can reasonably extract a differentiated signal from a discrete signal with random noise. An angular position signal is input to the tracking differentiator, an angular position tracking signal and an angular velocity estimation signal are output, and the algorithm implementation mode is the key for judging whether the angular velocity estimation effect is excellent or not. By means of flexible interfaces of the FPGA, powerful parallel operation and high-precision counting capacity, the method for estimating the shaft angle speed has the advantages that the shaft angle speed estimation method is high in data updating rate, independent in channel and high in algorithm convergence speed, high-quality feedback information is provided for the semi-strapdown stable platform, accuracy of optical axis pointing is improved, and the method has a wide application prospect.

Drawings

FIG. 1 flow chart of the present invention

Detailed Description

An axial angle speed estimation method applied to a semi-strapdown stable platform is characterized in that by means of a flexible interface of an FPGA and powerful parallel operation and timing functions, high-speed parallel acquisition of axial angle positions is achieved, a tracking differential algorithm is achieved through a hardware description language, axial angle estimation speed is synchronously output, and therefore axial angle speed with high data updating rate, independent channels and high algorithm convergence speed is obtained. The axial angular velocity obtained by the method has higher precision in a wider range, provides high-quality feedback information for the semi-strapdown stable platform, enhances the anti-interference capability of the stable platform, and improves the pointing accuracy of the optical axis. Two main aspects are involved:

firstly, an axis angular velocity estimation algorithm. The method mainly comprises the following steps:

the first step, determining the execution frequency, data width and integration step length of the shaft angle estimation algorithm, the determination method is as follows: the execution frequency of the shaft angle estimation algorithm is less than or equal to the data reading frequency of the encoder and less than or equal to the updating frequency of the encoder; the data bit width of the algorithm is more than or equal to the data bit width of the shaft-position encoder; the integration step h is initially set to the algorithm execution period (in units of s).

Secondly, parameters of a tracking differentiator are preliminarily determined: velocity factor r, velocity factorThe sub-initial value is selected to be any value larger than 0 and smaller than 1/(2h), wherein the integration step is the filtering factor h₀Set to the integration step size value.

Thirdly, executing a nonlinear differential tracking algorithm, which is shown in the attached figure 1 in detail as follows:

1) the nonlinear tracking differentiator parameter is initialized. Tracking position x₁(k) Estimate the velocity x₂(k) And intermediate variables e (k), d₀、y、a₀Are all initialized to 0;

2) judging whether an algorithm starting execution signal is received, if so, sending a synchronous clock signal, reading a position value of an encoder in a register, converting the position value into a single-precision floating point type, and entering the step 3), and if not, continuing to execute the step 2);

3) sequentially calculating the calculation in formula (1), and storing the variables e (k), d in registers₀、y、a₀A value;

4) the value of the variable a is calculated. Determine if y is greater than d₀If yes, executing the formula (2) for calculation, otherwise executing the formula (3) for calculation;

5) the value of the variable fhan is calculated. Judging whether | a | is larger than d, if so, executing the calculation of the formula (4), otherwise, executing the calculation of the formula (4)

Performing the calculation of formula (5);

fhan＝-r·sign(a) (4)

6) calculating tracking position x₁(k +1), estimating the velocity x₂(k +1) value.

7) The calculated tracking position signal x₁(k +1) and the estimated velocity signal x₂(k +1) converting the data into integer data and storing the integer data into a corresponding register;

8) and periodically executing the steps 2) to 7).

And secondly, a performance debugging method of the shaft angular velocity estimation method. The invention also discloses a performance debugging method of the shaft angular velocity estimation method. The shaft angular velocity estimation algorithm is executed periodically and circularly, and the estimated angular velocity is obtained. Changing the speed factor r and the filtering factor h in the second step₀The initial value can change the estimation effect of the shaft angular velocity estimation method, gradually adjust the velocity factor and the filter factor, and realize the velocity estimation performance with different effects. The speed factor and filtering factor parameter debugging method comprises the following steps: increasing the speed factor r can increase the speed estimation bandwidth, increase the estimation noise, and decreasing the speed factor r can decrease the estimation noise, but decrease the speed estimation bandwidth. Increasing the filter factor h₀The speed estimation noise can be reduced but the estimation bandwidth can also be reduced.

In order to make the technical scheme and the advantages more clear, the shaft angular velocity estimation method applied to the semi-strapdown stable platform is instantiated and explained aiming at the encoder with the maximum refreshing frequency of 2-path data of 5k Hz and the data bit width of 16 bits. The detailed steps are as follows:

firstly, an axis angular velocity estimation algorithm. The method mainly comprises the following steps:

the method comprises the steps that firstly, the maximum data refresh rate of an encoder is 5k Hz, the data bit width is 16 bits, according to a method that the execution frequency of a shaft angle estimation algorithm is less than or equal to the data reading frequency of the encoder and less than or equal to the updating frequency of the encoder, the data reading frequency of the encoder is selected to be 4k Hz, and the execution frequency of the shaft angle estimation algorithm is 2k Hz; according to the method that the bit width of the algorithm data is larger than or equal to the data bit width of the shaft-angle encoder, the data bit width in the FPGA estimation algorithm is selected to be 32 bits, and the integration step length h is initialized and set to be 0.0005s of the algorithm execution period.

Secondly, the initial value of the speed factor is selected to be 500, and the filtering factor h is selected₀Set to the integration step size value 0.0005.

Thirdly, executing a nonlinear differential tracking algorithm, specifically as follows:

1) the nonlinear tracking differentiator parameter is initialized. Tracking position x₁(k) Estimate the velocity x₂(k) Intermediate variables e (k), d₀、y、a₀Are all initialized to 0;

2) judging whether an algorithm starting execution signal is received, if so, sending a synchronous clock signal, reading a position value of an encoder in a register, converting the position value into a single-precision floating point type, and entering the step 3), and if not, continuing to execute the step 2);

3) sequentially calculating the calculation in formula (1), and storing the variables e (k), d in registers₀、y、a₀A value;

4) calculating the value of variable a, and judging whether | y | is greater than d₀If yes, executing a formula (2), otherwise, executing a formula (3);

5) judging whether | a | is larger than d, if so, executing a formula (4), and otherwise, executing a formula (5);

6) calculating the tracking position x according to equation (6)₁(k +1), estimating the velocity x₂(k +1) value.

7) The calculated position signal x₁(k +1) and the estimated velocity signal x₂(k +1) converting the data into integer data and storing the integer data into a corresponding register;

8) the steps 2) to 7) are executed periodically and circularly.

And secondly, debugging the performance of the shaft angular speed estimation method. The specific debugging steps are as follows:

1) executing an axis angular velocity estimation algorithm, collecting an estimated velocity signal, and comparing and checking the estimated velocity signal with data collected by a standard velocity estimation device (such as a gyroscope, a velocimeter and the like);

2) if the phase lag is found to be too large, increasing the speed factor r in the step two to 1000 according to the requirement;

3) re-executing the step 1), acquiring speed data, comparing with a standard speed estimation device to find that the speed measurement noise is too large, and increasing the filter factor to be 0.0008;

4) re-executing the step 1), collecting speed data, comparing with a standard speed estimation device, and finding that the requirements are met;

5) and completing debugging of the speed estimation algorithm.

The tracking differential algorithm is realized through FPGA parallel operation, the speed is high, the updating frequency is high, and the convergence speed and the noise of the tracking differential algorithm can be flexibly adjusted. The axial angle estimation speed realized by the FPGA is equivalent to the bandwidth and the time delay of an inertia measurement element, effective information fusion can be carried out, the visual axial angular speed of the stabilized platform is obtained through reconstruction, the disturbance suppression capability of the photoelectric stabilized platform is enhanced, the visual axial pointing accuracy is improved, and the method has wide application prospect.