METHOD FOR DETERMINING A TRAJECTORY FOR A MOBILE DEVICE

Description

[0001] The present invention relates to a method for determining a trajectory along which a mobile device, e.g. a robot or an at least partially automated vehicle, is to move in an environment, a computing unit and a computer program for carrying out the method, and a mobile device. Background of the invention

[0002] Mobile devices, such as at least partially automated vehicles or robots, typically move along a trajectory or path in an environment such as a home, a garden, a factory hall, or on the street, in the air, or in water. The trajectory is planned or determined to be as short as possible if a specific destination is to be reached. Obstacles or objects in the environment should be taken into account.

[0003] Methods for trajectory planning are known, for example, from US 2021 / 402601 A1, US 2006 / 235610 A1 and TAHA ELMOKADEM: "Advanced Algorithms of Collision Free Navigation and Flocking for Autonomous UAVs", ARXIV.ORG, CORNELL UNIVERSITY LIBRARY, 201 OLIN LIBRARY CORNELL UNIVERSITY ITHACA, NY 14853, October 30, 2021 (2021-10-30), XP091084043. Disclosure of the invention

[0004] According to the invention, a method for determining a trajectory, a computing unit and a computer program for implementing the method, as well as a mobile device with the features of the independent patent claims are proposed. Advantageous embodiments are the subject of the dependent claims and the following description.

[0005] The invention relates to mobile devices that move or are intended to move along a trajectory in an environment. A trajectory comprises a movement path with positions and / or orientations of the mobile device and additionally, for example, points in time at which these positions and / or orientations are to apply; this therefore also specifies a certain speed along or on the movement path. This speed can also be variable along the movement path, e.g. lower on curves than on straight sections. Examples of such mobile devices are robots and / or drones and / or partially or (fully) automated vehicles (on land, water or in the air). Possible robots include, for example, household robots such as vacuum and / or floor cleaning robots, floor or street cleaning devices or lawnmower robots, but also other so-calledService robots, as well as at least partially automated moving vehicles, e.g. passenger transport vehicles or goods transport vehicles (also so-called industrial trucks, e.g. in warehouses, also automated forklifts), but also aircraft such as so-called drones or watercraft.

[0006] Determining motion paths, and in particular trajectories—a motion planning problem for mobile devices such as robots—is frequently investigated. However, there are still aspects that are not solved, or not completely solved, or not sufficiently well solved, if only because such problems can become almost arbitrarily complex. Current and future requirements, for example, in industry, include supporting various types of robot kinematics, considering dynamic properties and constraints, planning trajectories between humans and intelligently responding to their actions, as well as coordination with other mobile devices in the environment or workspace.

[0007] For example, model predictive control (MPC) and other approaches based on numerical optimization can be used to achieve a certain degree of flexibility. Explicit consideration of a predictive model, as in the context of MPC, provides a natural interface for adapting to different kinematics and dynamic properties. It also enables the modeling of the autonomously evolving environment (e.g., based on predictions of the movement of other vehicles, humans, and machines) and even the explicit modeling of interactions.

[0008] In principle, various MPC-based methods can be considered for navigation. These can, for example, consider stochastic systems and optimize any nonlinear, nonconvex cost function. However, due to their stochastic nature, it is difficult to achieve reliable and consistent behavior.

[0009] Against this background, it is proposed to first provide a target (in particular a global target within the environment) to which the mobile device is to move, as well as a current position and / or orientation (so-called pose) of the mobile device, and also environmental information, which in particular includes distances of the mobile device to boundaries of the environment and / or obstacles in the environment; such environmental information can, for example, include a point set (or point cloud) that indicates distances of the mobile device (or a corresponding sensor of the device) to boundaries of the environment (such as walls) and / or obstacles in the environment (such as static or mobile objects within the environment), wherein the point set has been obtained in particular by means of a laser and / or lidar, i.e., for example, a laser and / or lidar sensor.Alternatively or in addition to the target, a reference path can also be provided along which or in the direction of which the mobile device should move (at least approximately).

