Real-time statics obstacle avoidance planning method for rope-driven flexible serial robot

Abstract

The present application belongs to the field of trajectory planning of rope-driven flexible continuum robots, and provides a real-time statics obstacle avoidance planning method for rope-driven flexible continuum robots, comprising the following steps: firstly, according to the type of mechanical unit contained in the robot model, the statics equation based on position finite element is constructed through the virtual work principle and the rope driving constraint; secondly, combining the statics balance equation, the strong nonlinear programming problem involving state constraints, control constraints and high-latitude strong nonlinear statics balance constraints is converted into a weak nonlinear instantaneous optimal control problem; then the control quantity is solved through a standard nonlinear optimization solver, and the state variable is further updated through a single-step or multi-step, synchronous or asynchronous updating strategy; finally, it is judged whether to jump out of the loop according to the task completion. Compared with the existing obstacle avoidance planning problem of kinematics-based rope-driven flexible continuum robots, the accuracy of the robot model is improved, and the calculation efficiency and robustness of the planning are guaranteed.

Description

Technical Field

[0001] This invention belongs to the field of trajectory planning for rope-driven flexible continuous robots, and relates to a real-time static obstacle avoidance motion planning method for rope-driven flexible robots. Background Technology

[0002] Flexible continuous robots have attracted widespread attention and research in academia due to their slender arms, self-compliance, and flexible movement. Rope actuation, as a power transmission method for flexible continuous robots, features distal transmission and low vibration, without sacrificing the slenderness of the robotic arm, thus showing promising prospects in industrial applications. However, its compliance advantage also brings challenges to continuous robots, such as modeling accuracy, solution complexity, and strong coupling nonlinearity of the controller. Compared with traditional rigid robots, the complexity of flexible continuous robots is mainly reflected in the following aspects: a large number of degrees of freedom, a large and complex solution scale for rigid-flexible coupled systems, and the system exhibiting task redundancy and underactuation.

[0003] Currently, modeling of tethered flexible robots typically employs kinematic models with assumptions of constant or variable curvature. However, this modeling approach exhibits significant errors when the flexible robot is subjected to its own deformation and gravity. Methods based on such inaccurate models can lead to loss of motion accuracy or even failure in open-loop or closed-loop control. Therefore, in terms of modeling, static models have become a computational method that can accurately simulate state information and is faster than dynamic models. When static models are combined with traditional robot planning methods, such as the Astar algorithm, RRT algorithm, and artificial potential field method, the complexity and solution time increase significantly when considering the large-scale static constraint equations of flexible continuous robots. If inverse statics methods are used, the complex phenomenon of control redundancy due to drive redundancy leads to non-smooth, underdetermined nonlinear equations in numerical computation, making them prone to numerical ill-conditioning and resulting in poor robustness. Furthermore, inverse statics methods struggle to consider the constraint information of state and control variables. Another approach is to use optimal control algorithms such as Discrete LQR or MPC. However, due to the highly nonlinear nature of the static constraints, as well as other control and state constraints of the model, it is difficult to approximate them using linearized models, which increases the stability and time required for the solution. In fact, considering the use of static models for trajectory planning is challenging and time-consuming. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes a static motion planning method for tethered flexible robots. This method, based on a position finite element static model and incorporating the concept of a quasi-Newtonian method with static sensitivity fusion, aims to solve the static motion planning problem for tethered flexible continuous robots with state and input saturation constraints. The goal is to provide a fast, accurate, and robust control strategy framework, offering a reference for safe obstacle avoidance planning.

[0005] To achieve the above objectives, the technical means employed in this invention are as follows: A real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot is proposed. First, based on the types of mechanical units contained in the robot model, its static equations based on the position finite element method are constructed using the principle of virtual work and rope-driven constraints. Second, combining the static equilibrium equations, the strongly nonlinear programming problem involving state constraints, control constraints, and high-dimensional strongly nonlinear static equilibrium constraints is transformed into a weakly nonlinear instantaneous optimal control problem. Then, the control variables are solved using a standard nonlinear optimization solver, and the state variables are further updated using single-step or multi-step, synchronous or asynchronous update strategies. Finally, the loop exit is determined based on the task completion status. The method comprises the following steps: The first step is to establish the static equations of the flexible robot based on the position finite element method and with rope-driven constraints.

