WO2023024278A1

WO2023024278A1 - Robot joint pose optimization method, robot control method, and robot

Info

Publication number: WO2023024278A1
Application number: PCT/CN2021/131673
Authority: WO
Inventors: 刘益彰; 熊友军; 罗璇; 张志豪; 陈春玉; 葛利刚
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-08-24
Filing date: 2021-11-19
Publication date: 2023-03-02
Also published as: CN113618741A; CN113618741B

Abstract

Disclosed are a robot joint pose optimization method, a robot control method, and a robot. The method comprises: constructing a first optimization function using joint angular velocity as an optimization variable and robot terminal velocity as a control target, the first optimization function comprising an inequality constraint satisfied by the joint angular velocity and an equality constraint including a slack variable; constructing a second optimization function using joint angular velocity as the optimization variable and a joint angle away from a joint limit as a control target, the second optimization function comprising constraint conditions having the same slack variable satisfied by the joint angular velocity; solving the first optimization function to obtain a solution for the slack variable, and using the solution of the slack variable in the second optimization function to obtain an optimal joint angular velocity, thereby solving to obtain an optimal joint angle for optimizing the joint pose of the robot. The present method can achieve an optimal solution for a joint pose, causing each joint angle to consistently have a larger activity margin while still ensuring the priority of speed of task completion.

Description

Robot joint pose optimization method, robot control method and robot

Cross References to Related Applications

This application claims the priority of the Chinese patent application with the application number 2021109769813 and titled "robot joint pose optimization method, robot control method and robot" filed with the China Patent Office on August 24, 2021, the entire contents of which are incorporated by reference in this application.

technical field

The present 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 technique

Robots generally have multiple degrees of freedom, and through the coordination of multiple degrees of freedom to complete various tasks, in order to increase the range of motion and flexibility of the robot, many robots have added redundant joints, but the processing of redundant joints generally requires Very complex logic processing and classification calculations, which bring many challenges to the control of the robot. For example, in the motion planning of robotic arms or multi-legged robots with redundant degrees of freedom, some methods such as Newton's method are used to calculate the kinematic inverse solution in motion planning, which not only takes a long time to calculate, but also is difficult to calculate. , which leads to a longer total time for motion planning and lower planning efficiency.

application content

The embodiment of the present application provides a robot joint pose optimization method, a robot control method, and a robot. The optimization method uses the idea of double-layer optimization to deal with joint redundancy, and can obtain better joint angles to ensure that the robot can be completed first. The speed task of the joints in the Cartesian space also makes each joint angle have a large range of motion.

Embodiments of the present application provide a method for optimizing robot joint poses, including:

Constructing a first optimization function with the joint angular velocity as the optimization variable and the robot terminal velocity as the control target, the first optimization function includes the inequality constraint and the equality constraint including the slack variable that the joint angular velocity satisfies;

Constructing a second optimization function with the joint angular velocity as the optimization variable, and the joint angle of the robot being away from the joint limit as the control target, the second optimization function includes the constraint condition that the joint angular velocity satisfies and contains the same slack variable;

Solving according to the first optimization function to obtain the 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;

An optimal joint angle of the robot is obtained according to the optimal joint angular velocity, and a pose of the robot joint is optimized according to the optimal joint angle of the robot.

In some embodiments, the inequality constraint satisfied by the joint angular velocity is obtained through the following steps, including:

Constructing joint angle constraints that each joint of the robot satisfies in a corresponding control cycle, and converting the joint angle constraints into a first constraint condition with joint angular velocity as a variable;

Constructing joint angular acceleration constraints that each joint satisfies in the corresponding control period, and converting the joint angular acceleration constraints into a second constraint condition with joint angular velocity as a variable;

Based on the first constraint condition, the second constraint condition, and the self-limit of the joint angular velocity, a synthetic constraint to obtain the joint angular velocity is constructed as the inequality constraint.

In some embodiments, the first constraint condition is:

in,

and

are the upper and lower limits of the i-th joint angle, respectively;

