Amphibious robot positioning method and system in cross-domain environment based on factor graph optimization

Abstract

The invention belongs to the field of intelligent positioning of robots, and provides a method and a system for positioning an amphibious robot in a cross-domain environment based on factor graph optimization. The position relation between the vision measurement unit and the water surface and the position relation between the radar measurement unit and the water surface can be accurately obtained, then the working state of the radar measurement unit and the working state of the vision measurement unit can be adaptively adjusted, and therefore a target odometer factor graph capable of representing the pose prediction result of the amphibious robot can be accurately obtained; the cost sum of all the odometer factors is minimized through the target factor graph optimization function, and the pose prediction result of each time node can be optimized according to the relevance among the odometer factors, so that the pose of the amphibious robot in the semi-transition state can be accurately predicted. Therefore, the positioning accuracy of the amphibious robot in the target time interval can be improved.

Description

Technical Field

[0001] This application belongs to the field of robot intelligent positioning, and more specifically, relates to a method and system for amphibious robot positioning in a cross-domain environment based on factor graph optimization. Background Technology

[0002] With the development of intelligent sensing technology, in order to break the constraints of a single working environment, researchers at home and abroad have been developing amphibious cross-domain unmanned systems to meet more complex actual task requirements and replace humans in performing more complex tasks.

[0003] During amphibious transitions between land and water, amphibious robots repeatedly switch between air and water. Traditional single-sensor positioning methods, such as laser sensors, become ineffective underwater due to surface fluctuations. While positioning methods combining laser and visual sensors can accurately measure amphibious robots both submerged and fully submerged, the complex water environments and repeated entry and exit of amphibious robots mean they are often in a semi-submerged state during these transitions. The images captured by the visual sensors are then affected by light refraction, deviating from the true values. Existing hybrid positioning methods, which still rely on the fusion of visual and radar measurement data, result in significant positioning errors during these transitions. Summary of the Invention

[0004] To address the aforementioned deficiencies in existing technologies, this application provides a method and system for amphibious robot localization in cross-domain environments based on factor graph optimization, aiming to solve the problem of inaccurate amphibious robot localization in cross-domain processes using existing methods.

[0005] In a first aspect, this application provides a method for amphibious robot localization in a cross-domain environment based on factor graph optimization, characterized by comprising: A target coordinate system is established based on the position information of the amphibious robot, and the position information of the radar measurement unit and the vision measurement unit in the target coordinate system within the target time interval is obtained; Acquire depth information of the amphibious robot, and based on the depth information, the position information of the radar measurement unit and the vision measurement unit, determine the operating status of the radar measurement unit and the vision measurement unit within the target time interval; Based on the measurement results of the radar measurement unit, vision measurement unit, and amphibious robot's IMU (Inertial Measurement Unit) within the target time interval, as well as the operating status of the radar measurement unit and vision measurement unit, the target odometry factor map corresponding to the amphibious robot is obtained. The target odometry factor map is used to characterize the target pose prediction result of the amphibious robot within the target time interval. A target factor graph optimization function is constructed, and the target odometry factor graph is optimized based on the target factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

[0006] Furthermore, the operational status of the radar measurement unit and the visual measurement unit within the target time interval is determined, including: The tilt angle of the amphibious robot along the direction of gravity is obtained, and the first water entry depth of the amphibious robot is obtained based on the depth information. Based on the tilt angle, the first water depth, and the position information of the radar measurement unit, the second water depth of the radar measurement unit is determined, and the operating status of the radar measurement unit is determined based on the second water depth. The operating status of the radar measurement unit is characterized as normal operation when the second water depth is less than zero.

[0007] Furthermore, the operational status of the radar measurement unit and the visual measurement unit within the target time interval is determined, including: Based on the tilt angle, the first water immersion depth, and the position information of the visual measurement unit, the third water immersion depth of the visual measurement unit is determined, and the operating status of the visual measurement unit is determined based on the third water immersion depth. When the absolute value of the third water immersion depth is greater than the first preset value, the operating status of the visual measurement unit is characterized as normal operation.

