Vector wheel control system and method based on series-parallel PID (Proportion Integration Differentiation) control

Abstract

The vector wheel control system comprises an angle ring, a position ring and a speed ring, the angle ring and the position ring are connected in parallel to serve as an inner ring, and the speed ring serves as an outer ring; the angle ring comprises an angle ring PID control module used for generating an angle correction value; the position loop comprises a position loop PID control module used for generating a position correction value. The speed ring comprises a speed ring PID control module used for generating a speed correction value. The dynamic performance of the system is remarkably improved through the quick response capability of the angle ring, and the method is particularly suitable for quick posture adjustment of the Mecanum wheel; the large-range adjustment capability of the position ring ensures the steady-state precision of the system in a complex motion scene; the speed ring serves as an outer ring to coordinate work of the angle ring and the position ring, and the overall performance of the system is further improved.

Description

Technical Field

[0001] The present invention relates to motion control technology, and in particular to a vector wheel control system and method based on series-parallel PID control. Background Art

[0002] Vector wheels such as Mecanum wheels are wheels that can achieve omnidirectional movement and are widely used in fields such as mobile robots and automation equipment. Their multi-degree-of-freedom motion characteristics pose higher requirements for the dynamic performance and steady-state accuracy of the control system. Traditional control methods are prone to problems such as overshoot and oscillation in complex motion scenarios. Existing control methods usually use a single position loop or speed loop as the inner loop and cannot balance the requirements of fast response and large-range adjustment. Summary of the Invention

[0003] Object of the Invention: The object of the present invention is to provide a vector wheel control system based on series-parallel PID control, which solves the problem of slow dynamic response in the prior art of Mecanum wheel control methods and significantly improves the control performance of the system through the structure of parallel connection of the angle loop and the position loop as the inner loop and the speed loop as the outer loop; the second object of the present invention is to provide a vector wheel control method based on series-parallel PID control.

[0004] Technical Solution: A vector wheel control system based on series-parallel PID control of the present invention includes an angle loop, a position loop, and a speed loop. The angle loop and the position loop are connected in parallel as the inner loop, and the speed loop is used as the outer loop; the angle loop includes an angle loop PID control module for generating an angle correction value u θ ; the position loop includes a position loop PID control module for generating a position correction value u p ; the speed loop includes a speed loop PID control module for generating a speed correction value; wherein, the angle correction value u θ and the position correction value u p are combined with a weighting coefficient α to achieve parallel connection to obtain a comprehensive control signal; the comprehensive control signal obtains the duty ratio of the motor through the speed PID control module.

[0005] Preferably, it includes a mobile robot; the angle loop further includes a gyroscope for collecting the actual angle of the mobile robot; the position loop further includes a laser range finder for collecting the actual position of the mobile robot; the position loop further includes an encoder for collecting the actual rotation speed of the motor.

[0006] Preferably, the input of the angle loop PID control module is the deviation between the target angle and the actual angle; the input of the position loop PID control module is the deviation between the target position and the actual position; the input of the speed loop PID control module is the deviation between the target speed and the actual speed.

[0007] A vector wheel control method based on series-parallel PID control of the present invention includes the steps:

[0008] (1) Collect the real-time angle value of the mobile robot through a gyroscope, establish an angle error-related equation, and according to the PID formula, derive the correction value equation of the angle loop, and solve to obtain the angle correction value u θ ;

[0009] (2) Collect the real-time position value of the mobile robot through a laser range finder. The position matrix can be obtained by using the correlation matrix equation. Combine the Pearson correlation coefficient to derive the position loop correction value u p , establish a position error-related equation, and then according to the PID formula, derive the correction value equation of the position loop, and solve to obtain the angle correction value u θ Then obtain the accurate position of the mobile robot through the Kalman filter equation;

[0010] (3) Combine the angle correction value u θ and the position correction value u p through the weighting coefficient α to realize the parallel connection of the angle loop and the speed loop, and obtain the comprehensive control signal of the inner loop; the comprehensive control signal is the output value u of the inner loop inner ;

[0011] (4) Detect the real-time speed of the mobile robot through an encoder, and combine the output value u of the inner loop inner as the input of the speed loop, and calculate the speed correction value u through the correction equation of the speed loop v ;

