CN113618741A - Robot joint pose optimization method, robot control method and robot - Google Patents

Robot joint pose optimization method, robot control method and robot Download PDF

Info

Publication number
CN113618741A
CN113618741A CN202110976981.3A CN202110976981A CN113618741A CN 113618741 A CN113618741 A CN 113618741A CN 202110976981 A CN202110976981 A CN 202110976981A CN 113618741 A CN113618741 A CN 113618741A
Authority
CN
China
Prior art keywords
joint
robot
optimization
angular velocity
optimization function
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110976981.3A
Other languages
Chinese (zh)
Other versions
CN113618741B (en
Inventor
刘益彰
熊友军
罗璇
张志豪
陈春玉
葛利刚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Youbixuan Intelligent Robot Co ltd
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN202110976981.3A priority Critical patent/CN113618741B/en
Publication of CN113618741A publication Critical patent/CN113618741A/en
Priority to PCT/CN2021/131673 priority patent/WO2023024278A1/en
Application granted granted Critical
Publication of CN113618741B publication Critical patent/CN113618741B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

Abstract

The embodiment of the application provides a robot joint pose optimization method, a robot control method and a robot, wherein the method comprises the following steps: constructing a first optimization function taking the joint angular velocity as an optimization variable and the robot tail end velocity as a control target, wherein the first optimization function comprises inequality constraints met by the joint angular velocity and equality constraints containing relaxation variables; constructing a second optimization function taking the joint angular speed as an optimization variable and the joint angle far away from the joint limit as a control target, wherein the second optimization function comprises constraint conditions which are met by the joint angular speed and contain the same relaxation variable; and solving the first optimization function to obtain a solution of a slack variable, and solving the second optimization function by using the solution of the slack variable to obtain the optimal joint angular velocity, so as to obtain the optimal joint angle to optimize the joint pose of the robot. The scheme can realize optimization solution of the joint pose so as to ensure that each joint angle always has larger movement margin and the like on the premise of ensuring the prior completion of the speed task.

Description

Robot joint pose optimization method, robot control method and robot
Technical Field
The application relates to the technical field of robot control, in particular to a robot joint pose optimization method, a robot control method and a robot.
Background
The robot generally has multiple degrees of freedom, various tasks are completed through coordination of the multiple degrees of freedom, redundant joints are added to many robots in order to increase the motion range and flexibility of the robot, but the processing of the redundant joints generally requires very complex logic processing and classification calculation, which brings many challenges to the control of the robot. For example, in the motion planning of a mechanical arm or a multi-legged robot with redundant degrees of freedom, some methods such as newton's method are used to calculate the inverse kinematics solution in the motion planning, which not only requires a long calculation time but also requires a high calculation difficulty, resulting in a long total time for the motion planning, and a low planning efficiency.
Disclosure of Invention
The embodiment of the application provides a robot joint pose optimization method, a robot control method and a robot, wherein joint redundancy is processed by adopting a double-layer optimization idea, a better joint angle can be obtained through solving, so that a speed task of a robot joint in a Cartesian space is preferentially completed, and each joint angle has a larger movement margin.
The embodiment of the application provides a robot joint pose optimization method, which comprises the following steps:
constructing a first optimization function taking the joint angular velocity as an optimization variable and the robot tail end velocity as a control target, wherein the first optimization function comprises inequality constraints satisfied by the joint angular velocity and equality constraints containing relaxation variables;
constructing a second optimization function taking the joint angular speed as an optimization variable and the joint angle of the robot far away from the joint limit as a control target, wherein the second optimization function comprises a constraint condition which is satisfied by the joint angular speed and contains the same relaxation variable;
solving according to the first optimization function to obtain a solution of the slack variable, and obtaining an optimal joint angular velocity according to the solution of the slack variable and the second optimization function;
and obtaining the optimal joint angle of the robot according to the optimal joint angular velocity, and optimizing the pose of the robot joint according to the optimal joint angle of the robot.
In some embodiments, the inequality constraint satisfied by the joint angular velocity is obtained by:
constructing joint angle constraints which are met by each joint of the robot in a corresponding control period, and converting the joint angle constraints into first constraint conditions with joint angular velocity as a variable;
constructing joint angular acceleration constraints which are met by each joint in the corresponding control period, and converting the joint angular acceleration constraints into second constraint conditions with joint angular velocities as variables;
and constructing a synthetic constraint of the joint angular velocity as the inequality constraint based on the first constraint condition, the second constraint condition and the self limit of the joint angular velocity.
In some embodiments, the first constraint is:
Figure BDA0003227895810000021
wherein the content of the first and second substances,
Figure BDA0003227895810000022
and
Figure BDA0003227895810000023
the upper limit and the lower limit of the ith joint angle are respectively;
Figure BDA0003227895810000024
is the ith joint angle at time t;
Figure BDA0003227895810000025
is the ithJoint angular velocity of the joint; and T is the control instruction cycle of the robot.
In some embodiments, the second constraint is:
Figure BDA0003227895810000031
wherein the content of the first and second substances,
Figure BDA0003227895810000032
and
Figure BDA0003227895810000033
the upper limit and the lower limit of the ith joint angular acceleration are respectively;
Figure BDA0003227895810000034
and
Figure BDA0003227895810000035
the ith joint angular velocity at time t-1 and time t, respectively.
In some embodiments, the first optimization function takes the square of the relaxation variable as an optimization index;
the expression of the first optimization function is:
Figure BDA0003227895810000036
wherein w is a relaxation variable, J is a velocity Jacobian matrix of the robot,
Figure BDA0003227895810000037
a joint angular velocity vector for all joints of the robot;
Figure BDA0003227895810000038
is the terminal velocity vector of the robot;
Figure BDA0003227895810000039
and
Figure BDA00032278958100000310
respectively, the upper limit and the lower limit of the ith joint angular velocity.
In some embodiments, the second optimization function takes the Euclidean distance between the joint angle and the joint limit as an optimization index;
the calculation formula of the Euclidean distance is as follows:
Figure BDA00032278958100000311
wherein the content of the first and second substances,
Figure BDA00032278958100000312
wherein q is the Euclidean distance;
Figure BDA00032278958100000313
and
Figure BDA00032278958100000314
the upper limit and the lower limit of the ith joint angle are respectively;
Figure BDA00032278958100000315
is the ith joint angle at time t.
In some embodiments, the constraints of the second optimization function with the same slack variables are the same as the equality constraints of the first optimization function, the second optimization function further comprising the inequality constraints satisfied by the joint angular velocities;
the expression of the second optimization function is:
Figure BDA0003227895810000041
wherein the content of the first and second substances,
Figure BDA0003227895810000042
Figure BDA0003227895810000043
is the ith joint angle at the time t-1; t is the control instruction cycle of the robot; w is the relaxation variable, J is the velocity Jacobian matrix of the robot,
Figure BDA0003227895810000044
a joint angular velocity vector for all joints of the robot;
Figure BDA0003227895810000045
is the terminal velocity vector of the robot;
Figure BDA0003227895810000046
and
Figure BDA0003227895810000047
the upper limit and the lower limit of the ith joint angle are respectively;
Figure BDA0003227895810000048
and
Figure BDA0003227895810000049
the upper limit and the lower limit of the ith joint angular velocity are respectively;
Figure BDA00032278958100000410
and
Figure BDA00032278958100000411
the upper limit and the lower limit of the ith joint angular acceleration are respectively;
Figure BDA00032278958100000412
and
Figure BDA00032278958100000413
the ith joint angular velocity at time t-1 and time t, respectively.
An embodiment of the present application further provides a robot control method, including:
acquiring state parameters related to joint angular velocity of the robot in a current control instruction period;
optimizing the joint pose of the next control instruction cycle by using the robot joint pose optimization method according to the state parameters;
and controlling the robot to execute corresponding operation according to the optimized joint pose.
Embodiments of the present application also provide a robot including a processor and a memory, the memory storing a computer program, the processor being configured to execute the computer program to implement the robot joint pose optimization method or the robot control method described above.
Embodiments of the present application also provide a readable storage medium storing a computer program that, when executed by a processor, implements the robot joint pose optimization method or the robot control method described above.
The embodiment of the application has the following beneficial effects:
the robot joint pose optimization method provided by the embodiment of the application adopts a double-layer optimization idea to process joint redundancy, the same relaxation variables are introduced into the two optimization functions and corresponding constraint conditions are added by constructing a first optimization function taking joint angular velocity as an optimization variable and robot tail end velocity as a control target and a second optimization function taking joint angle far away from joint limit as a control target, the two optimization functions are combined to carry out joint angle optimal solution, the solved solution can be guaranteed to preferentially complete Cartesian space tasks, and enough allowance is left for each joint angle on the basis, so that joint limit is far away. In addition, the joint pose optimization method is suitable for joint angle optimization of a rotary joint robot, is also suitable for joint position optimization of a translational joint and a translational-rotary hybrid robot, and has strong universality.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained from the drawings without inventive effort.
Fig. 1 is a first flowchart of a robot joint pose optimization method according to an embodiment of the present disclosure;
FIG. 2 is a second flowchart of the robot joint pose optimization method according to the embodiment of the present application;
FIG. 3 is a schematic flow chart illustrating a robot control method according to an embodiment of the present disclosure;
fig. 4 is a schematic structural diagram of a robot joint pose optimization apparatus according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments.
The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, presented in the accompanying drawings, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present application without making any creative effort, shall fall within the protection scope of the present application.
The quadratic programming problem is mainly a process of selecting an optimal solution from a plurality of solutions under the condition of satisfying equality and inequality constraints. Exemplarily, the main form of the quadratic programming problem is as follows:
Figure BDA0003227895810000061
wherein the content of the first and second substances,
Figure BDA0003227895810000062
for optimization index, H is a Hessian matrix; x is an n-dimensional optimization variable; f is a row vector; a. theeq·x＝beqFor equality constraint, AeqIs a matrix of mxn (m is less than or equal to m) dimensions, beqIs a row vector of m; and b is more than or equal to Ax is inequality constraint, A is an n-column matrix, and b is a column vector.
For robots, the range of motion and flexibility of the robot, etc. are often increased by adding redundant joints. The existing processing of redundant joints is generally very complex and long in calculation time, and certain limitations exist in practical use. In contrast, in consideration of the characteristics of the self structure of the robot, the robot often has multiple limitations such as joint angles, joint angular velocities, joint moments and the like in different application occasions, in order to obtain an optimal inverse kinematics solution which can be used for executing corresponding tasks and meets the multiple corresponding limitations, the embodiment of the application performs optimal solution by using a quadratic programming problem, and specifically performs optimal solution of the joint angles by constructing a double-layer optimization index (i.e., a double-layer quadratic optimization problem).
According to the robot joint pose optimization method, the first optimization function taking the joint angular speed as the optimization variable and the robot tail end speed as the control target and the second optimization function taking the joint angular speed as the optimization variable and the joint angle far away from the joint limit as the control target are established, the same relaxation variable is introduced into the two optimization functions, corresponding constraint conditions are added, the joint angle optimal solution is carried out by combining the two optimization functions, the solved solution can be guaranteed to preferentially complete the task of a Cartesian space, each joint is far away from the joint limit as far as possible on the basis, and the purpose of joint pose optimization is achieved. The method for optimizing the pose of the robot joint will be described with reference to the following embodiments.
Example 1
Fig. 1 is a first flowchart of a robot joint pose optimization method according to an embodiment of the present application.
Exemplarily, the robot joint pose optimization method comprises the following steps:
and step S110, constructing a first optimization function taking the joint angular velocity as an optimization variable and the robot tail end velocity as a control target, wherein the first optimization function comprises inequality constraints satisfied by the joint angular velocity and equality constraints containing relaxation variables.
In order to ensure that the tip of the robot can preferentially execute the task in the cartesian space, a quadratic programming problem, namely the first optimization function, is constructed by taking the joint angular velocity as a control variable and the tip velocity as a control target. It will be appreciated that the "first" of the first optimization functions is primarily used to distinguish from other optimization functions mentioned later.
Exemplarily, the first optimization function introduces a slack variable, and obtains the index to be optimized based on the slack variable. For example, in one embodiment, the optimization indicator may be the square of the slack variable, or the like, but may also take other forms related to the slack variable, and is not limited herein.
Meanwhile, the first optimization function is added with corresponding constraint conditions which mainly comprise equality constraint which is satisfied by the joint angular velocity and contains a relaxation variable and inequality constraint which is directly or indirectly related to the joint angular velocity, so that the solved optimal solution of the joint angular velocity can enable the velocity index of the tail end to be reached under each joint constraint condition.
Because the velocity Jacobian matrix of the robot can reflect the relationship between the angular velocity of each joint and the velocity of the tail end, under the condition of introducing a relaxation variable, an equality constraint equation which is satisfied among the angular velocity of the joint, the relaxation variable and the velocity of the tail end is constructed.
The inequality constraints for the first optimization function may include, but are not limited to, joint angle constraints, joint angular velocity physical constraints, joint angular acceleration constraints, and the like, for example.
Exemplarily, as shown in fig. 2, the inequality constraint satisfied by the joint angular velocity can be obtained by the following steps including:
and step S210, constructing joint angle constraints which are met by each joint of the robot in a corresponding control cycle, and converting the joint angle constraints into first constraint conditions with joint angular velocity as a variable.
Because the joint structure of the robot is limited, the positions or angles of some joints cannot be reached, namely joint limit exists, joint limit constraints which are met by the angles of all the joints of the robot are constructed firstly, and then corresponding joint angular velocity constraints are obtained through conversion.
It should be understood that, for a translational joint type robot, since there is no rotation, the joint limit mentioned above mainly refers to the limit of the joint position; for a robot with a rotary joint, the joint limit is mainly the limit of the joint angle.
Taking a robot of a rotary joint type as an example, it is assumed that the upper limit of the joint angle is
Figure BDA0003227895810000081
The lower limit is
Figure BDA0003227895810000082
The control command cycle of the robot is T,
Figure BDA0003227895810000083
for the ith joint angle at time t,
Figure BDA0003227895810000084
the joint angular velocity of the ith joint, the joint angle at the time t +1
Figure BDA0003227895810000085
Comprises the following steps:
Figure BDA0003227895810000086
correspondingly, according to the joint angle constraint, there are:
Figure BDA0003227895810000091
then, the transformed first constraint equation with the joint angular velocity as a variable can be obtained as:
Figure BDA0003227895810000092
it is understood that, for the translational joint robot, the joint angles and the joint angular velocities of the corresponding joints described above can be adaptively replaced by the translational position and the translational velocity, and the principle is similar, so the description is not given. The same applies to a robot with a translation-rotation mixed motion.
And S220, constructing joint angular acceleration constraints which are met by each joint in the corresponding control period, and converting the joint angular acceleration constraints into second constraint conditions with the joint angular velocity as a variable.
Since the output torque of a single joint of the robot is limited, the angular acceleration of the joint is correspondingly limited, and similarly, the constraint met by the angular acceleration of each joint of the robot is constructed in the embodiment, and then the constraint is converted to obtain the corresponding joint angular velocity constraint.
Taking the example of a rotary joint type robot, the upper limit of the angular acceleration of the joint is assumed to be
Figure BDA0003227895810000093
The lower limit is
Figure BDA0003227895810000094
the ith joint angular velocity at time t is
Figure BDA0003227895810000095
The joint angular velocity at time t +1
Figure BDA0003227895810000096
Comprises the following steps:
Figure BDA0003227895810000097
correspondingly, according to the joint angular acceleration constraint, there are:
Figure BDA0003227895810000098
then, the transformed second constraint equation with the joint angular velocity as a variable can be obtained as:
Figure BDA0003227895810000099
wherein the content of the first and second substances,
Figure BDA00032278958100000910
is the ith joint angular velocity at time t-1.
Step S230, constructing a composite constraint that obtains the joint angular velocity as the inequality constraint based on the first constraint, the second constraint, and the self-constraint of the joint angular velocity.
The robot also has the constraint of joint angular velocity, and the upper limit of the ith joint angular velocity is assumed to be
Figure BDA0003227895810000101
The lower limit is
Figure BDA0003227895810000102
The self-constraint on the angular velocity of the joint can then be expressed as:
Figure BDA0003227895810000103
because the angular velocity of each joint at the same time t needs to satisfy multiple restrictions at the same time, for the convenience of solution, the constraint synthesis of the angular velocity of the joint is obtained by combining the added constraint equation and the self-restriction. Exemplarily, taking three constraints of joint angle, joint angular velocity, and joint angular acceleration as examples, there are:
Figure BDA0003227895810000104
thus, the composite constraint is described by an inequality constraint, having:
Figure BDA0003227895810000105
it is understood that the synthetic inequality constraint of the joint angular velocity obtained by using the joint angle (or joint position) constraint, the joint angular velocity constraint and the joint angular acceleration constraint is only an example of adding inequality, and in practical application, more constraint conditions can be added, and the constraint conditions are not limited herein.
For the first optimization function, if the square of the slack variable is used as the optimization index and corresponding equality constraint equation and inequality constraint are constructed, the first optimization function can be written as follows:
Figure BDA0003227895810000106
by optimally solving the optimization function, the terminal velocity of the robot can be controlled to be within the limits of joints, the self-restraint of the angular velocity of the joints and the restraint of the angular acceleration of the joints
Figure BDA0003227895810000107
About the angular velocity of the joint
Figure BDA0003227895810000111
And the solution of the relaxation variable w.
It can be understood that by using the relaxation variable as the optimization index, even if the first optimization function has conflict in the constraint conditions, the approximate solution of the constraint equation can still be solved, and the first optimization function can be used for the next processing; otherwise, if the above constraints themselves conflict, such as the equality constraint is not solved, the solution of the optimization scheme of the whole quadratic programming will be wrong.
And step S120, constructing a second optimization function taking the joint angular speed as an optimization variable and the limit of the joint angle of the robot far away from the joint as a control target, wherein the second optimization function comprises the constraint condition which is satisfied by the joint angular speed and contains the same relaxation variable.
To solve the problem of joint redundancy, the present embodiment will construct another quadratic programming problem, i.e. the second optimization function described above, with the goal that each joint angle is far away from the joint limit. For example, in one embodiment, the second optimization function uses the euclidean distance between the joint angle and the joint limit as the optimization index, but it is needless to say that other calculation formulas capable of reflecting the distance between the joint angle and the joint limit may be used instead of the euclidean distance, and the present invention is not limited thereto.
Exemplarily, the calculation formula of the euclidean distance is:
Figure BDA0003227895810000112
wherein the content of the first and second substances,
Figure BDA0003227895810000113
wherein q is the Euclidean distance;
Figure BDA0003227895810000114
and
Figure BDA0003227895810000115
the upper limit and the lower limit of the ith joint angle are respectively;
Figure BDA0003227895810000116
is the ith joint angle at time t.
In this embodiment, in order to make the finally solved joint angular velocity gradually approach to the optimal solution on the premise of ensuring the cartesian space control terminal velocity, the joint angular velocity of the second optimization function also needs to add corresponding constraint conditions.
For example, in one embodiment, the joint angular velocity of the second optimization function may add an equality constraint containing the same slack variable, and an inequality constraint related to the joint angular velocity, based on the constraints of the first optimization function described above.
For the second optimization function, taking the farthest distance between each joint angle and the joint limit as an optimization index as an example, and adding an equality constraint equation and an inequality constraint which are the same as those of the first optimization function, the second optimization function can be written as follows:
Figure BDA0003227895810000121
wherein the content of the first and second substances,
Figure BDA0003227895810000122
Figure BDA0003227895810000123
is the ith joint angle at time t-1.
It can be understood that when the constraint condition setting of the second optimization function is the same as that of the first optimization function, the solution obtained by the solution of the first optimization function can meet the requirement of the terminal speed under the condition of meeting the joint limit, the joint angular speed self-constraint and the joint angular acceleration constraint, namely, the speed task is preferentially executed, and meanwhile, the maximum movement margin of each joint angle can be ensured, so that the purpose of optimizing the joint pose is achieved.
And S130, solving according to the first optimization function to obtain a solution of the slack variable, and obtaining the optimal joint angular velocity according to the solution of the slack variable and the second optimization function.
Exemplarily, based on a first optimization function and a second optimization function, an open-source solver can be used for firstly carrying out optimal solution on the first optimization function so as to obtain a solution of the joint angular velocity and the relaxation variable; and then, the solved solution of the relaxation variable is used for carrying out optimal solution on the second optimization function, and the optimal joint angular velocity can be obtained at the moment, namely the required joint angular velocity.
And S140, obtaining the optimal joint angle of the robot according to the optimal joint angular velocity, and optimizing the pose of the robot joint according to the optimal joint angle of the robot.
Exemplarily, because the joint angular velocity and the joint angle of the robot have an integral relation, after the optimal joint angular velocity is solved, the optimal joint angle can be obtained through integral operation. Furthermore, the pose (such as the angle or the position) of the robot joint can be adjusted according to the optimal joint angle so as to ensure that the robot can execute corresponding task operation.
The robot joint pose optimization method of the embodiment processes the problem of joint redundancy through a double-layer optimization idea, by constructing a first optimization function with joint angular velocity as an optimization variable and robot tip velocity as a control target and introducing a relaxation variable, and a second optimization function taking the joint angle away from the joint limit as a control target and introducing the same relaxation variable, and adding the synthetic constraint conditions of the angle, the angular velocity, the angular acceleration and the like of each joint to the two optimization functions, wherein the second optimization function is further solved for joint angular velocity within the joint angle constraint range satisfied by the first optimization function, therefore, the speed task of the Cartesian space can be preferentially completed under various joint constraint conditions, and on the basis, enough allowance is left for each joint angle, namely, a larger movement allowance is provided. In addition, the method has universality and can be used for automatically optimizing joint positions or joint angles of various robots such as rotary joints, translational-rotational hybrid motion and the like.
Example 2
Referring to fig. 3, based on the method in embodiment 1, this embodiment provides a robot control method, for example, the robot may be a rotary joint type robot, a translational joint type robot, a hybrid translational-rotary motion robot, and the like, which is not limited herein. In the application, the robot with the rotary joint type refers to a robot which drives a robot joint connected with a steering engine to realize the motion of a corresponding rotary angle through the steering engine outputting a rotary angle. The translational joint type robot is characterized in that the robot drives a robot joint connected with a steering engine to realize the motion of a corresponding translational angle through the steering engine outputting linear displacement.
In this embodiment, the robot has redundant joints, and various tasks can be completed by coordinating these redundant joints. Exemplarily, as shown in fig. 3, the robot control method includes:
step S310, acquiring state parameters related to the joint angular velocity of the robot in the current control instruction cycle.
And step S320, optimizing the joint pose of the next control instruction cycle by using a robot joint pose optimization method according to the state parameters.
Exemplarily, which state parameters need to be acquired specifically can be determined according to the constructed first optimization function and the second optimization function, and the state parameters may be directly acquired by a sensor or the like, or may be acquired by performing operation on acquired data, which is not limited herein.
For example, taking the first optimization function as an example, the joint angle, the desired end velocity, the jacobian matrix, and the like corresponding to the control command cycle may be acquired, and then known quantities may be substituted into the optimization function to solve for the optimization variable of the joint angular velocity and the slack variable, and the second optimization function may be similar to the above.
It is to be understood that, as to the robot joint pose optimization method adopted in the present embodiment, specific reference is made to the description of embodiment 1 above, and the description is not repeated here.
And step S330, controlling the robot to execute corresponding operation according to the optimized joint pose.
Exemplarily, after the joint angle or the joint position required by the next control instruction cycle of the robot is solved, the joint angle or the joint position is sent to the corresponding joint motor as an instruction, so that the robot performs a corresponding operation, for example, an obstacle avoidance task, a human-computer interaction task, and the like of the robot.
The robot control method of the embodiment automatically optimizes the joint pose by using the robot joint pose optimization method, so that the robot joint position always has the maximum motion margin and the like on the premise of ensuring the Cartesian space pose.
Example 3
Referring to fig. 4, based on the method of embodiment 1, this embodiment proposes a robot joint pose optimization apparatus 100, exemplarily, the robot joint pose optimization apparatus 100 includes:
the building module 110 is configured to build a first optimization function with the joint angular velocity as an optimization variable and the robot tip velocity as a control target, where the first optimization function includes an inequality constraint satisfied by the joint angular velocity and an equality constraint including a slack variable.
The building module 110 is further configured to build a second optimization function with the joint angular velocity as an optimization variable and the joint angle of the robot away from the joint limit as a control target, where the second optimization function includes a constraint condition that the joint angular velocity satisfies and contains the same slack variable.
And the solving module 120 is configured to solve according to the first optimization function to obtain a solution of the slack variable, and obtain an optimal joint angular velocity according to the solution of the slack variable and the second optimization function.
The solving module 120 is further configured to obtain an optimal joint angle of the robot according to the optimal joint angular velocity.
And the optimizing module 130 is used for optimizing the pose of the robot joint according to the optimal joint angle of the robot.
It is to be understood that the apparatus of the present embodiment corresponds to the method of embodiment 1 described above, and the alternatives of embodiment 1 described above are equally applicable to the present embodiment, and therefore, the description thereof will not be repeated.
The present application also provides a robot exemplarily including a processor and a memory, wherein the memory stores a computer program, and the processor causes the robot to execute the functions of each of the modules in the above-described robot joint pose optimization method or the above-described robot joint pose optimization apparatus by executing the computer program.
The present application also provides a readable storage medium for storing the computer program used in the robot.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative and, for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, each functional module or unit in each embodiment of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only for the specific embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present application, and shall be covered by the scope of the present application.

Claims (10)

1. A robot joint pose optimization method is characterized by comprising the following steps:
constructing a first optimization function taking the joint angular velocity as an optimization variable and the robot tail end velocity as a control target, wherein the first optimization function comprises inequality constraints satisfied by the joint angular velocity and equality constraints containing relaxation variables;
constructing a second optimization function taking the joint angular speed as an optimization variable and the joint angle of the robot far away from the joint limit as a control target, wherein the second optimization function comprises a constraint condition which is satisfied by the joint angular speed and contains the same relaxation variable;
solving according to the first optimization function to obtain a solution of the slack variable, and obtaining an optimal joint angular velocity according to the solution of the slack variable and the second optimization function;
and obtaining the optimal joint angle of the robot according to the optimal joint angular velocity, and optimizing the pose of the robot joint according to the optimal joint angle of the robot.
2. The robot joint pose optimization method according to claim 1, wherein the inequality constraints that the joint angular velocities satisfy are obtained by steps including:
constructing joint angle constraints which are met by each joint of the robot in a corresponding control period, and converting the joint angle constraints into first constraint conditions with joint angular velocity as a variable;
constructing joint angular acceleration constraints which are met by each joint in the corresponding control period, and converting the joint angular acceleration constraints into second constraint conditions with joint angular velocities as variables;
and constructing a synthetic constraint of the joint angular velocity as the inequality constraint based on the first constraint condition, the second constraint condition and the self limit of the joint angular velocity.
3. The robot joint pose optimization method according to claim 2, wherein the first constraint condition is:
Figure FDA0003227895800000021
wherein the content of the first and second substances,
Figure FDA0003227895800000025
and
Figure FDA0003227895800000026
the upper limit and the lower limit of the ith joint angle are respectively;
Figure FDA0003227895800000027
is the ith joint angle at time t;
Figure FDA0003227895800000028
the joint angular velocity of the ith joint; and T is the control instruction cycle of the robot.
4. The robot joint pose optimization method according to claim 3, wherein the second constraint condition is:
Figure FDA0003227895800000022
wherein the content of the first and second substances,
Figure FDA0003227895800000029
and
Figure FDA00032278958000000210
the upper limit and the lower limit of the ith joint angular acceleration are respectively;
Figure FDA00032278958000000211
and
Figure FDA00032278958000000212
the ith joint angular velocity at time t-1 and time t, respectively.
5. The robot joint pose optimization method according to claim 4, wherein the first optimization function takes a square of a relaxation variable as an optimization index;
the expression of the first optimization function is:
Figure FDA0003227895800000023
wherein w is a relaxation variable, J is a velocity Jacobian matrix of the robot,
Figure FDA00032278958000000213
a joint angular velocity vector for all joints of the robot;
Figure FDA00032278958000000214
is the terminal velocity vector of the robot;
Figure FDA00032278958000000215
and
Figure FDA00032278958000000216
respectively, the upper limit and the lower limit of the ith joint angular velocity.
6. The robot joint pose optimization method according to claim 1, wherein the second optimization function takes a Euclidean distance between a joint angle and a joint limit as an optimization index;
the calculation formula of the Euclidean distance is as follows:
Figure FDA0003227895800000024
Figure FDA0003227895800000031
wherein q is the Euclidean distance;
Figure FDA0003227895800000033
and
Figure FDA0003227895800000034
the upper limit and the lower limit of the ith joint angle are respectively;
Figure FDA0003227895800000035
is the ith joint angle at time t.
7. The robot joint pose optimization method according to claim 6, wherein the constraint condition containing the same relaxation variable of the second optimization function is the same as the equality constraint of the first optimization function, the second optimization function further including the inequality constraint that the joint angular velocity satisfies;
the expression of the second optimization function is:
Figure FDA0003227895800000032
wherein the content of the first and second substances,
Figure FDA0003227895800000036
Figure FDA0003227895800000037
is the ith joint angle at the time t-1; t is the control instruction cycle of the robot; w is the relaxation variable, J is the velocity Jacobian matrix of the robot,
Figure FDA0003227895800000038
a joint angular velocity vector for all joints of the robot;
Figure FDA0003227895800000039
is the terminal velocity vector of the robot;
Figure FDA00032278958000000310
and
Figure FDA00032278958000000311
the upper limit and the lower limit of the ith joint angle are respectively;
Figure FDA00032278958000000312
and
Figure FDA00032278958000000313
the upper limit and the lower limit of the ith joint angular velocity are respectively;
Figure FDA00032278958000000314
and
Figure FDA00032278958000000315
the upper limit and the lower limit of the ith joint angular acceleration are respectively;
Figure FDA00032278958000000316
and
Figure FDA00032278958000000317
the ith joint angular velocity at time t-1 and time t, respectively.
8. A robot control method, comprising:
acquiring state parameters related to joint angular velocity of the robot in a current control instruction period;
optimizing joint poses of a next control instruction cycle according to the state parameters using the robot joint pose optimization method according to any one of claims 1 to 7;
and controlling the robot to execute corresponding operation according to the optimized joint pose.
9. A robot characterized by comprising a processor and a memory, the memory storing a computer program for executing the computer program to implement the robot joint pose optimization method of any one of claims 1-7 or the robot control method of claim 8.
10. A readable storage medium characterized by storing a computer program which, when executed by a processor, implements the robot joint pose optimization method according to any one of claims 1 to 7 or the robot control method according to claim 8.
CN202110976981.3A 2021-08-24 2021-08-24 Robot joint pose optimization method, robot control method and robot Active CN113618741B (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110976981.3A CN113618741B (en) 2021-08-24 2021-08-24 Robot joint pose optimization method, robot control method and robot
PCT/CN2021/131673 WO2023024278A1 (en) 2021-08-24 2021-11-19 Robot joint pose optimization method, robot control method, and robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110976981.3A CN113618741B (en) 2021-08-24 2021-08-24 Robot joint pose optimization method, robot control method and robot

Publications (2)

Publication Number Publication Date
CN113618741A true CN113618741A (en) 2021-11-09
CN113618741B CN113618741B (en) 2022-07-29

Family

ID=78387594

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110976981.3A Active CN113618741B (en) 2021-08-24 2021-08-24 Robot joint pose optimization method, robot control method and robot

Country Status (2)

Country Link
CN (1) CN113618741B (en)
WO (1) WO2023024278A1 (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114227687A (en) * 2021-12-28 2022-03-25 深圳市优必选科技股份有限公司 Robot control method and device, terminal equipment and storage medium
WO2023024278A1 (en) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 Robot joint pose optimization method, robot control method, and robot

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040254679A1 (en) * 2003-04-10 2004-12-16 Kenichiro Nagasaka Robot movement control system
CN110103225A (en) * 2019-06-04 2019-08-09 兰州大学 A kind of the mechanical arm repeating motion control method and device of data-driven
CN110861088A (en) * 2019-11-29 2020-03-06 沈阳通用机器人技术股份有限公司 Motion optimization method of redundant degree of freedom robot
CN111037560A (en) * 2019-12-25 2020-04-21 广东省智能制造研究所 Cooperative robot compliance control method and system
CN111300414A (en) * 2020-03-06 2020-06-19 陕西理工大学 Dual-criterion redundant mechanical arm self-movement planning method
CN111538949A (en) * 2020-07-10 2020-08-14 深圳市优必选科技股份有限公司 Redundant robot inverse kinematics solving method and device and redundant robot
CN111975768A (en) * 2020-07-08 2020-11-24 华南理工大学 Mechanical arm motion planning method based on fixed parameter neural network
CN113070881A (en) * 2021-04-02 2021-07-06 深圳市优必选科技股份有限公司 Robot motion control method and device and robot
CN113084791A (en) * 2019-12-23 2021-07-09 深圳市优必选科技股份有限公司 Mechanical arm control method, mechanical arm control device and terminal equipment

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5034235B2 (en) * 2006-01-16 2012-09-26 ソニー株式会社 Control system, control method, and computer program
JP2011183527A (en) * 2010-03-10 2011-09-22 Toyota Motor Corp Joint target value determining method of redundant robot and control device of redundant robot
CN106625666B (en) * 2016-12-16 2019-03-01 广州视源电子科技股份有限公司 The control method and device of redundant mechanical arm
CN106584461B (en) * 2016-12-21 2019-02-15 西安科技大学 The inverse kinematics personification arm configuration optimization method of seven degrees of freedom copy man mechanical arm under multi-constraint condition
CN107234617B (en) * 2017-07-10 2020-04-28 北京邮电大学 Obstacle avoidance path planning method guided by artificial potential field irrelevant to obstacle avoidance task
JP7237447B2 (en) * 2017-09-29 2023-03-13 キヤノン株式会社 Information processing method, program, recording medium, information processing device, robot system, and article manufacturing method
CN110561441B (en) * 2019-10-23 2022-09-27 中山大学 Single 94LVI iterative algorithm for pose control of redundant manipulator
CN112873208A (en) * 2021-01-29 2021-06-01 佛山树客智能机器人科技有限公司 Anti-noise and dynamic constraint robot real-time motion planning method and device
CN113618741B (en) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 Robot joint pose optimization method, robot control method and robot

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040254679A1 (en) * 2003-04-10 2004-12-16 Kenichiro Nagasaka Robot movement control system
CN110103225A (en) * 2019-06-04 2019-08-09 兰州大学 A kind of the mechanical arm repeating motion control method and device of data-driven
CN110861088A (en) * 2019-11-29 2020-03-06 沈阳通用机器人技术股份有限公司 Motion optimization method of redundant degree of freedom robot
CN113084791A (en) * 2019-12-23 2021-07-09 深圳市优必选科技股份有限公司 Mechanical arm control method, mechanical arm control device and terminal equipment
CN111037560A (en) * 2019-12-25 2020-04-21 广东省智能制造研究所 Cooperative robot compliance control method and system
CN111300414A (en) * 2020-03-06 2020-06-19 陕西理工大学 Dual-criterion redundant mechanical arm self-movement planning method
CN111975768A (en) * 2020-07-08 2020-11-24 华南理工大学 Mechanical arm motion planning method based on fixed parameter neural network
CN111538949A (en) * 2020-07-10 2020-08-14 深圳市优必选科技股份有限公司 Redundant robot inverse kinematics solving method and device and redundant robot
CN113070881A (en) * 2021-04-02 2021-07-06 深圳市优必选科技股份有限公司 Robot motion control method and device and robot

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023024278A1 (en) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 Robot joint pose optimization method, robot control method, and robot
CN114227687A (en) * 2021-12-28 2022-03-25 深圳市优必选科技股份有限公司 Robot control method and device, terminal equipment and storage medium
CN114227687B (en) * 2021-12-28 2023-08-15 深圳市优必选科技股份有限公司 Robot control method and device, terminal equipment and storage medium

Also Published As

Publication number Publication date
WO2023024278A1 (en) 2023-03-02
CN113618741B (en) 2022-07-29

Similar Documents

Publication Publication Date Title
CN111538949B (en) Redundant robot inverse kinematics solving method and device and redundant robot
CN113618741B (en) Robot joint pose optimization method, robot control method and robot
CN113618739B (en) Robot dynamic obstacle avoidance method and device and robot
CN108319138A (en) A kind of sliding formwork of drive lacking unmanned boat-contragradience double loop Trajectory Tracking Control method
CN110757455B (en) Speed planning method
CN112775976A (en) Task execution control method and device, control equipment and readable storage medium
CN114466730B (en) Motion planning for optimizing speed of a robot while maintaining limits on acceleration and jerk
CN113618742A (en) Robot obstacle avoidance method and device and robot
An et al. Task planning and collaboration of jellyfish-inspired multiple spherical underwater robots
Xue et al. Proximal policy optimization with reciprocal velocity obstacle based collision avoidance path planning for multi-unmanned surface vehicles
CN111913489A (en) Underwater robot multi-propeller coupling control method and system
Kumar et al. Real-time optimal motion planning for autonomous underwater vehicles
CN114274147B (en) Target tracking control method and device, mechanical arm control equipment and storage medium
Farid et al. Computationally efficient algorithm to generate a waypoints-based trajectory for a quadrotor UAV
US11007639B2 (en) Joint control method for serial robot and serial robot using the same
CN112356032A (en) Posture smooth transition method and system
CN112276952A (en) Robust simultaneous stabilization method and system for multi-robot system
WO2022179213A1 (en) Decoupling control method and apparatus for humanoid robot, and humanoid robot
Ding et al. Research on static fault-tolerant control method of thruster based on MPC
Wehbe et al. Novel three-dimensional optimal path planning method for vehicles with constrained pitch and yaw
Arrizabalaga et al. Spatial motion planning with pythagorean hodograph curves
CN114326769B (en) Robot motion correction method and device, robot control equipment and storage medium
CN110543181B (en) Under-actuated angle decoupling attitude control method and system
Shang et al. Finite-time tracking for UUV with system uncertainty using fast adaptive nonsingular terminal high-order sliding mode control
CN117407630B (en) Inverse kinematics analysis and solution system for solving shoulder and elbow angles through cooperative mechanical arm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20231127

Address after: Room 601, 6th Floor, Building 13, No. 3 Jinghai Fifth Road, Beijing Economic and Technological Development Zone (Tongzhou), Daxing District, Beijing, 100176

Patentee after: Beijing Youbixuan Intelligent Robot Co.,Ltd.

Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province

Patentee before: Shenzhen Youbixuan Technology Co.,Ltd.

TR01 Transfer of patent right