Rope-driven snakelike arm self-adaptive control method and system based on rigidity pre-distribution

Abstract

The invention relates to a rope-driven snakelike arm self-adaptive control method and system based on rigidity pre-distribution, and the method comprises the steps: obtaining the expected tail end rigidity and the actual tail end rigidity of a rope-driven snakelike arm, and building a total fitness function according to the rigidity of each joint of the rope-driven snakelike arm; taking the escape gradient as guidance, calculating the rope tension of each joint of the rope-driven snakelike arm by utilizing a total fitness function, and controlling the rigidity of each joint and the rigidity of the tail end by controlling the rope tension; a self-adaptive damping change rate is designed based on the contact force error to establish a self-adaptive admittance control model, and parameters of the self-adaptive admittance control model are adjusted through the contact force error monitored in real time; and a Lyapunov energy function is used for calculating the position correction amount to correct the reference trajectory, the corrected reference trajectory is converted into a control instruction, and the rope-driven snake-shaped arm is driven to move. According to the method disclosed by the invention, the self-adaptive impedance control of the rope-driven snake-shaped arm can be realized, the method is suitable for different environments, and the operation efficiency is improved.

Description

Technical Field

[0001] The present disclosure relates to the technical field related to robot motion control, and in particular to a rope-driven serpentine arm adaptive control method and system based on stiffness pre-distribution. Background Art

[0002] The rope-driven serpentine arm is a flexible multi-segment robot that uses a power element on the base as a power source and ropes to drive each joint. In the rope-driven serpentine arm, rope-driven joints are a common control method, which has the characteristics of high degree of freedom, simple structure, and high power density. In addition, the motion control unit of the rope-driven serpentine arm is located at the end base, which means that when designing the structure of each joint of the robot, only the fixed position of the rope needs to be considered, without considering the installation position of the power element. This control method can significantly reduce the size of the robot, enabling it to meet the special operation requirements in extreme environments such as aircraft engines and nuclear power plants.

[0003] There are still some problems in the interactive control process of the rope-driven serpentine arm control system: 1) For a single joint, in order to avoid the problem of over-constraint, two of the three ropes control the two degrees of freedom in a joint by position control, and the other rope adopts tension control, but there is no clear reference method for the magnitude of the tension; 2) The rope-driven serpentine arm adopts a rope-driven driving method. The rope runs through each joint as a flexible structure, so the stiffness of the rope-driven serpentine arm will greatly affect the control accuracy and motion characteristics of the robot; 3) At present, the impedance control on the rope-driven serpentine arm can only operate in the same environment after the impedance parameters are fixed. In a constantly changing environment, the rope-driven serpentine arm cannot adapt well to changes in the external environment. Therefore, it is very meaningful to study the adaptive impedance control method of the rope-driven serpentine arm combined with stiffness pre-distribution. Summary of the invention

[0004] In order to solve the problem that the traditional rope-driven serpentine arm control method cannot adapt well to the changes of the external environment and has low operating efficiency under the condition of constantly changing environment, the present disclosure proposes a rope-driven serpentine arm adaptive control method based on stiffness pre-distribution to solve the above problems.

[0005] According to one aspect of the present disclosure, a rope-driven serpentine arm adaptive control method based on stiffness pre-distribution is provided, comprising:

[0006] S10, obtaining the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establishing a planetary fitness function based on the expected end stiffness and the actual end stiffness, establishing a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establishing a total fitness function based on the planetary fitness function value and the characteristic fitness function value;

[0007] S20, guided by the escape gradient, using the total fitness function through a meta-heuristic optimization algorithm to calculate the rope tension of each joint of the rope-driven snake arm, and controlling the stiffness of each joint and the end stiffness by controlling the rope tension;

[0008] S30, designing an adaptive damping change rate based on the contact force error, establishing an adaptive admittance control model according to the adaptive damping change rate, and adjusting parameters of the adaptive admittance control model through the contact force error monitored in real time;

[0009] S40. According to the parameters of the adaptive admittance control model, the position correction amount is calculated using the Lyapunov energy function, the reference trajectory is corrected according to the obtained position correction amount, and the corrected reference trajectory is converted into a control instruction to drive the rope-driven serpentine arm to move.

[0010] Preferably, a planetary fitness function is established based on the expected end stiffness and the actual end stiffness, and a characteristic fitness function is established according to the stiffness of each joint of the rope-driven serpentine arm. The planetary fitness function is expressed as:

[0011] f ind (X)=|K xd -K x (X)| 2 ,

[0012] In the formula, K xd is the expected end stiffness, K x (X) is the actual end stiffness;

[0013] The feature fitness function is expressed as:

[0014]

[0015] Where n is the number of joints or arms, X i represents the i-th dimension feature of individual X, K ci (X i ) represents the joint stiffness matrix of the i-th joint of individual X.

