Robot navigation method, apparatus, device, and medium

Abstract

The application discloses a robot navigation method, device, equipment and medium, and relates to the technical field of industrial automation. The method comprises the following steps: determining a collection pose of a robot in a map coordinate system based on a first map, inertial measurement data and point cloud data collected by the robot; determining the passable cost of each position in a current motion area associated with the collection pose according to the point cloud data; writing the collection pose into a coordinate transformation relationship from a odometer coordinate system to a robot base coordinate system according to a transformation relationship from the map coordinate system to the odometer coordinate system; obtaining a current pose of the robot in the robot base coordinate system based on the two-dimensional navigation framework; and controlling the robot to travel along the passable position according to the passable cost of each position in the current motion area and the current pose of the robot. The technical scheme of the embodiment of the application improves the navigation success rate of the robot in a three-dimensional terrain.

Description

Technical Field

[0001] This invention relates to the field of industrial automation technology, and in particular to a robot navigation method, device, equipment, and medium. Background Technology

[0002] With the rapid development of multi-sensor fusion, 3D environmental perception and embodied intelligence technologies, mobile robot autonomous navigation systems are evolving from structured flat scenes to complex 3D terrain, dynamic and uncertain environments and all-terrain adaptive directions. Their positioning accuracy, terrain passability and intelligent decision-making capabilities will continue to improve.

[0003] Currently, mobile robot autonomous navigation systems generally adopt a technical architecture that combines two-dimensional positioning and two-dimensional navigation frameworks. The navigation framework represented by the Robot Operating System Navigation Stack (ROSNavigation) uses a two-dimensional occupancy grid map and a two-dimensional positioning module as its core, simplifying the robot's pose to a planar position (x, y) and yaw angle. The cost map also only distinguishes between binary states of "accessible" and "obstacle".

[0004] The aforementioned navigation framework operates successfully in flat, structured environments, but when facing three-dimensional terrains such as slopes, steps, and undulating ground, it cannot output the robot's height and roll and pitch attitude changes on slopes or steps. At the same time, it categorizes point clouds above the ground as obstacles, causing slopes or steps that the robot could pass through to be misjudged as insurmountable, and the planner cannot plan uphill or step-crossing paths. Summary of the Invention

[0005] This invention provides a robot navigation method, apparatus, device, and medium that can improve the success rate of robot navigation in three-dimensional terrain.

[0006] According to one aspect of the present invention, a robot navigation method is provided, the method comprising: Based on the first map, inertial measurement data and point cloud data collected by the robot, the robot's acquisition pose in the map coordinate system is determined, and the acquisition pose includes position information and three-dimensional posture. Based on the point cloud data, determine the passability cost of each position in the current motion region associated with the acquired pose; Obtain the coordinate transformation links natively supported by the 2D navigation framework. The coordinate transformation links include the coordinate transformation relationships between the map coordinate system, the odometry coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation. According to the transformation relationship from the map coordinate system to the odometer coordinate system, the acquired pose is written into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. Based on the two-dimensional navigation framework, the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system is queried to obtain the current pose of the robot in the robot base coordinate system; Based on the two-dimensional navigation framework, the traversability cost of the robot at each position in the current movement area is fused with a pre-built second map, and the robot is controlled to travel along traversable positions according to the current pose. The second map is a height map obtained by performing height filtering and traversability discrimination on the first map. The first map, the second map, and the current movement area are all located in the map coordinate system.

[0007] According to another aspect of the present invention, a robot navigation device is provided, the device comprising: The pose determination module is used to determine the robot's acquired pose in the map coordinate system based on the first map, inertial measurement data and point cloud data collected by the robot. The acquired pose includes position information and three-dimensional posture. The cost determination module is used to determine the passability cost of each position in the current motion region associated with the acquired pose based on the point cloud data. The transformation link acquisition module is used to acquire the coordinate transformation links natively supported by the two-dimensional navigation framework. The coordinate transformation links include the coordinate transformation relationship between the map coordinate system, the odometry coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation. The pose writing module is used to write the acquired pose into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system according to the transformation relationship from the map coordinate system to the odometer coordinate system, so as to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. The current pose acquisition module is used to query the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system based on the two-dimensional navigation framework, and obtain the current pose of the robot in the robot base coordinate system; The navigation control module is used to fuse the traversability cost of the robot at each position in the current motion area with a pre-built second map based on the two-dimensional navigation framework, and control the robot to travel along traversable positions according to the current pose. The second map is a height map obtained by performing height filtering and traversability discrimination on the first map. The first map, the second map and the current motion area are all located in the map coordinate system.

[0008] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor, the computer program being executed by the at least one processor to enable the at least one processor to perform the robot navigation method according to any embodiment of the present invention.

[0009] According to another aspect of the present invention, a computer-readable storage medium is provided, the computer-readable storage medium storing computer instructions for causing a processor to execute and implement the robot navigation method according to any embodiment of the present invention.

[0010] According to another aspect of the present invention, a computer program product is provided, the computer program product comprising a computer program that, when executed by a processor, implements the robot navigation method according to any embodiment of the present invention.

[0011] The technical solution of this invention determines the acquired pose, which includes position information and three-dimensional attitude, based on inertial measurement data and point cloud data. This ensures that the positioning result includes height, roll, and pitch attitude information, thus avoiding the problem that two-dimensional positioning cannot output the robot's height and roll / pitch attitude changes on slopes or steps. By determining the dynamic traversability cost of each location in the current motion area based on point cloud data, the traversability cost can reflect the traversability difficulty of the terrain where the robot is currently located in real time, thus solving the problem that static maps cannot respond to dynamic terrain changes such as newly appearing or removed steps and temporary ramps during runtime. Furthermore, by acquiring the coordinate transformation link natively supported by the two-dimensional navigation framework and writing the acquired pose into the coordinate transformation relationship from the odometry coordinate system to the robot's base coordinate system, and fixing the transformation relationship from the map coordinate system to the odometry coordinate system in this coordinate transformation link as a unit transformation, the six-degree-of-freedom acquired pose is accessed into the coordinate transformation link in a format natively supported by the framework, thereby avoiding the direct acquisition of the three-dimensional positioning pose. This addresses issues such as conflicting or non-unique coordinate transformation relationships and timestamp jumps that occur during connection. By fusing the dynamic traversable cost with a second map obtained through height filtering and traversability discrimination, the static global baseline and dynamic local perception complement and coordinate within a unified framework. This ensures the accessibility of global path planning and enables real-time response to local terrain, avoiding the problem of standard 2D cost maps indiscriminately classifying all point clouds above the ground as obstacles, causing ramps or steps that the robot could pass to be misjudged as insurmountable. Simultaneously, the current pose is obtained by querying coordinate transformation relationships based on the 2D navigation framework. Based on the result of fusing the dynamic traversable cost of each position in the current movement area with the second map, the robot is controlled to travel along traversable positions. This allows the planner to obtain complete pose information and terrain traversable cost information simultaneously in the same coordinate system, enabling the robot to stably plan and pass ramps and steps, and respond to dynamic terrain changes, thereby improving the robot's navigation success rate and passage safety in 3D terrain.

[0012] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart of a robot navigation method provided according to an embodiment of the present invention; Figure 2 This is a schematic diagram of a robot system that implements a robot navigation method according to an embodiment of the present invention; Figure 3 This is a schematic diagram of a scenario for determining the accessibility of a grid 8-neighborhood using both steps and slope, provided by an embodiment of the present invention. Figure 4 This is a schematic diagram of a top clearance observation scenario provided according to an embodiment of the present invention; Figure 5 This is a schematic diagram of the fusion of an online dynamic cost layer and an offline static prior layer in a cost map according to an embodiment of the present invention; Figure 6 This is a schematic diagram of the structure of a robot navigation device according to an embodiment of the present invention; Figure 7 This is a schematic diagram of the structure of an electronic device that implements the robot navigation method of this invention. Detailed Implementation

[0015] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0016] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0017] Figure 1This is a flowchart illustrating a robot navigation method provided in an embodiment of the present invention. This embodiment is applicable to autonomous robot navigation, particularly suitable for robots navigating autonomously in three-dimensional terrain environments including slopes, steps, and undulating surfaces. The method can be executed by a robot navigation device, which can be implemented in hardware and / or software. This robot navigation device can be configured in an electronic device that carries robot navigation functions, such as a robot system.

[0018] See Figure 1 The robot navigation method shown includes: S101. Based on the first map, the inertial measurement data and point cloud data collected by the robot, determine the robot's acquisition pose in the map coordinate system. The acquisition pose includes position information and three-dimensional posture.

[0019] The first map is a priori 3D point cloud map, generated by mapping and scanning before the robot's operation. This first map serves as a reference for positioning and registration. Inertial measurement data refers to the data output by the robot's onboard inertial measurement unit (IMU). IMU data can include angular velocity and acceleration data, and is used for motion distortion correction and state recursion of the point cloud data. Point cloud data refers to the 3D point cloud acquired by the robot's onboard LiDAR. Acquired pose refers to the pose acquired by the robot in the map coordinate system. Acquired pose is typically a six-degree-of-freedom pose, which can include 3D position information (x, y, and z coordinates) and 3D attitude information (roll, pitch, and yaw angles). The acquired pose is located in the map coordinate system, providing accurate pose input with a unified spatial reference for subsequent map queries and coordinate transformations. The map coordinate system is a globally fixed reference coordinate system, serving as the common spatial reference for all map data, such as the first map, the second map (discussed later), the current motion area, and the positioning results. Once the map coordinate system is determined during the mapping phase, it remains fixed and does not change with robot movement. The origin is typically located at the initial mapping position or a fixed reference point in the scene, and the coordinate axes are aligned with the horizontal plane and the direction of gravity. The acquired pose can be estimated by fusing inertial measurement data with point cloud data. For example, when the robot is climbing a slope, the acquired pose includes not only the robot's planar position on the slope but also the roll and pitch angle changes caused by the slope, ensuring that the localization results accurately reflect the robot's physical attitude.

[0020] S102. Based on the point cloud data, determine the passability cost of each position in the current motion region associated with the acquired pose.

[0021] The current motion area is a local region determined based on the robot's current pose, covering a preset range near the robot's current position. The current motion area can change and update in real time as the robot moves. For example, when the robot moves to a certain position, based on the x and y coordinates in the captured pose, a local area with a preset side length (e.g., 3 meters by 3 meters) centered on the robot's current position is selected as the current motion area for subsequent real-time terrain analysis.

