Robot excitation trajectory planning method and system based on weighting condition number of observation matrix

Abstract

The invention belongs to the field of robot kinetic parameter identification, and discloses a robot excitation trajectory planning method and system based on an observation matrix weighting condition number, and the method comprises the steps: building a robot kinetic parameter model under a linear friction model, and carrying out the linear parameterization processing, converting the model into a minimum parameter set form to obtain a base parameter and an observation matrix thereof; randomly generating enough values of joint positions, speeds and accelerations, substituting the values into the observation matrix, and stacking according to columns to obtain an observation matrix corresponding to a full motion range; performing singular value decomposition on the observation matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix and a right singular vector matrix; calculating a weight matrix of the excitation track observation matrix; and performing excitation trajectory planning by taking the conditional number of the product of the trajectory observation matrix and the weight matrix as a target function. According to the method, various robot configurations and joint limiting can be effectively dealt with, and the precision of torque prediction after parameter identification is improved.

Description

Technical Field

[0001] The present invention belongs to but is not limited to the technical field of robot dynamics parameter identification, and in particular relates to a robot excitation trajectory planning method and system based on observation matrix weighted condition number. Background Art

[0002] In the field of robot dynamic parameter identification, the planning of excitation trajectory is crucial for the identification of dynamic model parameters and the establishment of dynamic model. Existing methods mostly use trajectories in the form of finite Fourier series and use the minimization of the observation matrix condition number as the objective function. However, traditional methods have the following limitations:

[0003] Abnormal condition number of the observation matrix: Due to differences in the characteristics of robot joints, such as different joint reduction ratios, inconsistent dimensions of linear and rotational degrees of freedom joint angles, etc., the singular values ​​of the observation matrix may be unbalanced in distribution, resulting in a persistently high condition number and difficulty in convergence of the optimization, especially for robots with special configurations (such as robots with mixed joint types or strict joint limits).

[0004] Insufficient correlation between trajectory planning and torque prediction accuracy: Condition number optimization only reflects the numerical stability of parameter identification and is not directly related to the actual torque prediction error. High parameter identification accuracy does not equate to good model torque prediction performance, which is the core requirement of actual control. Traditional methods lack a unified framework from parameter identification to model verification, and cannot ensure the generalization ability of identified parameters.

[0005] The above problems limit the application of traditional incentive trajectory planning methods in complex robotics scenarios.

[0006] In view of the above analysis, the technical problems that need to be solved urgently in the prior art are:

[0007] There is an urgent need for a trajectory optimization strategy that can suppress condition number anomalies and directly relate to torque prediction accuracy. Summary of the invention

[0008] In view of the problems existing in the prior art, the present invention provides a robot excitation trajectory planning method and system based on the weighted condition number of the observation matrix.

[0009] The present invention is implemented as follows: a robot excitation trajectory planning method based on the weighted condition number of the observation matrix, characterized in that the robot excitation trajectory planning method based on the weighted condition number of the observation matrix specifically includes:

[0010] S1: Based on Newton-Euler method, Lagrange method, etc., the robot dynamic parameter model under the linear friction model is established, the parameters in the model are linearly parameterized, and the model is converted into the minimum parameter set form to obtain the basis parameters and its observation matrix;

[0011] S2: According to the range of joint position, velocity, and acceleration, randomly generate enough values ​​of joint position, velocity, and acceleration, substitute them into the measurement matrix and stack them by column to obtain the measurement matrix corresponding to the full range of motion;

[0012] S3: Perform singular value decomposition on the measurement matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix and a right singular vector matrix;

[0013] S4: Calculate the weight matrix of the excitation trajectory observation matrix according to the singular value matrix and the right singular vector matrix;

[0014] S5: The condition number of the product of the trajectory observation matrix and the weight matrix is ​​used as the objective function for incentive trajectory planning.

[0015] Furthermore, for the serial robot with n joints, the dynamic parameter model of S1 can be generalized as:

[0016]

[0017] in: represents the joint torque, Respectively represent the angle, angular velocity and angular acceleration of the robot joint, is the inertia matrix of the robot, represents the centrifugal force matrix, is the gravitational torque, is the friction torque. When the friction torque is a linear friction torque, the friction force of the joint k is usually expressed as:

[0018]

