Computer-implemented method for creating an environment map for operating a mobile agent

Abstract

A computer-implemented method for operating a mobile agent in an environment on the basis of a pose of the mobile agent. The location of the mobile agent is determined by way of the following steps in order to operate the mobile agent: capturing sensor data regarding walls and/or objects located in an environment, wherein the sensor data indicate the orientation and distance of the walls and/or objects in an agent-based agent coordinate system; establishing the pose of the mobile agent in the environment using a SLAM algorithm while taking account of a Manhattan orientation.

Claims

1. A computer-implemented method for operating a mobile agent in an environment based on a pose of the mobile agent, the method comprising: determining a location of the mobile agent using the following steps to operate the mobile agent: capturing sensor data regarding walls and/or objects located in the environment, wherein the sensor data indicate an orientation and distance of the walls and/or objects in an agent-based agent coordinate system; establishing the pose of the mobile agent in the environment using a SLAM algorithm while taking into account a Manhattan orientation, wherein: the SLAM algorithm provides for a minimization of an error function that is dependent on measurement errors in the sensor data and on the Manhattan orientation, the SLAM algorithm corresponds to a graph-based SLAM algorithm based on a SLAM graph having nodes and transformation edges, wherein the pose nodes each indicate a pose of the mobile agent as established from the sensor data, and wherein the transformation edges between each two pose nodes indicate a pose change, established from the sensor data, between the poses assigned to the two pose nodes, wherein a Manhattan node is added, and at least one Manhattan edge is added between the Manhattan node and at least one of the pose nodes depending on the Manhattan orientation, the SLAM algorithm uses an error function, which is determined from the SLAM graph and takes account of error probabilities assigned to the pose nodes and the Manhattan node and transformation edges and the at least one Manhattan edge, which establishes corrected poses assigned to the pose nodes and the Manhattan node of the SLAM graph, wherein the mobile agent is operated depending on the established pose and/or wherein an environment map is established using a current pose of the mobile agent and the mobile agent is operated depending on the environment map, and wherein the Manhattan orientation is determined depending on a dominant orientation of straight-line structures in adjacent local cells resulting from combining points in a point set that is determined from the sensor data and that indicates coordinates of the captured walls and/or objects.

2. The method as recited in claim 1, wherein, to determine the dominant orientation of the local cells for each cell orientation of a cell, a similarity to all the other cell orientations is established, and a similarity measure is established for each cell as a sum of similarities of the respective cell in relation to the other cells, wherein the Manhattan orientation corresponds to a cell orientation of one of the cells that is selected according to the similarity measure, the similarity measure of the selected cell being a maximum.

3. The method as recited in claim 2, wherein the similarities are determined depending on a Manhattan-normalized orientation difference between two of the cell orientations in each case, wherein the similarities are established depending on an exponential function of a negative Manhattan-normalized difference.

4. The method as recited in claim 1, wherein the Manhattan orientation is taken into account only when more than a predefined number of cell orientations of the cells are located within a Manhattan-normalized orientation difference in relation to the Manhattan orientation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Specific embodiments will be explained in more detail below on the basis of the figures.

(2) FIG. 1 is a schematic illustration of a mobile agent within an environment to be captured.

(3) FIG. 2 is a flow chart to illustrate a method for establishing a pose of the mobile agent within an environment, in accordance with an example embodiment of the present invention.

(4) FIG. 3 is a schematic illustration of a graph that is created within the framework of a SLAM algorithm in order to establish the pose of the mobile agent.

(5) FIG. 4 is a flow chart to illustrate the establishment of a Manhattan orientation, in accordance with an example embodiment of the present invention.

(6) FIG. 5A is a schematic illustration of a capturing of an environment map using a conventional SLAM algorithm; and

(7) FIG. 5B is an illustration of a captured environment map based on a SLAM algorithm while taking account of a Manhattan world concept, in accordance with an example embodiment of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

(8) FIG. 1 is a schematic illustration of a mobile agent 1 in an environment defined by walls 2 and objects 3. The environment corresponds to a typical environment, as is common for internal spaces, factory buildings, or urban areas, and has walls and/or object edges extending in parallel with one another and/or perpendicularly to one another.

