Automatic running control system and method for trolley

Abstract

The invention relates to an automatic running control system and method for a trolley, belongs to the technical field of automatic control, is used for controlling the trolley to run automatically on a set track, and comprises a sensor module, a main control module, a speed control module and a direction control module. The sensor module is used for identifying a set track and acquiring the speed and the driving direction of the trolley in real time; the main control module is used for generating a first driving control instruction and a second driving control instruction when the speed and the driving direction of the trolley are not the target speed and the target direction; the speed control module is used for responding to the first driving control instruction and adjusting the speed of the trolley to the target speed; and the direction control module is used for calibrating the gyroscope, after a Kalman filter is adopted to predict calibrated data in real time, a second driving control instruction is responded, the driving direction of the trolley is adjusted to the target direction based on a direction ring-motor ring cascade PID, and the control precision and stability are improved.

Description

Technical Field

[0001] The present invention relates to the field of automatic control technology, and in particular to a small vehicle automatic driving control system and method. Background Art

[0002] The smart car must automatically drive on the designated path during the race. In order to ensure that the smart car drives smoothly on the entire track, the speed and direction of the smart car need to be controlled. The purpose of controlling the speed of the smart car is to allow the smart car to complete the race as quickly as possible within the specified time. The purpose of controlling the direction of the smart car is to accurately and smoothly follow the path at the maximum safe speed when driving on a curve, so that the car can pass the track at the fastest speed without running out of the track or pressing the track line.

[0003] However, when the smart car drives automatically on a designated path, it is easily affected by external interference and uncertain factors such as changes in its own parameters, resulting in low control accuracy and insufficient stability of the smart car. Summary of the invention

[0004] In view of this, it is necessary to provide a small car automatic driving control system and method to solve the technical problems of low control accuracy and insufficient stability when the intelligent small car automatically drives on a specified path.

[0005] In order to solve the above problems, the present invention provides a trolley automatic driving control system, which is used to control the trolley to automatically drive on a set track. The trolley automatic driving control system includes a sensor module, a main control module, a speed control module and a direction control module; The sensor module is used to identify the set trajectory and obtain the speed and driving direction of the car in real time; The main control module is used to generate a first drive control instruction and a second drive control instruction when the speed and the driving direction of the vehicle are not the target speed and the target direction; The speed control module is used to respond to the first drive control instruction and adjust the speed of the vehicle to a target speed; The direction control module is used to calibrate the gyroscope of the sensor module using a zero-point calibration method, and after using a Kalman filter to make real-time predictions on the calibrated data, respond to the second drive control instruction, and adjust the driving direction of the car to the target direction based on the direction ring-motor ring cascade PID.

[0006] In a possible implementation, the sensor module includes an infrared tracking unit, a gyroscope sensor, and a Hall encoder; The infrared tracking unit is used to identify a set driving trajectory, wherein the driving trajectory includes a black arc driving trajectory; The gyro sensor is used to obtain the rotation angle and angular velocity of the car in real time, and determine the driving direction of the car based on the rotation angle and angular velocity of the car; The Hall encoder is used to measure the rotation speed of the trolley motor, and the speed of the trolley is determined based on the rotation speed of the trolley motor.

[0007] In a possible implementation, the main control module includes a main control unit of model MSPMOG3507; The main control unit is used to receive input signals from the sensor module, the speed control module and the direction control module, and output a first drive control signal and a second drive control signal after performing logic control processing on the input signals.

[0008] In a possible implementation, the speed control module includes a PID control unit; The PID control unit is used to determine the speed error based on the acquired vehicle speed and the target speed, calculate the speed adjustment amount based on the speed error, dynamically adjust the duty cycle of the PWM signal through the PWM technology based on the speed adjustment amount, and generate a first drive control signal after inputting the duty cycle of the PWM signal into the main control module.

[0009] In a possible implementation, the direction control module includes a zero point calibration unit, a Kalman filter unit, and a direction loop-motor loop cascade PID control unit; The zero point calibration unit is used to perform yaw calibration on the gyroscope using a zero point calibration method; The Kalman filter unit is used to iteratively optimize the data collected by the calibrated gyroscope through the Kalman filter to obtain the optimal estimated value of the vehicle angle; The direction ring-motor ring cascade PID control unit is used to determine the direction error based on the optimal estimated value of the angle of the trolley and the target direction, input the direction error into the direction ring of the direction ring-motor ring cascade PID to obtain a corrected direction error, input the corrected direction error into the motor ring of the direction ring-motor ring cascade PID, perform PID operation on the corrected direction error through the motor ring, obtain the duty cycle of the PWM signal of the two-wheel differential of the trolley, input the duty cycle of the PWM signal of the two-wheel differential of the trolley into the main control module, and generate a second drive control signal.

