Motion control method and system of road marking machine

Abstract

The invention provides a motion control method and system for a road marking machine, and the method comprises the steps: obtaining a steering wheel steering angle of a steering wheel when a steering instruction sent by the steering wheel of the road marking machine through a steering motor is received; determining a wheel steering angle in the road marking machine based on the steering wheel steering angle; calculating a wheel linear velocity of each front wheel based on the wheel steering angle; and converting the wheel linear speed of each front wheel into a motor rotating speed of a driving motor for controlling the front wheel, so as to control a preset steering wheel motor and the driving motor to drive the road marking machine to move based on the wheel steering angle and the motor rotating speed. By applying the method provided by the invention, the precision of the road marking machine in the moving process can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of road marking machine control, and particularly to a motion control method and system for a road marking machine. Background Art

[0002] With the rapid development of traffic construction, the road mileage is increasing continuously, and the requirements for the construction efficiency and quality of road markings are also rising day by day. The application scenarios of road marking machines are no longer limited to traditional roads, parking lots, etc., but also extended to various complex road conditions such as airport runways, highway ramps, and urban expressways. Different application scenarios put forward different requirements for the steering performance of road marking machines. For example, in mountain roads with many curves or narrow parking lot passages, the marking machine needs to be able to achieve more flexible and accurate steering.

[0003] In the related art, to meet these requirements, there are usually two steering methods for road marking machines: mechanical steering and hydraulic steering. Some small road marking machines or those with low requirements for steering accuracy use mechanical steering, which relies on manual operation and transmits force through mechanical components to achieve steering; while some large or heavy road marking machines use a hydraulic steering system, which provides pressure oil through a hydraulic pump and controls the actions of hydraulic cylinders or hydraulic motors through a control valve to achieve the steering of the steering wheels, with characteristics such as large steering force and stable operation, and is suitable for marking operations under heavy loads and complex road conditions.

[0004] However, the steering accuracy of road marking machines in the prior art is relatively low, and the stability and shock absorption ability are poor. Summary of the Invention

[0005] In view of this, the present invention provides a motion control method for a road marking machine, by which the accuracy of the road marking machine during motion can be improved.

[0006] A motion control method for a road marking machine, the method comprising: When receiving a steering instruction sent by a steering motor of a road marking machine, obtaining the steering angle of the steering wheel; Based on the steering angle of the steering wheel, determining the steering angle of the wheels of the road marking machine; Calculating the linear velocity of each front wheel in the road marking machine based on the steering angle of the wheels; Converting the linear velocity of each front wheel into the motor speed of the drive motor controlling the front wheel; Based on the steering angle of the wheels and the motor speed, controlling a preset steering wheel motor and drive motor to drive the road marking machine to move.

[0007] For the above method, optionally, determining the wheel steering angle of the road marking machine based on the steering wheel steering angle includes: Performing dead zone filtering on the steering wheel steering angle according to a preset dead zone threshold; After passing the dead zone filtering, obtaining the maximum steering wheel steering angle and the maximum wheel steering angle of the road marking machine; Calculating the wheel steering angle of the road marking machine based on the steering wheel steering angle, the maximum steering wheel steering angle, and the maximum wheel steering angle.

[0008] For the above method, optionally, calculating the wheel linear speed of each front wheel in the road marking machine based on the wheel steering angle includes: Determining the rotation direction of the steering wheel; Applying a preset first linear speed formula to calculate the wheel linear speed of the front wheel close to the rotation direction side; Applying a preset second linear speed formula to calculate the wheel linear speed of the front wheel far from the rotation direction side; Wherein, the first linear speed formula is: ; The second linear speed formula is: ; Wherein, is the linear speed of the road marking machine, L is the wheelbase, W is the track width, k is the tire slip correction coefficient, is the wheel steering angle.

[0009] For the above method, optionally, before obtaining the steering wheel steering angle of the steering wheel, the method further includes: Sending communication requests to each motor of the road marking machine, and each of the motors at least includes the steering wheel motor, the steering motor, and the driving motor; When receiving the response messages fed back by each motor based on the communication requests, initializing the road marking machine.