is the i-th joint angle at time t;

is the joint angular velocity of the i-th joint; T is the control command cycle of the robot.

In some embodiments, the second constraint is:

in,

and

are the upper limit and lower limit of the i-th joint angular acceleration, respectively;

and

are the i-th joint angular velocity at time t-1 and time t, respectively.

In some embodiments, the first optimization function takes the square of the slack variable as the optimization index;

The expression of the first optimization function is:

Wherein, w is the slack variable, J is the speed Jacobian matrix of the robot,

is the joint angular velocity vector of all joints of the robot;

is the terminal velocity vector of the robot;

and

are the upper and lower limits of the i-th joint angular velocity, respectively.

In some embodiments, the second optimization function uses the Euclidean distance between the joint angle and the joint limit as the optimization index;

The formula for calculating the Euclidean distance is:

in,

Wherein, q is described Euclidean distance;

and

are the upper and lower limits of the i-th joint angle, respectively;

is the i-th joint angle at time t.

In some embodiments, the constraints containing the same slack variable of the second optimization function are the same as the equality constraints of the first optimization function, and the second optimization function further includes that the joint angular velocity satisfies The inequality constraints of ;

The expression of the second optimization function is:

in,

is the i-th joint angle at time t-1; T is the control instruction period of the robot; w is the relaxation variable, J is the speed Jacobian matrix of the robot,

is the joint angular velocity vector of all joints of the robot;

is the terminal velocity vector of the robot;

and

are the upper and lower limits of the i-th joint angle, respectively;

and

are the upper and lower limits of the i-th joint angular velocity, respectively;

and

are the i-th joint angular velocity at time t-1 and time t, respectively.

Embodiments of the present application also provide a robot control method, including:

Obtain the state parameters related to the joint angular velocity of the robot in the current control command cycle;

Utilizing the above-mentioned robot joint pose optimization method according to the state parameters to optimize the joint pose of the next control command cycle;

The robot is controlled to perform corresponding operations according to the optimized joint poses.

An embodiment of the present application also provides a robot, the robot includes a processor and a memory, the memory stores a computer program, and the processor is used to execute the computer program to implement the above-mentioned robot joint pose optimization method or robot Control Method.

Embodiments of the present application also provide a readable storage medium, which stores a computer program, and when the computer program is executed by a processor, implements the above-mentioned robot joint pose optimization method or robot control method.

Embodiments of the application have the following beneficial effects:

The robot joint pose optimization method of the embodiment of the present application adopts the idea of double-layer optimization to deal with joint redundancy, by constructing the first optimization function with the joint angular velocity as the optimization variable, the robot terminal speed as the control target, and the joint angle away from the joint The limit is the second optimization function of the control target. The same slack variable is introduced into the two optimization functions and corresponding constraints are added, and the joint angle is optimally solved by combining the two optimization functions, which can ensure that the obtained solution can Prioritize the completion of the Cartesian space task, and on this basis, leave enough margin for each joint angle, so as to stay away from the joint limit. In addition, the joint pose optimization method is not only suitable for joint angle optimization of rotational joint robots, but also suitable for joint position optimization of translational joints and translation-rotation hybrid robots, and has strong universality.

Description of drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the accompanying drawings used in the embodiments. It should be understood that the following drawings only show some embodiments of the present application, so It should be regarded as a limitation on the scope, and those skilled in the art can also obtain other related drawings based on these drawings without creative work.

FIG. 1 is a schematic diagram of the first flow chart of the robot joint pose optimization method of the embodiment of the present application;

FIG. 2 is a schematic diagram of the second flow chart of the robot joint pose optimization method of the embodiment of the present application;

FIG. 3 is a schematic flow diagram of a robot control method in an embodiment of the present application;

FIG. 4 is a schematic structural diagram of a robot joint pose optimization device according to an embodiment of the present application.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all of them.

The components of the embodiments of the application generally described and illustrated in the figures herein may be arranged and designed in a variety of different configurations. Accordingly, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative efforts belong to the scope of protection of the present application.

