WO2023115784A1 - Robot motion information planning method and related apparatus - Google Patents

Abstract

A robot motion information planning method. The method comprises: obtaining a target optimization function used for measuring the motion time and motion energy consumption of a robot, wherein the target optimization function is constructed by means of a target dynamic model; under a constraint condition of motion information, determining the minimum value of the target optimization function when the robot moves along a target trajectory; and according to target motion information corresponding to the minimum value, generating motion planning information when the robot moves along the target trajectory. A robot motion information planning apparatus, a robot implementing the method, and a computer readable storage medium. According to the target optimization function in the method, all joints of the robot and the friction force of the joints are taken into consideration, such that the optimization precision of the final time and energy consumption can be improved.

Description

Robot motion information planning method and related device

Cross References to Related Applications

This disclosure claims priority to a Chinese patent application with application number 202111577344.5 entitled "Robot Motion Information Planning Method and Related Devices" filed with the State Intellectual Property Office of China on December 22, 2021, the entire contents of which are incorporated herein by reference. In public.

technical field

The present disclosure relates to the field of automatic control, in particular, to a robot motion information planning method and a related device.

Background technique

At present, the industry mainly controls the movement of the robot through manual teaching. The trajectory of the robot between multiple teaching points is planned according to the Cartesian space line, arc or spline transition curve. The speed along the path generally adopts polynomial or The spline fitting form solves the robot trajectory with velocity and acceleration constraints.

However, the study found that such algorithms do not consider the constraints of time, energy consumption and other factors, and cannot meet the requirements for high efficiency and low energy consumption of industrial robots in actual industrial scenarios.

Contents of the invention

In order to overcome at least one deficiency in related technologies, the present disclosure provides a robot motion information planning method and related devices, including:

The present disclosure provides a robot motion information planning method, which is applied to a robot, and the method may include:

Obtaining an objective optimization function for measuring the movement time and energy consumption of the robot, wherein the objective optimization function is constructed by the objective dynamic model of the robot;

Under the constraints of motion information, determine the minimum value of the objective optimization function when the robot moves along the target trajectory;

According to the target motion information corresponding to the minimum value, the motion planning information when the robot moves along the target trajectory is generated.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that, under the constraints of motion information, determining the minimum value of the target optimization function when the robot moves along the target trajectory includes:

performing a second-order cone programming transformation on the objective optimization function to obtain a first optimization function;

Using the Runge-Kutta method to discretize the first optimization function and the constraint conditions, respectively, to obtain a second optimization function after the first optimization function is discretized and a discrete constraint condition after the constraint conditions are discretized;

The second optimization function is initialized by the joint function of the robot at each discrete point to obtain the initialized third optimization function, wherein all the discrete points constitute the target trajectory, and the joint function of each discrete point Used to indicate the angle, angular velocity and angular acceleration of each joint when the end of the robot is located at the discrete point;

Under the discrete constraints, the minimum value of the third optimization function is calculated by a preset solving tool.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the preset solution tool is YALMIP.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the second optimization function is initialized through the joint functions of the robot at each discrete point, and the initialized third optimization function is obtained Previously, the method further included:

Performing normalization processing on the target trajectory to obtain a normalized trajectory;

According to each discrete point of the obtained normalized trajectory, the joint function of each discrete point is respectively determined through inverse kinematics of the robot.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the acquisition of the target optimization function used to measure the motion time and motion energy consumption of the robot includes:

Constructing the target dynamic model, wherein the target dynamic model includes the friction coefficient of the robot and the dynamic parameters of all joints;

According to the target dynamic model, the target optimization function is constructed, wherein the target optimization function includes the movement time and the movement energy consumption, and the movement time passes the length of the target trajectory and the end of the robot The motion speed is obtained, and the motion energy consumption is obtained from the mapping relationship between joint torque and joint drive current in the target dynamic model.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the construction of the target dynamic model includes:

According to the Newton-Euler formula, iteratively deduce the expression of the first dynamic model of each joint of the robot;

Adding an inertial term and a frictional term to the total joint torque, wherein the inertial term and the frictional term are added to the first dynamic model for each joint to obtain a second dynamical model;

Replacing the inertia tensor relative to the center of mass of the connecting rod in the second dynamic model into the joint origin inertia tensor to obtain a third dynamic model;

linearizing the third kinetic model to obtain a fourth kinetic model;

Decomposing the fourth kinetic model by QR, converting the fourth kinetic model into a form of a minimum parameter set;

According to the observation matrix of the minimized parameter set, the matrix condition number is calculated to determine the Fourier series excitation trajectory;

According to the sampled torque and joint angle information, multiple linear regression is performed on the overdetermined linear equation to solve the dynamic minimum parameter set;

Kinetic standard parameter conversion;

Obtain the full kinetic state-space equations.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the target trajectory is a normalized trajectory after normalization processing, and the expression of the target optimization function is:

表示所述机器人末端沿所述归一化轨迹运动时的速度，κ _mi为力矩常数，表示所述机器人的关节力矩与驱动电流的转换关系， ⁱR表示第i个关节对应电机的电阻，

表示第i个关节对应电机的力矩，γ表示所述运动时间与所述运动能耗之间的权重。 In the formula, s represents the normalized trajectory,

Indicates the speed at the end of the robot when it moves along the normalized trajectory, κ _mi is a moment constant, which indicates the conversion relationship between the joint torque of the robot and the drive current, ⁱ R indicates the resistance of the motor corresponding to the i-th joint,

Indicates the torque of the motor corresponding to the i-th joint, and γ indicates the weight between the movement time and the movement energy consumption.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that the expression of the target dynamic model is:

的表达式为： in,

The expression is:

表示科里奥利力和离心力矩阵，G(q)表示重力矢量，

