CN109968358B - Redundant robot full-joint obstacle avoidance track optimization method considering motion stability - Google Patents

Abstract

The invention discloses a redundant robot full-joint obstacle avoidance track optimization method considering motion stability, and particularly relates to a method for carrying out interpolation in joint space by using a quintic polynomial to obtain a continuous and smooth joint motion speed and acceleration curve so as to ensure the motion stability of the actual operation of a robot. Meanwhile, taking a spherical enveloping obstacle as an example, a minimum distance model between the robot and the obstacle is established. And finally, establishing an obstacle avoidance track optimization model of the redundant robot by taking kinematics and obstacle avoidance indexes as constraint conditions, the minimum motion time as a target and the redundant joint angular displacement and time as variables, and providing a certain method basis for the safe and stable operation control of the industrial robot in a complex environment.

Description

Redundant robot full-joint obstacle avoidance track optimization method considering motion stability

Technical Field

The invention belongs to the field of redundant robot motion control research, and relates to a redundant robot full-joint obstacle avoidance track optimization method considering motion stability.

Background

The redundant robot is used as a robot with joint space dimension larger than task space dimension, has higher motion flexibility, and can meet other specific requirements under the condition of ensuring the motion performance, such as obstacle avoidance, singular configuration avoidance, joint torque optimization and the like. Therefore, the application of the redundant robot in industrial production is more and more widely required. In the face of complex obstacle environments, robot obstacle avoidance trajectory planning has become one of the research hotspots concerned by scholars at home and abroad. The obstacle avoidance trajectory planning means that under a given environmental condition, a barrier-free path pointing from an initial position to a target position is planned, and the safety and reliability of the robot motion are guaranteed. At present, two methods of robot obstacle avoidance are mainly a free space method and an artificial potential field method, wherein the former method mainly comprises four steps of establishing a C space, determining unit connectivity, determining a search method and optimizing a path, and can ensure the completeness of a solution, but the following problems exist when the problem of obstacle avoidance is solved: (1) because the C space needs to be obtained through discretization, the occupied memory amount is huge in order to meet the precision requirement; (2) when the robot joints are large, the free path search efficiency may be reduced. The above problems have resulted in a limited application of the free space method. The artificial potential field method comprises four steps of establishing a geometrical model of the obstacle, calculating the distance between the obstacle and the mechanical arm, defining a potential function and solving the driving force of the joint, and compared with a free space method, the method has stronger applicability to a dynamic obstacle environment, but still has the following problems: (1) under the condition of dense obstacles, the calculation result falls into local minimum due to the combined action of a plurality of attraction potential functions and repulsion potential functions, and the robot movement cannot stop before; (2) the obstacle geometric model needs to be simplified when the distance is solved, and the simplification process can cause waste of free space. In addition, the above two methods cannot ensure the movement efficiency and the movement stability. Therefore, the method for planning the redundant robot track is provided, the moving efficiency can be improved and the moving stability can be ensured while the obstacle is reliably avoided, and the method is a key problem to be solved by the patent.

Disclosure of Invention

The invention aims to provide a redundant robot track optimization method considering motion stationarity. The method is mainly characterized in that a continuous and smooth joint motion speed and acceleration curve is obtained by adopting a quintic polynomial interpolation algorithm, so that the motion stability is ensured, meanwhile, the kinematic parameters and the obstacle avoidance indexes are used as constraint conditions, the optimized track is obtained by taking the minimized motion time as a target, and the motion efficiency is improved.

The invention is realized by adopting the following technical means: a redundant robot full-joint obstacle avoidance track optimization method considering motion stability is achieved through the following steps that an initial value is obtained for the angular displacement of a redundant degree of freedom joint, and a redundant robot inverse solution model is established based on a momentum theory.

And mapping the target pose of the tail end of the robot in the operation space to a joint space based on the inverse solution model to obtain the final angular displacement corresponding to each joint, and performing interpolation operation between the initial angular displacement and the final angular displacement of each joint by using a quintic polynomial to obtain an angular displacement track point row of each joint.

According to the geometric structure of the robot, the robot is equivalent to a whole consisting of cylinders with different diameters, angular displacement track points of joints are taken as input, and the position of a motion path point of the central line endpoint of each cylinder is calculated based on a rotation theory.

And establishing a minimum distance model between the robot and the obstacle by taking the end point position of the central line of each cylinder and the obstacle position vector as parameters for predicting obstacle avoidance indexes at different motion moments.

And determining an optimization problem by taking the redundant joint angular displacement and the motion time as variables, simultaneously considering kinematic constraint and obstacle space constraint and taking the motion time as an optimization target.

