A Parallel Puncture Robot Trajectory Control Method Based on Event-Triggered Model Predictive Control

Abstract

This invention discloses a trajectory control method for a parallel puncture robot based on event-triggered model predictive control, belonging to the field of robot control technology. First, a kinematic system model of the puncture robot needs to be established. Event-triggered model predictive control is then used to ensure safety, reduce the number of optimization problems solved, and simultaneously improve the motion tracking control performance of the robot's puncture needle. Real-time state information of each joint and the motion state of the reference trajectory are obtained at the current moment, and a finite-time domain MPC constrained optimization problem is constructed. The constrained optimization problem is solved to obtain the predicted optimal control sequence at the current moment. According to the designed event-triggered mechanism, the first s optimal controls are applied to the system. The next trigger moment is updated; at the new trigger moment, the system state is obtained and the MPC constrained optimization problem is updated. This process is iterated until the task ends. This invention can solve the problems of practical constraints, limited computational resources, unmet performance requirements, and safety in puncture robot control.

Description

Technical Field

[0001] This invention belongs to the field of medical puncture robot control technology, specifically a trajectory control method for puncture robots based on model predictive control. Background Technology

[0002] In recent years, with the continuous development of medical technology, we can now effectively treat most common diseases. However, some complex and difficult-to-treat diseases still pose a threat to our health, including prostate cancer, one of the most common cancers in men. Statistics show that there are more than 190,000 new cases of prostate cancer in the United States alone each year. To diagnose and treat prostate cancer, many patients will need to undergo abdominal prostatectomy and biopsy. However, because the prostate is surrounded by numerous nerves, blood vessels, and other tissues, this surgery usually requires dissecting or retracting a large area of ​​surrounding supporting structures, nerves, and blood vessels to access the prostate. After the surgery, the urethra, bladder, and other surrounding tissues also need to be re-sutured. Due to the complexity of the procedure and potential postoperative complications such as urinary incontinence and erectile dysfunction, this traditional surgical approach sometimes significantly reduces the patient's quality of life.

[0003] To minimize damage to surrounding structures, medical researchers have begun exploring the application of minimally invasive techniques in the treatment of prostate cancer. For example, transurethral methods utilize an endoscope to reach the lesion, followed by laser removal of the prostate or collection of samples. However, due to the limited space in the urethra, this method demands extremely high flexibility and precision from the medical devices. Notably, puncture robot technology has made some breakthroughs in this field. Through precise control, they can direct the needle directly to the lesion, significantly reducing damage to surrounding healthy tissue. However, due to technological complexity and cost issues, the application of puncture robots in the diagnosis and treatment of prostate cancer still requires further exploration and optimization. Summary of the Invention

[0004] To address the aforementioned problems, this invention proposes a parallel puncture robot trajectory control method based on event-triggered model predictive control that can achieve optimal motion control under conditions that comprehensively consider system constraints and safety requirements. This method solves the problems of practical constraints, limited computational resources, unmet performance requirements, and safety in puncture robot control.

[0005] This invention relates to an event-triggered parallel puncture robot trajectory control method. The method uses a remote center of motion (RCM) constraint mechanism to provide an attitude adjustment platform for the puncture biopsy instrument. Based on the real-time navigation system, the relative positional relationship between the puncture target and the needle body, as well as the real-time status of the human body environment, are obtained, and a reasonable puncture path is planned. Finally, the puncture instrument is controlled to track the target puncture path based on an event-triggered model prediction algorithm.

[0006] This invention presents an event-triggered trajectory control method for a parallel puncture robot. Based on event-triggered Model Predictive Control (MPC), optimal tracking control performance can be achieved while satisfying constraints. It is worth noting that although researchers have used MPC to address motion control issues in medical robots, these methods have not simultaneously considered the safety of keeping the puncture needle within a specific area, thus limiting the application of puncture robots in practical scenarios. Clearly, introducing RCM constraints into the MPC optimization problem can effectively improve robot safety. Event-triggered mechanisms allow for parameter adjustment based on the actual working scenario, reducing computational load and improving the environmental adaptability of the puncture robot.

[0007] This invention presents an event-triggered parallel puncture robot trajectory control method. First, a kinematic system model of the puncture robot is established. Event-triggered modeling is used for predictive control to ensure safety, reduce the number of optimization problems solved, and simultaneously improve the motion tracking control performance of the robot's puncture needle. Real-time state information of each joint and the motion state of the reference trajectory are acquired at the current moment, and a finite-time domain MPC constrained optimization problem is constructed. The constrained optimization problem is solved to obtain the predicted optimal control sequence at the current moment. According to the designed event-triggered mechanism, the first s optimal controls are applied to the system. The next trigger moment is updated; the system state is acquired at the new trigger moment, and the MPC constrained optimization problem is updated. This process iterates until the task ends. The above process models the puncture robot system, mainly based on the motion state of each joint to obtain the needle tip's motion trajectory. Then, the obtained model is used as the predictive model in the event-triggered MPC optimization problem. Based on the tracking control objective, an appropriate optimization objective function and system constraints are selected to construct the MPC constrained optimization problem, and an event-triggered mechanism is built. Solving the MPC optimization problem once applies multiple optimal control inputs to the puncture robot system.

