A gait optimization method and system for an on-track walking assembly robot

Abstract

The present invention discloses a gait optimization method and system for an on-track walking assembly robot, the method comprising: constructing a robot-structure coupling dynamics model and obtaining the coupling relationship between the robot joint motion and the structural vibration; constructing a joint trajectory function based on the sum of a quintic polynomial and a finite-term cosine Fourier series; constructing an objective function according to the relationship between structural vibration and joint motion; finding the joint trajectory parameters that minimize the objective function based on a PSO algorithm, and substituting the parameters into the joint trajectory function to obtain the joint trajectory. The system comprises: a coupling dynamics model construction module, a joint trajectory function construction module, an objective function construction module, and an optimization module. By using the present invention, the gait trajectory during the robot motion process is optimized, which helps to reduce the force on the structure caused by the robot motion, so that the residual vibration generated by the structure is minimized. The present invention can be widely used in the field of robot control.

Description

Technical Field

[0001] The present invention relates to the field of robot control, and in particular to a gait optimization method and system for an on-track walking assembly robot. Background Art

[0002] Using space robots to automatically assemble modular elements is an effective way to achieve on-orbit construction of super-large spacecraft. The three-branch space robot that realizes on-orbit assembly of discrete modular structures in structured environments is different from the general robots working in unstructured environments. Due to the specific characteristics of the structured assembly environment and the multi-branch structural design, the three-branch space robot has flexible robot movement, efficient space assembly, simple structure and control method. However, when the space three-branch robot walks on-orbit to assemble large structures, the robot movement will excite structural vibration, which will reduce the robot's positioning accuracy and operational flexibility, and even cause structural fatigue. Summary of the invention

[0003] In order to solve the above technical problems, the purpose of the present invention is to provide a gait optimization method and system for an on-track walking assembly robot, which optimizes the gait trajectory of the robot during movement, helps to reduce the force exerted on the structure by the robot movement, and can minimize the residual vibration generated by the structure.

[0004] The first technical solution adopted by the present invention is: a gait optimization method for an on-track walking assembly robot, comprising the following steps:

[0005] Construct a robot-structure coupling dynamics model and obtain the coupling relationship between robot joint motion and structural vibration;

[0006] Construct the joint trajectory function based on the sum of the fifth-order polynomial and the finite-term cosine Fourier series;

[0007] Construct the objective function based on the relationship between structural vibration and joint motion;

[0008] Based on the PSO algorithm, the joint trajectory parameters that minimize the objective function are found, and the parameters are substituted into the joint trajectory function to obtain the joint trajectory.

[0009] Furthermore, the step of constructing a robot-structure coupling dynamics model and obtaining the coupling relationship between the robot joint motion and the structural vibration specifically includes:

[0010] The robot dynamics model is constructed based on the Langrange equation and the force exerted by the robot motion on the structure is solved to obtain the coupling force.

[0011] Construct the dynamic equation of the structure and substitute the coupling force into the dynamic equation of the structure to construct the robot-structure coupling dynamic model;

[0012] The coupling relationship between robot joint motion and structural vibration is obtained based on the robot-structure coupling dynamics model.

[0013] Further, the robot dynamics model formula is expressed as follows:

[0014]

[0015] In the above formula, H b represents the equivalent mass of the branch end, H bm (θ) represents the coupling inertia matrix between the robot and the structure, H m (θ) represents the inertia matrix of the three-branch robot, z represents the deformation of the base in the vertical direction, θ represents the joint vectors of the robot, represents the nonlinear velocity-dependent term of the structure, represents the nonlinear velocity-related term of the robot, g z It represents the support force of the structure on the robot, and τ represents the torque vector of each joint of the robot.

[0016] Furthermore, the formula of the robot-structure coupling dynamics model is expressed as follows:

[0017]

[0018] In the above formula, M s represents the equivalent mass matrix of the structure, K s represents the equivalent stiffness matrix of the structure, p represents the modal coordinates of the structure, B(x) represents the vibration mode of the structure, H b represents the equivalent robot mass at the end of the branch, H bm (θ) represents the coupled inertia matrix between the robot and the structure, represents the nonlinear velocity-dependent term of the structure, and θ represents the joint vectors of the robot.

