Model prediction-based mirror-holding robot control method and device, and electronic equipment

Abstract

The application discloses a model prediction-based mirror-holding robot control method and device and electronic equipment, wherein the mirror-holding robot comprises a mechanical arm and an endoscope installed at a terminal joint of the mechanical arm; in each control period, the control method comprises the following steps: acquiring a spatial position of a target tracking point; determining an expected position of an optical center of the endoscope when the target tracking point is located at the center of an image of the endoscope; predicting a motion trajectory of the optical center of the endoscope at multiple future time points based on the expected position of the optical center of the endoscope by using a model prediction control algorithm; constructing a control condition and generating an optimal control sequence meeting the control condition; and controlling the motion of the mechanical arm according to one of the optimal control sequence, so that the target tracking point is kept at the center of the image. The application has the advantages of improved tracking response, constraint processing capability, robustness and control flexibility.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically to a model-predictive control method, device, and electronic device for a mirror-holding robot. Background Technology

[0002] In traditional endoscopic surgery, surgeons need to manually control the position of the endoscope to obtain a clear surgical field. Maintaining this position for extended periods can easily lead to surgeon fatigue, especially in the confined space of an operating table where it's difficult to find a comfortable posture. Hand tremors caused by fatigue can affect image quality and surgical efficiency. Ordinary endoscope supports cannot accommodate the flexible movement of the lens during surgery. Using a robotic arm to replace medical staff in supporting the endoscope can effectively improve these problems.

[0003] In related technologies, the endoscopic robotic arm adopts a control scheme based on image-based visual servoing (IBVS). Its shortcomings are: lack of prediction mechanism, the control system can only react to the current position of the instrument, and the phenomenon of tracking delay will occur during the rapid movement of the instrument, which leads to problems such as tracking response lag. Summary of the Invention

[0004] This invention aims to address one of the technical problems in related technologies to a certain extent. To this end, this invention provides a model-predictive-based control method, device, and electronic device for a mirror-holding robot, which can improve tracking response and control flexibility.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: A model-predictive control method for an endoscope-holding robot, the robot comprising a robotic arm and an endoscope mounted on the end joint of the robotic arm, wherein in each control cycle, the control method comprises the following steps: Obtain the spatial position of the target tracking point and determine the desired position of the optical center of the endoscope when the target tracking point is located at the center of the endoscope image; Based on the desired position of the endoscope optical center, the motion trajectory of the endoscope optical center at multiple future moments is predicted using a model predictive control algorithm. Construct control conditions and generate an optimal control sequence that satisfies the control conditions; The robotic arm is controlled to move according to one of the optimal control sequences, thereby keeping the target tracked at the center of the image.

[0006] Optionally, the step of predicting the motion trajectory of the endoscope optical center at multiple future moments using a model predictive control algorithm based on the desired position of the endoscope optical center includes: Based on the spatial location of the target tracking point, the motion trajectory of the target tracking point at multiple future moments is predicted using a model predictive control algorithm. Based on the motion trajectory of the target tracking point, the motion trajectory of the endoscope optical center at multiple future moments is predicted using a model predictive control algorithm.

[0007] Optionally, the model predictive control algorithm includes establishing discrete state-space equations describing the joint motion of the robotic arm, including: The state vector is determined based on the joint angles and angular velocities of the robotic arm, and the control input is determined based on the joint angular acceleration of the robotic arm. Based on the state vector and the control input, a continuous state-space equation is constructed; Based on the sampling time, the continuous state-space equation is discretized using the Euler method to obtain the discrete state-space equation.

[0008] Optionally, predicting the motion trajectory of the target at multiple future moments includes: Obtain the historical spatial location data of the target tracking point; Based on the historical spatial location data of the target tracking, the motion velocity of the target is estimated using the Kalman filter method; Based on the spatial location and speed of the target tracking point at the current moment, predict the trajectory of the target tracking point at multiple future moments.

[0009] Optionally, the control conditions include a telecentric constraint to ensure that the endoscope optical center is located on a sphere with the constraint position as the center and the distance between the constraint position and the endoscope optical center as the radius.

[0010] Optionally, the telecentric constraint is constructed as a linear equality constraint with respect to joint angular velocity, the linear equality constraint including: Based on the constraint of the endoscope's motion around the constraint position, the constant distance relationship between the optical center of the endoscope and the constraint position is differentiated to obtain the orthogonal relationship between the velocity of the optical center of the endoscope and the constraint position. The optical center velocity of the endoscope is expressed as the product of the position Jacobian matrix of the robotic arm and the joint angular velocity. Substituting this into the orthogonality relation, a linear equation constraint regarding the joint angular velocity is obtained.

[0011] Optionally, the model predictive control algorithm includes establishing discrete state-space equations describing the joint motion of the robotic arm, including: determining a state vector based on the joint angles and angular velocities of the robotic arm, and determining the control input based on the joint angular accelerations of the robotic arm; the control conditions further include: State constraints include limits on the joint angles and angular velocities of the robotic arm; and control constraints include limits on the angular acceleration of the robotic arm joints.

[0012] Optionally, the step of constructing control conditions and generating an optimal control sequence that satisfies the control conditions further includes: Establish control condition optimization, which includes minimizing the deviation between the actual spatial position of the endoscope optical center and the motion trajectory; Adjust the weight parameters of state constraints and control constraints; In order to obtain the optimal control sequence for multiple future time points.

