A real-time solution method for endoscope posture roll angle based on FPGA

Abstract

The application discloses a real-time solution method for an endoscope posture horizontal roll angle based on FPGA, relates to the technical field of inertial navigation and posture measurement, and discloses the FPGA hardware pipeline architecture of the application, which eliminates the uncertainty caused by software scheduling and interruption, fixes the solution delay to several clock periods, stabilizes the phase lag, and is beneficial to high-bandwidth control; acceleration solution and angular velocity integration are executed in parallel, the sampling frequency can reach several kHz without increasing the calculation load, and the throughput is much higher than that of a serial processor; through customization of fixed-point digit width (Q1.14, Q9.6, Q10.6) and shift addition / subtraction chains instead of multipliers, the power consumption is significantly reduced compared with a DSP at the same sampling rate, and the application can run on a low-end FPGA.

Description

Technical Field

[0001] This invention relates to the field of medical endoscope image processing and inertial attitude measurement technology, and in particular to a real-time calculation method for the roll angle of an endoscope based on FPGA. Background Technology

[0002] In modern medical surgical endoscopes (such as gastroscopes, colonoscopes, and laparoscopes) and industrial pipeline endoscope systems, real-time lens attitude perception is crucial for automatic image stabilization, horizontal correction, and 3D reconstruction. Among these, the precise calculation of the roll angle directly determines whether the endoscopic output image will exhibit dizzying rotation, and is a core parameter for maintaining the operator's "head-up" visual habit.

[0003] In existing endoscopic attitude measurement systems, roll angle calculation is primarily implemented using software programs written by a microprocessor (such as an MCU, DSP, or ARM). This approach typically employs a serial execution software architecture, with the core processing flow as follows: First, the microprocessor sequentially reads the three-axis acceleration and three-axis angular velocity data from a miniature IMU (Inertial Measurement Unit) sensor via I2C or SPI bus using interrupts or polling, completing serial data acquisition. Then, a C / C++ mathematical operation program runs within the processor core, calculating the roll angle based on acceleration and integrating the angular velocity. Subsequently, the processor calls the floating-point unit in the mathematical library to execute Kalman filtering or complementary filtering algorithms, fusing the two data sets and outputting the final roll angle value to drive the backend image processor (ISP) for real-time image rotation compensation. All calculations are completed under the operating system's task scheduling or main loop, sharing CPU resources with other system tasks (such as communication and video encoding / decoding).

[0004] This solution, based on a general-purpose microprocessor (software implementation), has the following significant drawbacks in high-performance endoscope posture control and real-time image correction scenarios: Video tearing is caused by latency jitter: Because microprocessors use a serial execution method with instruction pipelines and are affected by interrupt responses, task switching, and system scheduling, the delay time from acquiring sensor data to outputting the angle is not completely fixed. When the endoscope rotates or jitters rapidly, this minute latency jitter can cause phase lag in the backend image rotation correction, leading to video tearing or visual lag, and exacerbating the dizziness of the operating physician.

[0005] Computational load limits sampling frequency: Processing high-order filtering algorithms consumes a significant number of CPU clock cycles. Increasing the IMU sampling frequency to capture high-frequency hand vibrations would drastically increase the processor load, forcing developers to simplify the algorithm and thus sacrificing dynamic correction accuracy.

[0006] Misalignment of multi-source data introduces spatial errors: Microprocessors typically require CPU intervention when reading IMU data, making true parallel synchronous acquisition impossible. In multi-sensor fusion, the time difference in data arrival introduces additional alignment errors, reducing the accuracy of static measurements by the endoscope.

[0007] High power consumption and heat generation: The space at the tip of the endoscope probe is extremely limited and the heat dissipation requirements are stringent (the surface temperature rise of medical-grade probes is strictly limited). In order to complete complex trigonometric functions and multiplications, the microprocessor frequently accesses memory, resulting in power consumption and heat generation far exceeding that of dedicated hardware circuits, which can easily lead to probe overheating.

[0008] Therefore, there is an urgent need for a real-time method for calculating the roll angle of the endoscope attitude that can eliminate delay jitter, improve sampling throughput, consume extremely low power and have no risk of overheating. Summary of the Invention