[0006] (1) in, For the robot's generalized coordinates, The number of generalized coordinates, whose physical meaning is the position information of each node of the robot in the global coordinate system. The dimension of the matrix is OK List. For the system constraints, the Lagrange multipliers, The number of Laplace multipliers represents the driving force of the rope. With mechanical constraints Constraint reaction force The number of those actively driven. These are the constraint equations of the system. To drive constraints, For mechanical constraints. To constrain the partial derivatives with respect to generalized coordinates. This is the control quantity for the robot, specifically the driving length of the rope. The generalized force of the system can be expressed as: (2) in, and These represent the robot's generalized internal forces and generalized external forces, respectively. The system at this point is a mechanical system containing both motion-driven constraints and mechanical constraints.

[0007] The second step is to establish the control problem for continuous robot obstacle avoidance. For continuous robots, their tasks typically require their end effector to move from one specified position and orientation to another specified position and orientation: (3) in, This represents the robot's end-effector pose. The desired end-effector pose of the robot. and These are the robot's end-effector position and attitude vector, respectively.

[0008] In order to reflect the actual task objectives of continuous robots, such as minimizing the robot's driving force, and to ensure the uniqueness of the solution, it is also necessary to define the performance indicators of continuous robots. : (4) Furthermore, for safety considerations, restrictions need to be placed on state and control variables. These include limits on the maximum tension of the motor system, limitations on the drivable length of the rope reserved in the structural design, and preventing the robot from colliding with obstacles during movement. Since the rope is a unidirectional force structure, it can only withstand tension and not compression, so a minimum driving force limit needs to be given to prevent the rope from slackening and detaching from drive structures such as pulleys, causing errors in rope-driven operation. Such restrictions can be described by the following inequality constraints: (5) (6) (7) in, The minimum distance between the robot and the obstacle. The minimum distance between the robot and the obstacle. The safe distance for robots to avoid collisions. Indicates the driving force of the rope. This indicates the minimum driving force limit for the rope. Indicates the maximum driving force limit of the rope; This represents the control quantity of the robot, i.e., the driving length of the rope. Indicates the minimum driving length of the rope. This indicates the maximum driving length of the rope.

[0009] In summary, the optimal control problem for static programming of a continuous robot can be obtained, which can be expressed as follows: (8) The third step is the construction and solution of the instantaneous optimal statics algorithm. Convert the control quantity into an instantaneous control quantity The static equations are transformed into a discrete problem for the current step, which can be expressed as: (9) in, The length of the rope that has already been driven is a known quantity relative to the next step. The length of the next drive by the controller; Represents the generalized coordinates of the current control step; Represents the Lagrange multiplier of the current control step; Let represent the generalized force of the current control step. Equation (8) is then transformed into the following instantaneous optimal control problem construction format: (10) in, Indicates the length of the rope that has been driven; The rope driving force represents the current control step; This indicates the amount of drive of the control rope in the current control step; This indicates the minimum distance between the current control step and the obstacle; We can use the shortest distance to the target point and the minimum driving force as indicators to transform the strong constraint of equality constraints into a soft constraint of indicators, which can be written as: (11) (12) (13) in, This indicates the current end-effector pose of the controlled robot. This represents the weight matrix for the end-effector pose index; This represents the weight matrix of the driving force indicators. This represents the robot end-effector pose calculated in the previous step; This indicates the sensitivity of the robot's end-effector pose to the control input in the previous control step. This indicates the robot's sensitivity to generalized coordinates in the previous control step; This indicates the sensitivity of the robot's generalized coordinates to the control quantity in the previous control step. This represents the Lagrange multiplier obtained in the previous step; This indicates the sensitivity of the Lagrange multiplier to the control quantity in the previous control step.

