Robot joint excitation trajectory generation method and robot joint dynamics parameter identification method

Abstract

The application provides a robot joint excitation trajectory generation method and a dynamics parameter identification method. When the excitation trajectory is generated, under the joint motion constraint, the parameter sensitivity of the joint driving torque with respect to each to-be-identified parameter is determined according to the joint motion state sequence of the candidate excitation trajectory, and a parameter information matrix is established; an evaluation value is formed according to the determinant of the sum of the parameter information matrix and a regularization matrix and the condition number of the parameter information matrix, the trajectory parameters are iteratively updated according to the evaluation value, and a target excitation trajectory is output. When the parameter identification is performed, the target excitation trajectory is executed, and joint motion state data and measured driving torque data are collected; the global joint estimation of the rigid body dynamics parameters and the friction parameters is performed; the sampling data weight is determined according to the initial estimation residual, and the local joint update is performed, and the parameter identification result is output. The application improves the joint driving torque prediction accuracy and the joint driving torque prediction accuracy.

Description

Technical Field

[0001] This invention relates to the field of robot dynamics parameter identification technology, specifically to a method for generating robot joint excitation trajectories and a method for identifying robot joint dynamics parameters. Background Technology

[0002] Robot dynamics models describe the relationship between the motion states of robot joints and their driving torques, forming the basis for functions such as robot trajectory control, gravity compensation, collision detection, drag teaching, and force control. To obtain the parameters in the dynamics model, robot joints are typically controlled to execute pre-designed excitation trajectories, collecting data such as joint angles, angular velocities, angular accelerations, and driving torques. The collected data is then used to identify the robot's dynamic parameters. Existing methods for identifying robot dynamic parameters usually reorganize the rigid body dynamics model into a form linearly related to inertial parameters or a minimal set of dynamic parameters, and then evaluate and design excitation trajectories based on the observation matrix corresponding to this linearized model.

[0003] In particular, robot joints exhibit nonlinear frictional characteristics during startup and low-speed movement, representing the difference between static friction and Coulomb friction. To describe these nonlinear frictional characteristics, existing technologies incorporate the Stribeck nonlinear friction model into the robot joint dynamics model. However, existing technologies typically process rigid body dynamic parameters and nonlinear friction parameters in the dynamics model separately. For example, they first design the excitation trajectory and identify the rigid body dynamic parameters based on a linearized rigid body dynamics model, and then further estimate the nonlinear friction parameters based on the difference between the measured joint driving torque and the torque calculated by the rigid body dynamics model.

[0004] However, because the nonlinear friction parameters are not considered together with the rigid body dynamics parameters as trajectory evaluation objects during the excitation trajectory design and generation stage, the designed excitation trajectory is mainly used to excite rigid body dynamics parameters such as inertia and gravity, making it difficult to provide sufficient identification information for the nonlinear friction parameters, which mainly have a significant impact in low-speed transition states. Furthermore, the torque difference values ​​subsequently used to identify the nonlinear friction parameters not only include the actual friction torque but may also be mixed with identification errors, model errors, and measurement noise from the previous stage of rigid body dynamics parameter identification. These errors may be further absorbed into the identification results of the nonlinear friction parameters, causing the errors of rigid body dynamics parameters and nonlinear friction parameters to mutually propagate, leading to instability in the identification results of some parameters and affecting the accuracy of the joint driving torque prediction by the dynamic model during robot startup and low-speed motion. Summary of the Invention

[0005] The applicant discovered that when evaluating candidate excitation trajectories using a fixed observation matrix based on a linearized rigid body dynamics model, rigid body dynamics parameters such as inertial parameters and mass moments can typically participate in the calculation of joint driving torque in a linear form. After the candidate excitation trajectory is determined, a fixed observation matrix can be established based on the corresponding joint angles, joint angular velocities, and joint angular accelerations, independent of the values ​​of the corresponding parameters to be identified, and the candidate excitation trajectory can be evaluated accordingly. In contrast, at least some nonlinear friction parameters participate in the calculation of joint driving torque in a nonlinear manner. For example, the Stribeck characteristic velocity lies within an exponential function. After incorporating these nonlinear friction parameters and rigid body dynamics parameters into the dynamics model, it is difficult to further rearrange the dynamics model into a linear form of joint driving torque obtained by multiplying the fixed observation matrix by the parameter vector to be identified; the parameter sensitivity of the joint driving torque relative to these nonlinear friction parameters may also change with the joint motion state and the values ​​of the corresponding parameters. Therefore, it is difficult to directly use the traditional fixed linear observation matrix to evaluate candidate excitation trajectories.

[0006] Based on this, the first objective of the present invention is to improve the coverage of identification information of nonlinear friction parameters that participate in the calculation of joint driving torque in a nonlinear manner in the data generated by the existing excitation trajectory, so that the data obtained after the robot executes the target excitation trajectory can more fully reflect the influence of nonlinear friction parameters on joint driving torque.

[0007] The applicant further discovered that increasing the number of parameters was not the primary challenge after using both rigid body dynamics parameters and nonlinear friction parameters as identification targets for the excitation trajectory. More importantly, the two types of parameters correspond to different physical effects, and the joint motion states required to significantly influence the joint driving torque are also different. For example, inertial parameters mainly have a significant impact when joint angular acceleration changes, while nonlinear friction parameters such as the Stribeck characteristic velocity mainly have a significant impact during startup and low-speed transition. Therefore, the data generated by the same candidate excitation trajectory may not reflect the effects of each parameter to be identified to the same extent. The parameter sensitivity formed by some parameters under the corresponding candidate excitation trajectory may be weak, and the changes in joint driving torque caused by changes in different parameters may be quite similar. If the candidate excitation trajectory cannot take into account the different requirements of different parameters for the joint motion state, even with increased data volume or the use of more complex parameter estimation methods, problems such as insufficient identification information for some parameters, difficulty in distinguishing the effects of different parameters, and susceptibility of identification results to measurement errors may still occur.

[0008] Therefore, the second objective of this invention is to improve the effectiveness and stability of the data collected from the same target excitation trajectory for the joint identification of rigid body dynamic parameters and nonlinear friction parameters, reduce identification instability caused by insufficient identification information of some parameters or difficulty in distinguishing the effects of different parameters, and help improve the accuracy of joint driving torque prediction of the dynamic model in robot startup and low-speed motion states.

[0009] To achieve the above objectives, the present invention adopts the following technical solution: A first aspect of the present invention provides a method for generating a robot joint excitation trajectory, comprising: under robot joint motion constraints, iteratively updating trajectory parameters based on evaluation values ​​until a preset termination condition is met, and determining the trajectory parameter with the minimum evaluation value from the iteration process, and outputting a target excitation trajectory based on the trajectory parameter; wherein the evaluation value is generated in each trajectory parameter update process through the following steps: determining a joint motion state sequence corresponding to a candidate excitation trajectory based on the current trajectory parameters; the trajectory parameters are parameters to be optimized in a parameterized expression representing the change law of joint angles over time of the candidate excitation trajectory; the joint motion state sequence includes joint angles, joint angular velocities, and joint angular accelerations of the candidate excitation trajectory at multiple sampling moments; determining the sensitivity of the joint driving torque output by the dynamic model of the robot joint to be identified relative to each parameter to be identified in the dynamic model at the multiple sampling moments based on the dynamic model of the robot joint to be identified and the joint motion state sequence; the dynamic model includes a rigid body dynamics part and a friction part, the friction part including a nonlinear friction term; the parameters to be identified include rigid body dynamic parameters corresponding to the rigid body dynamics part and friction parameters corresponding to the friction part, and so on. The friction parameters include nonlinear friction parameters that participate in the calculation of the joint driving torque in a nonlinear manner; a parameter information matrix is ​​established based on the parameter sensitivity of each parameter to be identified at the multiple sampling times; the elements in the parameter information matrix corresponding to any two parameters to be identified are obtained by multiplying and summing the parameter sensitivity of the two parameters to be identified at the multiple sampling times; a first evaluation result is obtained based on the determinant of the sum of the parameter information matrix and the regularization matrix, and a second evaluation result is obtained based on the condition number of the parameter information matrix; the regularization matrix is ​​of the same order as the parameter information matrix, and each diagonal of the regularization matrix... The elements are all positive preset regularization values, and the rest are zero; a monotonically decreasing mapping is performed on the first evaluation result to obtain a first evaluation value, and a monotonically increasing mapping is performed on the second evaluation result to obtain a second evaluation value, so that both the first evaluation value and the second evaluation value represent the optimization of the candidate excitation trajectory in the direction of decreasing value, and the difference in the range of numerical change between the first evaluation value and the second evaluation value is reduced; an evaluation value corresponding to the current trajectory parameter is formed based on the first evaluation value and the second evaluation value, and the smaller the evaluation value, the better the candidate excitation trajectory corresponding to the current trajectory parameter.

