A three-limb robot gait planning method based on truss steady climbing

Abstract

The embodiment of the application provides a three-branch robot gait planning method based on truss stable climbing, comprising the following steps: designing robot two climbing branch end trajectory control points according to a truss structure and a three-branch robot kinematics model, planning an end trajectory sequence of the climbing branch; constructing a performance evaluation index of the end trajectory of the climbing branch, constructing a multi-objective optimization model of the end trajectory according to the performance evaluation index, and obtaining an optimal time interval sequence between the end trajectory control points; constructing a performance evaluation index of a configuration of a carrying branch at each control point of the end trajectory of the climbing branch, constructing a configuration optimization method of the carrying branch at the control point according to the performance evaluation index, and obtaining an optimal configuration of the carrying branch at each control point; planning a joint trajectory sequence of the carrying branch according to the configuration of the carrying branch at each control point; and constructing a three-branch robot truss stable climbing gait planning method according to the end trajectory sequence of the climbing branch and the joint trajectory sequence of the carrying branch.

Description

Technical Field

[0001] This invention relates to a gait planning method for a three-branch robot based on truss-based stable climbing, belonging to the field of space robot motion planning. Background Technology

[0002] With the deepening of space exploration and the continuous improvement of space technology, the construction of large spacecraft such as large-aperture space telescopes, space solar power stations, and Hall thrusters for interstellar travel has become a project under development or planning for major spacefaring nations. To deploy such large spacecraft in orbit, limitations in launch capacity and fairing size necessitate multiple launches to send the spacecraft's structure or functional payloads into orbit. Once in orbit, deployment mechanisms are used for on-orbit assembly of these payloads to complete the construction and deployment of the large spacecraft. Since the main structure of large spacecraft is typically supported by large space trusses, the assembly process involves the long-distance on-orbit transport of various payloads, including truss sections and functional modules. The stability, safety, and efficiency of this multi-payload, large-scale, and high-precision transport mission deserve significant attention. Currently, on-orbit transport tasks in space activities mainly rely on astronauts performing extravehicular activities (EVAs) to transport payloads to the deployment location for assembly. This not only seriously threatens the lives of astronauts but also suffers from limited operational range, low efficiency, and excessive resource consumption. Therefore, using space robots to replace astronauts for on-orbit transport tasks is of great significance for achieving precise assembly of various payloads and on-orbit deployment of large spacecraft. Due to the wide range and large span of the main structure of large spacecraft, single-chain space robots, with their fixed bases, have limited working ranges and cannot meet the requirements for long-distance payload transport in on-orbit deployment payload transport tasks. Therefore, in order to balance the needs of payload transport and truss climbing, on-orbit transport space robots need to exhibit multi-branched structural features.

[0003] There are many existing gait planning methods for three-branch robots. The method that uses joint space planning for the two climbing branches of a three-branch robot to plan the gait is the most common. This method usually focuses more on the motion trajectory and less on the climbing performance. In addition, it only plans the two branches of the robot and ignores the role of the third branch. It is not much different from the gait planning of two-branch robots and cannot reflect the advantages of three-branch robots in performing climbing tasks. Summary of the Invention

[0004] In view of this, the present invention provides a gait planning method for a three-branch robot based on smooth truss climbing, so as to enable the three-branch robot to successfully complete the on-orbit transport task on the truss.

[0005] This invention provides a gait planning method for a three-branch robot based on truss-based smooth climbing, comprising:

[0006] Step S1: Based on the truss structure and the kinematic model of the three-branch robot, design the end trajectory control points of the two climbing branches of the three-branch robot, and plan the end trajectory sequence of the climbing branches;

[0007] Step S2: Construct a performance evaluation index for the climbing branch end trajectory, and construct a multi-objective optimization model for the climbing branch end trajectory based on the performance evaluation index to obtain the optimal time interval sequence between control points of the climbing branch end trajectory;

[0008] Step S3: Construct a performance evaluation index for the configuration of the transport branch at each control point of the trajectory at the end of the climbing branch. Based on this performance evaluation index, construct a method for optimizing the configuration of the transport branch at the control point to obtain the optimal configuration of the transport branch at each control point.

[0009] Step S4: Based on the configuration of the transport branch at each control point, plan the joint trajectory sequence of the transport branch;

[0010] Step S5: Based on the end trajectory sequence of the climbing branch and the joint trajectory sequence of the transport branch, construct a gait planning method for the smooth climbing of the three-branch robot gantry.

[0011] In the above method, step S1 includes:

[0012] The trajectory at the end of the climbing branch is set as a B-spline curve, with a control point sequence of {P} and a number of control points of n.

[0013] {Pi|i=0,1,…n-1}

[0014] In the formula, P i The control points for the terminal trajectory of the climbing branch are the control polygons of the terminal trajectory, which are obtained by sequentially linking these control points. Therefore, the mathematical expression C(u) for the (k+1)th order B-spline curve is as follows:

[0015]

[0016] In the formula, N i,k (u) is a k-th degree B-spline basis function, also known as a piecewise B-spline mixture function. Basis function N i,k (u) has the following de Boer-Cox recurrence relation:

