CN117742328A - Speed control method for smooth movement of robot - Google Patents

Abstract

The invention relates to the technical field of robot movement control, and discloses a speed control method for smooth movement of a robot, which comprises the following specific steps: s1, receiving a target speed, a current speed and a maximum acceleration by the robot; s2, calculating a target speed increment from the target speed and the current speed, and obtaining a maximum speed increment from the maximum acceleration, wherein the target speed increment and the maximum speed increment both comprise a speed value and a speed direction; s3, determining an actual speed increment according to the difference value of the speed directions of the target speed increment and the maximum speed increment; s4, controlling the movement of the robot according to the target speed and the actual speed increment; the problem that in the prior art, the robot moves yaw in the speed change process due to acceleration limitation is solved.

Description

Speed control method for smooth movement of robot

Technical Field

The invention relates to the technical field of robot movement control, in particular to a speed control method for smooth movement of a robot.

Background

The existing mobile robot adopts a wheel type travelling mechanism, namely wheels are adopted as a main travelling mode, typically, if two motors are adopted to provide driving force for the wheels of a chassis of the robot, the two motors respectively control two different wheels, when the rotating speeds of the two wheels are the same, the robot moves in a linear mode, and when the rotating speeds of the two wheels are different, the robot rotates around a central point, namely, the linear speed and the angular speed of the robot are controlled; along with the popularization and application of robots in a large quantity, a plurality of robots can work simultaneously in the same robot work area, so that the moving routes of the robots need to be planned, and collision interference is avoided.

In order to enable the robot to move along the track, the rotating speeds of the two motors are required to be controlled, the existing control mode is a linear acceleration and deceleration control algorithm, namely, the target speed is calculated according to the difference between the current position and the target position, and then acceleration and deceleration control is carried out on the robot to enable the robot to reach the target speed;

however, the speed change of the robot is affected by acceleration, especially when the robot moves along an arc track, the speed limit may cause that the time required for the linear speed and the angular speed of the robot to reach the target speed from the current speed is different, so that a yaw phenomenon exists, and the upper path following unit is required to continuously calculate the deviation value to correct the yaw of the robot, so that the control pressure of the path following unit is larger, and if the displacement paths of the two robots are relatively close to each other, the robot can be scraped and bumped even if the yaw is small.

Disclosure of Invention

The invention aims to provide a speed control method for smooth movement of a robot, which aims to solve the problem that in the prior art, the robot moves to yaw in the speed change process due to acceleration limitation.

In order to achieve the above purpose, the invention adopts the following technical scheme: a speed control method for smooth movement of a robot comprises the following specific steps:

s1, receiving a target speed, a current speed and a maximum acceleration by the robot;

s2, calculating a target speed increment from the target speed and the current speed, and obtaining a maximum speed increment from the maximum acceleration, wherein the target speed increment and the maximum speed increment both comprise a speed value and a speed direction;

s3, determining an actual speed increment according to the difference value of the speed directions of the target speed increment and the maximum speed increment;

and S4, controlling the movement of the robot according to the target speed and the actual speed increment.

The principle and the advantages of the scheme are as follows: limiting and adjusting the speed change of the robot according to the target speed and the maximum acceleration, so that the actual speed direction of the robot during the speed change is consistent with the target speed direction, the movement track of the robot is identical with the preset track of the path planning even during the speed change, and meanwhile, the limitation of the self acceleration of the robot is considered, so that the actual speed value of the robot during the speed change is smaller than the maximum speed increment value, and the robot can perform according to the calculation result;

according to the scheme, the linear speed and the angular speed of the robot are comprehensively considered as a whole, so that the linear speed and the angular speed are mutually influenced, the actual situation of the robot in the actual arc speed change process is more met, the yaw phenomenon is avoided, the auxiliary effect of the upper path following in the arc path is reduced, the operation amount is reduced, and the response of the robot is more sensitive and cruises more accurately.

Preferably, as an improvement, the speed of the robot includes a linear speed and an angular speed;

setting a target speed vector from the target speed incrementSetting a maximum speed vector from said maximum speed increment +.>

And establishing a rectangular coordinate system, wherein the x-axis is linear velocity, the y-axis is angular velocity, and normalizing the linear velocity and the angular velocity of the target velocity increment and the linear velocity and the angular velocity of the maximum velocity increment to obtain the target velocity vectorAnd the maximum speed vector>Is positioned in the same rectangular coordinate system.

The improved beneficial effects are that: the method provides an imaging calculation mode, can highlight the direction and numerical characteristics of the speed, and is convenient for accurately and directly reflecting the constraint relation of the target speed and the maximum acceleration to the variable speed motion of the robot.

