FPGA Design of Motion Control System for Medium-Sized Group Robot

0 Introduction
At present, omnidirectional mobile robots have become the most ideal choice in the RoboCup medium-sized group soccer robot competition due to their excellent flexibility. The motion control of robots has always been the main factor that directly affects the performance of robots, and it is also one of the hot spots in the research of mobile robots. This paper studies a method of using FPGA technology to realize the motion control system of a three-wheeled omnidirectional mobile robot. Compared with the dual DSP structure, DSP+CPLD structure, and DSP+ASIC structure, this method has the characteristics of simplicity, reliability, and strong scalability. In addition, FPGA design is simple, easy to use, and has a short development cycle, and can realize a true SOPC system.

1 Motion model of omnidirectional mobile robot
Assume that the speed of the robot in the world coordinate system is ε=[vx, vy, φ], then when vx=O, vy≠0, φ=O, the robot moves in a straight line in the front and back directions, when vx≠0, vy=0, φ=0, the robot moves in a straight line in the left and right directions, and when vx=0, vy=0, φ≠0, the robot rotates. In Figure 1, ω1, ω2, ω3 are the rotational angular velocities of the three active wheels, R is the radius of the omnidirectional wheel; L1, L2, L3 are the horizontal distances from the center of the robot body to the center of the three sets of omnidirectional wheels, and L1=L2=L3=L is assumed. α is the angle between the first two wheels, and the other two angles are both 180°-α/2. The relationship between the speed in the robot coordinate system and the speed of the three wheels is as follows:

From formula (1), we can see that after knowing the speed requirement of the robot in the plane world coordinate system, we can get the speed requirement of the active wheel, and then send the corresponding control signal to the motor.
2 Motion control scheme The overall design idea of ​​this system is shown in Figure 2. First, the RS 232 interface is used to realize the communication between the PC and the underlying control chip FPGA. After receiving the speed in the relevant robot coordinate system, the FPGA converts the speed value in the robot coordinate system into the angular velocity of the robot's three omnidirectional wheels, calculates the corresponding duty cycle with the obtained angular velocity value, generates a PWM waveform with the corresponding duty cycle, and connects the output signal to the DC servo motor driver. Then, the orthogonal encoder disk signal is collected through the FPGA to calculate the actual angular velocity value of the wheel and perform PID speed closed-loop control. In view of the advantage of FPGA module replication, PID closed-loop control is performed for each omnidirectional wheel here.

3 System hardware design
The block diagram of the three-wheeled omnidirectional mobile robot system is shown in Figure 3. The host computer mainly completes the acquisition, processing, and path planning of image information, and realizes communication with the referee box outside the field. The lower computer is mainly FPGA, which mainly realizes the acquisition of three-wheel encoding signals, PID speed closed-loop control, kicking control, motor control signal generation, and other sensor information acquisition, and is responsible for information interaction with the upper computer. This design only completes the motion control part of the lower computer.

3.1 Orthogonal encoding signal acquisition and speed measurement implementation
The output signal of the incremental photoelectric encoder is shown in Figure 4.

The two-phase signals A and B are orthogonal square wave pulse trains with a phase difference of 90°. Each pulse represents that the object under test has rotated a certain angle, and the phase relationship between A and B reflects the rotation direction of the object under test. The 4-fold frequency multiplication and direction identification circuit is designed in FPGA. This design uses 2 outputs: one output direction and the other output pulse, and simulates the direction identification frequency multiplication circuit, as shown in Figure 5.

There are three methods for measuring speed based on pulse counting: M method, T method and M/T method. M method is suitable for high-speed measurement, but has a large error at low speed; while T method, on the contrary, is accurate at low speed, but has a large error at high speed.

This design adopts the method described in the literature. As shown in Figure 6, the reference gate time is set to a fixed value. It is only used as a reference signal and a coding signal to jointly determine the actual gate time. The gate time determined in this way is a whole period of the measured signal, which can effectively improve the measurement accuracy. The measured speed is:

3.2 Incremental PID control principle and its FPGA implementation
The mathematical model of the actual robot inevitably has a certain degree of parameter uncertainty, and the orthogonal omnidirectional wheels of the three-wheeled omnidirectional mobile robot will alternately contact the ground when walking and generate some uncertain friction torque, which will bring difficulties to the precise control of the robot. In order to accurately control the three-wheeled omnidirectional mobile robot, the system uses the PID speed closed-loop control algorithm to adjust the speed of the robot's three omnidirectional wheels.
Let the sampling period be TS, and the continuous PID formula can be discretized to obtain the digital PID algorithm expression:


Where: k is the sampling number; u(k) is the computer output value at the kth sampling moment; e(k) is the computer input error value at the kth sampling moment; e(k-1) is the input error value at the k-1th sampling moment; Kp is the proportional coefficient; KI is the integral coefficient; KD is the differential coefficient.
Although this algorithm is relatively intuitive, since it is a full output, each output is related to all past states. When calculating, e(k) must be accumulated, and the amount of computer calculation is large.
Therefore, the incremental PID algorithm is generated:



The above formula (7) is an incremental PID control algorithm. Only the control increment is output, the impact of false action is small, and the control increment is only related to the most recent sampling values. It is easy to obtain a better control effect through weighted processing.
According to the above formula derivation, combined with the working characteristics of FPGA, this paper designs an incremental PID implementation structure suitable for FPGA.
As can be seen from Figure 7, the incremental PID control algorithm program structure can be weighted calculated with only the most recent 3 error sampling values. This can be completely implemented in parallel inside the FPGA. The structure of the shift part is similar to the implementation structure of the FIR filter. The difficulty lies in the skilled operation of signed numbers during FPGA design and ensuring that the accumulator does not overflow.

An efficient hardware testing means and system testing method, which can obtain and display the real-time signal of the programmable system on chip (SOPC), can be downloaded to the FPGA along with the design file, and is used to capture the status of the internal nodes and I/O pins of the FPGA, just like using a real logic analyzer, to simulate the design online, but without affecting the operation of the hardware system. In order to verify whether the measured actual speed value of the omnidirectional wheel is accurate, the designed speed measurement module is simulated online. Each omnidirectional wheel is set to rotate at a fixed speed, and the measured actual speed value is compared with the set speed value, as shown in Figure 8.

The PID module was also tested online in the embedded logic analyzer. Experimental conditions: Under no-load conditions, the motor speed was frequently changed, and the speed value and set value after PID adjustment inside the FPGA were observed through the embedded logic analyzer. Figure 9 shows the speed set value and feedback speed value of the No. 1 omnidirectional wheel.

The three-wheel omnidirectional mobile robot is different from the two-wheel differential robot in that it has great flexibility. Moreover, due to the different loads of the three omnidirectional wheels, the robot cannot walk in a precise straight line. To achieve precise control of the robot, a prerequisite is to enable it to walk in a very straight line. In order to test the control performance of the robot, the following experiment was designed: the robot walks in four directions, forward, backward, left and right, at a fixed speed. First, observe the situation without adding the PID control algorithm, and then observe the situation with adding the PID control algorithm. The results of the experiment are shown in Table 1.

Analysis: Since the loads on the three omnidirectional wheels of the robot are different, that is, under the same duty cycle PWM, the actual speeds of the three wheels are not the same, which makes it impossible to accurately synthesize the speed of the robot with the three wheels, thus affecting the control trajectory of the robot. According to Figure 1, the loads on wheels 1 and 3 of the robot are similar, and the load on wheel 2 is larger. When the PID controller is not added, although the forward and backward movement is approximately a straight line within a certain range, the speed of the robot cannot reach the expected set speed. The left and right movement trajectory is a circle, and the set left and right movement speed of the robot also determines whether the robot turns in a clockwise or counterclockwise direction. After adding the PID control algorithm, the speed of the wheel is corrected, and the robot can move forward and backward, left and right at the expected set speed, especially the left and right movement is approximately a straight line within a certain range, no longer a circle. It can be seen that the PID closed-loop control algorithm significantly improves the control performance of the robot.

5 Conclusion
Aiming at the current common solution of realizing the underlying motion control system of a soccer robot with DSP as the core, a method of realizing the underlying motion control system of a three-wheeled omnidirectional mobile soccer robot using FPGA is proposed. Through the application practice on the three-wheeled soccer robot, it is found that this solution implemented by FPGA has good real-time performance and high accuracy. In addition, due to the multi-pin characteristics of FPGA itself, it has strong scalability. For example, it can configure other sensors such as digital compass and other peripheral information sensors through the serial port. At the same time, this design has important reference value and practical value for studying the implementation of multi-motor robot motion control system.
In addition, due to the strong follow-up performance of the omnidirectional wheel and easy slipping, the direction is easily affected when implementing precise control, and the PID closed-loop control algorithm has a long response time, and the parameters still need more time to debug. In future research, we will study more precise control algorithms to achieve precise control of the robot.