[0010] In a possible implementation, the iterative optimization of the data collected by the gyroscope by using a Kalman filter to obtain the angle prediction value of the vehicle includes: Acquire an angular velocity value of a gyroscope, construct a state equation based on the angular velocity value of the gyroscope, and obtain a predicted value of a current angle based on the state equation; Defining covariance noise, and constructing a prediction covariance matrix based on the covariance noise; Calculating the Kalman gain based on the prediction covariance matrix, the observation matrix, and the measurement noise covariance matrix; The optimal state estimation is performed based on the Kalman gain and the predicted value of the current angle to obtain the optimal estimated value of the car angle, and the covariance matrix is ​​updated through the Kalman gain.

[0011] In a possible implementation, it also includes an acoustic and optical feedback module and a motor drive module; The sound and light feedback module is used to emit sound and light feedback when the car travels to the marked point on the track; The motor drive module is used to control the rotation speed of the vehicle motor based on the first drive control signal and the second drive control signal of the main control module to adjust the speed and driving direction of the vehicle.

[0012] On the other hand, the present invention also provides a method for controlling automatic driving of a trolley, which is implemented by the automatic driving control system of the trolley as described above, and comprises the following steps: Based on the sensor module, the set trajectory is identified and the speed and driving direction of the car are obtained in real time; Based on the main control module, when the speed and the driving direction of the vehicle are not the target speed and the target direction, a first driving control instruction and a second driving control instruction are generated; adjusting the speed of the vehicle to a target speed based on the speed control module responding to the first drive control instruction; Based on the direction control module, the gyroscope of the sensor module is calibrated by a zero-point calibration method, and the calibrated data is predicted in real time by a Kalman filter. Then, in response to the second drive control instruction, the driving direction of the car is adjusted to the target direction based on the direction ring-motor ring cascade PID.

[0013] In a possible implementation, the step of adjusting the speed of the vehicle to a target speed in response to the first drive control instruction includes: The speed error is input into the PID control unit to obtain the speed adjustment amount, where the proportional term of the PID determines the acceleration or deceleration force according to the speed error, the steady-state error of the speed is eliminated through the integral term of the PID, and the differential term of the PID adjusts the control output according to the speed change rate; Based on the speed adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through PMW technology. After the duty cycle of the PWM signal is input into the main control module, a first drive control signal is generated, and the motor power of the car is controlled based on the first drive control signal. Based on the motor power of the car, the driving speed of the car is adjusted to the target speed through the motor drive module.

[0014] In a possible implementation, responding to the second drive control instruction and adjusting the driving direction of the vehicle to a target direction based on a direction ring-motor ring cascade PID includes: The direction error is input into the direction loop-motor loop cascade PID to obtain the direction adjustment amount, where the proportional term of the PID adjusts the direction strength according to the angle error, and the differential term of the PID adjusts the direction according to the rate of change of the angle error; Based on the direction adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through PMW technology. After the duty cycle of the PWM signal is input into the main control module, a second drive control signal is generated. The motor drive module is controlled based on the second drive control signal, and the driving direction of the car is adjusted to the target direction through the motor drive module.

[0015] The beneficial effects of the present invention are as follows: the main control module is used to generate a first drive control instruction and a second drive control instruction when the speed and the driving direction of the car are not the target speed and the target direction; the speed control module is used to respond to the first drive control instruction and adjust the speed of the car to the target speed; the direction control module is used to calibrate the gyroscope of the sensor module by using a zero-point calibration method, and use a Kalman filter to perform real-time prediction on the calibrated data, thereby reducing the influence of the external environment on the sensor data and the influence of the changes in its own parameters, and responding to the second drive control instruction, adjusting the driving direction of the car to the target direction based on the direction ring-motor ring cascade PID, adjusting the driving direction of the car by the direction ring-motor ring cascade PID, adjusting the posture of the car, making the car body balanced and turning normally, and realizing precise control of the car speed by adjusting the speed of the car, and ensuring that the car travels stably according to the set trajectory by adjusting the speed and driving direction of the car, thereby improving the control accuracy and stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 A schematic diagram of the structure of an embodiment of the automatic driving control system of a trolley provided by the present invention; Figure 2 A schematic structural diagram of an embodiment of a sensor module of an automatic driving control system for a small vehicle provided by the present invention; Figure 3 A schematic structural diagram of an embodiment of a main control module of the automatic driving control system of a trolley provided by the present invention; Figure 4A schematic structural diagram of an embodiment of a speed control module of the automatic driving control system of a trolley provided by the present invention; Figure 5 A schematic structural diagram of an embodiment of a direction control module of an automatic driving control system for a trolley provided by the present invention; Figure 6 A flow chart of an embodiment of the automatic driving control method of a trolley provided by the present invention; Figure 7 A schematic diagram of a trajectory set for the automatic driving control method of a small vehicle provided by the present invention; Figure 8 A schematic diagram of a PID incremental algorithm for the automatic driving control method of a small vehicle provided by the present invention. DETAILED DESCRIPTION

