Collaborative anti-collision control method and system for multi-flexible robot arm and storage medium

Abstract

The application discloses a kind of collaborative anti-collision control method, system and storage medium of multiple flexible mechanical arms, applied to the system including at least two flexible mechanical arms;Including: for each flexible mechanical arm, establish the forward kinematics mapping relationship from its driving source output to its end position;The arm body configuration of flexible mechanical arm is abstracted as spatial curve, and the minimum safety distance constraint between spatial curve is established as anti-collision constraint;Receive the target end position of each flexible mechanical arm, and calculate current end position;With the upper limit and lower limit of each arm driving source output, anti-collision constraint as constraint condition, the unified constraint optimization problem with minimizing the error between current end position and target end position as optimization goal is constructed, the optimization problem is solved, and the optimal driving source output for controlling at least two flexible mechanical arms collaborative motion is obtained.The application can realize high-precision, collaborative control and efficient active anti-collision without sensing.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically to a collaborative anti-collision control method, system, and storage medium for multiple flexible robotic arms. Background Technology

[0002] In precision procedures such as minimally invasive surgery, multiple small-diameter, high-degree-of-freedom flexible robotic arms are often required to work collaboratively in confined spaces. Existing robot control methods have several shortcomings. Many control algorithms are primarily designed for single robotic arms and rely on sensors mounted at the arm's end effector to acquire real-time position information. This results in limited degrees of freedom and poor real-time computation for highly redundant robotic arms. On the one hand, for extremely small-diameter flexible surgical robotic arms, it is impossible to integrate position sensors at the front end to collect real-time information, making sensorless control a necessity. On the other hand, for multiple flexible robotic arms, such as… Figure 1 As shown, in actual control, the base coordinate system of the flexible robotic arm remains stationary, and the end effector point... P The controlled object refers to the end-effector position (relative to the base coordinate system) of each flexible robotic arm that needs to be controlled. P 1 and P The position point of 2. For highly redundant, multi-segment, and multi-degree-of-freedom flexible robotic arms, the kinematic modeling is relatively complex. At the same time, it is also necessary to avoid the interference and collision between the motion trajectories of the robotic arms. Therefore, how to establish an algorithm model to solve for the optimal drive source output (displacement or angle corresponding to each degree of freedom) is a key and difficult point in control. Summary of the Invention

[0003] To address the shortcomings of the prior art, this invention provides a collaborative anti-collision control method, system, and storage medium for multiple flexible robotic arms, enabling high-precision control of a single highly redundant dexterous surgical robotic arm and collaborative control of multiple flexible robotic arms without position sensors, thus avoiding mutual collisions and interference.

[0004] To achieve the above-mentioned technical objectives, the technical solution adopted by the present invention is as follows: A collaborative collision avoidance control method for multiple flexible robotic arms, applied to a system comprising at least two flexible robotic arms; including: S1. For each flexible robotic arm, establish a positive kinematic mapping relationship from its drive source output to its end position; where the drive source output is a set of control parameters, and the end position is the three-dimensional coordinates of the end of the flexible robotic arm in the workspace. S2. The arm configuration of the at least two flexible robotic arms is abstracted as a spatial curve, and the minimum safe distance constraint between the spatial curves is established as a collision avoidance constraint. S3. Receive the target end positions of at least two flexible robotic arms; calculate the current end position of each flexible robotic arm based on the forward kinematic mapping relationship and the current state output by each arm's drive source; S4. Construct an optimization objective function with the goal of minimizing the error between the current end position and the target end position of at least two flexible robotic arms. Construct a unified constraint optimization problem with the upper and lower limits of the output of each arm's drive source and the anti-collision constraint as constraints. Solve the optimization problem to obtain the optimal drive source output for controlling the coordinated movement of at least two flexible robotic arms.

[0005] As a preferred technical solution, the flexible robotic arm is a coaxial push-pull flexible robotic arm.

[0006] As a preferred technical solution, a single flexible robotic arm consists of two coaxially sleeved push-pull flexible robotic arms, each containing two bending segments. Each bending segment has bending freedom, rotational freedom, and linear movement freedom, and the single flexible robotic arm has a six-degree-of-freedom structure.

[0007] As a preferred technical solution, the forward kinematic mapping relationship is established using a homogeneous coordinate transformation matrix.