[0012] (5) Observe the output value changes of the angle loop, position loop and speed loop through the upper computer, adjust the PID parameters, and limit the amplitude of the output values of the angle loop, position loop and speed loop.

[0013] Preferably, the angle error-related equation in the step (1) is as follows:

[0014]

[0015] In the above formula, e θ is the angle error, angle, and angle_min are the real-time angle and the reference angle respectively;

[0016] The correction value equation of the angle loop in the step (1) is as follows:

[0017]

[0018] Among them, e θ is the angle error, e θ_last is the previous angle error, K pθ is the angle proportional gain, Kiθ is the angular integral gain, K dθ is the angular differential gain.

[0019] Preferably, the position error correlation equation in step (2):

[0020]

[0021] where dx is the lateral distance, dy is the longitudinal distance, and n is the number of divisions of the distance.

[0022] Then, according to the PID formula, the correction value equation of the position loop is obtained:

[0023]

[0024] where e p is the position error, e p_last is the previous position error, e p_integral is the sum of errors, K pp is the position proportional gain, K ip is the position integral gain, K dp is the position differential gain;

[0025] The exact position of the mobile robot is obtained through the Kalman filter equation:

[0026]

[0027] where kfp_Now_P is the system estimation covariance at time k, kfp_Last_P is the system covariance at time k - 1, kfp_Q is the process noise covariance, kfp_Kg is the Kalman gain, kfp_R is the observation noise covariance, and kfp_out is the Kalman filter output value.

[0028] Preferably, the inner loop output value u inner The calculation formula is:

[0029] u inner = α × u θ + (I - α) × u p

[0030] In the above formula, α is the weighting coefficient, which is used to adjust the contribution ratio of the angle loop and the position loop. The weighting coefficient α is calculated according to the mass and center of gravity of the mobile robot and obtained through experiments or simulations according to actual needs.

[0031] Preferably, the correction equation of the speed loop in step (4) is:

[0032]

[0033] Among them, X represents the wheel number, X_Goal is the target speed of the X-th wheel, X_Encoder is the real-time speed of the X-th wheel, and e v is the speed error, e v_last is the previous speed error, and e v_pre is the speed error of the time before last time. K pv is the speed proportional gain, and K iv is the speed integral gain, and K dv is the speed derivative gain; the final control signal is calculated through the correction equation of the speed loop, and this control signal is converted into the duty ratio of the motor to drive the motor of the Mecanum wheel.

[0034] Equation for the target speed X_Goal:

[0035]

[0036] Among them, u inner is the total output value of the parallel inner loop, and speed is the specified reference speed.

[0037] Preferably, the speed correction value u v is converted into the duty ratio of the motor to drive the motor of the vector wheel.

[0038] Preferably, the method further includes optimizing the PID parameters and the weighting coefficient α through dynamic response tests and steady-state accuracy tests.

[0039] Beneficial effects: Compared with the prior art, the remarkable effect of the present invention is that: the dynamic performance of the system is significantly improved through the fast response ability of the angle loop, which is particularly suitable for the fast attitude adjustment of the Mecanum wheel; the large-range adjustment ability of the position loop ensures the steady-state accuracy of the system under complex motion scenarios; the speed loop, as the outer loop, coordinates the work of the angle loop and the position loop, further improving the overall performance of the system; through the combination of the three-loop PID control, the present system has a strong ability to suppress external interference. Description of the Drawings

[0040] Figure 1 is the structural block diagram of the present invention;

[0041] Figure 2 is the schematic diagram of the mobile robot model of the present invention;

[0042] Figure 3 is the speed curve graph during the movement of the traditional control method;

[0043] Figure 4 is the speed curve graph during the movement of the present invention. Detailed Embodiments

[0044] The technical solution of the present invention will be further described below in conjunction with embodiments and drawings.

[0045] As Figure 1 shown, the present invention proposes a three-ring series-parallel PID control system for Mecanum wheels, and its architecture includes an angle loop, a position loop, and a speed loop. The angle loop and the position loop are connected in parallel as the inner loop, and the speed loop is used as the outer loop. The outputs of the inner loop are combined through a weighting coefficient and used as the input of the speed loop. Finally, the optimal duty cycle of the motor is obtained through the correction equation of the speed loop. The control method is as follows:

[0046] Step 1: Collect the real-time angle value of the mobile robot through a gyroscope, establish the relevant angle error equation, and derive the correction value equation of the angle loop according to the PID formula to solve for the angle correction value u θ ;

[0047] The relevant angle error equation is as follows:

[0048]

[0049] In the above formula, e θ is the angle error, angle and angle_min are the real-time angle and the reference angle respectively;

[0050] The correction value equation of the angle loop is as follows:

[0051]

[0052] In the above formula, e θ_last is the angle error in the previous time period, K pθ is the angle proportional gain, K iθ is the angle integral gain, K dθ is the angle differential gain; limit the output result, and control the maximum rotation speed of the output of the angle loop within 10% of the full rotation speed of the motor to achieve the effect of high-speed fine-tuning.

[0053] Step 2: Collect the real-time position value of the mobile robot through a laser range finder, and use the correlation matrix equation to obtain the position matrix, so as to calculate the distance from the target value more directly. Combine the Pearson correlation coefficient to derive the position loop correction value u p , establish the position error equation, and then according to the PID formula, derive the correction value equation of the position loop to solve for the angle correction value u θ ,

[0054] The relevant position error equation:

[0055]

[0056] In the above formula, dx is the horizontal distance, dy is the vertical distance, and n is the number of times of dividing the distance.

[0057] Then, according to the PID formula, the correction value equation of the position loop is obtained:

[0058]

[0059] In the above formula, e p is the position error, e p_last is the previous position error, e p_integral is the sum of errors, K pp is the position proportional gain, K ip is the position integral gain, K dp is the position derivative gain.

[0060] The accurate position of the mobile robot is obtained through the Kalman filter equation:

[0061]

[0062] In the above formula, kfp_Now_P is the system estimation covariance at time k, kfp_Last_P is the system covariance at time k - 1, kfp_Q is the process noise covariance, kfp_Kg is the Kalman gain, kfp_R is the observation noise covariance, and kfp_out is the Kalman filter output value.

[0063] Step 3: Detect the real-time speed through the GMR encoder. Combine the outputs of the angle loop and the position loop, calculate the target speed and perform PID correction, and finally output the motor duty cycle. Among them, the GMR encoder is coaxially connected to the DC motor to ensure synchronous rotation of the two. The output end of the encoder is connected to the input end of the system through a signal processing circuit. After pulse signal processing and filtering processing, it is corrected by combining the outputs of the angle loop and the position loop to obtain the duty cycle of the output motor; list the correction equation of the speed loop according to the PID formula, and combine steps (2) and (3) to obtain the target speed equation;

[0064] The correction equation of the speed loop is:

[0065]

[0066] In the above formula, X represents the wheel number, X_Goal is the target speed of the X-th wheel, X_Encoder is the real-time speed of the X-th wheel, e v is the speed error, e v_last is the previous speed error, e v_pre is the speed error of the time before last, K pv is the speed proportional gain, K iv is the speed integral gain, K dv is the speed derivative gain;

[0067] The final control signal is calculated through the correction equation of the speed loop, and this control signal is converted into the duty cycle of the motor to drive the motor of the Mecanum wheel.

[0068] From equations (2) and (4), the target speed X_Goal equation can be obtained:

[0069]

[0070] where u inner is the total output value of the parallel inner loop, speed is the specified reference speed, and α is the weighting coefficient used to adjust the contribution ratio of the angle loop and the position loop; the weighting coefficient α is calculated based on the mass and center of gravity of the mobile robot.

[0071] Step 4: Use the host computer to observe the changes in the output values of the three loops, adjust the PID parameters, and reasonably limit the amplitudes of the output values of the three loops.

[0072] The specific method for deriving the PID parameter adjustment and assignment limit is as follows: First, roughly adjust the PID parameters of the speed loop so that the output value of the speed loop for the reference speed reaches an ideal stable value, and the speeds of the four wheels are approximately the same. The parameters of K iv and K dv should not be too large. Different from the traditional speed loop, when the motor is blocked, there is no oscillation exceeding the reference speed, but it needs to reach the reference speed quickly and smoothly to prevent high-frequency oscillation when connecting the subsequent three loops. The speed loop limit should be lower than the maximum speed of the motor. From formula (6), we can get:

[0073]

[0074] Then, adjust the PID parameters of the angle loop. Connect the angle loop and the speed loop in series. It is necessary to make very fine adjustments to the K pθ , K iθ , and K dθ of the angle loop. The output limit of the angle loop should be lower than 10% of the maximum speed of the motor.

[0075] From formulas (2) and (8), we can get

[0076]

[0077] Substitute X_Goal into the speed loop. At this time, u v is the final output value.

[0078] Finally, adjust the PID parameters of the position loop. First, connect the position loop and the angle loop in parallel, and then connect them in series with the speed loop. The position loop reduces the change rate of its output parameters through Kalman filtering. The limit is equal to the difference between the maximum speed of the motor and the output value of the angle loop. Therefore, there is a difference between strong control and weak change with the angle loop. According to the actual ground friction, adjust the K pp and Kip , K dp Adjust the parameter to improve the effect of sudden stop after high-speed operation. From formulas (4), (7), and (9), it can be obtained that

[0079]

[0080] Among them, the weighting coefficient α is used to adjust the contribution ratio of the angle loop and the position loop. The value of α is determined based on the mass and center-of-gravity distribution of the mobile robot, as well as the dynamic response requirements and steady-state accuracy requirements of the system. The specific method for deriving the weighting parameter α is as follows: Analyze the mass distribution of the mobile robot to determine its center-of-gravity position. Through software simulation, test the dynamic response of the system under different working conditions (such as rapid turning, curvilinear motion, etc.). Under steady-state operation conditions, test the steady-state error of the system. Through the above analysis, the weighting coefficient α can be calculated. Further, the angle loop has a fast response speed and is suitable for handling high-frequency dynamic changes (such as attitude adjustment). The role of the position loop: The position loop has a slow response speed but is suitable for handling large-range position adjustment. If the system requires a fast response (such as rapid turning of Mecanum wheels), then α should be relatively large (close to 1) to enhance the contribution of the angle loop. If the system requires high-precision position control (such as precise positioning of a mobile robot), then α should be relatively small (close to 0) to enhance the contribution of the position loop.

[0081] Formula for α:

[0082]

[0083] Among them, the dynamic response requirements and steady-state accuracy requirements are quantified through experiments or simulations.

[0084] Embodiment 1

[0085] As Figure 2 , 3 and shown in 4, when the mobile robot weighs 15 kg and the moving speed is 2 m / s, implementing the present invention, the simulation results are as Figure 4 shown, Figure 4 is the waveform of the speed change during the complete process from the start to the sudden stop of the mobile robot. In the starting stage, as can be seen from Figure 4 , the motor reaches the target speed and remains stable within 0.2 s, while the speed curve of the traditional PID method Figure 3 takes nearly 2 s to reach a relatively stable target speed and causes speed fluctuations of the motor; in the curvilinear motion stage, Figure 4 has obvious periodic fluctuations caused by angle adjustment, while Figure 3 does not perform angle adjustment; in the sudden stop stage, as can be seen from Figure 4 , the motor reduces its speed and finally stops stably within 0.2 s. From the comparison of the simulation results Figure 3It can be seen that it has good dynamic and steady-state performance under different motion postures.

Claims

1. A vector wheel control system based on series-parallel PID control, characterized in that: It includes an angle loop, a position loop and a speed loop. The angle loop and the position loop are in parallel as the inner loop, and the speed loop is the outer loop. The angle loop includes an angle-loop PID control module for generating an angle correction value u θ ; the position loop includes a position-loop PID control module for generating a position correction value u p ; the speed loop includes a speed-loop PID control module for generating a speed correction value; Among them, the angle correction value u θ and the position correction value u p are combined with the weighting coefficient α to achieve parallel connection, and a comprehensive control signal is obtained; the comprehensive control signal is used by the speed PID control module to obtain the duty cycle of the motor.

