A new automatic iterative correction method for the heading angle offset of a robot

Abstract

The present invention discloses a new automatic iterative correction method for the heading angle offset of a robot. S1: Record the movement trajectory of the robot from point A to point B to obtain a set of coordinate points, forming a point set P. S2: For each point in P, calculate the distance to the line segment AB to obtain a set L. S3: Sort L and select the point farthest from the line segment AB as D. Draw a tangent DF to the trajectory curve through point D, parallel to AB, and the included angle between DF and DB is equal to θ. S4: Draw a perpendicular line from point D to the line segment AB and intersect at point E, and calculate the coordinates of point E. S5: Calculate ∠DBE, θ = ∠DBE. S6: Update the heading angle of the robot after compensating the θ value. S7: Apply the new one for control in the next two-point straight line segment, and repeat S1-S6, so as to iteratively correct automatically. For each walking trajectory of the robot, calculate θ, and update the compensation value once before the start of the next trajectory. Iterate in this way to continuously and automatically correct the heading angle of the robot, making the trajectory approach a straight line.

Description

Technical Field

[0001] The present invention relates to the technical field of robot heading angle correction, and particularly to a new automatic iterative correction method for robot heading angle offset. Background Art

[0002] When a robot performs positioning and navigation control in a plane, the first thing to solve is to walk along a straight line between two points. For a robot platform that cannot translate laterally, such as a two-wheel differential and Ackerman structure robot, the most basic two-point straight-line closed-loop control algorithm is as follows:

[0003] As Figure 3 shown, assume that the robot walks in a straight line from point A to point B. The distance L between the current position P of the robot and the target point B is used as the position deviation to input into the PID regulator. After the PID regulator outputs, the linear velocity of the robot is obtained; the direction of the line connecting the current position P of the robot and the target point B is used as the target heading, and the angular deviation d between the target heading and the current heading of the robot is used as the input of another PID regulator. After this PID regulator outputs, the angular velocity of the robot is obtained; ideally, during the movement process, the control angular deviation d is made equal to 0, and the position deviation L is continuously reduced until L is also equal to 0. Then the robot will walk from A to B along the straight line segment AB. However, for this control method, there is a prerequisite, that is, the heading of the robot body must match the heading of the working coordinate system. Generally, in the working coordinate system, the heading of the positive y-axis direction is defined as 0 degrees, the heading increases counterclockwise, and the heading decreases clockwise. The heading of the positive x-axis direction is -90 degrees. We define as the heading angle in the working coordinate system. The robot body will also output a robot heading angle, denoted as which generally does not match the heading of the working coordinate system Therefore, it is necessary to calibrate in advance, calculate the offset angle between the two headings and add this offset angle to the robot heading angle for compensation. Let the compensated robot heading angle be Then Ideally, we hope that the compensated robot heading angle is equal to the heading angle of the working coordinate system, that is However, affected by the calibration accuracy and there will still be a deviation θ. As Figure 4 shown, assume that the arrow pointing to the upper right is the 0° heading considered by the robot, and the arrow pointing directly upward is the corresponding 0° heading in the working coordinate system. The included angle between the two is the deviation θ. Then in the working coordinate system, the actual heading angle of the robot is -θ. Therefore, the robot heading angle still needs to subtract a θ to make the two headings completely coincide, that is At this time and In the case of the deviation θ, the robot will go astray using the two-point straight line control algorithm described above. Figure 5 As shown by the thick line, this is a curve. In the figure, point A is the starting point and point B is the target point. The robot needs to be controlled to move from point A to point B. The thin straight line AB is the ideal motion trajectory of the robot, and the thick curve is the actual motion trajectory of the robot. Any point P on this curve represents the position passed by the robot at a certain moment. The direction of the tangent of the curve passing through point P represents the actual heading of the robot at that moment. The direction of the line connecting point P and point B is the expected heading of the robot. The angle between these two directions is the heading angle deviation θ of the robot. In the case of the deviation θ, the motion trajectory of the robot is a spiral curve, and its mathematical description is also relatively complicated. In theory, the robot can only get infinitely close to point B in the end, but cannot reach point B. However, a threshold can be set in the control. When the position difference between the robot and the target point is less than this threshold, the robot is considered to have completed the trajectory.

