CN113885535A

CN113885535A - Impact-constrained robot obstacle avoidance and time optimal trajectory planning method

Info

Publication number: CN113885535A
Application number: CN202111410601.6A
Authority: CN
Inventors: 张邦成; 赵航; 李也; 高智; 柳虹亮; 杨磊; 付昕亮; 常笑鹏; 邵昱博; 夏奇; 张帆
Original assignee: Changchun University of Technology
Current assignee: Changchun University of Technology
Priority date: 2021-11-25
Filing date: 2021-11-25
Publication date: 2022-01-04
Anticipated expiration: 2041-11-25
Also published as: CN113885535B

Abstract

The invention relates to a track planning method for an industrial robot, in particular to an impact-constrained robot obstacle avoidance and time optimal track planning method. The trajectory planning system can automatically plan a smooth trajectory meeting the limits of speed, acceleration and jerk on an off-line basis under the condition of inputting limited parameters. The system mainly comprises a path planning module, a spline interpolation module, an obstacle avoidance processing module and a time optimization track planning module. The path planning module introduces a target bias and target preference thought under a barrier repulsive force field, the spline interpolation module adopts a quintic spline interpolation function to carry out interpolation smoothing on key nodes of the path, the obstacle avoidance processing module carries out collision avoidance processing on the industrial robot and the barrier by using a geometric fine detection method, a robot connecting rod and a welding gun are enveloped by using a capsule body, the barrier is enveloped by using a sphere, and the time optimization track module combines a grading system and a greedy strategy with a traditional whale optimization algorithm, so that the global search capability of the algorithm is improved.

Description

Impact-constrained robot obstacle avoidance and time optimal trajectory planning method

Technical Field

The invention belongs to the field of industrial robot trajectory planning, and particularly relates to an impact-constrained robot obstacle avoidance and time optimal trajectory planning method.

Background

Industrial robots are machine devices that are organically combined by machinery, electronics, and computers, realize various industrial processing and manufacturing functions, and are widely used in various industrial fields such as flexible production lines and automated factories. At industrial field, only need for the operation starting and stopping point of industrial robot motion, the robot can accomplish corresponding action through a plurality of operation point, for the compliance of guaranteeing the motion between the robot starting and stopping point, reduces mechanical structure wearing and tearing to act fast under the prerequisite that satisfies each item index of servo motor, need carry out the orbit planning to the action process of robot.

The motion trail of the industrial robot directly influences the energy consumption and the working time of the industrial robot, and is a key technology for guaranteeing the execution efficiency and the running performance of the robot. Generally speaking, with the increase of the number of obstacles and the increase of the size of the obstacles in the operation space, the solving space between the operation points of the robot is also rapidly complicated, the time optimal trajectory planning problem can be converted into a complex nonlinear optimization process for solving the interval time of each control node, and the method for solving the problem comprises a genetic algorithm, an artificial bee colony algorithm, a frog leaping algorithm, a Memetic algorithm and the like. Because the factors considered by the trajectory planning are numerous and complex, it is difficult to manually obtain a reasonable trajectory.

In conclusion, the industrial robot and the structured obstacles are fully considered in the trajectory planning, an ideal trajectory which meets the requirements of operation tasks, is collision-free and has short movement time is planned, and the method is of great importance for improving the working efficiency of the industrial robot.

Disclosure of Invention

Aiming at the problem that an obstacle exists in the path planning of the industrial robot, the invention provides an integrated solution of obstacle avoidance and time optimal path planning of the industrial robot.

The purpose of the invention is realized by the following technical scheme: the system finishes the track planning of the industrial robot by adopting a modularized method, and mainly comprises a path planning module, a spline interpolation module, an obstacle avoidance processing module and a time optimization track planning module. On the basis of an RRT algorithm, a path planning module introduces a target bias and a target preference idea based on the safety constraint of an obstacle repulsive force field, compresses a search space and accelerates the search speed of the shortest path of the industrial robot; the spline interpolation module adopts a quintic spline interpolation function to carry out interpolation smoothing on the path key nodes, and an AT matrix is constructed by taking the initial and termination speeds and the initial and termination accelerations as fixed boundary conditions; the obstacle avoidance processing module performs collision avoidance processing on the industrial robot and the obstacle by using a geometric fine detection method, and obtains a robot joint and a base TCP point by using the forward and reverse kinematics of the robot, wherein the robot connecting rod and the welding gun are enveloped by using a capsule body, and the obstacle is enveloped by using a sphere; the time optimization track module combines a level system and a greedy strategy with a traditional whale optimization algorithm, and improves the global search capability of the algorithm.

An impact-constrained robot obstacle avoidance and time optimal trajectory planning method comprises the following steps:

s1: initializing an environment model, an obstacle environment and a robot model, and initializing the whale number, the iteration times and the initial population solution in the whale population;

s2: setting the posture of a welding gun and the initial step length of an improved RRT algorithm, setting the initial and final speeds of a Cartesian space of the robot to be 0m/s, and setting the initial and final accelerations to be 0m/s 2;

s3: inputting X, Y, Z axis range of working environment, inputting robot DH model parameters and inputting initial node q_initWith the target node q_goalThe speed, acceleration and jerk limit values of the TCP point of the input robot;