[0008] Since the radar and vision measurement units are fixed in their positions on the amphibious robot, their positions can be calculated using the robot's depth and tilt angle. Because the radar measurement unit uses laser point cloud measurements, it cannot be used underwater due to water surface fluctuations; therefore, its operation is considered normal when the second entry depth is less than zero. Similarly, the vision measurement unit experiences severe water surface refraction when near the surface, causing image distortion; therefore, its operation is considered normal when the absolute value of the third entry depth is greater than a first preset value. This adaptive switching of operating states reduces the risk of data deviation from the vision measurement unit, which could contaminate the target odometry factor map data when the amphibious robot is in a semi-submerged state.

[0009] Furthermore, obtain the target odometry factor map corresponding to the amphibious robot, including: Based on the measurement results of the radar measurement unit and IMU within the target time interval, a radar odometry factor is constructed, which is used to characterize the first pose prediction result of the amphibious robot. Based on the measurement results of the visual measurement unit and the IMU within the target time interval, a visual odometry factor is constructed, which is used to characterize the second pose prediction result of the amphibious robot. Based on the radar odometry factor corresponding to the radar measurement unit in normal operation and the visual odometry factor corresponding to the visual measurement unit in normal operation, a target odometry factor map is obtained.

[0010] The radar odometry factor is constructed to preserve the amphibious robot's pose prediction results acquired during the normal operation of the radar measurement unit. This process includes all time periods within the target time interval when the radar measurement unit is not in the water, including a portion of the time when the amphibious robot is in a semi-submerged state. The visual odometry factor, on the other hand, preserves the amphibious robot's pose prediction results acquired when the visual measurement unit's distance from the water surface exceeds a first preset value. Thus, a target odometry factor map can be obtained based on the radar and visual odometry factors. The target odometry factors in the target odometry factor map can be used to characterize all target pose prediction results of the amphibious robot within the target time interval.

[0011] Furthermore, the radar odometry factor is constructed, including: Based on the measurement results of the radar measurement unit within the target time interval, the dynamic distribution information of the laser point cloud measured by the radar measurement unit is obtained, and based on the dynamic distribution information of the laser point cloud, the first pose prediction result of the amphibious robot in the target coordinate system is obtained. The measurement results of the IMU are pre-integrated, and the radar odometry factor is constructed based on the pre-integrated measurement results of the IMU and the first pose prediction results.

[0012] Furthermore, visual odometry factors are constructed, including: Based on the measurement results of the visual measurement unit within the target time interval, a set of visual image frames of the visual measurement unit is obtained, and the second pose prediction result of the amphibious robot in the target coordinate system is obtained based on the set of visual image frames. The IMU measurement results are pre-integrated, and the visual odometry factor is constructed based on the pre-integrated IMU measurement results and the second pose prediction results.

[0013] Furthermore, a target factor graph optimization function is constructed, including: Based on the deviation between the IMU measurement results and the first pose prediction results, a first cost function is constructed. The first cost function is used to optimize the radar odometry factor in the target odometry factor map. Based on the deviation between the IMU measurement results and the second pose prediction results, a second cost function is constructed. The second cost function is used to optimize the visual odometry factor in the target odometry factor map. Based on the first cost function and the second cost function, a target factor graph optimization function is constructed.

[0014] Furthermore, the final pose prediction results of the amphibious robot within the target time interval are obtained, including: Based on the first cost function, obtain the cost sum of radar odometry factors in the target odometry factor map within the target time interval; Based on the second cost function, obtain the cost sum of the visual odometry factors within the target odometry factor map in the target time interval; Based on the cost sum of radar odometry factors and visual odometry factors, the cost sum of all odometry factors in the target odometry factor map is obtained, and the cost sum of odometry factors is minimized based on the target factor map optimization function to obtain the final pose prediction result.

[0015] Since the visual measurement unit does not enter normal operation within a certain distance from the water surface before or after entering or exiting the water due to severe water refraction, the target factor graph optimization function is used to minimize the cost of the odometry factors. This optimizes the second pose prediction result of the amphibious robot when the visual measurement unit passes through this distance, thereby enabling accurate prediction of the amphibious robot's pose at the corresponding time node in this short segment. At the same time, the pose prediction results at each time node can be optimized based on the correlation between all odometry factors, thereby improving the positioning accuracy of the amphibious robot within the target time interval.

