CN103576688B - A kind of robot is counterclockwise clockwise motion control method more first - Google Patents

Abstract

The invention discloses a method for controlling the movement of a robot counterclockwise and then clockwise. The method comprises the following steps: setting and inputting a plurality of robot movement parameters; calculating the coordinates of the first center of the robot movement; and calculating the second center of the movement of the robot coordinates; calculate the first unit vector pointing to the second center of the first circle; calculate the angle between the first unit vector and the second unit vector; calculate the coordinates of the first conversion point of the robot motion; calculate the second of the robot motion The coordinates of the conversion point; calculate the second unit vector from the first conversion point to the second conversion point; calculate the first rotation angle of the robot motion; calculate the second rotation angle of the robot motion; Control with a clockwise motion. The invention realizes the control of the counterclockwise and then clockwise movement of the robot by using the coordinate rotation transformation method in combination with the knowledge of robotics. The invention is simple and effective.

Description

A control method for robot movement counterclockwise and then clockwise

technical field

The invention relates to the technical field of robots, in particular to a method for controlling the counterclockwise and then clockwise movement of a robot.

Background technique

In recent years, a wide range of application requirements in surveying, target acquisition, search and rescue, surveillance, and environmental monitoring have led to rapid development of mobile robot technology. Among them, navigation technology is one of the core technologies of mobile robot research, and path planning is one of the basic links of navigation. The basic idea of robot path planning is to find a collision-free optimal or nearly optimal path from the starting point to the target point according to certain criteria (such as the shortest time, the least energy, the shortest path, etc.). Path planning can be divided into global path planning and local path planning. The main algorithms of global path planning include visual map method, grid decoupling method, probability map method, topology method and neural network method; the main algorithm of local path planning is artificial potential field method, fast random search tree (RRT) and fuzzy logic algorithm, etc.

Contents of the invention

The object of the present invention is to propose a method for controlling the movement of the robot counterclockwise first and then clockwise, so as to plan the CSC path of the robot, so that the robot moves counterclockwise first and then clockwise according to the planned route.

A method for controlling the movement of a robot counterclockwise and then clockwise includes the following steps:

Step S1: Set and input multiple robot motion parameters, the motion parameters at least include: the coordinates of the initial point S of the robot motion (x _s , y _s ), the initial direction unit vector Ps of the robot at the initial point S (p _xs , p _ys ), the coordinates of the robot’s moving target point G (x _g , y _g ), the target direction unit vector Pg (p _xg , p _yg ) of the robot at the target point G, and the allowable turning radius R of the robot;

Step S2: Based on the initial point S, the initial direction unit vector Ps and the turning radius R, calculate the coordinates of the first center Os of the robot moving counterclockwise around the first center Os from the initial point S along the initial direction unit vector Ps with the turning radius R (x _os , y _os );

Step S3: Based on the calculation of the target point G, the target direction unit vector Pg and the turning radius R, the robot moves clockwise around the second circle center Og with the turning radius R to reach the second circle center Og of the target point G and the target direction unit vector Pg The coordinates of (x _og , y _og );

Step S4: Calculate the first unit vector Q(q _x , q _y ) pointing from the first center Os to the second center Og based on the coordinates of the first center Os and the second center Og;

Step S5: Calculate the angle γ between the first unit vector Q and the second unit vector W based on the first center Os, the second center Og and the turning radius R;

Step S6: Based on the first center Os, the turning radius R and the first unit vector Q, calculate the first conversion point Ws at which the robot moves counterclockwise around the first center Os to move in a straight line along the direction of the second unit vector W The coordinates of (x _ws , y _ws );

Step S7: Calculate based on the second center of circle Og, turning radius R and first unit vector Q to obtain the second transition point Wg at which the robot moves from a straight line along the direction of the second unit vector W to moving clockwise around the second center of circle Og coordinates(x _wg , y _wg );

Step S8: Calculate the second unit vector W(w _x , w _y ) pointing from the first conversion point Ws to the second conversion point Wg;