Aiming at the optimization problem, an optimization algorithm (such as a particle swarm algorithm, a genetic algorithm and the like) is adopted to obtain an optimization track.

The invention has the characteristics that the provided track optimization method can ensure the motion stability of the robot and improve the motion efficiency while reliably avoiding the obstacle, and provides a certain method support for the reliable obstacle avoidance control of the redundant robot in the obstacle environment.

Drawings

FIG. 1 is a schematic view of an eight-axis robotic arm;

FIG. 2 is a schematic view of the rotational movement of the tip about joints 3, 4 and 5;

FIG. 3 is a schematic view of the rotational movement about joints 6 and 7;

FIG. 4 an optimized course of exercise time

FIG. 5 is a schematic diagram of an optimized trajectory;

fig. 6 is a graph of the minimum distance between the robot and the obstacle over time.

Detailed Description

The present invention will be described in detail below with reference to the accompanying drawings and examples.

A redundant robot full-joint obstacle avoidance track optimization method considering motion stability comprises the following implementation steps,

step (1) establishing a redundancy robot inverse solution model;

as shown in fig. 1, since the eight-axis robot has two redundant joints, in order to establish an inverse solution model,first the angular displacement theta of the joints 1 and 2₁And theta₂An initial value is taken, and then based on the initial condition, the problem is converted into an inverse solution problem of the six-axis robot.

a. Solving for theta₃、θ₄And theta₅；

Taking the intersection point of the axes of the joints 6, 7 and 8 as a nominal end, and taking the initial position vector as q_s＝[L₄ -L₁ -L₃ -L₅]The target position vector of the end is given as q according to the task requirements_e＝[x y z]Then, the movement around the joints 3, 4 and 5 based on the rotational theoretical end is described as follows,

in the formula

Index coordinate form for expressing motion rotation of joint i, and rotation vector omega is rotated by initial unit of joint i_iAnd the coordinate q of a point on the axis_iThe values are obtained by calculation and are shown in formulas (2) and (3). q. q.s_s' and q_e1' are respectively represented by q_s′＝[q_s 1]^TAnd

wherein q is_e′＝[q_e 1]^T.

In the diagram of the rotational movement (FIG. 2) c₁And c₂Representing the track intersection point of the tail end in the rotation process according to the geometrical relation of three rotation tracks (vector line segment around the axis)When rotating, the length of the rotating shaft is unchanged; the plane of the rotation trajectory is perpendicular to the rotation vector), the following system of equations is obtained,

let c₁And c₂The vector of the position of the point is c₁＝[x₁ y₁ z₁]And c₂＝[x₂ y₂ z₂]The variable x is represented by the simultaneous system of equations (4)₁,y₁,z₁,x₂,y₂And z₂The solution is as follows,

formula (III) A, B, A₁And B₁Is a constant associated with the motion parameter and the target position vector,

A＝(L₂+L₃)/L₄；

B＝[x_e1 ²+(L₁+y_e1)²+(L₂-z_e1)²-L₂ ²-L₅ ²+L₄ ²+L₃ ²]/(2L₄)；A₁＝A²+1；

B₁＝2(AB-L₂)；C₁＝B²+L₂ ²-x_e1 ²-(L₁+y_e1)²-(L₂-z_e1)²。

the inverse solution problem for joints 3, 4 and 5 translates into 3 Paden-Kahan subproblems 1,

angular displacement (theta) of joints 3, 4 and 5 based on the solution method of sub-problem 1₃、θ₄And theta₅) The solution is as follows,

b. solving for theta₆And theta₇

The rotational movement of the joints 6, 7 and 8 is shown as,

in the formula

