A multimodal embodied navigation method, system and storage medium

Abstract

The application discloses a multimodal embodied navigation method, system and storage medium, the method comprising: acquiring multimodal sensor data and generating multimodal perception features; constructing a factor graph, configuring a cross-modal consistency factor in which the modal credibility and the local alignment parameter are state variables to be optimized; based on the multimodal perception features, jointly optimizing the robot pose and the state variables in the factor graph, obtaining the optimal pose and the pose covariance; updating the target reachable probability grid based on the optimal pose and the pose covariance, the grid stores the success probability of the grid node reaching the target, and the navigation trajectory maximizing the success rate is planned accordingly. The application variableizes the modal weight, solves the problem that the fixed weight cannot adapt to sensor failure, couples the positioning uncertainty to the navigation decision, solves the problem that the traditional planning ignores the positioning risk, and improves the robustness and safety of the robot in a dynamic and complex environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot autonomous navigation, and in particular to a multi-modal embodied navigation method, system and storage medium. BACKGROUND

[0002] With the development of embodied intelligence technology, the autonomous navigation capability of mobile robots in complex unstructured environments has become a key to its landing. In order to overcome the perception limitations of a single sensor in a specific scenario, multi-modal fusion positioning is used with multiple sensors such as lidar, vision camera, inertial measurement unit (IMU), etc., which helps to achieve high-precision and high-robustness navigation and can ensure the stable operation of the robot in scenes with severe light changes, single geometric features or dense dynamic obstacles.

[0003] Current research focuses on multi-sensor fusion schemes based on filtering or factor graph optimization. Existing frameworks use loose or tight coupling to stack visual odometry, lidar odometry and IMU pre-integration constraints into the optimization objective. In some schemes, the data weight (or inverse of the covariance matrix) of each sensor modality is fixed as a hyperparameter at system initialization, or only a simple threshold switching is performed according to the signal-to-noise ratio of the sensor itself. At the navigation decision level, some path planning algorithms are based on occupancy grid maps, aiming to minimize path length or time cost, and use A* or DWA algorithms to search for the optimal path, where A* is a heuristic search algorithm that adds an estimated cost to the target point based on the Dijkstra algorithm, which can find the optimal path from the starting point to the end point faster.

[0004] However, the adaptability of the existing technology in extreme environments still has the problem of rigid fusion mechanism and fragmented sensing control. Therefore, further research and innovation are needed to solve the above problems existing in the prior art. SUMMARY

[0005] The present application provides a multi-modal embodied navigation method, system and storage medium.

[0006] Technical solution: In a first aspect, a multi-modal embodied navigation method includes:

[0007] Obtaining current image data and lidar point cloud of the robot, generating multi-modal perception features based on a pre-set perception model, including network predicted depth map, image semantic probability distribution and point cloud semantic label;

[0008] Constructing a factor graph, configuring a cross-modal consistency factor in the factor graph, which takes modality credibility and local alignment parameters as state variables to be optimized;

[0009] Based on the multimodal perception features and the laser radar point cloud, joint nonlinear optimization is performed on the robot pose in the factor graph and the state variables to be optimized to obtain an optimal pose and a pose covariance;

[0010] Based on the optimal pose and the pose covariance, a target reachable probability grid is updated, and a navigation trajectory is planned based on the target reachable probability grid to control the movement of the robot.

[0011] In a second aspect, a multimodal embodied navigation system is provided, comprising:

[0012] A perception module is configured to obtain current image data and laser radar point cloud of the robot, and generate multimodal perception features based on a preset perception model;

[0013] An optimization module is configured to construct a factor graph, and configure a cross-modal consistency factor taking the modality confidence and the local alignment parameter as state variables to be optimized in the factor graph; based on the multimodal perception features and the laser radar point cloud, joint nonlinear optimization is performed on the robot pose in the factor graph and the state variables to be optimized to obtain an optimal pose and a pose covariance;

[0014] A navigation module is configured to update a target reachable probability grid based on the optimal pose and the pose covariance, and plan a navigation trajectory based on the target reachable probability grid to control the movement of the robot.

[0015] In a third aspect, a computer readable storage medium is provided, which stores a computer program, and the computer program is executed by a processor to implement the method of any one of the first aspect.

[0016] Beneficial effects: The multimodal embodied navigation method provided by the present application adopts a cross-modal consistency factor layer and a target reachable probability grid technology, solves the rigidification of the fusion mechanism and the fragmentation of the sensing and control links, and improves the robustness and safety of the robot in a dynamic and complex environment. The related technical effects will be described in detail below in conjunction with specific embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0017] Fig. 1 A flowchart of a multimodal embodied navigation method provided by an embodiment of the present application.

[0018] Fig. 2 A flowchart of an example of configuring a cross-modal consistency factor taking the modality confidence and the local alignment parameter as state variables to be optimized in a factor graph provided by an embodiment of the present application.

[0019] Fig. 3 A flowchart of another example of configuring a cross-modal consistency factor taking the modality confidence and the local alignment parameter as state variables to be optimized in a factor graph provided by an embodiment of the present application.

[0020] Fig. 4A flowchart for updating a target reachable probability grid based on an optimal pose and a pose covariance is provided for embodiments of the present application. DETAILED DESCRIPTION

[0021] In order for those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should fall within the scope of protection of the present application.

[0022] It should be noted that the terms first, second, etc. in the specification of the present application and in the above-described drawings are used to distinguish similar objects, and do not necessarily have to be used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms include and have and any variations thereof are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not have to be limited to the clearly listed steps or units, but can include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0023] In order to solve the above problems, the applicant has conducted in-depth retrieval and analysis, and found that:

[0024] Correspondingly, the modal weight of the existing method is mostly a preset fixed value. When a certain sensor encounters instantaneous failure (such as a camera encountering strong light glare, a radar entering a long corridor), the fixed weight cannot timely suppress the interference of the wrong data, resulting in divergence of the positioning result and even system crash.

[0025] Next, the existing navigation planning strategy usually assumes that the positioning is accurate, ignoring the dynamic distribution characteristics of the positioning error with the degradation of environmental features; the blind planning mode of positioning can cause the robot to risk crossing high-uncertainty areas (such as feature sparse areas) in order to pursue the shortest path, which is easy to cause the navigation task to fail due to loss of positioning.

[0026] In order to solve these problems, in combination with Figs. 1 to 4 The present application is specifically illustrated by the following embodiments.

[0027] In some embodiments, an exemplary scheme of a multi-modal embodied navigation method is provided. The problems of positioning failure in dynamic or degraded scenes and the difficulty of dynamic adaptation of navigation strategies to environmental uncertainty caused by fixed sensor weights in traditional navigation methods are solved. Specifically, the present scheme includes:

[0028] Step S101, acquire the current image data and lidar point cloud of the robot, and generate multi-modal perception features based on a preset perception model.

[0029] In this step, the robot can be a wheeled robot, a tracked robot, or a legged robot. For example, in practical applications, the system can be deployed on a robot dog platform. Image data is collected in real time by a vision sensor installed on the front of the robot, which can be a monocular camera, a binocular camera, or a panoramic camera, for obtaining visual texture information of the environment. Lidar point cloud is collected in real time by a lidar sensor installed on the top of the robot, which can be a mechanical rotating or solid-state lidar, for obtaining three-dimensional geometric structure information of the environment.

