Six-degree-of-freedom pose estimation method, device and equipment of miniaturized instrument, and medium

Abstract

The application discloses a six-degree-of-freedom pose estimation method, device and equipment of miniaturized instruments and a medium, relates to the field of space tracking and pose estimation, and comprises the following steps: constructing a dual five-degree-of-freedom sensor assembly; the dual five-degree-of-freedom sensor assembly comprises a first five-degree-of-freedom electromagnetic sensor and a second five-degree-of-freedom electromagnetic sensor, and the dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector; a first position vector and a direction angle of the first five-degree-of-freedom electromagnetic sensor in a global coordinate system are acquired, and a second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system is acquired; based on the first position vector and the second position vector, a second vector of a measurement center of the second five-degree-of-freedom electromagnetic sensor relative to a measurement center of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system is determined; rotation transformation information corresponding to rotation of the first vector to the second vector is determined, and the six-degree-of-freedom pose of a to-be-tracked part is determined by using the rotation transformation information and the direction angle.

Description

Technical Field

[0001] This application relates to the fields of spatial tracking and pose estimation, and in particular to a method, apparatus, device and medium for six-degree-of-freedom pose estimation of miniaturized devices. Background Technology

[0002] In medical applications such as image-guided surgery, interventional radiology, and robot-assisted therapy, real-time tracking of the six-degree-of-freedom (DOF) pose of micro-instruments in three-dimensional space is a core technological prerequisite for achieving precise control. Currently, the mainstream spatial pose tracking solution in the industry is electromagnetic tracking technology, and the accompanying electromagnetic sensors are the core hardware for pose detection. Based on the degree of freedom detected, they can be divided into two categories: five-degree-of-freedom (5DOF) sensors and six-degree-of-freedom (6DOF) sensors. However, a single six-degree-of-freedom sensor requires the integration of three sets of orthogonal coils, resulting in a large physical size that cannot be integrated into micro-instruments with a diameter of only a few millimeters, such as catheters, guidewires, puncture needles, and flexible endoscopes. Five-degree-of-freedom sensors are significantly smaller in size and can meet the integration requirements of micro-instruments, but their output lacks roll angle information.

[0003] To resolve this contradiction, the concept of using two five-degree-of-freedom sensors to derive six-degree-of-freedom information has been conceptually mentioned in the industry. However, existing technical literature and publicly available information do not disclose any implementable, robust, and efficient specific algorithms, nor do they provide solutions for optimizing accuracy or suppressing noise through advanced sensor fusion. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a method, apparatus, device, and medium for six-degree-of-freedom pose estimation of miniaturized devices, which can stably and accurately obtain complete six-degree-of-freedom poses on miniaturized devices with limited physical size. The specific solution is as follows: In a first aspect, this application provides a six-degree-of-freedom pose estimation method for miniaturized devices, including: A dual 5-degree-of-freedom (D5F) sensor assembly is constructed. This assembly is fixed to the tracked portion of a target instrument and includes a first 5-degree-of-freedom electromagnetic sensor, a second 5-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second 5-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual 5-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second 5-degree-of-freedom electromagnetic sensor relative to the measurement center of the first 5-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual 5-degree-of-freedom sensor assembly. This first vector remains constant during the use of the dual 5-degree-of-freedom sensor assembly. The local coordinate system characterizes the spatial position of the sensors within the assembly and has a fixed relative position and attitude with the first 5-degree-of-freedom electromagnetic sensor. The first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system are obtained; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; Based on the first position vector and the second position vector, a second vector is determined relative to the measurement center of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system. The rotation transformation information corresponding to rotating the first vector to the second vector is determined, and the six-degree-of-freedom pose of the part to be tracked is determined using the rotation transformation information and the orientation angle.

[0005] Optionally, the six-DOF pose estimation method for the miniaturized device further includes: The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier based on a non-collinear configuration. The non-collinear configuration means that the line connecting the measurement center of the first five-degree-of-freedom electromagnetic sensor and the measurement center of the second five-degree-of-freedom electromagnetic sensor is not parallel to the expected rotation axis of the target instrument.

