Method and device for preventing robot from swinging left and right and computer equipment

Abstract

The invention discloses a method and device for preventing a robot from swinging leftwards and rightwards and computer equipment, and the method comprises the following steps: carrying out speed sampling in a dynamic speed window at a fixed step length, judging each group of speed obtained by sampling, and setting the angular speed to be zero when the absolute value of the angular speed is smaller than a set threshold value; speed combinations causing swinging of the robot are eliminated through constraint on centrifugal force; and trajectory prediction is carried out on all the selected speed combinations, each trajectory is scored through an evaluation function, and the speed combination corresponding to the trajectory with the highest score is output to control movement of the robot. According to the method, through the multi-dimensional optimization dwa algorithm, the problem of motion swing of the robot is effectively solved on the premise that hardware upgrading is not needed, and the motion stability of the robot is remarkably improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular to a method, a device and a computer device for preventing a robot from swaying left and right. Background Art

[0002] The DWA controller is a commonly used local planning algorithm for mobile robots. It directly outputs velocity commands, making it suitable for use as a controller. However, the DWA algorithm itself lacks control rate convergence. This is because it lacks a complete kinematic model and only uses the kinematic model for trajectory prediction, which clearly lacks control convergence.

[0003] When the robot uses the DWA algorithm to track the global trajectory under full load, due to the heavy weight and inertia of the vehicle body, the linear velocity is high. Due to the defects of DWA itself, the speed corresponding to the optimal trajectory selected in real time is always in dynamic change, resulting in angular velocity oscillation. Angular velocity oscillation will cause the robot to sway left and right in an S-shape, and this situation will become more and more obvious once it occurs.

[0004] Existing technologies typically increase power by adjusting the motor's response speed or replacing it with a higher-torque motor, or by replacing the tires to increase friction. However, these methods are not adaptable to the environment, especially on smooth surfaces. The robot's side-to-side swaying is more pronounced during operation, affecting both the robot's operation and the user's experience. Summary of the Invention

[0005] In view of the above-mentioned deficiencies in the prior art, the technical problem to be solved by the present invention is to provide a method, device and computer equipment that can effectively solve the problem of robot swinging under the DWA algorithm and prevent the robot from swaying left and right.

[0006] To solve the above technical problems, the present invention adopts a technical solution: providing a method for preventing a robot from swaying left and right, comprising the following steps:

[0007] The speed is sampled in the dynamic speed window with a fixed step size, and each group of speeds obtained by sampling is judged. When the absolute value of the angular velocity is less than the set threshold, the angular velocity is set to zero;

[0008] By constraining the centrifugal force, we can eliminate the velocity combination that causes the robot to swing;

[0009] The trajectory of all selected speed combinations is predicted and each trajectory is scored through the evaluation function. The speed combination corresponding to the trajectory with the highest score is output to control the movement of the robot.

[0010] Further, the fixed step length includes a first fixed step length and a second fixed step length;

[0011] In the step of performing speed sampling in the dynamic speed window at a fixed step length, judging each group of sampled speeds, and setting the angular velocity to zero when the absolute value of the angular velocity is less than the set threshold, the following sub-steps are included:

[0012] Calculate the sampling range of the linear velocity according to the current linear velocity and the maximum linear acceleration, and calculate the sampling range of the angular velocity according to the current angular velocity and the maximum angular acceleration. The sampling range of the linear velocity and the sampling range of the angular velocity form a dynamic speed window;

[0013] Sample in the sampling range of the linear velocity at a preset first fixed step length, and sample in the sampling range of the angular velocity at a preset second fixed step length to obtain a number of speed combinations;

[0014] Traverse all speed combinations, judge whether the absolute value of the angular velocity in the speed combination is less than the set angular velocity threshold. If it is less than the set angular velocity threshold, set the angular velocity to 0. Otherwise, the angular velocity remains unchanged.

[0015] Further, in the step of excluding the speed combinations that cause the robot to swing by constraining the centrifugal force, the following sub-steps are included:

[0016] Calculate the path curvature of the current position according to the preset global trajectory;

[0017] Calculate the maximum centrifugal force at the current position according to the path curvature;

