Bronchoscope bending control method based on double electromagnetic sensor structure

Abstract

The application discloses a bronchoscope bending control method based on a double electromagnetic sensor structure, first and second electromagnetic sensors are arranged at the starting end and the terminal end of the bending section of the bronchoscope respectively; the first rotation matrix, the starting end coordinate, the second rotation matrix and the terminal coordinate of the two electromagnetic sensors are determined by the parameters collected by the two electromagnetic sensors, the bending angle of the bending section is determined according to the first rotation matrix and the second rotation matrix, and the deflection angle of the bending section is determined according to the first rotation matrix, the starting end coordinate and the terminal coordinate; the required first motor driving amount and the second motor driving amount are determined 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, and are used for controlling the bending and the deflection of the bronchoscope. The application can reduce the cost of the bronchoscope and improve the control precision of the bronchoscope.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of medical devices, and particularly relates to a bronchoscope bending control method based on a double electromagnetic sensor structure. BACKGROUND

[0002] Lung cancer is one of the highest incidence of cancer, in the process of diagnosis and treatment, the doctor will first judge the tumor location according to the CT image, and manually drive the bronchoscope to reach the lesion for exploration. The current conventional bronchoscope is an open loop control directly driven by a motor, and the bending accuracy of the conventional bronchoscope will be affected by the interference of the complex structure of the human airway during the operation.

[0003] In addition, during the operation, it is easy to obtain the camera picture information at the end of the bronchoscope, and it is not easy to obtain the current deflection angle and bending angle information of the bronchoscope, so the end of the bronchoscope is easy to cause tissue damage to the patient's trachea, thereby causing complications such as pneumothorax, bleeding, etc. The existing bronchoscope on the market that provides attitude control usually uses a fiber optic sensor (FBGS) to obtain position and attitude information, and then calculates the bending angle and deflection angle of the end of the bronchoscope, which specifically includes: (1) position detection based on bronchoscope image; (2) position detection based on single electromagnetic sensor. However, the existing technical means has the following disadvantages:

[0004] (1) The sensor is high in cost, fragile in structure and short in service life;

[0005] (2) The motion image of different frames in the image is often used to position the motion trajectory of the bronchoscope, but this kind of method has cumulative error in the positioning process, in addition, the image texture shot by the bronchoscope is weak, the light is unstable, which brings great error to 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) The position detection based on single electromagnetic sensor cannot calculate the bending angle and deflection angle of the end of the bronchoscope due to the lack of a suitable reference coordinate system, and it is difficult to achieve the control of the desired bending angle and the desired deflection angle.

[0007] The disclosure of the above background art content is only used to assist in understanding the inventive concept and technical solutions of the present application, and it does not necessarily belong to the prior art of the present patent application, nor does it necessarily give technical teaching; in the absence of explicit evidence that the above content has been disclosed before the filing date of the present patent application, the above background art should not be used to evaluate the novelty and inventiveness of the present application. SUMMARY

[0008] The purpose of the present application is to provide a bronchoscope bending control method based on a double 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 scheme adopted by the present application is as follows:

[0010] A bronchoscope bending control method based on a double electromagnetic sensor structure, comprising the following steps:

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

[0012] The starting end coordinate P begin (k) of the bending section is determined according to the position parameter collected by the first electromagnetic sensor at time k, and the first rotation matrix R begin (k) of the first electromagnetic sensor relative to the geodetic coordinate system is determined according to the quaternion collected by the first electromagnetic sensor;

[0013] The terminal end coordinate P end (k) of the bending section is determined according to the position parameter collected by the second electromagnetic sensor at time k, and the second rotation matrix R end (k) of the second electromagnetic sensor relative to the geodetic coordinate system is determined according to the quaternion collected by the second electromagnetic sensor;

[0014] The bending angle θ(k) of the bending section at time k is determined according to the first rotation matrix R begin (k) and the second rotation matrix R end (k), and the deflection angle φ(k) of the bending section at time k is determined according to the first rotation matrix R begin (k), the starting end coordinate P begin (k), and the terminal end coordinate P end (k);