s4: to target node q according to obstacle_goalCalculating the range of the repulsive force field according to the distance and the number of the obstacles;

s5: judging whether the initial node and the termination node meet the specification, namely whether the initial node and the termination node are in an observation range, whether the robot at the position reversely solves the existence and whether the robot is out of the range of a repulsive force field of the obstacle, and whether the robot at the position collides with the obstacle;

s6: if the node is not in the observation range or in the repulsive field range of the obstacle, or the robot is not in inverse solution at the node, and the robot collides with the obstacle, repeating the step S2, otherwise, performing the step S7;

s7: finding a random node q in a random search tree_randNearest node q_nearest；

S8: determining new generation node q by target bias and target preference idea_new；

S9: judging new generation node q_newWhether the robot collides with the barrier or not and whether the robot has an inverse solution at the node or not;

s10: if a node q is newly generated_newIf the robot collides with the barrier or the robot does not have an inverse solution, abandoning the node, storing a new node with a lower dynamic step length coefficient, adding the new node into the random search tree, and jumping to the step S11, otherwise, directly jumping to the step S11;

s11: newly selecting and generating node q in Near field_newThe nearest node is used as a parent node q of a newly generated node_parent；

S12: computing from an initial node q_initFather node q_parentNew generation node q_newTo other q within Near neighborhood_nearFrom the original initial node q is judged_initNode q into Near neighborhood_nearWhether or not to generate a new node q than the way_newIs longer;

s13: if from the original initial node q_initNode q into Near neighborhood_nearDistance ratio of (q) path to newly generate node q_newIs longer, the node is formed as a new node q_nearAdding the child nodes into the random search tree, namely rewiring in Near neighborhood;

s14: judging new generation node q_newWith the target node q_goalWhether the distance of (d) is less than a prescribed distance;

s15: if a node q is newly generated_newWith the target node q_goalIs less than the prescribed distance, the target node q is connected_goalAdding the search tree into a random search tree, terminating the search process, otherwise jumping to the step S7, and entering the next search process;

s16: backtracking and extracting all nodes in a TCP point path of the robot soldering turret from the initial node to the termination node;

s17: the time interval between two adjacent nodes of the robot is used as the position dimension of whales in the improved whale optimization algorithm;

s18: calculating a cost function value of a whale optimization algorithm through a penalty function;

s19: judging random number xi₃Whether < 0.5 holds:

s20: if the random number xi₃If the result is less than 0.5, performing contraction circle search, and jumping to the step S21, otherwise, performing spiral search, and jumping to the step S25;

s21: judgment determinant

Whether the result is true or not;

s22: if determinant

If so, jumping to the step S23, otherwise, surrounding the current optimal whale position by the current whale contraction circle searching mode, and jumping to the step S25；

S23: introducing a grade system strategy to obtain the average position dimensionality of the optimal whale, the suboptimal whale and the optimal whale again in the current population, wherein the current whale surrounds the average position dimensionality in a shrinkage circle searching mode;

s24: introducing a greedy strategy idea, and only selecting a current cost optimal value from the updated position of the whale;

s25: judging whether the maximum iteration times is reached, if so, jumping to the step S26, otherwise, entering the next iteration search, and jumping to the step S18;

s26: and outputting the obstacle avoidance and time optimal smooth track of the industrial robot.

Due to the adoption of the technical scheme, the invention has the following beneficial effects:

the invention establishes a set of comprehensive industrial robot track optimization model, and limited parameters including X, Y, Z axis range of working environment, robot DH model parameters and initial node q are input_initWith the target node q_goalThe position of the robot, the limit values of the speed, the acceleration and the jerk of the TCP point of the robot, namely the obstacle avoidance and the time optimal smooth track of the robot can be automatically planned off line. The method provided by the invention has good reference and application values for engineering machinery automation and industrial robot technology, can limit the impact of the tail end of the robot, prolong the service life of the robot, improve the working efficiency of the robot and greatly shorten the preliminary planning and debugging time of engineers.

Drawings

FIG. 1 illustrates the RRT reselection optimization process of the present invention

FIG. 2 illustrates the RRT rewiring process of the present invention

FIG. 3 is a target bias and target preference expansion strategy based on repulsive force field

FIG. 4 shows a robot collision detection strategy based on geometric fine detection according to the present invention

FIG. 5 shows the location update strategy of the whale contraction circle search surrounding prey of the present invention

FIG. 6 is a flow chart of the time-optimal trajectory planning of the present invention

FIG. 7 is an X-axis position of a robotic end effector of the present invention

FIG. 8 is an X-axis velocity of a robotic end effector of the present invention

FIG. 9 is an X-axis acceleration of a robotic end effector of the present invention

FIG. 10 is an X-axis jerk of a robotic end effector of the present invention

FIG. 11 is a Y-axis position of a robotic end effector of the present invention

FIG. 12 is a Y-axis velocity of a robotic end effector of the present invention

FIG. 13 is a Y-axis acceleration of the robotic end effector of the present invention

FIG. 14 is a Y-axis jerk of a robotic end effector of the present invention

