CN113043284A - Multi-constraint inverse solution method for redundant robot - Google Patents

Multi-constraint inverse solution method for redundant robot Download PDF

Info

Publication number
CN113043284A
CN113043284A CN202110443343.5A CN202110443343A CN113043284A CN 113043284 A CN113043284 A CN 113043284A CN 202110443343 A CN202110443343 A CN 202110443343A CN 113043284 A CN113043284 A CN 113043284A
Authority
CN
China
Prior art keywords
robot
joint
optimization criterion
angle
continuous
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.)
Granted
Application number
CN202110443343.5A
Other languages
Chinese (zh)
Other versions
CN113043284B (en
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 University of Technology
Original Assignee
Jiangsu University of 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 University of Technology filed Critical Jiangsu University of Technology
Priority to CN202110443343.5A priority Critical patent/CN113043284B/en
Publication of CN113043284A publication Critical patent/CN113043284A/en
Application granted granted Critical
Publication of CN113043284B publication Critical patent/CN113043284B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1643—Programme controls characterised by the control loop redundant control
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a multi-constraint inverse solution method for a redundant robot, which has the technical scheme key points that: step1, establishing a continuous weighting matrix related to the joint angle of the robot; step2, constructing a repulsive velocity potential field which pushes the joint away from a limit angle; step3, establishing a weighted gradient projection method redundant robot inverse solution; step4, defining an optimization criterion function to regularize continuous variable coefficients; step5, designing an optimization criterion function regularization processing principle; in order to better realize joint angle avoidance limit, the invention provides a continuous weighting matrix and a repulsion velocity potential field function, which push away a robot joint from the joint angle limit of the robot, and maintain the redundancy characteristic of a robot system; the invention provides an optimization criterion function regularization processing principle for calculating the scalar coefficient of the optimization criterion function on line in real time, so that the scalar coefficient is prevented from being determined by a gradient projection method through a large amount of simulation, and the working efficiency of the algorithm is improved.

Description

Multi-constraint inverse solution method for redundant robot
Technical Field
The invention relates to the field of robot motion control, in particular to a multi-constraint inverse solution method for a redundant robot.
Background
Generally, a robot with redundant motion has redundant degrees of freedom relative to an operation task, constraint tasks such as joint angle avoidance limit, collision avoidance, obstacle avoidance and singular pose avoidance can be realized on a kinematic level under the condition that the operation of a robot terminal task is not influenced, and constraint tasks such as joint torque avoidance limit and energy optimization can also be realized on a dynamic level. However, due to the existence of redundant degrees of freedom, each end pose of the robot corresponds to countless sets of joint angle positions under the action of the underdetermined jacobian matrix, so that an optimal set of joint positions is required to be searched under constraint conditions to solve the redundant inverse kinematics of the robot.
At present, the common inverse kinematics methods for the redundant robot include a gradient projection method and a weighted least square method, but the gradient projection method and the weighted least square method have serious algorithm defects. In the gradient projection method, a constraint task is used as an optimization criterion function, the selection of scalar coefficients of the constraint task is usually based on experience and repeated tests, if the selection of the coefficients is improper, the performance of the constraint task cannot be guaranteed, and the existing method still does not completely solve the problem of the optimization criterion function scalar coefficient selection optimization in the gradient projection method. On the other hand, joint avoidance angle limit is a constraint problem which is primarily considered for redundant robot motion control, while the weighted least square method can solve the problem of redundant robot joint avoidance angle limit, but the joint angular velocity solved by the weighted least square method depends on a weight factor. That is, when the robot joint angle is at the limit position, the weight factor corresponding to the joint is infinite, so that the joint speed is limited to zero, the joint stays near the limit of the joint angle, the joint cannot be moved away from the limit angle, and the robot system easily loses the redundancy characteristic. Therefore, the optimal selection of the scalar coefficient of the optimization criterion function and the joint avoidance angle limit are key problems for solving the multi-constraint inverse solution of the redundant robot.
Disclosure of Invention
In view of the problems mentioned in the background art, the present invention is to provide a redundant robot multi-constraint inverse solution method to solve the problems mentioned in the background art.
The technical purpose of the invention is realized by the following technical scheme:
a multi-constraint inverse solution method for a redundant robot comprises the following steps:
step1, establishing a continuous weighting matrix related to the joint angle of the robot;
step2, constructing a repulsive velocity potential field which pushes the joint away from a limit angle;
step3, establishing a weighted gradient projection method redundant robot inverse solution;
step4, defining an optimization criterion function to regularize continuous variable coefficients;
step5, designing an optimization criterion function regularization processing principle;
and Step6, solving the multi-constraint inverse kinematics of the redundant robot.
Preferably, in Step1, the weighting matrix W is continuouscContinuous weighting matrix W related to the angle limit of the robot joint avoidingcIs defined as:
Wc＝diag(wc(qi))，i＝1,…,n；
wherein n is the degree of freedom of the robot joint; q. q.siIs the ith joint angle;
continuous weighting matrix factor wc(qi) Comprises the following steps:
Figure BDA0003035956530000021
wherein ,qim i and qimaxRespectively a minimum limit and a maximum limit of a joint angle;
Figure BDA0003035956530000022
qitmin＝(1-Ω)qimin+Ωqimaxthreshold values of positive and negative limits, respectively; omega is the width of the damping area; gΩ(. is a cubic function, g)Ω(d)＝-2d3+3d2；wc(qi) The introduction of (2) divides the range of joint angles into three parts: damping zone-flexible zone-damping zone.
Preferably, in Step1, the weighting matrix W is continuouscFor constructing a weighting matrix WbAnd redundant robot weighted jacobian matrix JwbThe description is as follows:
Wb＝In-Wc；
Jwb＝JWb；
based on weighted Jacobian matrix JwbEstablishing a redundant robot weighted null-space matrix
Figure BDA0003035956530000031
Comprises the following steps:
Figure BDA0003035956530000032
wherein ,
Figure BDA0003035956530000033
is a weighted Jacobian matrix JwbThe pseudo-inverse of (1);
repulsive velocity potential field T acting in the joint angle damping arearFor pushing away the joint angle away from the extreme angle, described as:
Tr＝diag(tr(qi))，i＝1,…,n；
Figure BDA0003035956530000034
wherein ,trmaxThe maximum repulsive angular velocity of the joint.
Preferably, said repulsive velocity potential field function TrM-dimensional main task acting on redundant robot tail end
Figure BDA0003035956530000039
Said weighted null space of
Figure BDA0003035956530000035
And establishing a weighted gradient projection method redundancy robot inverse solution which is as follows:
Figure BDA0003035956530000036
wherein
Figure BDA0003035956530000037
Is the robot joint velocity.
Preferentially, based on the damped least square method, the weighted gradient projection method redundant robot inverse solution is redefined, namely:
Figure BDA0003035956530000038
Figure BDA0003035956530000041
Figure BDA0003035956530000042
Figure BDA0003035956530000043
wherein ,ρmaxIs the maximum damping factor; ε is the singular region size threshold; sigmaminIs JwbA minimum singular value; rhowbIs JwbA damping factor;
Figure BDA0003035956530000044
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (d);
Figure BDA0003035956530000045
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (a).
Preferably, the redundant robot is based on a regularized continuous scalar coefficient kj(Hnj) According to the configuration of the current robot configuration, online self-adaptive continuous adjustment optimization criterion function scalar coefficients are described as follows:
kj(Hnj)＝±fnorm(Hnj)，j＝1,…,s
wherein ,HnjIs an optimization criterion function H after regularizationj；fnorm(. is) a continuous scalar coefficient function if HnjTo maximize, take the positive sign, if HnjTaking a negative sign for minimization; s is the number of total constraint tasks.
Preferably, said continuous scalar coefficient function fnorm(Hnj) Is defined as:
Figure BDA0003035956530000046
Figure BDA0003035956530000047
Figure BDA0003035956530000048
Figure BDA0003035956530000049
Figure BDA0003035956530000051
Figure BDA0003035956530000052
Figure BDA0003035956530000053
wherein ,
Figure BDA0003035956530000054
and
Figure BDA0003035956530000055
are each HnjMaximum and minimum values of;
Figure BDA0003035956530000056
are respectively
Figure BDA0003035956530000057
A threshold value of (d); a, b, c, d are each a continuous scalar coefficient function fnorm(Hnj) The coefficient of (a); λ is
Figure BDA0003035956530000058
And
Figure BDA0003035956530000059
the bandwidth in between.
Preferentially, in Step5, the optimization criterion function regularization processing principle is used to regularize different optimization criterion functions so that the optimization criterion functions have the same dimension and the same amplitude, and the optimization criterion function regularization processing principle reasonably allocates scalar coefficients and balances the roles of the different optimization criterion functions in the redundant robot inverse solution, where the optimization criterion function regularization processing principle includes two types of principles, including:
A. the regularization processing principle of the obstacle avoidance optimization criterion function is defined as:
N∞(Hj)＝1-exp(ac-Hj)；
wherein ,N∞(∞) 1 represents the optimal regularization result, N∞(ac) 0 is the corresponding worst regularization result; under the condition of obstacle avoidance, N∞Infinity represents the robot-obstacle distance, N∞(ac) Indicating a collision of the robot with an obstacle, acIs a collision distance threshold;
b, carrying out the following steps; the joint avoidance angle limit optimization criterion function has the regularization processing principle defined as:
Nc(Hj)＝exp(bc-Hj)；
wherein ,Nc(bc) 1 denotes the optimal regularization result, Nc(∞) 0 represents the worst regularization result. In the limit of joint avoidance angle, Nc(bc) Indicating that the robot joint is at an intermediate angle, bcFor optimal joint angular position, Nc(∞) indicates that the joint is at positive and negative limits;
the multi-constraint inverse kinematics solution of the redundant robot is based on a continuous weighting matrix and an optimization criterion function regularization processing principle and is defined as follows:
Figure BDA0003035956530000061
wherein ,
Figure BDA0003035956530000062
in summary, the invention mainly has the following beneficial effects:
in order to better realize joint angle avoidance limit, the invention provides a continuous weighting matrix and a repulsion velocity potential field function, which push away a robot joint from the joint angle limit of the robot, and maintain the redundancy characteristic of a robot system; the optimization criterion function regularization processing principle is used for calculating the scalar coefficient of the optimization criterion function on line in real time, so that the scalar coefficient is prevented from being determined by a gradient projection method through a large amount of simulation, and the working efficiency of the algorithm is improved; the invention is not only effective for solving the inverse kinematics of common redundant robots, such as planar three-degree-of-freedom redundant robot and redundant seven-degree-of-freedom robot, but also effective for solving the inverse kinematics of super-redundant degree-of-freedom robot, such as eight-degree-of-freedom, nine-degree-of-freedom, twelve-degree-of-freedom and the like.
Drawings
FIG. 1 is a flow chart of a method of the present invention;
FIG. 2 is a graph of continuous weighting matrix factor in accordance with the present invention;
FIG. 3 is a functional regularization processing principle diagram of an optimization criterion in the present invention;
FIG. 4 is a diagram of the hardware architecture of the test system of the present invention;
FIG. 5 is a graph showing the results of a normalized joint angle test according to the present invention;
FIG. 6 is a graph showing the results of a normalized joint angular velocity test according to the present invention;
FIG. 7 is a graph of scalar coefficient change according to the present invention;
fig. 8 is a graph showing the variation of the weighting factors according to the present invention.
FIG. 9 is a graph showing the variation of the state error in the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
Referring to fig. 1 to 9, a redundant robot multi-constraint inverse solution method includes the following steps:
the method comprises the following steps:
step1, establishing a continuous weighting matrix related to the joint angle of the robot;
step2, constructing a repulsive velocity potential field which pushes the joint away from a limit angle;
step3, establishing a weighted gradient projection method redundant robot inverse solution;
step4, defining an optimization criterion function to regularize continuous variable coefficients;
step5, designing an optimization criterion function regularization processing principle;
and Step6, solving the multi-constraint inverse kinematics of the redundant robot.
Preferably, in Step1, the weighting matrix W is continuouscContinuous weighting matrix W related to the angle limit of the robot joint avoidingcIs defined as:
Wc＝diag(wc(qi))，i＝1,…,n；
wherein n is the degree of freedom of the robot joint; q. q.siIs the ith joint angle;
continuous weighting matrix factor wc(qi) Comprises the following steps:
Figure BDA0003035956530000081
wherein ,qim i and qimaxRespectively a minimum limit and a maximum limit of a joint angle;
Figure BDA0003035956530000082
qmitmiinn＝(1-Ω)qimin+Ωqimaxthreshold values of positive and negative limits, respectively; omega is the width of the damping area; gΩ(. is a cubic function, g)Ω(d)＝-2d3+3d2；wc(qi) The introduction of (2) divides the range of joint angles into three parts: damping zone-flexible zone-damping zone.
Preferably, in Step1, the weighting matrix W is continuouscFor constructing a weighting matrix WbAnd redundant robot weighted jacobian matrix JwbThe description is as follows:
Wb＝In-Wc；
Jwb＝JWb；
based on weighted Jacobian matrix JwbEstablishing a redundant robot weighted null-space matrix
Figure BDA0003035956530000083
Comprises the following steps:
Figure BDA0003035956530000084
wherein ,
Figure BDA0003035956530000085
is a weighted Jacobian matrix JwbThe pseudo-inverse of (1);
repulsive velocity potential field T acting in the joint angle damping arearFor pushing away the joint angle away from the extreme angle, described as:
Tr＝diag(tr(qi))，i＝1,…,n；
Figure BDA0003035956530000086
wherein ,trmaxThe maximum repulsive angular velocity of the joint.
Preferably, said repulsive velocity potential field function TrThe weighted null space acting on the redundant robot terminal m-dimensional primary task x
Figure BDA0003035956530000091
And establishing a weighted gradient projection method redundancy robot inverse solution which is as follows:
Figure BDA0003035956530000092
wherein
Figure BDA0003035956530000093
Is the robot joint velocity.
Preferentially, based on the damped least square method, the weighted gradient projection method redundant robot inverse solution is redefined, namely:
Figure BDA0003035956530000094
Figure BDA0003035956530000095
Figure BDA0003035956530000096
Figure BDA0003035956530000097
wherein ,ρmaxIs the maximum damping factor; ε is the singular region size thresholdA value; sigmaminIs JwbA minimum singular value; rhowbIs JwbA damping factor;
Figure BDA0003035956530000098
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (d);
Figure BDA0003035956530000099
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (a).
Wherein the redundant robot is based on a regularization-processed continuous scalar coefficient kj(Hnj) According to the configuration of the current robot configuration, online self-adaptive continuous adjustment optimization criterion function scalar coefficients are described as follows:
kj(Hnj)＝±fnorm(Hnj)，j＝1,…,s
wherein ,HnjIs an optimization criterion function H after regularizationj；fnorm(. is) a continuous scalar coefficient function if HnjTo maximize, take the positive sign, if HnjTaking a negative sign for minimization; s is the number of total constraint tasks.
Wherein the continuous scalar coefficient function fnorm(Hnj) Is defined as:
Figure BDA0003035956530000101
Figure BDA0003035956530000102
Figure BDA0003035956530000103
Figure BDA0003035956530000104
Figure BDA0003035956530000105
Figure BDA0003035956530000106
Figure BDA0003035956530000107
wherein ,
Figure BDA0003035956530000108
and
Figure BDA0003035956530000109
are each HnjMaximum and minimum values of;
Figure BDA00030359565300001010
are respectively
Figure BDA00030359565300001011
A threshold value of (d); a, b, c, d are each a continuous scalar coefficient function fnorm(Hnj) The coefficient of (a); λ is
Figure BDA00030359565300001012
And
Figure BDA00030359565300001013
the bandwidth in between.
In Step5, an optimization criterion function regularization processing principle is used to perform regularization processing on different optimization criterion functions so that the optimization criterion functions have the same dimension and the same amplitude, the optimization criterion function regularization processing principle reasonably distributes scalar coefficients and balances the functions of the different optimization criterion functions in the inverse solution of the redundant robot, and the optimization criterion function regularization processing principle includes two types of principles, including:
A. the regularization processing principle of the obstacle avoidance optimization criterion function is defined as:
N∞(Hj)＝1-exp(ac-Hj)；
wherein ,N∞(∞) 1 represents the optimal regularization result, N∞(ac) 0 is the corresponding worst regularization result; under the condition of obstacle avoidance, N∞Infinity represents the robot-obstacle distance, N∞(ac) Indicating a collision of the robot with an obstacle, acIs a collision distance threshold;
b, carrying out the following steps; the joint avoidance angle limit optimization criterion function has the regularization processing principle defined as:
Nc(Hj)＝exp(bc-Hj)；
wherein ,Nc(bc) 1 denotes the optimal regularization result, Nc(∞) 0 represents the worst regularization result. In the limit of joint avoidance angle, Nc(bc) Indicating that the robot joint is at an intermediate angle, bcFor optimal joint angular position, Nc(∞) indicates that the joint is at positive and negative limits;
the multi-constraint inverse kinematics solution of the redundant robot is based on a continuous weighting matrix and an optimization criterion function regularization processing principle and is defined as follows:
Figure BDA0003035956530000111
wherein ,
Figure BDA0003035956530000112
the invention provides a continuous weighting matrix and a repulsion velocity potential field function, which are used for pushing away a robot joint from the joint angle limit of the robot, so that the redundancy characteristic of a robot system is maintained; the optimization criterion function regularization processing principle is used for calculating the scalar coefficient of the optimization criterion function on line in real time, so that the scalar coefficient is prevented from being determined by a gradient projection method through a large amount of simulation, and the working efficiency of the algorithm is improved; the invention is not only effective for solving the inverse kinematics of common redundant robots, such as planar three-degree-of-freedom redundant robot and redundant seven-degree-of-freedom robot, but also effective for solving the inverse kinematics of super-redundant degree-of-freedom robot, such as eight-degree-of-freedom, nine-degree-of-freedom, twelve-degree-of-freedom and the like.
Example 2
Referring to fig. 1 to 9, a redundant robot multi-constraint inverse solution method includes the following steps:
step1, establishing a continuous weighting matrix related to the joint angle of the robot;
step2, constructing a repulsive velocity potential field which pushes the joint away from a limit angle;
step3, establishing a weighted gradient projection method redundant robot inverse solution;
step4, defining an optimization criterion function to regularize continuous variable coefficients;
step5, designing an optimization criterion function regularization processing principle;
and Step6, solving the multi-constraint inverse kinematics of the redundant robot.
Firstly, when a transfer transformation matrix equation is established according to the D-H parameters, a kinematics positive solution expression is solved, wherein the transfer transformation matrix is as follows:
Figure BDA0003035956530000121
wherein ,(qi,αi,ai,di) And D-H parameters of the robot.
The kinematic positive solution relational expression is as follows:
Figure BDA0003035956530000122
the Jacobian matrix J can be obtained by the positive kinematic equation of the robot:
Figure BDA0003035956530000123
binary, continuous weighting matrix
Although the weighted least square method can effectively avoid the joint angle limit, the weighting matrix factor of the method is suddenly increased near the limit, so that the robot joint is easy to shake, and the speed is discontinuous. In order to eliminate the phenomenon, the invention improves the factor curve of the weighting matrix, designs a continuous weighting matrix, and comprises the following steps:
Wc＝diag(wc(qi))；
Figure BDA0003035956530000131
wherein ,qiIs the ith joint angle; q. q.sitmax＝(1-Ω)qimax+Ωqimin，qitmin＝(1-Ω)qimin+ΩqimaxThe threshold values of the positive limit and the negative limit are respectively, the damping region with the bandwidth of omega is formed by the positive limit and the negative limit, the whole joint angle range is divided into three parts, namely the damping region, the flexible region and the damping region, as shown in figure 2. gΩ(. is) a cubic function defined as gΩ(d)＝-2d3+3d2And the smoothness and flexibility are better compared with those of a quadratic function.
As can be seen from FIG. 2, the continuous weighting matrix WcA damping area is introduced, and a factor w is ensuredc(qi) The value changes smoothly and continuously in the range of joint angles and has a bounded property wc(qi)∈[0,1]. When q isiIn its flexible region, wc(qi) 0. When q isiCrossing joint angle limit threshold qitmaxOr qitminWhen wc(qi) Monotonically increasing, smoothly transitioning from 0 to a maximum of 1.
Thirdly, weighting a Jacobian matrix and a weighted null space matrix:
continuous weighting matrix WcFor constructing redundant robot weighted Jacobian matrix JwbDrawingThe method comprises the following steps:
Wb＝In-Wc；
Jwb＝JWb；
based on weighted Jacobian matrix JwbEstablishing a redundant robot weighted zero-space matrix
Figure BDA0003035956530000132
Comprises the following steps:
Figure BDA0003035956530000133
wherein ,
Figure BDA0003035956530000134
is a weighted Jacobian matrix JwbThe pseudo-inverse of (1);
fourthly, repelling a velocity potential field:
when continuous weighting matrix factor wc(qi) → 1, continuous weighting matrix WcThe joint q cannot be connectediPushed away from its extreme angle. Therefore, a repulsive velocity potential field T is constructedrActing on the joint angle damping area, pushing away the joint angle and keeping away from the limit angle, and describing as:
Tr＝diag(tr(qi))，i＝1,…,n；
Figure BDA0003035956530000141
wherein ,trmaxThe maximum repulsive angular velocity of the joint.
Fifthly, a weighted gradient projection method:
to introduce a repulsive velocity potential field TrDoes not affect the main task at the tail end of the robot and repels the velocity potential field function TrM-dimensional main task acting on redundant robot tail end
Figure BDA0003035956530000142
Weighted null space of
Figure BDA0003035956530000143
And establishing a weighted gradient projection method redundancy robot inverse solution which is as follows:
Figure BDA0003035956530000144
considering the influence of the singularity problem on the inverse solution of the redundant robot, redefining the inverse solution of the redundant robot by the weighted gradient projection method based on the damped least square method, namely:
Figure BDA0003035956530000145
Figure BDA0003035956530000146
Figure BDA0003035956530000147
Figure BDA0003035956530000148
wherein ,ρmaxIs the maximum damping factor; ε is the singular region size threshold; sigmaminIs a weighted Jacobian matrix JwbThe smallest singular value.
Sixth, continuous scalar coefficients:
generally, in the inverse kinematics of a redundant robot, multiple constraint tasks are usually introduced into an original algorithm in the form of a weighted sum function, different weights are supplemented to each optimization criterion function, and except for a main task, the constraint task with the larger weight preferably meets the constraint condition. The multi-constraint weighted sum function Φ is:
Figure BDA0003035956530000151
wherein ,kjFor the jth optimization criterion function HjA fixed scalar coefficient of (d); s is the number of total constraint tasks;
Figure BDA0003035956530000152
as a function of the optimization criterion HjIs measured.
However, the fixed scalar coefficients that are empirically assigned to each optimization criterion function have the disadvantage that it may not be possible to effectively balance the constraints in the control, and even one optimization criterion function may be decisive among many constraints, ignoring the presence of other constraints. Therefore, the redundant robot is based on the continuous scalar coefficient k of the regularization processj(Hnj) Introduced into the robot system, which is configured according to the current robot configuration, continuously adjusts the scalar coefficient of the optimization criterion function in an online self-adaptive way, and is described as follows:
kj(Hnj)＝±fnorm(Hnj)，j＝1,…,s；
wherein ,HnjIs an optimization criterion function H after regularizationj；fnorm(. is) a continuous scalar coefficient function if HnjTo maximize, take the positive sign, if HnjTo minimize, the minus sign is taken.
Continuous scalar coefficient function fnorm(Hnj) Is defined as:
Figure BDA0003035956530000153
Figure BDA0003035956530000154
Figure BDA0003035956530000155
Figure BDA0003035956530000161
Figure BDA0003035956530000162
Figure BDA0003035956530000163
Figure BDA0003035956530000164
wherein ,
Figure BDA0003035956530000165
and
Figure BDA0003035956530000166
are each HnjMaximum and minimum values of; λ is
Figure BDA0003035956530000167
And
Figure BDA0003035956530000168
the bandwidth in between.
Seventhly, optimizing a criterion function regularization processing principle:
the optimization criterion function regularization processing principle is used for regularizing different optimization criterion functions, so that the optimization criterion functions have the same dimension and the same amplitude, scalar coefficients can be reasonably distributed, and the functions of the different optimization criterion functions in the inverse solution of the redundant robot are balanced. The optimization criteria function regularization processing principle includes two types of principles, as shown in fig. 3, namely:
A. the regularization processing principle of the optimization criterion function like obstacle avoidance is defined as:
N∞(Hj)＝1-exp(ac-Hj)；
wherein ,N∞The value is 1 ∞Regularization result, N∞(ac) 0 is the corresponding worst regularization result. Under the condition of obstacle avoidance, N∞Infinity represents the robot-obstacle distance, N∞(ac) Indicating a collision of the robot with an obstacle, acIs the collision distance threshold.
B. The regularization processing principle of the optimization criterion function like the joint avoidance angle limit is defined as follows:
Nc(Hj)＝exp(bc-Hj)；
wherein ,Nc(bc) 1 denotes the optimal regularization result, Nc(∞) 0 represents the worst regularization result. In the limit of joint avoidance angle, Nc(bc) Indicating that the robot joint is at an intermediate angle, bcFor optimal joint angular position, Nc(∞) indicates that the joint is at positive and negative limits.
Eighthly, redundant robot multi-constraint inverse kinematics:
in complex multi-constraint cases, the optimization criteria functions may be many and different in priority. In a special case, if multiple optimization criterion functions are activated simultaneously, given the same priority, this may affect the solution of the redundant robot inverse kinematics. In order to effectively manage different optimization criterion functions, the invention is in a weighted null space
Figure BDA0003035956530000171
Different optimization criterion functions are endowed with different priorities, and a multi-level redundant robot multi-constraint inverse solution is formed, namely:
Figure BDA0003035956530000172
wherein ,
Figure BDA0003035956530000173
the multi-constraint inverse kinematics solution of the redundant robot is based on a continuous weighting matrix and an optimization criterion function regularization processing principle, a multi-constraint inverse kinematics solution method which is except for a main task and has joint angle avoidance limit priority higher than the priority of other sub-constraint tasks is realized, and the solution problem of sub-task conflict in the inverse kinematics of the redundant robot is solved.
The specific algorithm process is explained as follows:
in this embodiment, a redundant seven-degree-of-freedom robot is used for test verification of an embodiment object, a hardware structure of a test system is shown in fig. 4, and D-H parameters of the robot are shown in table 1. In consideration of the fact that when the robot end main task needs to perform a fast operation, the joint angular velocity of the robot may exceed the corresponding joint angular velocity limit, resulting in a severe velocity saturation phenomenon. Therefore, the joint avoidance angle limit and the joint avoidance angular velocity limit will be considered in the present embodiment as the multiple constraint conditions in the redundant seven-degree-of-freedom robot inverse kinematics solution. The joint avoidance angular velocity limit optimization criterion function can be expressed as:
Figure BDA0003035956530000181
wherein ,
Figure BDA0003035956530000182
the positive and negative limits of the angular speed of the joint i are respectively shown in the table 1; in addition to this, the present invention is,
Figure BDA0003035956530000183
the optimization criteria function regularization processing principle shown in fig. 3 is satisfied.
Table 1 redundant seven-degree-of-freedom robot D-H parameters:
Figure BDA0003035956530000184
based on the joint avoidance angular velocity limit, the redundant robot multi-constraint inverse kinematics solution is as follows:
Figure BDA0003035956530000185
wherein the regularization processing function λ is 0.3;
Figure BDA0003035956530000186
is the robot tip desired velocity; κ is the position feedback gain, set to 80; ee∈R6×1Indicating a position error between a desired trajectory and an actual trajectory of the robot
Figure BDA0003035956530000187
pd∈R3×1，p∈R3×1Respectively an ideal position and an actual position of the tail end of the robot; rd＝(nd,sd,ad)∈R3×3，R＝(n,s,a)∈R3×3Which are the ideal and actual rotation matrices of the robot tip, respectively.
The specific parameters of the test are as follows:
the initial pose of the robot is psAnd end point pose of pfAnd the tracking track of the robot is a reciprocating straight line ps→pf→ps，ps＝[406.3,-238,1040](mm)，os＝[-80.86,92.267,79.244](°)，pf＝[591.7,-208.3,596.7](mm)，of＝[-92.1895,125.3982,43.4023](°); acceleration amax＝1000mm/s2(ii) a Maximum damping factor ρmax＝0.02。
In order to clearly display whether the angle of the robot joint exceeds the angle limit, the embodiment of the invention adopts a normalization method to process the angle of the robot joint, if the absolute value of the processed result is more than or equal to 1, the joint avoidance angle limit is considered to fail, otherwise, the joint avoidance angle limit can be effectively avoided. Similarly, the joint avoidance angular velocity limit may also be expressed using a normalized expression:
joint avoidance angle limit:
Figure BDA0003035956530000191
joint avoidance angular velocity limit:
Figure BDA0003035956530000192
the results of the weighted gradient projection based multi-constraint inverse kinematics test of the redundant robot are shown in fig. 5 to 9. As seen in FIG. 5, q is2Approaching the limit joint angle at t-1.89 s, and in the joint angle damping region, the weighting factor wb(q2) Gradually dropping to 0.933 as shown in fig. 8. Due to the continuous weighting matrix WcNot equal to 0, a repulsive velocity potential field function activation, acting in the weight matrix null space, with q2Corresponding zero-space repulsion velocity direction and q2Run in the opposite direction, so q2Can be far away from the limit joint angle under the action of the repulsive velocity potential field function. Due to the adoption of the continuous scalar coefficient, as shown in fig. 7, the scalar coefficient is adjusted in real time according to the angular velocity of the joint of the robot shown in fig. 6, so that the joint angular velocity is prevented from being optimized while avoiding the joint angle limit, and the introduction of the continuous scalar coefficient also ensures the redundancy characteristic of the robot. As can be seen in fig. 6, joints 2, 4 and 5 effectively avoid breaking through their extreme joint angular velocities. In addition, as can be seen from the state error shown in fig. 9, the weighted gradient projection method has better end pose accuracy for controlling the operation of the robot. Therefore, the weighted gradient projection method provided by the invention can solve the inverse kinematics multi-constraint problem under the condition of ensuring the continuity of the joint angle and the joint angular velocity of the robot.
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (8)

1. A multi-constraint inverse solution method for a redundant robot is characterized by comprising the following steps: the method comprises the following steps:
step1, establishing a continuous weighting matrix related to the joint angle of the robot;
step2, constructing a repulsive velocity potential field which pushes the joint away from a limit angle;
step3, establishing a weighted gradient projection method redundant robot inverse solution;
step4, defining an optimization criterion function to regularize continuous variable coefficients;
step5, designing an optimization criterion function regularization processing principle;
and Step6, solving the multi-constraint inverse kinematics of the redundant robot.
2. The method of claim 1, wherein the method comprises the following steps: in Step1, the continuous weighting matrix WcContinuous weighting matrix W related to the angle limit of the robot joint avoidingcIs defined as:
Wc＝diag(wc(qi))，i＝1,…,n；
wherein n is the degree of freedom of the robot joint; q. q.siIs the ith joint angle;
continuous weighting matrix factor wc(qi) Comprises the following steps:
Figure FDA0003035956520000011
wherein ,qimin and qimaxRespectively a minimum limit and a maximum limit of a joint angle;
Figure FDA0003035956520000012
qitmin＝(1-Ω)qimin+Ωqimaxthreshold values of positive and negative limits, respectively; omega is the width of the damping area; gΩ(. is a cubic function, g)Ω(d)＝-2d3+3d2；wc(qi) The introduction of (2) divides the range of joint angles into three parts: damping zone-flexible zone-damping zone.
3. The method of claim 1, wherein the method comprises the following steps: in Step1, the continuous weighting matrix WcFor constructing a weighting matrix WbAnd redundant robot weighted jacobian matrix JwbThe description is as follows:
Wb＝In-Wc；
Jwb＝JWb；
based on weighted Jacobian matrix JwbEstablishing a redundant robot weighted null-space matrix
Figure FDA0003035956520000021
Comprises the following steps:
Figure FDA0003035956520000022
wherein ,
Figure FDA0003035956520000023
is a weighted Jacobian matrix JwbThe pseudo-inverse of (1);
repulsive velocity potential field T acting in the joint angle damping arearFor pushing away the joint angle away from the extreme angle, described as:
Tr＝diag(tr(qi))，i＝1,…,n；
Figure FDA0003035956520000024
wherein ,trmaxThe maximum repulsive angular velocity of the joint.
4. The method of claim 3, wherein the method comprises the following steps: said repulsive velocity potential field function TrM-dimensional main task acting on redundant robot tail end
Figure FDA0003035956520000025
Said weighted null space of
Figure FDA0003035956520000026
And establishing a weighted gradient projection method redundancy robot inverse solution which is as follows:
Figure FDA0003035956520000027
wherein
Figure FDA0003035956520000028
Is the robot joint velocity.
5. The method of claim 4, wherein the method comprises the following steps: based on a damped least square method, redefining the inverse solution of the weighted gradient projection method redundant robot, namely:
Figure FDA0003035956520000031
Figure FDA0003035956520000032
Figure FDA0003035956520000033
Figure FDA0003035956520000034
wherein ,ρmaxIs the maximum damping factor; ε is the singular region size threshold; sigmaminIs JwbA minimum singular value; rhowbIs JwbA damping factor;
Figure FDA0003035956520000035
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (d);
Figure FDA0003035956520000036
is provided with a damping factor rhowbWeighted jacobian pseudo-inverse of (a).
6. The method of claim 1, wherein the method comprises the following steps: continuous scalar coefficient k of redundant robot based on regularization processingj(Hnj) According to the configuration of the current robot configuration, online self-adaptive continuous adjustment optimization criterion function scalar coefficients are described as follows:
kj(Hnj)＝±fnorm(Hnj)，j＝1,…,s
wherein ,HnjIs an optimization criterion function H after regularizationj；fnorm(. is) a continuous scalar coefficient function if HnjTo maximize, take the positive sign, if HnjTaking a negative sign for minimization; s is the number of total constraint tasks.
7. The method of claim 6, wherein the method comprises the following steps: said continuous scalar coefficient function fnorm(Hnj) Is defined as:
Figure FDA0003035956520000037
Figure FDA0003035956520000038
Figure FDA0003035956520000039
Figure FDA0003035956520000041
Figure FDA0003035956520000042
Figure FDA0003035956520000043
Figure FDA0003035956520000044
wherein ,
Figure FDA0003035956520000045
and
Figure FDA0003035956520000046
are each HnjMaximum and minimum values of;
Figure FDA0003035956520000047
are respectively
Figure FDA0003035956520000048
A threshold value of (d); a, b, c, d are each a continuous scalar coefficient function fnorm(Hnj) The coefficient of (a); λ is
Figure FDA0003035956520000049
And
Figure FDA00030359565200000410
the bandwidth in between.
8. The method of claim 1, wherein the method comprises the following steps: in Step5, an optimization criterion function regularization processing principle is used to perform regularization processing on different optimization criterion functions so that the optimization criterion functions have the same dimension and the same amplitude, and the optimization criterion function regularization processing principle reasonably distributes scalar coefficients and balances the functions of the different optimization criterion functions in the redundant robot inverse solution, where the optimization criterion function regularization processing principle includes two types of principles, including:
A. the regularization processing principle of the obstacle avoidance optimization criterion function is defined as:
N∞(Hj)＝1-exp(ac-Hj)；
wherein ,N∞(∞) 1 represents the optimal regularization result, N∞(ac) 0 is the corresponding worst regularization result; under the condition of obstacle avoidance, N∞Infinity represents the robot-obstacle distance, N∞(ac) Indicating a collision of the robot with an obstacle, acIs a collision distance threshold;
b, carrying out the following steps; the joint avoidance angle limit optimization criterion function has the regularization processing principle defined as:
Nc(Hj)＝exp(bc-Hj)；
wherein ,Nc(bc) 1 denotes the optimal regularization result, Nc(∞) 0 represents the worst regularization result. In the limit of joint avoidance angle, Nc(bc) Indicating that the robot joint is at an intermediate angle, bcFor optimal joint angular position, Nc(∞) indicates that the joint is at positive and negative limits;
the multi-constraint inverse kinematics solution of the redundant robot is based on a continuous weighting matrix and an optimization criterion function regularization processing principle and is defined as follows:
Figure FDA0003035956520000051
wherein ,
Figure FDA0003035956520000052
CN202110443343.5A 2021-04-23 2021-04-23 Multi-constraint inverse solution method for redundant robot Active CN113043284B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110443343.5A CN113043284B (en) 2021-04-23 2021-04-23 Multi-constraint inverse solution method for redundant robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110443343.5A CN113043284B (en) 2021-04-23 2021-04-23 Multi-constraint inverse solution method for redundant robot

Publications (2)

Publication Number Publication Date
CN113043284A true CN113043284A (en) 2021-06-29
CN113043284B CN113043284B (en) 2023-04-28

Family

ID=76520214

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110443343.5A Active CN113043284B (en) 2021-04-23 2021-04-23 Multi-constraint inverse solution method for redundant robot

Country Status (1)

Country Link
CN (1) CN113043284B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848914A (en) * 2021-09-28 2021-12-28 福州大学 Collision coefficient artificial potential field method local path planning method in dynamic environment
CN114643583A (en) * 2022-05-05 2022-06-21 中国科学院合肥物质科学研究院 Method for avoiding singular points of redundant mechanical arm

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4999553A (en) * 1989-12-28 1991-03-12 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Method and apparatus for configuration control of redundant robots
US20050143860A1 (en) * 2003-12-26 2005-06-30 Japan Aerospace Exploration Agency Method of controlling redundant manipulator
CN106335062A (en) * 2016-11-11 2017-01-18 清研华宇智能机器人（天津）有限责任公司 A General Operating Planning for Seven-axis Redundant Industrial Robots
CN110861088A (en) * 2019-11-29 2020-03-06 沈阳通用机器人技术股份有限公司 Motion optimization method of redundant degree of freedom robot
CN111515932A (en) * 2020-04-23 2020-08-11 东华大学 Man-machine co-fusion assembly line implementation method based on artificial potential field and reinforcement learning

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4999553A (en) * 1989-12-28 1991-03-12 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Method and apparatus for configuration control of redundant robots
US20050143860A1 (en) * 2003-12-26 2005-06-30 Japan Aerospace Exploration Agency Method of controlling redundant manipulator
CN106335062A (en) * 2016-11-11 2017-01-18 清研华宇智能机器人（天津）有限责任公司 A General Operating Planning for Seven-axis Redundant Industrial Robots
CN110861088A (en) * 2019-11-29 2020-03-06 沈阳通用机器人技术股份有限公司 Motion optimization method of redundant degree of freedom robot
CN111515932A (en) * 2020-04-23 2020-08-11 东华大学 Man-machine co-fusion assembly line implementation method based on artificial potential field and reinforcement learning

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848914A (en) * 2021-09-28 2021-12-28 福州大学 Collision coefficient artificial potential field method local path planning method in dynamic environment
CN113848914B (en) * 2021-09-28 2024-04-26 福州大学 Method for planning local path by collision coefficient artificial potential field method in dynamic environment
CN114643583A (en) * 2022-05-05 2022-06-21 中国科学院合肥物质科学研究院 Method for avoiding singular points of redundant mechanical arm
CN114643583B (en) * 2022-05-05 2023-11-10 中国科学院合肥物质科学研究院 Redundancy mechanical arm singular point avoiding method

Also Published As

Publication number Publication date
CN113043284B (en) 2023-04-28

Similar Documents

Publication Publication Date Title
CN108932216B (en) Robot inverse kinematics solving method based on particle swarm optimization algorithm
CN109901397B (en) Mechanical arm inverse kinematics method using particle swarm optimization algorithm
CN113043284A (en) Multi-constraint inverse solution method for redundant robot
CN108326852B (en) Multi-objective optimization space manipulator trajectory planning method
CN105138000A (en) Seven-freedom-degree space manipulator track planning method optimizing position and posture disturbance of pedestal
CN111665855B (en) Robot presetting formation control method
Sverdrup-Thygeson et al. Kinematic singularity avoidance for robot manipulators using set-based manipulability tasks
Pereira et al. Adaptive formation control using artificial potentials for Euler-Lagrange agents
Wang et al. A multi-target trajectory planning of a 6-dof free-floating space robot via reinforcement learning
Ren et al. Modified Newton's method applied to potential field-based navigation for nonholonomic robots in dynamic environments
Han Fractional-order sliding mode constraint control for manipulator systems using grey wolf and whale optimization algorithms
Zhang et al. Trajectory optimization and tracking control of free-flying space robots for capturing non-cooperative tumbling objects
Liu et al. Obstacle avoidance for redundant manipulators based on a novel gradient projection method with a functional scalar
CN116540721A (en) Space robot optimal track planning method based on improved genetic particle swarm algorithm
Dong et al. Back-stepping approach for rolling motion control of an under-actuated two-wheel spherical robot
Ak et al. Trajectory tracking control of an industrial robot manipulator using fuzzy SMC with RBFNN
Lim et al. Tension optimization for cable-driven parallel manipulators using gradient projection
Zheng et al. Adaptive super-twisting-like sliding mode control with prescribed performance for robot manipulators
CN110703792B (en) Underwater robot attitude control method based on reinforcement learning
Assal et al. Neural network learning from hint for the inverse kinematics problem of redundant arm subject to joint limits
Zhang et al. Motion planning of underwater vehicle-manipulator system with joint limit
Xu et al. A predictive control method for stabilizing a manipulator-based uav landing platform on fluctuating marine surface
Smith et al. Endpoint position control using biological concepts
Obregón et al. Synchronized mobile manipulators for kinematic cooperative tasks: Control design and analysis
Mansfeld et al. Improving the performance of auxiliary null space tasks via time scaling-based relaxation of the primary task

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
GR01 Patent grant
GR01 Patent grant