[0010] By adopting the above technical solution, this invention, based on a dynamic model that simultaneously includes rigid body dynamics and friction components, determines the parameter sensitivity of the joint driving torque output by the dynamic model relative to each parameter to be identified at multiple sampling times of the candidate excitation trajectory. The parameter sensitivity reflects the degree of influence of changes in the parameters to be identified on the joint driving torque output by the dynamic model under the corresponding joint motion state. Therefore, even if some nonlinear friction parameters in the friction component participate in the calculation of the joint driving torque in a nonlinear manner and cannot form a fixed linear observation matrix determined solely by the joint motion state together with the rigid body dynamic parameters, these nonlinear friction parameters can still enter the evaluation process of the candidate excitation trajectory through the parameter sensitivity of the joint driving torque relative to itself. This ensures that the generation of the excitation trajectory no longer solely serves the identification of rigid body dynamic parameters.

[0011] Furthermore, this invention establishes a parameter information matrix based on the parameter sensitivity of each parameter to be identified at multiple sampling times. The elements in the parameter information matrix corresponding to any two parameters to be identified are obtained by multiplying and summing the parameter sensitivity of those two parameters at multiple sampling times. Therefore, this parameter information matrix not only summarizes the overall intensity of the influence of each parameter to be identified on the joint driving torque along the entire candidate excitation trajectory, but also reflects the correlation formed when the parameter sensitivity of different parameters to be identified changes with the candidate excitation trajectory. Since the parameter information matrix corresponds to both rigid body dynamics parameters and friction parameters, whether the candidate excitation trajectory contains a joint motion state that effectively reflects the action of nonlinear friction parameters, and whether different parameters to be identified form easily distinguishable torque change patterns under this trajectory, can all be reflected in this parameter information matrix. Therefore, evaluating and updating trajectory parameters based on the parameter information matrix helps to ensure that the changes in joint angles, joint angular velocities, and joint angular accelerations contained in the target excitation trajectory simultaneously meet the identification requirements of rigid body dynamics parameters and nonlinear friction parameters, improving the coverage of identification information for nonlinear friction parameters in subsequent data acquisition.

[0012] Meanwhile, the physical effects corresponding to rigid body dynamics parameters and friction parameters differ, and the joint motion states required to generate significant torque effects also differ. For example, inertial parameters typically generate significant torque effects when joint angular acceleration changes, while some nonlinear friction parameters mainly generate significant torque effects during startup and low-speed transitions. Therefore, under the same candidate excitation trajectory, there may be both parameter directions with strong identification information and those with weak identification information, or there may be situations where the sensitivity change patterns of different parameters are quite similar. If only the overall identification information formed by all the parameters to be identified is emphasized, the evaluation results may be mainly affected by the parameter directions with strong identification information, leaving some friction parameters unexcited; if only the information differences between different parameter directions are emphasized, candidate excitation trajectories with relatively balanced different parameter directions but insufficient overall identification information may be obtained.

[0013] Therefore, this invention obtains a first evaluation result based on the determinant of the sum of the parameter information matrix and the regularization matrix, and a second evaluation result based on the condition number of the parameter information matrix. The first evaluation result can reflect the overall scale of identification information formed by all the parameters to be identified, which helps to avoid insufficient overall identification information provided by the target excitation trajectory. The second evaluation result can reflect the degree of imbalance of identification information in different parameter directions of the parameter information matrix, which helps to reduce numerical ill-conditioning caused by some parameter directions being too strong and others being too weak, or by the inability to distinguish the effects of different parameters. Thus, by evaluating the candidate excitation trajectory together with the first and second evaluation results, the overall scale of identification information can be improved while taking into account the distribution of identification information in different parameter directions, reducing the possibility that the evaluation result is overly dominated by a few strong information parameter directions.

[0014] Furthermore, the regularization matrix and the parameter information matrix are of the same order and have the same diagonal elements. Therefore, even when the candidate excitation trajectory provides insufficient excitation to some of the parameters to be identified, or when the parameter information matrix is ​​close to singular, it can still reduce the risk of evaluation failure caused by a zero determinant or an excessively small value, and improve the numerical stability of the first evaluation result during the trajectory iteration process. This regularization process does not change the fundamental relationship of using the parameter information matrix to evaluate the identification information of each parameter to be identified, but rather ensures that candidate excitation trajectories of different qualities can all obtain a first evaluation result that can be used for comparison, thereby helping to maintain the continuity and stability of the trajectory parameter iteration update process.

[0015] Furthermore, a larger determinant of the parameter information matrix generally indicates a larger overall scale of identification information, while a smaller condition number generally indicates a more balanced identification information across different parameter directions. These two factors exhibit different directions of advantage and disadvantage, and varying numerical ranges. Therefore, this invention applies a monotonically decreasing mapping to the first evaluation result and a monotonically increasing mapping to the second evaluation result. This ensures that both the first and second evaluation quantities represent the optimization of the candidate excitation trajectory in a decreasing direction, and reduces the difference in their numerical ranges. This avoids the two evaluation results from being unable to participate in trajectory evaluation due to opposite directions of advantage and disadvantage, and reduces the possibility that one evaluation result, due to its larger numerical range, might mask the other. This ensures that both the overall scale of identification information and the degree of imbalance in identification information across different parameter directions can effectively influence the evaluation value corresponding to the current trajectory parameters.

[0016] Therefore, this invention, under the constraint of robot joint motion, updates trajectory parameters based on the evaluation values ​​regenerated in each iteration, and determines the trajectory parameters with the minimum evaluation values ​​during the iteration process. This ensures that the target excitation trajectory conforms to the robot's joint motion range and execution capability, while also taking into account the overall identification information requirements of rigid body dynamic parameters and friction parameters, as well as the distinguishability of the effects of different parameters. When the robot subsequently executes the target excitation trajectory, the same batch of joint motion state data and driving torque data collected can more fully reflect the influence of rigid body dynamic parameters and nonlinear friction parameters that participate in the calculation of joint driving torque in a nonlinear manner. This helps to reduce the risk of identification instability caused by insufficient excitation of some parameters, difficulty in distinguishing the effects of different parameters, or ill-conditioned information matrix values, and also helps to improve the accuracy of joint driving torque prediction by the dynamic model in robot startup and low-speed motion states.

[0017] In at least one embodiment of the robot joint excitation trajectory generation method provided by the present invention, preferably, the rigid body dynamics part includes an inertial term and a gravity term, the friction part includes a Coulomb friction term, a Stribeck nonlinear friction term characterizing the difference between static friction and Coulomb friction, and a viscous friction term; the rigid body dynamics parameters include rotational inertia parameters, mass moment parameters, and gravity offset angle, the friction parameters include Coulomb friction parameters, the difference between static friction parameters and Coulomb friction parameters (which can be positive or negative), Stribeck characteristic velocity, and viscous friction parameters, wherein the Stribeck characteristic velocity participates in the calculation of the joint driving torque in a nonlinear manner.

[0018] By adopting the above technical solution, the joint driving torque is described by inertia, gravity and different friction terms, and the corresponding parameters are used as the parameters to be identified. This allows the target excitation trajectory to simultaneously meet the identification requirements of inertia, gravity and nonlinear friction characteristics, and improves its applicability to robot start-up and low-speed transition states.

[0019] In at least one embodiment of the robot joint excitation trajectory generation method provided by the present invention, preferably, a finite-term Fourier series is used as the parameterized expression of the candidate excitation trajectory, and the sine coefficients, cosine coefficients, and constant bias terms in the finite-term Fourier series are used as the trajectory parameters; by analytically differentiating the finite-term Fourier series, the joint angular velocity and joint angular acceleration corresponding to the candidate excitation trajectory are obtained; the robot joint motion constraints include joint angle constraints, joint angular velocity constraints, joint angular acceleration constraints, and trajectory period boundary continuity constraints; the trajectory period boundary continuity constraints include that the joint angle, joint angular velocity, and joint angular acceleration at the start and end times of the trajectory period are equal.

[0020] By adopting the above technical solution, the optimization of a continuous trajectory is transformed into the optimization of a finite number of trajectory parameters through a finite number of Fourier series, and consistent joint angles, joint angular velocities, and joint angular accelerations are obtained through analytical differentiation. Combined with robot joint motion constraints and periodic boundary continuity constraints, the target excitation trajectory can maintain smooth periodic transitions while satisfying the robot's execution capabilities.

[0021] In at least one embodiment of the robot joint excitation trajectory generation method provided by the present invention, preferably, the partial derivative of the joint driving torque output by the dynamic model with respect to each of the parameters to be identified is used as the parameter sensitivity of the corresponding parameter to be identified at the corresponding sampling time; the parameter sensitivity of each parameter to be identified at the plurality of sampling times is arranged with the sampling time as the row and the parameter to be identified as the column to form a parameter sensitivity matrix; the parameter information matrix is ​​obtained by multiplying the transpose of the parameter sensitivity matrix with the parameter sensitivity matrix, and the parameter information matrix is ​​the Fisher information matrix corresponding to each of the parameters to be identified.