[0018] Calculate the centrifugal force of each group of speed combinations and judge whether the centrifugal force is greater than the maximum centrifugal force. If the centrifugal force is greater than the maximum centrifugal force, exclude the speed combination. Otherwise, retain the speed combination.

[0019] Further, the calculation formula of the maximum centrifugal force is as follows:

[0020] Fc max = m * v1 2 / r(one)

[0021] ()In formula (one), Fc max represents the maximum centrifugal force, m represents the mass of the robot, v represents the desired linear velocity, and r represents the current turning radius, which is obtained by differentiating the path curvature of the current position.

[0022] Further, when calculating the centrifugal force of each group of speed combinations, the formula used is as follows:

[0023] Fc = m * v * w (two)

[0024] In formula (2), Fc represents the centrifugal force corresponding to the speed combination, m represents the mass of the robot, v represents the linear velocity in the speed combination, and w represents the angular velocity in the speed combination.

[0025] Furthermore, after the steps of predicting trajectories for all selected speed combinations and scoring each trajectory using an evaluation function, and outputting the speed combination corresponding to the trajectory with the highest score to control the movement of the robot, the following steps are also included:

[0026] Set the attenuation function to dynamically adjust the output linear velocity based on the robot's current linear velocity and angular velocity. Furthermore, the preset attenuation function is as follows:

[0027] v new =v'*e -k*|w' | (Three)

[0028] In formula (3), v new represents the adjusted linear velocity, v' represents the current linear velocity, w' represents the current angular velocity, and k represents the attenuation coefficient, which is 0.75.

[0029] In order to solve the above technical problems, another technical solution adopted by the present invention is to provide a device for preventing the robot from swaying left and right, comprising:

[0030] The sampling module is used to sample the speed in the dynamic speed window with a fixed step size, judge each group of speeds obtained by sampling, and set the angular velocity to zero when the absolute value of the angular velocity is less than the set threshold;

[0031] An elimination module, used for eliminating the velocity combination causing the robot to swing by constraining the centrifugal force;

[0032] The trajectory prediction module is used to predict the trajectories of all selected speed combinations and score each trajectory through the evaluation function, and output the speed combination corresponding to the trajectory with the highest score to control the movement of the robot.

[0033] To solve the above technical problems, another technical solution adopted by the present invention is: providing a computer device, including a memory and a processor, the memory storing a computer program, and the processor implementing the steps of the method for preventing the robot from swaying left and right when executing the computer program.

[0034] In order to solve the above technical problems, another technical solution adopted by the present invention is: providing a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the method for preventing the robot from swaying left and right are implemented.

[0035] The method, device and computer equipment for preventing the robot from swaying left and right of the present invention have at least the following beneficial effects: the present invention avoids the S-shaped sway caused by the frequent switching of angular velocity between positive and negative small values ​​in the traditional DWA algorithm by forcibly introducing a zero angular velocity combination during speed sampling, ensures that the robot gives priority to a straight trajectory, and eliminates the lateral sway caused by inertia from the root; calculates the maximum allowable centrifugal force based on the global trajectory curvature, and screens speed combinations that exceed the safety threshold, effectively preventing swaying caused by excessive centrifugal force; dynamically adjusts the attenuation function of the linear velocity through angular velocity feedback, realizes intelligent switching between high-speed straight driving and low-speed turning, avoids sharp turns at high speed, and improves the stability of the robot movement; the present invention achieves anti-sway control only through algorithm optimization, without replacing the motor or tires, and can solve the problem of heavy-load swaying of the robot in heavy-load scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0037] Figure 1 This is a flow chart of an embodiment of a method for preventing a robot from swaying left and right according to the present invention.

[0038] Figure 2 for Figure 1 Flowchart of step S100 in FIG.

[0039] Figure 3 for Figure 1 Flowchart of step S200 in FIG.

[0040] Figure 4 This is the relationship between the linear velocity and angular velocity after attenuation by the attenuation function.

[0041] Figure 5 This is a structural block diagram of an embodiment of a device for preventing a robot from swaying left and right according to the present invention. DETAILED DESCRIPTION

