Active positioning method and device for mobile robot in indoor dynamic scene

Abstract

The invention provides a mobile robot active positioning method and device for an indoor dynamic scene, and the method comprises the steps: collecting indoor environment data, and constructing an indoor global map which comprises the geometric structure and semantic information of an environment; sensing data collected by the mobile robot in real time are matched with an indoor global map, the pose of the mobile robot in the three-dimensional space is estimated in real time, and the uncertainty index of pose estimation is calculated; evaluating information gains of different potential observation points based on uncertainty indexes of pose estimation, and selecting the observation point with the maximum information gain as a candidate target point; planning a path to the candidate target point; the mobile robot collects sensing data at the candidate target points, the sensing data are matched with the indoor global map again, and the pose of the mobile robot is updated; the indoor environment change is monitored in real time, the indoor global map is dynamically updated, and the indoor global map is regularly corrected and optimized.

Description

Technical Field

[0001] The present application relates to the technical field of active positioning of mobile robots, and in particular to a method and device for active positioning of mobile robots in indoor dynamic scenes. Background Art

[0002] With the development of smart health care, smart home, warehousing and logistics, medical assistance and other fields, indoor mobile robots are increasingly used in various industries. In order to achieve efficient and robust autonomous navigation in complex and changing indoor environments, accurate and long-life active positioning is one of the key technologies and an important prerequisite for indoor mobile robots to complete all complex tasks. Indoor environments are usually highly dynamic and diverse, such as frequent changes in furniture placement, interference from human activities, the appearance of temporary obstacles, changes in lighting conditions, etc., which brings many challenges to the precise positioning of mobile robots.

[0003] Traditional positioning methods are mostly based on passive sensing and rely on static environmental features, making it difficult to quickly restore accurate posture when the environment changes or positioning fails. In addition, factors such as narrow passages, multiple dynamic targets, small obstacles, and lighting changes in indoor environments further increase the difficulty of accurate positioning.

[0004] Therefore, the present application provides a method and device for active positioning of a mobile robot in indoor dynamic scenes. Summary of the invention

[0005] The embodiments of the present application provide a method and device for active positioning of a mobile robot in an indoor dynamic scene, so as to enable the mobile robot to be accurately and actively positioned in an indoor dynamic scene.

[0006] In a first aspect, an embodiment of the present application provides a method for active positioning of a mobile robot in an indoor dynamic scene, comprising:

[0007] Collecting indoor environment data and constructing an indoor global map, wherein the indoor global map includes geometric structure and semantic information of the environment;

[0008] Matching the sensor data collected by the mobile robot in real time with the indoor global map, estimating the position and posture of the mobile robot in three-dimensional space in real time, and calculating the uncertainty index of the position and posture estimation;

[0009] Evaluate the information gain of different potential observation points based on the uncertainty index of the pose estimation, and select the observation point with the largest information gain as the candidate target point;

[0010] Planning a path to reach the candidate target point;

[0011] The mobile robot collects sensor data at the candidate target point, and re-matches the sensor data with the indoor global map to update the position and posture of the mobile robot;

[0012] Monitor indoor environmental changes in real time, dynamically update the indoor global map, and regularly calibrate and optimize the indoor global map.

[0013] In a feasible implementation, matching the sensor data collected by the mobile robot in real time with the indoor global map includes:

[0014] High-confidence feature points are extracted from the sensor data collected by the mobile robot in real time, and aligned with the feature points in the indoor global map through a feature matching algorithm.

[0015] In a feasible implementation, matching the sensor data collected by the mobile robot in real time with the indoor global map to estimate the position and posture of the mobile robot in three-dimensional space in real time includes:

[0016] Each set of sensor observations, loop closure detection or odometer information is considered as a constraint on the posture, that is, a corresponding factor is formed in the indoor global map, and then a factor graph structure is formed, in which the node represents the robot posture and the factor represents the constraint relationship;

[0017] For each factor in the factor graph structure, a corresponding residual function is defined according to feature matching and sensor observation;

[0018] The errors of all factors are weighted and summed to form the overall objective function;

[0019] The nonlinear optimization method is used to iteratively solve the minimization problem. Each iteration calculates the residual and Jacobian matrix, continuously updates the pose vector until convergence, and obtains a high-precision pose estimation result.

[0020] The residual function includes:

[0021] The robot's position at different times or key frames is recorded as (x1, x2, ..., x n );

[0022] The mobile robot moves from position x i Movement to position x j The prediction transformation is z ij , the relative transformation actually observed by the sensor is Defining the error vector where x i With x j Expressed as a pose on the Lie group, is the measured relative pose transformation.

[0023] In one feasible implementation, the errors of all factors are weighted and summed to form the overall objective function:

[0024]

[0025] Where Ω ij is the inverse of the information matrix or covariance matrix, C is the set of all pose pairs with constraints;

[0026] Minimize the overall objective function to obtain a set of pose estimates x1,...,x with the smallest overall error n ;

[0027] Each iteration continuously updates the pose vector x by calculating the residual and Jacobian matrix until convergence, where

[0028] x (k+1) =x (k) +Δx,

[0029] Where Δx is approximately calculated from the gradient or Hessian matrix of the current iteration step according to the iterative algorithm rules.