[0010] For the above method, optionally, after initializing the road marking machine, the method further includes: Judging whether the motion control system of the road marking machine is calibrated; In the case where the motion control system is not calibrated, re-initializing the road marking machine; If the initialization fails or the motion control system is still not calibrated after initialization, sending out a warning message.

[0011] Optionally, before determining the steering angle of the wheels of the road marking machine, the method further includes: Based on the steering instruction, determine the rotation type when the steering wheel rotates. The rotation type is manual control rotation or automatic return rotation; When the rotation type is manual control rotation, calculate the feedback torque of the road marking machine based on the steering angle of the steering wheel, and send the feedback torque to the steering motor of the road marking machine; When the rotation type is automatic return rotation, detect the current position of the steering wheel motor; determine the actual steering angle of the road marking machine based on the current position; calculate the difference between the actual steering angle and the steering angle of the steering wheel; based on the difference and applying a preset PID control algorithm, calculate the return torque of the steering wheel; filter the return torque, and superimpose the filtered return torque on the feedback torque output by the previous manual control rotation of the steering wheel to obtain a superimposed torque; send the superimposed torque to the steering motor of the road marking machine.

[0012] Optionally, the calculating the feedback torque of the road marking machine based on the steering angle of the steering wheel includes: When the steering angle of the steering wheel is greater than a preset angle threshold, apply a preset knowledge smoothing transition formula to calculate the feedback torque of the road marking machine; When the steering angle of the steering wheel is not greater than the angle threshold, apply a preset linear formula based on the steering stiffness coefficient and the steering damping coefficient to calculate the feedback torque of the road marking machine.

[0013] A motion control system of a road marking machine includes: A steering motor, a steering wheel motor, a driving motor and a controller; The steering motor is used to send a steering instruction to the controller when the steering wheel of the road marking machine rotates; The steering wheel motor is used to control the driving direction of the road marking machine; The driving motor is used to drive the road marking machine to move; The controller is used to execute the above-mentioned motion control method of the road marking machine.

[0014] Optionally, for the above-mentioned motion control system of the road marking machine, each wheel of the road marking machine includes a set of front wheels and a set of rear wheels; The steering wheel motor is arranged between the set of rear wheels; The steering wheel motor controls the driving direction of the road marking machine, and specifically is used to directly control the set of rear wheels to steer, and control the set of front wheels to steer through the driving motor.

[0015] For the above motion control system of the road marking machine, optionally, each of the front wheels in the set of front wheels is respectively connected to a driving motor; Wherein, the driving motor drives the road marking machine to move, specifically for: driving the front wheel connected thereto to move, so that the front wheel drives the rear wheel on the same side as it to move when moving.

[0016] Compared with the prior art, the present invention has the following advantages: The present invention provides a motion control method for a road marking machine, and the method includes: when receiving a steering instruction sent by the steering wheel of the road marking machine through the steering motor, obtaining the steering angle of the steering wheel; determining the wheel steering angle of the road marking machine based on the steering angle of the steering wheel; calculating the wheel linear speed of each front wheel in the road marking machine based on the wheel steering angle; converting the wheel linear speed of each front wheel into the motor speed of the driving motor controlling the front wheel; and controlling the preset steering wheel motor and driving motor to drive the road marking machine to move based on the wheel steering angle and the motor speed. By applying the method provided by the present invention, the accuracy of the road marking machine during movement can be improved. Description of the Drawings

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to the provided drawings without creative efforts.

[0018] Figure 1 It is a schematic structural diagram of a motion control system of a road marking machine provided by an embodiment of the present invention; Figure 2 It is a schematic structural diagram of a road marking machine provided by an embodiment of the present invention; Figure 3 It is a top view of a road marking machine provided by an embodiment of the present invention; Figure 4 It is a method flow chart of a motion control method for a road marking machine provided by an embodiment of the present invention; Figure 5 It is another method flow chart of a motion control method for a road marking machine provided by an embodiment of the present invention; Figure 6 It is still another method flow chart of a motion control method for a road marking machine provided by an embodiment of the present invention. Detailed Embodiments

[0019] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0020] In this application, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. The terms "comprising", "including" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements but also other elements not specifically listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of additional identical elements in the process, method, article or device including the element.

[0021] The present invention can be used in many general or special computing device environments or configurations. For example: personal computers, server computers, handheld or portable devices, tablet devices, multi-processor devices, distributed computing environments including any of the above devices or equipment, and so on.