[0006] Optionally, obtaining the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system, includes: Based on a preset time step, the measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor are collected synchronously. The measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor includes a first position vector and orientation angle in the global coordinate system, and the measurement data corresponding to the second five-degree-of-freedom electromagnetic sensor includes a second position vector in the global coordinate system.

[0007] Optionally, determining the rotation transformation information corresponding to rotating the first vector to the second vector includes: Perform a cross product calculation on the first vector and the second vector to obtain the rotation axis that rotates the first vector to the second vector; Perform a dot product between the first vector and the second vector to obtain the rotation angle that rotates the first vector to the second vector; Based on the rotation axis and the rotation angle, and using the quaternion construction rules to generate quaternions; the quaternions are used to characterize the three-dimensional rotation transformation from a first vector in the local coordinate system to a second vector in the global coordinate system.

[0008] Optionally, determining the six-degree-of-freedom pose of the tracked part using the rotation transformation information and the orientation angle includes: The quaternion and the orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system are fused using an extended Kalman filter. The fused data is then subjected to noise removal and data deviation correction to obtain the target quaternion. The six-degree-of-freedom pose of the part to be tracked is determined based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0009] Optionally, determining the six-degree-of-freedom pose of the tracked part based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system includes: The first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system is used as the three-dimensional position reference of the part to be tracked, and the six-degree-of-freedom pose of the part to be tracked is synthesized by combining the target quaternion. The six-degree-of-freedom pose includes the three-dimensional position coordinates of the part to be tracked in the global coordinate system and three spatial rotation direction angles, namely roll angle, pitch angle and yaw angle.

[0010] Optionally, after determining the six-DOF pose of the part to be tracked using the rotation transformation information and the orientation angle, the method further includes: The six-degree-of-freedom pose is converted into a homogeneous transformation matrix, which is then used to perform pose tracking and motion control on the target device. The homogeneous transformation matrix is ​​based on the rotation matrix corresponding to the target quaternion and the translation vector corresponding to the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0011] Secondly, this application provides a six-degree-of-freedom pose estimation device for miniaturized instruments, comprising: A component construction module is used to construct a dual five-degree-of-freedom (D5F) sensor assembly. The dual five-degree-of-freedom sensor assembly is fixed to the tracked part of the target instrument and includes a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly. The first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly. The local coordinate system is used to characterize the spatial position of the sensors inside the assembly and has a fixed relative position and relative attitude with the first five-degree-of-freedom electromagnetic sensor. The data acquisition module is used to acquire the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; A vector determination module is used to determine, based on the first position vector and the second position vector, a second vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system. The pose determination module is used to determine the rotation transformation information corresponding to rotating the first vector to the second vector, and to determine the six-degree-of-freedom pose of the part to be tracked using the rotation transformation information and the orientation angle.

[0012] Thirdly, this application provides an electronic device, comprising: Memory, used to store computer programs; A processor is used to execute the computer program to implement the aforementioned six-degree-of-freedom pose estimation method for miniaturized devices.

[0013] Fourthly, this application provides a computer-readable storage medium for storing a computer program, wherein the computer program, when executed by a processor, implements the aforementioned six-degree-of-freedom pose estimation method for miniaturized devices.