[0030] In one possible implementation, the uncertainty index of the pose estimation is calculated, including:

[0031] In the process of solving the pose by graph optimization, the uncertainty of the pose is estimated by calculating the approximate Hessian matrix or the inverse of the incremental equation;

[0032] Use covariance matrix or information entropy to represent the uncertainty of pose;

[0033] After the graph optimization converges, its approximate Hessian matrix can be expressed as

[0034]

[0035] Among them J ij is the error function e ij Jacobian matrix of pose variables;

[0036] If the inverse of H is recorded as P = H -1 , then P can be viewed as the approximate covariance matrix of pose estimation, and its diagonal block P i Or the corresponding sub-block can be regarded as the covariance of the i-th pose, thus reflecting the pose estimation accuracy;

[0037] and / or;

[0038] For a Gaussian distribution N(μ,P), its information entropy can be expressed as:

[0039]

[0040] Where d is the state dimension. When det(P) is larger, the information entropy is higher, which means the uncertainty is also higher.

[0041] In a feasible implementation, based on the uncertainty index of the current posture, the information gain of different potential observation points is evaluated using the information entropy reduction and mutual information, including:

[0042] The current mobile robot's estimated distribution of the environment's pose is p(x), and its uncertainty is represented by entropy H(x);

[0043] If the robot plans to reach a candidate observation point o, it will obtain new observation data z o , fusion of the observed data z o After that, the distribution of pose estimation becomes p(x|z o ), the corresponding entropy becomes H(x|z o ), the information gain is defined as:

[0044] IG(o)=H(x)-H(x|z o ).

[0045] In a feasible implementation, planning a path to reach the candidate target point includes:

[0046] The path planning method is used to combine the geometric structure and obstacle information in the indoor global map to plan the optimal path from the current posture to the target observation point;

[0047] Discretize the indoor global map into a grid map or build a graph structure based on topological nodes; for the grid map, each traversable grid is a node, and edges are established between each adjacent grid; for the topological map, the key turning points, entrances, and channel intersections of the walkable area are used as nodes to build a graph structure;

[0048] Execute the A* algorithm and maintain f(n)=g(n)+h(n) for each grid node n, where g(n) represents the actual cost from the starting position to n, and h(n) is the heuristic function estimated to the target observation point;

[0049] The A* algorithm expands the node with the smallest cost and updates the g values ​​of its neighboring nodes until it reaches the goal.

[0050] In a feasible implementation, planning a path to reach the candidate target point includes:

[0051] Perform collision detection and feasibility verification on the planned path;

[0052] In a grid map or a geometric map, the mobile robot is simplified into a circular or polygonal model, and collision detection is performed on each node or discrete segment in turn on the planned path. If it is detected that the mobile robot overlaps with an obstacle at some point or the distance is less than a safety threshold, the path is determined to be infeasible;

[0053] Incorporating the future position of the dynamic obstacle into the safety zone judgment, and replanning or locally optimizing the path if the dynamic obstacle is predicted to conflict with the planned path of the mobile robot;

[0054] In narrow channel scenarios, check whether the minimum distance between the mobile robot and both sides of the channel meets a certain safety margin.

[0055] In a feasible implementation, the gradient descent method is applied to refine the initial planned path according to the real-time environmental changes to improve the smoothness and adaptability of the path, including:

[0056] The discrete path nodes obtained by A* or Dijkstra are denoted as {p0,p1,...,p N};

[0057] Use the path parameter {p i}Description, in order to make the path smooth and away from obstacles, define the objective function:

[0058]

[0059] Among them: the first term is the path length constraint; the second term is the obstacle avoidance constraint, d(p i ,O) is point p i Some penalty function of the distance to the obstacle O, if p i The value increases when approaching obstacles; the third term is the smoothness constraint, which ensures the smoothness of the path through the second-order difference term; α and β are weight coefficients used to balance the influence of each constraint;

[0060] {p i} is set to the initial value after discrete solution or interpolation of A* / Dijkstra;

