Bronchoscope bending control method based on double electromagnetic sensor structure

Abstract

The invention discloses a bronchoscope bending control method based on a double-electromagnetic-sensor structure. A first electromagnetic sensor and a second electromagnetic sensor are arranged at the starting end and the tail end of a bending section of a bronchoscope respectively; determining a first rotation matrix, a starting end coordinate, a second rotation matrix and a tail end coordinate of the electromagnetic sensor relative to a geodetic coordinate system by using parameters collected by the two electromagnetic sensors, determining a bending angle of the bending section according to the first rotation matrix and the second rotation matrix, and determining a deflection angle of the bending section according to the first rotation matrix, the starting end coordinate and the tail end coordinate; according to the bending angle and the deflection angle of the bronchoscope and the expected bending angle and the expected deflection angle of the bronchoscope, the required first motor driving amount and the required second motor driving amount are determined and used for controlling bending and deflection of the bronchoscope. The cost of the bronchoscope can be reduced, and the control precision of the bronchoscope can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of medical devices, and in particular to a bronchoscope bending control method based on a dual electromagnetic sensor structure. Background Art

[0002] Lung cancer is one of the most common cancers. During diagnosis and treatment, doctors first determine the location of the tumor based on CT images and manually drive the bronchoscope to the lesion for exploration. Currently, conventional bronchoscopes are directly driven by motors and are open-loop controlled. During surgery, the bending accuracy of conventional bronchoscopes will be affected by the interference of the complex structure of the human airway.

[0003] In addition, during the operation, it is easy to obtain the image information of the camera at the end of the bronchoscope, but it is not easy to obtain the current deflection angle and bending angle information of the bronchoscope. Therefore, the end of the bronchoscope is prone to cause tissue damage to the patient's trachea, thereby causing complications such as pneumothorax and bleeding. Currently, bronchoscopes on the market that provide posture control usually use fiber optic sensors (FBGS) to obtain position and posture information, and then calculate the bending angle and deflection angle of the end of the bronchoscope, specifically including: (1) posture detection based on bronchoscope images; (2) position detection based on a single electromagnetic sensor. However, the existing technical means have the following shortcomings:

[0004] (1) The sensor is expensive, fragile, and has a short service life;

[0005] (2) The motion trajectory of the bronchoscope is often located using moving images from different frames of the image. However, this type of method has cumulative errors in the positioning process. In addition, the images taken by the bronchoscope have weak textures and unstable lighting, which leads to large errors in the calculation of the motion trajectory and makes it difficult to estimate the bending angle and deflection angle of the end of the bronchoscope.

[0006] (3) Based on the posture detection of a single electromagnetic sensor, due to the lack of a suitable reference coordinate system, the bending angle and deflection angle of the bronchoscope tip cannot be calculated, making it difficult to achieve the desired bending angle and desired deflection angle control.

[0007] The disclosure of the above background technology content is only used to assist in understanding the inventive concept and technical solution of the present invention. It does not necessarily belong to the prior art of this patent application, nor does it necessarily provide technical guidance. In the absence of clear evidence that the above content has been disclosed before the filing date of this patent application, the above background technology should not be used to evaluate the novelty and creativity of the present application. Summary of the invention

[0008] The purpose of the present invention is to provide a bronchoscope bending control method based on a dual electromagnetic sensor structure, which can reduce the cost of the bronchoscope and improve the control accuracy of the bronchoscope.

[0009] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0010] A bronchoscope bending control method based on a dual electromagnetic sensor structure comprises the following steps:

[0011] A first electromagnetic sensor is arranged at the starting end of the curved section of the bronchoscope, and a second electromagnetic sensor is arranged at the end of the curved section of the bronchoscope;

[0012] Determine the starting end coordinate P according to the position parameters collected by the first electromagnetic sensor at time k begin (k), the starting end coordinates are the coordinates of the starting end of the curved segment in the geodetic coordinate system, and a first rotation matrix R of the first electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the first electromagnetic sensor begin (k);

[0013] Determine the terminal coordinate P according to the position parameters collected by the second electromagnetic sensor at time k end (k), the end coordinates are the coordinates of the end of the curved segment in the geodetic coordinate system, and a second rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the second electromagnetic sensor end (k);