[0016] Preferably, a total fitness function is established based on the planetary fitness function value and the characteristic fitness function value, which is expressed as:

[0017]

[0018] Preferably, guided by the escape gradient, the gradient direction update formula is:

[0019]

[0020] In the formula, represents the coordinates of the “spacecraft” in the solution space in the i-th dimension, λ irepresents the learning rate, It means that the fitness function is The gradient in direction, Represents the fitness function.

[0021] Preferably, an adaptive damping change rate is designed based on the contact force error, and the adaptive damping change rate is expressed as:

[0022]

[0023] In the formula, b0 is the initial value of the damping coefficient, Δb(t) is the change law of the adaptive damping coefficient, and b p is the force error proportional gain coefficient, b i is the force error integral gain coefficient, b d is the force error differential gain coefficient, is the error change rate, Δf(t) is the contact force error, and ε is a very small positive constant used to avoid When the value approaches 0, the denominator will be zero, which will cause calculation errors.

[0024] Preferably, an adaptive admittance control model is established according to the adaptive damping change rate, and the adaptive admittance control model is expressed as:

[0025]

[0026] Where e(t) is the position error between the actual position and the reference trajectory, k is the one-dimensional expression of the desired stiffness coefficient matrix, and m is the one-dimensional expression of the desired inertia coefficient matrix.

[0027] Preferably, the position correction is calculated using the Lyapunov energy function, and the position correction is expressed as:

[0028]

[0029] Where ΔX represents the correction amount of the reference trajectory, f Δ (t) represents the time-varying compensation function, e represents the contact force error, p(t) and d(t) represent the time-varying proportional coefficient of the contact force error and the time-varying error change rate coefficient, respectively, M is the mass coefficient matrix, B is the damping coefficient matrix, K is the stiffness coefficient matrix, p0, d0 and f Δ0 is the initial value of the integral, For speed, is the acceleration.

[0030] According to one aspect of the present disclosure, a rope-driven serpentine arm adaptive control system based on stiffness pre-distribution is provided, comprising:

[0031] A total fitness function building module is used to obtain the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establish a planetary fitness function based on the expected end stiffness and the actual end stiffness, establish a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establish a total fitness function based on the planetary fitness function value and the characteristic fitness function value;

[0032] The joint stiffness and terminal stiffness control module is guided by the escape gradient and uses the total fitness function through a meta-heuristic optimization algorithm to calculate the rope tension of each joint of the rope-driven snake arm, and controls the stiffness of each joint and the terminal stiffness by controlling the rope tension;

[0033] The parameter adjustment module of the adaptive admittance control model designs the adaptive damping change rate based on the contact force error, establishes the adaptive admittance control model according to the adaptive damping change rate, and adjusts the parameters of the adaptive admittance control model through the contact force error monitored in real time;

[0034] The reference trajectory correction module calculates the position correction value using the Lyapunov energy function according to the parameters of the adaptive admittance control model, corrects the reference trajectory according to the obtained position correction value, converts the corrected reference trajectory into a control command, and drives the rope-driven serpentine arm to move.

[0035] According to one aspect of the present disclosure, a computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the above-mentioned rope-driven serpentine arm adaptive control method based on stiffness pre-distribution is implemented.

[0036] Compared with the prior art, the beneficial effects of the present disclosure are:

[0037] 1) The present invention adopts and improves the Kepler optimization algorithm, based on Kepler's three laws of planetary motion, introduces an exploration rule guided by the escape gradient, and establishes a global comprehensive evaluation index based on the stiffness model based on the structural characteristics and expected stiffness of each joint of the rope-driven serpentine arm and the expected stiffness index of the end, so that it can reasonably adjust the optimization strategy according to the search progress at different stages, improve the speed of getting rid of the local optimal solution and the convergence speed, and at the same time improve the control accuracy.

[0038] 2) The present invention analyzes the dynamic performance of impedance control and adopts a variable damping coefficient to adaptively adjust the impedance controller parameters according to the analysis results. An adaptive damping change rate is established according to the contact force error, the force error accumulation and the force error step rate. The dynamic performance of the system can be adaptively adjusted to ensure the control accuracy and compliance of the rope-driven serpentine arm during movement.

[0039] 3) The present invention utilizes the Lyapunov energy function to calculate the position correction amount, and compensates the position of the reference trajectory through an impedance controller based on the expected reference trajectory. Under the premise of avoiding the influence of environmental estimation error on the contact force control accuracy, it solves the influence of changes in environmental parameters on the system contact force control accuracy, and effectively improves the system's adaptability to the environment.

[0040] It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the disclosure.

