Simultaneous localization and mapping based on mutual observation of heterogeneous unmanned systems

Abstract

The present invention relates to a simultaneous positioning and mapping method based on mutual observation between heterogeneous unmanned systems, specifically for intelligent transportation. One implementation of the method addresses the problem of unmanned vehicles and drones jointly performing tasks (such as inspections, cargo delivery, and surveys) within the same spatial and temporal range. This method achieves accurate positioning through observation of the environment and each other between the drone and the vehicle, without relying on external information such as GNSS and RTK, but using only onboard / onboard sensing equipment. This method achieves superior positioning accuracy for the collaborative unmanned vehicle and drone, providing positioning information for further high-precision operations.

Description

Technical Field

[0001] The present disclosure relates to intelligent transportation, and in particular to a simultaneous positioning and mapping method based on mutual observation of heterogeneous unmanned systems. Background Art

[0002] Accurate environmental perception and self-state estimation are important foundations for unmanned systems to autonomously perform various high-level tasks. In general scenarios with normal lighting, rich textures, and distinct structural features, existing simultaneous localization and mapping (SLAM) methods have achieved very stable performance. For example, on classic indoor datasets, the highest SLAM positioning accuracy has reached the centimeter level. However, the simultaneous localization and mapping of robots in perception-degraded environments (such as tunnels, mines, and narrow corridors) remains a challenging problem. The main manifestations are: (1) Occlusion between the robot and the open space prevents the unmanned system from receiving external positioning signals such as GPS or RTK; (2) Highly similar structures increase the probability of odometry mismatching, which in turn easily leads to incorrect loop detection. Once the incorrect loop is introduced into map optimization, it will cause serious damage to the global map; (3) The large spatial scale causes the cumulative error of the odometry to increase significantly with the running time, and even the use of loop correction methods has very limited improvement in accuracy.

[0003] In order to achieve precise positioning in complex environments without relying on external information, it is an effective solution to coordinate UAVs and unmanned vehicles and give full play to their respective advantages to solve the problem of positioning loss caused by degradation of environmental perception conditions and self-motion.

[0004] (1) Publication No. CN210377164U discloses an air-ground collaborative operation system, such as Figure 1As shown, the system includes: an unmanned vehicle (UAV) that constructs a three-dimensional map of the target scene by moving on the ground and locates its own position in the map coordinate system of the three-dimensional map in real time; a drone (UAV) for performing operations on the target; a data collector (DCU) for obtaining a first relative position between the UAV and the UAV, and a second relative position between the target and the UAV; and a data processor (DCU) connected to the UAV, the UAV, and the DCU for obtaining a third relative position between the UAV and the target based on the UAV's position in the map coordinate system, the first relative position, and the second relative position. The processor then transmits the third relative position to the UAV, allowing the UAV to adjust its position to a preset operating range based on the third relative position. This air-ground collaborative operation system uses the UAV to assist the UAV in accurately locating itself within the target scene. While this method can also achieve the effect of UAV-UAV collaborative operations, it only places AprilTags on the UAV to assist the UAV in positioning, without considering the use of mutual observation information between the UAV and the UAV for collaborative positioning, thus insufficiently utilizing available information.

[0005] (2) Publication No. CN217716438U discloses a collaborative navigation device for unmanned vehicles and unmanned aerial vehicles, such as Figure 2 As shown, it includes a drone, a QGC ground station, and an unmanned vehicle. The drone is equipped with a GPS positioning device, a lithium battery pack, a first wireless image transmission module, a binocular camera, a second wireless data transmission module, a support plate, landing gear, and a flight controller. The flight controller includes a main controller, a first I / O controller, a first IMU inertial navigation module, and a GPS module. The QGC ground station is equipped with a first wireless data transmission module. The unmanned vehicle is equipped with an MC bottom-level control module, a host computer graphical interface display module, a speed controller, a steering controller, a second I / O controller, and a second wireless image transmission module. The MC bottom-level control module consists of an onboard controller and a second IMU inertial navigation module. This utility model solves the problems of traditional navigation devices with small recognition range, large delay, and significant influence from GPS signals. It designs a collaborative navigation device for unmanned vehicles and drones. However, this utility model relies on the GPS module to provide global positioning information, which cannot be used in scenarios where GPS signals are absent, such as mines, caves, and canyons with tall buildings.