Get a point q_s2＝[0 -L₁ -L₃ -L₅]This point is on the joint axis 8 and outside the joint axes 6 and 7, thus rotating around the joints 6 and 7 to q_e2(q_e2′＝g₁q_s2') the motion is described as follows,

in the diagram of the rotational movement (FIG. 3) c₃Representing point q_s2The following system of equations is obtained from the geometrical knowledge about the intersection of the rotational trajectories of the joints 6 and 7,

the coordinate vector of the track intersection point is obtained by solving

The movements around the joints 6 and 7 are indicated as follows,

in a similar way, based on the solution method of the Pasen-Kahan subproblem 1, theta₆And theta₇The solution is as follows,

c. solving for theta₈

Taking a point q outside the joint shaft 8_s3＝[L₄ 0 -L₃ -L₅]The rotational movement of the point about the axis 8 is then expressed as,

in the formula

Then based on the problem of 1, theta of the Paden-Kahan son₈As indicated by the general representation of the,

θ₈＝atan2[L₁(z_e3+L₃+L₅)，L₁(y_e3+L₁)] (14)

step (2) obtaining a joint space track by utilizing quintic polynomial interpolation;

solving the target angular displacement theta of the joint i corresponding to the end target pose through the above inverse solution model_i. Secondly, based on the quintic polynomial interpolation, the angular displacement, the angular velocity and the angular acceleration of the joint i are expressed as follows,

in order to ensure the continuity of the angular velocity and the angular acceleration of each joint and prevent the joint from vibrating due to the sudden change of joint variables when the joint is started or stopped, the following equation set is obtained,

in the formula t_eRepresenting the movement time.

By solving for the parameter a_1,i,a_2,i,a_3,i,a_4,i,a_5,i,a_6,iThe value of (a) is as follows,

finally, the angular displacement of the joint which can ensure the motion stability of each joint is expressed as follows,

θ_i(t)＝6θ_it⁵/t_e ⁵-15θ_it⁴/t_e ⁴+10θ_it³/t_e ³ (18)

calculating the position of a motion path point of the central line end point of each cylinder;

according to the eight-axis robot diagram (fig. 1), the robot can be equivalent to a mechanical structure consisting of five arms, and the arm i is equivalent to a radius R_i(mm) cylinders, the position coordinates at time t based on the momentum theory from the initial position of the centerline end of each cylinder are calculated as follows,

wherein J_i(t) and J_i+1(t) respectively represents the current position coordinates of the center line end point of the ith cylinder, and i is more than or equal to 1 and less than or equal to 5. Step (4), establishing a minimum distance model between the robot and the obstacle;

avoiding spherical obstacle, and setting the radius of the spherical obstacle as R_o(mm) with a coordinate of center position of O ═ x_o y_o z_o]Taking obstacle center O at the center line J of the arm lever i_iJ_i+1The upper vertical foot is O', the step (3) is takenCalculated centerline endpoint J_iAnd J_i+1Respectively, is J_i＝[x_J,i y_J,i z_J,i]And J_i+1＝[x_J,i+1 y_J,i+1 z_J,i+1]Then straight line J_iJ_i+1The parametric equation of (a) is expressed as follows,

wherein the parameter 0 ≦ η ≦ 1 indicates that the point is in the line segment J_iJ_i+1Upper, otherwise, the point is outside the line segment.

Let the coordinate position of the foot O 'be O' ═ x_o′ y_o′ z_o′]The following equation is used to obtain,

when the foot falls on the center line segment J_iJ_i+1Above, let O' ═ x_o′ y_o′ z_o′]At this time, the minimum distance between the arm i and the obstacle is represented as,

when the foot is not dropped on the center line segment J_iJ_i+1The minimum distance between the arm i and the obstacle is expressed as,

the minimum distance between the robot and the obstacle is expressed as,

d＝min[d_i|1≤i≤5] (24)

step (5) optimizing problem description;

in the obstacle avoidance trajectory planning problem, redundant joints are usedAngular displacement theta₁，θ₂And time of movement t_eFor optimizing variables, kinematic constraints (the available range of angular displacement, angular velocity and angular acceleration of each joint) and obstacle space constraints (i.e. the minimum distance d between the robot and the obstacle should not be less than the safety threshold d required by the task) are considered at the same time_tWith the goal of minimizing the motion time as an optimization, the optimization problem can be described as follows,

in the formula [ theta ]_i,min，θ_i,maxFor each of the joint angular displacement limit values,

and

respectively represents the maximum allowable value of angular velocity and angular acceleration of each joint, and i is more than or equal to 1 and less than or equal to 8.

Optimizing the track;

the following initial conditions are set, and the particle swarm algorithm is adopted to search the optimal track so as to ensure the motion performance.

a. The pose of the end target of the robot is taken as follows,

b. 3 spherical obstacles with center position coordinates O₁＝[300 -500 -200]； O₂＝[500 -500 50]And O₃＝[500 100 -50]The radii were all 10 mm. Taking a safety threshold d_t＝10mm。

c. The values of the kinematic parameters of the robot and the radius of the equivalent cylinder of the arm lever are shown in table 1,

table 18 axis robot related parameters

Parameter(s)

L1

L2

L3

L4

L5

L6

R1～R3

R4

R5

Value (mm)

300

300*tan15°

409-L2

55

364

30

25

20

10

d. The values of parameters in the optimization constraints are shown in table 2,

TABLE 2 optimization of constraint parameters