[0019] Among them, F r,k represents the friction force of joint k, F v,k represents the viscous friction parameter of joint k, F c,k Represents the Coulomb friction parameter of joint k. After the model is converted into the minimum parameter set form, it can be expressed as:

[0020]

[0021] in: represents the dynamic basis parameter, m represents the number of basis parameters, Represents the observation matrix corresponding to the basis parameters.

[0022] Furthermore, S2 randomly generates k (k needs to satisfy n·k>>m) groups of joint position, velocity and acceleration according to the joint position range, velocity and acceleration limits:

[0023]

[0024] where q i , and Respectively represent the generated joint position, velocity and acceleration of the i-th group, q max and q min Indicates the upper and lower limits of the joint position. and represents the joint velocity and acceleration limits, rand(x,y) represents the random number generation function, which generates random numbers in the interval [x,y] with an average probability;

[0025] Then substitute the k sets of joint position, velocity and acceleration data randomly generated above into Get k observation matrices Stacking them gives:

[0026]

[0027] in is the observation matrix corresponding to the full motion range, which reflects the general situation of the observation matrix of all trajectories of the robot within the feasible range.

[0028] Furthermore, the formula of S3 is as follows:

[0029]

[0030] in, represents the left singular vector matrix, represents the singular value matrix, represents the right singular vector matrix.

[0031] Further, in S4, firstly, the singular value matrix is ​​extracted The first m rows of the matrix

[0032] Σ a =[E m×m 0]·Σ

[0033] in is the identity matrix;

[0034] The weights are then calculated as follows:

[0035]

[0036] in This is the required weight matrix.

[0037] Furthermore, the S5 uses a finite Fourier series as the excitation trajectory, which is in the form of:

[0038]

[0039] where q i (t) represents the position of joint i at time t, ω f is the fundamental frequency of the Fourier series, L is the order of the finite Fourier series, and are respectively called the l-th order sine and cosine Fourier coefficients of joint i, q i,0 It is called the position bias term;

[0040] Then the Fourier coefficients of the sine and cosine terms of the above excitation trajectory and the position offset term are used as parameters The condition number of the product of the measurement matrix corresponding to the excitation trajectory and the weight matrix λ is used as the objective function to plan the excitation trajectory; the calculation method of the measurement matrix corresponding to the excitation trajectory is as follows:

[0041] First, the period T Divide by time interval dt:

[0042] t=0,dt,2dt,…,j·dt

[0043] in [·] represents the rounding function, and then the position, velocity and acceleration values ​​q(t) of all joints at each discrete time point are calculated. Substitution Stack by columns to get the measurement matrix corresponding to the excitation trajectory

[0044]

[0045] Then the observation matrix Y tarj The product of the weight matrix λ (the observation matrix Right multiply the weight matrix ) is used as the optimization objective function to stimulate trajectory planning:

[0046]

[0047] in represents the parameter to be optimized, cond(·) represents the function for finding the matrix condition number, q i (t), and They represent the position, velocity, and acceleration of joint i at time t, respectively. i,min q i,min and q i,max represents the lower and upper limits of the position of joint i, and They represent the velocity and acceleration limits of joint i respectively.

[0048] Another object of the present invention is to provide a robot excitation trajectory planning system based on the weighted condition number of the observation matrix, the system specifically comprising:

[0049] A robot dynamics parameter model building module is used to build a robot dynamics parameter model under a linear friction model;

[0050] The measurement matrix acquisition module is used to obtain the measurement matrix corresponding to the full motion range;

[0051] A singular value decomposition module is used to perform singular value decomposition on the observation matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix and a right singular vector matrix;

[0052] A weight matrix calculation module, used for calculating a weight matrix of an excitation trajectory observation matrix according to a singular value matrix and a right singular vector matrix;

[0053] The incentive trajectory planning module is used to perform incentive trajectory planning using the condition number of the product of the trajectory observation matrix and the weight matrix as the objective function.

[0054] In combination with the above technical solutions and the technical problems solved, the advantages and positive effects of the technical solutions to be protected by the present invention are as follows:

[0055] The present invention improves the objective function calculation method of incentive trajectory planning and can effectively cope with various robot configurations and joint limits.

[0056] The present invention provides a planning method for torque prediction accuracy directly oriented to a parameter model, which can effectively improve the accuracy of torque prediction after parameter identification.

[0057] The expected benefits and commercial value of the technical solution of the present invention after transformation are:

[0058] In view of the in-depth application of robots in more and more fields, the present invention has huge expected benefits and commercial value.

[0059] The technical solution of the present invention fills the technical gap in the industry at home and abroad:

[0060] The present invention fills the gap in the torque prediction accuracy of the parameter model that cannot be directly oriented to the current excitation trajectory planning. The excitation trajectory planned using the weighted method of the present invention has better torque prediction performance.

[0061] The technical solution of the present invention solves the technical problems that people have been eager to solve but have never been able to solve successfully:

[0062] The present invention solves the problem of observation matrix anomalies of robots with special configurations (such as robots that use both rotary and linear joints, robots that use both direct motor connections and large reduction ratio reducers, etc.) and robots with special joint limits (such as a small peak velocity of a joint). By weighting the observation matrix, the inherent singular value anomalies of the observation matrix are balanced, so that the excitation trajectory can be carried out normally. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 It is a schematic diagram of the overall process of robot excitation trajectory planning based on the weighted condition number of the observation matrix provided by the present invention;

[0064] Figure 2 It is a module diagram of a robot excitation trajectory planning system based on the observation matrix weighted condition number provided by the present invention;

[0065] Figure 3 This is the planning situation of two objective functions in the first joint limit situation provided by the present invention;

[0066] Figure 4 It is the planning situation of two objective functions in the second joint limit situation provided by the present invention;

[0067] Figure 5 is the distribution of the measurement matrix condition number and the measurement matrix weighted condition number of the ten excitation trajectories provided by the example of the present invention;

[0068] Figure 6 is the comprehensive error result of the cross-identification verification of the ten excitation trajectories provided in the example of the present invention;

[0069] Figure 7 It is a box plot of joint error distribution of the cross-identification verification of the ten excitation trajectories provided in the example of the present invention. DETAILED DESCRIPTION

[0070] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0071] like Figure 1 As shown, an embodiment of the present invention provides a robot excitation trajectory planning method based on the weighted condition number of the observation matrix, and the method specifically includes:

[0072] S1: Based on Newton-Euler method, Lagrange method, etc., the robot dynamic parameter model under the linear friction model is established, the parameters in the model are linearly parameterized, and the model is converted into the minimum parameter set form to obtain the basis parameters and its observation matrix;

[0073] S2: According to the range of joint position, velocity, and acceleration, randomly generate enough joint position, velocity, and acceleration values ​​and substitute them into the observation matrix And stack them by columns to get the measurement matrix Y corresponding to the full motion range full ;

[0074] S3: The measurement matrix Y corresponding to the full range of motion full Perform singular value decomposition to obtain the left singular vector matrix, the singular value matrix and the right singular vector matrix;

[0075] S4: Calculate the weight matrix λ of the excitation trajectory observation matrix according to the singular value matrix Σ and the right singular vector matrix V;

[0076] S5: The condition number of the product of the trajectory observation matrix and the weight matrix λ is used as the objective function for incentive trajectory planning.

[0077] For S1, for a serial robot with n joints, the dynamic parameter model can be generalized as:

[0078]

[0079] in: represents the joint torque, Respectively represent the angle, angular velocity and angular acceleration of the robot joint, is the inertia matrix of the robot, represents the centrifugal force matrix, is the gravitational torque, is the friction torque.

[0080] When the friction torque is a linear friction torque, the friction force of the joint k is usually expressed as:

[0081]

[0082] Among them, F r,k represents the friction force of joint k, F v,k represents the viscous friction parameter of joint k, F c,k represents the Coulomb friction parameter of joint k.

[0083] The model can be expressed as follows after being converted into the minimum parameter set form:

[0084]

[0085] in: Represents the dynamic basis parameters, there are m basis parameters in total, Represents the observation matrix corresponding to the basis parameters.

[0086] S2 randomly generates k (k must satisfy n·k>>m) groups of joint position, velocity and acceleration according to the joint position range, velocity and acceleration limits:

[0087]

[0088] where q i , and Respectively represent the generated joint position, velocity and acceleration of the i-th group, q max and q min Indicates the upper and lower limits of the joint position. and represents the joint velocity and acceleration limits, rand(x,y) represents the random number generation function, which generates random numbers in the interval [x,y] with an average probability.

[0089] Then substitute the k sets of joint position, velocity and acceleration data randomly generated above into Get k observation matrices Stacking them gives:

[0090]

[0091] in is the observation matrix corresponding to the full motion range, which reflects the general situation of the observation matrix of all trajectories of the robot within the feasible range.

[0092] The formula for S3 is as follows:

[0093]

[0094] in, represents the left singular vector matrix, represents the singular value matrix, represents the right singular vector matrix.

[0095] In S4, first, the singular value matrix is ​​extracted The first m rows of the matrix

[0096] Σ a =[E m×m 0]·Σ

[0097] in is the identity matrix.

[0098] The weights are then calculated as follows:

[0099]

[0100] in This is the required weight matrix.

[0101] The S5 uses a finite Fourier series as the excitation trajectory, which is in the form of:

[0102]

[0103] where q i (t) represents the position of joint i at time t, ω f is the fundamental frequency of the Fourier series, L is the order of the finite Fourier series, a l i and b l i are respectively called the l-th order sine and cosine Fourier coefficients of joint i, q i,0 It is called the position bias term.

[0104] Then the Fourier coefficients of the sine and cosine terms of the above excitation trajectory and the position offset term are used as parameters The excitation trajectory is planned using the condition number of the product of the observation matrix and the weight matrix λ corresponding to the excitation trajectory as the objective function.

[0105] The calculation method of the observation matrix corresponding to the excitation trajectory is as follows:

[0106] First, the period T Divide by time interval dt:

[0107] t=0,dt,2dt,…,j·dt

[0108] in [·] represents the rounding function, and then the position, velocity and acceleration values ​​q(t) of all joints at each discrete time point are calculated. Substitution Stack by columns to get the measurement matrix corresponding to the excitation trajectory

[0109]

[0110] Then the observation matrix Y tarj The product of the weight matrix λ (the observation matrix Right multiply the weight matrix ) is used as the optimization objective function to stimulate trajectory planning:

[0111]

[0112] in represents the parameter to be optimized, cond(·) represents the function for finding the matrix condition number, q i (t), and represent the position, velocity, and acceleration values of joint i at time t, respectively, where q i,min q i,min and q i,max represent the lower and upper limits of the position of joint i, and represent the velocity and acceleration limits of joint i, respectively.

[0113] As Figure 2 shown, a robot excitation trajectory planning system based on the weighted condition number of the observation matrix provided by an embodiment of the present invention specifically includes:

[0114] A robot dynamic parameter model construction module for establishing a robot dynamic parameter model under a linear friction model;

[0115] An observation matrix acquisition module for obtaining an observation matrix corresponding to the full motion range;

[0116] A singular value decomposition module for performing singular value decomposition on the observation matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix, and a right singular vector matrix;

[0117] A weight matrix calculation module for calculating a weight matrix of the excitation trajectory observation matrix according to the singular value matrix and the right singular vector matrix;

[0118] An excitation trajectory planning module for performing excitation trajectory planning with the condition number of the product of the trajectory observation matrix and the weight matrix as the objective function.

[0119] An embodiment of the present invention provides an excitation trajectory planning example and a parameter identification example for a collaborative robot (model number HSRCo610-1400) to illustrate the usage examples and effects of the method proposed by the present invention. A further description is given below with reference to the accompanying drawings.

[0120] S1. Based on the Newton-Euler method, the Lagrange method, etc., establish a dynamic parameter model of the HSRCo610-1400 robot under a linear friction model, perform linear parameterization on the parameters in the model, and transform the model into the form of a minimum parameter set to obtain the base parameters and their observation matrix.

[0121] S2. According to the ranges of joint position, velocity, and acceleration, randomly generate 1000 sets of joint position, velocity, and acceleration values, substitute them into the observation matrix and stack them by columns to obtain an observation matrix Y corresponding to the full motion range full .

[0122] Use two ranges of joint position, velocity, and acceleration respectively. The first is as shown in the following table:

[0123]

[0124] The second type is based on the first type, only the sixth axis is changed as follows: the speed limit is reduced to 0.1rad / s, and the acceleration limit is reduced to 0.2rad 2 / s, and other parameters remain the same. Compared with the first case, the condition number of the second case has inherent singular value anomalies due to its inherent speed limitation.

[0125] The condition number cond(Y full1 )=80.94, in the second case, cond(Y full2 )=971.6414.

[0126] S3, matrix Y full (Y full1 and Y full2 ) performs singular value decomposition to obtain the left singular vector matrix, the singular value matrix and the right singular vector matrix.