表示摩擦力矢量，

表示关节角加速度，

表示关节角速度，F _c、F _v分别表示摩擦力系数的向量，

表示对关节角速度进行符号化处理。 In the formula, M(q) represents the inertia matrix,

Represents the Coriolis force and centrifugal force matrix, G(q) represents the gravity vector,

represents the friction vector,

represents the joint angular acceleration,

represents the joint angular velocity, F _c and F _v represent the vectors of the friction coefficient respectively,

Indicates the symbolic processing of the joint angular velocity.

Optionally, the robot motion information planning method provided in the present disclosure is characterized in that, according to the target motion information corresponding to the minimum value, generating motion planning information when the robot moves along the target trajectory includes:

Obtaining target motion information corresponding to the minimum value;

The target motion information is interpolated by a quintic polynomial to obtain motion planning information when the robot moves along the target trajectory.

The present disclosure also provides a robot motion information planning device, which is applied to a robot, and the robot motion information planning device may include:

A function module configured to obtain a target optimization function for measuring the movement time and energy consumption of the robot, wherein the target optimization function can be constructed through the target dynamic model of the robot;

An optimization module configured to determine the minimum value of the target optimization function when the robot moves along the target trajectory under the constraints of motion information;

The planning module is configured to generate motion planning information when the robot moves along the target trajectory according to the target motion information corresponding to the minimum value.

The present disclosure also provides a robot, the robot may include a processor and a memory, the memory may store a computer program, and when the computer program is executed by the processor, the robot motion information planning method may be realized .

Optionally, the robot provided in the present disclosure is characterized in that the robot further includes a communication unit.

The present disclosure also provides a computer-readable storage medium. The computer-readable storage medium can store a computer program. When the computer program is executed by a processor, the robot motion information planning method can be realized.

Compared with related technologies, the present disclosure has the following beneficial effects:

In the robot motion information planning method and related devices provided by the present disclosure, the target dynamic model includes the dynamic parameters of all joints of the robot and the friction coefficients of each joint; The target optimization function of the time and energy consumption required for the target trajectory movement; because the target optimization function considers all joints of the robot and the friction of each joint, the optimization accuracy of the final time and energy consumption can be improved.

Description of drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, 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 disclosure, and therefore are not 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 structural diagram of a robot provided by an embodiment of the present disclosure;

FIG. 2 is a schematic flow chart of a robot motion planning method provided by an embodiment of the present disclosure;

FIG. 3 is a schematic diagram of a target trajectory provided by an embodiment of the present disclosure;

Fig. 4A-Fig. 4C are torque actual measurement-prediction comparison chart provided by the embodiment of the present disclosure;

Figures 5A-5B are actual measurement diagrams of the parameters of the polynomial trajectory method provided by the embodiment of the present disclosure;

6A-6B are actual measurement diagrams of the parameters of the robot motion information planning method provided by the embodiment of the present disclosure;

Fig. 7 is a schematic diagram of a robot motion information planning device provided by an embodiment of the present disclosure.

Icons: 120-memory; 130-processor; 140-communication unit; 201-function module; 202-optimization module; 203-planning module.

Detailed ways

In order to make the purpose, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings in the embodiments of the present disclosure. Obviously, the described embodiments It is a part of the embodiments of the present disclosure, but not all of them. The components of the disclosed embodiments 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 present disclosure provided in the accompanying drawings is not intended to limit the scope of the claimed disclosure, but merely represents selected embodiments of the present disclosure. Based on the embodiments in the present disclosure, all other embodiments obtained by persons of ordinary skill in the art without creative efforts fall within the protection scope of the present disclosure.

It should be noted that like numerals and letters denote similar items in the following figures, therefore, once an item is defined in one figure, it does not require further definition and explanation in subsequent figures.

In the related technologies of controlling robot motion by manual teaching, the limitations of time, energy consumption and other factors are not considered, and it cannot meet the requirements of high efficiency and low energy consumption of industrial robots in actual industrial scenarios.

For example, it is assumed that the robot is a 6-axis industrial robot, and the end of the industrial robot moves from position A to position B along the target trajectory after traction teaching; however, in the actual moving process, the industrial robot can pass various Way to move from position A to position B along the target trajectory. For example, the industrial robot can move the end from position A to position B according to different moving speeds or torques. The study found that different working methods will cause the robot to consume different time and energy consumption; however, related technologies do not take time, energy consumption and other factors into consideration.

In view of this, this embodiment provides a robot motion information planning method applied to a robot. In this method, the robot obtains an objective optimization function for measuring movement time and energy consumption, and then, under constraints, solves the minimum value of the objective optimization function; since the objective optimization function is constructed based on the robot’s objective dynamic model and the target dynamics model is used to reflect the motion state of the robot during motion, so that the motion planning information when the robot moves along the target trajectory can be generated according to the target motion information at the minimum value of the target optimization function.

As shown in FIG. 1 , the robot may also include a memory 120 , a processor 130 , and a communication unit 140 in addition to the robot body. Wherein, the components of the memory 120 , the processor 130 and the communication unit 140 are electrically connected to each other directly or indirectly to realize data transmission or interaction. For example, these components may be electrically connected to each other through one or more communication buses or signal lines.

The memory 120 can be, but not limited to, random access memory (Random Access Memory, RAM), read-only memory (Read Only Memory, ROM), programmable read-only memory (Programmable Read-Only Memory, PROM), erasable In addition to read-only memory (Erasable Programmable Read-Only Memory, EPROM), electrically erasable read-only memory (Electric Erasable Programmable Read-Only Memory, EEPROM), etc. Wherein, the memory 120 is configured to store a program, and the processor 130 executes the program after receiving an execution instruction.