[0017]

[0018] In the above equation, if the denominator is 0: if the numerator is also 0, then this term is also 0; if the numerator is not 0, then the denominator is conventionally set to 1. In the equation, u i Let be a continuously changing sequence of non-decreasing values ​​called a node vector, as shown below:

[0019] [u0,u1,…u k ,u k+1 ,…u n ,u n+1 ,…u n+k ]

[0020] In the above method, step S2 includes:

[0021] Step S21: Construct performance evaluation metrics for the climbing branch end trajectory, with the following specific metrics:

[0022] (1) Time objective function

[0023] The running time of each gait of the three-branch robot is defined as the sum of the time intervals between the control points of the trajectory at the end of the climbing branch. Therefore, minimizing the time is set as the first optimization objective, and its objective function equation is as follows:

[0024]

[0025] In the formula, n is the number of control points of the B-spline curve; Δt i This represents the time interval between the i-th control point and the (i+1)-th control point.

[0026] (2) Stationary objective function

[0027] Joint angular acceleration represents the rate of change of the joint angle, and it is directly related to the forces acting on the joint. The smoothness of the robot's motion trajectory is represented by the integral of the robot's joint impacts, and minimizing this is taken as the second optimization objective. The objective function equation is shown below:

[0028]

[0029] In the formula, n is the number of robot joints; This represents the joint impact of the i-th joint in relation to time.

[0030] (3) Energy consumption objective function

[0031] During robot movement, joint motors continuously output joint torques to cause corresponding joint movements. Therefore, by using the output torques of each joint to deduce joint energy consumption, the objective function equation for optimization objective three is established as follows:

[0032]

[0033] In the formula, n is the number of robot joints; τ represents the time-dependent angular velocity of the i-th joint; i (t) represents the time-dependent torque of the i-th joint, and its expression is as follows:

[0034]

[0035] Where, τ i n is the output torque of the i-th joint; i Let k be the external torque acting on the i-th member; i This represents the unit vector indicating the rotation direction of the i-th joint.

[0036] Step S22: Construct the constraints for the optimization model. The specific constraints are as follows:

[0037] (1) Joint angle constraint

[0038] Based on the mechanical structure, internal wiring layout, and installation location of the three-branch robot, joint angle constraints are established for the joint motion range of each branch to limit its movement space. For the three-branch robot studied in this paper, the joint angle constraints for each branch are as follows:

[0039] |θ i |≤|θ imax |=180°

[0040] In the formula, θ i Let θ be the angle of the i-th joint. imax Let be the maximum rotation angle of the i-th joint.

[0041] (2) Joint angular velocity constraint

[0042] Based on the relationship between joint angular velocity and robot control precision, and considering the upper limit of joint motor rotation speed, the value of joint angular velocity cannot be increased indefinitely. Therefore, the robot's joint angular velocity constraints are set as follows:

[0043]

[0044] In the formula, Let i be the angle of the i-th joint; Let be the maximum joint angular velocity of the i-th joint.

[0045] Step S23: Based on the above optimization objectives and constraints, construct a multi-objective optimization model for the trajectory at the end of the climbing branch, and solve for the optimal time interval sequence between control points, as shown in the following flowchart:

[0046] Based on three optimization objectives and two constraints, a multi-objective trajectory optimization model is constructed. The time interval sequence between control points at the end of the climbing branch is solved to obtain the multi-objective optimized trajectory at the end of the climbing branch. The specific process for solving the multi-objective optimized trajectory is as follows: Figure 4 As shown.

[0047] In the above method, step S3 includes:

[0048] (1) Climbing stability

[0049] When a three-branch robot climbs a truss surface, excessively rapid robot movement can easily cause excitation disturbances to the truss or the load being transported by the handling branches, affecting the robot's climbing stability. Therefore, the offset of the robot's center of mass relative to a reference position during the three-branch robot's gait is defined as an evaluation index for the robot's motion stability. The center of mass r of the three-branch robot... σ The calculation formula is:

[0050]

[0051] In the formula, M1, M2, and M3 represent the total mass of the three branches of the three-branch robot; m 1,i m 2,i m 3,i The masses of the joint links of the robot's three branches are r, respectively. 1,i r 2,i r 3,i These are the radius vectors of the mass points of each joint link of the robot's three branches relative to the origin O.

[0052] Therefore, the first optimization objective g1 is...

[0053]

[0054] In the formula, r is the centroid of the three-branch robot at the current control point.

[0055] (2) Load capacity

[0056] When a robot interacts with its environment, forces and torques are generated at the points of contact. The external force F acting on the end effector of the robotic arm... n With torque m n The combined six-dimensional vector

[0057]

[0058] Defined as the terminal generalized force vector. A seven-dimensional vector composed of the driving torques of each joint of the third branch of a three-branch robot.

[0059]

[0060] This is defined as a joint torque vector. The joint torque vector is used as the input to the robot's third branch drive mechanism, and the generalized force generated at the branch end is used as the output of the transport branch. The relationship between the two is established as follows.