[0017] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0018] The invention discloses a trolley automatic driving control system. Figure 1 is a schematic diagram of the automatic driving control system of the car provided by the embodiment of the present invention, please refer to Figure 1 , a trolley automatic driving control system is used to control the trolley to automatically drive on a set track. The trolley automatic driving control system 100 includes a sensor module 110, a main control module 120, a speed control module 130 and a direction control module 140; The sensor module 110 is used to identify the set trajectory and obtain the speed and driving direction of the car in real time; The main control module 120 is used to generate a first driving control instruction and a second driving control instruction when the speed and the driving direction of the vehicle are not the target speed and the target direction; The speed control module 130 is used to respond to the first driving control instruction and adjust the speed of the vehicle to the target speed; The direction control module 140 is used to calibrate the gyroscope of the sensor module using a zero-point calibration method, and after using a Kalman filter to make real-time predictions on the calibrated data, respond to the second drive control instruction, and adjust the driving direction of the car to the target direction based on the direction ring-motor ring cascade PID.

[0019] In some embodiments, see Figure 2 , the sensor module 110 includes an infrared tracking unit 111, a gyro sensor 112 and a Hall encoder 113; The infrared tracking unit 111 is used to identify a set driving track, wherein the driving track includes a black arc driving track; The gyro sensor 112 is used to obtain the rotation angle and angular velocity of the car in real time; The Hall encoder 113 is used to measure the rotation speed and rotation direction of the trolley motor, and determine the speed of the trolley based on the rotation speed of the trolley motor.

[0020] In some embodiments, see Figure 3 , the main control module 120 includes a main control unit 121 of model number MSPMOG3507; The main control unit 121 is used to receive input signals from the sensor module, the speed control module and the direction control module, perform logic control processing on the input signals, and then output a first drive control signal and a second drive control signal.

[0021] In some embodiments, see Figure 4 , the speed control module 130 includes a PID control unit 131; The PID control unit 131 is used to determine the speed error based on the acquired vehicle speed and the target speed, calculate the speed adjustment amount based on the speed error, dynamically adjust the duty cycle of the PWM signal through the PWM technology based on the speed adjustment amount, and generate a first drive control signal after inputting the duty cycle of the PWM signal to the main control module 120; The working principle of the speed control module 130 is as follows: the Hall encoder measures the motor speed of the car and outputs it as a pulse signal; the PID controller reads the error between the current speed and the target speed to calculate the control amount, and dynamically adjusts the duty cycle of the PWM signal according to the control amount to control the motor speed to eliminate the error, achieve precise control of the vehicle speed, and ensure that the car travels stably along the planned path.