The communication unit 140 is configured to send and receive data through the network. The network can include wired network, wireless network, optical fiber network, telecommunication network, intranet, Internet, local area network (Local Area Network, LAN), wide area network (Wide Area Network, WAN), wireless local area network (Wireless Local Area Networks, WLAN), Metropolitan Area Network (MAN), Wide Area Network (Wide Area Network, WAN), Public Switched Telephone Network (PSTN), Bluetooth network, ZigBee network, or Near Field Communication (NFC) network, etc., or any combination thereof. In some embodiments, a network may include one or more network access points. For example, a network may include wired or wireless network access points, such as base stations and/or network switching nodes, through which one or more components of the service request processing system may connect to the network to exchange data and/or information.

The processor 130 may be an integrated circuit chip with signal processing capabilities, and the processor may include one or more processing cores (for example, a single-core processor or a multi-core processor). For example only, the above-mentioned processor may include a central processing unit (Central Processing Unit, CPU), an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), an application specific instruction set processor (Application Specific Instruction-set Processor, ASIP), graphics processing Unit (Graphics Processing Unit, GPU), Physical Processing Unit (Physics Processing Unit, PPU), Digital Signal Processor (Digital Signal Processor, DSP), Field Programmable Gate Array (Field Programmable Gate Array, FPGA), Programmable Logic Device (Programmable Logic Device, PLD), controller, microcontroller unit, reduced instruction set computer (Reduced Instruction Set Computing, RISC), or microprocessor, etc., or any combination thereof.

Based on the above related introductions, the robot motion information planning method provided by this implementation will be introduced in detail below in conjunction with FIG. 2 . However, it should be understood that the operations of the flowcharts may be performed out of order, and steps without logical context may be performed in reverse order or simultaneously. In addition, those skilled in the art may add one or more other operations to the flowchart, or remove one or more operations from the flowchart under the guidance of the present disclosure. As shown in Figure 2, the method may include:

S101. Obtain an objective optimization function for measuring the movement time and energy consumption of the robot.

Among them, the objective optimization function can be constructed through the objective dynamic model of the robot.

S102, under the constraints of the motion information, determine the minimum value of the target optimization function when the robot moves along the target trajectory.

Wherein, the motion information may include joint angles, speeds, and torques of each joint of the robot. For example, assuming that the robot is a 6-axis industrial robot, constraints on joint angle (rad), speed (rad/s), and torque (Nm) can be configured for each joint motor. For example, the constraints can be shown in the following table, where i in the table represents the numbers of the six joints:

S103, according to the target motion information corresponding to the minimum value, generate motion planning information when the robot moves along the target trajectory.

It should be understood that the target trajectory is the motion trajectory designed by the user for the end of the robot according to the work task; and, in order to reduce the calculation amount required for solving the target optimization function, the target optimization trajectory is discretized.

Exemplarily, continuing to take the industrial robot in the above example as an example, the dotted line in Fig. 3 represents the target trajectory when the end of the industrial robot moves from position A to position B. Therefore, sampling can be performed at preset distances along the target trajectory to obtain multiple sampling points of the target trajectory. The target optimization function is a function about the sampling points, which means that the target motion information corresponding to the minimum value includes the respective angles, angular velocities, and torques of the six joints when the end of the robot is located at each sampling point.

Therefore, the target motion information is also in a discrete state. In order to make the end of the industrial machine move more smoothly along the target trajectory, it is necessary to generate a continuous motion plan for the industrial machine based on the target motion information. information.

Based on the above-mentioned embodiment, the robot obtains an objective optimization function used to measure movement time and energy consumption, and then, under constraints, solves the minimum value of the objective optimization function; since the objective optimization function is based on the robot's objective dynamic model Construction; and the target dynamics model describes the motion state of the robot, so that the motion planning information of the robot when moving along the target trajectory can be generated according to the target motion information at the minimum value of the target optimization function.

As an optional implementation manner, interpolation processing may be performed on discrete target motion information to generate motion planning information when the robot moves along the target trajectory. Therefore, the above step S103 may include the following implementation manners:

S103-1. Acquire target motion information corresponding to the minimum value.

S103-2, performing interpolation processing on the target motion information through a quintic polynomial to obtain motion planning information when the robot moves along the target trajectory.

Exemplarily, continue referring to the multiple sampling points in FIG. 3 , which are denoted as P ₁ , P ₂ , P ₃ , P ₄ , and P ₅ respectively; for the convenience of description, P ₂ and P ₃ are selected for illustration. When the objective optimization function is minimized, the angles, speeds and moments of the six joints can be solved when the end of the industrial robot is at P ₂ ; and the angles, speeds and moments of the six joints when the end is at P ₃ can be obtained. Therefore, when the end of the industrial robot moves from _P2 to _P3 , it is necessary to fit the respective angles, speeds and moments of the six joints by interpolation.

In this embodiment, a quintic polynomial is selected to fit the motion information during the period from P ₂ to P ₃ , so that the industrial machine can move smoothly from P ₂ to P ₃ . Of course, the specific fitting method is not limited to the quintic polynomial, and those skilled in the art can make appropriate adjustments according to actual needs.

The study also found that since the robot trajectory optimization is constrained by the end path given by the work task, on the premise of satisfying the forward and reverse kinematics relationship and the target dynamic model, the target optimization function is constructed around the efficiency, energy consumption, smoothness and other indicators , under the boundary conditions of robot joint speed limit, torque limit, etc., the optimal solution algorithm is used to obtain the displacement, velocity, acceleration and other variable results of each joint with the running time; however, the robot kinematics and dynamics boundary conditions are highly coupled Formal inequality constraints, it is difficult to solve multiple optimization objectives under such complex constraints. Therefore, the trajectory optimization of related industrial robots mainly focuses on the trajectory planning with a single objective time optimal.