[0014] In this application, a dual five-degree-of-freedom (D5DOF) sensor assembly is constructed. The dual five-degree-of-freedom sensor assembly is fixed to the tracked part of a target device and includes a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly. This first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly. The local coordinate system is used to characterize the spatial position of the sensors inside the assembly, and is related to the first five-degree-of-freedom electromagnetic sensor. The first five-degree-of-freedom electromagnetic sensor has a fixed relative position and relative attitude; the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the same global coordinate system are obtained; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; based on the first position vector and the second position vector, a second vector is determined relative to the measurement center of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system; rotation transformation information corresponding to rotating the first vector to the second vector is determined, and the six-degree-of-freedom pose of the part to be tracked is determined using the rotation transformation information and the orientation angle. As can be seen from the above, on the one hand, this application replaces a single large-size six-degree-of-freedom sensor by constructing an assembly rigidly fixed by two five-degree-of-freedom electromagnetic sensors. The two five-degree-of-freedom (DOF) sensors are significantly smaller in physical size, enabling their integration into miniature instruments such as catheters and guidewires. This overcomes the fundamental bottleneck at the hardware level that single six-degree-of-freedom (6DOF) sensors cannot be used in minimally invasive surgical instruments due to size limitations. Furthermore, this application rigidly fixes the dual five-DOF sensors and pre-calibrates the first vector of their measurement centers in the local coordinate system. Combined with the real-time acquired position vectors of the two sensors in the global coordinate system, a second vector is determined. By solving the rotation transformation information from the first vector to the second vector, the roll angle, which cannot be directly measured by the dual five-DOF sensors, is derived. Based on the roll angle, the six-DOF pose of the tracked part is determined, filling the gap in roll angle detection provided by a single five-DOF sensor. This achieves effective derivation from five-DOF detection data to complete six-DOF pose information. Simultaneously, this application achieves accurate derivation of the roll angle through the fusion calculation of rotation transformation information and the sensor's measured orientation angle, replacing the simple geometric calculation method in existing technologies. This effectively improves the accuracy of pose estimation and solves the problems of existing conceptual solutions lacking specific algorithms, being susceptible to noise interference, and having insufficient accuracy. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of this application. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0016] Figure 1 This is a flowchart of a six-degree-of-freedom pose estimation method for a miniaturized device disclosed in this application; Figure 2 This is a schematic diagram of the spatial relationship of a sensor disclosed in this application; Figure 3 This is a schematic diagram of a six-degree-of-freedom pose estimation device for a miniaturized device disclosed in this application. Figure 4 This is a schematic diagram of the structure of an electronic device disclosed in this application. Detailed Implementation

[0017] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0018] Currently, the concept of deriving six-degree-of-freedom (DOF) information using a combination of two five-DOF sensors has been conceptually mentioned in the industry. However, existing technical literature and publicly available information do not disclose a feasible, robust, and efficient specific algorithm, nor do they provide solutions for optimizing accuracy or suppressing noise through advanced sensor fusion. Therefore, this application provides a six-DOF pose estimation method for miniaturized devices, which can stably and accurately acquire complete six-DOF pose on physically limited miniature devices through an engineering-implementable technical solution.

[0019] See Figure 1 As shown, this application embodiment provides a six-degree-of-freedom pose estimation method for a miniaturized device, including: Step S11: Construct a dual five-degree-of-freedom (D5DOF) sensor assembly; the dual five-degree-of-freedom sensor assembly is fixed to the tracking part of the target device, including a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field, and the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor are rigidly fixed to the target carrier; the dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly, and the first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly; the local coordinate system is used to characterize the spatial position of the sensor inside the assembly, and has a fixed relative position and relative attitude with the first five-degree-of-freedom electromagnetic sensor.

[0020] In this embodiment, the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor can be rigidly fixed to the target carrier based on a non-collinear configuration; wherein, the non-collinear configuration means that the line connecting the measurement center of the first five-degree-of-freedom electromagnetic sensor and the measurement center of the second five-degree-of-freedom electromagnetic sensor is not parallel to the expected rotation axis of the target instrument.

[0021] like Figure 2 As shown, standard commercial electromagnetic tracking systems, such as NDI Aurora, can be used. The system includes a field generator for establishing the measurement space, a system control unit (SCU) for processing data, and a sensor interface unit (SIU) for digitizing sensor signals. The core hardware of this embodiment is the sensor assembly, including a first 5DOF electromagnetic sensor (sensor 1) and a second 5DOF electromagnetic sensor (sensor 2). The two sensors can be fixed in a fixed spatial relationship to a rigid, non-metallic, non-conductive substrate or fixture, such that the displacement vector from the measurement center of sensor 1 to the measurement center of sensor 2 is a known, constant vector in the local coordinate system of the assembly. This vector is defined as the baseline vector V. local That is, the first vector.

[0022] It should be noted that in this embodiment, one of the 5DOF sensors can also be replaced with a simpler 3DOF (position only) sensor, i.e., a "5DOF + 3DOF" combination. The cost and size of a 3DOF position sensor are typically much smaller than a 5DOF sensor, and this combination can achieve extreme cost compression in applications requiring only a single vector to determine the roll angle. Furthermore, based on the dual 5DOF sensor system in this embodiment, a miniature IMU (Inertial Measurement Unit) can be integrated. During rapid movement or rotation, the IMU can fill the attitude gaps between NDI (Network Device Interface) data frames, achieving extremely smooth and low-latency tracking. When the NDI sensor is briefly obstructed, the IMU can perform dead reckoning based on the last position and attitude, maintaining attitude output for several hundred milliseconds until the NDI sensor signal recovers. The NDI system then continuously corrects the IMU's accumulated drift over a long period.