Step S9: Calculate based on the first center Os, the initial point S, and the first conversion point Ws to obtain the first rotation angle α _s that the robot moves counterclockwise around the first center Os from the initial point S to the first conversion point Ws;

Step S10: Based on the second center of circle Og, the target point G, and the second conversion point Wg, calculate the second rotation angle β _g that the robot passes through from the second conversion point Wg clockwise around the second center of circle Og to the target point G;

Step S11: Based on the motion path parameters calculated in the steps S2-S10, control the first counterclockwise and then clockwise motion of the robot, wherein, the first counterclockwise and then clockwise motion path of the robot is: from the initial point S Move counterclockwise around the first center of circle Os with the turning radius R and turn through the first turning angle α _s to reach the first conversion point Ws, from the first conversion point Ws to the second conversion point Wg in a straight line along the direction of the second unit vector W, and from the second The second conversion point Wg moves clockwise around the second center of circle Og with the turning radius R and turns over the second turning angle _βg to reach the target point G.

In combination with robotics, the present invention proposes a counterclockwise and then clockwise motion control method for a robot, which is implemented through coordinate rotation transformation, which is simple and effective.

Description of drawings

Fig. 1 is a schematic diagram of the method for controlling the movement of the robot first counterclockwise and then clockwise according to the present invention.

detailed description

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be described in further detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

Fig. 1 is the method for controlling the movement of the robot counterclockwise and then clockwise according to the present invention. As shown in Fig. 1, the method for controlling the movement of the robot anticlockwise and then clockwise in the present invention includes the following steps:

Step S1: Set and input multiple robot motion parameters, the motion parameters at least include: the coordinates of the initial point S of the robot motion (x _s , y _s ), the initial direction unit vector Ps of the robot at the initial point S (p _xs , p _ys ), the coordinates of the robot’s moving target point G (x _g , y _g ), the target direction unit vector Pg (p _xg , p _yg ) of the robot at the target point G, and the allowable turning radius R of the robot;

Step S2: Based on the initial point S, the initial direction unit vector Ps and the turning radius R, calculate the coordinates of the first center Os of the robot moving counterclockwise around the first center Os from the initial point S along the initial direction unit vector Ps with the turning radius R (x _os , y _os );

This step is specifically: first rotate the initial direction unit vector Ps 90 degrees counterclockwise, then multiply it by the turning radius R, and then add the multiplication result to the initial point S coordinates to obtain the coordinates of the first center Os.

Step S3: Based on the calculation of the target point G, the target direction unit vector Pg and the turning radius R, the robot moves clockwise around the second circle center Og with the turning radius R to reach the second circle center Og of the target point G and the target direction unit vector Pg The coordinates of (x _og , y _og );

This step is specifically: first rotate the target direction unit vector Pg 90 degrees clockwise, then multiply it by the turning radius R, and then add the multiplication result to the coordinates of the target point G to obtain the coordinates of the second circle center Og.

Step S4: Calculate the first unit vector Q(q _x , q _y ) pointing from the first center Os to the second center Og based on the coordinates of the first center Os and the second center Og;

This step is specifically: Subtract the coordinates of the first center Os from the coordinates of the second center Og, and then divide the subtraction result by the length between the first center Os and the second center Og to obtain the first unit vector Q .

Step S5: Calculate the angle γ between the first unit vector Q and the second unit vector W based on the first center Os, the second center Og and the turning radius R;

This step is specifically as follows: multiply 2 by R and divide by the length value between the first circle center Os and the second circle center Og, and then perform the arc sine of the calculation result to obtain the included angle γ.

Step S6: Based on the first center Os, the turning radius R and the first unit vector Q, calculate the first conversion point Ws at which the robot moves counterclockwise around the first center Os to move in a straight line along the direction of the second unit vector W The coordinates of (x _ws , y _ws );