At the same time, in order to reduce the complexity of solving the problem, the dynamic constraints of the robot are greatly simplified in the trajectory optimization research, and the influence of viscous friction is ignored when calculating the joint torque of the robot, and, for more than three joints (joint 4, joint 5. The industrial robot of joint 6) only retains some elements in the dynamic parameter matrix, and the rest of the elements are set to zero.

Therefore, compared with the single time optimization objective, the objective function considering time-energy consumption optimization includes the torque term, and there is a large gap between the torque value required for the optimized trajectory solved by the simplified target dynamics model and the actual output torque of the robot. deviation, the robot motion is driven by the output torque of the joint motor, such inaccurate expected torque cannot accurately adjust the joint motion, and cannot adapt to the best dynamic performance of the system to achieve the ideal optimization effect.

In view of this, building an accurate target dynamic model for the robot is of great significance to improving the optimization accuracy of the robot's time energy consumption; therefore, the above step S101 may include the following implementation methods:

S101-1, constructing a target dynamics model.

Wherein, compared with the conventional dynamic model, the target dynamic model in this embodiment may include the friction coefficient of the robot and the dynamic parameters of all joints.

S101-2. Construct a target optimization function according to the target dynamic model.

Among them, the target optimization function can include motion time and motion energy consumption. The motion time is obtained by the length of the target trajectory and the motion speed of the robot end. The motion energy consumption is obtained by the mapping relationship between joint torque and joint drive current in the target dynamic model. .

Therefore, in the above-mentioned embodiment, the target dynamics model includes the dynamic parameters of all joints of the robot and the friction coefficients of each joint; An objective optimization function of time and energy consumption is required; since the objective optimization function considers all joints of the robot and the friction of each joint, the optimization accuracy of final time and energy consumption can be improved.

After the verification of the 6-axis industrial robot, the target dynamic model including the friction coefficient of the industrial robot and the dynamic parameters of all joints is adopted. The predicted results and actual measured torques of joints 4, 5 and 6 are shown in the figure 4A-4C are shown.

Among them, the absolute value of the maximum actual measured torque of the 4-axis is 33.54Nm, which is 6.476Nm different from the prediction result of the target dynamic model. Therefore, the accuracy of the model is verified. For other dynamic models that ignore the dynamic parameters above the rear three axes, all of them are regarded as 0, which will lead to a large error in the energy consumption item described by the torque superposition in the optimization function, which will seriously affect the trajectory optimization results. By comparison, compared with the target dynamic model in this embodiment, the torque error is reduced from 33.54Nm to 6.476Nm.

However, this embodiment considers the influence of inertia, Coriolis force, centripetal force, gravity, and frictional force. The first dynamic model expression of each joint of the robot can be established first, and then the dynamic parameters in the torque expression are identified, so that Obtain the target dynamic model including the friction coefficient of the robot and the dynamic parameters of all joints. Therefore, under this inventive concept, the specific implementation of the above step S101-1 is described in detail below:

S101-1-1, iteratively deriving a first dynamic model expression of each joint of the robot according to the Newton-Euler formula.

S101-1-2, add the inertia term and friction term to the total joint torque.

Exemplarily, continuing to take the industrial robot in the above example as an example, the joint moments caused by inertia, Coriolis force, centripetal force, and gravity are expressed as ^NE τ _i , i=1,2,3,... ,6, where i represents the number of each of the 6 joints.

For the i-th joint, the inertia item and the friction item are added to the first dynamic model, and the obtained second dynamic model is expressed as:

和

分别表示该工业机器人第i个关节的转速、角加速度，I _ai表示第i个关节的惯性力矩，f _vi和f _ci分别表示第i个关节的粘滞摩擦力和库伦摩擦力系数。 In the formula, ^r τ _i represents the moment of inertia caused by the rotor of the i-th joint of the industrial robot and the gear of the actuator, ^f τ _i represents the frictional moment of the i-th joint of the industrial robot,

and

represent the rotational speed and angular acceleration of the i-th joint of the industrial robot, I _ai represents the moment of inertia of the i-th joint, f _vi and f _ci represent the viscous friction and Coulomb friction coefficient of the i-th joint, respectively.

S101-1-3, replacing the inertia tensor relative to the center of mass of the connecting rod in the second dynamic model with the joint origin inertia tensor to obtain a third dynamic model.

Among them, the influence of joint inertia on joint torque is usually reflected in the form of inertia tensor, that is to say, the expression ^NE τ _i contains the inertia tensor of the i-th joint. In order to enable those skilled in the art to implement this solution, the following continues to take the industrial robot in the above example as an example to explain the inertia tensor:

Assuming that a three-dimensional Cartesian coordinate system is established with a point on the rigid body, if the rigid body rotates around the x-axis, a quantity is needed to describe the inertia of the rigid body when it rotates around the x-axis, and two quantities are required to describe the rigid body respectively The effect on the y-axis and z-axis when rotating around the x-axis. Similarly, when a rigid body rotates around the z-axis and the y-axis, three quantities are required to describe the inertial influence of the rigid body; that is, a total of nine quantities are required for the inertia tensor. Therefore, the inertia tensor of the i-th joint is usually Expressed as:

For this industrial robot, the inertia tensor relative to the center of mass of the link in the second dynamic model expression is converted into the inertia tensor relative to the joint origin, which can be expressed as:

In the formula, m _i represents the mass of the i-th connecting rod, I ₃ represents the 3×3 identity matrix, P _Ci =[x _Ci y _{Ciz Ci} ] ^T represents the position coordinate of the i-th connecting rod's centroid.

S101-1-4, linearize the third kinetic model to obtain a fourth kinetic model.