[0016] Secondly, this application also provides an amphibious robot localization system based on factor graph optimization in a cross-domain environment, for implementing any of the methods in the first aspect, including: The data acquisition module is used to acquire depth information from the depth gauge, and measurement results from the radar measurement unit, vision measurement unit, and IMU within the target time interval. The operation status determination unit is used to acquire the depth information of the depth gauge, the position information of the radar measurement unit and the vision measurement unit, and determine the operation status of the radar measurement unit and the vision measurement unit based on the depth information, the position information of the radar measurement unit and the vision measurement unit. The odometry factor acquisition module is used to acquire the radar odometry factor and visual odometry factor within the target time interval; The target factor map acquisition module is used to acquire the target odometry factor map based on the radar odometry factor and visual odometry factor within the target time interval. The target factor graph optimization module is used to optimize the target odometry factor graph based on the factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

[0017] Thirdly, this application also provides an electronic device, comprising: at least one memory for storing a program; and at least one processor for executing the program stored in the memory, wherein when the program stored in the memory is executed, the processor is configured to execute the method described in any possible implementation of the first aspect.

[0018] Overall, compared with the prior art, the above-described technical solutions conceived by this invention have the following beneficial effects: 1. The method of this application can accurately obtain the positional relationship between the visual measurement unit and the radar measurement unit and the water surface by using the depth information of the amphibious robot, the positional relationship between the radar measurement unit and the visual measurement unit, and thus can adaptively adjust the working state of the radar measurement unit and the visual measurement unit, avoiding the distorted data obtained by the visual measurement unit when it is in a half-crossing state during the crossing process from affecting the accuracy of robot positioning, thereby accurately obtaining the target odometry factor map that can characterize the pose prediction result of the amphibious robot; 2. By minimizing the cost of all odometry factors through the target factor map optimization function, the pose prediction result at each time node can be optimized according to the correlation between each odometry factor, thereby accurately predicting the pose of the amphibious robot when it is in a half-crossing state, thereby improving the positioning accuracy of the amphibious robot within the target time interval. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in this application or related technologies, the accompanying drawings used in the description of the embodiments or related technologies will be briefly introduced one by one below. Obviously, the accompanying drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 This is a flowchart illustrating the amphibious robot localization method in a cross-domain environment based on factor graph optimization provided in this application embodiment.

[0021] Figure 2 This is a schematic diagram of the structure of the amphibious robot provided in the embodiments of this application.

[0022] Figure 3 This is a schematic diagram of the workflow of the amphibious robot provided in the embodiments of this application.

[0023] Figure 4 This is a schematic diagram of the optimization process of the target odometer factor map provided in the embodiments of this application.

[0024] Figure 5 This is a schematic diagram of the structure of an amphibious robot positioning system in a cross-domain environment based on factor graph optimization, provided in an embodiment of this application.

[0025] Figure 6This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0026] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings.

[0027] In the following description, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance. The following description provides multiple embodiments of this application, which can be substituted or combined with each other. Therefore, this application can also be considered to include all possible combinations of the same and / or different embodiments described. Thus, if one embodiment includes features A, B, and C, and another embodiment includes features B and D, then this application should also be considered to include embodiments containing one or more other possible combinations of A, B, C, and D, even if such embodiments are not explicitly described in the following text.

[0028] The following description provides examples and does not limit the scope, applicability, or examples set forth in the claims. Changes may be made to the function and arrangement of the described elements without departing from the scope of this application. Various processes or components may be appropriately omitted, substituted, or added to the examples. For example, the described methods may be performed in a different order than described, and various steps may be added, omitted, or combined. Furthermore, features described with respect to some examples may be combined into other examples.

[0029] Figure 1 This is a flowchart illustrating the amphibious robot localization method based on factor graph optimization in a cross-domain environment provided in this application embodiment. Figure 1 As shown, the method includes at least the following steps: S1. Establish a target coordinate system based on the position information of the amphibious robot, and obtain the position information of the radar measurement unit and the vision measurement unit in the target coordinate system within the target time interval.

[0030] In this application embodiment, the executing entity of the method can be the MCU of the amphibious robot or a remote data processing platform, such as... Figure 2 As shown, in this embodiment, the radar measurement unit can be a lidar, and the vision measurement unit can be a vision camera. The lidar can predict the amphibious robot's pose (position and attitude) changes by measuring the dynamic distribution of laser point clouds at intersections during the amphibious robot's movement.

