Continuous body robot position tracking and joint limiting avoiding method

Abstract

The invention discloses a continuum robot position tracking and joint limiting avoidance method, and belongs to the field of robot control. According to the method, a current path is obtained through forward kinematics modeling, joint limiting constraint and ideal path generation are combined, a nonlinear programming problem with the maximum path deviation and the minimum path deviation as the target is constructed, and joint increment is iteratively solved to achieve end tracking and overall path fitting. According to the method, the problem that inverse kinematics is difficult to solve is avoided, joint overrun is explicitly avoided, and the method is suitable for high-precision and high-reliability control in a narrow space and an obstacle environment.

Description

Technical Field

[0001] This invention relates to the field of robot position tracking technology, and in particular to a method for position tracking and joint limit avoidance of a continuum robot. Background Technology

[0002] Continuum robots, composed of multiple compliant segments connected in series, possess high degrees of freedom, high flexibility, and high deformability, making them suitable for tasks such as pipeline inspection, confined space exploration, and medical intervention. However, their control presents the following challenges:

[0003] Inverse kinematics is difficult to solve and has poor real-time performance.

[0004] It is difficult to balance end-point positioning and overall shape constraints during path planning;

[0005] Compliant joints have a maximum bending angle limit, and improper control can easily lead to structural damage.

[0006] Tracking errors are difficult to converge in dynamic tasks.

[0007] Existing methods are mostly based on inverse kinematics or Jacobi iterative control, which do not fully consider the synergistic optimization of joint physical constraints and overall path shape, leading to interference, jamming or control failure in real-world constrained environments. Summary of the Invention

[0008] The problem solved by this invention is to provide a method for position tracking and joint constraint avoidance of a continuum robot. This method obtains the current path through forward kinematics modeling, and combines joint constraint with ideal path generation to construct a nonlinear programming problem with the objective of minimizing the maximum deviation of the path. Iteratively solving the joint increments achieves end-effector tracking and overall path alignment. This invention avoids the difficulty of inverse kinematics solution, explicitly avoids joint over-limit, and is suitable for high-precision and high-reliability control in narrow spaces and obstacle environments.

[0009] To achieve the above objectives, the present invention adopts the following technical solution: a method for position tracking and joint constraint avoidance of a continuum robot, comprising the following steps: S1 establishing a forward kinematic model of the continuum robot and calculating the current path point set based on joint variables; S2 mapping the bending angle of a single joint based on segment-level joint variables and determining the maximum bending angle constraint based on material and geometric parameters; S3 generating an ideal path point set based on the current pose of the end effector and the desired motion direction; S4 aligning the current path and the ideal path through interpolation to construct an optimization problem with the maximum path deviation as the objective function; S5 solving the nonlinear programming under joint constraint to obtain the optimal joint increment; S6 updating the joint variables and iteratively executing until the end effector reaches the target point.

[0010] Preferably, the forward kinematics model describes the end pose of each segment through a homogeneous transformation matrix, and the path point set is composed of the end positions of each segment.

[0011] Preferably, the joint limiting constraint converts the segment-level bending angle into a single joint bending angle through a mapping function, and calculates the maximum bending angle based on the disc radius and joint length.

[0012] Preferably, the ideal path point set is generated by advancing the current endpoint point by a fixed step size along the desired direction.

[0013] Preferably, the objective function is the maximum Euclidean distance deviation between the corresponding points of the current path and the ideal path.

[0014] Preferably, the nonlinear programming is solved using gradient descent, sequential quadratic programming, or interior point method.

[0015] A continuum robot control system includes a processor and a memory, the memory storing a computer program that, when executed by the processor, implements the method according to any one of claims 1 to 6.

[0016] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method of any one of claims 1 to 6.

[0017] The beneficial effects of this invention are: the method obtains the current path through forward kinematics modeling, combines joint limit constraints and ideal path generation, constructs a nonlinear programming problem with the goal of minimizing the maximum deviation of the path, and iteratively solves the joint increment to achieve end-point tracking and overall path fit; this invention avoids the difficulty of inverse kinematics solution, explicitly avoids joint over-limit, and is suitable for high-precision and high-reliability control in narrow spaces and obstacle environments. Attached Figure Description

