Continuous body mechanical arm based on sine and cosine particle swarm optimization algorithm and inverse kinematics solving method thereof

Abstract

The invention discloses an inverse kinematics solving method for a continuum mechanical arm based on a sine and cosine particle swarm optimization algorithm, the mechanical arm comprises two two-degree-of-freedom continuous sections which are connected by a rigid straight section and stacked on a rotatable and translational base rod, and a constant curvature arc hypothesis is adopted for a single continuous section. Firstly, a kinematic equation of the continuum mechanical arm is established, and the position of an end effector of the mechanical arm is deduced; then, an optimization objective function and constraint conditions of the inverse kinematics problem of the mechanical arm are determined, and configuration variables are searched for the position of an end effector; optimizing the configuration variables by adopting a sine and cosine particle swarm optimization algorithm; the position of the end effector is updated through the obtained kinematics equation, and the target function value of the generation is calculated; and finally, obtaining the optimal position of the end effector. Compared with the classical particle swarm algorithm, the solution of the inverse kinematics of the continuum mechanical arm has better convergence speed and calculation precision.

Description

Technical Field

[0001] The present invention belongs to the field of continuum robot arm control, and in particular relates to a continuum robot arm based on a sine-cosine particle swarm optimization algorithm and an inverse kinematics solution method thereof. Background Art

[0002] Laparoscopic minimally invasive surgery involves inserting surgical instruments and endoscopes into the patient's body, and the doctor uses the surgical instruments under the guidance of images to complete the surgical task. Compared with surgery with large incisions, minimally invasive surgery has attracted attention in recent years due to its fast recovery, less trauma, and low postoperative infection rate. Continuous robots are suitable for laparoscopic minimally invasive surgery due to their small size and curved shape.

[0003] In order to complete more complex tasks, continuum robots are often designed as multi-segment structures. The bending deformation of each continuum segment in the robot arm will change the position and posture of its tip, and there is a strong coupling relationship between the continuous segments. Therefore, it is difficult to directly obtain the inverse kinematics analytical solution of the continuum manipulator.

[0004] At present, the numerical iteration method based on Jacobian matrix is ​​one of the most commonly used inverse kinematics solution methods, but the iterative solution method has low computational efficiency. Due to its many advantages, meta-heuristic algorithms have been widely promoted.

[0005] As a swarm intelligence optimization algorithm, particle swarm optimization algorithm has the advantage of high stability. However, particle swarm optimization algorithm is prone to converge prematurely in the optimization process, thus falling into local optimum.

[0006] Therefore, it is necessary to develop an inverse kinematics solution method for continuum manipulator based on sine-cosine particle swarm algorithm. Summary of the invention

[0007] The technical problem to be solved by the present invention is to provide a continuum robot arm based on a sine-cosine particle swarm optimization algorithm and an inverse kinematics solution method thereof, which solves the problem in the prior art that the particle swarm optimization algorithm falls into a local minimum during the optimization solution process.

[0008] The present invention adopts the following technical solutions to solve the above technical problems:

[0009] The inverse kinematics solution method of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm includes the following steps:

[0010] Step 1: Establish the kinematic equation of the continuum robot arm and derive the position of the end effector of the robot arm;

[0011] Step 2: Determine the optimization objective function and constraints of the inverse kinematics problem of the robot arm and find the configuration variables for the position of the end effector;

[0012] Step 3: Use sine-cosine particle swarm optimization algorithm to optimize the configuration variables;

[0013] Step 4: Use the kinematic equation obtained in step 1 to update the position of the end effector and calculate the objective function value of this generation;

[0014] Step 5: Repeat steps 3 and 4 until a preset number of iterations is reached to obtain the optimal position of the end effector.

[0015] The robot arm comprises two two-degree-of-freedom continuous segments connected by a rigid straight segment and stacked on a rotatable and translatable base rod. For a single continuous segment, a constant curvature arc assumption is adopted.

