CONTROL OF A MOBILE SYSTEM IN AN ENVIRONMENT COMPRISING OBSTACLES

Abstract

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

Description

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

[0001] The present invention lies in 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 the navigation of autonomous vehicles in unstructured environments. Technological background

[0004] The doctoral thesis of D. loan entitled “Safe Navigation Strategies within Cluttered Environment” from the University of Paris-Saclay, published on June 30, 2021 and referenced below [loan 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 passage points, and segments avoiding the obstacles and having ends interconnected to 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; and - a definition of corridors respectively around the segments of the path, each corridor being convex and avoiding obstacles.

[0005] More precisely, in the method of [loan 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, the Dijkstra algorithm is used so that the selected path is the most optimal, for example the shortest.

[0006] The 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 The ball, the latter covering 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 the largest possible without the ball intersecting one of the obstacles.

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

[0008] [loan 2021] does not address the issue of the disturbances that the mobile system may undergo.

[0009] In the literature, disturbances can be taken into account by enlarging the modeled obstacles by homothety. For safety, an enlargement generally larger than necessary is carried out, which can lead, for example, to two obstacles touching and thus give up the paths which 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 control is implemented to move the mobile system in a space denoted X to reach a part Xy of this space. However, this document is not concerned with the particular case of movement in an environment comprising obstacles.

[0012] Furthermore, the article by Mirabilio, M., Olaru, S., Dôrea, CE, lovine, 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 the corridors. This method is based 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 desired 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] A method is therefore proposed 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, onto 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 to 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 dragging a center of this projection on the segment remains entirely in the corridor of this segment; - For each segment of the path where the projection exits the corridor of that segment, modify the partition to remove the segment or segments of the partition from which the considered segment of the path was determined, then return to the path determination step, this time starting from the modified partition; and - If the projection remains entirely within all the corridors, command 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 within 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 colliding with obstacles, despite the perturbations.

[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] Also optionally, the segments of the path are segments of the partition.

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

[0020] Also optionally, 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] Also optionally, the method comprises 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 following segment of the path.

[0022] Also optionally, the method comprises a displacement of passage points of the path to increase the size of at least the smaller corridor.

[0023] Also optionally, the displacement of the waypoints of the path includes a minimization of a cost function taking into account the positions of the waypoints of the path.

[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] Also provided is 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 to 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 sliding a center of this projection on the segment remains entirely in 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) of 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: - [Fig.l] is a functional diagram of a mobile system comprising a control system according to the invention, - [Fig.2] is a top view of a two-dimensional environment in which the mobile system is intended to move, - [Fig.3] is a functional diagram of the control system; - [Fig.4] is a block diagram of a method for determining a trajectory desired in the environment, - [Fig.5] is a three-dimensional graph representing an RPI set and its projection forming a spatial zone of uncertainty, - [Fig.6] is a two-dimensional graph representing more specifically the spatial zone of uncertainty, - [Fig.7] illustrates the use of the spatial uncertainty zone to define a tube in which the mobile system remains during its movement, - [Fig.8] is a top view of a model of the environment, - [Fig.9] is a top view of the modeled environment and a partition of the latter, - [Fig. 10] is a top view of the modeled environment and its partition completed with segments connecting respectively a starting point and an arrival point to the initial partition, - [Fig.11] is a view similar to that of [Fig.10], with a path following the completed score, - [Fig.12] is an enlargement of a part of [Fig.11], illustrating the determination of a corridor along a segment of the path, - [Fig. 13] is a view similar to that of [Fig. 11], with corridors around the path segments, - [Fig.14] is a view similar to that of [Fig.13], where only part of the path is represented, with the result of merging segments of the path superimposed, - [Fig. 15] is a view similar to that of [Fig. 15], illustrating the case where the segment resulting from the fusion is too narrow, - [Fig. 16] is a view similar to that of [Fig. 13], after the path segment merging phase, - [Fig. 17] is a view similar to that of [Fig. 16], after a phase of optimizing the positions of the ends of the segments of the path, and - [Fig. 18] is a block diagram of a control step of the mobile system. Detailed description of the invention