[0041] Further features and aspects of the present disclosure will become apparent from the following detailed description of exemplary embodiments with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The drawings herein are incorporated into the specification and constitute a part of the specification. These drawings illustrate embodiments consistent with the present disclosure and are used to illustrate the technical solutions of the present disclosure together with the specification.

[0043] Figure 1 A flow chart of a rope-driven serpentine arm adaptive control method based on stiffness pre-distribution is shown;

[0044] Figure 2 A flow chart showing a control method for adaptive variable impedance control;

[0045] Figure 3 The schematic diagram of the rope-driven serpentine arm control method based on force / position hybrid control is shown;

[0046] Figure 4 A schematic diagram of the Kepler optimization method based on escape gradient in an example of the present disclosure is shown;

[0047] Figure 5 A flow chart of the Kepler optimization method based on escape gradient in an example of the present disclosure is shown;

[0048] Figure 6 The structural block diagram of the rope-driven serpentine arm adaptive control system based on stiffness pre-distribution in an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0049] Various exemplary embodiments, features and aspects of the present disclosure will be described in detail below with reference to the accompanying drawings. The same reference numerals in the accompanying drawings represent elements with the same or similar functions. Although various aspects of the embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless otherwise specified.

[0050] The word “exemplary” is used exclusively herein to mean “serving as an example, example, or illustration.” Any embodiment described herein as “exemplary” is not necessarily to be construed as preferred or advantageous over other embodiments.

[0051] The term "and / or" herein is only a description of the association relationship of the associated objects, indicating that there may be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the term "at least one" herein represents any combination of at least two of any one or more of a plurality of. For example, including at least one of A, B, and C can represent including any one or more elements selected from the set consisting of A, B, and C.

[0052] In addition, in order to better illustrate the present disclosure, numerous specific details are given in the following specific embodiments. It should be understood by those skilled in the art that the present disclosure can also be implemented without certain specific details. In some examples, methods, means, components and circuits well known to those skilled in the art are not described in detail in order to highlight the subject matter of the present disclosure.

[0053] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0054] Example 1

[0055] Based on the above ideas, the present disclosure proposes a rope-driven serpentine arm adaptive control method based on stiffness pre-distribution. Figure 1 A flow chart of a rope-driven serpentine arm adaptive control method based on stiffness pre-distribution is shown. The method comprises:

[0056] S10, obtaining the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establishing a planetary fitness function based on the expected end stiffness and the actual end stiffness, establishing a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establishing a total fitness function based on the planetary fitness function value and the characteristic fitness function value;

[0057] S20, guided by the escape gradient, using the total fitness function through a meta-heuristic optimization algorithm to calculate the rope tension of each joint of the rope-driven snake arm, and controlling the stiffness of each joint and the end stiffness by controlling the rope tension;

[0058] S30, designing an adaptive damping change rate based on the contact force error, establishing an adaptive admittance control model according to the adaptive damping change rate, and adjusting parameters of the adaptive admittance control model through the contact force error monitored in real time;

[0059] S40. According to the parameters of the adaptive admittance control model, the position correction amount is calculated using the Lyapunov energy function, the reference trajectory is corrected according to the obtained position correction amount, and the corrected reference trajectory is converted into a control instruction to drive the rope-driven serpentine arm to move.

[0060] The flowchart of the control method of adaptive variable impedance control provided by the embodiment of the present disclosure is as follows: Figure 2 As shown, the rope-driven serpentine arm robot is controlled by a force controller and a position controller, adaptive position compensation is performed based on the current contact environment according to positive kinematics, adaptive damping change rate is designed based on the contact force error, adaptive admittance control is performed according to the damping coefficient change rate, and stiffness pre-distribution is further performed according to the adaptive admittance control to achieve the purpose of driving the rope-driven serpentine arm to move. The rope-driven serpentine arm adaptive control method based on stiffness pre-distribution includes the following steps:

[0061] S10. Obtain the expected terminal stiffness and the actual terminal stiffness of the rope-driven serpentine arm, establish a planetary fitness function based on the expected terminal stiffness and the actual terminal stiffness, establish a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establish a total fitness function based on the planetary fitness function value and the characteristic fitness function value.

[0062] In this embodiment, based on the over-constrained control scheme of three ropes controlling two degrees of freedom of the rope-driven serpentine arm, force / position hybrid control is adopted for each rope-driven joint, that is, two of the three ropes are used for position control and the other one is used for force control. Figure 3 This is a schematic diagram of the control method of a rope-driven serpentine arm based on force / position hybrid control. It describes in detail how to achieve force / position hybrid control of the rope-driven serpentine arm by controlling the over-constrained control of two degrees of freedom through three ropes. First, the system obtains the current position and force of the rope-driven serpentine arm through sensors, and controls the control force controller and position controller respectively according to statics and the inverse solution of the rope space. Specifically, according to the preset control strategy, the position of two ropes is controlled and the force of the third rope is controlled. Furthermore, the control parameters of the single-joint model of the rope-driven serpentine arm robot are adjusted in real time through the control force controller and the position controller. This method not only meets the certain position accuracy of the robot arm, but also enables the rope-driven serpentine arm to have certain mechanical characteristics.