[0016] No. The forward kinematics of the segment is summarized in the following formula:

[0017]

[0018] in Representing coordinate system In the base ring coordinate system The origin to end ring coordinate system The position vector of the origin; represents the length of a single-segment continuum arm; Represents the coordinate axis from the bending plane To the bending plane coordinate axis about The rotation angle of Represents the coordinate axis from the bending plane to the bending plane The bending angle.

[0019] In step 2, the objective function is defined as follows:

[0020]

[0021] The constraints are as follows:

[0022]

[0023] in, is the end effector position vector, is the desired position vector of the end effector, is the rotation variable vector, is the initial vector of the rotation variable, is the bending variable vector, is the initial vector of bending variables, , , are all proportional factors.

[0024] In step 3, the specific process of optimizing the configuration variables using the sine-cosine particle swarm optimization algorithm is as follows:

[0025] Step 3.1: Initialize the parameters, randomly generate the initial value of the end effector position under the constraints, and set the number of iterations and the number of particles per generation;

[0026] Step 3.2, calculate the objective function value of the particle, and select the global optimal position and the individual optimal position;

[0027] Step 3.3, determine whether the increment of the global optimal value is within the EPS range; if so, execute step 3.4, otherwise, execute step 3.5;

[0028] Step 3.4, each particle updates its own speed and position according to the optimal position experienced by the local optimal particle and the global optimal position;

[0029] Step 3.5: Update each particle in The position vector at iteration .

[0030] In step 3.4, update your speed and location The formula is as follows:

[0031]

[0032] in, is a random number between (0,1), and They are respectively The learning coefficients associated with each iteration are is the global optimal position, is the individual optimal position, is the weight factor.

[0033] In step 3.5, update each particle in The formula for the position in iterations is as follows:

[0034]

[0035] in, and Respectively Second and The position variable in the iteration, , , is a random number between (0,1), For the influence factor, specify the next position of the solution.

[0036] A continuum robot based on a sine-cosine particle swarm optimization algorithm comprises a robot and a driving unit thereof, wherein the robot comprises a base fixing section, a proximal continuum section, an intermediate transition section, a distal continuum section and a tool connecting section; the proximal continuum section and the distal continuum section of the robot are connected by the intermediate transition section and stacked on the base fixing section, so as to realize multi-degree-of-freedom movement; the distal continuum section is connected to a surgical instrument through a tool connecting section; the driving unit of the robot comprises a continuum section driving unit, a straight line segment driving unit and a torsion driving unit.

[0037] It also includes a host computer for sending control signals to the drive unit to control the movement of the robotic arm.

[0038] The intermediate transition section is a rigid straight section connection.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] 1. Compared with the classic particle swarm algorithm, the solution of inverse kinematics of continuum manipulator has better convergence speed and calculation accuracy.

[0041] 2. This method also overcomes the problem of low computational efficiency of numerical iteration methods based on Jacobian matrix. In the future, the proposed method can also be extended to industrial robot arms in unknown environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 The present invention is a flow chart of the inverse kinematics solution method of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm.

[0043] Figure 2 It is a structural schematic diagram of the robot arm control computer equipment of the present invention. DETAILED DESCRIPTION

[0044] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the scheme of the present invention, and provides a detailed implementation method and a specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0045] The inverse kinematics solution method of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm includes the following steps:

[0046] Step 1: Establish the kinematic equation of the continuum robot arm and derive the position of the end effector of the robot arm;

[0047] Step 2: Determine the optimization objective function and constraints of the inverse kinematics problem of the robot arm and find the configuration variables for the position of the end effector;

[0048] Step 3: Use sine-cosine particle swarm optimization algorithm to optimize the configuration variables;

[0049] Step 4: Use the kinematic equation obtained in step 1 to update the position of the end effector and calculate the objective function value of this generation;

