Indoor binocular navigation high-precision positioning device and positioning method

Abstract

The invention belongs to the technical field of binocular camera navigation, and discloses an indoor binocular navigation high-precision positioning device and a positioning method. The device comprises an active scanning and visual guiding assembly and a reference target assembly, the active scanning and visual guiding assembly is rigidly installed on a binocular navigation camera, and the reference target assembly is fixed in the indoor environment. The active scanning assembly realizes target coarse positioning through an RGB camera, completes fine alignment with a two-dimensional holder in combination with a laser ranging module, and obtains pose information of a reference target in real time. The method comprises the following steps: initializing the system, establishing an initial point set, re-measuring the coordinates of a target after the camera moves, calculating a coordinate system transformation matrix through point cloud registration, and automatically updating the registration information of the navigation system. According to the method, self-adaptive registration can be realized after the pose of the camera is changed, the operation interruption is avoided, and the continuity, the stability and the operation efficiency of a surgical robot system are improved.

Description

Technical Field

[0001] This invention belongs to the field of binocular camera navigation technology, and particularly relates to an indoor binocular navigation high-precision positioning device and positioning method. Background Technology

[0002] Optical navigation-based surgical robot systems are a key technology in modern precision medicine. They use binocular navigation cameras to track the spatial positions of surgical instruments and the patient in real time, essentially giving the surgical robot "eyes." This system requires the unified registration of the robotic arm coordinate system, the medical image (such as CT / MRI) coordinate system, and the patient's real-world spatial coordinate system. This registration process is highly dependent on the pose of the binocular navigation cameras. In traditional applications, the binocular navigation cameras are typically assumed to remain stationary during surgery.

[0003] However, this assumption of a fixed pose has significant limitations in practical clinical applications. During surgery, medical staff may accidentally touch the camera or actively move it to avoid obstacles, causing a change in its pose. Once the camera pose changes, the original coordinate system transformation becomes invalid, and the accuracy and reliability of the entire navigation system cannot be guaranteed. At this point, the surgical procedure must be interrupted, and the tedious coordinate system registration operation must be performed again. This not only significantly prolongs the surgical time, increases the patient's anesthesia and infection risks, but also disrupts the surgical rhythm, reduces surgical efficiency, and causes great inconvenience to medical staff.

[0004] To address the aforementioned issues, existing technologies either employ mechanical locking devices to forcibly fix the camera, sacrificing operational flexibility, or rely on manual re-registration by the operator, failing to achieve automation and real-time performance. Currently, there is a lack of an effective technical solution that can automatically, quickly, and accurately re-determine the spatial pose of the camera and adaptively update the registration matrix without human intervention after a change in camera pose.

[0005] Therefore, there is an urgent need for a new positioning device and method that can monitor the pose changes of the binocular navigation camera in real time and automatically re-register the coordinate system after the pose changes, thereby ensuring the continuous and stable operation of the surgical robot system and truly improving the convenience and reliability of its clinical application. Summary of the Invention

[0006] The purpose of this invention is to provide an indoor binocular navigation high-precision positioning device and positioning method to solve the above-mentioned technical problems.

[0007] To address the aforementioned technical problems, the device of this invention achieves adaptive registration of the surgical robot navigation system after camera movement by real-time monitoring and automatic compensation of camera pose changes. This avoids the frequent manual re-registration required in traditional methods, significantly improving surgical efficiency and ease of operation. The specific technical solution is as follows: An indoor binocular navigation high-precision positioning device includes an active scanning and visual guidance component and a reference target component. The active scanning and visual guidance component is fixedly mounted on the binocular navigation camera, forming a rigid whole with it. The reference target component is fixedly installed in the indoor environment. The active scanning and visual guidance component acquires the pose information of the reference target component in real time, establishes the pose change relationship between the binocular navigation camera and the reference target component, and automatically compensates for the displacement change of the binocular navigation camera, thereby realizing adaptive registration of the surgical robot navigation system after the binocular navigation camera moves.

[0008] Furthermore, the active scanning and visual guidance component includes a laser rangefinder and intensity measurement module, a two-dimensional gimbal, and an RGB camera. The RGB camera and the laser rangefinder and intensity measurement module are both fixedly mounted on the two-dimensional gimbal, which drives the rotation in two degrees of freedom: pitch and yaw. The RGB camera is used to obtain the coarse positioning of the reference target component, and the laser rangefinder and intensity measurement module is used to obtain the fine alignment of the reference target component.

