Three-dimensional space positioning system and method based on laser and camera fusion

Abstract

The invention provides a three-dimensional space positioning system based on laser and camera fusion, and relates to the field of three-dimensional space positioning, the three-dimensional space positioning system comprises a ground terminal subsystem and a mobile terminal subsystem, the ground terminal subsystem comprises a laser emitter, a ground camera and a mechanical arm, and the mobile terminal subsystem comprises a mobile device and a far-end camera. According to the positioning system, the laser sensing technology and the camera vision technology are fused, and space positioning of mobile terminal equipment is achieved through cooperative work of the ground terminal and the mobile terminal; the method not only gets rid of the constraint of traditional satellite navigation, but also avoids the common accumulative error problem of an inertial sensor, thereby showing excellent stability and positioning precision in various complex environments; the three-dimensional space positioning system can be widely applied to the fields of mobile robots, unmanned aerial vehicle inspection and the like, powerful technical support is provided for guaranteeing operation safety and achieving precise control, and the popularization value is high.

Description

Technical Field

[0001] The present invention relates to the field of three-dimensional space positioning, and in particular to a three-dimensional space positioning system and method based on laser and camera fusion. Background Art

[0002] As advanced automation tools, facade mobile robots and drones can safely and efficiently replace humans to perform tasks in dangerous or difficult-to-reach areas such as high altitudes and facades. They perform long-term, uninterrupted inspection and maintenance in high-altitude environments, not only providing high-definition visual vision, but also accurately measuring and collecting key data, providing strong information support for the formulation of subsequent work strategies and plans, greatly enhancing the safety and efficiency of operations.

[0003] Mobile robots or drones demonstrate a high degree of automation when executing preset programs and tasks. However, in complex three-dimensional space working environments, such as high-altitude facade operations, their actual motion trajectories often deviate from the expected trajectories due to interference from natural factors such as gravity, friction, and wind resistance, posing challenges to precise operations. To solve this problem, three-dimensional spatial positioning technology has emerged, which enables mobile robots and drones to achieve precise positioning and navigation in complex and changing environments. By capturing and analyzing the precise position information of the robot in three-dimensional space in real time, this technology not only effectively avoids collisions with obstacles, but also ensures that the robot or drone can strictly follow the predetermined path and complete the given task safely and accurately, thereby significantly improving work efficiency and accuracy.

[0004] Although satellite positioning systems (such as GPS) can provide three-dimensional spatial location information, in actual applications, their signals are easily blocked or reflected, resulting in signal leakage, insufficient or even excessive reception of reflected signals, which in turn affects the accurate measurement of elevation information. The real-time performance and accuracy of the satellite positioning system will be significantly affected, making it difficult to meet the needs of high-precision operations.

[0005] On the other hand, although the 3D positioning method based on the inertial measurement unit (IMU) does not rely on external signals, it is limited by the setting of its relative coordinate system and the error accumulated over time, and the positioning error will gradually increase. Therefore, during long-term use, the positioning accuracy of the IMU will gradually decrease, and it is difficult to ensure long-term stable and accurate positioning.

[0006] To solve the above problems, it is usually necessary to combine multiple positioning technologies, such as integrating satellite positioning, inertial navigation, visual recognition and other sensor data to achieve more stable and accurate three-dimensional spatial positioning. This multi-sensor fusion technology can compensate for each other's shortcomings and improve the overall performance and reliability of the positioning system.

[0007] Therefore, how to solve the problem of large positioning errors caused by elevation, cumulative errors and relative coordinate systems in existing positioning technologies has become an urgent problem to be solved by technical personnel in this field. Summary of the invention

[0008] The technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide a three-dimensional spatial positioning system and method based on the fusion of laser and camera. The present invention not only breaks away from the constraints of traditional satellite navigation, but also avoids the common cumulative error problem of inertial sensors, thereby demonstrating excellent stability and positioning accuracy in various complex environments.

[0009] The present invention is achieved through the following technical solutions:

[0010] A three-dimensional spatial positioning system based on laser and camera fusion, including a ground terminal system and a mobile terminal system, wherein;

[0011] The ground terminal system includes a laser transmitter, a ground camera and a mechanical arm, wherein the mechanical arm is arranged on the ground, and the laser transmitter and the ground camera are respectively connected to the ends of the mechanical arm;