[0008] The specific method is as follows:

[0009] First, the kinematic system of the puncture robot is modeled, mainly including the joints and the puncture needle model. Then, the obtained mathematical model is used as a prediction model for event-triggered MPC (Multi-Process Control) and appropriate optimization objective functions and system constraints are designed based on the control task and safety requirements. The system state at the current moment is sampled to construct a finite-time domain MPC constrained optimization problem; an event triggering mechanism is designed; the constrained optimization problem is solved to obtain the predicted optimal control sequence at the current moment, and the next triggering moment t is determined according to the event triggering mechanism. next The first s optimal controls are applied to the puncture robot system; at the new trigger moment, the new system state is obtained and the MPC constraint optimization problem is updated, and the process is repeated until the control task ends.

[0010] The advantages of this invention are:

[0011] 1. The parallel puncture robot trajectory control method of the present invention fully considers the robot's motion control objective in the designed objective function. By optimizing this objective, the optimal trajectory tracking performance can be achieved while satisfying motion control.

[0012] 2. In the parallel puncture robot trajectory control method of the present invention, the designed event-triggered MPC can reduce the number of times the optimization problem is solved by setting the event triggering mechanism, which greatly saves the amount of computation required to solve the optimization problem.

[0013] 3. In the parallel puncture robot trajectory control method of the present invention, the designed event-triggered MPC motion control uses the actual control input range and state range as hard constraints of the optimization problem, which is more in line with practical applications; in particular, the introduction of RCM constraints in the MPC optimization problem can ensure that the puncture needle completes the designated task under the premise of ensuring safety.

[0014] 4. In the parallel puncture robot trajectory control method of the present invention, the designed event-triggered MPC method can obtain the optimal control input with a shorter prediction time domain under the condition of satisfying the constraints, and reduce the number of times to solve the optimization problem, greatly saving the amount of computation, so that the proposed method can be applied in real time on the puncture robot. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the handheld puncture robot.

[0016] Figure 2 This is a schematic diagram showing the structural parameters of a handheld puncture robot.

[0017] Figure 3 This is a flowchart of the iterative learning and predictive control method for the handheld puncture robot of the present invention. Detailed Implementation

[0018] The present invention will now be described in further detail with reference to the accompanying drawings.

[0019] This invention relates to a trajectory control method for a parallel puncture robot based on event-triggered model predictive control (MTC). The method mainly consists of two parts: the first part involves modeling the puncture robot system, including its joints and puncture needle; the second part uses the mathematical model obtained in the first part as a predictive model for event-triggered MPC, and then selects a suitable objective function and system constraints to construct an MPC constrained optimization problem based on the control objective. Furthermore, an event-triggered mechanism is constructed, and solving the MPC optimization problem applies multiple optimal control inputs to the puncture robot system. The specific steps are as follows:

[0020] Step 1: Modeling the parallel puncture robot system and selecting corresponding parameters.

[0021] like Figure 1 As shown, in the structure of the puncture robot, A and B are the planes containing two branches. Each branch consists of an end link and a front link connected end-to-end, with the front end of the front link connected to the end of the puncture needle. The drive joint angles θ1 and θ2 of the two branches and the length l1 of the link A1B1 ensure that the remote center of motion constraint (RCM) is located at point O. A1 and B1 are the two endpoints of the end of the end branch connected to the drive joint in plane A, respectively. The robot's coordinate system reference coordinate system O-xyz is constructed with its origin coinciding with point O. The x-axis is aligned with the direction of the angle bisector of ∠A1OB1, the z-axis points upwards, and the y-axis is determined by the right-hand rule. e1, e2, and e3 represent the direction vectors of the x, y, and z axes, respectively. a1 and a2 represent the direction vectors of the lines connecting the ends of the two end branches to the front end of the puncture needle, respectively. b1 and b2 represent the direction vectors of the lines connecting the front ends of the two end branches to the end of the puncture needle, respectively. w1, w2, and w3 represent the direction vectors of the lines connecting the ends of the two end links to the front end, and the direction vector of the line connecting the front end of the puncture needle to the end, respectively. Since planes A and B coincide with the puncture needle axis, we can establish joint inputs θ1 and θ2 and output rotation angles ψ of the x and y axes in O-xyz. x ψ y The relationship between them is as follows:

[0022] Step 1.1: Planes A and B intersect at the z-axis, and the corresponding normal vectors n of the two planes are... A ,n B It can be represented as