[0022] An embodiment of the present invention provides a motion control method for a road marking machine. The method is mainly applied to the controller of the motion control system of the road marking machine. The motion control system of the road marking machine can be an ABS (Antilock Braking System), and the structural schematic diagram of the motion control system is as Figure 1 shown, and specifically includes: a steering motor, a steering wheel motor, a drive motor, and a controller.

[0023] Among them, referring to Figure 2 , Figure 2 is the structural diagram of the road marking machine. In the road marking machine, the steering wheel is connected to the steering motor through a bearing. The steering motor is used to send a steering command to the controller when the steering wheel of the road marking machine rotates; The road marking machine includes a set of front wheels and a set of rear wheels. The tire diameter of the front wheels is larger than that of the rear wheels. The steering wheel motor is arranged between a set of rear wheels (as Figure 3The top view of the road marking machine shown), which is used to control the driving direction of the road marking machine. Specifically, it directly controls a set of rear wheels to turn, and controls a set of front wheels to turn through a drive motor. A corresponding drive motor is provided for each front wheel of the road marking machine (such as Figure 3 The top view of the road marking machine shown), which is used to drive the road marking machine to move. Specifically, it drives the front wheel connected to it to move, so that the front wheel drives the rear wheel on the same side as it when moving.

[0024] The controller is electrically connected to the steering motor, the steering wheel motor and the drive motor, and is used to execute the motion control method of the road marking machine. The method flow chart of the motion control method of this road marking machine is as Figure 4 shown, specifically including: S10: When receiving a steering instruction sent by the steering wheel of the road marking machine through the steering motor, obtain the steering angle of the steering wheel.

[0025] When the user turns the steering wheel or the road marking machine automatically returns to the correct position, the speed, torque, etc. of the steering motor connected to the steering wheel in the road marking machine change, and a steering instruction related to the change in speed, torque, etc. is sent to the controller. The controller obtains the steering angle of the steering wheel through the steering instruction. The steering motor of the road marking machine is used to feedback the steering wheel angle and provide force feedback.

[0026] S11: Based on the steering angle of the steering wheel, determine the wheel steering angle of the road marking machine.

[0027] During the driving process of the road marking machine, turning the steering wheel drives the road marking machine to turn. After obtaining the steering angle of the steering wheel, it is necessary to convert the steering angle of the steering wheel into the wheel steering angle of the rear wheels in the road marking machine, so as to control the turning angle of the road marking machine through the wheel steering angle.

[0028] Since the steering wheel motor in the road marking machine is arranged between the rear wheels, this wheel steering angle is the steering angle for the controller to control the steering wheel motor to drive the rear wheels to turn.

[0029] There is a preset steering ratio between the steering angle of the steering wheel and the wheel steering angle. Through this steering ratio, the wheel steering angle of the wheel can be calculated according to the steering angle of the steering wheel.

[0030] S12: Calculate the wheel linear speed of each front wheel in the road marking machine based on the wheel steering angle.

[0031] During the turning process of the road marking machine, the wheels on the left and right sides of the road marking machine need to be controlled at different linear speeds to ensure that the road marking machine can turn smoothly.

[0032] In the road marking machine of the present invention, a corresponding drive motor is provided for each front wheel, and the drive motor drives the wheel connected thereto to move.

[0033] Among them, the calculation process of the wheel linear velocity is related not only to the wheel steering angle, but also to the wheel track, the wheel wheelbase and the current speed of the road marking machine. The wheel track refers to the center distance between the left and right wheels; the wheel wheelbase refers to the center distance between the front and rear wheels.

[0034] S13: Convert the wheel linear velocity of each front wheel into the motor speed of the drive motor controlling the front wheel.

[0035] Each wheel of the road marking machine is respectively connected to a drive motor, and the controller controls each drive motor to drive the wheels to perform differential control when the road marking machine turns. The steering wheel motor is used to drive each wheel to turn according to the wheel steering angle, and the drive motor is used to drive the wheel connected to the drive motor to move at the wheel corresponding linear velocity according to the corresponding motor speed.

[0036] S14: Based on the wheel steering angle and the motor speed, control the steering wheel motor and the drive motor to drive the road marking machine to move.