[0063] The Kepler optimization algorithm based on escape gradient is used to pre-distribute the stiffness of the rope-driven serpentine arm, such as Figure 4As shown in the figure, the main contents of this method are as follows: the Kepler optimization algorithm is used as the meta-heuristic optimization algorithm for stiffness pre-allocation. According to the Kepler planetary motion rules, the algorithm is guided to search in the direction of minimizing the error, further reducing the possibility of falling into the local optimal solution. By integrating the three laws of Kepler into the optimization process, based on the spacecraft and the spacecraft orbit, it is expected to effectively find the global optimal solution of the inverse solution problem of the stiffness model in the planets (candidate solutions), thereby improving the performance and accuracy of the system. Figure 5 This is a flow chart of the Kepler optimization method based on escape gradient, which describes in detail the specific steps and processes of the optimization algorithm, including initialization, planetary motion simulation, application of escape gradient rule, and output of optimization results. The specific steps are as follows:

[0064] Initialization: Randomly generate the position and speed of the planets. First, establish a planetary group, evaluate the position goodness of each planet according to the fitness function, then establish the solar system, and calculate the Euclidean distance between the sun and each planet, and further obtain the position and speed of the planet according to the universal gravitation. Calculate fitness: According to the current position and speed of the planet, record the direction of the planet's movement to ensure that the fitness of the planet at the next moment is not inferior to that of the previous moment, and calculate the fitness value of each planet. Exploration condition judgment: Determine whether the planet meets the exploration conditions based on the planet's fitness value and gradient information. If the gradient of the planet's position is greater than a certain threshold (Escape gradient), the planet is considered to meet the exploration conditions. If the conditions are met, the algorithm will enter the next step and launch a "spacecraft" along the gradient direction of the planet for exploration; otherwise, skip the exploration step and directly update the position and velocity of the planet. Spacecraft exploration: launch a spacecraft to explore along the negative gradient direction. Update planet position and velocity: adjust the position and velocity of the planet according to the exploration results. Optimal solution judgment: judge whether the optimal solution is reached. If not, continue to iterate. If the gradient value explored by the spacecraft is less than a certain threshold |δfit k (t)|<δ min If the solution reaches the optimal or local optimal state (i.e. the gradient approaches zero), the solution is considered to have reached the optimal or local optimal state, and a new planet is established at the spacecraft and incorporated into the solar system, thereby determining the planet with the best fitness among all the planets as the new sun; otherwise, the solution returns to the fitness calculation step, continues to iteratively calculate the fitness value of the planet, and conducts the next round of exploration and update until t <t max Output the final solution. This rule effectively improves the global search capability of the algorithm and further reduces the possibility of falling into the local optimal solution.

[0065] The adaptability of each planet in the environment is called planet fitness, and the fitness of each dimension of the planet in the solution space is called characteristic fitness. The planet fitness function is established based on the expected end stiffness and the actual end stiffness, and the characteristic fitness function is established according to the stiffness of each joint of the rope-driven serpentine arm. The planet fitness function is expressed as:

[0066] f ind (X)=|K xd -K x (X)| 2 ,

[0067] In the formula, K xd is the expected end stiffness, K x (X) is the actual end stiffness.

[0068] Among them, the method for evaluating the similarity of the stiffness of the end of the manipulator is to approximately evaluate whether the two matrices are similar by comparing the size of their moduli. Although this method is simple, it is very effective in the evaluation process of this embodiment. In the rope-driven serpentine arm, due to the similarity of each joint, the joints of the rope-driven serpentine arm tend to be more uniform in stiffness distribution. The characteristic fitness function is expressed as:

[0069]

[0070] Where n is the number of joints or arms. There are 6 joints in the research object of this embodiment, so n=6, X i represents the i-th dimension feature of individual X, K ci (X i ) represents the joint stiffness matrix of the i-th joint of individual X.

[0071] Taking into account that the planetary fitness value and the characteristic fitness value have relatively obvious effects at the same time, the total fitness function is established based on the planetary fitness function value and the characteristic fitness function value, which is expressed as:

[0072]

[0073] S20. Guided by the escape gradient, the total fitness function is used to calculate the rope tension of each joint of the rope-driven snake arm through a meta-heuristic optimization algorithm, and the stiffness of each joint and the end stiffness are controlled by controlling the rope tension.