[0022] In some embodiments, see Figure 5 , the direction control module 140 includes a zero point calibration unit 141, a Kalman filter unit 142 and a direction loop-motor loop cascade PID control unit 143; The working principle of the direction control module is as follows: the gyroscope, as an attitude sensor, can detect and measure the angular motion state of the car, including the rotation angle and angular velocity. The gyroscope has zero drift error and angle error in measurement, and the zero-point calibration method is used to eliminate the zero drift error; the Kalman filter technology is used to filter the angular velocity collected by the gyroscope, correct the angle error, and obtain a more accurate attitude angle estimation to improve the accuracy of the car's angle measurement. The gyroscope adjusts the car's attitude based on the attitude angle estimation, so that the car body is balanced and can turn and move forward normally, thereby achieving precise control of the car's direction; The zero point calibration unit 141 is used to calibrate the yaw of the gyroscope using the zero point calibration method. The yaw of the gyroscope of the smart car has zero drift error and angle error. The zero drift error is usually caused by a slight change in the mechanical structure or electronic components inside the gyroscope, resulting in a continuous output change of the gyroscope even without angular velocity input, especially in the case of long-term operation or large temperature changes, resulting in serious cumulative errors. The zero point calibration method is used to regularly calibrate the gyroscope to reduce the impact of zero drift; the yaw calibration process is as follows: place the smart car on a flat ground and turn on the power switch. Wait for the system initialization of the car to be completed, and the gyroscope is also in a state of preparing for calibration. Enter the gyroscope calibration mode through the button switch. After entering the calibration mode, make sure that the car is completely still. This is because the gyroscope needs to be calibrated based on the posture of the car when it is stationary. Place the car stably without any external force or vibration. The car will automatically detect the data of the gyroscope and perform yaw calibration. This process may take some time. Do not move the car during this period. The OLED display will send back real-time data to display the calibration data. When the calibration is completed, the indicator light on the car will light up and the buzzer will sound. After the calibration is completed, you can do some simple tests to verify the calibration effect. If you feel that the yaw control is still inaccurate, you can calibrate again. When using the car, be careful to avoid strong vibration and magnetic field interference to ensure the accuracy and stability of the gyroscope. The Kalman filter unit 142 is used to iteratively optimize the data collected by the calibrated gyroscope through the Kalman filter to obtain the optimal estimate of the angle of the car. The angle error is the difference between the angle value measured by the gyroscope and the actual angle value. This error is caused by the manufacturing accuracy of the gyroscope, changes in the working environment (such as the influence of environmental factors such as temperature, humidity, and air pressure), and wear and aging during use. The data fusion technology (Kalman filter) is used to process the data of the gyroscope, which can effectively suppress or eliminate the influence of magnetic field interference and improve the accuracy and stability of the gyroscope. The process of Kalman filter processing the gyroscope data is as follows: obtaining the angular velocity value of the gyroscope, constructing a state equation based on the angular velocity value of the gyroscope, and obtaining the predicted value of the current angle based on the state equation; defining covariance noise, and constructing a predicted covariance matrix based on the covariance noise; based on the predicted The Kalman gain is calculated based on the covariance matrix, observation matrix, and measurement noise covariance matrix; the optimal state estimation is performed based on the Kalman gain and the predicted value of the current angle to obtain the optimal estimated value of the car angle, and the covariance matrix is ​​updated through the Kalman gain. The specific process is: the Kalman filter predicts and updates the gyroscope data in two steps, including the prediction step and the update step. The prediction step is: the current state is estimated based on the state at the previous moment, and the state of the gyroscope at the current moment is predicted based on the historical data and model of the gyroscope. First, the current angle is predicted: based on the angular velocity value of the gyroscope, the predicted value of the angle is obtained by integral calculation, the angular velocity of the smart car is measured by the gyroscope, and the rotation angle is obtained by integrating the angular velocity. Considering the angular velocity error value and the sensor sampling time, the state equation is constructed to predict the current angle. The calculation formula of the state equation is: , in, for The prior state estimate at time , that is, the estimate of the system state before obtaining the measurement value at the current time, is the optimal state estimate at the previous moment, is the state transfer matrix, which describes the state change law of the system from time k-1 to time k, and reflects the dynamic characteristics of the system. For example, in the calculation of the offset angle, if the angle of the system has a certain change law over time, the state transfer matrix can be used to describe this change. is the control input matrix, is the control input at the previous moment. The system has external control effects. This part represents the influence of the control input on the system state. In some cases, when there is no external control input, the control input can be zero. Secondly, the prediction covariance matrix: by defining the system process covariance noise, the prediction covariance matrix is ​​calculated according to the system model. This process involves the uncertainty of the error variance of the angle value and the angular velocity value. These uncertainties are represented by constants adjusted by experience. The calculation formula of the prediction covariance matrix is: , in, for The prior error covariance estimate at time t represents the uncertainty of the prior state estimate. It is a matrix whose diagonal elements represent the variance of each state variable and the off-diagonal elements represent the covariance between the state variables. is the error covariance estimate of the previous moment, is the transpose of the matrix, is the process noise covariance matrix, which represents the influence of uncertainty factors on the system during the state transition process. For example, in the calculation of the offset angle, the error is caused by the uncertainty within the system or the unmodeled dynamic factors. The update steps are as follows: based on the observation of the current state and the estimate of the previous moment, the optimal state value of the system is estimated, and the prediction result is compared with the actual observation data to adjust the prediction value to obtain a more accurate current state estimate; first, the Kalman gain K‌ is calculated based on the predicted covariance matrix, the measured covariance and the measurement coefficient matrix. The Kalman gain is used to balance the prior estimate and the posterior measurement when updating the state estimate. The calculation formula of the Kalman gain is: , in, is the Kalman gain, which determines the weight of the measured value and the prior estimate in the updated state estimate. The observation matrix links the state variables of the system with the measured values. In the calculation of the offset angle, if there is a specific relationship between the measured value obtained by a certain measurement method and the angular state of the system, the observation matrix can be used to describe this relationship. is the measurement noise covariance matrix, which represents the noise influence during the measurement process, such as the measurement error of the sensor; secondly, the state estimate is updated, and the prior state estimate is corrected by the Kalman gain and the measurement value to obtain the posterior state estimate, that is, the optimal estimated state, completing the data fusion process and obtaining the optimal angle estimate after fusion. The calculation formula of the optimal state estimate is: , in, for The posterior state estimate at time t is combined with the optimal state estimate after the measurement value. for The measured value at the moment; finally, update the covariance and use the Kalman gain to update the covariance matrix to obtain a more accurate state estimate. The calculation formula for the covariance update is: , in, is the updated error covariance estimate, is the identity matrix; Through iterative calculations of the Kalman filter, the gyroscope data is continuously adjusted and optimized to obtain a more accurate estimation of the vehicle's attitude angle, providing relatively accurate measurement values ​​under both dynamic and static conditions, thereby improving the navigation and control performance of the smart car; The direction ring-motor ring cascade PID control unit 143 is used to determine the direction error based on the optimal estimated value of the angle of the trolley and the target direction, input the direction error into the direction ring of the direction ring-motor ring cascade PID, obtain the corrected direction error, input the corrected direction error into the motor ring of the direction ring-motor ring cascade PID, perform PID operation on the corrected direction error through the motor ring, obtain the duty cycle of the PWM signal of the two-wheel differential of the trolley, input the duty cycle of the PWM signal of the two-wheel differential of the trolley into the main control module, and generate a second drive control signal; The control strategy of the direction control module adopts a cascade PID closed loop of direction loop and motor loop. The direction error is input into the direction loop, and the direction loop corrects the direction error to obtain the corrected error. The corrected error is input into the motor loop for PID calculation. The motor loop outputs the PWM of the differential speed of the two wheels of the car to the main control module of the car, and the direction of the car is controlled through the main control module.