The quadratic programming problem is mainly the process of selecting the optimal solution from multiple solutions under the constraints of equality and inequality. Exemplarily, the main form of the quadratic programming problem is as follows:

in,

is an optimization index, H is a Hessian matrix; x is an n-dimensional optimization variable; f is a row vector; A _eq x=b _eq is an equality constraint, A _eq is an mxn (m≤n) dimensional matrix, and b _eq is m Row and column vector; Ax≤b is an inequality constraint, A is a matrix with n columns, and b is a column vector.

For robots, the range of motion and flexibility of the robot are often increased by adding redundant joints. The existing processing of redundant joints is generally very complicated and takes a long time to calculate, and there are certain limitations in practical use. In this regard, considering the characteristics of the robot's own structure, it often has various constraints such as joint angles, joint angular velocities, and joint torques in different application scenarios. Optimal kinematics inverse solution, the embodiment of the present application will use the quadratic programming problem to optimize the solution, specifically, the joint angle is optimized and solved by constructing a double-layer optimization index (ie, a double-layer quadratic optimization problem).

The robot joint pose optimization method of the embodiment of the present application constructs the first optimization function with the joint angular velocity as the optimization variable and the robot end speed as the control target, and the joint angular velocity as the optimization variable and the joint angle away from the joint limit as the control target The second optimization function, and introduce the same slack variable to these two optimization functions and add corresponding constraints. By combining these two optimization functions to solve the joint angle optimally, it can be guaranteed that the solution obtained can be completed in Cartesian space first. The task, and on this basis, make each joint as far away as possible from the joint limit, so as to achieve the purpose of joint pose optimization. The method for optimizing the joint pose of the robot will be described below with reference to the embodiments.

Example 1

FIG. 1 is a schematic diagram of a first flowchart of a method for optimizing a joint pose of a robot according to an embodiment of the present application.

Exemplarily, the robot joint pose optimization method includes:

Step S110, constructing a first optimization function with the joint angular velocity as the optimization variable and the robot terminal velocity as the control objective, the first optimization function includes the inequality constraints that the joint angular velocity satisfies and the equality constraints including slack variables.

In order to ensure that the end of the robot can perform tasks in Cartesian space first, a quadratic programming problem is constructed here with the joint angular velocity as the control variable and the end velocity as the control target, which is the above-mentioned first optimization function. It can be understood that the "first" in the first optimization function is mainly used to distinguish it from other optimization functions mentioned later.

Exemplarily, the first optimization function will introduce a slack variable, and obtain the index to be optimized based on the slack variable. For example, in an implementation manner, the optimization index may be the square of the slack variable, etc., of course, may also take other forms related to the slack variable, which are not limited here.

At the same time, the first optimization function will also be added with corresponding constraints, which mainly include equality constraints with slack variables that the joint angular velocity satisfies, and inequality constraints that are directly or indirectly related to the joint angular velocity, so as to ensure that the obtained joint angular velocity The optimal solution can achieve the speed index of the terminal under the constraints of each joint.

Since the velocity Jacobian matrix of the robot can reflect the relationship between the angular velocity of each joint and the terminal velocity, in the case of introducing the slack variable, the equality constraint equation satisfied by the joint angular velocity, the slack variable and the terminal velocity will be constructed here.

The inequality constraints of the first optimization function may include, but are not limited to, joint angle constraints, physical constraints on joint angular velocity itself, and joint angular acceleration constraints, for example.

Exemplarily, as shown in FIG. 2, the inequality constraints satisfied by the joint angular velocity can be obtained through the following steps, including:

Step S210, constructing joint angle constraints that each joint of the robot satisfies in a corresponding control period, and converting the joint angle constraints into a first constraint condition with joint angular velocity as a variable.

Due to the limitation of the joint structure of the robot, some joint positions or angles are unreachable, that is, there are joint limits. In this embodiment, the joint limit constraints satisfied by each joint angle of the robot are first constructed, and then converted to obtain the corresponding joint angular velocity constraints.