[0061] Using the principle of virtual work, the generalized end force vector F corresponding to the joint moment vector τ is solved. nWhen the robotic arm is in equilibrium, the total virtual work generated by any virtual displacement is zero; that is, the virtual work generated by virtual displacement in the joint space is equal to the virtual work generated by virtual displacement in the operating space.

[0062] τ T ·δq=F T ·D

[0063] In the formula, τ T F is the transpose of the joint moment vector τ; δq is the virtual displacement in joint space; T is the transpose of the end-effector generalized force vector; D is the virtual displacement in the operation space.

[0064] The virtual displacement δq in joint space is not independent of the virtual displacement D in operation space; they must satisfy the geometric constraints specified by the Jacobian matrix of the robot's handling branch.

[0065] D=Jδq

[0066] In the formula, J is the Jacobian matrix of the robot handling branch.

[0067] Substituting, we can obtain

[0068] τ=J T F

[0069] F = (J T ) -1 τ

[0070] Therefore, the second optimization objective is:

[0071]

[0072] In the formula, f x ,f y ,f z F respectively n Components in three directions.

[0073] (3) Flexibility

[0074] Flexibility refers to the ability of a robot to control the speed of its end effector through joint movement, and it measures the isotropy of the robot's end effector.

[0075] To quantitatively evaluate the motion flexibility of robot branches, maneuverability is used as a metric for characterization:

[0076]

[0077] In the formula, ω is a measure of operability; det() is used to calculate the determinant of the square matrix; J(q) is the Jacobian matrix, J T (q) is the transpose of the Jacobian matrix; λi For matrix J(q)J T The eigenvalues ​​of (q) are λ1≥λ2≥…≥λ m , i = 1, 2, ..., m; m is matrix J(q)J T The number of rows of (q); σ i Let ω be the singular value of the Jacobian matrix J(q). The operability index ω is between [0,1]. When w=0, the robotic arm is in a singular state. The larger the value of ω, the better the flexibility of the robot's branches.

[0078] For a given robot, once the structural parameters are determined, only changes in the joint angles of the robot branches affect their flexibility. Different robot branch configurations exhibit varying degrees of flexibility. Therefore, the third optimization objective is:

[0079] g3 = max(ω1, ω2, ..., ω n )

[0080] In the formula, n is the number of all configurations corresponding to the robot's handling branch.

[0081] In the above method, step S4 includes:

[0082] Based on the configuration of the transport branch at each control point, a series of angle values ​​(t) for a certain joint. i ,q i The expression for the quintic spline curve is as follows:

[0083] θ i (t)=a i (tt i ) 5 +b i (tt i ) 4 +c i (tt i ) 3 +d i (tt i ) 2 +e i (tt i )+q i

[0084] In the formula, θ i (t) represents the joint trajectory of this joint angle between the i-th control point and the (i+1)-th control point, where t is the current time, and t∈[t]. i ,t i+1 ], i = 1, 2, ..., n-1, where n is the number of control points in the end-point trajectory of this gait, q i Let be the joint angle corresponding to the i-th control point. i ,bi ,c i ,d i ,e i Therefore, the unknown quantity of the joint trajectory between the joint angle at the i-th control point and the (i+1)-th control point is...

[0085] Taking the derivatives of each order on both sides of the equation, we can obtain the following equation.

[0086]

[0087] In the formula, The joint angular velocity, Joint angular acceleration, Add accelerometer to the joint angle. Let 'a' be the derivative of the accelerometer with respect to time at the joint angle. Based on this joint angle at the initial and final moments, and at each control point, 'a' can be calculated. i ,b i ,c i ,d i ,e i The value of can be used to solve for the joint trajectory of the transport branch.

[0088] In the above method, step S5 includes:

[0089] Step S51: Based on the positional relationship between the truss members and the kinematic model of the three-branch robot, solve the robot's workspace and classify various climbing gaits;

[0090] Step S52: Divide the three branches of the three-branch robot into two climbing branches and one transport branch according to their functions;

[0091] Step S53: Solve the end trajectory sequence of the climbing branch and the joint trajectory sequence of the transport branch respectively;

[0092] Step S54: Use the inverse kinematics solution of the three-branch robot to map the end trajectory sequence of the climbing branch to the joint space, and solve for the trajectory sequence of each joint of the climbing branch;

[0093] Step S55: Construct a smooth climbing gait for the three-branch robot truss by combining the joint trajectory sequences of the two branches. Attached Figure Description

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

[0095] Figure 1 This is an operation flowchart provided in the embodiments of the present invention;

[0096] Figure 2 This is a schematic diagram of the initial configuration and link coordinate system of the three-branch robot used in this embodiment of the invention;

[0097] Figure 3 This is a diagram showing the trajectory of the two climbing branches at the end in an embodiment of the present invention;

[0098] Figure 4 This is a flowchart of the multi-objective optimization process for the trajectory at the end of a climbing branch in an embodiment of the present invention;