Preferably, as a modification, S3 includes calculating the target speed vectorAnd the maximum speed vectorThe direction angle difference of (a), namely:

wherein theta is the direction angle difference, w _A For the vectorV of the y-axis component of (v) _A For the vector->W is the x-axis component of (2) _B For the vector->V of the y-axis component of (v) _B For the vector->An x-axis component of (2);

when theta is greater than 0, then the maximum velocity increment, i.e., the maximum angular velocity increment, is too large, v _B The linear velocity as the actual velocity increment, that is, the actual linear velocity increment;

when theta is less than 0, the linear velocity of the maximum velocity increment, i.e. the maximum linear velocity increment, is too large, w _B An angular velocity that is an actual velocity increment, that is, an actual angular velocity increment;

when theta is equal to 0, the maximum speed increment is taken as the actual speed increment.

The improved beneficial effects are that: the calculation mode is provided, the specific relation between the target speed and the maximum acceleration can be judged only by calculating the azimuth angle speed difference to be positive number, negative number or zero, the calculation of the actual speed increment is facilitated in the next step, the specific data of the direction angle difference are not required to be accurately calculated in the calculation process, only fuzzy calculation is required, the precision requirement is reduced, the calculation speed is higher, the robot can perform speed change faster, and the yaw risk is reduced by reducing delay.

Preferably, as a modification, when theta is greater than 0,

when the theta is less than 0, then,

where act_w_inc is an actual angular velocity increment, act_v_inc is an actual linear velocity increment, w_inc is a target angular velocity increment, which is the angular velocity of the target velocity increment, and v_inc is a target linear velocity increment, which is the linear velocity of the target velocity increment.

The improved beneficial effects are that: the calculation mode of the actual speed increment is provided, so that the actual speed increment is in the maximum speed increment range, and the speed directions of the actual angular speed increment and the actual linear speed increment are the same as the target speed direction.

Preferably, as an improvement, in S4, a command speed is calculated from the current speed and the actual speed increment, and the robot is controlled to move according to the command speed, where the calculation mode of the command speed is as follows:

cmd_linear＝vel_linear+act_v_inc；

cmd_angular＝vel_angular+act_w_inc；

wherein cmd_linear is a linear speed of the command speed, i.e., a command linear speed, and cmd_angular is an angular speed of the command speed, i.e., a command angular speed; the vel_linear is the current linear velocity, which is the current linear velocity, and the vel_angular is the angular velocity of the current velocity, which is the current angular velocity.

The improved beneficial effects are that: a calculation mode of the command speed is provided, and the movement of the robot is directly controlled at the command speed.

Preferably, as an improvement, the normalization processing includes:

wherein,for the vector->Is a mould length->For the vector->Is a die length of (2); max_v_inc is the maximum linear velocity increment and max_w_inc is the maximum angular velocity increment.

The improved beneficial effects are that: because the units of the linear velocity and the angular velocity are different, the units of the linear velocity and the angular velocity are unified for the convenience of calculation, so that the final result is more accurate, and the subsequent calculation is also convenient.

Preferably, in a modification, S1 further includes determining whether the target speed and the current speed are equal, if so, keeping the current speed for running, and if not, proceeding to S2.

The improved beneficial effects are that: eliminating the need for shifting and reducing the amount of calculation.

Drawings

FIG. 1 is a flow chart of an embodiment of the present invention.

Fig. 2 is a vector diagram of an embodiment of the present invention.

Detailed Description

The following is a further detailed description of the embodiments:

example 1

The specific speed smoothing flow is shown in figure 1, the robot receives a target speed, namely a target linear speed and a target angular speed, compares the target speed with a current speed, namely a current linear speed and a current angular speed, and judges whether the target speed is equal to the current speed or not; (1) If the current speed is equal to the target speed, keeping the current speed to continue running, and dynamically receiving the target speed for judgment; (2) If the current speed is not equal to the target speed, the maximum acceleration of the motor is combined with the maximum linear acceleration and the maximum angular acceleration to calculate the target speed increment and the maximum speed increment.

Calculating a target speed increment from the target speed and the current speed, and obtaining a maximum speed increment from the maximum acceleration; in the speed smoothing process, namely, the target speed increment and the maximum speed increment in the current control period (period), the specific calculation formula is as follows:

v_inc＝target_linear-vel_linear；

w_inc＝target_angular-vel_langular；

max_v_inc＝accel_limit_v×period；

max_w_inc＝accel_limit_w×period；