[0031] A lidar and a vision camera are mounted sequentially from top to bottom on the amphibious robot. A depth gauge is also installed on the bottom of the robot to measure its depth in the water. The lidar's operating range does not include the water, so it is positioned above the vision camera. In this embodiment, the target coordinate system is established based on the amphibious robot's position information. The center of the target coordinate system can coincide with either the amphibious robot's center of mass or the center of mass of its IMU (Integrated Mutor Unit). To facilitate the calculation of the lidar, depth gauge, and vision camera's position information, the center of the target coordinate system in this embodiment is set to coincide with the IMU's center of mass.

[0032] S2. Obtain the depth information of the amphibious robot, and based on the depth information, the position information of the radar measurement unit and the vision measurement unit, determine the operating status of the radar measurement unit and the vision measurement unit within the target time interval.

[0033] In the embodiments of this application, such as Figure 3 As shown in the figure, the lidar and vision camera correspond to the radar measurement unit and vision measurement unit in this embodiment, respectively. During amphibious robot operations, it switches between land and underwater states. Due to the influence of waves in the water, the radar measurement unit cannot perform laser point cloud mapping underwater. Furthermore, the positioning accuracy of the vision measurement unit above the water surface is not as accurate as that of the radar measurement unit. Therefore, adaptively adjusting the operating state based on the water depth of the radar and vision measurement units helps reduce the amphibious robot's power consumption and avoids introducing a large amount of large-error interference data when multiple sensors simultaneously perform pose prediction.

[0034] In one possible implementation, determining the operating status of the radar measurement unit and the visual measurement unit within a target time interval includes: The tilt angle of the amphibious robot along the direction of gravity is obtained, and the first water entry depth of the amphibious robot is obtained based on the depth information. Based on the tilt angle, the first water entry depth, and the position information of the radar measurement unit, the second water entry depth of the radar measurement unit is determined, and the operating status of the radar measurement unit is determined based on the second water entry depth. The operating status of the radar measurement unit is characterized as normal operation when the second water entry depth is less than zero. Based on the tilt angle, the first water immersion depth, and the position information of the visual measurement unit, the third water immersion depth of the visual measurement unit is determined, and the operating status of the visual measurement unit is determined based on the third water immersion depth. When the absolute value of the third water immersion depth is greater than the first preset value, the operating status of the visual measurement unit is characterized as normal operation.

[0035] In the embodiments of this application, such as Figure 3As shown in the figure, the lidar and vision camera correspond to the radar measurement unit and vision measurement unit in this embodiment, respectively. In this embodiment, the tilt angle of the amphibious robot along the direction of gravity can be obtained in various ways, such as using a depth gauge level or the amphibious robot's built-in gyroscope. Since the depth gauge, vision measurement unit, and radar measurement unit are fixed in this embodiment, the water entry depth of the vision measurement unit and radar measurement unit can be determined using the depth measured by the depth gauge and the relative positional relationship between the amphibious robot and the vision and radar measurement units. Furthermore, since the vision camera is mainly used to acquire the amphibious robot's underwater pose, the vision measurement unit is positioned below the radar measurement unit, and the third water entry depth corresponding to the vision measurement unit is greater than the first water entry depth of the radar measurement unit.

[0036] Since the visual measurement unit is affected by the light refracted by the water surface before and after being fully submerged in water and at a certain distance from the water surface, it cannot obtain effective image frames. Therefore, a first preset value is set to adjust the operating state of the visual measurement unit. The first preset value can be set in advance according to factors such as the lens position of the visual measurement unit and the degree of turbidity of the liquid.

[0037] S3. Based on the measurement results of the radar measurement unit, the vision measurement unit and the IMU of the amphibious robot within the target time interval, as well as the operating status of the radar measurement unit and the vision measurement unit, obtain the target odometry factor map corresponding to the amphibious robot. The target odometry factor map is used to characterize the target pose prediction result of the amphibious robot in the target time interval.

