Modularized robot trajectory optimization method

Abstract

The embodiment of the invention provides a modular robot trajectory optimization method, which realizes modular robot joint trajectory optimization, and comprises the following steps: obtaining a modular robot kinematic chain matrix and a modular robot expected tail end motion trajectory, obtaining a current motion participation degree-of-freedom number of the modular robot according to the modular robot kinematic chain matrix, and obtaining a modular robot joint trajectory; the motion distribution vector of the modular robot is obtained according to the current motion participation degree of freedom of the modular robot, the trajectory optimization method of the modular robot is designed according to the expected tail end motion trajectory of the modular robot and the motion distribution vector of the modular robot, and the motion distribution vector of the modular robot is optimized based on a grey wolf optimization algorithm framework. And after the motion distribution vector of the modular robot is coded, the optimal joint track of the modular robot is obtained. According to the technical scheme provided by the embodiment of the invention, joint trajectory optimization with small end trajectory error, small total energy consumption of movement and balanced energy consumption among modules of the modular robot can be realized.

Description

[Technical field]

[0001] The invention relates to a modular robot trajectory optimization method and belongs to the field of robot trajectory optimization. [Background technology]

[0002] With the development of the times and technology, countries around the world have invested heavily in space exploration. With the successful construction of my country's space station, it marks an important step for my country in the fields of aerospace and deep space exploration. With it come more arduous space tasks such as lunar construction, Mars exploration, and asteroid defense. Space missions involve a large number of task objects such as trusses, connectors, cables, etc. with different masses, stiffness, and sizes, and the operating range and precision are also very different. The existing space robots are all designed and developed for a certain task. Because of their fixed properties such as connection relationship, joint arrangement, and degree of freedom, the performance adjustment range is limited and it is difficult to be competent for complex and diverse on-orbit tasks. The most significant advantage of modular robots is that they have reconfigurable characteristics. By changing the number of modules, module connection relationship, and module connection orientation, different task requirements can be met. Therefore, using modular robots to perform space missions is a key development direction of my country's aerospace engineering.

[0003] In modular robots, classic motion planning algorithms such as search-based and sampling-based have significantly improved modular autonomy and task execution efficiency. However, due to the variability of modular robot morphology and the diversity of task requirements, these algorithms need to be adaptively improved when applied to modular robots. Furthermore, whether modular robots are performing specific tasks or performing self-reconstruction movements, their essence is the movement of each joint trajectory, so that the end moves according to the predetermined trajectory. In other words, indicators such as operation time and end accuracy in modular robot tasks are closely related to the motion trajectory of the joints, and the motion trajectory of the robot can be improved through trajectory optimization. Especially in a changing task environment, effective trajectory optimization can not only ensure that the robot moves accurately according to the predetermined trajectory, but also ensure the best use of resources through optimization strategies, indirectly improving the reliability and task completion of the robot. Therefore, trajectory optimization of modular robots is a key link in using modular robots to perform space tasks. [Summary of the invention]

[0004] In view of this, the present invention provides a modular robot trajectory optimization method to achieve joint trajectory optimization of the modular robot.

[0005] An embodiment of the present invention provides a modular robot trajectory optimization method, comprising:

[0006] According to the set base module and terminal module in the modular robot motion, the modular robot motion chain matrix and the expected terminal motion trajectory of the modular robot are obtained.

[0007] According to the modular robot kinematic chain matrix, the number of degrees of freedom of the current motion of the modular robot is obtained.

[0008] According to the number of degrees of freedom involved in the current motion of the modular robot, the motion distribution vector of the modular robot is obtained.

[0009] According to the expected terminal motion trajectory of the modular robot and the motion distribution vector of the modular robot, a trajectory optimization method for the modular robot is designed. Based on the framework of the Grey Wolf optimization algorithm, the motion distribution vector of the modular robot is encoded to obtain the optimal joint trajectory of the modular robot.

[0010] The modular robot refers to a robot composed of multiple modules, and the motion distribution vector refers to the arrangement relationship vector of the motion joints and stationary joints of the modular robot.