[0099] Figure 5 This is a Pareto front diagram of the multi-objective optimization of the climbing branch end trajectory in this embodiment of the invention;

[0100] Figure 6 This is the Pareto front diagram of the preferred transport branch configuration in this embodiment of the invention. Detailed Implementation

[0101] To better understand the technical solution of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0102] It should be understood that the described embodiments are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0103] This invention provides a gait planning method for a three-branch robot based on truss-based smooth climbing. Please refer to [link / reference]. Figure 1 The method includes the following steps:

[0104] Step 101: Based on the truss structure and the kinematic model of the three-branch robot, design the end trajectory control points of the two climbing branches of the three-branch robot, and plan the end trajectory sequence of the climbing branches.

[0105] The trajectory at the end of the climbing branch is set as a B-spline curve, with a control point sequence of {P} and a number of control points of n.

[0106] {P i |i=0,1,…n-1}

[0107] In the formula, P i The control points for the terminal trajectory of the climbing branch are the control polygons of the terminal trajectory, which are obtained by sequentially linking these control points. Therefore, the mathematical expression C(u) for the (k+1)th order B-spline curve is as follows:

[0108]

[0109] In the formula, N i,k (u) is a k-th degree B-spline basis function, also known as a piecewise B-spline mixture function. Basis function N i,k (u) has the following de Boer-Cox recurrence relation:

[0110]

[0111] In the above equation, if the denominator is 0: if the numerator is also 0, then this term is also 0; if the numerator is not 0, then the denominator is conventionally set to 1. In the equation, u i Let be a continuously changing sequence of non-decreasing values ​​called a node vector, as shown below:

[0112] [u0,u1,…u k ,u k+1 ,…u n ,u n+1 ,…u n+k ]

[0113] Step 102: Construct a performance evaluation index for the climbing branch end trajectory, and construct a multi-objective optimization model for the climbing branch end trajectory based on the performance evaluation index to obtain the optimal time interval sequence between control points of the climbing branch end trajectory.

[0114] Step S21: Construct performance evaluation metrics for the climbing branch end trajectory, with the following specific metrics:

[0115] (1) Time objective function

[0116] The running time of each gait of the three-branch robot is defined as the sum of the time intervals between the control points of the trajectory at the end of the climbing branch. Therefore, minimizing the time is set as the first optimization objective, and its objective function equation is as follows:

[0117]

[0118] In the formula, n is the number of control points of the B-spline curve; Δt i This represents the time interval between the i-th control point and the (i+1)-th control point.

[0119] (2) Stationary objective function

[0120] Joint angular acceleration represents the rate of change of the joint angle, and it is directly related to the forces acting on the joint. The smoothness of the robot's motion trajectory is represented by the integral of the robot's joint impacts, and minimizing this is taken as the second optimization objective. The objective function equation is shown below:

[0121]

[0122] In the formula, n is the number of robot joints; This represents the joint impact of the i-th joint in relation to time.

[0123] (3) Energy consumption objective function

[0124] During robot movement, joint motors continuously output joint torques to cause corresponding joint movements. Therefore, by using the output torques of each joint to deduce joint energy consumption, the objective function equation for optimization objective three is established as follows:

[0125]

[0126] In the formula, n is the number of robot joints; τ represents the time-dependent angular velocity of the i-th joint; i (t) represents the time-dependent torque of the i-th joint, and its expression is as follows:

[0127]

[0128] Where, τ i n is the output torque of the i-th joint; i Let k be the external torque acting on the i-th member; i This represents the unit vector indicating the rotation direction of the i-th joint.

[0129] Step S22: Construct the constraints for the optimization model. The specific constraints are as follows:

[0130] (1) Joint angle constraint

[0131] Based on the mechanical structure, internal wiring layout, and installation location of the three-branch robot, joint angle constraints are established for the joint motion range of each branch to limit its movement space. For the three-branch robot studied in this paper, the joint angle constraints for each branch are as follows:

[0132] |θ i |≤|θ imax |=180°

[0133] In the formula, θ i Let θ be the angle of the i-th joint. imax Let be the maximum rotation angle of the i-th joint.

[0134] (2) Joint angular velocity constraint

[0135] Based on the relationship between joint angular velocity and robot control precision, and considering the upper limit of joint motor rotation speed, the value of joint angular velocity cannot be increased indefinitely. Therefore, the robot's joint angular velocity constraints are set as follows:

[0136]

[0137] In the formula, Let i be the angle of the i-th joint; Let be the maximum joint angular velocity of the i-th joint.

[0138] Step S23: Based on the above optimization objectives and constraints, construct a multi-objective optimization model for the trajectory at the end of the climbing branch, and solve for the optimal time interval sequence between control points, as shown in the following flowchart:

[0139] Based on three optimization objectives and two constraints, a multi-objective trajectory optimization model is constructed. The time interval sequence between control points at the end of the climbing branch is solved to obtain the multi-objective optimized trajectory at the end of the climbing branch. The specific process for solving the multi-objective optimized trajectory is as follows: Figure 4 As shown.

