Robot body kinetic model identification method under constraint of limited space

Abstract

The invention discloses a method for identifying a kinetic model of a robot body under the constraint of a limited space. The method comprises the following steps: establishing a kinetic model of a robot; selecting path points by taking limited Fourier series in a free space as a reference trajectory, and finishing point-to-point collision avoidance path planning by using an OMPL library; performing parameterization processing on collision avoidance path points by using a B spline curve under a speed-path decoupling (PVD) framework, and converting solution of an excitation track into optimization of a parameter speed; and finally, considering the physical constraint of the robot, taking the condition number of the regression matrix as a target function, and solving an excitation track by using a nonlinear optimization algorithm to complete the identification of the robot kinetic model. According to the method, the environmental constraints of the robot are creatively considered, the collision avoidance path is parameterized, the solution of the excitation trajectory is converted into a one-dimensional parameter planning problem, the multiple constraints of the robot are considered to complete the solution of the parameter trajectory, and the kinetic model identification of the robot in the limited space is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot dynamics model identification, and particularly relates to a method for identifying the dynamics model of a robot body under restricted space constraints. Background Art

[0002] In recent years, with the progress of technology and the loss of the demographic dividend, traditional manufacturing industries have faced challenges such as high labor costs, low efficiency, and poor working environments. The rapid development of new-generation information technologies has met the urgent needs of the manufacturing industry for automated and digital processing. Technologies such as artificial intelligence and robots are accelerating their integration into traditional manufacturing industries and are widely used in fields such as component processing and equipment manufacturing, becoming the core driving force for promoting the development of the manufacturing industry. An accurate dynamics model is an important prerequisite for achieving high-precision motion control of robots. At the same time, an accurate dynamics model can also be used in aspects such as robot trajectory planning, drag teaching, collision detection, and high-precision force control method design. Therefore, how to quickly and accurately identify the dynamics model of a robot under the condition of considering multiple constraints is an urgent problem to be solved.

[0003] In traditional identification methods, most practices when designing the excitation trajectory are to limit the position of the robot end-effector within a cuboid, cylinder, or spherical space and assume that there are no obstacles in this space, and then optimize the excitation trajectory based on finite Fourier series in the joint space. However, such simple constraint conditions often do not conform to the complex working environment of robots. In practical applications, there are more or less obstacles in the working space of robots, and these obstacles will cause the originally optimized excitation trajectory based on finite Fourier series to be unable to execute normally, resulting in the failure of the robot dynamics identification experiment. Therefore, under the condition of considering environmental constraints, designing a reasonable excitation trajectory to complete the identification of the parameters of the robot body dynamics model is a problem to be studied and solved. Summary of the Invention

[0004] Aiming at the deficiencies of the prior art, the present invention provides a method for identifying the dynamics model of a robot body under restricted space constraints, particularly a method for identifying the dynamics model of a robot body with optimal condition number under restricted space constraints. This method realizes the identification of the dynamics model of a robot under restricted space.

[0005] To achieve the above object, the present invention provides a method for identifying the dynamics model of a robot body under restricted space constraints, including the following steps:

[0006] S1. Establish the body dynamics model of the robot based on the Newton-Euler method;

[0007] S2. Under the condition that the environmental information is known, select path points with the finite Fourier series in free space as the reference trajectory, and use the OMPL library to complete the point-to-point collision avoidance path planning to obtain collision avoidance path points;

[0008] S3. Interpolate the path points using a cubic B-spline curve under the velocity-path decoupling (PVD) framework, parameterize the trajectory, and transform the optimization of the excitation trajectory into the optimization of the parameter velocity;

[0009] S4. Considering the constraints of the robot joint angle, angular velocity, and angular acceleration, use the condition number of the regression matrix as the objective function, and use the nonlinear optimization algorithm to complete the solution of the excitation trajectory and complete the identification of the robot dynamics model.

[0010] Preferably, in step S1, establish the body dynamics model of the robot based on the Newton-Euler method, which specifically includes the following steps:

[0011] S1-1. Use the MDH method to establish the coordinate systems of each link of the robot, establish the kinematic model of the robot, and the homogeneous transformation matrix from coordinate system i-1 to coordinate system i i-1 T i is:

[0012]

[0013] where, α i-1 、a i-1 、θ i 、d i are the DH parameters of the robot;

[0014] S1-2. Based on the Newton-Euler method, establish the body dynamics model of the robot and establish the Coulomb-viscous friction force model. The formula is as follows:

[0015]

[0016] where, M(q) is the robot inertia matrix, is the centrifugal force and Coriolis force matrix, G(q) is the gravity matrix, is the friction force matrix, sgn(·) is the sign function;

[0017] S1-3. Linearize the robot dynamics model and find the minimum inertia parameter set of the robot dynamics model. The formula is as follows:

[0018]

[0019] where, represents the minimum inertia parameter set of the robot dynamics model, is the corresponding regression matrix.

[0020] Preferably, in step S2, under the condition that the environmental information is known, path points are selected with the finite Fourier series in free space as the reference trajectory, and the OMPL library is used to complete the point-to-point collision avoidance path planning to obtain collision avoidance path points, which specifically includes the following steps:

[0021] S2-1. Establish an environmental map for the robot to work with an octree map;

[0022] S2-2. Select path points with the finite Fourier series in free space as the reference trajectory. The angles, angular velocities, and angular accelerations of the finite Fourier series trajectory in the robot joint space can be expressed as:

[0023]

[0024] where ω f = 2πf f is the fundamental frequency of the Fourier series, and T is the period of the Fourier series; a i,l , b i,l are the coefficients of the Fourier series; q i0 is the offset (constant term) of the Fourier series; i = 1, 2,..., n, where n is the number of degrees of freedom of the robot; N is the order of the Fourier series, that is, the number of harmonic terms of the Fourier series; then this Fourier series has a total of 2N + 1 parameters, q i0 , a i,l and b i,l are the parameters to be optimized for the excitation trajectory;

[0025] S2-3. When optimizing the excitation trajectory, consider the physical constraints of the robot joint angles, angular velocities, and angular accelerations. Further, the initial and end velocities and accelerations of the robot motion are zero, and the condition number of the regression matrix is used as the objective function of the optimization problem; the objective function and constraints of this optimization problem are as follows:

[0026]

[0027]

[0028] where t0 and t f respectively represent the start and end times of the trajectory, q int represents the initial position of the robot, q int , q int , respectively represent the minimum and maximum values of the robot joint angles, angular velocities, and angular accelerations, p end represents the position of the robot end effector, and S represents the subset of the working space where the robot does not collide;

[0029] S2-4. Using the finite Fourier series trajectory in free space as the reference trajectory, sample at a fixed time step Δt on this trajectory to obtain n path points. Then, using the collision avoidance path planning algorithm, generate n - 1 segments of collision avoidance trajectories between the selected path points and save the planned robot joint angles at a certain frequency f, thereby generating a collision avoidance path that is as close as possible to the excitation trajectory.

[0030] Preferably, in step S3, use a cubic B-spline curve to interpolate the path points in the velocity-path decoupling framework, parameterize the trajectory, and transform the optimization of the excitation trajectory into the optimization of the parameter velocity. The specific steps are as follows:

[0031] S3-1. Establish a velocity-path decoupling (PVD) framework, with the formula as follows:

[0032] q d (t):=P(s(t))

[0033]

[0034] Wherein, and represent the parameter velocity and parameter acceleration respectively;

[0035] S3-2. Use a cubic B-spline curve to interpolate the path points and parameterize the trajectory:

[0036]

[0037] Wherein, represents the k-th B-spline curve between the interpolation points and , d p represents the degree of the B-spline curve, C = {C i |i = 0, 1, …, n p} represents the control point set of the B-spline curve, n p +1 represents the number of control points of the B-spline curve, represents the k-th order basis function of the B-spline curve, which can be calculated by the De Boor-Cox recurrence formula. The specific expression is as follows:

[0038]