[0042] The present invention will be further described below with reference to the accompanying drawings.

[0043] This solution uses the DWA controller. Because DWA requires the fastest possible speed and alignment with the target point, it generates both high linear velocity and high angular velocity when the trajectory curves with large curvature. This, due to the heavy weight and high inertia of the robot, can cause swaying. Therefore, this paper proposes a solution to address the swaying issue of robots under the DWA algorithm, particularly for fully or heavily loaded robots. This issue rarely occurs with lightly loaded robots and is therefore not considered in this solution.

[0044] See also Figure 1 , is a flow chart of an embodiment of a method for preventing a robot from swaying left and right according to the present invention. This embodiment specifically includes the following steps:

[0045] S100: Velocity sampling and setting zero angular velocity.

[0046] Specifically, the speed is sampled in the dynamic speed window with a fixed step size, and each group of speeds obtained by sampling is judged. When the absolute value of the angular velocity is less than the set threshold, the angular velocity is set to zero. Figure 2 , this step S100 includes the following sub-steps:

[0047] S110: Calculate a dynamic speed window.

[0048] Specifically, the linear velocity sampling range is calculated based on the current linear velocity and the maximum linear acceleration, and the angular velocity sampling range is calculated based on the current angular velocity and the maximum angular acceleration. The linear velocity sampling range and the angular velocity sampling range constitute a dynamic velocity window. The calculation of the linear velocity sampling range and the angular velocity sampling range is a mature technology and will not be further described here.

[0049] S120, speed sampling.

[0050] Specifically, sampling is performed in the sampling range of the linear velocity with a preset first fixed step size, and sampling is performed in the sampling range of the angular velocity with a preset second fixed step size to obtain a plurality of speed combinations. The fixed step size includes a preset first fixed step size and a preset second fixed step size. For example, if the sampling range of the linear velocity is [0.2, 0.3] m / s, the sampling range of the angular velocity is [-0.1, 0.1] rad / s, the first fixed step size is 0.02 m / s, and the second fixed step size is 0.01 rad / s, then the sampled speed combinations include (0.2 m / s, -0.1 rad / s), (0.2 m / s, -0.09 rad / s), ..., (0.3 m / s, 0.09 rad / s), (0.4 m / s, 0.1 rad / s), etc. 42 speed combinations.

[0051] S130: Set zero angular velocity.

[0052] Specifically, all velocity combinations are traversed to determine whether the absolute value of the angular velocity in the velocity combination is less than a set angular velocity threshold. If so, the angular velocity is set to zero; otherwise, the angular velocity remains unchanged. In this embodiment, the angular velocity threshold is set to 0.05 rad / s. In actual operation, the fixed step size should not be set too small to achieve zero angular velocity during velocity sampling. If the step size is too small, the computational complexity will increase, making it impossible to control the robot's motion in real time. Furthermore, due to the influence of acceleration, it is difficult for velocity sampling to achieve zero angular velocity. When the robot is traveling in a straight line, the angular velocity should ideally be zero. However, because the DWA algorithm struggles to achieve zero angular velocity, the DWA algorithm may output small non-zero values. Even if these non-zero angular velocities are small, they can cause the robot to continuously make small left-right adjustments, resulting in high-frequency, small-amplitude left-right swaying during straight-line travel, similar to an "S"-shaped trajectory. This swaying phenomenon is particularly pronounced on robots with large loads, especially fully loaded ones, because even small changes in angular velocity can cause large lateral oscillations due to their high inertia. If there is no option for zero angular velocity, the algorithm will switch back and forth between positive and negative angular velocity sampling points. For example, the angular velocity is selected as 0.03rad / s in one cycle and -0.02rad / s in the next cycle, causing the robot to sway from side to side. In this embodiment, by forcibly including a combination of zero angular velocity in the velocity sampling, it is ensured that there is always a straight line trajectory when predicting the trajectory (as long as the linear velocity is greater than zero). In this way, when the environment allows straight travel, the DWA evaluation function will tend to choose this straight line trajectory because its direction score is high and the angular velocity is zero, which makes the control more stable, thus avoiding swaying.