FIG. 15 is a Z-axis position of a robotic end effector of the present invention

FIG. 16 is a Z-axis velocity of a robotic end effector of the present invention

FIG. 17 is a Z-axis acceleration of a robotic end effector of the present invention

FIG. 18 illustrates Z-axis jerk of a robotic end effector of the present invention

FIG. 19 is an iterative evolution curve of cost values of the algorithm of the present invention

FIG. 20 is a comparison graph of the RRT, RRT and improved RRT algorithms of the present invention

Detailed Description

The invention is further described in detail with reference to the accompanying drawings, and the impact-constrained robot obstacle avoidance and time optimal trajectory planning method comprises a path planning module, a spline interpolation module, an obstacle avoidance processing module and a time optimal trajectory planning module.

The path planning module introduces a target bias and a target preference expansion idea to solve the problem of large operation amount when the RRT algorithm expands the nodes, and the newly generated node q_newThe method has strong randomness, although the optimal performance can be gradually achieved, the blind expansion strategy causes the waste of computing resources and greatly influences the convergence speed. The idea of target bias extension is to add targetsMarking point q_goalComponent in the direction, new node q_newTo q_goal and q_randThe target preference idea is to direct the target node q towards with a minimum probability_goalThe direction expands to be closer to the ideal path. Target bias policy and selection probability p of target preference policy_tAnd guiding the expansion direction of the random tree T, and expanding the random tree T to the target at a higher speed while keeping the randomness of the expanded tree.

Wherein eta is the initial step size of RRT growth,

is a neighboring node q_nearTo a random node q_randThe vector of (a) is determined,

is a neighboring node q_nearTo the target node q_goalThe vector rho is a dynamic adjustment coefficient of RRT, and a new node q is ensured_newApproaching to the target node q in direction and step length_goal。

Defining q in a repulsive field_newPoints can not be added into the random tree T, d is the minimum distance between the current node and the target node, and if the minimum distance satisfies | | q_new-q_goalIf the | is less than the d constraint, the approach to the target node is attempted.

wherein ,

is a repulsive force field coefficient and has positive correlation with the environmental scale. Rho is at R^cShortest distance, rho, between target node and obstacle in space₀The maximum influence action range of the obstacle, the value of the coefficient and the obstacleThe number of objects is related to the size.

When the action range of the repulsive force field covers the initial node q_initThe action range of the repulsive force field is narrowed until the initial node q_initOutside the action range of the repulsive force field, when the repulsive force field range covers the target node q_goalThe repulsive force field range is cleared.

A state space is a set of all possible states in a system, and is defined as X ∈ R^d，R^dRepresenting a d-dimensional space, d ∈ N, and generally d ≧ 2, X_freeRepresenting a collision free configuration space. The directed graph G (V, E) of the expanded random tree T consists of a vertex set V and an edge set E, wherein V is X_freeE is a V × V subset, and the directed path on G is a vertex sequence (V)₁,v₂,…,v_n) And edge set (v)_i,v_i+1) E is epsilon E, wherein i is more than or equal to 1 and less than or equal to n-1.

The Cartesian space planning can intuitively show the planning process of the robot, an environment information model of the Cartesian space needs to be established in the early stage of motion planning, 20 multiplied by 20 surfaces are used for representing the envelope sphere of the obstacle, and the combined set is xi_kIs shown, i.e.

A clear path in state space X can be defined in the form of a list σ: [0, t]Where t ∈ N, and for any τ ∈ R, σ (τ) ∈ X_freeAnd given an initial state of σ (0) ═ X_initAnd the target state is σ (t) ═ q_goal。

Let X_freeThe set of all directed graphs on is represented as

wherein ,

vertex X ∈ X_freeAnd the sequence number N belongs to N, and the Near function is defined as

A set of nodes V' close to x is returned,

(G, x, n) is defined as the set of all vertices of a closed hypersphere of radius r centered at x.

Wherein γ is a constant radius, ζ_dRepresenting the volume of a unit sphere in d-dimensional euclidean space.

At newly created node q_newNearby searching neighboring node q within defined radius r_nearAs alternative q_newAn alternative to the parent node. Returning the distance q by means of a Near function_newSet of nodes X on random tree T not exceeding radius r_near. Then at X_nearIn finding q_newThe optimal parent node of (1), i.e. sampling point q for all circular areas_near∈X_nearComparing q successively_initTo q_newQ of minimum cost_nearIt is referred to as q_newOf parent node q_parentAnd q is_newThe process of adding RRT to the random tree T to reselect the parent node is shown in fig. 1.

At q is_newReselecting parent node q_parentAnd then, rewiring the random tree T to further reduce the cost of connection between the nodes of the random tree T as much as possible. To X_nearExcept for parent node q_parentExcept all q_nearRewiring, i.e. for all q_near∈X_near-q_parentIf q is_initTo q_newTo q_nearDistance less than q_initTo q_parentTo q_nearBy a distance of (c), cutting off q_nearConnection to the original parent node, will (q)_new,q_near) And changing the original path as a new node relation. In order to collect information of space and obstacles when using the extended nodes, target bias and target preference strategies are adopted and collected in the exploration processTo accommodate tree expansion.