It should be understood that for robots with translational joints, since there is no rotation, the above-mentioned joint limit mainly refers to the limit of the joint position; and for robots with rotary joints, the above-mentioned joint limit mainly refers to Limits on joint angles.

Taking the robot of rotary joint type as an example, suppose the upper limit of the joint angle is

The lower limit is

The control command period of the robot is T,

is the i-th joint angle at time t,

is the joint angular velocity of the i-th joint, then the joint angle at time t+1

for:

Correspondingly, according to the joint angle constraints, there are:

Therefore, the converted first constraint equation with the joint angular velocity as a variable can be obtained as:

It can be understood that for translational joint robots, the joint angles and joint angular velocities of the above-mentioned corresponding joints can be adaptively replaced by translational positions and translational velocities. The principle is similar, so no further description is given. The same applies to robots with translational-rotational mixed motion.

Step S220, construct the joint angular acceleration constraints that each joint satisfies in the corresponding control period, and convert the joint angular acceleration constraints into a second constraint condition 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 will be limited accordingly. Similarly, this embodiment will first construct the constraints that the angular acceleration of each joint of the robot satisfies, and then convert to obtain the corresponding joint angular velocity constraint.

Still taking the robot of rotary joint type as an example, suppose the upper limit of joint angular acceleration is

The lower limit is

The i-th joint angular velocity at time t is

Then the joint angular velocity at time t+1

for:

Correspondingly, according to the joint angular acceleration constraints, there are:

Therefore, the converted second constraint equation with the joint angular velocity as a variable can be obtained as:

in,

is the angular velocity of the ith joint at time t-1.

Step S230, based on the first constraint condition, the second constraint condition and the self-constraint of the joint angular velocity, construct a composite constraint to obtain the joint angular velocity as the above-mentioned inequality constraint.

The robot also has joint angular velocity constraints, assuming that the upper limit of the i-th joint angular velocity is

The lower limit is

Then, the self-constraint of joint angular velocity can be expressed as:

Since the angular velocity of each joint at the same time t needs to satisfy multiple constraints at the same time, for the convenience of solution, the above-mentioned added constraint equation and its own constraints are combined to obtain the constraint synthesis of the joint angular velocity. Exemplarily, taking the addition of three constraints of joint angle, joint angular velocity and joint angular acceleration as an example, there are:

Then, the composite constraint is described by an inequality constraint, then:

It can be understood that using the above-mentioned joint angle (or joint position) constraint, joint angular velocity constraint and joint angular acceleration constraint to obtain the composite inequality constraint of joint angular velocity is only an example of adding inequality, and in actual application, you can also add more There are many constraints, which are not limited here.

For the first optimization function, if the square of the slack variable is used as the optimization index, and the corresponding equality constraint equation and inequality constraint are constructed, the first optimization function can be written as follows:

By optimally solving the optimization function, it can be obtained that under the constraints of joint limit, joint angular velocity and joint angular acceleration, the end velocity of the robot is

of the joint angular velocity

And the solution of the slack variable w.

It can be understood that taking the slack variable as the optimization index can ensure that even if the first optimization function has conflicting constraints, it will still find a close solution to the constraint equation, which can be used in the next step; otherwise, if the above constraints Conflicts in the conditions themselves, such as no solution to the equality constraints, will lead to errors in the solution of the entire quadratic programming optimization scheme.

Step S120, constructing a second optimization function with the joint angular velocity as the optimization variable and the robot's joint angle away from the joint limit as the control objective, the second optimization function includes the constraint conditions with the same slack variable that the joint angular velocity satisfies.

In order to solve the problem of joint redundancy, in this embodiment, another quadratic programming problem, that is, the above-mentioned second optimization function, will be constructed with the goal that each joint angle is 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. Of course, in addition to the Euclidean distance, other calculation formulas that can reflect the joint angle and the joint limit can also be used , is not limited here.

Exemplarily, the formula for calculating the Euclidean distance is:

in,

Among them, q is the Euclidean distance;

and