[0022] Accessibility cost refers to a numerical value representing the ease of passage at each location within the current movement area. Accessibility cost is used to distinguish the suitability of passage at different locations, providing a basis for path selection during path planning in the 2D navigation framework. Accessibility cost is determined based on point cloud data collected by the robot in real time. When real-time point cloud data exists, the accessibility cost is determined based on this data. The current movement area provides a dynamically calculated range of locations for accessibility cost to avoid global calculation and thus reduce real-time computation. Accessibility cost is dynamic data. In some embodiments, the accessibility cost is determined by the ratio of the slope angle and step height determined from the real-time point cloud data to the robot's accessibility threshold. In other embodiments, the slope angle and step height are determined using point cloud data within the current movement area collected in real time, and the ratio of the slope angle and step height to the robot's accessibility threshold is calculated to determine the accessibility cost.

[0023] S103. Obtain the coordinate transformation links natively supported by the 2D navigation framework. The coordinate transformation links include the coordinate transformation relationships between the map coordinate system, the odometry coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation.

[0024] The 2D navigation framework refers to the standard navigation framework in the robot operating system, which natively supports a three-layer coordinate transformation link between the map coordinate system, the odometry coordinate system, and the robot base coordinate system. The odometry coordinate system is a local reference coordinate system with the robot's starting position as its origin, continuously calculated and updated as the robot moves. The odometry coordinate system is typically established based on integral calculations of the robot's own motion, such as those from wheel encoders and inertial measurement units. Due to slippage in wheel encoders and zero bias in inertial measurement units, the integral calculations accumulate errors over time, causing the robot's pose in the odometry coordinate system to gradually deviate from the true value. After prolonged operation, the pose error in the odometry coordinate system can reach several meters or even more, therefore the odometry coordinate system cannot be used alone for global positioning. The robot base coordinate system is a three-dimensional spatial reference coordinate system fixedly connected to the robot body. Its origin is usually located at the center of the robot's chassis or the center of rotation of the torso, and its coordinate axes are fixed to the robot body. The robot base coordinate system moves with the robot and serves as the reference benchmark for data from the robot's various sensors. All of the robot's sensors, such as LiDAR, inertial measurement unit, and depth camera, are installed with reference to the robot's base coordinate system. Sensor data must ultimately be uniformly transformed to this coordinate system for fusion processing. The 2D navigation framework corrects drift by transforming from the map coordinate system to the odometry coordinate system, and then obtains the robot's pose in the global map by transforming from the odometry coordinate system to the robot's base coordinate system.

[0025] Unit transformation refers to the transformation relationship from the map coordinate system to the odometry coordinate system being an identity matrix, meaning the two coordinate systems are numerically equivalent. This unit transformation relationship is fixed and configured once during robot system initialization and continuously published externally at a fixed frequency. Since registration-based localization based on the first map does not have cumulative drift, the acquired pose is already in the map coordinate system. The map coordinate system and odometry coordinate coefficients are equivalent; therefore, the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation. The coordinate transformation link is a standard mechanism used by the 2D navigation framework to obtain the robot's pose. By querying this link, the robot's pose in the global map can be calculated. Thus, by obtaining the coordinate transformation link natively supported by the 2D navigation framework and confirming its unit transformation configuration, a foundation is laid for subsequently integrating the six-DOF acquired pose in the framework's native format.

[0026] In one example, within the ROS Navigation framework, the coordinate transformation link corresponds to a three-layer structure in the TF (TransformLibrary) tree: map coordinate system (map), odometry coordinate system (odom), and robot base coordinate system (base_link). In this embodiment of the invention, the transformation from map coordinate system (map) to odometry coordinate system (odom) is fixed at a unit transformation, making the coefficient values ​​of the odom system equivalent to those of the map.

[0027] S104. According to the transformation relationship from the map coordinate system to the odometer coordinate system, write the acquired pose into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system.

[0028] The acquired pose refers to the robot's pose in the map coordinate system. Since the transformation from the map coordinate system to the odometry coordinate system is a unit transformation (meaning the two coordinate coefficients are equivalent), the acquired pose value can be directly used as the value for the coordinate transformation from the odometry coordinate system to the robot's base coordinate system. For example, the six-DOF pose of the robot's base coordinate system relative to the map coordinate system output at time t is T. M_B(t) The global anchoring transformation from the map coordinate system to the odometer coordinate system is T. M_O The dynamic transformation from the odometry coordinate system to the robot base is T. O_B(t) Then T is satisfied. M_B(t) =T M_O ×T O _B(t) Therefore, T O_B(t) = T M_O multiplying the inverse matrix by T M_B(t) In a preferred embodiment, the three-dimensional positioning result is already located in the navigation map coordinate system, let T... M_O If T is the identity matrix, then O_B(t) =T M_B(t) This approach eliminates the need for asynchronous coupling between the localization pose and the odometry pose to repeatedly calculate dynamic compensation. Instead, it directly publishes the complete six-DOF acquired pose as the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system. The transformation relationship from the numerically identical map coordinate system to the odometry coordinate system forms the coordinate transformation link natively supported by the 2D navigation framework, thus completing the navigation interface adaptation.

[0029] Specifically, writing can refer to using the positional information from the acquired pose as translational quantities and the 3D pose as attitude quantities, and filling it into the coordinate transformation data structure from the odometry coordinate system to the robot base coordinate system. By utilizing unit transformations, the six-DOF acquired pose can be directly written into the coordinate transformation link in a format natively supported by the 2D navigation framework, thus avoiding the incompatibility issue between the 3D positioning pose and the 2D navigation framework interface. This achieves the conversion of the 3D positioning pose into the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, preserving the 3D pose and avoiding compression into a 2D pose during the integration phase.

[0030] In one example, the positioning module outputs the acquired pose as x = 1.0, y = 2.0, z = 0.5, along with the corresponding roll, pitch, and yaw angles. These values ​​are then directly entered into the coordinate transformation relationship between the odometry coordinate system and the robot base coordinate system. The translation amounts are x, y, and z, and the attitude values ​​are the complete three-dimensional attitude, requiring no coordinate transformation calculations.

[0031] S105. Based on the two-dimensional navigation framework, query the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system to obtain the current pose of the robot in the robot base coordinate system.

[0032] The current pose refers to the robot pose obtained by querying the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system in the coordinate transformation link of the 2D navigation framework. This current pose is numerically consistent with the acquired pose, but because it is obtained through the coordinate transformation link natively supported by the 2D navigation framework, it can be directly used by the path planning and motion control modules in the framework. This allows the 2D navigation framework to obtain the robot pose containing complete six degrees of freedom information through its natively supported coordinate transformation link, ensuring the uniqueness and continuity of pose information within the framework.

[0033] In one example, the planner in the 2D navigation framework queries the map to odom to base_link link through the TF tree, and reads the robot's position x = 1.0, y = 2.0, z = 0.5 and the complete 3D pose from the odom to base_link transformation, which is used as the input of the planning algorithm.

[0034] S106. Based on the two-dimensional navigation framework, the passability cost of the robot at each position in the current movement area is fused with the pre-built second map, and the robot is controlled to travel along the passable position according to the current pose. The second map is a height map obtained by performing height filtering and passability discrimination on the first map. The first map, the second map and the current movement area are all located in the map coordinate system.

[0035] The second map is a height raster map obtained by offline processing of the first map. Each raster records the ground elevation value and passability markers. The first map, the second map, and the current movement area are all located in the same coordinate system, namely the global map coordinate system determined during the mapping phase. This ensures that the positioning results, the global map, and the local map are consistent. Figure 3The consistent spatial reference avoids the bias and computational overhead caused by cross-coordinate system transformation. Height filtering refers to filtering point cloud data according to height thresholds to remove ground noise and ceiling outliers. Accessibility determination refers to the operation of building offline layers and determining whether each grid location is accessible to the robot. Accessible locations are marked as accessible, and inaccessible locations are marked as obstacles or inaccessible, etc. In some embodiments, accessibility determination refers to using a dual criterion of step height threshold and slope angle threshold to determine each grid, marking steps and slopes within the robot's capability range as accessible. In fact, both the first map and the second map are static maps built offline. The current movement area is dynamic data that is updated in real time. The accessibility cost of each location within the current movement area is calculated based on the real-time collected point cloud data, so the accessibility cost is also dynamic data that is updated in real time.

[0036] When real-time point cloud data is missing, the passable cost can be determined based on prior height information in the second map. For example, when the lidar encounters glass, water surfaces, or sparsely populated areas at long distances, real-time point cloud data may be missing. When real-time point cloud data is missing, the dynamic cost layer corresponding to the current movement area prioritizes maintaining the value from the previous moment. If there is no valid value from the previous moment, it reverts to the passable cost determined by the prior height information of the corresponding location in the second map to ensure navigation continuity.

[0037] The 2D navigation framework adds traversable cost information to the corresponding locations in the second map, forming a dynamic-static fusion cost map. After acquiring the current pose, the 2D navigation framework searches for a path from the current position to the target position on the fused cost map, prioritizing terrain with low traversable costs and controlling the robot to travel along that path. This allows the 2D navigation framework to simultaneously obtain complete pose information and terrain traversal cost information in the same coordinate system, thereby generating a planned path that balances safety and accessibility, and controlling the robot to stably traverse 3D terrain such as slopes and steps.

[0038] In one example, flat ground in the current movement area has low traversal cost, while steep slopes or high steps have high traversal cost. When searching for a path, the planner of the two-dimensional navigation framework prioritizes flat ground with low cost and only selects steep slopes or steps with higher cost when necessary, thereby ensuring the robot's passage safety.