[0018] Figure 1 This is a diagram illustrating the overall architecture of the algorithm of this invention;

[0019] Figure 2 This is a geometric configuration diagram of the robot of the present invention;

[0020] Figure 3 This is a diagram of the kinematic model of the robot of this invention. Detailed Implementation

[0021] 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, and 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.

[0022] Specific implementation examples are given below.

[0023] Example 1: A position tracking system for a continuum robot based on forward kinematics and nonlinear programming

[0024] This embodiment provides a position tracking and joint constraint avoidance system for a continuum robot, applicable to a continuum robot structure with four segments, each segment having three sections, and each section containing two orthogonally arranged compliant joints; the system hardware includes a controller, a servo driver, a propulsion mechanism, and the continuum robot body; the software includes a forward kinematics modeling module, a path planning module, a joint constraint module, a nonlinear optimization solution module, and an iterative control module;

[0025] 1. System initialization and parameter configuration

[0026] The robot's structural parameters are as follows:

[0027] Number of segments: 4;

[0028] Number of sections per segment: 3;

[0029] Section length l i,j =30 mm;

[0030] disk radius R disk =10mm;

[0031] Single joint length l joint =3mm;

[0032] The number of joints per segment is n=2;

[0033] Maximum single joint bending angle θ max ≈17.18°;

[0034] The initial joint variables are set as follows:

[0035] Bending angle θ b (i,j)=0;

[0036] Direction angle θ d (i,j)=0;

[0037] The target trajectory is a straight line path from the starting point to the target point, with a step size δ. p =5mm, the termination condition is that the end position error is less than 1 mm or the number of iterations exceeds 200;

[0038] 2. Forward Kinematics Modeling and Path Generation

[0039] Formula 1: ;

[0040] Formula 2: ;

[0041] Formula 3: ;

[0042] Formula 4: ;

[0043] Formula 5: ;

[0044] Formula 6: ;

[0045] Formula 7: ;

[0046] Formula 8: ;

[0047] Calculate the end point positions P1, P2, P3, and P4 of each segment according to formulas 1 to 8; under the current joint variables, use the homogeneous transformation matrix... Multiplying these segments together yields the end poses of each segment, forming the current path point set:

[0048] ;

[0049] 3. Joint Constraint Modeling

[0050] Formula 9: ;

[0051] Formula 10: ;

[0052] Formula 11: ;

[0053] Formula 12: ;

[0054] According to formulas 9 to 12, the bending angle θ of the section is... b and direction angle θ d Mapped to single-joint bending angles θ1 and θ2 in two orthogonal planes:

[0055] ,

[0056] ,

[0057] The maximum bending angle of a single joint is:

[0058] ,

[0059] Therefore, the joint limit constraints are: 0≤θ1,θ2≤θmax;

[0060] Convert to section-level constraint boundary β − and β + In this embodiment, β is taken as: −=0,β + =n*θ max ;

[0061] 4. Ideal path generation and deviation calculation

[0062] Based on the current endpoint P4 and the desired forward direction (a straight line in this embodiment), with a step size δ p Promoting the generation of ideal path point set I path To improve calculation accuracy, the current path C is... path Perform cubic spline interpolation to obtain the dense path point set C. dense ;

[0063] Calculate the Euclidean distance deviation between corresponding points: ;

[0064] Take the maximum deviation as the objective function: ;

[0065] 5. Solving nonlinear programming problems

[0066] Construct the following optimization problem:

[0067] ;

[0068] The optimal joint increment for the current step is obtained by using the Sequential Quadratic Programming (SQP) method. and ;

[0069] 6. Joint Update and Iteration Control

[0070] Update joint variables:

[0071] ← + , ← + ;

[0072] Repeat steps 2 to 6 until the endpoint reaches the target point or the termination condition is met;

[0073] 7. Simulation Verification Results

[0074] Simulations were performed in the MATLAB / Simulink environment. The robot has 24 degrees of freedom and a total length of approximately 360 mm. Simulation results show:

[0075] The end-position tracking error is consistently less than 0.8 mm;

[0076] The bending angle of each compliant joint did not exceed 17°;

