Control of a mobile system in an environment containing obstacles

Abstract

The invention relates to a method using predictive control employing a tube-based model in the presence of disturbances, the tube being defined by a set centred on successive nominal states. The method comprises: - obtaining an orthogonal projection of the set defining the tube, onto a space through which the environment extends; - obtaining a partition of the environment; - determining a path (CH) comprising segments (S2-4, S5-8) each determined on the basis of one or more segments of the partition; - defining corridors (Q2-4, Q5-8) around respective segments (S2-4, S5-8) of the path (CH); - verifying that the projection remains entirely in each corridor (Q2-4, Q5-8); - modifying the partition to remove any segment or segments where the projection departs from the corridor, to restart determining the path (CH); and - if the projection remains entirely in all the corridors (Q2-4, Q5-8), controlling the mobile system to successively pass through the corridors (Q2-4, Q5-8).

Description

Description TITLE: CONTROL OF A MOBILE SYSTEM IN AN ENVIRONMENT COMPRISING OBSTACLES Technical field of the invention

[0001] The present invention relates to the field of navigation of mobile systems in cluttered environments.

[0002] The invention relates more specifically to a method for controlling a mobile system in an environment comprising obstacles, an associated computer program and an associated control system.

[0003] It finds a privileged application for the planning of navigation of autonomous vehicles in unstructured environments. Technological background

[0004] D. Ioan's doctoral thesis entitled "Safe Navigation Strategies within Cluttered Environment" from the University of Paris-Saclay, published on June 30, 2021 and referenced hereinafter [Ioan 2021], describes a method for controlling a mobile system in an environment, by means of a predictive control (from the English "Model based Predictive Control" or "Model Predictive Control"), this method comprising: - the environment comprising obstacles, obtaining a partition of the environment, comprising so-called waypoints, and segments avoiding the obstacles and having ends interconnected at the waypoints; - determining a path comprising successive segments avoiding the obstacles, each segment of the path being determined from one or more segments of the partition; and - defining corridors respectively around the segments of the path, each corridor being convex and avoiding the obstacles.

[0005] More precisely, in the method of [Ioan 2021], the partition is obtained by convex lifting. The partition comprises linear segments interconnected at their ends. A path between a starting point and an arrival point of the mobile system is determined, this path comprising a succession of segments of the partition. More precisely, Dijkstra's algorithm is used so that the selected path is the most optimal, for example the shortest.

[0006] Corridors are defined, for each segment of the path, by translating a center of a ball (in the mathematical sense) on said segment. When moving the ball, the latter covers a surface (or a volume) forming the corridor along the segment. This ball has a variable radius which is adjusted (either fixed for each segment, or adjusted along the segment) so that this radius is as large as possible without the ball intersecting one of the obstacles.

[0007] Predictive control is then implemented to ensure that the mobile system follows the desired trajectory, i.e., the determined path. Predictive control uses a search area limited to corridors, in order to avoid any collision with obstacles.

[0008] [Ioan 2021] does not address the issue of disruptions that the mobile system may experience.

[0009] In the literature, disturbances can be taken into account by enlarging the modeled obstacles by homothety. For safety reasons, an enlargement that is generally larger than necessary is carried out, which can lead, for example, to two obstacles touching each other and thus forego the paths that would pass between these two obstacles.

[0010] In a more general context, the article by DQ Mayne, SV Rakovic, R. Findeisen and F. Allgöwer entitled "Robust output feedback model predictive control of constrained linear systems", published in automatica 42 (2006) 1217-1222, describes a method for controlling a mobile system in an environment, by means of a tube-based predictive control in the presence of disturbances, this control using a state representation of the mobile system including the disturbances and a nominal state representation of the mobile system corresponding to the state representation without the disturbances, this control defining a tube bounding the state of the mobile system, this tube being defined by a set centered on the successive nominal states.

[0011] The command is implemented to move the mobile system in a noted space to reach a part !of this space. However, this document does not address the specific case of in an environment with obstacles.

[0012] Furthermore, the article by Mirabilio, M., Olaru, S., Dórea, CE, Iovine, A., & Di Benedetto, MD entitled “Path generation based on convex lifting: optimization of the corridors”, published in IFAC-PapersOnLine, 55(16), 260-265 in 2022, referenced hereinafter [Mirabilio 2022], describes an iterative method, based on the convex lifting technique, to optimize the average size of corridors. This method relies on the iterative enlargement of the modeled obstacles, while performing, at each iteration, a partitioning by convex lifting, as much as possible.

[0013] It may thus be desirable to provide a method for determining a desired trajectory which makes it possible to overcome at least some of the aforementioned problems and constraints. Summary of the invention

[0014] There is therefore proposed a method for controlling a mobile system in an environment, by means of a tube-based predictive control in the presence of disturbances, this control using a state representation of the mobile system including the disturbances and a nominal state representation of the mobile system corresponding to the state representation without the disturbances, this control defining a tube bounding the state of the mobile system, this tube being defined by a set centered on the successive nominal states, characterized by: - ​​obtaining an orthogonal projection of the definition set of the tube, on a space in which the environment extends; - the environment comprising obstacles, obtaining a partition of the environment, comprising so-called passage points, and segments avoiding the obstacles and having ends interconnected at the passage points;- a determination of a path comprising successive segments avoiding obstacles, each segment of the path being determined from one or more segments of the partition; - a definition of corridors respectively around the segments of the path, each corridor being convex and avoiding obstacles; - for each segment of the path, a verification that the projection moved by sliding a center of this projection onto the segment remains entirely within the corridor of this segment; - for each segment of the path where the projection leaves the corridor of this segment, a modification of the partition to remove the segment(s) from the partition from which the considered segment of the path was determined, then a return to the path determination step, this time from the modified partition; and - if the projection remains entirely within all the corridors, a command to the mobile system to successively pass through the corridors.