[0039] The technical solution of this invention determines the acquired pose, which includes position information and three-dimensional attitude, based on inertial measurement data and point cloud data. This ensures that the positioning result includes height, roll, and pitch attitude information, thus avoiding the problem that two-dimensional positioning cannot output the robot's height and roll / pitch attitude changes on slopes or steps. By determining the dynamic traversability cost of each location in the current motion area based on point cloud data, the traversability cost can reflect the traversability difficulty of the terrain where the robot is currently located in real time, thus solving the problem that static maps cannot respond to dynamic terrain changes such as newly appearing or removed steps and temporary ramps during runtime. Furthermore, by acquiring the coordinate transformation link natively supported by the two-dimensional navigation framework and writing the acquired pose into the coordinate transformation relationship from the odometry coordinate system to the robot's base coordinate system, and fixing the transformation relationship from the map coordinate system to the odometry coordinate system in this coordinate transformation link as a unit transformation, the six-degree-of-freedom acquired pose is accessed into the coordinate transformation link in a format natively supported by the framework, thereby avoiding the direct acquisition of the three-dimensional positioning pose. This addresses issues such as conflicting or non-unique coordinate transformation relationships and timestamp jumps that occur during connection. By fusing the dynamic traversable cost with a second map obtained through height filtering and traversability discrimination, the static global baseline and dynamic local perception complement and coordinate within a unified framework. This ensures the accessibility of global path planning and enables real-time response to local terrain, avoiding the problem of standard 2D cost maps indiscriminately classifying all point clouds above the ground as obstacles, causing ramps or steps that the robot could pass to be misjudged as insurmountable. Simultaneously, the current pose is obtained by querying coordinate transformation relationships based on the 2D navigation framework. Based on the result of fusing the dynamic traversable cost of each position in the current movement area with the second map, the robot is controlled to travel along traversable positions. This allows the planner to obtain complete pose information and terrain traversable cost information simultaneously in the same coordinate system, enabling the robot to stably plan and pass ramps and steps, and respond to dynamic terrain changes, thereby improving the robot's navigation success rate and passage safety in 3D terrain.

[0040] In an optional embodiment of the present invention, the step of writing the acquired pose into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system according to the transformation relationship from the map coordinate system to the odometer coordinate system, to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system, includes: taking the position information and three-dimensional attitude in the acquired pose as the unit transformation according to the transformation relationship from the map coordinate system to the odometer coordinate system, and using them as the translation and attitude quantities in the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system, to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system.

[0041] In this context, a unit transformation refers to a transformation relationship with an identity matrix, meaning the coordinate transformation between two coordinate systems involves no translation, no rotation, and a scale of 1, ensuring that the coordinate values ​​of the same point in space are equal in both coordinate systems. Position information refers to the acquired x, y, and z three-dimensional coordinate values ​​from the pose. Translation refers to the three components representing position offset in the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, namely x, y, and z. Three-dimensional attitude refers to the acquired complete three-dimensional attitude information from the pose, which may include roll, pitch, and yaw angles. Attitude quantities refer to the components representing rotation in the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system.

[0042] Specifically, since the transformation from the map coordinate system to the odometry coordinate system is a unit transformation, meaning the two coordinate coefficients are equivalent, the robot's acquired pose in the map coordinate system is numerically directly equal to its pose in the odometry coordinate system. The position information and 3D attitude from the acquired pose can be directly used as the translation and attitude quantities in the coordinate transformation from the odometry coordinate system to the robot's base coordinate system, without requiring any coordinate transformation calculations. This write operation completely fills the six-DOF acquired pose into the corresponding position in the coordinate transformation link natively supported by the 2D navigation framework, enabling the 2D navigation framework to obtain the robot pose containing complete six-DOF information through the standard coordinate transformation link.

[0043] In one example, the acquired pose obtained from localization is x = 1.0, y = 2.0, z = 0.5, along with the corresponding roll, pitch, and yaw angles. These values ​​are then directly entered into the coordinate transformation relationship between the odometry coordinate system and the robot base coordinate system. The translation is x = 1.0, y = 2.0, z = 0.5, and the attitude is a complete three-dimensional attitude without any coordinate transformation calculations.

[0044] As can be seen, by using the position information and 3D attitude from the acquired pose as translation and attitude quantities in the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, the 2D navigation framework can obtain the complete six-degree-of-freedom current pose consistent with the acquired pose by querying this coordinate transformation relationship. This not only preserves the height information and roll and pitch attitude information, but also avoids the additional errors introduced by coordinate transformation calculation. At the same time, it eliminates the problem of non-unique coordinate transformation tree and timestamp jump caused by the dynamic change of the transformation relationship from the map coordinate system to the odometry coordinate system, ensuring the continuity and stability of the coordinate transformation relationship.

[0045] In an optional embodiment of the present invention, before determining the traversable cost of each position in the current motion area associated with the acquired pose based on the robot's acquired pose and the point cloud data, the method further includes: performing height filtering and rasterization on the first map to generate a height raster map; calculating the maximum allowable height difference for each first grid cell in the height raster map based on the horizontal distance between the first grid cell and its neighboring grid cells, the robot's step height threshold, and the slope angle threshold; calculating the proportion of reachable grid cells for the first grid cell based on the height difference between the first grid cell and its neighboring grid cells and the maximum allowable height difference; determining the traversability of the first grid cell based on the proportion of reachable grid cells, the point cloud density, and the top clearance, obtaining the traversability determination result of the first grid cell, where the top clearance is the difference between the lowest point above the grid cell and the ground height; and marking the traversability determination result of each first grid cell in the height raster map to obtain the second map.

[0046] Height filtering refers to filtering points in the first map according to a preset height threshold, retaining valid environmental terrain point clouds. The first map is a point cloud map, consisting of points. The points in the first map are static data, while the point cloud data collected by the robot is dynamic data. For example, height filtering can remove noise near the ground and point clouds from ceilings or suspended objects above the robot's passage height. Rasterization refers to dividing the filtered continuous point cloud space into regularly arranged grid cells according to a preset resolution. Each grid cell is called a grid, and each grid is used to store the height statistics of the point cloud falling within its range. The height grid map is a two-dimensional grid structure formed after rasterization processing, with each grid recording the ground height information at that location. Each first grid refers to each grid cell in the height grid map, which is processed one by one during traversal. Neighbor grids refer to the surrounding grids adjacent to the current first grid. For example, a neighbor grid can be eight grid cells, namely the grids in the top, bottom, left, right, and four diagonal directions. Horizontal distance refers to the projected distance on the horizontal plane from the center of the current first grid cell to the center of a neighboring grid cell, determined by both grid resolution and neighbor orientation. Step height threshold refers to the maximum vertical obstacle height the robot can stably traverse, determined by mechanical factors such as ground clearance, suspension travel, foot size, and wheel diameter. Slope angle threshold refers to the maximum slope angle at which the robot can stably climb without slipping or tipping over, determined by factors such as the coefficient of friction between the robot's drive wheels and the ground, center of gravity position, and motor power. Maximum allowable height difference refers to the maximum allowable difference in height between the current first grid cell and a neighboring grid cell, calculated by multiplying the step height threshold and the tangent of the horizontal distance by the slope angle threshold. For example, the maximum allowable height difference can be calculated using max(H_step, d×tanθ_max), where H_step is the step height threshold, θ_max is the slope angle threshold, and d is the horizontal distance. By calculating the maximum height difference as described above, the two judgment dimensions of step height threshold and slope angle threshold are unified into a single comparison standard. When the actual height difference does not exceed this allowable value, the neighboring grid is determined to be reachable. Height difference refers to the absolute value of the difference in ground height between the current first grid and a neighboring grid. The reachable grid ratio refers to the proportion of neighboring grids of the current first grid whose actual height difference does not exceed the allowable maximum height difference out of the total number of neighboring grids. Point cloud density refers to the number of valid point clouds falling within the range of the current first grid. Top clearance refers to the vertical distance between the lowest point above the current first grid and the ground level. The lowest point above can refer to the lowest point above the ground. Specifically, the lowest point above is the point with the smallest height value among all point clouds above the ground (i.e., the first point cloud encountered when looking upwards from the ground). The lowest point above is a key measurement point determining whether the robot's head will collide with the top.

[0047] Point cloud density and top clearance are both determined based on the rasterized statistical results of the first map. The accessibility determination result is the classification result output after determining the current first grid cell. The accessibility determination result includes three types: accessible, obstacle, and unknown.

[0048] In one example, when the grid resolution is 0.05 meters, the horizontal distance between the current first grid cell's directly above neighbor is 0.05 meters, and the horizontal distance between its diagonal neighbors is approximately 0.05 meters multiplied by the square root of 2, which equals 0.07 meters. If the step height threshold is 0.15 meters and the slope angle threshold is 30 degrees, then the maximum allowable height difference between the directly above neighbors is max(step height threshold 0.15, horizontal distance 0.05 multiplied by tan30° (slope angle threshold) ≈ 0.029) = 0.15 meters, and the maximum allowable height difference between the diagonal neighbors is max(step height threshold 0.15, horizontal distance 0.07 multiplied by tan30° (slope angle threshold) ≈ 0.040) = 0.15 meters. If the actual height difference of six out of the eight neighbors of the current first grid cell does not exceed the corresponding maximum allowable height difference, then the percentage of reachable grid cells is 75%. If the percentage reaches a preset threshold, such as 60%, and the point cloud density meets the standard, and the top clearance is greater than the sum of the robot height and the safety margin, then the grid is marked as passable.

[0049] As can be seen, by performing height filtering and rasterization on the first map through offline layer construction, a height raster map is generated, ensuring that each raster records ground height information. Invalid point clouds are removed from the first map, and continuous point clouds are discretized into structured rasteres. The maximum allowable height difference is calculated based on step height thresholds and slope angle thresholds, unifying the two judgment dimensions into a single comparison standard, while also covering the robot's ability to navigate vertical obstacles and sloping terrain. The proportion of reachable rasteres is statistically analyzed by comparing the actual height difference with the maximum allowable height difference, ensuring that navigability judgment considers both the current raster and the surrounding terrain. The system considers the connectivity of shapes; finally, it integrates the proportion of reachable grids, point cloud density, and top clearance to determine traversability, marking steps and ramps within the robot's range as traversable. At the same time, it eliminates misjudgments due to insufficient data by using point cloud density and eliminates the risk of collision by using top clearance. This avoids the problem of standard two-dimensional cost maps classifying all point clouds above the ground as obstacles, causing ramps or steps that the robot could pass to be misjudged as insurmountable. This allows the planner to obtain a priori map containing traversable terrain information, providing an accurate offline map basis for subsequent planning of uphill or step-crossing paths.

[0050] In some embodiments, the second map generation process includes: reading the first map, performing height filtering according to a preset height threshold to remove ground noise and ceiling outliers; constructing an elevation map by dividing the map into grids according to a preset resolution, taking configurable quantiles for the ground height of each grid, and recording the number of point clouds and the lowest point above the ground; performing a top clearance determination: taking the difference between the lowest point above the ground and the ground height as the clearance, and marking it as a top obstacle when the clearance is less than the sum of the robot height and the safety margin; performing a dual-criteria determination grid-by-grid and neighbor-by-neighbor: calculating the maximum allowable height difference for 8 neighbors based on horizontal distance, and statistically analyzing the proportion of reachable neighbors; marking it as passable when the reachability proportion reaches a threshold, the point density meets the standard, and there is no top obstacle; marking it as an obstacle when the reachability proportion is too low, the maximum height difference exceeds the step height threshold, or there is a top obstacle; and marking the rest as unknown. Post-process each grid, removing isolated passable grids and filling isolated obstacles and unknown grids surrounded by passable grids; downsample to the output resolution, marking obstacles if there are obstacles within the block, marking passable if there are passable grids, otherwise marking unknown; output a standard two-dimensional occupied grid map file, i.e., the second map, for loading by the two-dimensional navigation framework.