[0039] Wherein, U = {u i |i = 0, 1, …, m p} is the knot vector of the B-spline curve;

[0040] S3-3. Solve the control points of the cubic B-spline curve based on path point constraints and fixed boundary conditions. Further, the geometric path of the interpolated trajectory is strictly consistent with the geometric path defined by the original trajectory. The solution formula for the control points is as follows:

[0041]

[0042] where c1 and c2 are the first-order derivatives at the set first and Nth path points respectively, which are constant values.

[0043] Preferably, in step S4, considering the constraints of the robot joint angles, angular velocities, and angular accelerations, taking the condition number of the regression matrix as the objective function, use the nonlinear optimization algorithm to complete the solution of the excitation trajectory and complete the identification of the robot dynamics model. The specific steps are as follows:

[0044] S4-1. Convert the constraint conditions into constraint conditions in the parameter space:

[0045]

[0046] S4-2. Taking the parameter velocity as the optimization variable, establish the objective function of the nonlinear optimization problem; further, the joint velocity and joint acceleration of the robot are both related to the parameter velocity and similarly the regression matrix can also be expressed as a function of Therefore, we choose the parameter velocity as the optimization variable for this problem to solve the parameter trajectory with the optimal condition number. The entire optimization problem is expressed as follows:

[0047]

[0048] where cond(W) represents the condition number of the regression matrix .

[0049] The beneficial effects produced by adopting the technical solution of the present invention are as follows: The present invention innovatively proposes a method for identifying the robot body dynamics model with the optimal condition number under limited space constraints. On the one hand, it is different from the traditional method that restricts the position of the robot end in a cuboid, cylinder, or spherical space and assumes that there are no obstacles in this space. This method considers the complex working environment of the robot and innovatively considers the environmental constraints of the robot during the identification process. On the other hand, in order to simplify the optimization problem, parameterize the collision avoidance path, transform the optimization of the excitation trajectory into a parameter planning problem, reduce the multi-dimensional optimization problem to a one-dimensional problem, and consider the multiple constraints of the robot to complete the solution of the parameter trajectory, realizing the identification of the dynamics model parameters of the robot under limited space. Brief Description of the Drawings

[0050] Figure 1 Flow chart of a method for identifying the dynamic model of a robot body under restricted space constraints according to the present invention;

[0051] Figure 2 Principle block diagram of a method for identifying the dynamic model of a robot body under restricted space constraints according to the present invention;

[0052] Figure 3 Identification effect diagram of a method for identifying the dynamic model of a robot body under restricted space constraints according to the present invention. Detailed implementation manners

[0053] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0054] Embodiment 1

[0055] The present invention provides a method for identifying the dynamic model of a robot body facing the optimal condition number under restricted space constraints, as Figure 1 shown, including the following steps:

[0056] S1. Establish the dynamic model of the robot body based on the Newton-Euler method;

[0057] S2. Under the condition that the environmental information is known, select path points with the finite Fourier series in free space as the reference trajectory, and use the OMPL library to complete the point-to-point collision avoidance path planning to obtain collision avoidance path points;

[0058] S3. Interpolate the path points using a cubic B-spline curve under the velocity-path decoupling (PVD) framework, parameterize the trajectory, and transform the optimization of the excitation trajectory into the optimization of the parameter velocity;

[0059] S4. Considering the constraints of the robot joint angle, angular velocity, and angular acceleration, use the condition number of the regression matrix as the objective function, and use a nonlinear optimization algorithm to complete the solution of the excitation trajectory and complete the identification of the robot dynamic model.

[0060] The principle framework of the above steps is as Figure 2 shown.

[0061] Step S1 includes:

[0062] S1-1. Establish the coordinate systems of each link of the robot using the MDH method and establish the kinematic model of the robot;

[0063] S1-2. Establish the body dynamics model of the robot based on the Newton-Euler method and consider the Coulomb-viscous friction model;

[0064] S1-3. Linearize the robot dynamics model;

[0065] The calculation formula of S1-1 is as follows:

[0066]