[0008] As a preferred technical solution, the relationship between the output of the driving source and the terminal position is expressed as follows: ; Where B is the base point and P is the end point. T z For along z Move in the axial direction T x To perform coordinate transformations along the x-axis, R z To bypass z Axis rotation, R y For coordinate transformation around the y-axis, q p , q d These are the linear displacements of the near and far segments, respectively. α p , α d These are the rotation angles of the proximal and distal segments, respectively. θ p , θ d These are the bending angles of the proximal and distal segments, respectively. L p , L d These are the lengths of the bendable segments at the proximal and distal ends, respectively.

[0009] As a preferred technical solution, in step S4, the constructed optimization objective function is: ; in, f 1( A 1) f 2( A 2) These represent the current end-effector positions of the two flexible robotic arms, , These represent the target end positions of the two flexible robotic arms, respectively. g 1. g 2 represents the upper and lower limits of the output from the drive sources of the two flexible robotic arms, respectively. g 3 represents the minimum safe distance constraint. λ 1. λ 2. λ 3 is a constant.

[0010] As a preferred technical solution, in step S4, the Newton-Raphson iterative algorithm is used to solve the optimization problem.

[0011] The present invention also discloses a collaborative collision avoidance control system for multiple flexible robotic arms, comprising at least two flexible robotic arms and a controller, the controller being configured to perform the method described above.

[0012] A computer-readable storage medium having stored thereon computer program instructions, which, when executed by a processor, cause the processor to perform the method described above.

[0013] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention enables high-precision sensorless collaborative control. By establishing a precise forward kinematic mapping relationship and an optimization framework aimed at minimizing end-effector position error, it can achieve high-precision control of a single highly redundant and dexterous surgical robotic arm without relying on end-effector sensors. It can also plan the spatial configuration of multiple robotic arms working together to complete surgical tasks, avoiding mutual collisions and interference. This is beneficial for achieving refined, minimally invasive, and efficient surgical operations in complex, narrow, and tortuous surgical areas, and solves the problem of sensorless control.

[0014] This invention enables active collision avoidance by abstracting the flexible robotic arm into a spatial parameter curve and establishing a minimum distance constraint, which greatly simplifies the computational complexity of collision detection and makes collision avoidance processing more efficient and accurate. Attached Figure Description

[0015] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0016] Figure 1 This is a schematic diagram of the application scenario of the dual-arm collaborative control of the present invention; Figure 2 This is a schematic diagram illustrating the bending principle of the coaxial push-pull flexible robotic arm in an embodiment of the present invention.

[0017] Figure 3 This is a schematic diagram of the kinematic model of a single six-degree-of-freedom flexible robotic arm in an embodiment of the present invention; Figure 4 This is a schematic diagram of the dual-arm anti-collision mathematical model in an embodiment of the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0019] A collaborative collision avoidance control method for multiple flexible robotic arms, applied to a system comprising at least two flexible robotic arms, includes: S1. For each flexible robotic arm, establish a positive kinematic mapping relationship from its drive source output to its end effector position; wherein, the drive source output is a set of control parameters, such as displacement or angle corresponding to each degree of freedom, and the end effector position is the three-dimensional coordinates of the flexible robotic arm in the workspace; the mapping relationship is essentially a function that can uniquely calculate the end effector position of the flexible robotic arm based on the given drive source output parameters.

[0020] S2. The arm configuration of the at least two flexible robotic arms is abstracted as a space curve, simplifying the mathematical description of the configuration, and a minimum safe distance constraint between the space curves is established as an anti-collision constraint; the anti-collision constraint means that the shortest distance between any two points on these two space curves must always be greater than or equal to a preset minimum safe distance E.

[0021] S3. Receive the target end-effector positions of at least two flexible robotic arms; calculate the current end-effector position of each flexible robotic arm based on the forward kinematic mapping relationship and the current state of each arm's drive source; S4. Construct an optimization objective function with the goal of minimizing the error between the current end position and the target end position of at least two robotic arms. Construct a unified constraint optimization problem with the upper and lower limits of the output of each arm's drive source (such as the maximum rotation angle of the motor) and the anti-collision constraint as constraints. Solve the optimization problem to obtain the optimal drive source output for controlling the coordinated movement of at least two robotic arms.