[0006] (3) Publication No. CN114923477A discloses a multi-dimensional air-ground collaborative mapping system and method based on vision and laser SLAM technology, such as Figure 3As shown in Figure 1, the system includes a ground station, a drone, and an unmanned vehicle. The drone comprises an overall physical architecture, an onboard embedded computer, and image sensors; the overall physical architecture includes a frame, quadrotors, and motors; the onboard embedded computer includes a communication module and an embedded processor, and the image sensors include a binocular camera and an RGB-D camera. The unmanned vehicle includes an embedded processor, a lidar sensor, and a communication module. The ground station includes display controls, control controls, and communication controls. The method utilizes control and mapping commands output by the ground station to fuse the 3D point cloud map constructed by the drone with the 2D planar grid map constructed by the drone using SLAM technology. This invention improves the safety of related operations by remotely controlling the collection of environmental information, eliminates positioning errors caused by factors such as weak GPS signals, and improves mapping accuracy. However, this method only uses structural information in the scene for map fusion between the drone and the drone, and does not utilize mutual observation between the drone and the vehicle, resulting in insufficient information utilization. Furthermore, in perception-degraded environments, map matching errors are prone to occur due to the heterogeneous perspectives of the drone and the vehicle, leading to significant drift in the entire positioning system.

[0007] (4) Publication No. CN111707988A discloses an unmanned vehicle positioning system and positioning method based on an unmanned vehicle-mounted UWB base station, such as Figure 4 As shown, the method includes four UWB anchor nodes on the unmanned vehicle, a positioning calculation module and a UWB tag of the drone, and establishes a coordinate system on the unmanned vehicle. During the movement of the drone and unmanned vehicle formation, the four UWB anchor nodes on the unmanned vehicle respectively obtain the distance information of the drone's UWB tag in real time, and calculate the position of the drone in the coordinate system on the unmanned vehicle based on the three-dimensional Euclidean distance calculation method, providing position information for maintaining the formation. However, this method requires the installation of UWB signal transceiver modules on the drone and the unmanned vehicle, which increases the hardware cost and limits the application scenarios of the method.

[0008] (5) The paper “Omni-Swarm: A Decentralized Omnidirectional Visual–Inertial–UWB State Estimation System for Aerial Swarms” uses mutual observation between multiple drones and UWB distance measurement to locate the drone cluster, such as Figure 5This paper proposes a decentralized omnidirectional visual-inertial (UWB) state estimation system (Omni-Swarm) for aerial swarms. To address observability issues, complex initialization, insufficient accuracy, and lack of global consistency, an omnidirectional perception system is introduced as the swarm's front-end. This system consists of omnidirectional sensors, including stereo fisheye cameras and ultra-wideband (UWB) sensors, and algorithms, including fisheye visual-inertial odometry (VIO), to achieve multi-UAV map-based localization and visual object detection. Graph-based optimization and forward propagation serve as the back-end of Omni-Swarm to fuse the front-end's measurement results. Experimental results show that the proposed decentralized state estimation method for swarm systems achieves centimeter-level relative state estimation accuracy while ensuring global consistency. Although this method utilizes mutual observation information between UAVs for localization, it lacks autonomy, meaning it cannot distinguish between the positioning results of UAVs in good and poor perception conditions. This leads to the disadvantage that drift in the positioning results of one UAV in the swarm can cause instability in the positioning of the entire swarm. Summary of the Invention

[0009] In response to the above-mentioned existing technologies, the main problem that the present invention solves is at least how to achieve accurate positioning through observation of the environment and each other between the drone and the unmanned vehicle when the unmanned vehicle and the drone jointly perform tasks (such as inspection, cargo transportation, and survey) within the same time and space range, without relying on external information such as GNSS and RTK, and only using airborne / vehicle-mounted sensing equipment, so that the collaborative positioning accuracy of the unmanned vehicle and drone is better than the positioning accuracy of a single machine, providing positioning information for performing further high-precision operations.

[0010] In order to solve the above technical problems, the technical solutions of the present invention are as follows.