[0037] The controller of the road marking machine is connected to multiple motors, including at least a steering motor, a steering wheel motor and a drive motor. Among them, the controller detects whether the steering wheel rotates and the steering wheel steering angle when it rotates through the steering motor; the controller is used to control the steering wheel motor to drive the wheels to turn according to the wheel steering angle; the controller is used to control the drive motor to drive the wheel connected to it to move at the corresponding linear velocity of the wheel. Therefore, if the road marking machine needs to turn, the controller controls the steering wheel motor of the road marking machine to drive the wheels to turn according to the wheel steering angle while controlling the drive motor to drive the wheels to move at the corresponding motor speed.

[0038] The drive motor of the present invention can communicate with the controller through the CANopen communication protocol. The steering wheel motor has CANopen communication and an absolute encoder.

[0039] An embodiment of the present invention provides a motion control method for a road marking machine, which realizes differential control of each wheel when the road marking machine turns through a controller. When the controller detects a change in parameters such as the rotational speed or torque of the steering motor, it obtains the steering angle of the steering wheel. According to a preset steering ratio, the wheel steering angle can be deduced from the steering angle of the steering wheel. Since the speeds required by the left and right wheels are different when the road marking machine turns, after obtaining the wheel steering angles of each wheel, the linear velocity of each wheel is calculated respectively to achieve differential control of different wheels. In a road marking machine, the drive motor is the power source for the road marking machine to travel. Each wheel of the road marking machine is provided with a corresponding drive motor, which determines the acceleration performance, maximum speed and cruising range of the road marking machine. After obtaining the linear velocity of each wheel, the linear velocity is converted into the motor rotational speed for the drive motor to control the wheel, so as to drive the wheel to move according to its corresponding linear velocity of the wheel according to the motor rotational speed.

[0040] The present invention mainly obtains the linear velocity of each wheel by deducing the wheel steering angle from the steering angle of the steering wheel after detecting the rotation of the steering wheel. Then, after converting the linear velocity of the wheel into the motor rotational speed of the drive motor, the drive motor is controlled to drive the wheel to move according to the motor rotational speed, realizing automatic differential control, so that the road marking machine can be smoother when turning, and at the same time, the safety of the wheel during application can be guaranteed.

[0041] In one embodiment, based on the steering angle of the steering wheel, the method for determining the wheel steering angle corresponding to each wheel in the road marking machine is as follows: Dead zone filtering is performed on the steering angle of the steering wheel according to a preset dead zone threshold; after passing through the dead zone filtering, the maximum steering angle of the road marking machine and the maximum steering angle of the wheel are obtained; based on the steering angle of the steering wheel, the maximum steering angle of the steering wheel and the maximum steering angle of the wheel, the wheel steering angle of the road marking machine is calculated.

[0042] Among them, the dead zone threshold is a steering angle range that ignores the slight jitter of the steering wheel. When it is detected that the steering wheel turns, it is necessary to pre-judge whether the steering of the steering wheel is effective according to the dead zone threshold. Performing dead zone filtering on the steering angle of the steering wheel according to the dead zone threshold means judging whether the steering angle of the steering wheel is greater than the dead zone threshold. If so, it passes through the dead zone filtering and continues to calculate the wheel steering angle; otherwise, this steering of the steering wheel is ignored.

[0043] The steering angle of the steering wheel and the wheel steering angle can be converted through the steering transmission ratio. Through the maximum steering angle of the steering wheel and the maximum steering angle of the wheel, the steering transmission ratio can be obtained, and then according to the steering transmission ratio and the current steering angle of the steering wheel, the wheel steering angle can be obtained.

[0044] The calculation formula of the steering transmission ratio is: ; The calculation formula for the wheel steering angle is: ; Wherein, is the maximum wheel steering angle, is the maximum steering wheel steering angle, is the current steering wheel steering angle.

[0045] After the embodiment of the present invention detects that the steering wheel rotates, dead zone processing is pre - performed to determine whether the rotation of the steering wheel belongs to a slight jitter. For example, when an operator accidentally touches the steering wheel and causes the steering wheel to rotate by 2°, at this time, the steering wheel steering angle of the steering wheel belongs to the dead zone threshold (0, ±5°], and the slight jitter of the steering wheel can be ignored. In the case of not being a slight jitter, first determine the steering transmission ratio of the road marking machine according to the maximum steering wheel steering angle and the maximum wheel steering angle, and then combine the steering transmission ratio and the current steering wheel steering angle to convert and obtain the wheel steering angle.