[0127] S4. Calculate the weight matrix λ of the excitation trajectory observation matrix based on the singular value matrix Σ and the right singular vector matrix V. The weight matrices corresponding to the ranges of the two joint positions, velocities, and accelerations are λ 1 and λ 2 .

[0128] S5. The condition number of the product of the trajectory observation matrix and the weight matrix λ is used as the objective function for incentive trajectory planning. The particle swarm optimization algorithm (PSO) is selected as the optimization method, and the penalty function method is used to deal with constraints.

[0129] For the first case, the traditional observation matrix condition number cond(Y tarj ) as the objective function and the weighted observation matrix condition number cond(Y tarj ·λ 1 ) as the optimization effect of the objective function Figure 3 shown.

[0130] For the second case, the traditional observation matrix condition number cond(Y tarj ) as the objective function and the weighted observation matrix condition number cond(Y tarj ·λ 2 ) as the optimization effect of the objective function Figure 4 shown.

[0131] By comparing the planning effects of the excitation trajectory in these two cases, it is found that when the weighted measurement matrix condition number is used as the objective function, the excitation trajectory can be effectively planned without the objective function being too large. In particular, for the second case, the measurement matrix condition number cond(Y tarj ) is always greater than 1000, indicating that the optimization of the excitation trajectory is ineffective. The method proposed in this paper using the weighted measurement matrix condition number as the objective function can effectively balance the inherent imbalance of the measurement matrix singular value size and ensure the effective implementation of the optimization.

[0132] Furthermore, in order to verify the planning effect of the torque prediction accuracy of the present invention directly facing the parameter model, an identification experiment is carried out in the first case (under the first range of joint position, velocity, and acceleration), respectively using the observation matrix condition number cond(Y tarj ) and the weighted observation matrix condition number cond(Y tarj ·λ 1 ) is used as the objective function for multiple groups of excitation trajectories, and cross-parameter identification is performed to verify whether the torque prediction accuracy is improved.

[0133] The observation matrix condition number cond(Y tarj ) as the objective function to plan five trajectories, named: N-1, N-2, N-3, N-4 and N-5, with the weighted observation matrix condition number cond(Y tarj ·λ 1 ) as the objective function to plan five trajectories, named: W-1, W-2, W-3, W-4 and W-5, calculate their condition numbers (referring to the condition number of the observation matrix corresponding to the trajectory, the same below) and weighted condition numbers (referring to the condition number of the matrix obtained by multiplying the observation matrix corresponding to the trajectory by the weight matrix, the same below) and list them in the following table:

[0134]

[0135]

[0136] The condition numbers and weighted condition number distributions of these ten trajectories are shown in Figure 5 Among them, the N-1~5 trajectories have lower condition numbers, while W-1~5 have lower weighted condition numbers. By verifying the torque prediction performance of the parameters identified by these two types of trajectories, the torque prediction performance using weighted condition numbers and traditional condition numbers as objective functions can be compared.

[0137] Use these ten trajectories to perform parameter identification and calculate the comprehensive error on each of the ten trajectories. Use the weighted least squares method for identification, and the error is calculated according to the following formula:

[0138]

[0139] Among them, τ i,actu represents the actual torque of joint i, τ i,pred represents the predicted force data of joint i, ||·|| represents the binary norm calculation function, ξ i represents the error of joint i, and ξ represents the comprehensive error of six joints.

[0140] The combined cross-validation errors for these ten trajectories are listed in Figure 6 In the , the trajectory using weighted condition number as the objective function obtained an average error of 12.87%, on the contrary, the trajectory with condition number as the objective function had an average error of 14.98%.

[0141] At the same time, the error ξ of different joints i Compare and divide according to whether its training trajectory is W-1~5 or N-1~5, and calculate ξ i Box plots of the error distribution at different joints are shown in Figure 7 For each joint, the trajectory using the weighted condition number as the objective function (W-1~5) has a lower error.

[0142] This shows that although the condition number of W-1~5 is large (the weighted condition number is low), its prediction performance is better than that of the trajectory with low condition number (high weighted condition number) (N-1~5). It can be inferred that the weighted condition number index is more correlated with the prediction performance, and the trajectory planned using the weighted condition number as the objective function has better moment prediction performance.