[0140] Step 103: Construct a performance evaluation index for the configuration of the transport branch at each control point of the climbing branch's end trajectory. Based on this performance evaluation index, construct a method for optimizing the configuration of the transport branch at each control point to obtain the optimal configuration of the transport branch at each control point.

[0141] (1) Climbing stability

[0142] When a three-branch robot climbs a truss surface, excessively rapid robot movement can easily cause excitation disturbances to the truss or the load being transported by the handling branches, affecting the robot's climbing stability. Therefore, the offset of the robot's center of mass relative to a reference position during the three-branch robot's gait is defined as an evaluation index for the robot's motion stability. The center of mass r of the three-branch robot... σ The calculation formula is:

[0143]

[0144] In the formula, M1, M2, and M3 represent the total mass of the three branches of the three-branch robot; m 1,i m 2,i m 3,i The masses of the joint links of the robot's three branches are r, respectively. 1,i r 2,i r 3,i These are the radius vectors of the mass points of each joint link of the robot's three branches relative to the origin O.

[0145] Therefore, the first optimization objective g1 is...

[0146]

[0147] In the formula, r is the centroid of the three-branch robot at the current control point.

[0148] (2) Load capacity

[0149] When a robot interacts with its environment, forces and torques are generated at the points of contact. The external force F acting on the end effector of the robotic arm... n With torque m n The combined six-dimensional vector

[0150]

[0151] Defined as the terminal generalized force vector. A seven-dimensional vector composed of the driving torques of each joint of the third branch of a three-branch robot.

[0152]

[0153] This is defined as a joint torque vector. The joint torque vector is used as the input to the robot's third branch drive mechanism, and the generalized force generated at the branch end is used as the output of the transport branch. The relationship between the two is established as follows.

[0154] Using the principle of virtual work, the generalized end force vector F corresponding to the joint moment vector τ is solved. n When the robotic arm is in equilibrium, the total virtual work generated by any virtual displacement is zero; that is, the virtual work generated by virtual displacement in the joint space is equal to the virtual work generated by virtual displacement in the operating space.

[0155] τ T ·δq=F T ·D

[0156] In the formula, τ T F is the transpose of the joint moment vector τ; δq is the virtual displacement in joint space; T is the transpose of the end-effector generalized force vector; D is the virtual displacement in the operation space.

[0157] The virtual displacement δq in joint space is not independent of the virtual displacement D in operation space; they must satisfy the geometric constraints specified by the Jacobian matrix of the robot's handling branch.

[0158] D=Jδq

[0159] In the formula, J is the Jacobian matrix of the robot handling branch.

[0160] Substituting, we can obtain

[0161] τ=J T F

[0162] F = (J T ) -1 τ

[0163] Therefore, the second optimization objective is:

[0164]

[0165] In the formula, f x ,f y ,f z F respectively n Components in three directions.

[0166] (3) Flexibility

[0167] Flexibility refers to the ability of a robot to control the speed of its end effector through joint movement, and it measures the isotropy of the robot's end effector.

[0168] To quantitatively evaluate the motion flexibility of robot branches, maneuverability is used as a metric for characterization:

[0169]

[0170] In the formula, ω is a measure of operability; det() is used to calculate the determinant of the square matrix; J(q) is the Jacobian matrix, J T (q) is the transpose of the Jacobian matrix; λ i For matrix J(q)J T The eigenvalues ​​of (q) are λ1≥λ2≥…≥λ m , i = 1, 2, ..., m; m is matrix J(q)J T The number of rows of (q); σ i Let ω be the singular value of the Jacobian matrix J(q). The operability index ω is between [0,1]. When w=0, the robotic arm is in a singular state. The larger the value of ω, the better the flexibility of the robot's branches.

[0171] For a given robot, once the structural parameters are determined, only changes in the joint angles of the robot branches affect their flexibility. Different robot branch configurations exhibit varying degrees of flexibility. Therefore, the third optimization objective is:

[0172] g3 = max(ω1, ω2, ..., ω n )

[0173] In the formula, n is the number of all configurations corresponding to the robot's handling branch.

[0174] Step 104: Based on the configuration of the transport branch at each control point, plan the joint trajectory sequence of the transport branch.

[0175] Based on the configuration of the transport branch at each control point, a series of angle values ​​(t) for a certain joint. i ,q i The expression for the quintic spline curve is as follows:

[0176] θ i(t)=a i (tt i ) 5 +b i (tt i ) 4 +c i (tt i ) 3 +d i (tt i ) 2 +e i (tt i )+q i

[0177] In the formula, θ i (t) represents the joint trajectory of this joint angle between the i-th control point and the (i+1)-th control point, where t is the current time, and t∈[t]. i ,t i+1 ], i = 1, 2, ..., n-1, where n is the number of control points in the end-point trajectory of this gait, q i Let be the joint angle corresponding to the i-th control point. i ,b i ,c i ,d i ,e i Therefore, the unknown quantity of the joint trajectory between the joint angle at the i-th control point and the (i+1)-th control point is...

[0178] Taking the derivatives of each order on both sides of the equation, we can obtain the following equation.