[0030] Further, the preset perception model runs on an on-board computing platform, such as a processor based on a Rockchip RK3588. The perception model is an integrated deep learning network framework, which takes raw image frames and point cloud data as input, and outputs multi-modal perception features. The multi-modal perception features specifically include a network predicted depth map, an image semantic probability distribution, and a point cloud semantic label.

[0031] The network predicted depth map is a two-dimensional floating-point matrix consistent with the resolution of the input image, and the value of each element in the matrix represents the relative distance of the corresponding pixel point to the optical center of the camera. The image semantic probability distribution is a three-dimensional tensor, which not only contains the semantic class prediction of each pixel point, but also contains the confidence probability vector of the prediction. The point cloud semantic label is an integer class label assigned to each lidar point, used to distinguish different objects such as ground, building, vehicle, pedestrian, etc.

[0032] Based on this, the system converts the original physical signals into computer-understandable high-level semantic and geometric features, providing a data basis for subsequent consistency analysis.

[0033] Step S102, construct a factor graph, and configure a cross-modal consistency factor in the factor graph, which takes the modal credibility and local alignment parameters as state variables to be optimized;

[0034] Based on the multi-modal perception features and the lidar point cloud, the robot pose and the state variables to be optimized in the factor graph are jointly nonlinearly optimized to obtain the optimal pose and pose covariance.

[0035] Wherein, the factor graph is a probabilistic graphical model, used to represent the constraint relationship between the robot state and the observation data. Unlike the traditional factor graph that only optimizes the pose, the embodiment introduces the modal confidence and the local alignment parameter as state variables to be optimized in the factor graph. The modal confidence is a dynamic weight vector, used to quantify the reliability of each sensor modal data at the current time. For example, when strong light directly causes vision failure, the confidence of the vision modal will automatically decrease; in the long corridor and other geometric feature degradation scenarios, the confidence of the laser radar modal will automatically decrease. The local alignment parameter is used to describe the local linear transformation relationship between the visual depth and the laser radar depth, including the scale factor and the offset. By taking the above parameters as state variables into optimization, the system can adaptively correct the data deviation between different sensors.

[0036] Further, the cross-modal consistency factor is an error term connecting the state variables, which forces the data of different modalities to be consistent in geometric structure and semantic attribute. The joint nonlinear optimization is usually implemented by Levenberg-Marquardt algorithm or Gauss-Newton method. The optimizer adjusts the values of the robot pose, the modal confidence and the local alignment parameter through continuous iteration, so that the total residual error of all factors reaches the minimum.

[0037] On this basis, the output optimal pose is the accurate position and attitude of the robot in the world coordinate system, and the pose covariance is a variance matrix, the size of the diagonal elements of which reflects the uncertainty degree of the pose estimation.

[0038] The above process realizes the leap from passive fusion to active adaptive fusion, and improves the robustness of the system in extreme environments.

[0039] Step S103, update the target reachable probability grid based on the optimal pose and the pose covariance, and plan a navigation trajectory based on the (updated target reachable probability grid) to control the movement of the robot.

[0040] In this step, the target reachable probability grid is an environmental representation data structure. Unlike the traditional grid map that only records the obstacle occupancy, it stores the success probability of safely reaching the goal from each position in space. The optimal pose is used to determine the exact position of the robot in the grid, and the pose covariance is used to correct the probability values in the grid.

[0041] Specifically, when the positioning uncertainty is large, i.e., the trace of the covariance is large, the system will penalize the success probability of the far grid, so that the attraction of the high-risk area is reduced. The process of planning the navigation trajectory is not to find the shortest geometric curve, but to search for the path with the maximum cumulative success rate on the probability grid. This strategy makes the robot dare to explore the shortcut when the positioning is accurate, and tends to stick to the wall or move along the known feature-rich area when the positioning is erratic.

[0042] Further, the generated navigation trajectory is converted into motor speed instructions by the underlying controller to drive the robot to move towards the target point. Through the sensory-control coupling mechanism, the system realizes embodied intelligence, i.e., dynamically adjusting the behavior strategy according to the state of its own perception ability.

[0043] According to one aspect of the present application, a multi-modal embodied navigation method is provided, comprising:

[0044] Obtaining current image data and laser radar point cloud of the robot, processing the image data and laser radar point cloud based on a preset perception model to generate a network predicted depth map, an image semantic probability distribution and a point cloud semantic label;

[0045] Constructing a factor graph containing a cross-modal consistency factor, the cross-modal consistency factor introducing the modality reliability and the local alignment parameter as state variables to be optimized;

[0046] Based on the network predicted depth map, the image semantic probability distribution, the point cloud semantic label and the laser radar point cloud, the robot pose, the modality reliability and the local alignment parameter in the factor graph are jointly nonlinearly optimized to obtain an optimal pose and a pose covariance;

[0047] Updating the target reachable probability grid based on the optimal pose and the pose covariance, the target reachable probability grid storing the success probability of the grid node reaching the navigation target;

[0048] Planning a navigation trajectory based on the target reachable probability grid and controlling the robot to move.

[0049] In some other embodiments, an optional implementation of joint optimization based on the cross-modal consistency factor layer (CMCL) is described. In particular, how to construct specific depth consistency residuals, semantic consistency residuals and regularization terms to solve the modality reliability and alignment parameters, which are used to solve the problem that vision and laser radar are difficult to accurately align in terms of data dimension, scale and semantic level and are easily disturbed by the environment. In this embodiment, the following steps are included:

[0050] Step S201, introducing a local scale parameter and a local offset parameter as local alignment parameters, and introducing a depth modality reliability as a modality reliability;

[0051] projecting the lidar point cloud to the image plane where the network predicted depth map lies, to obtain the lidar geometric depth at the projected point;

[0052] constructing a linear mapping model between the network predicted depth map and the lidar geometric depth based on the local scale parameter and the local offset parameter, and calculating a depth consistency residual according to the linear mapping model;

[0053] wherein the depth consistency residual is configured to be dynamically weighted with the depth modality confidence.

[0054] In other words, the depth consistency belongs to a kind of cross-modality consistency factor.

[0055] In this step, the local scale parameter is denoted as a, and the local offset parameter is denoted as β. Both of them are used to linearly correct the monocular vision predicted relative depth, so that it is aligned with the absolute metric depth of the lidar. For example, in a specific optimization window, each frame of image corresponds to a set of to-be-optimized a and β. The depth modality confidence, whose value range is usually between 0 and 1, represents the reliability of the current frame of visual depth information.

[0056] In order to calculate the residual, cross-modality projection needs to be performed. For example, the coordinates of a certain point in the lidar coordinate system are read, which are transformed to camera coordinates through an extrinsic matrix to obtain camera coordinates, and then projected to the pixel coordinate plane through a camera intrinsic matrix to obtain pixel coordinates. At this time, the Z-axis component of the camera coordinates is the lidar geometric depth D lidar of the point. The system simultaneously reads the network predicted depth value D net at the pixel coordinates. The linear mapping model assumes that in the ideal case, D net after scaling and translation should be equal to D lidar , that is, D net = a x D lidar + β.