[0053] S200, speed screening.

[0054] Specifically, the centrifugal force constraint is used to eliminate the velocity combination that causes the robot to swing. Figure 3 , this step S200 includes the following sub-steps:

[0055] S210: Calculate the path curvature.

[0056] Specifically, the path curvature of the current position is calculated based on the preset global trajectory. The calculation of the path curvature is a mature technology and will not be described in detail here. The global trajectory is the path from the starting point to the end point generated by the global path planning algorithm. The global trajectory is composed of a series of points. As a local planner, dwa needs to track this global trajectory, that is, in each control cycle, according to the target point or direction of the global trajectory, adjust the linear velocity and angular velocity of the robot so that the robot moves along the global trajectory while avoiding obstacles. It should be noted that although dwa tracks the global trajectory, the actual route taken by the robot is only close to the global trajectory, and there is still a difference from the global trajectory.

[0057] S220: Calculate the maximum eccentricity.

[0058] Specifically, the maximum centrifugal force at the current position is calculated based on the path curvature. The calculation formula of the maximum centrifugal force is as follows:

[0059] Fc max =m*v1 2 / r

[0060] Among them, Fc max represents the maximum centrifugal force, m represents the robot's mass, v1 represents the desired linear velocity, and r represents the current turning radius, which is derived by derivation of the path curvature at the current position. v1 can be substituted for both the desired linear velocity and the robot's actual linear velocity. The desired linear velocity is the velocity that encourages the robot to move at the highest possible linear velocity.

[0061] S230: Eliminate speed combinations.

[0062] Specifically, the centrifugal force of each speed combination is calculated and it is determined whether the centrifugal force is greater than the maximum centrifugal force. If the centrifugal force is greater than the maximum centrifugal force, the speed combination is eliminated; otherwise, the speed combination is retained. The formula used to calculate the centrifugal force of each speed combination is as follows:

[0063] Fc=m*v*w

[0064] Wherein, Fc represents the centrifugal force corresponding to the speed combination, m represents the mass of the robot, v represents the linear velocity in the speed combination, and w represents the angular velocity in the speed combination.

[0065] S300: trajectory prediction.

[0066] Specifically, trajectories are predicted for all selected speed combinations and scored using an evaluation function. The speed combination corresponding to the highest-scoring trajectory is output to control the robot's motion. When scoring trajectories, the evaluation function typically includes the following objectives: ① aligning the forward direction with the target point; ② maximizing the speed; ③ avoiding collisions; and ④ ensuring the shortest braking distance is safe. Each speed combination can be forward-predicted into a trajectory. All trajectories are then scored sequentially using the aforementioned evaluation function to select the optimal trajectory. The speed combination corresponding to the optimal trajectory is the speed the robot needs to execute. Trajectory prediction and scoring are mature technologies, and the specific processes will not be detailed here.

[0067] In order to further improve the stability of the robot's movement, as a preferred embodiment, the following steps are further provided:

[0068] S400, smooth speed.

[0069] Specifically, an attenuation function is set to dynamically adjust the output linear velocity according to the current linear velocity and angular velocity of the robot. An attenuation function is set to dynamically adjust the output linear velocity according to the current linear velocity and angular velocity of the robot. The preset attenuation function is as follows:

[0070] v new =v'*e -k*|w'|

[0071] Among them, v new represents the adjusted linear velocity, v' represents the current linear velocity, w' represents the current angular velocity, and k represents the attenuation coefficient, which is 0.75. It should be understood that k needs to be determined based on the experimental results. In this embodiment, when k is 0.75, the robot moves most smoothly. The relationship between the output linear velocity and angular velocity at this time is as follows: Figure 4 This attenuation function can reduce the linear velocity as the absolute value of the angular velocity increases, thereby avoiding excessive centrifugal force generated during high-speed turns. This allows the robot to automatically decelerate when turning and move at full speed when going straight, avoiding sharp turns at high speeds and improving the stability of the robot's movement.