[0022] The flexible robotic arm in this invention is a coaxial push-pull type, consisting of two coaxially assembled hollow tubes, inner and outer. Grooves are created on the front end walls of both the inner and outer tubes using ultra-precision laser cutting technology. The outer tube encloses the inner tube, and the two tubes are coaxially assembled and welded together at the front end. The grooves on the inner and outer tubes have a 180-degree phase difference. When the inner and outer tubes undergo relative axial displacement (pushing or pulling), the uneven stiffness caused by the grooves and the asymmetrical structure of the inner and outer tubes will result in controllable bending deformation at the front end.

[0023] In step S1, the bending motion of a coaxial push-pull flexible robotic arm with a single bending segment is first modeled. This includes establishing the push-pull displacement. D p With bending angle θ The relationship between (central angles) is based on the bendable length of the inner tube. L i and the bendable length of the outer tube L o And the distances between the center line of the arc and the two neutral layers are respectively d o , d i Calculate the bending radius and central angle.

[0024] Specifically, for a single curved segment, record L It is the length of the bendable section, and it is the push / pull displacement. R Let be the inner and outer diameter parameters of the pipe. The following describes how to solve for the push-pull displacement given these pipe parameters. D p The relationship between bending angle (central angle) and bending angle: D p Positive numbers represent pulling, negative numbers represent pushing; the flexible length of the inner tube. L i and the bendable length of the outer tube L o ,like Figure 2 As shown in (a), the calculations under these two modes are as follows: If we consider the bent segment as a circular arc, then the bending radii for the outer and inner tubes can be calculated as follows: The distances between the centerline of the arc and the two neutral layers are respectively d o , d i This can be considered as the distance from the center line, such as Figure 2 As shown in (b), the central angle of the curved arc can be calculated as: like Figure 2 As shown in (c), since the bending of a single segment is only planar bending, increasing the rotational degree of freedom... α It can achieve 3D bending.

[0025] Modeling a single flexible robotic arm.