This step is specifically: first rotate the first unit vector Q clockwise by (90-γ) degrees, then multiply it by the turning radius R, and then add the multiplication result to the coordinates of the first center Os to obtain the first conversion point Coordinates of Ws.

Step S7: Based on the second center Og, the turning radius R and the first unit vector Q, calculate the second transition point Wg at which the robot changes from linear motion in the direction of the second unit vector W to clockwise movement around the second center Og coordinates(x _wg , y _wg );

This step is specifically: first rotate the first unit vector Q counterclockwise by (90+γ) degrees, then multiply it by the turning radius R, and then add the result of the multiplication to the coordinates of the second circle center Og to obtain the second conversion point Wg coordinate of.

Step S8: Calculate the second unit vector W(w _x , w _y ) pointing from the first conversion point Ws to the second conversion point Wg;

This step is specifically as follows: subtract the coordinates of the first conversion point Ws from the coordinates of the second conversion point Wg, and then divide the subtraction result by the length value between the first conversion point Ws and the second conversion point Wg to obtain the second conversion point Wg Two unit vectors W.

Step S9: Calculate based on the first center Os, the initial point S, and the first conversion point Ws to obtain the first rotation angle α _s that the robot moves counterclockwise around the first center Os from the initial point S to the first conversion point Ws;

Said step S9 further comprises the following steps:

Step S91: Calculate the first vector MS pointing to the initial point S from the first center Os: subtract the coordinates of the first center Os from the coordinates of the initial point S to obtain the first vector MS;

Step S92: Calculating the second vector NS pointing to the first conversion point Ws from the first center Os: subtracting the coordinates of the first center Os from the coordinates of the first conversion point Ws to obtain the second vector NS;

Step S93: Calculate the angle α between the first vector MS and the second vector NS by arccosine;

Step S94: Judging whether the first rotation angle α _s turned counterclockwise from the initial point S around the first center Os to the first conversion point Ws is greater than 180 degrees, if α _s is less than or equal to 180 degrees, then set α _s equal to α, otherwise Let α _s equal to 2π-α;

Described step S94 is specifically:

When p _ys <0 and , α _s =2π-α; when p _ys <0 and ${\mathrm{the\; y}}_{\mathrm{ws}}<{\mathrm{the\; y}}_{\mathrm{the\; s}}-\frac{p_{\mathrm{xs}}}{p_{\mathrm{ys}}}(x_{\mathrm{ws}}-x_{\mathrm{the\; s}})$ , α _s =α;

When p _ys > 0 and , α _s =2π-α; when p _ys >0 and ${\mathrm{the\; y}}_{\mathrm{ws}}>{\mathrm{the\; y}}_{\mathrm{the\; s}}-\frac{p_{\mathrm{xs}}}{p_{\mathrm{ys}}}(x_{\mathrm{ws}}-x_{\mathrm{the\; s}})$ , α _s =α;

When p _ys =0 and x _ws ≤0, α _s =2π-α; when p _ys =0 and x _ws >0, α _s =α.

Step S10: Based on the second center of circle Og, the target point G, and the second conversion point Wg, calculate the second rotation angle β _g that the robot passes through from the second conversion point Wg clockwise around the second center of circle Og to the target point G;

The step S10 further includes the following steps:

Step S101: Calculate the third vector MG pointing to the target point G from the second circle center Og: subtract the coordinates of the second circle center Og from the coordinates of the target point G to obtain the third vector MG;

Step S102: Calculate the fourth vector NG pointing to the second conversion point Wg from the second center Og: subtract the coordinates of the second center Og from the coordinates of the second conversion point Wg to obtain the fourth vector NG;

Step S103: Calculate the angle β between the third vector MG and the fourth vector NG by using arc cosine;

Step S104: Judging whether the angle β _g turned from the second conversion point Wg clockwise around the second center Og to the target point G is greater than 180 degrees, if β _g is less than or equal to 180 degrees, then set β _g equal to β, otherwise set β _g is equal to 2π-β.

The step S104 is specifically:

When p _yg <0 and , β _g = β; when p _yg <0 and ${\mathrm{the\; y}}_{\mathrm{w\; g}}<{\mathrm{the\; y}}_g-\frac{p_{\mathrm{x\; g}}}{p_{\mathrm{yg}}}(x_{\mathrm{w\; g}}-x_g)$ , β _g =2π-β;

When p _yg > 0 and , β _g = β; when p _yg > 0 and ${\mathrm{the\; y}}_{\mathrm{w\; g}}>{\mathrm{the\; y}}_g-\frac{p_{\mathrm{x\; g}}}{p_{\mathrm{yg}}}(x_{\mathrm{w\; g}}-x_g)$ , β _g =2π-β;

When p _yg =0 and x _wg ≤0, β _g =β; when p _yg =0 and x _wg >0, β _g =2π-β.

Step S11: Based on the motion path parameters calculated in the steps S2-S10, control the counterclockwise and then clockwise motion of the robot, wherein the counterclockwise and then clockwise motion path parameters include: the coordinates of the first circle center Os (x _os , y _os ), the coordinates of the first conversion point Ws (x _ws , y _ws ), the first rotation angle α _s from the initial point S moving counterclockwise around the first center Os to the first conversion point Ws, the first Two unit vectors W(w _x , w _y ), the second circle center Og coordinates (x _og , y _og ), the second conversion point Wg coordinates (x _wg , y _wg ), from the second conversion point Wg around the second circle clockwise The second circle center Og moves to the second corner β _g where the target point G turns. The movement path of the robot first counterclockwise and then clockwise is as follows: the initial point S moves counterclockwise around the first center of circle Os with the turning radius R and turns over the first turning angle α _s to reach the first conversion point Ws, and the first conversion point Ws Ws moves linearly along the direction of the second unit vector W to the second conversion point Wg, and from the second conversion point Wg moves clockwise around the second center of circle Og with the turning radius R and turns over the second turning angle _βg to reach the target point G.

The specific embodiments described above have further described the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above descriptions are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims (7)