[0014] According to the first rotation matrix R begin (k), the second rotation matrix R end (k) determining the bending angle θ(k) of the bending segment at time k, and according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) determining the deflection angle φ(k) of the bending segment at time k;

[0015] According to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope d (k) and the desired deflection angle φ d (k) determining a first motor drive amount u1(k) and a second motor drive amount u2(k) required for the bronchoscope to reach a desired bending angle and a desired deflection angle, and controlling the bending and deflection of the bronchoscope based on the first motor drive amount u1(k) and the second motor drive amount u2(k).

[0016] Further, according to any one of the above-mentioned technical solutions or a combination of multiple technical solutions, according to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope d (k) and the desired deflection angle φ d(k) determining the first motor driving amount u1(k) and the second motor driving amount u2(k), comprising the following steps:

[0017] Calculate the first virtual state quantity S1(k) and the second virtual state quantity S2(k), where S1(k)=θ(k)×r0×cos(φ(k), S2(k)=θ(k)×r0×sin(φ(k));

[0018] Calculate the expected first virtual state quantity S 1d (k) and the desired second virtual state quantity S 2d (k), where S 1d =θ d (k)×r0×cos(φ d (k)), S 2d =θ d (k)×r0×sin(φ d (k));

[0019] According to the first virtual state quantity S1(k), the second virtual state quantity S2(k) and the expected first virtual state quantity S 1d (k), the desired second virtual state quantity S 2d (k) A PID controller is used to determine a first motor driving amount u1(k) and a second motor driving amount u2(k) required for the bronchoscope to reach the desired bending angle and the desired deflection angle.

[0020] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, a PID controller is used to determine the first motor drive amount u1(k) and the second motor drive amount u2(k) required for the bronchoscope to reach a desired bending angle and a desired deflection angle, including:

[0021]

[0022] Among them, K p1 , K p2 , K i1 , K i1 , K d1 and K d1 is the parameter of PID controller, T s represents the sampling time, e1(k) and e1(k-1) are the first error values ​​at time k and time k-1 respectively, the first error value is the error value between the expected first virtual state quantity and the first virtual state quantity, e2(k) and e2(k-1) are the second error values ​​at time k and time k-1 respectively, the second error value is the error value between the expected second virtual state quantity and the second virtual state quantity.

[0023] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the calculation formula of the first error value and the second error value at time k is:

[0024] e1(k)=S 1d (k)-S1(k);

[0025] e2(k)=S 2d (k)-S2(k).

[0026] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the calculation formula of the deflection angle φ(k) of the bending segment at time k is:

[0027]

[0028] Wherein, arctan is represented as the inverse tangent function, which represents the coordinate of the end of the curved segment relative to its starting end in the x-axis direction, and y b (k) represents the coordinate of the end of the curved segment relative to its starting end in the y-axis direction.

[0029] Furthermore, based on any one of the above technical solutions or a combination of multiple technical solutions, it also includes according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) Determine the coordinate system of the starting end of the curved segment and the coordinate system of the end of the curved segment relative to the starting end. begin P end (k) begin P end (k) = [x b (k), y b (k), z b (k)];

[0030] begin P end The calculation formula of (k) is:

[0031] begin p end (k) = R begin -1 (k)×[p end (k)-p begin (k)]

[0032] Among them, R begin -1 (k) represents R begin The inverse matrix of (k).

[0033] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the parameters collected by the electromagnetic sensor include position parameters [x, y, z], and the position parameters represent the spatial position of the electromagnetic sensor;

[0034] The starting point coordinate P begin (k) is: p begin =[x begin ,y begin ,z begin ] T ,[x begin ,y begin , z begin ] is a position parameter collected by the first electromagnetic sensor;

[0035] The end coordinate is P end (k) is: p end =[x end ,y end ,z end ] T ,[x end ,y end , z end ] is the position parameter collected by the second electromagnetic sensor.

[0036] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the calculation formula of the bending angle θ(k) of the bending segment at time k is:

[0037]