S101-1-5, QR decomposes the fourth kinetic model, and transforms it into a minimum parameter set form.

It should be understood that, in order to reduce the introduction of too many calculation parameters, this embodiment separates the inertial parameters by linearizing the target dynamic model; then, performs QR decomposition on the linearized target dynamic model, The parameters that have little or no influence on the torque are eliminated to obtain the minimum parameter set.

Continuing to take the industrial robot in the above example as an example, it can be seen from the above expression that the i-th joint of the industrial robot contains 13 parameters to be identified, which can be expressed as:

Ω _i =[m _i x _Ci y _Ci z _Ci I _xxi I _yyi I _zzi I _xyi I _xzi I _yzi I _ai f _vi f _ci ] ^T

Therefore, it means that the third dynamic model includes 13*6=78 parameters to be identified. Linearize the third kinetic model containing 78 parameters to be identified, and the result is expressed as:

表示6×78线性回归矩阵。 In the formula, τ=[τ ₁ ,τ ₂ ,...,τ ₆ ] ^T represents the respective moment matrices of the six joints, Ω=[Ω ₁ ,Ω ₂ ,...,Ω ₆ ] ^T represents the parameters to be identified matrix,

Represents a 6×78 linear regression matrix.

Assume that the minimum parameter set form of the fourth kinetic model is expressed as:

可以通过以下实施方式进行求解： in,

It can be solved by the following implementation methods:

和

的随机值对

计算78次，从而构建一个新的矩阵WW，然后，对WW进行QR分解： Use q,

and

random value pair

Calculate 78 times to construct a new matrix WW, and then perform QR decomposition on WW:

对应的列按顺序排列组成矩阵W _z，剩余列按顺序组成矩阵W _min，最后，最小参数值r等于W的秩，且r与W _min的列相同。 In the formula, Q represents an orthogonal matrix, and R represents an upper triangular matrix. If the value of element R _{i, i in row i and column i} in R is zero, then

The corresponding columns are arranged in order to form a matrix W _z , and the remaining columns are arranged in order to form a matrix W _min . Finally, the minimum parameter value r is equal to the rank of W, and r is the same as the column of W _min .

Among them, the minimum parameter set Ω _min is obtained through the following implementation methods:

1) Introduce a permutation matrix Θ, which satisfies the following relationship:

是由R中R _i,i≠0的列构成的矩阵，R ₁是r×r的上三角正则矩阵，R ₂是r×(78-r)矩阵，

则表示由R中R _i,i＝0的列构成的矩阵，由此可得出： In the formula, Θ represents an orthogonal matrix,

is a matrix composed of columns of R _i,i ≠0 in R, R ₁ is an upper triangular regular matrix of r×r, R ₂ is an r×(78-r) matrix,

Then it represents the matrix formed by the columns of R _i,i =0 in R, thus it can be drawn:

2) Convert the fourth kinetic model into the following form:

In the formula, Ω _ind is an r×1 vector composed of independent parameters in matrix Ω, and Ω _com is a (78-r)×1 vector composed of recombined parameters.

In order to eliminate W _z in the above formula, it is equivalently transformed:

表示矩阵

独立列的子集。 In the formula, Ω _min represents the minimum set of parameter vectors to be identified,

representation matrix

A subset of individual columns.

S101-1-6. Calculate the condition number of the matrix according to the observation matrix of the minimized parameter set Ω _min , so as to determine the excitation trajectory of the Fourier series.

It should be understood that the dynamics model cannot accurately describe the dynamic state of the industrial robot in the example at present, and relevant parameters still need to be adjusted. In order to adjust the relevant parameters, it is necessary to plan multiple excitation trajectories for collecting experimental data for the industrial robot; Finally, the relevant parameters of the kinetic model are adjusted according to the comparison results.

Therefore, continuing to take the industrial robot in the above example as an example, the selected excitation trajectory with parameters is expressed as:

In the formula, j represents the number of items in the Fourier series, t represents the running time, ω _f represents the fundamental frequency, q _i0 represents the original constant in the Fourier excitation trajectory of the i-th joint, u _j and v _j represent is the undetermined coefficient in the Fourier excitation trajectory, and the fundamental frequency of each joint trajectory is the same.

Assuming that the sampling time is t _s , 2π/(t _s ·ω _f ) sampling points are obtained in the preset period, and the joint torque and joint angle are sampled at multiple sampling periods at t=t ₁ ,t ₂ ,... , to get overdetermined linear equations, the overdetermined linear equations are expressed as:

Γ＝Ψ·Ω _min +ε

In the formula, Ψ is the observation matrix, and its expression is:

In the formula, Γ=[τ(t ₁ ) ^T τ(t ₂ ) ^T …] represents the vector composed of torque at each sampling period, and ε represents the vector composed of noise and error generated during the sampling process.

最小化为优化目标，其中λ _max(Ψ)和λ _min(Ψ)表示矩阵Ψ的最大和最小特征值，对傅里叶激励轨迹的参数u _j、v _j和q _i0进行优化设计。 The condition number of the matrix Ψ

Minimization is the optimization objective, where λ _max (Ψ) and λ _min (Ψ) represent the maximum and minimum eigenvalues of the matrix Ψ, and the parameters u _j , v _j and q _i0 of the Fourier excitation trajectory are optimally designed.

S101-1-7, according to the sampled torque and joint rotation angle information, perform multiple linear regression on the above-mentioned overdetermined linear equation, and solve the dynamic minimum parameter set.

S101-1-8, conversion of kinetic standard parameters.

Continuing to take the industrial robot in the above example as an example, substituting the multiple sets of joint angles and joint moments obtained by sampling into the minimum parameter set of the fourth dynamic model of the industrial robot, and then performing multiple linear regression based on the least squares method to solve the dynamics Minimal set of parameters:

Ω _min ＝(Ψ ^T Ψ) ^-1 Ψ ^T Γ

并根据该独立参数矩阵获得标准参数向量Ω： Then, calculate the independent parameter matrix of Ω _ind obtained in step S101-1-5

And obtain the standard parameter vector Ω according to this independent parameter matrix:

In the formula, Θ represents an orthogonal matrix, and the expression is:

S101-1-9, obtaining a complete dynamical state space equation.

After the above steps, the identified parameters are substituted into the fourth dynamic model to obtain a complete dynamic state space model, that is, the expression of the target dynamic model of the industrial robot in the above example is:

的表达式为： in,

The expression is:

表示科里奥利力和离心力矩阵，G(q)表示重力矢量，

表示摩擦力矢量，

表示关节角加速度，

表示关节角速度，F _c、F _v分别表示摩擦力系数的向量，

表示对关节角速度进行符号化处理。 In the formula, M(q) represents the inertia matrix,

Represents the Coriolis force and centrifugal force matrix, G(q) represents the gravity vector,

represents the friction vector,

represents the joint angular acceleration,

represents the joint angular velocity, F _c and F _v represent the vectors of the friction coefficient respectively,

Indicates the symbolic processing of the joint angular velocity.

Based on the constructed target dynamics model, the target optimization function of the robot can be obtained. The study found that the objective optimization function describes the sum of the motion time and power consumption of the industrial robot, and the time and power consumption belong to different types of dimensions. Therefore, in order to unify the two, before solving the minimum value of the objective optimization function , the target trajectory needs to be normalized, and the robot's motion information is adaptively transformed according to the normalized trajectory.

Exemplarily, assume that when t=0, the end of the robot is located at the starting point of the target trajectory, and when t= _tf , the end of the robot moves to the end of the target trajectory, and normalizing the target trajectory indicates that the robot is currently at the target The proportion of positions in the trajectory in the target trajectory.

In this embodiment, the functional relationship between the normalized trajectory and time is expressed as s(t), then when t=0, s=0; when t=t _f , s=1.

Continuing to take the industrial robot as an example, based on the normalized trajectory, the expressions of the joint functions of each discrete point are:

The joint angle function is expressed as: q(s):

The joint angular velocity function is expressed as:

The joint angular acceleration function is expressed as:

为归一化轨迹速度，

为归一化轨迹加速度。为便于描述

定义为a(s)，

定义为b(s)。 In the formula,

is the normalized trajectory velocity,

is the normalized trajectory acceleration. for ease of description

defined as a(s),

Defined as b(s).

According to the normalized trajectory and motion information of the robot, the minimum value of the objective optimization function can be solved in the following manner, that is, step S102 can include:

S102-1, converting the target optimization function to a second-order cone programming to obtain a first optimization function.

Continuing to take the industrial robot in the above embodiment as an example, according to the target dynamic model of the industrial robot, the following target optimization function can be obtained:

表示机器人末端沿归一化轨迹运动时的速度，κ _mi为第i个关节对应的力矩常数，表示机器人的关节力矩与驱动电流的转换关系， ⁱR表示第i个关节对应电机的电阻，

表示第i个关节对应电机的力矩，γ表示运动时间与运动能耗之间的权重。 where s represents the normalized trajectory,

Indicates the speed of the end of the robot moving along the normalized trajectory, κ _mi is the torque constant corresponding to the i-th joint, which indicates the conversion relationship between the joint torque of the robot and the drive current, ⁱ R indicates the resistance of the motor corresponding to the i-th joint,

Indicates the torque of the i-th joint corresponding to the motor, and γ indicates the weight between motion time and motion energy consumption.

Among them, the weight γ is used to adjust the respective proportions of the industrial robot’s motion time and motion energy consumption; that is to say, when the task of the industrial robot is sensitive to time, the weight can be adjusted to improve the motion time in the objective optimization function Similarly, when the work tasks of industrial robots are sensitive to energy consumption, the weight can be adjusted to increase the proportion of motion energy consumption in the objective optimization function.

Since the objective optimization function is obtained based on the normalized trajectory, the constraints of the industrial robot also need to be adjusted following the normalized trajectory. Therefore, the constraints of the industrial robot are expressed as:

b'(s)=2a(s), b(s)>0, b(0)=0, b(1)=0

In the formula, the upper line and the lower line represent the maximum value and the minimum value of the corresponding motion information, respectively. For the convenience of calculation, this embodiment introduces the following two variables to simplify the target optimization function of industrial robots, and convert all non-convex functions (for example, functions that require a root sign) into functions that can be conically programmed:

相比后的权重系数，相应的表达式为： In the formula, α represents the energy consumption item and extreme value of the simplified function

After comparing the weight coefficient, the corresponding expression is:

The simplified function is transformed into a second-order cone programming, and the obtained first optimization function can be expressed as:

S102-1. Using the Runge-Kutta method to discretize the first optimization function and the constraint conditions, respectively, to obtain a second optimization function after the first optimization function is discretized and a discrete constraint condition after the constraint conditions are discretized.

Continuing to take the industrial robot as an example, the above-mentioned first optimization function of the industrial robot and the constraints of the industrial robot are discretely processed, and the obtained second optimization function can be expressed as:

In the formula, the normalized trajectory at the jth discrete point of the path is denoted as s _j , s ₀ is the normalized trajectory at the starting point of the path, s _N is the normalized trajectory at the end point of the path, and the j+1 segment is discrete The path interval is Δs _j =s _j+1 -s _j ,

The obtained discrete constraints can be expressed as:

Among them, a(s _j ), b(s _j ), b(s _j+1 ), h _j (s) satisfy the following relationship:

b(s _j+1 )-b(s _j )=2a(s _j )Δs _j

S102-1. Initialize the second optimization function through joint functions of the robot at each discrete point, to obtain an initialized third optimization function.

Among them, all discrete points constitute the target trajectory, and the joint function of each discrete point is used to represent the angle, angular velocity and angular acceleration of each joint when the end of the robot is located at the discrete point.

S102-1. Under discrete constraint conditions, calculate a minimum value of the third optimization function by using a preset solving tool.

Wherein, the preset solving tool may be YALMIP. That is to say, the angle function, angular velocity function and angular acceleration function of the industrial robot on the normalized trajectory at each discrete point are brought into the second optimization function of the industrial robot to obtain the third optimization function of the industrial robot; finally, through the tool YALMIP The minimum value of the objective optimization function can be obtained.

The robot motion planning method provided in this embodiment is verified by a 6-axis industrial robot. Among them, in the verification process, the limit value of the servo drive control system is set to 0.75 times of the maximum torque to avoid damage to the servo drive control system. The verification results show that under different weight states, the solution time is controlled between 0.5s and 1s. Therefore, the robot motion planning method has good solution efficiency. The specific solution time (s) and the running time of the optimized trajectory (s ) as shown in the table below:

optimization instructions Trajectory running time Solution time α=10 ^0.4 1.3330 0.7323 α=1 1.3062 0.6901 α=0 1.0612 0.6924

Moreover, the verification results also show that when the limit value of the servo drive control system is set to 0.75 times the maximum torque and 0.25 times the maximum speed, and the optimal weight coefficient is set to α=10 ^0.4 , the running time of the trajectory is 2.4715s. The measured overall energy consumption of the industrial machine robot is 0.172Wh.

The running time of related methods (for example, the polynomial trajectory method) is 3.472s, and the energy consumption is 0.214Wh. Therefore, when the industrial robot works according to the motion planning information planned by the robot motion planning method in this case, the path of the trajectory The time is 28.8% shorter than related methods.

Among them, when based on the polynomial trajectory method, the six joint torque and velocity sampling data are shown in Figure 5A-Figure 5B; when based on the robot motion planning method in this embodiment, the six joint torque and velocity sampling data are shown in Figure 6A-Figure 6B shown. It can be seen that when the motion planning information planned by the robot motion planning method is used to work, the maximum absolute value of the robot joint speed is increased, which can effectively reduce the running time of the robot; and at the same time, the torque of the motor after optimization also reaches the limit The relatively large capability in the range, therefore, validates the effectiveness of the algorithm and constraint design.

Based on the same inventive concept as the robot motion planning method, this embodiment also provides related devices, including:

This embodiment also provides a robot motion information planning device, which is applied to a robot. Wherein, the robot motion information planning device optionally includes at least one functional module that can be stored in the memory in the form of software. As shown in Figure 7, in terms of functions, the robot motion information planning device can include:

The function module 201 is configured to obtain an objective optimization function for measuring the movement time and energy consumption of the robot, wherein the objective optimization function can be constructed through the objective dynamic model of the robot.

In this embodiment, the function module 201 is configured to implement step S101 in FIG. 2 . For a detailed description of the function module 201 , refer to the detailed description of step S101 .

The optimization module 202 is configured to determine the minimum value of the target optimization function when the robot moves along the target trajectory under the constraints of the motion information.

In this embodiment, the optimization module 202 is configured to implement step S102 in FIG. 2 , and for a detailed description of the optimization module 202 , refer to the detailed description of step S102 .

The planning module 203 is configured to generate motion planning information when the robot moves along the target trajectory according to the target motion information corresponding to the minimum value.

In this embodiment, the planning module 203 is configured to implement step S103 in FIG. 2 . For a detailed description of the planning module 203 , refer to the detailed description of step S103 .

It should be noted that, in an optional implementation manner, the robot motion information planning device may further include other functional modules for implementing other steps or sub-steps of the robot motion information planning method. In other optional implementation manners, the function module 201, the optimization module 202 and the planning module 203 can also be used to implement other steps or sub-steps of the robot motion information planning method.

This embodiment also provides a robot. The robot may include a processor and a memory, and the memory may store a computer program. When the computer program is executed by the processor, the robot motion information planning method may be implemented.

This embodiment also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the robot motion information planning method can be implemented.

It should be noted that the terms "first", "second", "third" and so on are only used for distinguishing descriptions, and should not be understood as indicating or implying relative importance. Furthermore, the term "comprises", "comprises" or any other variation thereof is intended to cover a non-exclusive inclusion such that a process, method, article or apparatus comprising a set of elements includes not only those elements, but also includes elements not expressly listed. other elements of or also include elements inherent in such a process, method, article, or device. Without further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article or apparatus comprising said element.

It should also be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are only illustrative. For example, the flowcharts and block diagrams in the accompanying drawings show the architecture, functions and possible implementations of devices, methods and computer program products according to multiple embodiments of the present disclosure. operate. 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 some 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 should also be noted that each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations, can be implemented by a dedicated hardware-based system that performs the specified function or action , or may be implemented by a combination of dedicated hardware and computer instructions.

In addition, each functional module in each embodiment of the present disclosure may be integrated together 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 essence of the technical solution of the present disclosure or the part that contributes to the related technology or the part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several The instructions are used to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in various embodiments of the present disclosure. The aforementioned storage medium can 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. medium.

The above are just various implementations of the present disclosure, but the scope of protection of the present disclosure is not limited thereto. Anyone skilled in the art can easily think of changes or substitutions within the technical scope of the present disclosure. should be covered within the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be defined by the protection scope of the claims.

Industrial Applicability