[0067] where α i-1 , a i-1 , θ i , d i are the DH parameters of the robot. In this embodiment, the ROKAE xMate ER7 Pro collaborative arm is used as the implementation platform, and its DH parameters are:

[0068] Joint i <![CDATA[a i-1 (mm)]]> <![CDATA[α i-1 (rad)]]> <![CDATA[d i (mm)]]> <![CDATA[θ i (rad)]]> 1 0 -π / 2 404 <![CDATA[θ1]]> 2 0 π / 2 0 <![CDATA[θ2]]> 3 0 -π / 2 437.5 <![CDATA[θ3]]> 4 0 π / 2 0 <![CDATA[θ4]]> 5 0 -π / 2 412.5 <![CDATA[θ5]]> 6 0 π / 2 0 <![CDATA[θ6]]> 7 0 0 275.5 <![CDATA[θ7]]>

[0069] The calculation formula of S1-2 is as follows:

[0070]

[0071] where M(q) is the robot inertia matrix, is the centrifugal and Coriolis force matrix, G(q) is the gravity matrix, is the friction matrix, and sgn(·) is the sign function.

[0072] The calculation formula of S1-3 is as follows:

[0073]

[0074] where represents the minimum inertia parameter set of the robot dynamics model, is the corresponding regression matrix.

[0075] S2 includes:

[0076] S2-1. Establish the environmental map for the robot to work using the octree map;

[0077] S2-2. Select path points with the finite Fourier series in free space as the reference trajectory:

[0078] S2-3. Consider the physical constraints of the robot joint angles, angular velocities, and angular accelerations. Further, the initial and end velocities and accelerations of the robot movement are zero, and the condition number of the regression matrix is used as the objective function of the optimization problem;

[0079] S2-4. Use the finite Fourier series trajectory in free space as the reference trajectory, sample it at a fixed time step Δt on this trajectory to obtain n path points; then use the collision avoidance path planning algorithm to generate n-1 segments of collision avoidance trajectories between the selected path points and save the planned robot joint angles at a certain frequency f, so as to generate a collision avoidance path as close as possible to the excitation trajectory;

[0080] The calculation formula of S2-2 is as follows:

[0081]

[0082] where, ω f = 2πf f is the fundamental frequency of the Fourier series, and T is the period of the Fourier series; a i,l , b i,l are the coefficients of the Fourier series; q i0 is the offset (constant term) of the Fourier series; i = 1, 2,..., n, where n is the number of degrees of freedom of the robot; N is the order of the Fourier series, that is, the number of harmonic terms of the Fourier series; then this Fourier series has a total of 2N + 1 parameters, q i0 , a i,l and b i,l are the parameters to be optimized for the excitation trajectory.

[0083] The calculation formula of S2-3 is as follows:

[0084]

[0085] where, t0 and t f represent the start and end times of the trajectory respectively, q int represents the initial position of the robot, q int , q int , represent the minimum and maximum values of the robot joint angle, angular velocity and angular acceleration respectively, p end represents the position of the robot end effector, and S represents the subset of the working space where the robot does not collide. After optimization and solution, the final parameters are:

[0086] Joint i 1 2 3 4 5 6 7 <![CDATA[q i0 > 0.0561 0.3124 0.3292 0.1852 0.2564 0.3741 0.1101 <![CDATA[a i,1 > 0.0911 -0.0306 0.1145 -0.1492 0.1291 -0.1501 0.1460 <![CDATA[b i,1 > 0.0116 -0.1442 0.0100 -0.0182 0.0082 0.0812 0.0064 <![CDATA[a i,2 > 00146 -0.2195 0.1475 -0.0522 0.0138 0.0935 -0.0695 <![CDATA[b i,2 > 0.1035 0.1254 0.2078 -0.5553 0.1879 -0.7277 0.1724 <![CDATA[a i,3 > 0.4108 0.0840 0.4244 -0.3822 0.4503 -0.8194 -0.3396 <![CDATA[b i,3 > -0.0966 -0.0536 0.0871 -0.1675 0.0288 -0.6049 -0.2357 <![CDATA[a i,4 > 0.4475 0.0948 0.3584 0.2100 0.4222 0.0716 0.1468 <![CDATA[b i,4 > -0.1820 0.3432 -0.1318 0.1731 -0.1380 -0.2138 0.0706 <![CDATA[a i,5 > -0.9640 0.0713 -1.0449 0.3737 -1.0155 0.8044 0.1164 <![CDATA[b i,5 > 0.1598 -0.2637 -0.0319 0.1877 0.0163 0.8088 0.0147

[0087] S3 includes:

[0088] S3-1. Establish a velocity-path decoupling (PVD) framework;

[0089] S3-2. Use cubic B-spline curves to interpolate the path points and parameterize the trajectory;

[0090] S3-3. Solve for the control points of the cubic B-spline curve based on path point constraints and fixed boundary conditions;

[0091] The calculation formula of S3-1 is as follows:

[0092] q d (t):=P(s(t))

[0093]

[0094] Where, and represent the parametric velocity and parametric acceleration respectively.

[0095] The calculation formula of S3-2 is as follows:

[0096]

[0097] Where, represents the interpolation point and the k-th segment of the B-spline curve between, d p represents the degree of the B-spline curve, C={C i |i=0,1,…,n p} represents the set of control points of the B-spline curve, n p +1 represents the number of control points of the B-spline curve, represents the k-th order basis function of the B-spline curve, which can be calculated by the De Boor-Cox recurrence formula, and the specific expression is as follows:

[0098]

[0099] Where, U={u i |i=0,1,…,m p} is the knot vector of the B-spline curve.

[0100] The calculation formula of S3-3 is as follows:

[0101]

[0102] Where, c1 and c2 are the first-order derivatives at the set first and Nth path points respectively, which are constant values. The control point results of the first segment of the trajectory are as follows in the table:

[0103] Joint i 1 2 3 4 5 6 7 <![CDATA[C0]]> -0.2240 0.2906 -0.2195 0.8088 -0.2179 1.3655 -0.2240 <![CDATA[C1]]> 0.0620 0.5899 0.05980 1.1163 0.0590 1.6235 0.0620 <![CDATA[C2]]> -0.0240 0.4906 -0.0195 1.0088 -0.0179 1.5655 -0.0240 <![CDATA[C3]]> 0.0342 0.5890 0.0182 1.1315 0.01270 1.5394 0.0335

[0104] S4 includes:

[0105] S4-1. Transform the constraint conditions into constraint conditions in the parametric space;

[0106] S4-2. Establish the objective function of the non-linear optimization problem with the parametric velocity as the optimization variable. Further, the joint velocity and joint acceleration of the robot are related to the parametric velocity and the regression matrix can also be expressed as a function of Therefore, select the parametric velocity as the optimization variable for this problem to solve the parametric trajectory with the optimal condition number;

[0107] The calculation formula of S4-1 is as follows:

[0108]

[0109] The calculation formula of S4-2 is as follows:

[0110]

[0111] where cond(W) represents the condition number of the regression matrix .

[0112] The final number of parametric velocities is 463, and some of the results are as follows:

[0113]

[0114] The present invention innovatively proposes a method for identifying the dynamic model of a robot body with optimal condition number under limited space constraints. On the one hand, different from the traditional method that limits the position of the robot end in a cuboid, cylinder or spherical space and assumes that there are no obstacles in this space, this method considers the complex working environment of the robot and innovatively considers the environmental constraints of the robot during the identification process. On the other hand, in order to simplify the optimization problem, the collision avoidance path is parameterized, the optimization of the excitation trajectory is transformed into a parameter planning problem, the multi-dimensional optimization problem is reduced to one dimension, and the solution of the parameter trajectory is completed considering the multiple constraints of the robot, realizing the identification of the dynamic model parameters of the robot under limited space.

[0115] Experimental results and their descriptions:

[0116] In order to simulate the situation of obstacles in the actual environment, two identical cuboid obstacles are respectively set in the front left and directly right of the robot. The size of the obstacle is 300mm×300mm×1700mm. An octree map is used to describe the environmental map of the robot, and the map resolution is set to 100mm. The excitation trajectory that meets the environmental constraints is optimized by the method proposed in this paper, and the parameters of the robot body dynamic model are identified using this trajectory. In order to verify the effectiveness of this method, the excitation trajectory designed by this method is used for dynamic model identification, and the joint torque during the robot movement is calculated using this dynamic model.Figure 3 It shows the difference between the model calculated values and the measured values. From the figure, it can be found that the model calculated values basically coincide with the measured values, with a small error, indicating that the identified dynamic model is accurate. Table 1 shows the dynamic models obtained using different excitation trajectories and the root mean square errors for joint torque prediction. C1 represents the identification using the collision avoidance trajectory planned by the OMPL library, and C2 represents the method proposed in the present invention. As can be seen from the table, the root mean square error of each joint torque of method C1 is lower than that of method C2, which proves that the robot dynamic model is more accurate from the side. The excitation trajectory designed by the present method can consider the environmental constraints of the robot and improve the identification accuracy.

[0117] Table 1 Root Mean Square Errors between the Calculated Values and Measured Values of the Torque Model

[0118] Method Joint 1 Joint 2 Joint 3 Joint 4 Joint 5 Joint 6 Joint 7 C1 0.2701 1.0825 0.6391 0.7066 0.0885 0.6929 0.1263 C2 1.4152 3.4968 6.5939 3.6259 0.5174 1.3137 1.3712

[0119] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0120] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one or more flows or multiple flows and / or blocks Figure 1 one or more blocks or multiple blocks.

[0121] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means for implementing the functions specified in Figure 1 one or more flows or multiple flows and / or blocks Figure 1 one or more blocks or multiple blocks.

[0122] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus, so that a series of operation steps are performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in one process or a plurality of processes and / or blocks Figure 1 one process or a plurality of processes and / or blocks Figure 1 steps of the functions specified in one block or a plurality of blocks.

[0123] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: modifications or equivalent replacements can still be made to the specific embodiments of the present invention. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be covered by the protection scope of the claims of the present invention.

Claims

1. A method for identifying the dynamic model of a robot body under confined space constraints, characterized in that: The identification method comprises the following steps: S1. Establish the robot's body dynamics model based on the Newton-Euler method; S2. Under the condition that the environmental information is known, the finite Fourier series in free space is used as the reference trajectory to select the path points, and the OMPL library is used to complete the point-to-point collision avoidance path planning to obtain the collision avoidance path points; S3, using cubic B-spline curves to interpolate path points in the speed-path decoupling framework, parameterizing the trajectory, and transforming the optimization of the excitation trajectory into the optimization of the parameter speed; S4. Considering the constraints of the robot's joint angle, angular velocity and angular acceleration, the condition number of the regression matrix is ​​used as the objective function, and the nonlinear optimization algorithm is used to solve the excitation trajectory and identify the robot's dynamic model.

2. A method for identifying a robot body dynamics model under confined space constraints as claimed in claim 1, characterized in that: The step S1, establishing the robot's body dynamics model based on the Newton-Euler method, specifically includes the following steps: S1-1. Use the MDH method to establish the coordinate systems of the robot's links, establish the robot's kinematic model, and then the transformation matrix from coordinate system i-1 to coordinate system i i-1 T i for: Among them, α i-1 、a i-1 ,θ i ,d i is the DH parameter of the robot; S1-2. Based on the Newton-Euler method, the robot's body dynamics model is established, and the Coulomb-viscous friction model is established. The formula is as follows: Where M(q) is the robot inertia matrix, are the centrifugal and Coriolis force matrices, G(q) is the gravity matrix, is the friction matrix, sgn(·) is the sign function; S1-3. Linearize the robot dynamics model and find the minimum inertia parameter set of the robot dynamics model. The formula is as follows: in, represents the minimum inertial parameter set of the robot dynamics model, is the corresponding regression matrix.

3. The method for identifying the dynamic model of a robot body under confined space constraints as claimed in claim 1, characterized in that: The step S2, under the condition that the environmental information is known, selects the path points by taking the finite Fourier series in free space as the reference trajectory, completes the point-to-point collision avoidance path planning by using the OMPL library, and obtains the collision avoidance path points, which specifically includes the following steps: S2-1. Use the octree map to build the robot's working environment map; S2-2. Select path points using the finite Fourier series in free space as the reference trajectory. The angle, angular velocity, and angular acceleration of the finite Fourier series trajectory in the robot joint space can be expressed as: Among them, ω f =2πf f is the fundamental frequency of the Fourier series, and T is the period of the Fourier series; a i,l 、b i,l are the coefficients of the Fourier series; q i0 is the offset (constant term) of the Fourier series; i = 1, 2, ..., n, n is the number of degrees of freedom of the robot; N is the order of the Fourier series, that is, the number of harmonic terms of the Fourier series; then the Fourier series has a total of 2N + 1 parameters, q i0 、a i,l and b i,l is the parameter to be optimized for the excitation trajectory; S2-3. When optimizing the excitation trajectory, the physical constraints of the robot's joint angle, angular velocity, and angular acceleration are considered. Furthermore, the initial and final velocities and accelerations of the robot's motion are zero, and the condition number of the regression matrix is ​​used as the objective function of the optimization problem. The objective function and constraints of this optimization problem are as follows: Among them, t0 and t f Respectively represent the start and end time of the trajectory, q int represents the initial position of the robot, q int ,q int , Respectively represent the minimum and maximum values ​​of the robot joint angle, angular velocity and angular acceleration, p end represents the position of the robot end effector, S represents the subset of the robot's workspace where no collision occurs; S2-4. Take the finite Fourier series trajectory in free space as the reference trajectory, sample at a fixed time step Δt on this trajectory, and obtain n path points; then use the collision avoidance path planning algorithm to generate n-1 collision avoidance trajectories between the selected path points and save the planned robot joint angles at a certain frequency f, thereby generating a collision avoidance path that is as close to the excitation trajectory as possible.

4. A method for identifying a robot body dynamics model under confined space constraints as described in any one of claims 1 to 3, characterized in that: The step S3 uses a cubic B-spline curve to interpolate the path points under the speed-path decoupling framework, performs parameterization on the trajectory, and converts the optimization of the excitation trajectory into the optimization of the parameter speed, which specifically includes the following steps: S3-1. Establish a velocity-path decoupling (PVD) framework, the formula is as follows: q d (t):=P(s(t)) in, and denote parameter velocity and parameter acceleration respectively; S3-2. Use cubic B-spline curve to interpolate the path points and parameterize the trajectory: in, Indicates interpolation points and The kth segment of the B-spline curve between p represents the degree of the B-spline curve, C = {C i |i=0,1,…,n p } represents the control point set of the B-spline curve, n p +1 indicates the number of control points of the B-spline curve. represents the k-order basis function of the B-spline curve, which can be calculated by the De Boor-Cox recursion formula. The specific expression is as follows: Among them, U={u i |i=0,1,…,m p } is the knot vector of the B-spline curve; S3-3. Based on the path point constraints and fixed boundary conditions, solve the control points of the cubic B-spline curve. The formula is as follows: Among them, c1 and c2 are the first-order derivatives at the set 1st and Nth path points, respectively, which are constant values.

5. A method for identifying a robot body dynamics model under confined space constraints as claimed in any one of claims 1 to 3, characterized in that: The step S4 considers the constraints of the robot's joint angle, angular velocity and angular acceleration, takes the condition number of the regression matrix as the objective function, uses a nonlinear optimization algorithm to solve the excitation trajectory, and completes the identification of the robot's dynamic model, which specifically includes the following steps: S4-1. Convert the constraints into the constraints in the parameter space: S4-2, taking the parameter speed as the optimization variable, establish the objective function of the nonlinear optimization problem; select the parameter speed as the optimization variable of this problem to solve the parameter trajectory with the optimal condition number. The whole optimization problem is expressed as follows: Among them, cond(W) represents the regression matrix condition number.

Images