wherein period is the current control period, v_inc is the target linear velocity increment in the current control period, w_inc is the target angular velocity increment in the current control period, target_linear is the target linear velocity, target_angular target angular velocity, vel_linear is the current linear velocity, and vel_angular is the current angular velocity;

max_v_inc is the maximum linear velocity increment in the current control period, max_w_inc is the maximum angular velocity increment in the current control period, accel_limit_v is the maximum linear acceleration of the robot, and accel_limit_w is the maximum angular acceleration of the robot.

Determining an actual speed increment according to the difference value of the speed directions of the target speed increment and the maximum speed increment, wherein the actual speed increment is the speed change amount actually required by the robot in the control period; in order to keep the information of the speed value and the speed direction in the target speed increment and the maximum speed increment, the characteristics of the target speed increment and the maximum speed increment are more intuitively represented by means of a rectangular coordinate system and a vector which can keep the numerical value and reflect the direction.

Since the robot in this example is a plane-moving robot, the motion of which can be decomposed into two components of linear velocity and angular velocity, a plane rectangular coordinate system is constructed with the x-axis being the linear velocity, the y-axis being the angular velocity, and the target velocity increment being set as a vectorSetting the maximum speed increment to vector +.>

As shown in fig. 2, vectorsThe abscissa of (2) is the target line speed increment, i.e., v_inc, vector in the current control periodThe ordinate of (2) is the target angular velocity increment in the current control period, i.e. w_inc, vector +.>Is the speed direction of the speed change from the current speed to the target speed, which coincides with the planned path, without taking into account the acceleration limit;

(Vector)the abscissa of (2) is the maximum linear velocity increment in the current control period, i.e. max_v_inc, vector +.>The ordinate of (2) is the maximum angular velocity increment in the current control period, i.e. max_w_inc, vector +.>The arrow of (2) is the speed direction of the robot performing variable speed motion with its own maximum acceleration in the current cycle.

If the robot wants to change speed to the target speed with maximum acceleration, it will follow the vectorIs driven in the speed direction due to the vector +.>Vector->The speed direction deviation exists, and the yaw phenomenon exists in the speed change process of the robot; in order for the robot to still travel along the planned path during the speed change, the robot is required to be aligned with the vector during the speed change>Is the same and also ensures that it is more likely to shorten the shift time to increase the efficiency, the vector is ∈ ->Taking as reference the ordinate of max_w_inc, a ordinate of max_w_inc, vector direction and vector +.>Identical vector->

(Vector)The abscissa of (2) is the actual linear velocity increment in the current control period, vector +.>Is the current controlActual angular velocity increment, vector +.>The arrow of (2) is the speed direction of the robot in the current period and under the self acceleration limit, and in the variable speed period, the speed direction coincides with the planned path.

For the convenience of calculation, the target speed increment and the maximum speed increment are respectively normalized, namely:

wherein w is _A Is vector quantityV of the y-axis component of (v) _A For vector->W is the x-axis component of (2) _B For vector->V of the y-axis component of (v) _B For vector->An x-axis component of (2); />For vector->Is a mould length->For vector->Is a die length of the die.

And provides a pass judgment vectorSum vector->To calculate the actual speed increment:

wherein theta is the direction angle difference;

(1) If the direction difference theta is greater than 0, the maximum angular velocity increment of the robot is overlarge, and the actual angular velocity increment is calculated according to the target velocity direction:

will v _B As an actual linear velocity increment, i.e., act_v_inc=max_v_inc, an actual angular velocity is calculatedThe degree increment, act_w_inc;

(2) If the direction difference theta is smaller than 0, the maximum linear velocity increment of the robot is overlarge, and the actual linear velocity increment is calculated according to the target velocity direction:

will w _B Calculating an actual linear velocity increment as an actual angular velocity increment, i.e., act_w_inc=max_w_inc;

(3) If the direction difference theta is equal to 0, the direction of the maximum speed increment is the same as the direction of the target speed increment, v _B As the actual linear velocity increment, i.e., act_v_inc=max_v_inc, w will be _B As an actual angular velocity increment, i.e., act_w_inc=max_w_inc.

Calculating a command speed by the current speed and the actual speed increment, controlling the robot to move according to the command speed, wherein the calculation mode of the command speed is as follows:

cmd_linear＝vel_linear+act_v_inc；

cmd_angular＝vel_angular+act_w_inc；

wherein cmd_linear is the linear speed of the command speed, i.e., the command linear speed, and cmd_angular is the angular speed of the command speed, i.e., the command angular speed.

And updating the speed command with the command speed, changing the current speed of the robot into a speed change period, comparing and judging the dynamically updated current speed with the target speed again, and repeating the flow.