are the upper and lower limits of the i-th joint angle, respectively;

is the i-th joint angle at time t.

In this embodiment, a double-layer quadratic programming problem is used to comprehensively solve an optimal joint angular velocity. In this embodiment, in order to make the joint angular velocity finally solved under the premise of ensuring the end velocity of the Cartesian space control, further make each joint If the angle gradually approaches the optimal solution, the joint angular velocity of the second optimization function also needs to add corresponding constraints.

For example, in one implementation, based on the constraint conditions of the above-mentioned first optimization function, the joint angular velocity of the second optimization function may add an equality constraint including the same slack variable and an inequality constraint related to the joint angular velocity.

For the second optimization function, if the optimization index of each joint angle is farthest from the joint limit as an example, and the same equality constraint equation and inequality constraint as the first optimization function are added, the second optimization function can be written as In the following form:

in,

is the i-th joint angle at time t-1.

It can be understood that when the constraint conditions of the second optimization function are set to be the same as those of the first optimization function, it can be guaranteed that the solution obtained by the first optimization function can satisfy the constraints of the joint limit, joint angular velocity and joint angular acceleration The demand for terminal speed is to realize the priority execution of speed tasks, and at the same time, it can also ensure that each joint angle has the maximum activity margin, so as to achieve the purpose of joint pose optimization.

In step S130, the solution of the slack variable is obtained by solving according to the first optimization function, and the optimal joint angular velocity is obtained according to the solution of the slack variable and the second optimization function.

Exemplarily, based on the first optimization function and the second optimization function, an open-source solver can be used to optimally solve the first optimization function to obtain the joint angular velocity and the solution of the slack variable; then, use the solved slack variable The solution of the second optimization function is optimally solved, and the optimal joint angular velocity can be obtained at this time, which is the required joint angular velocity.

In step S140, the optimal joint angle of the robot is obtained according to the optimal joint angular velocity, and the pose of the robot joint is optimized according to the optimal joint angle of the robot.

Exemplarily, since there is an integral relationship between the joint angular velocity and the joint angle of the robot, after the optimal joint angular velocity is obtained, the optimal joint angle can be obtained through an integral operation. Furthermore, the posture (such as angle or position) of the robot joints can be adjusted according to the optimal joint angles, so as to ensure that the robot can perform corresponding task operations.

The joint pose optimization method of the robot in this embodiment uses the idea of double-layer optimization to deal with the problem of joint redundancy, by constructing the first optimization function that takes the joint angular velocity as the optimization variable, the robot end speed as the control target and introduces the slack variable, and The second optimization function that takes the joint angle away from the joint limit as the control target and introduces the same slack variable, and adds synthetic constraints such as the angle, angular velocity, and angular acceleration of each joint to these two optimization functions, where the second optimization The function is further solved for joint angular velocity within the range of joint angle constraints satisfied by the first optimization function, so it can be realized that under various joint constraint conditions, the velocity task in Cartesian space can be completed first, and on this basis, each joint angle can be given Sufficient margin is left, that is, it has a large activity margin. In addition, the method is versatile and can be used to automatically optimize the joint positions or joint angles of various types of robots such as rotational joints, translational joints, and translational-rotational hybrid motions.

Example 2

Please refer to Fig. 3. Based on the method of the above-mentioned embodiment 1, this embodiment proposes a robot control method. -Robots and the like in mixed motion are not limited here. In this application, the rotating joint type robot means that the robot drives the robot joints connected to the steering gear through the steering gear outputting the rotation angle to realize the movement corresponding to the rotation angle. The robot of the translational joint type means that the robot drives the robot joints connected to the steering gear through the steering gear outputting the linear displacement to realize the movement corresponding to the translation angle.

In this embodiment, the robot has redundant joints, and various tasks can be completed by coordinating these redundant joints. Exemplarily, as shown in Figure 3, the robot control method includes:

Step S310, acquiring state parameters related to joint angular velocity of the robot in the current control command cycle.

Step S320, using the robot joint pose optimization method to optimize the joint pose of the next control command cycle according to the state parameters.