[0179]

[0180] In the formula, The joint angular velocity, Joint angular acceleration, Add accelerometer to the joint angle. Let 'a' be the derivative of the accelerometer with respect to time at the joint angle. Based on this joint angle at the initial and final moments, and at each control point, 'a' can be calculated. i ,b i ,c i ,d i ,e i The value of can be used to solve for the joint trajectory of the transport branch.

[0181] Step S51: Based on the positional relationship between the truss members and the kinematic model of the three-branch robot, solve the robot's workspace and classify various climbing gaits;

[0182] Step S52: Divide the three branches of the three-branch robot into two climbing branches and one transport branch according to their functions;

[0183] Step S53: Solve the end trajectory sequence of the climbing branch and the joint trajectory sequence of the transport branch respectively;

[0184] Step S54: Use the inverse kinematics solution of the three-branch robot to map the end trajectory sequence of the climbing branch to the joint space, and solve for the trajectory sequence of each joint of the climbing branch;

[0185] Step S55: Construct a smooth climbing gait for the three-branch robot truss by combining the joint trajectory sequences of the two branches.

[0186] According to the method provided in the embodiments of the present invention, a gait planning method for smooth climbing of a three-branch robot gantry was simulated.

[0187] The three-branch robot model used in this embodiment is as follows: Figure 1 As shown, the robot has three branches that are centrally symmetrical, and each branch has 7 degrees of freedom. Tables 1 and 2 are the initial DH parameters of the three-branch robot.

[0188] Table 1. DH parameters for branch 1 and branch 2

[0189]

[0190] Table 2 DH parameters for branch 1 and branch 3

[0191]

[0192] Based on the gait characteristics and truss structure, the end-point trajectory control points of the two climbing branches of the three-branch robot are designed. The end-point trajectories of the two branches are solved using B-spline curves, as shown below. Figure 3 As shown. Then, using time, joint impact, and energy consumption as trajectory optimization objectives, and based on the MATLAB language, according to... Figure 4 The process shown illustrates a simulation experiment for multi-objective trajectory optimization based on NSGA-II. The simulation experiment settings are shown below.

[0193] The poses of the end effector of branch 1 and the initial and final poses of the end effector of branch 2 of the three-branch robot are shown in Table 3 below:

[0194] Table 3. End poses of branch 1 and branch 2

[0195] Initial pose of branch 2 end endPE_2_ini={0,-0.4566025,0,0,90,-180} Desired pose at the end of branch 2 endPE_2_des={0.5,-0.4566025,0,0,90,-180}

[0196] The coordinates of the control points in the trajectory are shown in Table 4 below:

[0197] Table 4 Control Point Coordinates

[0198] <![CDATA[P1]]> 0 -0.4566025 0 <![CDATA[P2]]> 0.05 -0.4566025 0.1 <![CDATA[P3]]> 0.2 -0.4566025 0.1 <![CDATA[P4]]> 0.45 -0.4566025 0.1 <![CDATA[P5]]> 0.5 -0.4566025 0

[0199] The parameter settings for the NSGA-II algorithm program are shown in Table 5 below:

[0200] Table 5 Parameter Settings

[0201]

[0202] After initializing the simulation program, run the program and use the NSGA-II algorithm to optimize the multi-objective climbing trajectory. After 60 iterations (generations), the final simulation results are as follows: Figure 5 As shown in the figure, the three-dimensional surface formed by the scatter plot is the Pareto front of this multi-objective optimization result. The three coordinate axes in the figure represent the three objective functions of trajectory optimization. The values ​​of the decision variables and optimization objectives corresponding to points A, B, C, and D are shown in Table 6. Figure 5 It can be seen that the closer to point A, the shorter the climbing time; the closer to point B, the less energy is consumed during climbing; and the closer to point D, the smaller the impact on the joints during movement. Furthermore, as the movement time shortens, the total impact on the joints and the energy consumed during movement both increase; as the total impact on the joints decreases, the energy consumed during movement also decreases, while the movement time increases; and as energy consumption decreases, the total impact on the joints also decreases, while the movement time increases. The movement time and energy consumption / joint impact indicators conflict but are inversely proportional, consistent with theory, reflecting the conflict between the three optimization objectives of the robot during movement.

[0203] Table 6 Optimization Target Results for Points A, B, C, and D

[0204]

[0205] The optimal individual solution is selected based on the Pareto optimal solution set. First, the non-dominated solution set obtained by the multi-objective trajectory optimization algorithm is compared and selected. As the distance from point D increases, the time optimization objective decreases significantly, while the impact and energy consumption optimization objectives increase slightly. Second, considering that the optimized trajectory does not have a clear weighting for the three optimization objectives, the vertex C of the three-dimensional arc surface is ultimately selected as the optimization result. This is because point C has relatively low energy consumption and joint impact while the time spent moving does not increase significantly, making the values ​​of all three optimization objectives reach a reasonable level. Therefore, point C is ultimately chosen as the final result of this optimization.