[0077] The overall path conforms to the ideal path, with the maximum deviation in the middle section being less than 2 mm;

[0078] The average single-step calculation time is less than 10 ms, which meets the requirements of real-time control.

[0079] Example 2: A step-by-step advance control strategy suitable for narrow passages

[0080] Based on Example 1, this embodiment combines a linear propulsion mechanism to enable the robot to gradually enter and avoid obstacles in narrow passages.

[0081] 1. Channel Model and Constraint Mapping

[0082] Define the channel as a cylindrical space with a diameter of 40 mm; add a channel constraint term to the optimization objective function:

[0083] ;

[0084] in This represents the minimum distance between a path point and the channel wall. These are the weighting coefficients;

[0085] 2. Segmented advancement strategy

[0086] After each optimization step, the robot is propelled forward by a propulsion mechanism. p Then, the path planning and joint adjustments are re-implemented to achieve a cycle of "advancement - adjustment - re-advancement";

[0087] 3. Experimental Results

[0088] Tests were conducted on a physical prototype (using a NiTi alloy compliant joint), with a channel diameter of 40 mm and a length of 500 mm; experimental results showed that:

[0089] The robot was able to complete the entire process smoothly without any collisions.

[0090] The joints did not exhibit excessive or plastic deformation;

[0091] The end-positioning accuracy is maintained within 1.5 mm.

[0092] Example 3: Obstacle Avoidance Path Tracking in Multi-Obstacle Environments

[0093] This embodiment further considers the situation where there are multiple obstacles in the environment. By marking obstacle areas on the map and introducing an obstacle avoidance penalty in the optimization, safe path tracking is achieved.

[0094] 1. Obstacle Modeling

[0095] Represent the obstacle as a sphere or cylinder constrained region, and add an obstacle avoidance term to the objective function:

[0096] ;

[0097] in Let k be the distance from the path point to the k-th obstacle. To maintain a safe distance;

[0098] 2. Real-time replanning mechanism

[0099] In each optimization step, the obstacle position is updated based on the current sensor data, and the objective function is dynamically adjusted to achieve real-time obstacle avoidance.

[0100] 3. Simulation and physical verification

[0101] Tested on the V-REP simulation platform and physical prototype, the robot can stably track the desired trajectory in a dynamic environment with multiple obstacles, without collisions, and the joint limits are always met.

[0102] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for position tracking and joint constraint avoidance in a continuum robot, characterized in that, Includes the following steps: S1. Establish the forward kinematics model of the continuum robot and calculate the current path point set based on joint variables. S2. Map the single joint bending angle based on segment-level joint variables and determine the maximum bending angle constraint based on material and geometric parameters. S3. Generate the ideal path point set based on the current pose of the end effector and the desired motion direction. S4. Align the current path with the ideal path through interpolation and construct an optimization problem with the maximum path deviation as the objective function. S5. Solve the nonlinear programming under joint constraint to obtain the optimal joint increment. S6. Update the joint variables and iterate until the end effector reaches the target point.

2. The method for position tracking and joint limit avoidance of a continuum robot according to claim 1, characterized in that, The forward kinematics model describes the end pose of each segment through a homogeneous transformation matrix, and the path point set is composed of the end positions of each segment.

3. The method for position tracking and joint limit avoidance of a continuum robot according to claim 1, characterized in that, The joint limiting constraint converts the segment-level bending angle into a single joint bending angle through a mapping function, and calculates the maximum bending angle based on the disc radius and joint length.

4. The method for position tracking and joint limit avoidance of a continuum robot according to claim 1, characterized in that, The ideal path point set is generated by advancing the current endpoint point by a fixed step size along the desired direction.

5. The method for position tracking and joint limit avoidance of a continuum robot according to claim 1, characterized in that, The objective function is the maximum Euclidean distance deviation between the corresponding points of the current path and the ideal path.

6. The method for position tracking and joint limit avoidance of a continuum robot according to claim 1, characterized in that, The nonlinear programming problem is solved using gradient descent, sequential quadratic programming, or interior point method.

7. A control system for a continuum robot, characterized in that, It includes a processor and a memory, the memory storing a computer program that, when executed by the processor, implements the method of any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method of any one of claims 1 to 6.

Images