[0011] In the above method, the step of obtaining the kinematic chain matrix of the modular robot and the desired terminal motion trajectory of the modular robot according to the set base module and terminal module in the motion of the modular robot includes:

[0012]

[0013] Among them, KineChain is the modular robot kinematic chain matrix, s k1 represents the input interface of module unit k in the kinematic chain, s k2 represents the output interface of module unit k in the kinematic chain, d k It represents the connection orientation value (1 or 2) between the k-1 module unit and the k module unit, and the end d n Set to 0, n means there are n modules in total; is the expected end motion trajectory of the modular robot, which is a function of time t.

[0014] In the above method, the step of obtaining the motion distribution vector of the modular robot according to the number of degrees of freedom involved in the current motion of the modular robot comprises:

[0015]

[0016] Among them, n is the number of degrees of freedom of the current motion of the modular robot, s i is the capacity of the i-th joint, when s i =1 means that the current joint can rotate in this motion trajectory. i =0 means that the current joint remains stationary in this motion trajectory.

[0017] In the above method, the modular robot trajectory optimization method is designed according to the expected terminal motion trajectory of the modular robot and the motion distribution vector of the modular robot. After encoding the motion distribution vector of the modular robot based on the framework of the gray wolf optimization algorithm, the optimal joint trajectory of the modular robot is obtained, including:

[0018] According to the modular robot motion distribution vector, an encoding mechanism for the modular robot motion distribution vector is obtained.

[0019] According to the motion distribution vector of the modular robot, the Jacobian matrix calculation formula for the motion distribution vector of the modular robot is obtained.

[0020] Build a modular robot trajectory optimization model.

[0021] In the above method, the step of obtaining a coding mechanism for the motion distribution vector of the modular robot according to the motion distribution vector of the modular robot comprises:

[0022] The motion distribution vector of the modular robot is mapped to a decimal natural number. The mapping formula is:

[0023] S dec =s n ×2 n +s n-1 ×2 n-1 +…+s 1 ×2+s 0

[0024] Among them, S dec is the motion distribution in decimal notation, n is the number of degrees of freedom involved in the motion of the modular robot unit, s i is the capacity of the i-th joint in the corresponding motion distribution vector S; the motion distribution S in decimal representation dec When mapping to the modular robot motion distribution vector S, it is also based on the base conversion formula, but it is necessary to pay attention to filling zeros in the high bits when there are insufficient bits.

[0025] In the above method, the Jacobian matrix calculation formula for the modular robot motion distribution vector is obtained based on the modular robot motion distribution vector, including:

[0026] Spatial Jacobian matrix J s Based on the spinor, it can be expressed as: The calculation formula is:

[0027] J s =Ad(ζ i ) i

[0028] J sEach column vector is the motion spin coordinate of each joint i corresponding to the motion axis with respect to the base coordinate system in the current configuration. (There are n joints in total), Ad(·) represents the adjoint transformation, ζ i is the motion spinor coordinate in the current joint i coordinate system, and the spatial Jacobian matrix under the variable motion distribution is Recalculate:

[0029]

[0030] The matrix D is a diagonal matrix constructed based on the motion distribution vector S, where the diagonal elements d j Satisfy j =s j , combined with the position vector P of the robot's end in the base coordinate system under the current configuration st By transforming the spatial Jacobian matrix, the Jacobian matrix calculation formula of the modular robot under variable motion distribution is obtained as follows:

[0031]

[0032] Among them, I 3 is the three-dimensional unit matrix, O 3 is a three-dimensional zero matrix, from which the Jacobian matrix calculation formula for the motion distribution vector of the modular robot is obtained.

[0033] In the above method, the step of establishing a modular robot trajectory optimization model comprises:

[0034] Establish the relevant objective functions of total energy consumption, balanced energy consumption, and terminal error index of the modular robot trajectory optimization model; first, the relevant objective function of the total energy consumption index:

[0035]

[0036] Among them, q(t) is the joint motion trajectory of the modular robot, which is a function of time t, C i is the total energy consumption of the i-th module, N is the number of modules; followed by the objective function related to the balanced energy consumption index:

[0037]

[0038] Among them, max means obtaining the maximum energy consumption of the module, and min means obtaining the minimum energy consumption of the module; again, it is the objective function related to the terminal error index:

[0039]

[0040] in, is the expected end motion trajectory of the modular robot, and p(t) is the actual end motion trajectory of the modular robot, both of which are functions of time t. 0 is the start time of the movement, t F The moment when the movement ends;

[0041] Establish the constraints of the modular robot trajectory optimization model; first, the joint angle constraints:

[0042] θ min ≤θ i ≤θ max

[0043] Among them, θ min is the minimum joint angle limit, θ max is the maximum joint angle limit, θ i is the joint angle of the i-th joint; followed by the joint torque constraint:

[0044] τ min ≤τ i ≤τ max

[0045] Among them, τ min is the minimum joint torque limit, τ max is the maximum joint torque limit, τ i is the torque of the i-th joint; again is the joint velocity constraint:

[0046] v min ≤v i ≤v max

[0047] Among them, v min is the minimum joint velocity limit, v max is the maximum joint speed limit, v i is the velocity of the i-th joint; then is the maximum total energy consumption constraint:

[0048]

[0049] Among them, C i is the total energy consumption of the i-th module, N is the number of modules, E max is the maximum total energy consumption limit; finally, a modular robot trajectory optimization model is established:

[0050]

[0051] Among them, q(t) is the joint trajectory of the modular robot, which needs to be combined with the motion distribution vector S and the expected terminal motion trajectory of the modular robot. Calculate; first, the expected end motion trajectory of the module robot Perform differential calculation to obtain the desired terminal motion velocity sequence of the modular robot Secondly, the modular robot joint velocity sequence is calculated based on the following formula

[0052]

[0053] in, The pseudo-inverse of the Jacobian matrix of the motion distribution vector S is calculated; the joint trajectory of the modular robot is calculated again based on the following formula:

[0054]

[0055] Among them, q t+1 is the joint angle at time t+1 in q(t), q t is the joint angle at time t in q(t), for The joint velocity at time t, Δt is the interval between two moments, and the modular robot joint trajectory q(t) is obtained through such iteration; the motion distribution vector S is used as the decision variable in the optimization model.

[0056] It can be seen from the above technical solutions that the embodiments of the present invention have the following beneficial effects:

[0057] In the technical solution of the embodiment of the present invention, according to the set base module and terminal module in the modular robot movement, the modular robot motion chain matrix is ​​obtained, according to the modular robot motion chain matrix, the number of degrees of freedom participating in the current motion of the modular robot is obtained, according to the number of degrees of freedom participating in the current motion of the modular robot, the modular robot motion distribution vector is obtained, according to the modular robot terminal motion trajectory and the modular robot motion distribution vector, the modular robot trajectory optimization method is designed, based on the gray wolf optimization algorithm framework, the modular robot motion distribution vector is encoded, and the optimal joint trajectory of the modular robot is obtained. Therefore, the modular robot trajectory optimization can be realized, combined with the characteristics of the modular robot, it can be applied to modular robots of different types and tasks, and can also effectively avoid the excessive use of a single module, extend the service life, and enhance the reliability of the modular robot.

Brief Description of the Drawings

[0058] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It is obvious that the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creativity and labor.

[0059] Figure 1is a flow chart of a modular robot trajectory optimization method provided by an embodiment of the present invention;

[0060] Figure 2 is a schematic diagram of a modular robot in an embodiment of the present invention;

[0061] Figure 3 Schematic diagram of the expected terminal motion trajectory of the modular robot in an embodiment of the present invention. [Specific embodiment]

[0062] In order to better understand the technical solution of the present invention, the embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0063] It should be clear that the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0064] The present invention provides a modular robot trajectory optimization method, please refer to Figure 1 , which is a flow chart of the modular robot trajectory optimization method provided by an embodiment of the present invention, such as Figure 1 As shown, the method comprises the following steps:

[0065] Step 101, according to the set base module and terminal module in the modular robot motion, obtain the modular robot motion chain matrix and the modular robot expected terminal motion trajectory.

[0066] Modular Robot Schematic Reference Figure 2 As shown, there are 10 modular units with 22 degrees of freedom. This modular robot is composed of ten modular units connected in series, with the modular unit numbered 1 as the base node, and the modular unit numbered 2 connected to it and extending two branches, namely: 3→4→5→6 (kinematic chain 1) and 7→8→9→10 (kinematic chain 2), with joints 12 and 22 as the two ends, and the corresponding kinematic chain matrix is ​​established as follows:

[0067]

[0068] The expected terminal motion trajectory of the modular robot is as follows Figure 3 shown.

[0069] Step 102, obtaining the number of degrees of freedom involved in the current motion of the modular robot according to the kinematic chain matrix of the modular robot.

[0070] The number of degrees of freedom of kinematic chain 1 and kinematic chain 2 is obtained to be 9.

[0071] Step 103, obtaining a motion distribution vector of the modular robot according to the number of degrees of freedom involved in the current motion of the modular robot.

[0072] Set the motion distribution vector of kinematic chain 1 to S 1 , the dimension is 1×9; the motion distribution vector of kinematic chain 2 is S 2 , with dimensions of 1×9.

[0073] Step 104, the modular robot's expected terminal motion trajectory and the modular robot's motion distribution vector are designed, and a modular robot trajectory optimization method is designed. Based on the Grey Wolf optimization algorithm framework, the modular robot's motion distribution vector is encoded to obtain the modular robot's optimal joint trajectory.

[0074] Set the end trajectory of the two kinematic chains to move from the zero position to:

[0075]

[0076] The parameters of the Gray Wolf Optimization Algorithm are set as shown in the following table.

[0077]

[0078]

[0079] Based on the Gray Wolf optimization algorithm process:

[0080] ① Divide the levels: set the most suitable solution as alpha (α), the second and third most suitable solutions as beta (β) and delta (δ), and the remaining solutions as omega (ω). Subsequent searches are guided by α, β and δ.

[0081] ② Surround the prey: Use the following formula to simulate the wolf pack surrounding the prey, and the prey is the possible optimal solution position:

[0082] D=|CX p (t)-X t (t)|

[0083] X(t+1)=X(t)-AD

[0084] Among them, X p (t) is the position vector of the prey, X t (t) is the position of the wolf, A is a random vector that gradually shrinks, simulating the process of the encirclement shrinking and attacking, and C is a random vector. In this way, the arbitrary position of the wolf can be simulated, and by setting the way that A and C change with iterations, the process of gradually shrinking the encirclement can be simulated;

[0085] ③Searching for prey: The process of searching for prey is usually dominated by gray wolves of the α, β, and δ levels. The position of the wolf pack depends on the positions of the α, β, and δ gray wolves, and the position of the wolf pack is continuously updated iteratively based on this:

[0086] D i =|C j .X i -X|

[0087] X j =X i -A j .D i

[0088]

[0089] Among them, i={α β δ}, j={1 2 3}

[0090] Through random variable control, the α, β and δ gray wolves are closer and closer to the prey during the iteration process, so that the position of the entire gray wolf group converges to the vicinity of the prey. This process ensures that the GWO algorithm can find the optimal solution.

[0091] The motion distribution encoding and decoding pseudo code is as follows:

[0092]

[0093]

[0094] The trajectory optimization process is:

[0095] Step 1: According to the given task, clarify the modular robot topology and its terminal trajectory. Set the number of gray wolf populations, the number of Pareto solutions, and the maximum number of iterations.

[0096] Step 2: Generate an initialized population, encode the decision variables according to the above encoding method and set them as the wolf group positions, use the proposed inverse kinematics solution method under variable motion distribution to solve the joint angles and calculate the objective function as the cost.

[0097] Step 3: Based on the current population, generate a new generation of population by selecting alpha, beta, and delta type gray wolves as the dominant ones.