[0022] After adopting the above technical solution, the influence of each parameter on the joint driving torque is quantitatively represented by the partial derivatives, and the Fisher information matrix is ​​established by using the parameter sensitivity matrix. The identification information intensity of each parameter and the correlation between parameters can be summarized into the same matrix, providing a clear computational basis for evaluating candidate excitation trajectories using determinants and condition numbers.

[0023] In at least one embodiment of the robot joint excitation trajectory generation method provided by the present invention, preferably, the negative logarithm of the first evaluation result is taken to obtain the first evaluation quantity; the positive logarithm of the second evaluation result is taken to obtain the second evaluation quantity; and the first evaluation quantity and the second evaluation quantity are added to obtain the evaluation value corresponding to the current trajectory parameters.

[0024] By adopting the above technical solution, negative logarithmic processing can transform the goal of increasing the overall identification information scale into the goal of reducing the first evaluation quantity, while positive logarithmic processing can maintain the optimization direction of reducing the condition number and compress the numerical variation range of the two indicators. Adding the two together yields a unified evaluation value that can be directly compared in the trajectory optimization process.

[0025] In at least one embodiment of the robot joint excitation trajectory generation method provided by the present invention, preferably, a particle swarm optimization algorithm is used to iteratively update the trajectory parameters based on the evaluation value, including: initializing multiple sets of trajectory parameters and the parameter update amount corresponding to each set of trajectory parameters, and treating each set of trajectory parameters as the current position of a particle; in each iteration, updating the individual historical best trajectory parameters corresponding to each particle according to the evaluation value corresponding to the current position of each particle, and determining the global optimal trajectory parameters according to the individual historical best trajectory parameters of each particle; for each particle, subtracting the current position of the particle from the corresponding individual historical best trajectory parameters to obtain the first... A first parameter difference is obtained by subtracting the current position of the particle from the global optimal trajectory parameter; a second parameter difference is obtained by determining the parameter update amount of the particle in the current round based on the parameter update amount of the particle in the previous round, the first parameter difference, the second parameter difference, and random coefficients corresponding to the first parameter difference and the second parameter difference, respectively; the current position of the corresponding particle is updated based on the parameter update amount of the current round, and the updated current position is used as the trajectory parameter in the next round of iteration; the preset termination condition includes at least one of reaching a preset maximum number of iterations and the change in the optimal evaluation value obtained from multiple consecutive iterations being less than a preset convergence threshold.

[0026] After adopting the above technical solution, each particle can adjust its trajectory parameters based on its own historical best results and the global best results of the particle swarm, and maintain the diversity of the search by using random coefficients, thereby continuously searching for candidate solutions with smaller evaluation values ​​within the trajectory parameter space. By stopping the optimization by setting a preset number of iterations or convergence conditions, both the trajectory optimization effect and the computational cost can be balanced.

[0027] A second aspect of the present invention provides a method for identifying robot joint dynamic parameters, comprising: controlling a robot joint to be identified to execute a target excitation trajectory; collecting joint motion state data and measured driving torque data of the robot joint at multiple sampling times; the target excitation trajectory is obtained by any of the aforementioned robot joint excitation trajectory generation methods; the joint motion state data includes joint angle, joint angular velocity, and joint angular acceleration; determining the parameter search range of each parameter to be identified based on the physical value range of each parameter to be identified in the dynamic model of the robot joint; the dynamic model is a dynamic model used to determine parameter sensitivity when generating the target excitation trajectory; the dynamic model includes a rigid body dynamics part and a friction part; the friction part includes a nonlinear friction term; the parameters to be identified include rigid body dynamics parameters corresponding to the rigid body dynamics part and friction parameters corresponding to the friction part; determining multiple sets of parameter candidate values ​​within the parameter search range; each set of parameter candidate values ​​includes candidate values ​​corresponding to each rigid body dynamics parameter and each friction parameter. The joint motion state data and each set of candidate parameter values ​​are input into the dynamic model to obtain the predicted driving torque data corresponding to each set of candidate parameter values. Based on the residuals between the measured driving torque data and the predicted driving torque data corresponding to each set of candidate parameter values ​​at multiple sampling times, scalar residual evaluation results are formed for each set of candidate parameter values. With the goal of reducing the scalar residual evaluation results, the candidate parameter values ​​corresponding to the rigid body dynamic parameters and the friction parameters are globally jointly updated within the parameter search range to obtain the initial parameter estimates for each parameter to be identified. Based on the residuals at each sampling time corresponding to the initial parameter estimates, the sampling data weights used to reduce the influence of abnormal residuals on parameter solving are determined. Using the initial parameter estimates as the initial values ​​for parameter updates, a weighted residual evaluation result is formed based on the sampling data weights. With the goal of reducing the weighted residual evaluation results, the rigid body dynamic parameters and the friction parameters are locally jointly updated until the parameter identification termination condition is met, and the identification results for each parameter to be identified are output.

[0028] By adopting the above technical solution, this invention performs global joint initial estimation and local joint fine estimation of rigid body dynamic parameters and friction parameters using the same complete dynamic model. This avoids solving the rigid body dynamic parameters and friction parameters separately in stages, thereby reducing the risk of parameter errors from the previous stage being passed to the next stage. By first performing a global joint update to obtain the initial parameter estimates, and then determining the sampling data weights based on the residuals and performing a local joint update, the dependence of nonlinear parameter solutions on initial values ​​and the impact of abnormal sampling data on the final identification results can be reduced simultaneously, improving the stability of the complete parameter joint identification.

[0029] In at least one embodiment of the robot joint dynamics parameter identification method provided by the present invention, preferably, the rigid body dynamics part includes an inertial term and a gravity term, the friction part includes a Coulomb friction term, a Stribeck nonlinear friction term characterizing the difference between static friction and Coulomb friction, and a viscous friction term; the rigid body dynamics parameters include rotational inertia parameters, mass moment parameters, and gravity offset angles; the friction parameters include Coulomb friction parameters, the difference between static friction parameters and Coulomb friction parameters (which can be positive or negative), Stribeck characteristic velocities, and viscous friction parameters; the predicted driving torque is jointly determined by the inertial term, the gravity term, the Coulomb friction term, the Stribeck nonlinear friction term, and the viscous friction term; the residual is the difference between the measured driving torque and the predicted driving torque at the corresponding sampling time.

[0030] By adopting the above technical solution, the inertial torque, gravitational torque, and frictional torque are calculated together through the complete model, and all parameters are updated with the residual between the measured total driving torque and the predicted total driving torque. This allows the rigid body dynamic parameters and friction parameters to jointly interpret the measured data, reducing the risk that the rigid body dynamic model error is fixed and absorbed into the friction parameters.

[0031] In at least one embodiment of the robot joint dynamics parameter identification method provided by the present invention, preferably, before forming the scalar residual evaluation results corresponding to each set of parameter candidate values, the measured driving torque data is subjected to zero-phase low-pass filtering, and sampled data whose joint angular velocity is within a preset zero-velocity dead zone is removed. The filtered and unremoved measured driving torque data and their corresponding joint motion state data are used as parameter identification data. The preset zero-velocity dead zone is used to remove sampled data near zero velocity where the direction of joint angular velocity is difficult to determine stably. Based on the sum of squared residuals between the measured driving torque data and the predicted driving torque data at each sampling time in the parameter identification data, each set of parameters is formed. The scalar residual evaluation result corresponding to the candidate value; the residual scale is determined according to the residual at each sampling time corresponding to the initial parameter estimate, and the sampling data weight of the corresponding sampling data is determined according to the magnitude of the residual at each sampling time relative to the residual scale; the sampling data with residuals within a preset range has a first weight, the sampling data with residuals exceeding the preset range has a sampling data weight less than the first weight, and the sampling data weight decreases as the corresponding residual increases relative to the residual scale; the residuals at each sampling time are weighted according to the sampling data weight of each sampling data, and the weighted residual evaluation result is formed according to the weighted residuals at each sampling time.

[0032] By adopting the above technical solutions, zero-phase low-pass filtering can reduce data phase misalignment while suppressing high-frequency noise, and eliminating data with unstable velocity direction in the zero-velocity dead zone can reduce the error in friction direction judgment. Reducing the weight of data corresponding to larger residuals based on the residual scale can reduce the impact of abnormal measurements or instantaneous shocks on local parameter updates, thereby improving the robustness of parameter identification.

[0033] In at least one embodiment of the robot joint dynamics parameter identification method provided by the present invention, preferably, a particle swarm optimization algorithm is used to perform a global joint update on the rigid body dynamics parameters and the friction parameters with the goal of reducing the sum of squared residuals between the measured driving torque data and the predicted driving torque data corresponding to the parameter identification data, thereby obtaining the initial estimated values ​​of the parameters; based on the residuals corresponding to the initial estimated values ​​of the parameters, the residual scale is determined according to the median absolute deviation, and a Huber threshold is determined based on the residual scale; the Huber weights corresponding to the sampled data whose residual absolute values ​​do not exceed the Huber threshold are determined as the first weights, and the Huber weights corresponding to the sampled data whose residual absolute values ​​exceed the Huber threshold are determined based on the ratio between the Huber threshold and the residual absolute values; using the initial estimated values ​​of the parameters as initial values, the Huber weighted nonlinear least squares problem is solved using the trust region reflection algorithm, and the local joint update is performed on the rigid body dynamics parameters and the friction parameters to obtain the identification results of each of the parameters to be identified.