[0061] For the objective function E({p i About p i Find the gradient:

[0062] Calculate the partial derivatives of the path length term, obstacle avoidance term, and smoothness term respectively to obtain the specific gradient expression;

[0063]

[0064] Where η is the learning rate (or step size), and the iteration is repeated until convergence or the upper limit of the number of iterations is reached. After the iteration is completed, a smooth, obstacle-avoiding path {p i}.

[0065] In a second aspect, an embodiment of the present application provides an active positioning device for a mobile robot in an indoor dynamic scene, applying the active positioning method for a mobile robot in an indoor dynamic scene as described in the first aspect, including:

[0066] Controller;

[0067] A map construction module, electrically connected to the controller, the map construction module is used to collect indoor environment data and construct an indoor global map, the indoor global map includes the geometric structure and semantic information of the environment;

[0068] The map building module monitors the indoor environment changes in real time and dynamically updates the indoor global map;

[0069] A posture evaluation module, electrically connected to the controller, for matching sensor data collected by the mobile robot in real time with the indoor global map, estimating the posture of the mobile robot in three-dimensional space in real time, and calculating an uncertainty index of the posture estimation;

[0070] An information gain evaluation module, electrically connected to the controller, wherein the information gain evaluation module evaluates the information gain of different potential observation points based on the uncertainty index of the pose estimation, and selects the observation point with the largest information gain as the candidate target point;

[0071] A path planning module, electrically connected to the controller, and configured to plan a path to reach the candidate target point;

[0072] An adjustment module is electrically connected to the controller, and the adjustment module re-matches the sensor data at the candidate target point collected by the mobile robot with the indoor global map and updates the position and posture of the mobile robot.

[0073] In the first aspect, the embodiment of the present application provides an active positioning method for a mobile robot in an indoor dynamic scene, including: collecting indoor environmental data and constructing an indoor global map, wherein the indoor global map includes the geometric structure and semantic information of the environment. By acquiring and constructing a high-precision indoor global map, rich environmental information is provided, and the accuracy and reliability of the mobile robot's pose estimation are improved. The sensor data collected by the mobile robot in real time is matched with the indoor global map, the pose of the mobile robot in three-dimensional space is estimated in real time, and the uncertainty index of the pose estimation is calculated, which provides a decision basis for the movement of the mobile robot; the information gain of different potential observation points is evaluated based on the uncertainty index of the pose estimation, and the observation point with the largest information gain is selected as the candidate target point. It can be understood that by evaluating the information gain of different observation points and selecting the target point that can best reduce the pose uncertainty, the positioning process of the mobile robot can be optimized. Real-time monitoring of indoor environmental changes, dynamic updating of the indoor global map, improving the adaptability and robustness of the system in a dynamic environment, and ensuring the continuous accuracy of the robot positioning. Planning the path to the candidate target point; the mobile robot collects the sensor data at the candidate target point, and re-matches it with the indoor global map, and updates the pose of the mobile robot. The mobile robot optimizes the path based on real-time environmental changes to ensure the feasibility and safety of the path.

[0074] In the second aspect, an embodiment of the present application provides an active positioning device for a mobile robot in indoor dynamic scenes, applying the active positioning method for a mobile robot in indoor dynamic scenes as described in the first aspect, including a controller, a map construction module, a posture evaluation module, an information gain evaluation module, a path planning module and an adjustment module. Among them, the map construction module is electrically connected to the controller, and the map construction module is used to collect indoor environment data and construct an indoor global map, and the indoor global map includes the geometric structure and semantic information of the environment; and the map construction module monitors the changes in the indoor environment in real time and dynamically updates the indoor global map; the posture evaluation module is electrically connected to the controller, and the posture evaluation module is used to match the sensor data collected by the mobile robot in real time with the indoor global map, estimate the posture of the mobile robot in three-dimensional space in real time, and calculate the uncertainty index of the posture estimation; the information gain evaluation module is electrically connected to the controller, and the information gain evaluation module evaluates the information gain of different potential observation points based on the uncertainty index of the posture estimation, and selects the observation point with the largest information gain as the candidate target point; the path planning module is electrically connected to the controller, and the path planning module is used to plan the path to reach the candidate target point; the adjustment module is electrically connected to the controller, and the adjustment module re-matches the sensor data at the candidate target point collected by the mobile robot with the indoor global map and updates the posture of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] The drawings described herein are used to provide further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present application and do not constitute improper limitations on the present invention.

[0076] In the attached picture:

[0077] Figure 1 is a schematic diagram of a method for autonomous positioning of a mobile robot in an indoor dynamic scene provided by an embodiment of the present application;

[0078] Figure 2 is a schematic diagram of an indoor global map provided by an embodiment of the present application;

[0079] Figure 3 It is a schematic diagram of an autonomous positioning device for a mobile robot in an indoor dynamic scene provided by an embodiment of the present application.

[0080] Description of reference numerals:

[0081] 100-controller; 200-map building module; 300-pose evaluation module; 400-information gain evaluation module; 500-path planning module; 600-adjustment module. DETAILED DESCRIPTION

[0082] In order to enable those skilled in the art to better understand the technical solutions in the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present application.

[0083] In the description of the embodiments of the present application, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present application, "plurality" means at least two, for example, two, three, etc., unless otherwise clearly and specifically defined.

[0084] In this application, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0085] In the present application, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being "above", "above" or "above" a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" or "below" a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0086] With the development of smart health care, smart home, warehousing and logistics, medical assistance and other fields, indoor mobile robots are increasingly used in various industries. In order to achieve efficient and robust autonomous navigation in complex and changing indoor environments, accurate and long-life active positioning is one of the key technologies and an important prerequisite for indoor mobile robots to complete all complex tasks. Indoor environments are usually highly dynamic and diverse, such as frequent changes in furniture placement, interference from human activities, the appearance of temporary obstacles, changes in lighting conditions, etc., which brings many challenges to the precise positioning of mobile robots.

[0087] Traditional positioning methods are mostly based on passive sensing and rely on static environmental features, making it difficult to quickly restore accurate posture when the environment changes or positioning fails. In addition, factors such as narrow passages, multiple dynamic targets, small obstacles, and lighting changes in indoor environments further increase the difficulty of accurate positioning.

[0088] In order to improve the positioning accuracy of a mobile robot in an indoor dynamic scene, the present application provides a method and device for active positioning of a mobile robot in an indoor dynamic scene. The solution provided by the embodiment of the present application will be described in detail below in conjunction with the accompanying drawings of the specification.