[0038] Among them, |||| represents the second norm of the vector, R Zbegin (k) represents R begin The z-axis component of (k) in the geodetic coordinate system, R Zbegin T (k) represents R Zbegin The transposed matrix of (k), R Zend (k) represents R end (k) is the z-axis component of the vector in the geodetic coordinate system.

[0039] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the parameters collected by the electromagnetic sensor include quaternions, and the quaternion is expressed as [q0, q x ,q y ,q z ], the quaternion represents the posture of the electromagnetic sensor;

[0040] The quaternion collected by the first electromagnetic sensor is [q 0begin ,q xbegin ,qybegin ,q zbegin ], using the quaternion fed back by the first electromagnetic sensor to determine the first rotation matrix R of the first electromagnetic sensor relative to the earth coordinate system in the following manner: begin , The calculation formulas for each element in the first rotation matrix are as follows:

[0041]

[0042] The quaternion collected by the second electromagnetic sensor is [q 0end ,q xbegin ,q yend ,q zend ], the second rotation matrix R of the second electromagnetic sensor relative to the earth coordinate system is determined by using the quaternion collected by the second electromagnetic sensor in the following manner: end , The calculation formulas for each element in the second rotation matrix are as follows:

[0043]

[0044] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the parameters collected by the electromagnetic sensor include quaternions, and the quaternion is expressed as [q0, q x ,q y ,q z ], the quaternion represents the posture of the electromagnetic sensor;

[0045] Based on the quaternion collected by the electromagnetic sensor, a transformation matrix of the electromagnetic sensor relative to the earth coordinate system is determined according to the Hamilton right-handed rotation system.

[0046] The beneficial effects brought by the technical solution provided by the present invention are as follows:

[0047] a. The present invention sets an electromagnetic sensor at the starting end and the end of the bending section of the bronchoscope respectively, and uses the parameters collected by the two electromagnetic sensors to determine the posture of the bronchoscope in real time, including the deflection angle and the bending angle of the bronchoscope, and uses the real-time deflection angle, the bending angle and the expected deflection angle, The angle determines the required motor drive amount and controls the bronchoscope, and does not need to use an expensive FBGS, which can reduce the cost of the bronchoscope, and has no cumulative error, and can improve the control accuracy of the bronchoscope;

[0048] b. The present invention installs the first and second electromagnetic sensors separately at both ends of the curved section of the bronchoscope, so that the bending of the bronchoscope will not cause tensile loss to the sensor, and the service life of the electromagnetic sensor can be extended;

[0049] c. The present invention can accurately determine and calculate the deflection angle and bending angle of the bending segment in real time, and the value range of the calculated deflection angle is -180° to 180°, which is not only applicable to bronchoscopes that rotate 180°, but also to bronchoscopes that rotate 360°. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0051] Figure 1 A schematic flow chart of a bronchoscope bending control method provided for an exemplary embodiment of the present invention;

[0052] Figure 2 A schematic structural diagram of a bronchoscope in a straightened state provided by an exemplary embodiment of the present invention;

[0053] Figure 3 A schematic structural diagram of a bronchoscope in a bent state provided by an exemplary embodiment of the present invention;

[0054] Figure 4 A schematic diagram of the principle of dual-motor controlled bronchoscope bending provided for an exemplary embodiment of the present invention.

[0055] Among them, the figure marks include: 1-first guide wire, 2-second guide wire, 3-third guide wire, 4-fourth guide wire, 5-bronchoscope body, 61-first motor, 62-second motor. DETAILED DESCRIPTION

[0056] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0057] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, device, product or equipment that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0058] To facilitate understanding of the embodiments of the present invention, the symbols appearing in the embodiments of the present invention are defined as follows:

[0059] (1) Geodetic coordinate system o-xyz;

[0060] (2) The electromagnetic sensor returns seven parameters in real time, including three position parameters x, y, and z, and quaternions q0, q x ,q y ,q z ;

[0061] (3) The signs of the seven parameters returned by the first electromagnetic sensor at the beginning of the bend: x begin ,y begin , z begin ,q 0begin ,q xbegin ,q ybegin and q zbegin ;

[0062] (4) The signs of the seven parameters returned by the second electromagnetic sensor at the end of the bend: x end ,y end 、z end ,q 0end ,q xbegin ,q yend and q zend ;