[0010] In formulas (12) and (13) and The gradient information of the control quantity obtained through the static sensitivity method is as follows: (14) The above equation can be further simplified into a system of linear matrix equations, with the unknowns being... (15) in, and The expression is as follows: (16) (17) The performance metrics can then be further simplified to: (18) in, and The expressions are as follows: (19) (20) The gradient of the performance index with respect to the control variable can be expressed as: (twenty one) The gradient of the terminal state relative to the control quantity in equation (20) It can be represented as: (twenty two) Obstacle constraints can be expressed using the following formula: (twenty three) in, Indicates the distance between the obstacle and the detected point; This indicates the safe distance between the robot and the obstacle.

[0011] For ease of representation, we assume it to be a spherical obstacle. The distance between the obstacle and the detected point can then be obtained as follows: (twenty four) in, As the first robot Location extraction matrix of each detected point; For the first The center position vector of each obstacle. The gradient of the obstacle constraint with respect to the control variable. It can be represented as: (25) The final control quantity can be solved using an optimization solver based on the optimization problem established in (10). On the one hand, in the absence of obstacles... When the set is empty, the resulting optimization problem is a standard quadratic programming problem, solvable by a mature QP solver. However, when obstacles exist... If the set is not empty, the obstacle constraint is a nonlinear constraint, which can be solved using a nonlinear programming solver such as the SQP solver.

[0012] Fourth step, update the state variables The constraint equations in statics include both mechanical constraints and control constraints. If only control constraints are included... When updating state variables, a single-step Newton iteration update strategy can be chosen. However, when mechanical constraints are involved... In such cases, a nonlinear iterative strategy, such as Newton's iteration, must be used to ensure the stability of the constraints. When using a single-step Newton update strategy, it can be observed that the Jacobian matrix during the update is similar to that used when calculating the sensitivity. The matrices are the same, that is Therefore, the matrix can be pre-processed at this point. LU decomposition preprocessing reduces redundant computations and increases solution speed. The single-step and nonlinear Newton update strategies are shown below: Single-step Newton update strategy: (26) Nonlinear Newton update strategy: (27) in, Let be the number of Newton steps, until... , For a given individual, the residual convergent small quantity is generally taken as... This ensures that the residuals from the iteration satisfy the static equilibrium equations. This represents the state variable that is updated when the current control variable is substituted. This represents the state variables of the previous control step; The inverse matrix of the augmented tangential stiffness matrix of the equation; Represent the equation X; This indicates the control quantity for the current control step. Indicates in The state variables of the second Newton iteration; Indicates the first The state variables of the second Newton iteration; express The inverse matrix of the augmented tangential stiffness matrix in the second Newton iteration; Represented as and Substitute the state variable residuals into equation (1).

[0013] To improve computational efficiency in control, state updates can be further divided into asynchronous and synchronous update strategies. Specifically, the state variables' sensitivity to control is updated every [time period]. Update every control step or every other control step: (a) Synchronous update strategy The synchronous update strategy involves updating the state variables and the static sensitivity after each calculation of the control variable: after solving for the control variable in (10), the state variables are updated using equation (26) or equation (27). In the next step, the updated state variables are used to recalculate the sensitivity information and optimize the solution of the control variables.

[0014] (b) Asynchronous update strategy The asynchronous update strategy means that after each calculation of the control variable is completed, the static sensitivity is not updated; instead, the state variables are updated using the following update format: (28) In the above formula Represents the interval The static sensitivity matrix updated by equation (15) is then used to correct the objective function (18).

[0015] After updating the state variables, the current pose of the robot's end effector can be obtained. This can be used to determine whether the robot's current end-effector pose meets the task requirements.

[0016] Fifth step, determine whether the task is satisfied. when If the task requirements are met, the control task can be exited. For the accuracy requirements of the task. When If the task requirements are not met, continue repeating steps two through four until the task requirements are met, at which point the loop can exit.

[0017] Furthermore, the driving constraint mentioned in step one can be written as: (29) in, This is the extraction matrix for the rope driving direction.

[0018] Furthermore, in step two, the robot's current position and posture The following formulas can be used to obtain the results: (30) (31) (32) (33) In the above formula This is the extraction matrix for the robot's desired controlled points. Extract the matrix for the generalized coordinates that include the pose.