The spline interpolation module is a functional module for carrying out interpolation smoothing on some path key nodes which are generated by the path planning module and are close to the obstacle. On the premise of considering obstacle avoidance, in order to ensure the smoothness of the actual running track of the robot, the continuity of the speed and the acceleration, the invention adopts a quintic spline interpolation function to carry out smooth processing on the path.

For S (x) S₅(Π), given interval [ a, b]The upper n spline nodes a ═ x₁＜x₁＜…＜x_nA collision-free path in d-dimensional state space X may be defined in the form of a list σ: [0, t: [ b ]]For any τ ∈ R, there is σ (τ) ∈ X, and therefore σ (τ) is in dimension X₁,X₂,…,X_dThe lower interpolation can obtain a d-dimensional vector X uniquely determined by the system state_τ。

Introduction mark M_i＝S⁽⁴⁾(x_i)，A_i＝S″(x_i) I ∈ {1,2, …, n }, let h_i＝x_i+1-x_i，α_i＝x-x_iI ∈ {1,2, …, n-1} due to s⁽⁴⁾(x) In [ x ]_i,x_i+1]Is a linear function, therefore

Integrate equation (4) twice and substitute in s' (x)_i)＝A_i，s″(x_i+1)＝A_i+1In the interval [ x_i,x_i+1]Thereon is provided with

Integrate equation (5) twice and substitute for s (x)_i)＝y_i，s(x_i+1)＝y_i+1In the interval [ x_i,x_i+1]Thereon is provided with

On both sides of equation (6), the first derivative of x is

wherein ,s[x_i,x_i+1]＝(f(x_i+1)-f(x_i))/(x_i+1-x_i) The second derivative of x is calculated on both sides of equation (7)

From s' (x)_i+0)＝s″′(x_i-0) giving the relation between M and A and let λ_i＝h_i/6，μ_i＝2(λ_i-1+λ_i)，

l_i＝-(j_i+1-j_i) Then obtain

λ_i-1M_i-1+μ_iM_i+λ_iM_i+1+j_i-1A_i-1+l_iA_i+j_iA_i+1＝0 (9)

From s' (x)_i+0)＝s′(x_i-0) giving the relation between M and A and let

q_i＝8(p_i-1+p_i)/7，g_i＝f[x_i,x_i+1]-f[x_i,x_i-1]Then obtain

p_i-1M_i-1+q_iM_i+p_iM_i+1+λ_i-1T_i-1+μ_iT_i+λ_iT_i+1＝g_i (10)

Where i is 1,2, …, n-1, and equation (9) and equation (10) are combined to obtain an underdetermined equation set containing 2n unknowns 2n-4 equations.

To solve for 2n unknown parameters, 4 fixed boundary conditions are given, resulting in a deterministic solution to the system of equations.

wherein ,V₁ ^m，

And

the robot end effector corresponds to dimension X in X state space₁,X₂,…,X_dM e {1,2, …, d }.

Obtaining the 2n-2 unknowns M₁,M₂,A₂,…,A_n-1,M_nAnd a linear system of 2n-2 equations.

Where k is ∈ {2,3, …, n-3}, and the values of the elements in the 2k rows of the matrix C are as follows:

the values of the elements in the 2k +1 rows of the matrix C are as follows:

the values of the elements of the matrix G are as follows:

the other element values of the matrix G are as follows:

after supplementing 4 fixed boundary conditions V₁ ^m，

And

then, the four-order continuously-derivable conditional construction equation at the subinterval end point is solved by M_i,A_i(i∈{0,1,…,n-1})。

The obstacle avoidance processing module is a key link of track planning, and a new node q is added in the random tree T every time the RRT algorithm is improved_newAnd collision detection is required to generate an obstacle avoidance path. The robot geometric body modeling is a precondition for collision detection and distance calculation, and the collision detection and the distance calculation are a precondition for planning a collision-avoidance path. Aiming at the three-dimensional characteristics of the practical engineering environment obstacle, the robot connecting rod and the tail end actuator are enveloped by a capsule body, and the obstacle is enveloped by a sphere. The method not only makes the robot model and the obstacle model simpler, but also greatly reduces the calculation amount.

Setting the safe distance between the robot and the barrier as d_safeAnd solving the angle value of the joint q through the inverse kinematics of the robot, and if no solution exists, defaulting to the node unreachable. If the robot is at the new node q_newIf there are multiple sets of inverse solutions, then pass through the father node q_parentTo a new node q_newThe cost value of (2).

wherein ,C_pnIs a parent node q_parentTo a new node q_newWherein i ∈ [1, s)]The inverse kinematics solution of the robot has s groups.

And solving the coordinates of the end points of each connecting rod and the end effector of the robot through positive kinematics. Defining the robot connecting rod L and the barrier O as a point set, converting the collision detection problem of the connecting rod into whether the capsule body and the sphere collide or not, wherein the radius of the sphere is the radius R of the repulsive force field range of the barrier_repAnd the radius r of the connecting rod capsule body_sAnd (4) summing.

R＝R_rep+r_s (18)