2. The vector wheel control system based on series-parallel PID control according to claim 1, characterized in that: It includes a mobile robot; the angle loop further includes a gyroscope for collecting the actual angle of the mobile robot; the position loop further includes a laser range finder for collecting the actual position of the mobile robot; the position loop further includes an encoder for collecting the actual rotational speed of the motor.

3. The vector wheel control system based on series-parallel PID control according to claim 1, characterized in that: The input of the angle loop PID control module is the deviation between the target angle and the actual angle; the input of the position loop PID control module is the deviation between the target position and the actual position; the input of the speed loop PID control module is the deviation between the target speed and the actual speed.

4. A vector wheel control method based on series-parallel PID control, characterized in that: It includes the steps: (1) Collect the real-time angle value of the mobile robot through the gyroscope, establish the equation related to the angle error, and derive the correction value equation of the angle loop according to the PID formula to solve the angle correction value u θ ; (2) Collect the real-time position value of the mobile robot through a laser range finder sensor. The position matrix can be obtained by using the correlation matrix equation, and the position loop correction value u can be derived by combining the Pearson correlation coefficient. p , establish the position error correlation equation, and then according to the PID formula, derive the correction value equation of the position loop, and solve to obtain the angle correction value u. θ Then obtain the accurate position of the mobile robot through the Kalman filter equation; (3) Combine the angle correction value u θ and the position correction value u p through the weighting coefficient α to achieve the parallel connection of the angle loop and the speed loop, and obtain the comprehensive control signal of the inner loop; the comprehensive control signal is the output value u inner ; (4) Detect the real-time speed of the mobile robot through the encoder, and combine the output value u of the inner loop inner As the input of the speed loop, calculate the speed correction value u through the correction equation of the speed loop v ; (5) Observe the changes in the output values of the angle loop, position loop, and speed loop through the host computer, adjust the PID parameters, and limit the amplitude of the output values of the angle loop, position loop, and speed loop.

5. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The angle error correlation equation in the step (1) is as follows: In the above formula, e θ is the angular error, and angle and angle_min are the real-time angle and the reference angle respectively; The correction value equation of the angle loop in the step (1) is as follows: Among them, e θ is the angle error, e θ_last is the previous angle error, K pθ is the angle proportional gain, K iθ is the angle integral gain, K dθ is the angle derivative gain.

6. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The position error correlation equation in the step (2) is: Where, dx is the lateral distance, dy is the longitudinal distance, and n is the number of divisions of the distance. Then, according to the PID formula, the correction value equation of the position loop is obtained: Among them, e p is the position error, e p_last is the previous position error, e p_integral is the sum of errors, K pp is the position proportional gain, K ip is the position integral gain, K dp is the position derivative gain; The precise position of the mobile robot is obtained through the Kalman filter equation: Where, kfp_Now_P is the system estimation covariance at time k, kfp_Last_P is the system covariance at time k-1, kfp_Q is the process noise covariance, kfp_Kg is the Kalman gain, kfp_R is the observation noise covariance, and kfp_out is the Kalman filter output value.

7. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The inner loop output value u in step (3) inner The calculation formula is: u inner = α × u θ + (1 - α) × u p In the above formula, α is a weighting coefficient used to adjust the contribution ratio of the angle loop and the position loop. The weighting coefficient α is calculated according to the mass and center of gravity of the mobile robot and obtained through experiments or simulations according to actual requirements.

8. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The correction equation of the speed loop in the step (4) is: Among them, X represents the wheel number, X_Goal is the target speed of the X-th wheel, X_Encoder is the real-time speed of the X-th wheel, e v is the speed error, e v_last is the previous speed error, e v_pre is the speed error of the time before last, K pv is the speed proportional gain, K iv is the speed integral gain, K dv is the speed derivative gain; the final control signal is calculated through the correction equation of the speed loop, and this control signal is converted into the duty ratio of the motor to drive the motor of the Mecanum wheel. The target speed X_Goal equation: Among them, u inner is the total output value of the parallel inner loop, and speed is the specified reference speed.

9. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The speed correction value u v is converted into the duty cycle of the motor for driving the motor of the vector wheel.

10. The vector wheel control method based on series-parallel PID control according to claim 4, characterized in that: The method further includes optimizing the PID parameters and the weighting coefficient α through dynamic response tests and steady-state accuracy tests.

Images