[0072] In this embodiment, the linear velocity and angular velocity of the robot can also be limited by the desired linear velocity and angular velocity to ensure normal operation of the robot. In this embodiment, the desired linear velocity is set to 1.0 m / s, and the desired angular velocity is set to between ±1.05 rad / s.

[0073] See also Figure 5 , is a block diagram of an embodiment of a device for preventing a robot from swaying. The device for preventing a robot from swaying in this embodiment is used to implement the method for preventing a robot from swaying as described in the above embodiment. Specifically, the device for preventing a robot from swaying in this embodiment includes a sampling module 100, an exclusion module 200, and a trajectory prediction module 300. Among them:

[0074] The sampling module 100 is used to perform speed sampling in a dynamic speed window with a fixed step size, judge each group of speeds obtained by sampling, and set the angular velocity to zero when the absolute value of the angular velocity is less than a set threshold. Specifically, the sampling module 100 is used to calculate the sampling range of the linear velocity based on the current linear velocity and the maximum linear acceleration, and calculate the sampling range of the angular velocity based on the current angular velocity and the maximum angular acceleration. The sampling range of the linear velocity and the sampling range of the angular velocity constitute a dynamic speed window; sampling is performed in the sampling range of the linear velocity with a preset first fixed step size, and sampling is performed in the sampling range of the angular velocity with a preset second fixed step size to obtain a plurality of speed combinations; traversing all speed combinations, determining whether the angular velocity in the speed combination is greater than the set angular velocity threshold, and if it is greater than the set angular velocity threshold, setting the angular velocity to 0, otherwise, the angular velocity remains unchanged.

[0075] The exclusion module 200 is used to eliminate speed combinations that cause robot swing by constraining centrifugal force. Specifically, the exclusion module is used to calculate the path curvature at the current position based on a preset global trajectory; calculate the maximum centrifugal force at the current position based on the path curvature; calculate the centrifugal force of each speed combination and determine whether the centrifugal force is greater than the maximum centrifugal force. If the centrifugal force is greater than the maximum centrifugal force, the speed combination is eliminated; otherwise, the speed combination is retained.

[0076] The trajectory prediction module 300 is used to perform trajectory prediction on all selected speed combinations and score each trajectory using an evaluation function, and output the speed combination corresponding to the trajectory with the highest score to control the movement of the robot.

[0077] In order to further improve the smoothness of the robot's movement, as a preferred embodiment, a speed smoothing module 400 is further provided. The speed smoothing module 400 is used to set an attenuation function and dynamically adjust the output linear velocity according to the current linear velocity and angular velocity of the robot.

[0078] The present invention also discloses a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the method for preventing the robot from swaying left and right when executing the computer program.

[0079] The present invention also discloses a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the steps of the method for preventing the robot from swaying left and right are implemented.

[0080] The present invention avoids the S-shaped swing caused by the frequent switching of angular velocity between positive and negative small values ​​in the traditional DWA algorithm by forcibly introducing a zero angular velocity combination during speed sampling, ensures that the robot prioritizes a straight trajectory, and eliminates the lateral swing caused by inertia from the root; calculates the maximum allowable centrifugal force based on the global trajectory curvature, and screens speed combinations that exceed the safety threshold, effectively preventing the swing caused by excessive centrifugal force; dynamically adjusts the attenuation function of the linear velocity through angular velocity feedback, realizes intelligent switching between high-speed straight driving and low-speed turning, avoids sharp turns at high speed, and improves the stability of the robot movement; the present invention achieves anti-sway control only through algorithm optimization, and can solve the problem of heavy-load robot swing in heavy-load scenarios without replacing motors or tires.

[0081] The above description merely expresses the preferred embodiments of the present invention, and its description is relatively specific and detailed, but it should not be construed as limiting the scope of the patent. It should be noted that a person skilled in the art may make a number of variations and improvements without departing from the concept of the present invention, and these variations and improvements fall within the scope of protection of the present invention. Therefore, the scope of protection of the patent of the present invention shall be based on the appended claims.

Claims