[0063] (5) θ represents the bending angle: the angle between the plane at the end of the bending segment and the plane xoy;

[0064] (6) φ represents the deflection angle: the positive direction of the x-axis is 0° for the bending segment, and the counterclockwise rotation is the positive direction of the angle;

[0065] (7)θ d represents the desired bending angle, φ d represents the desired deflection angle;

[0066] (8)R begin is a first rotation matrix, which is a 3×3 matrix, representing the rotation matrix of the first electromagnetic sensor relative to the earth coordinate system o-xyz;

[0067] (9)R end is the second rotation matrix, which is a 3×3 matrix, representing the rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system o-xyz end ;

[0068] (10)P begin Indicates the coordinates of the starting point of the curved segment in the geodetic coordinate system;

[0069] (11)P end Indicates the coordinates of the end of the curved segment in the geodetic coordinate system;

[0070] (12) begin p end Indicates the coordinates of the end of the curved segment relative to the starting end, with the starting end of the curved segment as the coordinate system;

[0071] (13)p end Indicates the coordinates of the end of the curved segment in the geodetic coordinate system;

[0072] (14) r0 represents the radius of the cross section of the bronchoscope;

[0073] (15) S1 represents the first virtual state variable, S2 represents the second virtual state variable;

[0074] (16)S 1d represents the expected first virtual state variable, S 2d A second virtual state variable representing the expectation;

[0075] (17) u1 is the driving amount of the first motor, u2 is the driving amount of the second motor, and the bending and deflection control of the bronchoscope is achieved by the combined action of u1 and u2;

[0076] (18) k represents the kth sampling time.

[0077] In one embodiment of the present invention, a bronchoscope bending control method based on a dual electromagnetic sensor structure is provided. Figure 1 , the method comprises the following steps:

[0078] A first electromagnetic sensor is arranged at the starting end of the curved section of the bronchoscope, and a second electromagnetic sensor is arranged at the end of the curved section of the bronchoscope; the starting end of the curved section is an end close to the control end (motor), and the end of the curved section is an end far away from the control end and firstly inserted into the human body during surgery;

[0079] Determine the starting end coordinate P according to the position parameters collected by the first electromagnetic sensor at time kbegin (k), the starting end coordinates are the coordinates of the starting end of the curved segment in the geodetic coordinate system, and a first rotation matrix R of the first electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the first electromagnetic sensor begin (k);

[0080] Determine the terminal coordinate P according to the position parameters collected by the second electromagnetic sensor at time k end (k), the end coordinates are the coordinates of the end of the curved segment in the geodetic coordinate system, and a second rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the second electromagnetic sensor end (k);

[0081] According to the first rotation matrix R begin (k), the second rotation matrix R end (k) determining the bending angle θ(k) of the bending segment at time k, and according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) determining the deflection angle φ(k) of the bending segment at time k;

[0082] According to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope d (k) and the desired deflection angle φ d (k) determining a first motor drive amount u1(k) and a second motor drive amount u2(k), and controlling the bending and deflection of the bronchoscope, i.e., the bending section, based on the first motor drive amount u1(k) and the second motor drive amount u2(k).

[0083] In this embodiment, the structure of the bronchoscope is as follows: Figure 2 and Figure 3 As shown, it includes a curved section 5 and four guide wires, wherein the curved section 5 and Figure 2 and Figure 3 The first guide wire 1, the second guide wire 2, the third guide wire 3 and the fourth guide wire 4 are connected. Figure 4 As shown, the first motor 61 pulls the first guide wire 1 and the third guide wire 3 , and the second motor 62 pulls the second guide wire 2 and the fourth guide wire 4 to control the bending and deflection of the bending section 5 .

[0084] The parameters collected by the electromagnetic sensor include position parameters [x, y, z] and quaternions [q0, q x ,q y ,q z ], the position parameter represents the spatial position of the electromagnetic sensor, and the quaternion is expressed as [q0, qx ,q y ,q z ], the quaternion represents the posture of the electromagnetic sensor.