[0051] In an optional embodiment of the present invention, determining the traversable cost of each position in the current motion region associated with the acquired pose based on the point cloud data includes: determining the horizontal distance and height difference between each second grid and its neighboring grids in the current motion region associated with the acquired pose based on the point cloud data; calculating the slope angle and step height between the second grid and its neighboring grids for each second grid in the current motion region based on the horizontal distance and height difference between the second grid and its neighboring grids; determining the traversable cost of the second grid based on the slope angle and step height between the second grid and its neighboring grids, and using it as the traversable cost of the robot at each position in the current motion region.

[0052] In this context, the second grid refers to a grid cell within the current motion region, which is processed sequentially during the calculation. After the current motion region is rasterized, each location within the current motion region corresponds to a second grid cell within that rasterized region. Based on the point cloud data collected in real-time by the robot and the current motion region, the point cloud belonging to the current motion region is determined. The point cloud of the current motion region is then divided into grids to obtain the point cloud of each second grid cell within the current motion region. Height information (such as ground height) is added to the point cloud of the current motion region, and / or height processing is performed (referencing height filtering in the first map, etc.). The processed current motion region becomes a real-time local elevation map. Each second grid cell in the current motion region is traversed to determine neighboring grid cells. The horizontal distance is calculated based on the relative position between the second grid cell and its neighboring grid cells; the height difference is calculated based on the height information of the second grid cell and its neighboring grid cells.

[0053] Height difference refers to the absolute value of the difference in ground height between the current second grid cell and a neighboring grid cell. Slope angle is the arctangent of the ratio of the height difference between the current second grid cell and a neighboring grid cell to the horizontal distance, reflecting the slope of the terrain. Step height refers to the absolute value of the height difference between the current second grid cell and a neighboring grid cell, reflecting the vertical drop of the terrain. Accessibility cost is used to characterize the ease of passage at each location within the current movement area.

[0054] As can be seen, by calculating the slope angle and step height of each grid based on real-time point cloud data during online runtime, and determining the passable cost that represents the degree of passability, the passable cost can reflect the real-time state of the terrain at the current location. This allows it to respond to dynamic terrain changes such as newly appearing or removed steps, temporary ramps, and terrain collapses during runtime, providing real-time updated terrain passable cost information for the two-dimensional navigation framework.

[0055] In an optional embodiment of the present invention, determining the passability cost of the second grid based on the slope angle and step height between the second grid and its neighboring grids includes: determining the maximum slope angle and maximum step height of the second grid based on the slope angle and step height between the second grid and its neighboring grids; determining the passability margin of the second grid based on the ratio between the maximum slope angle and a slope angle threshold, and the ratio between the maximum step height and a step height threshold; when the passability margin is less than or equal to the margin boundary value corresponding to the robot, continuously mapping the passability margin of the second grid to a target value range to obtain the passability cost, wherein the passability cost is positively correlated with the passability margin; when the passability margin is greater than the margin boundary value, determining the passability cost of the second grid as an impassable cost, wherein the impassable cost is greater than the value in the target value range.

[0056] The maximum slope angle refers to the maximum slope angle between the current second grid cell and all its neighboring grid cells, representing the steepest terrain at that grid cell. The maximum step height refers to the maximum step height between the current second grid cell and all its neighboring grid cells, representing the maximum vertical drop of the terrain at that grid cell.

[0057] Accessibility margin refers to the degree to which the current terrain approaches the robot's accessibility limit. For example, the larger of the ratio of the maximum slope angle to a slope angle threshold and the ratio of the maximum step height to a step height threshold can be used as the accessibility margin. The formula for calculating the accessibility margin is: max(Δh_max / H_step, slope_max / θ_max), where Δh_max is the maximum step height, slope_max is the maximum slope angle, H_step is the step height threshold, and θ_max is the slope angle threshold. Continuous mapping refers to continuously assigning accessibility margin values ​​within the range of accessibility costs. Any value of the accessibility margin in the interval 0 to 1 uniquely corresponds to a accessibility cost, without discrete level divisions or jumps. Margin boundary values ​​are critical values ​​used to distinguish between accessible and inaccessible states; for example, a margin boundary value of 1. When the accessibility margin is less than or equal to the margin boundary value, the terrain is within the robot's capabilities. A accessibility margin closer to 1 indicates proximity to the capability limit. When the accessibility margin is greater than the margin boundary value, the terrain exceeds the robot's capabilities. The target value range refers to the interval within which the accessibility cost can be set; all accessibility costs fall within this range. Positive correlation refers to the monotonic relationship between accessibility margin and accessibility cost: a larger accessibility margin corresponds to a higher accessibility cost, and vice versa. Impassable cost is a fixed accessibility cost assigned when the accessibility margin exceeds the margin boundary value. Its value is greater than all values ​​within the target value range and is used to clearly distinguish between impassable and accessible areas.

[0058] In some embodiments, the continuous mapping rule is as follows: when the traversability margin is less than or equal to the margin boundary value and the clearance and density meet the requirements, the traversability margin is continuously mapped to the traversability cost within the target value range, with the cost increasing as the traversability margin approaches the margin boundary value; when the traversability margin is greater than the margin boundary value or the clearance is insufficient, the traversability cost becomes a fatal obstacle, i.e., an inaccessible cost; unobserved areas are not updated, retaining their prior or unknown state. Continuous mapping maps the traversability margin to the corresponding traversable cost within the target value range using a monotonic mapping function, which can be a linear or nonlinear mapping.

[0059] In one example, if the maximum slope angle of the current grid is 20 degrees, the slope angle threshold is 30 degrees, the maximum step height is 0.05 meters, and the step height threshold is 0.15 meters, then the ratio of the maximum slope angle to the slope angle threshold is 20 divided by 30, approximately equal to 0.67. The ratio of the maximum step height to the step height threshold is 0.05 divided by 0.15, approximately equal to 0.33. The traversable margin is the larger of these two values, approximately equal to 0.67, which is less than the margin boundary value of 1. This indicates that the grid terrain is within the robot's capabilities, and the corresponding traversable cost is determined based on the traversable margin. If the maximum slope angle of the current grid is 35 degrees and the maximum step height is 0.05 meters, then the ratio of the maximum slope angle to the slope angle threshold is 35 divided by 30, approximately equal to 1.17. The traversable margin is approximately equal to 1.17, which is greater than the margin boundary value of 1. The grid is determined to have an impassable cost.

[0060] It is evident that by comprehensively quantifying the terrain geometric features of slope angle and step height into a unified accessibility margin index, and continuously mapping the accessibility margin to accessibility cost, the accessibility cost continuously increases with the terrain hazard level within the accessible area. This achieves a continuous hierarchical expression of the safety level of different terrains within the accessible area. The planner of the two-dimensional navigation framework can distinguish between accessible and inaccessible areas, and within the accessible area, it can differentiate between high-risk terrain approaching the capability limit and low-risk terrain with ample margin. It prioritizes paths with greater safety margins, avoiding the risk of the planner being unable to distinguish between different hazard levels due to binary cost expression and thus approaching the capability limit. This improves the accessibility safety and stability of the robot's navigation in three-dimensional terrain.

[0061] In an optional embodiment of the present invention, the step of fusing the traversability cost of the robot at each position in the current motion area with a pre-built second map based on the two-dimensional navigation framework, and controlling the robot to travel along traversable positions according to the current pose, includes: fusing the second map and the traversability cost of the robot at each position in the current motion area based on the two-dimensional navigation framework to obtain a third map; planning a planned path for the robot from the current position to the target position on the third map based on the two-dimensional navigation framework; and controlling the robot to travel along the planned path based on the current pose according to the two-dimensional navigation framework.

[0062] The third map refers to the cost map obtained by fusing the second map with the traversable cost. Each grid in the third map has a traversable cost, and the third map is used for path planning by the 2D navigation framework. Fusion refers to overlaying the second map as a global static reference layer and the traversable cost as a local dynamic update layer within the cost map framework. The planned path refers to the sequence of path points searched by the planner of the 2D navigation framework on the third map from the robot's current position to the target position. The current position refers to the robot's real-time position in the map coordinate system, determined by the position information in the current pose. The target position refers to the final position that the robot's navigation task expects to reach. Controlling the robot to travel along the planned path based on the current pose means that the controller of the 2D navigation framework converts the planned path into speed control commands based on the current pose, driving the robot chassis along the planned path, and continuously providing feedback control based on the updated current pose during travel.

[0063] In one example, the second map is loaded as a static layer onto the cost map, providing global prior traversable baseline information. The traversable cost of each position of the robot in the current movement area is written into the cost map as a dynamic layer, providing local real-time terrain traversal cost information. The static and dynamic layers are fused to generate the third map, i.e., the cost map. The global planner of the 2D navigation framework searches for the path with the lowest total cost from the robot's current position to the target position on the third map, prioritizing terrain with low traversable costs and avoiding terrain with high traversable costs. The controller converts the planned path into speed control commands based on the current pose, driving the robot chassis to travel along the planned path.

[0064] Optionally, before executing the control flow for controlling the robot's movement described above, the following startup and integration steps can be performed: Using the transformation relationship from the map coordinate system to the odometry coordinate system as the unit transformation, the position information and 3D attitude from the acquired pose are used as the translation and attitude quantities in the coordinate transformation relationship from the odometry coordinate system to the robot's base coordinate system; the transformation relationship from the map coordinate system to the odometry coordinate system is published at a fixed frequency, and this transformation relationship is a unit transformation; a second map is loaded; a 2D navigation framework is started; and the passability cost is written into the cost map and fused with the second map. The 2D navigation framework obtains the current pose through the coordinate transformation link and obtains the fused cost map.

[0065] After establishing the coordinate transformation link and fusing the cost map as described above, the navigation task can be executed according to the control flow. The navigation task refers to the process by which the robot, starting from its current pose, moves to the target position using the fused cost map as the planning basis. In this embodiment of the invention, the target position is given in the form of a waypoint sequence, and the corresponding navigation task is executed through waypoint task nodes. Waypoint task nodes support multi-point patrol, cyclic navigation, and waypoint navigation, executing multi-point, cyclic, or waypoint navigation tasks through following waypoints or waypoint navigation behaviors. Specifically, the robot starts from its current pose and travels along each waypoint sequentially until all waypoints are completed or a new navigation command is received.