[0019] Further, the formula of the joint trajectory function is expressed as follows:

[0020] θ i (t) = λ i (t)+σ i (t)

[0021]

[0022] In the above formula, λ i (t) represents the fifth-order polynomial term, λ ij represents the coefficient of the quintic polynomial, σ i(t) represents the finite cosine Fourier series term, a im represents the Fourier series coefficients.

[0023] Further, the formula of the objective function is as follows:

[0024]

[0025] In the above formula, z max (a im ) is t f ~t end The maximum vibration displacement of the contact position between the structure and the robot foot under the unknown parameters within the time period, is t0~t f The maximum value of the angular acceleration of each joint in the time period, k z and k a Both represent weighting coefficients.

[0026] The second technical solution adopted by the present invention is: a gait optimization system for an on-track walking assembly robot, comprising:

[0027] The coupled dynamics model building module is used to build the robot-structure coupled dynamics model and obtain the coupling relationship between the robot joint motion and the structural vibration;

[0028] The joint trajectory function building module constructs the joint trajectory function based on the sum of the fifth-order polynomial and the finite-term cosine Fourier series;

[0029] An objective function building module is used to construct an objective function based on the relationship between structural vibration and joint motion;

[0030] The optimization module searches for the joint trajectory parameters that minimize the objective function based on the PSO algorithm, and substitutes the parameters into the joint trajectory function to obtain the joint trajectory.

[0031] The beneficial effects of the method and system of the present invention are as follows: the present invention establishes a coupling dynamic relationship between robot motion and structural vibration, proposes a robot gait optimization method based on the coupling relationship, optimizes the gait trajectory during robot motion, and enables the robot to suppress structural residual vibration while achieving point-to-point motion. It can be used in trajectory planning of space robots, vibration control of structures, and other aspects. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 It is a flowchart of the steps of a gait optimization method for an on-track walking assembly robot of the present invention;

[0033] Figure 2 is a schematic diagram of a robot walking on a flexible structure according to a specific embodiment of the present invention;

[0034] Figure 3 is a schematic diagram of a robot system subjected to coupling forces between the robot and the structure according to a specific embodiment of the present invention;

[0035] Figure 4 It is a schematic diagram of the structure of a specific embodiment of the present invention being subjected to the force of robot motion;

[0036] Figure 5 is a schematic diagram comparing joint trajectory information before and after optimization of a specific embodiment of the present invention;

[0037] Figure 6 2 is a schematic diagram comparing the coupling force and the deformation of the end of the structure before and after optimization of a specific embodiment of the present invention;

[0038] Figure 7 is a schematic diagram comparing joint torques before and after optimization of a specific embodiment of the present invention;

[0039] Figure 8 It is a structural block diagram of a gait optimization system for an on-track walking assembly robot of the present invention. DETAILED DESCRIPTION

[0040] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only provided for the convenience of explanation and description, and the order between the steps is not limited in any way. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0041] Figure 2 The schematic diagram of a three-branch space robot walking on a flexible structure. The robot consists of a central structure 3 and six connecting rods 1, 2, 4, 5, 6 and 7 on the three branches. The components are connected by revolute joints. Since each branch of the three-branch robot is the same, the dynamic properties of connecting rods 1, 5, and 7 are the same. Similarly, the dynamic properties of connecting rods 2, 4, and 6 are also the same. When the robot walks on the structure, first, the end of one branch is fixed to a certain position on the structure, and the end of the other branch moves forward until the end contacts the structure. Its end is fixed to the structure, and then the end of the branch originally fixed to the structure is released and continues to move forward. The two branches are alternately connected to the structure to complete a gait cycle. Because the analysis method for each step is the same, this paper selects the process of the robot taking the first step for dynamic analysis and structural vibration suppression research. M i ,I i ,L i represents the mass, moment of inertia, and length of each connecting rod, i = 1, 2, 3; θ i ,τ iRepresents the joint angles and torques applied to each joint of the three-branch robot, i=1,2,3,4,5,6. Figure 3 The z(x, t) in the figure represents the deformation in the vertical direction of the contact position between the flexible structure and the end of the robot branch, and f z Represents the support force of the structure on the end of the robot branch.