[0011] In a first aspect, the present invention proposes a method for simultaneous positioning and mapping based on mutual observation of heterogeneous unmanned systems, the method comprising the following steps:

[0012] S100, making the first robot stationary in the critical area and the second robot located in the exploration area, the first robot having a first binocular camera and the second robot having a second binocular camera;

[0013] S200, establishing an occupancy grid map based on the depth images of the first binocular camera and the second binocular camera, detecting obstacles in the exploration area, and generating a safe area;

[0014] S300, performing path planning within the safe area, adjusting the path based on the real-time field of view of the second binocular camera in the exploration area and the relative position between the second robot and the first robot until the second robot moves to the new critical area, thereby completing map construction of the exploration area;

[0015] The critical area is an area where the first robot or the second robot can keep itself in a stationary state relative to the environment, and the exploration area is an area where a map needs to be established.

[0016] In the above technical solution, the first and second robots are robots in a broad sense, and can be drones, unmanned vehicles, handheld devices, or other portable devices. The first and second robots collaborate to build a map, addressing situations where external positioning information is unavailable or severely inaccurate, yet environmental conditions in a specific area are required, such as in tunnels, mines, or uninhabited areas. By acquiring the relative positions of the robots in real time, the robots can avoid drift and loss of tracking due to the visual odometry of a single robot.

[0017] As a further improvement to the above technical solution, the method further includes the following steps:

[0018] S400: If there is still an exploration area, make the second robot stationary in the critical area for the first robot, make the first robot become the second robot, enter the new exploration area, and return to step S200.

[0019] In the above technical solution, an implementation method of generating a safe area is as follows:

[0020] Building a dense point cloud of the surrounding environment based on the first binocular camera and the second binocular camera;

[0021] Convert dense point clouds into occupancy grid maps , occupying the grid map There are three types of voxels: known obstacle voxels , known safe area voxels , unknown voxels ;

[0022] exist In the process, with the first robot as the center, within the first set distance, all the known safe area voxels in the blank area that are no more than the second set distance from the obstacle voxels are regarded as the safe area;

[0023] The first set distance is used to ensure the positioning accuracy between the first robot and the second robot.

[0024] In the above technical solution, S300 performs path planning using a graph-structure-based exploration path planning method.

[0025] In the above technical solution, the method uses the optical flow method to make the first robot stationary in the critical area.

[0026] In the above technical solution, the first robot has a first identifier, and the second robot has a second identifier. The first identifier and / or the second identifier are used to determine the relative position between the first robot and the second robot.

[0027] In the above technical solution, the relative position between the first robot and the second robot is estimated as follows:

[0028]

[0029] Where:

[0030] is the dynamic weight balancing factor; is the relative pose estimation of the second robot relative to the first robot obtained based on the first identifier, is the relative pose estimation of the first robot relative to the first robot obtained based on the second identifier, is the estimated value of the second robot's own posture obtained by its own binocular camera.

[0031] In the above technical solution, the dynamic weight balancing factor is estimated using the following formula:

[0032]

[0033] Where: is the maximum relative position between the first robot and the second robot, is the number of Fast feature points within the current field of view of the second robot’s binocular camera, is the distance between the first robot and the second robot.

[0034] In a second aspect, the present invention provides a computer-readable storage medium, characterized in that it stores a computer program that can be loaded by a processor and execute any of the above methods.

[0035] In a third aspect, the present invention proposes a simultaneous positioning and mapping device based on mutual observation of heterogeneous unmanned systems, the device comprising a first robot, a second robot, and a map construction module; the first robot is configured to be stationary in a critical area, and a first binocular camera is configured on the first robot; the second robot is configured to be movable in an exploration area, and a second binocular camera is configured on the second robot; the map construction module is configured to use a safe area generation unit and a path planning unit to complete map construction of the exploration area; the safe area generation unit is configured to: establish an occupancy grid map based on depth images of the first binocular camera and the second binocular camera, detect obstacles in the exploration area, and generate a safe area; the path planning unit is configured to: perform path planning in the safe area, and adjust the path based on the real-time field of view of the second binocular camera in the exploration area and the relative position between the second robot and the first robot until the second robot moves to a new critical area; the critical area is an area where the first robot or the second robot can keep itself stationary relative to the environment, and the exploration area is an area where a map needs to be established. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0037] Figure 1 , a structural block diagram of the air-ground collaborative operation system mentioned in the background technology;