[0023] In some embodiments, the automatic driving control system of the car further includes an acoustic and optical feedback module and a motor drive module; The sound and light feedback module is used to give sound and light feedback when the car reaches the mark point on the track; The motor drive module is used to control the rotation speed of the car motor based on the first drive control signal of the main control module and the first drive control signal to adjust the speed and driving direction of the car.

[0024] A specific embodiment of the present invention discloses a method for controlling automatic driving of a small vehicle, which is used to control the small vehicle to automatically drive on a set track. Figure 6 is a flow chart of the automatic driving control method of the car provided by the embodiment of the present invention, please refer to Figure 6 , the automatic driving control method of the car includes: S601, identifying the set trajectory based on the sensor module and obtaining the speed and driving direction of the car in real time; S602: generating a first driving control instruction and a second driving control instruction based on the main control module when the speed and the driving direction of the vehicle are not the target speed and the target direction; S603, adjusting the speed of the vehicle to a target speed based on the speed control module responding to the first drive control instruction; S604, based on the direction control module, the gyroscope of the sensor module is calibrated by a zero-point calibration method, and the calibrated data is predicted in real time by a Kalman filter, and then the driving direction of the car is adjusted to the target direction based on the direction ring-motor ring cascade PID in response to the second drive control instruction.

[0025] The track is set in the designated area, the area of ​​which is 220cm*120cm. For the schematic diagram of the set track, please refer to Figure 7 The radius of the two symmetrical semicircular arcs is 40cm. The arcs are black and 1.8cm wide. The four vertices of the arcs are A, B, C and D. A, B, C and D are identification points. The car starts to drive at the preset speed and angle, and detects in real time whether it deviates from the set track. When the car deviates, the car adjusts the steering through gyroscope control to ensure that it drives on the correct path. During driving, the car detects whether the key black line, that is, the BC segment or the DA segment, is recognized. When the car recognizes the identification points B, C, D, A, it sends out sound and light feedback and continues to drive along the predetermined path. When the black line cannot be detected, the car will automatically return to the initialization position and restart the task.

[0026] Compared with the prior art, the automatic driving control method of the trolley provided in the present embodiment is based on the sensor module to identify the set trajectory and obtain the speed and driving direction of the trolley in real time; based on the main control module, when the speed and driving direction of the trolley are not the target speed and the target direction, a first drive control instruction and a second drive control instruction are generated; based on the speed control module, the speed of the trolley is adjusted to the target speed in response to the first drive control instruction; based on the direction control module, the gyroscope of the sensor module is calibrated by a zero point calibration method, and the calibrated data is predicted in real time by using a Kalman filter, thereby reducing the influence of the external environment on the sensor data and the influence of the change of its own parameters; in response to the second drive control instruction, the driving direction of the trolley is adjusted to the target direction based on the direction ring-motor ring cascade PID, the driving direction of the trolley is adjusted by the direction ring-motor ring cascade PID, the posture of the trolley is adjusted, the body of the trolley is balanced and the steering is normal, and the speed of the trolley is adjusted to achieve accurate control of the speed of the trolley, and the speed and driving direction of the trolley are adjusted to ensure that the trolley travels stably according to the set trajectory, thereby improving the control accuracy and stability.

[0027] In some embodiments, in step S601, the set driving trajectory and the identification points on the trajectory are identified through the infrared tracking unit, the rotation angle and angular velocity of the car are obtained in real time through the gyroscope sensor, the driving direction of the car is determined based on the rotation angle and angular velocity of the car, and the motor speed of the car is measured through the Hall encoder, which outputs a pulse signal.