[0042] like Figure 1 As shown, the present invention provides a gait optimization method for an on-track walking assembly robot, the method comprising the following steps:

[0043] S1. Construct a robot-structure coupling dynamics model and obtain the coupling relationship between robot joint motion and structural vibration;

[0044] S1.1. Construct a robot dynamics model based on the Langrange equation and solve the force exerted by the robot motion on the structure to obtain the coupling force;

[0045] for Figure 2 The robot system walking on the structure shown in the figure uses the Lagrange equation to establish its dynamic model.

[0046]

[0047] Where L=TV is the Lagrangian operator, T represents the total kinetic energy of the system, V represents the total potential energy of the system, Q is the generalized force, and q represents the generalized coordinates. The total kinetic energy and potential energy of the system is the sum of the kinetic energy and potential energy of each link of the robot. By derivation, the dynamic equation of the robot walking on the structure is obtained as:

[0048]

[0049] The definitions of each symbol are as follows:

[0050] H b ∈R 1 : equivalent mass of branch end;

[0051] H bm ∈R 1×6 : The coupling inertia matrix between the robot and the structure;

[0052] H m ∈R 6×6 : Inertia matrix of the three-branch robot;

[0053] z∈R 1 : The deformation of the base (the end of the branch that is fixed to the structure) in the vertical direction;

[0054] θ∈R 1×6 : The joint vectors of the three-branch robot;

[0055] c b ∈R 1 : Nonlinear velocity-dependent term at the base (the end of the branch that is fixed to the structure);

[0056] c m ∈R 6×1 :Nonlinear velocity-related terms of a three-branch robot;

[0057] f z ∈R 1 : The support force of the structure on the base end of the branch robot;

[0058] τ∈R 6×1 : Joint torques of the three-branch robot.

[0059] S1.2, construct the dynamic equation of the structure and substitute the coupling force into the dynamic equation of the structure to construct the robot-structure coupling dynamic model;

[0060] The spatial structure is equivalent to an Euler-Bernoulli beam, such as Figure 4 As shown, the beam is at x = x v The force generated by the robot's movement is -f z The lateral vibration equation when acting is:

[0061]

[0062] Where ρ is the density of the beam, I is the moment of inertia of the cross section about the neutral axis, A is the cross-sectional area of ​​the beam, and E is the elastic modulus.

[0063] f z δ(xx v ) indicates that the beam is at x v The force at , δ is the Dirac function. Using the Ritz-Galerkin method, the lateral deformation of the Euler beam can be approximated as:

[0064]

[0065] where Φ i (x) represents the vibration mode function of the i-th mode, v i (t) represents the generalized coordinates (or deformation), and n represents the retained modal order. For a cantilever beam structure with a length of L, its vibration mode function is:

[0066]

[0067] where μ i L is the solution to the following characteristic equation:

[0068] cos(μL)·cosh(μL)=-1

[0069] Therefore, substituting equation (4) into the lateral vibration equation of the beam, we obtain:

[0070]

[0071] Multiply both sides of the above formula by Φ j (x) and integrating x along the beam length L gives:

[0072]

[0073] Using the distribution integral method, the first integral term is:

[0074]

[0075] According to the conditions of force and moment at the boundary, we get:

[0076]

[0077] So formula (6) can be written as follows:

[0078]

[0079] in:

[0080]

[0081]

[0082]

[0083] Written in matrix form:

[0084]

[0085] in,

[0086]

[0087] p(t)=[v1(t)v2(t)…v n (t)] T ,

[0088] B(x v )=[Φ1(x v )Φ2(x v )…Φ n (x v )] T .

[0089] M s , K s Represents the equivalent mass matrix and equivalent stiffness matrix of the structure.

[0090] S1.3. According to the robot-structure coupling dynamics model, the coupling relationship between the robot joint motion and the structural vibration is obtained.

[0091] From the upper part of the robot dynamics formula (2), we can know that the support force of the structure on the robot is f z , so the force exerted by the three-branch robot motion on the structure is -f z for:

[0092]

[0093] Substituting the above equation into the dynamic equation (9) of the beam, the lateral vibration equation of the beam under the motion of the robot can be obtained as follows:

[0094]

[0095] The lateral deformation of the beam given by equation (4) can be expressed in matrix form as:

[0096]

[0097] Same Approximately:

[0098]

[0099] Substituting equation (12) into equation (11), the coupled robot-structure dynamic equation is obtained as follows:

[0100]

[0101] Simplifying, we get:

[0102]

[0103] Where M = M s -B(x)H b B T (x), Formula (14) is the relationship between the robot joint motion and the structural vibration (modal coordinates). Solving it can find the influence of the robot joint motion on the structural vibration. Therefore, using this relationship, the joint trajectory that can suppress the structural vibration can be planned.

[0104] S2, constructing the joint trajectory function based on the sum of the fifth-order polynomial and the finite-term cosine Fourier series;

[0105] Quintic polynomials are often used for joint angle planning of point-to-point motion. Polynomial functions can fully meet the boundary conditions of point-to-point motion, but contain unnecessary high-order harmonics that excite system resonance, and polynomial functions cannot guarantee the convergence of the solution by increasing the terms of the polynomial. Using only quintic polynomials for trajectory planning of point-to-point motion also cannot achieve the effect of suppressing structural vibration; however, Fourier series expansion can guarantee convergence by increasing the number of terms, but it cannot meet the boundary conditions. Therefore, combining the advantages of the two functions, we consider using the quintic polynomial and the sum of finite-term cosine Fourier to plan the trajectory of the joint angle.

[0106] Each joint angle trajectory function:

[0107] θ i (t) = λ i (t)+σ i (t) (15)

[0108] in:

[0109]

[0110] Substituting the boundary conditions into equation (15) yields:

[0111] θ i (t0) = λ i (t0)+σ i (t0),

[0112] θ i (t f )=λ i (t f )+σ i (t f ),

[0113]

[0114]

[0115]