[0023] Step S12: Obtain the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space.

[0024] In this embodiment, obtaining the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system, may include: synchronously acquiring measurement data corresponding to the first and second five-degree-of-freedom electromagnetic sensors based on a preset time step; wherein, the measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor includes the first position vector and orientation angle in the global coordinate system, and the measurement data corresponding to the second five-degree-of-freedom electromagnetic sensor includes the second position vector in the global coordinate system.

[0025] For example, at each time step t, the SCU provides two synchronized 5DOF measurement data packets: a position vector P1=(x1,y1,z1) and an orientation angle (pitch1,yaw1) from sensor 1; and a position vector P2=(x2,y2,z2) and an orientation angle (pitch2,yaw2) from sensor 2. Sensor 1 can be designated as the master sensor, and its position and partial orientation will form the basis of the final 6DOF pose.

[0026] Step S13: Based on the first position vector and the second position vector, determine the second vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0027] In the global (field generator) coordinate system, a global vector, also known as the second vector, can be constructed using the position vector measured in step S12: V global =P2-P1.

[0028] Step S14: Determine the rotation transformation information corresponding to rotating the first vector to the second vector, and use the rotation transformation information and the direction angle to determine the six-degree-of-freedom pose of the part to be tracked.

[0029] In this embodiment, determining the rotation transformation information corresponding to rotating the first vector to the second vector may include: calculating the cross product of the first and second vectors to obtain the rotation axis that rotates the first vector to the second vector; calculating the dot product of the first and second vectors to obtain the rotation angle that rotates the first vector to the second vector; and generating a quaternion based on the rotation axis and rotation angle using quaternion construction rules; the quaternion is used to characterize the three-dimensional rotation transformation from the first vector in the local coordinate system to the second vector in the global coordinate system.

[0030] For example, the rotation axis can be obtained by calculating the cross product of the first vector and the second vector: ; The rotation angle can be calculated using the dot product: .

[0031] Furthermore, determining the six-degree-of-freedom pose of the tracked part using rotation transformation information and orientation angles can include: fusing the quaternion and the orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system using an extended Kalman filter, removing noise and correcting data deviations in the fused data to obtain the target quaternion. Then, the six-degree-of-freedom pose of the tracked part is determined based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0032] It should be noted that by fusing the calculated quaternion with the pitch and yaw angles measured by sensor 1 using an extended Kalman filter, a final, more accurate orientation quaternion can be generated. The final 6DOF pose of the tracked part is represented by the position P1 of sensor 1 and the final orientation quaternion.

[0033] The determination of the six-degree-of-freedom pose of the tracked part based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system may include: using the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system as the three-dimensional position reference of the tracked part, and combining it with the target quaternion to synthesize the six-degree-of-freedom pose of the tracked part; wherein the six-degree-of-freedom pose includes the three-dimensional position coordinates of the tracked part in the global coordinate system and three spatial rotation direction angles, the three spatial rotation direction angles including roll angle, pitch angle and yaw angle.

[0034] After determining the six-degree-of-freedom pose of the part to be tracked using rotation transformation information and orientation angle, the process may further include: converting the six-degree-of-freedom pose into a homogeneous transformation matrix, so as to use the homogeneous transformation matrix to perform pose tracking and motion control on the target device; the homogeneous transformation matrix is ​​based on the rotation matrix corresponding to the target quaternion and the translation vector corresponding to the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0035] In this embodiment, the final six-degree-of-freedom pose can be converted into a 4×4 homogeneous transformation matrix for use in standard surgical navigation or robotic software platforms.