[0009] Furthermore, the reference target assembly includes an Aruco positioning QR code sticker and at least three non-collinearly arranged laser corner reflectors, the positions of which are precisely calibrated and fixed relative to the center position of the Aruco positioning QR code sticker during installation.

[0010] Furthermore, the two-dimensional gimbal is equipped with a pitch encoder and a yaw encoder for high-precision feedback of the gimbal rotation angle. The motor of the two-dimensional gimbal is controlled by a servo driver, enabling rapid coarse positioning and fine-tuning.

[0011] Furthermore, the laser ranging and intensity measurement module includes: a laser emitting unit, a laser receiving unit, a signal processing unit, and a control and calculation unit. The laser emitting unit is used to emit modulated laser; The laser receiving unit is used to receive the reflected laser light returning from the laser corner reflector along its original path; The signal processing unit converts the photocurrent generated by the laser receiving unit into a voltage signal and divides it into two paths: The first channel: after processing by the integration and limiting circuit, a voltage signal proportional to the received light intensity is output and sent to the AD channel 2 of the control and calculation unit for measuring the laser intensity; The second distance measurement channel: The received signal and the local oscillator signal are mixed by a mixing phase detector circuit to demodulate a low-frequency signal containing phase difference information, which is then sent to the AD channel of the control and calculation unit for accurate distance measurement. The control and computing unit is responsible for controlling the DDS chip to generate signals, acquiring data from the RGB camera, reading encoder values, collecting AD channel data, executing the PID control algorithm to calculate the two-dimensional gimbal rotation command, and sending the final distance and angle data to the navigation host.

[0012] Furthermore, the RGB camera is fixed on a two-dimensional gimbal, and its optical axis maintains a fixed relative pose relationship with the optical axis of the laser emitter. This is used to quickly capture images of the Aruco positioning QR code sticker when the system is initialized or the target is lost, to achieve coarse positioning, and to guide the two-dimensional gimbal to roughly align the laser with the laser corner reflector area.

[0013] This invention also discloses a positioning method for an indoor binocular navigation high-precision positioning device, comprising the following steps: Step 1: System initialization: Measure the coordinates of each laser corner reflector in the gimbal coordinate system at the initial position to establish an initial point set; Step 2: Real-time pose monitoring and data acquisition: After the binocular navigation camera moves, remeasure the coordinates of each corner reflector in the current gimbal coordinate system and establish the current point set; Step 3: Calculate the relative transformation of the gimbal coordinate system: Calculate the transformation matrix from the current gimbal coordinate system to the initial gimbal coordinate system using the point cloud registration algorithm; Step 4: Derive and apply the new camera registration matrix: Using the relative transformation matrix, the known gimbal-stereo navigation camera fixed transformation matrix, and the initial registration matrix, calculate the transformation matrix from the stereo navigation camera coordinate system to the world coordinate system after movement; Step 5: Dynamic compensation: Apply a new transformation matrix to update the registration information of the navigation system to achieve adaptive registration.

[0014] Furthermore, step 1 includes the following steps: Place the binocular navigation camera equipped with active scanning and visual guidance components in the initial position; gimbal coordinate system CMM coordinate system with binocular navigation camera CAM Relatively fixed, its transformation relationship is a known fixed matrix. ; World coordinate system W The pose is determined by the fixed positions of three laser corner reflectors; Determine the initial coordinate system of the stereo navigation camera. CAM0 To the world coordinate system W Transformation matrix ; In the initial position, the active scanning and vision guidance components are used to sequentially and precisely measure the target spheres A, B, and C of the three laser corner reflectors. For each laser corner reflector, its position in the current gimbal coordinate system is recorded. CMM0 The three-dimensional coordinates are used to obtain the reference point set. .