(9) The mobile agent 1 has a control unit 11 for controlling the mobile agent 1. In addition, the mobile agent 1 has an environment-capturing sensor system 12 for capturing the direction and the distance from surrounding walls and objects 2, 3 in relation to an agent coordinate system A. In addition, the mobile agent 1 has a movement actuator system 13 for moving the mobile agent 1 within the environment in accordance with a function to be performed.

(10) The environment sensor system 12 can have at least one of the following sensor apparatuses: a LiDAR apparatus, a radar apparatus, a laser scanning apparatus, a camera, an inertial sensor system, an odometry sensor system, and the like. The sensor data are processed in the control unit 11, in accordance with a pre-processing depending on the sensor apparatuses used, in such a way that the orientation and the distance of a wall or an object 2, 3 in relation to the agent coordinate system A can be established and made available. To establish the environment map, the pose of the mobile agent 1 in relation to a fixed reference coordinate system K has to be established. The pose of the mobile agent 1 is then produced from the position shift between the agent coordinate system A of the mobile agent 1 and the reference coordinate system K, and from the rotation difference between the agent coordinate system and the reference coordinate system.

(11) From the pose of the mobile agent 1 in the reference coordinate system, it is then possible, on the basis of the sensor data, to determine the corrected position of the walls 2 and objects 3 in the agent coordinate system.

(12) To establish an environment map that indicates the position of the walls 2 and objects 3 in the reference coordinate system, and to establish the current pose of the mobile agent 1, a method is carried out in the control unit 11, as will be explained in more detail in conjunction with the flow chart in FIG. 2. The method can be implemented as a software and/or hardware algorithm in the controller. The method will be described below by way of example for a 2D environment.

(13) In step S1, using the environment-capturing sensor system 12, sensor data are captured in a capturing step, which sensor data indicate the location (distance and orientation in relation to the agent coordinate system) of one or more walls and objects 2, 3, from the perspective of the mobile agent 1. The sensor data indicate, point by point, the measured coordinates of a distance of walls 2 and obstacles/objects 3 in a respective orientation, such that the relative position of the relevant wall or relevant object 3 in relation to the mobile agent 1 can be determined from the angle of the orientation (in relation to the agent coordinate system) and from the distance. The sensor data can, for example, be determined using LiDAR captures and odometry captures.

(14) In step S2, a reference coordinate system is selected or updated. Reference coordinate systems are selected in accordance with parameterizable criteria. For this purpose, the reference coordinate system is updated when a movement of the mobile agent has taken place. Through the selection of the reference coordinate system, the sensor data can be assigned to a fixed position. Preferably, the reference coordinate system is updated only when a movement of the mobile agent has taken place.

(15) Initially, the reference coordinate system can be defined by the starting pose of the mobile agent 1. The reference coordinate system is re-selected when the pose change of the mobile agent 1 exceeds a threshold value (e.g., 2 m or 60) in relation to the current, latest valid reference coordinate system. The updated reference coordinate system is then positioned at the current pose of the mobile agent 1.

(16) Next, a SLAM graph is created or expanded, as shown by way of example in FIG. 3. The SLAM graph is created in order to determine the optimal pose for each pose node by way of optimization.

(17) For this purpose, in step S3, a pose node P (oval) is added to the SLAM graph once a reference coordinate system has been selected. The pose node P represents the measured pose of the mobile agent 1 in relation to the selected reference coordinate system. The measured pose is determined by the pose of the mobile agent 1 as established in the agent coordinate system and by the transformation (position of the origin of the agent coordinate system and the orientation thereof) between the agent coordinate system and the reference coordinate system. The transformation can be determined, for example, by a rotation matrix

(18) $R\left(\right)=\left(\begin{array}{cc}\cos \left(\right)& -\sin \left(\right)\\ \sin \left(\right)& \cos \left(\right)\end{array}\right)$
and a position shift