[0013] Optionally, the objective function of the optimal control sequence under the control conditions includes: ; in, This represents solving for the optimal control sequence, where the decision variables are... The goal is to minimize the value of the objective function. Indicates a control sequence; Indicates the prediction step size; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The actual location; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The desired location; This represents the semi-positive definite state weight matrix. This represents the positive semidefinite input weight matrix; Indicates in Predicting the future at any moment -1 control input at time point.

[0014] Optionally, controlling the movement of the robotic arm according to one of the optimal control sequences includes controlling the movement of the robotic arm according to the first control quantity in the optimal control sequence, so that the robotic arm drives the endoscope to rotate in the constrained position along two axes perpendicular to the axis of the endoscope rod, thereby keeping the endoscope at the center of the endoscope image.

[0015] Optionally, obtaining the spatial location of the target tracking point and determining the desired location of the endoscope optical center when the target tracking point is located at the center of the image includes: The current position of the target tracking point in the endoscope image is obtained. Based on the current position of the target tracking point in the endoscope image and the positional relationship between the robotic arm end joint and the endoscope, the spatial position of the target tracking point and the spatial position of the optical center of the endoscope are determined. When the target tracking point is located at the center of the image, the desired position of the endoscope optical center is determined based on the spatial position of the target tracking point and the spatial position of the endoscope optical center.

[0016] The model prediction-based control method for a mirror-holding robot provided in this invention transforms the mirror-holding robot control problem into a constrained predictive control problem. Compared to traditional image-based visual servoing tracking methods, it introduces a model predictive control framework, achieving forward-looking control by predicting the motion trend of the target tracking point, significantly reducing tracking lag; under optimized control conditions, it generates an optimal control sequence, achieving faster tracking response speed to the target tracking point and optimizing control flexibility.

[0017] Furthermore, the present invention also provides a control device for a mirror-holding robot, the control device comprising: The acquisition module acquires the spatial position of the target tracking point and determines the desired position of the optical center of the endoscope when the target tracking point is located at the center of the endoscope image. The first determining module, based on the desired position of the endoscope optical center, uses a model predictive control algorithm to predict the motion trajectory of the endoscope optical center at multiple future moments; The second determining module constructs control conditions and generates an optimal control sequence that satisfies the control conditions; The control module controls the movement of the robotic arm according to one of the optimal control sequences, thereby keeping the target tracking point located at the center of the image. The beneficial effect reasoning process of the mirror-holding robot control device provided by this invention is similar to that of the aforementioned model-prediction-based mirror-holding robot control method, and will not be repeated here.

[0018] Furthermore, the present invention also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the model prediction-based control method for a mirror-holding robot described above.

[0019] These features and advantages of the present invention will be disclosed in detail in the following specific embodiments and accompanying drawings. The preferred embodiments or means of the present invention will be shown in detail in conjunction with the accompanying drawings, but are not intended to limit the technical solutions of the present invention. In addition, each of these features, elements and components appearing in the following text and drawings is a plurality of, and different symbols or numbers are used for convenience of representation, but all represent parts with the same or similar construction or function. Attached Figure Description

[0020] The present invention will be further described below with reference to the accompanying drawings: Figure 1 A flowchart illustrating one embodiment of the mirror-holding robot control method provided in this invention; Figure 2 This is a schematic diagram showing the relative positions between the robotic arm and the endoscope provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the coordinate system positional relationship according to an embodiment of the present invention; Figure 4 A flowchart illustrating one implementation method for solving the optimal control sequence provided in this invention; Figure 5 A flowchart illustrating one implementation method for establishing discrete state-space equations provided in this embodiment of the invention; Figure 6 A flowchart illustrating an implementation method for predicting the position of a target tracking point at multiple future moments, as provided in an embodiment of the present invention; Figure 7 A flowchart illustrating one embodiment of constructing linear equality constraints provided in this invention; Figure 8 A flowchart illustrating one embodiment of the present invention for determining the desired position coordinates of the optical center of an endoscope; Figure 9 A block diagram of one embodiment of the visual tracking device for an endoscopic robotic arm provided in this invention; Figure 10 A block diagram illustrating one embodiment of the electronic device provided in this invention; Figure 11 A schematic diagram of a computer-readable medium provided in an embodiment of the present invention; Explanation of reference numerals in the attached figures 101: Processor, 102: Memory, 103: I / O interface, 104: Bus, 201: Acquisition module, 202: First determination module, 203: Second determination module, 204: Control module. Detailed Implementation

[0021] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described are intended to explain the present invention and should not be construed as limiting the invention.

[0022] The terms "an embodiment," "example," or "trademark" used in this specification refer to a particular feature, structure, or characteristic described in connection with the embodiment itself that may be included in at least one embodiment disclosed in this invention. The phrase "in an embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment.