[0038] In this embodiment, the IMU of the amphibious robot is a device that uses inertial measurement to measure the three-axis attitude angles (or angular rates) and acceleration of an object. The IMU is a standard unit built into the amphibious robot. An IMU typically includes three single-axis accelerometers and three single-axis gyroscopes. It can be used... Time's up IMU measurement prediction at time 10:00 Time's up Robot pose state transformation at all times :

[0039]

[0040]

[0041] in They represent The robot's velocity vector, rotation vector, and translation vector at any given moment. They represent Time's up The increments of the robot's velocity vector, rotation vector, and translation vector at any given time. express Time's up The time difference between moments This represents gravitational acceleration. The radar measurement unit calculates and predicts the amphibious robot's pose changes by measuring the distribution of point clouds. The visual measurement unit predicts the amphibious robot's pose changes over a short period by matching image features between frames. During the amphibious robot's crossing of the water, the visual measurement unit may measure a large amount of distorted data when close to the water surface. To avoid excessive deviation in the data measured by the radar or visual measurement unit at this time, which would interfere with the final pose prediction result, the visual measurement unit needs to be turned off, and a target odometry factor map needs to be built based on the odometry factors measured in other motion states. In the target odometry factor map, variable nodes typically represent the robot's pose at different times, and factor nodes represent the constraints between these poses. These constraints come from odometry measurement data, and each factor node corresponds to an odometry measurement value, which describes the relative motion between the robot's poses at two adjacent times. By constructing and optimizing the odometry factor map, the robot's pose can be estimated more accurately, reducing the impact of error accumulation, especially the measurement error when the robot is in a semi-aquatic state.

[0042] In one possible implementation, obtaining the target odometry factor map corresponding to the amphibious robot includes: Based on the measurement results of the radar measurement unit and IMU within the target time interval, a radar odometry factor is constructed, which is used to characterize the first pose prediction result of the amphibious robot. Based on the measurement results of the visual measurement unit and the IMU within the target time interval, a visual odometry factor is constructed, which is used to characterize the second pose prediction result of the amphibious robot. Based on the radar odometry factor corresponding to the radar measurement unit in normal operation and the visual odometry factor corresponding to the visual measurement unit in normal operation, a target odometry factor map is obtained.

[0043] In this embodiment of the application, the odometer factor in the target odometer factor map consists of radar odometer factor and visual odometer factor. For example... Figure 3As shown, the radar measurement unit is operational when it is not fully submerged in the water, continuously acquiring radar odometry factors at various times. The time interval between adjacent moments can be set as needed. The visual measurement unit is operational except during periods when its distance from the water surface is less than a first preset value, continuously acquiring visual odometry factors at various times. Because the radar measurement unit is located above the visual measurement unit, it can still function normally during the periods when the visual measurement unit is not operating, thus acquiring radar odometry factors that can be used to predict the amphibious robot's pose at that moment.

[0044] In one possible implementation, the radar odometry factor is constructed, including: Based on the measurement results of the radar measurement unit within the target time interval, the dynamic distribution information of the laser point cloud measured by the radar measurement unit is obtained, and based on the dynamic distribution information of the laser point cloud, the first pose prediction result of the amphibious robot in the target coordinate system is obtained. The measurement results of the IMU are pre-integrated, and the radar odometry factor is constructed based on the pre-integrated measurement results of the IMU and the first pose prediction results.

[0045] In the embodiments of this application, such as Figure 4 As shown, in the target coordinate system, the radar odometry factor is constrained by IMU pre-integration and radar measurement unit point cloud matching, where for the IMU, its measured angular velocity... and acceleration It can be represented as:

[0046] in These are the actual values ​​of angular velocity and acceleration, respectively. For the measurement noise of angular velocity and acceleration, Let be the rotation matrix of the target coordinate system. These are the zero-bias errors of angular velocity and acceleration, respectively. Let gravitational acceleration be the acceleration due to gravity. According to the above formula, we can... Time's up IMU measurement prediction at time 10:00 Time's up Robot state transition at any moment .

[0047] For the radar measurement unit, the relative pose is calculated by matching laser point clouds. Let the radar measurement unit be in... Time and The state pose at time t is:

[0048] in for The pose of the radar measurement unit at any given time. These represent the rotation and translation of the radar measurement unit, respectively. By pre-integrating the angular velocity and acceleration recorded by the IMU, the translational and rotational increments of the amphibious robot from time i to time j based on the IMU data can be obtained. , Then the radar odometry factor can be constructed. The construction process is as follows:

[0049] in , This represents the robot's translation and rotation increments from time i to time j, obtained based on radar point cloud matching. Represents the matrix logarithm, used to convert a rotation matrix into an increment of angular velocity.

[0050] In one possible implementation, constructing the visual odometry factor includes: Based on the measurement results of the visual measurement unit within the target time interval, a set of visual image frames of the visual measurement unit is obtained, and the second pose prediction result of the amphibious robot in the target coordinate system is obtained based on the set of visual image frames. The IMU measurement results are pre-integrated, and the visual odometry factor is constructed based on the pre-integrated IMU measurement results and the second pose prediction results.

[0051] In the embodiments of this application, such as Figure 4 As shown, the visual odometry factor calculates the relative pose by extracting visual features from each frame of the image and matching them between frames. Let the visual measurement unit be in... Time and The pose at time t is:

[0052] in for The pose of the visual measurement unit at any given time. and These represent the rotation and translation of the visual measurement unit, respectively.

[0053] Through the formula: , , This allows us to obtain the amphibious robot's translational increment from time i to time j corresponding to the visual measurement unit. Rotational increment This allows for the construction of visual odometry factors. The construction process is as follows:

[0054] in, The matrix logarithm is used to convert the rotation matrix into an increment of angular velocity. In this embodiment, the pre-integration processing of the IMU measurement results is performed to effectively fuse the IMU measurement data with the data from the vision measurement unit or radar measurement unit. If the IMU data is integrated from the initial moment to calculate the robot pose at the current moment, the computational load would be excessive, and integration would need to be re-performed when the reference frame changes. The main purpose of pre-integration processing is to reduce the computational load and to easily update the integration results when the reference frame changes.

[0055] In one possible implementation, constructing a target factor graph optimization function includes: Based on the deviation between the IMU measurement results and the first pose prediction results, a first cost function is constructed. The first cost function is used to optimize the radar odometry factor in the target odometry factor map. Based on the deviation between the IMU measurement results and the second pose prediction results, a second cost function is constructed. The second cost function is used to optimize the visual odometry factor in the target odometry factor map. Based on the first cost function and the second cost function, a target factor graph optimization function is constructed.

[0056] In this embodiment of the application, during the factor graph optimization process, record robot state at any moment The expression is: ,in Let be a rotation matrix, representing the robot's orientation. For position vectors, , This is the velocity vector.

[0057] For radar odometry factor, radar odometry factor in The measured value at time is Its error term is The corresponding first cost function ,in It is the information matrix corresponding to the measured values ​​of the radar odometer factor.

[0058] For the visual odometry factor, the visual measurement unit in The measured value at time is Its error term is The corresponding second cost function ,in It is an information matrix corresponding to the measured values ​​of the visual odometry factor.

[0059] like Figure 4As shown, to obtain the final pose prediction result, the sum of the costs of all odometry factors needs to be minimized. Therefore, the expression for the factor graph optimization function is:

[0060] in It is the set of pose states of the amphibious robot to be optimized at each time point within the target time interval.

[0061] S4. Construct the target factor graph optimization function, and optimize the target odometry factor graph based on the target factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

[0062] In one possible implementation, the final pose prediction result of the amphibious robot within the target time interval is obtained, including: Based on the first cost function, obtain the cost sum of radar odometry factors in the target odometry factor map within the target time interval; Based on the second cost function, obtain the cost sum of the visual odometry factors within the target odometry factor map in the target time interval; Based on the cost sum of radar odometry factors and visual odometry factors, the cost sum of all odometry factors in the target odometry factor map is obtained, and the cost sum of odometry factors is minimized based on the target factor map optimization function to obtain the final pose prediction result.

[0063] Figure 5 This is a schematic diagram of the structure of an amphibious robot localization system based on factor graph optimization in a cross-domain environment, as provided in the embodiments of this application. Figure 5 As shown, the system includes at least: The data acquisition module is used to acquire the depth information of the amphibious robot, as well as the measurement results from the radar measurement unit, vision measurement unit, and IMU within the target time interval. The operation status determination unit is used to acquire the depth information of the amphibious robot, the position information of the radar measurement unit and the vision measurement unit, and determine the operation status of the radar measurement unit and the vision measurement unit based on the depth information and the position information of the radar measurement unit and the vision measurement unit. The odometry factor acquisition module is used to acquire the radar odometry factor and visual odometry factor within the target time interval; The target factor map acquisition module is used to acquire the target odometry factor map based on the radar odometry factor and visual odometry factor within the target time interval. The target factor graph optimization module is used to optimize the target odometry factor graph based on the factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