Exemplarily, which state parameters need to be obtained can be determined according to the constructed first optimization function and the second optimization function. These state parameters can be directly collected by sensors, etc., or can be obtained through calculation and processing of collected data. What is obtained later is not limited here.

For example, taking the above-mentioned first optimization function as an example, the joint angle corresponding to the control command cycle, the expected terminal velocity, and the Jacobian matrix can be obtained, and then the known quantities are substituted into the optimization function to be used to calculate the joint angular velocity. The optimization variable and the slack variable are solved, and the second optimization function is the same.

It can be understood that for the method for optimizing the pose and posture of the robot joints adopted in this embodiment, refer to the description of the above-mentioned embodiment 1 for details, and the description will not be repeated here.

Step S330, controlling the robot to perform corresponding operations according to the optimized joint poses.

Exemplarily, after the joint angles or joint positions required for the next control command cycle of the robot are obtained, these joint angles or joint positions are sent as commands to the corresponding joint motors, so that the robot performs corresponding operations, for example, can It is robot obstacle avoidance task, human-computer interaction task, etc.

The robot control method of this embodiment uses the robot joint pose optimization method to automatically optimize the joint pose, so that the robot joint position can always have the largest activity margin under the premise of ensuring the Cartesian space pose. .

Example 3

Please refer to FIG. 4. Based on the method of the above-mentioned embodiment 1, this embodiment proposes a robot joint pose optimization device 100. Exemplarily, the robot joint pose optimization device 100 includes:

The construction module 110 is used to construct a first optimization function with the joint angular velocity as the optimization variable and the robot terminal velocity as the control target, the first optimization function includes the inequality constraint and the equality constraint including the slack variable that the joint angular velocity satisfies.

The construction module 110 is also used to construct a second optimization function with the joint angular velocity as the optimization variable and the joint angle of the robot away from the joint limit as the control target, the second optimization function includes the same slack that the joint angular velocity satisfies Variable constraints.

The solving module 120 is configured to solve according to the first optimization function to obtain the solution of the slack variable, and obtain the optimal joint angular velocity according to the solution of the slack variable and the second optimization function.

The solving module 120 is also used to obtain the optimal joint angle of the robot according to the optimal joint angular velocity.

An optimization module 130, configured to optimize the poses of the robot joints according to the optimal joint angles of the robot.

It can be understood that the device in this embodiment corresponds to the method in the above-mentioned embodiment 1, and the optional items in the above-mentioned embodiment 1 are also applicable to this embodiment, so the description will not be repeated here.

The present application also provides a robot. Exemplarily, the robot includes a processor and a memory, wherein the memory stores a computer program, and the processor executes the computer program so that the robot performs the above-mentioned robot joint pose optimization method Or the functions of each module in the above-mentioned robot joint pose optimization device.

The present application also provides a readable storage medium for storing the computer program used in the above robot.

In the several embodiments provided in this application, it should be understood that the disclosed devices and methods may also be implemented in other ways. The device embodiments described above are only illustrative. For example, the flow charts and structural diagrams in the accompanying drawings show the possible implementation architecture and functions of devices, methods and computer program products according to multiple embodiments of the present application. and operation. In this regard, each block in a flowchart or block diagram may represent a module, program segment, or part of code that includes one or more Executable instructions. 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 in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved. It is also to be noted that each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams, can be implemented by a dedicated hardware-based system that performs the specified function or action. may be implemented, or may be implemented by a combination of special purpose hardware and computer instructions.

In addition, each functional module or unit in each embodiment of the present application may be integrated to form an independent part, each module may exist independently, or two or more modules may be integrated to form an independent part.

If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage media include: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disc, etc., which can store program codes. .

The above is only a specific implementation of the application, but the scope of protection of the application is not limited thereto. Anyone familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the application. Should be covered within the protection scope of this application.

Claims

A robot joint pose optimization method, characterized in that, comprising:

Constructing a first optimization function with the joint angular velocity as the optimization variable and the robot terminal velocity as the control target, the first optimization function includes the inequality constraint and the equality constraint including the slack variable that the joint angular velocity satisfies;

Constructing a second optimization function with the joint angular velocity as the optimization variable, and the joint angle of the robot being away from the joint limit as the control target, the second optimization function includes the constraint condition that the joint angular velocity satisfies and contains the same slack variable;

Solving according to the first optimization function to obtain the 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;

An optimal joint angle of the robot is obtained according to the optimal joint angular velocity, and a pose of the robot joint is optimized according to the optimal joint angle of the robot.
The robot joint pose optimization method according to claim 1, wherein the inequality constraint satisfied by the joint angular velocity is obtained through the following steps, comprising:

Constructing joint angle constraints that each joint of the robot satisfies in a corresponding control cycle, and converting the joint angle constraints into a first constraint condition with joint angular velocity as a variable;

Constructing joint angular acceleration constraints that each joint satisfies in the corresponding control period, and converting the joint angular acceleration constraints into a second constraint condition with joint angular velocity as a variable;

Based on the first constraint condition, the second constraint condition, and the self-limit of the joint angular velocity, a synthetic constraint to obtain the joint angular velocity is constructed as the inequality constraint.
The robot joint pose optimization method according to claim 2, wherein the first constraint condition is:

in,
and
are the upper and lower limits of the i-th joint angle, respectively;
is the i-th joint angle at time t;
is the joint angular velocity of the i-th joint; T is the control command period of the robot.
The robot joint pose optimization method according to claim 3, wherein the second constraint condition is:

in,
and
are the upper limit and lower limit of the i-th joint angular acceleration, respectively;
and
are the i-th joint angular velocity at time t-1 and time t, respectively.
The robot joint pose optimization method according to claim 4, wherein the first optimization function takes the square of the slack variable as the optimization index;

The expression of the first optimization function is:

Wherein, w is the slack variable, J is the speed Jacobian matrix of the robot,
is the joint angular velocity vector of all joints of the robot;
is the terminal velocity vector of the robot;
and
are the upper and lower limits of the i-th joint angular velocity, respectively.
The robot joint pose optimization method according to claim 1, wherein the second optimization function uses the Euclidean distance between the joint angle and the joint limit as the optimization index;

The formula for calculating the Euclidean distance is:

Wherein, q is described Euclidean distance;
and
are the upper and lower limits of the i-th joint angle, respectively;
is the i-th joint angle at time t.
The robot joint pose optimization method according to claim 6, wherein the constraint condition containing the same slack variable of the second optimization function is the same as the equality constraint of the first optimization function, so The second optimization function also includes the inequality constraint that the joint angular velocity satisfies;

The expression of the second optimization function is:

in,
is the i-th joint angle at time t-1; T is the control instruction period of the robot; w is the relaxation variable, J is the speed Jacobian matrix of the robot,
is the joint angular velocity vector of all joints of the robot;
is the terminal velocity vector of the robot;
and
are the upper and lower limits of the i-th joint angle, respectively;
and
are the upper and lower limits of the i-th joint angular velocity, respectively;
and
are the upper limit and lower limit of the i-th joint angular acceleration, respectively;
and
are the i-th joint angular velocity at time t-1 and time t, respectively.
A method for controlling a robot, comprising:

Obtain the state parameters related to the joint angular velocity of the robot in the current control command cycle;

Utilizing the robot joint pose optimization method according to any one of claims 1 to 7 to optimize the joint pose of the next control command cycle according to the state parameters;

The robot is controlled to perform corresponding operations according to the optimized joint poses.
A robot, characterized in that the robot comprises a processor and a memory, the memory stores a computer program, and the processor is used to execute the computer program to implement the robot according to any one of claims 1-7 The joint pose optimization method or the robot control method described in claim 8.
A readable storage medium, characterized in that it stores a computer program, and when the computer program is executed by a processor, it implements the robot joint pose optimization method or claim according to any one of claims 1-7 The robot control method described in 8.