[0023] Currently, the commonly used robotic arm control scheme based on image vision servoing is used to achieve instrument tracking; however, this scheme still has many shortcomings: Lack of foresight leads to delayed tracking response: Control based solely on the current error is reactive control, which cannot predict the movement trend of the device. This results in significant tracking delays when the device moves rapidly, potentially leading to a "always chasing" lag tracking. It has weak constraint handling capabilities and struggles to coordinate multiple constraints in a unified, rigorous, and systematic manner. Insufficient robustness: This approach is not sensitive to interferences such as tissue deformation, respiratory movements, and visual obstruction during surgery, and is prone to oscillations or unstable movements, which reduces surgical safety and visual comfort. To address the problems of tracking lag, difficulty in coordinating multiple constraints, and insufficient robustness in existing endoscopic robotic arms, this invention proposes a model predictive control (MPC) method that integrates model predictive control (MPC) and multi-coordinate system visual perception. The visual tracking problem of the endoscopic robotic arm is constructed as a predictive control problem with multiple constraints. First, by acquiring the spatial position of the target tracking point, the expected position of the endoscope's optical center when the target tracking point is located at the center of the image is determined. Then, using the MPC algorithm, the trajectory of the endoscope's optical center is proactively predicted at multiple future moments in the rolling time domain optimization. Simultaneously, control conditions are constructed, such as incorporating telecentric constraints, state constraints, and control constraints. By solving the objective function of the optimal control sequence with control conditions, the globally optimal control sequence is obtained. Finally, only one control variable in the sequence is executed to drive the robotic arm, thereby achieving predictive tracking and anti-interference control of surgical instrument movement while strictly ensuring the safety of telecentric motion.

[0024] As a first aspect of this embodiment, a model-predictive-based control method for a mirror-holding robot is provided, such as... Figure 1 , Figure 2 As shown, the endoscope-holding robot includes a robotic arm and an endoscope mounted on the end joint of the robotic arm. In each control cycle, the control method includes the following steps: In step S110, the spatial position of the target tracking point is obtained, and the desired position of the optical center of the endoscope is determined when the target tracking point is located at the center of the endoscope image. In step S120, based on the desired position of the endoscope optical center, a model predictive control algorithm is used to predict the motion trajectory of the endoscope optical center at multiple future moments. In step S130, control conditions are constructed, and an optimal control sequence that satisfies the control conditions is generated; In step S140, the movement of the robotic arm is controlled according to one of the optimal control sequences, thereby keeping the target tracking point located at the center of the image.

[0025] like Figure 2 , Figure 3 As shown, the world coordinate system is typically provided by the robotic arm manufacturer or is user-defined. For example, the origin of the world coordinate system is located at the origin of the robotic arm, the Z-axis is perpendicular to the ground and pointing upwards, and the X and Y axes are located on a horizontal plane. The robotic arm's end joints grip the endoscope rod, and the coordinates of the geometric intersection point Ct in the robotic arm's end-effector coordinate system (Xt, Yt, Zt) are (0, 0, ...). ),in This is the perpendicular distance from the end joint of the robotic arm to the axis of the endoscope shaft (i.e., the distance shown in the figure). Telecentric point. That is, the surgical incision point and the optical center of the endoscope. The target tracking point is located at the end of the mirror rod on the side furthest from the geometric intersection Ct. These are the target points that the endoscope lens needs to track, such as surgical instruments, instrument tips, anatomical structures / tissues, artificial markers, etc. The distance from the distal joint of the robotic arm to the optical center of the endoscope is... The camera coordinate system (Xc, Yc, Zc) is based on the optical center of the endoscope lens. The origin of the coordinate system is the Z-axis, which points towards the object being photographed. The X-axis and Y-axis are parallel to the X-axis and Y-axis of the image coordinate system, respectively. The image coordinate system has its origin at the center of the image plane captured by the endoscope lens. Cx , Cy The X and Y axes are respectively related to the pixel coordinate system. u shaft and v The axes are parallel, and the units are physical lengths. The pixel coordinate system, on the other hand, has its origin at the top-left corner of the image plane captured by the endoscope lens, and is measured in pixels.

[0026] In endoscopic surgery, the endoscope performs a telecentric motion around the surgical incision point. This motion can be decomposed into four constrained degrees of freedom: rotation about the endoscope shaft axis, movement along the endoscope shaft axis past the telecentric point, and rotation along two axes perpendicular to the endoscope shaft axis. The visual effects caused by these four degrees of freedom are as follows: rotation about the endoscope shaft axis causes the image to rotate; movement along the endoscope shaft axis past the telecentric point causes the image to zoom in and out; and rotation along the two axes perpendicular to the endoscope shaft axis causes the image to translate vertically or horizontally.

[0027] In actual surgical scenarios, the viewing angle is basically fixed, that is, the viewing angle is determined according to the actual anatomical relationship. Changing the viewing angle will cause discomfort to the surgeon. Therefore, except for tasks such as extensive exploration of the abdominal cavity that are not suitable for robotic arm operation, the degree of freedom of "rotation around the axis of the endoscope rod" is dispensable in the endoscopic surgery process. This embodiment of the invention does not control or limit this.

[0028] Zooming in and out of the image to adjust the field of vision is a common operation during surgery. However, it is difficult to capture the doctor's intention to zoom in or out of the image. Using a robotic arm for control can easily lead to misoperation, increasing the doctor's visual fatigue and reducing surgical efficiency. Therefore, this embodiment of the invention does not control this degree of freedom, but instead gives control to the doctor through external input (e.g., foot pedal) commands.