[0015] Furthermore, the point set is obtained through the following method: Coarse positioning: Control the rotation of the two-dimensional gimbal so that the RGB camera captures the Aruco positioning QR code sticker. Based on the fixed relationship between the Aruco positioning QR code sticker and the laser corner reflector 302, the laser emitted by the laser ranging and intensity measuring module is roughly aligned with the target corner reflector area. Precise alignment: Activate the laser rangefinder and intensity measurement module, read the reflected intensity signal, and fine-tune the yaw angle θ and pitch angle φ of the two-dimensional gimbal through the PID algorithm until the reflected light intensity reaches the maximum value, indicating that the laser has been precisely aligned with the center of the corner reflector; Data acquisition: Locking the 2D gimbal angle: Record the yaw angle encoder value θ, the pitch angle encoder value φ, and the distance value d output by the laser rangefinder and intensity measurement module at this time; Coordinate calculation: Calculate the coordinates of the corner reflector in the current gimbal coordinate system based on d, θ, and φ. CMM Three-dimensional coordinates , i =A, B, C, representing the nth target ball.

[0016] Furthermore, step 3 calculates the coordinates from the current gimbal coordinate system by solving a least-squares problem. CMM1 To the initial gimbal coordinate system CMM0 relative transformation matrix .

[0017] The indoor binocular navigation high-precision positioning device and positioning method of the present invention have the following advantages: 1. Achieve adaptive registration after the binocular navigation camera moves: The active scanning and visual guidance components monitor the pose changes of the binocular navigation camera relative to the fixed reference target in real time and automatically calculate the compensation matrix. Re-registration can be completed without manual intervention, which greatly improves the continuity and stability of the surgical robot system.

[0018] 2. Improve surgical efficiency and safety: Avoid surgical interruptions and repeated registration operations caused by camera movement, significantly shorten surgical time, reduce the risk of anesthesia and infection for patients, and reduce the workload of medical staff.

[0019] 3. High-precision positioning and fast response: Combining the dual mechanisms of coarse positioning with RGB camera and fine alignment with laser rangefinding, along with a high-precision encoder and PID control algorithm, it achieves fast and accurate aiming and measurement of the laser corner reflector, ensuring the accuracy and real-time performance of pose data.

[0020] 4. High system integration and easy installation: The active scanning component is rigidly connected to the binocular navigation camera, and the reference target is fixed in the indoor environment. The overall structure is compact, easy to deploy, and does not affect the original surgical procedure and spatial layout.

[0021] 5. Strong robustness and reliability: The target design, which combines Aruco QR code and laser corner reflector, can quickly restore alignment through visual guidance even if the target is lost or the ambient light changes, and the system has strong fault tolerance.

[0022] 6. High degree of automation, reducing human error: The entire process is automatically controlled by MCU, including target search, alignment, data acquisition and coordinate calculation, which minimizes the error introduced by manual operation and improves the overall accuracy and consistency of the navigation system.

[0023] In summary, this invention not only solves the registration failure problem caused by camera pose changes in traditional binocular navigation systems, but also achieves significant improvements in accuracy, efficiency, automation, and system reliability, and has important clinical application value and prospects for promotion. Attached Figure Description

[0024] Figure 1 This is a structural block diagram of the indoor binocular navigation high-precision positioning device of the present invention; Figure 2 This is a schematic diagram of the laser corner reflector structure of the present invention; Figure 3 This is a schematic diagram of the two-dimensional gimbal structure of the present invention; Figure 4 This is a schematic diagram of the arrangement structure of the Aruco positioning QR code sticker and laser corner reflector of the present invention; Figure 5 This is a block diagram of the laser rangefinder and intensity measurement module of the present invention; The markings in the diagram are as follows: 100, Active scanning and visual guidance component; 10, Laser rangefinder and intensity measurement module; 20, 2D gimbal; 30, RGB camera; 200, Binocular navigation camera; 300, Reference target component; 301, Aruco positioning QR code sticker; 302, Laser corner reflector; 400, Navigation host. Detailed Implementation

[0025] To better understand the purpose, structure, and function of this invention, the following detailed description of an indoor binocular navigation high-precision positioning device and positioning method, in conjunction with the accompanying drawings, is provided.

[0026] like Figure 1 As shown, this invention discloses an indoor binocular navigation high-precision positioning device for real-time monitoring of camera pose changes and adaptive registration. It includes an active scanning and visual guidance component 100 and a reference target component 300. The active scanning and visual guidance component 100 is fixedly mounted on the binocular navigation camera 200, forming a rigid integral unit with it. The reference target component 300 is fixedly mounted at a fixed position such as the indoor ceiling or floor. The active scanning and visual guidance component 100 acquires the pose information of the reference target component 300 in real time, establishes the pose change relationship between the binocular navigation camera 200 and the reference target component 300, and automatically compensates for it, achieving adaptive registration of the surgical robot navigation system after the binocular navigation camera 200 moves.