[0034] After adopting the above technical solutions, particle swarm optimization can obtain a more reasonable and complete initial parameter estimate within the parameter search range, reducing the sensitivity of local solutions to the initial value; the median absolute deviation and Huber weights can reduce the impact of outlier data according to the actual residual level; and the trust region reflection algorithm can further reduce the weighted residual near the initial parameter estimate, thereby taking into account global search capability, local solution accuracy and robustness to outlier data. Attached Figure Description

[0035] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments are briefly introduced. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0036] Figure 1 This is a flowchart of robot joint target excitation trajectory generation and dynamic parameter identification according to an embodiment of the present invention; Figure 2 The joint angle, joint angular velocity, and joint angular acceleration curves of the robot joint target excitation trajectory generated for embodiments of the present invention; Figure 3 This is a comparison chart of the traces of the covariance matrix of parameter estimation when using dual-criteria fusion optimization and single-condition number optimization in an embodiment of the present invention; Figure 4 This is a graph of the joint driving torque before filtering, collected in an embodiment of the present invention. Figure 5 This is a graph of the joint driving torque after zero-phase low-pass filtering according to an embodiment of the present invention; Figure 6 This is a fitting curve of the measured joint driving torque and the model predicted joint driving torque in an embodiment of the present invention; Figure 7 This is a curve showing the measured friction force and the fitting curve of the Stribeck friction model in an embodiment of the present invention. Detailed Implementation

[0037] The specific embodiments of this application will be described in detail below with reference to the accompanying drawings. The following embodiments are used to illustrate the actual implementation process of the technical solution of this application and are not intended to limit the scope of protection of this application.

[0038] Robot joints typically consist of a motor, reducer, transmission components, and load. During joint movement, the driving torque is used not only to overcome equivalent inertia and gravity, but also to overcome friction generated by the reducer and transmission components. Especially during startup, reversal, and low-speed operation, friction exhibits a significant nonlinear change with speed. If only a linear combination of Coulomb friction and viscous friction is used, friction can easily compensate for inertia or gravity, thus affecting the accuracy of dynamic parameter identification.

[0039] The motion trajectory used for dynamic parameter identification directly affects the parameter information contained in the collected data. Insufficient angular variation in the trajectory makes it difficult to distinguish between mass moment and gravitational offset angle; insufficient angular acceleration variation makes it difficult to fully demonstrate the equivalent moment of inertia; and the lack of stable motion segments and low-speed transition segments can easily lead to correlations between Coulomb friction, Stribeck characteristic velocity, and viscous friction parameters. Therefore, before conducting the identification experiment, it is necessary to first generate a target excitation trajectory that satisfies the robot's motion constraints and sufficiently excites each parameter to be identified.

[0040] The robot joint dynamics parameter identification process in this embodiment includes two interconnected stages. The first stage is the generation of the target excitation trajectory. First, the excitation degree of candidate trajectories on all parameters to be identified is evaluated based on the joint dynamics model. Then, the target excitation trajectory is obtained through iterative optimization. The second stage is the parameter identification and solution process. The joint to be identified is controlled to execute the target excitation trajectory. Joint motion state and driving torque data are collected, and data preprocessing, global joint initial estimation, and local joint fine estimation are performed sequentially. Finally, the dynamic parameters of the robot joint are output.

[0041] The execution order of the above two stages and their sub-steps is as follows: Figure 1 As shown. Figure 1 In this process, S100 includes S110 to S150, and S200 includes S210 to S230; the target excitation trajectory output by S150 serves as the execution input for S210.

[0042] The two processes described above are explained below.

[0043] S100, Generate the target excitation trajectory.

[0044] In this embodiment, the target excitation trajectory is generated iteratively through evaluation values ​​under the constraints of robot joint motion. Each iteration forms candidate excitation trajectories and corresponding motion state sequences based on the current trajectory parameters, and calculates the evaluation value. Upon reaching the termination condition, the trajectory parameter with the smallest evaluation value is selected and the target excitation trajectory is output. The specific process is described below according to the order in which the evaluation values ​​are formed.

[0045] This process is completed before the actual identification experiment and includes establishing a dynamic model of the joint to be identified, generating candidate excitation trajectories, calculating the parameter information corresponding to the candidate trajectories, forming trajectory evaluation values, and iteratively updating the trajectory parameters.

[0046] S110, Establish the dynamic model of the robot joint to be identified.

[0047] The dynamic model of the robot joint to be identified includes a rigid body dynamics component and a friction component. The rigid body dynamics parameters and friction parameters together constitute the parameters to be identified; among them, the friction component includes nonlinear friction terms that participate in the calculation of joint driving torque in a nonlinear manner.

[0048] Furthermore, the rigid body dynamics component includes inertial and gravitational terms, and the friction component includes Coulomb friction terms, Stribeck nonlinear friction terms characterizing the difference between static and Coulomb friction, and viscous friction terms. The rigid body dynamics parameters include rotational inertia parameters, mass moment parameters, and gravitational offset angles. The friction parameters include Coulomb friction parameters, the difference between static and Coulomb friction parameters (which can be positive or negative), Stribeck characteristic velocities, and viscous friction parameters. The Stribeck characteristic velocity participates in the calculation of the joint driving torque in a nonlinear manner. This embodiment uses the following single-joint equivalent dynamic model for illustration.

[0049] This embodiment uses a single-joint equivalent dynamic model to describe the joint to be identified. The rigid body dynamics part of this model includes inertial and gravitational terms, while the friction part includes Coulomb friction terms, Stribeck nonlinear friction terms characterizing the difference between static and Coulomb friction, and viscous friction terms. At any sampling time... The predicted joint driving torque output by the model is:

[0050] In the formula, , and In order, they are joint angle, joint angular velocity, and joint angular acceleration; The equivalent rotational inertia on the joint side; For mass moment; It is the acceleration due to gravity; This is the gravitational offset angle; The parameters are Coulomb friction parameters; These are static friction parameters; This is the difference between the static friction parameter and the Coulomb friction parameter; Stribeck characteristic velocity; These are viscous friction parameters. The parameters to be identified are uniformly arranged as follows:

[0051] With the above parameter vector, the inertia term, gravity term, and the three types of friction terms are all calculated using the same set of parameters. Equivalent moment of inertia. It is mainly reflected through the change of angular acceleration, mass moment and gravity offset angle This is primarily reflected by the change in gravitational torque at different joint angles. Coulomb friction parameters. It is mainly reflected through the stable motion segments in the positive and negative directions, the difference. and Stribeck characteristic velocity The viscous friction parameters are mainly reflected during the start-up and low-speed transition phases. This is mainly reflected through the higher speed range and the speed change range.

[0052] in, Located inside the exponential function. When the joint angular velocity is... When they are on the same order of magnitude, the Stribeck nonlinear friction term follows Changes have occurred. Therefore, The calculation of joint driving torque in a nonlinear manner also requires that the candidate trajectory include motion states that can reflect low-speed friction changes.

[0053] S120 generates candidate excitation trajectories and their joint motion state sequences.

[0054] For the current trajectory parameters, candidate excitation trajectories and their joint motion state sequences at multiple sampling times are determined. The trajectory parameters are used to characterize the change of joint angles of the candidate excitation trajectories over time; the joint motion state sequence includes joint angles, joint angular velocities, and joint angular accelerations.

[0055] Furthermore, a finite-term Fourier series is used as the parameterized expression of the candidate excitation trajectory, and the sine coefficients, cosine coefficients, and constant bias terms in the finite-term Fourier series are used as the trajectory parameters. By analytically differentiating the finite-term Fourier series, the joint angular velocity and joint angular acceleration corresponding to the candidate excitation trajectory are obtained. The robot joint motion constraints include joint angle constraints, joint angular velocity constraints, joint angular acceleration constraints, and trajectory period boundary continuity constraints. The expression, differentiation, and constraint checking process of the candidate trajectory are explained in detail below.

[0056] In this embodiment, a finite-term Fourier series is used to represent the candidate excitation trajectory. Constant bias term. Coefficients of each sine term and the coefficients of each cosine term As trajectory parameters that need to be optimized; For the first Item frequency, This represents the number of harmonic terms. The angular offset used to represent the trajectory itself, to distinguish it from the gravity offset angle in the dynamic model. The joint angles of the candidate trajectory are:

[0057] Analytically differentiating the above joint angle expression over time yields the joint angular velocity and joint angular acceleration of the candidate trajectory at each sampling time:

[0058]