[0064] like Figure 6 As shown, Figure 6 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. The electronic device may include: a processor 601, a communications interface 602, a memory 603, and a communication bus 604, wherein the processor 601, the communications interface 602, and the memory 603 communicate with each other through the communication bus 604. The processor 601 can call software instructions in the memory 603 to execute the methods described in the above embodiments.

[0065] Furthermore, the logical instructions in the aforementioned memory 603 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application.

[0066] Based on the methods in the above embodiments, this application provides a computer-readable storage medium storing a computer program that, when run on a processor, causes the processor to execute the methods in the above embodiments.

[0067] Based on the methods in the above embodiments, this application provides a computer program product that, when run on a processor, causes the processor to execute the methods in the above embodiments.

[0068] It is understood that the processor in the embodiments of this application can be a CPU (Central Processing Unit), or other general-purpose processors, DSPs (Digital Signal Processors), ASICs (Application Specific Integrated Circuits), FPGAs (Field Programmable Gate Arrays), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. A general-purpose processor can be a microprocessor or any conventional processor.

[0069] The method steps in this application embodiment can be implemented in hardware or by a processor executing software instructions. The software instructions can consist of corresponding software modules, which can be stored in random access memory (RAM), flash memory, ROM (Read-only Memory), PROM (Programmable ROM), EPROM (Erasable PROM), EEPROM (Electrically Erasable EPROM), registers, hard disks, portable hard disks, CD-ROMs, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor, enabling the processor to read information from and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can reside in an ASIC.

[0070] In the above embodiments, implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, as a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the flow or function according to the embodiments of this application is generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in or transmitted through a computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line DSL) or wireless (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., SSD (Solid State Disk)).

[0071] It is understood that the various numerical designations used in the embodiments of this application are merely for the convenience of description and are not intended to limit the scope of the embodiments of this application.

[0072] Those skilled in the art will readily understand that the above are merely preferred embodiments of this application and are not intended to limit this application. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A method for amphibious robot localization in a cross-domain environment based on factor graph optimization, characterized in that, include: A target coordinate system is established based on the position information of the amphibious robot, and the position information of the radar measurement unit and the vision measurement unit in the target coordinate system within the target time interval is obtained; The depth information of the amphibious robot is acquired, and based on the depth information, the position information of the radar measurement unit and the vision measurement unit, the operating status of the radar measurement unit and the vision measurement unit within the target time interval is determined. Based on the measurement results of the radar measurement unit, the vision measurement unit, and the IMU of the amphibious robot within the target time interval, as well as the operating status of the radar measurement unit and the vision measurement unit, a target odometry factor map corresponding to the amphibious robot is obtained. The target odometry factor map is used to characterize the target pose prediction result of the amphibious robot in the target time interval. A target factor graph optimization function is constructed, and the target odometry factor graph is optimized based on the target factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

2. The amphibious robot localization method in a cross-domain environment according to claim 1, characterized in that, Determining the operating status of the radar measurement unit and the visual measurement unit within the target time interval includes: The tilt angle of the amphibious robot along the direction of gravity is obtained, and the first water entry depth of the amphibious robot is obtained based on the depth information; Based on the tilt angle, the first water immersion depth, and the position information of the radar measurement unit, the second water immersion depth of the radar measurement unit is determined, and the operating status of the radar measurement unit is determined based on the second water immersion depth. The operating status of the radar measurement unit is characterized as normal operation when the second water immersion depth is less than zero.

3. The amphibious robot localization method in a cross-domain environment according to claim 2, characterized in that, Determining the operating status of the radar measurement unit and the visual measurement unit within the target time interval includes: Based on the tilt angle, the first water immersion depth, and the position information of the visual measurement unit, the third water immersion depth of the visual measurement unit is determined, and the operating state of the visual measurement unit is determined based on the third water immersion depth. The operating state of the visual measurement unit is characterized as normal operation when the absolute value of the third water immersion depth is greater than a first preset value.