[0074] Since the original Kepler optimization method has a large orbit and a low speed for planets farther from the sun, and since there are many random numbers in the process of gravity and speed calculation, there may be a large difference between the speed direction of the planet and the fitness gradient direction here, and the optimization efficiency is significantly reduced compared to the inner ring. Therefore, this embodiment, based on the exploration of space by humans in modern society, establishes an exploration optimization rule guided by the current position gradient of the outer ring planet by analogy with the third cosmic speed, which can speed up the escape from the local optimal solution when the sun falls into the local optimal solution and reduce the optimization running time.

[0075] In this embodiment, by analogy with the escape velocity (the third cosmic velocity), the escape gradient δ is defined esc As a "spacecraft" in the solution space, the conditional threshold for launching an exploration of the unknown solution space is: when the gradient of the location of the distant planet group in any direction is greater than the escape gradient δ esc When The planet meets the exploration conditions, and a spacecraft is launched to explore along the negative gradient direction of the planet's position in the solution space, guided by the escape gradient. The gradient direction update formula is:

[0076]

[0077] In the formula, represents the coordinates of the “spacecraft” in the solution space in the i-th dimension, λ i represents the learning rate, It means that the fitness function is The gradient in direction, Represents the fitness function. When the spacecraft explores to a gradient value less than a certain value δ min ,Right now When , the spacecraft is considered to have reached the optimal solution or the local optimal solution, and this place is considered to be the "habitable place" expected by the parent star. A new planet is established at this coordinate and incorporated into the solar system, following the three laws of Kepler in the solution space. It can speed up the process of getting rid of the local optimal solution when the sun falls into the local optimal solution, and reduce the running time of the algorithm in the complex solution space.

[0078] S30. Design an adaptive damping change rate based on the contact force error, establish an adaptive admittance control model according to the adaptive damping change rate, and adjust the parameters of the adaptive admittance control model through the contact force error monitored in real time.

[0079] In this embodiment, the dynamic performance of the impedance model is analyzed and adaptive variable impedance parameters are established to adjust the dynamic performance of the system. The contact environment is equivalent to a first-order spring model, and the influence of the position tracking error on the system is ignored, that is, X = X c , where the environmental stiffness is K e , then:

[0080] F e =K e (XX e )=K e (X c -X e ),

[0081] The admittance control model can be expressed as:

[0082]

[0083] In the formula, X r is the reference trajectory of the rope-driven serpentine arm, X c is the target trajectory output by the rope-driven serpentine arm, X is the actual position of the rope-driven serpentine arm, and X e is the environmental position, F e is the contact force between the rope-driven serpentine arm and the environment, F r is the desired contact force, ΔF=F r -F e is the contact force error, M is the mass coefficient matrix, B is the damping coefficient matrix, and K is the stiffness coefficient matrix. Since the position error between the actual position of the robot and the reference trajectory can be expressed as:

[0084] E=XX r =X c -X r ,

[0085] Then the admittance control model is expressed as:

[0086]

[0087] Since the environmental parameters are unknown, it is impossible to obtain an accurate reference trajectory. r =X e , and according to the impedance model and environmental contact model, the transfer function between the position error and the expected contact force is:

[0088]

[0089] The characteristic equation of the admittance control system is expressed as:

[0090] ms 2 +bs+k+k e =0,

[0091] Among them, m, b, k and k e They are M, B, K and K e According to the transfer function of a typical second-order system, the damping ratio ζ of the admittance control and the undamped natural frequency ω can be obtained. n for:

[0092]

[0093] Since ζ and ω n Directly affects the dynamic performance of the system. Considering that the mass coefficient m is more sensitive to the dynamic performance of the system, the stiffness coefficient k and the environmental stiffness coefficient k e Together they affect the dynamic performance of the system, and the size of the damping coefficient b directly affects the size of the system damping ratio ζ. Therefore, assuming that the mass coefficient and stiffness coefficient are both constants, the system response is dynamically adjusted by designing the adaptive rate of the damping coefficient.

[0094] From the above analysis, we can see that the initial environment position x e Can replace x r , then e=x c -x e , so the admittance control can be expressed as:

[0095]

[0096] Where, Δf(t), e(t) is time-varying. In order to further ensure that Δf ss →0, the damping coefficient is adaptively adjusted to compensate for the time-varying contact force error, thereby adjusting the impedance characteristics of the robot arm.

[0097] According to the above analysis of the dynamic performance of impedance control, this embodiment adopts variable damping to realize adaptive variable admittance coefficient. The adaptive damping change rate is established according to the contact force error, the force error accumulation and the force error rate to realize adaptive dynamic adjustment of the damping coefficient. The adaptive damping change rate is expressed as:

[0098]

[0099] In the formula, b0 is the initial value of the damping coefficient, Δb(y) is the change law of the adaptive damping coefficient, and b p is the force error proportional gain coefficient, b i is the force error integral gain coefficient, b d is the force error differential gain coefficient, is the error change rate, Δf(t) is the contact force error, ε=10 -8 The main purpose of introducing this parameter is to adjust the error rate in the actual adaptive adjustment process. It may approach 0, resulting in The denominator in one term is 0. In order to avoid this situation, the constant ε is introduced.