[0029] Because surgical instruments are inherently dangerous, they generally cannot leave the surgical field of view during surgery. Furthermore, the surgeon's attention is focused on the surgical area directly where the instruments are positioned. Therefore, determining the endoscopic view's movement intention based on the instrument's location is reliable. In this embodiment of the invention, through steps S110-S140, the degrees of freedom for the endoscope to rotate along two axes perpendicular to the endoscope shaft in the constrained position are determined, enabling vertical or horizontal translation of the view. In one implementation, the target tracking point is first acquired. The pixel coordinates (u, v) are converted into target tracking points. Coordinates in the world coordinate system And determine the optical center of the endoscope. Coordinates in the world coordinate system This means obtaining the spatial location of the target tracking point, and also obtaining the telecentric point. Coordinates in the world coordinate system That is, the constraint bit. Then based on... , , and the distal point of the endoscope Motion constraints determine the target tracking point When located at the center of the image, the optical center of the endoscope Desired position coordinates =( , Then based on =( , The MPC algorithm was used to predict the optical center of the endoscope at multiple future moments. movement trajectory (k+i|k), where k represents the discrete-time index (time number), and the optimal control sequence satisfying the preset control conditions is solved. One control variable from the optimal control sequence is applied to the robotic arm to control its movement, causing the endoscope to rotate along two axes perpendicular to the endoscope shaft axis, thereby keeping the target tracking point at the center of the image.

[0030] Specifically, regarding how to obtain the centroid... Coordinates in the world coordinate system There are already feasible implementations in this field, and the embodiments of the present invention do not specifically limit them.

[0031] The model predictive control method for an endoscopic robotic arm provided in this invention transforms the visual tracking problem of the endoscopic robotic arm into a predictive control problem subject to multiple constraints. Compared with traditional image-based visual servoing tracking methods: It introduces a model predictive control framework, achieving forward-looking control by predicting the motion trend of the target tracking point, significantly reducing tracking lag; it explicitly incorporates joint angle / velocity / acceleration limits, telecentric motion constraints, etc., into the optimization problem, achieving unified and coordinated processing of multiple constraints and ensuring the robotic arm operates within safe boundaries; it employs a rolling time-domain optimization mechanism, resolving the optimization problem in each control cycle based on the actual state at the current moment as the initial state, and executing only the first control variable in the optimal control sequence, achieving feedback correction at each step. When model errors or external disturbances occur, this mechanism can adjust subsequent control commands in a timely manner, effectively suppressing the impact of disturbances on tracking accuracy and enhancing the robustness of the system; it clarifies the control allocation between the robotic arm responsible for translational degrees of freedom and the doctor responsible for scaling degrees of freedom, improving the safety and flexibility of human-machine collaboration. Therefore, this invention achieves high-precision and robust prediction and tracking of target tracking points while ensuring the safety of telecentric motion.

[0032] In some embodiments, such as Figure 4 As shown, based on the desired position of the endoscope optical center, a model predictive control algorithm is used to predict the motion trajectory of the endoscope optical center at multiple future moments, including: In step S210, based on the spatial location of the target tracking point, a model predictive control algorithm is used to predict the motion trajectory of the target tracking point at multiple future moments; In step S220, based on the motion trajectory of the target tracking point, the motion trajectory of the endoscope optical center at multiple future moments is predicted using a model prediction control algorithm.

[0033] This embodiment uses a model predictive control algorithm to predict the motion trajectory of the target tracking point at multiple future time points. Using the motion trajectory of the endoscope's optical center at these future time points as the tracking target and the current actual state as the initial state, it constructs and solves an optimization problem containing preset control conditions. This achieves predictive control and unified handling of multiple constraints for the robotic arm's motion. Compared to traditional methods, this mechanism enables the robotic arm to proactively respond to the motion trend of the target tracking point and generate an optimal control sequence while meeting preset control conditions, thereby improving the real-time performance, safety, and motion stability of the tracking.

[0034] In some embodiments, such as Figure 5 As shown, establishing the discrete state-space equations describing the joint motion of the robotic arm includes: In step S310, the state vector is determined based on the joint angle and joint angular velocity of the robotic arm, and the control input is determined based on the joint angular acceleration of the robotic arm. In step S320, a continuous state-space equation is constructed based on the state vector and the control input; In step S330, based on the sampling time, the continuous state-space equation is discretized using the Euler method to obtain the discrete state-space equation.

[0035] Taking a 7-DOF robotic arm as an example, let the joint angles of the robotic arm be denoted as . The joint angular velocity is Define the state vector as: X= ,in, Represents the joint angle vector It belongs to the 7-dimensional real space. Represents joint angular velocity It belongs to the 7-dimensional real number space.

[0036] Use the joint angular acceleration of the robotic arm as the control input: ,in, Represents joint angular acceleration It belongs to the 7-dimensional real number space.

[0037] Constructing continuous state-space equations: = ; in, Indicates continuous time. Represents the state vector The derivative with respect to time, i.e., the rate of change of the state, , , This indicates the number of joints in a robotic arm (e.g., a 7-DOF robotic arm). (7) The state matrix represents the joint angular acceleration vector of the robotic arm. In express That is, the rate of change of the joint angle is equal to the joint angular velocity, input matrix In express That is, the rate of change of joint angular velocity is equal to the control input.

[0038] Based on sampling time By discretizing the continuous state-space equations using the Euler method, we obtain the discrete state-space equations: = = ; Where k represents the discrete-time index (time sequence number). B and B represent the state matrix and input matrix, respectively. express 3D identity matrix This indicates the number of joints in a robotic arm (e.g., a 7-DOF robotic arm). (7).

[0039] This embodiment defines joint angles and angular velocities as state vectors, and joint angular acceleration as control inputs. A continuous state-space equation is constructed and discretized using the Euler method, resulting in a linear discrete state-space model for model predictive control. This model accurately describes the kinematic characteristics of the robotic arm within the sampling period, providing a concise and precise mathematical foundation for subsequent trajectory prediction and optimization.