[0038] Figure 2 , a schematic diagram of the structure of the unmanned vehicle and unmanned aerial vehicle collaborative navigation device mentioned in the background technology;

[0039] Figure 3 , the overall structural block diagram of the multi-dimensional air-ground collaborative mapping system based on vision and laser SLAM technology mentioned in the background technology;

[0040] Figure 4 , a schematic diagram of the positioning principle of the unmanned vehicle positioning system and positioning method based on the UWB base station on the unmanned vehicle mentioned in the background technology;

[0041] Figure 5 , a schematic diagram of positioning a drone cluster using mutual observation between multiple drones and UWB distance measurement as mentioned in the background technology;

[0042] Figure 6 , a schematic diagram of a camera structure according to an embodiment of the present invention;

[0043] Figure 7 , a schematic diagram of the architecture of a method for simultaneous positioning and mapping of an unmanned vehicle and a drone observing each other according to one embodiment of the present invention;

[0044] Figure 8 , a schematic diagram of an application scenario of a method for simultaneous positioning and mapping of an unmanned vehicle and a drone observing each other according to an embodiment of the present invention;

[0045] Figure 9 , a schematic diagram of a method flow chart under one embodiment of the present invention. DETAILED DESCRIPTION

[0046] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments.

[0047] In one embodiment, the first robot is a drone and the second robot is an unmanned vehicle. The drone and the unmanned vehicle jointly perform tasks (such as inspection, cargo transportation, and surveys) within the same time and space. Both the drone and the unmanned vehicle are equipped with a binocular camera, a fisheye camera, and multiple AprilTag QR code markers, such as Figure 6 As shown in the figure, the binocular camera is used to perceive the surrounding environment for positioning, mapping, and obstacle avoidance. The fisheye camera is used for mutual observation between the UAV and the unmanned vehicle. The AprilTag QR code is used as a marker on the UAV or unmanned vehicle to determine the relative position of the UAV and the unmanned vehicle.

[0048] In an environment with good perception conditions, the unmanned vehicle can carry the drone and perform positioning, mapping and obstacle avoidance only by relying on its own binocular camera; when the environmental perception conditions degrade, the unmanned vehicle and the drone observe each other and perform simultaneous positioning and mapping. The method architecture is as follows Figure 7 shown.

[0049] Among them, the degradation of perception conditions is mainly manifested in:

[0050] (1) There is an obstruction between the unmanned system and the open space, which makes it impossible for the unmanned system to receive external positioning signals such as GPS or RTK;

[0051] (2) Highly similar structures increase the probability of odometry mismatching, which in turn easily leads to erroneous loop detection. Once erroneous loops are introduced into map optimization, they will cause serious damage to the global map.

[0052] (3) The larger spatial scale causes the cumulative error of the odometer to increase significantly with the running time. Even if the loop correction method is used, the improvement in accuracy is very limited.

[0053] See also Figure 8 In the scenario shown, an unmanned vehicle and a drone start from a starting point and pass through an area with degraded perception. The drone and vehicle collaborate to locate and construct a map of the degraded area through mutual observation. This prevents visual odometry drift and tracking loss that can occur when a single vehicle passes through the degraded area. Throughout this process, the drone and vehicle are constantly able to observe each other, alternating between serving as reference points until they complete the task of passing through the degraded area.

[0054] based on Figure 7 The method architecture, the specific steps are shown in steps S101 to S401 below, such as Figure 9 As shown:

[0055] S101. Make the reference robot stationary in the critical area and make the exploration robot located in the exploration area.

[0056] Here, the term "robot" refers to drones or unmanned vehicles. The robots in a swarm are categorized based on their perceived environment. Robots in critical areas are designated as reference robots, while those that need to explore areas with degraded perception are designated as exploration robots. Initial role assignment is random, meaning both drones and unmanned vehicles can serve as both exploration and reference robots.