[0143] The present invention solves the problem that when the traditional method is used for excitation trajectory planning, the structural anomaly of the observation matrix condition number may be too large to effectively reduce the optimization. The specific description is as follows:

[0144] When the reduction ratios of the reducers are different, or the speed range of a joint is too small, the condition number of the calculated trajectory observation matrix will be abnormal due to the large contrast in the size of the singular values, resulting in an excessively large condition number.

[0145] This method uses k groups (n·k>>m) of joint angle positions, velocities, and accelerations randomly generated within the feasible range of the joint to calculate Y full , which represents the observation matrix corresponding to all possible trajectories, and the weight λ satisfies: cond(Y full ·λ)=1:

[0146]

[0147] Therefore, the weight λ realizes the normalization of the singular values ​​of the observation matrix in the feasible space of all joints. Therefore, the use of this weighted method can effectively avoid the inherent singular value anomaly of the robot and realize the effective planning of the excitation trajectory.

[0148] The present invention realizes a planning method for the torque prediction accuracy directly facing the parameter model, which effectively improves the torque prediction accuracy of the dynamic model. The details are as follows:

[0149] When solving the linear equation system Ax=b, when the observed value b changes by δb, the parameter to be identified x will change by δx, which satisfies the following relationship:

[0150]

[0151] Therefore, the condition number reflects the upper limit of the change δx of the observed value x when the observed value b changes δb. However, when considering the accuracy of the parameter prediction model rather than the accuracy of the parameters, it is necessary to establish a unified framework of parameter identification and model prediction:

[0152]

[0153] in represents the parameter identification process, where τ tarj represents the torque of the excitation trajectory for identification, represents the observation matrix of the excitation trajectory for identification, and χ represents the basis parameters of identification; Represents the model accuracy verification process, where τ full represents the joint torques for any feasible verification trajectory, Represents the set of observation matrices corresponding to any feasible trajectory, that is, the observation matrix corresponding to the full motion range mentioned above.

[0154] So the torque prediction accuracy error The upper limit of can be expressed as:

[0155]

[0156] When considering the torque prediction accuracy directly, cond(Y full )·cond(Y tarj ) as the objective function, the traditional method only uses cond(Y tarj ) as the optimization objective, so it cannot directly ensure the torque prediction accuracy of the model.

[0157] When the weighting method proposed in the present invention is used, a unified framework of parameter identification and model prediction is established as follows:

[0158]

[0159] Where λ is the weighting matrix solved by the method in this paper. So the torque prediction accuracy error The upper limit of can be expressed as:

[0160]

[0161] Since cond(Y full λ)=1, so only cond(Y tarj λ) can fully ensure cond(Y full λ)·cond(Y tarj λ) is directly based on the condition number cond(Y tarj λ) is used as the objective function to perform excitation trajectory planning, which can realize a planning method that directly targets the torque prediction accuracy of the parameter model.

[0162] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. It can be understood by a person of ordinary skill in the art that the above-mentioned devices and methods can be implemented using computer executable instructions and / or contained in a processor control code, such as a carrier medium such as a disk, CD or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. Such code is provided on the carrier medium. The device and its modules of the present invention can be implemented by hardware circuits such as very large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, and can also be implemented by a combination of the above-mentioned hardware circuits and software, such as firmware.

[0163] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principle of the present invention should be covered by the protection scope of the present invention.

Claims

1. A robot excitation trajectory planning method based on the weighted condition number of the observation matrix, characterized in that: The method specifically includes: S1: Based on Newton-Euler method, Lagrange method, etc., the robot dynamic parameter model under the linear friction model is established, the parameters in the model are linearly parameterized, and the model is converted into the minimum parameter set form to obtain the basis parameters and its observation matrix; S2: According to the range of joint position, velocity, and acceleration, randomly generate enough values ​​of joint position, velocity, and acceleration, substitute them into the measurement matrix and stack them by column to obtain the measurement matrix corresponding to the full range of motion; S3: Perform singular value decomposition on the measurement matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix and a right singular vector matrix; S4: Calculate the weight matrix of the excitation trajectory observation matrix according to the singular value matrix and the right singular vector matrix; S5: The condition number of the product of the trajectory observation matrix and the weight matrix is ​​used as the objective function for incentive trajectory planning.