[0100] Furthermore, an adaptive admittance control model is established according to the adaptive damping change rate, and the adaptive admittance control model is expressed as:

[0101]

[0102] Where e(t) is the position error between the actual position and the reference trajectory, k is the one-dimensional expression of the desired stiffness coefficient matrix, and m is the one-dimensional expression of the desired inertia coefficient matrix.

[0103] The above adaptive variable admittance coefficient method dynamically adjusts the damping coefficient through the gain coefficient. Among them, the value of the gain coefficient determines whether the system can operate stably. Therefore, it is necessary to analyze the stability of the system and give the constraints of the gain coefficient.

[0104] Performing Laplace transform on the above adaptive admittance control model yields:

[0105]

[0106] where e(s) = x c (s)-x e (s), and substituting the one-dimensional representation of the environmental contact force into it, we can obtain:

[0107]

[0108] The contact force error Δf(s) = f r (s)-f e Substituting (s) into the above formula, the transfer function between the contact force error and the expected 5 force can be obtained as follows:

[0109]

[0110] The Routh stability criterion is used to analyze the stability of the characteristic equation. In order to ensure the stability of the system, all the elements in the first column of the Routh table are positive values, and the value of the gain coefficient should satisfy:

[0111]

[0112] S40. According to the parameters of the adaptive admittance control model, the position correction amount is calculated using the Lyapunov energy function, the reference trajectory is corrected according to the obtained position correction amount, and the corrected reference trajectory is converted into a control instruction to drive the rope-driven serpentine arm to move.

[0113] In this embodiment, since a small estimation error of environmental parameters will greatly affect the contact force control accuracy, this embodiment adopts the Lyapunov energy function to calculate the position correction amount. Based on the expected reference trajectory, the position of the reference trajectory is compensated by the impedance controller to improve the system's adaptability to the environment.

[0114] After the rope-driven serpentine arm comes into contact with the target task point, the contact force error between the robot and the environment is obtained by constructing the robot impedance model and the environment stiffness model:

[0115] ΔF=F r -K e (X r +G(s)ΔF-X e ),

[0116] in,

[0117]

[0118] F e =K e (XX e )=K e (X c -X e ),

[0119] Where G(s) is the robot impedance model, F e Represents the environmental stiffness model.

[0120] When t→∞, s→0, the steady-state error of the contact force is:

[0121]

[0122] Therefore, when K e (X e -X r )+F r =0 satisfies the system steady-state error ΔF ss =0, so the relationship between the reference trajectory and the environmental variables is:

[0123]

[0124] By adjusting the environmental parameter K e and X e The reference trajectory input when the rope-driven serpentine arm is working is adjusted based on the estimation, thereby reducing the interference of position control on the end contact force control.

[0125] However, since a small estimation error of the environmental parameters will greatly affect the contact force control accuracy, the Lyapunov energy function is used to calculate the position correction, and the reference trajectory correction is designed as follows:

[0126]

[0127] Among them, X r represents the expected reference trajectory, X crepresents the corrected reference trajectory, ΔX represents the correction amount of the reference trajectory, and f Δ (t) represents the time-varying compensation function, e represents the contact force error, p(t) and d(t) represent the time-varying proportional coefficient of the contact force error and the time-varying error change rate coefficient, respectively.

[0128] Substituting the impedance model and simplifying it, the expression for contact force error is:

[0129]

[0130] The state space equation is further established as follows:

[0131]

[0132] make Then the state space equation can be expressed as:

[0133]

[0134] Since the goal of this embodiment is to expect the contact force error to be 0, we have:

[0135]

[0136] Similarly, the state space is expressed as:

[0137]

[0138] Define the error between the expected error and the actual error as E e =E d -E p , then substituting the above two state equations into the state space equation for the error is:

[0139]

[0140] In order to ensure that the error between the actual contact force error and the expected contact force error approaches 0, the Lyapunov second method is used to make the system asymptotically stable. Therefore, the Lyapunov function is constructed and the state space equation of the above error is guaranteed to be asymptotically stable.

[0141]

[0142] The position correction is calculated using the Lyapunov energy function and is expressed as:

[0143]

[0144] Where ΔX represents the correction amount of the reference trajectory, f Δ(t) represents the time-varying compensation function, e represents the contact force error, p(t) and d(t) represent the time-varying proportional coefficient of the contact force error and the time-varying error change rate coefficient, respectively, M is the mass coefficient matrix, B is the damping coefficient matrix, K is the stiffness coefficient matrix, p0, d0 and f Δ0 is the initial value of the integral, where p 12 and p 22 are two constants of a positive definite matrix in the Lyapunov function, For speed, is the acceleration.