[0012] The mobile terminal system includes a mobile device and a remote camera, wherein a preset target point is arranged on the outer surface of the mobile device, the remote camera is fixed on the mobile device, the remote camera faces the area for photographing the preset target point, and the distance between the remote camera and the preset target point is fixed;

[0013] The laser projection point of the laser transmitter locks the preset target area; the laser transmitter is used to measure the distance between the end of the robot arm and the preset target on the mobile terminal system;

[0014] The ground camera captures the image information of the mobile terminal system in real time, and guides the robotic arm to perform precise tracking through visual feedback, so that the laser projection point is always located in the preset target area of ​​the mobile device;

[0015] The robotic arm automatically adjusts its posture to follow the movement of the mobile terminal system based on the ground camera feedback and the distance data of the laser transmitter, so that the laser projection point of the laser transmitter coincides with the preset target point on the mobile terminal system.

[0016] According to the above technical solution, preferably, both the mechanical arm and the mobile device are provided with posture sensors.

[0017] According to the above technical solution, preferably, the remote camera captures the position of the laser projection point in the remote camera coordinate system, combines the posture sensor data on the mobile device and the coordinate information of the remote camera, obtains the spatial position of the laser projection point in the remote device's own coordinate system, and controls the robotic arm to accurately track the preset target.

[0018] According to the above technical solution, preferably, it also includes a spatial solver subsystem, which has an embedded spatial calculation model. The spatial calculation model integrates the joint positions of the robotic arm, the distance data of the laser emitter, the spatial position of the laser projection point in the remote device's own coordinate system, and the relative posture relationship between the remote camera coordinate system and the global coordinate system of the ground end, to obtain the position of the origin of the remote camera coordinate system in the global coordinate system of the ground end.

[0019] According to the above technical solution, preferably, the robotic arm can adjust the three-dimensional angle in real time, and the robotic arm includes a multi-joint robotic arm or a two-dimensional gimbal.

[0020] According to the above technical solution, preferably, the mobile device includes a drone or a mobile robot.

[0021] The present invention also discloses a three-dimensional space positioning method based on laser and camera fusion, which adopts the above-mentioned three-dimensional space positioning system based on laser and camera fusion, and specifically includes the following steps:

[0022] Step 1: Place the mobile terminal system in the clear field of view of the ground camera by manually controlling the robotic arm, and then project the laser beam of the laser transmitter to the preset target area of ​​the mobile terminal system. During this period, the ground terminal system measures and records the distance d between the end of the robotic arm and the laser projection point in real time;

[0023] Step 2: The remote camera of the mobile terminal system captures the position information of the laser projection point in the remote camera coordinate system, and sends the position information to the ground terminal system to adjust and control the robotic arm;

[0024] Step 3: The ground terminal system uses the MDH model to finally determine and output the position of the laser projection point on the mobile terminal system in the global coordinate system of the ground terminal based on the position of each joint of the robot arm, the distance d between the end of the robot arm and the laser projection point, and the position information of the laser projection point in the remote camera coordinate system. The full name of the MDH model is the Modified Denavit-Hartenberg model;

[0025] Where i represents the i-th joint, Represents the transformation matrix from the i-th joint to the i+1-th joint;

[0026] Step 4: The distance between the telephoto camera and the laser projection area is fixed. After the telephoto camera is calibrated, the position of the laser projection point in the telephoto camera coordinate system is calculated. where z c is the distance from the remote camera to the laser projection area, z c After the remote camera and the laser projection area are fixed, the value is determined to be constant;c ,y c ,z c Represents the 3D spatial position of the laser projection point in the far camera coordinate system.

[0027] Step 5: The position of the laser projection point in the known remote camera coordinate system The position of the laser projection point in the global coordinate system of the ground end And the relative posture relationship between the remote camera coordinate system and the ground end global coordinate system Based on this, solve the position of the origin of the remote camera coordinate system in the global coordinate system of the ground end.

[0028] According to the above technical solution, preferably, the relative posture relationship between the remote camera coordinate system and the ground end global coordinate system in step 5 is It can be directly calculated by the attitude sensors installed on the mobile terminal system and the ground terminal system, or obtained indirectly by using the optimization estimation algorithm.

[0029] The beneficial effects of the present invention are:

[0030] (1) This patent proposes a highly integrated three-dimensional spatial positioning system that integrates laser sensing technology and camera vision technology to achieve spatial positioning of mobile devices through the collaborative work of the ground terminal and the mobile terminal;

[0031] (2) The three-dimensional spatial positioning system of the present invention not only breaks away from the constraints of traditional satellite navigation, but also avoids the common cumulative error problem of inertial sensors, thereby showing excellent stability and positioning accuracy in various complex environments;

[0032] (3) The three-dimensional spatial positioning system of the present invention can be widely used in the fields of mobile robots, drone inspections, etc., providing strong technical support for ensuring operational safety and achieving precise control;

[0033] (4) The three-dimensional spatial positioning system of the present invention has a long-range and short-range dual-end detection architecture and spatial positioning capabilities, which also lays a solid foundation for the future development trend of intelligent and unmanned operation;

[0034] (5) The present invention realizes comprehensive tracking, precise measurement and positioning of the remote device on the ground side, and implements fine secondary calibration of the laser projection point at the remote side, ensuring that the robotic arm accurately follows the preset target of the remote device. The preset target is located in the camera coordinate system and can be flexibly set to coincide with the origin, thereby effectively breaking through the distance limitation and significantly improving the accuracy and stability of spatial positioning, providing a solid technical guarantee and solution for the spatial positioning needs in various complex and changing environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 A schematic diagram showing the working principle of the system of the present invention is shown;

[0036] Figure 2 A schematic diagram of the system structure of the present invention is shown;

[0037] Figure 3 A schematic diagram showing a laser projection point and a preset target point on a mobile device of the present invention is shown;

[0038] Description of reference numerals:

[0039] 1. Laser transmitter; 2. Ground camera; 3. Robotic arm; 4. Mobile device; 5. Remote camera; 6. Preset target. DETAILED DESCRIPTION

[0040] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and the best embodiment. Based on the embodiments in the invention, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the invention.

[0041] In the description of the invention, it should be noted that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside" and "outside" etc. indicating directions or positional relationships are based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, they cannot be understood as limitations on the invention.

[0042] Example 1

[0043] As shown in the figure, the present invention provides a three-dimensional spatial positioning system based on laser and camera fusion, including a ground terminal system, a mobile terminal system and a space solver system, wherein;

[0044] The ground terminal system includes a laser transmitter 1, a ground camera 2 and a mechanical arm 3, wherein the mechanical arm is arranged on the ground, and the laser transmitter and the ground camera are respectively connected to the ends of the mechanical arm;

[0045] The mobile terminal system includes a mobile device 4 and a remote camera 5. A preset target point 6 is provided on the outer surface of the mobile device. The remote camera is fixed on the mobile device. The remote camera faces the area for shooting the preset target point, and the distance between the remote camera and the preset target point is fixed.

[0046] The laser projection point of the laser transmitter locks the preset target area; the laser transmitter is used to measure the distance between the end of the robot arm and the preset target on the mobile terminal system;

[0047] The ground camera captures the image information of the mobile terminal system in real time, and guides the robotic arm to perform precise tracking through visual feedback, so that the laser projection point is always located in the preset target area of ​​the mobile device;

[0048] The robotic arm automatically adjusts its posture to follow the movement of the mobile terminal system according to the feedback from the ground camera and the distance data of the laser transmitter, so that the laser projection point of the laser transmitter coincides with the preset target point on the mobile terminal system. Both the robotic arm and the mobile device are provided with posture sensors;

[0049] The remote camera captures the position of the laser projection point in the remote camera coordinate system, combines the posture sensor data on the mobile device and the coordinate information of the remote camera, obtains the spatial position of the laser projection point in the remote device's own coordinate system, and controls the robotic arm to accurately track the preset target.

[0050] The solver subsystem has an embedded spatial calculation model, which integrates the joint positions of the robotic arm, the distance data of the laser emitter, the spatial position of the laser projection point in the remote device's own coordinate system, and the relative posture relationship between the remote camera coordinate system and the ground-end global coordinate system to obtain the position of the origin of the remote camera coordinate system in the ground-end global coordinate system.

[0051] Optionally, in a possible implementation, the robotic arm can adjust the three-dimensional angle in real time, and the robotic arm includes a multi-joint robotic arm or a two-dimensional gimbal.

