Research on extraction method of axis angle rate of stable platform based on FPGA

Aiming at the absolute photoelectric encoder used in the stable platform, a FPGA-based scheme which can read encoder data and measure velocity is proposed. The scheme uses the nonlinear tracking differentiator theory to design the speed measurement of the stable platform, which can avoid the error amplification caused by the conventional differential speed measurement. And this solution can implement in engineering easily because it does not require additional hardware circuitry. Numerical simulation analysis and experimental results show that the scheme can accurately read the encoder signal and estimate the encoder’s angular rate based on the position signal. The experiment proves that the method is accurate and effective.


Introduction
The photoelectric encoder is a photoelectric angular displacement sensor integrating light, machine and electricity. It has been widely used in the field of automatic control because of its advantages of high precision, small volume and anti-interference. In servo control systems, it is usually necessary to accurately measure the angular rate of the system. And the servo control systems can form a closedloop control by using the angular rate as a feedback quantity. Therefore, using the position signal of the photoelectric encoder to extract the angular rate plays an important role in practical engineering.
Traditional photoelectric encoder speed measurement methods include M method, T method and M/T method [1]. In addition, the Kalman filter technique [2] and the state observer principle [3] can also be used to estimate the angular rate. However, both methods need to establish an object model to extract state information, but the actual system model parameters cannot be accurately measured. In view of the above problems, the nonlinear tracking differentiator theory is applied to the shaft encoder speed measurement, which can quickly and effectively estimate the input signal and its differential signal from the noise-contaminated signal.
This paper introduces the working principle and SSI interface protocol of absolute photoelectric encoder, and studies the differential speed measurement method based on nonlinear tracking differentiator theory. Finally, the absolute photoelectric encoder data reading and velocity measurement are realized by FPGA, which effectively improve the real-time and accuracy of the measurement.

SSI interface protocol
The object of this paper is the absolute photoelectric encoder, whose position signal transmission follows the SSI interface protocol. The SSI interface absolute photoelectric encoder uses two pairs of differential signal lines to transmit data: one pair is the data line and the other pair is the synchronous clock line. The sampling process is controlled by sending a synchronous clock line Clock through the host computer. The SSI interface data transmission timing diagram is shown in Figure 1. In Figure 1, T is the data read cycle of each bit, 1 is the delay time of the encoder transmit data, 2 is the monostable trigger time, is the number of bits of the transmitted data, MSB is the highest bit of the valid bit, and LSB is the lowest bit of the valid bit. The data transfer start bit is the first falling edge of the clock signal. When the falling edge of the clock is detected, the encoder starts to calculate the position value and saves it in the buffer. When the rising edge of the clock signal, the previously stored data will be sent, until all the stored data has been sent. When the lowest bit LSB of the encoder data is transmitted, the data line will remain low for a period of time, and after the 2 time is extended, the data output is pulled from the low level to the high level. The data output will wait for the next clock signal to appear and continue transmitting data. When the absolute encoder has no data transmission, both the clock line and the data line are high. The number of synchronous clocks sent depends on the number of bits of data that need to be sent.
According to the characteristics of the SSI interface protocol, this paper will write the program through Verilog language and use FPGA to realize the data acquisition function of the absolute photoelectric shaft angle encoder.

Nonlinear tracking differentiator
The nonlinear tracking differentiator was originally proposed by the researcher Han Jing-qing of the Chinese Academy of Sciences [6], and is mainly used to solve the problem of reasonably extracting continuous signals and differential signals in discontinuous or random noise measurement signals. In practical applications, researchers have given a discrete form of nonlinear tracking differentiator through the integrated function of the discrete system's fastest control [7]. The formula is shown below. 1 1 2 (1): h is the integral step size, r is the velocity factor, 0 h is the filtering factor, and fhan is the fastest-speed control synthesis function of the discrete system. fhan 's equation is as follows: (2) As the filter factor is increased to improve the noise suppression capability, it also brings a certain phase delay to the system. The phase delay problem can be compensated by the method of predictive compensation [8]. The specific method is to add the filtered tracking signal to the acquired differential signal and multiply the prediction step size as an approximation signal of the input signal, namely: It can be seen from equation (3) that  nh is the forecast time and the value is slightly larger than the filter factor 0 h .

Designing a Nonlinear Tracking Differentiator in an FPGA
The FPGA uses parallel computing internally and has a wealth of programmable logic resources, which can be programmed and debugged on-site as needed. Therefore, FPGA has been favored by developers in the fields of digital signal processing, communication, and industrial control. This paper uses Xilinx's Artix-7 chip to design a nonlinear tracking differentiator as shown in equation (3). It can be seen that the data involved in the operation of the nonlinear tracking differentiator is in floating point format, and the representation of the floating point number in the FPGA follows the IEEE 754 standard, so the floating point number is adopted by the "floating-point" IP core in the software Vivado2015.4. This IP core can perform a series of floating-point operations on the FPGA and supports single-double floating-point precision operations. This article uses singleprecision floating-point operations.