[0098] Step 4: Verify the individual validity according to the constraints, and select the non-dominated solution according to the cost to update the Pareto solution set.

[0099] Step 5: Determine whether the evolutionary generation reaches the maximum value set in step 1. If the condition is met, the individual with the largest fitness in the evolution process is taken as the optimal case. Otherwise, go to step 3.

[0100] The technical solution of the embodiment of the present invention has the following beneficial effects:

[0101] In the technical solution of the embodiment of the present invention, according to the set base module and terminal module in the modular robot movement, the modular robot motion chain matrix is ​​obtained, according to the modular robot motion chain matrix, the number of degrees of freedom participating in the current motion of the modular robot is obtained, according to the number of degrees of freedom participating in the current motion of the modular robot, the modular robot motion distribution vector is obtained, according to the modular robot terminal motion trajectory and the modular robot motion distribution vector, the modular robot trajectory optimization method is designed, based on the gray wolf optimization algorithm framework, the modular robot motion distribution vector is encoded, and the optimal joint trajectory of the modular robot is obtained. Therefore, the modular robot trajectory optimization can be realized, combined with the characteristics of the modular robot, it can be applied to modular robots of different types and tasks, and can also effectively avoid the excessive use of a single module, extend the service life, and enhance the reliability of the modular robot.

[0102] 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 in the scope of protection of the present invention.

[0103] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A modular robot trajectory optimization method, characterized in that: The method comprises: According to the set base module and terminal module in the modular robot motion, the modular robot motion chain matrix and the expected terminal motion trajectory of the modular robot are obtained. According to the modular robot kinematic chain matrix, the number of degrees of freedom of the current motion of the modular robot is obtained. According to the number of degrees of freedom involved in the current motion of the modular robot, the motion distribution vector of the modular robot is obtained. According to the expected terminal motion trajectory of the modular robot and the motion distribution vector of the modular robot, a trajectory optimization method for the modular robot is designed. Based on the framework of the Grey Wolf optimization algorithm, the motion distribution vector of the modular robot is encoded to obtain the optimal joint trajectory of the modular robot. The modular robot refers to a robot composed of multiple modules, and the motion distribution vector refers to the arrangement relationship vector of the motion joints and stationary joints of the modular robot.

2. The method according to claim 1, characterized in that According to the set base module and terminal module in the modular robot motion, the modular robot motion chain matrix and the expected terminal motion trajectory of the modular robot are obtained, including: Among them, KineChain is the modular robot kinematic chain matrix, s k1 represents the input interface of module unit k in the kinematic chain, s k2 represents the output interface of module unit k in the kinematic chain, d k It represents the connection orientation value between the k-1 module unit and the k module unit, and n represents the number of modules.

3. The method according to claim 1, characterized in that According to the number of degrees of freedom involved in the current motion of the modular robot, the motion distribution vector of the modular robot is obtained, including: Among them, n is the number of degrees of freedom of the current motion of the modular robot, s j is the capacity of the i-th joint, when s i =1 means that the current joint can rotate in this motion trajectory. i =0 means that the current joint remains stationary in this motion trajectory.

4. The method according to claim 1, characterized in that: According to the expected terminal motion trajectory of the modular robot and the motion distribution vector of the modular robot, a modular robot trajectory optimization method is designed. Based on the framework of the Grey Wolf optimization algorithm, the motion distribution vector of the modular robot is encoded to obtain the optimal joint trajectory of the modular robot, including: According to the modular robot motion distribution vector, an encoding mechanism for the modular robot motion distribution vector is obtained. According to the motion distribution vector of the modular robot, the Jacobian matrix calculation formula for the motion distribution vector of the modular robot is obtained. Build a modular robot trajectory optimization model.

