CN115625712A - Robot joint constraint trajectory planning method, device, equipment and medium - Google Patents

Robot joint constraint trajectory planning method, device, equipment and medium Download PDF

Info

Publication number
CN115625712A
CN115625712A CN202211407053.6A CN202211407053A CN115625712A CN 115625712 A CN115625712 A CN 115625712A CN 202211407053 A CN202211407053 A CN 202211407053A CN 115625712 A CN115625712 A CN 115625712A
Authority
CN
China
Prior art keywords
path
joint
parameter
target
determining
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
CN202211407053.6A
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.)
Jieka Robot Co ltd
Original Assignee
Jieka Robot Co ltd
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 Jieka Robot Co ltd filed Critical Jieka Robot Co ltd
Priority to CN202211407053.6A priority Critical patent/CN115625712A/en
Publication of CN115625712A publication Critical patent/CN115625712A/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
    • 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
    • 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

Landscapes

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

Abstract

The invention discloses a robot joint constraint track planning method, which comprises the following steps: determining a fifth parameter according to the joint moment constraint condition, and determining a sixth parameter according to the joint acceleration constraint condition; determining a path speed range of a preset path point according to the joint speed constraint condition, and determining a target constraint condition of the target function according to the path speed range, the fifth parameter and the sixth parameter; and optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, and determining the planning joint position corresponding to the target robot at the track planning moment according to the optimal path speed. According to the invention, the joint constraint track planning is carried out on the target robot by adding the joint torque constraint condition, the joint acceleration constraint condition and the joint speed constraint condition, so that the problems of joint overload or low joint performance utilization of the target robot are avoided, and the track planning precision of the robot is improved.

Description

