Robot obstacle avoidance method based on segmented iteration drag line

Abstract

The invention discloses a robot obstacle avoidance method based on segmented iteration drag lines, belongs to the technical field of robot obstacle avoidance path planning, and aims to solve the problems that a robot obstacle avoidance algorithm with redundant degrees of freedom needs a large amount of environmental data sets, then obstacle avoidance path planning is performed through intelligent algorithms such as a neural network and a genetic algorithm, the calculated amount is large, and the obstacle avoidance path planning efficiency is high. In order to solve the problems that in the prior art, the requirement for the performance of software and hardware of a system is high, the method comprises the following steps that a drag line equation is deduced, recursive operation is conducted on the drag line equation, an obstacle collision detection area is set through an artificial potential field, and the segmented iteration drag line obstacle avoidance method is provided. The method does not need a large amount of iterative calculation like a neural network method, improves the calculation efficiency, can meet the real-time requirement of a robot system, and enables the motion form of the robot to be smoother and more natural through the gradual convergence geometric characteristic of the drag line, and is small in energy consumption.

Description

Technical Field

[0001] The invention relates to the technical field of robot obstacle avoidance path planning, and in particular to a robot obstacle avoidance method based on piecewise iterative tractography. Background Art

[0002] In narrow and obstructed working environments such as aircraft engine internal inspection, post-disaster rescue, and tracheal intubation medical surgery, robots with redundant degrees of freedom are used to complete work tasks. Their characteristics are that they can autonomously change the posture of the robotic arm according to environmental information to complete obstacle avoidance, and finally go deep into the complex environment to perform operations. Traditional redundant degrees of freedom robot obstacle avoidance algorithms generally require a large amount of environmental data sets, and then use intelligent algorithms such as neural networks and genetic algorithms to plan obstacle avoidance paths. The amount of calculation is large and the requirements for system hardware and software performance are high. Exploring a purely geometric obstacle avoidance algorithm to improve computational efficiency, reduce unnecessary robot movement, and improve motion stability is a technical problem that needs to be solved urgently.

[0003] Existing robot obstacle avoidance algorithms with redundant degrees of freedom generally require a large amount of environmental data sets, and then use intelligent algorithms such as neural networks and genetic algorithms to plan obstacle avoidance paths. This requires a large amount of calculation and places high demands on the system's software and hardware performance.

[0004] To solve the above problems, a robot obstacle avoidance method based on piecewise iterative tractrix was proposed. Summary of the invention

[0005] The purpose of the present invention is to provide a robot obstacle avoidance method based on piecewise iterative draglines, which works on pure geometric principles, thereby solving the problem that the robot obstacle avoidance algorithm with redundant degrees of freedom in the above background requires a large amount of environmental data sets, and then uses intelligent algorithms such as neural networks and genetic algorithms to plan obstacle avoidance paths, which has a large amount of calculation and requires high system hardware and software performance.

[0006] To achieve the above object, the present invention provides the following technical solution: a robot obstacle avoidance method based on piecewise iterative tractography, comprising the following implementation steps:

[0007] S1: Derive the dragline equation;

[0008] S2: recursive operation of the tractography equation;

[0009] S3: The obstacle collision detection area is set using artificial potential field, and a piecewise iterative dragline obstacle avoidance method is proposed.

[0010] Furthermore, the specific steps for deriving the dragline equation in S1 are as follows:

[0011] S101: Given a connecting rod of length L, placed along the z-axis, point B at one end of the connecting rod is pulled to move along the y-axis, then the trajectory of point A at the other end is the dragline. From the conditions for the formation of the dragline, it can be known that the velocity vector of point A at the end always points to point B along the connecting rod L, that is, it is aligned with the tangent of the dragline. From the differential equation of the tangent at point A at one end of the connecting rod L, the dragline equation can be derived as:

[0012]

[0013] Furthermore, the specific steps of recursive operation of the tractography equation in S2 are as follows:

[0014] S201: (y, z) is the coordinate of point A, L is the length of the connecting rod, and the closed form calculation is used for the dragline equation to solve the inverse kinematics problem of the multi-joint super-redundant degree of freedom robot, so as to achieve a more natural and smooth movement of the super-redundant robot. The formula obtained is as follows:

[0015]

[0016] Furthermore, in S3, an artificial potential field is used to set the obstacle collision detection area, and a segmented iterative dragline obstacle avoidance method is proposed. The specific steps are as follows:

[0017] S301: Virtually separate the hyper-redundant robot joints that do not meet the obstacle avoidance condition;