[0040] In some embodiments, such as Figure 6 As shown, the prediction of the target's trajectory at multiple future moments includes: In step S410, the historical location data of the target tracking point is obtained; In step S420, the target tracking speed is estimated using the Kalman filter method based on the historical spatial location data of the target tracking. In step S430, based on the spatial position and speed of the target tracking point at the current moment, the trajectory of the target tracking point at multiple future moments is predicted.

[0041] Obtain target tracking points Historical location data is used to obtain the estimated velocity of the target tracking point through Kalman filtering. This allows for the prediction of target tracking points at multiple future moments. Location: (k+i|k)= k represents the discrete-time index (time sequence number). Indicates the sampling time.

[0042] This embodiment estimates the velocity of the target tracking point using Kalman filtering and predicts its position at multiple future moments based on its current position, enabling the model predictive control framework to anticipate the target tracking point's movement trend. Compared to reactive methods that control solely based on current errors, this predictive mechanism effectively reduces tracking delays caused by control lags, improving the real-time performance of tracking.

[0043] In some embodiments, such as Figure 7 As shown, the telecentric constraint is constructed as a linear equality constraint with respect to joint angular velocity, the linear equality constraint including: In step S510, based on the constraint of the endoscope moving around the constraint position, the constant distance relationship between the optical center of the endoscope and the constraint position is differentiated to obtain the orthogonal relationship between the velocity of the optical center of the endoscope and the constraint position. In step S520, the optical center velocity of the endoscope is expressed as the product of the position Jacobian matrix of the robotic arm and the joint angular velocity. Substituting this into the orthogonality relation, a linear equation constraint regarding the joint angular velocity is obtained.

[0044] Due to telecentric constraints, the optical center of the endoscope is controlled during the robotic arm's motion. Always at the centroid For the center of the ball, On a sphere with radius [radius], the optical center of the endoscope can be constructed. With the centroid The distance between them is constant: However, this equation is nonlinear and needs to be linearized to simplify the solution. Taking the derivative of both sides of the equation with respect to time, we obtain the orthogonality relationship: 2 .

[0045] The optical center of the endoscope Velocity is represented by the Jacobian matrix of the robotic arm's position. With joint angular velocity The product of: Substituting the above orthogonality relation, we obtain the information regarding the joint angular velocity. Continuous form of centroidal constraints: Discretizing the continuous form of the centroid constraint and applying it to each prediction step of MPC yields a linear equality constraint with respect to the joint angular velocity: .

[0046] This embodiment transforms the nonlinear constraint that the endoscope needs to move around the telecentric point into a linear equation constraint about the joint angular velocity, and embeds it into each prediction step of the model predictive control. This ensures that the optical center of the endoscope always moves on a sphere with the telecentric point as the center during the optimization process, ensuring that the endoscope moves around the fixed telecentric point throughout the entire process, thus avoiding surgical risks that may be caused by deviation from the telecentric point.

[0047] In some embodiments, the control conditions include a telecentric constraint, which is used to ensure that the endoscope optical center is located on a sphere with the constraint position as the center and the distance between the constraint position and the endoscope optical center as the radius. In this embodiment, the constraint position is the telecentric point, and the telecentric constraint is constructed as a linear equality constraint with respect to the joint angular velocity. The linear equality constraint is determined based on the endoscope optical center point, the telecentric point, and the position Jacobian matrix of the robotic arm.

[0048] In some embodiments, the control conditions further include: State constraints include limits on the joint angles and angular velocities of the robotic arm; and control constraints include limits on the angular acceleration of the robotic arm joints.

[0049] Specifically, the state vector is: X= ,in, Represents the joint angle vector It belongs to the 7-dimensional real space. Represents joint angular velocity It belongs to 7-dimensional real space. State constraints are constructed, including the range of joint angles and joint angular velocities of the robotic arm: , .

[0050] The control input is: ,in, Represents joint angular acceleration It belongs to 7-dimensional real space. Construct control constraints, limiting the joint angular acceleration range of the robotic arm: .

[0051] Establish a transformation matrix T_0_c = T_0_n * T_n_c from the world coordinate system to the camera coordinate system, where T_0_n represents the transformation matrix from the world coordinate system to the robot arm's end effector coordinate system, which is related to the joint angles. The matrix; T_n_c represents the transformation matrix from the robot arm's end-effector coordinate system to the camera coordinate system, which is determined by the robot arm's fixed position. Endoscope optical center point. These are the elements in the first three rows of the fourth column of T_0_c, which are visible. It's about joint angles. The function, denoted as . It is the joint angle To improve solution efficiency, the nonlinear function mapped to the robotic arm's end effector needs to be linearized for each step k. The corresponding joint state is denoted as At time k, Perform a first-order Taylor expansion: The derivative term, namely the Jacobian matrix of the robotic arm's position, is a 3x7 matrix, denoted as . ): .

[0052] therefore, and The linearized relationship between them can be written as: , Therefore, each MPC prediction step can be obtained. Linearized endoscope optical center coordinates: 。

[0053] use Linear equality constraints can be constructed: , This constraint is the centroid constraint applied in each prediction step of MPC. and All can be predicted Calculations show that The relationship with q can be calculated by establishing a transformation matrix from the world coordinate system to the camera coordinate system using the forward kinematics of the robotic arm.