[0019] Furthermore, in step three, the gradient of the terminal state relative to the control variable can be obtained by the following equation: (34) (35) (36) In the above formula Let be the antisymmetric matrix corresponding to the vector.

[0020] Furthermore, in step four, the update of the next state variable (26) or (27) is different from the predicted state variable (12) in the objective function. In the next state variable update, the generalized force information under the generalized coordinates in the previous step is used instead of directly using the sensitivity information for linear update.

[0021] The beneficial effects of this invention are as follows: (1) This invention, based on the static position finite element method, combines the sensitivity method in mechanics to obtain the state variables and the gradient of the Laplace multipliers with respect to the control quantity. This allows for more accurate prediction of state quantity information in control, and the physical quantities have intuitive characteristics, facilitating sensor measurement and actuator application. Furthermore, by transforming the solution of the control quantity from a high-dimensional, strongly coupled nonlinear constraint optimization problem into a low-dimensional, weakly nonlinear optimization problem, the stability and computational efficiency of the control solution are improved, while ensuring the accuracy of the numerical solution.

[0022] (2) This invention employs a synchronous or asynchronous update control strategy. On the one hand, this strategy can further reduce the computation time of control solution according to different control tasks. On the other hand, in asynchronous update, the update method of the state variables can provide a good initial value for the nonlinear iterative process, thereby ensuring the stability of the algorithm solution.

[0023] (3) Compared with planning using kinematic models, this invention, based on the equilibrium equations of mechanics, firstly, selects the driving length of the rope as the control variable instead of the driving force, maintaining convenience and accuracy when applied to the actual system. Secondly, this choice ensures that the maximum force condition of the motor is met when the rope driving length control law is applied. Finally, using static equations can more accurately reproduce the deformation of flexible structures (such as elastic rods, springs, and ropes) in the real system compared to kinematic equations, making open-loop and closed-loop control more precise. Attached Figure Description

[0024] Figure 1 This is a flowchart illustrating the specific implementation of the present invention.

[0025] Figure 2 The present invention is a model of a rope-driven flexible continuous robot: Figure (a) is the physical model; Figure (b) is the simulation model.

[0026] Figure 3 The implementation of the planning algorithm in the absence of obstacles is as follows: Figure (a) shows the comparison between the final and initial configurations and the end trajectory curve when only the target position is given; Figure (b) shows the rope length driving law when only the target position is given, and Figure (c) shows the corresponding rope force driving law; Figure (d) shows the comparison between the final and initial configurations and the end trajectory curve when the same target position and target attitude are given; Figure (e) shows the rope length driving law when both the target position and target attitude are given, and Figure (f) shows the corresponding rope force driving law.

[0027] Figure 4 The implementation of the planning algorithm when there are 4 spherical obstacles is as follows: Figure (a) shows the planning given the target position and obstacles; Figure (b) shows the control law of the driving length of the rope; Figure (c) shows the driving force curve of the rope; Figure (d) shows the constraint comparison curve with and without considering obstacle constraints; Figure (e) shows the end trajectory curve with and without considering obstacle constraints.

[0028] Figure 5 The implementation of the planning algorithm for a pipeline scenario with multiple spherical obstacles and a movable base for the robot is as follows: (a) Comparison of the final configuration with the initial configuration and the end trajectory curve; Figure (b) shows the driving force curve of the rope; Figure (c) shows the driving length curve of the rope; Figure (d) shows the driving law of the end base; Figure (e) shows the constraint condition curve of the obstacle. Detailed Implementation

[0029] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, 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 those skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] Combination Figure 1 The method involved in this invention was verified. The rope-driven continuous robot verified by the algorithm can be referred to... Figure 2 As shown. The reason for the tensioning integral continuous robot lies in its mechanical units with different stiffnesses, exhibiting the characteristics of a rigid-flexible coupled system, making it representative. For example... Figure 2 The continuous tensioning robot shown consists of rods, springs, and actively driven ropes, enabling a wide range of contraction, spatial bending, and torsional movements. The rods are relatively rigid, while the springs and ropes are relatively flexible, forming a geometrically nonlinear system with rigid-flexible coupling. Traditional kinematic programming methods suffer from significant discrepancies between the kinematic model and the actual model due to the presence of the flexible structure, making approximation difficult, computationally intensive, and lacking robustness. Therefore, this patent employs a static instantaneous optimal control method, improving computational efficiency and robustness.