[0066] As can be seen, by fusing the second map as a static layer and the traversable cost as a dynamic layer to generate the third map, namely the unified cost map, the global prior traversable baseline information provided by the second map is retained, while the local real-time terrain traversable cost information reflected by the traversable cost is added. This allows the planner of the two-dimensional navigation framework to obtain complete pose information and terrain traversable cost information simultaneously in the same coordinate system. Thus, when planning a path, it can take into account both global path accessibility and local traversable safety, and control the robot to move along the planned path based on the current pose. This enables the robot to stably plan and pass through slopes and steps, while responding to dynamic terrain changes and prioritizing paths with greater safety margins, thereby improving the robot's navigation success rate and traversal safety in three-dimensional terrain.

[0067] In an optional embodiment of the present invention, determining the robot's acquisition pose in the map coordinate system based on a first map, inertial measurement data collected by the robot, and point cloud data includes: constructing a spatial index based on the first map; performing motion distortion correction on the point cloud data collected by the robot according to the inertial measurement data collected by the robot to obtain corrected point cloud data; determining the robot's state prediction value and state covariance prediction value according to the robot's inertial measurement data; registering the corrected point cloud data with the first map through the spatial index to obtain point cloud registration residuals; and performing fusion estimation based on the state prediction value, the state covariance prediction value, and the point cloud registration residuals to obtain the robot's acquisition pose in the map coordinate system.

[0068] The process of determining the robot's pose is essentially its localization process. Specifically, the localization process includes initialization and map loading, data synchronization, forward propagation and point cloud distortion correction by the inertial measurement unit (IMU), iterative measurement updates, pose publication, and optional relocalization. During initialization and map loading, configuration parameters are loaded. In pure localization mode, the first map is downloaded to construct an Incremental K-Dimensional Tree (ikd Tree). When ultra-wideband (UWB) is enabled, the base station state and extrinsic parameters are initialized. During data synchronization, the IMU data within a laser scanning time window is aligned and packaged with optional UWB data. During forward propagation and point cloud distortion correction by the IMU, the forward propagation state is pre-integrated using the IMU, and motion compensation is applied to the laser points. During pose publication, the IMU converts the pose to the robot's extrinsic parameters, resulting in a six-DOF pose for the base, and publishes the odometry information. Real-time local elevation and clearance observations are performed based on the global elevation map and the current pose, calculating the body's forward local clearance and publishing it for use by the online layer.

[0069] In the above process, the construction of the spatial index serves the initialization phase, providing efficient search support for subsequent point cloud registration; motion distortion correction corresponds to the processing of laser point clouds in the data synchronization and forward propagation phases; the recursion of state prediction values, the construction of registration and point cloud registration residuals, and the fusion estimation based on the above prior information and measurement information together constitute the core content of iterative measurement updates.

[0070] Spatial indexing refers to a data structure built upon the first map. It supports efficient nearest neighbor search between point cloud data and the first map, allowing for the rapid identification of the local point cloud region in the first map that is spatially closest to a given laser point. Motion distortion correction involves using high-frequency inertial measurement data to compensate the pose of each laser point in a frame of laser point cloud, transforming each laser point to the same coordinate system at the same moment, eliminating point cloud trailing and distortion caused by the robot's continuous movement during a single laser scan frame. The state prediction value is an estimate of the robot's state obtained through pre-integration recursion of the inertial measurement data. The state covariance prediction value quantifies the degree of uncertainty in the state prediction value. Registration refers to spatially aligning the corrected point cloud data with the first map, determining the spatial transformation relationship between the two by finding the corresponding positions of each laser point in the corrected point cloud in the first map. The point cloud registration residual is the distance vector between each laser point in the corrected point cloud and the corresponding local plane in the first map. It reflects the deviation between the current pose estimate and the true pose, serving as the measurement update input in the fusion estimation.

[0071] The above steps together constitute the core of iterative measurement update: the state prediction value is recursively obtained from inertial measurement data, providing prior information for iterative measurement update; the construction of registration and point cloud registration residuals provides measurement information for iterative measurement update. In the iterative measurement update stage, fusion estimation is performed to fuse the state prediction value with the point cloud registration residual, iteratively correcting deviations in the state prediction value, and finally outputting the posterior state, i.e., the acquisition pose. Fusion estimation refers to the operation of fusing and updating the state prediction value with the point cloud registration residual, outputting the posterior state, i.e., the acquisition pose. In some embodiments, fusion estimation can be the operation of fusing and updating the state prediction value with the point cloud registration residual under the iterative error state Kalman filter framework, outputting the posterior state, i.e., the acquisition pose. The measurement update stage includes multiple information sources such as laser observation, optional ultra-wideband observation, and optional zero-velocity correction. In laser observation, the nearest neighbor point of the downsampled laser point is searched in the ikd Tree and a local plane is fitted to construct the point-to-plane residual, which is the distance vector between the laser point and the local plane, serving as the main information source for measurement update. When ultra-wideband (UWB) observation is enabled, the predicted ranging value is the scale multiplied by the distance between the tag and the base station, plus the bias. The tag position includes position, attitude, UWB extrinsic parameters, and velocity multiplied by delay compensation. A ranging residual and Jacobian matrix are constructed. Base station coordinates, ranging bias, scale, and delay are estimated online as needed, and outliers are eliminated using a chi-square threshold. The UWB ranging residual is incorporated into the measurement update as an additional absolute distance constraint. When zero-velocity correction is enabled, zero-velocity and zero-angular-velocity constraints are used to suppress drift of the inertial measurement unit during robot stationary segments. After iterative convergence of all measurement information on the manifold, the posterior state, i.e., the acquired pose, is output.

[0072] In one example, an ikd Tree is constructed based on the first map as a spatial index to store the spatial location information of all point clouds in the first map for subsequent nearest neighbor search. High-frequency inertial measurement data is used to calculate the robot's pose change at each moment during a laser scan frame, transforming each laser point from the sensor coordinate system at the time of acquisition to the sensor coordinate system at the end of the scan, eliminating point cloud trailing caused by robot motion. The corrected point cloud data is registered with the first map to obtain the point cloud registration residual, which serves as the measurement update input for the fusion estimation. The state prediction value is fused with the point cloud registration residual to estimate the corrected posterior state, i.e., the acquired pose.

[0073] In addition to the normal localization process described above, the localization process also supports a relocalization mechanism. Relocalization is triggered by the initial pose command. It involves iterative coarse registration of the current laser scan frame with the first map, searching within the yaw angle range to restore the robot's pose in the global map. The effective matching ratio is verified. If the effective ratio reaches a preset threshold, the system switches back to normal localization mode, using the relocalized pose as the current localization result for subsequent fusion estimation and pose publishing. If the effective ratio does not reach the preset threshold, the candidate initial values ​​are adjusted and registration is retried. If the number of attempts exceeds a preset number, relocalization is deemed a failure and the system resets, awaiting the next initial pose command.

[0074] It is evident that constructing a spatial index based on the first map improves the nearest neighbor search efficiency when registering the corrected point cloud data with the first map. Using inertial measurement data to correct motion distortion in the point cloud data eliminates point cloud trailing and distortion caused by robot motion, ensuring the accuracy of the point cloud data and the effectiveness of the registration. Using inertial measurement data to determine the predicted state value and the predicted state covariance value provides prior information and uncertainty quantification for fusion estimation. Registering the corrected point cloud data with the first map establishes a geometric constraint relationship between the point cloud observation and the prior map, providing reliable measurements for pose correction. The state prediction value is updated with the point cloud registration residual through fusion estimation, outputting the corrected acquired pose. This positioning method, based on registration with the prior 3D point cloud map, anchors the positioning result in the global map coordinate system, eliminating cumulative drift and outputting a six-DOF pose containing position information and 3D attitude, providing an accurate global positioning foundation for subsequent navigation.

[0075] In a scenario Figure 2 This is a schematic diagram of a system for implementing a robot navigation method according to an embodiment of the present invention. Figure 2 The robot navigation system shown includes a sensor layer, a localization module, an offline mapping layer, an online dynamic cost layer (online dynamic traversability cost layer), a localization adapter, and a two-dimensional navigation framework. The first map is pre-collected, generated, and stored during the mapping phase, and is loaded by the localization module for registration and localization, while also being used by the offline mapping layer.

[0076] The sensor layer, located at the front end of the system, includes a 3D LiDAR, an inertial measurement unit (IMU), and an optional ultra-wideband (UWB) tag. The 3D LiDAR acquires environmental point cloud data, the IMU outputs angular velocity and acceleration, and the UWB tag provides global ranging constraints. The sensor layer provides this data as input to the positioning module.

[0077] The localization module receives environmental point cloud data, angular velocity and acceleration, and optional global ranging constraints from the sensor layer. It loads a first map and uses an iterative error state Kalman filter to perform tightly coupled estimation of laser, inertial, and optional ultra-wideband data. Registration and relocalization on the first map are performed using an ikd Tree. The module outputs the acquired pose and covariance. The acquired pose includes position information, roll angle, pitch angle, and yaw angle. The covariance is output along with the acquired pose to characterize the uncertainty of the pose estimation. The localization module outputs the corrected point cloud data along with the acquired pose to the online dynamic cost layer and also outputs the acquired pose to the offline construction layer and localization adapter.

[0078] The positioning process of the positioning module may include: First, data synchronization is performed by aligning and packaging the inertial measurement data within a laser scanning time window with the optional ultra-wideband data.

[0079] Then, the state is propagated forward using the pre-integration of the inertial measurement unit to obtain the state prediction value; at the same time, motion distortion compensation is performed on the laser point cloud to obtain the corrected point cloud data, which is used as the input for iterative measurement update.

[0080] In the iterative measurement update phase, iterative error state Kalman filtering is performed on the manifold. During laser observation, the corrected point cloud data is used as input. The downsampled laser points are searched for nearest neighbors in the ikd Tree and fitted to a local plane, constructing the point-to-plane residual and Jacobian matrix. When ultra-wideband (UWB) observation is enabled, UWB ranging data is used as input. The scaling factor is calculated multiplied by the distance between the tag location (UWB tags are configured on the robot body) and the base station, and the offset is added to obtain the ranging prediction value. The difference between the actual UWB measurement value and the ranging prediction value is used as the ranging residual. The ranging residual and Jacobian matrix are constructed, and the base station coordinates, ranging offset, scale, and time delay are estimated online as needed. Outliers are eliminated using a chi-square threshold.