[0057] The exploration area is the area where a map needs to be established. In the current implementation, it is a perception-degraded area. The exploration robot can perform robust positioning by fusing the AprilTag posted by the reference robot and the results of its own visual odometry.

[0058] The critical position is the area where the first robot or the second robot can keep itself in a stationary state relative to the environment. In the current embodiment, the critical position is the position where the perception condition can meet the requirement of the robot to ensure that it is stationary based on the optical flow method. Whether a certain location meets the critical position requirement can be measured by the texture richness within the field of view of the binocular camera. In one embodiment, if the number of Fast feature points extracted within the field of view of a robot exceeds a certain threshold , then this location is suitable as a critical position. The Fast feature point extraction method iterates through all pixels. The only criterion for determining whether the current pixel is a feature point is to draw a circle with a radius of 3 pixels around the current pixel (16 points on the circle). The number of points whose pixel values differ significantly from the pixel value at the center of the circle is counted. If there are more than 9 points with significant differences, the pixel at the center of the circle is considered a feature point.

[0059] S201: Based on the depth images of the first binocular camera and the second binocular camera, establish an occupancy grid map, detect obstacles in the exploration area, and generate a safe area.

[0060] In this step, based on the perception fusion results of the exploration robot and the reference robot's binocular cameras, obstacles in the area to be explored are detected, and a safe exploration area is generated for the exploration robot.

[0061] Specifically, the binocular cameras of drones and unmanned vehicles use the binocular ranging principle to build a dense point cloud of the surrounding environment and convert the dense point cloud into a voxel with a side length of Occupancy grid map , there are three types of voxels, namely known obstacle voxels , known safe area voxels , unknown voxels As the robot moves through space performing tasks, The voxels representing obstacles are dynamically increased.

[0062] Since the UAV and the UGV each carry a binocular camera, the occupancy grid maps they generate have their own independent coordinates, which are recorded as and In this embodiment, The initial position is The position of the drone and the unmanned vehicle is determined by mutual positioning using AprilTag at time t. and Relative to The pose transformation.

[0063] exist In the figure, the reference robot is the center of the circle. The distance between all blank areas and obstacle pixels within the distance does not exceed the set safe area distance All known safe area voxels are the safe area , the robot can plan a path within this area. For the maximum relative position of the reference robot and the exploration robot, set The purpose of this is to prevent the AprilTag positioning accuracy from decreasing due to the distance between the reference robot and the exploration robot being too far.

[0064] S301. Perform path planning in the safe area. Based on the real-time field of view of the second binocular camera in the exploration area and the relative position between the reference robot and the exploration robot, adjust the path until the exploration robot moves to the new critical area, thereby completing the map construction of the exploration area.

[0065] In this step, one implementation method is to use a graph-based exploration path planning method to plan an exploration path for the exploration robot. After integrating the dense point clouds of the exploration robot and the reference robot into a global map to form an occupancy grid map, obstacle voxels and unknown voxels are determined, and a safe area is demarcated within a certain range. Within the safe area, a series of nodes in space are obtained through random sampling with a density not exceeding a certain value. The nodes are connected according to a threshold to form a graph structure, and the Dijkstra shortest path algorithm is used for local trajectory planning. Specifically:

[0066] First, determine the end point of local path planning. Define the exploration gain of a location as the location The number of unknown voxels in the neighborhood. The location with the largest exploration gain is the end point of local path planning .

[0067] Secondly, generate the graph structure. Generate random sampling points in the safe area and filter out the random sampling points whose distance to the nearest neighbor is less than and the distance is less than The points are connected with undirected edges, and finally a graph structure is generated locally. 、 is the set hyperparameter.

[0068] Finally, the Dijkstra shortest path algorithm is executed in the graph structure, starting from the current position of the exploration robot and ending at the end point determined within the safe area. The nodes passed through are interpolated to obtain the path Traj, and the interpolation method is such as cubic B-spline, Newton interpolation, etc.

[0069] The observation results of the reference robot, the observation results of the exploration robot, and the results of the exploration robot's visual odometry are linearly fused according to the distance between the two robots and the environmental perception conditions within the exploration robot's field of view to obtain the constructed map.