[0059] Therefore, each set of trajectory parameters can be converted into joint angles, joint angular velocities, and joint angular accelerations at multiple sampling times, forming a sequence of joint motion states corresponding to the candidate excitation trajectory. This state sequence is used both to check whether the trajectory can be safely executed by the robot joints and as input for subsequent calculations of parameter sensitivity.

[0060] Before evaluating candidate trajectories, it is necessary to check whether the joint angles, joint angular velocities, and joint angular accelerations exceed the allowable range of the joint to be identified at all sampling times.

[0061] At the same time, the trajectory cycle also needs to be checked. Are the start and end states continuous?

[0062] When each frequency is matched with the trajectory period, the Fourier basis functions can ensure a natural connection between the start and end states of the period. For general frequency settings, the above relationships are still verified item by item. If a candidate trajectory exceeds any motion constraint, it can be judged as infeasible and excluded from the update of the historical best trajectory; alternatively, a penalty term that increases with the exceedance of the limit can be added to the evaluation value. Regardless of the processing method used, the final output target excitation trajectory must satisfy the constraints of joint angle, joint angular velocity, joint angular acceleration, and period boundary continuity.

[0063] S130: Calculate parameter sensitivity based on candidate trajectories and establish a parameter information matrix.

[0064] At each sampling time, the partial derivative of the joint driving torque with respect to each parameter to be identified is used as the corresponding parameter sensitivity; the parameter sensitivity at multiple sampling times is arranged with the sampling time as the row and the parameter to be identified as the column to form a parameter sensitivity matrix.

[0065] The product of the sensitivity values ​​of any two parameters to be identified is accumulated over multiple sampling times to obtain the corresponding elements in the parameter information matrix. This parameter information matrix is ​​obtained by multiplying the transpose of the parameter sensitivity matrix with the parameter sensitivity matrix, and forms the Fisher information matrix corresponding to each parameter to be identified. The following section explains the setting of the nominal parameters, the calculation of each partial derivative, and the formation process of the matrix.

[0066] Due to Stribeck characteristic velocity When used in a nonlinear manner in model calculations, the sensitivity depends not only on the motion state provided by the candidate trajectory but also on the parameter values ​​used in the calculation. To ensure that different candidate trajectories can be compared against the same benchmark before the experiment, a fixed set of nominal parameter vectors is set before trajectory optimization begins:

[0067] The nominal parameter vector can be determined based on previous identification values, mechanical design estimates, or initial values ​​within the physical range of the parameters for the same type of joint. During the same trajectory optimization process, all candidate trajectories use the same set of nominal parameters, thus ensuring the comparability of evaluation results obtained from different candidate trajectories.

[0068] In the At each sampling time, the candidate trajectory is given. , and The nominal parameters are then substituted into the dynamic model to calculate the partial derivatives of the predicted joint driving torque with respect to the seven parameters to be identified. The partial derivatives are as follows:

[0069]

[0070]

[0071] exist The sign function is not directly calculated at =0. The derivative of . Trajectory generation and subsequent data processing avoid the zero-velocity neighborhood where the velocity direction is difficult to determine stably, while retaining the area outside the zero-velocity neighborhood where the absolute velocity value is close to . The low-speed state is used to obtain parameter information for the Stribeck transition section.

[0072] The first The seven partial derivatives corresponding to the sampling time point are arranged in the order of the parameters to be identified to form the parameter sensitivity matrix. Okay. Yes. After calculating at each sampling time, we obtain Parametric sensitivity matrix with 7 rows and 7 columns :

[0073] Subsequently, the products of the parameter sensitivities of any two parameters to be identified at each sampling time are summed to obtain the elements in the parameter information matrix corresponding to those two parameters. This parameter information matrix is ​​the Fisher information matrix corresponding to the seven parameters to be identified, and its calculation relationship is as follows:

[0074] The diagonal elements of the Fisher information matrix reflect the cumulative information intensity of the corresponding parameter across the entire candidate trajectory, while the off-diagonal elements reflect the correlation between the torque responses of different parameters. Therefore, this matrix can simultaneously represent the total amount of information provided by the candidate trajectory and whether different parameters are easily confused with each other.

[0075] S140, The evaluation value of the candidate excitation trajectory is formed based on the parameter information matrix.

[0076] The first evaluation result is obtained based on the determinant of the sum of the parameter information matrix and the regularization matrix of the same order, and the second evaluation result is obtained based on the condition number of the parameter information matrix. Each diagonal element of the regularization matrix is ​​the same positive preset regularization value, and the remaining elements are zero.

[0077] The first evaluation value is obtained by taking the negative logarithm of the first evaluation result; the second evaluation value is obtained by taking the positive logarithm of the second evaluation result; the first and second evaluation values ​​are added together to obtain the evaluation value corresponding to the current trajectory parameters. Both evaluation values ​​represent candidate excitation trajectory optimization by decreasing their values, and the specific relationship is as follows.

[0078] When the parameter information matrix is ​​close to singular, its determinant may be zero or too small. To improve computational stability, a seventh-order identity matrix is ​​used. With positive preset regularization quantity Construct a regularization matrix The first evaluation result is obtained based on the determinant of the sum of the parameter information matrix and the regularization matrix. The second evaluation result is obtained based on the condition number of the parameter information matrix. :

[0079] The larger the value, the greater the overall information content formed by the seven parameters; The smaller the value, the more balanced the information distribution across different parameter directions. To ensure that both evaluation results change in the direction where smaller values ​​indicate a better trajectory, and to reduce the difference in the numerical range of the two evaluation results, [the following is necessary:] ... Perform a negative logarithmic mapping, for Perform a positive logarithmic mapping, then add the two terms together:

[0080] Therefore, each set of current trajectory parameters corresponds to a scalar evaluation value. When both candidate trajectories satisfy motion constraints, the candidate trajectory with the smaller evaluation value provides more comprehensive and balanced parameter information. Regularization quantity The regularization scale remains unchanged during a single trajectory optimization process to prevent different candidate trajectories from losing comparability due to variations in the regularization scale.

[0081] The first and second evaluation results can also be mapped using other methods that maintain the monotonic direction described above. For example, they can be based on the candidate trajectories in the same round. and The maximum and minimum values ​​form a normalized evaluation metric:

[0082] Then use positive weights and Evaluation value is generated:

[0083] The above replacement method also reduces the first evaluation value when the total amount of information increases, and reduces the second evaluation value when the number of conditions decreases. This embodiment preferably uses the sum of the negative logarithm and the positive logarithm, eliminating the need to redetermine the maximum and minimum values ​​of the candidate set in each round of calculation.

[0084] S150, the trajectory parameters are iteratively updated based on the evaluation value to obtain the target excitation trajectory.

[0085] The particle swarm optimization algorithm is used to iteratively update trajectory parameters based on evaluation values. Multiple sets of trajectory parameters and their respective update values ​​are initialized to the current positions of multiple particles. In each iteration, the historical best trajectory parameters of each particle are updated according to the evaluation value corresponding to its current position, and the global best trajectory parameters are determined accordingly.

[0086] For each particle, the difference between its historical best trajectory parameters and its current position is used as the first parameter difference, and the difference between its global best trajectory parameters and its current position is used as the second parameter difference. The parameter update amount for the current round is determined based on the parameter update amount from the previous round, the first parameter difference, the second parameter difference, and the corresponding random coefficients, and the particle position is updated accordingly. The preset termination condition includes at least one of reaching a preset maximum number of iterations and the change in the optimal evaluation value obtained from multiple consecutive iterations being less than a preset convergence threshold. The specific execution method is explained below using the particle swarm update formula.

[0087] This embodiment uses a particle swarm optimization algorithm to update the Fourier trajectory parameters. Multiple sets of trajectory parameters and their update values ​​are initialized, and each set of trajectory parameters is treated as the current position of a particle. The corresponding parameter update amount is used as the particle velocity. After each candidate trajectory evaluation, the individual historical best position of each particle is updated based on its evaluation value. And determine the global optimal position from all individual historical optimal positions. .

[0088] In the In each iteration, the particle calculates the parameter update amount for the current round based on the parameter update amount from the previous round, the difference between its historical best position and its current position, and the difference between its global best position and its current position, and updates its current position accordingly:

[0089]

[0090] In the formula, Inertial weights; and The learning coefficient; and The values ​​are independently generated random numbers between 0 and 1. After each position update, the joint motion state sequence corresponding to that position is recalculated, motion constraints are checked, and the parameter sensitivity matrix, Fisher information matrix, and evaluation value are calculated sequentially.

[0091] The update stops when the number of iterations reaches the preset maximum number of iterations, or when the change in the optimal evaluation value obtained in multiple consecutive rounds is less than the preset convergence threshold. The trajectory parameters with the smallest evaluation value that satisfy the motion constraints throughout the entire iteration process are substituted into a Fourier series to output the target excitation trajectory that can be executed by the robot controller. This completes the target excitation trajectory generation process before the experiment.