[0081] The system constructs a gain and jointly updates the state variables based on this gain. The state variables include position, attitude, velocity, inertial measurement unit (IMU) bias, gravity, extrinsic parameters, and ultra-wideband state. It checks if the iteration has converged. If not, it returns to the iterative measurement update phase to continue execution; if converged, it outputs the posterior six-DOF state. The posterior six-DOF state is then converted into the robot's extrinsic parameters via the IMU to obtain the base six-DOF pose, and odometry information including the acquired pose and covariance is published.

[0082] Optionally, when zero-velocity correction is enabled, it suppresses the drift of the inertial measurement unit by constraining it with zero velocity and zero angular velocity during the stationary phase of the robot.

[0083] The above-described normal positioning process continues to run. When the robot loses its position, the repositioning process of the positioning module is triggered by receiving the initial pose command. The repositioning process may include: Upon receiving the initial pose command, the repositioning flag is reset and the filter is reset to set the prior with the initial pose.

[0084] Perform inertial measurement unit initialization and complete zero bias and gravity alignment.

[0085] Search for candidate initial values ​​within the yaw angle range, and perform iterative nearest point coarse registration between the current laser scan frame and the first map. Point-to-point or point-to-area iterative nearest point method can be used.

[0086] Calculate the proportion of valid matching points, i.e., the percentage of point clouds within a distance threshold. Determine if the valid proportion meets the standard. If it does, resume pose acquisition and switch back to normal localization mode. If it does not meet the standard, adjust the candidate initial values ​​and retry registration. If the number of attempts exceeds a preset number, the relocalization is deemed a failure and the system is reset, waiting for the next initial pose command.

[0087] The offline mapping layer receives the acquired pose from the positioning module. During the offline mapping phase, the first map undergoes height filtering, planar projection, and rasterization. A grid-by-grid, neighbor-by-neighbor accessibility determination is then performed using a dual criterion of step height threshold and slope angle threshold. Top clearance determination and point density confidence determination are then overlaid to generate the second map. The offline mapping layer loads this second map as a static prior layer into the cost map of the 2D navigation framework. During the offline mapping phase, the first map, after height filtering, planar projection, and rasterization, is converted into the second map file via an offline map link and loaded into the 2D navigation framework by the map server.

[0088] The construction process of the second map in offline layer building: Read the first map, perform height filtering according to a preset height threshold to remove ground noise and ceiling outliers; divide the map into grids according to a preset resolution to build an elevation map, taking the configurable quantile for the ground height of each grid, and recording the number of point clouds and the lowest point above the ground; perform mean smoothing on the effective area; perform top clearance judgment: the difference between the lowest point above the ground and the ground height is taken as the top clearance, and when the top clearance is less than the sum of the robot height and the safety margin, it is marked as a top obstacle; perform dual-criteria judgment grid by grid and neighbor by neighbor: calculate the maximum allowable height difference for 8 neighbors based on horizontal distance, and count the proportion of reachable neighbors; when When the reachability ratio reaches the threshold, the point cloud density meets the standard, and there are no top obstacles, the grid is marked as passable; when the reachability ratio is too low, the maximum height difference exceeds the step height threshold, or there are top obstacles, the grid is marked as an obstacle; otherwise, it is marked as unknown. Post-processing is performed to remove isolated passable grids and fill isolated obstacle and unknown grids surrounded by passable grids; the grid is downsampled to the output resolution, and obstacle dilation can be performed optionally. If there are obstacles within a block, it is marked as an obstacle; if there are passable grids, it is marked as passable; otherwise, it is marked as unknown. A standard two-dimensional occupied grid map file, i.e., the second map, is output. For example, passable grids are marked as 254, obstacle grids as 0, and unknown grids as 205, for use by the two-dimensional navigation framework.

[0089] A schematic diagram of the dual-criteria determination method in offline layer creation is shown below. Figure 3 As shown, Figure 3 A schematic diagram is shown of a scenario where accessibility is determined using both steps and slope criteria in an 8-neighborhood grid.

[0090] Figure 3 (a) is a top view, with the central grid cell located in the center, and the eight surrounding neighboring grid cells located at the top, bottom, left, right, and four diagonal directions. The horizontal distance d between the positive neighbors is equal to the grid resolution r, and the horizontal distance d between the diagonal neighbors is equal to the grid resolution multiplied by the square root of 2.

[0091] Figure 3 (b) is a cross-sectional view showing the accessibility criteria for three adjacent height relationships. When the actual height difference is less than or equal to the step height threshold, the neighbor is considered passable, corresponding to stepped terrain; when the slope angle is less than or equal to the slope angle threshold, the neighbor is considered passable, corresponding to sloping terrain; when the terrain is a wall or platform, and the actual height difference exceeds the maximum allowable height difference, the neighbor is considered an obstacle.

[0092] The condition for neighborhood reachability is: the actual height difference is less than or equal to the maximum allowable height difference.

[0093] Figure 4 A schematic diagram of the overhead clearance observation scenario is shown. (As shown) Figure 4As shown, the robot's base coordinate system is located at the center of the robot body and has a pose acquisition function, which includes position information and three-dimensional orientation. The robot's forward local elevation window is a scrolling window that covers a preset terrain area in front of the robot. The ground elevation includes various terrains such as flat land and slopes.