[0046] By means of dead zone processing and calculating the steering transmission ratio, the present invention can avoid the steering of the steering wheel due to slight jitter, and at the same time, can ensure the accurate conversion between the steering wheel steering angle and the wheel steering angle, can easily achieve precise steering of a small angle, effectively avoid collision accidents, and improve the operation safety and efficiency.

[0047] In an embodiment of the present invention, during the turning process of the road marking machine, it is necessary to reasonably match the speeds of each wheel to ensure the stable driving of the road marking machine. Therefore, the process of calculating the wheel linear speed corresponding to each wheel based on the wheel steering angle corresponding to each wheel is as follows: Determine the rotation direction of the steering wheel; apply a preset first linear speed formula to calculate the wheel linear speed of the front wheel on the side close to the rotation direction; apply a preset second linear speed formula to calculate the wheel linear speed of the front wheel on the side far from the rotation direction; wherein, the first linear speed formula is: ; The second linear speed formula is: ; Wherein, is the linear speed of the road marking machine of the road marking machine, L is the wheelbase, W is the track width, k is the tire slip correction coefficient, is the wheel steering angle.

[0048] The controller is connected to the handle of the road marking machine, reads the speed signal input by the handle according to the J1939 protocol, and converts it into the linear speed of the road marking machine according to the input speed signal. After obtaining the linear speed of the road marking machine, the linear speed of the road marking machine can also be limited to ensure that the linear speed is within a reasonable range. When the handle of the road marking machine communicates with the controller, by following the J1939 protocol, it is convenient for the driver to input control instructions such as speed.

[0049] The controller can obtain the turning direction required by the road marking machine through the steering wheel steering angle (for example: when the steering wheel steering angle is -50°, it means the road marking machine needs to turn to the left; when the steering wheel steering angle is +50°, it means the road marking machine needs to turn to the right). The wheel on the side close to the turning direction is the inner wheel, and the wheel on the side far from the turning direction is the outer wheel. Since the driving paths between the two wheels are inconsistent, in order to ensure the smooth turning of the road marking machine and reduce tire wear, it is necessary to calculate the required wheel linear speed of each wheel by combining the linear speed of the road marking machine, the wheelbase, the track width between the wheels, the tire slip correction coefficient, and the wheel steering angle. The present invention fully considers the factors of tire slip and steering angle, realizes the reasonable speed distribution of the left and right front wheels, not only improves the acceleration performance and driving stability of the road marking machine, but also reduces tire wear during turning, reduces energy consumption, and extends the battery life of the road marking machine.

[0050] In the method provided by the embodiment of the present invention, to ensure the safety of the road marking machine during movement, when the movement control system of the road marking machine starts, initialization processing needs to be performed, referring to Figure 5 , and the specific process of the initialization process is as follows: S20: Send a communication request to each motor of the road marking machine.

[0051] Each of the motors at least includes a steering wheel motor, a steering motor, and a driving motor.

[0052] The controller and each motor of the present invention all belong to the movement control system of the wheels.

[0053] S21: Whether each motor responds to the communication request.

[0054] When receiving the response message fed back by each motor based on the communication request, execute S22. When not receiving the response message fed back by each motor based on the communication request, execute S25 to issue a warning message.

[0055] S22: Initialize the road marking machine.

[0056] Among them, initializing the road marking machine means setting initial values for each system parameter of the motion control system. For example: setting the initial steering angle of the steering wheel motor to be less than 1°, setting the maximum current, the highest temperature, the lowest battery power, etc.

[0057] S23: Determine whether the initialization is successful.

[0058] If so, execute S24; otherwise, execute S25.

[0059] S24: When the initialization is successful, determine whether the motion control system of the road marking machine is calibrated.

[0060] That is, determine whether each system parameter of the motion control system is the initial value. If so, end the initialization process; otherwise, execute S22 again.

[0061] S25: If the initialization fails or the motion control system is still not calibrated after N initializations, send a warning message.