[0027] The active scanning and visual guidance component 100 includes: a laser rangefinder and intensity measurement module 10, a 2D gimbal 20, and an RGB camera 30. Both the RGB camera 30 and the laser rangefinder and intensity measurement module 10 are fixedly mounted on the 2D gimbal 20, and are rotated by the 2D gimbal 20 in two degrees of freedom: pitch and yaw. The RGB camera 30 is used to acquire coarse positioning of the reference target component 300, and the laser rangefinder and intensity measurement module 10 is used to acquire fine alignment of the reference target component 300.

[0028] like Figure 2 Figure 4 As shown, the reference target assembly 300 includes an Aruco positioning QR code sticker 301 and at least three laser corner reflectors 302 with an incident surface diameter of 1 cm. The three laser corner reflectors 302 are arranged non-collinearly, and their positions relative to the center of the Aruco positioning QR code sticker 301 are precisely calibrated and fixed during installation. Each laser corner reflector 302 has a target sphere at its center for reflecting the laser beam.

[0029] like Figure 3 As shown, the 2D gimbal 20 is equipped with a pitch encoder and a yaw encoder for high-precision feedback of the gimbal's rotation angle. The motors of the 2D gimbal 20 are controlled by servo drives, enabling rapid coarse positioning and fine-tuning.

[0030] The laser ranging and intensity measurement module 10 is the core ranging unit, and its internal structure is as follows: Figure 5 As shown, it includes: a laser emitting unit, a laser receiving unit, a signal processing unit, and a control and computing unit.

[0031] The laser emitting unit includes a DDS chip, a laser drive circuit, and a laser emitting module. Because the diameter of the laser corner reflector's receiving surface is 1cm, an excessively large laser spot size would affect ranging accuracy. Through comparison, the TO56CP-10000009 laser emitting module was selected, ensuring that the spot diameter is less than or equal to 5mm at a maximum working distance of 5 meters. The DDS chip is a high-precision Si5351 chip, used to generate a high-frequency master oscillation signal, which is sent to the laser drive circuit. The laser drive circuit then drives the laser emitting module to emit modulated laser light.

[0032] The laser receiving unit includes a lens, a laser receiver, and a boost circuit. The lens receives reflected laser light, which passes through the laser receiver. The laser receiver uses an APD diode (preferably AD230-8r) to receive the reflected laser light returning from the laser corner reflector 302. The receiving center frequency of the laser receiver is consistent with the emission center frequency of the laser emitting module. The boost circuit provides the necessary high-voltage bias to the APD.

[0033] The signal processing unit includes an IV transimpedance converter circuit, an integral limiting circuit, and a mixer phase detector circuit. The photocurrent generated by the laser receiving unit is first converted into a voltage signal by the transimpedance amplifier circuit (IV transimpedance converter circuit). This signal is then split into two paths: The first channel (light intensity measurement channel): The signal is processed by an integration and limiting circuit composed of a precision operational amplifier, and outputs a voltage signal that is proportional to the received light intensity. This signal is then sent to the AD channel 2 of the control and computing unit (MCU) to measure the laser intensity.

[0034] The second path (range measurement path): The received signal is mixed with the local oscillator signal (also generated by the DDS chip) through a mixing and phase detection circuit composed of an LT5560 mixer and an AD8302 phase detector. A low-frequency signal containing phase difference information is demodulated and sent to the AD channel of the control and computing unit (MCU) for accurate range measurement.

[0035] Control and Calculation Unit: Built-in MCU (microcontroller), with a 2D gimbal motor encoder interface, USB interface and Bluetooth interface. The 2D gimbal motor encoder interface is used to receive motor encoder data of the 2D gimbal; the USB interface is used to connect to the RGB camera 30; the Bluetooth interface is used to connect to the navigation host. The control and calculation unit is responsible for controlling the DDS chip to generate signals, acquiring data from the RGB camera 30, reading encoder values, collecting AD channel data, executing the PID control algorithm to calculate the 2D gimbal rotation command, and sending the final distance and angle data to the navigation host 400 through the Bluetooth module.

[0036] The RGB camera 30 is fixed on the 2D gimbal 20, and its optical axis maintains a fixed relative pose relationship with the optical axis of the laser emitter (which has been calibrated at the factory). It is used to quickly capture the image of the Aruco positioning QR code sticker 301 when the system is initialized or the target is lost, to achieve coarse positioning, and guide the 2D gimbal 20 to roughly align the laser with the area of ​​the laser corner reflector 302.