[0004] In order to make the robot walk in a straight line as much as possible, we need to find a way to measure the value of θ, and then compensate the robot's heading angle twice, so that and As far as possible, the existing method is to offset the distance d from the robot's current position to the straight line segment AB. h PID adjustment is performed, and the output of PID is used as θ to perform secondary compensation on the robot heading angle. However, this method also has disadvantages. h The change of obviously lags behind the change of heading angle. This PID control system is prone to oscillation, causing the robot to swing left and right along the straight line AB, forming an S-shaped trajectory. After long-term research, the inventor invented a new automatic iterative correction method for the robot's heading angle bias. Summary of the invention

[0005] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a new automatic iterative correction method for the heading angle bias of a robot.

[0006] The object of the present invention is achieved by the following technical solution: a new automatic iterative correction method for robot heading angle bias, comprising the following steps:

[0007] S1: Record the motion trajectory of the robot from point A to point B, and obtain a set of discrete coordinate points distributed in time. This set of coordinate points constitutes the point set P;

[0008] S2: For each point in the point set P, calculate the distance to the line segment AB to obtain the distance set L;

[0009] S3: Sort the set L, select the point farthest from the line segment AB, denote this point as D, and draw the tangent line DF of the trajectory curve passing through point D. Then DF is parallel to the line segment AB, and the included angle between DF and DB is equal to θ;

[0010] S4: Draw a perpendicular line from point D to the line segment AB and intersect AB at point E, and calculate the coordinates of point E and the lengths of the three sides of the right triangle BDE;

[0011] S5: Calculate ∠DBE,

[0012]

[0013] Then θ = ∠DBE;

[0014] S6: Update the heading angle of the compensated robot according to the value of θ

[0015] S7: Apply the new robot heading angle in the next two - point straight - line segment for control, and repeat steps S1 - S6, and iterate for automatic correction in this way.

[0016] Preferably, in step S1, densely record the movement trajectory of the robot from point A to point B.

[0017] Preferably, in step S2, for each point in the point set P, the steps to calculate the distance to the line segment AB are as follows:

[0018] S21: Let the coordinates of the i - th point Pi i in the set P be (x i , y i ), and the coordinates of point A and point B be (x A , y A ) and (x B , y B ) respectively;

[0019] S22: The two - point form straight - line equation passing through points A and B is Converted to the general form:

[0020] (y B -y A )x+(x A -x B )y+x B y A -x A y B =0

[0021] S23: Let a = y B -y A , b = x A -x B , c = xB y A -x A y B , then the distance l from point P i to line segment AB i is:

[0022]

[0023] Preferably, in step S3, taking point A as the origin and the line AB as the x-axis to establish a new coordinate system, differentiating the curve ADB along the x-axis direction, from point A to point D, the derivative is greater than zero and gradually decreases, from point D to point B, the derivative is less than zero. Since the derivative of the curve is continuous, that is, the derivative at point D is 0, it is obtained that the slope of the tangent line DF is 0 and is parallel to the line segment AB.

[0024] Preferably, in step S4, the steps for calculating the coordinates of point E are as follows:

[0025] S41: Let the coordinates of point A be (x A , y A ), the coordinates of point B be (x B , y B ), and the coordinates of point D be (x D , y D ). According to the steps S22 and S23, the straight-line equation of AB is

[0026] ax + by + c = 0;

[0027] where a = y B -y A , b = x A -x B , c = x B y A -x A y B ;

[0028] S42: The equation of the perpendicular line to the straight line AB is:

[0029] bx - ay + m = 0;

[0030] where m is a parameter variable. Substituting the coordinates of point D into the equation, we can get:

[0031] m = ay D -bx D ;

[0032] By the system of equations:

[0033]

[0034] The coordinates of the intersection point E are obtained as

[0035] Preferably, in step S6, the heading angle of the compensated robot is updated according to the value of θ That is:

[0036]

[0037] The present invention has the following advantages: According to each walking trajectory of the robot, the present invention calculates the heading of the compensated robot and the heading of the working coordinate system The deviation θ is updated before the start of the next trajectory So that the compensation value is updated once for each section walked. By such cyclic iteration, the robot can continuously and automatically correct the heading angle, making the trajectory closer and closer to a straight line. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a schematic structural diagram of the mathematical plane of the automatic iterative correction method;

[0039] Figure 2 It is a schematic structural diagram of the positional relationship between DF and AB;

[0040] Figure 3 It is a schematic structural diagram of the basic two-point straight line closed-loop control algorithm;