[0062] In the embodiment of the present invention, when powering on, the initialization function (INITIALIZE) is executed. First, CAN communication detection is performed. The CANopen_TestConnection function is used to detect whether the CAN communication connections of the drive motor, the steering wheel motor, and the steering motor are normal respectively. If any connection is abnormal, the communication error flag (bCommError) is set to TRUE, and the motion control system enters the emergency stop process; if the communication is normal, then the zero point of the steering wheel motor is read. The CANopen_SDO_Read function is used to read the encoder position of the steering wheel motor, and the initial value of the rear wheel steering angle is calculated according to the encoder resolution (ENCODER_RES is 131072). It is also determined whether the motion control system has been calibrated (when ABS(fSteerRearAngle)<0.01745, that is, less than 1°, it is considered calibrated, and bIsHomed is set to TRUE). At the same time, the controller and the filter are reset to prepare for subsequent normal operation. In the main loop (MAIN_CYCLE, with a period of 20 ms), first, it is checked whether the motion control system has been calibrated. If not, initialization is attempted again. If the initialization fails, an early warning (EMERGENCY_STOP) is triggered; then, the safety status (judged by the Safety.Check function based on parameters such as the set maximum current of 40.0, the maximum temperature of 80.0, and the minimum battery power of 20.0) and the communication status are checked. If there are any abnormalities, an emergency stop is also triggered. If everything is normal, the user can control the road marking machine to start driving through the handle and the steering wheel. After the controller detects the speed signal of the handle, according to the speed signal, it controls the road marking machine to travel at the linear speed of the road marking machine corresponding to the speed signal. After detecting the steering signal of the steering wheel, the processing corresponding to S10 - S14 above is executed, which will not be elaborated here.

[0063] In the method provided by the embodiment of the present invention, during the rotation of the steering wheel, to avoid the user accidentally touching the steering wheel, it is necessary to feedback the torque of the steering wheel to the user. Therefore, before determining the wheel steering angle corresponding to each wheel in the road marking machine, it is also necessary to calculate the force feedback to the user. Refer to Figure 6 , and the process of feedback torque is as follows: S30: Based on the steering instruction, determine the rotation type when the steering wheel rotates.

[0064] Among them, the rotation type is manual control rotation or automatic return rotation. If it is manual control rotation, execute S31 - S32; if it is automatic return control, execute S33 - S37.

[0065] S31: When the rotation type is manual control rotation, calculate the feedback torque of the road marking machine based on the steering wheel steering angle, and continue to execute S32.

[0066] Among them, the method for calculating the feedback torque of the road marking machine is as follows: (1) When the steering angle of the steering wheel is greater than the preset angle threshold, apply the preset knowledge smoothing transition formula to calculate the feedback torque of the road marking machine; The knowledge smoothing transition formula is: ； Where, is the maximum torque in the normal area, is the maximum torque of the motion control system, is the attenuation coefficient, represents the relative distance between the current angle and the "normal - limit" switching point (80% of the maximum angle), and the absolute value is taken to ensure left - right symmetry.

[0067] (2) When the steering angle of the steering wheel is not greater than the angle threshold, apply the preset linear formula based on the steering stiffness coefficient and the steering damping coefficient to calculate the feedback torque of the road marking machine; Taking the angle threshold as 80% as an example, the linear formula is: ; Where, is the steering stiffness coefficient, is the steering damping coefficient, represents the steering wheel rotation speed.

[0068] S32: Send the feedback torque to the steering motor of the road marking machine.

[0069] Among them, when the user manually controls the rotation of the steering wheel, the steering motor provides force feedback to the user based on the feedback torque.

[0070] S33: When the rotation type is automatic return rotation, determine the actual steering angle of the road marking machine.

[0071] Among them, the actual steering angle is obtained by real - time measurement according to the angle sensor on the road marking machine, and the angle sensor sends the measured actual steering angle to the controller.

[0072] S34: Calculate the difference between the actual steering angle and the steering wheel steering angle.

[0073] The difference is: ; Where, is the actual steering angle.

[0074] S35: Based on the difference and applying the preset PID control algorithm, calculate the return torque of the steering wheel.