[0027] With reference to [Fig.l], 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 11 and to move the mobile system 100 according to this command u. 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 steering system for modifying the orientation of the mobile system 100, for example by modifying an orientation of one or more 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 commands 11 suc transferable. The control system 104 comprises 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 accessible by the network interface through a communication partition) or 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 linked 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 (Global Positioning System) receiver, for measuring the position of the mobile system 100, for example Cartesian coordinates X, Y of the mobile system 100. The mobile system 100 may further comprise an orientation sensor 108 for measuring the orientation 0 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 radiofrequency 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 furthermore unstructured, that is to say that the obstacles are distributed randomly in space, unlike road networks where the obstacles (that which is outside the roads) are organized according to a predetermined structure or arrangement.

[0039] The environment 200 can thus be a forest comprising trees, an underwater bed comprising 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 example of 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 arrival point PA of the mobile system 100, a partition R0 of a model ENV of the environment 200 (hereinafter called the modeled environment ENV) and a spatial zone of uncertainty 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 segments of the path.

[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 X by forming a basis for the latter. The state space X has a dimension n 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 X, 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 coordinates X, Y 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 X is usually included in .

[0053] Furthermore, since the state variables comprise at least the spatial coordinates of the mobile system 100, the state space X 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 comprise, for example, possible state errors w (for example, modeling errors) that are assumed to be included in a set W and / or possible measurement errors v that are assumed to be included in a set V.

[0055] Furthermore, a state representation of the mobile system 100, taking into account the disturbances H', v, 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] ( Y — Ax + Bu + W where: x denotes the state vector at the current time; -Y rv . \ y-, [ y = Cx + v denotes the state vector at the next instant; 11 denotes the command at the instant in

[0056]

[0057]

[0058]

[0059]

[0060] current; 3' denotes the current measurements obtained by the sensors 106, 108; w denotes the current state errors; v denotes the current measurement errors; A, B, C are respectively the state, control and measurement matrices; with ieXc R”, ue U c Rp, y G Rm, GW, and v EV; and with X, U, W and V compact sets including for example the origin. Furthermore, still during step 402, a state observer OBS is defined. The latter is designed to provide an observed state r, close to the state x of the mobile system 100, from the measurement X. For example, it may be the Luenberger observer defined by the following equation: [Math. 2] OBS: X+ — Àx + Bu + Liy-y] where: is the state observed at the current time; x is the state observed at the next instant; y is the measurement observed at the current instant; and L is a so-called gain matrix chosen so that p(A-LC) < 1, with P an operator giving the eigenvalues ​​of the matrix A - LC. Alternatively, a Kalman observer could for example be used. Further, a nominal state representation R of the mobile system 100 is defined. The latter is identical to the state representation R of the mobile system 100, but without the disturbances w, v. Thus, in the case of the example above, the nominal representation R can be given by the following equation: [Math. 3] (x+ = Ax + Bû [ J = Cx where :1 is the nominal state at the current time; x+ is the state nominal at the next instant; “ is a nominal command at the current instant; and X is the nominal measurement at the current time. Furthermore, a predictive control C is defined. The latter is designed to give the nominal command w from the observed state x inferred from the measurement X by the observer, using the nominal representation R. Furthermore, a corrective control law C can be defined. The latter is designed to give a corrective command Tl from the observed state 3 and the nominal state x of the mobile system 100. The corrective command W is intended to be combined with the nominal command u in order to compensate for disturbances, i.e. an error between the observed state and the nominal state. The combination of these two commands a, U thus gives the command 11 supplied to the maneuvering device 102. For example, the nominal commands tl and corrective commands u are added: U = ü + ü. The corrective control law C is for example defined by the following equation: [Math. 4] U = K(xx) where: U is the corrective control at the current time; x is the observed state at the current time; x is the nominal state at the current time; and K is a corrective control matrix (usually called "gain") chosen such that p( A 4- BK) < 1, with P an operator giving the eigenvalues ​​of the matrix A + BK.

[0061] Thus, according to the previous equations, the error e between the observed state $ and the nominal state x follows the equation: [Math. 5] e+- (A + BK) e + ôwith $ = LCx + Lv-

[0062] Similarly, there is an error X between the actual state x and the observed state £, defined by X = x - x and following the equation: [Math. 6] _ï+ = (A-LC) x + ôwith S = w - Lv-

[0063] During a step 404, a set S is determined from the sets of disturbances W, V and the corrective control law C so that the state x of the mobile system 100 remains at each instant in this set S centered on the nominal state A at this instant. Alternatively, the corrective control law C can be determined at the same time as the set S.

[0064] For this, for example, a robust positive invariant set § for the equation [Math. 5] of the error e and a robust positive invariant set § for the equation [Math. 6] of the error X are determined. A set is robust positive invariant (from the English "Robust Positive Invariant Set", subsequently called "RPI set") for a discretized autonomous system: a+ — p) with PGB, if, for all a belonging to this set, "+ also belongs to this set.