5. The method according to claim 4, characterized in that According to the modular robot motion distribution vector, an encoding mechanism for the modular robot motion distribution vector is obtained, including: The motion distribution vector of the modular robot is mapped to a decimal natural number. The mapping formula is: S dec =s n ×2 n +s n-1 ×2 n-1 +…+s1×2+s0 Among them, S dec is the motion distribution in decimal notation, n is the number of degrees of freedom involved in the motion of the modular robot unit, s j is the capacity of the i-th joint in the corresponding motion distribution vector S; the motion distribution S in decimal representation dec When mapping to the modular robot motion distribution vector S, it is also based on the base conversion formula, but it is necessary to pay attention to filling zeros in the high bits when there are insufficient bits.

6. The method according to claim 4, characterized in that According to the motion distribution vector of the modular robot, the Jacobian matrix calculation formula for the motion distribution vector of the modular robot is obtained, including: Spatial Jacobian matrix J s Based on the spinor, it can be expressed as: The calculation formula is: J s =Ad(ζ i )g i Among them, Js 各 The column vector is the motion spinor coordinates of the motion axis corresponding to each joint i in the current configuration with respect to the base coordinate system n represents the number of joints, Ad(·) represents the adjoint transformation, ζ i is the motion spinor coordinate in the current joint i coordinate system, and the spatial Jacobian matrix under the variable motion distribution is Recalculate: The matrix D is a diagonal matrix constructed based on the motion distribution vector S, where the diagonal elements d j Satisfy j =s j , combined with the position vector P of the robot's end in the base coordinate system under the current configuration st By transforming the spatial Jacobian matrix, the Jacobian matrix calculation formula of the modular robot under variable motion distribution is obtained as follows: Among them, I3 is the three-dimensional unit matrix and O3 is the three-dimensional zero matrix.

7. The method according to claim 4, characterized in that Build a modular robot trajectory optimization model, including: Establish the relevant objective functions of total energy consumption, balanced energy consumption, and terminal error index of the modular robot trajectory optimization model; first, the relevant objective function of the total energy consumption index: Among them, q(t) is the joint motion trajectory of the modular robot, which is a function of time t, C i is the total energy consumption of the i-th module, N is the number of modules; followed by the objective function related to the balanced energy consumption index: Among them, max means obtaining the maximum energy consumption of the module, and min means obtaining the minimum energy consumption of the module; again, it is the objective function related to the terminal error index: in, is the expected end motion trajectory of the modular robot, p(t) is the actual end motion trajectory of the modular robot, both are functions of time t, t0 is the start time of the motion, t F The moment when the movement ends; Establish the constraints of the modular robot trajectory optimization model; first, the joint angle constraints: i min ≤θ i ≤θ max Among them, θ min is the minimum joint angle limit, θ max is the maximum joint angle limit, θ i is the joint angle of the i-th joint; followed by the joint torque constraint: t min ≤τ i ≤τ max Among them, τ min is the minimum joint torque limit, τ max is the maximum joint torque limit, τ i is the torque of the i-th joint; again is the joint velocity constraint: in min ≤in i ≤in max Among them, v min is the minimum joint velocity limit, v max is the maximum joint speed limit, v i is the velocity of the i-th joint; then is the maximum total energy consumption constraint: Among them, C i is the total energy consumption of the i-th module, N is the number of modules, E max is the maximum total energy consumption limit; finally, a modular robot trajectory optimization model is established: find q(t) stθ min ≤θ i ≤θ max , in min ≤in i ≤in max , t min ≤τ i ≤τ max , Among them, g(t) is the joint trajectory of the modular robot, which needs to be combined with the motion distribution vector S and the expected terminal motion trajectory of the modular robot. Calculate; first, the expected end motion trajectory of the module robot Perform differential calculation to obtain the desired terminal motion velocity sequence of the modular robot Secondly, the modular robot joint velocity sequence is calculated based on the following formula in, The pseudo-inverse of the Jacobian matrix of the motion distribution vector S is calculated; the joint trajectory of the modular robot is calculated again based on the following formula: Among them, q t+1 is the joint angle at time t+1 in q(t), q t is the joint angle at time t in q(t), for The joint velocity at time t, Δt is the interval between two moments, and the modular robot joint trajectory q(t) is obtained through such iteration; the motion distribution vector S is used as the decision variable in the optimization model.

Images