[0009] The purpose of this section is to outline some aspects of the embodiments of the present invention and to briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section, as well as in the abstract and title of the present application, to avoid obscuring the purpose of this section, the abstract and title of the invention. Such simplifications or omissions shall not be used to limit the scope of the present invention.

[0010] In view of the aforementioned existing problems, the present invention is proposed.

[0011] To solve the above-mentioned technical problems, the present invention provides the following technical solution: The data synchronization acquisition module inside the FPGA reads the raw data from the accelerometer and gyroscope inside the endoscope probe through the bus interface, and converts the raw data into acceleration and angular velocity values ​​in the first fixed-point format according to the preset range.

[0012] The accelerometer module inside the FPGA uses the CORDIC computation IP core to calculate the first roll angle corresponding to the acceleration value. At the same time, the angular velocity integral module calculates the second roll angle based on the sum of the product of the output roll angle of the previous cycle, the current angular velocity value, and the sampling period. The two modules are executed in parallel in a pipeline manner.

[0013] The normalization correction module calculates the difference between the first roll angle and the second roll angle, determines whether the difference exceeds 180 degrees or is less than -180 degrees, and if so, adds or subtracts 360 degrees to the difference and outputs the shortest path deviation value.

[0014] The complementary filtering module multiplies the shortest path deviation value with a preset weight coefficient, and then adds the product to the second roll angle to obtain the final endoscope roll angle, which is then output to the image processing unit for visual horizontal correction.

[0015] As a preferred embodiment of the present invention, the data synchronization acquisition module uses a state machine to drive the IIC or SPI bus, converts the accelerometer data into Q1.14 format, and converts the gyroscope data into angular velocity through a shift addition / subtraction chain.

[0016] As a preferred embodiment of the present invention, the acceleration calculation module calculates arctan2 through the CORDIC IP core and uses a shift addition / subtraction chain to perform multiplication operations from radians to angles, converting the angle range into a Q9.6 fixed-point number of [0°, 360°].

[0017] In a preferred embodiment of the present invention, the angular velocity integral module assigns the output of the acceleration solution module to the initial integral value during the first calculation, thereby achieving zero-delay initial alignment.

[0018] In a preferred embodiment of the present invention, the normalization correction module determines whether the deviation exceeds ±180° using a comparator and performs addition / subtraction of 360° correction.

[0019] In a preferred embodiment of the present invention, the complementary filtering module transforms the filtering formula into:

[0020] in, This is the final output roll angle. The second roll angle is output by the angular velocity integral module. These are the weighting coefficients for the gyroscope channels in the complementary filter. This is the shortest path deviation value output by the normalization correction module.

[0021] As a preferred embodiment of the present invention, each module is isolated by an inter-stage register to form a pipelined operation triggered by a single clock cycle.

[0022] The beneficial effects of this invention are as follows: The FPGA hardware pipeline architecture eliminates the uncertainty caused by software scheduling and interrupts, the calculation delay is fixed at a few clock cycles, the phase lag is stable, which is beneficial for high bandwidth control; the acceleration calculation and angular velocity integration are executed in parallel, the sampling frequency can reach several kHz without increasing the computational load, and the throughput is much higher than that of serial processors; by using customized fixed-point bit widths (Q1.14, Q9.6, Q10.6) and shift-add / subtract chain to replace multipliers, the power consumption is significantly reduced compared to DSP at the same sampling rate, and it can run on low-end FPGAs; the acceleration angle is forcibly assigned to the initial value of the integral during the first calculation, eliminating the startup ramp-up process of the complementary filter and enabling it to be used immediately upon power-on; the circumferential topology normalization correction mechanism ensures that there is no abrupt output at the 0° / 360° boundary of the angle, avoiding control loop oscillation. Attached Figure Description

[0023] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein: Figure 1 This is an architecture diagram of a real-time calculation method for the roll angle of an endoscope based on FPGA, as shown in this invention. Figure 2 This is a diagram defining the coordinate system of the endoscope body used in this invention (left-handed coordinate system: X-axis forward, Y-axis to the right, Z-axis upward, and roll angle is the angle of rotation around the X-axis). Figure 3 This is a schematic diagram of the CORDIC and shift-add chain of the acceleration calculation module in this invention; Figure 4 This is a schematic diagram of the pipeline and initial value loading logic of the angular velocity integration module in this invention; Figure 5 This is a schematic diagram of the circumferential topology comparator logic of the normalization correction module in this invention; Figure 6 This is a schematic diagram of the single multiplier fusion structure of the complementary filtering module in this invention. Detailed Implementation

