CN110948482A - Redundant robot trajectory planning method - Google Patents

Redundant robot trajectory planning method Download PDF

Info

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
Application number
CN201911076827.XA
Other languages
Chinese (zh)
Inventor
郑雪芳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu Vocational College of Information Technology
Original Assignee
Jiangsu Vocational College of Information Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Jiangsu Vocational College of Information Technology filed Critical Jiangsu Vocational College of Information Technology
Priority to CN201911076827.XA priority Critical patent/CN110948482A/en
Publication of CN110948482A publication Critical patent/CN110948482A/en
Pending legal-status Critical Current

Links

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

Redundant robot trajectory planning method
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:
Figure BDA0002262727130000021
in the formula (I), the compound is shown in the specification,
Figure BDA0002262727130000022
in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed,
Figure BDA0002262727130000023
Figure BDA0002262727130000024
in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,
Figure BDA0002262727130000025
j (q) is the Jacobian matrix for the redundant robot,
Figure BDA0002262727130000026
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 equation
Figure BDA0002262727130000027
Obtaining angular velocity of joint
Figure BDA0002262727130000028
A time-varying trajectory;
Figure BDA0002262727130000029
wherein the content of the first and second substances,
Figure BDA00022627271300000210
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:
Figure BDA00022627271300000211
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,
Figure BDA00022627271300000212
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,
Figure BDA00022627271300000213
the angular velocity representing the angle of the joint,
Figure BDA00022627271300000214
angular acceleration representing joint angle, pair
Figure BDA00022627271300000215
Derived angular acceleration
Figure BDA00022627271300000216
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:
Figure BDA00022627271300000217
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:
Figure BDA0002262727130000031
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:
Figure BDA0002262727130000032
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:
Figure BDA0002262727130000033
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:
Figure BDA0002262727130000034
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:
Figure BDA0002262727130000035
in the formula, ntβ is a weight value;
Figure BDA0002262727130000036
represents the decision radius of the decision domain of the individual i at the t-th iteration and meets the condition
Figure BDA0002262727130000037
rsIs 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:
Figure BDA0002262727130000041
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:
Figure BDA0002262727130000042
the location of firefly i is updated according to the following equation:
Figure BDA0002262727130000043
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:
Figure BDA0002262727130000044
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:
Figure BDA0002262727130000061
in the formula (I), the compound is shown in the specification,
Figure BDA0002262727130000062
in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed,
Figure BDA0002262727130000063
Figure BDA0002262727130000064
in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,
Figure BDA0002262727130000065
j (q) is the Jacobian matrix for the redundant robot,
Figure BDA0002262727130000066
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 solved
Figure BDA0002262727130000067
Obtaining angular velocity of joint
Figure BDA0002262727130000068
A time-varying trajectory;
Figure BDA0002262727130000069
wherein the content of the first and second substances,
Figure BDA00022627271300000610
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:
Figure BDA00022627271300000611
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,
Figure BDA00022627271300000612
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,
Figure BDA00022627271300000613
the angular velocity representing the angle of the joint,
Figure BDA00022627271300000614
angular acceleration representing joint angle, pair
Figure BDA00022627271300000615
Derived angular acceleration
Figure BDA00022627271300000616
Step 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:
Figure BDA00022627271300000617
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:
Figure BDA0002262727130000071
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:
Figure BDA0002262727130000072
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:
Figure BDA0002262727130000073
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:
Figure BDA0002262727130000074
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:
Figure BDA0002262727130000081
in the formula, ntβ is a weight value;
Figure BDA0002262727130000082
express anThe decision radius of the decision domain of the body i in the t-th iteration meets the condition
Figure BDA0002262727130000083
rsIs 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:
Figure BDA0002262727130000084
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:
Figure BDA0002262727130000085
the location of firefly i is updated according to the following equation:
Figure BDA0002262727130000086
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:
Figure BDA0002262727130000087
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
Figure BDA0002262727130000091
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
Figure BDA0002262727130000092
Figure BDA0002262727130000101
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:
Figure FDA0002262727120000011
in the formula (I), the compound is shown in the specification,
Figure FDA0002262727120000012
in order to redundancy the speed of the robot end in the operating space, i.e. the operating speed,
Figure FDA0002262727120000013
Figure FDA0002262727120000014
in order to redundancy the angular velocity of the robot degrees of freedom in the joint space, i.e. the joint angular velocity,
Figure FDA0002262727120000015
j (q) is the Jacobian matrix for the redundant robot,
Figure FDA0002262727120000016
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 equation
Figure FDA0002262727120000017
Obtaining angular velocity of joint
Figure FDA0002262727120000018
A time-varying trajectory;
Figure FDA0002262727120000019
wherein the content of the first and second substances,
Figure FDA00022627271200000110
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:
Figure FDA00022627271200000111
wherein T ∈ Rn×1For moment vectors, M (q) e Rn×nIs a matrix of the inertia, and the inertia matrix,
Figure FDA00022627271200000112
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,
Figure FDA0002262727120000021
the angular velocity representing the angle of the joint,
Figure FDA0002262727120000022
angular acceleration representing joint angle, pair
Figure FDA0002262727120000023
Derived angular acceleration
Figure FDA0002262727120000024
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:
Figure FDA0002262727120000025
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.:
Figure FDA0002262727120000026
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:
Figure FDA0002262727120000027
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:
Figure FDA0002262727120000028
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:
Figure FDA0002262727120000029
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:
Figure FDA0002262727120000031
in the formula, ntβ is a weight value;
Figure FDA0002262727120000032
represents the decision radius of the decision domain of the individual i at the t-th iteration and meets the condition
Figure FDA0002262727120000033
rsIs 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:
Figure FDA0002262727120000034
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:
Figure FDA0002262727120000035
the location of firefly i is updated according to the following equation:
Figure FDA0002262727120000036
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:
Figure FDA0002262727120000037
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.
CN201911076827.XA 2019-11-06 2019-11-06 Redundant robot trajectory planning method Pending CN110948482A (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (7)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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