[0036] As can be seen from the above, this embodiment successfully solves the problems of traditional single six-degree-of-freedom sensors being too large to be integrated into micro-devices and single five-degree-of-freedom sensors lacking roll angles and unable to provide complete pose information by designing a rigid combination of dual five-degree-of-freedom electromagnetic sensors and using a dedicated pose estimation algorithm. This embodiment employs a miniaturized five-degree-of-freedom (DOF) sensor architecture at the hardware level, perfectly adapting to the spatial integration requirements of miniaturized and flexible instruments such as catheters and guidewires. The dual sensors can be flexibly arranged according to the shape of the tracked instrument, significantly improving the freedom of product design and the efficiency of customization and miniaturization. At the algorithm level, a concrete, robust, and computationally efficient six-degree-of-freedom (DOF) pose derivation scheme is proposed. The roll angle is calculated through the rotational transformation relationship between a local fixed baseline vector and a global real-time relative vector. An extended Kalman filter is used to fuse and optimize the calculated data with the sensor's measured data, effectively suppressing noise and improving the accuracy and stability of pose estimation. Furthermore, the physical baseline can be flexibly set to a larger size, resulting in roll angle measurement accuracy and stability under high-speed rotation / interference scenarios superior to traditional single-unit six-degree-of-freedom sensors. In addition, the solution in this embodiment has a cost advantage, replacing an expensive single-unit six-degree-of-freedom sensor with two low-cost five-degree-of-freedom sensors. It achieves real-time, high-precision output of complete six-degree-of-freedom pose solely through algorithm optimization without adding complex hardware modules, providing high-precision and high-stability miniature instrument tracking capabilities for image-guided surgery, interventional radiology, and other fields.

[0037] See Figure 3As shown in the embodiments, this application also discloses a six-degree-of-freedom pose estimation device for miniaturized instruments, comprising: Component construction module 11 is used to construct a dual five-degree-of-freedom (D5DOF) sensor assembly. The dual five-degree-of-freedom sensor assembly is fixed to the tracked part of the target device and includes a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly. The first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly. The local coordinate system is used to characterize the spatial position of the sensors inside the assembly and has a fixed relative position and relative attitude with the first five-degree-of-freedom electromagnetic sensor. The data acquisition module 12 is used to acquire the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; The vector determination module 13 is used to determine, based on the first position vector and the second position vector, a second vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system. The pose determination module 14 is used to determine the rotation transformation information corresponding to rotating the first vector to the second vector, and to determine the six-degree-of-freedom pose of the part to be tracked using the rotation transformation information and the orientation angle.

[0038] In some specific embodiments, the six-DOF pose estimation device for the miniaturized device further includes: A sensor fixing unit is used to rigidly fix the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor to the target carrier based on a non-collinear configuration. The non-collinear configuration means that the line connecting the measurement center of the first five-degree-of-freedom electromagnetic sensor and the measurement center of the second five-degree-of-freedom electromagnetic sensor is not parallel to the expected rotation axis of the target instrument.

[0039] In some specific embodiments, the data acquisition module 12 includes: The data acquisition unit is used to synchronously acquire measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor based on a preset time step. The measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor includes a first position vector and orientation angle in the global coordinate system, and the measurement data corresponding to the second five-degree-of-freedom electromagnetic sensor includes a second position vector in the global coordinate system.

[0040] In some specific embodiments, the pose determination module 14 includes: A rotation axis determination unit is used to perform a cross product calculation on the first vector and the second vector to obtain the rotation axis that rotates the first vector to the second vector; The rotation angle determination unit is used to perform a dot product calculation on the first vector and the second vector to obtain the rotation angle that rotates the first vector to the second vector; The quaternion generation unit is used to generate quaternions based on the rotation axis and the rotation angle, and using quaternion construction rules; the quaternion is used to characterize the three-dimensional rotation transformation from a first vector in the local coordinate system to a second vector in the global coordinate system.

[0041] In some specific embodiments, the pose determination module 14 includes: The data processing unit is used to fuse the quaternion and the orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system using an extended Kalman filter, and to remove noise and correct data deviations in the fused data to obtain the target quaternion. The pose determination submodule is used to determine the six-degree-of-freedom pose of the part to be tracked based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0042] In some specific embodiments, the pose determination submodule includes: The pose determination unit is used to use the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system as the three-dimensional position reference of the part to be tracked, and to combine the target quaternion to synthesize the six-degree-of-freedom pose of the part to be tracked. The six-degree-of-freedom pose includes the three-dimensional position coordinates of the part to be tracked in the global coordinate system and three spatial rotation direction angles, namely roll angle, pitch angle and yaw angle.