[0085] In one embodiment of the present invention, the starting point coordinate is represented by p begin =[x begin ,y begin ,z begin ] T ,[x begin ,y begin , z begin ] is the position parameter collected by the first electromagnetic sensor. The terminal coordinate is represented by p end =[x end ,y end ,z end ] T ,[x end ,y end , z end ] is the position parameter collected by the second electromagnetic sensor.

[0086] The quaternion collected by the first electromagnetic sensor is [q 0begin ,q xbegin ,q ybegin ,q zbegin ], the first rotation matrix R of the first electromagnetic sensor relative to the earth coordinate system is determined by using the quaternion fed back by the first electromagnetic sensor in the following manner: begin , The calculation formulas for each element in the first rotation matrix are as follows:

[0087]

[0088] The quaternion collected by the second electromagnetic sensor is [q 0end ,q xbegin ,q yend ,q zend ], using the quaternion collected by the second electromagnetic sensor to determine the second rotation matrix R of the second electromagnetic sensor relative to the earth coordinate system in the following manner: end , The calculation formulas for each element in the second rotation matrix are as follows:

[0089]

[0090] It should be noted that the calculation method of the first rotation matrix and the second rotation matrix is ​​not limited to the method provided in the above embodiment. In other embodiments, other methods may also be used to determine the first rotation matrix and the second rotation matrix. For example, based on the quaternion collected by the electromagnetic sensor, the rotation matrix of the electromagnetic sensor relative to the earth coordinate system is determined according to the Hamilton right-handed rotation system. Specifically, based on the quaternion [q0, q1, q2, q2], the transformation matrix R from the coordinate system of the electromagnetic sensor to the earth coordinate system is determined according to the Hamilton right-handed rotation system. Ham The calculation formula can be expressed as:

[0091]

[0092] In one embodiment of the present invention, according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) Determine the coordinate system of the starting end of the curved segment and the coordinate system of the end of the curved segment relative to the starting end. begin P end (k) is expressed as [x b (k), y b (k), z b (k)]. begin P end The calculation formula of (k) is as follows:

[0093] begin p end (k) = R begin -1 (k)×[p end (k)-p begin (k)]

[0094] Among them, R begin -1 (k) represents R begin The inverse matrix of (k).

[0095] In this embodiment, the calculation formula of the deflection angle φ(k) of the bending segment at time k is:

[0096]

[0097] Wherein, arctan is represented as the inverse tangent function, which represents the coordinate of the end of the curved segment relative to its starting end in the x-axis direction, and y b (k) represents the coordinate of the end of the curved segment relative to its starting end in the y-axis direction.

[0098] The calculation formula of the bending angle θ(k) of the bending segment at time k is:

[0099]

[0100] Among them, |||| represents the bi-norm of the vector, R Zbegin (k) represents the component vector of the z-axis of the current coordinate system in the geodetic coordinate system, R Zbegin T (k) represents R Zbegin The transposed matrix of (k), R Zend (k) represents R end (k)'s z-axis component vector in the geodetic coordinate system. It should be noted that the first column in the first rotation matrix / the second rotation matrix represents its x-axis component vector in the geodetic coordinate system, the second column in the first rotation matrix / the second rotation matrix represents its y-axis component vector in the geodetic coordinate system, and the third column in the first rotation matrix / the second rotation matrix represents its z-axis component vector in the geodetic coordinate system.

[0101] In the deflection angle calculation method provided in this embodiment, the value range of φ(k) is -180° to 180°, and its application range is wide, not only applicable to bronchoscopes that rotate by 180°, but also applicable to bronchoscopes that rotate by 360°.

[0102] It should be noted that in other embodiments, the deflection angle φ(k) and the bending angle θ(k) may be calculated by other methods. In another embodiment, the calculation formulas for the deflection angle φ(k) and the bending angle θ(k) are as follows:

[0103]

[0104] In this calculation method, the range of φ(k) is -90° to 90°, which is narrow in scope and applicable to bronchoscopes that rotate 180°, but not 360°. Therefore, this method is generally used in theoretical derivation, because it is not a piecewise function and will not change suddenly, but it is easy to have defects in engineering applications.