The collision detection of the robot enveloping capsule body and the obstacle sphere is converted into the judgment of the position relation between the line segment and the sphere, and then the sphere center O and the line segment L_MNShortest distance d_minIs shown as

Wherein, O is the sphere center of the repulsive force field range of the obstacle, and M, N is the end point of the robot connecting rod.

Line segment L_MNThe expression of an included angle < OMN and an angle < ONM formed by the end point M, N and the sphere center O of the sphere is

New node q in cartesian space_newJoint state of and parent node q_parentIs defined as the euclidean distance of their joint space vectors.

The range of the repulsion field between the robot enveloping cylinder and the barrier can be closeWhat appears to be the positional relationship of the line segment to the ball, the line segment

And

enumerating line segment L_MNRelationship to sphere O:

s1: and judging whether the initial node and the termination node meet the specification, namely whether the initial node and the termination node are in an observation range, whether the robot at the position reversely solves the problem and whether the robot is in the range of a repulsive force field of the obstacle, and if the initial node and the termination node are not in the observation range, the reversely solves the problem or collide with the obstacle, rearranging the initial node and the termination node. And (3) obtaining the inverse solution group number n of the robot through inverse kinematics of the robot, wherein n is less than or equal to 8, and if n is 0, the robot does not have an inverse solution at the node, discarding the node. Computing a new node q_newEach set of inverse solutions of (a) and parent node q_parentAnd finding the joint inverse solution closest to the father node according to the distance cost of the angle value of each joint. The coordinates of the robot base origin, the robot joint origin and the end effector origin are obtained through positive kinematics, and the robot connecting rod is approximately regarded as a line segment L_MNAnd the line segment end points are used for solving each origin for positive motion.

S2: judging the line segment L_MNShortest distance d from the center of sphere_minThe magnitude relation with the sum of the repulsive force field range radius and the safety distance,

representing line segments

The shortest distance to the center of the sphere if

The robot does not collide with the obstacle; otherwise, S2 and S3 of collision detection are performed.

S3: judging the line segment L_MNWhether the end point M, N of (a) is within the sphere,

representing line segments

Endpoint N of₂The distance from the sphere center O of the sphere,

representing line segments

End point M of₂Distance from the sphere center O of the sphere, if line segment

Either end point is in the sphere and the sphere is provided with a ball,

the robot collides. If the line segment

Both end points are arranged in the sphere body,

the robot collides. If the line segment

Neither end point is within the sphere, S3 of collision detection is performed.

S4: line segment L_MNJudging the line segment L under the condition that the two end points are out of the sphere range_MNWhether or not to cross the sphere if the line segment

Two end points M of₃、N₃Angle OM between ball center O of ball body₃N₃And < ON₃M₃The angle is acute, the angle OMN is less than or equal to pi/2 and the angle ONM is less than or equal to pi/2, and the robot collides with the obstacle. If the line segment

Two end points M of₄、N₄Angle OM between ball center O of ball body₄N₄And < ON₄M₄When an obtuse angle exists, the angle OMN is more than pi/2U ONM is more than pi/2, and the robot does not collide with the obstacle.

The time optimization trajectory planning module sets the starting and stopping speed and the starting and stopping acceleration of the robot end effector and limits the speed, the acceleration and the jerk of the robot. The phenomenon that the tail end executor is damaged by impact of the tail end of the robot, and a transmission mechanism, a motor and a speed reducer at a joint of the robot are preferably considered, and the robot time optimal trajectory planning based on the robot motion constraint penalty function is described as follows:

wherein ,t_iIs the time interval between two nodes, n is the number of nodes, k_jIs a penalty function that associates a penalty with a violation of a constraint, c_jIs a scalar function and relates to the realization of the constraint imposed by the mechanical limits of the robot, and L represents the total number of the motion constraint conditions of the robot.

The motion constraints of the robot are as follows:

wherein sup (v) is a maximum velocity of the robot end effector, sup (a) is a maximum acceleration of the robot end effector, and sup (j) is a maximum impact of the robot end effector.

The whale optimization algorithm is a random swarm intelligence optimization algorithm constructed by simulating search foraging behaviors of whale contraction enclosure and spiral updating positions. The whale position information of the invention is the time interval between nodes and a coefficient vector A_iAnd C_iThe following equations (25) to (27) show.

a＝2-2t/t_max (25)

A_i＝2a·ξ₁-a (26)

C_i＝2·ξ₂ (27)

wherein ,ξ₁，ξ₂Is the interval [0,1]A decreases linearly from 2 to 0, t in an iterative process_maxThe maximum number of iterations is indicated.

In the development phase of the whale algorithm, the whale is held in a position to shrink the surrounding prey and the "9" shaped bubble network approaches the prey. Then

Can be described by a mathematical model of equation (28):

let xi₃Is [0,1 ]]According to a random decision probability factor, two position updating strategies of contracting circular enclosure and contracting spiral approach to a prey are based on xi₃Are selected with a probability of 50% each. X_p(t) individuals with the best position of the present whale population, X_i(t) is the current individual position of whale, A_i||C_iX_p(t)-X_i(t) | | is the shrink wrapping step length. b is a constant defining the shape of a logarithmic spiral, and l is [ -1,1 [)]So as to adjust the spiral direction and better search the optimal solution space. Xi₃Selecting a shrinkage enclosure prey when the shrinkage enclosure prey is less than 0.5, so that the algorithm converges to an optimal solution; when xi₃And when the value is more than or equal to 0.5, the contraction spiral is selected to be close to the prey, so that the situation that the local optimal solution is trapped by only using contraction enclosure is prevented.