[0023]

[0024] Among them, β=∠A1OB1; and it is a simplified calculation formula, in which: sθ1=sin(θ1), sθ2=sin(θ2), sβ=sin(β), cθ1=cos(θ1), cθ2=cos(θ2), cβ=cos(β).

[0025] Step 1.2: Determine the axial direction vector w3 of the puncture needle as follows:

[0026]

[0027] In the formula, cψ x =cos(ψ x ), cψ y =cos(ψ y );sψ x =sin(ψ) x ),sψ y =sin(ψ) y ).

[0028] Step 1.3: Determine n A n B The relationship between w3 and w3 is as follows:

[0029] n A ×n B =kw3,

[0030] Right now:

[0031]

[0032] Where, k = ||n A ×n B ||.

[0033] Step 1.4: Based on the above kinematic relationships, we can obtain ψ x =θ1, determine the analytical solution ψ of the forward kinematics of the puncture robot. y .

[0034]

[0035] In the formula, tψ y =tan(ψ) y ).

[0036] Step 1.5: Let θ1 = ψ x Determine θ2 of the inverse kinematics representation of the puncture robot.

[0037]

[0038]

[0039] Step 1.6: Determine the length l1 of link A1B1 and the length l2 of link A2B2 of the puncture robot. A2 and B2 are the two endpoints of the front branch of the branch connected to the drive joint in plane B, respectively. Based on the geometric relationship of the puncture robot structure, we can obtain:

[0040]

[0041] In the formula, l3, a, b are the connecting rods OC and OA, respectively. i and AB i The length of the needle is known, and point C is the junction between the tip of the needle and the two branches. The mathematical symbol T represents transpose.

[0042]

[0043]

[0044]

[0045] Step 1.7: If we choose the state of the puncture robot system at time t as x(t) and the joint angle as θ(t), then the puncture robot system model can be represented by a general nonlinear function as follows:

[0046] x(t)=f(θ(t))

[0047] in, and Let represent the n-dimensional and m-dimensional real number spaces, respectively.

[0048] Step 2: Motion control design based on event-triggered MPC.

[0049] Step 2.1: Set the reference trajectory and define the initial state x of the reference trajectory. r (0), using the puncture robot model established in step 1 to generate a reference trajectory, and setting the initial state of the actual puncture robot as x(0); setting the objective function of the MPC constraint optimization problem at the current time t as:

[0050]

[0051] Where T is the prediction time domain, s is the number of sequences input into the system; s∈[0,T], x(s|t) is the system's prediction of the error state at time t+s at time t, and θ(s|t) is the system's prediction of the control input at time t+s at time t. L(x(s|t),θ(s|t)) is the stage objective function:

[0052] L(x(s|t),θ(s|t))=α||x(s|t)-x r (t+s)|| 2

[0053] Where α = 10, is the weighting coefficient, and x r (t+s) represents the reference trajectory state at time t+s.

[0054] Step 2.2: Set constraints for the MPC optimization problem.

[0055] 1) Initial state x(0|t)=x(t)=[0.5 0.5 0.1] T These include the initial states of the piercing robot's end effector in the x, y, and z directions, respectively.

[0056] State position.

[0057] 2) The state constraint is x min ≤x≤x max ;where x max and x min These represent the robot's maximum and minimum state values, respectively.

[0058] 3) RCM constraint set Ω={x i |||x i -d r ||≤∈}, where ∈ represents the range of the RCM constraint region, d r It is the target location, x i This is a set of constraints, namely the kinematic boundary of the puncture robot.

[0059] 4) The robot joint angle constraint is θ max ≤θ(s|t)≤θ max , where θ max For the maximum joint angle, θ min This is the minimum joint angle.

[0060] Step 2.3: Set the prediction time domain of MPC to T=10, the sampling interval to δ=0.5s, and the weight parameter of the objective function to α=10.

[0061] Step 2.4: Based on the reference trajectory x set in Step 2.1 r (t), set the initial state of the robot as x(0).

[0062] Step 2.5: Based on the puncture robot model established in Step 1, predict the future dynamics of the system, solve the MPC-constrained optimization problem at time t, and obtain the predicted optimal control sequence θ at time t. * (s|t), whose corresponding optimal predicted state sequence is x. * (s|t).

[0063] The specific form of the MPC-constrained optimization problem at time t is:

[0064]

[0065] stx(0|t)=x(t)

[0066] x(s|t)=f(θ(s|t))

[0067] xi ∈Ω

[0068] x min ≤x≤x max

[0069] θ max ≤θ(s|t)≤θ max

[0070] Step 2.6: Construct an event triggering mechanism.

[0071]

[0072] Among them, t next Let x(t+s) be the next trigger time, and u be the value at which x(t+s) is the value at which * (s|t), s∈[t,t] next The actual state after the action is applied to the system, with the constant σ = 1 being the trigger threshold.

