CN110948482A - Redundant robot trajectory planning method - Google Patents
Redundant robot trajectory planning method Download PDFInfo
- Publication number
- CN110948482A CN110948482A CN201911076827.XA CN201911076827A CN110948482A CN 110948482 A CN110948482 A CN 110948482A CN 201911076827 A CN201911076827 A CN 201911076827A CN 110948482 A CN110948482 A CN 110948482A
- Authority
- CN
- China
- Prior art keywords
- joint
- firefly
- redundant robot
- angular velocity
- updating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 70
- 241000254158 Lampyridae Species 0.000 claims abstract description 56
- 230000006870 function Effects 0.000 claims abstract description 45
- GNBHRKFJIUUOQI-UHFFFAOYSA-N fluorescein Chemical compound O1C(=O)C2=CC=CC=C2C21C1=CC=C(O)C=C1OC1=CC(O)=CC=C21 GNBHRKFJIUUOQI-UHFFFAOYSA-N 0.000 claims abstract description 32
- 239000012636 effector Substances 0.000 claims abstract description 24
- 230000007246 mechanism Effects 0.000 claims abstract description 10
- 230000010354 integration Effects 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000033001 locomotion Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 238000013459 approach Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 2
- 238000004519 manufacturing process Methods 0.000 abstract description 3
- 238000004088 simulation Methods 0.000 description 15
- 238000005457 optimization Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 7
- 238000013473 artificial intelligence Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000005065 mining Methods 0.000 description 2
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000029264 phototaxis Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
- Numerical Control (AREA)
Abstract
The invention relates to the field of intelligent manufacturing, in particular to a redundant robot trajectory planning method, which is characterized by comprising the following steps: the method comprises the steps of establishing a kinematics and dynamics model of the redundant robot, and obtaining a mathematical relation between the angular velocity of a joint and the pose of an end effector; and (3) solving a joint angle by adopting a closed-loop pseudo-inverse method, and planning a track: when the joint angle is solved, an objective function for constraining the redundant robot track planning is constructed, and the accumulated error generated during integration, namely the integration error, is corrected through the minimum value of the objective function solved by the firefly algorithm; the improved firefly algorithm comprises four steps of individual initialization, decision domain updating, location updating and fluorescein updating, wherein the location updating step adopts a generalized reverse learning mechanism to update the location. The method has the advantages of high robustness and accuracy, and strong real-time property and effectiveness.
Description
Technical Field
The invention relates to the field of intelligent manufacturing, in particular to a redundant robot trajectory planning method.
Background
With the development of robot technology, artificial intelligence, sensor technology and electronic information technology, industrial robots play an increasingly important role in the field of intelligent equipment manufacturing. Traditional industrial robots have been unable to meet more elaborate and complex task requirements. The redundant robot is an anthropomorphic mechanical arm, has better self-motion characteristic, is convenient to process the problems of moment optimization, joint limit avoidance and the like, and can better adapt to complex working conditions; generally, the number of joint degrees of freedom of a redundant robot is larger than the dimension of a description operation space, and infinite groups of solutions exist in inverse kinematics, so that the difficulty of application of the redundant robot is that a group of proper joint angles are selected through trajectory planning to complete a specific operation task, and indexes such as tracking accuracy, joint angular displacement and angular speed of the redundant robot meet requirements;
at present, a redundant robot trajectory planning method mainly comprises a numerical iteration method, a geometric method and an algebraic solution method; the numerical iteration method converts the inverse kinematics problem of the robot into a function optimization problem, and adopts an artificial intelligence algorithm to perform optimization calculation, so that the method is simple to operate, but has large calculation amount and low speed; the geometric method obtains an analytic solution under specific conditions by deducing the relationship between the pose of the end effector and each joint angle, has the advantages of high calculation speed and high calculation precision, but has a complex mathematical modeling process; the algebraic solution uses a Jacobian matrix and its deformation to solve the differential relationship between the joint velocity and the linear velocity and angular velocity of the end effector, and the closed-loop pseudo-inverse method (CLP), gradient descent method and the like are common, but the accumulated error in the solution process can affect the track tracking precision.
Disclosure of Invention
The invention aims to provide a redundant robot trajectory planning method which is high in robustness and accuracy and strong in instantaneity and effectiveness.
In order to solve the technical problems, the technical scheme of the invention is as follows: a redundant robot trajectory planning method comprises the following steps:
step 1: establishing a kinematics and dynamics model of the redundant robot to obtain a mathematical relation between the joint angular velocity and the pose of the end effector;
step 2: and (3) solving a joint angle by adopting a closed-loop pseudo-inverse method, and planning a track:
when the reference pose and the actual pose of the robot end effector are known, in one iteration, the joint angular velocity error is solved through the pseudo-inverse of a Jacobian matrix, and a compensation joint angle is obtained by integrating the joint angular velocity error; repeating iteration until the angular velocity error of the joint approaches to 0, realizing the inverse solution of the kinematics of the robot, solving the joint angle describing the motion track, and finishing the track planning;
in the step of integrating the joint angular velocity error to obtain a compensated joint angle, constructing a target function for restricting redundant robot track planning, and correcting an accumulated error generated in the integration, namely an integration error, by using the minimum value of the target function solved by a firefly algorithm;
the improved firefly algorithm comprises four steps of individual initialization, decision domain updating, location updating and fluorescein updating, wherein the location updating step adopts a generalized reverse learning mechanism to update the location.
According to the scheme, the step 1 specifically comprises the following steps:
step 1.1): establishing a forward kinematic equation of the redundant robot:
x=f(q) (1)
wherein n is the number of degrees of freedom of the redundant robot, q represents the joint angle of the redundant robot in the joint space, and q is [ q ]1,q2,…,qn]T(ii) a x represents the joint pose of the redundant robot end in the operation space, and x is [ x ]1,x2,…,xm]TWhere m is the number of operating space variables, m<n;
Step 1.2): differentiating the forward kinematic equation to generate a corresponding differential kinematic equation expressed as:
in the formula (I), the compound is shown in the specification,in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed, in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,j (q) is the Jacobian matrix for the redundant robot,a joint angle vector q (t) may be calculated with a given path point x (t);
step 1.3): calculating the angular velocity of the joint according to the differential kinematic equationObtaining angular velocity of jointA time-varying trajectory;
wherein the content of the first and second substances,the generalized inverse of the Jacobian matrix, the solution of which can make the module of the joint angular velocity obtain the local minimum;
step 1.4): for an n-dof redundant robot, the kinetic equation can be described as:
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,is a matrix containing Coriolis forces and centrifugal forces, G (q) e Rn×1Is a gravity matrix; q represents the angle of the joint angle,the angular velocity representing the angle of the joint,angular acceleration representing joint angle, pairDerived angular acceleration
According to the scheme, in the step 2, in the closed-loop pseudo-inverse method, the joint angle can be obtained by integral calculation according to the following formula:
wherein x isrefIs the reference pose of the end effector in the operating space, and x is the actual pose of the end effector in the operating space.
According to the scheme, in the step 2, the constraint conditions are 4, namely:
1) by an objective function f1The maximum joint change of the anterior joint and the posterior joint in the time domain is minimized, namely:
f1=max{[qn(k+1)-qn(k)]2} (6)
2) by an objective function f2The square sum of the joint angular velocities of the anterior joint and the posterior joint in the time domain is minimized, namely:
3) by an objective function f3The square sum of the joint angular accelerations of the anterior joint and the posterior joint in the time domain is minimized, namely:
4) by an objective function f4The square sum of the joint moments of the front joint and the rear joint in the time domain is minimized, namely:
assume the current position of the redundant robot end effector is Pc=(xc,yc,zc) Target positionIs set to Pf=(xf,yf,zf) Then position error PeComprises the following steps:
thus, the overall objective function in redundant robot trajectory planning is:
Fi=αPe+(1-α)fi,i=1,2,3,4 (11)
wherein α is 0.5.
According to the scheme, in the step 2, the position x of each firefly individual i in the firefly algorithm is improvedi(t) using an objective function f (x)i(t)) evaluation, namely the mechanical arm trajectory planning problem can be converted into a minimum problem of solving an objective function, and the improved firefly algorithm specifically comprises the following steps:
s1: individual initialization: randomly obtaining the position of firefly i (i ═ 1, …, M), namely:
xi(t)=xL+rand(0,1)·(xU-xL) (12)
in the formula, M is the population size, rand is a random function, and xU and xL are threshold values of the space to be searched respectively.
S2: and (3) updating a decision domain: each firefly transmits information to other peers in its domain according to the size of the decision domain, and the scope of the decision domain is updated according to equation (13), that is:
in the formula, ntβ is a weight value;represents the decision radius of the decision domain of the individual i at the t-th iteration and meets the conditionrsIs the maximum perceived radius; n is a radical ofi(t) is the t-th iterationThe neighborhood set of the time generation individual i has the following constraint conditions:
in the formula Ii(t) fluorescein value, x, of the current individualj(t) and lj(t) the location and fluorescein value of the neighborhood individual j, respectively;
s3: and (3) updating the position: if the fluorescein value of the neighborhood individual is larger than that of the current individual, the population moves to the neighborhood individual, and the probability that the firefly i moves to the firefly j is as follows:
the location of firefly i is updated according to the following equation:
where s is the step size of the move.
In the M-dimensional search space, xiTo be feasible, the generalized inverse learning mechanism GOLM is used to define its inverse solution as:
xi *=Δ-xi(17)
taking into account the bounding nature x of the location solution when updating the firefly locationi∈[xL,xU]Let Δ be k (x)U-xL) Then the following GOLM search strategy in the improved algorithm is defined:
wherein k is a random number between 0 and 1;
s4: and (3) updating fluorescein: the magnitude of the fluorescein value depends on the objective function f (x)i(t)) which updates the formula:
li(t)=(1-ρ)li(t-1)+γf(xi(t)) (19)
in the formula, rho epsilon (0,1) is a fluorescein volatilization value, and gamma is a fluorescein updating speed;
s5: out-of-cycle conditions: in the neighborhood set, when firefly i touches firefly j with a higher fluorescein value, if the distance between the two is smaller than the sensing radius, the firefly i uses the probability Pij(t) selecting and moving towards neighboring individuals; further, the firefly i updates the position and the fluorescein value until the iteration number of the algorithm reaches TmaxAnd recording and outputting the optimal solution.
The invention has the following beneficial effects:
aiming at the track planning in the redundant robot task space, a closed-loop pseudo-inverse method based on an improved firefly algorithm is provided, firstly, a kinematics and dynamics model of the redundant robot is established, and a mathematical relation between the joint angular velocity and the end effector pose is obtained; furthermore, a closed-loop pseudo-inverse method is introduced to solve the joint angle, the defect of the closed-loop pseudo-inverse method is made up by adopting an improved firefly algorithm, the performance of the closed-loop pseudo-inverse method is improved by the improved firefly algorithm, and the problem of trajectory planning of the redundant robot is solved; meanwhile, the generalized reverse learning mechanism is adopted to solve the problems that the traditional firefly algorithm is insufficient in overall mining capacity and easy to fall into a local optimal value, the convergence rate and the robustness of the firefly algorithm are improved, the algorithm can jump out of the local optimal constraint, the search performance is improved, and the method has good robustness and accuracy and strong instantaneity and effectiveness.
Drawings
FIG. 1 is an overall flow chart of an embodiment of the present invention;
FIG. 2 is a flowchart of step 2 of an embodiment of the present invention;
FIG. 3 is a diagram illustrating an overall structure of a redundant robot with an SRS structure according to an embodiment of the present invention;
FIG. 4 is a diagram showing the results of comparing the IGSO algorithm with the GSO algorithm and the IABC algorithm in example 1 according to the present invention;
FIG. 5 is a graph showing the convergence rate of the IGSO algorithm, the GSO algorithm and the IABC algorithm after comparing them in example 1;
FIG. 6 is a graph illustrating the optimization effects of comparing four objective functions by using IGSO, GSO and IABC algorithms in example 1 of the present invention;
FIG. 7 is a drawing of a straight-line trajectory planning using the IGSO algorithm in example 2 according to the present invention;
fig. 8 is a circular arc trajectory planning diagram of example 3 in which the IGSO algorithm is used.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
Referring to fig. 1 to 8, the present invention is a redundant robot trajectory planning method, which includes the steps of:
step 1: establishing a kinematics and dynamics model of the redundant robot to obtain a mathematical relation between the joint angular velocity and the pose of the end effector; the method specifically comprises the following steps:
step 1.1): suppose that the redundant robot has n degrees of freedom and the joint angle is q ═ q1,q2,…,qn]T(ii) a The number of operating space variables is m, and m<n, joint position x ═ x1,x2,…,xm]TThen, a forward kinematics equation for the redundant robot is established:
x=f(q) (1)
wherein n is the number of degrees of freedom of the redundant robot, q represents the joint angle of the redundant robot in the joint space, and q is [ q ]1,q2,…,qn]T(ii) a x represents the joint pose of the redundant robot end in the operation space, and x is [ x ]1,x2,…,xm]TWhere m is the number of operating space variables, m<n;
Step 1.2): differentiating the forward kinematic equation, and taking a derivative with respect to time to generate a corresponding differential kinematic equation expressed as:
in the formula (I), the compound is shown in the specification,in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed, in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,j (q) is the Jacobian matrix for the redundant robot,a joint angle vector q (t) may be calculated with a given path point x (t);
step 1.3): according to the differential kinematic equation (2), the angular velocity of the joint is reversely solvedObtaining angular velocity of jointA time-varying trajectory;
wherein the content of the first and second substances,the generalized inverse of the Jacobian matrix, the solution of which can make the module of the joint angular velocity obtain the local minimum;
step 1.4): for an n-dof redundant robot, the kinetic equation can be described as:
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,is a matrix containing Coriolis forces and centrifugal forces, G (q) e Rn×1Is a gravity matrix; q represents the angle of the joint angle,the angular velocity representing the angle of the joint,angular acceleration representing joint angle, pairDerived angular accelerationStep 2: and (3) solving a joint angle by adopting a closed-loop pseudo-inverse method, and planning a track:
for the trajectory planning in the redundant robot task space, the essence is to find a smooth and feasible path between the starting point and the target point of the end effector, knowing the reference pose and the actual pose of the end effector of the robot, in one iteration, solving the joint angular velocity error through the pseudo-inverse of the Jacobian matrix, and integrating the joint angular velocity error to obtain a compensated joint angle; repeating iteration until the angular velocity error of the joint approaches to 0, realizing the inverse solution of the kinematics of the robot, solving the joint angle describing the motion track, and finishing the track planning; in the closed-loop pseudo-inverse method, the joint angle can be obtained by integrating the following equation:
wherein x isrefIs the reference pose of the end effector in the operating space, and x is the actual pose of the end effector in the operating space.
When the closed loop pseudo-inverse GLP is integrated, an accumulated error is easy to generate, so that an inverse solution error is increased; therefore, an improved firefly algorithm (IGSO algorithm) is introduced to improve the performance of the GLP method and better solve the problem of the track planning of the redundant robot; the method specifically comprises the following steps: in the step of integrating the joint angular velocity error to obtain a compensated joint angle, constructing a target function for restricting redundant robot track planning, and correcting an accumulated error, namely an integral error, generated in the integration process through the target function by adopting an improved firefly algorithm; the improved firefly algorithm comprises four steps of individual initialization, decision domain updating, location updating and fluorescein updating, wherein the location updating step adopts a generalized reverse learning mechanism to update the location.
When constructing the objective function, the embodiment of the present invention considers the constraint conditions in 4:
1) by an objective function f1The maximum joint change of the anterior joint and the posterior joint in the time domain is minimized, namely:
f1=max{[qn(k+1)-qn(k)]2} (6)
2) by an objective function f2The square sum of the joint angular velocities of the anterior joint and the posterior joint in the time domain is minimized, namely:
3) by an objective function f3The square sum of the joint angular accelerations of the anterior joint and the posterior joint in the time domain is minimized, namely:
4) by an objective function f4The square sum of the joint moments of the front joint and the rear joint in the time domain is minimized, namely:
assume the current position of the redundant robot end effector is Pc=(xc,yc,zc) Target position is Pf=(xf,yf,zf) Then position error PeComprises the following steps:
thus, the overall objective function in redundant robot trajectory planning is:
Fi=αPe+(1-α)fi,i=1,2,3,4 (11)
wherein α represents the weight, and α is 0.5 according to the empirical value.
The performance of the CLP method is improved through an improved GSO algorithm (IGSO); the GSO algorithm is a heuristic artificial intelligence algorithm for simulating the phototaxis behavior of the firefly, and comprises four parts of initializing an individual, updating a decision domain, updating a position and updating fluorescein; position x of each firefly individual ii(t) using an objective function f (x)i(t)) evaluating, namely the mechanical arm track planning problem can be converted into a minimum value problem of solving an objective function; the detailed steps of the algorithm are described as follows:
s1: individual initialization: randomly obtaining the position of firefly i (i ═ 1, …, M), namely:
xi(t)=xL+rand(0,1)·(xU-xL) (12)
in the formula, M is the population size, rand is a random function, and xU and xL are threshold values of the space to be searched respectively.
S2: and (3) updating a decision domain: each firefly transmits information to other peers in its domain according to the size of the decision domain, and the scope of the decision domain is updated according to equation (13), that is:
in the formula, ntβ is a weight value;express anThe decision radius of the decision domain of the body i in the t-th iteration meets the conditionrsIs the maximum perceived radius; n is a radical ofi(t) is a neighborhood set of the individual i at the t-th iteration, and the constraint conditions are as follows:
in the formula Ii(t) fluorescein value, x, of the current individualj(t) and lj(t) the location and fluorescein value of the neighborhood individual j, respectively;
s3: and (3) updating the position: if the fluorescein value of the neighborhood individual is larger than that of the current individual, the population moves to the neighborhood individual, and the probability that the firefly i moves to the firefly j is as follows:
the location of firefly i is updated according to the following equation:
where s is the step size of the move.
In the step, a generalized inverse learning mechanism (GOLM) is adopted to solve the problems that the global exploitation capability is insufficient and the global exploitation capability is easy to fall into a local optimal value in the traditional firefly algorithm, namely a GSO algorithm; the idea of GOLM is that for any feasible solution, a reverse solution is calculated and evaluated, and then a better solution is selected from the reverse solution as a next generation individual; in the M-dimensional search space, xiTo be feasible, the generalized inverse learning mechanism GOLM is used to define its inverse solution as:
xi *=Δ-xi(17)
taking into account the bounding nature x of the location solution when updating the firefly locationi∈[xL,xU]Let Δ be k (x)U-xL) Then improve GOL in the algorithmThe M search strategy is defined as follows:
wherein k is a random number between 0 and 1;
s4: and (3) updating fluorescein: the magnitude of the fluorescein value depends on the objective function f (x)i(t)) which updates the formula:
li(t)=(1-ρ)li(t-1)+γf(xi(t)) (19)
in the formula, rho epsilon (0,1) is a fluorescein volatilization value, and gamma is a fluorescein updating speed;
s5: out-of-cycle conditions: in the neighborhood set, when firefly i touches firefly j with a higher fluorescein value, if the distance between the two is smaller than the sensing radius, the firefly i uses the probability Pij(t) selecting and moving towards neighboring individuals; further, the firefly i updates the position and the fluorescein value until the iteration number of the algorithm reaches TmaxAnd recording and outputting the optimal solution.
The following illustrates the simulation process of an embodiment of the invention:
generally, the main body structure of the redundant robot mainly shows four forms of SRS, RSS, UUS-A and UUS-B. The SRS structure can be more similar to the structural characteristics of human arms, so that a redundant robot with the modularized shoulder-wrist joint offset SRS structure is selected as a research object, as shown in FIG. 3; the robot adopts a modular reconfigurable design, the number of degrees of freedom can be adjusted by increasing or reducing connecting rods, and the improved DH parameters are shown in a table 1.
TABLE 1 improved DH parameters of SRS Structure redundant Robots
It should be noted here that obtaining the jacobian matrix and the pseudo-inverse matrix of the redundant robot is the prior art, and the process is not described again; the effectiveness of the SRS structure redundancy robot trajectory planning method based on the IGSO algorithm is verified by a simulation example.
Simulation example 1
In this example, the objective function is selected to be F4Target position is xref=[-0.5,0.5,1]TAnd solving the solution of the CLP method by an optimization algorithm without considering attitude influence. The simulation environment is Win 10Intel (R) core (TM) i7-4720HQ CPU @2.60GHz, and the parameter settings of the three algorithms are shown in Table 2 by adopting a conventional GSO algorithm and an IABC algorithm to compare and analyze with an IGSO algorithm under Matlab 2018b software.
TABLE 2 parameter settings for the three algorithms
Each algorithm runs 10 times, each iteration is performed for 50 generations, then the optimal result is selected and recorded, as shown in fig. 4 and 5, it can be seen that the three algorithms can enable the robot end effector to approach the reference position well, wherein the target function value obtained by the IGSO algorithm is 0.00521 which is 83.50% and 42.07% higher than that obtained by the GSO algorithm and the IABC algorithm respectively; meanwhile, as can be seen from fig. 5, the IGSO algorithm can obtain an ideal objective function value through a small number of iterations, the convergence rate is fast, the simulation time of the GSO algorithm is 1.63s, the simulation time of the IABC algorithm is 0.98s, and the simulation time of the IGSO algorithm is 0.70 s. Compared with other two algorithms, the optimization rate of the algorithm provided by the invention is respectively improved by 57.06% and 28.57%, which shows that the algorithm provided by the invention has high convergence rate and strong information mining capability.
Further, fig. 6 shows that the optimization effects of the four objective functions are compared on the three algorithms respectively, and the simulation conditions are the same as the above, and it can be seen from fig. 6 that the IGSO algorithm can help the GLP method to obtain the best optimization result no matter what objective function is adopted, which indicates that the algorithm provided by the present invention has better robustness.
Simulation example 2
Book calculatorIn the example, the starting point of the redundant robot end effector is [0, -1,0 ]]TThe target point is [ -1,0,1 [)]T. And (3) planning the track of the robot by combining an IGSO algorithm and a CLP method, so that the end effector walks on a straight line, the middle part has 20 path points, and the simulation time is 2 s. In order to ensure that the change of the joint angle of the robot in the motion process is continuous, an objective function is selected as F1The result of the trajectory planning is shown in fig. 7, and it can be seen from fig. 7 that the redundant robot can achieve smooth point-to-point motion.
Simulation example 3
In this example, the starting point of the redundant robot end effector is [0.8,0.6,0.4 ]]TThe target point is [1,0,0.4 ]]TUsing an objective function F3Planning the arc track of the target; the method provided by the invention is subjected to visual simulation in a Robotics Toolbox, the simulation time is 2s, and the result is shown in FIG. 8. As can be seen from FIG. 8, with the help of the method provided by the invention, a redundant robot with an SRS structure can walk a relatively smooth and continuous track, meanwhile, errors of the planned circular arc track on X, Y and the Z axis are given in Table 3, and the error indexes are all within the allowable error range of the actual operation of the robot.
TABLE 3 errors of circular arc trajectories
Error of the measurement | X axis | Y-axis | Z axis |
Maximum error | 0.027m | 0.013m | 0.005m |
Standard deviation of | 0.0059 | 0.0087 | 0.0072 |
The invention provides a closed-loop pseudo-inverse method based on an improved firefly algorithm in consideration of good universality and convenience for real-time control of an algebraic solution, and the closed-loop pseudo-inverse method is used for processing point-to-point motion, linear trajectory planning and circular arc trajectory planning of a redundant robot with an SRS structure; simulation results prove that the method has better robustness and accuracy; according to the invention, linear interpolation and circular interpolation are carried out on the redundant robot with the SRS structure, smooth and continuous tracks can be obtained, and certain referential property is provided for actual operation; the method adopts a generalized reverse learning mechanism to improve the traditional firefly algorithm and enhance the search performance of the firefly algorithm; simulation results prove that the optimization capability of the algorithm is stronger than that of a GSO algorithm and an IABC algorithm.
The foregoing is a more detailed description of the present invention that is presented in conjunction with specific embodiments, and the practice of the invention is not to be considered limited to those descriptions. For those skilled in the art to which the invention pertains, several simple deductions or substitutions can be made without departing from the spirit of the invention, and all shall be considered as belonging to the protection scope of the invention.
Claims (5)
1. A redundant robot trajectory planning method is characterized by comprising the following steps:
step 1: establishing a kinematics and dynamics model of the redundant robot to obtain a mathematical relation between the joint angular velocity and the pose of the end effector;
step 2: and (3) solving a joint angle by adopting a closed-loop pseudo-inverse method, and planning a track:
when the reference pose and the actual pose of the robot end effector are known, in one iteration, the joint angular velocity error is solved through the pseudo-inverse of a Jacobian matrix, and a compensation joint angle is obtained by integrating the joint angular velocity error; repeating iteration until the angular velocity error of the joint approaches to 0, realizing the inverse solution of the kinematics of the robot, solving the joint angle describing the motion track, and finishing the track planning;
in the step of integrating the joint angular velocity error to obtain a compensated joint angle, constructing a target function for restricting redundant robot track planning, and correcting an accumulated error generated in the integration, namely an integration error, by using the minimum value of the target function solved by a firefly algorithm;
the improved firefly algorithm comprises four steps of individual initialization, decision domain updating, location updating and fluorescein updating, wherein the location updating step adopts a generalized reverse learning mechanism to update the location.
2. The redundant robot trajectory planning method according to claim 1, wherein step 1 specifically comprises:
step 1.1): establishing a forward kinematic equation of the redundant robot:
x=f(q) (1)
wherein n is the number of degrees of freedom of the redundant robot, q represents the joint angle of the redundant robot in the joint space, and q is [ q ]1,q2,…,qn]T(ii) a x represents the joint pose of the redundant robot end in the operation space, and x is [ x ]1,x2,…,xm]TWhere m is the number of operating space variables, m<n;
Step 1.2): differentiating the forward kinematic equation to generate a corresponding differential kinematic equation expressed as:
in the formula (I), the compound is shown in the specification,in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed, in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,j (q) is the Jacobian matrix for the redundant robot,a joint angle vector q (t) may be calculated with a given path point x (t);
step 1.3): calculating the angular velocity of the joint according to the differential kinematic equationObtaining angular velocity of jointA time-varying trajectory;
wherein the content of the first and second substances,the generalized inverse of the Jacobian matrix, the solution of which can make the module of the joint angular velocity obtain the local minimum;
step 1.4): for an n-dof redundant robot, the kinetic equation can be described as:
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,is a matrix containing Coriolis forces and centrifugal forces, G (q) e Rn×1Is a gravity matrix; q represents the angle of the joint angle,the angular velocity representing the angle of the joint,angular acceleration representing joint angle, pairDerived angular acceleration
3. The redundant robot trajectory planning method according to claim 1, wherein in the step 2, in the closed-loop pseudo-inverse method, the joint angle is obtained by integral calculation as follows:
wherein x isrefIs the reference pose of the end effector in the operating space, and x is the actual pose of the end effector in the operating space.
4. The redundant robot trajectory planning method according to claim 1, wherein in the step 2, the constraint conditions are 4, namely:
1) by an objective function f1The maximum joint change of the anterior joint and the posterior joint in the time domain is minimized, namely:
f1=max{[qn(k+1)-qn(k)]2} (6)
2) by an objective function f2So that the joint angular velocity of the front joint and the rear joint in the time domainThe sum of the squares of the degrees is minimal, i.e.:
3) by an objective function f3The square sum of the joint angular accelerations of the anterior joint and the posterior joint in the time domain is minimized, namely:
4) by an objective function f4The square sum of the joint moments of the front joint and the rear joint in the time domain is minimized, namely:
assume the current position of the redundant robot end effector is Pc=(xc,yc,zc) Target position is Pf=(xf,yf,zf) Then position error PeComprises the following steps:
thus, the overall objective function in redundant robot trajectory planning is:
Fi=αPe+(1-α)fi,i=1,2,3,4 (11)
wherein α is 0.5.
5. The redundant robot trajectory planning method of claim 4, wherein in the step 2, the position x of each individual firefly i in the firefly algorithm is improvedi(t) using an objective function f (x)i(t)) evaluation, namely the mechanical arm trajectory planning problem can be converted into a minimum problem of solving an objective function, and the improved firefly algorithm specifically comprises the following steps:
s1: individual initialization: randomly obtaining the position of firefly i (i ═ 1, …, M), namely:
xi(t)=xL+rand(0,1)·(xU-xL) (12)
wherein M is the population size, rand is a random function, xUAnd xLRespectively, the threshold values of the space to be searched.
S2: and (3) updating a decision domain: each firefly transmits information to other peers in its domain according to the size of the decision domain, and the scope of the decision domain is updated according to equation (13), that is:
in the formula, ntβ is a weight value;represents the decision radius of the decision domain of the individual i at the t-th iteration and meets the conditionrsIs the maximum perceived radius; n is a radical ofi(t) is a neighborhood set of the individual i at the t-th iteration, and the constraint conditions are as follows:
in the formula Ii(t) fluorescein value, x, of the current individualj(t) and lj(t) the location and fluorescein value of the neighborhood individual j, respectively;
s3: and (3) updating the position: if the fluorescein value of the neighborhood individual is larger than that of the current individual, the population moves to the neighborhood individual, and the probability that the firefly i moves to the firefly j is as follows:
the location of firefly i is updated according to the following equation:
where s is the step size of the move.
In the M-dimensional search space, xiTo be feasible, the generalized inverse learning mechanism GOLM is used to define its inverse solution as:
xi *=Δ-xi(17)
taking into account the bounding nature x of the location solution when updating the firefly locationi∈[xL,xU]Let Δ be k (x)U-xL) Then the following GOLM search strategy in the improved algorithm is defined:
wherein k is a random number between 0 and 1;
s4: and (3) updating fluorescein: the magnitude of the fluorescein value depends on the objective function f (x)i(t)) which updates the formula:
li(t)=(1-ρ)li(t-1)+γf(xi(t)) (19)
in the formula, rho epsilon (0,1) is a fluorescein volatilization value, and gamma is a fluorescein updating speed;
s5: out-of-cycle conditions: in the neighborhood set, when firefly i touches firefly j with a higher fluorescein value, if the distance between the two is smaller than the sensing radius, the firefly i uses the probability Pij(t) selecting and moving towards neighboring individuals; further, the firefly i updates the position and the fluorescein value until the iteration number of the algorithm reaches TmaxAnd recording and outputting the optimal solution.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911076827.XA CN110948482A (en) | 2019-11-06 | 2019-11-06 | Redundant robot trajectory planning method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911076827.XA CN110948482A (en) | 2019-11-06 | 2019-11-06 | Redundant robot trajectory planning method |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110948482A true CN110948482A (en) | 2020-04-03 |
Family
ID=69976103
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911076827.XA Pending CN110948482A (en) | 2019-11-06 | 2019-11-06 | Redundant robot trajectory planning method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110948482A (en) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111538949A (en) * | 2020-07-10 | 2020-08-14 | 深圳市优必选科技股份有限公司 | Redundant robot inverse kinematics solving method and device and redundant robot |
CN111673742A (en) * | 2020-05-22 | 2020-09-18 | 江苏信息职业技术学院 | Industrial robot trajectory tracking control algorithm |
CN112596373A (en) * | 2020-10-27 | 2021-04-02 | 西北工业大学 | Unmanned aerial vehicle attitude control parameter intelligent setting method based on quantum firefly algorithm |
CN113043277A (en) * | 2021-04-01 | 2021-06-29 | 中联重科股份有限公司 | Multi-joint mechanism trajectory planning method and device, electronic equipment and storage medium |
CN113927584A (en) * | 2021-10-19 | 2022-01-14 | 深圳市优必选科技股份有限公司 | Robot control method, device, computer readable storage medium and robot |
CN114505865A (en) * | 2022-03-15 | 2022-05-17 | 上海大学 | Pose tracking-based mechanical arm path generation method and system |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104042344A (en) * | 2013-03-15 | 2014-09-17 | 三星电子株式会社 | Robot and method of controlling the same |
CN105005301A (en) * | 2015-05-25 | 2015-10-28 | 湘潭大学 | Method for planning operation point sequence and path of industrial robot based on swarm intelligence algorithm |
CN107538493A (en) * | 2017-10-20 | 2018-01-05 | 湘潭大学 | A kind of industrial robot method for planning track for avoiding cable interference constraint |
CN109822571A (en) * | 2019-02-18 | 2019-05-31 | 中国铁建重工集团有限公司 | A kind of assembling machine Mechanical arm control method, device and equipment |
CN110104216A (en) * | 2019-01-28 | 2019-08-09 | 西北工业大学深圳研究院 | A kind of collaboration paths planning method for kinematic redundancy dual-arm space robot |
CN110103220A (en) * | 2019-05-20 | 2019-08-09 | 华南理工大学 | Robot high-speed, high precision motion trail planning method, device, equipment and medium |
CN110209048A (en) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | Robot time optimal trajectory planning method, equipment based on kinetic model |
-
2019
- 2019-11-06 CN CN201911076827.XA patent/CN110948482A/en active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104042344A (en) * | 2013-03-15 | 2014-09-17 | 三星电子株式会社 | Robot and method of controlling the same |
CN105005301A (en) * | 2015-05-25 | 2015-10-28 | 湘潭大学 | Method for planning operation point sequence and path of industrial robot based on swarm intelligence algorithm |
CN107538493A (en) * | 2017-10-20 | 2018-01-05 | 湘潭大学 | A kind of industrial robot method for planning track for avoiding cable interference constraint |
CN110104216A (en) * | 2019-01-28 | 2019-08-09 | 西北工业大学深圳研究院 | A kind of collaboration paths planning method for kinematic redundancy dual-arm space robot |
CN109822571A (en) * | 2019-02-18 | 2019-05-31 | 中国铁建重工集团有限公司 | A kind of assembling machine Mechanical arm control method, device and equipment |
CN110103220A (en) * | 2019-05-20 | 2019-08-09 | 华南理工大学 | Robot high-speed, high precision motion trail planning method, device, equipment and medium |
CN110209048A (en) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | Robot time optimal trajectory planning method, equipment based on kinetic model |
Non-Patent Citations (4)
Title |
---|
HUI WANG等: "Enhancing particle swarm optimization using generalized opposition-based learning", 《INFORMATION SCIENCES》 * |
MARIA DA GRAÇA MARCOS等: "Trajectory planning of redundant manipulators using genetic algorithms", 《COMMUNICATIONS IN NONLINEAR SCIENCE AND NUMERICAL SIMULATION》 * |
罗天洪等: "基于情景萤火虫算法的机器人路径规划", 《计算机应用》 * |
罗天洪等: "基于时变萤火虫群算法的冗余机器人手臂逆解", 《计算机集成制造***》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111673742A (en) * | 2020-05-22 | 2020-09-18 | 江苏信息职业技术学院 | Industrial robot trajectory tracking control algorithm |
CN111538949A (en) * | 2020-07-10 | 2020-08-14 | 深圳市优必选科技股份有限公司 | Redundant robot inverse kinematics solving method and device and redundant robot |
CN111538949B (en) * | 2020-07-10 | 2020-10-16 | 深圳市优必选科技股份有限公司 | Redundant robot inverse kinematics solving method and device and redundant robot |
CN112596373A (en) * | 2020-10-27 | 2021-04-02 | 西北工业大学 | Unmanned aerial vehicle attitude control parameter intelligent setting method based on quantum firefly algorithm |
CN112596373B (en) * | 2020-10-27 | 2023-05-23 | 西北工业大学 | Unmanned aerial vehicle attitude control parameter intelligent setting method based on quantum firefly algorithm |
CN113043277A (en) * | 2021-04-01 | 2021-06-29 | 中联重科股份有限公司 | Multi-joint mechanism trajectory planning method and device, electronic equipment and storage medium |
CN113043277B (en) * | 2021-04-01 | 2022-06-07 | 中联重科股份有限公司 | Multi-joint mechanism trajectory planning method and device, electronic equipment and storage medium |
CN113927584A (en) * | 2021-10-19 | 2022-01-14 | 深圳市优必选科技股份有限公司 | Robot control method, device, computer readable storage medium and robot |
CN114505865A (en) * | 2022-03-15 | 2022-05-17 | 上海大学 | Pose tracking-based mechanical arm path generation method and system |
CN114505865B (en) * | 2022-03-15 | 2024-06-25 | 上海大学 | Pose tracking-based mechanical arm path generation method and system |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110948482A (en) | Redundant robot trajectory planning method | |
CN111538949B (en) | Redundant robot inverse kinematics solving method and device and redundant robot | |
Xu et al. | Dynamic neural networks based kinematic control for redundant manipulators with model uncertainties | |
Tang et al. | Swarm robots search for multiple targets based on an improved grouping strategy | |
Liu et al. | Obstacle avoidance path planning of space manipulator based on improved artificial potential field method | |
WO2007076119A2 (en) | Reconstruction, retargetting, tracking, and estimation of pose of articulated systems | |
CN104076685B (en) | A kind of space manipulator paths planning method for reducing pedestal attitude disturbance | |
CN114995479A (en) | Parameter control method of quadruped robot virtual model controller based on reinforcement learning | |
Xu et al. | Robot trajectory tracking control using learning from demonstration method | |
CN110244714A (en) | Robot list leg swing phase double-closed-loop control method based on sliding formwork control | |
Kon et al. | Mixed integer programming-based semiautonomous step climbing of a snake robot considering sensing strategy | |
US20230359207A1 (en) | Trajectory planning method, computer-readable storage medium, and robot | |
CN114932549A (en) | Motion planning method and device of spatial redundant mechanical arm | |
CN113858207B (en) | Inverted floor control method and device for humanoid robot, electronic equipment and storage medium | |
CN112684709B (en) | Cluster tracking kinematics modeling method, device and storage medium | |
Yunqiang et al. | Research on multi-objective path planning of a robot based on artificial potential field method | |
Cisneros et al. | Partial yaw moment compensation using an optimization-based multi-objective motion solver | |
Dubey et al. | Path planning of mobile robot using reinforcement based artificial neural network | |
WO2024021744A1 (en) | Method and apparatus for controlling legged robot, electronic device, computer-readable storage medium, computer program product and legged robot | |
CN116551669A (en) | Dynamic jump and balance control method for humanoid robot, electronic equipment and medium | |
CN112380655B (en) | Robot inverse kinematics solving method based on RS-CMSA algorithm | |
CN115781666A (en) | Control method for robot whole body simulation system | |
Auzan et al. | Humanoid Walking Control Using LQR and ANFIS | |
Bidokhti et al. | Direct kinematics solution of 3-rrr robot by using two different artificial neural networks | |
Wang et al. | Type-2 Fuzzy Logic Controller Using SRUKF-Based State Estimations for Biped Walking Robots. |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200403 |
|
RJ01 | Rejection of invention patent application after publication |