[0089] Figure 1 is a schematic diagram of a method for autonomous positioning of a mobile robot in an indoor dynamic scene provided by an embodiment of the present application; Figure 2 It is a schematic diagram of an indoor global map provided by an embodiment of the present application.

[0090] Reference Figure 1 and Figure 2 As shown, in the first aspect, the embodiment of the present application provides a method for active positioning of a mobile robot in an indoor dynamic scene, comprising:

[0091] S100: Collect indoor environment data and construct an indoor global map, where the indoor global map includes geometric structure and semantic information of the environment.

[0092] The mobile robot is equipped with a variety of sensors, including RGB-D cameras, LiDAR, depth cameras, etc., to collect indoor environmental data, including the location, shape, size and color characteristics of objects. Then, a graph-optimized simultaneous localization and mapping (SLAM) algorithm is used to build an indoor global map with object geometry and semantic information based on the indoor environmental data. Exemplarily, the high-precision mapping algorithm can be ORB-SLAM3, LOAM or Cartographer.

[0093] In addition, during the mapping process, deep learning models can be used to semantically identify and classify objects in the environment, integrating semantic information to enhance the expressive power of the global map.

[0094] S200: Matching the sensor data collected by the mobile robot in real time with the indoor global map, estimating the position and posture of the mobile robot in three-dimensional space in real time, and calculating the uncertainty index of the position and posture estimation.

[0095] For example, high-confidence feature points are extracted from the sensor data collected in real time, and aligned with the feature points in the indoor global map through feature matching algorithms (such as SIFT and ORB). Based on the feature matching results, a graph optimization algorithm is applied to estimate the robot's posture with high precision, and the uncertainty index of the posture estimation is calculated, such as using the posture covariance matrix or information entropy to represent the uncertainty index of the posture estimation.

[0096] In this application, based on the multimodal feature matching results of the indoor dynamic environment, the mobile robot pose estimation problem can be modeled as a minimization problem based on graph optimization. The specific steps are as follows:

[0097] The specific steps are as follows:

[0098] 1. Build a factor graph model:

[0099] The robot's position at different times or key frames is recorded as (x1, x2, ..., x n ). Each set of sensor observations, loop closure detection or odometer information can be regarded as a constraint on the posture, that is, a corresponding factor is formed in the internal global map, and then a factor graph structure is formed, where the nodes represent the robot posture and the factors represent the constraint relationship.

[0100] 2. Define the error function:

[0101] For each factor in the factor graph structure, the corresponding residual function is defined based on feature matching and sensor observation. i Movement to position x j The prediction transformation is z ij , and the relative transformation obtained according to the actual observation of the sensor is The error vector can be defined where x i With x j It can be expressed as a pose on a Lie group (such as SE(3)), is the measured relative pose transformation. This error can usually be mapped to Lie algebra (such as se(3)) for operation.

[0102] 3. Optimize the overall objective function:

[0103] The weighted sum of the errors of all factors forms the overall objective function:

[0104]

[0105] Where Ω ij is the information matrix (or the inverse of the covariance matrix), and C is the set of all pose pairs with constraints. By minimizing this objective function, a set of pose estimates x1,...,x that minimize the overall error can be obtained. n .

[0106] 4. Numerical solution

[0107] The above minimization problem is solved iteratively using nonlinear optimization methods (such as Gauss-Newton method, Levenberg-Marquardt or Dogleg method). Each iteration continuously updates the pose vector x by calculating the residual and Jacobian matrix until convergence: x (k+1) =x (k) +Δx,

[0108] Among them, Δx is obtained by approximating the gradient or Hessian matrix of the current iteration step according to the iterative algorithm rules. Finally, after convergence, a high-precision pose estimation result is obtained.

[0109] In some examples, during the process of solving the pose through graph optimization, the uncertainty of the pose is estimated by calculating the approximate Hessian matrix or the inverse of the incremental equation. For example, the uncertainty of the pose can be expressed in the form of a covariance matrix or information entropy.

[0110] When the covariance matrix is ​​used to represent the uncertainty of the pose;

[0111] After the graph optimization converges, its approximate Hessian matrix can be expressed as

[0112]

[0113] Among them J ij is the error function e ij The Jacobian matrix of the pose variables. If the inverse of H is recorded as P = H -1 , then P can be regarded as the approximate covariance matrix of pose estimation. Its diagonal block P i (or the corresponding sub-block) can be regarded as the covariance of the i-th pose, thus reflecting the accuracy of pose estimation.

[0114] When information entropy is used to represent the uncertainty of posture;

[0115] For a Gaussian distribution N(μ,P), its information entropy can be expressed as:

[0116]