[0092] Without altering the data relationships between the motion state sequence, parameter sensitivity, parameter information matrix, dual evaluation quantities, and trajectory parameter iterative updates, candidate trajectories can also employ other parameterizable and differentiable periodic expressions. For example, the control point coordinates of a periodic B-spline can be used as trajectory parameters, and angular velocity and angular acceleration can be obtained through the first and second derivatives of the spline; alternatively, a piecewise polynomial can be used, with continuous constraints on angle, angular velocity, and angular acceleration applied at the connection points between adjacent segments.

[0093] After obtaining the target excitation trajectory described above, the dynamic parameter identification process can begin.

[0094] S200 uses the target excitation trajectory to identify the dynamic parameters of robot joints.

[0095] Dynamic parameter identification includes data acquisition, global joint initial estimation, residual weighting, and local joint fine estimation. After the robot joints to be identified execute the target excitation trajectory, joint motion state data and measured driving torque data are collected at multiple sampling times. Within the parameter search range, the rigid body dynamic parameters and friction parameters are globally jointly updated to obtain initial parameter estimates. Then, based on the residuals, the weights of the sampled data are determined, and local joint updates are performed starting from the initial parameter estimates. After the parameter identification termination condition is met, the identification results of each parameter to be identified are output. The following sections describe these steps in the above order.

[0096] This process is executed after the target excitation trajectory is generated, including controlling joint movement and collecting data, synchronously preprocessing the collected data, performing a global joint initial estimation of seven parameters within the physical search range, and performing a local joint fine estimation based on the residual weights.

[0097] S210 controls the joint to be identified to execute the target excitation trajectory and generates parameter identification data.

[0098] The robot joints to be identified are controlled to execute the target excitation trajectory, and joint motion state data and measured driving torque data are collected at multiple sampling moments. The joint motion state data includes joint angle, joint angular velocity, and joint angular acceleration.

[0099] Before generating scalar residual evaluation results corresponding to each set of candidate parameter values, the measured driving torque data is subjected to zero-phase low-pass filtering, and sampled data whose joint angular velocity is within the preset zero-velocity dead zone are removed. The filtered and unremoved measured driving torque data and their corresponding joint motion state data are used as parameter identification data. The specific control command execution, motion state acquisition, driving torque conversion, and synchronization preprocessing process are as follows.

[0100] The robot controller converts the target excitation trajectory into target joint angles or target joint velocities for each control cycle. The servo driver drives the motor and reducer to move the joint to be identified according to the control commands. The encoder is used to collect joint angles; the joint angular velocity can be directly output by the encoder, or it can be obtained by filtering the difference between joint angles at adjacent times; the joint angular acceleration is obtained by filtering the difference between joint angular velocities.

[0101] The current detection unit collects motor current with the same timestamp as the data for each motion state, and calculates the measured driving torque on the joint side based on the motor torque coefficient, reduction ratio, and transmission efficiency.

[0102] In the formula, This is the motor torque coefficient. For the first Motor current at each sampling time, The reduction ratio, For reducer efficiency. At the same sampling time... , , and Together they form an observation sample.

[0103] Before solving for the parameters, the measured driving torque is subjected to zero-phase low-pass filtering. Zero-phase processing can be achieved by first performing forward filtering along the time direction using the same digital low-pass filter, and then processing the filtering result in reverse, thereby reducing the phase deviation between the state data and torque data caused by unidirectional filtering.

[0104] Subsequently, samples whose joint angular velocities fall within a preset zero-velocity dead zone are removed. When deleting velocity data at a specific moment, the joint angle, joint angular acceleration, and measured driving torque at that moment are simultaneously deleted to avoid misalignment between motion state and torque data. The zero-velocity dead zone is used to avoid the nearest zero-velocity interval where the velocity direction is difficult to determine stably; low-velocity samples outside the dead zone but close to the Stribeck characteristic velocity are still retained. Filtered and unremoved observation samples are used as parameter identification data.

[0105] S220 performs a global joint preliminary estimate of rigid body dynamic parameters and friction parameters within the parameter search range.

[0106] The parameter search range is determined based on the physical value range of the parameter to be identified, and multiple sets of candidate parameter values ​​are determined within this range. Each set of candidate parameter values ​​includes candidate values ​​for each rigid body dynamic parameter and each friction parameter.

[0107] The joint motion state data and candidate values ​​of each group of parameters are input into the dynamic model to obtain the predicted driving torque data corresponding to each group of candidate values. A scalar residual evaluation result is formed based on the residual between the measured and predicted driving torque data. With the goal of reducing the scalar residual evaluation result, the candidate values ​​of the rigid body dynamic parameters and friction parameters are globally and jointly updated to obtain the initial estimates of each parameter to be identified. The predicted driving torque is jointly determined by the inertial term, gravity term, Coulomb friction term, Stribeck nonlinear friction term, and viscous friction term. The residual is the difference between the measured and predicted driving torque at the same sampling time. The global update process can be implemented according to the following search boundary, predicted torque, and residual objective.

[0108] according to , , , , , and The physical feasible range is defined with upper and lower bounds for the parameters, and multiple sets of parameter candidate values ​​are generated within this seven-dimensional search space. Each set of parameter candidate values ​​simultaneously includes three rigid body dynamic parameters and four friction parameters.

[0109] For each set of candidate parameter values, compare them with the values ​​at each sampling time. , and The dynamic model established using common input S110 is used. The inertial, gravity, Coulomb friction, Stribeck nonlinear friction, and viscous friction terms are added together at the same sampling time to obtain the predicted driving torque corresponding to the candidate values ​​of this set of parameters. The difference between the measured driving torque and the predicted driving torque is:

[0110] The scalar residual evaluation results corresponding to the candidate parameter values ​​are obtained by summing the squared residuals at all valid sampling times:

[0111] Particle swarm optimization algorithm is used to reduce The objective is to simultaneously update the rigid body dynamics and friction parameters within the physical boundaries of seven parameters. These seven parameters vary together under the same residual objective and jointly interpret the same batch of measured driving torque data. After satisfying the global search termination condition, the initial estimates of the seven parameters and the corresponding pointwise residuals are output.

[0112] S230: Determine the weights of the sampled data based on the global initial residual estimate, and perform local joint fine estimation.

[0113] The residual scale is determined based on the residuals corresponding to the initial parameter estimates at each sampling time; the sampling data weights are determined based on the magnitude of each residual relative to the residual scale; the residuals at each sampling time are weighted using the sampling data weights, and the weighted residuals are used to form a weighted residual evaluation result.

[0114] In one specific implementation, the residual scale is determined based on the median absolute deviation of the residuals corresponding to the initial parameter estimates, and the Huber threshold is determined based on the residual scale. The Huber weights corresponding to the sampled data whose residual absolute values ​​do not exceed the Huber threshold are determined as the first weights, and the Huber weights corresponding to the sampled data whose residual absolute values ​​exceed the Huber threshold are determined based on the ratio between the Huber threshold and the residual absolute values. Using the initial parameter estimates as initial values, the Huber weighted nonlinear least squares problem is solved using the trust region reflection algorithm to locally and jointly update the rigid body dynamic parameters and friction parameters.

[0115] The pointwise residuals obtained from the global search may contain large residuals caused by measurement noise, transient shocks, or unmodeled disturbances. To reduce the impact of these residuals on local parameter updates, the median absolute deviation is used to estimate the residual scale. and according to Determine Huber threshold :

[0116] The weights of the sampled data are determined based on the relationship between the absolute value of the residual at each sampling time and the Huber threshold.

[0117] When the absolute value of the residual does not exceed the Huber threshold, the corresponding sample adopts the first weight of 1; when the absolute value of the residual exceeds the Huber threshold, the weight decreases according to the ratio of the threshold to the absolute value of the residual. Thus, samples within the normal range retain their original role, and samples corresponding to larger residuals still participate in the solution, but will not be over-dominated by the squared residuals in parameter updates.

[0118] Based on the weights of the above sampled data, a Huber-weighted nonlinear least squares objective is formed:

[0119] Using the parameter vector obtained from the global joint initial estimation as the initial point of the trust region reflection algorithm, the algorithm reduces the value within the same seven-dimensional physical boundary as the global search. The trust region reflection algorithm establishes a local approximation near the current parameters and adjusts the trust region radius according to the degree of agreement between the actual decrease in the objective function and the predicted decrease. When the local update approaches the parameter boundary, the parameters are kept within the physical boundary through the reflection mechanism.

[0120] In one embodiment, the MAD scale and Huber weights are calculated once based on the global initial residual estimate, and a local joint update is performed under this set of fixed weights. In another embodiment, the residuals, MAD scale, and Huber weights can be recalculated after each local update, forming an iterative reweighting process. When the change in the weighted objective function, the parameter update amount, or the number of iterations meets the preset termination condition, the final identification results of the seven parameters to be identified are output.

[0121] The weights for sampled data can also employ other continuously decaying functions that satisfy the condition that normal residuals have larger weights and abnormal residuals have smaller weights as the residuals increase. For example, the weights within a preset range can be set to 1, and a continuously bounded reciprocal weight can be used beyond that range. Regardless of the weighting function used, the turning point is determined based on the residual scale formed by the initial global residual estimate.