[0015] The first motor drive amount u1(k) and the second motor drive amount u2(k) required to make the bronchoscope reach the desired bending angle and the desired deflection angle are determined according to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ d (k) and the desired deflection angle φ d (k) of the bronchoscope, and the bending and deflection of the bronchoscope are controlled 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 technical solutions or a combination of multiple technical solutions, the first motor drive amount u1(k) and the second motor drive amount u2(k) required to make the bronchoscope reach the desired bending angle and the desired deflection angle are determined according to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the desired bending angle θ 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), comprising the steps of:

[0017] calculating a first virtual state amount S1(k) and a second virtual state amount S2(k), wherein S1(k) = θ(k) x r0 x cos(φ(k)), S2(k) = θ(k) x r0 x sin(φ(k));

[0018] calculating an expected first virtual state amount S 1d (k) and an expected second virtual state amount S 2d (k), wherein S 1d = θ d (k) x r0 x cos(φ d (k)), S 2d = θ d (k) x r0 x sin(φ d (k));

[0019] determining the first motor drive amount u1(k) and the second motor drive amount u2(k) required for the bronchoscope to reach the expected bending angle and the expected deflection angle according to the first virtual state amount S1(k), the second virtual state amount S2(k), the expected first virtual state amount S 1d (k), and the expected second virtual state amount S 2d (k) by using a PID controller.

[0020] Further, any one of the technical solutions or the combination of the technical solutions described above, the first motor drive amount u1(k) and the second motor drive amount u2(k) required for the bronchoscope to reach the expected bending angle and the expected deflection angle are determined by using a PID controller, comprising:

[0021]

[0022] wherein K p1 , K p2 , K i1 , K i1 , K d1 , and K d1 are parameters of the PID controller, T s represents a sampling time, e1(k) and e1(k-1) are respectively a first error value at a k moment and a k-1 moment, the first error value being an error value between the expected first virtual state amount and the first virtual state amount, and e2(k) and e2(k-1) are respectively a second error value at the k moment and the k-1 moment, the second error value being an error value between the expected second virtual state amount and the second virtual state amount.

[0023] Further, any one of the preceding technical solutions or a combination of the preceding technical solutions, the calculation formula of the first error value and the second error value at the k moment is:

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

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

[0026] Further, any one of the preceding technical solutions or a combination of the preceding technical solutions, the calculation formula of the deflection angle φ(k) of the curved segment at the k moment is:

[0027]

[0028] wherein arctan represents an inverse tangent function, x b (k) represents a coordinate of an end of the curved segment relative to a starting end thereof in the y axis direction.

[0029] Further, any one of the preceding technical solutions or a combination of the preceding technical solutions, further comprising determining, according to the first rotation matrix R begin (k), the starting end coordinate P begin (k), and the end coordinate P end (k), a coordinate system with the starting end of the curved segment, and the end of the curved segment relative to the coordinate of the starting end P begin P end (k), begin P end (k) = [x b (k), y b (k), z b (k) ] ;

[0030] begin P end (k), the calculation formula is:

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

[0032] wherein R begin -1 (k) represents an inverse matrix of R begin (k).

[0033] Further, any one of the above technical solutions or a combination of multiple technical solutions, the parameter collected by the electromagnetic sensor includes a position parameter [x, y, z], and the position parameter represents a spatial position of the electromagnetic sensor.

[0034] The starting end 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 a position parameter collected by the second electromagnetic sensor.

[0036] Further, any one of the above technical solutions or a combination of multiple technical solutions, the calculation formula of the bending angle θ (k) of the bending section at the k moment is:

[0037]

[0038] Wherein, ||| represents the two-norm of a vector, R Zbegin (k) represents the z-axis of R begin (k) in the geodetic coordinate system, R Zbegin T (k) represents the transpose matrix of R Zbegin (k), R Zend (k) represents the z-axis of R end (k) in the geodetic coordinate system.

[0039] Further, any one of the above technical solutions or a combination of multiple technical solutions, the parameter collected by the electromagnetic sensor includes a quaternion, and the quaternion is represented as [q0, q x , q y , q z ], and the quaternion represents the attitude of the electromagnetic sensor.

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

[0041]