[0054] In some embodiments, the objective function of the constraint-adjusted optimal control sequence includes: (one); In formula (1), This represents solving for the optimal control sequence, where the decision variables are... The goal is to minimize the value of the objective function. Indicates a control sequence; Indicates the prediction step size; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The actual location; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The desired location; This represents the semi-positive definite state weight matrix. This represents the positive semidefinite input weight matrix; Indicates in Predicting the future at any moment -1 control input at time point.

[0055] In some embodiments, controlling the movement of the robotic arm according to one of the optimal control sequences includes controlling the movement of the robotic arm according to the first control quantity of the optimal control sequence, causing the robotic arm to rotate the endoscope in a constrained position along two axes perpendicular to the endoscope shaft axis, thereby maintaining the endoscope at the center of the endoscopic image. The optimal control sequence that satisfies preset control conditions is as follows: , Take the first control quantity The force is applied to the robotic arm, controlling its movement and causing the endoscope to rotate in the constrained position along two axes perpendicular to the endoscope shaft axis, thereby keeping the target tracking point at the center of the image.

[0056] In some embodiments, such as Figure 8 As shown, the step of acquiring the spatial position of the target tracking point and determining the desired position of the endoscope optical center when the target tracking point is located at the center of the image includes: In step S610, the current position of the target tracking point in the endoscope image is obtained. Based on the current position of the target tracking point in the endoscope image and the positional relationship between the robotic arm end joint and the endoscope, the spatial position of the target tracking point and the spatial position of the optical center of the endoscope are determined. In step S620, when the target tracking point is located at the center of the image, the desired position of the endoscope optical center is determined based on the acquired spatial position of the target tracking point and the spatial position of the endoscope optical center.

[0057] Specifically, the centroid The coordinates in the world coordinate system are , light center point Coordinates in the world coordinate system During the movement of the robotic arm, the optical center point to the centroid The distance is constant: = .

[0058] From the centroid Pointing to target tracking point Direction vector .

[0059] Calculate the direction vector Length of the module : .

[0060] Target tracking point When located at the center of the image, the target tracking point Located on the axis of the endoscope rod, the optical center point Centcentric point Target tracking point The three points are collinear, and based on spatial geometric relationships, we can obtain: = .

[0061] Expanding the above equation, we get: = .

[0062] This embodiment, based on the constraint that the endoscope needs to move around the telecentric point, utilizes the fixed distance relationship between the optical center and the telecentric point, and the collinear geometric relationship among the target tracking point, the telecentric point, and the endoscope's optical center, to directly and analytically solve for the expected position coordinates of the endoscope's optical center when the target tracking point is located at the center of the image. This method does not rely on iterative optimization; it is entirely based on the spatial geometric relationship and known structural parameters at the current moment for direct calculation. The results are deterministic and computationally efficient, providing an accurate expected reference trajectory for subsequent model predictive control.

[0063] In some embodiments, obtaining the spatial location of the target tracking point includes: Obtain the pixel coordinates of the target tracking point in the endoscopic image; Convert the pixel coordinates to coordinates in the image coordinate system; Convert the coordinates in the image coordinate system to coordinates in the camera coordinate system; Establish the transformation matrix from the world coordinate system to the camera coordinate system; Based on the transformation matrix from the world coordinate system to the camera coordinate system, the pixel coordinates are converted into the coordinates of the target tracking point in the world coordinate system.

[0064] Track the target point Transform the pixel coordinates (u, v) to the image coordinate system: = ; in, This represents the physical size of a pixel along the X-axis of the image plane, where This represents the physical size of a pixel along the Y-axis of the image plane. and The coordinates of the origin of the image coordinate system in the pixel coordinate system.

[0065] Based on the stereo similarity between the image coordinate system and the camera coordinate system, we can conclude that: = .in, This represents depth information, and f represents focal length. This represents the X-axis coordinate of the camera coordinate system. This represents the X-axis coordinate of the image coordinate system. Since the depth information of the target tracking point in the 3D endoscope is z = (f*b) / d (where f represents the focal length, b represents the baseline distance, and d represents the parallax), we can obtain Xc = *b / d. Similarly, we can obtain Yc= *b / d. Finally, the target tracking point can be... The coordinates in the camera coordinate system are represented as follows: = .

[0066] Establish a transformation matrix T_0_c = T_0_n * T_n_c from the world coordinate system to the camera coordinate system, where T_0_n represents the transformation matrix from the world coordinate system to the robot arm end effector coordinate system, and T_n_c represents the transformation matrix from the robot arm end effector coordinate system to the camera coordinate system.

[0067] Combining the transformation matrix T_0_c from the world coordinate system to the camera coordinate system, and the target tracking point coordinates in the camera coordinate system The target tracking point can be obtained. Coordinates in the world coordinate system: .

[0068] Furthermore, the optical center of the endoscope can be determined based on the transformation matrix T_0_c from the world coordinate system to the camera coordinate system. Coordinates in the world coordinate system =(T_0_c(1,4),T_0_c(2,4),T_0_c(3,4)), where T_0_c(i,j) represents the element in the i-th row and j-th column of matrix T_0_c.

[0069] This embodiment uses a multi-coordinate system layer-by-layer transformation to analytically convert the pixel coordinates of the target tracking point into three-dimensional coordinates in the world coordinate system. This method does not rely on iterative optimization and is entirely based on analytical calculations using camera parameters, mechanical structure constants, and stereo vision principles. The results are highly deterministic and can provide accurate world coordinates for the spatial position of the target tracking point, laying the foundation for subsequent solutions for telecentric constraints and geometric intersections.