[0122] The following section uses specific data to illustrate the above process of generating the target excitation trajectory and solving the parameter identification.

[0123] This embodiment uses the sixth joint, the wrist joint, of a six-DOF force-controlled grinding collaborative arm as the object to be identified. Motor torque coefficient. Take 0.132 Reduction ratio Taking a ratio of 100:1, transmission efficiency Take 75%, gravitational acceleration Take 9.81 m / s 2 .

[0124] During the target excitation trajectory generation phase, candidate trajectories employ a 5-term finite Fourier series with frequencies ranging from 0.05 to 0.5 Hz. The constraints for joint angle, joint angular velocity, and joint angular acceleration are as follows: , and It is required that the three motion states at the start and end times of the trajectory period be equal.

[0125] To ensure that the sensitivity of the nonlinear parameters has a defined calculation benchmark, this embodiment uses the following seven-parameter nominal vector:

[0126] The aforementioned nominal vector remains unchanged throughout the evaluation of all candidate trajectories. Each set of Fourier coefficients is expanded into discrete samples. , and The sequence is then used to form a parameter sensitivity matrix based on seven partial derivatives. The regularization matrix is ​​taken as follows: ,in It is a positive number that remains unchanged during a single trajectory optimization process.

[0127] The particle swarm consists of 300 particles, with a maximum of 150 iterations. The inertia weight varies from 0.1 to 1.1, and the function tolerance is... .according to Evaluate each particle and obtain the target excitation trajectory. , . Figure 2 The curves showing the joint angles, joint angular velocities, and joint angular accelerations of the target excitation trajectory are displayed.

[0128] The target excitation trajectory is sent to the robot controller for execution, and data is continuously collected for 20 seconds at a sampling frequency of 100 Hz, with a sampling interval of 0.01 s. At each sampling moment, the joint angle and motor current are recorded, and the measured joint driving torque is obtained according to the aforementioned torque conversion formula.

[0129] The measured driving torque was subjected to a 4th-order Butterworth low-pass filter, first in the forward direction and then in the reverse direction, with a cutoff frequency of 10 Hz. Subsequently, synchronous filtering was performed. The motion state and torque samples yielded 1981 valid data points. Figure 4 and Figure 5 The joint driving torques before and after filtering are shown respectively.

[0130] The search boundaries for the seven parameters to be identified are set as follows: 0 to 2 , It ranges from 0 to 2 kg·m. 0 to 10 , -10 to 1 , The range is 0–10 rad / s. 0 to 10 , for- ~ rad.

[0131] A global joint search of seven parameters using the sum of squared residuals as the objective yields the initial parameter estimation vector:

[0132] Calculate 1981 residuals using this parameter vector, and obtain the results according to the MAD formula. The corresponding Huber threshold is:

[0133] For example, when the absolute value of the residual at a certain sampling time is 0.30 When the residual does not exceed the threshold, the weight is 1; when the absolute value of the residual is 1.20... When, the corresponding weight is:

[0134] Formed using the above weights Using the global joint initial estimate of the particle swarm optimization as the starting point, the trust region reflection algorithm is used to perform local joint updates within the above physical boundaries to obtain the final parameter vector:

[0135] According to this embodiment The parameters are defined so that the static friction parameters can be recovered from the final parameter vector:

[0136] Substitute the final parameters back into the complete dynamic model and calculate the predicted joint driving torque point by point. Figure 6 The fitted curves of the measured joint driving torque and the predicted joint driving torque are shown. Root mean square error. and coefficient of determination Calculate according to the following formulas respectively:

[0137]

[0138] Calculated , To observe the fitting of the friction model separately, the inertial and gravitational terms corresponding to the final parameters are subtracted from the measured total driving torque to obtain an approximation of the measured friction force:

[0139] Figure 7 The approximation of the measured friction force and the fitting relationship between the Stribeck friction model are shown. This embodiment also performed parameter estimation covariance matrix traces for 10 sets of experiments: when using a fusion of Fisher information matrix determinant and condition number for evaluation, the average value was 0.5192; when using only the condition number for evaluation, the average value was 0.7386, a reduction of 29.7% in the former case. The results are as follows... Figure 3 As shown.

[0140] The data above shows that the target excitation trajectory generation stage changes the Fourier trajectory parameters, while the parameter identification and solution stage changes the seven dynamic parameters. Although the optimization variables differ between the two stages, the same seven-parameter dynamic model is used, ensuring that the trajectory information evaluation before the experiment corresponds to the joint parameter identification after the experiment.

[0141] As another specific example of a candidate excitation trajectory, a third harmonic trajectory, which can be directly calculated, can be used. The frequency of the first harmonic... Take 0.10 Hz, Take 0.35 rad. Take 0.08 rad; the frequency of the second harmonic. Take 0.25 Hz, Take 0.12 rad, Take -0.06 rad; the frequency of the third harmonic. Take 0.40 Hz, Take 0.05 rad. Take 0.03 rad; constant bias term The value is set to 0.20 rad, and the trajectory period is set to 20 s.

[0142] Substituting the above values ​​into the Fourier formulas for angle, angular velocity, and angular acceleration, and utilizing the fact that the absolute value of trigonometric functions does not exceed 1, we can obtain the following conservative upper bounds:

[0143] Therefore, the triharmonic trajectory satisfies , and Motion constraints are defined as follows: Since all frequencies are integer multiples of 0.05 Hz, the joint angles, joint angular velocities, and joint angular accelerations are equal at the start and end of the 20 s period. This set of parameters can be used as an initial position in the particle swarm, or multiple sets of particle positions can be generated in its neighborhood, and the target excitation trajectory can be searched again according to the aforementioned Fisher information evaluation process.

[0144] The above values ​​are used to illustrate the specific implementation of the method in this application. When applying this method to other robot joints, the number of trajectory terms, motion constraints, sampling frequency, filter cutoff frequency, zero-velocity dead zone, parameter search boundary, and termination condition can be adjusted according to the joint's rated motion range, reducer parameters, sensor resolution, candidate trajectory frequency band, and the physical range of each parameter to be identified. At the same time, the data relationship between the dynamic model, trajectory state sequence, parameter sensitivity, parameter information matrix, dual evaluation quantities, global joint initial estimation, and weighted local joint fine estimation is maintained.

[0145] The above embodiments are used to illustrate the technical solution of this application. Those skilled in the art can make corresponding adjustments to the specific parameters, trajectory expression form, optimization algorithm implementation method, and data acquisition configuration without departing from the technical concept of this application.

Claims

1. A method for generating robot joint excitation trajectories, characterized in that, include: Under the constraints of robot joint motion, the trajectory parameters are iteratively updated based on the evaluation value until a preset termination condition is met, and the trajectory parameter with the minimum evaluation value is determined from the iteration process. The target excitation trajectory is then output based on the trajectory parameter. The evaluation value is generated during each trajectory parameter update process through the following steps: The joint motion state sequence corresponding to the candidate excitation trajectory is determined based on the current trajectory parameters; the trajectory parameters are the parameters to be optimized in the parameterized expression representing the change law of the joint angle of the candidate excitation trajectory over time; the joint motion state sequence includes the joint angle, joint angular velocity and joint angular acceleration of the candidate excitation trajectory at multiple sampling times; Based on the dynamic model of the robot joint to be identified and the sequence of joint motion states, the parameter sensitivity of the joint driving torque output by the dynamic model relative to each parameter to be identified in the dynamic model is determined at the multiple sampling times. The dynamic model includes a rigid body dynamics part and a friction part, and the friction part includes a nonlinear friction term. The parameters to be identified include the rigid body dynamics parameters corresponding to the rigid body dynamics part and the friction parameters corresponding to the friction part, and the friction parameters include nonlinear friction parameters that participate in the calculation of the joint driving torque in a nonlinear manner. A parameter information matrix is ​​established based on the parameter sensitivity of each parameter to be identified at the multiple sampling times; the element in the parameter information matrix corresponding to any two parameters to be identified is obtained by multiplying and summing the parameter sensitivity of the two parameters to be identified at the multiple sampling times. The first evaluation result is obtained based on the determinant of the sum of the parameter information matrix and the regularization matrix, and the second evaluation result is obtained based on the condition number of the parameter information matrix; the regularization matrix is ​​of the same order as the parameter information matrix, each diagonal element of the regularization matrix is ​​the same positive preset regularization amount, and the remaining elements are zero. A monotonically decreasing mapping is performed on the first evaluation result to obtain a first evaluation quantity, and a monotonically increasing mapping is performed on the second evaluation result to obtain a second evaluation quantity, so that both the first evaluation quantity and the second evaluation quantity represent the optimization of the candidate excitation trajectory in the direction of decreasing values, and the difference in the range of numerical changes between the first evaluation quantity and the second evaluation quantity is reduced. An evaluation value corresponding to the current trajectory parameters is formed based on the first evaluation value and the second evaluation value. The smaller the evaluation value, the better the candidate excitation trajectory corresponding to the current trajectory parameters.