[0206] Subsequently, the configuration of the three-branch robot's handling branch was optimized based on climbing stability, load capacity, and flexibility. The parameter settings of the algorithm program are shown in Table 7.

[0207] Table 7 Parameter Setting Table

[0208]

[0209]

[0210] After initialization, the simulation program was run, and the NSGA-II algorithm was used for multi-objective trajectory optimization of the climbing trajectory. After 200 iterations (generations), the final simulation results are as follows: Figure 6 As shown.

[0211] The technical solutions of the embodiments of the present invention have the following beneficial effects:

[0212] In the technical solution of this invention, a gait planning method for a three-branch robot based on smooth truss climbing is designed. After modeling the known truss environment and the three-branch robot, control points for the end trajectories of the robot's two climbing branches are designed. The B-spline curve planning method is used to plan the end trajectory sequence of the climbing branches. A performance evaluation index for the end trajectory of the climbing branches is constructed. The NSGA-II algorithm is used to perform multi-objective optimization on the end trajectory of the climbing branches to obtain the optimal time interval sequence between the end trajectory control points. A performance evaluation index for the configuration at the control points of the transport branch is constructed. A configuration optimization strategy at the control points of the transport branch is constructed to obtain the optimal configuration of the transport branch at each control point. A quintic spline curve planning method is used to plan the trajectory sequence of the transport branch. Based on the end trajectory sequence of the climbing branches and the joint trajectory sequence of the transport branch, a gait planning method for smooth truss climbing of the three-branch robot is constructed to plan the climbing gait of the three-branch robot on the truss.

[0213] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0214] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A gait planning method for a three-branch robot based on truss-based stable climbing, characterized in that, The method includes: Step S1: Based on the truss structure and the kinematic model of the three-branch robot, design the end trajectory control points of the two climbing branches of the three-branch robot, and plan the end trajectory sequence of the climbing branches; Step S2: Construct a performance evaluation index for the climbing branch end trajectory, and construct a multi-objective optimization model for the climbing branch end trajectory based on the performance evaluation index to obtain the optimal time interval sequence between control points of the climbing branch end trajectory; Step S3: Construct a performance evaluation index for the configuration of the transport branch at each control point of the trajectory at the end of the climbing branch. Based on this performance evaluation index, construct a method for optimizing the configuration of the transport branch at the control point to obtain the optimal configuration of the transport branch at each control point. Step S4: Based on the configuration of the transport branch at each control point, plan the joint trajectory sequence of the transport branch; Step S5: Based on the end trajectory sequence of the climbing branch and the joint trajectory sequence of the transport branch, construct a gait planning method for the smooth climbing of the three-branch robot gantry; The performance evaluation indicators mentioned in step S3 include: (1) Climbing stability The offset of the robot's center of mass relative to a reference position during the gait of a three-branch robot is defined as an evaluation index for the robot's motion stability. The center of mass of the three-branch robot... The calculation formula is: In the formula, These represent the total mass of the three branches of the three-branch robot; These represent the masses of the joint links of the robot's three branches. The mass points of each joint link of the robot's three branches relative to the origin. The radius vector; Therefore, the first optimization objective for In the formula, Let the center of mass of the three-branch robot be the current control point; (2) Load capacity When a robot interacts with its environment, forces and torques are generated at the points of contact, which affect the external forces acting on the end effector of the robotic arm. With torque The combined six-dimensional vector Defined as the terminal generalized force vector, it is a seven-dimensional vector composed of the driving torques of each joint of the transport branch of the three-branch robot. The joint torque vector is set as the input to the robot's handling branch drive mechanism, and the generalized force generated at the end of the branch is used as the output of the handling branch. The relationship between the two is established as follows: Using the principle of virtual work, the vector of joint torques can be solved. Corresponding end generalized force vector When the robotic arm is in equilibrium, the sum of the virtual work generated by any virtual displacement is zero. That is, the virtual work generated by virtual displacement in the joint space is equal to the virtual work generated by virtual displacement in the operating space. In the formula, Joint torque vector transpose; This represents the virtual displacement in the joint space; This is the transpose of the terminal generalized force vector; This refers to virtual displacement within the operating space; Virtual displacement in joint space Virtual displacement with operating space It is not independent; it should satisfy the geometric constraints specified by the Jacobian matrix of the robot handling branch, i.e. In the formula, The Jacobian matrix for the robot's handling branch; Substituting, we can obtain Therefore, the second optimization objective is: In the formula, They are respectively Components in three directions; (3) Flexibility To quantitatively evaluate the motion flexibility of robot branches, maneuverability is used as a metric for characterization: In the formula, It serves as a metric for operability; To calculate the determinant of a square matrix; For Jacobian matrices, This is the transpose of the Jacobian matrix; For matrix eigenvalues, , ; For matrix number of rows; Jacobian matrix Singular values; operability index exist Between, when At that time, the robotic arm was in a strange state. The larger the value, the better the flexibility of the robot's branches; For a given robot, once the structural parameters are determined, only changes in the joint angles of the robot branches will affect the flexibility of the robot branches. The flexibility of the robot branches varies depending on their configuration; therefore, the third optimization objective is: In the formula, This represents the number of all configurations corresponding to the robot's handling branches.