1. A method for preventing a robot from swaying left and right, characterized in that: The following steps are involved: The speed is sampled in the dynamic speed window with a fixed step size, and each group of speeds obtained by sampling is judged. When the absolute value of the angular velocity is less than the set threshold, the angular velocity is set to zero; By constraining the centrifugal force, we can eliminate the velocity combination that causes the robot to swing; The trajectory of all selected speed combinations is predicted and each trajectory is scored through the evaluation function. The speed combination corresponding to the trajectory with the highest score is output to control the movement of the robot.

2. The method for preventing a robot from swaying left and right as claimed in claim 1, wherein: The fixed step length includes a first fixed step length and a second fixed step length; The step of sampling the velocity in the dynamic velocity window with a fixed step size, judging each set of velocity obtained by the sampling, and setting the angular velocity to zero when the absolute value of the angular velocity is less than a set threshold, includes the following sub-steps: Calculating a sampling range of linear velocity based on the current linear velocity and the maximum linear acceleration, and calculating a sampling range of angular velocity based on the current angular velocity and the maximum angular acceleration, wherein the sampling range of linear velocity and the sampling range of angular velocity constitute a dynamic velocity window; Sampling is performed within a sampling range of linear velocity using a preset first fixed step size, and sampling is performed within a sampling range of angular velocity using a preset second fixed step size, to obtain a plurality of velocity combinations; All speed combinations are traversed to determine whether the absolute value of the angular velocity in the speed combination is less than the set angular velocity threshold. If it is less than the set angular velocity threshold, the angular velocity is set to 0; otherwise, the angular velocity remains unchanged.

3. The method for preventing a robot from swaying left and right according to claim 1, wherein: The step of eliminating the velocity combination that causes the robot to swing by constraining the centrifugal force includes the following substeps: Calculate the path curvature of the current position based on the preset global trajectory; calculating the maximum centrifugal force at the current position according to the path curvature; The centrifugal force of each speed combination is calculated and it is determined whether the centrifugal force is greater than the maximum centrifugal force. If the centrifugal force is greater than the maximum centrifugal force, the speed combination is excluded; otherwise, the speed combination is retained.

4. The method for preventing a robot from swaying left and right as claimed in claim 3, wherein: The calculation formula of the maximum centrifugal force is as follows: Fc max = m * v1 2 / r(1) In formula (1), Fc max represents the maximum centrifugal force, m represents the mass of the robot, v represents the desired linear velocity, and r represents the current turning radius, which is obtained by derivation of the path curvature at the current position.

5. The method for preventing a robot from swaying left and right as claimed in claim 3, wherein: The formula used to calculate the centrifugal force for each speed combination is as follows: Fc=m*v*w(2) In formula (2), Fc represents the centrifugal force corresponding to the speed combination, m represents the mass of the robot, v represents the linear velocity in the speed combination, and w represents the angular velocity in the speed combination.

6. The method for preventing a robot from swaying left and right as claimed in claim 1, wherein: After the steps of predicting trajectories for all selected speed combinations and scoring each trajectory using an evaluation function, and outputting the speed combination corresponding to the trajectory with the highest score to control the motion of the robot, the following steps are also included: Set the attenuation function to dynamically adjust the output linear velocity according to the robot's current linear velocity and angular velocity.

7. The method for preventing a robot from swaying left and right as claimed in claim 5, wherein: The preset attenuation function is as follows: v new =v'*e -k*| w '| (three) In formula (3), v new represents the adjusted linear velocity, v' represents the current linear velocity, w' represents the current angular velocity, and k represents the attenuation coefficient, which is 0.

75.

8. A device for preventing a robot from swaying left and right, characterized in that: include: The sampling module is used to sample the speed in the dynamic speed window with a fixed step size, judge each group of speeds obtained by sampling, and set the angular velocity to zero when the absolute value of the angular velocity is less than the set threshold; An elimination module, used for eliminating the velocity combination causing the robot to swing by constraining the centrifugal force; The trajectory prediction module is used to predict the trajectories of all selected speed combinations and score each trajectory through the evaluation function, and output the speed combination corresponding to the trajectory with the highest score to control the movement of the robot.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method for preventing a robot from swaying left and right as claimed in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method for preventing a robot from swaying left and right as claimed in any one of claims 1 to 7 are implemented.

Images