[0037] Brief description of working principle: After the device is started, the MCU controls the rotation of the 2D gimbal 20. After the RGB camera 30 searches for the Aruco positioning QR code sticker 301, it calculates the approximate direction of the laser corner reflector 302 based on the pre-calibrated relative position relationship between the QR code and the corner reflector. The 2D gimbal 20 then drives the laser ranging and intensity measurement module 10 to turn in that direction. Next, the MCU turns on the laser emitter and reads the intensity value of AD channel 2. It then fine-tunes the gimbal angle through a PID control algorithm to maximize the reflected laser intensity, indicating that the laser is precisely aligned with the center of the laser corner reflector 302. After alignment, the MCU switches to ranging mode, reads the signal of AD channel 1 and calculates the precise distance, while simultaneously recording the angle values ​​of the pitch and yaw encoders of the 2D gimbal 20. This process is repeated for the three laser corner reflectors 302 to obtain a complete set of measurement data.

[0038] like Figure 4 As shown, an indoor binocular navigation high-precision positioning method of the present invention includes the following steps: Step 1: System initialization: Measure the coordinates of each laser corner reflector 302 in the gimbal coordinate system at the initial position to establish an initial point set.

[0039] Place the binocular navigation camera 200, which is equipped with the active scanning and visual guidance component 100, in the initial position.

[0040] gimbal coordinate system CMM coordinate system with binocular navigation camera CAM Relatively fixed, its transformation relationship is a known fixed matrix. .

[0041] World coordinate system W The pose of three fixed laser corner reflectors 302 is determined, for example: the line CA connecting the target spheres A, B, and C of the three corner reflectors is taken as the X-axis, the line CB connecting them is taken as the Y-axis, and the Z-axis is perpendicular to the plane established by CA and CB. The world coordinate system is established according to the right-hand screw rule. W The world coordinate system here W The method of determining it is not unique, but once determined, it remains fixed.

[0042] The initial coordinate system of the stereo navigation camera is determined using conventional registration methods (such as point registration and area registration). CAM0 To the world coordinate systemW Transformation matrix .

[0043] In the initial position, the active scanning and vision guidance component 100 sequentially and precisely measures the target spheres A, B, and C of the three laser corner reflectors 302. For each laser corner reflector 302, its position in the current gimbal coordinate system is recorded. CMM0 The three-dimensional coordinates are used to obtain the reference point set. , For target ball A relative to the current gimbal coordinate system CMM0 pose coordinates below, For target ball B relative to the current gimbal coordinate system CMM0 pose coordinates below, For target ball C relative to the current gimbal coordinate system CMM0 The pose coordinates are obtained by calculating the distance measured by the laser rangefinder and intensity measurement module 10 and the encoder angle of the 2D gimbal 20. The specific methods for obtaining the distance and angle are as follows: Coarse positioning: Control the rotation of the two-dimensional gimbal 20 so that the RGB camera 30 captures the Aruco positioning QR code sticker 301. Based on the fixed relationship between the Aruco positioning QR code sticker 301 and the laser corner reflector 302, the laser emitted by the laser ranging and intensity measuring module 10 is roughly aligned with the target corner reflector area.

[0044] Precise alignment: Activate the laser rangefinder and intensity measurement module 10, read the reflection intensity signal of AD channel 2, and fine-tune the yaw angle θ and pitch angle φ of the two-dimensional gimbal 20 through the PID algorithm until the reflected light intensity reaches the maximum value, indicating that the laser has been precisely aligned with the center of the corner reflector.

[0045] Data Acquisition: Lock the 2D gimbal to angle 20. Record the yaw angle encoder value θ, the pitch angle encoder value φ, and the distance value d output by the laser rangefinder and intensity measurement module 10 at this time.

[0046] Coordinate calculation: Calculate the coordinates of the corner reflector in the current gimbal coordinate system based on d, θ, and φ. CMM Three-dimensional coordinates , i =A, B, C, representing the nth target ball.

[0047] Step 2: Real-time pose monitoring and data acquisition: After the binocular navigation camera moves, remeasure the coordinates of each corner reflector 302 in the current gimbal coordinate system and establish the current point set.