[0117] Where d is the state dimension (in three-dimensional pose estimation, if translation + rotation is considered as 6 dimensions, then d = 6; if only the pose in the plane is considered, then d = 3. The larger det(P) is, the higher the information entropy is, which means that the uncertainty is also higher.

[0118] In summary, graph optimization can not only obtain high-precision pose estimation values, but also obtain the covariance matrix or information entropy of each pose node during the solution process, thereby quantifying the uncertainty of pose estimation and providing a basis for subsequent information gain evaluation.

[0119] For example, the confidence of the current pose estimation can be quantified by statistical methods or information entropy calculation, which serves as a basis for subsequent information gain evaluation.

[0120] Among them, the method of quantifying the confidence of the current pose estimation based on statistical methods is as follows:

[0121] Based on the covariance matrix P, we can use methods such as confidence ellipsoid analysis to measure uncertainty. If x~N(μ,P), then (x-μ) · P -1 (x-μ) follows a chi-square distribution. Therefore, a confidence threshold can be defined, such as To represent the acceptable area under the confidence level α. When the area is too large, it means that the uncertainty is high.

[0122] The statistical method to quantify the confidence of the current pose estimate is as follows:

[0123] As shown above, if the pose estimate is approximately Gaussian, then

[0124]

[0125] To measure uncertainty. Relative changes can also be calculated, such as comparing the current posture entropy H(x) with the posture entropy H(x) at historical moments. prev ) to determine the degree of increase or decrease in uncertainty.

[0126] S300: Evaluate the information gain of different potential observation points based on the uncertainty index of the pose estimation, and select the observation point with the largest information gain as the candidate target point.

[0127] Specifically, based on the uncertainty index of the current posture, the information gain of different potential observation points is evaluated using the information entropy reduction and mutual information.

[0128] Assume that the current robot's estimated distribution of the environment's posture is p(x), and its uncertainty can be expressed by entropy H(x). If the robot plans to reach a candidate observation point o, it can obtain new observation data z o After fusing this observation, the distribution of pose estimation becomes p(x|z o ), the corresponding entropy becomes H(x|z o ). Then the information gain (IG) can be defined as:

[0129] IG(o)=H(x)-H(x|z o ).

[0130] In the case of Gaussian approximation, H(x) and H(x|z o ) can be calculated through the covariance matrix (or information matrix). If the current estimated covariance is P, in the fusion observation z o The covariance then becomes P o ,but:

[0131]

[0132] Where P o It can be obtained by Bayesian update or extended Kalman filter formula, graph optimization incremental update and other methods.

[0133] The specific steps can be broken down into:

[0134] 1. Predicted distribution: Before reaching the observation point o, the robot’s posture distribution is p(x)~N(μ,P).

[0135] 2. Observation uncertainty modeling: Assume that a new observation z is obtained at observation point o o , whose noise satisfies N(0,R), then the update equation can be constructed in combination with the system observation model.

[0136] 3. Update distribution: Incorporate new observations into the graph optimization or filtering framework to obtain the updated distribution p(x|z o )~N(μ o ,P o ).

[0137] 4. Information gain calculation: According to the above formula Or IG(o)=H(x)-H(x|z o ) is quantified. The larger the information gain, the more the observation point can reduce the uncertainty of pose estimation.

[0138] In addition, in some examples, a joint information gain evaluation index is constructed by weighted summation of information entropy and mutual information. Mutual information is the uncertainty reduced by a random variable due to the knowledge of another random variable. The mutual information is calculated as follows:

[0139] MI(x;z o )=H(x)-H(x|z o ),

[0140] Multiple potential observation points are predefined in the indoor global map, and based on the information gain calculation results, observation points that can significantly reduce the pose uncertainty are identified. The specific identification steps are as follows:

[0141] 1. Predefined observation points: On the indoor global map, select several representative locations or locations with wide field of view as candidate observation points {o1, o2, ..., o k For example, it can be set up at the corners of corridors, around major obstacles, in the center of open areas, etc.

[0142] 2. Information gain evaluation: For each observation point o i , calculate its information gain (IG(o i )).

[0143] 3. Threshold or ranking screening:

[0144] Two threshold screening or ranking screening methods are used to identify the observation points that have the most significant effect on reducing pose uncertainty.

[0145] The specific threshold screening is: set the information gain threshold γ, if IG(o i )≥γ, it is considered that the observation point can significantly reduce the uncertainty of pose estimation.

[0146] The ranking screening method is specifically based on IG(o i ) are sorted from large to small, and several observation points with high rankings are selected as candidate points.

[0147] The identified candidate observation points are sorted by information gain, and the observation points with the largest information gain are preferentially selected as the target points. Through the above steps, the set of observation points that has the most significant effect on reducing the uncertainty of the pose can be effectively identified. Specifically, the sorted set can be expressed as Where π is the index sorted from largest to smallest. As the optimal observation point, if there is environmental obstruction or other external interference during the actual execution, you can try For backup.

[0148] S400: Planning a path to the candidate target point.

[0149] Specifically, a path planning method is used to plan a preliminary path from the current position to the target observation point in combination with the geometric structure and obstacle information in the indoor global map. Exemplarily, the path planning method may be an A* algorithm or a Dijkstra algorithm.

[0150] The initial path planning steps are as follows:

[0151] Discretize the global indoor map into a grid map or build a graph structure based on topological nodes. For a grid map, each traversable grid is a node, and edges are established between each adjacent grid; for a topological map, the key turning points, entrances, and intersections of the walkable area can be used as nodes to build a graph structure.

[0152] Then execute the A* algorithm or Dijkstra algorithm to plan the path.

[0153] When executing the A* algorithm, f(n) = g(n) + h(n) is maintained for each grid node n, where g(n) represents the actual cost from the starting position to n, and h(n) is the heuristic function (such as Euclidean distance or Manhattan distance) estimated to the target observation point. The algorithm expands the node with the minimum cost and updates the g value of its neighboring nodes until it reaches the target.

[0154] If there is no obvious heuristic function or the exact shortest path is required, the Dijkstra algorithm can be used. Let g(n) represent the minimum cost from the starting point to node n, and search by giving priority to the node with the smallest global cost.

[0155] Reconstruct the optimal path: After searching for the target observation point o, the preliminary path sequence p = {n start ,...,n goal}.

[0156] In some examples, to ensure that the planned path is valid, collision detection and feasibility verification are performed on the planned path to ensure the safety of the path in narrow environments, small obstacles, and dynamic targets.

[0157] For example, collision detection can be performed on the planned path. Specifically, in a grid map or a geometric map, the mobile robot is simplified into a circular or polygonal model, and collision detection is performed on each node or discrete segment on the planned path in turn. If it is detected that a certain point overlaps with an obstacle or the distance is less than a safety threshold, the path is determined to be infeasible.

[0158] Dynamic obstacles can be predicted for the planned path. Specifically, for possible dynamic targets, their motion prediction models (such as linear speed, direction, and other indicators) can be considered in the algorithm, and the future position of dynamic obstacles can be included in the safe area judgment during planning and verification. If the prediction conflicts with the path, re-planning or local optimization is required.

[0159] The planned path can be verified for passage width. Specifically, in a narrow passage scenario, the minimum distance between the robot and both sides of the passage can be checked to see if it meets a certain safety margin δ. If δ is too small, the robot will not be able to pass through, and it will be necessary to re-plan or switch to a better observation point.

[0160] In some examples, after path planning is completed, the path can be optimized, for example, by applying a local path optimization algorithm (such as a gradient descent method) to refine the initial planned path, improve the smoothness and adaptability of the path, and obtain the final path.

[0161] Exemplarily, the preliminary path optimization steps are as follows:

[0162] The discrete path nodes obtained by A* or Dijkstra are denoted as {p0,p1,...,p N}. The path can be viewed as a series of control points, represented by interpolation (such as spline interpolation) or point-to-point straight line connections.

[0163] Suppose the path can be represented by a set of parameters {p i} Description, in order to make the path smooth and away from obstacles, the objective function can be defined:

[0164]

[0165] Among them: the first term is the path length constraint; the second term is the obstacle avoidance constraint, d(p i ,O) is point p i Some penalty function for the distance to the obstacle O (if p i The value increases when approaching an obstacle); the third term is the smoothness constraint, which ensures the smoothness of the path through the second-order difference term. α and β are weight coefficients used to balance the influence of each constraint.

[0166] Finally, {p i} is set to the discrete solution of A* / Dijkstra or the initial value after interpolation; then the objective function E({p i About p i Find the gradient:

[0167] Calculate the partial derivatives of the path length term, obstacle avoidance term, and smoothness term respectively to obtain the specific gradient expression;

[0168]

[0169] Where η is the learning rate (or step size), and the iteration is repeated until convergence or the upper limit of the number of iterations is reached. After the iteration is completed, a smooth, obstacle-avoiding path {p i}.

[0170] If a new obstacle is detected or an existing obstacle moves during the optimization process, d(p i ,O) calculation and continue to iterate to ensure the adaptability of the path in dynamic scenes. After the iteration, a smooth, obstacle-avoiding path that meets the real-time environmental constraints can be obtained. i} and sends it to the mobile robot for execution.

[0171] S500: The mobile robot collects sensor data at the candidate target point, and re-matches it with the indoor global map to update the position and posture of the mobile robot.

[0172] The mobile robot moves along the planned path, and during the movement, it continuously collects sensor data at the candidate target points, matches the newly collected sensor data with the indoor global map, updates the pose estimation, reduces uncertainty, and achieves rapid convergence of active positioning. At the same time, the mobile robot feeds back the real-time pose update results to the information gain evaluation step, dynamically adjusts the subsequent observation point selection and path planning, and continuously optimizes the pose estimation accuracy.

[0173] S600: Monitor indoor environment changes in real time, dynamically update the indoor global map, and regularly calibrate and optimize the indoor global map.

[0174] During operation, the mobile robot monitors environmental changes in real time, such as furniture movement, personnel activities and other dynamic feature changes. At the same time, based on the real-time collected data, it dynamically updates the relevant information in the global map to ensure the consistency between the global map and the actual environment. The robot maintains the accuracy and consistency of the global map through regular calibration and optimization, improves the accuracy and robustness of the robot's positioning, and ensures the continuous accuracy of the robot's positioning.

[0175] It should be understood that, although the various steps in the flowcharts involved in the above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0176] Based on the same inventive concept, the embodiment of the present application also provides an autonomous positioning device for a mobile robot in an indoor dynamic scene for implementing the autonomous positioning method for a mobile robot in an indoor dynamic scene involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in the embodiments of one or more autonomous positioning devices for a mobile robot in an indoor dynamic scene provided below can refer to the limitations of the autonomous positioning method for a mobile robot in an indoor dynamic scene above, and will not be repeated here.

[0177] Figure 3 The figure is a schematic diagram of an autonomous positioning device for a mobile robot in an indoor dynamic scene provided by an embodiment of the present application.

[0178] Reference Figure 3As shown, in the second aspect, an embodiment of the present application provides an active positioning device for a mobile robot in an indoor dynamic scene, applying the active positioning method for a mobile robot in an indoor dynamic scene as described in the first aspect, including a controller 100, a map construction module 200, a posture evaluation module 300, an information gain evaluation module 400, a path planning module 500 and an adjustment module 600. Among them, the map construction module 200 is electrically connected to the controller 100, and the map construction module 200 is used to collect indoor environment data and construct an indoor global map, and the indoor global map includes the geometric structure and semantic information of the environment; and the map construction module 200 monitors the changes in the indoor environment in real time and dynamically updates the indoor global map; the posture evaluation module 300 is electrically connected to the controller 100, and the posture evaluation module 300 is used to match the sensor data collected by the mobile robot in real time with the indoor global map, estimate the posture of the mobile robot in three-dimensional space in real time, and calculate the uncertainty index of the posture estimation; the information gain evaluation module 400 is electrically connected to the controller 100, and the information gain evaluation module 400 evaluates the information gain of different potential observation points based on the uncertainty index of the posture estimation, and selects the observation point with the largest information gain as the candidate target point; the path planning module 500 is electrically connected to the controller 100, and the path planning module 500 is used to plan the path to the candidate target point; the adjustment module 600 is electrically connected to the controller 100, and the adjustment module 600 re-matches the sensor data at the candidate target point collected by the mobile robot with the indoor global map and updates the posture of the mobile robot.

[0179] Each module in the above-mentioned mobile robot autonomous positioning device for indoor dynamic scenes can be implemented in whole or in part by software, hardware and their combination. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0180] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0181] It is easy to understand that those skilled in the art can combine, split, reorganize, etc. the embodiments of the present application to obtain other embodiments based on the several embodiments provided in the present application, and these embodiments do not exceed the protection scope of the present application.