[0018] S302: Through discrete virtual joints, the hyper-redundant robot meets the obstacle avoidance condition, and the end effector P1 can track the target point;

[0019] S303: The discrete joints are combined again by using the bidirectional iterative dragline method.

[0020] Furthermore, in S303, the discrete joints are combined again by using the bidirectional iterative dragline method, and the specific steps are as follows:

[0021] S3031: Redefine the position update direction and iteration step size;

[0022] S3032: Calculate whether the super-redundant robot meets the obstacle avoidance condition. The formula is as follows:

[0023] ||P tar -P obs ||≤ε0.

[0024] Furthermore, in S3032, it is calculated whether the super-redundant robot meets the obstacle avoidance condition, and the specific steps are as follows:

[0025] S30321: Assume virtual discrete joint P 3a Without considering obstacle avoidance conditions, the position update direction is:

[0026]

[0027] S30322: When the virtual discrete joint P 3a When obstacle avoidance conditions need to be considered. According to the repulsive force F defined by the artificial potential field rep and attraction F att , recalculate the super redundant robot from P1 to P 3a The update direction of the iterative trajectory of the segment D 3a , according to the artificial potential field method, P 3a The repulsive force of the point update iterative motion is F rep3a =(y r3a ,z r3a ), the attraction is F att3a =(y a3a , z a3a ), then the resultant force F 3a for:

[0028] F 3a =F rep3a +F att3a =(y r3a +y a3a ,z r3a +z a3a );

[0029]

[0030] Furthermore, the linear equation in the update direction is given by the point-slope equation:

[0031]

[0032] Furthermore, in order to achieve fast convergence, the iteration step size is calculated using the closest distance point (vertical foot) from the virtual joint P3b (y3b, z3b) to the point-slope equation:

[0033]

[0034] Where j is the number of iterations, and the calculation formula for the iteration step is:

[0035] d 3aj =sqrt((y 3a -y 3aj ) 2 +(z 3a -z 3aj ) 2 ).

[0036] Furthermore, suppose that after the jth iteration, P 3a and P 3b Overlap, and set the iteration termination condition to:

[0037]

[0038] Furthermore, when the distance Δl3 between the two virtual discrete joints is less than the set value δ3 and meets the obstacle avoidance condition, the iteration ends and the inverse kinematics solution (α i ,β i ), and start the next obstacle avoidance motion planning, and finally reach the target position.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] 1. The present invention expands and innovates the geometric characteristics of the tractrix, proposes the bidirectional segmented iterative tractrix principle, and designs a new obstacle avoidance algorithm for a multi-joint redundant degree of freedom robot, thus realizing adaptive obstacle avoidance motion planning for a multi-joint redundant degree of freedom robot.

[0041] 2. The obstacle avoidance algorithm uses a purely geometric calculation method, which does not require a large number of iterative calculations like neural networks and other methods, thereby improving the calculation efficiency and meeting the real-time requirements of the robot system. The geometric characteristics of the gradual convergence of the dragline also make the robot's movement more smooth and natural, with low energy consumption. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is a schematic diagram of the iterative trolley traction motion principle of the present invention;

[0043] Figure 2 The discrete bidirectional iterative tracometer obstacle avoidance principle of the present invention is as follows: (a) the i-th trajectory position; (b) the j-th position update iteration; (c) the (j+1)-th position update iteration; (d) the (i+1)-th trajectory position;

[0044] Figure 3 It is a bidirectional iterative trolley flow chart of the present invention;

[0045] Figure 4 It is the overall flow chart of the present invention;

[0046] Figure 5 A flowchart of the specific steps of deriving the dragline equation of the present invention;

[0047] Figure 6 The flowchart is a specific step of recursively calculating the dragline equation of the present invention;

[0048] Figure 7 The present invention uses artificial potential field to set obstacle collision detection area, and proposes a specific step flow chart of a segmented iterative tractography obstacle avoidance method;

[0049] Figure 8It is a flowchart of the specific steps of re-combining discrete joints by using the bidirectional iterative dragline method of the present invention;

[0050] Fig. 9 The flowchart is a specific step of calculating whether the hyper-redundant robot meets the obstacle avoidance condition of the present invention. DETAILED DESCRIPTION