The solution obtained in the whale optimization algorithm development stage may be a local optimal solution, so that a global optimal solution needs to be found by exploring other feasible solutions, namely the exploration stage of the whale optimization algorithm. In the exploration phase, the random search target is prevented from falling into the local optimal solution, and the formula (28) shows that A_i∈[-a,a]When not ventilatingA_iAnd when | ≧ 1, the search target is forced to be far away from the optimal solution. Randomly selecting other whale (non-current optimal solution) positions X by current whale_randAs target location, current whale X_iThe location update formula of (a) is:

X_i(t+1)＝X_rand(t)-A_i·||C_iX_rand(t)-X_i(t)|| (29)

the optimal position of other whales in the whale population is not considered in the whale optimization algorithm, a grade system strategy is introduced, and the optimal position { X ] in the first 3 whale populations is selected_1st,X_2nd,X_3rdAnd (6) obtaining a potential whale population optimal individual position, and obtaining a new position updating function:

wherein ,f_tot(t)＝f(X_1st(t))+f(X_2nd(t))+f(X_3rd(t))。

The search range of the whale optimization algorithm is expanded through a grading system, but the cost value of the updated position is not necessarily superior to that of the current position in the position updating process of the whale individual. In order to make the algorithm converge to the optimal solution, a greedy strategy is introduced to carry out position updating on the individuals with the optimal cost values. The location update function is

Interval [0,1]Probability factor xi of random decision₄Determining whether the whale position is updated or not, only when xi₄< 0.5 and f_tmp＞f_curOnly then a greedy strategy is used to search the population towards the optimal direction.

In order to verify the performance of the impact-constrained robot obstacle avoidance and time optimal trajectory planning method, the coordinates of the base of the robot are [0,0 ]]The x-axis range of the robotic workstation is [ -6,6 [ -6]The y-axis range is [ -6,6]Z is in the range of 1.5,5.5]. Is provided withThe coordinates of the sphere center of the enveloping sphere of the fixed obstacle are [ -0.1,1.6,2.2]And [0.4,1.5,2.2 ]]The radii were all 0.15m, and the safety distance between the obstacle and the robot was set to 0.05 m. The robot welding gun is in a posture of a deflection angle of-172.5463 degrees rotating around an x axis, a pitch angle of-35.9320 degrees rotating around a y axis and a roll angle of 77.4537 degrees rotating around a z axis. The initial position coordinate of the welding gun is [0.9,1.5,2.3 ]]The target position coordinate is [ -0.5,1.6,2.3 [)]. Default initial velocity V for quintic spline interpolation₁ ^mAnd termination rate

Is 0m/s, default initial acceleration

And terminating the acceleration

Is 0m/s²The crossover probability of the genetic algorithm is p_c0.1, the mutation probability is p_m0.05. The number of whales and individuals in the genetic algorithm population is 100, and the evolution algebra is 600.

It can be seen that the optimized path of the improved RRT algorithm of the impact-constrained robot obstacle avoidance and time optimal trajectory planning method is 15.96% shorter than that of the RRT algorithm, and compared with the progressively optimal RRT algorithm, the optimized path of the improved RRT algorithm is improved by a small margin, and the convergence speed is greatly improved. The improved whale optimization algorithm is superior to the traditional genetic algorithm, the track running time is reduced by 43.65%, and compared with the whale optimization algorithm, the track running time is reduced by 28.49%. The impact of the robot working system is effectively inhibited, the robot obstacle avoidance track with optimal time and continuous acceleration is obtained, and effective reference can be provided for robot track planning.

Claims

1. A robot obstacle avoidance and time optimal track planning method of impact constraint is characterized by comprising a path planning module, a spline interpolation module, an obstacle avoidance processing module and a time optimal track planning module, wherein the path planning module firstly introduces a target bias idea and a target bias idea based on the safety constraint of an obstacle repulsion field on the basis of RRT algorithm, compresses a search space and accelerates the search speed of the shortest path of an industrial robot, secondly, the spline interpolation module adopts a quintic spline interpolation function to carry out interpolation smoothing on key nodes of the path, an AT matrix is constructed by taking initial and terminal speeds and initial and terminal accelerations as fixed boundary conditions, and secondly, the obstacle avoidance processing module carries out obstacle avoidance processing on the industrial robot and the obstacle by using a geometric fine detection method, and obtains a joint, a joint and a time optimal track of the robot by using positive and negative kinematics of the robot, And (3) a base TCP point, wherein the robot connecting rod and the welding gun are enveloped by a capsule body, the barrier is enveloped by a sphere, and finally, a time optimization track module combines a grade system and a greedy strategy with a traditional whale optimization algorithm, so that the global search capability of the algorithm is improved.

