Detailed Description
In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in which it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
It should be noted that the terms "first," "second," "third," "fourth," "fifth," "sixth," "candidate," and "target" in the description and claims of the present invention and the above-described drawings are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented in sequences other than those illustrated or otherwise 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 conventional robot track planning method generally only considers the static constraint of Cartesian space, such as the static constraint of track position, speed, acceleration, jerk and the like, but does not consider the joint constraint of joint space, so that the problems of joint overload or low joint performance utilization rate and the like of the robot can occur, and the accuracy of the joint constraint track planning of the robot is greatly influenced.
Example 1
Fig. 1 is a flowchart of a joint constraint trajectory planning method of a robot according to an embodiment of the present invention, where the method may be performed by a joint constraint trajectory planning device of a robot, and the joint constraint trajectory planning device of the robot may be implemented in hardware and/or software, and the joint constraint trajectory planning device of the robot may 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 mounted with at least one robot joint representing a connection 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 links to any position and angle. The number of the robot joints in the target robot may be set and adjusted according to the working scene of the target robot, for example, the target robot may include six robot joints, i.e., a six-axis robot.
The joint position represents the position where each machine joint of the target robot is located, which may be represented by an n-dimensional vector, n being the number of machine joints comprised by the target robot. The joint velocity represents the running speed of the machine joint, and the joint acceleration represents the running acceleration of the machine joint.
The path position is position information which can be calculated according to the running time of the target robot, namely, the path position and the running time of the target robot have a functional relation of s=f (T), wherein s represents the path position and T represents the running time of the target robot. The path speed is in the form of the first derivative of the path position, i.e. the path speed has a functional relationship with the run time of the target robot: Where s denotes a path position, T denotes a run time of the target robot,Representing the path speed. The path acceleration is in the form of the second derivative of the path position, i.e. the path acceleration has a functional relationship with the runtime of the target robot: Where s denotes a path position, T denotes a run time of the target robot,Indicating the path acceleration.
In one embodiment, the joint position and the path position have a nonlinear relationship, that is, a relationship between the joint position and the path position can form a nonlinear polynomial equation, and optionally, the joint position and the path position form a relationship between the third-order nonlinear polynomial equation, that is, a first nonlinear relationship between the joint position and the path position can be represented by q=as3+bs2 +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 nonlinear relationship, that is, the relationship between the joint velocity and the path velocity can also form a nonlinear polynomial equation, that is, a second nonlinear relationship between the joint velocity and the path velocity can be expressed as follows: Wherein,For joint velocity, s is path position, a, b, c, d is polynomial coefficient,Is the path speed.
The joint acceleration and the path velocity and the path acceleration also have a nonlinear relationship, that is, the relationship between 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: Wherein,Is joint acceleration, s is path position, a, b, c, d is polynomial coefficient,In order for the path velocity to be the same,Is the path acceleration.
Alternatively, the first nonlinear relationship, the second nonlinear relationship, and the third nonlinear relationship may be represented by:
Where s represents the path position, q represents the joint position,The path velocity is indicated as such,Indicating the acceleration of the joint,Representing path acceleration, q () is a cubic nonlinear polynomial.
By setting the first nonlinear relationship between joint position and path position to be q=q(s), the second nonlinear relationship between joint velocity and path velocity isThe third nonlinear relationship between the joint acceleration and the path velocity and the path acceleration is thatTherefore, the track planning is converted from a multidimensional problem to a two-dimensional problem, so that the subsequent constraint on joint information is converted into the constraint on path information, and the constraint condition is simplified.
S102, determining a fourth nonlinear relation between the joint moment of the target robot and the path speed and the path acceleration according to the first nonlinear relation, the second nonlinear relation, the third nonlinear relation and the robot dynamics equation.
Wherein the robot dynamics equation is an equation describing the relationship between the force and the motion of the robot mechanism.
In one embodiment, the parameter replacement is performed on the "joint position parameter" in the robot dynamics equation according to the first nonlinear relationship, the parameter replacement is performed on the "joint velocity parameter" in the robot dynamics equation according to the second nonlinear relationship, and the parameter replacement is performed on the "joint acceleration parameter" in the robot dynamics equation according to the third nonlinear relationship.
Optionally, S102 includes:
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:
where A () represents the inertial matrix, B () represents the coriolis force and centripetal force coefficient matrix, f () represents the gravitational moment, and τ represents the joint moment.
Specifically, "q" in the robot dynamics equation is replaced with "q(s)",Replaced byReplaced byAnd obtaining a fourth nonlinear relation according to the robot dynamics equation after parameter replacement:
by 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, the joint moment is converted into a path information representation form, and the constraint condition for constraining the joint moment subsequently is simplified.
S103, determining a first parameter related to the path speed and a second parameter related to the path acceleration in a fourth nonlinear relation, and determining a third parameter related to the path speed and a fourth parameter related to the path acceleration in a third nonlinear relation.
In one embodiment, a parameter before "path speed" in the fourth nonlinear relationship is taken as a first parameter associated with "path speed", and a parameter before "path acceleration" in the fourth nonlinear relationship is taken as a second parameter associated with "path acceleration". The parameter before the "path speed" in the third nonlinear relationship is taken as a third parameter associated with the "path speed", and the parameter before the "path acceleration" in the third nonlinear relationship is taken as a fourth parameter associated with the "path acceleration".
Optionally, S103 includes:
The path speed in the fourth nonlinear relationThe previous parameter "a (q (s)) q"(s) +q '(s)T B (q (s)) q'(s) "is taken as the first parameter, path accelerationThe previous parameter "a (q (s)) q'(s)" is used as the second parameter.
The path speed in the third nonlinear relationThe former parameter "q"(s) "is used as the third parameter, path accelerationThe previous parameter "q'(s)" is used as the fourth parameter.
By setting the first parameter to be "a (q (s)) q"(s) +q '(s)T B (q (s)) q'(s) ", the second parameter to be" a (q (s)) q '(s) ", the third parameter to be" q "(s)", and the fourth parameter to be "q'(s)", a data base is laid for the subsequent determination of the target constraint condition of the objective function based on 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 joint moment constraint condition of the first nonlinear constraint condition and the fourth nonlinear relation, and determining a sixth parameter in the second nonlinear constraint condition according to the joint acceleration constraint condition of the second nonlinear constraint condition and the third nonlinear relation.
Wherein 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 consists of a minimum value of the joint acceleration and a maximum value of the joint acceleration.
Optionally, in S104, "determining the fifth parameter in the first nonlinear constraint according to the joint moment constraint of the first nonlinear constraint and the fourth nonlinear relation", includes:
Substituting the joint moment constraint condition into the first nonlinear constraint condition, and determining a fifth parameter in the first nonlinear constraint condition.
Wherein the first nonlinear constraint condition is:
Fτ≤g1;
Wherein,G1 denotes a fifth parameter.
The joint moment constraint conditions are as follows:
τmin≤τ≤τmax;
Where τ represents the joint moment, τmin represents the minimum of the joint moment, and τmax represents the maximum of the joint moment.
Specifically, the fifth parameter g1 is determined by the following equation set:
it can be seen that the fifth parameter
By solving for the fifth parameterAnd laying a data foundation for determining target constraint conditions of the target function according to the fifth parameter.
Optionally, in S104, "determining the sixth parameter in the second nonlinear constraint according to the second nonlinear constraint and the joint acceleration constraint of the third nonlinear relationship", includes:
Substituting the joint acceleration constraint condition into the second nonlinear constraint condition, and determining a sixth parameter in the second nonlinear constraint condition.
Wherein the second nonlinear constraint is:
Wherein,G2 denotes the sixth parameter, which is,
The joint acceleration constraint conditions are as follows:
Wherein,Indicating the acceleration of the joint,Representing the minimum value of the joint acceleration,Represents the maximum value of the joint acceleration.
Specifically, the sixth parameter g2 is determined by the following equation set:
It can be seen that the sixth parameter
By solving for the sixth parameterAnd a data foundation is laid for determining target constraint conditions of the target function according to the sixth parameter.
S105, determining a standard path position corresponding to the preset path point of the target robot, determining a path speed range of the preset path point according to joint speed constraint conditions of the standard path position and the second nonlinear relation, and determining target constraint conditions 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 path positions of the generated preset path points are standard path positions corresponding to the preset path points. The objective function is a function for solving an optimal path speed 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 condition 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. Further, a target constraint of the target function is formed 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 speed range of the preset path point according to the joint speed constraint condition of the standard path position and the second nonlinear relationship", includes:
And merging 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, joint speed constraint conditions are:
Wherein,Indicating the velocity of the joint and,Representing the minimum value of the joint velocity,Representing the maximum value of the joint velocity.
Specifically, according to the second nonlinear relationship: In a second nonlinear relationshipIncorporated into joint velocity constraintsObtaining the joint velocity relation:
moving "q'(s)" in the joint velocity relationship to both sides of the inequality, yields the velocity of the pathPath speed range of (c):
by determining the path speed range of the preset path point, a data foundation is laid for determining the target constraint condition according to the path speed range.
Optionally, in S105, "determining the 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", 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, 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, and determining the target constraint condition of the target function.
Wherein, the objective function is:
Wherein,To-be-optimized parameters of the objective function, u represents the path accelerationX represents the path speedSquare value of (2)H represents a zero matrix and z is [2Δs, -1]. The Δs represents a distance value between a current preset path point and an adjacent preset path point, for example, assuming that an optimal path speed of the target robot at an ith preset path point is calculated, the ith preset path point is taken as the current preset path point, and an (i+1) th preset path point adjacent to the ith preset path point is taken as the adjacent preset path point, so that the Δ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:
Wherein lbA denotes a lower-limit parameter, ubA denotes an upper-limit parameter, lbA and ubA are each constructed based on the fifth parameter and the sixth parameter,Representing a constraint matrix, based on the first parameter, the second parameter, the third parameter and the fourth parameter,
Lb represents the parameters to be optimized of the objective functionIs represented by the lower limit value of ub, and represents the parameter to be optimized of the objective functionUpper limit value of (2).U_low and u_up represent a range lower limit value and a range upper limit value of a path acceleration range set in advance, respectively.
And (3) assuming that the optimal path speed of the target robot at the ith preset path point is calculated, determining a standard path position si of the ith preset path point, determining a target polynomial coefficient associated with the ith preset path point according to the association relation between the preset path point and the polynomial coefficient of the third nonlinear polynomial q (), and setting the target polynomial coefficient as the polynomial coefficient of the third nonlinear polynomial q ().
Substituting the standard path position si into the 'f (q (s))' of lbA and ubA, and calculating to obtain specific values of lbA and ubA. Substituting the standard path position si intoIs calculated from "q '(s)", "q"(s) "," a (q (s)) q '(s) ", and" a (q (s)) q '(s) +q '(s)T B (q (s)) q '(s) "Specific values of (2). Substituting the standard path position si into the path speedThe lower limit value x_low of the path speed range and the 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, determining a constraint matrix of the target constraint condition according to a first parameter, a second parameter, a third parameter and a fourth parameter, substituting a standard path position into the lower limit parameter, the upper limit parameter, the constraint matrix and a path speed range, determining a target constraint condition of a 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.
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 corresponding planning joint position of the target robot at the track planning moment according to the optimal path speed, the standard path position and the track planning moment.
The track planning time represents the time when the joint position of the target robot needs to be predicted, in other words, when the joint positions corresponding to the target robot at all track planning times are predicted, the track planning of the joint constraint track of the target robot is completed, and the movement of the target robot can be controlled based on the joint constraint track planning result.
In one embodiment, a preset optimization algorithm is used to determine an optimal path speed of the target robot at each preset path point based on the objective function and the objective constraint condition of the objective function. Optionally, firstly, according to the sequence of each preset path point, carrying out optimization solution from front to back to determine the path accelerationAnd the square value of the path velocityAnd then carrying out optimization solution 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 range.
According to the standard path positions of the preset path points, the interval distance value of the candidate interval paths among the preset path points is determined, and then according to the interval distance value and the optimal path speed of the preset path points, the candidate interval time required by the target robot to complete the running of the candidate interval paths is determined. And 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 track planning time, further determining a planning path speed corresponding to the target robot at the track planning time according to the starting time of the target standard time period and the track planning time, determining a planning path position corresponding to the target robot at the track planning time according to the planning path speed, and finally determining a planning joint position corresponding to the target robot at the track planning time according to the planning 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 joint acceleration and path speed and path acceleration; determining a first parameter associated with the path speed and a second parameter associated with the path acceleration in the fourth nonlinear relation, and determining a third parameter associated with the path speed and a fourth parameter associated with the path acceleration in the third nonlinear relation, determining a fifth parameter in the first nonlinear constraint according to joint moment constraints of the first nonlinear constraint and the fourth nonlinear relation, and determining a sixth parameter in the second nonlinear constraint according to joint acceleration constraints of the second nonlinear constraint and the third nonlinear constraint, determining a standard path position corresponding to a target robot at a preset path point, and determining a path speed range of the preset path point according to joint speed constraints of the standard path position and the second nonlinear relation, and optimizing a function of the target robot at the target path point 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, the sixth parameter and the third constraint, and the optimal path speed, and optimizing a function of the target robot at the target path point according to the optimal path position, the method comprises the steps of determining a planned joint position corresponding to a target robot at a track planning moment according to a standard path position and the track planning moment, and carrying out joint constraint track planning on the target robot by adding a joint moment constraint condition, a joint acceleration constraint condition and a joint speed constraint condition, so that the track planning in Cartesian space can also meet joint constraint, the problems of joint overload or low joint performance utilization of the target robot are avoided, and the accuracy of the joint constraint track planning of the robot is improved.
Example two
Fig. 2 is a flowchart of a joint constraint trajectory planning method of a robot according to a second embodiment of the present invention, where the "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 of the present invention may be further optimized and expanded, and may be combined with the foregoing various optional embodiments. As shown in fig. 2, the method includes:
S201, determining the interval distance value of the candidate interval path between the preset path points according to the standard path positions of the preset path points, and determining the candidate interval time required by the target robot to complete the candidate interval path according to the interval distance value and the optimal path speed of the preset path points.
In one embodiment, the distance value of the candidate distance path between adjacent preset path points is calculated according to the standard path positions of the adjacent preset path points. Namely, the interval distance value of the candidate interval path between each preset path point is determined according to the following formula, namely Deltasi＝si+1-si, wherein i is more than or equal to 1, and si+1 and si are adjacent preset path points.
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 speeds 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: I.e.Wherein,Represents the optimal path speed for the i-th preset path point,Representing the optimal path speed of the (i+1) th preset path point, Δsi represents the distance value of the candidate interval path between the (i) th preset path point and the (i+1) th preset path point, and Δti represents 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 when the target robot runs to each candidate interval path, and determining the target standard time period to which the track planning time belongs from the candidate standard time periods.
In one embodiment, from the first candidate interval time, the candidate interval times are superimposed, and a 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 moment as the 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 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 the 2 nd candidate interval path, and the i < th > preset path point and the i+1 < th > preset path point are the i < th > candidate interval path.
Assuming that the candidate interval time required for the target robot to travel to complete the 1 st candidate interval path is Δt1, the candidate interval time required for the target robot to travel to complete the 2 nd candidate interval path is Δt2, and the candidate interval time required for the target robot to travel to complete the i-th candidate interval path is Δti.
The candidate standard period for the target robot to travel to the 1 st candidate interval path is 0 to Δt1, the candidate standard period for the 2 nd candidate interval path is Δt1～(Δt1+Δt2), the candidate standard period for the 3 rd candidate interval path is (Δt1+Δt2)～(Δt1+Δt2+Δt3), and the candidate standard period for the travel completion of the i-th candidate interval path is
Assuming that the trajectory planning time is current_time, which belongs to the candidate standard period (Δt1+Δt2)～(Δt1+Δt2+Δt3), then (Δt1+Δt2)～(Δt1+Δt2+Δt3) is taken as the target standard period.
S203, determining a target interval time corresponding to the target standard time period from the candidate interval time, and determining a target interval path corresponding to the target standard time period from the candidate interval paths.
In one embodiment, the target interval time is determined based on a difference between a termination time and a start time of the target standard time period. For example, assuming that the target standard period is (Δt1+Δt2)～(Δt1+Δt2+Δt3), a difference Δt3 between the ending time (Δt1+Δt2+Δt3) and the starting time (Δt1+Δt2) is taken 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 relation between the candidate interval time and the candidate interval path. For example, the candidate interval time required for the target robot to travel to complete the 1 st candidate interval path is Δt1, the candidate interval time required for the target robot to travel to complete the 2 nd candidate interval path is Δt2, the candidate interval time required for the target robot to travel to complete the i-th candidate interval path is Δti, and the target interval time associated therewith is the 3 rd candidate interval path assuming that the target interval time is Δt3.
S204, determining the corresponding planned joint position of the target robot at the track planning moment according to the target interval time, the target interval path, the target standard time period and the track planning moment.
The method comprises the steps of determining the interval distance value of a candidate interval path between preset path points according to the standard path positions of the preset path points, determining the candidate interval time required by the target robot to complete the candidate interval path according to the interval distance value and the optimal path speed of the preset path points, superposing the candidate interval time, determining the candidate standard time period from which the target robot is driven to each candidate interval path, determining the target standard time period to which the track planning moment belongs from the candidate standard time period, determining the target interval time corresponding to the target standard time period from the candidate interval time, and determining the target interval path corresponding to the target standard time period from the candidate interval path, and determining the planning joint position corresponding to the target robot at the track planning moment according to the target interval time, the target interval path, the target standard time period and the track planning moment.
Optionally, S204 includes the following steps A, B and C:
A. And 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 start optimal path speed and the end optimal path speed is determined, and an average path acceleration of the target path interval is determined based on a ratio between the speed difference and the target path interval.
Illustratively, assume that the starting optimal path speed for the starting path point isThe termination optimal path speed of the termination path point isIf the target interval time is Deltatn, the average path acceleration of the target interval path is
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 planning 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, according to the track planning time and the starting time of the target standard time period, determining a time difference value, determining a product result between the time difference value and the average path acceleration, and further determining the planning path speed corresponding to the target robot at the track planning time according to the product result and the starting optimal path speed of the starting path point.
Exemplary, assuming that the track planning time is current_time, the starting time of the target standard time period is tn, and the average path acceleration isThe initial optimal path speed of the initial path point isThe planned path speed of the target robot corresponding to the track planning moment(currenttime-tn)。
C. 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 a planned joint position according to the planned path position.
In one embodiment, an average path speed of the planned path speed and the initial optimal path speed is determined, and a planned path position corresponding to the target robot at the track planning time is determined according to a product result of the average path speed and the time difference. Exemplary, assume a time difference of currenttime-tn and a planned path speed ofThe initial optimal path speed isThen the path location is planned
And determining polynomial coefficients in a first nonlinear relation according to a target standard time period, substituting the planned path position into the first nonlinear relation, and determining the planned joint position.
The method comprises the steps of determining an initial optimal path speed of an initial path point and a final optimal path speed of a final path point in a target interval path, determining 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 a track planning time and an initial time of a target standard time period, determining a planning path speed corresponding to the target robot at the track planning time according to the time difference value and the average path acceleration, determining a planning path position corresponding to the target robot at the track planning time according to the planning path speed, the initial optimal path speed and the time difference value, and determining a planning joint position according to the planning path position, so that the effect of joint constraint track planning of the target robot is achieved.
Optionally, in step C, "determining the planned joint position according to the planned path position" includes:
According to the target standard time period and the association relation between the candidate standard time period and the candidate polynomial coefficient, determining the target polynomial coefficient associated with the target standard time period, taking the target polynomial coefficient as the polynomial coefficient in the first nonlinear relation to obtain the optimal nonlinear relation, substituting the planned path position into the optimal nonlinear relation, and determining the planned joint position.
For the third nonlinear polynomial q (), in the first nonlinear relation q=q(s), an association relation between the candidate polynomial coefficients is established in advance for each candidate standard time period, that is, the candidate polynomial coefficients associated with the candidate standard time period can be directly determined according to any candidate standard time period.
In one embodiment, the target standard period is matched with the candidate standard period, and the candidate polynomial coefficient associated with the candidate standard period matched with the target standard period is used as the target polynomial coefficient. And setting coefficients in a third-order nonlinear polynomial q () in the first nonlinear relation q=q(s) as target polynomial coefficients to obtain an optimal nonlinear relation, substituting the planned path position of the target robot at the track planning moment into the optimal nonlinear relation, and determining the planned joint position of the target robot at the track planning moment.
For example, assuming that the determined target polynomial coefficients are a1, b1, c1, d1, the coefficient a in q=as3+bs2 +cs+d is set to a1, the coefficient b is set to b1, the coefficient c is set to c1, and the coefficient d is set to d1, resulting in the optimal nonlinear relationship q=a1s3+b1s2 +c1s+d1. Assuming that the planned path position corresponding to the target robot at the track planning moment is scur, the planned joint position corresponding to the target robot at the track planning moment
According to the target standard time period and the association relation between the candidate standard time period and the candidate polynomial coefficient, the target polynomial coefficient associated with the target standard time period is determined, the target polynomial coefficient is used as the polynomial coefficient in the first nonlinear relation to obtain the optimal nonlinear relation, the planned path position is substituted into the optimal nonlinear relation to determine the planned joint position, the effect of dynamically determining the polynomial coefficient in the first nonlinear relation according to the difference of the target standard time period 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 device 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 speed and the path speed, and a third nonlinear relationship between the joint acceleration and the path speed and the path acceleration of the target robot;
a nonlinear relationship determination second module 32, configured to determine a fourth nonlinear relationship between the joint moment and the path speed and the path acceleration of the target robot according to the first nonlinear relationship, the second nonlinear relationship, the third nonlinear relationship, and the robot dynamics equation;
A first parameter determining module 33, configured to determine a first parameter associated with the path speed and a second parameter associated with the path acceleration in a fourth nonlinear relationship, and determine a third parameter associated with the path speed and a fourth parameter associated with the path acceleration in a third nonlinear relationship;
A parameter determination second 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;
a constraint condition determining module 35, 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 joint speed constraint condition of the standard path position and 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;
The planned joint position determining module 36 is configured to optimize a path speed of the target robot at a preset path point according to the target constraint condition and the objective function, determine an 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 track planning time according to the optimal path speed, the standard path position, and the track planning time.
Optionally, the first nonlinear relationship is:
q=q(s);
the second nonlinear relationship is:
the third nonlinear relationship is:
Where s represents the path position, q represents the joint position,The path velocity is indicated as such,Indicating the acceleration of the joint,Representing path acceleration, q () is a cubic nonlinear polynomial.
Optionally, the nonlinear relationship determines the second module 32, specifically for:
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:
The robot dynamics equation is:
where A () represents the inertial matrix, B () represents the coriolis force and centripetal force coefficient matrix, f () represents the gravitational moment, and τ represents the joint moment.
Optionally, the parameter determining first module 33 is specifically configured to:
taking the following parameters in the fourth nonlinear relation as the first parameters:
A(q(s))q′′(s)+q′(s)TB(q(s))q′(s);
taking the following parameters in the fourth nonlinear relation as second parameters:
A(q(s))q′(s)。
optionally, the parameter determining first module 33 is specifically further configured to:
Taking the following parameters in the third nonlinear relation 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, and determining a fifth parameter in the first nonlinear constraint condition;
wherein, joint moment constraint conditions are:
τmin≤τ≤τmax;
Where τ represents the joint moment, τmin represents the minimum of the joint moment, τmax represents the maximum of the joint moment;
The first nonlinear constraint is:
Fτ≤g1;
Wherein,G1 denotes the fifth parameter, which is,
Optionally, the parameter determining second module 34 is specifically further configured to:
substituting the joint acceleration constraint condition into a second nonlinear constraint condition, and determining a sixth parameter in the second nonlinear constraint condition;
wherein, joint acceleration constraint conditions are:
Wherein,Indicating the acceleration of the joint,Representing the minimum value of the joint acceleration,A maximum value representing the joint acceleration;
the second nonlinear constraint is:
Wherein,G2 denotes the sixth parameter, which is,
Optionally, the constraint condition determining module 35 is specifically configured to:
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 a preset path point;
Wherein, joint speed constraint conditions are:
Wherein,Indicating the velocity of the joint and,Representing the minimum value of the joint velocity,A maximum value representing the joint velocity;
The joint velocity relationship is:
the path speed range is:
Optionally, the constraint condition determining module 35 is specifically further 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 a lower limit parameter, an upper limit parameter, a constraint matrix and a path speed range to determine a target constraint condition of a target function;
Wherein, the objective function is:
Where u represents the path accelerationX represents the path speedSquare value of (2)H represents a zero matrix, z is [2 delta s-1 ], and delta s represents a distance value between a current preset path point and an adjacent preset path point;
The target constraint conditions are:
Wherein lbA denotes a lower-limit parameter,UbA denotes the upper-limit parameter of the system,U_low and u_up represent the range lower limit value and the range upper limit value of the path acceleration range, respectively, x_low and x_up represent the range lower limit value and the range upper limit value of the path speed range,Representing the constraint matrix(s),
Optionally, the planned joint position determination module 36 is specifically configured to:
Determining the interval distance value of the candidate interval path between the preset path points according to the standard path positions of the preset path points, and determining the candidate interval time required by the target robot to complete the candidate interval path according to the interval distance value and the optimal path speed of the preset path points;
Superposing the candidate interval time, determining candidate standard time periods when the target robot runs to each candidate interval path, and determining a target standard time period to which the track planning moment belongs from the candidate standard time periods;
Determining a target interval time corresponding to the target standard time period from the candidate interval time, and determining a target interval path corresponding to the target standard time period from the candidate interval paths;
And determining the corresponding planning joint position of the target robot at the track planning moment according to the target interval time, the target interval path, the target standard time period and the track planning moment.
Optionally, the planned joint position determination module 36 is specifically further configured to:
determining an initial optimal path speed of an initial path point and a termination optimal path speed of a termination path point in the target interval path, and determining an average path acceleration of the target interval path according to the initial optimal path speed, the termination 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 the planning path speed of 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 a planned joint position according to the planned path position.
Optionally, the planned joint position determination module 36 is specifically further configured to:
According to the target standard time period and the association relation between the candidate standard time period and the candidate polynomial coefficient, determining a target polynomial coefficient associated with the target standard time period, and taking the target polynomial coefficient as the polynomial coefficient in the first nonlinear relation to obtain an optimal nonlinear relation;
Substituting the planned path position into the optimal nonlinear relation, and determining 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 the corresponding functional modules and beneficial effects of the execution method.
Example 4
Fig. 4 shows a schematic 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. Electronic equipment may also represent various forms of mobile devices, such as personal digital processing, cellular telephones, smartphones, 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, etc., in which 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 the storage unit 48 into the Random Access Memory (RAM) 43. In the RAM 43, various programs and data required for the operation of the electronic device 40 may 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.
Various components in the electronic device 40 are connected to the I/O interface 45, including an input unit 46, such as a keyboard, mouse, etc., an output unit 47, such as various types of displays, speakers, etc., a storage unit 48, such as a magnetic disk, optical disk, etc., 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.
The processor 41 may be various 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, digital Signal Processors (DSPs), and any suitable processor, controller, microcontroller, etc. The processor 41 performs the various methods and processes described above, such as the joint constraint trajectory planning method of the robot.
In some embodiments, the joint constraint trajectory planning method of the robot may be implemented as a computer program, which is tangibly embodied in a computer-readable storage medium, such as the 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 joint constraint trajectory planning method of the robot described above may be performed. Alternatively, in other embodiments, the processor 41 may be configured to perform the joint constraint trajectory planning method of the robot in any other suitable way (e.g., by means of firmware).
Various implementations of the systems and techniques described here above may be implemented in digital electronic circuitry, integrated circuit systems, field Programmable Gate Arrays (FPGAs), application Specific Integrated Circuits (ASICs), application Specific Standard Products (ASSPs), systems On Chip (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and/or combinations thereof. These various embodiments may include being 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 a special or general purpose programmable processor, operable to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.
A computer program for carrying out methods of the present invention may 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 implemented. The computer program may execute entirely on the machine, partly on the machine, as a stand-alone software package, partly on the machine and partly on a remote machine or entirely on the 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. The 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 portable 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 the user and a keyboard and a pointing device (e.g., a mouse or a trackball) by which the 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 may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback), and input from the user may be received in any form, including acoustic input, speech input, or tactile input.
The systems and techniques described here can be implemented in a computing system that includes a background 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 background, 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 a Local Area Network (LAN), a Wide Area Network (WAN), a blockchain network, and the Internet.
The computing system may include clients and servers. The client and server are typically 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 hosts and VPS service are overcome.
It should be appreciated that various forms of the flows shown above may be used to reorder, add, or delete steps. For example, the steps described in the present invention may be performed in parallel, sequentially, or in a different order, so long as the desired results of the technical solution of the present invention are achieved, and the present invention is not limited herein.
The above embodiments do not limit the scope of the present invention. It will be apparent to those skilled in the art that various modifications, combinations, sub-combinations and alternatives are possible, depending on design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of the present invention.