[0043] In some specific embodiments, the pose determination module 14 further includes: The application unit is used to convert the six-degree-of-freedom pose into a homogeneous transformation matrix, so as to use the homogeneous transformation matrix to perform pose tracking and motion control on the target device; the homogeneous transformation matrix is ​​based on the rotation matrix corresponding to the target quaternion and the translation vector corresponding to the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

[0044] Furthermore, embodiments of this application also disclose an electronic device, Figure 4 This is a structural diagram of an electronic device 20 according to an exemplary embodiment. The content of the diagram should not be construed as limiting the scope of this application.

[0045] Figure 4 This is a schematic diagram of the structure of an electronic device 20 provided in an embodiment of this application. Specifically, the electronic device 20 may include: at least one processor 21, at least one memory 22, a power supply 23, a communication interface 24, an input / output interface 25, and a communication bus 26. The memory 22 stores a computer program, which is loaded and executed by the processor 21 to implement the relevant steps in the six-degree-of-freedom pose estimation method for miniaturized devices disclosed in any of the foregoing embodiments. Alternatively, the electronic device 20 in this embodiment may specifically be an electronic computer.

[0046] In this embodiment, the power supply 23 is used to provide operating voltage for each hardware device on the electronic device 20; the communication interface 24 can create a data transmission channel between the electronic device 20 and external devices, and the communication protocol it follows can be any communication protocol applicable to the technical solution of this application, and is not specifically limited here; the input / output interface 25 is used to acquire external input data or output data to the outside world, and its specific interface type can be selected according to specific application needs, and is not specifically limited here.

[0047] In addition, the memory 22, as a carrier for resource storage, can be a read-only memory, random access memory, disk or optical disk, etc. The resources stored thereon can include operating system 221, computer program 222, etc., and the storage method can be temporary storage or permanent storage.

[0048] The operating system 221 is used to manage and control the various hardware devices on the electronic device 20 and the computer program 222, which may be Windows Server, Netware, Unix, Linux, etc. In addition to including a computer program capable of performing the six-degree-of-freedom pose estimation method for the miniaturized device executed by the electronic device 20 as disclosed in any of the foregoing embodiments, the computer program 222 may further include computer programs capable of performing other specific tasks.

[0049] Furthermore, this application also discloses a computer-readable storage medium for storing a computer program; wherein, when the computer program is executed by a processor, it implements the aforementioned six-degree-of-freedom pose estimation method for miniaturized devices. Specific steps of this method can be found in the corresponding content disclosed in the foregoing embodiments, and will not be repeated here.

[0050] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to in the method section.

[0051] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0052] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly by hardware, a software module executed by a processor, or a combination of both. The software module can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

[0053] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0054] The technical solutions provided in this application have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this application. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A six-degree-of-freedom pose estimation method for a miniaturized device, characterized in that, include: Construct a dual five-degree-of-freedom sensor assembly; The dual five-degree-of-freedom sensor assembly is fixed to the tracking part of the target device, including a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly. The first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly. The local coordinate system is used to characterize the spatial position of the sensors inside the assembly and has a fixed relative position and relative attitude with the first five-degree-of-freedom electromagnetic sensor. The first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system are obtained; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; Based on the first position vector and the second position vector, a second vector is determined relative to the measurement center of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system. The rotation transformation information corresponding to rotating the first vector to the second vector is determined, and the six-degree-of-freedom pose of the part to be tracked is determined using the rotation transformation information and the orientation angle.

2. The six-degree-of-freedom pose estimation method for miniaturized devices according to claim 1, characterized in that, Also includes: The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier based on a non-collinear configuration. The non-collinear configuration means that the line connecting the measurement center of the first five-degree-of-freedom electromagnetic sensor and the measurement center of the second five-degree-of-freedom electromagnetic sensor is not parallel to the expected rotation axis of the target instrument.

3. The six-degree-of-freedom pose estimation method for miniaturized devices according to claim 1, characterized in that, The step of obtaining the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system, includes: Based on a preset time step, the measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor and the second five-degree-of-freedom electromagnetic sensor are collected synchronously. The measurement data corresponding to the first five-degree-of-freedom electromagnetic sensor includes a first position vector and orientation angle in the global coordinate system, and the measurement data corresponding to the second five-degree-of-freedom electromagnetic sensor includes a second position vector in the global coordinate system.