2. The impact-constrained robot obstacle avoidance and time optimal trajectory planning method according to claim 1, characterized in that the path planning module introduces a target bias and a target preference expansion idea to solve the problem of large computation when the RRT algorithm expands the nodes, and newly generated nodes q are generated_newThe method has strong randomness, although the method can be gradually optimized, blind expansion strategy causes the waste of computing resources and greatly influences convergence speed, and the idea of target bias expansion is to add a target node q_goalComponent in the direction, new node q_newTo q_goal and q_randThe target preference idea is to direct the target node q towards with a minimum probability_goalDirection is expanded to be closer to the ideal path, the target bias strategy and the selection probability p of the target preference strategy_tAnd guiding the expansion direction of the random tree T, and expanding the random tree T to the target at a higher speed while keeping the randomness of the expanded tree.

Wherein eta is the initial step size of RRT growth,

wherein ,

is a repulsive force field coefficient and has positive correlation with the environmental scale, and rho is in R^cShortest distance, rho, between target node and obstacle in space₀The maximum influence action range of the obstacles is defined, the value of the coefficient is related to the number of the obstacles, and when the action range of the repulsive force field covers the initial node q_initThe action range of the repulsive force field is narrowed until the initial node q_initOutside the action range of the repulsive force field, when the repulsive force field range covers the target node q_goalThe repulsive force field range is cleared.

3. The impact-constrained robot obstacle avoidance and time optimal trajectory planning method according to claim 1, characterized in that the spline interpolation module is a functional module for performing interpolation smoothing on some path key nodes close to the obstacle generated by the path planning module.

Integrate equation (3) twice and substitute in s' (x)_i)＝A_i，s″(x_i+1)＝A_i+1In the interval [ x_i,x_i+1]Thereon is provided with

Integrate equation (4) twice and substitute into s (x)_i)＝y_i，s(x_i+1)＝y_i+1In the interval [ x_i,x_i+1]Thereon is provided with

On both sides of equation (5) there is a first derivative of x

wherein ,s[x_i,x_i+1]＝(f(x_i+1)-f(x_i))/(x_i+1-x_i) The second derivative of x is calculated on both sides of the formula (6)

l_i＝-(j_i+1-j_i) Then obtain

λ_i-1M_i-1+μ_iM_i+λ_iM_i+1+j_i-1A_i-1+l_iA_i+j_iA_i+1＝0 (8)

From s' (x)_i+0)＝s′(x_i-0) giving the relation between M and A and let

q_i＝8(p_i-1+p_i)/7，g_i＝f[x_i,x_i+1]-f[x_i,x_i-1]Then obtain

p_i-1M_i-1+q_iM_i+p_iM_i+1+λ_i-1T_i-1+μ_iT_i+λ_iT_i+1＝g_i (9)

Where i is 1,2, …, n-1, and after equation (8) and equation (9) are combined, an underdetermined equation set containing 2n unknowns and 2n-4 equations is obtained, and in order to find 2n unknowns, 4 fixed boundary conditions are given, thereby obtaining a definite solution to the equation set.

wherein ,V₁ ^m，

And

the robot end effector corresponds to dimension X in X state space₁,X₂,…,X_dM e {1,2, …, d } for 2n-2 unknowns M₁,M₂,A₂,…,A_n-1,M_nAnd a linear system of 2n-2 equations.

the values of the elements in the 2k +1 rows of the matrix C are as follows:

the values of the elements of the matrix G are as follows:

the other element values of the matrix G are as follows:

after supplementing 4 fixed boundary conditions V₁ ^m，

And

4. The impact-constrained robot obstacle avoidance and time optimal trajectory planning method according to claim 1, wherein the obstacle avoidance processing module sets a safe distance d between the robot and the obstacle_safeSolving the angle value of the joint q through inverse kinematics of the robot, if no solution exists, defaulting to be the node unreachable, and if the robot is at a new node q_newIf there are multiple sets of inverse solutions, then pass through the father node q_parentTo a new node q_newThe cost value of (2).

wherein ,C_pnIs a parent node q_parentTo a new node q_newWherein i ∈ [1, s)]The robot inverse kinematics is solved by s groups, the end point coordinates of each connecting rod and the end effector of the robot are solved by positive kinematics, the connecting rod L and the barrier O of the robot are defined as a point set, the collision detection problem of the connecting rods is converted into whether collision occurs between a capsule body and a sphere, and the radius of the sphere is the radius R of the repulsion field range of the barrier_repAnd the radius r of the connecting rod capsule body_sAnd (4) summing.

R＝R_rep+r_s (17)

Collision detection conversion judgment method for robot enveloping capsule body and barrier ball bodyThe position relationship between the broken line segment and the ball is determined by the center O and the line segment L_MNShortest distance d_minIs shown as

Wherein O is the sphere center of the repulsive force field range of the obstacle, M, N is the end point of the robot connecting rod, and the line segment L_MNThe expression of an included angle < OMN and an angle < ONM formed by the end point M, N and the sphere center O of the sphere is

The robot enveloping cylinder and the obstacle repulsion field range can be approximately seen as the position relation between a line segment and a ball, wherein the line segment

And