Based on the above initial conditions, an optimized course of the movement time, an optimized trajectory (as shown in fig. 4 and 5), and a variation curve of the minimum distance between each obstacle and the robot with the movement time (as shown in fig. 6) can be obtained. Table 3 shows the optimization results for each joint variable. Secondly, the optimal movement time is 4.4416s, and the minimum distance between the robot and the obstacle is 20 mm.

TABLE 3 optimization results of variables of each joint

Joint	1	2	3	4	5	6	7	8
									Angular displacement (degree)	-25.836	-48.37	68.485	-131.532	105.209	-108.979	3.667	35.519
Maximum angular velocity (°/s)	10.906	20.419	28.91	55.525	44.413	46.004	1.5479	14.994
									Maximum angular acceleration (°/s)2)	7.5433	14.123	19.996	38.403	30.718	31.818	1.0706	10.37

Claims (1)

1. A redundant robot full-joint obstacle avoidance track optimization method considering motion stability is characterized in that an initial value is taken for redundant degree-of-freedom joint angular displacement, and a redundant robot inverse solution model is established based on a momentum theory;

mapping the target pose of the tail end of the robot in the operation space to a joint space based on the inverse solution model to obtain the final angular displacement corresponding to each joint, and performing interpolation operation between the initial angular displacement and the final angular displacement of each joint by using a quintic polynomial to obtain an angular displacement track point row of each joint;

according to the geometric structure of the robot, the robot is equivalent to a whole consisting of cylinders with different diameters, angular displacement track points of joints are taken as input, and the position of a motion path point of the central line endpoint of each cylinder is calculated based on a rotation theory;

establishing a minimum distance model between the robot and the obstacle by taking the end point position of the central line of each cylinder and the obstacle position vector as parameters, wherein the minimum distance model is used for predicting obstacle avoidance indexes at different movement moments;

determining an optimization problem by taking redundant joint angular displacement and motion time as variables, simultaneously considering kinematic constraint and obstacle space constraint and taking the motion time as an optimization target;

the method is characterized in that: the concrete implementation steps are as follows,

step (1) establishing a redundancy robot inverse solution model;

since the eight-axis robot has two redundant joints, to build the inverse solution model, the angular displacement θ of joints 1 and 2 is first calculated₁And theta₂Taking an initial value, and converting the problem into an inverse solution problem of the six-axis robot based on an initial condition;

a. solving for theta₃、θ₄And theta₅；

Taking the intersection point of the axes of the joints 6, 7 and 8 as a nominal end, and taking the initial position vector as q_s＝[L₄ -L₁ -L₃ -L₅]Given a target location vector of the end as q according to task requirements_e＝[x y z]Then, based on the theoretical end of rotation, around joints 3, 4 and 5The movement is described as follows,

in the formula

Index coordinate form for expressing motion rotation of joint i, and rotation vector omega is rotated by initial unit of joint i_iAnd the coordinate q of a point on the axis_iThe values are obtained by calculation and are shown as formulas (2) and (3); q. q.s_s' and q_e1' are respectively represented by q_s′＝[q_s 1]^TAnd

wherein q is_e′＝[q_e 1]^T；

In a rotary motion c₁And c₂The track intersection point of the tail end in the rotating process is shown, and the length of the vector line segment is unchanged when the vector line segment rotates around the axis according to the geometric relation of the three rotating tracks; the rotation trajectory plane is perpendicular to the rotation vector, the following equation set is obtained,

let c₁And c₂The vector of the position of the point is c₁＝[x₁ y₁ z₁]And c₂＝[x₂ y₂ z₂]Disclosure of the inventionSimultaneous equations (4), variable x₁,y₁,z₁,x₂,y₂And z₂The solution is as follows,

formula (III) A, B, A₁And B₁Is a constant associated with the motion parameter and the target position vector,

A＝(L₂+L₃)/L₄；

B＝[x_e1 ²+(L₁+y_e1)²+(L₂-z_e1)²-L₂ ²-L₅ ²+L₄ ²+L₃ ²]/(2L₄)；A₁＝A²+1；

B₁＝2(AB-L₂)；C₁＝B²+L₂ ²-x_e1 ²-(L₁+y_e1)²-(L₂-z_e1)²；

the inverse solution problem for joints 3, 4 and 5 translates into 3 Paden-Kahan subproblems 1,

solving method based on sub-problem 1, angular displacement theta of joints 3, 4 and 5₃、θ₄And theta₅The solution is as follows,

b. solving for theta₆And theta₇

The rotational movement of the joints 6, 7 and 8 is shown as,

in the formula