4. The six-degree-of-freedom pose estimation method for miniaturized devices according to claim 1, characterized in that, The step of determining the rotation transformation information corresponding to the first vector and the second vector includes: Perform a cross product calculation on the first vector and the second vector to obtain the rotation axis that rotates the first vector to the second vector; Perform a dot product between the first vector and the second vector to obtain the rotation angle that rotates the first vector to the second vector; Based on the rotation axis and the rotation angle, and using the quaternion construction rules to generate quaternions; the quaternions are used to characterize the three-dimensional rotation transformation from a first vector in the local coordinate system to a second vector in the global coordinate system.

5. The six-degree-of-freedom pose estimation method for miniaturized devices according to claim 4, characterized in that, The process of determining the six-degree-of-freedom pose of the part to be tracked using the rotation transformation information and the orientation angle includes: The quaternion and the orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system are fused using an extended Kalman filter. The fused data is then subjected to noise removal and data deviation correction to obtain the target quaternion. The six-degree-of-freedom pose of the part to be tracked is determined based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

6. The six-degree-of-freedom pose estimation method for miniaturized devices according to claim 5, characterized in that, The step of determining the six-degree-of-freedom pose of the tracked part based on the target quaternion and the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system includes: The first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system is used as the three-dimensional position reference of the part to be tracked, and the six-degree-of-freedom pose of the part to be tracked is synthesized by combining the target quaternion. The six-degree-of-freedom pose includes the three-dimensional position coordinates of the part to be tracked in the global coordinate system and three spatial rotation direction angles, namely roll angle, pitch angle and yaw angle.

7. The six-degree-of-freedom pose estimation method for miniaturized devices according to any one of claims 1 to 6, characterized in that, After determining the six-degree-of-freedom pose of the part to be tracked using the rotation transformation information and the orientation angle, the method further includes: The six-degree-of-freedom pose is converted into a homogeneous transformation matrix, which is then used to perform pose tracking and motion control on the target device. The homogeneous transformation matrix is ​​based on the rotation matrix corresponding to the target quaternion and the translation vector corresponding to the first position vector of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system.

8. A six-degree-of-freedom pose estimation device for a miniaturized instrument, characterized in that, include: The component building module is used to build dual five-degree-of-freedom sensor components; The dual five-degree-of-freedom sensor assembly is fixed to the tracking part of the target device, including a first five-degree-of-freedom electromagnetic sensor, a second five-degree-of-freedom electromagnetic sensor, and a target carrier that does not interfere with the electromagnetic field. The first and second five-degree-of-freedom electromagnetic sensors are rigidly fixed to the target carrier. The dual five-degree-of-freedom sensor assembly is pre-calibrated with a first vector, which is the position vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the local coordinate system of the dual five-degree-of-freedom sensor assembly. The first vector remains constant during the use of the dual five-degree-of-freedom sensor assembly. The local coordinate system is used to characterize the spatial position of the sensors inside the assembly and has a fixed relative position and relative attitude with the first five-degree-of-freedom electromagnetic sensor. The data acquisition module is used to acquire the first position vector and orientation angle of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system, and the second position vector of the second five-degree-of-freedom electromagnetic sensor in the global coordinate system; the orientation angle includes pitch angle and yaw angle, and the global coordinate system is used to characterize the spatial position of the dual five-degree-of-freedom sensor assembly in the three-dimensional measurement space; A vector determination module is used to determine, based on the first position vector and the second position vector, a second vector of the measurement center of the second five-degree-of-freedom electromagnetic sensor relative to the measurement center of the first five-degree-of-freedom electromagnetic sensor in the global coordinate system. The pose determination module is used to determine the rotation transformation information corresponding to rotating the first vector to the second vector, and to determine the six-degree-of-freedom pose of the part to be tracked using the rotation transformation information and the orientation angle.

9. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor for executing the computer program to implement the six-degree-of-freedom pose estimation method for a miniaturized device as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Used to store a computer program, which, when executed by a processor, implements the six-degree-of-freedom pose estimation method for a miniaturized device as described in any one of claims 1 to 7.

Images