2. The robot excitation trajectory planning method based on the observation matrix weighted condition number as claimed in claim 1, characterized in that: For S1, for a serial robot with n joints, the dynamic parameter model can be generalized as: in: represents the joint torque, Respectively represent the angle, angular velocity and angular acceleration of the robot joint, is the inertia matrix of the robot, represents the centrifugal force matrix, is the gravitational torque, is the friction torque. When the friction torque is a linear friction torque, the friction force of the joint k is usually expressed as: Among them, F r,k represents the friction force of joint k, F v,k represents the viscous friction parameter of joint k, F c,k Represents the Coulomb friction parameter of joint k. After the model is converted into the minimum parameter set form, it can be expressed as: in: Represents the dynamic basis parameters, there are m basis parameters in total, Represents the observation matrix corresponding to the basis parameters.

3. The robot excitation trajectory planning method based on the observation matrix weighted condition number as claimed in claim 1, characterized in that: S2 randomly generates k (k must satisfy n·k>>m) groups of joint position, velocity and acceleration according to the joint position range, velocity and acceleration limits: where q i , and Respectively represent the generated joint position, velocity and acceleration of the i-th group, q max and q min Indicates the upper and lower limits of the joint position. and represents the joint velocity and acceleration limits, rand(x,y) represents the random number generation function, which generates random numbers in the interval [x,y] with an average probability; Then substitute the k sets of joint position, velocity and acceleration data randomly generated above into Get k observation matrices Stacking them gives: in is the observation matrix corresponding to the full motion range, which reflects the general situation of the observation matrix of all trajectories of the robot within the feasible range.

4. The robot excitation trajectory planning method based on the observation matrix weighted condition number as claimed in claim 1, characterized in that: The formula for S3 is as follows: in, represents the left singular vector matrix, represents the singular value matrix, represents the right singular vector matrix.

5. The robot excitation trajectory planning method based on the observation matrix weighted condition number as claimed in claim 1, characterized in that: In S4, first, the singular value matrix is ​​extracted The first m rows of the matrix S a =[E m×m 0]·S in is the identity matrix; The weights are then calculated as follows: in This is the required weight matrix.

6. The robot excitation trajectory planning method based on the observation matrix weighted condition number as claimed in claim 1, characterized in that: The S5 uses a finite Fourier series as the excitation trajectory, which is in the form of: where q i (t) represents the position of joint i at time t, ω f is the fundamental frequency of the Fourier series, L is the order of the finite Fourier series, and are respectively called the l-th order sine and cosine Fourier coefficients of joint i, q i,0 It is called the position bias term; Then the Fourier coefficients of the sine and cosine terms of the above excitation trajectory and the position offset term are used as parameters The condition number of the product of the measurement matrix corresponding to the excitation trajectory and the weight matrix λ is used as the objective function to plan the excitation trajectory; the calculation method of the measurement matrix corresponding to the excitation trajectory is as follows: First, the period T Divide by time interval dt: t=0,dt,2dt,…,j·dt in [·] represents the rounding function, and then the position, velocity and acceleration values ​​q(t) of all joints at each discrete time point are calculated. Substitution Stack by columns to get the measurement matrix corresponding to the excitation trajectory Then the observation matrix Y tarj The product of the weight matrix λ (the observation matrix Right multiply the weight matrix ) is used as the optimization objective function to stimulate trajectory planning: in represents the parameter to be optimized, cond(·) represents the function for finding the matrix condition number, q i (t), and They represent the position, velocity, and acceleration of joint i at time t, respectively. i,min q i,min and q i,max represents the lower and upper limits of the position of joint i, and They represent the velocity and acceleration limits of joint i respectively.

7. A robot excitation trajectory planning system based on the observation matrix weighted condition number as described in claims 1-6, characterized in that: The system specifically includes: A robot dynamics parameter model building module is used to build a robot dynamics parameter model under a linear friction model; The measurement matrix acquisition module is used to obtain the measurement matrix corresponding to the full motion range; A singular value decomposition module is used to perform singular value decomposition on the observation matrix corresponding to the full motion range to obtain a left singular vector matrix, a singular value matrix and a right singular vector matrix; A weight matrix calculation module, used for calculating a weight matrix of an excitation trajectory observation matrix according to a singular value matrix and a right singular vector matrix; The incentive trajectory planning module is used to perform incentive trajectory planning using the condition number of the product of the trajectory observation matrix and the weight matrix as the objective function.

Images