[0015] Thus, thanks to the invention, and in particular thanks to the verification that the projection remains entirely in each of the corridors and to the modification of the partition when this verification is negative, it is possible to find, if they exist, corridors allowing the mobile system to move in the environment without collision with the obstacles, despite the disturbances.

[0016] The invention may further comprise one or more of the following optional features, in any technically possible combination.

[0017] Optionally, for each segment of the path, the corridor of this segment is defined as the area swept by the projection when its center is slid over the segment considered, with a scaling of the projection during its sliding by a homothety factor such that the corridor obtained is flush with at least one of the obstacles but without intersecting a single one, the homothety factor defining a size of the corridor.

[0018] Optionally, the path segments are also segments of the partition.

[0019] Also optionally, the determination of the path includes: - a determination of the path such that it includes a succession of segments of the partition; and - a merging of successive segments of the path.

[0020] Optionally also, the method comprises a search for successive segments of the path such that the corridor of the segment resulting from the merging of these successive segments is at least as large as the smallest corridor of the successive merged segments, and the successive segments found are merged.

[0021] Optionally also, the method includes a search for a segment of the path connecting two waypoints separated from each other by a distance less than a predefined threshold, and the segment found is merged with the previous segment or the next segment of the path.

[0022] Optionally also, the method includes moving waypoints of the path to increase the size of at least the smaller corridor.

[0023] Also optionally, moving the path waypoints involves minimizing a cost function taking into account the positions of the path waypoints.

[0024] There is also provided a computer program downloadable from a communications network and / or recorded on a computer-readable medium, characterized in that it comprises instructions for executing the steps of the method according to the invention, when said program is executed on a computer.

[0025] There is also provided a system for controlling a mobile system in an environment, characterized by: - ​​a control module using a tube-based model predictive control in the presence of disturbances, this control using a state representation of the mobile system including the disturbances and a nominal state representation of the mobile system corresponding to the state representation without the disturbances, this control defining a tube bounding the state of the mobile system, this tube being defined by a set centered on the successive nominal states; - a module for obtaining an orthogonal projection of the definition set of the tube, onto a space in which the environment extends; - the environment comprising obstacles, a module for obtaining a partition of the environment, comprising so-called passage points, and segments avoiding the obstacles and having ends interconnected at the passage points;- a module for determining a path comprising successive segments avoiding obstacles, each segment of the path being determined from one or more segments of the partition; - a module for defining corridors around the segments of the path, each corridor being convex and avoiding obstacles; - a verification module, for each segment of the path, that the projection moved by dragging a center of this projection onto the segment remains entirely within the corridor of this segment; and - a partition modification module, for each segment of the path where the projection leaves the corridor of this segment, to remove the segment(s) from the partition from which the considered segment of the path was determined, then a return to the path determination step, this time from the modified partition. Brief description of the figures

[0026] The invention will be better understood with the aid of the following description, given solely by way of example and with reference to the appended drawings in which: - Figure 1 is a functional diagram of a mobile system comprising a control system according to the invention, - Figure 2 is a top view of a two-dimensional environment in which the mobile system is intended to move, - Figure 3 is a functional diagram of the control system;- Figure 4 is a block diagram of a method for determining a desired trajectory in the environment, - Figure 5 is a three-dimensional graph representing an RPI set and its projection forming a spatial uncertainty zone, - Figure 6 is a two-dimensional graph representing more specifically the spatial uncertainty zone, - Figure 7 illustrates the use of the spatial uncertainty zone to define a tube in which the mobile system remains during its movement, - Figure 8 is a top view of a modeling of the environment, - Figure 9 is a top view of the modeled environment and a partition thereof, - Figure 10 is a top view of the modeled environment and its partition completed with segments respectively connecting a starting point and an arrival point to the initial partition,; - Figure 11 is a view similar to that of Figure 10, with a path following the completed partition, - Figure 12 is an enlargement of a part of Figure 11, illustrating the determination of a corridor along a segment of the path, - Figure 13 is a view similar to that of Figure 11, with corridors around the segments of the path, - Figure 14 is a view similar to that of Figure 13, where only a part of the path is represented, with the result of merging segments of the path superimposed, - Figure 15 is a view similar to that of Figure 15, illustrating the case where the segment resulting from the merger is too narrow, - Figure 16 is a view similar to that of Figure 13, after the phase of merging segments of the path, - Figure 17 is a view similar to that of Figure 16, after a phase of optimizing the positions of the ends of the segments of the path,and - Figure 18 is a block diagram of a control step of the mobile system. Detailed description of the invention,

[0027] With reference to Figure 1, a mobile system 100 according to the invention will now be described.

[0028] The mobile system 100 is capable of moving in a terrestrial (e.g., earth, sand, ice, snow), aerial, space, marine and / or underwater environment. For example, the mobile system 100 may be a car, a robot, an airplane, a terrestrial / aerial / space / underwater drone, a boat or a submarine. The mobile system may be intended to transport objects, such as goods, animals and / or people.

[0029] The mobile system 100 firstly comprises a maneuvering device 102 designed to receive a command "and to move the mobile system 100 according to this command". The maneuvering device 102 comprises, for example, when the mobile system 100 is rolling: at least one drive wheel, a motor for rotating each drive wheel and a direction for modifying the orientation of the mobile system 100, for example by modifying an orientation of a or several steered wheels, for example the drive wheel(s). When the mobile system 100 is flying, the maneuvering device 102 may for example comprise at least one rotor.