[0105] In one embodiment of the present invention, according to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope, d (k) and the desired deflection angle φ d (k) determining the first motor driving amount u1(k) and the second motor driving amount u2(k), comprising the following steps:

[0106] Calculate the first virtual state quantity S1(k) and the second virtual state quantity S2(k), where S1(k)=θ(k)×r0×cos(φ(k), S2(k)=θ(k)×r0×sin(φ(k));

[0107] Calculate the expected first virtual state quantity S 1d (k) and the desired second virtual state quantity S 2d (k), where S 1d =θ d (k)×r0×cos(φ d (k)), S 2d =θ d (k)×r0×sin(φ d (k));

[0108] According to the first virtual state quantity S1(k), the second virtual state quantity S2(k) and the expected first virtual state quantity S 1d (k), the desired second virtual state quantity S 2d (k) Using PID control technology to determine the first motor driving amount u1(k) and the second motor driving amount u2(k).

[0109] Preferably, using PID control technology to determine the first motor drive amount u1(k) and the second motor drive amount u2(k) includes:

[0110]

[0111] Among them, K p1 , K p2 , K i1 , K i1 , K d1 and K d1 is the parameter of PID controller, T s represents the sampling time, e1(k) and e1(k-1) are the first error values ​​at time k and time k-1 respectively, e1(k)=S 1d The first error value is the error value between the first desired virtual state and the first virtual state, e2(k) and e2(k-1) are the second error values ​​at time k and time k-1 respectively, e2(k)=S1(k); 2d (k)-S2(k), the second error value is the error value between the expected second virtual state quantity and the second virtual state quantity.

[0112] In other embodiments of the present invention, the first motor driving amount u1(k) and the second motor driving amount u2(k) may also be determined by an incremental PID or acquired by other controllers.

[0113] It should be noted that, in this article, relational terms such as are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0114] The above is only a specific implementation of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A bronchoscope bending control method based on a dual electromagnetic sensor structure, characterized in that: The following steps are involved: A first electromagnetic sensor is arranged at the starting end of the curved section of the bronchoscope, and a second electromagnetic sensor is arranged at the end of the curved section of the bronchoscope; Determine the starting end coordinate P according to the position parameters collected by the first electromagnetic sensor at time k begin (k), the starting end coordinates are the coordinates of the starting end of the curved segment in the geodetic coordinate system, and a first rotation matrix R of the first electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the first electromagnetic sensor begin (k); Determine the terminal coordinate P according to the position parameters collected by the second electromagnetic sensor at time k end (k), the end coordinates are the coordinates of the end of the curved segment in the geodetic coordinate system, and a second rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternion collected by the second electromagnetic sensor end (k); According to the first rotation matrix R begin (k), the second rotation matrix R end (k) determining the bending angle θ(k) of the bending segment at time k, and according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) determining the deflection angle φ(k) of the bending segment at time k; According to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope d (k) and the desired deflection angle φ d (k) determining a required first motor drive amount u1(k) and a second motor drive amount u2(k), and controlling the bending and deflection of the bronchoscope based on the first motor drive amount u1(k) and the second motor drive amount u2(k).