2. The robot joint excitation trajectory generation method as described in claim 1, characterized in that, The rigid body dynamics part includes inertial terms and gravity terms, and the friction part includes Coulomb friction terms, Stribeck nonlinear friction terms characterizing the difference between static friction and Coulomb friction, and viscous friction terms. The rigid body dynamic parameters include rotational inertia parameters, mass moment parameters, and gravity offset angle. The friction parameters include Coulomb friction parameters, the difference between static friction parameters and Coulomb friction parameters, Stribeck characteristic velocity, and viscous friction parameters. The Stribeck characteristic velocity participates in the calculation of the joint driving torque in a nonlinear manner.

3. The robot joint excitation trajectory generation method as described in claim 1, characterized in that, A finite-term Fourier series is used as the parameterized expression of the candidate excitation trajectory, and the sine coefficients, cosine coefficients, and constant bias terms in the finite-term Fourier series are used as the trajectory parameters. By analytically differentiating the finite number of Fourier series, the joint angular velocity and joint angular acceleration corresponding to the candidate excitation trajectory are obtained; The robot joint motion constraints include joint angle constraints, joint angular velocity constraints, joint angular acceleration constraints, and trajectory period boundary continuity constraints; the trajectory period boundary continuity constraints include ensuring that the joint angle, joint angular velocity, and joint angular acceleration are equal at the start and end times of the trajectory period.

4. The robot joint excitation trajectory generation method as described in claim 1, characterized in that, The partial derivatives of the joint driving torque output by the dynamic model with respect to each of the parameters to be identified are used as the parameter sensitivity of the corresponding parameter to be identified at the corresponding sampling time. The parameter sensitivity of each parameter to be identified at the multiple sampling times is arranged in rows with the sampling time as the row and the parameter to be identified as the column to form a parameter sensitivity matrix; The parameter information matrix is ​​obtained by multiplying the transpose of the parameter sensitivity matrix with the parameter sensitivity matrix, and the parameter information matrix is ​​the Fisher information matrix corresponding to each of the parameters to be identified.

5. The robot joint excitation trajectory generation method as described in claim 1, characterized in that, Take the negative logarithm of the first evaluation result to obtain the first evaluation value; The second evaluation result is obtained by taking the positive logarithm of the second evaluation result; The first evaluation value is added to the second evaluation value to obtain the evaluation value corresponding to the current trajectory parameters.

6. The robot joint excitation trajectory generation method as described in claim 1, characterized in that, The trajectory parameters are iteratively updated based on the evaluation value using a particle swarm optimization algorithm, including: Initialize multiple sets of trajectory parameters and the parameter update amount corresponding to each set of trajectory parameters, and treat each set of trajectory parameters as the current position of a particle; In each iteration, the individual historical best trajectory parameters of each particle are updated according to the evaluation value corresponding to the current position of each particle, and the global best trajectory parameters are determined according to the individual historical best trajectory parameters of each particle. For each particle, the first parameter difference is obtained by subtracting the particle's current position from the corresponding individual historical best trajectory parameter, and the second parameter difference is obtained by subtracting the particle's current position from the global best trajectory parameter. The parameter update amount of the particle in the current round is determined based on the parameter update amount of the particle in the previous round, the first parameter difference, the second parameter difference, and the random coefficients corresponding to the first parameter difference and the second parameter difference, respectively. The current position of the corresponding particle is updated according to the parameter update amount of the current round, and the updated current position is used as the trajectory parameter in the next round of iteration; The preset termination condition includes at least one of the following: reaching a preset maximum number of iterations and the change in the optimal evaluation value obtained from multiple consecutive iterations being less than a preset convergence threshold.

7. A method for identifying robot joint dynamic parameters, characterized in that, include: Control the joints of the robot to be identified to execute the target excitation trajectory, and collect the joint motion state data and measured driving torque data of the robot joints at multiple sampling times; The target excitation trajectory is obtained by the robot joint excitation trajectory generation method according to any one of claims 1 to 6, and the joint motion state data includes joint angle, joint angular velocity and joint angular acceleration. Based on the physical value range of each parameter to be identified in the dynamic model of the robot joint to be identified, the parameter search range of each parameter to be identified is determined; the dynamic model is a dynamic model used to determine the parameter sensitivity when generating the target excitation trajectory, the dynamic model includes a rigid body dynamics part and a friction part, the friction part includes a nonlinear friction term, and the parameters to be identified include the rigid body dynamics parameters corresponding to the rigid body dynamics part and the friction parameters corresponding to the friction part; Within the parameter search range, multiple sets of parameter candidate values ​​are determined, and each set of parameter candidate values ​​includes candidate values ​​corresponding to each rigid body dynamics parameter and each friction parameter; The joint motion state data and each set of candidate parameter values ​​are respectively input into the dynamic model to obtain the predicted driving torque data corresponding to each set of candidate parameter values; Based on the residuals between the measured driving torque data at multiple sampling times and the predicted driving torque data corresponding to each group of parameter candidate values, scalar residual evaluation results are formed for each group of parameter candidate values. With the goal of reducing the scalar residual evaluation results, the parameter candidate values ​​corresponding to the rigid body dynamics parameters and the friction parameters are globally jointly updated within the parameter search range to obtain the initial parameter estimates for each of the parameters to be identified. Based on the residuals at each sampling time corresponding to the initial estimate of the parameters, determine the sampling data weights used to reduce the impact of abnormal residuals on parameter solving; Using the initial estimated value of the parameter as the initial value for parameter update, a weighted residual evaluation result is formed based on the weight of the sampled data. With the goal of reducing the weighted residual evaluation result, the rigid body dynamics parameter and the friction parameter are locally jointly updated until the parameter identification termination condition is met, and the identification result of each parameter to be identified is output.

8. The robot joint dynamics parameter identification method as described in claim 7, characterized in that, The rigid body dynamics part includes inertial terms and gravity terms, and the friction part includes Coulomb friction terms, Stribeck nonlinear friction terms characterizing the difference between static friction and Coulomb friction, and viscous friction terms. The rigid body dynamic parameters include rotational inertia parameters, mass moment parameters, and gravitational offset angle; the friction parameters include Coulomb friction parameters, the difference between static friction parameters and Coulomb friction parameters, Stribeck characteristic velocity, and viscous friction parameters. The predicted driving torque is determined by the inertia term, the gravity term, the Coulomb friction term, the Stribeck nonlinear friction term, and the viscous friction term. The residual is the difference between the measured driving torque and the predicted driving torque at the corresponding sampling time.

9. The robot joint dynamics parameter identification method as described in claim 7, characterized in that, Before forming the scalar residual evaluation results corresponding to each group of parameter candidate values, the measured driving torque data is subjected to zero-phase low-pass filtering, and the sampled data in which the joint angular velocity is in the preset zero-velocity dead zone is removed. The filtered and unremoved measured driving torque data and their corresponding joint motion state data are used as parameter identification data. The preset zero-velocity dead zone is used to remove sampled data near zero velocity, where the direction of joint angular velocity is difficult to determine stably; Based on the sum of squared residuals between the measured driving torque data and the predicted driving torque data at each sampling time in the parameter identification data, the scalar residual evaluation results corresponding to each group of parameter candidate values ​​are formed. The residual scale is determined based on the residuals at each sampling time corresponding to the initial estimated value of the parameter, and the sampling data weight is determined based on the magnitude of the residuals at each sampling time relative to the residual scale. Sampling data with residuals within a preset range have a first weight, and sampling data with residuals exceeding the preset range have a sampling data weight less than the first weight. The sampling data weight decreases as the corresponding residual increases relative to the residual scale. The residuals at each sampling time are weighted according to the sampling data weights of each sampled data, and the weighted residual evaluation result is formed based on the weighted residuals at each sampling time.

10. The robot joint dynamics parameter identification method as described in claim 9, characterized in that, The particle swarm optimization algorithm is used to reduce the sum of squared residuals between the measured driving torque data and the predicted driving torque data corresponding to the parameter identification data. The rigid body dynamic parameters and the friction parameters are then globally jointly updated to obtain the initial estimated values ​​of the parameters. Based on the residuals corresponding to the initial estimates of the parameters, the residual scale is determined according to the median absolute deviation, and the Huber threshold is determined based on the residual scale. The Huber weight corresponding to the sampled data whose absolute residual value does not exceed the Huber threshold is determined as the first weight, and the Huber weight corresponding to the sampled data whose absolute residual value exceeds the Huber threshold is determined according to the ratio between the Huber threshold and the absolute residual value. Using the initial estimated values ​​of the parameters as initial values, the Huber weighted nonlinear least squares problem is solved using the trust region reflection algorithm. The rigid body dynamic parameters and the friction parameters are then jointly updated locally to obtain the identification results of each of the parameters to be identified.

Images