[0048] When the pose of the binocular navigation camera 200 moves, the active scanning and visual guidance component 100 is activated.

[0049] Control the 2D gimbal 20 to accurately measure the target spheres A, B, and C of the three corner reflectors in sequence, and obtain their coordinates in the gimbal coordinate system after movement. CMM1 Given the current coordinates, obtain the current point set. The calculation method is the same as above.

[0050] Step 3: Calculate the relative transformation of the gimbal coordinate system: Calculate the transformation matrix from the current gimbal coordinate system to the initial gimbal coordinate system using the point cloud registration algorithm.

[0051] For the initial point set and current point set Perform point cloud registration.

[0052] By solving the least squares problem, the coordinates from the current gimbal coordinate system are calculated. CMM1 To the initial gimbal coordinate system CMM0 relative transformation matrix (Including rotation matrix) Translation vector (Calculated based on the angle values ​​of the pitch and yaw encoders).

[0053]

[0054] This solution can be obtained using classical algorithms such as SVD decomposition.

[0055] Step 4: Derive and apply the new camera registration matrix: Using the relative transformation matrix, the known gimbal-stereo navigation camera fixed transformation matrix, and the initial registration matrix, calculate the transformation matrix from the stereo navigation camera coordinate system to the world coordinate system after movement.

[0056] Using the relative transformation matrix obtained in step 3 Known gimbal-binocular navigation camera fixed transformation matrix and the initial registration matrix Calculate the new binocular navigation camera coordinate system CAM1 to the world coordinate system after the movement. W Transformation matrix The calculation formula is as follows:

[0057] Step 5: Dynamic compensation: Apply a new transformation matrix to update the registration information of the navigation system to achieve adaptive registration.

[0058] According to the new binocular navigation camera coordinate system CAM1 To the world coordinate system W Transformation matrix And the transformation matrix from the fixed CT coordinate system to the world coordinate system. The new transformation matrix of the binocular navigation camera 200 relative to the CT coordinate system can be obtained. The navigation host 400 applies this new transformation matrix. The registration information of the navigation system is updated to complete adaptive registration without the need for manual reoperation.

[0059] It is understood that the present invention has been described through some embodiments, and those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention.

Claims

1. An indoor binocular navigation high-precision positioning device, characterized in that, The active scanning and visual guiding assembly (100) is fixedly installed on the binocular navigation camera (200) and becomes a rigid whole with the binocular navigation camera (200); the reference target assembly (300) is fixedly installed in an indoor environment, the active scanning and visual guiding assembly (100) acquires the pose information of the reference target assembly (300) in real time, establishes the pose change relationship of the binocular navigation camera (200) relative to the reference target assembly (300) and automatically compensates the displacement change amount of the binocular navigation camera (200), so that the adaptive registration of the surgical robot navigation system after the binocular navigation camera (200) is moved is realized.

2. The indoor binocular navigation high-precision positioning device according to claim 1, characterized in that, The active scanning and visual guiding assembly (100) comprises a laser ranging and intensity measuring module (10), a two-dimensional holder (20) and an RGB camera (30), the RGB camera (30) and the laser ranging and intensity measuring module (10) are both fixedly installed on the two-dimensional holder (20) and are driven by the two-dimensional holder (20) to rotate in two degrees of freedom of pitch and yaw; the RGB camera (30) is used for acquiring coarse positioning of the reference target assembly (300), and the laser ranging and intensity measuring module (10) is used for acquiring fine alignment of the reference target assembly (300).

3. The indoor binocular navigation high-precision positioning device according to claim 2, characterized in that, The reference target assembly (300) comprises an Aruco positioning two-dimensional code sticker (301) and at least three non-collinear laser angle reflectors (302), the positions of the laser angle reflectors (302) relative to the central position of the Aruco positioning two-dimensional code sticker (301) are accurately calibrated and fixedly unchanged during installation.

4. The indoor binocular navigation high-precision positioning device according to claim 2, characterized in that, The two-dimensional holder (20) is provided with a pitch angle encoder and a yaw angle encoder, which are used for feeding back the angle values of the rotation of the holder with high precision, and the motor of the two-dimensional holder (20) is controlled by a servo driver, so that rapid coarse positioning and fine adjustment can be realized.