2. The bronchoscope bending control method based on the dual electromagnetic sensor structure according to claim 1 is characterized in that: According to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ of the bronchoscope d (k) and the desired deflection angle φ d (k) determining the first motor driving amount u1(k) and the second motor driving amount u2(k), comprising the following steps: Calculate the first virtual state quantity S1(k) and the second virtual state quantity S2(k), where S1(k)=θ(k)×r0×cos(φ(k), S2(k)=θ(k)×r0×sin(φ(k)); Calculate the expected first virtual state quantity S 1d (k) and the desired second virtual state quantity S 2d (k), where S 1d =θ d (k)×r0×cos(φ d (k)), S 2d =θ d (k)×r0×sin(φ d (k)); According to the first virtual state quantity S1(k), the second virtual state quantity S2(k) and the expected first virtual state quantity S 1d (k), the desired second virtual state quantity S 2d (k) A PID controller is used to determine a first motor driving amount u1(k) and a second motor driving amount u2(k) required for the bronchoscope to reach the desired bending angle and the desired deflection angle.

3. The bronchoscope bending control method based on the dual electromagnetic sensor structure according to claim 2 is characterized in that: The PID controller is used to determine the first motor drive amount u1(k) and the second motor drive amount u2(k) required for the bronchoscope to reach a desired bending angle and a desired deflection angle, including: Among them, K p1 , K p2 , K i1 , K i1 , K d1 and K d1 is the parameter of PID controller, T s represents the sampling time, e1(k) and e1(k-1) are the first error values ​​at time k and time k-1 respectively, the first error value is the error value between the expected first virtual state quantity and the first virtual state quantity, e2(k) and e2(k-1) are the second error values ​​at time k and time k-1 respectively, the second error value is the error value between the expected second virtual state quantity and the second virtual state quantity.

4. The bronchoscope bending control method based on the dual electromagnetic sensor structure according to claim 3 is characterized in that: The calculation formulas of the first error value and the second error value at time k are: e1(k)=S 1d (k)-S1(k); e2(k)=S 2d (k)-S2(k)。 5. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 1, characterized in that: The calculation formula of the deflection angle φ(k) of the bending segment at time k is: Wherein, arctan is represented as the inverse tangent function, which represents the coordinate of the end of the curved segment relative to its starting end in the x-axis direction, and y b (k) represents the coordinate of the end of the curved segment relative to its starting end in the y-axis direction.

6. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 5, characterized in that: Also includes according to the first rotation matrix R begin (k), starting point coordinate P begin (k) and the end coordinate P end (k) Determine the coordinate system of the starting end of the curved segment and the coordinate system of the end of the curved segment relative to the starting end. begin P end (k) begin P end (k) = [x b (k), y b (k), z b (k)]; begin P end The calculation formula of (k) is: begin p end (k)=R begin -1 (k)×[p end (k)-p begin (k)] Among them, R begin -1 (k) represents R begin The inverse matrix of (k).

7. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 6, characterized in that: The parameters collected by the electromagnetic sensor include position parameters [x, y, z], which represent the spatial position of the electromagnetic sensor; The starting point coordinate P begin (k) is: p begin =[x begin ,y begin ,z begin ] T ,[x begin ,y begin , z begin ] is a position parameter collected by the first electromagnetic sensor; The end coordinate is P end (k) is: p end =[x end ,y end ,z end ] T ,[x end ,y end , z end ] is the position parameter collected by the second electromagnetic sensor.

8. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 1, characterized in that: The calculation formula of the bending angle θ(k) of the bending segment at time k is: Among them, |||| represents the second norm of the vector, R Zbegin (k) represents R begin The z-axis component of (k) in the geodetic coordinate system, R Zbegin T (k) represents R Zbegin The transposed matrix of (k), R Zend (k) represents R end (k) is the z-axis component of the vector in the geodetic coordinate system.

9. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 1, characterized in that: The parameters collected by the electromagnetic sensor include quaternions, which are expressed as [q0, q x ,q y ,q z ], the quaternion represents the posture of the electromagnetic sensor; The quaternion collected by the first electromagnetic sensor is [q 0begin ,q xbegin ,q ybegin ,q zbegin ], the first rotation matrix R of the first electromagnetic sensor relative to the earth coordinate system is determined by using the quaternion fed back by the first electromagnetic sensor in the following manner: begin , The calculation formulas for each element in the first rotation matrix are as follows: The quaternion collected by the second electromagnetic sensor is [q 0end ,q xbegin ,q yend ,q zend ], determine the second rotation matrix R of the second electromagnetic sensor relative to the earth coordinate system by using the quaternion collected by the second electromagnetic sensor in the following manner: end , The calculation formulas for each element in the second rotation matrix are as follows:

10. The bronchoscope bending control method based on a dual electromagnetic sensor structure according to claim 1, characterized in that: The parameters collected by the electromagnetic sensor include quaternions, which are expressed as [q0, q x ,q y ,q z ], the quaternion represents the posture of the electromagnetic sensor; Based on the quaternion collected by the electromagnetic sensor, a rotation matrix of the electromagnetic sensor relative to the earth coordinate system is determined according to the Hamilton right-hand rotation system.

Images