(19) $T(x,y=\left(\begin{array}{c}x\\ y\end{array}\right).$
A pose of the mobile agent 1 is defined by a vector: p.sup.3 (i.e., three values) where p=(x, y, ). In addition to the pose nodes P, a SLAM graph has transformation edges TK. The transformation edges TK (squares) are added in step S4 and each link two pose nodes i and j, which define poses p.sub.i=(x.sub.i, y.sub.i, .sub.i) and p.sub.j=(x.sub.j, y.sub.j, .sub.j), and can be indicated by a vector p.sub.ji=(x.sub.ji, y.sub.ji, .sub.ji).sup.3, where the following applies in relation to the rotation R and the translation T:
R.sub.j=R.sub.ji*R.sub.i, where R.sub.j=R(.sub.j),R.sub.ji=R(.sub.ji),R.sub.i=R(.sub.i)
T.sub.j=T.sub.i+R.sub.i*T.sub.ji, where T.sub.i=T(x.sub.j,y.sub.j),T.sub.i=T(x.sub.ii,y.sub.ii),T.sub.ji=T(x.sub.ji,y.sub.ji)
or p.sub.ip.sub.ji=p.sub.j and p.sub.ji=p.sub.jp.sub.i

(20) Transformation edges TK which can result from odometry measurements and the identification of a pose that has already been approached are added to the SLAM graph. These represent the measured relative transformation between two pose nodes P, in particular between the current pose node P and one or more previously reached pose nodes in the SLAM graph.

(21) In addition, when a previously approached position of the mobile agent 1 is recognized, loop closure edges S are inserted between two corresponding pose nodes P to which the same position and/or orientation can be assigned. The loop closure edges S can accordingly indicate a transformation that corresponds to a position change of 0 and a rotation change of 0.

(22) Loops of this kind can be recognized in a multi-step method. In a first step, nodes are identified that, due to their current estimated pose, are potential candidate nodes for a loop closure edge, e.g., when their distance is below a threshold value, e.g., 1 m. In a second step, an optimal transformation between the potential candidate nodes is established by a scan matching method. In a third step, all the previously established potential candidate nodes are checked for mutual consistency, e.g., by a spectral clustering method. A potential candidate node is inserted into the graph as a loop closure edge only when the check is successful. An example implementation of this method is described in detail, for example, in Edwin B. Olson, 2008, Robust and Efficient Robotic Mapping, PhD dissertation, Massachusetts Institute of Technology, Cambridge, MA, USA.

(23) In step S5, a check is carried out as to whether a Manhattan orientation can be detected. For this purpose, a method is carried out as explained in more detail below in conjunction with the flow chart in FIG. 4. Depending on the detection of the Manhattan orientation, Manhattan nodes MK and Manhattan edges M are added to the SLAM graph.

(24) In step S6, the SLAM graph is optimized, as described, for example, in Grisetti, G. et al., A Tutorial on Graph-Based SLAM, IEEE Intelligent Transportation Systems Magazine, 2.4 (2010), pp. 31-43. Carrying out the optimization method is necessary since the sensor data are affected by noise or measurement errors, meaning that there is no precise solution for the SLAM graph. Instead, an error function corresponding to a sum function over all the edges is minimized, in which case, for example, for each edge i, j the error function is defined as e_ij=(p.sub.jp.sub.i)(m.sub.ji), where p.sub.j,p.sub.i corresponds to the poses of the pose nodes i,j and m.sub.ji corresponds to the movement measurement of the transformation edge ij. The overall error function corresponds to
e=(e.sub.ji.sub.jie.sub.ji)

(25) where .sub.ji corresponds to the inverse of the covariance assigned to the measurements m.sub.ji.

(26) The error functions are minimized using conventional optimization algorithms, for example Gauss-Newton or Levenberg-Marquardt. The result of the optimization method are poses of the mobile agent 1 that are corrected for the pose nodes of the graph. Therefore, from the last-added pose node P, it is possible to establish the current pose of the mobile agent 1 in the reference coordinate system and, possibly, to create the environment map on the basis of the sensor data. On the basis of the environment map and the current pose of the mobile agent 1, trajectory planning can then be performed, in a conventional manner.

(27) FIG. 4 is a flow chart for illustrating the incorporation of the Manhattan world concept into the above-described method for determining the location of the mobile agent 1 in the reference coordinate system K and the position of the environment map.

(28) In step S11, a check is carried out as to whether a Manhattan orientation is present. For this purpose, the sensor data are grouped into cells as a point cloud of captured positions of walls 2 or objects 3. The cells can, for example, have a predefined size of, e.g., 1 m1 m or the like. For all the cells of this kind containing at least three captured points, the average and the covariance matrix of the points are calculated. This is carried out on the basis of a method that is conventional and described, for example, as the basis for the so-called NDT-based scan matching method in Biber, W. et al., The normal distributions transform: A new approach to laser scan matching, Proceedings 2003, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003), Vol. 3, 2003.