[0070] In some embodiments, establishing the transformation matrix from the world coordinate system to the camera coordinate system includes: Using the forward kinematics of the robotic arm, obtain the transformation matrix from the world coordinate system to the coordinate system of the robotic arm's end effector; Based on the distance from the end joint of the robotic arm to the endoscope rod and the distance from the end joint of the robotic arm to the optical center end face of the endoscope, determine the transformation matrix from the end-arm coordinate system to the camera coordinate system; Based on the transformation matrix from the world coordinate system to the robot arm end effector coordinate system and the transformation matrix from the robot arm end effector coordinate system to the camera coordinate system, determine the transformation matrix from the world coordinate system to the camera coordinate system.

[0071] Based on the forward kinematics of the robotic arm, the transformation matrix of a single joint is: ; in, Indicates the first There are m joints; the transformation matrix from the m-th joint to the n-th joint is: = ; n is the number of joints in the robotic arm, and the transformation matrix T_0_n from the world coordinate system to the end effector coordinate system of the robotic arm can then be obtained.

[0072] The transformation matrix from the robotic arm's end-effector coordinate system to the camera coordinate system is determined by the camera's mechanical mounting method. Since the two are fixedly connected and the endoscope length is also a fixed value, the transformation matrix from the robotic arm's end-effector coordinate system to the camera coordinate system is a constant matrix. For example, based on... Figure 2 Given the example camera coordinate system, we can obtain: = ; in, This indicates the distance from the end joint of the robotic arm to the optical center of the endoscope. This indicates the distance from the end joint of the robotic arm to the endoscope rod.

[0073] By combining the transformation matrix T_0_n from the world coordinate system to the robot arm end effector coordinate system and the transformation matrix T_n_c from the robot arm end effector coordinate system to the camera coordinate system, we can establish the transformation matrix T_0_c = T_0_n * T_n_c from the world coordinate system to the camera coordinate system.

[0074] This embodiment constructs a transformation matrix from the world coordinate system to the robot arm's end-effector coordinate system using the robot arm's forward kinematics. Then, based on the fixed installation geometry between the robot arm's end-effector and the endoscope, a constant transformation matrix is ​​determined from the robot arm's end-effector coordinate system to the camera coordinate system. Finally, multiplying the two matrices yields the complete transformation matrix from the world coordinate system to the camera coordinate system. This method fully utilizes the robot arm's structural parameters and known joint angles for analytical calculation, without relying on external calibration or iterative optimization. The established transformation matrix is ​​deterministic and real-time, providing a unified coordinate transformation benchmark for subsequent conversion of pixel coordinates to world coordinates.

[0075] As a second aspect of the invention, a visual tracking device for an endoscopic robotic arm is provided, such as... Figure 9 As shown, the visual tracking device includes: The acquisition module 201 acquires the spatial position of the target tracking point and determines the desired position of the optical center of the endoscope when the target tracking point is located at the center of the endoscope image. The first determining module 202, based on the expected position of the endoscope optical center, uses a model predictive control algorithm to predict the motion trajectory of the endoscope optical center at multiple future moments. The second determining module 203 constructs control conditions and generates an optimal control sequence that satisfies the control conditions; The control module 204 controls the movement of the robotic arm according to one of the optimal control sequences, thereby keeping the target tracking point located at the center of the image.

[0076] The visual tracking principle of the endoscopic robotic arm provided in this embodiment has been explained in detail above and will not be repeated here.

[0077] As a third aspect of this application, an electronic device is provided, such as Figure 10 As shown, the electronic device includes: One or more processors 101; The memory 102 stores one or more computer programs, which, when executed by the one or more processors 101, cause the one or more processors 101 to implement the model predictive control-based control method for a mirror-holding robot provided in the first aspect of this application.

[0078] The electronic device may also include one or more I / O interfaces 103 connected between the processor 101 and the memory 102, configured to enable information interaction between the processor 101 and the memory 102.

[0079] The processor 101 is a device with data processing capabilities, including but not limited to a central processing unit (CPU); the memory 102 is a device with data storage capabilities, including but not limited to random access memory (RAM, more specifically SDRAM, DDR, etc.), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), and flash memory (FLASH); the I / O interface (read-write interface) is connected between the processor and the memory, enabling information exchange between the processor and the memory, including but not limited to a data bus (Bus).

[0080] In some embodiments, the processor 101, memory 102, and I / O interface 103 are interconnected via bus 104, and thus connected to other components of the computing device.

[0081] In addition, such as Figure 11 As shown, the present invention also provides a computer-readable medium having a computer program stored thereon, which, when executed by a processor, implements the model predictive control method for an endoscopic robotic arm provided in the first aspect of the present invention.

[0082] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. Accordingly, the computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can implement the methods of any of the above embodiments. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0083] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Those skilled in the art should understand that the present invention includes, but is not limited to, the contents described in the accompanying drawings and the specific embodiments above. Any modifications that do not depart from the functional and structural principles of the present invention will be included within the scope of the claims.

Claims

