Induction type mechanical arm endoscope lens control method

Abstract

The invention provides an induction type mechanical arm endoscope lens control method, which comprises the following steps: S1, initialization calibration: recording reference position data, and loading preset parameters; s2, head motion detection: capturing head three-dimensional rotation angular velocity, acceleration and magnetic field direction data, and generating head posture information and eye movement posture information in a quaternion form; s3, calculating and outputting: performing weighted fusion on the head posture information and the eye movement posture information, and calculating a rotation angle by using a remote center point RCMpos constraint; obstacle avoidance and anti-shake processing are carried out; and S4, the mechanical arm is controlled to rotate. The head and the eyes jointly control the rotation angle of the lens, the head provides a main rotation direction, the eyes are finely adjusted, and efficiency and fineness are both considered; quaternion is adopted for angle calculation, and the universal lock problem of traditional Euler angle control is avoided; the safety of the safety mechanical arm is improved by adopting triple design of center coordinate constraint, anti-shake and obstacle avoidance, and the mechanical arm or a lens is prevented from colliding with surrounding tissues.

Description

Technical Field

[0001] This invention relates to the field of laparoscopic surgical equipment, and more particularly to a method for controlling the laparoscopic lens of a sensor-operated robotic arm. Background Technology

[0002] Currently, manual control remains the fundamental method for controlling laparoscopic surgical lenses. A dedicated laparoscopic arm operator must adjust the lens's angle, depth, and focus in real-time based on verbal instructions from the surgeon to maintain a clear field of vision and assist in exposing the surgical area. Existing laparoscopic robotic arms are mainly controlled in two ways: one is manual operation (such as joysticks or handles), which requires the surgeon to be distracted by controlling the instruments, making it impossible to fully focus on the surgical field. Manual operation is also susceptible to hand tremors, resulting in insufficient precision and posing risks to aseptic technique. The other is single-sensor control. While pure head posture control can free up the hands, the large range of head movements makes it difficult to achieve precise operations such as focusing on the lesion. Furthermore, while robotic arms exist for holding the scope, most use Euler angles for calculation, leading to gimbal lock issues, causing problems such as robotic arm movement jamming and sudden posture changes, which also affect the control accuracy of laparoscopic surgery. Additionally, current technology lacks obstacle avoidance and anti-shake control, as the robotic arm may collide with tissue, causing complications. Summary of the Invention

[0003] To address the above problems, this invention proposes the following technical solution: a method for controlling the endoscope lens of a sensor-based robotic arm, comprising the following steps: S1: Initialization calibration: The user remains still and gazes at the preset reference point. The eye-tracking module records the initial gaze direction of the eyeball and the reference position data of the pupil. The robotic arm is located at the preset initial position, records the reference value, and loads the preset parameters.

[0004] Furthermore, the initial calibration preset parameters include the mapping coefficient K and the safety threshold d. safe Settings, fusion coefficient fusion weight interpolation coefficients RCM pos Coordinates, eye-tracking calibration coefficients .

[0005] All key parameters are calibrated and loaded in one go before the operation. No medical staff need to intervene in parameter settings during the operation. They can focus on the operation itself, avoid being distracted by parameter adjustments, shorten the operation preparation time, and improve the smoothness of the overall operation process.

[0006] S2: Motion Detection: When the user starts working, the IMU sensor captures the head's three-dimensional rotational angular velocity, acceleration, and magnetic field direction data at a certain frequency. First, the angular velocity of the gyroscope is integrated to obtain the initial head rotation angle. Then, the acceleration and magnetic field direction data are combined and filtered to eliminate integration errors and obtain the corrected angle. Quaternion calculation is performed based on the original angle to generate head posture information in quaternion form. At the same time, eye movement data is captured, and interference noise is eliminated through filtering algorithms. The turning angle of the eyeball relative to the reference position or the coordinates of the gaze point are calculated, and quaternion calculation is performed to generate eye movement posture information.

[0007] Furthermore, step S2 includes the following steps: S21: Initial angle calculation: The angular velocity of the gyroscope's three axes is captured by the IMU sensor. ;in ωt represents the angular velocity along the x, y, and z axes at time t; Then, by integrating, the angular velocity is converted into an angle, eliminating the influence of the time interval: ,

[0008] in: That is, the initial angle is 0, and Substituting them sequentially, we obtain the original angles. ; in: These represent the cumulative rotation angles of the head around the x, y, and z axes relative to the initial calibration position at time t.

[0009] Based on integral logic with a fixed acquisition frequency, a continuous record of head motion trajectory is formed, avoiding attitude discontinuity caused by discrete data and adapting to the need for continuous data in real-time control.