[0070] S401. If there is still an exploration area, the exploration robot is made the reference robot and is stationary in the critical area, and the reference robot is made the exploration robot and enters the new exploration area, and returns to step S201.

[0071] During the exploration robot's execution of the exploration task, the reference robot remains stationary at a critical position, that is, the perception conditions at this position can meet the requirements of ensuring that it is stationary. The exploration robot determines its own position by observing the reference robot and combining the observations of itself sent by the reference robot until the exploration robot reaches the next critical position, changes the robot role and continues the above steps.

[0072] The specific process is as follows:

[0073] First, according to the trajectory Traj obtained in step 3, the proportional-integral-derivative (PID) method is used to control the exploration robot to track the trajectory.

[0074] Secondly, in the process of exploring the robot's trajectory tracking, the reference robot is in a critical position, and the optical flow method is used to ensure that its position is stationary at the point , at this time the reference robot’s posture is .

[0075] While the exploration robot is tracking the trajectory, the two robots observe each other's AprilTag and obtain the following results: The exploration robot obtains its relative pose estimation relative to the reference robot by observing the AprilTag of the reference robot , the reference robot observes the exploration robot to obtain the pose estimate of itself relative to the exploration robot The exploration robot obtains its own pose estimation based on the visual odometry based on its own binocular camera .

[0076] Finally, the pose of the exploration robot can be expressed as:

[0077]

[0078] in, is the dynamic weight balancing factor, and its value is related to the distance between the two robots And explore the number of Fast feature points within the robot's current field of view Specifically, it is:

[0079]

[0080] in, is the maximum distance between two robots.

[0081] After the exploration robot reaches the focal point of the local path planning, it determines whether the location is a critical location. If so, the exploration robot and the reference robot are swapped, and the process returns to step S201 until the task is completed and the robot reaches an environmentally friendly area. If not, the exploration robot retreats along the original path until it reaches a critical location, swaps the exploration robot and the reference robot, and returns to step S201 until the entire area to be mapped is fully mapped.

[0082] In summary, the above implementation relies solely on binocular and fisheye cameras onboard drones and unmanned vehicles to achieve mutual observation and positioning in harsh perception environments. Previous approaches to robust multi-robot positioning in perception-degraded environments have largely relied on increasing sensor redundancy or relying on external positioning signals. For example, adding active light sources enables simultaneous positioning and mapping in low-light environments, adding ultra-wideband (UWB) positioning with anchor points enables positioning in complex environments like underground mines, and transmitting pseudo-GPS satellite signals from base stations enables positioning in tunnels. While these methods improve the ability of robots to simultaneously locate and map in perception-degraded environments, increasing the number and variety of sensors and auxiliary equipment carried by the robots increases the complexity of the entire unmanned system. This often limits the payload and computing power of mobile platforms such as drones and unmanned vehicles, limiting their scope of application. This method, however, relies on low-cost binocular and fisheye cameras and AprilTag QR code tags to achieve simultaneous positioning and mapping in perception-degraded environments. The impact of this on positioning is taken into account in the autonomous trajectory planning of drones and unmanned vehicles. While previous trajectory planning methods focused on minimizing distance or time, the trajectory planning approach employed in the aforementioned implementation comprehensively considers the robot's positioning capabilities within the scene. This approach maximizes the ability to prevent visual odometry-based positioning loss or drift, providing precise, unknown environmental information for robots to perform various tasks in complex environments.

[0083] It should be noted that the number of binocular cameras configured on the drone or unmanned vehicle in the above embodiment can be changed, and the number of drones or unmanned vehicles can also be changed, further improving to multi-machine positioning and map construction.

[0084] It should be noted that: in the above implementation, when the drone or unmanned vehicle is a handheld device or a portable device, the above implementation method can be used to manually assist in positioning and map construction of unknown areas. In the use of the above method, the corresponding pose estimation becomes a position estimation.

[0085] Through the above description of the embodiments, those skilled in the art will clearly understand that the present disclosure can be implemented using software plus necessary general-purpose hardware. Of course, it can also be implemented using dedicated hardware, including application-specific integrated circuits, dedicated CPUs, dedicated memories, and dedicated components. Generally speaking, any function performed by a computer program can be easily implemented using corresponding hardware. Moreover, the specific hardware structures used to implement the same function can also be diverse, such as analog circuits, digital circuits, or dedicated circuits. However, for the present disclosure, software implementation is often the preferred embodiment.