1. a kind of robot anticlockwise and clockwise motion control method again, it is characterized in that, the method comprises the following steps: Step S1: Set and input a plurality of robot motion parameters, the motion parameters at least include: the robot motion initial point S coordinates (x _s , y _s ), the initial direction unit vector Ps of the robot at the initial point S (p _xs , p _ys ), the coordinates of the moving target point G of the robot (x _g , y _g ), the target direction unit vector Pg(p _xg , p _yg ) of the robot at the target point G, and the allowable turning radius R of the robot; Step S2: Based on the initial point S, the initial direction unit vector Ps and the turning radius R, calculate the coordinates of the first center Os of the robot moving counterclockwise around the first center Os from the initial point S along the initial direction unit vector Ps with the turning radius R (x _os , y _os ); Step S3: Based on the calculation of the target point G, the target direction unit vector Pg and the turning radius R, the robot moves clockwise around the second circle center Og with the turning radius R to reach the second circle center Og of the target point G and the target direction unit vector Pg The coordinates of (x _og , y _og ); Step S4: Calculate the first unit vector Q(q _x , q _y ) pointing from the first center Os to the second center Og based on the coordinates of the first center Os and the second center Og; Step S5: Calculate the angle γ between the first unit vector Q and the second unit vector W based on the first center Os, the second center Og, and the turning radius R, where 2 times R is divided by the first center Os and the length value between the two points of the second circle center Og, and then carry out the arc sine of the calculation result to obtain the included angle γ; Step S6: Based on the first center Os, the turning radius R and the first unit vector Q, calculate the first conversion point Ws at which the robot moves counterclockwise around the first center Os to move in a straight line along the direction of the second unit vector W The coordinates of (x _ws , y _ws ); Step S7: Calculate based on the second center of circle Og, turning radius R and first unit vector Q to obtain the second transition point Wg at which the robot moves from a straight line along the direction of the second unit vector W to moving clockwise around the second center of circle Og coordinates(x _wg , y _wg ); Step S8: Calculate the second unit vector W(w _x , w _y ) pointing from the first conversion point Ws to the second conversion point Wg; Step S9: Calculate based on the first center Os, the initial point S, and the first conversion point Ws to obtain the first rotation angle α _s that the robot moves counterclockwise around the first center Os from the initial point S to the first conversion point Ws; Step S10: Based on the second center of circle Og, the target point G, and the second conversion point Wg, calculate the second rotation angle β _g that the robot passes through from the second conversion point Wg clockwise around the second center of circle Og to the target point G; Step S11: Based on the motion path parameters calculated in the steps S2-S10, control the first counterclockwise and then clockwise motion of the robot, wherein, the first counterclockwise and then clockwise motion path of the robot is: from the initial point S Move counterclockwise around the first center of circle Os with the turning radius R and turn through the first turning angle α _s to reach the first conversion point Ws, from the first conversion point Ws to the second conversion point Wg in a straight line along the direction of the second unit vector W, and from the second The second conversion point Wg moves clockwise around the second center of circle Og with the turning radius R and turns over the second turning angle β _g to reach the target point G; Wherein, said step S9 further comprises the following steps: Step S91: Calculate the first vector MS pointing to the initial point S from the first circle center Os; Step S92: Calculating the second vector NS pointing to the first conversion point Ws from the first circle center Os; Step S93: Calculate the angle α between the first vector MS and the second vector NS by arccosine; Step S94: Judging whether the first rotation angle α _s turned counterclockwise from the initial point S around the first center Os to the first conversion point Ws is greater than 180 degrees, if α _s is less than or equal to 180 degrees, then set α _s equal to α, otherwise Let α _s equal to 2π-α; The step S10 further includes the following steps: Step S101: Calculate the third vector MG pointing to the target point G from the second circle center Og; Step S102: Calculate the fourth vector NG pointing to the second conversion point Wg from the second circle center Og; Step S103: Calculate the angle β between the third vector MG and the fourth vector NG by using arc cosine; Step S104: Judging whether the angle β _g turned from the second conversion point Wg clockwise around the second center Og to the target point G is greater than 180 degrees, if β _g is less than or equal to 180 degrees, then set β _g equal to β, otherwise set β _g is equal to 2π-β. 2. The method according to claim 1, characterized in that the step S2 is specifically: first rotate the initial direction unit vector Ps 90 degrees counterclockwise, then multiply it by the turning radius R, and then multiply the multiplication result with the initial The coordinates of the first circle center Os are obtained by adding the coordinates of the points S. 3. The method according to claim 1, characterized in that the step S3 is specifically: first rotate the target direction unit vector Pg 90 degrees clockwise, then multiply it by the turning radius R, and then multiply the multiplication result with the target The coordinates of the point G are added to obtain the coordinates of the second circle center Og. 4. The method according to claim 1, wherein the step S4 is specifically: subtracting the coordinates of the first center Os from the coordinates of the second center Og, and then dividing the subtraction result by the first center Os and The length value between the two points of the second circle center Og is the first unit vector Q. 5. The method according to claim 1, characterized in that the step S6 is specifically: first rotate the first unit vector Q clockwise by (90-γ) degrees, then multiply it by the turning radius R, and then multiply the Add the multiplication result to the coordinates of the first circle center Os to obtain the coordinates of the first conversion point Ws. 6. The method according to claim 1, characterized in that the step S7 is specifically: first rotate the first unit vector Q counterclockwise by (90+γ) degrees, then multiply it by the turning radius R, and then multiply the Add the multiplication result to the coordinates of the second circle center Og to obtain the coordinates of the second conversion point Wg. 7. The method according to claim 1, wherein the step S8 is specifically: subtracting the coordinates of the first conversion point Ws from the coordinates of the second conversion point Wg, and then dividing the result of the subtraction by the first conversion The length value between the point Ws and the second conversion point Wg is the second unit vector W.