[0010] S22: Corrected Angle: The angle in the direction of gravity is calculated from acceleration; then the yaw angle in the direction of the geomagnetic field is calculated from the direction of the magnetic field; then filtering and fusion are performed to obtain the head-corrected angle. ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion.

[0011] Furthermore, step S22, which corrects the angle, includes the following sub-steps: S221: The angle of gravity direction is obtained from acceleration calculation. , ; in: , , Let be the acceleration components along the x, y, and z axes at time t.

[0012] The angle is calculated using the gravitational acceleration component, introducing Earth's gravity as a stable reference frame. This overcomes the limitations of gyroscopes, which can only collect relative angular velocity and are prone to integral drift, providing a basis for absolute angle calibration of head posture. Furthermore, the gravitational acceleration measurement is unaffected by the instantaneous head movement speed; even with rapid head rotation, it can still stably output the gravitational direction angle, providing a stable calibration benchmark for subsequent fusion filtering.

[0013] S222: Calculate the geomagnetic yaw angle using the magnetic field direction: ; in: , Let be the magnetic field strength at time t along the x and y axes.

[0014] It solves the technical pain point that neither gyroscope integration nor accelerometer gravity calibration can effectively obtain the yaw angle (horizontal rotation angle), and achieves full-dimensional calibration of the head's three-dimensional attitude; it corrects the integral drift of the gyroscope in the yaw direction, and avoids excessive cumulative error of the yaw angle after long-term operation.

[0015] S223: Filtering and Fusion: Angle correction is performed by filtering and fusing the angles of gravity direction and geomagnetic direction yaw. ; in The fusion coefficient; When i = x, y It uses gravity direction angle fusion; When i=z, The geomagnetic direction yaw angle is integrated. The final head angle after correction is obtained: ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion.

[0016] Furthermore, the fusion coefficient Take a value of 0.8 to 0.98.

[0017] The advantage of gyroscopes is their good dynamic performance, but they are prone to drift. The advantage of acceleration and magnetic force is their high static accuracy, but their dynamic response is slow. After fusion, they can complement each other's shortcomings. The output correction angle has neither the cumulative drift of gyroscopes nor dynamic lag, which greatly improves the reliability of angle data.

[0018] S23: Quaternion solution: Adjusting the head angle Convert to quaternion: q0, q1, q2, and q3 are calculated using the following formula:

[0019]

[0020]

[0021] ; Finally, the head pose information Q is obtained. head (t) represents the rotation of the head relative to its initial position.

[0022] Quaternion calculations are used to solve the gimbal lock defect of Euler angles, ensuring the accuracy of full-dimensional rotation. The four-dimensional hyperspherical representation of quaternions avoids the risk of gimbal lock, ensuring that the posture information is unambiguous and without dimensional loss when the head rotates in all dimensions, avoiding the failure of robotic arm control caused by gimbal lock, and ensuring the precise synchronization between lens turning and head movement.

[0023] S24: Eye-tracking data processing: Converting pupil offset data collected by the eye-tracking module into steering angle. ; in: For eye movement calibration coefficients; The number of pixels the pupil shifts at time t; Then perform moving average filtering: ; in: N is the average number of frames; Obtain smoothed eye movement angle information ;in Let t be the smooth turning angle of the eyeball relative to the reference fixation point in the x-axis and y-axis directions at time t.

[0024] Furthermore, the average number of frames N is set to 5~10; The number of pixels of pupil offset at time t is captured and obtained by the eye-tracking module.

[0025] Eye-tracking signals are susceptible to interference from physiological noise such as eye tremors, blinks, and micro-jumps of the eyeballs. By using N-frame sliding flattening, high-frequency micro-noise can be effectively filtered out, and smooth eye-tracking angle information can be output, avoiding unnecessary micro-shaking of the robotic arm due to noise and ensuring the smoothness of lens turning.

[0026] S25: Quaternion Calculation of Eye Movement Data: Calculating the Smoothed Eye Movement Angle Information Quaternion transformation is performed to obtain eye pose information: .

[0027] S3: Calculate and output: The main control chip weights and fuses the head pose information and eye movement pose information, and then uses the remote center point RCM as the output. posThe fixed position is used as a constraint condition, and the angle that the robotic arm needs to rotate is calculated according to the mapping coefficient; at the same time, the distance data between the lens and the tissue is monitored in real time by the distance sensor, obstacle avoidance judgment is made, and then the control command is output after processing by the anti-shake algorithm.

[0028] Furthermore, S3: Calculate and output the following steps: S31: Head-eye weighted fusion: using head pose information Q head (t) and eye pose information Quaternion multiplication is used to achieve rotational superposition, resulting in the fusion angle; the quaternion multiplication formula is as follows: ,in: The fusion weights are used to obtain the fused quaternions. .