5. The indoor binocular navigation high-precision positioning device according to claim 3, characterized in that, The laser ranging and intensity measuring module (10) comprises a laser emitting unit, a laser receiving unit, a signal processing unit and a control and calculation unit, The laser emitting unit is used for emitting modulated laser; The laser receiving unit is used for receiving reflected laser returned from the laser angle reflector (302) by the original path; The signal processing unit converts the photoelectric current generated by the laser receiving unit into a voltage signal and respectively measures the laser intensity and the distance; The control and calculation unit is responsible for controlling the DDS chip to generate a signal, acquiring the data of the RGB camera (30), reading the encoder value, collecting the AD channel data, executing a PID control algorithm to calculate the rotation instruction of the two-dimensional holder (20), and sending the final distance and angle data to a navigation host (400).

6. The indoor binocular navigation high-precision positioning device according to claim 3, characterized in that, The RGB camera (30) is fixed on the two-dimensional holder (20), and the optical axis thereof maintains a fixed relative pose relationship with the optical axis of the laser emitter, which is used for quickly capturing the image of the Aruco positioning two-dimensional code sticker (301) when the system is initialized or the target is lost, realizing coarse positioning and guiding the two-dimensional holder (20) to roughly align the laser with the laser angle reflector (302) region.

7. A method of positioning of an indoor binocular navigation high-precision positioning device according to any one of claims 1-6, characterized in that, The method comprises the following steps: Step 1: System initialization: measure the coordinates of each laser corner reflector (302) in the gimbal coordinate system at the initial position, and establish the initial point set; Step 2: Real-time pose monitoring and data collection: re-measure the coordinates of each corner reflector (302) in the current gimbal coordinate system after the binocular navigation camera moves, and establish the current point set; Step 3: Calculate the relative transformation of the gimbal coordinate system: calculate the transformation matrix from the current gimbal coordinate system to the initial gimbal coordinate system through the point cloud registration algorithm; Step 4: Derive and apply the new camera registration matrix: use the relative transformation matrix, the known gimbal-binocular navigation camera fixed transformation matrix, and the initial registration matrix to calculate the transformation matrix from the moving binocular navigation camera coordinate system to the world coordinate system; Step 5: Dynamic compensation: update the registration information of the navigation system by applying the new transformation matrix, and realize adaptive registration.

8. The positioning method according to claim 7, characterized in that, The step 1 includes the following steps: Place the binocular navigation camera (200) equipped with the active scanning and visual guidance assembly (100) at the initial position; gimbal coordinate system CMM with binocular navigation camera coordinate system CAM relatively fixed, the transformation relationship of which is a known fixed matrix ; The world coordinate system W is determined by the poses of the three fixed laser corner reflectors (302); Determining initial binocular navigation camera coordinate system CAM0 to world coordinate system W transformation matrix ; In the initial position, the control active scanning and visual guidance assembly (100) sequentially measures the target balls A, B, C of the three laser angle reflectors (302) accurately, for each laser angle reflector (302), records its three-dimensional coordinates in the current PTZ coordinate system CMM0 , to obtain the reference point set .

9. The positioning method of claim 7, wherein, The point set is obtained by the following method: Coarse positioning: control the two-dimensional gimbal (20) to rotate, so that the RGB camera (30) captures the Aruco positioning QR code sticker (301), and according to the fixed relationship between the Aruco positioning QR code sticker (301) and the laser corner reflector 302, the emitted laser of the laser ranging and intensity measurement module (10) is roughly aligned with the target corner reflector area; Fine alignment: turn on the laser ranging and intensity measurement module (10), read the reflected intensity signal, and fine-tune the yaw angle θ and pitch angle φ of the two-dimensional gimbal (20) through the PID algorithm until the reflected light intensity reaches the maximum value, indicating that the laser has accurately aligned with the center of the corner reflector; Data collection: lock the two-dimensional gimbal (20) angle: record the yaw angle encoder value θ, pitch angle encoder value φ of the two-dimensional gimbal (20) at this time, and the distance value d output by the laser ranging and intensity measurement module (10); Coordinate calculation: calculate the 3D coordinate of the corner reflector in the current PTZ coordinate system according to d, θ, φ CMM , i =A, B, C, indicate the number of the target ball.​ 10. The positioning method of claim 7, wherein, The step 3 calculates the relative transformation matrix from the current PTZ coordinates system CMM1 to the initial PTZ coordinates system CMM0 by solving a least square problem .

Images