[0042] The quaternion collected by the second electromagnetic sensor is [q 0end xbegin yend zend The quaternion collected by the second electromagnetic sensor is used to determine a second rotation matrix R of the second electromagnetic sensor relative to the earth coordinate system by the following method end The calculation formula of each element in the second rotation matrix is as follows:

[0043]

[0044] Further, any of the technical solutions or a combination of the technical solutions described above, the parameters collected by the electromagnetic sensor include a quaternion, the quaternion is represented as [q0, q x y z The quaternion represents the attitude of the electromagnetic sensor.

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

[0046] The technical solutions provided by the application have the following beneficial effects:

[0047] a. The application determines the attitude of the bronchoscope in real time by using the parameters collected by the two electromagnetic sensors, including the deflection angle and the bending angle of the bronchoscope, and determines the required motor driving amount by using the real-time deflection angle and bending angle and the expected deflection angle and bending angle, and controls the bronchoscope, without using expensive FBGS, which can reduce the cost of the bronchoscope and improve the control accuracy of the bronchoscope without cumulative error.

[0048] b. The first and second electromagnetic sensors are separately installed at the two ends of the bending section of the bronchoscope, which can delay the service life of the electromagnetic sensor without stretching loss caused by the bending of the bronchoscope.

[0049] ​​​​​​​​c.The present application can accurately determine the deflection angle and the bending angle of the curved section in real time, and the value range of the calculated deflection angle is-180° to 180°, which is not only suitable for 180° rotation bronchoscope, but also suitable for 360° rotation bronchoscope. BRIEF DESCRIPTION OF DRAWINGS

[0050] In order to make the technical personnel of the present application better understand the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings, obviously, the described embodiments are only a part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should belong to the scope of protection of the present application.

[0051] Figure 1 The flowchart of the bronchoscope bending control method provided for an exemplary embodiment of the present application is shown in the figure.

[0052] Figure 2 The structure diagram of the straightened bronchoscope provided for an exemplary embodiment of the present application is shown in the figure.

[0053] Figure 3 The structure diagram of the curved bronchoscope provided for an exemplary embodiment of the present application is shown in the figure.

[0054] Figure 4 The principle diagram of the double-motor control bronchoscope bending provided for an exemplary embodiment of the present application is shown in the figure.

[0055] Among them, the reference signs 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 make the technical personnel of the present application better understand the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings, obviously, the described embodiments are only a part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should belong to the scope of protection of the present application.

[0057] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments of the application only and is not intended to be limiting of the present application. For example, as used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof. As used herein, the term "and / or" includes any and all combinations of one or more of the associated listed items. Expressions such as "at least one of," when preceding a list of two or more items, denote that at least one of the listed items is present at an amount of one or more. Expressions such as "one or more of," when preceding a list of two or more items, denote that one or more, but possibly more than one, of the listed items is present. Expressions such as "one of," when preceding a list of two or more items, denote that only one of the listed items is present.

[0058] For the convenience of understanding the embodiments of the present application, the symbols appearing in the embodiments of the present application are defined as follows:

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

[0060] (2) Electromagnetic sensor returns 7 parameters in real time, including 3 position parameters x, y, z, and quaternions q0, q x , q y , q z concerning sensor attitude information;

[0061] (3) Symbols of 7 parameters returned by the first electromagnetic sensor at the starting end of the curved section: x begin , y begin , z begin , q 0begin , q xbegin , q ybegin and q zbegin ;

[0062] (4) Symbols of 7 parameters returned by the second electromagnetic sensor at the end of the curved section: x end , y end , z end , q 0end , q xbegin , q yend and q zend ;

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

[0064] (6) φ represents the deflection angle: the positive direction of the angle is counterclockwise rotation with the positive direction of the x-axis as 0°;

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

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

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

[0068] (10)P begin represents the coordinate of the starting end of the bending section in the earth coordinate system;

[0069] (11)P end represents the coordinate of the end of the bending section in the earth coordinate system;

[0070] (12) begin p end represents the coordinate of the end of the bending section relative to the starting end in the coordinate system of the starting end of the bending section;

[0071] (13)p end represents the coordinate of the end of the bending section in the earth coordinate system;

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

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

[0074] (16)S 1d represents the desired first virtual state variable, and S 2d represents the desired second virtual state variable;

[0075] (17)u1 is the first motor driving quantity, and u2 is the second motor driving quantity, which together realize the bending and deflection control of the bronchoscope;

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

[0077] In an embodiment of the present application, a bronchoscope bending control method based on a double electromagnetic sensor structure is provided, as shown in Figure 1 , the method comprising the following steps:

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

[0079] The starting end coordinate P is determined according to the position parameter collected by the first electromagnetic sensor at the k timebegin (k), the start end coordinate being a coordinate of a start end of the curved segment in a geodetic coordinate system, and the first rotation matrix R begin (k) of the first electromagnetic sensor relative to the geodetic coordinate system being determined according to a quaternion collected by the first electromagnetic sensor

[0080] (k) being determined according to a position parameter collected by the second electromagnetic sensor at the k moment end (k), the end coordinate being a coordinate of an end of the curved segment in the geodetic coordinate system, and the second rotation matrix R end (k) of the second electromagnetic sensor relative to the geodetic coordinate system being determined according to a quaternion collected by the second electromagnetic sensor

[0081] (k) being determined according to the first rotation matrix R begin (k), the second rotation matrix R end (k) begin (k), the start end coordinate P begin (k), and the end coordinate P end (k)

[0082] (k) and the deflection angle φ(k) of the bronchoscope, and the expected bending angle θ d (k) and the expected deflection angle φ d (k) of the bronchoscope, the first motor drive amount u1(k) and the second motor drive amount u2(k) are determined, and the bending and deflection of the bronchoscope, i.e. the curved segment, are controlled based on the first motor drive amount u1(k) and the second motor drive amount u2(k).

[0083] In the embodiment, the structure of the bronchoscope is as shown in Figure 2 and Figure 3 , which includes a curved segment 5 and four guide wires, the curved segment 5 being connected with a first guide wire 1, a second guide wire 2, a third guide wire 3 and a fourth guide wire 4 as shown in Figure 2 and Figure 3 . As shown in Figure 4 , the bending and deflection of the curved segment 5 are realized by pulling the first guide wire 1 and the third guide wire 3 through a first motor 61, and pulling the second guide wire 2 and the fourth guide wire 4 through a second motor 62.

[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 parameters representing the spatial position of the electromagnetic sensor, and the quaternions representing the rotation of the electromagnetic sensor.x , q y , q z ], which represents the attitude of the electromagnetic sensor.

[0085] In one embodiment of the present application, the start end coordinates are represented as p begin = [x begin , y begin , z begin ] T , [x begin , y begin , z begin ] are position parameters collected by the first electromagnetic sensor. The end coordinates are represented as p end = [x end , y end , z end ] T , [x end , y end , z end ] are position parameters collected by the second electromagnetic sensor.

[0086] The quaternion collected by the first electromagnetic sensor is [q 0begin , q xbegin , q ybegin , q zbegin ], and the first rotation matrix R begin , of the first electromagnetic sensor relative to the earth coordinate system is determined by the following way:

[0087]

[0088] The quaternion collected by the second electromagnetic sensor is [q 0end , q xbegin , q yend , q zend ], and the second rotation matrix R end , of the second electromagnetic sensor relative to the earth coordinate system is determined by the following way:

[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, and in other embodiments, other methods can 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 geodetic coordinate system is determined according to the Hamilton right-hand rotation system. Specifically, based on the quaternion [q0, q1, q2, q2], the conversion matrix R Ham of the coordinate system where the electromagnetic sensor is located to the geodetic coordinate system is determined according to the Hamilton right-hand rotation system

[0091]

[0092] In an embodiment of the present application, the starting end coordinate P begin (k) and the end coordinate P begin (k) are used to determine the coordinate system with the starting end of the curved segment as the coordinate system, and the end of the curved segment relative to the coordinate end P begin (k) of the starting end is expressed as [x end (k), y b (k), z b (k)]. b P begin (k) is calculated according to the following formula:

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

[0094] wherein R begin -1 (k) represents the inverse matrix of R begin (k).

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

[0096]

[0097] wherein arctan represents the inverse tangent function, x b (k) represents the coordinate of the end of the curved segment relative to the coordinate of the starting end in the x-axis direction, and y Zbegin (k) represents the coordinate of the end of the curved segment relative to the coordinate of the starting end in the y-axis direction.

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

[0099]

[0100] wherein || || denotes the two-norm of a vector, R Zbegin (k) denotes the partial vector of the current coordinate system z-axis on the geodetic coordinate system, R Zbegin T (k) denotes the partial vector of the current coordinate system z-axis on the geodetic coordinate system, R Zbegin (k) denotes the transpose matrix of R R Zend (k) denotes the partial vector of the current coordinate system z-axis on the geodetic coordinate system, R end (k) denotes the partial vector of the current coordinate system z-axis on the geodetic coordinate system, R It should be noted that the first list in the first rotation matrix / second rotation matrix represents the partial vector of its x-axis on the geodetic coordinate system, the second list in the first rotation matrix / second rotation matrix represents the partial vector of its y-axis on the geodetic coordinate system, and the third list in the first rotation matrix / second rotation matrix represents the partial vector of its z-axis on the geodetic coordinate system.

[0101] The deflection angle calculation method provided in the embodiment has a value range of -180° to 180° for φ(k), and is widely used, and is not only suitable for a bronchoscope with 180° rotation, but also suitable for a bronchoscope with 360° rotation.

[0102] It should be noted that in other embodiments, the deflection angle φ(k) and the bending angle θ(k) can also have other calculation methods. In another embodiment, the calculation formula of the deflection angle φ(k) and the bending angle θ(k) is as follows:

[0103]

[0104] In this calculation method, the value range of φ(k) determined is -90° to 90°, and the use range is relatively narrow, which is suitable for a bronchoscope with 180° rotation, but not suitable for a bronchoscope with 360° rotation. Therefore, this method is generally used in theoretical derivation, because it will not be abrupt if it is not a piecewise function, but it is easy to have defects in engineering application.

[0105] In one embodiment of the present application, according to the bending angle θ(k) and the deflection angle φ(k) of the bronchoscope and the expected bending angle θ d (k) and the expected deflection angle φ d (k) of the bronchoscope, the first motor driving amount u1(k) and the second motor driving amount u2(k) are determined, including the following steps:

[0106] The first virtual state quantity S1(k) and the second virtual state quantity S2(k) are calculated, wherein S1(k)=θ(k)×r0×cos(φ(k)), and S2(k)=θ(k)×r0×sin(φ(k)).

[0107] calculating the expected first virtual state quantity S 1d (k) and the expected second virtual state quantity S 2d (k), wherein S 1d = θ d (k) x r0 x cos(φ d (k)), and S 2d = θ d (k) x r0 x sin(φ d (k).

[0108] According to the first virtual state quantity S1(k), the second virtual state quantity S2(k), the expected first virtual state quantity S 1d (k), and the expected second virtual state quantity S 2d (k), the first motor driving quantity u1(k) and the second motor driving quantity u2(k) are determined by using a PID control technique.

[0109] Preferably, determining the first motor driving quantity u1(k) and the second motor driving quantity u2(k) by using a PID control technique comprises:

[0110]

[0111] wherein K p1 , K p2 , K i1 , K i1 , K d1 , and K d1 are parameters of the PID controller, T s represents a sampling time, e1(k) and e1(k-1) are respectively the first error value at the k time and the k-1 time, e1(k) = S 1d (k) - S1(k); 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 respectively the second error value at the k time and the k-1 time, e2(k) = S 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 application, the first motor driving quantity u1(k) and the second motor driving quantity u2(k) can also be determined by using an incremental PID or other controllers.

[0113] It is to be noted that, as used in this specification and the appended claims, the singular forms "a," "an" and "the" include plural referents unless the context clearly dictates otherwise. Thus, for example, reference to "a component" can include a combination of two or more components, and the term "an element" can include comparable reference to a plurality of elements. Also, as used in this specification and the appended claims, the term "or" as used in the context of "A / B or C" means any of the following: A; B; or C. Also, the term "comprising" as used in the claims should not be interpreted as being restricted to the means listed thereafter; it does not exclude other elements or steps. It is thus to be interpreted as specifying the presence of the stated features, integers, steps or components as referred to, but does not preclude the presence or addition of one or more other features, integers, steps or components, or groups thereof. Moreover, it is to be understood that the application can be carried out by specifically designed hardware- and / or software-coded means and / or connected units.

[0114] The foregoing is considered as illustrative only of the principles of the application. Further, since numerous modifications and changes will readily occur to those skilled in the art, it is not desired to limit the application to the exact construction and practice described. Accordingly, all such variations are intended to be included within the scope of the present application as defined in the claims below and their equivalents.

Claims

1. A method for controlling the bending of a bronchoscope based on a dual electromagnetic sensor structure, characterized in that, Includes the following steps: A first electromagnetic sensor is installed at the beginning of the curved section of the bronchoscope, and a second electromagnetic sensor is installed at the end of the curved section of the bronchoscope. The starting coordinate P is determined based on 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 the first rotation matrix R of the first electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternions collected by the first electromagnetic sensor. begin (k); The end coordinate P is determined based on 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 section in the geodetic coordinate system, and the second rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system is determined based on the quaternions collected by the second electromagnetic sensor. end (k); According to the first rotation matrix R begin (k), Second rotation matrix R end (k) Determine the bending angle θ(k) of the curved segment at time k, and according to the first rotation matrix R begin (k) Starting coordinates P begin (k) and end coordinates P end (k) Determine the deflection angle φ(k) of the curved segment at time k; Based on the bronchoscope's bending angle θ(k) and deflection angle φ(k), and the desired bending angle θ of the bronchoscope... d (k) and the desired deflection angle φ d (k) Determine the required first motor drive amount u1(k) and second motor drive amount u2(k), and control the bending and deflection of the bronchoscope based on the first motor drive amount u1(k) and second motor drive amount u2(k); Based on the bronchoscope's bending angle θ(k) and deflection angle φ(k), and the desired bending angle θ of the bronchoscope... d (k) and the desired deflection angle φ d (k) Determining the first motor drive quantity u1(k) and the second motor drive quantity u2(k) includes the following steps: Calculate the first virtual state variable S1(k) and the second virtual state variable S2(k), where S1(k) = θ(k) × r0 × cos(φ(k)) and 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 (k)=θ d (k)×r0×cos(φ d (k)), S 2d (k)=θ d (k)×r0×sin(φ d (k)); Based on the first virtual state variable S1(k), the second virtual state variable S2(k), and the desired first virtual state variable S... 1d (k) Expected second virtual state quantity S 2d (k) A PID controller is used to determine the first motor drive amount u1(k) and the second motor drive amount u2(k) required to make the bronchoscope reach the desired bending angle and the desired deflection angle.

2. The method for controlling the bending of a bronchoscope based on a dual electromagnetic sensor structure according to claim 1, characterized in that, The first motor drive quantity u1(k) and the second motor drive quantity u2(k) required to make the bronchoscope achieve the desired bending angle and the desired deflection angle are determined using a PID controller, including: Among them, K p1 K p2 K i1 K i2 K d1 and K d2 For the parameters of the PID controller, T s The sampling time is represented by e1(k) and e1(k-1), which 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.

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

5. The method for controlling the bending of a bronchoscope based on a dual electromagnetic sensor structure according to claim 4, characterized in that, It also includes the first rotation matrix R begin (k) Starting coordinates P begin (k) and end coordinates P end (k) Determine the coordinate system with the starting end of the curved segment as the coordinate system, and the coordinate system of the ending 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 formula for calculating (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).

6. The method for controlling the bending of a bronchoscope based on a dual electromagnetic sensor structure according to claim 5, 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 end coordinate P begin (k) is: p begin =[x begin ,y begin ,z begin ] T , [x begin y begin , z begin The position parameters are collected using 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 The position parameters are acquired using the second electromagnetic sensor.

7. The method for controlling the bending of a bronchoscope based on a dual electromagnetic sensor structure according to claim 1, characterized in that, The formula for calculating the bending angle θ(k) of the curved segment at time k is: Where |||| denotes 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 transpose of (k), R Zend (k) represents R end The z-axis component vector of (k) in the geodetic coordinate system.

8. The method for controlling the bending of a bronchoscope 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 represented as [q0, q...]. x q y q z The quaternion represents the attitude of the electromagnetic sensor; The quaternion collected by the first electromagnetic sensor is The first rotation matrix R of the first electromagnetic sensor relative to the geodetic coordinate system is determined using the quaternions fed back by the first electromagnetic sensor in the following manner. begin , The formulas for calculating each element in the first rotation matrix are as follows: The quaternion collected by the second electromagnetic sensor is The second rotation matrix R of the second electromagnetic sensor relative to the geodetic coordinate system is determined using the quaternions acquired by the second electromagnetic sensor in the following manner. end , The formulas for calculating each element in the second rotation matrix are as follows:

9. The method for controlling the bending of a bronchoscope 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 represented as [q0, q...]. x q y q z The quaternion represents the attitude of the electromagnetic sensor; Based on the quaternions collected by the electromagnetic sensor, the rotation matrix of the electromagnetic sensor relative to the geodetic coordinate system is determined according to the Hamiltonian right-hand rotation system.

Images