[0073] Step 2.7: Calculate the first s optimal control values ​​u * (s|t), s∈[t,t] next [This applies to the system until the new triggering time t] next When the time comes, the new sampled state is used as the initial state to construct the MPC constraint optimization problem at the new time moment, and the above steps 2.5, 2.6, and 2.7 are repeated in a rolling iteration until the control process ends.

Claims

1. A trajectory control method for a parallel puncture robot based on event-triggered model predictive control, characterized in that: First, a kinematic system model of the puncture robot is established, including models of each joint and the puncture needle. The obtained model is readily used as a predictive model for event-triggered MPC, and then the objective function and system constraints are designed and optimized based on the control task and security requirements. Further sample the system state at the current moment to construct a finite-time domain MPC-constrained optimization problem; Further design an event triggering mechanism; solve the constrained optimization problem to obtain the optimal control sequence predicted at the current time, and determine the next triggering time based on the event triggering mechanism. , will go An optimal control is applied to the puncture robot system; at each new trigger moment, a new system state is acquired and the MPC constraint optimization problem is updated, and the process is iterated until the control task ends. The above motion control design based on event-triggered MPC is as follows: Step A: Set the reference trajectory and define its initial state. A reference trajectory is generated using the established puncture robot model, and the initial state of the actual puncture robot is set as follows. Set the current time. The objective function of the MPC-constrained optimization problem is: ； in, To predict the time domain, , For the system in Always Prediction of time error state, For the system in Always Predicting and controlling input at all times; For the phased objective function: ； in, , where is the weighting coefficient. The reference trajectory state at time t+s; Step B: Set constraints for the MPC optimization problem; 1) Initial state Each includes the end of the puncture robot. Three directions The initial state position on; 2) State constraints are ;in, and These are the robot's maximum and minimum state values, respectively. 3) RCM constraint set ,in, Indicates the range of the RCM constraint region. It is the target location. This is a set of constraints, namely the kinematic boundary of the puncture robot; 4) Robot joint angle constraints are , in, For the maximum joint angle, This is the minimum joint angle; Step C: Set the prediction time domain for MPC Sampling interval The weight parameters of the objective function ; Step D: Based on the reference trajectory set in step 2.1 Set the robot's initial state as ; Step E: Based on the puncture robot model established in Step 1, predict the future dynamics of the system and solve... The time-bounded MPC-constrained optimization problem is obtained. Optimal control sequence predicted at time step The corresponding optimal predicted state sequence is The specific form of the time-bound MPC-constrained optimization problem is as follows: ； ； ； ； ； ； Step F: Construct an event triggering mechanism; ； in, For the next triggering time, In order to be in The actual state of the system after the action, constant This is the trigger threshold; Step G: Calculate the previous... Optimal control It acts on the system until a new triggering time. When the time comes, the new sampled state is used as the initial state to construct the MPC-constrained optimization problem for the new time step; Step H: Repeat steps E, F, and G above until the iterative predictive control task ends.

2. The parallel puncture robot trajectory control method based on event-triggered model predictive control as described in claim 1, characterized in that: The method for establishing the kinematic system model of the puncture robot is as follows: First, define the structure of the puncture robot, where A and B are the planes containing the two branches, and the driving joint angles of the two branches are... , and connecting rod length This allows the remote motion center constraint to be located at point O; These are the end points of the end branches connected to the drive joint in plane A; the robot coordinate system reference coordinate system O- The origin of the construction coincides with point O; since planes A and B coincide with the axis of the puncture needle, joint input is established. , and output middle Axis rotation angle , The relationship between them is as follows: Step 1: Planes A and B intersect at... Axis, the corresponding normal vectors of the two planes , It can be represented as , ； in, Furthermore, to simplify the formula, in the formula: = = , Step 2: Determine the direction vector of the puncture needle axis for: ； In the formula, = , ; Step 3: Confirm , and The relationship between them is: ； Right now: ； in, ; Step 4: Based on the above kinematic relationships, we can obtain... Determine the analytical solution for the forward kinematics of the puncture robot. ; ； In the formula, ; Step 5: Let Determine the inverse kinematic representation of the puncture robot ； Step 6: Determine the linkage of the puncture robot length ,link length , These are the two endpoints of the front branch that connects to the drive joint in plane B. Based on the geometric relationship of the puncture robot structure, we can obtain: ； In the formula These are connecting rods , and The length of is known, and point C is the junction of the tip of the puncture needle and the two branches; the mathematical symbol T represents transpose; ; , It is perpendicular to the normal vectors of planes A and B; ； ； ； Step 7: Select a puncture robot system in The state at time is The joint angle is The puncture robot system model can then be represented by a general nonlinear function as follows: ； in, , and They represent and The real space of dimension .

Images