[0028] In some embodiments, in step S602, the main control module receives input signals from the sensor module, the speed control module, and the direction control module, and performs logic control processing on the input signals. When the speed and driving direction of the car are not the target speed and the target direction, a first drive control instruction and a second drive control instruction are generated. When the car drives to the identification point on the track, the sound and light feedback module is called, and sound and light feedback is sent through the sound and light feedback module, that is, the status feedback is provided using the LED indicator light and the buzzer.

[0029] In some embodiments, in step S603, based on the speed control module responding to the first drive control instruction, the speed of the car is adjusted to the target speed. The process is: the driving speed of the car is obtained through the Hall encoder, the speed error is calculated based on the driving speed of the car and the target speed, and the speed error is input into the PID controller to obtain the speed adjustment amount, wherein the proportional term of the PID determines the acceleration or deceleration force according to the speed error, the steady-state error of the speed is eliminated through the integral term of the PID, and the differential term of the PID adjusts the control output according to the speed change rate; based on the speed adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through the PMW technology, and after the duty cycle of the PWM signal is input into the main control module, a first drive control signal is generated, and the motor power of the car is controlled based on the first drive control signal, and based on the motor power of the car, the driving speed of the car is adjusted to the target speed through the motor drive module.

[0030] In some embodiments, in step S604, the gyroscope of the sensor module is calibrated by a zero-point calibration method based on the direction control module, and the calibrated data is predicted in real time using a Kalman filter. In response to the second drive control instruction, the driving direction of the car is adjusted to the target direction based on the direction ring-motor ring cascade PID. When the car is driving in a straight line, there is a direction deviation and the car is driving on a black arc track. In response to the second drive control instruction, the driving direction of the car is adjusted to the target direction. First, when driving on the set track, the rotation angle and angular velocity of the car are obtained in real time through the gyroscope, and the driving direction of the car is determined based on the rotation angle and angular velocity of the car. That is, after the gyroscope data is calibrated by the zero-point calibration unit, the data collected by the calibrated gyroscope is predicted by the Kalman filter. Perform iterative optimization to obtain the optimal estimate of the car's angle. According to the optimal estimate of the car's angle, determine the car's driving direction. According to the car's driving direction and the target direction, determine the direction error. Input the direction error into the direction loop-motor loop cascade PID. Through PID operation, obtain the direction adjustment amount. That is, after the deviation obtained by the gyroscope is input into the direction loop, it is loaded into the motor loop. Among them, the proportional term of the PID adjusts the direction according to the angle error. The differential term of the PID adjusts the direction according to the rate of change of the angle error. Based on the direction adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through the PMW technology. After the duty cycle of the PWM signal is input into the main control module, a second drive control signal is generated. The motor drive module is controlled based on the second drive control signal. The driving direction of the car is adjusted to the target direction through the motor drive module.The direction adjustment process of the direction loop-motor loop cascade PID is as follows: for the direction loop PID, first perform linear scaling and filtering of the original error to match the hardware control of the subsequent parameters, and adopt the strategy of position PID parameters. The I term in the position PID is used to eliminate static error and improve control accuracy. The larger the I term, the more lagging it will be. Too large a term will cause the system to oscillate. In the time domain, the I term is equivalent to integrating the error, and the direction adjustment generally requires dozens of operating cycles. Therefore, the integral term will lag behind the increase of imbalance. When the attitude is adjusted to the positive direction, the I term will cause the error to become larger due to the previous integration, resulting in erroneous deviation control in the correct direction, thus causing system oscillation. Therefore The reference value of the integral value is not great, so the I item is discarded, and only the P item and D item parameters in the PID are used to adjust the direction loop. After adjustment, an error Dev is output, and the error Dev is input into the motor loop. The two loops are kept synchronous and at the same frequency, that is, the target speed of the left and right wheels is the current speed of the left and right wheels plus or minus Dev, and PID control is performed separately. For the motor loop, the PID incremental algorithm is used. The PID incremental algorithm collects the actual speed through the encoder and compares it with the target speed to make a difference. The difference is passed into the PID operation. After the PID parameters are adjusted through experience, the output PWM can perfectly control the vehicle angle, so that it can stably and accurately drive along the predetermined straight line. ;

[0031] PID incremental algorithm is a commonly used algorithm in control. It has been widely used in industrial process control because of its simple control method, good stability, high reliability and easy on-site debugging. With output The relationship between can be expressed as: , in, is the PID output, is the proportionality coefficient, is the error, is the integration coefficient, is the integral of the error, is the differential coefficient, is the rate of change of error; The discretized PID formula can be expressed as: , in, For output, For the The deviation at the time of sampling is discretized into a position algorithm, from which the incremental algorithm is derived. The calculation formula of the incremental algorithm is: , in, For the output, please refer to the PID incremental algorithm diagram. Figure 8 ,like Figure 8 As shown in FIG. 1 , the incremental algorithm saves the deviation values ​​of the first three sampling moments, namely, the historical deviation, the current deviation, and the recent deviation, which not only saves resources but also does not produce a large accumulated error. The incremental algorithm can speed up the system response and has a higher control accuracy.