4. The amphibious robot localization method in a cross-domain environment according to claim 3, characterized in that, The step of obtaining the target odometry factor map corresponding to the amphibious robot includes: Based on the measurement results of the radar measurement unit and the IMU of the amphibious robot within the target time interval, a radar odometry factor is constructed, which is used to characterize the first pose prediction result of the amphibious robot. Based on the measurement results of the visual measurement unit and the IMU within the target time interval, a visual odometry factor is constructed, which is used to characterize the second pose prediction result of the amphibious robot. The target odometer factor map is obtained based on the radar odometry factor corresponding to the radar measurement unit in normal operation and the visual odometry factor corresponding to the visual measurement unit in normal operation.

5. The amphibious robot localization method in a cross-domain environment according to claim 4, characterized in that, The construction of the radar odometry factor includes: Based on the measurement results of the radar measurement unit within the target time interval, the dynamic distribution information of the laser point cloud measured by the radar measurement unit is obtained, and based on the dynamic distribution information of the laser point cloud, the first pose prediction result of the amphibious robot in the target coordinate system is obtained. The measurement results of the IMU are pre-integrated, and a radar odometry factor is constructed based on the pre-integrated measurement results of the IMU and the first pose prediction results.

6. The amphibious robot localization method in a cross-domain environment according to claim 5, characterized in that, The construction of the visual odometry factor includes: Based on the measurement results of the visual measurement unit within the target time interval, a set of visual image frames of the visual measurement unit is obtained, and based on the set of visual image frames, the second pose prediction result of the amphibious robot in the target coordinate system is obtained. The measurement results of the IMU are pre-integrated, and a visual odometry factor is constructed based on the pre-integrated measurement results of the IMU and the second pose prediction results.

7. The amphibious robot localization method in a cross-domain environment according to claim 6, characterized in that, The optimization function for constructing the target factor graph includes: Based on the deviation between the IMU measurement results and the first pose prediction results, a first cost function is constructed. The first cost function is used to optimize the radar odometry factor in the target odometry factor map. Based on the deviation between the IMU measurement results and the second pose prediction results, a second cost function is constructed. The second cost function is used to optimize the visual odometry factor within the target odometry factor map. Based on the first cost function and the second cost function, a target factor graph optimization function is constructed.

8. The amphibious robot localization method in a cross-domain environment according to claim 7, characterized in that, The process of obtaining the final pose prediction result of the amphibious robot within the target time interval includes: Based on the first cost function, obtain the cost sum of the radar odometry factors in the target odometry factor map within the target time interval; Based on the second cost function, the cost sum of the visual odometry factors in the target odometry factor map within the target time interval is obtained; Based on the cost sum of the radar odometry factor and the visual odometry factor, the cost sum of all odometry factors in the target odometry factor map is obtained, and the cost sum of the odometry factors is minimized based on the target factor map optimization function to obtain the final pose prediction result.

9. An amphibious robot localization system based on factor graph optimization in a cross-domain environment, used to implement the method as described in any one of claims 1-8, characterized in that, include: The data acquisition module is used to acquire the depth information of the amphibious robot, as well as the measurement results from the radar measurement unit, vision measurement unit, and IMU within the target time interval. The operation status determination unit is used to acquire the depth information of the amphibious robot, the position information of the radar measurement unit and the vision measurement unit, and determine the operation status of the radar measurement unit and the vision measurement unit based on the depth information and the position information of the radar measurement unit and the vision measurement unit. The odometry factor acquisition module is used to acquire the radar odometry factor and the visual odometry factor within the target time interval. The target factor map acquisition module is used to acquire a target odometry factor map based on the radar odometry factor and the visual odometry factor within the target time interval. The target factor graph optimization module is used to optimize the target odometry factor graph based on the factor graph optimization function to obtain the final pose prediction result of the amphibious robot within the target time interval.

10. An electronic device, characterized in that, include: At least one memory for storing computer programs; At least one processor is configured to execute a program stored in the memory, wherein when the program stored in the memory is executed, the processor is configured to perform the method as described in any one of claims 1-8.

Images