[0050] Step 5: Repeat steps 3 and 4 until a preset number of iterations is reached to obtain the optimal position of the end effector.

[0051] Specific embodiments, such as Figure 1 , Figure 2 As shown,

[0052] A continuum robot based on a sine-cosine particle swarm optimization algorithm comprises a robot and a driving unit thereof, wherein the robot comprises a base fixing section, a proximal continuum section, an intermediate transition section, a distal continuum section and a tool connecting section; the proximal continuum section and the distal continuum section of the robot are connected by the intermediate transition section and stacked on the base fixing section, so as to realize multi-degree-of-freedom movement; the distal continuum section is connected to a surgical instrument through a tool connecting section; the driving unit of the robot comprises a continuum section driving unit, a straight line segment driving unit and a torsion driving unit.

[0053] It also includes a host computer for sending control signals to the drive unit to control the movement of the robotic arm.

[0054] The intermediate transition section is a rigid straight section connection.

[0055] The continuum robot is a wire-driven continuum robot, which consists of a superelastic carbon fiber rod, a base plate, a spacer plate, etc. The change in the length of the drive rope controls the movement of the robot, thereby adjusting the position of the end of the robot.

[0056] The inverse kinematics solution method of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm includes the following steps:

[0057] 1: Establish the kinematic equations of the continuum robot. The robot used consists of two two-degree-of-freedom continuous segments connected by a rigid straight segment and stacked on a rotatable and translatable base rod. Establish a base coordinate system at the bottom of the base rod of the continuum robot, and establish coordinate systems at the end of the base rod, the end face of the continuous segment, the end of the straight rod, and the end face of the second continuous segment. Establish a world coordinate system at the base of the continuum robot The first continuous segment is stacked at the far end of the base rod, with the coordinate system From the world coordinate system along The second continuous segment is connected to the first continuous segment through the middle rigid straight segment. Coordinate system is the coordinate system at the end of the first continuous segment It is obtained by translating L, where L is the length of the rigid straight segment between two consecutive segments.

[0058] For a single continuous segment, the constant curvature arc assumption is used. The forward kinematics of the segment is summarized in the following formula:

[0059]

[0060] in, Representing coordinate system In the base ring coordinate system The origin to end ring coordinate system The position vector of the origin. Represents the length of a single-segment continuum arm. Represents the coordinate axis from the bending plane To the bending plane coordinate axis about The rotation angle of . Represents the coordinate axis from the bending plane to the bending plane The bending angle of . Coordinate transformation matrix The coordinate system and Connect.

[0061]

[0062] The kinematics of the robot arm are derived from the relationship between the coordinate systems. The derivation of the end position of the robot arm is as follows:

[0063] .

[0064] 2: Determine the optimization objective function and constraints of the inverse kinematics problem of the manipulator. The solution of the inverse kinematics problem is reduced to the process of finding configuration variables based on the position of the end effector. The position of the end effector can be obtained through sensors. The inverse kinematics problem of the manipulator is a multi-objective optimization problem. The objective function is to minimize the change in the rotation angle and bending angle when the end effector reaches the target coordinates to save time and energy.

[0065] The sine-cosine particle swarm optimization algorithm is used to optimize the configuration variables to meet the following goals: 1) Make the distance between the end effector and the target position in the rectangular coordinate system small enough to reach the target position; 2) Minimize the changes in the rotation angle and bending angle to meet the goal of optimal time and energy of the robot arm. The robot arm parameter constraints are as follows:

[0066]

[0067] The objective function It is used to determine whether the optimal solution is obtained. Its definition is as follows:

[0068]

[0069] in, is the end effector position vector, is the desired position vector of the end effector, is the rotation variable vector, is the initial vector of the rotation variable, is the bending variable vector, is the initial vector of bending variables. , , are all proportional factors.

[0070] In this embodiment, the desired position of the end effector , the constraints of each rotation angle and bending angle are as follows:

[0071]