[0041] Figure 4 It is a schematic structural diagram of the positional relationship between the robot heading angle and the coordinate system heading angle;

[0042] Figure 5 It is a schematic structural diagram of the actual robot motion trajectory under the control algorithm; DETAILED DESCRIPTION OF THE EMBODIMENTS

[0043] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Usually, the components of the embodiments of the present invention described and shown here can be arranged and designed in various different configurations.

[0044] Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed present invention, but merely represents selected embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0045] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0046] It should be noted that like reference numerals and letters denote like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined and explained in subsequent figures.

[0047] In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, or the orientation or positional relationship in which the product of the present invention is customarily placed during use, or the orientation or positional relationship commonly understood by those skilled in the art. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as a limitation on the present invention. In addition, the terms "first", "second", etc. are only used for distinguishing descriptions and cannot be understood as indicating or implying relative importance.

[0048] In the description of the present invention, it should also be noted that unless otherwise clearly specified and defined, the terms "set", "install", "connect", "couple" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific situations.

[0049] In this embodiment, as Figure 1 shown, a new automatic iterative correction method for the heading angle offset of a robot, characterized in that it comprises the following steps:

[0050] S1: Record the movement trajectory of the robot from point A to point B to obtain a set of discrete coordinate points distributed in time, and this set of coordinate points forms a point set P; preferably, in step S1, densely record the movement trajectory of the robot from point A to point B.

[0051] S2: For each point in the point set P, calculate the distance to the line segment AB to obtain a set L of distances;

[0052] S3: Sort the set L, select the point farthest from the line segment AB, and set this point as D. Make a tangent DF to the trajectory curve passing through point D, then DF is parallel to the line segment AB, and the included angle between DF and DB is equal to θ;

[0053] S4: Draw a perpendicular line from point D to line segment AB and intersect AB at point E, and calculate the coordinates of point E and the lengths of the three sides of right triangle BDE; specifically, since DE is perpendicular to AB and DF is parallel to AB, then DE is also perpendicular to DF.

[0054] S5: Calculate ∠DBE,

[0055]

[0056] Then θ = ∠DBE;

[0057] S6: Update the heading angle of the compensated robot according to the value of θ

[0058] S7: Apply the new robot heading angle in the next two - point straight - line segment for control, and repeat steps S1 - S6, and iterate in this way for automatic correction. According to each walking trajectory of the robot, calculate the heading of the compensated robot and the deviation θ from the heading of the working coordinate system and update it before the start of the next trajectory so that the compensation value is updated for each section walked. By looping and iterating in this way, the robot can continuously and automatically correct the heading angle, making the trajectory closer and closer to a straight line.

[0059] Furthermore, in step S2, for each point in point set P, the steps to calculate the distance to line segment AB are as follows:

[0060] S21: Let the coordinates of the i - th point Pi i in set P be (x i , y i ), and the coordinates of point A and point B be (x A , y A ) and (x B , y B ) respectively;

[0061] S22: The two - point form straight - line equation passing through points A and B is Converted to the general form:

[0062] (y B - y A )x+(x A - x B )y+x B y A - x A y B = 0

[0063] S23: Let a = y B - y A , b = x A - xB where \(c = x\) B y A -x A y B then the distance \(l\) from point \(P\) i to line segment \(AB\) is i as follows:

[0064]

[0065] Furthermore, as shown in Figure 2 , in step \(S3\), taking point \(A\) as the origin and the line \(AB\) as the \(x\)-axis to establish a new coordinate system, differentiating the curve \(ADB\) along the \(x\)-axis direction. From point \(A\) to point \(D\), the derivative is greater than zero and gradually decreases. From point \(D\) to point \(B\), the derivative is less than zero. Since the derivative of the curve is continuous, that is, the derivative at point \(D\) is \(0\), it is obtained that the slope of the tangent line \(DF\) is \(0\), which is parallel to the line segment \(AB\). Specifically, according to the geometric meaning of the derivative, the derivative at the tangent point is equal to the slope of the tangent line. Since the derivative at point \(D\) is \(0\), the slope of the tangent line \(DF\) is \(0\). Therefore, \(DF\) is parallel to the \(x\)-axis, that is, \(DF\) is parallel to \(AB\).

[0066] In this embodiment, in step \(S4\), the steps for calculating the coordinates of point \(E\) are as follows:

[0067] S41: Let the coordinates of point \(A\) be \((x\) A , \(y\) A ), the coordinates of point \(B\) be \((x\) B , \(y\) B ), and the coordinates of point \(D\) be \((x\) D , \(y\) D ). According to step \(S22\) and step \(S23\), the equation of the line \(AB\) is

[0068] \(ax + by + c = 0\);

[0069] where \(a = y\) B -y A , \(b = x\) A -x B , \(c = x\) B y A -x A y B ;

[0070] S42: The equation of the perpendicular line to line \(AB\) is:

[0071] \(bx - ay + m = 0\);

[0072] where \(m\) is a parameter. Substituting the coordinates of point \(D\) into the equation, we get:

[0073] \(m = ay\) D -bx D ;

[0074] Through the system of equations:

[0075]

[0076] The coordinates of the intersection point E are obtained as

[0077] Furthermore, in step S6, the heading angle of the compensated robot is updated according to the value of θ That is:

[0078]

[0079] Specifically, the new robot heading angle is applied in the next two-point straight line segment for control, and steps S1 to S6 are repeated. θ is continuously calculated according to the next trajectory segment, and then updated continuously By such iteration, the robot can continuously and automatically correct the heading angle offset according to the empirical trajectory, making its walking trajectory closer and closer to a straight line.

[0080] Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A new automatic iterative correction method for the heading angle offset of a robot, characterized in that: It includes the following steps: S1: Record the motion trajectory of the robot from point A to point B to obtain a set of discrete coordinate points distributed in time, and this set of coordinate points forms a point set P; S2: For each point in the point set P, calculate the distance to the line segment AB to obtain a set of distances L; S3: Sort the set L, select the point farthest from the line segment AB, and set this point as D. Draw a tangent line DF to the trajectory curve passing through point D, then DF is parallel to the line segment AB, and the included angle between DF and DB is equal to θ; S4: Draw a perpendicular line from point D to the line segment AB and intersect AB at point E, and calculate the coordinates of point E and the lengths of the three sides of the right triangle BDE; S5: Calculate ∠DBE, then θ = ∠DBE; S6: Update the heading angle of the compensated robot according to the value of θ S7: Apply the new robot heading angle to the two-point straight line segment in the next section for control, and repeat steps S1 to S6, and iterate in this way for automatic correction.

2. The automatic iterative correction method for the heading angle offset of a new robot according to claim 1, characterized in that: In the step S1, densely record the motion trajectory of the robot from point A to point B.

3. A new automatic iterative correction method for the heading angle offset of a robot according to claim 2, characterized in that: In the step S2, for each point in the point set P, the steps to calculate the distance to the line segment AB are as follows: S21: Let the i-th point P in the set P i The coordinates of (x i ,y i ), the coordinates of point A and point B are (x A ,y A ) and (x B ,y B ); S22: The two-point form linear equation passing through points A and B is Converted to the general form: (y B -y A )x+(x A -x B )y+x B y A -x A y B = 0 S23: Let a = y B -y A , b = x A -x B , c = x B y A -x A y B , then the distance l from point P i to line segment AB i is:

4. A new automatic iterative correction method for the heading angle offset of a robot according to claim 3, characterized in that: In the step S3, with point A as the origin and the line where AB is located as the x-axis, establish a new coordinate system. Differentiate the curve ADB along the x-axis direction. From point A to point D, the derivative is greater than zero and gradually decreases. From point D to point B, the derivative is less than zero. Since the derivative of the curve is continuous, that is, the derivative at point D is 0, it is obtained that the slope of the tangent line DF is 0 and is parallel to the line segment AB.

5. A new automatic iterative correction method for the heading angle offset of a robot according to claim 4, characterized in that: In the step S4, the steps to calculate the coordinates of point E are as follows: S41: Let the coordinates of point A be (x A , y A ), the coordinates of point B be (x B , y B ), and the coordinates of point D be (x D , y D ). According to the steps S22 and S23, the linear equation of AB is ax + by + c = 0; where a = y B -y A and b = x A -x B and c = x B y A -x A y B ; S42: The perpendicular line equation of the line AB is: bx - ay + m = 0; where m is a parameter variable. Substitute the coordinates of point D into the equation to obtain: m = ay D -bx D ; Through the system of equations: The coordinates of the intersection point E are obtained as 6. A new automatic iterative correction method for the heading angle offset of a robot according to claim 5, characterized in that: In the step S6, update the heading angle of the compensated robot according to the value of θ That is:

Images