The present disclosure provides a robot motion information planning method and related devices, wherein the target dynamic model includes the dynamic parameters of all joints of the robot and the friction coefficients of each joint; The target optimization function of the time and energy consumption required for the terminal to move along the target trajectory; since the target optimization function considers all joints of the robot and the friction of each joint, the optimization accuracy of the final time and energy consumption can be improved.

In addition, it can be understood that the robot motion information planning method and related devices of the present disclosure are reproducible and can be used in various applications, for example, in the field of industrial robots.

Claims (13)

A robot motion information planning method is characterized in that it is applied to a robot, and the method includes: Obtaining an objective optimization function for measuring the movement time and energy consumption of the robot, wherein the objective optimization function is constructed by the objective dynamic model of the robot; Under the constraints of motion information, determine the minimum value of the objective optimization function when the robot moves along the target trajectory; According to the target motion information corresponding to the minimum value, motion planning information when the robot moves along the target trajectory is generated. The robot motion information planning method according to claim 1, wherein the minimum value of the target optimization function is determined when the robot moves along the target trajectory under the constraints of motion information, including: performing a second-order cone programming transformation on the objective optimization function to obtain a first optimization function; Using the Runge-Kutta method to discretize the first optimization function and the constraint conditions, respectively, to obtain a second optimization function after the first optimization function is discretized and a discrete constraint condition after the constraint conditions are discretized; The second optimization function is initialized by the joint function of the robot at each discrete point to obtain the initialized third optimization function, wherein all the discrete points constitute the target trajectory, and the joint function of each discrete point Used to indicate the angle, angular velocity and angular acceleration of each joint when the end of the robot is located at the discrete point; Under the discrete constraints, the minimum value of the third optimization function is calculated by a preset solving tool. The robot motion information planning method according to claim 2, wherein the preset solving tool is YALMIP. The robot motion information planning method according to claim 2 or 3, wherein the second optimization function is initialized through the joint functions of the robot at each discrete point to obtain the initialized third optimization function Previously, the method further included: Performing normalization processing on the target trajectory to obtain a normalized trajectory; According to each discrete point of the obtained normalized trajectory, the joint function of each discrete point is respectively determined through inverse kinematics of the robot. The robot motion information planning method according to claim 1, wherein said acquisition of an objective optimization function for measuring the motion time and motion energy consumption of the robot comprises: Constructing the target dynamic model, wherein the target dynamic model includes the friction coefficient of the robot and the dynamic parameters of all joints; According to the target dynamic model, the target optimization function is constructed, wherein the target optimization function includes the movement time and the movement energy consumption, and the movement time passes the length of the target trajectory and the end of the robot The motion speed is obtained, and the motion energy consumption is obtained from the mapping relationship between joint torque and joint drive current in the target dynamic model. The robot motion information planning method according to claim 5, wherein said building said target dynamics model comprises: According to the Newton-Euler formula, iteratively deduce the expression of the first dynamic model of each joint of the robot; Adding an inertial term and a frictional term to the total joint torque, wherein the inertial term and the frictional term are added to the first dynamic model for each joint to obtain a second dynamical model; Replacing the inertia tensor relative to the center of mass of the connecting rod in the second dynamic model into the joint origin inertia tensor to obtain a third dynamic model; linearizing the third kinetic model to obtain a fourth kinetic model; Decomposing the fourth kinetic model by QR, converting the fourth kinetic model into a form of a minimum parameter set; According to the observation matrix of the minimized parameter set Ω _min , the matrix condition number is calculated to determine the Fourier series excitation trajectory; According to the sampled torque and joint angle information, multiple linear regression is performed on the overdetermined linear equation to solve the dynamic minimum parameter set; Kinetic standard parameter conversion; Obtain the full kinetic state-space equations. The robot motion information planning method according to claim 4, wherein the target trajectory is a normalized trajectory after normalization processing, and the expression of the target optimization function is:

In the formula, s represents the normalized trajectory,

Indicates the speed at the end of the robot when it moves along the normalized trajectory, κ _mi is a moment constant, which indicates the conversion relationship between the joint torque of the robot and the drive current, ⁱ R indicates the resistance of the motor corresponding to the i-th joint,

Indicates the torque of the motor corresponding to the i-th joint, and γ indicates the weight between the movement time and the movement energy consumption. The robot motion information planning method according to claim 4, wherein the expression of the target dynamic model is:

in,

The expression is:

In the formula, M(q) represents the inertia matrix,

Represents the Coriolis force and centrifugal force matrix, G(q) represents the gravity vector,

represents the friction vector,

represents the joint angular acceleration,

represents the joint angular velocity, F _c and F _v represent the vectors of the friction coefficient respectively,

Indicates the symbolic processing of the joint angular velocity. The robot motion information planning method according to claim 1, wherein said generating motion planning information when the robot moves along the target trajectory according to the target motion information corresponding to the minimum value includes: Obtaining target motion information corresponding to the minimum value; The target motion information is interpolated by a quintic polynomial to obtain motion planning information when the robot moves along the target trajectory. A robot motion information planning device is characterized in that it is applied to a robot, and the robot motion information planning device includes: A function module configured to obtain a target optimization function for measuring the movement time and energy consumption of the robot, wherein the target optimization function is constructed by a target dynamic model of the robot; An optimization module configured to determine the minimum value of the target optimization function when the robot moves along the target trajectory under the constraints of motion information; The planning module is configured to generate motion planning information when the robot moves along the target trajectory according to the target motion information corresponding to the minimum value. A robot, characterized in that the robot includes a processor and a memory, and the memory stores a computer program, and when the computer program is executed by the processor, the method described in any one of claims 1 to 9 is realized. Robot motion information planning method. The robot according to claim 11, further comprising a communication unit. A computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot motion information described in any one of claims 1 to 9 is realized planning method.

Images