[0072] The initial value of the continuum robot is . Each scale factor , , .

[0073] The objective function is used to determine whether the particles generated by the sine-cosine particle swarm optimization algorithm are sufficient to meet the set goals. Only when the objective function is small enough can the optimal particles be obtained. The objective function can be further expressed as:

[0074] .

[0075] 3: Parameter initialization. In order to ensure the diversity of the first generation of particles, the position , The initial value of is randomly generated under the constraints, the number of particles in each generation is M, and the initial value of the particles is as follows:

[0076]

[0077] 4: Calculate the objective function value of the first generation of particles and select the global optimal position and the individual optimal position When the increment of the global optimal value is within the EPS range, each particle updates its own speed according to the optimal position experienced by the local optimal particle and the global optimal position. and location The update formula is as follows:

[0078]

[0079] Among them, rand(1) is a random number between (0,1), and are the learning coefficients associated with the tth iteration, The value of is linearly reduced to weaken the influence of the local optimal particles. The value of increases linearly to enhance the influence of the global optimal particle. and It is obtained from the following formula:

[0080]

[0081] in is the maximum number of iterations, is the weight factor, is the global optimal position, is the individual optimal position. In this embodiment, , , , . Weight factor , the maximum number of iterations of the algorithm is 300, and the number of particles per generation is 30.

[0082] 5: When the increment of the global optimal value is not within the EPS range, apply the following relationship to update .

[0083]

[0084] in, and Respectively Second and The position variable in the iteration; , , is a random number between (0,1), is the influence factor, specifying the next position of the solution. In order to gradually narrow the range of random solutions, The value of decreases gradually from 1 to 0, and the expression is:

[0085]

[0086] In this embodiment, the constant , EPS=0.02. In the search phase, the sine-cosine particle swarm optimization algorithm can generate more random solutions to increase the diversity of solutions and avoid falling into the local optimum. In the development phase, the randomness of the solution is reduced to ensure rapid convergence to the global optimum.

[0087] 6: Update the position of the end effector using the kinematic equations obtained in step 1 and calculate the objective function value of this generation. Update the global optimal position and the individual optimal position . When the maximum number of iterations is reached, the iteration ends.

[0088] Furthermore, in step 2, the objective function is used to determine whether the particles generated by the sine-cosine particle swarm optimization algorithm are sufficient to meet the set goals. Only when the objective function is small enough can the optimal particles be obtained. The objective function can be further expressed as:

[0089]

[0090] Furthermore, in step 6, when the objective function value of the particle is less than the individual optimal position The objective function value of the particle is updated to update the individual optimal position. The objective function value of is used to update the global optimal position.

[0091] On the other hand, the present invention also provides a computer device, comprising at least a processor and a memory, wherein instructions related to a method for solving inverse kinematics of a continuum robot arm based on a sine-cosine particle swarm optimization algorithm are stored in the memory and run on the processor to complete the solution of the inverse kinematics of the continuum robot arm described above.

[0092] If the above functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc., which can store program codes.

[0093] It should be understood that the present solution is not limited to the above-mentioned implementation mode, and the devices and structures not described in detail should be understood to be implemented in a common manner in the art; ordinary technicians in the art can make many modifications and changes based on the above-mentioned method and the technical solution of the present invention without departing from the scope of the technical solution of the present invention. Therefore, any technical solution that can be obtained by technicians in the technical field through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the existing technology should be within the scope of protection determined by the claims.

Claims

1. A continuum manipulator inverse kinematics solution method based on sine-cosine particle swarm optimization algorithm, characterized by: The steps include: Step 1: Establish the kinematic equation of the continuum robot arm and derive the position of the end effector of the robot arm; Step 2: Determine the optimization objective function and constraints of the inverse kinematics problem of the robot arm and find the configuration variables for the position of the end effector; Step 3: Use sine-cosine particle swarm optimization algorithm to optimize the configuration variables; Step 4: Use the kinematic equation obtained in step 1 to update the position of the end effector and calculate the objective function value of this generation; Step 5: Repeat steps 3 and 4 until a preset number of iterations is reached to obtain the optimal position of the end effector.