[0094] Top clearance ( Figure 4 The calculation method for the top clearance (Δ) is as follows: subtract the ground elevation and then the installation offset from the z-component of the acquired pose, and then perform amplitude limiting. The specific formula is: Top clearance = z-component - ground elevation - offset (amplitude limiting). This top clearance observation value serves as the basic observation data for the online dynamic cost layer, complementing and verifying the real-time local elevation map, and is input into the online dynamic cost layer for use.

[0095] The online dynamic cost layer receives the corrected point cloud data and acquired pose from the localization module. During runtime, it constructs a real-time local elevation map of the surrounding area, calculates gradients and slopes grid-by-grid, and uses the same dual criteria as the offline layer for accessibility determination. It then calculates the accessibility margin and maps it to an accessibility cost. The online dynamic cost layer writes this accessibility cost back to the cost map and merges it with the offline static prior layer.

[0096] The online real-time gradient slope detection and walkable cost feedback process executed by the online dynamic cost layer can include: Subscribe to the corrected point cloud data and acquired pose output by the positioning module.

[0097] A real-time local elevation map is constructed with the robot as the center and a preset scrolling window. The height is counted by grid, and the missing grid is reverted to the prior elevation or marked as unknown.

[0098] Gradient and slope calculations are performed grid by grid. The height difference and slope converted by horizontal distance are calculated for 8 neighbors. The dual criteria of step height threshold and slope angle threshold consistent with offline calculations are used to obtain the maximum allowable height difference of each neighbor.

[0099] Calculate the passability margin and apply top headroom and point density determinations.

[0100] Costs are mapped based on the accessibility margin, and the costs are labeled in the second map to obtain the current movement area. Accessibility margin is mapped to accessible cost and inaccessible cost, with inaccessible cost being greater than all accessible costs; if an unobserved area is not updated, the prior or unknown state is retained.

[0101] The local elevation grid is transformed to the map coordinate system according to the acquisition pose and written into the corresponding cost unit to compensate for the robot's own tilt on the slope.

[0102] The second map and the robot's traversability costs at each location within the current movement area are fused to obtain a third map. The fusion method is either conservative or confidence-weighted. Safety is prioritized in the fusion method. Conservative means that for any grid cell, if the static layer marks it as an obstacle, the merged cell is directly marked as an obstacle, and any traversability determination for that grid cell by the dynamic layer is ignored; the real-time cost of the dynamic layer only applies to areas marked as traversable or unknown by the static layer. Confidence-weighted fusion is based on the localization covariance or point cloud density confidence level. The dynamic layer updates as the scrolling window moves with the robot, responding in real-time to terrain changes.

[0103] The integrated third map is used by the planner and controller of the two-dimensional navigation framework for path planning and obstacle avoidance, prioritizing low-cost, flatter, and more traversable terrain.

[0104] Figure 5 This diagram illustrates the fusion of the online dynamic cost layer and the offline static prior layer in a cost map.

[0105] Figure 5 (a) is the offline static prior layer, i.e., the second map. The second map is a standard two-dimensional occupancy raster map generated by the offline construction layer. Each raster is marked as passable, obstructed, or unknown, where passable is marked as 0 (white), obstructed as 254 (black), and unknown as 205 (gray). It is loaded as a static layer onto the cost map.

[0106] Figure 5 (b) is the online dynamic cost layer. The online dynamic cost layer is dynamic layer data generated based on the real-time local elevation map during online operation. It contains continuously graded accessibility costs and can reflect new terrain changes that occur during operation, such as the addition of steps. It is written into the cost map as a dynamic layer.

[0107] Figure 5 (c) shows the fused third map. The static and dynamic layers are fused in the cost map using either conservative values ​​or confidence weighting to generate the third map, i.e., the cost map. In the fused third map, terrain close to the robot's capability limit has a higher traversable cost, while terrain with ample traversable margin has a lower traversable cost. The planner prioritizes terrain with lower traversable costs for path planning based on the third map. The dynamic layer updates as the scrolling window moves with the robot, responding in real time to terrain changes, achieving complementary fusion of offline static prior information and online dynamic terrain information.

[0108] The positioning adapter, acting as a middleware node between the positioning module and the 2D navigation framework, seamlessly integrates non-standard 3D positioning sources into the standard navigation process. The positioning adapter receives the acquired pose output from the positioning module and decomposes the global pose into a global anchoring segment and a dynamic six-DOF segment. The global anchoring segment sets the transformation from the map coordinate system to the odometry coordinate system as a unit transformation. During the adaptation process, after receiving a positioning message, the positioning adapter reads the position vector and the complete attitude quaternion, checks the validity of the quaternion, and normalizes it. The 3D attitude is not forcibly projected as a pure yaw angle; that is, the roll and pitch angle information is completely preserved in the coordinate transformation link. The complete acquired pose is injected into the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, preserving the z-component in the position information and the roll and pitch angles in the 3D attitude, thus achieving coordinate and interface adaptation. Specifically, the six-DOF pose of the robot base coordinate system relative to the map coordinate system output by the positioning module at time t is T. M_B(t) The global anchoring transformation from the map coordinate system to the odometer coordinate system is T. M_O The dynamic transformation from the odometry coordinate system to the robot base is T. O_B(t) Then T is satisfied. M_B(t) =T M_O ×T O_B(t) Therefore, T O_B(t) = T M_O multiplying the inverse matrix by T M_B(t) The 3D positioning result is already in the navigation map coordinate system, let T... M_O If T is the identity matrix, then O_B(t) =T M_B(t) The positioning adapter outputs the coordinate transformation link to the 2D navigation framework. It can also encapsulate the acquired pose and covariance into a standard pose message. The message coordinate system is set to the map coordinate system. The pose retains the complete position and complete quaternion. The covariance is copied from the positioning message for use by visualization, status logging, or navigation components that only accept the standard pose interface.

[0109] 3D positioning ensures six degrees of freedom accuracy, the positioning adapter completes interface conversion, and the first map synchronously generates a 2D navigation map, ultimately achieving high-efficiency 2D calculation. The adapter parameters include at least the positioning odometry topic, map coordinate system, odometry coordinate system, robot base coordinate system, global anchoring transformation maintenance frequency, and whether to publish standard pose messages.

[0110] In this embodiment of the invention, the positioning module performs registration-based pure positioning based on a first map, and the output pose is naturally located in the global map coordinate system without cumulative drift. The positioning adapter sets the transformation from the map coordinate system to the odometry coordinate system as a unit transformation, injecting the complete acquired pose into the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system. This ensures both the uniqueness of the coordinate transformation tree and the continuity of the coordinate transformation relationship, avoiding instability of the navigation system due to timestamp jumps, while also completely preserving the z-component in the position information and the roll and pitch angles in the three-dimensional pose. An offline static prior layer ensures global integrity and consistency, while an online dynamic cost layer responds to unmapped and dynamic terrain and provides a safety margin. The dynamic layer updates with the robot's movement as the window scrolls, responding to terrain changes in real time. The two are fused in the standard cost map, using either conservative values ​​or confidence weighting, where confidence weighting is based on the positioning covariance or point cloud density confidence. The online layer outputs continuously graded costs based on the traversability margin, allowing the planner to prioritize terrain with smoother slopes and greater margins. Because the transformation relationship from the map coordinate system to the odometer coordinate system is fixed as a unit transformation, and there is no longer a need for repeated inversions and multiplications of two dynamic transformations, noise is avoided from being transmitted to this transformation relationship, thereby reducing coordinate transformation jitter and improving the stability of the navigation system. This invention embodiment does not require modification of the two-dimensional navigation framework. By providing the coordinate transformation chain required by its native architecture, the three-dimensional positioning results can be directly used to serve standard maps, planning, and control modules. Through parameterization of topic name, coordinate system name, and publication frequency, different positioning algorithms can be replaced without modifying the core of the navigation framework. A second map is generated from the first map used for three-dimensional positioning, ensuring that the positioning map and navigation map have a unified data source and coordinate reference.

[0111] The main chain of the coordinate transformation tree of the positioning adapter includes the world coordinate system, map coordinate system, odometer coordinate system, and robot base coordinate system. The transformation from the world coordinate system to the map coordinate system is a static unit transformation; the transformation from the map coordinate system to the odometer coordinate system is a unit transformation, aligning the map coordinate system and the odometer coordinate system to form a global anchoring segment. The acquired pose is injected into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. This transformation contains complete position information and three-dimensional attitude. The position information retains the z-component, and the three-dimensional attitude retains complete quaternions, including roll angle, pitch angle, and yaw angle, forming a dynamic six-degree-of-freedom segment. The transformation from the robot base coordinate system to each sensor coordinate system (including the LiDAR coordinate system and the inertial measurement unit coordinate system) is a fixed extrinsic parameter. The three key design features of the coordinate transformation tree are: global anchoring segments maintain immutability, meaning the map coordinate system and the odometry coordinate system coincide; six degrees of freedom are fully injected into the dynamic segments, with translation retaining the z-component and rotation retaining roll, pitch, and yaw angles; the 2D module extracts x, y, and yaw angles from the complete pose as needed, while the 3D module can still read the z-component, roll, and pitch angles from the same transformation chain. The global anchoring segment and the dynamic six-degree-of-freedom segment together form a standard coordinate transformation link from the map coordinate system to the odometry coordinate system to the robot base coordinate system, creating a standard navigation coordinate transformation tree that retains height and complete attitude information for use by the 2D navigation framework.

[0112] The specific execution flow of the positioning adapter is as follows: After the positioning adapter starts, it first loads parameters, including the map coordinate system, odometry coordinate system, robot base coordinate system, publishing frequency, and topic name. Then, it publishes the unit transformation from the map coordinate system to the odometry coordinate system at a fixed frequency. This transformation is static and identical, and the timestamp is continuously refreshed.

[0113] The positioning adapter runs two looping threads in parallel: a coordinate transformation maintenance thread and a pose access callback thread.

[0114] In the coordinate transformation maintenance thread, the timer continuously triggers callbacks at a fixed frequency (e.g., 10Hz) and performs the following operations in a loop: subscribe to collect pose information, construct the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, the translation amount of the coordinate transformation relationship includes the x, y, and z components in the position information, the attitude quantity is a complete quaternion, and publish the coordinate transformation relationship.

[0115] In the pose input callback thread, the following operations are performed in a loop: First, the acquired pose, which includes position information and 3D pose, is read. Then, the validity of the quaternions in the 3D pose is checked and normalized. During adaptation, the 3D pose is not forcibly projected as a pure yaw angle; that is, roll and pitch angle information are fully preserved in the coordinate transformation link. Next, a coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system is constructed and published. Optionally, a standard pose message is published in the map coordinate system. This standard pose message includes the acquired pose and pose covariance. The message coordinate system is set to the map coordinate system. The pose retains the complete position and complete quaternions, and the pose covariance is copied from the positioning message. This standard pose message can be used for visualization, status logging, task management, or navigation components that only accept the standard pose interface. After completing one operation, the system waits for the next frame of positioning data and repeats the above process.

[0116] The coordinate transformation maintenance thread runs continuously at a fixed frequency, while the pose access callback thread is triggered and executed as positioning data arrives. The two threads work in parallel to jointly complete the coordinate transformation link maintenance and pose publishing functions of the positioning adapter.

[0117] The 2D navigation framework includes modules such as a map server and cost map, global path planning, local trajectory control and obstacle avoidance, behavior tree, and task execution. During operation, the 2D navigation framework starts the global cost map, local cost map, planner, controller, and behavior tree navigator. The global cost map operates in the map coordinate system, while the local cost map operates in the odometry coordinate system. The map server loads a second map generated from the first map through height filtering, planar projection, and rasterization, for use by the cost map. During the conversion process, based on the point cloud height and distribution within each grid, and the top clearance determination result (the difference between the lowest point above the grid and the ground height), when the top clearance is less than the sum of the robot's height and safety margin, it is marked as a top obstacle, and the grid is marked as passable, obstacle, or unknown area. The 2D navigation framework obtains the robot's current pose through the coordinate transformation link output by the positioning adapter. This coordinate transformation link is from the map coordinate system to the odometry coordinate system to the robot's base coordinate system. At the same time, it receives the fusion result of the offline static prior layer and the online dynamic cost layer as the third map, namely the cost map. The fusion method is to take conservative values ​​or confidence weighting. The confidence weighting is based on the localization covariance or point cloud density confidence. The 2D navigation framework maintains a unique coordinate transformation link, performs global path planning and local trajectory control. Waypoint task nodes perform multi-point, cyclic or path navigation tasks by following waypoints or waypoints, and send the planning and control results to the robot for execution.

[0118] Figure 6This is a schematic diagram of a robot navigation device provided in an embodiment of the present invention. The embodiments of the present invention are applicable to autonomous robot navigation, especially suitable for robots navigating autonomously in three-dimensional terrain environments including slopes, steps, and undulating ground. The device can execute robot navigation methods and can be implemented in hardware and / or software. The device can be configured in an electronic device that carries robot navigation functions.

[0119] See Figure 6 The robot navigation device shown includes: The pose determination module 601 is used to determine the robot's acquired pose in the map coordinate system based on the first map, inertial measurement data and point cloud data collected by the robot, wherein the acquired pose includes position information and three-dimensional posture. The cost determination module 602 is used to determine the passable cost of each position in the current motion region associated with the acquired pose based on the point cloud data. The transformation link acquisition module 603 is used to acquire the coordinate transformation links natively supported by the two-dimensional navigation framework. The coordinate transformation links include the coordinate transformation relationship between the map coordinate system, the odometer coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometer coordinate system is fixed as a unit transformation. The pose writing module 604 is used to write the acquired pose into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system according to the transformation relationship from the map coordinate system to the odometer coordinate system, so as to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. The current pose acquisition module 605 is used to query the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system based on the two-dimensional navigation framework, and obtain the current pose of the robot in the robot base coordinate system. The navigation control module 606 is used to fuse the traversability cost of the robot at each position in the current motion area with a pre-built second map based on the two-dimensional navigation framework, and control the robot to travel along traversable positions according to the current pose. The second map is a height map obtained by performing height filtering and traversability discrimination on the first map. The first map, the second map and the current motion area are all located in the map coordinate system.

[0120] In an optional embodiment, the pose writing module 604 is specifically used for: Using the transformation relationship from the map coordinate system to the odometer coordinate system as the unit transformation, the position information and three-dimensional attitude in the collected pose are used as the translation and attitude quantities in the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system, thus obtaining the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system.

[0121] In an optional embodiment, the robot navigation device further includes a map building module; the map building module is configured to: Before determining the passable cost of each position in the current motion area associated with the acquired pose based on the point cloud data, the first map is height filtered and rasterized to generate a height raster map. For each first grid cell in the height grid map, the maximum allowable height difference is calculated based on the horizontal distance between the first grid cell and its neighboring grid cells, the robot's step height threshold, and the slope angle threshold. Based on the height difference between the first grid and its neighboring grids and the maximum allowed height difference, the percentage of reachable grids of the first grid is calculated. Based on the proportion of reachable grid cells, point cloud density, and top clearance of the first grid cell, the passability of the first grid cell is determined, and the passability determination result of the first grid cell is obtained. The top clearance is the difference between the lowest point above the grid cell and the ground. The accessibility determination results of each first grid cell are marked in the height grid map to obtain the second map.

[0122] In an optional embodiment, the cost determination module 602 is configured to: Based on the point cloud data, determine the horizontal distance and height difference between each second grid cell and its neighboring grid cells in the current motion region associated with the acquired pose; For each second grid cell in the current movement area, calculate the slope angle and step height between the second grid cell and its neighboring grid cells based on the horizontal distance and height difference between the second grid cell and its neighboring grid cells; Based on the slope angle and step height between the second grid and its neighboring grids, the passability cost of the second grid is determined and used as the passability cost of the robot at each position in the current movement area.

[0123] In an optional embodiment, the passability cost determination module is specifically used for: The maximum slope angle and maximum step height of the second grid are determined based on the slope angle and step height between the second grid and its neighboring grids. The passability margin of the second grid is determined based on the ratio between the maximum slope angle of the second grid and the slope angle threshold, and the ratio between the maximum step height of the second grid and the step height threshold. When the accessibility margin is less than or equal to the margin boundary value corresponding to the robot, the accessibility margin of the second grid is continuously mapped to the target value range to obtain the accessibility cost. The accessibility cost is positively correlated with the accessibility margin. When the passability margin is greater than the margin boundary value, the passability cost of the second grid is determined to be the inaccessible cost, and the inaccessible cost is greater than the value in the target range.

[0124] In one alternative embodiment, the navigation control module 606 is specifically used for: Based on the two-dimensional navigation framework, the second map and the traversability cost of the robot at each position in the current movement area are fused to obtain the third map; Based on a two-dimensional navigation framework, the robot plans its path from its current position to the target position on a third map. Based on a two-dimensional navigation framework, the robot is controlled to travel along a planned path according to its current pose.

[0125] In an optional embodiment, the pose determination module 601 is specifically used for: A spatial index is constructed based on the first map; Based on the inertial measurement data collected by the robot, motion distortion correction is performed on the point cloud data collected by the robot to obtain the corrected point cloud data; Based on the inertial measurement data collected by the robot, determine the robot's state prediction value and state covariance prediction value; By using spatial indexing, the corrected point cloud data is registered with the first map to obtain the point cloud registration residual; The robot's pose in the map coordinate system is obtained by fusing the predicted state value, the predicted state covariance value, and the point cloud registration residual.

[0126] The robot navigation device provided in the embodiments of the present invention can execute the robot navigation method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0127] The acquisition, storage, and application of map data, point cloud data, and inertial measurement data involved in the technical solutions of this invention comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0128] Figure 7A schematic diagram of an electronic device 700 that can be used to implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0129] like Figure 7 As shown, the electronic device 700 includes at least one processor 701 and a memory, such as a read-only memory (ROM) 702 and a random access memory (RAM) 703, communicatively connected to the at least one processor 701. The memory stores computer programs executable by the at least one processor. The processor 701 can perform various appropriate actions and processes based on the computer program stored in the ROM 702 or loaded into the RAM 703 from storage unit 708. The RAM 703 can also store various programs and data required for the operation of the electronic device 700. The processor 701, ROM 702, and RAM 703 are interconnected via a bus 704. An input / output (I / O) interface 705 is also connected to the bus 704.