[0145] The disclosed embodiment integrates adaptive parameters and reference trajectory corrections as the adaptive impedance control part of the rope-driven serpentine arm, and combines stiffness pre-distribution to enable the robot to flexibly respond to complex external environments and provide a solid guarantee for its stable and efficient operation.

[0146] Example 2

[0147] As another aspect of the embodiment of the present disclosure, a rope-driven serpentine arm adaptive control system 100 based on stiffness pre-distribution is also provided. Figure 6 As shown, including:

[0148] A total fitness function construction module 1 is used to obtain the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establish a planetary fitness function based on the expected end stiffness and the actual end stiffness, establish a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establish a total fitness function based on the planetary fitness function value and the characteristic fitness function value;

[0149] The joint stiffness and terminal stiffness control module 2 is guided by the escape gradient and uses the total fitness function to calculate the rope tension of each joint of the rope-driven snake arm through a meta-heuristic optimization algorithm, and controls the stiffness of each joint and the terminal stiffness by controlling the rope tension;

[0150] The parameter adjustment module 3 of the adaptive admittance control model designs an adaptive damping change rate based on the contact force error, establishes an adaptive admittance control model according to the adaptive damping change rate, and adjusts the parameters of the adaptive admittance control model through the contact force error monitored in real time;

[0151] The reference trajectory correction module 4 calculates the position correction value using the Lyapunov energy function according to the parameters of the adaptive admittance control model, corrects the reference trajectory according to the obtained position correction value, converts the corrected reference trajectory into a control instruction, and drives the rope-driven serpentine arm to move.

[0152] In the absence of any contradiction, the above modules in the system of the embodiment of the present disclosure can implement any implementation of the above method.

[0153] Based on the description of the above embodiments, it can be seen that the embodiments of the present disclosure can achieve the following technical effects:

[0154] 1) The present invention adopts and improves the Kepler optimization algorithm, based on Kepler's three laws of planetary motion, introduces an exploration rule guided by the escape gradient, and establishes a global comprehensive evaluation index based on the stiffness model based on the structural characteristics and expected stiffness of each joint of the rope-driven serpentine arm and the expected stiffness index of the end, so that it can reasonably adjust the optimization strategy according to the search progress at different stages, improve the speed of getting rid of the local optimal solution and the convergence speed, and at the same time improve the control accuracy.

[0155] 2) The present invention analyzes the dynamic performance of impedance control and adopts a variable damping coefficient to adaptively adjust the impedance controller parameters according to the analysis results. An adaptive damping change rate is established according to the contact force error, the force error accumulation and the force error step rate. The dynamic performance of the system can be adaptively adjusted to ensure the control accuracy and compliance of the rope-driven serpentine arm during movement.

[0156] 3) The present invention utilizes the Lyapunov energy function to calculate the position correction amount, and compensates the position of the reference trajectory through an impedance controller based on the expected reference trajectory. Under the premise of avoiding the influence of environmental estimation error on the contact force control accuracy, it solves the influence of changes in environmental parameters on the system contact force control accuracy, and effectively improves the system's adaptability to the environment.

[0157] The disclosed embodiment also provides an electronic device, comprising: a processor; a memory for storing instructions executable by the processor; wherein the processor is configured to implement the above-mentioned rope-driven serpentine arm adaptive control method based on stiffness pre-allocation. The electronic device can be provided as a terminal, a server or other forms of equipment.

[0158] The embodiment of the present disclosure also provides a computer-readable storage medium, on which computer program instructions are stored, and when the computer program instructions are executed by a processor, the above-mentioned rope-driven serpentine arm adaptive control method based on stiffness pre-distribution is implemented. The computer-readable storage medium may be a non-volatile computer-readable storage medium.

[0159] Those skilled in the art will understand that, in the above-mentioned rope-driven serpentine arm adaptive control method and system based on stiffness pre-distribution in the specific implementation mode, the writing order of each step does not mean a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of each step should be determined by its function and possible internal logic.

[0160] The flow chart and block diagram in the accompanying drawings show the possible architecture, function and operation of the system, method and computer program product according to multiple embodiments of the present disclosure. In this regard, each square box in the flow chart or block diagram can represent a part of a module, program segment or instruction, and a part of the module, program segment or instruction includes one or more executable instructions for realizing the specified logical function. In some alternative implementations, the function marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two continuous square boxes can actually be executed substantially in parallel, and they can sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs the specified function or action, or can be implemented with a combination of special hardware and computer instructions.