[0032] PWM technology adjusts the average voltage of the motor by changing the duty cycle of the pulse signal (that is, the proportion of the high level time to the entire cycle), thereby controlling the speed of the motor. The period of the PWM signal is T, and the duty cycle is D ( ,in is high level time), the average voltage of the motor It can be approximately expressed as the power supply voltage The product of the duty cycle D is , since the motors of the two wheels receive PWM signals with the same duty cycle, their average voltage will also be the same, ensuring that the speeds of the two wheels are proportional.

[0033] In some embodiments, the car is tested on the set trajectory, and the test requirements are: first, the car is placed at position A, and the car can automatically drive to point B and stop. There is an audible and visual prompt when parking, and the time is no more than 15 seconds; second, the car is placed at position A, and after the car can automatically drive to point B, it drives along the semi-arc to point C, and then automatically drives from point C to point D, and finally drives along the semi-arc to point A and stops. Every time a point is passed, there is an audible and visual prompt once, and it takes no more than 30 seconds to complete a circle; third, the car is placed at position A, and after the car can automatically drive to point C, it drives along the semi-arc to point B, and then automatically drives from point B to point D, and finally drives along the semi-arc to point A and stops. Every time a point is passed, there is an audible and visual prompt once, and it takes no more than 40 seconds to complete a circle; fourth, the car automatically drives 4 circles and stops along the path of the third point, and the less time the better; according to the test requirements, the number of seconds for the car to complete the trajectory is recorded, please refer to Table 1, Table 1

[0034] Please refer to Table 2 for the completion of the car task. Table 2

[0035] According to the experimental results, it can be seen that the car can accurately travel on the track along the prescribed route and the projection point just covers the given identification point ABCD, and gives sound and light feedback, with a quick response, accurate recognition results, and a short time.

[0036] In summary, the automatic driving control system and method of the trolley provided by the present invention are used to control the trolley to automatically drive on a set trajectory. The automatic driving control system of the trolley includes a sensor module, a main control module, a speed control module and a direction control module; the sensor module is used to identify the set trajectory and obtain the speed and driving direction of the trolley in real time; the main control module is used to generate a first drive control instruction and a second drive control instruction when the speed and driving direction of the trolley are not the target speed and the target direction; the speed control module is used to respond to the first drive control instruction and adjust the speed of the trolley to the target speed; the direction control module is used to calibrate the gyroscope of the sensor module using a zero-point calibration method, and use a Kalman filter to perform real-time prediction on the calibrated data, and then respond to the second drive control instruction to adjust the driving direction of the trolley to the target direction based on the direction ring-motor ring cascade PID, thereby improving control accuracy and stability.

[0037] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A car automatic driving control system, characterized in that: Used to control the car to automatically drive on the set track. The car automatic driving control system includes a sensor module, a main control module, a speed control module and a direction control module; The sensor module is used to identify the set trajectory and obtain the speed and driving direction of the car in real time; The main control module is used to generate a first drive control instruction and a second drive control instruction when the speed and the driving direction of the vehicle are not the target speed and the target direction; The speed control module is used to respond to the first drive control instruction and adjust the speed of the vehicle to a target speed; The direction control module is used to calibrate the gyroscope of the sensor module using a zero-point calibration method, and after using a Kalman filter to make real-time predictions on the calibrated data, respond to the second drive control instruction, and adjust the driving direction of the car to the target direction based on the direction ring-motor ring cascade PID.

2. The automatic driving control system of a small vehicle according to claim 1 is characterized in that: The sensor module includes an infrared tracking unit, a gyroscope sensor and a Hall encoder; The infrared tracking unit is used to identify a set driving trajectory, wherein the driving trajectory includes a black arc driving trajectory; The gyro sensor is used to obtain the rotation angle and angular velocity of the car in real time, and determine the driving direction of the car based on the rotation angle and angular velocity of the car; The Hall encoder is used to measure the rotation speed of the trolley motor, and the speed of the trolley is determined based on the rotation speed of the trolley motor.

3. The automatic driving control system of a small vehicle according to claim 1 is characterized in that: The main control module includes a main control unit of model MSPMOG3507; The main control unit is used to receive input signals from the sensor module, the speed control module and the direction control module, and output a first drive control signal and a second drive control signal after performing logic control processing on the input signals.