[0130] Multiple components in electronic device 700 are connected to I / O interface 705, including: input unit 706, such as keyboard, mouse, etc.; output unit 707, such as various types of displays, speakers, etc.; storage unit 708, such as disk, optical disk, etc.; and communication unit 709, such as network card, modem, wireless transceiver, etc. Communication unit 709 allows electronic device 700 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0131] Processor 701 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 701 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, digital signal processors (DSPs), and any suitable processor, controller, microcontroller, etc. Processor 701 performs the various methods and processes described above, such as robot navigation methods.

[0132] In some embodiments, the robot navigation method may be implemented as a computer program tangibly included in a computer-readable storage medium, such as storage unit 708. In some embodiments, part or all of the computer program may be loaded and / or installed on electronic device 700 via ROM 702 and / or communication unit 709. When the computer program is loaded into RAM 703 and executed by processor 701, one or more steps of the robot navigation method described above may be performed. Alternatively, in other embodiments, processor 701 may be configured to perform the robot navigation method by any other suitable means (e.g., by means of firmware).

[0133] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0134] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0135] In the context of this invention, a computer-readable storage medium can be a tangible medium that may include or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0136] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0137] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or middleware components (e.g., application servers), or frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0138] A computing system can include clients and servers. Clients and servers are generally geographically separated and typically interact via communication networks. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system. It addresses the shortcomings of traditional physical hosts and VPS (Virtual Private Server) services, such as high management difficulty and weak business scalability.

[0139] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0140] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A robot navigation method, characterized in that, The method includes: Based on the first map, inertial measurement data and point cloud data collected by the robot, the robot's acquisition pose in the map coordinate system is determined, and the acquisition pose includes position information and three-dimensional posture. Based on the point cloud data, determine the passability cost of each position in the current motion region associated with the acquired pose; Obtain the coordinate transformation links natively supported by the 2D navigation framework. The coordinate transformation links include the coordinate transformation relationships between the map coordinate system, the odometry coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation. According to the transformation relationship from the map coordinate system to the odometer coordinate system, the acquired pose is written into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. Based on the two-dimensional navigation framework, the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system is queried to obtain the current pose of the robot in the robot base coordinate system; Based on the two-dimensional navigation framework, the traversability cost of the robot at each position in the current movement area is fused with a pre-built second map, and the robot is controlled to travel along traversable positions according to the current pose. The second map is a height map obtained by performing height filtering and traversability discrimination on the first map. The first map, the second map, and the current movement area are all located in the map coordinate system.

2. The method according to claim 1, characterized in that, The step of writing the acquired pose into the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system according to the transformation relationship from the map coordinate system to the odometry coordinate system, to obtain the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system, includes: Using the transformation relationship from the map coordinate system to the odometer coordinate system as the unit transformation, the position information and three-dimensional attitude in the acquired pose are used as the translation and attitude quantities in the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system, thus obtaining the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system.

3. The method according to claim 1, characterized in that, Before determining the traversable cost of each position in the current motion region associated with the acquired pose based on the point cloud data, the method further includes: The first map is height filtered and rasterized to generate a height raster map; For each first grid cell in the height grid diagram, the maximum allowable height difference is calculated based on the horizontal distance between the first grid cell and its neighboring grid cells, the robot's step height threshold, and the slope angle threshold. Based on the height difference between the first grid and its neighboring grids and the maximum allowed height difference, the percentage of reachable grids of the first grid is calculated. Based on the proportion of reachable grid cells, point cloud density, and top clearance of the first grid cell, the first grid cell is judged to be passable, and the passability judgment result of the first grid cell is obtained. The top clearance is the difference between the lowest point above the grid cell and the ground. The accessibility determination results of each of the first grid cells are marked in the height grid map to obtain the second map.

4. The method according to claim 1, characterized in that, The step of determining the traversable cost of each position in the current motion region associated with the acquired pose based on the point cloud data includes: Based on the point cloud data, determine the horizontal distance and height difference between each second grid cell and its neighboring grid cells in the current motion region associated with the acquired pose; For each second grid cell in the current motion area, the slope angle and step height between the second grid cell and its neighboring grid cells are calculated based on the horizontal distance and height difference between the second grid cell and its neighboring grid cells. Based on the slope angle and step height between the second grid and its neighboring grids, the passability cost of the second grid is determined and used as the passability cost of the robot at each position in the current movement area.

5. The method according to claim 4, characterized in that, The step of determining the passability cost of the second grid based on the slope angle and step height between the second grid and its neighboring grids includes: The maximum slope angle and maximum step height of the second grid are determined based on the slope angle and step height between the second grid and its neighboring grids. The passability margin of the second grid is determined based on the ratio between the maximum slope angle of the second grid and the slope angle threshold, and the ratio between the maximum step height of the second grid and the step height threshold. When the traversability margin is less than or equal to the margin boundary value corresponding to the robot, the traversability margin of the second grid is continuously mapped to the target value range to obtain the traversability cost, which is positively correlated with the traversability margin. When the passability margin is greater than the margin boundary value, the passability cost of the second grid is determined to be the impassable cost, and the impassable cost is greater than the value in the target value range.

6. The method according to claim 1, characterized in that, The step of fusing the traversability cost of the robot at each position in the current movement area with a pre-built second map based on the two-dimensional navigation framework, and controlling the robot to travel along traversable positions according to the current pose, includes: Based on the two-dimensional navigation framework, the second map and the traversability cost of the robot at each position in the current movement area are fused to obtain the third map; Based on the two-dimensional navigation framework, the robot plans a path from its current position to the target position on the third map; Based on the two-dimensional navigation framework, the robot is controlled to travel along the planned path according to the current pose.

7. The method according to claim 6, characterized in that, The determination of the robot's pose in the map coordinate system based on the first map, inertial measurement data collected by the robot, and point cloud data includes: A spatial index is constructed based on the first map; Based on the inertial measurement data collected by the robot, motion distortion correction is performed on the point cloud data collected by the robot to obtain the corrected point cloud data; Based on the inertial measurement data collected by the robot, the predicted state value and the predicted state covariance value of the robot are determined; Using the spatial index, the corrected point cloud data is registered with the first map to obtain the point cloud registration residual; The robot's acquisition pose in the map coordinate system is obtained by fusing the predicted state value, the predicted state covariance value, and the point cloud registration residual.

8. A robot navigation device, characterized in that, The device includes: The pose determination module is used to determine the robot's acquired pose in the map coordinate system based on the first map, inertial measurement data and point cloud data collected by the robot. The acquired pose includes position information and three-dimensional posture. The cost determination module is used to determine the passability cost of each position in the current motion region associated with the acquired pose based on the point cloud data. The transformation link acquisition module is used to acquire the coordinate transformation links natively supported by the two-dimensional navigation framework. The coordinate transformation links include the coordinate transformation relationship between the map coordinate system, the odometry coordinate system and the robot base coordinate system, and the transformation relationship from the map coordinate system to the odometry coordinate system is fixed as a unit transformation. The pose writing module is used to write the acquired pose into the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system according to the transformation relationship from the map coordinate system to the odometer coordinate system, so as to obtain the coordinate transformation relationship from the odometer coordinate system to the robot base coordinate system. The current pose acquisition module is used to query the coordinate transformation relationship from the odometry coordinate system to the robot base coordinate system based on the two-dimensional navigation framework, and obtain the current pose of the robot in the robot base coordinate system; The navigation control module is used to fuse the traversability cost of the robot at each position in the current motion area with a pre-built second map based on the two-dimensional navigation framework, and control the robot to travel along traversable positions according to the current pose. The second map is a height map obtained by performing height filtering and traversability discrimination on the first map. The first map, the second map and the current motion area are all located in the map coordinate system.

9. An electronic device, characterized in that, The electronic device includes: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor to enable the at least one processor to perform the robot navigation method according to any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that cause a processor to execute the robot navigation method according to any one of claims 1-7.

Images