[0182] The above specific implementation methods further explain in detail the purpose, technical solutions and beneficial effects of the embodiments of the present application. It should be understood that the above are only specific implementation methods of the embodiments of the present application and are not used to limit the protection scope of the embodiments of the present application. Any modifications, equivalent substitutions, improvements, etc. made on the basis of the technical solutions of the embodiments of the present application should be included in the protection scope of the embodiments of the present application.

Claims

1. A method for active positioning of a mobile robot in an indoor dynamic scene, characterized in that: include: Collecting indoor environment data and constructing an indoor global map, wherein the indoor global map includes geometric structure and semantic information of the environment; Matching the sensor data collected by the mobile robot in real time with the indoor global map, estimating the position and posture of the mobile robot in three-dimensional space in real time, and calculating the uncertainty index of the position and posture estimation; Evaluate the information gain of different potential observation points based on the uncertainty index of the pose estimation, and select the observation point with the largest information gain as the candidate target point; Planning a path to reach the candidate target point; The mobile robot collects sensor data at the candidate target point, and re-matches the sensor data with the indoor global map to update the position and posture of the mobile robot; Monitor indoor environmental changes in real time, dynamically update the indoor global map, and regularly calibrate and optimize the indoor global map.

2. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: The matching of the sensor data collected by the mobile robot in real time with the indoor global map includes: High-confidence feature points are extracted from the sensor data collected by the mobile robot in real time, and aligned with the feature points in the indoor global map through a feature matching algorithm.

3. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: The matching of the sensor data collected by the mobile robot in real time with the indoor global map to estimate the position and posture of the mobile robot in three-dimensional space in real time includes: Each set of sensor observations, loop closure detection or odometer information is considered as a constraint on the posture, that is, a corresponding factor is formed in the indoor global map, and then a factor graph structure is formed, in which the node represents the robot posture and the factor represents the constraint relationship; For each factor in the factor graph structure, a corresponding residual function is defined according to feature matching and sensor observation; The errors of all factors are weighted and summed to form the overall objective function; The nonlinear optimization method is used to iteratively solve the minimization problem. Each iteration calculates the residual and Jacobian matrix, continuously updates the pose vector until convergence, and obtains a high-precision pose estimation result. The residual function includes: The robot's position at different times or key frames is recorded as (x1, x2, ..., x n ); The mobile robot moves from position x i Movement to position x j The prediction transformation is z ij , the relative transformation actually observed by the sensor is Defining the error vector where x i With x j Expressed as a pose on the Lie group, is the measured relative pose transformation.

4. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 3, characterized in that: The weighted sum of the errors of all factors forms the overall objective function: Where Ω ij is the inverse of the information matrix or covariance matrix, C is the set of all pose pairs with constraints; Minimize the overall objective function to obtain a set of pose estimates x1,...,x with the smallest overall error n ; Each iteration continuously updates the pose vector x by calculating the residual and Jacobian matrix until convergence, where x (k+1) =x (k) +Δx, Where Δx is approximately calculated from the gradient or Hessian matrix of the current iteration step according to the iterative algorithm rules.

5. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: Calculate uncertainty indicators for pose estimation, including: In the process of solving the pose by graph optimization, the uncertainty of the pose is estimated by calculating the approximate Hessian matrix or the inverse of the incremental equation; Use covariance matrix or information entropy to represent the uncertainty of pose; After the graph optimization converges, its approximate Hessian matrix can be expressed as Among them J ij is the error function e ij Jacobian matrix of pose variables; If the inverse of H is recorded as P = H -1 , then P can be viewed as the approximate covariance matrix of pose estimation, and its diagonal block P i Or the corresponding sub-block can be regarded as the covariance of the i-th pose, thus reflecting the pose estimation accuracy; and / or; For a Gaussian distribution N(μ,P), its information entropy can be expressed as: Where d is the state dimension. When det(P) is larger, the information entropy is higher, which means the uncertainty is also higher.

6. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: Based on the uncertainty index of the current position, the information gain of different potential observation points is evaluated using the information entropy reduction and mutual information, including: The current mobile robot's estimated distribution of the environment's pose is p(x), and its uncertainty is represented by entropy H(x); If the robot plans to reach a candidate observation point o, it will obtain new observation data z o , fusion of the observed data z o After that, the distribution of pose estimation becomes p(x|z o ), the corresponding entropy becomes H(x|z o ), the information gain is defined as: IG(o)=H(x)-H(x|z o ).

7. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: The planning of a path to reach the candidate target point includes: The path planning method is used to combine the geometric structure and obstacle information in the indoor global map to plan the optimal path from the current posture to the target observation point; Discretize the indoor global map into a grid map or build a graph structure based on topological nodes; for the grid map, each traversable grid is a node, and edges are established between each adjacent grid; for the topological map, the key turning points, entrances, and channel intersections of the walkable area are used as nodes to build a graph structure; Execute the A* algorithm and maintain f(n)=g(n)+h(n) for each grid node n, where g(n) represents the actual cost from the starting position to n, and h(n) is the heuristic function estimated to the target observation point; The A* algorithm expands the node with the smallest cost and updates the g values ​​of its neighboring nodes until it reaches the goal.

8. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 1, characterized in that: The planning of a path to reach the candidate target point includes: Perform collision detection and feasibility verification on the planned path; In a grid map or a geometric map, the mobile robot is simplified into a circular or polygonal model, and collision detection is performed on each node or discrete segment in turn on the planned path. If it is detected that the mobile robot overlaps with an obstacle at some point or the distance is less than a safety threshold, the path is determined to be infeasible; Incorporating the future position of the dynamic obstacle into the safety zone judgment, and replanning or locally optimizing the path if the dynamic obstacle is predicted to conflict with the planned path of the mobile robot; In narrow channel scenarios, check whether the minimum distance between the mobile robot and both sides of the channel meets a certain safety margin.

9. The method for active positioning of a mobile robot in an indoor dynamic scene according to claim 8, characterized in that: According to the real-time environmental changes, the gradient descent method is used to refine the initial planned path to improve the smoothness and adaptability of the path, including: The discrete path nodes obtained by A* or Dijkstra are denoted as {p0,p1,...,p N }; Use the path parameter {p i }Description, in order to make the path smooth and away from obstacles, define the objective function: Among them: the first term is the path length constraint; the second term is the obstacle avoidance constraint, d(p i ,O) is point p i Some penalty function of the distance to the obstacle O, if p i The value increases when approaching obstacles; the third term is the smoothness constraint, which ensures the smoothness of the path through the second-order difference term; α and β are weight coefficients used to balance the influence of each constraint; {p i } is set to the initial value after discrete solution or interpolation of A* / Dijkstra; For the objective function E({p i About p i Find the gradient: Calculate the partial derivatives of the path length term, obstacle avoidance term, and smoothness term respectively to obtain the specific gradient expression; Where η is the learning rate (or step size), and the iteration is repeated until convergence or the upper limit of the number of iterations is reached. After the iteration is completed, a smooth, obstacle-avoiding path {p i }.

10. An active positioning device for a mobile robot in an indoor dynamic scene, characterized in that: The method for active positioning of a mobile robot in an indoor dynamic scene according to any one of claims 1 to 9 comprises: Controller (100); A map construction module (200) is electrically connected to the controller (100), the map construction module (200) is used to collect indoor environment data and construct an indoor global map, the indoor global map includes the geometric structure and semantic information of the environment; The map construction module (200) monitors the indoor environment changes in real time and dynamically updates the indoor global map; A posture evaluation module (300) is electrically connected to the controller (100), and is used to match sensor data collected by the mobile robot in real time with the indoor global map, estimate the posture of the mobile robot in three-dimensional space in real time, and calculate an uncertainty index of the posture estimation; An information gain evaluation module (400) is electrically connected to the controller (100), wherein the information gain evaluation module (400) evaluates the information gain of different potential observation points based on the uncertainty index of the pose estimation, and selects the observation point with the largest information gain as a candidate target point; A path planning module (500) is electrically connected to the controller (100), and the path planning module (500) is used to plan a path to reach the candidate target point; An adjustment module (600) is electrically connected to the controller (100), and the adjustment module (600) rematches the sensor data at the candidate target point collected by the mobile robot with the indoor global map and updates the position and posture of the mobile robot.

Images