[0031] exist Figures 3-5 The robot's parameters are as follows: The radius of the ring is 35 mm; the number of active drive ropes is 6; the Young's modulus of the pressure bar is 84 GPa, the free length is 60.6 mm, and the area is 7.07e-6 m². 3 The spring has a Young's modulus of 0.5 GPa, a free length of 32.2 mm, and a cross-sectional area of ​​3.14e-6 m². 3 The ropes have a Young's modulus of 39 GPa, free lengths of 0.8 m and 0.5 m, and a cross-sectional area of ​​7.85e-7 m². 3 The initial pre-drive lengths of the six ropes are all [40, 20, 20, 20, 20, 20] mm, with corresponding tensions of [4.18, 5.34, 4.08, 5.34, 4.08, 5.34, 4.08, 5.34] N, used to maintain the robot's rigidity. The maximum drive length limits for the long and short active ropes are 0.18 m and 0.12 m, respectively. Figure 3 In the example, the maximum and minimum driving forces are given as 13N and 4N, respectively. Figures 4-5 The maximum and minimum driving forces are limited to 13 and 2 N, respectively.

[0032] (1) Step 1: Using the positional finite element method, the generalized forces of different elements (rod elements, spring elements, rope elements) are established using the principle of virtual work. The internal forces are integrated and assembled onto the relevant nodes using finite element assembly technology, and finally the force is obtained. With constraints The static equilibrium equation (1) is obtained. Simultaneously, the positional change of the rope end and the motor connection point is simulated (i.e.,...). Figure 2 The amount of upward or downward change in the position of the rope node is used to describe the driving length of the rope. .

[0033] (2) Step 2, using initial conditions: robot initial generalized coordinates Initial drive length and the initial Lagrange multipliers First, the sensitivity information of the static system with respect to the control variables is solved according to equation (15). and Then, using equations (11) and (23), the standard planning problem (8) is transformed into an instantaneous optimal control problem, expressed as equation (10). Figure 3 (a) only gives the desired end position, while (b) gives both the desired end position and the desired end normal vector orientation (positive x-axis). Figure 4 Given the desired end position and 4 spherical obstacles. Figure 5 Given the desired end position and 80 small ball constraints, a pipe obstacle with a radius of 50 mm is simulated.

[0034] (3) Step three: The transformed QP or nonlinear optimization problem (10) can be solved using a general QP or SQP nonlinear optimization solver. Compare it with the known quantity from the previous step. The sum of these values ​​gives the total driving force of the rope. .

[0035] (4) Step four: Calculate the total driving force of the rope. Subsequently, in the absence of obstacles, an asynchronous update strategy (28) can be used to update the state variables, while in the presence of obstacles, it is best to use a synchronous update strategy (26) to update the state variables so that they can more accurately meet the obstacle avoidance constraints. . Figure 4 and Figure 5 The obstacle constraint condition is the inverse of the minimum distance between the robot and the obstacle and the safe distance. That is, in Breach of contract at that time The constraints are met at that time. Figures 3-5 Both can complete the specified task under the conditions of setting the maximum and minimum driving force.

[0036] (5) The fifth step is to determine whether the designated task has been completed, i.e. , For a given task accuracy requirement, in Figures 3-5 The examples all give If the task requirements are not met, return to step two to continue the calculation; if the requirements are met, exit the calculation. Figures 3-5 In this study, when the robot system has 207 degrees of freedom, the calculation time for single-step control variables in the Matlab environment is less than 10ms, and all tasks eventually converge, satisfying the corresponding state constraints and control constraints. Therefore, this invention improves the computational efficiency and robustness of large-scale static motion planning.