[0052] Optionally, in a possible implementation, the mobile device includes a drone or a mobile robot.

[0053] The beneficial effects of this embodiment are:

[0054] (1) This patent proposes a highly integrated three-dimensional spatial positioning system that integrates laser sensing technology and camera vision technology to achieve spatial positioning of mobile devices through the collaborative work of the ground terminal and the mobile terminal;

[0055] (2) The three-dimensional spatial positioning system of the present invention not only breaks away from the constraints of traditional satellite navigation, but also avoids the common cumulative error problem of inertial sensors, thereby showing excellent stability and positioning accuracy in various complex environments;

[0056] (3) The three-dimensional spatial positioning system of the present invention can be widely used in the fields of mobile robots, drone inspections, etc., providing strong technical support for ensuring operational safety and achieving precise control;

[0057] (4) The three-dimensional spatial positioning system of the present invention has a long-range and short-range dual-end detection architecture and spatial positioning capabilities, which also lays a solid foundation for the future development trend of intelligent and unmanned operation;

[0058] (5) The present invention realizes comprehensive tracking, precise measurement and positioning of the remote device on the ground side, and implements fine secondary calibration of the laser projection point at the remote side, ensuring that the robotic arm accurately follows the preset target of the remote device. The preset target is located in the camera coordinate system and can be flexibly set to coincide with the origin, thereby effectively breaking through the distance limitation and significantly improving the accuracy and stability of spatial positioning, providing a solid technical guarantee and solution for the spatial positioning needs in various complex and changing environments.

[0059] Example 2

[0060] The present invention also discloses a three-dimensional space positioning method based on laser and camera fusion, which adopts the three-dimensional space positioning system based on laser and camera fusion of embodiment 1, and specifically includes the following steps:

[0061] Step 1: Place the mobile terminal system in the clear field of view of the ground camera by manually controlling the robotic arm, and then project the laser beam of the laser transmitter to the preset target area of ​​the mobile terminal system. During this period, the ground terminal system measures and records the distance d between the end of the robotic arm and the laser projection point in real time;

[0062] Step 2: The remote camera of the mobile terminal system captures the position information of the laser projection point in the remote camera coordinate system, and sends the position information to the ground terminal system to adjust and control the robotic arm;

[0063] Step 3: The ground terminal system uses the MDH model to finally determine and output the position of the laser projection point on the mobile terminal system in the global coordinate system of the ground terminal based on the position of each joint of the robot arm, the distance d between the end of the robot arm and the laser projection point, and the position information of the laser projection point in the remote camera coordinate system. The full name of the MDH model is the Modified Denavit-Hartenberg model, where i represents the i-th joint. Represents the transformation matrix from the i-th joint to the i+1-th joint.

[0064] Step 4: The distance between the telephoto camera and the laser projection area is fixed. After the telephoto camera is calibrated, the position of the laser projection point in the telephoto camera coordinate system is calculated. where z c is the distance from the remote camera to the laser projection area, z c After the remote camera and the laser projection area are fixed, the value is determined to be constant; c ,y c ,z c Represents the 3D spatial position of the laser projection point in the far camera coordinate system.

[0065] Step 5: The position of the laser projection point in the known remote camera coordinate system The position of the laser projection point in the global coordinate system of the ground end And the relative posture relationship between the remote camera coordinate system and the ground end global coordinate system Based on the relative posture relationship between the remote camera coordinate system and the ground end global coordinate system It can be directly calculated by the attitude sensors installed on the mobile terminal system and the ground terminal system, or indirectly obtained by using the optimization estimation algorithm to further solve the position of the origin of the remote camera coordinate system in the global coordinate system of the ground terminal.

[0066] Obviously, the above embodiments are merely examples for the purpose of clear explanation, and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the invention.

Claims