[0057] The depth consistency residual is the difference between the two sides of the equation. When the total cost is calculated, the residual is multiplied by the depth modality confidence. If the optimizer finds that the depth prediction of a certain frame of image is seriously inconsistent with the radar data, the influence of the frame of data on the pose solution can be reduced by reducing the value of the depth modality confidence, so as to automatically shield the error information.

[0058] wherein the depth consistency residual can be calculated by using the following formula:

[0059] r _k_j depth = w k depth x [(D k net (u _j_k - β _k ) / a_k -D k lidar (u _j_k )];

[0060] wherein, r _k_j depth represents the depth consistency residual of the jth projection point in the kth frame, w k depth represents the depth modality confidence, D k net (u _j_k ) represents the depth value of the network-predicted depth map at pixel u _j_k , D k lidar (u _j_k ) represents the lidar geometric depth, a _k represents the local scale parameter, b _k represents the local offset parameter.

[0061] Herein, the subscript k in the formula represents the key frame index, and j represents the feature point index in the frame. D k net is the original prediction value output by the depth neural network, and usually the value is dimensionless or only has relative significance. a _k and b _k as state variables allow each frame of image to have independent scale and offset correction, which helps to deal with the scale drift problem commonly seen in monocular vision.

[0062] For example, in some computing scenarios, it is assumed that the network-predicted depth value at the projection position of the 5th feature point in the 10th frame of image is 10.0, and the corresponding lidar geometric depth is 9.5. If the local scale parameter of the current iteration step is 1.0 and the local offset parameter is 0.2, then the corrected radar depth D lidar = 1.0 x 9.5 + 0.2 = 9.7.

[0063] At this time, the absolute error of the two is 0.3. If the depth modality confidence of the current frame is 0.8, then the final depth consistency residual r depth = 0.8 x 0.3 = 0.24. This residual value will enter the nonlinear optimizer to drive the system to adjust the pose or further adjust the values of the local scale parameter, the local scale parameter, and the depth modality confidence, which can be used to seek the global minimum error.

[0064] Step S202, introducing a semantic modality confidence as the modality confidence;

[0065] projecting the lidar point cloud to the image plane to determine the corresponding probability vector of the projection point in the image semantic probability distribution, and obtaining the point cloud semantic label corresponding to the projection point;

[0066] a statistical divergence between the corresponding probability vector and the point cloud semantic label is calculated to obtain a semantic consistency residual;

[0067] The semantic consistency residual is configured to be dynamically weighted with the semantic modality confidence.

[0068] In other words, semantic consistency belongs to one of the cross-modal consistency factors.

[0069] In this step, the semantic information is further utilized to enhance the constraint. The semantic modality confidence is used to represent the reliability of the semantic segmentation result in the current scene. For example, in the night or in the rain and fog, the image semantic segmentation may have a large number of errors, at which time the semantic modality confidence should be automatically reduced.

[0070] In one possible scenario, for each laser radar point projected onto the image, the system obtains its point cloud semantic label, for example, the label value 3 represents a vehicle. At the same time, the system queries the image semantic probability distribution at the projected pixel position to obtain a probability vector. Assuming that the total number of categories is 5, the vector can be [0.1, 0.1, 0.7, 0.05, 0.05], wherein the third element 0.7 corresponds to the probability of the vehicle class. The statistical divergence is used to measure the degree of agreement between the determined point cloud semantic label and the probability vector. Cross-entropy or KL divergence is usually used to calculate. If they are consistent, the divergence value is small; if the point cloud considers it to be a vehicle while the image considers it to be a building, the divergence value will be large. The residual is also weighted by the semantic modality confidence, which realizes adaptive processing of semantic conflicts in optimization.

[0071] The semantic consistency residual can be calculated by the following formula:

[0072] r _k_j sem =w k sem ×[-∑ _c=1 C p _k_j lidar (c)×log(p _k_j img (c))];

[0073] wherein r _k_j sem represents the semantic consistency residual of the jth projected point in the kth frame, w k sem represents the semantic modality confidence, C represents the total number of semantic categories, p _k_j lidar (c) represents the indication value of the cth category determined by the point cloud semantic label, p _k_j img(c) represents the probability value of the c-th category in the image semantic probability distribution. The cross-entropy form is adopted in the formula. p _k_j lidar (c) is usually a One-hot vector, that is, only at the class index corresponding to the point cloud label, it is 1, and the rest is 0. Continuing with the above example, if the point cloud label is a vehicle (index 3), then p lidar is 1 at c = 3, and the rest is 0. The value of the image probability vector at c = 3 is 0.7. Then the non-zero term in the summation is only 1 x log (0.7), which is about -0.357. After taking the negative sign, it is 0.357. If the current semantic modality credibility is 0.5, then the final semantic consistency residual is 0.5 x 0.357 = 0.1785. By minimizing the residual, the optimizer is forcing the semantic prediction distribution of the image to point to the category indicated by the point cloud as much as possible, or lowering the semantic modality credibility to ignore the constraint when alignment is not possible. The semantic consistency is used to assist the geometric positioning, especially in scenes where the geometric texture is missing but the semantic features are obvious.

[0074] Further, the factor graph is also configured with a modality credibility regularization term for constraining the state variable to be optimized; the modality credibility regularization term is constructed as a logarithmic penalty function for the modality credibility, preventing the modality credibility from converging to zero in the joint nonlinear optimization process.

[0075] Correspondingly, a regularization mechanism is introduced to prevent the optimizer from setting all weights to 0 in order to pursue zero residual. The specific form of the logarithmic penalty function can be r reg = λ _w -log (w); where λ _w is a preset regularization reference constant for controlling the starting threshold of the penalty, and w is the modality credibility regularization term weight. When w tends to 0, -log (w) tends to positive infinity, causing the total cost to rise.

[0076] Therefore, the optimizer is forced to find a balance point between reducing the residual (reducing the weight) and keeping the weight (increasing the weight). Only when the geometric or semantic error is indeed very large, it is more cost-effective to add a large regularization penalty than to keep the error, the weight will be pressed down. The above ensures that the system only reduces the weight when the modality is truly ineffective or conflicting, and maintains a high credibility of each modality under normal circumstances.

[0077] Optionally, in order to prevent the weight from collapsing, the modality credibility regularization term formula can also be:

[0078] r k reg = λ _w -log (w k depth -log (w ksem );

[0079] wherein r k reg is the modal confidence regularization residual value of the kth frame; λ _w is a preset regularization reference constant for controlling the starting threshold of the penalty; log is a natural logarithm operation; w k depth is the depth modal confidence of the kth frame; w k sem is the semantic modal confidence of the kth frame.

[0080] On this basis, joint nonlinear optimization is used to minimize the total cost function, which can be expressed as:

[0081] Cost = ω _1 ×∑‖r _IMU ‖ 2 +ω _2 ×∑‖r _Visual ‖ 2 +ω _3 ×∑‖r _LiDAR ‖ 2 +ω _4 ×(∑‖r depth ‖ 2 +∑‖r sem ‖ 2 +∑‖r reg ‖ 2 );

[0082] wherein r _IMU , r _Visual , r _LiDAR respectively represent inertial measurement unit pre-integration residual, visual re-projection residual and laser radar odometry residual, r depth , r sem , r reg respectively represent depth consistency residual, semantic consistency residual and modal confidence regularization term, ω _1 , ω _2 , ω _3 , ω _4 are weight coefficients of each residual term, and ‖.‖ represents L2 norm.

[0083] Specifically, the IMU constraint, visual geometric constraint, laser radar geometric constraint in the traditional SLAM and the cross-modal consistency constraint (CMCL) proposed by the present application are unified in one framework. r _IMU High-frequency inertial data is processed by using pre-integration theory to provide short-time high-precision relative pose constraint. r _Visual Re-projection error is calculated based on optical flow tracking or feature matching. r _LiDARRegistration error is calculated based on point-to-plane distance or point-to-line distance. (∑‖r depth ‖ 2 +∑‖r sem ‖ 2 +∑‖r reg ‖ 2 ) are CMCL contributions of the present application, respectively constraining depth consistency, semantic consistency and weight rationality. ω _1 , ω _2 , ω _3 , ω _4 are hyperparameters for balancing the magnitude of different factor groups.

[0084] On this basis, by solving the least squares problem through the Levenberg-Marquardt algorithm, the optimal key frame pose sequence and the optimal modal confidence and alignment parameters a, b corresponding to each frame can be obtained at the same time. It should be understood that the tightly coupled optimization mode realizes the fusion of full modal information.

[0085] The above, the embodiment adopts the cross-modal consistency factor layer (CMCL) technology, solves the problem that the positioning in dynamic environment fails due to the rigidity of the fusion mechanism. By explicitly introducing modal confidence and local alignment parameters as state variables to be optimized in factor graph optimization instead of fixed hyperparameters, the system has mathematical self-diagnosis capability.

[0086] When the visual or radar data is abnormal (such as residual increase), the optimizer will automatically reduce the confidence weight of the corresponding modal to minimize the total cost. This mechanism realizes the active suppression of sensor noise and failure, and guarantees the continuity and accuracy of positioning in extreme scenes such as strong light, rain and fog.

[0087] In some other embodiments, a specific implementation method of navigation decision based on target reachable probability grid is provided. The problem that traditional navigation algorithms only rely on obstacle cost map and ignore positioning uncertainty, resulting in that the robot blindly plans high-risk path when the positioning diverges, can be solved. In the present embodiment, the method can be carried out by the following steps:

[0088] Step S301, updating the target reachable probability grid based on the optimal pose and pose covariance, comprising:

[0089] Dividing the workspace of the robot into a grid set, maintaining a target reachable probability for each grid node in the grid set, the target reachable probability representing the success rate of collision-free reaching the navigation target from the grid node;

[0090] Initializing the target reachable probability of the grid node where the navigation target is located as 1.

[0091] In other words, the probability represents the success rate of reaching the navigation goal without collision from the grid node, combined with the current optimal pose and pose covariance;

[0092] In this step, the basic grid map environment needs to be constructed. The grid set is usually a two-dimensional array or a three-dimensional voxel space, and its resolution can be set according to the size of the robot, for example, set to 0.1m x 0.1m. In addition to maintaining the traditional obstacle occupancy probability, each grid node also needs to maintain the target reachability probability in floating point type. The update of the obstacle occupancy probability usually adopts the Bayesian log-odds update method.

[0093] Specifically, for each point scanned by the laser radar, the state of the grid where the point is located and the grid through which the beam passes is updated. If the beam hits a certain grid, the log-odds of the grid is increased by a preset occupancy factor; if the beam passes through a certain grid, the log-odds of the grid is reduced by a preset idle factor. The log-odds can be mapped back to the obstacle occupancy probability between 0 and 1 through the Sigmoid function.

[0094] On this basis, the target reachability probability does not represent whether the current grid has an obstacle, but represents the possibility that the robot can finally safely reach the target if it is in the current grid. When initialized, the system sets the target reachability probability value of the grid node corresponding to the end position to 1, indicating that if the robot is already at the target point, the success rate of reaching the target is 100%. The target reachability probability values of the remaining grids can be initialized to 0 or a heuristic initial value based on the Euclidean distance.

[0095] According to one aspect of the present application, the formula for updating the grid occupancy probability based on Bayesian log-odds is:

[0096] L(g _t )=L(g _t-1 )+log(p(z|occ) / (1-p(z|occ)));

[0097] p _occ (g)=1 / (1+exp(-L(g _t )));

[0098] Where L(g _t ) is the occupancy log-odds of grid g at time t, i.e. Log-odds; L(g _t-1 ) is the log-odds of the grid at the previous time; p(z|occ) is the sensor observation model, i.e. the probability of observing the current reading z under the condition that the grid is occupied; p _occ (g) is the obstacle occupancy probability of grid g mapped back through the Sigmoid function (value 0~1).

[0099] Step S302, based on the dynamic programming strategy, the target reachable probability of the current grid node is recursively updated using the local transfer success rate of the neighbor nodes until the probability values of all grid nodes converge;

[0100] In other words, based on the dynamic programming strategy, the target reachable probability of the current grid node is recursively updated using the local transfer success rate of the neighbor nodes in combination with the current optimal pose and pose covariance until the probability values of all grid nodes converge;

[0101] The target reachable probability of the current grid node is recursively updated using the local transfer success rate of the neighbor nodes, and the update formula is:

[0102] p _B (g)=max g＇∈N(g) (p _step (g→g')×p _B (g'));

[0103] Wherein, p _B (g) represents the target reachable probability of the current grid g, N(g) represents the neighbor node set of the current grid g, g' represents the neighbor node, p _B (g') represents the target reachable probability of the neighbor node, and p _step (g→g') represents the single-step transfer success rate from the current grid g to the neighbor node g', which is determined based on the obstacle occupancy probability of the grid.

[0104] Wherein, the dynamic programming strategy adopts a method similar to wavefront propagation or value iteration. The system starts from the target point and reversely propagates the success rate to the neighbor nodes in all directions. The neighbor node set is usually selected as eight-neighbor or sixteen-neighbor, which can cover more movement directions. Among them, the single-step transfer success rate depends not only on whether the target grid is free, but also on the distance of movement.

[0105] On the one hand, the calculation formula of the single-step transfer success rate p _step can be defined as:

[0106] p _step =(1-p _occ_g＇ )×f _d ;

[0107] In the formula, the distance attenuation function f _d can be in exponential form, such as exp(-λ×d), d is the distance, and λ is a preset distance attenuation coefficient for punishing the risk of long-distance movement, and p _occ_g＇ corresponds to the obstacle occupancy probability of the neighbor grid g'. That is, even if the target grid is free, the reliability of single-step movement will decrease if the distance is too far.

[0108] In another aspect, a 3x3 grid numerical case is provided. Assume the target point is located at coordinate (2, 2) with a target reachability probability p _B (2, 2) is 1.0. Consider its left neighbor node (2, 1). Assume the single-step transition success probability p _step from (2, 1) to (2, 2) is 0.9, based on this path, the p _B value of node (2, 1) will be preliminarily updated to 0.9 x 1.0 = 0.9.

[0109] If in the next iteration, the system traverses to another neighbor node (1, 1) of node (2, 1), and the p _B of this neighbor node (1, 1) has been updated to 0.95 through another better path, while it is known that the p _step from (2, 1) to (1, 1) is 0.98. At this time, the path probability via node (1, 1) is calculated as 0.98 x 0.95 = 0.931. Since 0.931 > 0.9, according to the maximization principle, the system will update the p _B of node (2, 1) to the maximum value 0.931. Since 0.931 is greater than 0.9, the system will select the maximum value, keeping 0.931 as the new probability of node (2, 1). The maximization operation makes each cell store the success probability under the currently known optimal strategy.

[0110] In yet another aspect, the single-step transition success probability formula can also be described as:

[0111] p _step (g→g')=(1-p _occ (g'))×exp(-λ _d ×dist(g,g'));

[0112] where p _step (g→g') is the single-step success probability of moving from the current grid g to the neighbor grid g'; p _occ (g') is the obstacle occupancy probability of the neighbor grid g'; λ _d is a preset distance attenuation coefficient for punishing the risk of long-distance movement; and dist(g, g') is the Euclidean distance between grid g and neighbor grid g'.

[0113] The step S303 of updating the target reachability probability grid based on the optimal pose and the pose covariance further comprises introducing a pose uncertainty penalty mechanism; the pose uncertainty penalty mechanism corrects the target reachability probability through the following formula:

[0114] p'B(g)=p _B (g)×exp(-γ×trace(Σ(T _k)))

[0115] where p' B(g) denotes the modified target reachability probability, γ denotes the uncertainty penalty coefficient, Σ(T _k ) denotes the pose covariance matrix corresponding to the optimal pose, and trace(.) denotes the trace operation of a matrix.

[0116] The pose covariance matrix is a square matrix that describes the distribution of the localization error, and its dimension is usually 6x6 or 3x3. The trace operation of a matrix refers to the sum of the elements on the main diagonal of the matrix, and this value reflects the overall uncertainty of the system. When the robot is in an environment rich in features and the sensor is working properly, the trace of the pose covariance matrix is small, and the modification term is close to 1, i.e., exp(-γ×trace(Σ(T _k )))≈1.

[0117] At this time, the modified target reachability probability is approximately equal to the original target reachability probability, and the planning of the robot is mainly affected by environmental obstacles. However, when the robot enters a long corridor or the sensor fails, the covariance output by the localization algorithm will quickly diverge, causing the trace to become large. At this time, the modification term will become a number less than 1, such as 0.5. That is, the success probability of all grids will be globally depressed.

[0118] In another alternative embodiment, the penalty coefficient γ can be a function of the distance of the grid, i.e., the farther the grid is from the robot, the greater the influence of the localization uncertainty. This mechanism will cause the target reachability probability value of the far distance to drop sharply, forcing the navigation algorithm to abandon long-distance planning and focus on the nearby safe area with higher probability, or trigger active shutdown to wait for the localization to recover.

[0119] It should be understood that this step is used to realize the capacitive coupling.

[0120] Step S304, planning the navigation trajectory, i.e., selecting the trajectory whose end point falls in the grid area with the maximum target reachability probability from the preset candidate trajectory library as the optimal navigation trajectory.

[0121] In this step, the preset candidate trajectory library is composed of a set of primitive trajectories that meet the kinematic constraints of the robot. For example, for a differential drive robot, circular arc trajectories with different linear and angular velocity combinations can be generated; for an Ackerman steering robot, curves that meet the maximum turning angle limit are generated. Each candidate trajectory extends a certain distance in space and terminates at a certain grid position. The system iterates through each trajectory in the trajectory library and queries the modified target reachability probability corresponding to its end grid. The system selects the trajectory with the maximum end probability value as the optimal trajectory and issues it to the underlying controller.

[0122] Optionally, in addition to maximizing the success probability, auxiliary cost terms such as path length or smoothness can be introduced to construct a comprehensive evaluation function by weighted summation. But the decision logic is always dominated by the probability grid to ensure safety first.

[0123] In yet some embodiments, a specific implementation process of the front-end perception model is provided. The specific construction, training and inference details of the preset perception model are described. In particular, how to provide feature input for the back-end optimization. Correspondingly, the present embodiment can be implemented in the following way:

[0124] Step S401, the preset perception model includes a depth estimation network, the depth estimation network is trained based on a hybrid loss function, and the hybrid loss function is represented as:

[0125] L(y * , y) = λ _1 ×L _grad (y * , y) + λ _2 ×L _SSIM (y * , y) + λ _3 ×L _normal (y * , y);

[0126] Wherein, y * represents a predicted depth value, y represents an actual depth value, L _grad represents a multi-scale gradient loss, L _SSIM represents a structural similarity loss, L _normal represents a surface normal loss, λ _1 , λ _2 , λ _3 are weights of each loss term.

[0127] In other words, the hybrid loss function formula for training the depth estimation network can also be described as:

[0128] L _total = λ _1 ×L _grad + λ _2 ×L _SSIM + λ _3 ×L _normal ;

[0129] Wherein, L _total is the total training loss value of the depth estimation network; L _grad is a multi-scale gradient matching loss for constraining the depth map edge; L _SSIM is a structural similarity loss for constraining local texture structure; L _normal is a surface normal loss for constraining plane geometric details, λ _1, λ _2 , λ _3 These are the weight coefficients for multi-scale gradient matching loss, structural similarity loss, and normal loss, respectively.

[0130] The depth estimation network can employ an encoder-decoder architecture, for example, using ResNet as the encoder to extract features and a multi-layer deconvolutional network as the decoder to recover the depth map. Hybrid loss functions can be used to address the problem that single-pixel-level errors are insufficient to recover geometric structures.

[0131] Specifically, the multi-scale gradient loss is achieved by calculating the gradient difference between the predicted depth map and the true depth map at multiple resolution scales. Gradient calculation typically utilizes the Sobel operator to perform convolutions in the horizontal and vertical directions. This loss term forces the network to focus on edge regions with abrupt depth changes, making the predicted object contours sharper. The structural similarity loss compares the brightness, contrast, and structural information of image patches, exhibiting good robustness to illumination changes and preventing local blurring of the depth map. The surface normal loss derives the 3D point cloud from the depth map and then calculates the surface normal vector using local plane fitting or vector cross product. By minimizing the angle error between the predicted and true normal vectors, this loss term can improve the smoothness of flat areas such as the ground and walls. These loss terms are balanced by a weight coefficient λ, for example, λ can be set... _1 λ is 1.0 _2 λ is 0.5 _3 A value of 0.2 is used to achieve a better trade-off between edge precision, overall structure, and surface detail.

[0132] It should be understood that this step defines the training objective of the deep estimation network.

[0133] Step S402: The pre-built perception model also includes a feature extraction network and a semantic segmentation network; the feature extraction network uses the SuperPoint network to extract image feature points and the SuperGlue network to perform feature matching; the semantic segmentation network uses the SAM (Segment Anything Model) network to generate the image semantic probability distribution. This is used to clarify the selection of sub-networks.

[0134] Among them, the SuperPoint network is a self-supervised learning feature point detection and description network. Compared with traditional ORB or SIFT algorithms, SuperPoint directly outputs heatmaps of feature points and fixed-length descriptor vectors, maintaining a high repetition rate even in regions with repetitive textures or drastic lighting changes. The SuperGlue network is a feature matcher based on graph neural networks, which uses an attention mechanism to aggregate the position and appearance information of feature points, eliminating incorrect matching pairs and providing data associations for visual factors.

[0135] Further, the SAM network is a Transformer-based large model segmentation architecture. In the present embodiment, the SAM network can be configured as an automatic full-image segmentation mode, or accept a prompt from a laser radar projection point for targeted segmentation. The semantic probability distribution output by the SAM has high edge fitting degree, and can accurately distinguish pedestrians, vehicles and backgrounds, providing semantic constraints for the CMCL layer.

[0136] As an example, if the computing resources are limited, a lightweight BiSeNet or SegFormer network can also be used instead of SAM, in exchange for higher inference speed.

[0137] In yet another embodiment, an optional implementation method for constructing and optimizing the modal confidence regularization mechanism is described. In particular, the mathematical construction mechanism of the modal confidence regularization term, the dynamic behavior in the gradient descent process, and the selection strategy of the hyperparameters are used to solve the technical problem that the adaptive weight is prone to numerical collapse (i.e., the weight tends to 0, causing the system to lose sight) during the optimization process. An example, the method includes:

[0138] Step S501, the factor graph is also configured with a modal confidence regularization term for constraining the state variables to be optimized; the modal confidence regularization term is constructed as a logarithmic penalty function for the modal confidence, preventing the modal confidence from converging to zero during the joint nonlinear optimization process.

[0139] In the present step, in the nonlinear least squares framework, the optimization goal is to find a set of state variables that minimizes the weighted residual sum of squares. For a certain residual in CMCL, the local cost function (J _local ) is usually in the form of J _local =w×r 2 ; where w corresponds to the weight coefficient, which is a coefficient to be optimized / preset, used to adjust the contribution of the residual in the cost calculation, and r describes the physical quantity residual of the deviation between the predicted value and the true value (or the observed value and the estimated value).

[0140] If no constraint is imposed on w, the optimizer can directly push w to 0 to minimize J _local , so that J _local becomes 0, but this will cause the sensor data to be ignored. To counteract the degenerative trend, a regularization term r reg =λ _w -log(w) is introduced. At this time, the total local cost function becomes J _total =w×r 2 +(λ _w -log(w)) 2 .

[0141] Further, a log-barrier can also be used. When w approaches 1, log(w) approaches 0, and the regularized cost is small, the system encourages high credibility; when w tries to approach 0, -log(w) tends to positive infinity, producing a large repulsive force to prevent w from further decreasing. This mechanism mathematically constitutes a soft constraint, ensuring that w is always in the reasonable interval of (0, 1], without the need for hard boundary truncation.

[0142] Step S502, in the joint nonlinear optimization process, the adaptive adjustment of the weight is realized by calculating the partial derivative of the total cost function with respect to the modal credibility; the adjustment process follows a dynamic balance mechanism of error driving and prior maintenance.

[0143] On this basis, the optimizer (such as Levenberg-Marquardt) will calculate the objective function J _total Regarding the first-order derivative (gradient) of w, ignoring constant terms and high-order terms, the simplified form of the gradient is: dJ / dw≈r 2 -1 / w. Setting the derivative to 0 to find the extreme point, the optimal weight w _opt ≈1 / r 2 .

[0144] In other words, the optimal value of the modal credibility is inversely proportional to the square of the geometric / semantic residual of the modal. That is, the larger the residual (i.e., the more inconsistent the sensor measurement and the model prediction, such as occlusion or failure), the smaller the optimal weight automatically calculated by the system; on the contrary, the smaller the residual, the larger the weight. The regularization reference constant plays a role of prior trust in this process, determining the extent to which the system tends to believe the sensor. By adjusting the regularization reference constant, the sensitivity of weight reduction can be controlled to prevent excessive weight jitter caused by measurement noise.

[0145] Step S503, different regularization reference constants are configured for different modal types, reflecting the reliability differences of different sensors in physical characteristics.

[0146] In this step, considering the different physical characteristics of lidar and vision camera, a unified regularization reference constant cannot be used. For the lidar depth modal, since its ranging principle is the active time-of-flight method (ToF), it is less affected by environmental light and has high intrinsic reliability, so a larger regularization reference constant (for example, in actual deployment, it can be set to 1.0) is configured, so that the corresponding weight coefficient has strong rigidity and is not easily reduced by noise interference.

[0147] For the visual semantic modality, due to its dependence on deep learning network reasoning, it is greatly affected by the distribution of training data and lighting conditions, has high uncertainty, and therefore a smaller regularization benchmark constant (e.g., 0.5) is configured to give it greater flexibility and allow it to quickly reduce the weight when a conflict is detected. The above differentiated regularization configuration strategy further improves the intelligent level of the system in handling multi-modal conflicts.

[0148] In yet other embodiments, exemplary schemes for a spatial data structure of a target reachability probability grid and dynamic updating are provided. The spatial data structure implementation of the target reachability probability grid (GRPG) in a large-scale scene, the time decay mechanism for dynamic obstacles, and the boundary processing of the probability grid solve the problems of probability likelihood field materialization and computational complexity.

[0149] In an example, the embodiment can perform the following steps to achieve:

[0150] Step S601, divide the workspace of the robot into a set of grids, and use a rolling window or spatial hash data structure to maintain the target reachability probability grid in a local range.

[0151] In this step, considering that it is unrealistic to maintain a global fine-grained grid in large-scale outdoor navigation, the embodiment uses a rolling window mechanism centered on the robot. For example, a local activation area of 50m x 50m is defined, and only in this area is the memory allocated and the target reachability probability calculated.

[0152] When the robot moves, the window moves with it, and old grids that move out of the boundary are released or cached to the disk, and new grids that enter the boundary are initialized. For sparse environments, a spatial hash table (Spatial Hash Map) can also be used, and only grid nodes containing obstacles or high-value information are created. The data structure of each grid node not only contains the current target reachability probability value, but also contains the last update timestamp, which is used to handle the probability decay in dynamic environments. This design limits the computational load to a local range around the robot, ensuring the real-time performance of the GRPG algorithm on embedded platforms.

[0153] Step S602, when recursively updating using the local transfer success rate of neighbor nodes, introduce a time dimension forgetting factor to handle the influence of dynamic obstacles on the target reachability probability.

[0154] In this step, the obstacles in the environment (such as pedestrians and vehicles) are moving, so the obstacle occupancy probability of the grid should not be permanently fixed. When calculating the one-step transfer success rate, the system checks the difference between the last update timestamp t _last of the grid and the current time t _now . A time decay formula is introduced, i.e.:

[0155] p _occ_curr =p _occ_stored ×exp(-β _time ×(t _now -t _last ));

[0156] Above, p _occ_curr corresponds to the current time grid of the obstacle occupancy probability, which is the real-time probability value after time decay correction, used for the calculation of the current single-step transition success rate, p _occ_stored is the historical obstacle occupancy probability stored in the grid, which is the probability value recorded at the last update (not permanently fixed), β _time is the time decay coefficient (hyperparameter), used to adjust the decay rate of the obstacle occupancy probability, the larger the value, the faster the decay, the more sensitive to dynamic obstacles, (t _now -t _last ) corresponds to the time difference between the current time and the last update time of the grid, which is the variable of time decay, the larger the difference, the lower the credibility of the historical occupancy probability, the more obvious the decay.

[0157] Further, if the grid has not been observed by the sensor for a long time (for example, the obstacle may have been removed), its occupancy probability will naturally decay over time, and the single-step transition success rate will gradually recover. Correspondingly, the target reachable probability will also dynamically recover. This mechanism enables the robot to re-explore the path that has been marked as blocked but may actually be open, embodying the dynamic adaptability of embodied intelligence.

[0158] Step S603, when initializing the target reachable probability of the grid node where the navigation target is located, a heuristic potential field based on the Euclidean distance is introduced as a far-field prior to guide the rapid propagation of the probability wave; used to accelerate the convergence speed of GRPG, i.e. approximate iterative update.

[0159] Correspondingly, in addition to setting the target reachable probability of the target point to 1, for the nodes on the rolling window boundary, it is no longer initialized to 0. The system uses the Euclidean distance D between the target point and the boundary node to precompute the heuristic probability p _heuristic =1 / (1+Ъ×D), Ъ is the distance decay coefficient (hyperparameter), used to adjust the decay rate of the heuristic probability with the Euclidean distance, the larger the value, the stronger the weakening effect of the distance on the probability. The heuristic probability is injected into GRPG as a boundary condition.

[0160] In view of this, at the beginning of the iteration of dynamic programming, there is already a probability potential gradient in the grid that points to the target, so that the value of the target reachable probability can be propagated faster from the target point to the current position of the robot. The hybrid initialization strategy combining global geometric heuristic and local obstacle information reduces the number of iterations and ensures that the decision result can be output in time in a high-frequency navigation cycle of 20 Hz or more.

[0161] Correspondingly, the application adopts a target reachable probability grid (GRPG) technology to solve the problem of high navigation risk caused by the disconnection of sensing and control. The scheme discards the traditional shortest path logic and instead pursues the maximum success probability. The trace of the pose covariance matrix output by the front-end optimization (representing the positioning uncertainty) is taken as a penalty term, and the success probability of the grid is exponentially attenuated, forcing the navigation algorithm to automatically avoid high-risk areas (such as feature-poor areas) that may lead to loss of positioning although the path is short. This sensing and control depth coupling strategy makes the robot's behavior show self-awareness of its own capabilities, improving the overall success rate of the navigation task.

[0162] In still other embodiments, a multi-modal embodied navigation system is provided, comprising:

[0163] a perception module configured to obtain current image data and lidar point cloud of the robot, and generate multi-modal perception features based on a preset perception model;

[0164] an optimization module configured to construct a factor graph, and configure a cross-modal consistency factor in the factor graph, the cross-modal consistency factor taking the modality credibility and the local alignment parameter as state variables to be optimized; and perform joint nonlinear optimization on the robot pose and the state variables to be optimized in the factor graph based on the multi-modal perception features and the lidar point cloud, to obtain an optimal pose and a pose covariance;

[0165] a navigation module configured to update a target reachable probability grid based on the optimal pose and the pose covariance, and plan a navigation trajectory based on the target reachable probability grid to control movement of the robot.

[0166] The system is integrated into a mobile robot platform, such as a quadruped robot dog. The system mainly includes a perception module, a computing unit, and an execution mechanism at the hardware level. The perception module includes an Intel RealSense D435i depth camera installed on the head, a Velodyne VLP-16 lidar installed on the back, and an IMU built-in. The above sensors are connected to the computing unit through a USB or Ethernet interface. The computing unit uses an embedded AI computing platform, such as a NVIDIA Jetson AGX Orin or a Sunway RK3588. The computing unit has a computer readable storage medium stored therein, and the computer readable storage medium stores a computer program for executing the above-mentioned embodiments.

[0167] In other words, a computer-readable storage medium is provided, which stores a computer program, and the computer program is executed by a processor to implement the method of each embodiment.

[0168] In actual operation, the perception module collects environmental data in real time. The optimization module runs on the CPU big core of the computing unit, and uses multi-threading technology to build and solve the factor graph. For example, a nonlinear optimization solver can be implemented using the GTSAM or g2o library. For the newly introduced modal confidence state variable, the optimization module will calculate the Jacobian matrix and update the weight value in each iteration.

[0169] When the robot dog enters an indoor dim corridor from an outdoor bright environment, the number and quality of visual feature points decrease, and the optimization module will automatically reduce the visual modal confidence from 0.9 to 0.1 while maintaining the confidence of the laser radar, realizing seamless switching of positioning sources. The navigation module uses GPU to accelerate the update calculation of the grid probability, and according to the algorithm of GRPG, it generates a safe path that avoids pedestrians and is far away from high uncertainty areas in real time, and sends speed instructions to the motion controller of the robot dog through the CAN bus.

[0170] In the above, the embodiment realizes the embodied intelligent navigation system with environmental adaptability and uncertainty perception ability through the cooperation of software and hardware.

[0171] According to one aspect of the present application, a cross-modal data association is provided, which mainly establishes a corresponding relationship between the observation data of visual and radar data, mainly including visual-LiDAR association and description-based matching. For visual-LiDAR association, the LiDAR point cloud can be projected onto the image through the external parameter calibration of the camera and LiDAR, and fused and verified with the depth map output by the depth estimation network, to improve the reliability of the depth information.

[0172] On this basis, the 2D feature points extracted on the image are associated with the 3D LiDAR feature points projected onto the image to provide visual-laser joint observation items for tight coupling optimization. For description-based matching, the descriptors extracted by the visual and LiDAR branches are used for cross-modal matching, especially in scenes with dramatic appearance changes, to provide additional association constraints.

[0173] According to another aspect of the present application, an exemplary scheme of tight coupling factor graph optimization is provided, specifically, all information is unified in a probabilistic graph model (factor graph) to optimize key frame poses within a sliding window. The factor graph optimization in the present application includes an IMU pre-integration factor, a visual re-projection factor, a LiDAR odometry factor, and a cross-modal consistency factor. The IMU pre-integration factor is used to connect adjacent key frame poses and provide high-frequency motion priors. The visual re-projection factor includes a traditional part and a learning enhanced part. The traditional part is based on the geometric error of the associated 3D map points (from LiDAR or triangulation) projected onto the 2D image, and the learning enhanced part will introduce photometric error and semantic re-projection error. The depth estimated by deep learning can be used as a priori, and the re-projection error forms a joint loss together.

[0174] Further, the LiDAR odometry factor calculates the point-to-edge / point-to-plane distance error of the matching points between the current frame LiDAR feature points and the local map, and simultaneously uses learning enhancement to use semantic information to match only the features of static objects (such as buildings and ground) and ignore dynamic objects (such as vehicles and pedestrians), thereby improving robustness in dynamic environments. The cross-modal consistency factor includes depth consistency and semantic consistency. The depth consistency requires that the depth estimation value (from deep learning) of a certain pixel in the image and the depth value of the LiDAR point projected near the pixel are statistically consistent. The semantic consistency requires that the object contour segmented from the image and the semantic label of the LiDAR point cloud projected onto the image remain consistent. In summary, the weighted sum of all factors is minimized:

[0175] Cost=ω _1 ×∑‖r _IMU ‖ 2 +ω _2 ×∑‖r _Visual ‖ 2 +ω _3 ×∑‖r _LiDAR ‖ 2 +ω _4 ×(∑‖r _Cross-Modal ‖ 2 );

[0176] In the formula, r _Cross-Modal corresponds to the cross-modal consistency residual.

[0177] By solving this nonlinear least squares problem (using algorithms such as Levenberg-Marquardt), the optimal and consistent key frame pose can be obtained.

[0178] On this basis, a body exploration obstacle avoidance method based on probability likelihood field is provided, which combines robot concept and body intelligent decision. Perception uncertainty of the robot to the environment, geometric properties of the obstacle and exploration target are uniformly mapped to a two-dimensional space called probability likelihood field. Decision (where to move) of the robot is no longer based on original sensor data, but based on solving optimal problem in this field, realizing safe and target-oriented movement.

[0179] The above, the application can unify perception, planning and control through a probability model, and provides a theoretical and practical framework for realizing autonomous and intelligent behavior of the robot in an uncertain environment.

[0180] The above describes preferred embodiments of the application in detail, but the application is not limited to specific details in the above-described embodiments, and various equivalent transformations can be made to the technical solutions of the application within the technical concept range of the application, and these equivalent transformations all belong to the protection range of the application.

Claims

1. A multimodal embodied navigation method, characterized in that, The method comprises the following steps: acquiring current image data and laser radar point cloud of the robot, generating multi-modal perception features based on a preset perception model, wherein the multi-modal perception features comprise a network predicted depth map, an image semantic probability distribution and a point cloud semantic label; constructing a factor graph, and configuring a cross-modal consistency factor in the factor graph, wherein the cross-modal consistency factor takes a modality credibility and a local alignment parameter as state variables to be optimized; performing joint nonlinear optimization on the robot pose and the state variables to be optimized in the factor graph based on the multi-modal perception features and the laser radar point cloud, to obtain an optimal pose and a pose covariance; updating a target reachable probability grid based on the optimal pose and the pose covariance, and planning a navigation trajectory based on the target reachable probability grid to control the movement of the robot; wherein the multi-modal perception features comprise the network predicted depth map; the cross-modal consistency factor in the factor graph, which takes the modality credibility and the local alignment parameter as the state variables to be optimized, comprises: introducing a local scale parameter and a local offset parameter as the local alignment parameter, and introducing a depth modality credibility as the modality credibility; projecting the laser radar point cloud to an image plane where the network predicted depth map is located to obtain a laser radar geometric depth at a projection point; constructing a linear mapping model between the network predicted depth map and the laser radar geometric depth based on the local alignment parameter, and calculating a depth consistency residual error based on the linear mapping model; wherein the depth consistency residual error is configured to be dynamically weighted with the depth modality credibility, and the depth consistency belongs to one of the cross-modal consistency factors; wherein the target reachable probability grid is updated based on the optimal pose and the pose covariance, which comprises: dividing a working space of the robot into a grid set, maintaining a target reachable probability for each grid node in the grid set, and the target reachable probability represents a success rate of collision-free reaching a navigation target from the grid node; initializing the target reachable probability of a grid node where the navigation target is located as 1; based on a dynamic programming strategy, combining the current optimal pose and the pose covariance, and using a local transition success rate of a neighbor node to recursively update the target reachable probability of a current grid node until the probability values of all grid nodes converge; planning the navigation trajectory, that is, selecting a trajectory whose end point falls in a grid area with the maximum target reachable probability from a preset candidate trajectory library as an optimal navigation trajectory.

2. The method of claim 1, wherein, The depth consistency residual error is calculated by the following formula: r _k_j depth =w k depth ×[(D k net (u _j_k )-β _k ) / α _k -D k lidar (u _j_k )]; where r _k_j depth denotes the depth consistency error of the jth projected point in the kth frame, w k depth denotes the depth modality confidence, D k net (u _j_k ) denotes the depth value of the network-predicted depth map at pixel u _j_k k lidar (u _j_k ) denotes the lidar geometric depth, a _k denotes the local scale parameter, b _k denotes the local offset parameter.​ 3. The method of claim 1, wherein, The multi-modal perception features further comprise the image semantic probability distribution and the point cloud semantic label; The cross-modal consistency factor in the factor graph, which takes the modality credibility and the local alignment parameter as the state variables to be optimized, further comprises: introducing a semantic modality credibility as the modality credibility; projecting the laser radar point cloud to the image plane to determine a corresponding probability vector of the projection point in the image semantic probability distribution, and obtaining a point cloud semantic label corresponding to the projection point; calculating a statistical divergence between the corresponding probability vector and the point cloud semantic label to obtain a semantic consistency residual error; wherein the semantic consistency residual error is configured to be dynamically weighted with the semantic modality credibility, and the semantic consistency belongs to one of the cross-modal consistency factors.

4. The method of claim 3, wherein, The semantic consistency residual error is calculated by the following formula: r _k_j sem =w k sem ×[-∑ _c=1 C p _k_j lidar (c)×log(p _k_j img (c))]; wherein r _k_j sem represents the semantic consistency residual of the jth projection point in the kth frame, w k sem represents the semantic modal confidence, C represents the total number of semantic categories, p _k_j lidar (c) represents the indication value of the cth category determined by the point cloud semantic label, p _k_j img (c) represents the probability value of the cth category in the image semantic probability distribution.

5. The method of claim 1, wherein, The factor graph is also configured with a modality confidence regularization term for constraining the state variables to be optimized; the modality confidence regularization term is constructed as a logarithmic penalty function for the modality confidence, preventing the modality confidence from converging to zero in the joint nonlinear optimization process.

6. The method of claim 5, wherein, The joint nonlinear optimization aims to minimize a total cost function expressed as: Cost = ω _1 x∑‖r _IMU ‖ 2 +ω _2 x∑‖r _Visual ‖ 2 +ω _3 x∑‖r _LiDAR ‖ 2 +ω _4 x(∑‖r depth ‖ 2 +∑‖r sem ‖ 2 +∑‖r reg ‖ 2 ); where r _IMU , r _Visual , r _LiDAR respectively represent the inertial measurement unit pre-integration residual error, the visual re-projection residual error and the lidar odometry residual error, r depth , r sem , r reg respectively represent the depth consistency residual error, the semantic consistency residual error and the modality confidence regularization term, ω _1 , ω _2 , ω _3 , ω _4 are the weight coefficients of each residual term, and ||.|| represents the L2 norm.

7. A multi-modal embodied navigation system for implementing the method of any one of claims 1 to 6, characterized in that, The method comprises: an awareness module configured to acquire current image data and lidar point cloud of the robot, and generate multi-modal awareness features based on a preset awareness model; an optimization module configured to construct a factor graph, configure a cross-modal consistency factor in the factor graph, and take the modality confidence and the local alignment parameter as state variables to be optimized; perform joint nonlinear optimization on the robot pose and the state variables to be optimized in the factor graph based on the multi-modal awareness features and the lidar point cloud, and obtain an optimal pose and a pose covariance; a navigation module configured to update a target reachable probability grid based on the optimal pose and the pose covariance, and plan a navigation trajectory based on the target reachable probability grid to control movement of the robot.

8. A computer-readable storage medium storing a computer program, the storage medium storing the computer program, wherein the computer program comprises instructions that, when executed by a computer, cause the computer to perform the method according to any one of claims 1 to 7. The computer program, when executed by a processor, implements the method of any one of claims 1 to 6.

Images