[0037] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot, characterized in that, The method first constructs the static equations based on the position finite element method, according to the type of mechanical units contained in the robot model, using the principle of virtual work and rope-driven constraints. Second, combining the static equilibrium equations, the strongly nonlinear programming problem involving state constraints, control constraints, and high-dimensional strongly nonlinear static equilibrium constraints is transformed into a weakly nonlinear instantaneous optimal control problem. Then, the control variables are solved using a standard nonlinear optimization solver, and the state variables are updated using single-step or multi-step, synchronous or asynchronous update strategies. Finally, the method determines whether to exit the loop based on the task completion status.

2. The real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot according to claim 1, characterized in that, It has the following steps: The first step is to establish the static equations of the flexible robot based on the position finite element method and with rope-driven constraints; (1) in, For the robot's generalized coordinates, The generalized coordinates represent the number of coordinates, and their physical meaning is the position information of each node of the robot in the global coordinate system. The dimension of the matrix is OK List; For the system constraints, the Lagrange multipliers, The number of Laplace multipliers represents the driving force of the rope. With mechanical constraints Constraint reaction force The number of actively driven components; These are the constraint equations of the system. To drive constraints, For mechanical constraints; To constrain the partial derivatives with respect to generalized coordinates; This refers to the control quantity of the robot, specifically the driving length of the rope. The generalized force of the system is represented as: (2) in, and These are the robot's generalized internal force and generalized external force, respectively. The system at this point is a mechanical system containing both motion-driven constraints and mechanical constraints; The second step is to establish the control problem for continuous robot obstacle avoidance. The task of a continuous robot requires its end effector to move from one specified position and orientation to another specified position and orientation: (3) in, This represents the robot's end-effector pose. The desired end-effector pose of the robot. and These are the robot's end-effector position and attitude vector, respectively. At the same time, it is also necessary to define the performance indicators of continuous robots. : (4) Restrictions are imposed on the state variables and control variables, and these restrictions are described by the following inequality constraints: (5) (6) (7) in, This indicates distance constraints related to obstacle avoidance; Indicates the driving force of the rope. This indicates the minimum driving force limit for the rope. Indicates the maximum driving force limit of the rope; This represents the control quantity of the robot, i.e., the driving length of the rope. Indicates the minimum driving length of the rope. Indicates the maximum driving length of the rope; In summary, the optimal control problem for static programming of a continuous robot is obtained, and its overall expression is: (8) The third step is the construction and solution of the instantaneous optimal statics algorithm. Convert the control quantity into an instantaneous control quantity The static equations are then transformed into a discrete problem for the current step, expressed as: (9) in, The length of the rope that has already been driven is a known quantity relative to the next step; The length of the next drive by the controller; Represents the generalized coordinates of the current control step; Represents the Lagrange multiplier of the current control step; Let represent the generalized force of the current control step; transform equation (8) into the following instantaneous optimal control problem construction format: (10) in, Indicates the length of the rope that has been driven; The rope driving force represents the current control step; This indicates the amount of drive of the control rope in the current control step; This indicates the minimum distance between the current control step and the obstacle; We can use the shortest distance to the target point and the minimum driving force as indicators to transform the strong constraint of equality constraints into a soft constraint of indicators, written as: (11) (12) (13) in, This indicates the current end-effector pose of the robot. This represents the weight matrix for the end-effector pose index. This represents the weight matrix of the driving force indicators; This represents the robot end-effector pose calculated in the previous step; This indicates the sensitivity of the robot's end-effector pose to the control input in the previous control step. This indicates the robot's sensitivity to generalized coordinates in the previous control step; This indicates the sensitivity of the robot's generalized coordinates to the control quantity in the previous control step; This represents the Lagrange multiplier obtained in the previous step; This indicates the sensitivity of the Lagrange multiplier to the control quantity in the previous control step; In formulas (12) and (13) and The gradient information of the control quantity obtained through the static sensitivity method is as follows: (14) The above equation can be further simplified into a system of linear matrix equations, with the unknowns being... (15) in, and The expression is as follows: (16) (17) The performance indicators can then be further simplified to: (18) in, and The expressions are as follows: (19) (20) The gradient of the performance index with respect to the control variable is expressed as: (21) The gradient of the terminal state relative to the control quantity in equation (20) Represented as: (22) Obstacle constraints can be expressed using the following formula: (23) in, Indicates the distance between the obstacle and the detected point; This represents the safe distance at which the robot will not collide. If we assume it to be a spherical obstacle, then the distance between the obstacle and the detected point can be obtained as follows: (24) in, As the first robot Location extraction matrix for each detected point; For the first The center position vector of each obstacle; the gradient of the obstacle constraint relative to the control variable. It can be represented as: (25) The final control quantity can be solved using an optimization solver based on the optimization problem established by formula (10); on the one hand, in the absence of obstacles, i.e. When the set is empty, the established optimization problem is a standard quadratic programming problem, which can be solved using a mature QP solver; on the other hand, when obstacles exist... If the set is not empty, the obstacle constraint is a nonlinear constraint and is solved using a nonlinear programming solver. Fourth step, update the state variables The constraint equations in statics include both mechanical and control constraints; if only control constraints are included... When updating state variables, a single-step Newton iteration update strategy can be used; however, when mechanical constraints are involved... When a nonlinear iterative strategy is used, it is necessary to ensure the stability of the constraints; when a single-step Newton update strategy is used, the Jacobian matrix during the update is different from that used when calculating the sensitivity. The matrices are the same, that is Then, at this point, the matrix should be pre-processed. Perform LU decomposition preprocessing to increase the solution speed; Fifth step, determine whether the task is satisfied. when If the task requirements are met, then the control task is exited; among which... For the accuracy requirements of the task; when If the task requirements are not met, continue repeating steps two through four until the task requirements are met, at which point the loop can exit.