[0086] Although the embodiments of the present invention have been described above with reference to the accompanying drawings, the present invention is not limited to the above-mentioned specific embodiments and application fields. The above-mentioned specific embodiments are merely illustrative and instructive, and are not restrictive. A person skilled in the art, guided by this specification and without departing from the scope of protection of the claims of the present invention, may also devise various forms, all of which fall within the scope of protection of the present invention.

Claims

1. A simultaneous positioning and mapping method based on mutual observation of heterogeneous unmanned systems, characterized by: The method comprises the following steps: S100, making the first robot stationary in the critical area and the second robot located in the exploration area, the first robot having a first binocular camera and the second robot having a second binocular camera; S200, establishing an occupancy grid map based on the depth images of the first binocular camera and the second binocular camera, detecting obstacles in the exploration area, and generating a safe area; S300, performing path planning within the safe area, adjusting the path based on the real-time field of view of the second binocular camera in the exploration area and the relative position between the second robot and the first robot until the second robot moves to the new critical area, thereby completing map construction of the exploration area; The critical area is an area where the first robot or the second robot can keep itself in a stationary state relative to the environment, and the exploration area is an area where a map needs to be established; S400: If there is still an exploration area, make the second robot stationary in the critical area for the first robot, make the first robot become the second robot, enter the new exploration area, and return to step S200.

2. The method according to claim 1, characterized in that The S200 includes: Building a dense point cloud of the surrounding environment based on the first binocular camera and the second binocular camera; Convert dense point clouds into occupancy grid maps , occupying the grid map There are three types of voxels: known obstacle voxels , known safe area voxels , unknown voxels ; exist In the process, with the first robot as the center, within the first set distance, all the known safe area voxels in the blank area that are no more than the second set distance from the obstacle voxels are regarded as the safe area; The first set distance is used to ensure the positioning accuracy between the first robot and the second robot.

3. The method according to claim 1, characterized in that The S300 performs path planning using a graph-structure-based exploration path planning method.

4. The method according to claim 1, wherein The method adopts an optical flow method to make the first robot stationary in a critical area.

5. The method according to claim 1, wherein The first robot has a first identifier, and the second robot has a second identifier. The first identifier and / or the second identifier are used to determine the relative position between the first robot and the second robot.

6. The method according to claim 5, characterized in that The relative position between the first robot and the second robot is estimated as follows: Where: is the dynamic weight balancing factor; is the relative pose estimation of the second robot relative to the first robot obtained based on the first identifier, is the relative pose estimation of the first robot relative to the first robot obtained based on the second identifier, is the estimated value of the second robot's own posture obtained by its own binocular camera.

7. The method according to claim 6, characterized in that The dynamic weight balancing factor is estimated using the following formula: Where: is the maximum relative position between the first robot and the second robot, is the number of Fast feature points within the current field of view of the second robot’s binocular camera, is the distance between the first robot and the second robot.

8. A computer-readable storage medium, characterized in that: A computer program is stored which can be loaded by a processor and execute the method according to any one of claims 1 to 7.

9. A simultaneous positioning and mapping device based on mutual observation of heterogeneous unmanned systems, characterized by: The device includes a first robot, a second robot, and a map construction module; The first robot is configured to be stationary in the critical area and is equipped with a first binocular camera; the second robot is configured to be movable in the exploration area and is equipped with a second binocular camera; The map construction module is configured to utilize the safe area generation unit and the path planning unit to complete the map construction of the exploration area; The safe area generating unit is configured to: establish an occupancy grid map based on the depth images of the first binocular camera and the second binocular camera, detect obstacles in the exploration area, and generate a safe area; The path planning unit is configured to: perform path planning in the safe area, and adjust the path based on the real-time field of view of the second binocular camera in the exploration area and the relative position between the second robot and the first robot until the second robot moves to a new critical area; The critical area is an area where the first robot or the second robot can keep itself in a stationary state relative to the environment, and the exploration area is an area where a map needs to be established.

Images