[0024] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0025] Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without inventive effort should fall within the scope of protection of this invention.

[0026] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0027] According to an embodiment of the present invention, in combination Figure 1 The overall architecture diagram shown and Figure 2The coordinate system of the endoscope body is defined as follows. The left-handed coordinate system is defined as follows: the positive X-axis points in the direction of the endoscope lens's forward line of sight, coinciding with the central longitudinal axis of the endoscope body; the Y-axis points to the right of the endoscope body plane; the Z-axis is perpendicular to the endoscope body plane and points upwards; the endoscope roll angle is the angle through which the endoscope body rotates around its central longitudinal axis, with right roll being positive, and a range of -180° to 180°. This invention provides a real-time calculation method for the endoscope attitude roll angle based on FPGA, comprising the following steps: S1: The data synchronization acquisition module in the FPGA reads the raw data from the accelerometer and gyroscope through the bus interface, and converts the raw data into acceleration and angular velocity values ​​in the first fixed-point number format according to the preset range.

[0028] Specifically, the data synchronization acquisition module uses a state machine to drive the IIC controller (or other control buses such as SPI) to initialize and configure the IMU chip after power-on, and to periodically read the accelerometer data after the configuration is complete and the IMU is working normally. ) and gyroscope ( The original 16-bit two's complement data.

[0029] Due to the compact space at the endoscope tip and the high cost of logic resources and power consumption for FPGA to directly process floating-point numbers, this module adjusts the accelerometer based on its range. and The 16-bit two's complement data is converted to a 16-bit signed fixed-point number, using the 16-bit word-length signed fixed-point number format Q1.14, where the sign bit occupies 1 bit, the integer bit occupies 1 bit, and the fractional bit occupies 14 bits.

[0030] At the same time, the gyroscope The original 16-bit two's complement data is converted according to the formula:

[0031] in, This is the original 16-bit binary two's complement data output by the gyroscope. The sensitivity coefficient of the gyroscope is 16.4 in this invention.

[0032] In the FPGA implementation, 1 / 16.4 is converted to 3996 using 16-bit fixed-point conversion. Therefore, the conversion formula is:

[0033] By decomposing the fixed coefficients that are not powers of 2 into the algebraic sum of multiple powers of 2 terms, and using the shifter and adder links inside the FPGA to replace the general DSP multiplication unit, we can obtain 3996 = 4096 - 64 - 32 - 4.

[0034] FPGA implements multiplication through a multi-stage pipeline of shifters and adders, and then right-shifts the result by 16 bits to obtain the result. Its format is a 16-bit signed fixed-point number format Q9.6 (1 sign bit, 9 integer bits, and 6 decimal bits).

[0035] As can be seen, the above steps achieve parallel synchronous acquisition and efficient localization of sensor data. Compared with the polling or DMA methods of microprocessors, the I / O latency is fixed and can be parallelized in multiple paths, fundamentally avoiding alignment errors introduced by the time difference in data arrival. Simultaneously, replacing multipliers with shift-add / subtract chain significantly reduces dynamic power consumption, offering a marked reduction in power consumption compared to DSP multiplication operations.

[0036] S2: The accelerometer module inside the FPGA uses the CORDIC computation IP core to calculate the first roll angle corresponding to the acceleration value. At the same time, the angular velocity integral module calculates the second roll angle based on the sum of the product of the output roll angle of the previous cycle and the current angular velocity value and the sampling period. The two modules are executed in parallel in a pipeline manner.

[0037] like Figure 3 and Figure 4 As shown, this parallel architecture eliminates the cumulative latency of traditional software serial execution.

[0038] Specifically, the acceleration calculation module calculates the roll angle based on acceleration using the following formula:

[0039] in,( ) represents the acceleration values ​​of the IMU sensor along the Y and Z axes.

[0040] The FPGA calculates arctan2 by calling the CORDIC computation IP core and outputs the roll angle in radians. Convert radians to degrees using the following formula:

[0041] In the FPGA implementation, 180 / π is processed into a 16-bit fixed-point form, so the calculation formula is transformed as follows:

[0042] By decomposing the fixed coefficients that are not powers of 2 into the algebraic sum of multiple powers of 2 terms, and using the shifter and adder links inside the FPGA to replace the general DSP multiplication unit, we can obtain 3754936 = 4194304 - 524288 + 65536 + 16384 + 4096 - 1024 - 64 - 8.

[0043] Furthermore, the FPGA implements multiplication through a multi-stage pipeline of shifters and adders, and shifts the result 16 bits to the right to obtain the roll angle value based on acceleration.

[0044] To maintain bit width consistency, this invention converts the angle value into a 16-bit signed fixed-point number format Q9.6 (1 sign bit, 9 integer bits, and 6 decimal bits) through a truncation operation.

[0045] The calculated roll angle range based on acceleration is [-180°, 180°]. To avoid negative values ​​in subsequent calculations, the roll angle range is converted to [0°, 360°]. In the FPGA implementation, the sign bit of the roll angle is checked. If the sign bit is 1 (indicating the current angle value is in the third or fourth quadrant), 360 is added before outputting; otherwise, the output is buffered. This results in a final roll angle range of [0°, 360°]. The format is Q9.6.

[0046] Furthermore, the angular velocity integration module specifically includes: Calculate the roll angle based on angular velocity using the following formula:

[0047] Specifically, firstly, the gyroscope output from the data synchronization acquisition module... The original 16-bit two's complement data is obtained according to the method in S1 above. (Format Q9.6), this invention will use the formula in After undergoing 16-bit fixed-point processing, the signal is used as the input signal. The FPGA then calculates the... and The result is obtained by multiplying using a multiplier and then shifting the result 16 bits to the right. The roll angle predicted by angular velocity is calculated using the following formula. :

[0048] in, The roll angle was previously calculated using the method of this invention; wherein, The sampling period has been processed into a 16-bit fixed-point form within the FPGA and used as the input signal.

[0049] It should be noted that if this is the first calculation, the result will be obtained from the acceleration value. value assigned to The above operations resolve the issue of severe jitter or slow ramp-up during the initial startup phase. By directly assigning values, the initial steady-state error is instantly eliminated, allowing subsequent filters to focus solely on dynamic correction rather than initial alignment.

[0050] It should be noted that the initialization trigger logic establishes an absolute connection between the sensor coordinate system and the geographic gravity coordinate system within the first effective sampling period after the hardware reset, providing a zero-deviation starting phase for subsequent incremental integration.

[0051] Final calculation The format is a 17-bit signed fixed-point number format Q10.6 (1 sign bit, 10 integer bits, and 6 decimal bits). Note that the integer bits are expanded to 10 bits here. During the FPGA internal processing, a temporary bit-width extension technique is used for intermediate variables, and saturation truncation is performed in the final output stage.

[0052] It should be noted that the two calculation modules execute in parallel, and each module employs a multi-stage pipeline, enabling the FPGA to receive a new set of data every clock cycle, achieving a throughput several orders of magnitude higher than that of a microprocessor. The initial calculation assignment directly eliminates the initial alignment error, and the transient response speed of this hardware logic is far superior to the convergence process of the software filter.

[0053] The initialization trigger logic is executed within the first valid sampling period after a hardware reset.

[0054] S3: The normalization correction module calculates the difference between the first roll angle and the second roll angle, determines whether the difference exceeds 180 degrees or is less than -180 degrees, and if so, adds or subtracts 360 degrees to the difference and outputs the shortest path deviation value.

[0055] like Figure 5 As shown, when the endoscope rotates 360° during diagnostic and treatment procedures, it is very easy to touch the 0° / 360° boundary. The normalization correction module uses an angle difference compensation mechanism based on circular topology to calculate the deviation:

[0056] The core logic of normalization is to ensure that the system always converges along the shortest path, i.e., the direction with the smallest phase difference.

[0057] In FPGA implementation, it is done by determining The value, if If it is greater than 180, then Subtract 360; if Less than -180, then 360 will be added; for example, if the measured angle of acceleration is 350° and the predicted angle is 5°, without normalization, then... The value is 345°. If normalization is performed, then... It is -15°.

[0058] Therefore, without normalization, when the angle jumps from 300° to 5°, the error instantly changes from 15° to 345°, disrupting the continuity of the angle output; the final output value is... Its format is a 16-bit signed fixed-point number format Q9.6 (1 sign bit, 9 integer bits, and 6 decimal bits).

[0059] S4: The complementary filtering module multiplies the shortest path deviation value with the preset weight coefficient, and then adds the product to the second roll angle to obtain the final roll angle and output it.

[0060] Specifically, such as Figure 6 As shown, the complementary filtering module fuses the signals from the gyroscope and accelerometer by setting two weighting coefficients (usually summing to 1). Its core formula is:

[0061] in, It is the output of the angular velocity integral module. , It is the angle output by the acceleration calculation module, and the angle output by the normalization correction module. for:

[0062] Therefore, the formula can be converted to:

[0063] This can then be transformed into:

[0064] Finally converted to:

[0065] The above mathematical transformations aim to optimize the FPGA logic hierarchy, reduce the number of operators on the data path, and thus further reduce hardware combinational logic latency.

[0066] Furthermore, during FPGA implementation, The input signal is converted to 10-bit fixed-point form, and then the output of the normalization correction module is processed by a multiplier. and fixed-point Multiply, then shift the product 10 places to the right to get the result. Then add the output of the angular velocity integral module. This is the final roll angle. Its format is a 16-bit signed fixed-point number format Q9.6 (1 sign bit, 9 integer bits, and 6 decimal bits).

[0067] It should be noted that the single multiplier structure significantly shortens the combinational logic delay, can increase the system clock frequency under the same FPGA process, reduces the timing convergence difficulty during high-frequency sampling, and saves DSP resources.

[0068] The S1 to S4 modules are isolated through inter-stage registers, forming a fully pipelined architecture. Each module operates independently and in parallel, with an overall latency fixed at a few clock cycles and no software jitter. Experiments show that, while meeting kHz-level sampling requirements, the dynamic power consumption of this invention is significantly reduced compared to high-end DSPs, the angle output is continuous without jumps, and the effective angle can be output in the first sampling cycle after startup.

[0069] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A real-time calculation method for the roll angle of an endoscope based on FPGA, characterized in that: include: The data synchronization acquisition module inside the FPGA reads the raw data from the accelerometer and gyroscope through the bus interface, and converts the raw data into acceleration and angular velocity values ​​in their respective fixed-point formats according to the preset range. The accelerometer module inside the FPGA uses the CORDIC computation IP core to calculate the first roll angle corresponding to the acceleration value. At the same time, the angular velocity integral module calculates the second roll angle based on the sum of the product of the output roll angle of the previous cycle, the current angular velocity value, and the sampling period. The two modules are executed in parallel in a pipeline manner. The normalization correction module calculates the difference between the first roll angle and the second roll angle, determines whether the difference exceeds 180 degrees or is less than -180 degrees, and if so, adds or subtracts 360 degrees to the difference and outputs the shortest path deviation value. The complementary filtering module multiplies the shortest path deviation value with the correction coefficient determined by the preset weight coefficient, and then adds the product to the second roll angle to obtain the final roll angle and output it.

2. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The data synchronization acquisition module uses a state machine-driven serial bus interface to convert accelerometer data into a preset setpoint format and convert gyroscope data into angular velocity through a shift addition / subtraction chain.

3. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The acceleration calculation module calculates arctan2 using the CORDIC IP core and uses a shifted addition / subtraction chain to perform multiplication operations from radians to angles, converting the angle range into a Q9.6 fixed-point number of [0°, 360°].

4. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The angular velocity integration module assigns the output of the acceleration solution module to the initial integration value during the first calculation, achieving zero-delay initial alignment.

5. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The normalization correction module uses a comparator to determine whether the deviation exceeds ±180° and performs 360° addition / subtraction correction.

6. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The complementary filtering module transforms the filtering formula as follows: in, This is the final output endoscope roll angle. The second roll angle is output by the angular velocity integral module. These are the weighting coefficients for the gyroscope channels in the complementary filter. This is the shortest path deviation value output by the normalization correction module.

7. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: Each module is isolated through inter-stage registers, forming a pipelined operation triggered by a single clock cycle.

8. The real-time calculation method for endoscope attitude roll angle based on FPGA as described in claim 1, characterized in that: The final roll angle output by the complementary filtering module is transmitted in real time to the back-end image processing unit (ISP). The image processing unit performs real-time reverse rotation compensation on the video image captured by the endoscope based on the final roll angle to achieve visual horizontal correction.

Images