[0065] Preferably, each of the sets S and § 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 designated mRPI (minimum Robust Positive Invariant). In practice, the mRPI set is very difficult to determine, so an approximation of the latter is preferably used instead. For example, each of the sets S and S may 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 mRPI set or an approximation of the latter to be determined.

[0066] Then, the set S is taken as the Minkowski sum of the sets RPI § and S: S = S®S.

[0067] As shown in Mayne et al. (2006), provided that the errors e and X are respectively in § and § at an initial time, the statex then remains in the set S centered on the nominal statex, which is expressed by the equation:%Ç {xj © S- In practice, if the errors e and À' at the initial time are respectively outside but close to § and §, then the statex can also remain in the set S centered on the nominal state Ë

[0068] During a step 406, the set S is projected orthogonally onto the spatial vector subspace, this orthogonal projection forming a spatial uncertainty zone Z. [Fig.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 0 of the mobile system 100. [Fig.6] illustrates more specifically the spatial uncertainty zone Z obtained by the projection illustrated in [Fig.5].

[0069] Subsequently, the projection of the statex, of the observed state -î and of the nominal statex on the spatial vector subspace will be positions noted respectively P, p and P.

[0070] Returning to [Fig.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, in general, the spatial uncertainty zone Z is defined so that, if the position P of the mobile system 100 is in the spatial uncertainty zone Z centered on the nominal position P, then the position p~ of the mobile system 100 at the following instant remains in this spatial uncertainty zone Z centered on the nominal position p+ at the following instant. This property derives directly from that of the set S from which the spatial uncertainty zone Z is obtained. With this property of the spatial uncertainty zone Z, the position P 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 P, obtained by applying the nominal commands u in the nominal representation R (equation [Math. 3]).

[0072] Returning to [Fig.4], during a step 410, the environment 200 is modeled to obtain the modeled environment ENV. An example of modeling is illustrated in [Fig.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 as far as possible. delisant. These F-shapes are preferably convex, since 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 that joins the pair of points is entirely contained inside the shape. Convex F-shapes 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 in order 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 account of the disturbances, as is commonly done in the state of the art.

[0077] Returning to [Fig.4], during a step 412, the modeled environment ENV is partitioned to provide the partition R0. An example of partition R0 is illustrated in [Fig.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 passage points.

[0079] For example, the partition R0 is obtained by applying the convex lifting technique described in the loan 2021 thesis referenced above. This technique uses configuration parameters e and M, which are chosen in the loan thesis such that e is much less 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 e and M can be chosen freely. Generally speaking, 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 [Fig.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 RO partition as the 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. The module 304 then adds the starting point PD and the starting segment SD to the partition R0.

[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] [Fig. 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 for example be carried out.

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

[0089] With reference to [Fig. 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 SEG segments). With this adjacency matrix, an algorithm such as the Djikstra algorithm can then be used to determine the succession P3.. .P8 of waypoints PP so that the path CH is as short as possible.

[0091] Returning to [Fig.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] [Fig. 12] illustrates an example of determining the corridor Q2 for the segment S2. [Fig. 13] illustrates an example of corridors QD, Q2...Q8, QA respectively obtained for the segments SD, S2...S8, SA.

[0094] Returning to [Fig.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] With reference to [Fig. 14], the first method is an attainability fusion method.

[0097] For 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 merged segments S4...S8. If the merging module 306 finds such successive segments, it merges them. Thus, in the example of [Fig.14], the segment S4-8 replaces segments S4.. .S8.

[0098] [Fig. 14] also illustrates a segment S4-A which 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] [Fig. 15] illustrates a case where the corridor QD-3 of the segment SD-3 resulting from the merging of the segments SD, S2, S3 is smaller than the smallest corridor Q2 of these segments SD, S2, S3. The merging module 306 therefore does not merge these segments SD, S2, S3.

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

[0101] For 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] [Fig. 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 [Fig.4], during a step 424, preferably carried out 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 passage 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 makes it possible to ensure that the corridors after movement do indeed exist (i.e. that 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 passage points of the path CH. For example, the cost function J depends on the sizes of the corridors of the segments of 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] J "VII0 II 2 b = ® = ......- ^'{A'-iHA']] (da")*b otherwise with Cl'{n_i})n} the size (i.e. the homothety factor, as explained previously) of the corridor of the segment connecting the waypoint n-1 to the waypoint n of the path before movement and N the number of waypoints of the path, and = ........ fl"{AM] {#]]'^ l / «''{A'-i]{A'}]aVeCâ size of the corridor of the segment connecting waypoint n-1 to waypoint n of the path after displacement, Q being the identity matrix, b — { a ' > d} being a boolean vector, Q being a positive definite weight matrix, preferably taken equal to diag^Ü ^ in order to give priority to the smallest corridors.

[0109] [Fig. 17] illustrates the path CH after step 424 of optimizing the size of the corridors, where it can be seen that the waypoints P2, P5 and P9 have changed position compared to [Fig. 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 {H(}, and the 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 RO partition by removing from this RO partition the SEG segment(s) from which the segment considered of the CH path was obtained.

[0113] Thus, this modification makes it possible to exclude the segment(s) of the RO partition 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 [Fig. 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 $ from the measurements y using the observer.

[0118] During a step 1804, the control module 314 determines reduced corridors {ÎI(} from the corridors { ü,} and the spatial zone of uncertainty Z. More precisely, each reduced corridor El; is included in n, @Z, for example taken equal to n;0Z, where © represents the Pontryagin difference.

[0119] During a step 1806, the control module 314 determines the nominal command 11 from the observed state $ and the reduced corridors { FI - For this, the control module 314 implements the predictive control C. The latter searches for several successive potential nominal commands making it possible to follow the path CH as best as possible, while ensuring that the nominal position resulting from these potential nominal commands remains in the reduced corridors { flj • For example, these potential nominal commands are searched for 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 u.

[0120] During a step 1808, the control module 314 determines the nominal state A from the nominal state x at the previous instant and the nominal command U at the previous instant, according to the nominal representation R: X = Ax + Bu. 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 M from the observed state x and the nominal state x, according to the corrective control law C: ïi - K ( x - L ) •

[0122] During a step 1812, the control module 314 determines the command 11 by adding the nominal command 11 and the corrective command Ü: U = M + W, and provides command 11 to the operating 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 the 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 R is not used, but, in general, it could be used in the observer or to define constraints of the predictive control C.

[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 foregoing detailed description of the invention, the terms used should not be construed as limiting the invention to the embodiments set forth in the present description, but should be construed to include all equivalents within the scope of the art to which the person skilled in the art can apply his general knowledge in implementing the teaching that has been disclosed to him.

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 (w, v), this control using a state representation (R) of the mobile system (100) including the disturbances (w, v) and a nominal state representation (R) of the mobile system (100) corresponding to the state representation (R) without the disturbances (vv, v), this control defining a tube bounding the state of the mobile system (100), this tube being defined by a set (S) centered on the successive nominal states (A), characterized by: - obtaining (408) an orthogonal projection (Z) of the set (S) defining the tube, onto 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); - a determination (418 - 430) of a path (CH) comprising successive segments (S2-4, S5-8) 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 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 of the path (CH) has been determined, then a return to step (418 - 430) of determining the path (CH), this time from the modified partition (RO); and - if the projection (Z) remains entirely in all the corridors (Q2-4, Q5-8), a command (432) of 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, wherein the segments of the path (CH) are segments (SEG) of the partition (R0).

4. Method (400) according to claim 2, wherein 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 The found segment is merged with the previous segment or the next 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 (M', v), this control using a state representation (R) of the mobile system (100) including the disturbances (M', v) and a nominal state representation (R) of the mobile system (100) corresponding to the state representation (R) without the disturbances (M', v), this control defining a tube bounding the state of the mobile system (100), this tube being defined by a set (S) centered on the successive nominal states (*); - a module (302) for obtaining an orthogonal projection (Z) of the set (S) 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 successive avoiding segments (S2-4, S5-8); the obstacles (202), each segment (S2-4, S5-8) of the path (CH) being determined from one or more segments (SEG) of the partition (RO); 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 the 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 (RO), 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) of the partition (RO) 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