[0029] Furthermore, the specific calculation formula for weighted quaternion exponentiation is as follows: ,in The rotation angle is... Similarly, calculate the axis of rotation. ; Fusion weights Take a value of 0.6 to 0.8.

[0030] Quaternions are used to fuse head and eye postures, directly using quaternions to represent the physical essence of three-dimensional rotation. The rotational movements of the head and eyeballs are accurately superimposed, which is more in line with the logic of real rigid body rotation. The fused posture information can more accurately locate the rotation.

[0031] S32: RCM constraint: derived from fused quaternions Calculate the rotation matrix Then calculate the coordinates of the robotic arm's camera. For camera coordinates Perform RCM constraint verification, if determined If the deviation from the RCM constraint is corrected, it is corrected until the constraint is satisfied, resulting in a quaternion that satisfies the RCM constraint. .

[0032] Furthermore, S32: RCM constraints include the following sub-steps: S321: Calculate the rotation matrix: from the fused quaternion Calculate the rotation matrix ; in: ; ; ; ; ; ; ; ; .

[0033] The abstract fusion pose quaternion is converted into a standardized 3×3 rotation matrix. Quaternions can only describe rotation relationships, while rotation matrices are the core carrier for calculating the transformation of point coordinates in three-dimensional space. This provides a mathematical basis for the subsequent calculation of the actual spatial position of the shot and solves the problem that mathematical poses alone cannot be directly associated with physical coordinates.

[0034] S322: RCM constraint determination: Calculate the coordinates of the robotic arm's camera. ,make sure Around The coordinates have no translation or rotation. The calculation formula is: ; Then through the initial coordinates and The initial rotation radius is calculated using the following formula: ; Among them: Pend 0−x Pend 0−y Pend 0−z Here are the x, y, and z coordinates of Pend0; X rcm Y rcm Z rcm : The x, y, and z coordinates of RCMpos; Similarly, using camera coordinates and Calculate the radius of rotation using coordinates: ; Where: P end−x P end−y P end−z for x, y, z coordinates; Then calculate the translation ΔL(t) = | |; If the translation ΔL(t) > 0.1 mm, then it is determined that... Deviation from RCM constraint.

[0035] Furthermore, The coordinates are usually taken as the location where the actual puncture point or the center of the surgery coincides.

[0036] S323: RCM constraint: If step S322 determines... If the deviation from the RCM constraint is found, a compensation quaternion is used to perform an iterative calculation correction formula. right Make corrections until the constraints are satisfied, resulting in a quaternion that satisfies the RCM constraints. The corrected formula is: .

[0037] Before outputting control commands to the robotic arm, verify whether the lens coordinates deviate from the RCM constraint and set a safety check. If a translation risk is found, a correction process can be triggered in time instead of directly driving the robotic arm to move, thus eliminating surgical risks such as tissue collision and instrument damage caused by lens translation.

[0038] S33: Obstacle Avoidance and Command Correction: Determine the distance sensor data d(t) and the safety threshold d safe If d(t)≥d safe No obstacle avoidance correction is performed. If 0.5d safe <d(t)<d safe This triggers obstacle avoidance correction; the robotic arm angle is adjusted in the opposite direction of the deviation. , For obstacle avoidance compensation quaternions; if d(t) < 0.5d safe , Pause rotation; output obstacle avoidance correction. .

[0039] Furthermore, the obstacle avoidance compensation quaternion is: ; in: The small angle for reverse correction is 0.5~5°. Unit vector of rotation axis: ; Safety threshold d safe Take 5~15mm.

[0040] S34: Shaking stabilization: Spherical linear interpolation is used for smooth transition to obtain the final quaternion. .

[0041] Spherical linear interpolation achieves smooth, jitter-free posture transitions by following the shortest arc path along the quaternion unit hypersphere. Compared to traditional linear interpolation, it ensures stable and abrupt angular velocity of the robotic arm rotation, completely eliminating lens shake caused by posture jumps and improving the stability of the surgical field of view.

[0042] Furthermore, the S34 image stabilization process also includes the following sub-steps: S341: Quaternion Normalization: After Obstacle Avoidance Correction Quaternion normalization is performed using the following formula: We obtain the normalized quaternion: .

[0043] S342: Hyperspherical Angle Calculation: Calculating the hyperspherical angle in four-dimensional space using normalized quaternions Q0 and Q1. cosθ= ; in: Q0 is the quaternion after image stabilization of the previous frame; Q1 is the quaternion after obstacle avoidance correction in the current frame; θ is the angle between Q0 and Q1 on the hypersphere; The hypersphere angle θ is then calculated to be arccos(cosθ).