Example 2

In the embodiment 1, the method is used for a robot with a linear speed and an angular speed of movement provided by differential motion of two motors, but the method is not only applicable to a differential chassis robot model, but also can be extended to other motion models such as an omnidirectional motion model;

the motion speed is decomposed into a plurality of components under the standard speed, the difference value of the target speed and the maximum speed under each component is compared, and the minimum component value of the maximum speed under the standard is taken as a basis to calculate the command speed which is the same as the target speed direction, so that the motion direction of the robot is always the same as the target speed direction in the speed change process.

The foregoing is merely exemplary of the present invention, and specific technical solutions and/or features that are well known in the art have not been described in detail herein. It should be noted that, for those skilled in the art, several variations and modifications can be made without departing from the technical solution of the present invention, and these should also be regarded as the protection scope of the present invention, which does not affect the effect of the implementation of the present invention and the practical applicability of the patent. The protection scope of the present application shall be subject to the content of the claims, and the description of the specific embodiments and the like in the specification can be used for explaining the content of the claims.

Claims (7)

1. The speed control method for the smooth movement of the robot is characterized by comprising the following specific steps:

s1, receiving a target speed, a current speed and a maximum acceleration by the robot;

s2, calculating a target speed increment from the target speed and the current speed, and obtaining a maximum speed increment from the maximum acceleration, wherein the target speed increment and the maximum speed increment both comprise a speed value and a speed direction;

s3, determining an actual speed increment according to the difference value of the speed directions of the target speed increment and the maximum speed increment;

and S4, controlling the movement of the robot according to the target speed and the actual speed increment.

2. The method for controlling the speed of smooth movement of a robot according to claim 1, wherein: the speed of the robot includes a linear speed and an angular speed;

setting a target speed vector from the target speed incrementSetting a maximum speed vector from said maximum speed increment +.>

And establishing a rectangular coordinate system, wherein the x-axis is linear velocity, the y-axis is angular velocity, and normalizing the linear velocity and the angular velocity of the target velocity increment and the linear velocity and the angular velocity of the maximum velocity increment to obtain the target velocity vectorAnd the maximum speed vector>Is positioned in the same rectangular coordinate system.

3. The method for controlling the speed of smooth movement of a robot according to claim 2, wherein: s3, calculating the target speed vectorAnd the maximum speed vector>The direction angle difference of (a), namely:

wherein theta is the direction angle difference, w _A For the vectorV of the y-axis component of (v) _A For the vector->W is the x-axis component of (2) _B For the vector->V of the y-axis component of (v) _B For the vector->An x-axis component of (2);

when theta is greater than 0, then the maximum velocity increment, i.e., the maximum angular velocity increment, is too large, v _B The linear velocity as the actual velocity increment, that is, the actual linear velocity increment;

when theta is less than 0, the linear velocity of the maximum velocity increment, i.e. the maximum linear velocity increment, is too large, w _B An angular velocity that is an actual velocity increment, that is, an actual angular velocity increment;

when theta is equal to 0, the maximum speed increment is taken as the actual speed increment.

4. A method for controlling the speed of smooth movement of a robot according to claim 3, wherein:

when theta is greater than 0, the process is performed,

when the theta is less than 0, then,

where act_w_inc is an actual angular velocity increment, act_v_inc is an actual linear velocity increment, w_inc is a target angular velocity increment, which is the angular velocity of the target velocity increment, and v_inc is a target linear velocity increment, which is the linear velocity of the target velocity increment.

5. The method according to claim 4, wherein in S4, a command speed is calculated from the current speed and the actual speed increment, the robot is controlled to move according to the command speed, and the command speed is calculated by:

cmd_linear＝vel_linear+act_v_inc；

cmd_angular＝vel_angular+act_w_inc；

wherein cmd_linear is a linear speed of the command speed, i.e., a command linear speed, and cmd_angular is an angular speed of the command speed, i.e., a command angular speed; the vel_linear is the current linear velocity, which is the current linear velocity, and the vel_angular is the angular velocity of the current velocity, which is the current angular velocity.

6. The method for controlling the speed of smooth movement of a robot according to claim 5, wherein the normalization process comprises:

wherein,for the vector->Is a mould length->For the vector->Is a die length of (2); max_v_inc is the maximum linear velocity increment and max_w_inc is the maximum angular velocity increment.

7. The method for controlling the speed of smooth movement of a robot according to claim 6, wherein: and S1, judging whether the target speed is equal to the current speed, if so, keeping the current speed for running, and if not, entering S2.