2. The method according to claim 1, characterized in that, Step S1 includes: Let the trajectory at the end of the climbing branch be a B-spline curve, and its control point sequence be... The number of control points is ; In the formula, The control points for the end trajectory of the climbing branch are the control polygons of the end trajectory, which are obtained by sequentially linking these control points; thus, the following can be obtained. Mathematical expression of B-spline curve As shown below: In the formula, for Secondary B-spline basis functions, also known as B-spline piecewise mixture functions; basis functions It has the following de Boer-Cox recurrence relation: If the denominator in the above equation is 0: if the numerator is also 0, then this term is also 0; if the numerator is not 0, then the denominator is conventionally set to 1; in the equation, Let be a continuously changing sequence of non-decreasing values ​​called a node vector, as shown below: 。 3. The method according to claim 1, characterized in that, Step S2 includes: Step S21: Construct performance evaluation metrics for the climbing branch end trajectory, with the following specific metrics: (1) Time objective function The running time of each gait of the three-branch robot is set as the sum of the time intervals between the trajectory control points at the end of the climbing branch. Therefore, minimizing the time is set as the first optimization objective, and its objective function equation is as follows: In the formula, This represents the number of control points for the B-spline curve. Indicates the first The control point and the first The time interval between control points; (2) Stationary objective function Joint angular acceleration represents the rate of change of joint angles. Joint angular acceleration is directly related to the forces acting on the joints. The smoothness of the robot's motion trajectory is represented by the integral of the robot's joint impacts, and minimizing it is taken as the second optimization objective. The objective function equation is shown below: In the formula, This refers to the number of robot joints; Indicates the first time related to time Joint impact on each joint; (3) Energy consumption objective function During robot movement, joint motors continuously output joint torques to cause corresponding joint movements. Therefore, by using the output torques of each joint to deduce joint energy consumption, the objective function equation for optimization objective three is established as follows: In the formula, This refers to the number of robot joints; Indicates the first time related to time Angular velocity of each joint; Indicates the first time related to time The torque of each joint is expressed as follows: in, For the first Output torque of each joint; For the first The external torque acting on the root member; Indicates the first Unit vector for the direction of joint rotation; Step S22: Construct the constraints for the optimization model. The specific constraints are as follows: (1) Joint angle constraint Based on the mechanical structure, internal wiring layout, and installation location of the three-branch robot, joint angle constraints are established for the joint motion range of each branch to limit its motion space. For the three-branch robot studied in this paper, the joint angle constraints for each branch are as follows: In the formula, For the first Each joint angle; For the first Maximum rotation angle of each joint; (2) Joint angular velocity constraints Based on the relationship between joint angular velocity and robot control precision, and considering the upper limit of joint motor rotation speed, the value of joint angular velocity cannot be increased indefinitely. Therefore, the robot joint angular velocity constraints are set as follows: In the formula, For the first Each joint angle; For the first Maximum joint angular velocity of each joint; Step S23: Based on the above optimization objectives and constraints, construct a multi-objective optimization model for the trajectory at the end of the climbing branch, and solve for the optimal time interval sequence between control points, as shown in the following flowchart: Based on three optimization objectives and two constraints, a multi-objective trajectory optimization model is constructed to solve the time interval sequence between the trajectory control points at the end of the climbing branch, thereby obtaining the multi-objective optimized trajectory at the end of the climbing branch.

4. The method according to claim 1, characterized in that, Step S4 includes: Based on the configuration of the transport branch at each control point, a series of angle values ​​for a certain joint. The expression for the quintic spline curve is as follows: In the formula, Therefore, the joint angle is in the... The control point and the first Joint trajectories between control points For the current time, , Therefore, the number of control points for the end trajectory in this gait. For the first The joint angles corresponding to each control point; Therefore, the joint angle is in the... The control point and the first Unknown quantities in the joint trajectory between control points; Taking the derivatives of each order on both sides of the equation, we can obtain the following equation. In the formula, The joint angular velocity, Joint angular acceleration, Add accelerometer to the joint angle. The derivative of the accelerometer with respect to time is given by the joint angle. Based on this joint angle at the initial and final moments, and the joint angles at each control point, the following solution is obtained: The value of can be used to solve for the joint trajectory of the transport branch.

5. The method according to claim 1, characterized in that, Step S5 includes: Step S51: Based on the positional relationship between the truss members and the kinematic model of the three-branch robot, solve the robot's workspace and classify various climbing gaits; Step S52: Divide the three branches of the three-branch robot into climbing branches and transport branches according to their functions; Step S53: Solve the end trajectory sequence of the climbing branch and the joint trajectory sequence of the transport branch respectively; Step S54: Use the inverse kinematics solution of the three-branch robot to map the end trajectory sequence of the climbing branch to the joint space, and solve for the trajectory sequence of each joint of the climbing branch; Step S55: Construct a smooth climbing gait for the three-branch robot truss by combining the joint trajectory sequences of the two branches.

Images