[0075] The calculation formula of the PID control algorithm is as follows: ; Among them, is the proportional coefficient, which is used to amplify or reduce the error value at the current moment ; is the difference at the current moment; is the integral coefficient, which is used to integrate the error over time (from the start time to the current time t). Its main function is to eliminate the steady-state error of the system. As long as there is an error, the adjustment amount will continue to accumulate. However, if the integral action is too strong, it may slow down the system response and increase the overshoot; is the error at different times; is the differential coefficient, which is used to adjust the output according to the change rate of the error to predict the change trend of the error and make adjustments in advance, which can reduce the overshoot, improve the system stability, and suppress the system oscillation; is the change rate of the error, which is used to represent how fast the error changes with time and reflects the change trend of the error.

[0076] S36: Filter the return torque and superimpose the filtered return torque on the feedback torque of the previous manual control rotation output of the steering wheel to obtain the superimposed torque.

[0077] Specifically, the double-stage EMA filtering method is used to filter the return torque.

[0078] S37: Send the superimposed torque to the steering motor of the road marking machine.

[0079] Among them, when the steering wheel automatically returns to the straight position, the steering motor provides force feedback to the user based on the superimposed torque.

[0080] In the method provided by the embodiments of the present invention, the way of processing the steering wheel includes reading the steering wheel angle, dead zone processing, calculating the rear-wheel steering angle, generating a dynamic force feedback torque (using different calculation methods according to different steering wheel angles, using an exponential smoothing transition formula when approaching the maximum angle, and using a linear formula based on the steering stiffness coefficient STEER_KP and the steering damping coefficient STEER_KD in other cases), and sending the feedback torque; performing auto-centering control (AUTO_CENTER), reading the original user torque and performing double-stage EMA filtering (the filtering coefficients are EMA_ALPHA1 = 0.3 and EMA_ALPHA2 = 0.2 respectively), dynamically adjusting the PID parameters (return proportional gain RETURN_KP and return derivative gain RETURN_KD) according to the filtered torque, triggering auto-centering when certain speed and torque conditions are met (fActualSpeed > 0.2 and ABS(fUserTorqueFiltered) < TORQUE_THRESHOLD (torque threshold (the maximum output of the motor)), TORQUE_THRESHOLD is 0.3 N·m), adjusting the return speed, obtaining the return torque through PID calculation and double-stage EMA filtering, and superimposing it on the force feedback torque; then performing rear-wheel steering control (CONTROL_STEER), calculating the target position of the steering wheel motor (nTargetPos) according to the rear-wheel steering angle and relevant mechanical parameters, and sending a control command through the CANopen_SDO_Write function; finally, performing drive differential control (CONTROL_DRIVE), calculating the rotational speeds of the left and right drive motors according to the above kinematic principle, combining with the dynamic speed limit mechanism, sending the speed command to the drive motor, and updating the actual speed (fActualSpeed) of the road marking machine at the same time.

[0081] By applying the method provided by the embodiments of the present invention, through the accurate conversion between the steering wheel motor angle and the rear-wheel steering angle, and the fine adjustment of the steering wheel force feedback, the driver can obtain a more real and accurate handling feel. In complex operation scenarios such as narrow channels and cargo stacking, it can easily achieve precise steering at a small angle, effectively avoid collision accidents, and improve operation safety and efficiency. In the case of the automatic centering of the road marking machine, by using dynamic PID and double EMA filtering technologies, the return force and speed can be automatically adjusted according to the driver's operation intention. When the driver releases the hand, the road marking machine can quickly and smoothly return to the straight driving state, adapt to different vehicle speeds and working conditions, and further improve the handling convenience and driving safety.

[0082] Each embodiment in this specification is described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for a system or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiment. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without creative work.

[0083] Those skilled in the art can further realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of both.

[0084] To clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described according to functions in the above description. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

[0085] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be obvious to those skilled in the art. The general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but will be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A motion control method for a road marking machine, characterized in that, The method includes: When receiving a steering instruction sent by a steering motor of a road marking machine for the steering wheel, obtaining the steering angle of the steering wheel; Based on the steering angle of the steering wheel, determining the wheel steering angle of the road marking machine; Calculating the wheel linear speed of each front wheel in the road marking machine based on the wheel steering angle; Converting the wheel linear speed of each of the front wheels into the motor speed of the drive motor controlling the front wheel; Based on the wheel steering angle and the motor speed, controlling a preset steering wheel motor and drive motor to drive the road marking machine to move.