1. A three-dimensional spatial positioning system based on laser and camera fusion, characterized in that: It includes a ground terminal system and a mobile terminal system, wherein; The ground terminal system includes a laser transmitter, a ground camera and a mechanical arm, wherein the mechanical arm is arranged on the ground, and the laser transmitter and the ground camera are respectively connected to the ends of the mechanical arm; The mobile terminal system includes a mobile device and a remote camera, wherein a preset target point is arranged on the outer surface of the mobile device, the remote camera is fixed on the mobile device, the remote camera faces the area for photographing the preset target point, and the distance between the remote camera and the preset target point is fixed; The laser projection point of the laser transmitter locks the preset target area; the laser transmitter is used to measure the distance between the end of the robot arm and the preset target on the mobile terminal system; The ground camera captures the image information of the mobile terminal system in real time, and guides the robotic arm to perform precise tracking through visual feedback, so that the laser projection point is always located in the preset target area of ​​the mobile device; The robotic arm automatically adjusts its posture to follow the movement of the mobile terminal system based on the ground camera feedback and the distance data of the laser transmitter, so that the laser projection point of the laser transmitter coincides with the preset target point on the mobile terminal system.

2. A three-dimensional spatial positioning system based on laser and camera fusion according to claim 1, characterized in that: The mechanical arm and the mobile device are both provided with posture sensors.

3. The three-dimensional spatial positioning system based on laser and camera fusion according to claim 2, characterized in that: The remote camera captures the position of the laser projection point in the remote camera coordinate system, combines the posture sensor data on the mobile device and the coordinate information of the remote camera, obtains the spatial position of the laser projection point in the remote device's own coordinate system, and controls the robotic arm to accurately track the preset target.

4. The three-dimensional spatial positioning system based on laser and camera fusion according to claim 3, characterized in that: It also includes a spatial solver subsystem, which has an embedded spatial calculation model. The spatial calculation model integrates the joint positions of the robotic arm, the distance data of the laser emitter, the spatial position of the laser projection point in the remote device's own coordinate system, and the relative posture relationship between the remote camera coordinate system and the global coordinate system of the ground end, to obtain the position of the origin of the remote camera coordinate system in the global coordinate system of the ground end.

5. The three-dimensional spatial positioning system based on laser and camera fusion according to claim 3, characterized in that: The robotic arm can adjust the three-dimensional angle in real time, and the robotic arm includes a multi-joint robotic arm or a two-dimensional pan-tilt platform.

6. The three-dimensional spatial positioning system based on laser and camera fusion according to claim 3, characterized in that: The mobile device includes a drone or a mobile robot.

7. A three-dimensional space positioning method based on laser and camera fusion, characterized in that: The three-dimensional spatial positioning system based on laser and camera fusion as described in any one of claims 4 to 7 specifically comprises the following steps: Step 1: Place the mobile terminal system in the clear field of view of the ground camera by manually controlling the robotic arm, and then project the laser beam of the laser transmitter to the preset target area of ​​the mobile terminal system. During this period, the ground terminal system measures and records the distance d between the end of the robotic arm and the laser projection point in real time; Step 2: The remote camera of the mobile terminal system captures the position information of the laser projection point in the remote camera coordinate system, and sends the position information to the ground terminal system to adjust and control the robotic arm; Step 3: The ground terminal system uses the MDH model to finally determine and output the position of the laser projection point on the mobile terminal system in the global coordinate system of the ground terminal based on the position of each joint of the robot arm, the distance d between the end of the robot arm and the laser projection point, and the position information of the laser projection point in the remote camera coordinate system. Where i represents the i-th joint, Represents the transformation matrix from the i-th joint to the i+1-th joint; Step 4: The distance between the telephoto camera and the laser projection area is fixed. After the telephoto camera is calibrated, the position of the laser projection point in the telephoto camera coordinate system is calculated. where z c is the distance from the remote camera to the laser projection area, z c Once the remote camera and the laser projection area are fixed, the value is determined to be constant; where x c ,y c ,z c Represents the three-dimensional spatial position of the laser projection point in the remote camera coordinate system; Step 5: The position of the laser projection point in the known remote camera coordinate system The position of the laser projection point in the global coordinate system of the ground end And the relative posture relationship between the remote camera coordinate system and the ground end global coordinate system Based on this, solve the position of the origin of the remote camera coordinate system in the global coordinate system of the ground end.

8. The three-dimensional space positioning method based on laser and camera fusion according to claim 7, characterized in that: The relative posture relationship between the remote camera coordinate system and the ground end global coordinate system in step 5 It can be directly calculated by the attitude sensors installed on the mobile terminal system and the ground terminal system, or obtained indirectly by using the optimization estimation algorithm.

Images