[0044] Furthermore, if cosθ < 0, it indicates that the interpolation path is a long arc, and the sign of Q1 is flipped; if cosθ ≈ 1, the two quaternions almost overlap, and linear interpolation Q is used directly. t = (1- Q0+ Q1), These are the interpolation coefficients.

[0045] S343: Interpolation calculation: Interpolation calculation is performed using the included angle of the hypersphere; The interpolation calculation formula is: ; in: These are the interpolation coefficients; the final quaternion is obtained after processing. .

[0046] Furthermore, interpolation coefficients The value is 0.1 to 0.3.

[0047] S35: Instruction Output: Output the final quaternion Converted to total user rotation angle f (t) = [ f-x , f-y , f-z Then, the target angle of the robotic arm is calculated using the mapping coefficient K. t−i (t)= fusion−f−i (t)⋅K f-i , Substitute these values ​​sequentially to obtain the rotation angle of the robotic arm. t= [ t-x , t-y , t-z Then rotate the robotic arm by an angle. t This is converted into a motor drive signal, which is then used to obtain control commands for the robotic arm.

[0048] Furthermore, the robotic arm's x-axis represents horizontal steering, its y-axis represents vertical steering, and its z-axis represents rotation, corresponding to K... f-x Take 3~6, ​​K f-y Take 2~5, K f-z Take a value of 0.5 to 1.

[0049] S4: Control the rotation of the robotic arm: The main control chip outputs control commands to drive the robotic arm motor, and the motor drives the robotic arm to rotate.

[0050] Compared with the prior art, the beneficial effects of the present invention are as follows: the head and eyes jointly control the lens rotation angle, with the head providing the main rotation direction and the eyes making fine adjustments, balancing efficiency and precision; quaternions are used for angle calculation to avoid the gimbal lock problem of traditional Euler angle control; and a triple design of center coordinate constraint, image stabilization, and obstacle avoidance is adopted to improve the safety of the robotic arm and prevent the robotic arm or lens from colliding with surrounding tissues. Detailed Implementation

[0051] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the embodiments of the present application: A method for controlling a laparoscopic lens of a robotic arm, comprising the following steps: To solve the above problems, the present invention proposes the following technical solution: A method for controlling a laparoscopic lens of a robotic arm, comprising the following steps: S1: Initialization calibration: The user remains still and gazes at the preset reference point. The eye-tracking module records the initial gaze direction of the eyeball and the reference position data of the pupil. The robotic arm is located at the preset initial position, records the reference value, and loads the preset parameters.

[0052] Furthermore, the initial calibration preset parameters include the mapping coefficient K and the safety threshold d. safe Settings, fusion coefficient Fusion coefficient fusion weight interpolation coefficients RCM pos Coordinates, eye-tracking calibration coefficients .

[0053] All key parameters are calibrated and loaded in one go before the operation. No medical staff need to intervene in parameter settings during the operation. They can focus on the operation itself, avoid being distracted by parameter adjustments, shorten the operation preparation time, and improve the smoothness of the overall operation process.

[0054] S2: Motion Detection: When the user starts working, the IMU sensor captures the head's three-dimensional rotational angular velocity, acceleration, and magnetic field direction data at a certain frequency. First, the angular velocity of the gyroscope is integrated to obtain the initial head rotation angle. Then, the acceleration and magnetic field direction data are combined and filtered to eliminate integration errors and obtain the corrected angle. Quaternion calculation is performed based on the original angle to generate head posture information in quaternion form. At the same time, eye movement data is captured, and interference noise is eliminated through filtering algorithms. The turning angle of the eyeball relative to the reference position or the coordinates of the gaze point are calculated, and quaternion calculation is performed to generate eye movement posture information.

[0055] Furthermore, step S2 includes the following steps: S21: Initial angle calculation: The angular velocity of the gyroscope's three axes is captured by the IMU sensor. ;in ωt represents the angular velocity along the x, y, and z axes at time t; Then, by integrating, the angular velocity is converted into an angle, eliminating the influence of the time interval: ,

[0056] in: That is, the initial angle is 0, and Substituting them sequentially, we obtain the original angles. in: These represent the cumulative rotation angles of the head around the x, y, and z axes relative to the initial calibration position at time t.

[0057] Based on integral logic with a fixed acquisition frequency, a continuous record of head motion trajectory is formed, avoiding attitude discontinuity caused by discrete data and adapting to the need for continuous data in real-time control.

[0058] S22: Corrected Angle: The angle in the direction of gravity is calculated from acceleration; then the yaw angle in the direction of the geomagnetic field is calculated from the direction of the magnetic field; then filtering and fusion are performed to obtain the head-corrected angle. ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion.

[0059] Furthermore, step S22, which corrects the angle, includes the following sub-steps: S221: Calculate the angle of gravity direction from acceleration: , ; in: , , Let be the acceleration components along the x, y, and z axes at time t.

[0060] The angle is calculated using the gravitational acceleration component, introducing Earth's gravity as a stable reference frame. This overcomes the limitations of gyroscopes, which can only collect relative angular velocity and are prone to integral drift, providing a basis for absolute angle calibration of head posture. Furthermore, the gravitational acceleration measurement is unaffected by the instantaneous head movement speed; even with rapid head rotation, it can still stably output the gravitational direction angle, providing a stable calibration benchmark for subsequent fusion filtering.

[0061] S222: Calculate the geomagnetic yaw angle using the magnetic field direction: ; in: , Let be the magnetic field strength at time t along the x and y axes.

[0062] It solves the technical pain point that neither gyroscope integration nor accelerometer gravity calibration can effectively obtain the yaw angle (horizontal rotation angle), and achieves full-dimensional calibration of the head's three-dimensional attitude; it corrects the integral drift of the gyroscope in the yaw direction, and avoids excessive cumulative error of the yaw angle after long-term operation.

[0063] S223: Filtering and Fusion: Angle correction is performed by filtering and fusing the angles of gravity direction and geomagnetic direction yaw. ; in The fusion coefficient; When i = x, y It uses gravity direction angle fusion; When i=z, The geomagnetic direction yaw angle is integrated. The final head angle after correction is obtained: ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion.

[0064] Furthermore, the fusion coefficient Take 0.95.

[0065] The advantage of gyroscopes is their good dynamic performance, but they are prone to drift. The advantage of acceleration and magnetic force is their high static accuracy, but their dynamic response is slow. After fusion, they can complement each other's shortcomings. The output correction angle has neither the cumulative drift of gyroscopes nor dynamic lag, which greatly improves the reliability of angle data.

[0066] S23: Quaternion solution: Adjusting the head angle Convert to quaternion: q0, q1, q2, and q3 are calculated using the following formula:

[0067]

[0068]

[0069] ; Finally, the head pose information Q is obtained. head (t) represents the rotation of the head relative to its initial position.

[0070] Quaternion calculations are used to solve the gimbal lock defect of Euler angles, ensuring the accuracy of full-dimensional rotation. The four-dimensional hyperspherical representation of quaternions avoids the risk of gimbal lock, ensuring that the posture information is unambiguous and without dimensional loss when the head rotates in all dimensions, avoiding the failure of robotic arm control caused by gimbal lock, and ensuring the precise synchronization between lens turning and head movement.

[0071] S24: Eye-tracking data processing: Converting pupil offset data collected by the eye-tracking module into steering angle. ; in: The eye-tracking calibration coefficient is set to 0.015° / pixel. The number of pixels the pupil shifts at time t; Then perform moving average filtering: ; in: N is the average number of frames; Obtain smoothed eye movement angle information ;in Let t be the smooth turning angle of the eyeball relative to the reference fixation point in the x-axis and y-axis directions at time t.

[0072] Furthermore, the average number of frames N is set to 5; The number of pixels of pupil offset at time t is captured and obtained by the eye-tracking module.

[0073] Eye-tracking signals are susceptible to interference from physiological noise such as eye tremors, blinks, and micro-jumps of the eyeballs. By using N-frame sliding flattening, high-frequency micro-noise can be effectively filtered out, and smooth eye-tracking angle information can be output, avoiding unnecessary micro-shaking of the robotic arm due to noise and ensuring the smoothness of lens turning.

[0074] S25: Quaternion Calculation of Eye Movement Data: Calculating the Smoothed Eye Movement Angle Information Quaternion transformation is performed to obtain eye pose information: .

[0075] S3: Calculate and output: The main control chip weights and fuses the head pose information and eye movement pose information, and then uses the remote center point RCM as the output. pos The fixed position is used as a constraint condition, and the angle that the robotic arm needs to rotate is calculated according to the mapping coefficient; at the same time, the distance data between the lens and the tissue is monitored in real time by the distance sensor, obstacle avoidance judgment is made, and then the control command is output after processing by the anti-shake algorithm.

[0076] Furthermore, S3: Calculate and output the following steps: S31: Head-eye weighted fusion: using head pose information Q head (t) and eye pose information Quaternion multiplication is used to achieve rotational superposition, resulting in the fusion angle; the quaternion multiplication formula is as follows: ,in: The fusion weights are used to obtain the fused quaternions. .

[0077] S25: Quaternion Calculation of Eye Movement Data: Calculating the Smoothed Eye Movement Angle Information Quaternion transformation is performed to obtain eye pose information: .

[0078] S3: Calculate and output: The main control chip weights and fuses the head pose information and eye movement pose information, and then uses the remote center point RCM as the output. pos The fixed position is used as a constraint condition, and the angle that the robotic arm needs to rotate is calculated according to the mapping coefficient; at the same time, the distance data between the lens and the tissue is monitored in real time by the distance sensor, obstacle avoidance judgment is made, and then the control command is output after processing by the anti-shake algorithm.

[0079] Furthermore, S3: Calculate and output the following steps: S31: Head-eye weighted fusion: using head pose information Q head (t) and eye pose information Quaternion multiplication is used to achieve rotational superposition, resulting in the fusion angle; the quaternion multiplication formula is as follows: ,in: The fusion weights are used to obtain the fused quaternions. .

[0080] Furthermore, S32: RCM constraints include the following sub-steps: S321: Calculate the rotation matrix: from the fused quaternion Calculate the rotation matrix ; in: ; ; ; ; ; ; ; ; .

[0081] The abstract fusion pose quaternion is converted into a standardized 3×3 rotation matrix. Quaternions can only describe rotation relationships, while rotation matrices are the core carrier for calculating the transformation of point coordinates in three-dimensional space. This provides a mathematical basis for the subsequent calculation of the actual spatial position of the shot and solves the problem that mathematical poses alone cannot be directly associated with physical coordinates.

[0082] S322: RCM constraint determination: Calculate the coordinates of the robotic arm's camera. ,make sure Around The coordinates have no translation or rotation. The calculation formula is: ; Then through the initial coordinates and The initial rotation radius is calculated using the following formula: ; Among them: Pend 0−x Pend 0−y Pend 0−z For P end (0) x, y, z coordinates; X rcm Y rcm Z rcm : The x, y, and z coordinates of RCMpos; Similarly, using camera coordinates and Calculate the radius of rotation using coordinates: ; Where: P end−x P end−y P end−z for x, y, z coordinates; Then calculate the translation ΔL(t) = | |; If the translation ΔL(t) > 0.1 mm, then it is determined that... Deviation from RCM constraint.

[0083] Furthermore, The coordinates are usually taken as the location where the actual puncture point or the center of the surgery coincides.

[0084] S323: RCM constraint: If step S322 determines... If the deviation from the RCM constraint is found, a compensation quaternion is used to perform an iterative calculation correction formula. right Make corrections until the constraints are satisfied, resulting in a quaternion that satisfies the RCM constraints. The corrected formula is: .

[0085] Before outputting control commands to the robotic arm, verify whether the lens coordinates deviate from the RCM constraint and set a safety check. If a translation risk is found, a correction process can be triggered in time instead of directly driving the robotic arm to move, thus eliminating surgical risks such as tissue collision and instrument damage caused by lens translation.

[0086] S33: Obstacle Avoidance and Command Correction: Determine the distance sensor data d(t) and the safety threshold d safe If d(t)≥d safe No obstacle avoidance correction is performed. If 0.5d safe <d(t)<d safe This triggers obstacle avoidance correction; the robotic arm angle is adjusted in the opposite direction of the deviation. , For obstacle avoidance compensation quaternions; if d(t) < 0.5d safe , Pause rotation; output obstacle avoidance correction. .

[0087] Furthermore, the obstacle avoidance compensation quaternion is: ; in: The small angle for reverse correction is 3°; Unit vector of rotation axis: ; Safety threshold d safe Take 10mm.

[0088] S34: Shaking stabilization: Spherical linear interpolation is used for smooth transition to obtain the final quaternion. .

[0089] Spherical linear interpolation achieves smooth, jitter-free posture transitions by following the shortest arc path along the quaternion unit hypersphere. Compared to traditional linear interpolation, it ensures stable and abrupt angular velocity of the robotic arm rotation, completely eliminating lens shake caused by posture jumps and improving the stability of the surgical field of view.

[0090] Furthermore, the S34 image stabilization process also includes the following sub-steps: S341: Quaternion Normalization: After Obstacle Avoidance Correction Quaternion normalization is performed using the following formula: We obtain the normalized quaternion: .

[0091] S342: Hyperspherical Angle Calculation: Calculating the hyperspherical angle in four-dimensional space using normalized quaternions Q0 and Q1. cosθ= ; in: Q0 is the quaternion after image stabilization of the previous frame; Q1 is the quaternion after obstacle avoidance correction in the current frame; θ is the angle between Q0 and Q1 on the hypersphere; The hypersphere angle θ is then calculated to be arccos(cosθ).

[0092] Furthermore, if cosθ < 0, it indicates that the interpolation path is a long arc, and the sign of Q1 is flipped; if cosθ ≈ 1, the two quaternions almost overlap, and linear interpolation Q is used directly. t = (1- Q0+ Q1), These are the interpolation coefficients.

[0093] S343: Interpolation calculation: Interpolation calculation is performed using the included angle of the hypersphere; The interpolation calculation formula is: ; in: These are the interpolation coefficients; the final quaternion is obtained after processing. .

[0094] Furthermore, interpolation coefficients It is 0.2.

[0095] S35: Instruction Output: Output the final quaternion Converted to total user rotation angle f (t) = [ f-x , f-y , f-z Then, the target angle of the robotic arm is calculated using the mapping coefficient K. t−i (t)= fusion−f−i (t)⋅K f-i , Substitute these values ​​sequentially to obtain the rotation angle of the robotic arm. t= [ t-x , t-y , t-z Then rotate the robotic arm by an angle. t This is converted into a motor drive signal, which is then used to obtain control commands for the robotic arm.

[0096] Furthermore, the robotic arm's x-axis represents horizontal steering, its y-axis represents vertical steering, and its z-axis represents rotation, corresponding to K... f-x =4,K f-y =3,K f-z =0.8.

[0097] S4: Control the rotation of the robotic arm: The main control chip outputs control commands to drive the robotic arm motor, and the motor drives the robotic arm to rotate.

[0098] The camera rotation angle is controlled by both the head and the eye. The head provides the main rotation direction, while the eye makes fine adjustments, balancing efficiency and precision. Quaternions are used for angle calculation to avoid the gimbal lock problem of traditional Euler angle control. The safety of the robotic arm is improved by adopting a triple design of center coordinate constraint, image stabilization, and obstacle avoidance to prevent the robotic arm or lens from colliding with surrounding tissues.

[0099] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for controlling the endoscope lens of a sensor-based robotic arm, characterized in that: Includes the following steps: S1: Initialization calibration: The user remains still and gazes at the preset reference point. The eye-tracking module records the initial gaze direction of the eyeball and the reference position data of the pupil. The robotic arm is in the preset initial position, records the reference value, and loads the preset parameters. S2: Motion Detection: When the user starts working, the IMU sensor captures the head's three-dimensional rotational angular velocity, acceleration, and magnetic field direction data; first, the angular velocity of the gyroscope is integrated to obtain the preliminary three-dimensional rotation angle of the head; The head's three-dimensional rotation angle is fused and filtered with acceleration and magnetic field direction data to eliminate integral errors and obtain the corrected angle. Quaternion calculations are performed based on the corrected angles to generate head pose information in quaternion form; at the same time, eye movement data is captured, and interference noise is eliminated through filtering algorithms to calculate the turning angle of the eye relative to the reference position or the coordinates of the gaze point, and quaternion calculations are performed to generate eye movement pose information. S3: Calculate and output: The main control chip weights and fuses the head pose information and eye movement pose information, and then uses the remote center point RCM as the output. pos The fixed position is used as a constraint condition, and the angle that the robotic arm needs to rotate is calculated according to the mapping coefficient; at the same time, the distance data between the lens and the tissue is monitored in real time by the distance sensor, obstacle avoidance judgment is made, and then the control command is output through the anti-shake algorithm. S4: Control the rotation of the robotic arm: The main control chip outputs control commands to drive the robotic arm motor, and the motor drives the robotic arm to rotate.

2. The inductive robotic arm endoscope lens control method as described in claim 1, characterized in that: S2 includes the following steps: S21: Initial angle calculation: The angular velocity ωg(t) of the gyroscope's three axes is obtained from the IMU sensor = [ ];in Let be the angular velocities along the x, y, and z axes at time t, respectively; then, the angular velocities are converted into angles through integration, where ... ωt represents the angular velocity along the x, y, and z axes at time t; S22: Correction angle: The angle in the direction of gravity is calculated from the acceleration; then the yaw angle in the direction of the geomagnetic field is calculated from the direction of the magnetic field. After filtering and fusion, the final head-corrected angle is obtained: ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion. S23: Quaternion solution: Adjusting the head angle Convert to quaternion: q0, q1, q2, and q3 are calculated using the following formula: Finally, we obtain the head pose information Q. head (t), representing the rotation of the head relative to the initial position; S24: Eye-tracking data processing: Converting pupil offset data collected by the eye-tracking module into steering angle. ;in: For eye movement calibration coefficients; Let be the number of pixels of pupil offset at time t; then perform a moving average filter: Where: N is the average number of frames; smoothed eye movement angle information is obtained. ;in denoted as the smooth turning angle of the eyeball relative to the reference fixation point in the x-axis and y-axis directions at time t; S25: Quaternion Calculation of Eye Movement Data: Calculating the Smoothed Eye Movement Angle Information Quaternion transformation is performed to obtain eye pose information: .

3. The inductive robotic arm endoscope lens control method as described in claim 2, characterized in that: S22 also includes the following sub-steps: S221: Calculation of the gravitational direction angle from acceleration: The gravitational direction angle is obtained by calculating from acceleration. , ;in: , , Let x be the acceleration components along the x, y, and z axes at time t; S222: Calculating the geomagnetic yaw angle using the magnetic field direction: Calculating the geomagnetic yaw angle using the magnetic field direction: ;in: , Let be the magnetic field strength at time t along the x and y axes; S223: Filtering and Fusion: Angle correction is performed by filtering and fusing the angles of gravity direction and geomagnetic direction yaw. ;in For fusion coefficients; when i = x, y, The fusion is performed using the direction and angle of gravity; when i=z, The geomagnetic direction yaw angle is used for fusion; finally, the head-corrected angle is obtained: ;in The corrected angles of the head around the x, y, and z axes of the three-dimensional rectangular coordinate system at time t are obtained after gravity direction calibration, geomagnetic direction calibration, and filtering fusion.

4. The inductive robotic arm endoscope lens control method as described in claim 1, characterized in that: S3: Calculate and output the following steps: S31: Head-eye weighted fusion: using head pose information Q head (t) and eye pose information Quaternion multiplication is used to achieve rotational superposition, resulting in the fusion angle. The formula for quaternion multiplication is as follows: ;in For weighting; Obtain the fused quaternion ; S32: RCM constraint: derived from fused quaternions Calculate the rotation matrix Then calculate the coordinates of the robotic arm's camera. For camera coordinates Perform RCM constraint verification, if determined If the deviation from the RCM constraint is corrected, it is corrected until the constraint is satisfied, resulting in a quaternion that satisfies the RCM constraint. ; S33: Obstacle Avoidance and Command Correction: Determine the distance sensor data d(t) and the safety threshold d safe If d(t)≥d safe No obstacle avoidance correction is performed. ; If 0.5⋅d safe <d(t)<d safe This triggers obstacle avoidance correction; the robotic arm angle is adjusted in the opposite direction of the deviation. , , For obstacle avoidance compensation quaternions; if d(t) < 0.5⋅d safe , Pause rotation; output obstacle avoidance correction. ; S34: Shaking stabilization: Spherical linear interpolation is used for smooth transition to obtain the final quaternion. ; S35: Instruction Output: Output the final quaternion Converted to total user rotation angle f (t) = [ f-x , f-y , f-z Then, the target angle of the robotic arm is calculated using the mapping coefficient K. t−i (t)= fusion−f−i (t)⋅K f-i , Substitute these values ​​sequentially to obtain the rotation angle of the robotic arm. t= [ t-x , t-y , t-z Then rotate the robotic arm by an angle. t This is converted into a motor drive signal, which is then used to obtain control commands for the robotic arm.

5. The inductive robotic arm endoscope lens control method as described in claim 4, characterized in that: S32: RCM constraints also include the following sub-steps: S321: Calculate the rotation matrix: from the fused quaternion Calculate the rotation matrix ,in ; ; ; ; ; ; ; ; ; S322: RCM constraint determination: Calculate the coordinates of the robotic arm's camera. ,make sure Around The coordinates have no translation or rotation. The calculation formula is: Then through the initial coordinates and The initial rotation radius is calculated using the following formula: ;wherein: Pend 0−x Pend 0−y Pend 0−z for x, y, z coordinates; X rcm Y rcm Z rcm The x, y, and z coordinates of RCMpos; similarly, the lens coordinates. and Calculate the radius of rotation using coordinates: Wherein: P end−x P end−y P end−z for Find the x, y, and z axis coordinates; then calculate the translation ΔL(t) = | |; If the translation ΔL(t) > 0.1 mm, then determine Deviation from RCM constraint; S323: RCM constraint: If step S322 determines... If the deviation from the RCM constraint is found, a compensation quaternion is used by iteratively calculating the correction formula. right Make corrections until the constraints are satisfied, resulting in a quaternion that satisfies the RCM constraints. The corrected formula is: .

6. The inductive robotic arm endoscope lens control method as described in claim 4, characterized in that: S34 image stabilization includes the following sub-steps: S341: Quaternion Normalization: After Obstacle Avoidance Correction Quaternion normalization is performed using the following formula: We obtain the normalized quaternion: ; S342: Hyperspherical Angle Calculation: The hyperspherical angle in four-dimensional space is calculated using normalized quaternions Q0 and Q1: cosθ = Where: Q0 is the quaternion after image stabilization in the previous frame; Q1 is the quaternion after obstacle avoidance correction in the current frame; θ is the angle between Q0 and Q1 on the hypersphere; the hypersphere angle θ is then calculated as arccos(cosθ). S343: Interpolation Calculation: Interpolation calculation is performed using the included angle of the hypersphere; the interpolation calculation formula is: ;in: These are the interpolation coefficients; the final quaternion is obtained after processing. .

Images