4. The automatic driving control system of a small vehicle according to claim 1 is characterized in that: The speed control module includes a PID control unit; The PID control unit is used to determine the speed error based on the acquired vehicle speed and the target speed, calculate the speed adjustment amount based on the speed error, dynamically adjust the duty cycle of the PWM signal through the PWM technology based on the speed adjustment amount, and generate a first drive control signal after inputting the duty cycle of the PWM signal into the main control module.

5. The automatic driving control system of a small vehicle according to claim 1 is characterized in that: The direction control module includes a zero point calibration unit, a Kalman filter unit and a direction ring-motor ring cascade PID control unit; The zero point calibration unit is used to perform yaw calibration on the gyroscope using a zero point calibration method; The Kalman filter unit is used to iteratively optimize the data collected by the calibrated gyroscope through the Kalman filter to obtain the optimal estimated value of the vehicle angle; The direction ring-motor ring cascade PID control unit is used to determine the direction error based on the optimal estimated value of the angle of the trolley and the target direction, input the direction error into the direction ring of the direction ring-motor ring cascade PID to obtain a corrected direction error, input the corrected direction error into the motor ring of the direction ring-motor ring cascade PID, perform PID operation on the corrected direction error through the motor ring, obtain the duty cycle of the PWM signal of the two-wheel differential of the trolley, input the duty cycle of the PWM signal of the two-wheel differential of the trolley into the main control module, and generate a second drive control signal.

6. The automatic driving control system of a small vehicle according to claim 5 is characterized in that: The method of iteratively optimizing the data collected by the gyroscope through the Kalman filter to obtain the angle prediction value of the car includes: Acquire an angular velocity value of a gyroscope, construct a state equation based on the angular velocity value of the gyroscope, and obtain a predicted value of a current angle based on the state equation; Defining covariance noise, and constructing a prediction covariance matrix based on the covariance noise; Calculating the Kalman gain based on the prediction covariance matrix, the observation matrix, and the measurement noise covariance matrix; The optimal state estimation is performed based on the Kalman gain and the predicted value of the current angle to obtain the optimal estimated value of the car angle, and the covariance matrix is ​​updated through the Kalman gain.

7. The automatic driving control system of a small vehicle according to claim 1 is characterized in that: It also includes an acoustic and optical feedback module and a motor drive module; The sound and light feedback module is used to emit sound and light feedback when the car travels to the marked point on the track; The motor drive module is used to control the rotation speed of the vehicle motor based on the first drive control signal and the second drive control signal of the main control module to adjust the speed and driving direction of the vehicle.

8. A method for controlling automatic driving of a small vehicle, characterized in that: The method is used to control a trolley to automatically travel on a set track, and is implemented by the trolley automatic travel control system according to any one of claims 1 to 7, comprising the following steps: Based on the sensor module, the set trajectory is identified and the speed and driving direction of the car are obtained in real time; Based on the main control module, when the speed and the driving direction of the vehicle are not the target speed and the target direction, a first driving control instruction and a second driving control instruction are generated; adjusting the speed of the vehicle to a target speed based on the speed control module responding to the first drive control instruction; Based on the direction control module, the gyroscope of the sensor module is calibrated by a zero-point calibration method, and the calibrated data is predicted in real time by a Kalman filter. Then, in response to the second drive control instruction, the driving direction of the car is adjusted to the target direction based on the direction ring-motor ring cascade PID.

9. The automatic driving control method of a small vehicle according to claim 8, characterized in that: The step of responding to the first driving control instruction and adjusting the speed of the vehicle to a target speed includes: The speed error is input into the PID control unit to obtain the speed adjustment amount, where the proportional term of the PID determines the acceleration or deceleration force according to the speed error, the steady-state error of the speed is eliminated through the integral term of the PID, and the differential term of the PID adjusts the control output according to the speed change rate; Based on the speed adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through PMW technology. After the duty cycle of the PWM signal is input into the main control module, a first drive control signal is generated, and the motor power of the car is controlled based on the first drive control signal. Based on the motor power of the car, the driving speed of the car is adjusted to the target speed through the motor drive module.

10. The automatic driving control method of a small vehicle according to claim 8, characterized in that: The step of responding to the second drive control instruction and adjusting the driving direction of the vehicle to the target direction based on the direction ring-motor ring cascade PID comprises: The direction error is input into the direction loop-motor loop cascade PID to obtain the direction adjustment amount, where the proportional term of the PID adjusts the direction strength according to the angle error, and the differential term of the PID adjusts the direction according to the rate of change of the angle error; Based on the direction adjustment amount, the duty cycle of the PWM signal is dynamically adjusted through PMW technology. After the duty cycle of the PWM signal is input into the main control module, a second drive control signal is generated. The motor drive module is controlled based on the second drive control signal, and the driving direction of the car is adjusted to the target direction through the motor drive module.

Images