(29) In step S12, the cells that have an angular structure are established. For this purpose, eigenvalues of the covariance matrix are determined. Only cells in which the ratio between the largest and the smallest eigenvalue is greater than a predefined threshold value, for example 100, are retained as the selected cells.

(30) In step S13, a check is carried out as to whether the number of these selected cells is lower than a predefined threshold value. If this is the case (option: yes), the method is terminated without establishing a Manhattan orientation, and the method in FIG. 2 continues by jumping to step S5. Otherwise (option: no), the method continues with step S14.

(31) In step S14, the orientation of the cells is calculated from their respective eigenvector (Ex, Ey) according to the largest eigenvector as a=a tan 2 (Ey, Ex).

(32) In step S15, the orientation is then normalized to a mid-range of 45 to +45. This normalization is generally referred to as the Manhattan normalization and is carried out by adding or subtracting 90 angles or multiples thereof.

(33) In step S16, the Manhattan orientation is calculated as the dominant orientation from the group of calculated cell orientations of the selected cells. This can be done using various methods.

(34) For example, the similarities s to other orientations can be calculated for each cell orientation in accordance with a method. The similarities between the orientations of the selected cells are calculated in each case, for example as the angular distance d between their Manhattan-normalized orientations in accordance with s=e.sup.d/2, where can be a selected parameter. The similarity value in relation to a cell is then established by adding up all the similarities s.

(35) The cell orientation having the highest similarity value is set as the Manhattan orientation for this capturing cycle.

(36) In step S17, the calculated Manhattan orientation is considered to be valid when more than a predefined number of cell orientations are within a defined Manhattan-normalized distance from the calculated Manhattan orientation. The Manhattan orientation represents a preferred direction in the environment, which is in a fixed relationship with the reference coordinate system.

(37) In step S18, a Manhattan node, which corresponds to the calculated Manhattan orientation, is added to the SLAM graph. A Manhattan edge is then added for a predefined minimum number of captured detections of Manhattan orientations, said Manhattan edge linking the corresponding pose nodes, which corresponds to the detection, to the newly added Manhattan node.

(38) The Manhattan edge is assigned a Manhattan error function, which is defined as
e=Manhattan_normalize(.sub.pose_edge+.sub.meas.sub.Manhattan)
where .sub.pose_edge corresponds to the orientation of the pose node, .sub.meas corresponds to the detected Manhattan orientation, .sub.Manhattan corresponds to the status of the Manhattan node, and Manhattan_normalize corresponds to the function of the Manhattan normalization, which calibrates the orientation to a range between 45 and 45.

(39) Each Manhattan edge is equipped with a robust kernel having an appropriate bandwidth (e.g., 0.1). The robust kernels allow for an accurate estimation of the correct Manhattan angle in the event that some edges having erroneous measurements are included in the graph. This technique corresponds to the state of the art and is described, for example, in Agarwal, P., Tipaldi, G. D., Spinello, L., Stachniss, C., & Burgard, W. (2013, May), Robust map optimization using dynamic covariance scaling, 2013 IEEE International Conference on Robotics and Automation (pp. 62-69).

(40) The method then continues with step S6 of the method in FIG. 2.

(41) FIG. 5A is a schematic illustration of a capturing of an environment map using a conventional SLAM algorithm. By comparison, FIG. 5B is an illustration of a captured environment map based on a SLAM algorithm while taking account of a Manhattan world concept.

(42) By expanding the SLAM graph with Manhattan nodes and Manhattan edges, a further group of variables to be optimized is added to the optimization problem. Furthermore, an additional error term is generated for each Manhattan edge, which error terms have to be taken into account in the optimization problem.

(43) The above-described method makes it possible to incorporate the Manhattan world concept into a graph-based SLAM algorithm. The incorporation is carried out such that there need be no reference to earlier captured sensor data. The SLAM graph is merely expanded with information resulting from findings of the Manhattan world concept.