[0026] A single flexible robotic arm consists of two coaxially fitted, push-pull flexible robotic arms, such as... Figure 3 As shown, it comprises two bending segments, namely a proximal segment and a distal segment. One section of the proximal segment is a compliant segment, and a second groove was cut using laser precision machining technology. Each bending segment has bending degrees of freedom (…). θ ), rotational degrees of freedom ( α ) and linear translational degrees of freedom ( q A single flexible robotic arm has a total of six degrees of freedom, as indicated by the following subscripts: p and d These represent the degrees of freedom parameters for the near and far segments, respectively. The relationship between the drive source output and the end effector position is derived using a homogeneous coordinate transformation matrix: B is the base point, and P is the end point. T Z For along z Move in the axial direction T x To perform coordinate transformations along the x-axis, R Z To bypass z Axis rotation, R y This is a coordinate transformation about the y-axis.q p , q d These are the linear displacements of the near and far segments, respectively. α p , α d These are the rotation angles of the proximal and distal segments, respectively. θ p , θ d These are the bending angles of the proximal and distal segments, respectively. L p , L d These are the lengths of the bendable segments at the proximal and distal ends, respectively.

[0027] Based on the above relationships, the driving source output is established using a homogeneous coordinate transformation matrix. With end position Relationship .

[0028] For the control of a single robotic arm, multiple degrees of freedom correspond to three position variables, exhibiting redundancy. An optimization scheme is used to solve for satisfying the end effector target position. Corresponding optimal driver source output : .

[0029] In step S2, the spatial configuration of each flexible robotic arm is described as a continuous spatial curve, such as... Figure 4 As shown, taking two flexible robotic arms as an example, parametric equations are used. f 1( A 1) and f 2( A 2) Description: To prevent the two flexible robotic arms from colliding, the distance between any two points on the curve is set. d ( s,t All of them need to be greater than the preset safety distance. E Therefore, it only needs to be described as the nearest distance. d ( s ,t (greater than) E The constraints.

[0030] In step S4, an optimization objective function H is constructed, which includes the current end-effector positions of the two flexible robotic arms. f 1( A 1) f 2( A 2) Position relative to the target end , The sum of error terms also includes the upper and lower limit constraints of the drive source outputs of the two flexible robotic arms. g 1. g 2 (for example, the displacement or angle of each drive source has physical limitations), and minimum safe distance constraints. g 3, represented as follows: ; in, λ 1. λ 2. λ 3 is a constant.

[0031] The Newton-Rephson iterative algorithm is used to continuously adjust the drive source outputs A1 and A2 of the two flexible robotic arms. In each iteration, the robotic arm configuration and end effector position under the current drive source output are calculated, and it is checked whether all constraints are satisfied. If the end effector position error is large or there is a risk of collision, the drive source output is adjusted according to the gradient information until the end effector position error is minimized and all constraints (including collision avoidance) are satisfied.

[0032] When more than two flexible robotic arms work together, it is only necessary to add the corresponding error terms and the upper and lower limits of the corresponding drive source output to the optimization objective function.

[0033] A collaborative collision avoidance control system for multiple flexible robotic arms includes at least two flexible robotic arms and a controller configured to perform the method described above.

[0034] A computer-readable storage medium having stored thereon computer program instructions, which, when executed by a processor, cause the processor to perform the method described above.

[0035] Of course, the present invention may have other various embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art can make various corresponding changes and modifications according to the present invention, but these corresponding changes and modifications should all fall within the protection scope of the appended claims.

Claims

1. A cooperative collision avoidance control method for multiple flexible robotic arms, applied to a system comprising at least two flexible robotic arms; characterized in that: include: S1. For each flexible robotic arm, establish a positive kinematic mapping relationship from its drive source output to its end position; where the drive source output is a set of control parameters, and the end position is the three-dimensional coordinates of the end of the flexible robotic arm in the workspace. S2. The arm configuration of the at least two flexible robotic arms is abstracted as a spatial curve, and the minimum safe distance constraint between the spatial curves is established as a collision avoidance constraint. S3. Receive the target end positions of at least two flexible robotic arms; calculate the current end position of each flexible robotic arm based on the forward kinematic mapping relationship and the current state output by each arm's drive source; S4. Construct an optimization objective function with the goal of minimizing the error between the current end position and the target end position of at least two flexible robotic arms. Construct a unified constraint optimization problem with the upper and lower limits of the output of each arm's drive source and the anti-collision constraint as constraints. Solve the optimization problem to obtain the optimal drive source output for controlling the coordinated movement of at least two flexible robotic arms.

2. The collaborative collision avoidance control method for multiple flexible robotic arms according to claim 1, characterized in that: The flexible robotic arm is a coaxial push-pull type flexible robotic arm.

3. The collaborative anti-collision control method for multiple flexible robotic arms according to claim 2, characterized in that: Each flexible robotic arm consists of two coaxially mounted push-pull flexible robotic arms, each containing two bending segments. Each bending segment has bending, rotational, and linear movement degrees of freedom, and each flexible robotic arm has a six-degree-of-freedom structure.

4. The collaborative collision avoidance control method for multiple flexible robotic arms according to claim 1, characterized in that: The forward kinematic mapping relationship is established using a homogeneous coordinate transformation matrix.

5. The collaborative collision avoidance control method for multiple flexible robotic arms according to claim 4, characterized in that: The relationship between the output of the driver source and the end position is expressed as follows: ； Where B is the base point and P is the end point. T z For along z Move in the axial direction T x To perform coordinate transformations along the x-axis, R z To bypass z Axis rotation, R y For coordinate transformation around the y-axis, q p , q d These are the linear displacements of the near and far segments, respectively. α p , α d These are the rotation angles of the proximal and distal segments, respectively. θ p , θ d These are the bending angles of the proximal and distal segments, respectively. L p , L d These are the lengths of the bendable segments at the proximal and distal ends, respectively.

6. The collaborative collision avoidance control method for multiple flexible robotic arms according to claim 1, characterized in that: In step S4, the constructed optimization objective function is: ； in, f 1( A 1) f 2( A 2) These represent the current end-effector positions of the two flexible robotic arms, , These represent the target end positions of the two flexible robotic arms, respectively. g 1. g 2 represents the upper and lower limits of the output from the drive sources of the two flexible robotic arms, respectively. g 3 represents the minimum safe distance constraint. λ 1. λ 2. λ 3 is a constant.

7. The collaborative collision avoidance control method for multiple flexible robotic arms according to claim 6, characterized in that: In step S4, the Newton-Raphson iterative algorithm is used to solve the optimization problem.

8. A collaborative collision avoidance control system for multiple flexible robotic arms, characterized in that: It includes at least two flexible robotic arms and a controller, the controller being configured to perform the method as described in any one of claims 1 to 7.

9. A computer-readable storage medium storing computer program instructions thereon, characterized in that: When the instruction is executed by the processor, it causes the processor to perform the method as described in any one of claims 1 to 7.

Images