2. The inverse kinematics solution method for a continuum manipulator based on a sine-cosine particle swarm optimization algorithm according to claim 1 is characterized in that: The robot arm comprises two two-degree-of-freedom continuous segments connected by a rigid straight segment and stacked on a rotatable and translatable base rod. For a single continuous segment, a constant curvature arc assumption is adopted.

3. The inverse kinematics solution method for a continuum manipulator based on a sine-cosine particle swarm optimization algorithm according to claim 2 is characterized in that: No. The forward kinematics of the segment is summarized in the following formula: in Representing coordinate system In the base ring coordinate system The origin to end ring coordinate system The position vector of the origin; represents the length of a single-segment continuum arm; Represents the coordinate axis from the bending plane To the bending plane coordinate axis about The rotation angle of Indicates the coordinate axis from the bending plane to the bending plane The bending angle.

4. The method for solving inverse kinematics of a continuum manipulator based on a sine-cosine particle swarm optimization algorithm according to claim 1, characterized in that: In step 2, the objective function is defined as follows: The constraints are as follows: in, is the end effector position vector, is the desired position vector of the end effector, is the rotation variable vector, is the initial vector of the rotation variable, is the bending variable vector, is the initial vector of bending variables, , , are all proportional factors.

5. The method for solving inverse kinematics of a continuum manipulator based on a sine-cosine particle swarm optimization algorithm according to claim 1, characterized in that: In step 3, the specific process of optimizing the configuration variables using the sine-cosine particle swarm optimization algorithm is as follows: Step 3.1: Initialize the parameters, randomly generate the initial value of the end effector position under the constraints, and set the number of iterations and the number of particles per generation; Step 3.2, calculate the objective function value of the particle, and select the global optimal position and the individual optimal position; Step 3.3, determine whether the increment of the global optimal value is within the EPS range; if so, execute step 3.4, otherwise, execute step 3.5; Step 3.4, each particle updates its own speed and position according to the optimal position experienced by the local optimal particle and the global optimal position; Step 3.5: Update each particle in The position vector at iteration .

6. The method for solving inverse kinematics of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm according to claim 5 is characterized in that: In step 3.4, update your speed and location The formula is as follows: in, is a random number between (0,1), and They are respectively The learning coefficients associated with each iteration are is the global optimal position, is the individual optimal position, is the weight factor.

7. The method for solving inverse kinematics of a continuum manipulator based on the sine-cosine particle swarm optimization algorithm according to claim 5, characterized in that: In step 3.5, update each particle The formula for the position in the iteration is as follows: in, and Respectively Second and The position variable in the iteration, , , is a random number between (0,1), For the influence factor, specify the next position of the solution.

8. A continuum robot based on the sine-cosine particle swarm optimization algorithm, characterized by: The invention comprises a robot arm and a driving unit thereof, wherein the robot arm comprises a base fixing section, a proximal continuum section, an intermediate transition section, a distal continuum section and a tool connecting section; the proximal continuum section and the distal continuum section of the robot arm are connected by the intermediate transition section and are stacked on the base fixing section, so as to realize multi-degree-of-freedom movement; the distal continuum section is connected to the surgical instrument through the tool connecting section; the driving unit of the robot arm comprises a continuum section driving unit, a straight segment driving unit and a torsion driving unit.

9. The continuum robot arm based on the sine-cosine particle swarm optimization algorithm according to claim 8, characterized in that: It also includes a host computer for sending control signals to the drive unit to control the movement of the robotic arm.

10. The continuum robot arm based on the sine-cosine particle swarm optimization algorithm according to claim 8, characterized in that: The intermediate transition section is a rigid straight section connection.

Images