Nonlinear tracking differentiator speed measurement performance simulation
The frequency characteristics of the nonlinear tracking differentiator are similar to those of the secondorder linear low-pass filter. The performance of the nonlinear tracking differentiator is greatly affected when the external angular disturbance frequency is high. Therefore, this paper focuses on numerical simulation and experimental verification under low frequency angular disturbance.
To verify the performance of the nonlinear tracking differentiator to extract the angular rate of the frame, the simulation uses a sinusoidal signal with white noise added. The equation is  In Figure 2, () rt is the input signal, () dr t is the ideal differential signal, 1 () xt is the tracking signal, 2 () xt is the rate estimation signal, e( ) t is the tracking rate error, and its stable standard deviation is 0.0694. The simulation results show that the nonlinear tracking differentiator can achieve a more accurate rate estimation function for the input signal, but the rate estimation signal 2 () xt of the nonlinear differentiator output is slightly delayed compared with the ideal differential signal () dr t .

Forecast compensation method simulation
The phase lag problem for the nonlinear differentiator rate estimation signal can be solved by using the method of predictive compensation, and the formula is as shown in equation (3) Figure 3 shows the simulation results of the sinusoidal signal of the nonlinear tracking differentiator after the prediction compensation. The simulation results show that the phase delay problem of the rate estimation signal 2 () xt output after the prediction compensation is greatly improved compared with the phase delay problem of the rate estimation signal in Figure 2. In Figure 3, the standard deviation of the rate estimation signal output by the prediction compensation algorithm is 0.0250, and the tracking error is smaller. Therefore, the prediction compensation method can effectively solve the problem of phase delay in the output signal of the nonlinear tracking differentiator.

Experimental verification
Through modeling and simulation analysis, it is known that it is feasible to use the nonlinear tracking differentiator model to extract the rate signal. In order to test the practical application effect of the nonlinear tracking differentiator model speed measurement, the SSI communication protocol and the nonlinear tracking differentiator are designed by FPGA. And the experiment is performed on a rotating mechanism. The position sensor of the rotating mechanism is an absolute photoelectric shaft encoder based on the SSI communication protocol. During the experiment verification, a sinusoidal signal with a frequency of 1HZ and an amplitude of 2 degrees is input to the mechanism to rotate, and the data acquisition of the absolute photoelectric shaft angle encoder and the speed measurement function of the nonlinear tracking differentiator are realized by the FPGA. The rate information is then calculated from the acquired absolute photoelectric axis encoder data by a conventional backward differential method. Finally, the axial angular velocity information estimated by the two methods is compared. The experimental results are shown in Figure 4. In Figure 4, () rt is the input sinusoidal signal, 1( ) mt is the rate signal output by the nonlinear tracking differentiator, and 2( ) mt is the rate signal obtained by the backward differential method. It can be seen from the Figure 4 that when a sinusoidal signal is input to the mechanism for operation, the nonlinear tracking differentiator can estimate the rate signal better, and the noise is small; and the rate estimation value obtained by the backward difference method fluctuates greatly and the noise is large. Therefore, the nonlinear tracking differentiator can obtain more accurate angular rate information than the backward differential method.

Conclusion
In this paper, a method based on FPGA absolute photoelectric encoder shaft angular rate extraction is presented. This method is based on the theory of nonlinear tracking differentiator, and uses Xilinx FPGA platform to design the data reading program of encoder and nonlinear tracking differentiator. The nonlinear tracking differentiator can filter the data of the optical encoder and estimate the axial angular rate of the encoder accurately and quickly. The nonlinear tracking differentiator model does not depend on the object model, and no additional hardware circuit support is required. Since this experiment is designed based on FPGA platform, the adjustable parameters include two parameters: speed factor and filter factor, as well as integral step size. In practical applications, the response time can be effectively adjusted and the noise interference can be effectively suppressed by changing the speed factor, the filtering factor and the integration step. This adjustment method is simple and easy to implement.
Through simulation analysis and experimental verification on the turntable, the nonlinear tracking differentiator can accurately and quickly estimate the angular rate information of the optical encoder through the position information of the photoelectric encoder when the turret is rotated. Therefore, it is feasible to use FPGA to design a nonlinear tracking differentiator to estimate the axial angular rate, and this scheme has good application value.