2. The method according to claim 1, wherein The determining the wheel steering angle of the road marking machine based on the steering angle of the steering wheel includes: Performing dead zone filtering on the steering angle of the steering wheel according to a preset dead zone threshold; After passing the dead zone filtering, obtaining the maximum steering angle of the steering wheel and the maximum wheel steering angle of the road marking machine; Calculating the wheel steering angle of the road marking machine based on the steering angle of the steering wheel, the maximum steering angle of the steering wheel, and the maximum wheel steering angle.

3. The method according to claim 1, characterized in that, The calculating the wheel linear speed of each front wheel in the road marking machine based on the wheel steering angle includes: Determining the rotation direction of the steering wheel; Applying a preset first linear speed formula to calculate the wheel linear speed of the front wheel close to the rotation direction side; Applying a preset second linear speed formula to calculate the wheel linear speed of the front wheel away from the rotation direction side; Wherein, the first linear speed formula is: ； The second linear speed formula is: ； Among them, is the linear speed of the road marking machine of the road marking machine, L is the wheelbase, W is the track width, k is the tire slip correction coefficient, is the wheel steering angle.

4. The method according to claim 1, wherein Before obtaining the steering angle of the steering wheel, the method further includes: Sending a communication request to each motor of the road marking machine, and each of the motors at least includes the steering wheel motor, the steering motor, and the drive motor; When receiving a response message fed back by each of the motors based on the communication request, initializing the road marking machine.

5. The method according to claim 4, wherein After initializing the road marking machine, the method further includes: Judging whether the motion control system of the road marking machine is calibrated; In the case that the motion control system is not calibrated, re-initializing the road marking machine; If the initialization fails or the motion control system is still not calibrated after initialization, sending out a warning message.

6. The method according to claim 1, wherein Before determining the wheel steering angle of the road marking machine, the method further includes: Based on the steering instruction, determining the rotation type when the steering wheel rotates, and the rotation type is manual control rotation or automatic return rotation; When the rotation type is manual control rotation, calculating the feedback torque of the road marking machine based on the steering angle of the steering wheel, and sending the feedback torque to the steering motor of the road marking machine. When the rotation type is auto - return rotation, detect the current position of the steering wheel motor; determine the actual steering angle of the road marking machine based on the current position; calculate the difference between the actual steering angle and the steering wheel steering angle; calculate the return torque of the steering wheel based on the difference and applying a preset PID control algorithm; filter the return torque and superimpose the filtered return torque on the feedback torque output by the last manual control rotation of the steering wheel to obtain a superimposed torque; send the superimposed torque to the steering motor of the road marking machine.

7. The method according to claim 6, wherein The calculating of the feedback torque of the road marking machine based on the steering wheel steering angle includes: When the steering wheel steering angle is greater than a preset angle threshold, calculate the feedback torque of the road marking machine by applying a preset knowledge smoothing transition formula; When the steering wheel steering angle is not greater than the angle threshold, calculate the feedback torque of the road marking machine by applying a preset linear formula based on the steering stiffness coefficient and the steering damping coefficient.

8. A motion control system for a road marking machine, characterized in that, It includes: A steering motor, a steering wheel motor, a driving motor and a controller; The steering motor is used to send a steering command to the controller when the steering wheel of the road marking machine rotates; The steering wheel motor is used to control the driving direction of the road marking machine; The driving motor is used to drive the road marking machine to move; The controller is used to execute the motion control method of the road marking machine according to any one of claims 1 - 7 above.

9. The motion control system of the road marking machine according to claim 8, wherein Each wheel of the road marking machine includes a set of front wheels and a set of rear wheels; The steering wheel motor is arranged between the set of rear wheels; The steering wheel motor controls the driving direction of the road marking machine, specifically used for: directly controlling the set of rear wheels to turn, and controlling the set of front wheels to turn through the driving motor.

10. The motion control system of the road marking machine according to claim 9, characterized in that, Each of the front wheels in the set of front wheels is respectively connected to a driving motor; Wherein, the driving motor drives the road marking machine to move, specifically used for: driving the front wheel connected to it to move, so that the front wheel drives the rear wheel on the same side as it to move when moving.

Images