[0010] Based on the environmental information, a movement area within the environment is then determined which extends from the current position of the mobile device in the direction of the target and / or the reference path and in which, in particular, there are no obstacles for the mobile device. This movement area does not have to extend to the target, but can, for example, only locally cover part of the possible path to the target. Such a movement area can, for example, also be referred to as a tunnel, since it does not contain any obstacles and the device can be moved more or less directly within it. In particular, the movement area can also be dynamic or allow certain deviations, for example if there are moving obstacles in the environment. The movement area is determined based on a Voronoi diagram of at least part of the environment.The Voronoi diagram, in turn, can be determined based on a grid and / or cell representation of the environment.

[0011] A Voronoi diagram, also known as a Thiessen polygon or Dirichlet decomposition, is a decomposition of space (here, the surrounding area or a part of it) into regions defined by a given set of points in the space, also called centers. Each region is defined by exactly one center and includes all points in the space that, with respect to the Euclidean metric, are closer to the center of the region than to any other center. Such regions are also called Voronoi regions. The Voronoi diagram is created from all points that have more than one nearest center and thus form the boundaries of the regions.

[0012] Within the movement range and based on the current position and / or orientation of the mobile device, the trajectory (which also includes a movement path) along which the mobile device is to move is then determined. The trajectory extends from the current position of the mobile device in the direction of the target or (along) the reference path. Here, too, the trajectory does not have to reach the target or cover the entire reference path, in particular not to an edge or end of the movement range, but can, for example, also locally cover part of the possible path to the target. Preferably, by repeatedly applying the procedure, in particular after each movement of the mobile device, the trajectory can be gradually determined up to the target or along the reference path. The trajectory can, in particular, also take into account the movement characteristics of the mobile device, for exampleWhether it can turn on the spot or, for example, requires a certain range of motion to turn around. Furthermore, the trajectory can be determined more or less freely within the range of motion (i.e., it doesn't have to be in the center of it), although there may be restrictions, e.g., due to the reference path (e.g., a specific lane).

[0013] Based on the trajectory, motion control variables (e.g., control variables for the drive system, such as torques, steering inputs, and the like) can then be determined and provided to the mobile device. In particular, the mobile device can also be controlled based on the motion control variables. For this purpose, a control and / or regulating unit for controlling a drive system can be provided, for example.

[0014] A method is therefore proposed that determines the movement range (the tunnel) from the currently available data, particularly in a single action by searching for a path (or possibly several paths), e.g., on or in a Voronoi diagram, which thereby immediately provides the widest movement range. In particular, when using the MPC strategy, it is also possible to perform reactive and robust obstacle avoidance and simultaneously calculate a speed profile optimized according to specific criteria along the path, i.e., a trajectory.

[0015] As a starting point for the considerations of the above-mentioned method, a set of ordinary differential equations can be considered, for example: x ˙ t = f x t , u t , x 0 = x ¯ , that describe the kinematics or dynamics of a mobile device to be controlled, where x and uThe states or inputs of the system are described. It can generally be assumed that the mobile device moves or navigates in a flat or two-dimensional environment, with the pose of the mobile device being uniquely defined by its state. However, this can also be applied to a three-dimensional environment.

[0016] The goal of the mobile device is to navigate or move to the specified destination. A reference path to the destination may also be given. In this case, it is not necessary that the entire reference path can be followed exactly, for example, if it leads through an obstacle. Nevertheless, for the purposes of explanation, it can be assumed that the destination can be reached without a collision.

[0017] Such a reference path can be used, in particular, to specify a preferred route that the mobile device should take if the route is not blocked by obstacles. In principle, only a reference path can be given, without a specific destination, e.g., if the mobile device should move (if possible) in a circle.

[0018] It is therefore desirable to have an approach for mobile device navigation (robot navigation) that achieves the following goals: collision-free movement, following the same homotopy class as the reference path to improve the understandability and efficiency of robot movement, satisfying state and input constraints, and quasi-minimal movement time to the goal.

[0019] The proposed approach, as mentioned, comprises two steps. First, the movement range (tunnel) is determined or constructed, which describes the collision-free space to the target or reference path, by searching for a path in the Voronoi diagram. Then, a trajectory is determined or planned within this movement range; this is preferably done using MPC by numerically solving an optimal control problem (OCP), which can be derived from model-predictive path-following methods. In the first step, no specific timing of the movement range needs to be specified, in contrast to, for example, elastic-band methods. In the MPC step, the optimizer can then, for example, be able to independently decide when and where it is within the movement range, for example by applying a path-following-like control approach.

[0020] By calculating or determining the range of motion based on a path in the Voronoi diagram, most of the relevant free space in the environment—that is, where there are no obstacles—can be effectively captured. This gives the numerical optimization algorithm the degrees of freedom to highly optimize the trajectory.

[0021] The range of motion typically lies in a homotopy class of possible paths to the goal. This allows for efficient parameterization of free space, but generally does not describe the entire free space. Therefore, the range of motion should be chosen appropriately, and the optimal path should be carefully defined in the Voronoi diagram.

[0022] First, we will explain in more detail how to determine the range of motion. This involves projecting the current position of the mobile device onto the Voronoi diagram. Then, a path is determined from the current position of the mobile device toward the target and / or (along) the reference path in the Voronoi diagram. The path is then extended at its beginning and / or end, taking into account the dimensions of the mobile device.

[0023] Then, the range of motion is determined based on the path, in particular using B-spline parameterization.

[0024] When projecting the current position p S of the mobile device on the Voronoi diagram is a target in particular, a point p V , which is the center of a circle with radius D ( p V ) that has the following properties: A minimum distance from the current position p S to an edge of the circle should D ( p S ), and the point p V is supposed to lie within the Voronoi diagram. This means that an intersection with the Voronoi diagram is sought that follows a path in the opposite direction to the nearest obstacle. Since such an intersection is not necessarily unique, the one with the greatest distance to obstacles is preferred.

[0025] When determining the path in the Voronoi diagram, one goal is in particular to find a continuous path from the point p V to the target. This can be done, for example, by referring to the reference path mentioned above. The path in the Voronoi diagram should be sufficiently far away from obstacles so that the robot can fit between the surrounding obstacles along the entire path.

[0026] When extending the path at its beginning and / or at its end, one goal in particular is to ensure that the mobile device, with its dimensions, fits within the movement range, i.e., that the entire footprint of the mobile device fits within the movement range both for the current state or position and at the destination. This step is useful because the introduced projection and the determination of the movement range take place for a single point or reference point on the mobile device, i.e., the mobile device is considered as a point. The beginning can be achieved by searching for a path from the point p V can be constructed on the Voronoi diagram until the entire footprint lies within the range of motion, avoiding intersections with the previously obtained optimal path. This can be done accordingly for the end of the path.

[0027] When determining the range of motion based on the path, particularly using B-spline parameterization, one goal is to represent the range of motion by a centerline and the radius for each point along the centerline. To obtain this parameterization, a sequential fitting method based on the path found in the Voronoi diagram can be used. First, for example, the mean spline is calculated by solving a regression problem. Then, for example, the radius spline is calculated for another sequence by solving another optimization problem. The goal is, in particular, to make the range of motion as wide as possible while keeping it within the obstacle-free space.

[0028] Within the movement range determined, for example, in the manner mentioned above, and based on the current position and / or orientation of the mobile device, the trajectory extending from the current position of the mobile device toward the target or reference path can then be determined. As mentioned, this can preferably be done within the framework of a model-based, preferably model-predictive, control (MPC).

[0029] In model predictive control, for example, a discrete optimal control problem (OCP) is solved at each sampling time instance. For this purpose, the dynamics of the mobile device can be discretized in an appropriate manner. The goal of this optimal control problem is, in particular, the planning of trajectories, especially forward trajectories toward the target, within the motion domain, which is preferably parameterized using B-spline methods.

[0030] A sufficient condition that ensures that the mobile device's footprint is not subject to collision in any state x can be obtained, for example, by approximating the footprint with one or more circles. One idea of ​​MPC combined with the motion range is to let the nonlinear optimization perform the check while the state trajectories are optimized.

[0031] A computing unit according to the invention, e.g. a control unit of a robot, is configured, in particular in terms of programming, to carry out a method according to the invention.

[0032] Implementing a method according to the invention in the form of a computer program or computer program product with program code for performing all method steps is also advantageous, as this entails particularly low costs, especially if an executing control unit is also used for additional tasks and is therefore already present. Suitable data storage devices for providing the computer program include, in particular, magnetic, optical, and electrical storage devices, such as hard disks, flash memories, EEPROMs, DVDs, and others. Downloading a program via computer networks (Internet, intranet, etc.) is also possible.

[0033] Further advantages and embodiments of the invention will become apparent from the description and the accompanying drawings.

[0034] It is understood that the features mentioned above and those to be explained below can be used not only in the combination specified in each case, but also in other combinations or on their own, without departing from the scope of the present invention.

[0035] The invention is illustrated schematically in the drawing using an embodiment and is described in detail below with reference to the drawing.

[0036] Character description Figure 1 schematically shows a mobile device in an environment for explaining the invention. Figure 2 schematically shows a flow of a method in a preferred embodiment of the invention. Figure 3 schematically shows a flow of a method in another preferred embodiment of the invention. Detailed description of the drawing

[0037] In Figure 1A mobile device 110 is schematically depicted in an environment, based on which the invention will be explained. For example, the mobile device 110 is a robot, such as a mopping or vacuuming robot. It is understood that the mobile device can also be of a different type, as explained above. The mobile device 110 has a computing unit 112 embodied as a control unit, as well as, for example, a lidar sensor 114.

[0038] The environment is illustrated, for example, as a room with walls 102 containing several obstacles 104. Using the lidar sensor 114, points or a set of points 116 can be detected as environmental information.

[0039] The mobile device 110 should have a current position and orientation (pose), which is represented by an arrow 120. A destination to which the mobile device 110 should move is represented by an arrow 122, along with the position and orientation that the mobile device 110 should then assume there.

[0040] Furthermore, a movement area 130 with center line 132, a reference path 140 and a trajectory 124 (movement path with pose and, if applicable, times or speed specifications) are shown, which will be discussed in more detail in the following explanations.

[0041] In Figure 2This schematically shows a flow of a method in a preferred embodiment of the invention, represented in blocks that represent the interrelationships between individual aspects. In a block 200, for example, within the framework of global planning, the destination to which the mobile device is to move from its current position (with its current orientation, i.e., pose) is determined or selected. The reference path can also be specified.

[0042] In a block 210, the range of motion (tunnel) is determined. This is based, at least in part, on the current pose, the environmental information (block 240), as well as the target and / or the reference path. In a block 220, a model-predictive control is performed; based, for example, on the range of motion and the current pose, a trajectory toward the target and corresponding motion control variables, i.e., control or regulated variables (block 250), are determined for controlling a drive of the mobile device. A movement of the mobile device then, in turn, influences the environmental information or the current pose.

[0043] In Figure 3is a schematic representation of a method sequence in a further preferred embodiment of the invention, specifically as a flow chart. In a step 300, input values ​​for the algorithm on which the method is based are first determined. These are, for example, the aforementioned environmental information, such as a set of points obtained from a lidar scan. These are also, for example, the current pose of the mobile device, which in turn can be determined or estimated based on the environmental information, such as, if applicable, further, e.g., previous poses. Furthermore, the input values ​​can include the target, in particular the pose of the mobile device at the target, and the reference path.

[0044] In a step 310, for example, a distance map can then be determined or calculated. In particular, the position of the mobile device in the environment is mapped to the nearest obstacle (the nearest obstacle can be determined, for example, based on the environmental information). For example, a grid or cell representation of the environment can be used. At the same time, the Voronoi diagram can then be obtained, as explained above.

[0045] In a step 320, for example, the current position of the mobile device can be projected onto the Voronoi diagram. In a step 330, a path from the current position of the mobile device toward the destination and / or along or toward the reference path can be determined in the Voronoi diagram. In a step 340, the path can be extended at the beginning and end, or even along its entire length.

[0046] In a step 350, the movement range can then be determined, for example, by fitting a B-spline parameterization to the path by solving a least squares regression problem. The path can then be sampled, and the range map can be evaluated. Then, by solving another optimization problem, the movement range can be made as wide as possible while remaining within the obstacle-free space. The tunnel (movement range) is then defined by the path for the centerline (see Figure 1 ) and a radius for each point along the centerline.

[0047] In step 360, the trajectory is then determined using model-predictive control. The state (pose) of the mobile device, the range of motion (tunnel), and the reference trajectory can be used for this purpose. For example, a state trajectory (which ultimately forms the desired trajectory) can be optimized such that it or the mobile device moves forward toward the target, within the range of motion. The footprint of the mobile device can be approximated by circles so that the mobile device is not subject to a collision in any state; these circles must therefore remain within the range of motion. It is useful to find a balance between the user's wishes or specifications and the fulfillment of constraints.

[0048] In a step 370, motion control variables for the mobile device can then be determined, by means of which the mobile device or its drive system is controlled. These motion control variables can include, for example, setpoints for speed and / or acceleration, but also torques for the drive system.

Claims

1. Method for determining a trajectory (124), along which a mobile device (110), in particular a robot, a drone or a vehicle moving in an at least partially automated manner, is intended to move in an environment (100), comprising: providing (300) a destination (122), to which the mobile device is intended to move, and / or a reference path (140), along which the mobile device is intended to move, a current position and / or orientation (120) of the mobile device, and environmental information (116) that includes in particular distances between the mobile device and boundaries of the environment and / or obstacles (104) in the environment; determining (320, 330, 340, 350), based on the environmental information, a movement range (130) within the environment, which extends from the current position of the mobile device in the direction of the destination and / or the reference path and in which in particular there are no obstacles for the mobile device; and determining (360), within the movement range and based on the current position and / or orientation of the mobile device, the trajectory (124) that extends from the current position of the mobile device in the direction of the destination and / or the reference path, characterized in that the movement range is determined based on a Voronoi diagram of at least one part of the environment, wherein determining the movement range comprises: projecting (320) the current position of the mobile device onto the Voronoi diagram; determining (330) a path from the current position of the mobile device in the direction of the destination and / or the reference path in the Voronoi diagram; extending (340) the path in the Voronoi diagram at its beginning and / or end, taking into account dimensions of the mobile device; and determining (350) the movement range based on the path in the Voronoi diagram.

2. Method according to Claim 1, wherein the Voronoi diagram is determined based on a grid and / or cell representation of the environment.

3. Method according to Claim 1 or 2, wherein the movement range is determined based on the path in the Voronoi diagram by means of B-spline parametrization.

4. Method according to one of the preceding claims, wherein the trajectory (124) is determined as part of model-based, preferably model predictive, control.

5. Method according to one of the preceding claims, wherein the environmental information (116) includes a set of points indicating distances between the mobile device and boundaries of the environment and / or obstacles in the environment.

6. Method according to Claim 5, wherein the set of points has been obtained by means of laser and / or lidar.

7. Method according to one of the preceding claims, additionally comprising: determining (370), based on the trajectory, movement control variables for the mobile device, and providing the movement control variables and / or moving the mobile device based on the movement control variables.

8. Computing unit (112) comprising a processor configured to perform the method according to one of the preceding claims.

9. Mobile device (100) configured to receive a trajectory (124) determined in accordance with a method according to one of Claims 1 to 6, or movement control variables determined according to Claim 7, having a drive system and a control and / or regulating unit for controlling the drive system based on the trajectory and / or the movement control variables, and in particular having a computing unit (112) according to Claim 8, and further in particular having at least one sensor unit (114) for capturing environmental information (116) that includes in particular distances between the mobile device and boundaries of an environment (100) and / or obstacles in the environment, wherein the mobile device is configured to provide the environmental information (116).

10. Mobile device (100) according to Claim 9, which is designed as a vehicle moving in an at least partially automated manner, in particular as a passenger transport vehicle or as a goods transport vehicle, or as a robot, in particular as a household robot, e.g. a suction and / or wiping robot, floor or road cleaning device or robotic lawnmower, or as a drone.

11. Computer program comprising instructions which, when the program is executed by a computer, cause the latter to perform the method according to Claims 1 to 7.

12. Computer-readable data carrier on which the computer program according to Claim 11 is stored.