[0051] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0052] In order to solve the problem of redundant freedom robot obstacle avoidance algorithm requires a large amount of environmental data set, and then use neural network, genetic algorithm and other intelligent algorithms to plan obstacle avoidance path, which has a large amount of calculation and high requirements on system hardware and software performance. Figure 1-Figure 9 As shown, the following preferred technical solutions are provided: The tractogram was first proposed by Huygens in 1692 and is also called the "tracing curve". Figure 1 The figure shows the introduction of the dragline by taking the motion of a single connecting rod on a plane as an example. A connecting rod of length L is given and placed along the z-axis. When point B at one end of the traction connecting rod moves along the y-axis, the trajectory of point A at the other end is the dragline. From the conditions for the formation of the dragline, it can be seen that the velocity vector of point A at the end always points to point B along the connecting rod L, that is, it is aligned with the tangent of the dragline. The dragline equation can be derived from the differential equation of the tangent at the end point A of the connecting rod L:

[0053]

[0054] Where (y, z) is the coordinate of point A, and L is the length of the connecting rod. Formula (1) can be calculated in closed form:

[0055]

[0056] The recursive operation of the dragline can be used to solve the inverse kinematics problem of a multi-joint super-redundant degree of freedom robot, thereby achieving a more natural and smooth movement of the super-redundant robot.

[0057] In order to realize the adaptive obstacle avoidance control of the hyper-redundant robot, this patent proposes a segmented iterative dragline obstacle avoidance method using the obstacle collision detection area set by the artificial potential field. Assume that the initial position of the hyper-redundant robot is as follows Figure 2As shown in (a), the target point H, the center of the obstacle circle O, the radius of the danger zone r1 and r2, and the end effector point P of the super-redundant robot are known. Due to the existence of obstacles on the motion path, the robot cannot safely reach the target position by simply traction. In order to achieve obstacle avoidance during the motion of the super-redundant robot, the joint points of the super-redundant robot that do not meet the obstacle avoidance conditions can be virtually separated, as shown in Figure 2 (b) As shown. Through the discrete virtual joints, the hyper-redundant robot meets the obstacle avoidance condition, and the end effector P1 can track the target point. However, in reality, the hyper-redundant robot is not discrete, so the present invention proposes a bidirectional iterative dragline method to combine the discrete joints again. Figure 2 As shown in (b), assuming that the P3 joint does not meet the obstacle avoidance condition at the jth position update, it is actually separated into two points P 3a and P 3b The discrete joint points are then approximated using the bidirectional iterative dragline method.

[0058] The key problem of overlapping discrete virtual joints is to redefine the position update direction and iteration step size. For this purpose, the present invention designs a specific flow chart of discrete bidirectional iterative dragline as shown in the following figure: Figure 3 As shown. First, calculate whether the super-redundant robot meets the obstacle avoidance condition:

[0059] ||P tar -P obs ||≤ε0 (3).

[0060] Among them, P tar are points on the robot joints and hinges, P obs is the obstacle center point, and ε0 is the obstacle detection radius. ta With P obs If the distance between them is less than ε0, the segmented iterative tracline obstacle avoidance operation is started. Otherwise, the bidirectional iterative tracline motion planning is started.

[0061] Assume that the virtual discrete joint P 3a Without considering obstacle avoidance conditions, the position update direction is:

[0062]

[0063] That is, the discrete joint points P of the super redundant robot 3a Along P 3a and P 3b The line connecting the two points is P 3b Move, at this time the iteration step length is d 3aj for:

[0064] d 3aj =sqrt((y 3a -y3b ) 2 +(z 3a -z 3b ) 2 ) (5).

[0065] When the virtual discrete joint P 3a When obstacle avoidance conditions need to be considered. According to the repulsive force F defined by the artificial potential field rep and attraction F att , recalculate the super redundant robot from P1 to P 3a The update direction of the iterative trajectory of the segment D 3a According to the artificial potential field method, P 3a The repulsive force of the point update iterative motion is F rep3a =(y r3a , z r3a ), the attraction is F att3a =(y a3a ,z a3a ), then the resultant force F 3a for:

[0066] F 3a =F rep3a +F att3a =(y r3a +y a3a ,z r3a +z a3a ) (6);

[0067]

[0068] The linear equation in the update direction is given by the point-slope equation:

[0069]

[0070] In order to achieve fast convergence, the iteration step size is calculated using the closest distance point (vertical foot) from the virtual joint P3b (y3b, z3b) to the linear equation formula (8):

[0071]

[0072] Where j is the number of iterations. The formula for calculating the iteration step length is:

[0073] d 3aj =sqrt((y 3a -y 3aj ) 2 +(z 3a -Z 3aj ) 2 (10).

[0074] In the hyper-redundant robot from P1 to P 3aAfter the position is updated, the super redundant robot is 3b To the position update of P5, such as Figure 2 As shown in (c), the update process is similar to the robot’s process from P1 to P 3a The update process of is similar and will not be repeated here. Assume that after the jth iteration, P 3a and P 3b Overlap, and set the iteration termination condition to:

[0075]

[0076] That is to say, when the distance Δl3 between the two virtual discrete joints is less than the set value δ3 and meets the obstacle avoidance condition, the iteration ends and the inverse kinematics solution (α i ,β i ), and start the next obstacle avoidance motion planning, and finally reach the target position.

[0077] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0078] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A robot obstacle avoidance method based on piecewise iterative tractography, characterized in that: The implementation steps include: S1: Derive the dragline equation; S2: recursive operation of the tractography equation; S3: The obstacle collision detection area is set using artificial potential field, and a piecewise iterative dragline obstacle avoidance method is proposed.

2. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 1, characterized in that: The specific steps for deriving the dragline equation in S1 are as follows: S101: Given a connecting rod of length L, placed along the z-axis, point B at one end of the connecting rod is pulled to move along the y-axis, then the trajectory of point A at the other end is the dragline. From the conditions for the formation of the dragline, it can be known that the velocity vector of point A at the end always points to point B along the connecting rod L, that is, it is aligned with the tangent of the dragline. From the differential equation of the tangent at point A at one end of the connecting rod L, the dragline equation can be derived as:

3. The robot obstacle avoidance method based on piecewise iterative tractography according to claim 2, characterized in that: The specific steps of recursive operation of the tractography equation in S2 are as follows: S201: (y, z) is the coordinate of point A, L is the length of the connecting rod, and the closed form calculation is used for the dragline equation to solve the inverse kinematics problem of the multi-joint super-redundant degree of freedom robot, so as to achieve a more natural and smooth movement of the super-redundant robot. The formula obtained is as follows:

4. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 3, characterized in that: In S3, an artificial potential field is used to set the obstacle collision detection area, and a segmented iterative dragline obstacle avoidance method is proposed. The specific steps are as follows: S301: Virtually separate the hyper-redundant robot joints that do not meet the obstacle avoidance condition; S302: Through discrete virtual joints, the hyper-redundant robot meets the obstacle avoidance condition, and the end effector P1 can track the target point; S303: The discrete joints are combined again by using the bidirectional iterative dragline method.

5. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 4, characterized in that: In S303, the discrete joints are combined again by using the bidirectional iterative dragline method. The specific steps are as follows: S3031: Redefine the position update direction and iteration step size; S3032: Calculate whether the super-redundant robot meets the obstacle avoidance condition. The formula is as follows: ||P tar -P obs ‖≤ε0.

6. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 5, characterized in that: In S3032, it is calculated whether the super-redundant robot meets the obstacle avoidance condition. The specific steps are as follows: S30321: Assume virtual discrete joint P 3a Without considering obstacle avoidance conditions, the position update direction is: S30322: When the virtual discrete joint P 3a When obstacle avoidance conditions need to be considered, the repulsive force F defined by the artificial potential field rep and attraction F att , recalculate the super redundant robot from P1 to P 3a The update direction of the iterative trajectory of the segment D 3a , according to the artificial potential field method, P 3a The repulsive force of the point update iterative motion is F rep3a =(y r3a , z r3a ), the attraction is F att3a =(y a3a , z a3a ), then the resultant force F 3a for: F 3a =F rep3a +F att3a =(y r3a +y a3a ,z r3a +z a3a ); 7. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 6, characterized in that: The linear equation in the update direction is given by the point-slope equation:

8. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 7, characterized in that: In order to achieve fast convergence, the iteration step size is calculated using the closest distance point (vertical foot) from the virtual joint P3b (y3b, z3b) to the point-slope equation: Where j is the number of iterations, and the calculation formula for the iteration step is: d 3aj =sqrt((y 3a -y 3aj ) 2 +(z 3a -z 3aj ) 2 )。 9. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 8, characterized in that: Assume that after the jth iteration, P 3a and P 3b Overlap, and set the iteration termination condition to:

10. The robot obstacle avoidance method based on piecewise iterative tractrix according to claim 9, characterized in that: When the distance Δl3 between the two virtual discrete joints is less than the set value δ3 and meets the obstacle avoidance condition, the iteration ends and the inverse kinematics solution (α i ,β i ), and start the next obstacle avoidance motion planning, and finally reach the target position.

Images