3. The real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot according to claim 2, characterized in that, In the fourth step, the single-step and nonlinear Newton update strategies are respectively as follows: Single-step Newton update strategy: (26) Nonlinear Newton update strategy: (27) in, Let be the number of Newton steps, until... , Let the residual convergence be a small quantity given by one person. This ensures that the residuals from the iteration satisfy the static equilibrium equations. This represents the state variable that is updated when the current control variable is substituted. This represents the state variables of the previous control step; The inverse matrix of the augmented tangential stiffness matrix of the equation; Represented as and Substituting the state variables into equation (1) yields the residuals; Indicates the control quantity for the current control step; Indicates in The state variables of the second Newton iteration; Indicates the first The state variables of the second Newton iteration; express The inverse matrix of the augmented tangential stiffness matrix in the second Newton iteration; Represented as and Substituting the state variables into equation (1) yields the residuals; To improve computational efficiency in control, state variable updates are further divided into asynchronous and synchronous update strategies. Specifically, the state variables' sensitivity to control is updated every [time period]. Update every control step or every other control step: (a) Synchronous update strategy The synchronous update strategy is to update the state variables and the static sensitivity after each calculation of the control quantity: after solving the control quantity (10), the state variables are updated using equation (26) or equation (27); in the next step, the sensitivity information is recalculated using the updated state variables to optimize the solution of the control variables. (b) Asynchronous update strategy The asynchronous update strategy means that after each calculation of the control variable is completed, the static sensitivity is not updated; instead, the state variables are updated using the following update format: (28) In the above formula Represents the interval The static sensitivity matrix updated by equation (15) is used to correct the objective function (18); After updating the state variables, the current pose of the robot's end effector can be obtained. This can be used to determine whether the robot's current end-effector pose meets the task requirements.

4. The real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot according to claim 2, characterized in that, In step one, the driving constraint can be written as: （29） in, This is the extraction matrix for the rope driving direction.

5. The real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot according to claim 2, characterized in that, In step two, the robot's current position and posture The following formulas can be used to obtain the results: （30） （31） （32） （33） In the above formula This is the extraction matrix for the robot's desired controlled points. Extract the matrix for the generalized coordinates that include the pose.

6. The real-time static obstacle avoidance planning method for a rope-driven flexible continuous robot according to claim 2, characterized in that, In step three, the gradient of the terminal state relative to the control variable can be obtained by the following formula: （34） （35） （36） In the above formula Let be the antisymmetric matrix corresponding to the vector.

Images