[0116] In the above formula, λ i (t) represents the fifth-order polynomial term, λ ij represents the coefficient of the quintic polynomial, σ i (t) represents the finite cosine Fourier series term, a im Represents the Fourier series coefficient, takes the Fourier series coefficient as the variable to be designed, and combines the Fourier series coefficient (a im ) and boundary conditions (θ, The coefficient λ of the quintic polynomial is ijIt can be obtained by formula (16). Therefore, when the Fourier series coefficient a im Once (m=1,2,...,M) is determined, the angle, angular velocity and angular acceleration of the joint trajectory are also determined. Therefore, the robot trajectory planning problem is transformed into determining the unknown parameter a im problem.

[0117] S3, constructing the objective function according to the relationship between structural vibration and joint motion;

[0118] S4. Based on the PSO algorithm, find the joint trajectory parameters that minimize the objective function, and substitute the parameters into the joint trajectory function to obtain the joint trajectory.

[0119] (14) The relationship between the structural vibration (determined by the modal coordinates) and the joint trajectory is established. Each joint adopts the trajectory planning method introduced above, and then the relationship between the modal coordinates and the joint motion can be derived. Therefore, the vibration of the structure can be described by the undetermined parameters. Assume that the undetermined parameters are represented by a im To describe, the vibration displacement of the structure can be expressed as:

[0120]

[0121] In order to avoid the influence of excessive acceleration on the stability of the robot during the movement of the robot, the constructed joint trajectory needs to meet the goal of minimizing the residual vibration of the structure and also limit the angular acceleration of each joint during the movement of the robot. Therefore, the following objective function is constructed:

[0122]

[0123] In the above formula, z max (a im ) is t f ~t end The maximum vibration displacement of the contact position between the structure and the robot foot under the unknown parameters within the time period, is t0~t f The maximum value of the angular acceleration of each joint in the time period, k z and k a Both represent weighting coefficients, which enable physical quantities of different dimensions to be superimposed after weighting. The weighting coefficients are generally set by the allowable error range. z Indicates the maximum allowable value of the residual vibration displacement of the structure, k a represents the maximum permissible value of the angular acceleration of each joint during the robot's motion. The purpose of optimization is to minimize the objective function. im )≤1, which can ensure z max (a im ), Within the allowable range, a set of parameters a that minimize the objective function can be obtained. im , substituting the optimized undetermined parameters into the joint trajectory function can obtain the optimized joint trajectory.

[0124] The purpose of robot gait optimization is to make the residual vibration displacement of the structure generated by the robot motion approach zero, that is, Figure 3 z(x,t) in the robot approaches zero. Based on the relationship between the robot joint motion and the structural vibration (modal coordinates) derived above, a parameterized method based on the sum of a quintic polynomial and a finite cosine Fourier series is proposed to plan the trajectory of the robot joint angle. The parameters of each joint trajectory in the robot joint space are optimized by the PSO optimization algorithm to find a set of joint trajectory parameters that minimize the vibration displacement of the structure. The vibration control problem is converted into a parameter optimization problem, which greatly simplifies the complexity of vibration control, suppresses structural vibration from the root, and improves assembly efficiency.

[0125] The simulation example is as follows:

[0126] like Figure 2 As shown in the figure, the six joints of the three-branch space robot are driven by motors, and the connecting rods move in the plane. The parameters of the robot's connecting rods and cantilever beam structure are as follows: the mass of each connecting rod of the three-branch robot is M1=M2=3kg, the length of the connecting rod is L1=L2=5m, and the moment of inertia of the connecting rod around the rotation axis is I1=I2=25kg·m 2 , the mass of the center part of the three-branch robot M3 = 2kg, the length of the center part connecting rod L3 = 2m, the moment of inertia of the center part I3 = 12kg·m 2 . Cantilever beam length L = 10m, density ρ = 122kg / m 3 , cross-sectional area A = 1m 2 , section moment of inertia The elastic modulus E = 24.5 MPa. The simulation time is 18 seconds. The robot moves from the initial position to the final position in the first 6 seconds, and the last 12 seconds are used to study the residual vibration. Other parameters are the number of Fourier series terms M = 3, the maximum allowable value of the residual vibration displacement of the structure k z =0.01m, the maximum allowable value k of the angular acceleration of each joint during the robot's motion a =0.2rad / s 2 In the PSO algorithm for finding the optimal parameters, the number of population particles and the maximum number of iterations are 200 and 2000 respectively, and the search range of the parameters to be optimized is: a im ∈[-2,2],(m=1,2,3).

[0127] Taking the point-to-point motion planning in joint space as an example, assuming that the time of trajectory planning is: t0~t f , the initial termination conditions of each joint angle of the three-branch robot are:

[0128] θ(t0)=[0,0,0,0,0,0];

[0129]

[0130]

[0131] In addition, in order to compare the effect of the proposed method on vibration suppression, the quintic polynomial (20) programming is used as a reference.

[0132] θ i (t) = a0 + a i t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (20)

[0133] like Figure 5 As shown in the figure, in order to optimize the angles of the front and rear joints, that is, to obtain the robot motion gait with the smallest residual vibration displacement of the structure, it can be seen that the trajectory planning method based on the superposition of the quintic polynomial three-term cosine Fourier series can plan the joint trajectory with the same initial and terminal joint angles, so that the robot can make full use of the movement of its redundant joints to suppress the vibration of the structure. The force on the structure caused by the robot movement and the vibration of the structure are shown in Figure 6 As shown in the figure, it can be seen from the simulation results that the joint trajectory planned by the general quintic polynomial makes the residual vibration (t>6s) of the structure in the range of [-0.006m, 0.006m]. The joint trajectory optimized based on the quintic polynomial superposition of three cosine Fourier series not only reduces the force of the robot movement on the structure, but also the residual vibration range of the structure is only [-0.00002m, 0.00002m], which greatly reduces the amplitude of the vibration and effectively suppresses the vibration of the structure. That is, both the coupling force and the structural vibration are effectively suppressed. Figure 7 To optimize the torque of the six joints before and after optimization, the results show that the torque of each joint after optimization is reduced to a certain extent compared with that before optimization.

[0134] like Figure 8 As shown, a gait optimization system for an on-track walking assembly robot comprises:

[0135] The coupled dynamics model building module is used to build the robot-structure coupled dynamics model and obtain the coupling relationship between the robot joint motion and the structural vibration;

[0136] The joint trajectory function building module constructs the joint trajectory function based on the sum of the fifth-order polynomial and the finite-term cosine Fourier series;

[0137] An objective function building module is used to construct an objective function based on the relationship between structural vibration and joint motion;

[0138] The optimization module searches for the joint trajectory parameters that minimize the objective function based on the PSO algorithm, and substitutes the parameters into the joint trajectory function to obtain the joint trajectory.

[0139] The contents of the above method embodiments are all applicable to the present system embodiments. The functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0140] An on-track walking assembly robot gait optimization device:

[0141] at least one processor;

[0142] at least one memory for storing at least one program;

[0143] When the at least one program is executed by the at least one processor, the at least one processor implements the gait optimization method for an on-track walking assembly robot as described above.

[0144] The contents of the above method embodiments are all applicable to the present device embodiments. The functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0145] A storage medium storing processor-executable instructions, characterized in that the processor-executable instructions are used to implement the above-mentioned on-track walking assembly robot gait optimization method when executed by the processor.

[0146] The contents of the above method embodiments are all applicable to the present storage medium embodiments. The functions specifically implemented by the present storage medium embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0147] The above is a specific description of the preferred implementation of the present invention, but the invention is not limited to the embodiments. Those skilled in the art may make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A gait optimization method for an on-track walking assembly robot, characterized in that: The following steps are involved: Construct a robot-structure coupling dynamics model and obtain the coupling relationship between robot joint motion and structural vibration; Construct the joint trajectory function based on the sum of the fifth-order polynomial and the finite-term cosine Fourier series; Construct the objective function based on the relationship between structural vibration and joint motion; Based on the PSO algorithm, find the joint trajectory parameters that minimize the objective function, and substitute the parameters into the joint trajectory function to obtain the joint trajectory; The step of constructing a robot-structure coupling dynamics model and obtaining the coupling relationship between the robot joint motion and the structural vibration specifically includes: Based on the Langrange equation, the robot dynamics model is constructed and the force exerted by the robot motion on the structure is solved to obtain the coupling force. Construct the dynamic equation of the structure and substitute the coupling force into the dynamic equation of the structure to construct the robot-structure coupling dynamic model; The coupling relationship between robot joint motion and structural vibration is obtained based on the robot-structure coupling dynamics model; The robot dynamics model formula is as follows: In the above formula, H b represents the equivalent mass of the branch end, H bm (θ) represents the coupling inertia matrix between the robot and the structure, H m (θ) represents the inertia matrix of the three-branch robot, z represents the deformation of the base in the vertical direction, θ represents the joint vectors of the robot, represents the nonlinear velocity-dependent term of the structure, represents the nonlinear velocity-related term of the robot, f z represents the support force of the structure on the robot, τ represents the moment vector of each joint of the robot; The robot-structure coupling dynamics model is formulated as follows: In the above formula, M s represents the equivalent mass matrix of the structure, K s represents the equivalent stiffness matrix of the structure, p represents the modal coordinates of the structure, B(x) represents the vibration mode of the structure, H b represents the equivalent robot mass at the end of the branch, H bm (θ) represents the coupled inertia matrix between the robot and the structure, represents the nonlinear velocity-dependent term of the structure; The formula of the joint trajectory function is as follows: i i (t)=λ i (t)+σ i (t) In the above formula, λ i (t) represents the fifth-order polynomial term, λ ij represents the coefficient of the quintic polynomial, σ i (t) represents the finite cosine Fourier series term, a im represents the Fourier series coefficients.

2. According to claim 1, a gait optimization method for an on-track walking assembly robot is characterized in that: The objective function formula is as follows: In the above formula, z max (a im ) is t f ~t end The maximum vibration displacement of the contact position between the structure and the robot foot under the unknown parameters within the time period, is t0~t f The maximum value of the angular acceleration of each joint in the time period, k z and k a Both represent weighting coefficients.

Images