Robot joint constraint trajectory planning method, device, equipment and medium
Technical Field
The invention relates to the technical field of computers, in particular to a method, a device, equipment and a medium for planning joint constraint tracks of a robot.
Background
The robot motion control system is a core component in the whole robot operation system, directly acts on a robot body, influences the operation process and result in a real physical environment, and is one of the most basic and key technologies in motion control.
Conventional trajectory planning methods typically only consider static constraints and do not consider joint constraints, which results in less accuracy for joint trajectory planning for the robot.
Disclosure of Invention
The invention provides a method, a device, equipment and a medium for planning a joint constraint track of a robot, which aim to solve the problem of low accuracy of planning the joint constraint track of the robot in the prior art.
According to an aspect of the present invention, there is provided a joint constraint trajectory planning method for a robot, including:
determining a first non-linear relationship between a joint position and a path position of the target robot, a second non-linear relationship between a joint velocity and a path velocity, and a third linear relationship between a joint acceleration and a path velocity and a path acceleration;
determining a fourth nonlinear relation among the joint moment of the target robot, the path speed and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and a robot dynamic equation;
determining a first parameter associated with the path velocity and a second parameter associated with the path acceleration in the fourth non-linear relationship, and determining a third parameter associated with the path velocity and a fourth parameter associated with the path acceleration in the third non-linear relationship;
determining a fifth parameter in the first nonlinear constraint condition according to a first nonlinear constraint condition and a joint moment constraint condition of the fourth nonlinear relationship, and determining a sixth parameter in the second nonlinear constraint condition according to a second nonlinear constraint condition and a joint acceleration constraint condition of the third nonlinear relationship;
determining a standard path position corresponding to a preset path point by the target robot, determining a path speed range of the preset path point according to the standard path position and a joint speed constraint condition of the second nonlinear relationship, and determining a target constraint condition of a target function according to the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter and the sixth parameter;
optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the trajectory planning time according to the optimal path speed, the standard path position and the trajectory planning time.
According to another aspect of the present invention, there is provided a joint constraint trajectory planning apparatus for a robot, including:
a nonlinear relationship determination first module for determining a first nonlinear relationship between the joint position and the path position, a second nonlinear relationship between the joint velocity and the path velocity, and a third nonlinear relationship between the joint acceleration and the path velocity and the path acceleration of the target robot;
a nonlinear relation determination second module for determining a fourth nonlinear relation between the joint moment of the target robot and the path velocity and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and the robot dynamics equation;
a first parameter determining module, configured to determine a first parameter associated with the path velocity and a second parameter associated with the path acceleration in the fourth non-linear relationship, and determine a third parameter associated with the path velocity and a fourth parameter associated with the path acceleration in the third non-linear relationship;
a second parameter determining module, configured to determine a fifth parameter in the first nonlinear constraint condition according to a first nonlinear constraint condition and a joint moment constraint condition of the fourth nonlinear relationship, and determine a sixth parameter in the second nonlinear constraint condition according to a second nonlinear constraint condition and a joint acceleration constraint condition of the third nonlinear relationship;
a constraint condition determining module, configured to determine a standard path position corresponding to a preset path point of the target robot, determine a path velocity range of the preset path point according to the standard path position and a joint velocity constraint condition of the second nonlinear relationship, and determine a target constraint condition of a target function according to the path velocity range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter, and the sixth parameter;
and the planned joint position determining module is used for optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the track planning time according to the optimal path speed, the standard path position and the track planning time.
According to another aspect of the present invention, there is provided an electronic apparatus including:
at least one processor; and
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores a computer program executable by the at least one processor, the computer program being executable by the at least one processor to enable the at least one processor to perform a method of planning a joint constraint trajectory of a robot according to any of the embodiments of the present invention.
According to another aspect of the present invention, there is provided a computer-readable storage medium storing computer instructions for causing a processor to implement a method for planning a joint constraint trajectory of a robot according to any one of the embodiments of the present invention when the computer instructions are executed.
According to the technical scheme of the embodiment of the invention, the joint constraint track planning is carried out on the target robot by adding the joint moment constraint condition, the joint acceleration constraint condition and the joint speed constraint condition, so that the problems of joint overload or low joint performance utilization of the target robot are avoided, and the accuracy of the joint track planning of the robot is improved.
It should be understood that the statements in this section are not intended to identify key or critical features of the embodiments of the present invention, nor are they intended to limit the scope of the invention. Other features of the present invention will become apparent from the following description.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a flowchart of a method for planning a joint constraint trajectory of a robot according to an embodiment of the present invention;
fig. 2 is a flowchart of a joint constraint trajectory planning method for a robot according to a second embodiment of the present invention;
fig. 3 is a schematic structural diagram of a joint constraint trajectory planning apparatus of a robot according to a third embodiment of the present invention;
fig. 4 is a schematic structural diagram of an electronic device for implementing the joint constraint trajectory planning method for the robot according to the embodiment of the present invention.
Detailed Description
In order to make the technical solutions of the present invention better understood, 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.
It should be noted that the terms "first", "second", "third", "fourth", "fifth", "sixth", "candidate", and "target" and the like in the description and claims of the present invention and the above drawings are used for distinguishing similar objects and not necessarily for describing a particular order or sequence. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those illustrated or described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
The traditional robot trajectory planning method usually only considers static constraints of cartesian space, such as static constraints of trajectory position, velocity, acceleration, jerk and the like, but does not consider joint constraints of joint space, which causes problems that the robot may have joint overload or low joint performance utilization rate and the like, and greatly affects the accuracy of the robot joint constraint trajectory planning.
Example one
Fig. 1 is a flowchart of a method for planning a joint constraint trajectory of a robot according to an embodiment of the present invention, where the embodiment is applicable to planning a joint constraint trajectory of each robot joint included in a target robot, and the method can be executed by a joint constraint trajectory planning apparatus of a robot, where the joint constraint trajectory planning apparatus of the robot can be implemented in a form of hardware and/or software, and the joint constraint trajectory planning apparatus of the robot can be configured in a robot body or a server. As shown in fig. 1, the method includes:
s101, determining a first nonlinear relation between the joint position and the path position of the target robot, a second nonlinear relation between the joint speed and the path speed, and a third nonlinear relation between the joint acceleration and the path speed and the path acceleration.
Wherein the target robot represents a robot having at least one robot joint installed therein, and the robot joint represents a connecting means between a plurality of machine links of the robot for enabling relative movement between the plurality of machine links, thereby providing the robot with the ability to move the machine link to an arbitrary position and angle. The number of machine joints in the target robot may be set and adjusted according to the target robot operation scenario, for example the target robot may comprise six machine joints, i.e. a six-axis robot.
The joint position represents the position at which each machine joint of the target robot is located, which can be represented by an n-dimensional vector, n being the number of machine joints the target robot includes. The joint velocity represents the operational velocity of the machine joint and the joint acceleration represents the operational acceleration of the machine joint.
The path position is a position information that can be calculated from the operating time of the target robot, i.e. the path position has a functional relationship with the operating time of the target robot: s = f (T), where s denotes the path position and T denotes the operating time of the target robot. The path velocity is in the form of the first derivative of the path position, i.e. the path velocity has a functional relationship with the target robot's running time:
Figure BDA0003936982450000061
wherein s represents a path position, T represents an operation time of the target robot,
Figure BDA0003936982450000062
representing the path velocity. The path acceleration is in the form of the second derivative of the path position, i.e. the path acceleration and the target robot's running timeHas a functional relationship between:
Figure BDA0003936982450000063
wherein s represents a path position, T represents an operation time of the target robot,
Figure BDA0003936982450000064
representing the path acceleration.
In one embodiment, the relationship between the joint position and the path position is a non-linear relationship, i.e. the relationship between the joint position and the path position can be a non-linear polynomial equation, and optionally, the relationship between the joint position and the path position is a cubic non-linear polynomial equation, i.e. the first non-linear relationship between the joint position and the path position can be expressed as follows: q = as 3 +bs 2 + cs + d, where q is the joint position, s is the path position, and a, b, c, d are polynomial coefficients.
The joint velocity and the path velocity also have a non-linear relationship, i.e., the joint velocity and the path velocity can also form a non-linear polynomial equation, i.e., a second non-linear relationship between the joint velocity and the path velocity can be expressed as follows:
Figure BDA0003936982450000065
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000066
is the joint velocity, s is the path position, a, b, c, d are polynomial coefficients,
Figure BDA0003936982450000067
is the path velocity.
The joint acceleration and the path velocity and the path acceleration also have a nonlinear relationship, that is, the joint acceleration and the path velocity and the path acceleration can also form a nonlinear polynomial equation, that is, a third nonlinear relationship between the joint acceleration and the path velocity and the path acceleration can be expressed as follows:
Figure BDA0003936982450000071
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000072
is the joint acceleration, s is the path position, a, b, c, d are polynomial coefficients,
Figure BDA0003936982450000073
as the speed of the path, it is,
Figure BDA0003936982450000074
is the path acceleration.
Optionally, the first non-linear relationship, the second non-linear relationship, and the third non-linear relationship may be expressed as follows:
Figure BDA0003936982450000075
where s denotes a path position, q denotes a joint position,
Figure BDA0003936982450000076
which is indicative of the speed of the path,
Figure BDA0003936982450000077
the acceleration of the joint is represented by,
Figure BDA0003936982450000078
representing the path acceleration, q () is a cubic nonlinear polynomial.
By setting a first nonlinear relationship between the joint position and the path position to q = q(s), and a second nonlinear relationship between the joint velocity and the path velocity to q = q(s)
Figure BDA0003936982450000079
A third non-linear relationship between joint acceleration and path velocity and path acceleration is
Figure BDA00039369824500000710
Thereby converting the track planning from a multidimensional problem to twoAnd the dimension problem enables subsequent constraint on joint information to be converted into constraint on path information, and constraint conditions are simplified.
S102, determining a fourth nonlinear relation among the joint moment of the target robot, the path speed and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and the robot dynamic equation.
Wherein the robot dynamics equation is an equation describing the relationship between the forces and motions of the robot mechanism.
In one embodiment, parameter replacement is carried out on the joint position parameters in the robot dynamics equation according to a first nonlinear relation; according to the second nonlinear relation, performing parameter replacement on the joint speed parameter in the robot dynamics equation; and performing parameter replacement on the joint acceleration parameters in the robot dynamics equation according to the third nonlinear relation.
Optionally, S102 includes:
and substituting the first nonlinear relation, the second nonlinear relation and the third nonlinear relation into a robot dynamics equation to obtain a fourth nonlinear relation.
Wherein, the robot dynamics equation is:
Figure BDA0003936982450000081
wherein a () represents an inertia matrix, B () represents a matrix of coriolis force and centripetal force coefficients, f () represents a gravitational moment, and τ represents a joint moment.
Specifically, "q" in the robot kinetic equation is replaced with "q(s)",
Figure BDA0003936982450000082
is replaced by
Figure BDA0003936982450000083
Figure BDA0003936982450000084
Instead of using
Figure BDA0003936982450000085
And obtaining a fourth nonlinear relation according to the robot kinetic equation after parameter replacement:
Figure BDA0003936982450000086
the fourth nonlinear relation is obtained by substituting the first nonlinear relation, the second nonlinear relation and the third nonlinear relation into the robot dynamics equation, so that the form of converting the joint moment into path information representation is realized, and the subsequent constraint condition for constraining the joint moment is simplified.
S103, determining a first parameter related to the path speed and a second parameter related to the path acceleration in the fourth non-linear relation, and determining a third parameter related to the path speed and a fourth parameter related to the path acceleration in the third non-linear relation.
In one embodiment, the parameter preceding the "path velocity" in the fourth non-linear relationship is taken as the first parameter associated with the "path velocity", and the parameter preceding the "path acceleration" in the fourth non-linear relationship is taken as the second parameter associated with the "path acceleration". The parameter before the "path velocity" in the third nonlinear relationship is taken as the third parameter associated with the "path velocity", and the parameter before the "path acceleration" in the third nonlinear relationship is taken as the fourth parameter associated with the "path acceleration".
Optionally, S103 includes:
the path velocity in the fourth non-linear relation
Figure BDA0003936982450000087
The previous parameter "A (q (s)) q"(s) + q'(s) T B (q (s)) q'(s) "as a first parameter, the path acceleration
Figure BDA0003936982450000088
The previous parameter "a (q (s)) q'(s)" serves as the second parameter.
Relating the path velocity in a third non-linear relationship
Figure BDA0003936982450000091
The previous parameter "q"(s) "is taken as a third parameter, the path acceleration
Figure BDA0003936982450000092
The previous parameter "q'(s)" is taken as the fourth parameter.
By setting the first parameter to "A (q (s)) q"(s) + q'(s) T B (q (s)) q '(s) ", the second parameter is" A (q (s)) q '(s) ", the third parameter is" q "(s)", the fourth parameter is "q '(s)", and a data base is laid for determining target constraint conditions of the target function according to the first parameter, the second parameter, the third parameter and the fourth parameter.
S104, determining a fifth parameter in the first nonlinear constraint condition according to the first nonlinear constraint condition and the joint moment constraint condition of the fourth nonlinear relationship, and determining a sixth parameter in the second nonlinear constraint condition according to the second nonlinear constraint condition and the joint acceleration constraint condition of the third nonlinear relationship.
The joint moment constraint condition is composed of a minimum value of the joint moment and a maximum value of the joint moment. The joint acceleration constraint condition is composed of a minimum value of the joint acceleration and a maximum value of the joint acceleration.
Optionally, the "determining a fifth parameter in the first nonlinear constraint condition according to the first nonlinear constraint condition and the joint moment constraint condition of the fourth nonlinear relationship" in S104 includes:
and substituting the joint moment constraint condition into the first nonlinear constraint condition to determine a fifth parameter in the first nonlinear constraint condition.
Wherein the first nonlinear constraint condition is as follows:
Fτ≤g 1 ；
wherein the content of the first and second substances,
Figure BDA0003936982450000093
g 1 a fifth parameter is indicated.
The joint moment constraint conditions are as follows:
τ min ≤τ≤τ max ；
wherein τ represents joint moment, τ min Representing the minimum value of the joint moment, τ max Representing the maximum value of the joint moment.
Specifically, the fifth parameter g is determined by the following equation set 1 ：
Figure BDA0003936982450000101
Knowing the fifth parameter
Figure BDA0003936982450000102
By solving for the fifth parameter
Figure BDA0003936982450000103
And a data base is laid for subsequently determining the target constraint condition of the target function according to the fifth parameter.
Optionally, the step S104 of determining a sixth parameter in the second nonlinear constraint according to the second nonlinear constraint and the joint acceleration constraint of the third nonlinear relationship includes:
and substituting the joint acceleration constraint condition into the second nonlinear constraint condition to determine a sixth parameter in the second nonlinear constraint condition.
Wherein the second nonlinear constraint condition is:
Figure BDA0003936982450000104
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000105
g 2 a sixth parameter is represented by a sixth parameter,
Figure BDA0003936982450000106
the joint acceleration constraint conditions are as follows:
Figure BDA0003936982450000107
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000108
the acceleration of the joint is represented by,
Figure BDA0003936982450000109
represents the minimum value of the acceleration of the joint,
Figure BDA00039369824500001010
representing the maximum value of the joint acceleration.
Specifically, the sixth parameter g is determined by the following equation set 2 ：
Figure BDA00039369824500001011
As can be seen, the sixth parameter
Figure BDA00039369824500001012
By solving for the sixth parameter
Figure BDA00039369824500001013
And a data base is laid for subsequently determining the target constraint condition of the target function according to the sixth parameter.
S105, determining a standard path position corresponding to the target robot in the preset path point, determining a path speed range of the preset path point according to the standard path position and a joint speed constraint condition of a second nonlinear relation, and determining a target constraint condition of the target function according to the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter and the sixth parameter.
The preset path points are discrete virtual path points generated by a preset discrete point generation algorithm, and the generated path positions of the preset path points are standard path positions corresponding to the preset path points. The objective function is a function for solving the optimal path velocity of the target robot at each preset path point when the target constraint condition is satisfied.
In one embodiment, a joint velocity relationship is generated according to the joint velocity constraint and the second nonlinear relationship, and a path velocity range of each preset path point is determined according to the joint velocity relationship and a standard path position of each preset path point. And further forming a target constraint condition of the target function based on the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter and the sixth parameter.
Optionally, in S105, "determining a path velocity range of the preset path point according to the standard path position and the joint velocity constraint condition of the second nonlinear relationship", includes:
and combining the joint speed constraint condition and the second nonlinear relation to obtain a joint speed relation, substituting the standard path position into the joint speed relation, and determining the path speed range of the preset path point.
Wherein, the joint speed constraint conditions are as follows:
Figure BDA0003936982450000111
wherein the content of the first and second substances,
Figure BDA0003936982450000112
the velocity of the joint is represented by,
Figure BDA0003936982450000113
the minimum value of the velocity of the joint is indicated,
Figure BDA0003936982450000114
representing the maximum value of the joint velocity.
Specifically, according to a second non-linear relationship:
Figure BDA0003936982450000115
in a second non-linear relationship
Figure BDA0003936982450000116
Incorporated into joint velocity constraints
Figure BDA0003936982450000117
Obtaining a joint velocity relation:
Figure BDA0003936982450000118
moving the ' q '(s) ' in the joint velocity relation to two sides of an inequality to obtain the velocity of the path
Figure BDA0003936982450000119
The path speed range of (1):
Figure BDA0003936982450000121
by determining the path speed range of the preset path point, a data base is laid for subsequently determining a target constraint condition according to the path speed range.
Optionally, the step S105 of determining the target constraint condition of the objective function according to the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter, and the sixth parameter includes:
determining a lower limit parameter and an upper limit parameter of the target constraint condition according to the fifth parameter and the sixth parameter, and determining a constraint matrix of the target constraint condition according to the first parameter, the second parameter, the third parameter and the fourth parameter; and substituting the standard path position into the lower limit parameter, the upper limit parameter, the constraint matrix and the path speed range to determine the target constraint condition of the target function.
Wherein the objective function is:
Figure BDA0003936982450000122
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000123
u represents the path acceleration as the parameter to be optimized of the objective function
Figure BDA0003936982450000124
x represents the path velocity
Figure BDA0003936982450000125
Square value of (2)
Figure BDA0003936982450000126
H represents a zero matrix, z is [2 Δ s, -1]. Δ s represents a distance value between the current preset path point and the adjacent preset path point, for example, assuming that an optimal path speed of the target robot at the ith preset path point is calculated, the ith preset path point is taken as the current preset path point, and the (i + 1) th preset path point adjacent to the ith preset path point is taken as the adjacent preset path point, so that Δ s is determined according to the distance value between the ith preset path point and the (i + 1) th preset path point.
Wherein the target constraint condition is:
Figure BDA0003936982450000127
wherein lbA represents a lower limit parameter, ubA represents an upper limit parameter, lbA and ubA are both constructed based on a fifth parameter and a sixth parameter,
Figure BDA0003936982450000128
Figure BDA0003936982450000129
a representation constraint matrix constructed based on the first parameter, the second parameter, the third parameter, and the fourth parameter,
Figure BDA0003936982450000131
lb represents the parameter to be optimized of the objective function
Figure BDA0003936982450000132
Ub denotes the parameter to be optimized of the objective function
Figure BDA0003936982450000133
The upper limit value of (3).
Figure BDA0003936982450000134
u _ low and u _ up represent a range lower limit value and a range upper limit value of a preset path acceleration range, respectively.
Assuming that the optimal path speed of the target robot at the ith preset path point is calculated, the standard path position s of the ith preset path point is determined i And determining a target polynomial coefficient associated with the ith preset path point according to the association relationship between the preset path point and the polynomial coefficient of the cubic nonlinear polynomial q (), and setting the target polynomial coefficient as the polynomial coefficient of the cubic nonlinear polynomial q ().
The standard path position s i Specific values of lbA and ubA are calculated by substituting "f (q (s))" for lbA and ubA. The standard path position s i Is substituted into
Figure BDA0003936982450000135
"q '(s)", "q"(s) "," A (q (s)) q '(s) ", and" A (q (s)) q "(s) + q '(s) T B (q (s)) q '(s)', obtained by calculation
Figure BDA0003936982450000136
The specific value of (a). The standard path position s i Substituted into path velocity
Figure BDA0003936982450000137
In the path speed range, the range lower limit value x _ low of the path speed range and the range upper limit value x _ up of the path speed range are calculated, and then the target constraint condition of the target function is determined.
Determining a lower limit parameter and an upper limit parameter of a target constraint condition according to a fifth parameter and a sixth parameter, and determining a constraint matrix of the target constraint condition according to the first parameter, the second parameter, the third parameter and the fourth parameter; and substituting the standard path position into the lower limit parameter, the upper limit parameter, the constraint matrix and the path speed range to determine the target constraint condition of the target function, and laying a data foundation for determining the optimal path speed of the target robot at each preset path point based on the target function and the target constraint condition subsequently.
And S106, optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the track planning time according to the optimal path speed, the standard path position and the track planning time.
The trajectory planning time represents the time when the joint position of the target robot needs to be predicted, in other words, when the corresponding joint position of the target robot at all the trajectory planning times is predicted, the joint constraint trajectory planning of the target robot is completed, and then the target robot can be controlled to move based on the joint constraint trajectory planning result.
In one embodiment, the optimal path velocity of the target robot at each preset path point is determined by a preset optimization calculation method based on the target function and the target constraint condition of the target function. Optionally, first, according to the sequence of each preset path point, performing optimization solution from front to back to determine the path acceleration
Figure BDA0003936982450000141
And the square of the path velocity
Figure BDA0003936982450000142
And (4) the value ranges which meet the target constraint condition are all obtained, and then the optimal solution is carried out from back to front according to the sequence of each preset path point so as to determine the optimal path speed and the optimal path acceleration which meet the target constraint condition from the value ranges.
And determining the interval distance value of the candidate interval paths between the preset path points according to the standard path position of each preset path point, and further determining the candidate interval time required by the target robot to finish each candidate interval path according to the interval distance value and the optimal path speed of each preset path point. Determining a candidate standard time period according to the candidate interval time, determining a target standard time period from the candidate standard time period according to the trajectory planning time, further determining a planned path speed corresponding to the target robot at the trajectory planning time according to the starting time and the trajectory planning time of the target standard time period, determining a planned path position corresponding to the target robot at the trajectory planning time according to the planned path speed, and finally determining a planned joint position corresponding to the target robot at the trajectory planning time according to the planned path position and the first nonlinear relation.
The method comprises the steps of determining a first nonlinear relation between a joint position and a path position of a target robot, a second nonlinear relation between a joint speed and a path speed, and a third nonlinear relation between a joint acceleration and a path speed and a path acceleration; determining a fourth nonlinear relation among the joint moment, the path speed and the path acceleration of the target robot according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and a robot dynamic equation; determining a first parameter associated with the path speed and a second parameter associated with the path acceleration in a fourth non-linear relationship, and determining a third parameter associated with the path speed and a fourth parameter associated with the path acceleration in a third non-linear relationship; determining a fifth parameter in the first nonlinear constraint condition according to the first nonlinear constraint condition and the joint moment constraint condition of the fourth nonlinear relationship, and determining a sixth parameter in the second nonlinear constraint condition according to the second nonlinear constraint condition and the joint acceleration constraint condition of the third nonlinear relationship; determining a standard path position corresponding to the target robot at the preset path point, determining a path speed range of the preset path point according to the standard path position and a joint speed constraint condition of a second nonlinear relation, and determining a target constraint condition of a target function according to the path speed range, the standard path position, a first parameter, a second parameter, a third parameter, a fourth parameter, a fifth parameter and a sixth parameter; the method comprises the steps of optimizing the path speed of a target robot at a preset path point according to a target constraint condition and a target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the track planning time according to the optimal path speed, a standard path position and the track planning time.
Example two
Fig. 2 is a flowchart of a joint constraint trajectory planning method for a robot according to a second embodiment of the present invention, and this embodiment further optimizes and expands "determining a planned joint position corresponding to a target robot at a trajectory planning time according to an optimal path speed, a standard path position, and a trajectory planning time" in the first embodiment, and may be combined with the above optional embodiments. As shown in fig. 2, the method includes:
s201, determining interval distance values of candidate interval paths among the preset path points according to the standard path positions of the preset path points, and determining candidate interval time required by the target robot to finish the candidate interval paths according to the interval distance values and the optimal path speeds of the preset path points.
In one embodiment, the distance value of the candidate interval path between the adjacent preset path points is calculated according to the standard path positions of the adjacent preset path points. Namely, the spacing distance value of the candidate spacing path between the preset path points is determined according to the following formula: Δ s i ＝s i+1 -s i Wherein i is not less than 1,s i+1 And s i Are adjacent predetermined waypoints.
And determining the candidate interval time required by the target robot to finish the candidate interval path according to the speed average value of the optimal path speed of the adjacent preset path points and the interval distance value of the candidate interval path between the adjacent preset path points.
Alternatively, the candidate interval time is determined by the following formula:
Figure BDA0003936982450000161
namely, it is
Figure BDA0003936982450000162
Wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003936982450000163
represents the optimal path velocity of the ith preset path point,
Figure BDA0003936982450000164
represents the optimal path velocity, Δ s, of the i +1 th preset path point i A value of a separation distance, Δ t, representing a candidate separation path between the ith preset path point and the (i + 1) th preset path point i Indicating the candidate interval time required for the target robot to travel to complete the candidate interval path.
S202, overlapping the candidate interval time, determining candidate standard time periods of the target robot driving to the candidate interval paths, and determining the target standard time period to which the track planning time belongs from the candidate standard time periods.
In one embodiment, the candidate interval times are superimposed starting from the first candidate interval time, and the candidate standard time period for the target robot to travel to each candidate interval path is determined. And taking the candidate standard time period at the track planning time as a target standard time period.
For example, it is assumed that a candidate interval path between the 1 st preset path point and the 2 nd preset path point is taken as the 1 st candidate interval path, a candidate interval path between the 2 nd preset path point and the 3 rd preset path point is taken as the 2 nd candidate interval path, … …, and a candidate interval path between the i th preset path point and the i +1 th preset path point is taken as the i th candidate interval path.
Suppose that the candidate interval time required for the target robot to travel to complete the 1 st candidate interval path is Δ t 1 The candidate interval time required for completing the 2 nd candidate interval route is Δ t 2 … …, the candidate interval time required for traveling the ith candidate interval route is Δ t i 。
The candidate standard time period of the target robot driving to the 1 st candidate interval path is 0-delta t 1 The candidate standard time period for traveling to the 2 nd candidate interval route is Δ t 1 ～(Δt 1 +Δt 2 ) The candidate criterion time period for traveling to the 3 rd candidate interval route is (Δ t) 1 +Δt 2 )～(Δt 1 +Δt 2 +Δt 3 ) … …, the standard time period for the candidate of the i-th candidate interval route is
Figure BDA0003936982450000171
Assume that the trajectory planning time is current _ time, which belongs to the candidate standard time period (Δ t) 1 +Δt 2 )～(Δt 1 +Δt 2 +Δt 3 ) Then will be (Δ t) 1 +Δt 2 )～(Δt 1 +Δt 2 +Δt 3 ) As a target standard time period.
S203, determining a target interval time corresponding to the target standard time period from the candidate interval times, and determining a target interval path corresponding to the target standard time period from the candidate interval paths.
In one embodiment, the end time and the start time are based on a target standard time periodThe difference between them, the target interval time is determined. For example, assume that the target standard time period is (Δ t) 1 +Δt 2 )～(Δt 1 +Δt 2 +Δt 3 ) Then the time (Δ t) will be expired 1 +Δt 2 +Δt 3 ) And start time (Δ t) 1 +Δt 2 ) Difference Δ t of 3 As the target interval time.
And determining a target interval path associated with the target interval time from the candidate interval paths according to the association relationship between the candidate interval time and the candidate interval paths. For example, the candidate interval time required for the target robot to travel to complete the 1 st candidate interval path is Δ t 1 The candidate interval time required for completing the 2 nd candidate interval route is Δ t 2 … …, the candidate interval time required for traveling the ith candidate interval route is Δ t i Assuming that the target interval time is Δ t 3 Then the target interval path associated with it is the 3 rd candidate interval path.
And S204, determining the planned joint position corresponding to the target robot at the track planning time according to the target interval time, the target interval path, the target standard time period and the track planning time.
Determining a spacing distance value of a candidate spacing path between each preset path point according to the standard path position of each preset path point, and determining candidate spacing time required by the target robot to finish each candidate spacing path according to the spacing distance value and the optimal path speed of each preset path point; overlapping the candidate interval time, determining candidate standard time periods of the target robot driving to each candidate interval path, and determining a target standard time period to which the track planning time belongs from the candidate standard time periods; determining a target interval time corresponding to the target standard time period from the candidate interval times, and determining a target interval path corresponding to the target standard time period from the candidate interval paths; and determining the planned joint position corresponding to the target robot at the track planning time according to the target interval time, the target interval path, the target standard time period and the track planning time, so that the effect of planning the joint constraint track of the target robot is realized.
Optionally, S204 includes the following steps A, B and C:
A. determining the initial optimal path speed of the initial path point and the final optimal path speed of the final path point in the target interval path, and determining the average path acceleration of the target interval path according to the initial optimal path speed, the final optimal path speed and the target interval time.
In one embodiment, a speed difference between the starting optimal path speed and the ending optimal path speed is determined, and an average path acceleration of the target interval path is determined according to a ratio of the speed difference to the target interval time.
Illustratively, assume a starting optimal path velocity of a starting path point of
Figure BDA0003936982450000181
The terminal optimal path speed of the terminal path point is
Figure BDA0003936982450000182
Target interval time of Δ t n Then the average path acceleration of the target interval path is
Figure BDA0003936982450000183
B. And determining a time difference value according to the track planning time and the starting time of the target standard time period, and determining the planned path speed of the target robot at the track planning time according to the time difference value and the average path acceleration.
In one embodiment, a time difference value is determined according to the track planning time and the starting time of the target standard time period, a product result between the time difference value and the average path acceleration is determined, and the planned path speed corresponding to the target robot at the track planning time is determined according to the product result and the starting optimal path speed of the starting path point.
For example, assume that the trajectory planning time is current _ time, and the starting time of the target standard time period is t n Average path acceleration of
Figure BDA0003936982450000184
The initial optimal path velocity of the initial path point is
Figure BDA0003936982450000191
The planned path speed corresponding to the target robot at the track planning time
Figure BDA0003936982450000192
(current time -t n )。
C. And determining the planned path position corresponding to the target robot at the track planning moment according to the planned path speed, the initial optimal path speed and the time difference value, and determining the planned joint position according to the planned path position.
In one embodiment, the planned path speed and the average path speed of the initial optimal path speed are determined, and the planned path position corresponding to the target robot at the trajectory planning time is determined according to the product of the average path speed and the time difference. For example, assume that the time difference is current time -t n The planned path speed is
Figure BDA0003936982450000193
Starting optimal path speed of
Figure BDA0003936982450000194
Then plan the path location
Figure BDA0003936982450000195
And determining polynomial coefficients in the first nonlinear relation according to the target standard time period, substituting the planned path position into the first nonlinear relation, and determining the planned joint position.
Determining the average path acceleration of the target interval path by determining the initial optimal path speed of the initial path point and the final optimal path speed of the final path point in the target interval path and according to the initial optimal path speed, the final optimal path speed and the target interval time; determining a time difference value according to the track planning time and the initial time of the target standard time period, and determining a planned path speed corresponding to the target robot at the track planning time according to the time difference value and the average path acceleration; and determining the planned path position corresponding to the target robot at the track planning moment according to the planned path speed, the initial optimal path speed and the time difference value, and determining the planned joint position according to the planned path position, thereby realizing the effect of planning the joint constraint track of the target robot.
Optionally, the step C of "determining the planned joint position according to the planned path position" includes:
determining a target polynomial coefficient associated with the target standard time period according to the target standard time period and the association relationship between the candidate standard time period and the candidate polynomial coefficient, and taking the target polynomial coefficient as a polynomial coefficient in a first nonlinear relationship to obtain an optimal nonlinear relationship; and substituting the planned path position into the optimal nonlinear relation to determine the planned joint position.
For the cubic nonlinear polynomial q () in the first nonlinear relation q = q(s), an association relation between the candidate polynomial coefficients and each candidate standard time segment is established in advance, that is, the candidate polynomial coefficients associated with each candidate standard time segment can be directly determined according to any candidate standard time segment.
In one embodiment, the target standard time segment is matched to the candidate standard time segment, and the candidate polynomial coefficient associated with the candidate standard time segment matched to the target standard time segment is taken as the target polynomial coefficient. And setting coefficients in a cubic nonlinear polynomial q () in the first nonlinear relation q = q(s) as coefficients of a target polynomial to obtain an optimal nonlinear relation, and substituting a planned path position corresponding to the target robot at the track planning moment into the optimal nonlinear relation to determine a planned joint position corresponding to the target robot at the track planning moment.
Exemplary, hypothetical determinationHas a1, b1, c1, d1, q = as 3 +bs 2 Setting the coefficient a to a1, the coefficient b to b1, the coefficient c to c1, and the coefficient d to d1 in + cs + d results in the optimal nonlinear relationship q = a1s 3 +b1s 2 + c1s + d1. Assuming that the planned path position corresponding to the target robot at the track planning time is s cur Then the target robot is at the planning joint position corresponding to the track planning time
Figure BDA0003936982450000201
Figure BDA0003936982450000202
Determining a target polynomial coefficient associated with the target standard time period according to the target standard time period and the association relationship between the candidate standard time period and the candidate polynomial coefficient, and taking the target polynomial coefficient as a polynomial coefficient in a first nonlinear relationship to obtain an optimal nonlinear relationship; the planned path position is substituted into the optimal nonlinear relation to determine the planned joint position, so that the effect of dynamically determining the polynomial coefficient in the first nonlinear relation according to different target standard time periods is achieved, and the accuracy of the planned joint position obtained through final calculation is guaranteed.
EXAMPLE III
Fig. 3 is a schematic structural diagram of a joint constraint trajectory planning apparatus for a robot according to a third embodiment of the present invention. As shown in fig. 3, the apparatus includes:
a nonlinear relationship determination first module 31 for determining a first nonlinear relationship between the joint position and the path position, a second nonlinear relationship between the joint velocity and the path velocity, and a third nonlinear relationship between the joint acceleration and the path velocity and the path acceleration of the target robot;
a nonlinear relation determination second module 32, configured to determine a fourth nonlinear relation between the joint moment of the target robot and the path velocity and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation, and the robot dynamics equation;
a first parameter determining module 33, configured to determine a first parameter associated with the path velocity and a second parameter associated with the path acceleration in a fourth non-linear relationship, and determine a third parameter associated with the path velocity and a fourth parameter associated with the path acceleration in a third non-linear relationship;
a second parameter determining module 34, configured to determine a fifth parameter in the first nonlinear constraint condition according to the first nonlinear constraint condition and the joint moment constraint condition of the fourth nonlinear relationship, and determine a sixth parameter in the second nonlinear constraint condition according to the second nonlinear constraint condition and the joint acceleration constraint condition of the third nonlinear relationship;
the constraint condition determining module 35 is configured to determine a standard path position corresponding to the preset path point of the target robot, determine a path speed range of the preset path point according to the standard path position and a joint speed constraint condition of the second nonlinear relationship, and determine a target constraint condition of the target function according to the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter, and the sixth parameter;
and a planned joint position determining module 36, configured to optimize the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determine the optimal path speed of the target robot at the preset path point, and determine a planned joint position corresponding to the target robot at the trajectory planning time according to the optimal path speed, the standard path position, and the trajectory planning time.
Optionally, the first nonlinear relationship is:
q＝q(s)；
the second nonlinear relationship is:
Figure BDA0003936982450000211
the third non-linear relationship is:
Figure BDA0003936982450000221
where s denotes a path position, q denotes a joint position,
Figure BDA0003936982450000222
which is indicative of the speed of the path,
Figure BDA0003936982450000223
the acceleration of the joint is represented by,
Figure BDA0003936982450000224
representing the path acceleration, q () is a cubic nonlinear polynomial.
Optionally, the second nonlinear relation determining module 32 is specifically configured to:
substituting the first nonlinear relation, the second nonlinear relation and the third nonlinear relation into a robot dynamics equation to obtain a fourth nonlinear relation;
wherein the fourth nonlinear relationship is:
Figure BDA0003936982450000225
the robot dynamics equation is:
Figure BDA0003936982450000226
where a () represents an inertia matrix, B () represents a matrix of coriolis force and centripetal force coefficients, f () represents a gravitational moment, and τ represents a joint moment.
Optionally, the parameter determining first module 33 is specifically configured to:
taking the following parameters in the fourth non-linear relationship as first parameters:
A(q(s))q″(s)+q′(s) T B(q(s))q′(s)；
taking the following parameters in the fourth non-linear relationship as second parameters:
A(q(s))q′(s)。
optionally, the parameter determining first module 33 is further specifically configured to:
taking the following parameters in the third non-linear relationship as third parameters:
q″(s)；
taking the following parameters in the third nonlinear relation as fourth parameters:
q′(s)。
optionally, the parameter determining second module 34 is specifically configured to:
substituting the joint moment constraint condition into the first nonlinear constraint condition to determine a fifth parameter in the first nonlinear constraint condition;
wherein, the joint moment constraint conditions are as follows:
τ min ≤τ≤τ max ；
wherein τ represents joint moment, τ min Representing the minimum value of the joint moment, τ max Represents the maximum value of the joint moment;
the first nonlinear constraint is:
Fτ≤g 1 ；
wherein the content of the first and second substances,
Figure BDA0003936982450000231
g 1 a fifth parameter is represented which is a function of,
Figure BDA0003936982450000232
optionally, the parameter determining second module 34 is further specifically configured to:
substituting the joint acceleration constraint condition into a second nonlinear constraint condition to determine a sixth parameter in the second nonlinear constraint condition;
wherein, the joint acceleration constraint conditions are as follows:
Figure BDA0003936982450000233
wherein the content of the first and second substances,
Figure BDA0003936982450000234
the acceleration of the joint is represented by,
Figure BDA0003936982450000235
represents the minimum value of the acceleration of the joint,
Figure BDA0003936982450000236
represents the maximum value of the joint acceleration;
the second nonlinear constraint is:
Figure BDA0003936982450000237
wherein the content of the first and second substances,
Figure BDA0003936982450000238
g 2 a sixth parameter is represented by a sixth parameter,
Figure BDA0003936982450000239
optionally, the constraint condition determining module 35 is specifically configured to:
combining the joint speed constraint condition with the second nonlinear relation to obtain a joint speed relation, substituting the standard path position into the joint speed relation, and determining a path speed range of a preset path point;
wherein, the joint velocity constraint conditions are as follows:
Figure BDA00039369824500002310
wherein the content of the first and second substances,
Figure BDA00039369824500002311
the velocity of the joint is represented by,
Figure BDA00039369824500002312
the minimum value of the velocity of the joint is indicated,
Figure BDA00039369824500002313
represents the maximum value of the joint velocity;
the joint velocity relationship is as follows:
Figure BDA0003936982450000241
the path speed ranges are:
Figure BDA0003936982450000242
optionally, the constraint condition determining module 35 is further specifically configured to:
determining a lower limit parameter and an upper limit parameter of the target constraint condition according to the fifth parameter and the sixth parameter, and determining a constraint matrix of the target constraint condition according to the first parameter, the second parameter, the third parameter and the fourth parameter;
substituting the standard path position into the lower limit parameter, the upper limit parameter, the constraint matrix and the path speed range to determine a target constraint condition of the target function;
wherein the objective function is:
Figure BDA0003936982450000243
wherein u represents the path acceleration
Figure BDA0003936982450000244
x represents the path velocity
Figure BDA0003936982450000245
Square value of
Figure BDA0003936982450000246
H represents a zero matrix, z is [2 Δ s, -1]Δ s represents a distance value between the current preset waypoint and the adjacent preset waypoint;
the target constraints are:
Figure BDA0003936982450000247
wherein lbA represents the lower limit parameter,
Figure BDA0003936982450000248
ubA denotes the upper limit parameter,
Figure BDA0003936982450000249
u _ low and u _ up represent the range lower limit and range upper limit of the path acceleration range, respectively, x _ low and x _ up represent the range lower limit and range upper limit of the path velocity range, respectively,
Figure BDA00039369824500002410
a constraint matrix is represented that is,
Figure BDA00039369824500002411
optionally, the planned joint position determining module 36 is specifically configured to:
determining a spacing distance value of a candidate spacing path between each preset path point according to the standard path position of each preset path point, and determining candidate spacing time required by the target robot to finish each candidate spacing path according to the spacing distance value and the optimal path speed of each preset path point;
overlapping the candidate interval time, determining candidate standard time periods of the target robot driving to each candidate interval path, and determining a target standard time period to which the track planning time belongs from the candidate standard time periods;
determining a target interval time corresponding to the target standard time period from the candidate interval times, and determining a target interval path corresponding to the target standard time period from the candidate interval paths;
and determining the planned joint position corresponding to the target robot at the track planning time according to the target interval time, the target interval path, the target standard time period and the track planning time.
Optionally, the planned joint position determining module 36 is further configured to:
determining the initial optimal path speed of an initial path point and the final optimal path speed of a final path point in the target interval path, and determining the average path acceleration of the target interval path according to the initial optimal path speed, the final optimal path speed and the target interval time;
determining a time difference value according to the track planning time and the initial time of the target standard time period, and determining a planned path speed corresponding to the target robot at the track planning time according to the time difference value and the average path acceleration;
and determining the planned path position corresponding to the target robot at the track planning moment according to the planned path speed, the initial optimal path speed and the time difference value, and determining the planned joint position according to the planned path position.
Optionally, the planned joint position determining module 36 is further configured to:
determining a target polynomial coefficient associated with the target standard time period according to the target standard time period and the association relationship between the candidate standard time period and the candidate polynomial coefficient, and taking the target polynomial coefficient as a polynomial coefficient in a first nonlinear relationship to obtain an optimal nonlinear relationship;
and substituting the planned path position into the optimal nonlinear relation to determine the planned joint position.
The joint constraint track planning device of the robot provided by the embodiment of the invention can execute the joint constraint track planning method of the robot provided by any embodiment of the invention, and has corresponding functional modules and beneficial effects of the execution method.
Example 4
FIG. 4 shows a schematic block diagram of an electronic device 40 that may be used to implement an embodiment of the invention. Electronic devices are intended to represent various forms of digital computers, such as laptops, desktops, workstations, personal digital assistants, servers, blade servers, mainframes, and other appropriate computers. The electronic device may also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smart phones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions, are meant to be exemplary only, and are not meant to limit implementations of the inventions described and/or claimed herein.
As shown in fig. 4, the electronic device 40 includes at least one processor 41, and a memory communicatively connected to the at least one processor 41, such as a Read Only Memory (ROM) 42, a Random Access Memory (RAM) 43, and the like, wherein the memory stores a computer program executable by the at least one processor, and the processor 41 may perform various suitable actions and processes according to the computer program stored in the Read Only Memory (ROM) 42 or the computer program loaded from a storage unit 48 into the Random Access Memory (RAM) 43. In the RAM 43, various programs and data necessary for the operation of the electronic apparatus 40 can also be stored. The processor 41, the ROM 42, and the RAM 43 are connected to each other via a bus 44. An input/output (I/O) interface 45 is also connected to bus 44.
A number of components in the electronic device 40 are connected to the I/O interface 45, including: an input unit 46 such as a keyboard, a mouse, etc.; an output unit 47 such as various types of displays, speakers, and the like; a storage unit 48 such as a magnetic disk, an optical disk, or the like; and a communication unit 49 such as a network card, modem, wireless communication transceiver, etc. The communication unit 49 allows the electronic device 40 to exchange information/data with other devices via a computer network such as the internet and/or various telecommunication networks.
Processor 41 may be a variety of general and/or special purpose processing components with processing and computing capabilities. Some examples of processor 41 include, but are not limited to, a Central Processing Unit (CPU), a Graphics Processing Unit (GPU), various specialized Artificial Intelligence (AI) computing chips, various processors running machine learning model algorithms, a Digital Signal Processor (DSP), and any suitable processor, controller, microcontroller, or the like. The processor 41 performs the various methods and processes described above, such as a joint constraint trajectory planning method for a robot.
In some embodiments, the joint constraint trajectory planning method of the robot may be implemented as a computer program tangibly embodied in a computer-readable storage medium, such as storage unit 48. In some embodiments, part or all of the computer program may be loaded and/or installed onto the electronic device 40 via the ROM 42 and/or the communication unit 49. When the computer program is loaded into the RAM 43 and executed by the processor 41, one or more steps of the method for planning a joint constraint trajectory of a robot described above may be performed. Alternatively, in other embodiments, the processor 41 may be configured by any other suitable means (e.g., by means of firmware) to perform a joint constraint trajectory planning method for the robot.
Various implementations of the systems and techniques described here above may be implemented in digital electronic circuitry, integrated circuitry, field Programmable Gate Arrays (FPGAs), application Specific Integrated Circuits (ASICs), application Specific Standard Products (ASSPs), system on a chip (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and/or combinations thereof. These various embodiments may include: implemented in one or more computer programs that are executable and/or interpretable on a programmable system including at least one programmable processor, which may be special or general purpose, receiving data and instructions from, and transmitting data and instructions to, a storage system, at least one input device, and at least one output device.
Computer programs for implementing the methods of the present invention can be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus, such that the computer programs, when executed by the processor, cause the functions/acts specified in the flowchart and/or block diagram block or blocks to be performed. A computer program can execute entirely on a machine, partly on a machine, as a stand-alone software package partly on a machine and partly on a remote machine or entirely on a remote machine or server.
In the context of the present invention, a computer-readable storage medium may be a tangible medium that can contain, or store a computer program for use by or in connection with an instruction execution system, apparatus, or device. A computer readable storage medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. Alternatively, the computer readable storage medium may be a machine readable signal medium. More specific examples of a machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
To provide for interaction with a user, the systems and techniques described here can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to a user; and a keyboard and a pointing device (e.g., a mouse or a trackball) by which a user can provide input to the electronic device. Other kinds of devices may also be used to provide for interaction with a user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form, including acoustic, speech, or tactile input.
The systems and techniques described here can be implemented in a computing system that includes a back-end component (e.g., as a data server), or that includes a middleware component (e.g., an application server), or that includes a front-end component (e.g., a user computer having a graphical user interface or a web browser through which a user can interact with an implementation of the systems and techniques described here), or any combination of such back-end, middleware, or front-end components. The components of the system can be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: local Area Networks (LANs), wide Area Networks (WANs), blockchain networks, and the internet.
The computing system may include clients and servers. A client and server are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a client-server relationship to each other. The server can be a cloud server, also called a cloud computing server or a cloud host, and is a host product in a cloud computing service system, so that the defects of high management difficulty and weak service expansibility in the traditional physical host and VPS service are overcome.
It should be understood that various forms of the flows shown above may be used, with steps reordered, added, or deleted. For example, the steps described in the present invention may be executed in parallel, sequentially, or in different orders, and are not limited herein as long as the desired results of the technical solution of the present invention can be achieved.
The above-described embodiments should not be construed as limiting the scope of the invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions may be made in accordance with design requirements and other factors. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (15)

1. A method for planning a joint constraint trajectory of a robot is characterized by comprising the following steps:
determining a first non-linear relationship between the joint position and the path position, a second non-linear relationship between the joint velocity and the path velocity, and a third non-linear relationship between the joint acceleration and the path velocity and the path acceleration of the target robot;
determining a fourth nonlinear relation among the joint moment of the target robot, the path speed and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and a robot dynamic equation;
determining a first parameter associated with the path velocity and a second parameter associated with the path acceleration in the fourth non-linear relationship, and determining a third parameter associated with the path velocity and a fourth parameter associated with the path acceleration in the third non-linear relationship;
determining a fifth parameter in the first nonlinear constraint condition according to a first nonlinear constraint condition and a joint moment constraint condition of the fourth nonlinear relationship, and determining a sixth parameter in the second nonlinear constraint condition according to a second nonlinear constraint condition and a joint acceleration constraint condition of the third nonlinear relationship;
determining a standard path position corresponding to a preset path point by the target robot, determining a path speed range of the preset path point according to the standard path position and a joint speed constraint condition of the second nonlinear relationship, and determining a target constraint condition of a target function according to the path speed range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter and the sixth parameter;
optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the trajectory planning time according to the optimal path speed, the standard path position and the trajectory planning time.
2. The method of claim 1, wherein the first non-linear relationship is:
q＝q(s)；
the second nonlinear relationship is:
Figure FDA0003936982440000021
the third nonlinear relationship is:
Figure FDA0003936982440000022
wherein s represents the path position, q represents the joint position,
Figure FDA0003936982440000023
is representative of the speed of the path or paths,
Figure FDA0003936982440000024
the acceleration of the joint is represented by a representation,
Figure FDA0003936982440000025
representing the path acceleration, q () is a cubic nonlinear polynomial.
3. The method of claim 2, wherein determining a fourth non-linear relationship between the joint moment of the target robot and the path velocity and the path acceleration from the first non-linear relationship, the second non-linear relationship, the third non-linear relationship, and the robot dynamics equation comprises:
substituting the first nonlinear relation, the second nonlinear relation and the third nonlinear relation into the robot dynamics equation to obtain a fourth nonlinear relation;
wherein the fourth nonlinear relationship is:
Figure FDA0003936982440000026
the robot dynamics equation is as follows:
Figure FDA0003936982440000027
where a () represents an inertia matrix, B () represents a matrix of coriolis force and centripetal force coefficients, f () represents a gravitational moment, and τ represents a joint moment.
4. The method of claim 3, wherein said determining a first parameter associated with said path velocity and a second parameter associated with said path acceleration in said fourth non-linear relationship comprises:
taking the following parameters in the fourth non-linear relationship as the first parameters:
A(q(s))q″(s)+q′(s) T B(q(s))q′(s)；
taking the following parameters in the fourth non-linear relationship as the second parameters:
A(q(s))q′(s)。
5. the method of claim 2, wherein said determining a third parameter associated with said path velocity and a fourth parameter associated with said path acceleration in said third non-linear relationship comprises:
taking the following parameters in the third non-linear relationship as the third parameters:
q″(s)；
taking the following parameters in the third nonlinear relationship as the fourth parameters:
q ′ (s)。
6. the method of claim 3, wherein determining a fifth parameter in the nonlinear constraint from the first nonlinear constraint and the joint moment constraint of the fourth nonlinear relationship comprises:
substituting the joint moment constraint condition into the first nonlinear constraint condition to determine a fifth parameter in the first nonlinear constraint condition;
wherein, the joint moment constraint conditions are as follows:
τ min ≤τ≤τ max ；
wherein τ represents joint moment, τ min Representing the minimum value of the joint moment, τ max Represents the maximum value of the joint moment;
the first nonlinear constraint condition is as follows:
Fτ≤g 1 ；
wherein the content of the first and second substances,
Figure FDA0003936982440000031
g 1 is indicative of the fifth parameter or parameters,
Figure FDA0003936982440000032
7. the method of claim 2, wherein determining a sixth parameter in the second non-linear constraint from the second non-linear constraint and the joint acceleration constraint of the third non-linear relationship comprises:
substituting the joint acceleration constraint condition into the second nonlinear constraint condition to determine a sixth parameter in the second nonlinear constraint condition;
wherein the joint acceleration constraint conditions are as follows:
Figure FDA0003936982440000041
wherein the content of the first and second substances,
Figure FDA0003936982440000042
the acceleration of the joint is represented by,
Figure FDA0003936982440000043
represents the minimum value of the acceleration of the joint,
Figure FDA0003936982440000044
represents the maximum value of the joint acceleration;
the second nonlinear constraint condition is as follows:
Figure FDA0003936982440000045
wherein the content of the first and second substances,
Figure FDA0003936982440000046
g 2 is indicative of the sixth parameter or parameters,
Figure FDA0003936982440000047
8. the method of claim 3, wherein determining the path velocity range for the preset path point based on the standard path position and the joint velocity constraint of the second non-linear relationship comprises:
combining the joint speed constraint condition and the second nonlinear relation to obtain a joint speed relation, substituting the standard path position into the joint speed relation, and determining a path speed range of the preset path point;
wherein the joint velocity constraint conditions are as follows:
Figure FDA0003936982440000048
wherein the content of the first and second substances,
Figure FDA0003936982440000049
the velocity of the joint is represented by,
Figure FDA00039369824400000410
the minimum value of the velocity of the joint is indicated,
Figure FDA00039369824400000411
represents the maximum value of the joint velocity;
the joint velocity relationship is as follows:
Figure FDA00039369824400000412
the path speed ranges are:
Figure FDA00039369824400000413
9. the method of claim 8, wherein determining the target constraint for the objective function based on the path velocity range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter, and the sixth parameter comprises:
determining a lower limit parameter and an upper limit parameter of the target constraint condition according to the fifth parameter and the sixth parameter, and determining a constraint matrix of the target constraint condition according to the first parameter, the second parameter, the third parameter and the fourth parameter;
substituting the standard path position into the lower limit parameter, the upper limit parameter, the constraint matrix and the path speed range to determine a target constraint condition of an objective function;
wherein the objective function is:
Figure FDA0003936982440000051
wherein u represents the path acceleration
Figure FDA0003936982440000052
x represents the path velocity
Figure FDA0003936982440000053
Square value of
Figure FDA0003936982440000054
H represents a zero matrix, z is [2 Δ s, -1]Δ s represents a distance value between the current preset waypoint and the adjacent preset waypoint;
the target constraint conditions are as follows:
Figure FDA0003936982440000055
wherein lbA represents the lower limit parameter,
Figure FDA0003936982440000056
ubA represents the upper limit parameter,
Figure FDA0003936982440000057
u _ low and u _ up respectively represent a range lower limit value and a range upper limit value of a path acceleration range, x _ low and x _ up respectively represent a range lower limit value and a range upper limit value of the path speed range,
Figure FDA0003936982440000058
the constraint matrix is represented by a matrix of constraints,
Figure FDA0003936982440000059
10. the method of claim 2, wherein the determining the planned joint position of the target robot at the trajectory planning time based on the optimal path velocity, the standard path position, and the trajectory planning time comprises:
determining a spacing distance value of a candidate spacing path between the preset path points according to the standard path position of each preset path point, and determining candidate spacing time required by the target robot to finish each candidate spacing path according to the spacing distance value and the optimal path speed of each preset path point;
superposing the candidate interval time, determining candidate standard time periods of the target robot driving to each candidate interval path, and determining a target standard time period to which the trajectory planning time belongs from the candidate standard time periods;
determining a target interval time corresponding to the target standard time period from the candidate interval times, and determining a target interval path corresponding to the target standard time period from the candidate interval paths;
and determining a planned joint position corresponding to the target robot at the track planning time according to the target interval time, the target interval path, the target standard time period and the track planning time.
11. The method of claim 10, wherein determining the planned joint position of the target robot at the trajectory planning time based on the target interval time, the target interval path, the target standard time period, and the trajectory planning time comprises:
determining the initial optimal path speed of an initial path point and the final optimal path speed of a final path point in the target interval path, and determining the average path acceleration of the target interval path according to the initial optimal path speed, the final optimal path speed and the target interval time;
determining a time difference value according to the track planning time and the starting time of the target standard time period, and determining a planned path speed corresponding to the target robot at the track planning time according to the time difference value and the average path acceleration;
and determining a planned path position corresponding to the target robot at the track planning moment according to the planned path speed, the initial optimal path speed and the time difference value, and determining the planned joint position according to the planned path position.
12. The method of claim 11, wherein the determining the planned joint position from the planned path position comprises:
determining a target polynomial coefficient associated with the target standard time period according to a target standard time period and an association relationship between the candidate standard time period and the candidate polynomial coefficient, and taking the target polynomial coefficient as a polynomial coefficient in the first nonlinear relationship to obtain an optimal nonlinear relationship;
and substituting the planned path position into the optimal nonlinear relation to determine the planned joint position.
13. A joint constraint trajectory planning device for a robot, comprising:
a nonlinear relationship determination first module for determining a first nonlinear relationship between the joint position and the path position, a second nonlinear relationship between the joint velocity and the path velocity, and a third nonlinear relationship between the joint acceleration and the path velocity and the path acceleration of the target robot;
a nonlinear relation determination second module for determining a fourth nonlinear relation between the joint moment of the target robot and the path velocity and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and the robot dynamics equation;
a first parameter determining module, configured to determine a first parameter associated with the path velocity and a second parameter associated with the path acceleration in the fourth non-linear relationship, and determine a third parameter associated with the path velocity and a fourth parameter associated with the path acceleration in the third non-linear relationship;
a second parameter determining module, configured to determine a fifth parameter in the first nonlinear constraint condition according to a first nonlinear constraint condition and a joint torque constraint condition of the fourth nonlinear relationship, and determine a sixth parameter in the second nonlinear constraint condition according to a second nonlinear constraint condition and a joint acceleration constraint condition of the third nonlinear relationship;
a constraint condition determining module, configured to determine a standard path position corresponding to a preset path point of the target robot, determine a path velocity range of the preset path point according to the standard path position and a joint velocity constraint condition of the second nonlinear relationship, and determine a target constraint condition of a target function according to the path velocity range, the standard path position, the first parameter, the second parameter, the third parameter, the fourth parameter, the fifth parameter, and the sixth parameter;
and the planned joint position determining module is used for optimizing the path speed of the target robot at the preset path point according to the target constraint condition and the target function, determining the optimal path speed of the target robot at the preset path point, and determining the planned joint position corresponding to the target robot at the track planning time according to the optimal path speed, the standard path position and the track planning time.
14. An electronic device, characterized in that the electronic device comprises:
at least one processor; and
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores a computer program executable by the at least one processor to enable the at least one processor to perform the method of joint constraint trajectory planning for a robot of any of claims 1-12.
15. A computer-readable storage medium storing computer instructions for causing a processor to perform the method of joint constraint trajectory planning for a robot of any of claims 1-12 when executed.
CN202211407053.6A 2022-11-10 2022-11-10 Robot joint constraint trajectory planning method, device, equipment and medium Pending CN115625712A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211407053.6A CN115625712A (en) 2022-11-10 2022-11-10 Robot joint constraint trajectory planning method, device, equipment and medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211407053.6A CN115625712A (en) 2022-11-10 2022-11-10 Robot joint constraint trajectory planning method, device, equipment and medium

Publications (1)

Publication Number Publication Date
CN115625712A true CN115625712A (en) 2023-01-20

Family

ID=84907706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211407053.6A Pending CN115625712A (en) 2022-11-10 2022-11-10 Robot joint constraint trajectory planning method, device, equipment and medium

Country Status (1)

Country Link
CN (1) CN115625712A (en)

Similar Documents

Publication Publication Date Title
CN110850807B (en) Singular point avoiding method, device, equipment and medium
CN113126643A (en) Intelligent robust reentry guidance method and system for hypersonic aircraft
Zhang et al. Disturbance‐observer‐based antiswing control of underactuated crane systems via terminal sliding mode
JP2018202993A (en) Train operation control device
WO2023173677A1 (en) Trajectory fusion method and apparatus, and device and storage medium
EP4151485A2 (en) Method and apparatus for determining information, storage medium and computer program product
CN114348026B (en) Vehicle control method, device, equipment and storage medium
CN114987546A (en) Training method, device and equipment of trajectory prediction model and storage medium
CN115625712A (en) Robot joint constraint trajectory planning method, device, equipment and medium
CN115946120A (en) Mechanical arm control method, device, equipment and medium
CN115841197A (en) Path planning method, device, equipment and storage medium
CN114859925A (en) Ship route planning method based on dynamic window method
CN113799798A (en) Method and device for determining driving track of vehicle, electronic equipment and memory
CN114620074A (en) Vehicle control method, device, electronic device and storage medium
CN114274148B (en) Track planning method and device, electronic equipment and storage medium
CN114771518B (en) Lane center guide wire generation method and device, electronic equipment and medium
CN114571457B (en) Singular pose avoidance method, apparatus, device and storage medium
CN116679573B (en) Consistency tracking control method and device, electronic equipment and storage medium
CN113075921B (en) Local path planning method and device for unmanned equipment
CN117260738A (en) Robot control method, device, equipment and storage medium
CN114771501A (en) Vehicle lateral control method and device, storage medium and automatic driving vehicle
CN114802203A (en) Longitudinal control method and device of vehicle, storage medium and automatic driving vehicle
CN117565893A (en) Vehicle control method and device, electronic equipment and storage medium
CN117804480A (en) Method, device, equipment and medium for generating tracking route of automatic driving vehicle
CN117901113A (en) Residual vibration control method, device, equipment and storage medium

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