[0030] The mobile system 100 further comprises a control system 104 configured to control the maneuvering device 102 by sending successive commands. The control system 104 comprises for this purpose the modules which will be described later.

[0031] Preferably, the control system 104 is a computer system comprising a data processing unit 104.2 (such as a microprocessor) and a main memory 104.4 (such as a RAM memory, from the English "Random Access Memory") accessible by the processing unit 104.2. The computer system 104 further comprises, for example, a partition interface and / or a computer-readable medium, such as for example a local medium 104.6 (such as a local hard disk) or a remote medium (such as a remote hard disk and accessible by the network interface through a communication partition) or even a removable medium (such as a USB key, from the English "Universal Serial Bus", or a CD, from the English "Compact Disc" or a DVD, from the English "Digital Versatile Disc") readable by means of an appropriate reader of the computer system (such as a USB port or a CD and / or DVD disk reader).

[0032] A computer program P containing instructions for the processing unit 104.2 is recorded on the medium 104.6 and / or downloadable via the network interface. This computer program P is, for example, intended to be loaded into the main memory 104.4, so that the processing unit 104.2 executes its instructions.

[0033] All or part of the modules of the control system 104 can thus be implemented in the computer program P, in the form of software modules. For example, all the modules can be software modules. Alternatively, all or part of these modules can be implemented in the form of hardware modules, that is to say in the form of an electronic circuit, for example micro-wired, not involving a computer program.

[0034] The mobile system 100 further comprises one or more sensors 106, 108 for providing, to the control system 104, measurements y of physical quantities related to the mobile system 100. For example, the mobile system 100 comprises a positioning sensor 106, such as a satellite positioning receiver, for example a GPS receiver (from the English “Global Positioning System”), to measure the position of the mobile system 100, for example the Cartesian coordinates X, Y of the mobile system 100. The mobile system 100 may further comprise an orientation sensor 108 to measure the orientation θ of the steered wheel(s) of the mobile system 100.

[0035] The mobile system 100 further comprises a communication device 110, preferably wireless, for example of the radio frequency type, for communicating (i.e. transmitting and / or receiving) data with the outside.

[0036] With reference to FIG. 2, the mobile system 100 is intended to move autonomously in an environment 200, from a starting point PD corresponding to its current position to an arrival point PA.

[0037] This environment 200 is cluttered, that is to say that it comprises obstacles 202 (only four are referenced in FIG. 2 for the sake of clarity), for example fixed. The mobile system 100 must avoid these obstacles during its movement, so as not to collide with them, because this could for example block the mobile system 100, or even damage it.

[0038] The environment 200 is for example also unstructured, that is to say that the obstacles are distributed randomly in space, unlike road networks where the obstacles (what is outside the roads) are organized according to a predetermined structure or arrangement.

[0039] Environment 200 can thus be a forest with trees, an underwater floor with rocks, or space with debris or satellites.

[0040] The movement of the mobile system 100 is likely to be subject to disturbances which may include state errors and / or measurement errors of the sensors 106, 108 of the mobile system 100. The state errors include, for example, modeling errors, as well as any external disturbance, such as, for example, wind pushing the mobile system 100 or sloping ground causing the mobile system 100 to slide.

[0041] With reference to FIG. 3, an exemplary implementation of the control device 104 will now be described in more detail.

[0042] The control system 104 firstly comprises an input module 302 designed to obtain the starting point PD and the ending point PA of the system mobile 100, a partition R0 of an ENV model of the environment 200 (hereinafter called the ENV modeled environment) and a spatial uncertainty zone Z.

[0043] The control system 104 further comprises a routing module 304 designed to determine a path extending from the starting point to the arrival point, this path passing through a succession of segments of the partition R0.

[0044] The control system 104 further comprises a merging module 306 designed to seek to remove waypoints from the path by merging at least one group of successive segments of the path.

[0045] The control system 104 further includes an enlargement module 308 configured to seek to enlarge corridors around the path segments.

[0046] The control system 104 further comprises a verification module 310 designed to verify that the path allows safe passage of the mobile system 100.

[0047] The control system 104 further comprises a module 312 for updating the partition R0 to remove segments whose corridors are too small.

[0048] The control system 104 further comprises a control module 314 configured to control the maneuvering device 102 to follow the path.

[0049] With reference to FIG. 4, a method 400 according to the invention for controlling the mobile system 100 will now be described.

[0050] The method 400 firstly comprises a modeling step 402.

[0051] During this modeling, state variables of the mobile system 100 are selected. These state variables comprise at least spatial coordinates of the mobile system 100 in a predefined reference frame. In two dimensions, these spatial coordinates are for example the Cartesian coordinates X, Y. The state variables may further comprise for example one or more of: a speed of the mobile system 100, an acceleration of the mobile system 100, an orientation of the mobile system 100, a speed of a steering member of the maneuvering device 102 (for example, a rotational speed of the wheels), currents or voltages of motors of the maneuvering device 102 when the mobile system 100 is electric.

[0052] The state variables thus define a state space by forming a basis for the latter. The state space has a dimension # equal to the number of selected state variables. Each state of the mobile system 100 is represented by a state vector of the state space, this state vector grouping the values ​​of the state variables for the state considered. Each state variable can take values ​​in a limited interval. For example, the X, Y coordinates can be limited to a predefined movement zone, for example to prevent the mobile system 100 from leaving it. When the mobile system 100 is a drone, its orientation along certain axes can be limited so that it does not turn over, for example. In addition, the mobile system 100 generally has physical constraints limiting the possible values ​​of the state variables: min-max speed, min-max acceleration, min-max steering angle on a car, etc.Thus, the state space is usually included in ℝ. % .