[0161] The embodiments of the present disclosure have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements to the technology in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A rope-driven serpentine arm adaptive control method based on stiffness pre-distribution, characterized in that: The steps include: S10, obtaining the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establishing a planetary fitness function based on the expected end stiffness and the actual end stiffness, establishing a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establishing a total fitness function based on the planetary fitness function value and the characteristic fitness function value; S20, guided by the escape gradient, using the total fitness function through a meta-heuristic optimization algorithm to calculate the rope tension of each joint of the rope-driven snake arm, and controlling the stiffness of each joint and the end stiffness by controlling the rope tension; S30, designing an adaptive damping change rate based on the contact force error, establishing an adaptive admittance control model according to the adaptive damping change rate, and adjusting parameters of the adaptive admittance control model through the contact force error monitored in real time; S40. According to the parameters of the adaptive admittance control model, the position correction amount is calculated using the Lyapunov energy function, the reference trajectory is corrected according to the obtained position correction amount, and the corrected reference trajectory is converted into a control instruction to drive the rope-driven serpentine arm to move.

2. The method according to claim 1, characterized in that The planetary fitness function is established based on the expected end stiffness and the actual end stiffness, and the characteristic fitness function is established according to the stiffness of each joint of the rope-driven serpentine arm. The planetary fitness function is expressed as: f ind (X)=|K xd -K x (X)| 2 , In the formula, K xd is the expected end stiffness, K x (X) is the actual end stiffness; The feature fitness function is expressed as: Where n is the number of joints or arms, X i represents the i-th dimension feature of individual X, K ci (X i ) represents the joint stiffness matrix of the i-th joint of individual X.

3. The method according to claim 2, characterized in that The total fitness function is established based on the planet fitness function value and the characteristic fitness function value, which is expressed as:

4. The method according to claim 1, characterized in that: Guided by the escape gradient, the gradient direction update formula is: In the formula, represents the coordinates of the "spacecraft" in the solution space in the i-th dimension, λ i represents the learning rate, It means that the fitness function is The gradient in direction, Represents the fitness function.

5. The method according to claim 1, characterized in that The adaptive damping change rate is designed based on the contact force error, and the adaptive damping change rate is expressed as: In the formula, b0 is the initial value of the damping coefficient, Δb(t) is the change law of the adaptive damping coefficient, and b p is the force error proportional gain coefficient, b i is the force error integral gain coefficient, b d is the force error differential gain coefficient, is the error change rate, Δf(t) is the contact force error, and ε is a very small positive constant used to avoid When the value approaches 0, the denominator will be zero, which will cause calculation errors.

6. The method according to any one of claims 1 or 5, characterized in that: An adaptive admittance control model is established according to the adaptive damping change rate. The adaptive admittance control model is expressed as: Where e(t) is the position error between the actual position and the reference trajectory, k is the one-dimensional expression of the desired stiffness coefficient matrix, and m is the one-dimensional expression of the desired inertia coefficient matrix.

7. The method according to claim 1, characterized in that The position correction is calculated using the Lyapunov energy function and is expressed as: Where ΔX represents the correction amount of the reference trajectory, f Δ (t) represents the time-varying compensation function, e represents the contact force error, p(t) and d(t) represent the time-varying proportional coefficient of the contact force error and the time-varying error change rate coefficient, respectively, M is the mass coefficient matrix, B is the damping coefficient matrix, K is the stiffness coefficient matrix, p0, d0 and f Δ0 is the initial value of the integral, For speed, is the acceleration.

8. The adaptive control system of the rope-driven serpentine arm based on stiffness pre-distribution is characterized in that: include: A total fitness function building module is used to obtain the expected end stiffness and the actual end stiffness of the rope-driven serpentine arm, establish a planetary fitness function based on the expected end stiffness and the actual end stiffness, establish a characteristic fitness function according to the stiffness of each joint of the rope-driven serpentine arm, and establish a total fitness function based on the planetary fitness function value and the characteristic fitness function value; The joint stiffness and terminal stiffness control module is guided by the escape gradient and uses the total fitness function through a meta-heuristic optimization algorithm to calculate the rope tension of each joint of the rope-driven snake arm, and controls the stiffness of each joint and the terminal stiffness by controlling the rope tension; The parameter adjustment module of the adaptive admittance control model designs the adaptive damping change rate based on the contact force error, establishes the adaptive admittance control model according to the adaptive damping change rate, and adjusts the parameters of the adaptive admittance control model through the contact force error monitored in real time; The reference trajectory correction module calculates the position correction value using the Lyapunov energy function according to the parameters of the adaptive admittance control model, corrects the reference trajectory according to the obtained position correction value, converts the corrected reference trajectory into a control command, and drives the rope-driven serpentine arm to move.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the rope-driven serpentine arm adaptive control method based on stiffness pre-distribution according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the rope-driven serpentine arm adaptive control method based on stiffness pre-distribution according to any one of claims 1 to 7 is implemented.

Images