Get a point q_s2＝[0 -L₁ -L₃ -L₅]This point is on the joint axis 8 and outside the joint axes 6 and 7, thus rotating around the joints 6 and 7 to q_e2，q_e2′＝g₁q_s2′，q_e2The movement of (a) is described as follows,

in a rotary motion c₃Representing point q_s2The following system of equations is obtained from the geometrical knowledge about the intersection of the rotational trajectories of the joints 6 and 7,

the coordinate vector of the track intersection point is obtained by solving

The movements around the joints 6 and 7 are indicated as follows,

in a similar way, based on the solution method of the Pasen-Kahan subproblem 1, theta₆And theta₇The solution is as follows,

c. solving for theta₈

Taking a point q outside the joint shaft 8_s3＝[L₄ 0 -L₃ -L₅]The rotational movement of this point about the joint axis 8 is then expressed as,

in the formula

Then based on the problem of 1, theta of the Paden-Kahan son₈As indicated by the general representation of the,

θ₈＝atan 2[L₁(z_e3+L₃+L₅)，L₁(y_e3+L₁)] (14)

step (2) obtaining a joint space track by utilizing quintic polynomial interpolation;

solving the target angular displacement theta of the joint i corresponding to the end target pose through the above inverse solution model_i(ii) a Secondly, based on the quintic polynomial interpolation, the angular displacement, the angular velocity and the angular acceleration of the joint i are expressed as follows,

in order to ensure the continuity of the angular velocity and the angular acceleration of each joint and prevent the joint from vibrating due to the sudden change of joint variables when the joint is started or stopped, the following equation set is obtained,

in the formula t_eRepresenting the movement time;

by solving forThe parameter a can be obtained_1,i,a_2,i,a_3,i,a_4,i,a_5,i,a_6,iThe value of (a) is as follows,

finally, the angular displacement of the joint which can ensure the motion stability of each joint is expressed as follows,

θ_i(t)＝6θ_it⁵/t_e ⁵-15θ_it⁴/t_e ⁴+10θ_it³/t_e ³ (18)

calculating the position of a motion path point of the central line end point of each cylinder;

the eight-axis robot can be equivalent to a mechanical structure formed by five arms, and the arm i is equivalent to a mechanical structure with the radius of R_i(mm) cylinders, the position coordinates at time t based on the momentum theory from the initial position of the centerline end of each cylinder are calculated as follows,

wherein J_i(t) and J_i+1(t) respectively representing the current position coordinates of the central line end point of the ith cylinder, wherein i is more than or equal to 1 and less than or equal to 5; step (4), establishing a minimum distance model between the robot and the obstacle;

avoid spherical obstacle, and have radius of R_o(mm) with a coordinate of center position of O ═ x_o y_o z_o]Taking the obstacle center O at the central line segment J of the arm lever i_iJ_i+1The upper vertical foot is O', and the central line end point J calculated in the step (3) is taken_iAnd J_i+1Respectively, is J_i＝[x_J,i y_J,i z_J,i]And J_i+1＝[x_J,i+1 y_J,i+1 z_J,i+1]Then straight line J_iJ_i+1The parametric equation of (a) is expressed as follows,

wherein the parameter 0 ≦ η ≦ 1 indicates that the point is in the center line segment J_iJ_i+1Upper, otherwise, point outside the center line segment;

let the coordinate position of the foot O 'be O' ═ x_o′ y_o′ z_o′]The following equation is used to obtain,

when the foot falls on the center line segment J_iJ_i+1Above, let O' ═ x_o′ y_o′ z_o′]At this time, the minimum distance between the arm i and the obstacle is represented as,

when the foot is not dropped on the center line segment J_iJ_i+1The minimum distance between the arm i and the obstacle is expressed as,

the minimum distance between the robot and the obstacle is expressed as,

d＝min[d_i|1≤i≤5] (24)

step (5) optimizing problem description;

in the problem of planning obstacle avoidance tracks, redundant joint angular displacement theta is adopted₁，θ₂And time of movement t_eFor optimizing variables, kinematic constraints and obstacle space constraints are considered at the same time, i.e. the minimum distance d between the robot and the obstacle should not be less than the safety threshold d required by the task_tWith the goal of minimizing the motion time as an optimization, the optimization problem can be described as follows,

f＝min{t_e}

in the formula [ theta ]_i,min，θ_i,maxFor each of the joint angular displacement limit values,

and

respectively representing the maximum allowable values of angular velocity and angular acceleration of each joint, wherein i is more than or equal to 1 and less than or equal to 8;

optimizing the track;

setting initial conditions, and searching for an optimal track by adopting a particle swarm algorithm to ensure the motion performance.

Images