1. A model-predictive control method for a scope-holding robot, the scope-holding robot comprising a robotic arm and an endoscope mounted on the end joint of the robotic arm, characterized in that, In each control cycle, the control method includes the following steps: Obtain the spatial position of the target tracking point and determine the desired position of the optical center of the endoscope when the target tracking point is located at the center of the endoscope image; Based on the desired position of the endoscope optical center, the motion trajectory of the endoscope optical center at multiple future moments is predicted using a model predictive control algorithm. Construct control conditions and generate an optimal control sequence that satisfies the control conditions; The robotic arm is controlled to move according to one of the optimal control sequences, thereby keeping the target tracking point at the center of the image.

2. The control method according to claim 1, characterized in that, The step of predicting the motion trajectory of the endoscope optical center at multiple future moments using a model predictive control algorithm, based on the desired position of the endoscope optical center, includes: Based on the spatial location of the target tracking point, the motion trajectory of the target tracking point at multiple future moments is predicted using a model predictive control algorithm. Based on the motion trajectory of the target tracking point, the motion trajectory of the endoscope optical center at multiple future moments is predicted using a model predictive control algorithm.

3. The control method according to claim 1 or 2, characterized in that, The model predictive control algorithm includes establishing discrete state-space equations describing the joint motion of the robotic arm, including: The state vector is determined based on the joint angles and angular velocities of the robotic arm, and the control input is determined based on the joint angular acceleration of the robotic arm. Based on the state vector and the control input, a continuous state-space equation is constructed; Based on the sampling time, the continuous state-space equation is discretized using the Euler method to obtain the discrete state-space equation.

4. The control method according to claim 2, characterized in that, The prediction of the target's trajectory at multiple future moments includes: Obtain the historical spatial location data of the target tracking point; Based on the historical spatial location data of the target tracking, the motion velocity of the target is estimated using the Kalman filter method; Based on the spatial location and speed of the target tracking point at the current moment, predict the trajectory of the target tracking point at multiple future moments.

5. The control method according to claim 1, characterized in that, The control conditions include a telecentric constraint, ensuring that the endoscope optical center is located on a sphere with the constraint position as the center and the distance between the constraint position and the endoscope optical center as the radius.

6. The control method according to claim 5, characterized in that, The telecentric constraint is constructed as a linear equality constraint with respect to joint angular velocity, the linear equality constraint including: Based on the constraint of the endoscope's motion around the constraint position, the constant distance relationship between the optical center of the endoscope and the constraint position is differentiated to obtain the orthogonal relationship between the velocity of the optical center of the endoscope and the constraint position. The optical center velocity of the endoscope is expressed as the product of the position Jacobian matrix of the robotic arm and the joint angular velocity. Substituting this into the orthogonality relation, a linear equation constraint regarding the joint angular velocity is obtained.

7. The control method according to claim 5, characterized in that, The control conditions also include: State constraints include joint angle limits and joint angular velocity limits for the robotic arm; And control constraints, including the limit range of the angular acceleration of the robotic arm joints.

8. The control method according to claim 7, characterized in that, The process of constructing control conditions and generating an optimal control sequence that satisfies the control conditions further includes: Establish control condition optimization, which includes minimizing the deviation between the actual spatial position of the endoscope optical center and the motion trajectory; Adjust the weight parameters of state constraints and control constraints; In order to obtain the optimal control sequence for multiple future time points.

9. The control method according to claim 8, characterized in that, The objective function of the optimal control sequence under the control conditions includes: ； in, This represents solving for the optimal control sequence, where the decision variables are... The goal is to minimize the value of the objective function. Indicates a control sequence; Indicates the prediction step size; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The actual location; Indicates in Predicting the future at any moment The endoscopic optical center at that moment The desired location; This represents the semi-positive definite state weight matrix. This represents the positive semidefinite input weight matrix; Indicates in Predicting the future at any moment -1 control input at time point.

10. The control method according to claim 1, characterized in that, The step of controlling the movement of the robotic arm according to one of the optimal control sequences includes controlling the movement of the robotic arm according to the first control quantity of the optimal control sequence, so that the robotic arm drives the endoscope to rotate in the constrained position along two axes perpendicular to the axis of the endoscope rod, thereby keeping the target tracking point located at the center of the endoscope image.

11. The control method according to claim 1, characterized in that, The step of acquiring the spatial position of the target tracking point and determining the desired position of the endoscope optical center when the target tracking point is located at the center of the image includes: The current position of the target tracking point in the endoscope image is obtained. Based on the current position of the target tracking point in the endoscope image and the positional relationship between the robotic arm end joint and the endoscope, the spatial position of the target tracking point and the spatial position of the optical center of the endoscope are determined. When the target tracking point is located at the center of the image, the desired position of the endoscope optical center is determined based on the spatial position of the target tracking point and the spatial position of the endoscope optical center.

12. A control device for a mirror-holding robot, characterized in that, The control device includes: The acquisition module acquires the spatial position of the target tracking point and determines the desired position of the optical center of the endoscope when the target tracking point is located at the center of the endoscope image. The first determining module, based on the desired position of the endoscope optical center, uses a model predictive control algorithm to predict the motion trajectory of the endoscope optical center at multiple future moments; The second determining module constructs control conditions and generates an optimal control sequence that satisfies the control conditions; The control module controls the movement of the robotic arm according to one of the optimal control sequences, thereby keeping the target tracking point located at the center of the image.

13. An electronic device, characterized in that, include: One or more processors; A memory having stored one or more computer programs that, when executed by one or more processors, cause the one or more processors to implement the model predictive control-based control method for a mirror-holding robot as described in any one of claims 1 to 11.

Images