enumerating line segment L_MNRelationship to sphere O:

s1: judging whether the initial node and the termination node meet the specification, namely whether the nodes are in an observation range, whether the robot at the position reversely solves the existence and whether the robot is in an obstacle repulsion field range, and if the nodes are initially in the obstacle repulsion field rangeIf the node and the termination node are not in the observation range, the inverse solution does not exist or the node and the obstacle collide, the initial node and the termination node are rearranged, the inverse solution group number n of the robot is obtained through the inverse kinematics of the robot, n is less than or equal to 8, if n is 0, the robot does not have the inverse solution at the node, the node is abandoned, and a new node q is calculated_newEach set of inverse solutions of (a) and parent node q_parentFinding the inverse solution of the joint closest to the father node according to the distance cost of each joint angle value, solving the coordinates of the robot base origin, the robot joint origin and the end effector origin through positive kinematics, and approximately regarding the robot connecting rod as a line segment L_MNAnd the line segment end points are used for solving each origin for positive motion.

representing line segments

The shortest distance to the center of the sphere if

The robot does not collide with the obstacle, otherwise, collision detection S2 and S3 are performed.

representing line segments

Endpoint N of₂The distance from the sphere center O of the sphere,

representing line segments

Either end point is in the sphere and the sphere is provided with a ball,

the robot collides and if the line segment

Both end points are arranged in the sphere body,

the robot will collide and the line segment will be formed

Neither end point is within the sphere, S3 of collision detection is performed.

Two end points M of₃、N₃Angle OM between ball center O of ball body₃N₃And < ON₃M₃Are all acute angles, the angle OMN is less than or equal to pi/2 and the angle ONM is less than or equal to pi/2, the robot and the obstacle collide, if the line segment is

5. The impact-constrained robot obstacle avoidance and time optimal trajectory planning method according to claim 1, wherein the time optimal trajectory planning module sets an initial speed, an initial acceleration, a final speed and a final acceleration of a robot end effector, and limits the speed, the acceleration and the jerk of the robot, and the phenomena that the end effector is damaged by an impact at the end of the robot and a transmission mechanism, a motor and a speed reducer at a joint of the robot are considered preferentially, and the robot time optimal trajectory planning based on a robot motion constraint penalty function is described as follows:

wherein ,t_iIs the time interval between two nodes, n is the number of nodes, k_jIs a penalty function that associates a penalty with a violation of a constraint, c_jIs a scalar function and relates to the realization of the constraint applied by the mechanical limit of the robot, L represents the total number of the motion constraint conditions of the robot, and the motion constraint conditions of the robot are as follows:

The whale optimization algorithm is a random colony intelligent optimization algorithm constructed by simulating search foraging behaviors of whale contraction enclosure and spiral updating position, and the whale position information is time interval between nodes and coefficient vector A_iAnd C_iThe following equations (24) to (26) show.

a＝2-2t/t_max (24)

A_i＝2a·ξ₁-a (25)

C_i＝2·ξ₂ (26)

wherein ,ξ₁，ξ₂Is the interval [0,1]A decreases linearly from 2 to 0, t in an iterative process_maxRepresenting the maximum number of iterations, the development phase of the whale algorithm, the whale with an inching to contract around the prey and the "9" shaped bubble network approaching the prey, then

The location update mathematical model of is described as

Let xi₃Is [0,1 ]]According to a random decision probability factor, two position updating strategies of contracting circular enclosure and contracting spiral approach to a prey are based on xi₃Are selected with a probability of 50% each, X_p(t) individuals with the best position of the present whale population, X_i(t) is the current individual position of whale, A_i||C_iX_p(t)-X_i(t) | | is the shrink wrapping step size, b is a constant defining the logarithmic spiral shape, and l is [ -1,1 | ]]In order to adjust the helix direction, to better search the optimal solution space, ξ₃Selecting a shrink surround prey when < 0.5, such that the algorithm converges to an optimal solution when ξ₃And when the value is more than or equal to 0.5, the contraction spiral is selected to be close to the prey, so that the situation that the local optimal solution is trapped by only using contraction enclosure is prevented.

The solution obtained in the whale optimization algorithm development stage is probably a local optimal solution, so that the global optimal solution needs to be searched by exploring other feasible solutions, namely the exploration stage of the whale optimization algorithm, the exploration stage randomly searches for a target to prevent the target from being trapped in the local optimal solution, and the formula (27) shows that A_i∈[-a,a]When | A_iWhen | ≧ 1, the search target is forced to be far away from the optimal solution, and the current whale randomly selects the position X of other whales (not the current optimal solution)_randAs target location, current whale X_iThe location update formula of (a) is:

X_i(t+1)＝X_rand(t)-A_i·||C_iX_rand(t)-X_i(t)|| (28)

wherein ,f_tot(t)＝f(X_1st(t))+f(X_2nd(t))+f(X_3rd(t)), the search range of the whale optimization algorithm is expanded through a level system, but the cost value of the updated position is not necessarily superior to that of the current position in the position updating process of whale individuals, in order to enable the algorithm to converge to an optimal solution, a greedy strategy is introduced to update the position of the individuals with the optimal cost value, and a position updating function is