[0053] Furthermore, since the state variables comprise at least the spatial coordinates of the mobile system 100, the state space includes a spatial vector subspace defined by these spatial coordinates.

[0054] Furthermore, one or more sets of possible disturbances that the mobile system 100 may experience are defined. The disturbances include, for example, possible state errors & (e.g., modeling errors) that are assumed to be included in a set ' and / or possible measurement errors (that are assumed to be included in a set ).

[0055] Furthermore, a state representation of the mobile system 100, taking into account the disturbances &, (, is defined. In the case of a linear and discretized modeling in time, the state representation is for example given by the following equation, called discrete state representation: [Math.1] .= / - + 1" + & of state at the current time; - . denotes the state vector at the next instant; " denotes the command at the current instant; 2 denotes the current measurements obtained by the sensors 106, 108; & denotes the current state errors; ( denotes the current measurement errors; / , 1, 3 are respectively the state, command and measurement matrices; with - ∈ ⊂ ℝ % , " ∈ 6 ⊂ ℝ 7 , 2 ∈ ℝ 8 , & ∈ ', and ( ∈ ) ; and with , 6, ' and ) compact sets including for example the origin.

[0056] Furthermore, still during step 402, a state observer OBS is defined. The latter is designed to provide an observed state -9, close to the state - of the mobile system 100, from measurement 2. For example, it may be the Luenberger observer defined by the following equation: [Math.2] :;< ∶ , -9. = / -9 + 1" + >(2 − 29)2 9 = 3-9 where: -9 is the state observed at the current time; -9. is the state observed at the next time; 29 is the measurement observed at the current time; and > is a so-called gain matrix chosen so that B( / − >3) < 1, with B an operator giving the eigenvalues ​​of the matrix / − >3.

[0057] Alternatively, a Kalman observer could for example be used.

[0058] Furthermore, a nominal state representation ℛ Eof the mobile system 100 is defined. The latter is identical to the state representation ℛ of the mobile system 100, but without the disturbances &, (. Thus, in the case of the example above, the nominal representation ℛ E can be given by the following equation: [Math.3] ̅. = / -̅ + 1"E̅ at the current time; -̅. is the nominal state at the next time; "E is a nominal command at the current time; and 2E is the nominal measurement at the current time.

[0059] Furthermore, a predictive control G is defined. The latter is designed to give the nominal command "E from the observed state -9 deduced from measurement 2 by the observer, using the nominal representation ℛ E .

[0060] In addition, a corrective control law G Hcan be defined. The latter is designed to give a corrective command "I from the observed state -9 and the nominal state -̅ of the mobile system 100. The corrective command "I is intended to be combined with the nominal command "E in order to compensate for disturbances, i.e. an error between the observed state and the nominal state. The combination of these two commands "E, "I thus gives the command " supplied to the maneuvering device 102. For example, the nominal commands "E and corrective commands "I are added: " = "E + "I. The corrective control law G H is for example defined by the following equation: [ Math.4] "I = J(-9 − -̅) corrective at the current time; -9 is the observed state at the current time; -̅ is the nominal state at the current time; and J is a corrective control matrix (usually called "gain") chosen such that B( / + 1J) <1, with B an operator giving the eigenvalues ​​of the matrix / + 1J.

[0061] Thus, according to the previous equations, the error K between the observed state -9 and the nominal state -̅ follows the equation: [ Math.5] course a P is sets of disturbances ', ) and of the corrective control law G H so that the state - of the mobile system 100 remains at each instant in this set P centered on the nominal state -̅ at this instant. Alternatively, the corrective control law G H can be determined at the same time as the set P.

[0064] For this, for example, a positive invariant robust set P Qfor the equation [Math.5] of the error K and an invariant positive robust set R P for the equation [Math.6] of the error -M are determined. A set is robust positive invariant (from the English "Robust Positive Invariant Set", subsequently called "RPI set") for a discretized autonomous system: S . = T ( S, V ) with V ∈ Β, if, for all S belonging to this set, S . belongs together.

[0065] Preferably, each of the sets P Q and P R is selected as the smallest RPI set that is fully included in all possible RPI sets. This smallest RPI set is usually referred to as the minimum and is denoted by mRPI (minimum Robust Positive Invariant). In practice, the mRPI set is very difficult to determine, so an approximation of it is preferably used instead. For example, each of the sets P Q And R P can be determined using the method presented in the article by Rakovic, SV, Kerrigan, EC, Kouramas, KI, & Mayne, DQ (2005) “Invariant approximations of the minimal robust positively invariant set”, IEEE Transactions on automatic control, 50(3), 406-410, which allows the determination of the mRPI set or an approximation of the latter.

[0066] Then the set P is taken as the Minkowski sum of the sets RPI P Q et P R : P = P R ⊕ P Q . in Mayne et al. (2006), provided that the errors K and -M are respectively in P Q And RP at an initial instant, the state - then remains in the set P centered on the nominal state -̅, which is expressed by the equation: - ⊆ {-̅} ⊕ P. In practice, if the errors K and -M at the initial instant are respectively outside but close to Q P and R P, then the state - can also remain in the set P centered on the nominal state -̅.

[0068] During a step 406, the set P is projected orthogonally onto the spatial vector subspace, this orthogonal projection forming a spatial uncertainty zone Z. Figure 5 illustrates an example of implementation of this step 406 in a case where the state variables are the coordinates X, Y and the orientation θ of the mobile system 100. Figure 6 illustrates more specifically the spatial uncertainty zone Z obtained by the projection illustrated in Figure 5.

[0069] Subsequently, the projection of the state -, the observed state -9 and the nominal state -̅ onto the spatial vector subspace will be positions noted respectively Z, Ẑ and Z̅.

[0070] Returning to Figure 4, during a step 408, the input module 302 obtains the spatial area of ​​uncertainty Z. For example, steps 402 to 406 are implemented outside the mobile system 100 and the input module 302 receives the spatial area of ​​uncertainty Z from outside the mobile system 100, for example by the communication device 110. Alternatively, steps 404 and 406 can be implemented by the input module 302, which obtains the spatial area of ​​uncertainty Z as a result of this implementation.

[0071] As illustrated in Figure 7, generally, the spatial uncertainty zone Z is defined such that, if the position Z of the mobile system 100 is within the spatial uncertainty zone Z centered on the nominal position Z̅, then the Z position . of the mobile system 100 at the next instant remains in this spatial zone of uncertainty Z centered on the nominal position Z̅ . at the next instant. This property derives directly from that of the set P from which the spatial uncertainty zone Z is obtained. With this property of the spatial uncertainty zone Z, the position Z of the mobile system 100 remains in a tube T defined by a sliding of the spatial uncertainty zone Z on the nominal trajectory grouping the successive nominal positions Z̅, obtained by applying the nominal commands "E in the nominal representation ℛ E (equation [Math.3]).

[0072] Returning to Figure 4, during a step 410, the environment 200 is modeled to obtain the modeled environment ENV. An example of modeling is illustrated in Figure 8.

[0073] Each obstacle 202 is for example modeled by one or more shapes F. The shapes F are chosen to contain the modeled obstacle 202, by being close to the latter, for example so that the obstacle 202 is flush with the shape(s) F modeling it. These shapes F are preferably convex, given that any geometric object can be approximated by a convex shape. As is known per se, a shape is convex when, for each pair of points included inside this shape, the segment which joins the pair of points is entirely contained inside the shape. The convex shapes F are for example polyhedra or polytopes, that is to say bounded polyhedra (“bounded polyhedron” in English).

[0074] The F shapes will subsequently be called modeled obstacles.

[0075] The modeled obstacles F can be enlarged to take into account the size of the mobile system 100. With this enlargement, it is possible to subsequently consider the mobile system 100 as a point, which simplifies the calculations.

[0076] On the other hand, it is not necessary to enlarge the modeled obstacles F to take into account the disturbances, as is commonly done in the state of the art.

[0077] Returning to Figure 4, during a step 412, the modeled environment ENV is partitioned to provide the partition R0. An example of partition R0 is illustrated in Figure 9.

[0078] The partition R0 comprises SEG segments avoiding the modeled obstacles F, i.e. not intersecting them. The SEG segments each have two ends, by which the SEG segments are interconnected so as to define CEL cells each surrounding at least one of the modeled obstacles F, for example a single modeled obstacle F as in the example illustrated. The junction points of the ends are subsequently called PP crossing points.

[0079] For example, the partition R0 is obtained by applying the convex lifting technique described in Ioan's 2021 thesis referenced above. This technique uses configuration parameters ε and M, which are chosen in Ioan's thesis such that ε is much lower than M. However, in the context of the present invention, the choice of these parameters is not critical in the sense that this choice is not decisive on the final result. Thus, the parameters ε and M can be chosen freely. Generally, any partition of the environment 200 can be used in the context of the present invention, without any particular restriction on the method used to determine this partition. Thus, other known partitioning techniques can alternatively be used to obtain the partition R0.

[0080] Returning to Figure 4, during a step 414, the input module 302 obtains the partition R0.

[0081] For example, steps 410 and 412 are implemented outside the mobile system 100 and the input module 302 receives the partition R0 from outside the mobile system 100, for example by the communication device 110. Alternatively, steps 410 and 412 may be implemented by the input module 302, which obtains the partition R0 as a result of this implementation.

[0082] During a step 416, the input module 302 obtains the starting point PD and the arrival point PA.

[0083] During a step 418, the routing module 304 determines a path CH extending from the starting point PD to the arrival point PA, this path CH passing through a succession of passage points PP of the partition R0.

[0084] For this, if the starting point PD is not one of the waypoints PP of the partition R0, the routing module 304 determines a starting segment SD connecting the starting point PD to one of the waypoints PP of the partition R0. This waypoint thus forms a second point of the path CH, after the starting point PD. The starting segment SD is determined so that it does not intersect any of the modeled obstacles F. Module 304 then adds to partition R0 the starting point PD and the starting segment SD.

[0085] Similarly, if the arrival point P10 is not one of the waypoints of the partition R0, the routing module 304 determines an arrival segment S9 connecting one of the waypoints P9 to the arrival point P10. This waypoint P9 thus forms a penultimate point of the initial path C1 before the arrival point P10. The arrival segment S9 is determined so that it does not intersect any of the obstacles F. The module 304 then adds the arrival point P10 and the arrival segment S9 to the partition R0.

[0086] Figure 10 illustrates an example of adding, on the one hand, the starting segment SD connecting the starting point PD to a waypoint P2 and, on the other hand, the arrival segment SA connecting the arrival point PA to a waypoint P9.

[0087] To add the departure segment SD and / or the arrival segment SA, the following steps can be performed, for example.

[0088] The CEL cell to which the point to be added belongs is first identified. Then, one or more PP waypoints of this CEL cell are identified, this or these PP waypoints being such that it is possible to define at least one segment connecting the point to be added to the PP waypoint considered without intersecting any modeled obstacle F. One of these PP waypoints is then selected, for example the closest to the point to be added.

[0089] With reference to figure 11, the routing module 304 then determines a succession P3…P8 of passage points PP of the partition R0 connecting the second point P2 of the path CH to the penultimate point P9 of the path CH.

[0090] Since the partition R0 forms an undirected graph having the waypoints PP as nodes and the segments SEG as links between the nodes, the routing module 304 can thus determine the succession P3…P8 of waypoints PP by constructing an adjacency matrix of the undirected graph, while taking into account the lengths of the segments SEG. Penalization factors can be taken into account in the adjacency matrix, so as to take into account a state of the physical link between the waypoints (e.g. application of a corrective factor to model the presence of mud along one or more segments SEG). With this adjacency matrix, an algorithm such as the Djikstra algorithm can then be used to determine the succession P3…P8 of PP crossing points so that the CH path is as short as possible.

[0091] Returning to Figure 4, during a step 420, the routing module 304 determines corridors QD, Q2…Q8, QA around the segments SD, S2…S8, SA connecting the waypoints PD, P2…P9, PA of the path CH.

[0092] For example, the corridor of a segment is defined as the part of the modeled environment ENV covered by a sliding of the spatial uncertainty zone Z modified according to a homothety factor, this sliding being obtained by sliding the center O of the spatial uncertainty zone Z on the segment considered, the homothety factor being chosen so that the corridor is flush with the modeled obstacles F, that is to say that the corridor is flush with at least one of the obstacles, but without intersecting a single one. The homothety factor therefore defines a size of the corridor. A corridor is therefore all the larger as its homothety factor is high.

[0093] Figure 12 illustrates an example of determining the corridor Q2 for the segment S2. Figure 13 illustrates an example of corridors QD, Q2…Q8, QA respectively obtained for the segments SD, S2…S8, SA.

[0094] Returning to Figure 4, during a step 422, the merging module 306 seeks to reduce the number of waypoints of the path CH by merging successive segments of the path CH. This reduction makes it possible in particular to accelerate the optimization step which will be described later. A merger of several successive segments consists of creating a new segment connecting the free end of the first of the successive segments to the free end of the last of the successive segments. The free end of a segment is that of the two ends which is not connected to the adjacent segment of the succession.

[0095] First and second merging methods, which will be described below, may be used by the merging module 306, for example the second after the first, or only one of the two.

[0096] Referring to Figure 14, the first method is a reachability fusion method.

[0097] To do this, the merging module 306 searches for successive segments S4…S8 of the path CH such that the corridor Q4-8 of the segment S4-8 resulting from the merging of these successive segments S4…S8 is at least as large as the smallest corridor Q5 of the successive segments S4…S8 merged. If the merging module 306 finds such successive segments, it merges them. Thus, in the example of Figure 14, the segment S4-8 replaces the segments S4…S8.

[0098] Figure 14 also illustrates a segment S4-A that would result from the merging of segments S4, S5, S6, S7, S8, SA. As can be seen, this segment S4-A intersects a modeled obstacle, so that its corridor is an empty space. The merging module 306 therefore does not merge these segments S4…SA.

[0099] Figure 15 illustrates a case where the corridor QD-3 of the segment SD-3 resulting from the merger of the segments SD, S2, S3 is smaller than the smallest corridor Q2 of these segments SD, S2, S3. The merger module 306 therefore does not merge these segments SD, S2, S3.

[0100] The second method is a reconciliation merge method.

[0101] To do this, the merging module 306 searches for a segment of the path CH whose ends are separated by a distance less than a predefined threshold. Then, the merging module 306 merges the segment found with the previous or the next one in the path CH. The predefined threshold is chosen according to the size of the mobile system 100 and sufficiently low so that the segment resulting from the merger has a size substantially similar to or larger than the smallest of the merged segments. For example, the predefined threshold is less than or equal to 10% of the largest dimension of the mobile system 100 or of the volume of the latter. Alternatively, the predefined threshold could be less than half of the smallest dimension of the mobile system 100.

[0102] Figure 16 illustrates the CH path after the merging step 422. As can be seen, segments S2 to S4 have been merged to form segment S2-4 (surrounded by corridor Q2-4) and segments S5 to S8 have been merged to provide segment S5-8 (surrounded by corridor Q5-8).

[0103] Returning to Figure 4, during a step 424, preferably performed after step 422, the enlargement module 308 seeks to increase the size of at least one of the corridors of the path CH, preferably those having the smallest sizes, by moving the crossing points of the path CH, except the starting point PD and the arrival point PA.

[0104] Preferably, the enlargement module 308 ensures that the displacement of the waypoints of the path CH increases or at least leaves unchanged the size of each corridor.

[0105] Also preferably, each waypoint of the CH path is moved only in the intersection of the two corridors of the two segments of the CH path connected to this waypoint. This ensures that the corridors after moving actually exist (i.e., they are not the empty set).

[0106] For example, for the implementation of this step 424, the enlargement module 308 minimizes a cost function J depending on the positions of the waypoints of the path CH. For example, the cost function J depends on the sizes of the corridors of the segments of the path CH.

[0107] The cost function J is, for example, chosen so that its minimization constitutes a non-convex optimization problem. Indeed, a convex optimization problem, even if it would be simpler to solve, would require too many simplifications to give good results. With a non-convex optimization problem, the minimization of the cost function can be advantageously solved by a differential evolution algorithm.

[0108] For example, the cost function J is as follows: [Math.7] c say the factor segment connecting waypoint n-1 to waypoint n of the path before moving and s the number of waypoints of the path, and a′′ = [a′′ hhmc, … … .. , a′′{ , aE = [1 / ahh mc, … , 1 / a′′{opm}{o}] with le point of of the path after displacement, t u being the identity matrix, b = { a′ > a hh} being a Boolean vector, w being a positive definite weight matrix, preferably taken equal to xeay(aE hh ) in order to give priority to smaller corridors.

[0109] Figure 17 illustrates the CH path after the corridor size optimization step 424, where it can be seen that the waypoints P2, P5 and P9 have changed position compared to Figure 16, allowing the size of the corridors to be increased.

[0110] During a step 426, the verification module 310 verifies that the projection Z moved by sliding its center O on the segment SD, S2-4, S5-8, SA remains entirely in the corridor QD, Q2-4, Q5-8, QA of this segment SD, S2-4, S5-8, SA. With the definition of the corridors QD, Q2-4, Q5-8, QA described previously, this verification amounts to ensuring that the corridor QD, Q2-4, Q5-8, QA of each segment SD, S2-4, S5-8, SA of the path CH has a size greater than or equal to one. The verification module 310 then provides the path CH and the corridors, generally noted thereafter {Π |}, and method 400 proceeds to step 432.

[0111] If at least one corridor has a size less than or equal to one, during a step 428, the verification module 310 then verifies that at least one merger has been carried out in step 422 or that the size of at least one corridor has increased in step 424. In this case, the method 400 returns to step 422.

[0112] Otherwise, during a step 430, for each segment of the path CH whose corridor has a size less than one, the update module 312 modifies the partition R0 by removing from this partition R0 the segment(s) SEG from which the segment considered of the path CH was obtained.

[0113] Thus, this modification makes it possible to exclude the segment(s) of the partition R0 along which the mobile system 100 would present a risk of collision with an obstacle.

[0114] After step 430, the method 400 returns to step 418, this time using the modified partition R0.

[0115] During a step 432, the control module 314 controls the mobile system 100 so that the mobile system 100 follows the path CH by successively passing through the corridors QD, Q2-4, Q5-8 and QA, to reach the arrival point PA.

[0116] With reference to Figure 18, an example implementation of step 432 will now be described in more detail.

[0117] During a step 1802, the control module 314 determines the estimated state -9 from the measurements 2 using the observer.

[0118] During a step 1804, the control module 314 determines reduced corridors { Pi Q | } from the corridors { Pi | } and the spatial uncertainty zone Z. More precisely, each corridor reduces Π Q | is included in Π | ⊖ ^, for example taken equal to Π | ⊖ ^, where ⊖ represents the Pontryagin difference.

[0119] During a step 1806, the control module 314 determines the nominal command "E from the observed state -9 and the reduced corridors {Π Q |}. To do this, the control module 314 implements the predictive control G. The latter searches for several successive potential nominal commands allowing the path CH to be followed as best as possible, while ensuring that the nominal position resulting from these potential nominal commands remains within the reduced corridors {Π Q |}. For example, these potential nominal commands are searched in such a way as to get as close as possible to the next waypoint. The first of these potential nominal commands is then taken as the nominal command "E.

[0120] During a step 1808, the control module 314 determines the nominal state -̅ from the nominal state -̅p at the previous instant and the nominal command "Ep at the previous instant, according to the nominal representation ℛE: -̅ = / -̅p + 1"Ep. The estimated state is for example taken as the nominal state at the initial instant.

[0121] During a step 1810, the control module 314 determines the corrective command "I from the observed state -9 and the nominal state -̅, according to the corrective control law G H : "I = J(-9 − -̅).

[0122] During a step 1812, the control module 314 determines the command " by adding the nominal command "E and the corrective command "I:" = "E + "I, and provides the command " to the maneuvering device 102 so that the latter can implement it.

[0123] In conclusion, it will be noted that the invention is not limited to the embodiments described above. It will indeed appear to those skilled in the art that various modifications can be made to the embodiments described above, in light of the teaching which has just been disclosed to them.

[0124] For example, in figures, the segments are always straight. However, they could alternatively be curved.

[0125] Furthermore, in the described example, the second line of the nominal representation ℛ E is not used, but, in general, it could be used in the observer or to define constraints of the predictive control G.

[0126] Furthermore, in the described example, the homothety factor is constant on each segment. In other embodiments, it could vary along at least one of the segments, so as to obtain a convex corridor.

[0127] In the detailed presentation of the invention given above, the terms used should not be interpreted as limiting the invention to the embodiments set forth in this description, but should be interpreted to include all equivalents the prediction of which is within the reach of those skilled in the art by applying their general knowledge to the implementation of the teaching just disclosed to them.

Claims

Claims [1] Method (400) for controlling a mobile system (100) in an environment (200), by means of a tube-based predictive control in the presence of disturbances (&, (), this control using a state representation (ℛ) of the mobile system (100) including the disturbances (&, () and a nominal state representation (ℛ E) of the mobile system (100) corresponding to the state representation (ℛ) without the disturbances (&, (), this command defining a tube bounding the state of the mobile system (100), this tube being defined by a set (P) centered on the successive nominal states (-̅), characterized by: - ​​obtaining (408) an orthogonal projection (Z) of the set (P) defining the tube, on a space in which the environment (200) extends; - the environment (200) comprising obstacles (202), obtaining (414) a partition (R0) of the environment (200), comprising so-called passage points (PP), and segments (SEG) avoiding the obstacles (202) and having ends interconnected to the passage points (PP); - determining (418 - 430) a path (CH) comprising segments (S2-4, S5-8) successive avoiding obstacles (202), each segment (S2-4, S5-8) of the path (CH) being determined from one or more segments (SEG) of the partition (R0);- a definition of corridors (Q2-4, Q5-8) respectively around the segments (S2-4, S5-8) of the path (CH), each corridor (Q2-4, Q5-8) being convex and avoiding the obstacles (202); - for each segment (S2-4, S5-8) of the path (CH), a verification (426) that the projection (Z) moved by sliding a center (O) of this projection (Z) on the segment (S2-4, S5-8) remains entirely in the corridor (Q2-4, Q5-8) of this segment (S2-4, S5-8); - for each segment (S2-4, S5-8) of the path (CH) where the projection (Z) leaves the corridor (Q2-4, Q5-8) of this segment (S2-4, S5-8), a modification (430) of the partition (R0) to remove the segment(s) (SEG) from the partition (R0) from which the segment (S2-4, S5-8) considered from the path (CH) was determined, then a return to the step (418 - 430) of determining the path (CH), this time from the modified partition (R0); and; - if the projection (Z) remains entirely in all the corridors (Q2-4, Q5-8), a command (432) from the mobile system (100) to successively pass through the corridors (Q2-4, Q5-8). [2] Method (400) according to claim 1, in which, for each segment (S2-4, S5-8) of the path (CH), the corridor (Q2-4, Q5-8) of this segment (S2-4, S5-8) is defined as the area swept by the projection (Z) when its center (O) is slid on the segment (S2-4, S5-8) considered, with a scaling of the projection (Z) during its sliding by a homothety factor such that the corridor (Q2-4, Q5-8) obtained is flush with at least one of the obstacles (F) but without intersecting a single one, the homothety factor defining a size of the corridor (Q2-4, Q5-8). [3] Method (400) according to claim 1 or 2, in which the segments of the path (CH) are segments (SEG) of the partition (R0).[4] Method (400) according to claim 2, in which the determination (418 - 430) of the path (CH) comprises: - a determination (418) of the path (CH) such that it comprises a succession of segments (SEG) of the partition (R0); and - a merging (422) of successive segments (S2, S3, S4, S5, S6, S7, S8) of the path (CH). [5] Method (400) according to claim 4, comprising a search for successive segments (S4, S5, S6, S7, S8) of the path (CH) such that the corridor (Q4-8) of the segment (S4-8) resulting from the merging of these successive segments (S4, S5, S6, S7, S8) is at least as large as the smallest corridor (Q5) of the successive segments (S4, S5, S6, S7, S8) merged, and in which the successive segments (S4, S5, S6, S7, S8) found are merged.[6] Method (400) according to claim 5, comprising a search for a segment of the path (CH) connecting two waypoints separated from each other by a distance less than a predefined threshold, and in which the segment found is merged with the previous segment or the following segment of the path (CH). [7] Method (400) according to any one of claims 4 to 6, comprising a displacement (424) of waypoints (P2, P5, P9) of the path (CH) to increase the size of at least the smaller corridor. [8] Method (400) according to claim 7, in which the displacement (424) of the waypoints (P2, P5, P9) of the path (CH) comprises a minimization of a cost function (J) taking into account the positions of the waypoints (P2, P5, P9) of the path (CH). [9] Computer program (P) downloadable from a communication network and / or recorded on a computer-readable medium (104.6), characterized in that it comprises instructions for executing the steps of the method according to any one of claims 1 to 8, when said program (P) is executed on a computer (104). [10] System (104) for controlling a mobile system (100) in an environment (200), characterized by: - ​​a control module (314) by means of a tube-based model predictive control in the presence of disturbances (&, (), this control using a state representation (ℛ) of the mobile system (100) including the disturbances (&, () and a nominal state representation (ℛE) of the mobile system (100) corresponding to the state representation (ℛ) without the disturbances (&, (), this command defining a tube bounding the state of the mobile system (100), this tube being defined by a set (P) centered on the successive nominal states (-̅); - a module (302) for obtaining an orthogonal projection (Z) of the set (P) defining the tube, on a space in which the environment (200) extends; - the environment (200) comprising obstacles (202), a module (302) for obtaining a partition (R0) of the environment (200), comprising so-called passage points (PP), and segments (SEG) avoiding the obstacles (202) and having ends interconnected to the passage points (PP); - a module (304, 306, 308) for determining a path (CH) comprising segments (S2-4, S5-8) successive avoiding the obstacles (202), each segment (S2-4, S5-8) of the path (CH) being determined from one or more segments (SEG) of the partition (R0);- a module (304) for defining corridors (Q2-4, Q5-8) around respectively the segments (S2-4, S5-8) of the path (CH), each corridor (Q2-4, Q5-8) being convex and avoiding obstacles (202); - a module (310) for verifying, for each segment (S2-4, S5-8) of the path (CH), that the projection (Z) moved by sliding a center (O) of this projection (Z) onto the segment (S2-4, S5-8) remains entirely within the corridor (Q2-4, Q5-8) of this segment (S2-4, S5-8); and - a module (312) for modifying the partition (R0), for each segment (S2-4, S5-8) of the path (CH) where the projection (Z) leaves the corridor (Q2-4, Q5-8) of this segment (S2-4, S5-8), to remove the segment(s) (SEG) from the partition (R0) from which the segment (S2-4, S5-8) considered from the path (CH) was determined, then a return to the step (418 - 430) of determining the path (CH), this time from the modified partition (R0).

Images