WO2024089595A1 - Model predictive control-based, low-speed trajectory planning with dynamic obstacle avoidance in unstructured environments - Google Patents

Abstract

An automotive electronic dynamics control system (10) for a motor-vehicle (2) equipped with and automotive automated driving system (1) designed to cause the motor-vehicle (2) to perform low-speed manoeuvres in automated driving. The electronic dynamics control system (10) is designed to implement a Driving Path Planner (11) designed to compute the planned driving path as a planned driving corridor within which the motor-vehicle (2) may be driven and made up of a series of driving path segments each with start and end waypoints and an orientation referenced in an inertial reference frame. The electronic dynamics control system (10) is further designed to implement a Model Predictive Control (MPC)-based Trajectory Planner and Controller (12) designed to compute a planned lateral trajectory for the motor-vehicle (2) during the low-speed manoeuvre performed in automated driving based on a linearized Lateral Trajectory Model which exhibits a singularity whenever the relative orientation of a couple of successive driving path segments of the planned driving path is equal to or higher than a given amount. The Trajectory Planner and Controller (12) is further designed to dynamically modify relative orientation (θ_ROT) of a motor-vehicle reference frame with respect to the inertial reference frame along the planned driving path so as to result in the relative orientations of all of the couples of successive driving path segments of the planned driving path being lower than the given amount. The Trajectory Planner and Controller (12) is further designed to compute rotation angles (θ_ROT) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path by first computing an equivalent driving path which approximates/simplifies the planned driving path in terms of waypoints and orientation while maintaining the same destination and is made up of a series of equivalent driving path segments; and then computing the rotation angles (θ_ROT) based on the computed an equivalent driving path.

Description

“MODEL PREDICTIVE CONTROL-BASED, LOW-SPEED TRAJECTORY _PLANNING WITH _DYNAMIC _OBSTACLE _AVOIDANCE IN _UNSTRUCTURED ENVIRONMENTS” CROSS-REFERENCE TO RELATED APPLICATIONS This Patent Application claims priority from Italian Patent Application No. 102022000022308 filed on October 28, 2022, the entire disclosure of which is incorporated herein by reference. T^ECHNICAL F^{IELD OF THE} I^NVENTION The present invention relates to model predictive control-based, low-speed trajectory planning with dynamic obstacle avoidance to cause autonomous motor vehicles to perform low-speed manoeuvres, such as valet parking and home-zones, in automated driving in unstructured environments. The invention finds application in any type of road motor vehicles, regardless of whether it is used for the transportation of people, such as a car, a bus, a camper, etc., or for the transportation of goods, such as an industrial vehicle (truck, B-train, trailer truck, etc.) or a light or medium-heavy commercial vehicle (light van, van, pick-up trucks, etc.). In this patent application, the term “trajectory” will be used to indicate the state of a motor-vehicle, defined as the set of temporal trends as position, orientation, and speed, which define the desired states of the motor-vehicle motion, over a period of time, to distinguish it from the term “path”, which is generally used to indicate a sequence of positions of a motor-vehicle, without worrying about speed or higher-order terms. STATE OF THE ART As is known, autonomous driving is one of the most challenging research fields in today's automotive industry, because it is expected to contribute to the quality road transportation under different aspects. Despite the fact that the improvements of active and passive safety equipment enabled to reduce the number of road accidents significantly in the last decades, still many accidents happen every day mainly due to human failure. Therefore, autonomous driving could further increase the safety level of transportation. Another important social expectation is the simultaneous increase of fuel consumption efficiency and decrease of pollution, which may also be enabled by the rise of automation. One of the most important topic in the autonomous driving is trajectory planning, which represents the motor-vehicle motion references design. Low-speed trajectory planning can involve a large variety of different scenarios, including structured and unstructured environments, pedestrians, cyclists, etc. In this context, real-time planning is crucial to the dexterity of autonomous motor vehicles when traversing environments with unknown obstacles. A huge number of different motor-vehicle trajectory planning approaches have been proposed, which can be approximately classified into three macro-categories: heuristic-based methods, geometric-based methods, and methods based on optimal control techniques. Heuristic-based approaches usually apply artificial intelligence techniques, such as machine learning methods, search-based methods and random sampling methods. Geometric-based methods are suitable mainly for low speed applications such as automated parking but, at higher speeds, these can’t consider the dynamic behaviour of motor-vehicle and therefore its stability. Most of the geometric-based and heuristic-based methods generate paths instead of trajectories. To obtain a trajectory, some speed profile could be used to convert the computed path into a trajectory. Optimal control-based methods, use optimal control techniques such as MPC (Model Predictive Control) and NLP (Non-Linear Programming) in order to generate the trajectory. Optimization techniques are used to find the appropriate control input sequence,

steering wheel angle and motor-vehicle longitudinal acceleration, that drives the motor-vehicle to the desired end-point. The behaviour of the system in term of system states to the given sequence of control actions is computed by a model-based prediction. These methods enable the direct definition of trajectories instead of paths. A real-time, MPC-based, low-speed trajectory planning is proposed in WO 2021/079338 A1 to the present Applicant. In particular, WO 2021/079338 A1 discloses an automotive electronic dynamics control system for a motor-vehicle equipped with an automotive automated driving system designed to cause the motor-vehicle to perform low-speed manoeuvres in automated driving and comprising an automotive sensory system designed to sense motor-vehicle-related quantities, and automotive actuators comprising an Electric Power Steering, a Braking System and a Powertrain (PT). The electronic dynamics control system is disclosed to be designed to implement a Driving Path Planner designed to receive data representative of static obstacles in the surroundings of the motor vehicle and representing static space constraints to the motion of the motor vehicle, and compute, based on the received data, a planned driving path for the motor vehicle during a low-speed manoeuvre performed in automated driving. The electronic dynamics control system is disclosed to be designed to further implement a Model Predictive Control (MPC)-based Trajectory Planner and Controller designed to receive from the Driving Path Planner data representative of the planned driving path and from the automotive sensory system data representative of positions and orientations of the motor vehicle and of dynamic obstacles in the surroundings of the motor vehicle and representing dynamic space constraints to the motion of the motor vehicle, and compute, based on the received data, a planned lateral trajectory and a planned longitudinal trajectory for the motor vehicle during the low-speed manoeuvre performed in automated driving. The electronic dynamics control system is disclosed to be designed to further implement a Motion Controller designed to receive from the Trajectory Planner and Controller data representative of the planned lateral and longitudinal trajectories, and compute commands for the Electric Power Steering based on the planned lateral trajectory, and for the Braking System and the Powertrain based on the planned longitudinal trajectory. The Driving Path Planner is disclosed to be designed to compute the planned driving path as an obstacle-free driving corridor within which the motor vehicle may be driven and made up of a series of driving path segments each with a length and an orientation referenced in an inertial reference frame. The MPC-based Trajectory Planner and Controller is disclosed to comprises an MPC-based Lateral Trajectory Planner and Controller designed to compute the planned lateral trajectory as a series of steering requests (δ) referenced in a motor vehicle reference frame, and an MPC-based Longitudinal Trajectory Planner and Controller designed to compute the planned longitudinal trajectory as a series of longitudinal acceleration requests. The Lateral Trajectory Planner and Controller is disclosed to be designed to further compute the planned lateral trajectory based on a linearized Model which exhibits a representation singularity whenever the relative orientation of a couple of successive driving path segments of the planned driving path is equal to or higher than a given amount, and to dynamically modify relative orientation of the motor- vehicle reference frame and the inertial reference frame along the planned driving path to cause the relative orientations of all of the couples of successive driving path segments of the planned driving path to be lower than the given amount. In particular, the Lateral Trajectory Planner and Controller is disclosed to be designed to compute the orientation of the motor-vehicle reference frame relative to the inertial reference frame based on, in particular as a (linear) interpolation of, the orientations of the driving path segment currently driven by the motor vehicle and of one or more of the next driving path segments. SUBJECT-MATTER AND SUMMARY OF THE INVENTION The aim of the present invention is to provide an improved MPC-based, low- speed trajectory planning with dynamic obstacle avoidance that is able to generate a dynamically feasible, comfortable, and customizable trajectory that allows motor- vehicles to perform low-speed manoeuvres in automated driving. According to the present invention, an automotive electronic dynamics control system for an autonomous motor vehicle is provided, as claimed in the appended claims. BRIEF DESCRIPTION OF THE DRAWINGS Figure 1 shows a block diagram of an automotive automated driving system to perform low-speed manoeuvres. Figure 2 shows an automotive sensory platform of the an automotive automated driving system. Figure 3 shows inertial and motor-vehicle reference frames for lateral control. Figure 4 shows a general block diagram of trajectory planning and control for low-speed manoeuvres. Figure 5 shows a general block diagram of longitudinal control setup. Figure 6 shows a flowchart of the rotation angle optimization according to the present invention. Figures 7 and 8 comparatively show choices of orientation according to the state of the art and to the present invention, respectively. Figures 9 and 10 comparatively show ad hoc free-corridor constructed according to the state of the art and to the present invention, respectively. DETAILED DESCRIPTION OF PREFERRED EMBODIMENTS OF THE INVENTION The present invention will now be described in detail with reference to the attached figures to allow a person skilled in the art to make and use it. Various modifications to the described embodiments will be immediately apparent to the persons skilled in the art and the generic principles described can be applied to other embodiments and applications without departing from the protective scope of the present invention, as defined in the attached claims. Therefore, the present invention should not be considered limited to the described and illustrated embodiments, but it must be accorded the widest protective scope in accordance with the described and claimed characteristics. Where not defined otherwise, all the technical and scientific terms used herein have the same meaning commonly used by persons skilled in the art pertaining to the present invention. In the event of a conflict, this description, including the definitions provided, will be binding. Furthermore, the examples are provided for illustrative purposes only and as such should not be considered limiting. In particular, the block diagrams included in the attached figures and described below are not intended as a representation of structural characteristics or constructive limitations, but must be interpreted as a representation of functional characteristics, i.e. intrinsic properties of the devices and defined by the obtained effects or functional limitations, which can be implemented in different ways so as to protect their functionalities (operating abilities). In order to facilitate the understanding of the embodiments described herein, reference will be made to some specific embodiments and a specific language will be used to describe the same. The terminology used in the present document has the purpose of describing only particular embodiments, and is not intended to limit the scope of the present invention. Figure 1 shows a block diagram of an automotive electronic automated driving system 1 of a motor vehicle 2 and designed to cause the motor vehicle 2 to perform low-speed manoeuvres in automated driving. As shown in Figure 1, the automated driving system 1 comprises: - automotive systems, of which only those involved in the implementation of the present invention will be described below, and comprising, inter alia, an automotive sensory system or platform 3 designed to detect motor-vehicle-related quantities comprising, by way of example, wheel angle, steering wheel angle, yaw rate, longitudinal and lateral acceleration, position, etc., and automotive actuators 4 comprising, inter alia, Electric Power Steering (EPS) 5, Braking System Module (BSM) 6, and PoWerTrain (PWT) 7; and - an automotive electronic control unit (ECU) 8 designed to communicate, via an automotive on-board communication network 9, such as a high-speed CAN, also known as C-CAN, FlexRAy or others, with the automotive sensory platform 3 and the automotive actuators 4, directly or indirectly, i.e., via dedicated automotive electronic control units, and to store and execute an automated driving software comprising software instructions which, when executed, cause the ECU 8 to become configured to communicate and cooperate with the with the automotive sensory platform 3 and the automotive actuators 4 to cause the motor vehicle 2 to perform low-speed manoeuvres in automated driving. The automotive sensory platform 3, an exemplary embodiment of which is shown in Figure 2, may comprise traditional normal production ESC inertial Active Chassis sensors comprising longitudinal and lateral acceleration sensors, yaw rate sensors, and environment sensors including a (dual antenna) GNSS receiver, one or different forward-looking stereo cameras, a normal production forward-looking camera, one or different lidar sensors, one or more radar sensors, and a number of ultrasonic sensors. EPS 5 comprises an electric motor operatively coupled to either a steering gear or a steering column and electrically controlled by the ECU 8 based on angular position and torque of the steering column sensed by the automotive sensory system or platform 3 to apply assistive steering torque and, resultingly, provide different amounts of assistance depending on driving conditions. A normal production EPS driven by a traditional Park Assist HWTO (Hand Wheel Torque Overlay) interface on the C-CAN 6 has been modified in terms of maximum motor-vehicle speed to result in the EPS providing not only assistance to the motor-vehicle driver but also a mechatronic unit to steer the motor-vehicle in a stand-still condition. BSM 6 represents a functional interface to realize acceleration and deceleration actions on the motor-vehicle 2. The ECU 8 provides on the C-CAN 9 a functional channel able to be a gateway of acceleration / positive torque to the powertrain ECU (ECM) and the way to decelerate the motor-vehicle 2 by brakes. The normal production parking longitudinal interface has been modified in order to achieve technical targets of present invention. Also Dual Dry Clutch Transmission (DDCT) ECU software has been modified in order to manage the longitudinal manoeuvres from/to 0/15 km/h around to 0 Nm of engine torque in a comfortable way. MOTOR-VEHICLE MODEL The Applicant has experienced that well-known dynamic models currently used for Trajectory Planning and Control for low-speed manoeuvres are undefined or ill-conditioned at low speeds. As shown to be effective in Polack et al., The Kinematic Bicycle Model: a Consistent Model for Planning Feasible Trajectories for Autonomous Vehicles, IEEE Intelligent Vehicles Symposium (IV), 2017, and Kong et al. Kinematic and dynamic vehicle models for autonomous driving control design, in 2015 IEEE Intelligent Vehicles Symposium 2015, pp.1094-1099, the model used for low speed applications (0÷20 km/h) is the kinematic bicycle model. Lateral Motor-Vehicle Model With regard to the lateral motor-vehicle model, the 4-DoF (Degree of Freedom) kinematic bicycle model is one of the simplest and well-conditioned models used in motion planning to approximate and plan the motion of a motor vehicle in the context of autonomous driving. Figure 3 shows the inertial and motor-vehicle reference frames for lateral control, in which the left and right wheels are approximated with two single wheels at points A and B, respectively for the front and the rear axles. The steering angles of the front and rear wheels are represented by δf ∈ R and δr ∈ R, respectively. The model is derived assuming that the side-slip angle of the front and rear wheels are zero and only the front wheel can be steered (δr =0). For that reason and simplicity, notation δ = δf ∈ [δmin, δmax] will be used, where δmin and δmax represent the minimum and maximum steering angles that depend on the motor- vehicle actuators. The resulting kinematic bicycle model may be described by the following state- space equations in the inertial reference frame: ^^ = ^^ ^^ ^^ ^^ ^^ (1) ^^ = ^^ ^^ ^^ ^^ ^^ (2) ^_{^ =} ^^ ^^ ^^ ^^ ^^ (3) ^^ ^^ = ^^ ⁽⁴⁾ In particular, x ∈ R and y ∈ R are the Cartesian coordinates of the motor- vehicle’s rear wheel, while ψ := [−π, π] ⊂ R describes the orientation (yaw angle) of the motor vehicle, v := [v_min, v_max] ⊂ R and a := [a _min, a _max] ⊂ R denote the velocity and longitudinal acceleration, respectively. Thus, the state and input vectors of this model can be defined as X=[x,y,ψ,v] and U=[δ,a], respectively. The described kinematic model is nonlinear. An approach to efficiently solve the trajectory planning problem is to use a “divide et impera” approach. Firstly, the original model can be divided into longitudinal and lateral dynamics, respectively, to obtain two independent yet simpler sub-models. Moreover, a nonlinear state and input transformation can be applied to the lateral model, namely the time-state control form (T-SCF) disclosed by Sampei, M. (1994), “A control strategy for a class of non- holonomic systems - time-state control form and its application” in Proceedings of 1994 33^rd IEEE Conference on Decision and Control, vol. 2, 1120–1121, and by Oyama, K. and Nonaka, K. (2013), “Model predictive parking control for non- holonomic vehicles using time-state control form”, 2013 European Control Conference (ECC), 458–465. This approach results in the lateral dynamics being linearized and representable as a differential equation w.r.t. the space, namely:

Longitudinal Motor-Vehicle Model The longitudinal dynamics of the motor vehicle can be accurately approximated with a double integrator system, in which the states are the travelled distance ξ ∈ R and the motor-vehicle speed v, and the control input is the acceleration a. The corresponding model, whose variables are expressed in the motor-vehicle frame, results to be very simple yet accurate, and it is represented by the following system of differential equations:

TRAJECTORY PLANNING DESIGN Figure 4 shows a general block diagram of a dynamics control system 10 designed to control lateral and longitudinal dynamics of the motor vehicle 2 during low-speed manoeuvres in automated driving. The dynamics control system 10 may be entirely implemented by the ECU 8 or its implementation may be distributed among different ECUs, according to a proprietary logical architecture that the automotive manufacturer will decide to adopt. For ease of description, in the following the dynamics control system 10 will be described to be entirely implemented by the ECU 8, without thereby this implying any loss of generality. The dynamics control system 10 is designed to implement a Driving Path Planner 11 designed to receive data representative of static obstacles, such as roads, buildings, etc., in the surroundings of the motor vehicle 2 and representing, in the form of, e.g., a high definition road map or a binary occupancy map, static space constraints to the motion of the motor vehicle 2, and to compute a planned driving path for the motor vehicle 2 during a low-speed manoeuvres performed in automated driving based on the positions of the static obstacles in the surroundings of the motor vehicle 2. In particular, the Driving Path Planner 11 is designed to compute the planned driving path as an obstacle-free or collision-free driving corridor or path within which the motor vehicle 2 may be driven and made up of waypoints that do not collide with any static objects in the environment and that form a series of driving path segments each defined by respective start and end waypoints, an orientation and a driving direction referenced in an inertial (or absolute) reference frame. In the literature several solutions have been proposed to solve the path planning problem, although most of them are limited to simulated scenarios and have not been applied to real prototypes. Within them, searches through graphs are very popular for their simplicity and effectiveness, e.g., by combining Voronoi decomposition diagram with path search algorithms like A*, or through explorations, e.g., RRT*. Other techniques include search based on point models, or search linked to kinematic models, e.g., the Hybrid A* algorithm, or optimal search via non-linear MPCs. The output of the Driving Path Planner 11 is a static path P := [p1, p2, …, pN], that guarantees a collision-free and drivable path from the starting waypoint p1 to the ending waypoint pN, where the i-th generic waypoint pi := (xi, yi, ψi, ri) and ri ∈ [0, 1] represents the motor vehicle’s motion direction, backward and forward, respectively. The dynamics control system 10 is further designed to implement a Model Predictive Control (MPC)-based Trajectory Planner and Controller 12 designed to receive from the Driving Path Planner 11 data representative of the planned driving path of the motor vehicle 2 and to compute, based thereon, a planned lateral trajectory and a planned longitudinal trajectory of the motor vehicle 2 as a series of high-level vehicle requests for steering wheel angle and acceleration, as described in more detail in the following. In particular, the MPC-based Trajectory Planner and Controller 12 comprises two distinct MPC-based Trajectory Planners and Controllers, a Lateral Trajectory Planner and Controller 12a designed to plan and control the Lateral dynamics, and a Longitudinal Trajectory Planner and Controller 12b designed to plan and control the Longitudinal dynamics. The Lateral Trajectory Planner and Controller 12a is designed to compute the planned lateral trajectory as a series of steering requests δ along the planned driving path in a motor-vehicle reference frame, and the Longitudinal Trajectory Planner and Controller 12b is designed to compute the planned longitudinal trajectory as a series of longitudinal acceleration requests a of the motor vehicle 2 along the planned driving path; Model Predictive Control (MPC) has been developed considerably over the last two decades. The main advantage of the MPC is the fact that it allows the current timeslot to be optimized while taking future time-slots into account. This is achieved by optimizing a finite time-horizon, but only implementing the first time-slot. MPC can manage future reference profiles and constraints and anticipate control actions accordingly. These limits may be imposed on any part of the system variables, such as states, outputs, inputs, and considering main actuators characteristics and operative limitations. The dynamics control system 10 is further designed to implement a Motion Controller 13 designed to receive from the Trajectory Planner and Controller 12 data representative of the planned lateral a longitudinal trajectories and to compute appropriate commands for the automated driving system 1, as described in more detail in the following. MPC-based Lateral Trajectory Planner and Controller Given the motor-vehicle dynamic model expressed by equation (5), the MPC- based Lateral Trajectory Planner and Controller 12a is designed to control the motor- vehicle lateral dynamics to track the optimal path according to motor-vehicle constraints on the steering angle δmin and δmax and environmental constraints on the obstacle-free driving corridor. Applying the Tustin discretization to equation (5), it may be obtained:

where ∆s is the discretization step w.r.t. the time-state z₁ = x, and it is positive when the motor vehicle 2 moves forward, and negative otherwise. The lateral control problem can be formulated as an MPC problem as follows:

subject to the following constraints:

tan(δmin) ≤ Γ( z3,µ2) ≤ tan(δmax) where H_lat ∈ N is the prediction horizon, and y_refk ∈ R^Hlat+1 is the centerline of the left and right free-spaces Y_maxk ∈ R^Hlat+1 and Y_mink ∈ R^Hlat+1. They are obtained by discretizing by ∆_s the segments that connect the path planning waypoints. The combination of these two vectors is generally referred to as the free corridor of the Trajectory Planning. Moreover,

is the linear approximation of:

obtained through a first-order Taylor expansion of Γ(.,.), that is defined to include the constraint on the steering angle (δ_min ≤ δ ≤ δ_max) in the transformed system. Preliminarily, it is worth mentioning that in equation (8) variables x, y are used to represent the position of the motor vehicle 2 in a conveniently roto-translated frame. Generally speaking, the MPC problem could be solved in the motor-vehicle coordinate system. However, the time-state control form (T-SCF) disclosed by Sampei (1994) introduces singularities and strongly limits the prediction horizon. To obtain better performance, an ad-hoc roto-translation of the reference frames and the lateral constraints, obtained as a translation w.r.t. the motor-vehicle position and a rotation of an angle θRot could be applied. The choice of this angle has significant effects on the performance of the MPC. Equation (8) shows the formulation of the Trajectory Planning as an MPC- based problem that focuses only on the lateral control of the motor vehicle. Since it is designed as a linear optimization problem, it turns out that the solution can be computed efficiently and guarantees a deviation from the reference path that shrinks to zero over the time-steps. However, this approach has two well-known limitations, namely: - the T-SCF-based model introduces singularities in ψ = ±π/2 and then must work in the range −π/2 < ψ < π/2, - there is a linearization error, despite small, related to the first-order approximation of Γ(z₃, μ₂). Thus, the applications of this method seems to be dramatically limited to the scenarios in which each turn is less than π/2. A solution to this problem is proposed in the aforementioned WO 2021/079338 A1 to the present Applicant and provides for rotating the planned path in the motor- vehicle reference frame in order to manage cases in which the motor vehicle has to follow tight turns, thus avoiding singularities. In particular, in the solution proposed in WO 2021/079338 A1, assuming that at a certain time-step k the motor vehicle is located at the ith segment of the planned driving path, which is defined as a straight line linking two consecutive waypoints, then the problem of the singularities can be addressed by dynamically determining a rotation angle θRot, defined as a linear interpolation of the orientations θi and θi+1 of the i_th segment and of the next one i_th+1. This solution also provides for a combination of this rotation angle with another one, computed over the successive segment, namely θ_i+2, eventually weighting it by a “corrective factor” W ∈ R. Although this approach allows to efficiently solve the lateral MPC problem while reducing the effects linked to the singularities, it considers only few consecutive waypoints, thus potentially obtaining a myopic predictive controller. An empirical solution to this problem could be to down-sample as much as possible the number of waypoints provided by the path planning. In this way, the length of each segment is maximized, with a consequent maximization of the range of view of the MPC, but worsening the approximation of the trajectory to be performed. The present invention overcomes this problem by improving and optimizing the dynamic choice of the three orientations used in the computation of θRot, and thus optimizing and maximizing the path section used for the prediction in the MPC and avoids unnecessary rotations. Figure 6 shows a flowchart of the rotation angle θRot optimization according to the present invention. As shown in Figure 6, starting from the first segment and taking the segment orientation as reference θ_ref, the MPC-based Lateral Trajectory Planner and Controller 12a is designed to compute and store in ΘOUT the last orientation of the equivalent segment created between the first waypoint and the next ones that satisfies

θ_new| ≤ θ_th, where θ_th is a user-settable threshold lower than π/2. The obtained segment orientation is then used as a new reference θref, and the process repeats until the last waypoint is reached. Then, at each time-step k, the functions PickIndex and PickOrientation, given the index i_k of the segment where the motor-vehicle is located, extract the information necessary to compute the rotation angle at that time, through the formula:

where: dk is the Euclidean distance at time-step k between the current position of the motor-vehicle (the motor-vehicle rear center axle) and the end waypoint of the driving path segment i_k containing the current position of the motor-vehicle, ^^_{1,0 ^^} is the distance between the end waypoint of the driving path segment i_k containing the current position of the motor-vehicle and the end waypoint of the next driving path segment following the one containing the current position of the motor- vehicle,

and ^^_{2 ^^} are the first two orientations of ΘOUT w.r.t. ik. The code of the rotation angle θRot optimization according to the present invention is reproduced here below for the sake of completeness. Rotation angle θRot optimization Require: XSEG,YSEG,ΘSEG,iMAX,ik Ensure: θRotk

while j ≤ i_MAX do θnew ← ComputeAngle(Xi,Yi,Xj+1,Yj+1) while |θref − θnew| ≤ θth do

through (9) Therefore, in less mathematical terms, the Lateral Trajectory Planner and Controller 12a is designed to compute the rotation angles ^^ _{^^ ^^ ^^} of which the motor- vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path by: - computing an equivalent driving path which approximates/simplifies the planned driving path in terms of waypoints and orientation while maintaining the same destination and is made up of a series of equivalent driving path segments, wherein: ◦ an equivalent driving path segment is computed to extend from a start waypoint of a planned driving path segment and an end waypoint of either the same or a different planned driving path segment along the planned driving path; ◦ an equivalent driving path segment is therefore defined by (the coordinates x_i, y_i) of the start and end waypoints of the planned driving path segment(s) between which the equivalent driving path segment extends, an equivalent orientation θ and a driving direction referenced in the inertial reference frame; and ◦ the end waypoint of an equivalent driving path segment is computed as the end waypoint of the last planned driving path segment that causes the equivalent orientation of the equivalent driving path segment relative to the equivalent orientation of the preceding equivalent driving path segment to be lower than the given amount; and - computing the rotation angles ^^ _{^^ ^^ ^^} of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path based on the computed equivalent driving path, in particular based on: ◦ the computed equivalent orientation ^^₀ of the equivalent driving path segment extending between planned driving path segments including the planned driving path segment i_k; and ◦ the computed equivalent orientation(s)

of one or more of the next equivalent driving path segments, conveniently the two next equivalent driving path segments, namely the equivalent driving path segments following the equivalent driving path segment extending between planned driving path segments including the planned driving path segment ik. Figures 7 and 8 comparatively show the choices of segment orientation according to WO 2021/079338 A1 to the present Applicant and to the present invention, respectively. In particular, in Figures 7 and 8, the light grey waypoints represent the waypoints delimiting the planned driving path segments; the light grey dashed lines, denoted with reference letter A, represent the planned driving segments; the dark grey dashed lines, denoted with reference letter B, represent the planned driving segments exploitable in the MPC horizon; the darker grey dashed lines, denoted with reference letter C, represent equivalent driving path segments that establish θ₀, θ₁ and θ₂ and the light grey cone forms the angle θ_th. The present invention solves the problem of the singularities without limiting the number of segments to take into account. The proposed approach provides two main benefits, i.e., i) it maximizes the prediction horizon that the MPC can use, and ii) it allows arbitrary dense path as inputs, with a significant improvement in the trajectory accuracy. In particular, Figures 9 and 10 comparatively show the ad hoc free-corridors constructed according to according to WO 2021/079338 A1 to the present Applicant and to the present invention, respectively, where XSEG and YSEG are vectors containing the waypoints (xi, yi) provided by the higher level path planning module. Moreover, Θ_SEG is the vector of orientations of the segments derived from these waypoints, and i_k is the segment i in which the motor vehicle is located at that time. Finally, i_MAX represents a stopping iteration condition, that can for example take into account the end of the trajectory or a change in the direction of motion. Although the rotation angle θ_Rot resulting from the proposed algorithm is bounded by construction, for the sake of practical applications, it could be convenient to apply an additional saturation to equation (9) to increase comfort. It can be expressed as follows:

where θ_SAT is the saturation angle w.r.t. the current orientation ^^_{0 ^^}, and its value depends on the particular application. MPC-based Longitudinal Trajectory Planner and Controller Like the MPC-based Lateral Trajectory Planner and Controller 12a, also for MPC-based Longitudinal Trajectory Planner and Controller 12b the control technique is the linear MPC. This allows speed reference and obstacle tracking to be managed in a single integrated approach. On the same stream of equation (7), the longitudinal dynamics expressed by equation (6) can be discretized leveraging the Tustin rule, obtaining the following discrete-time longitudinal model:

where: ^^ is the longitudinal position of the motor-vehicle from the start of the planned driving path, ^^ is the longitudinal speed, and Δ_t ∈ R is the discretization step w.r.t. time. The MPC-based Trajectory Planning problem for the longitudinal control can be then formulated as:

subject to the following constraints:

where H_Long is the prediction horizon, jmin and jmax are minimum and maximum allowed jerks, and vrefk is the motor vehicle speed reference. The reference on the travelled distance ξ_refk acts as an integral action to handle stationary errors due to exogenous disturbances and/or inaccuracies in the motion control module. It is obtained through as: ξrefk = (Dref − Dmeas) + vrefk∆tk,tk (12) where:

In equation (12), D_meas is the measured travelled distance (from sensors), D_ref is the expected travelled distance computed from the longitudinal control activation time (t0) to the current time step (tk). Moreover, ξmax determines the maximum value of the travelled distance ξ_Goal, and, whenever reached, it constrains the motor vehicle to stop. It is further exploited the constraint of the maximum travelled distance in other to take into account the different stop condition requests that could come from the Lateral controller, obtaining: ξ_maxk = min(ξ_Goal,ξ_Corridor,ξ_Obstacle) (13) Thus, the stop condition takes into account the goal position (ξ_Goal), dynamic objects (ξObstacle) and the availability of a sufficiently large road (ξ_Corridor) and takes the minimum of those stop conditions. Motion Controller The previously-described motor-vehicle models assume that the control inputs, i.e., the series of steering requests δ and the series of longitudinal acceleration requests a, can be directly controlled. However, low level controllers are needed to transform this control inputs into physical signals for the actuators. In detail, the steering angle control is realized by using the EPS torque interface available on the CAN. Similarly, the motor-vehicle longitudinal motion is controlled by means of acceleration/deceleration requests to the braking system through normal production Adaptive Cruise Control interface. Steering Control The EPS low level control loop has been designed by using a state feedback controller comprising: - a linear time invariant Kalman observer that filters/estimates the system states, useful also to estimate driver hands on the steering wheel, and - an optimal linear quadratic integral controller able to track the steering wheel angle reference from MPC-based Lateral Trajectory Planner and Controller 12a. Further details about a model-based development and related practical considerations may be found in Raffone E. (2016) A Reduced Order Steering State Observer for Automated Steering Control Functions, Proceedings of the 13^th Int. Conf. on Informatics in Control, Automation and Robotics, Lisbon, Portugal. Longitudinal Acceleration Control Conversely to the steering control, the longitudinal acceleration control is managed by multiple actuations (e.g., engine, electric motor, gear-box, and braking), hence the interfaces can change according to the motor-vehicle architecture, actuator controls, and their integration. In this case, a motor-vehicle is faced in which the motor-vehicle interfaces, for longitudinal dynamic control, are: - engine-torque request: delivered to Engine Control (Treqs) - deceleration request: delivered to Brakeing System (areqs) With the aim to keep the control architecture simply and easy to tune, no further feedback control loop is introduced at this level. Therefore, the longitudinal acceleration control is implemented by means of the structure shown in Figure 5. The command a_x is split in two channels, one for each interface. The Braking System includes a deceleration closed loop that allows to accept directly deceleration command (areqs) and to generate a braking torque Tbra. The Engine Control performs a closed-loop control based on an estimation of applied engine torque T_Eng. To match the available interface it has been implemented an inverse vehicle model, to convert ax in Treqs, which considers the current motor-vehicle configuration (e.g., gear ratio, inertia, friction) and mainly exogenous input (e.g. road slope, drag forces).

Claims

CLAIMS 1. An automotive electronic dynamics control system (10) for a motor-vehicle (2) equipped with an automotive automated driving system (1) designed to cause the motor-vehicle (2) to perform low-speed manoeuvres in automated driving; the automotive automated driving system (1) comprises: - an automotive sensory system (3) designed to sense motor-vehicle-related quantities, and - automotive actuators (4) comprising an Electric Power Steering (5), a Braking System (6), and a Powertrain (7); the electronic dynamics control system (10) is designed to implement: - a Driving Path Planner (11) designed to: ◦ receive data representative of static obstacles in the surroundings of the motor-vehicle (2) and representing static space constraints to the motion of the motor-vehicle (2), and ◦ compute, based on the received data, a planned driving path for the motor- vehicle (2) during a low-speed manoeuvre performed in automated driving; - a Model Predictive Control (MPC)-based Trajectory Planner and Controller (12) designed to: ◦ receive from the Driving Path Planner (11) data representative of the planned driving path and from the automotive sensory system (3) data representative of positions and orientations of the motor-vehicle (2) and of dynamic obstacles in the surroundings of the motor-vehicle (2) and representing dynamic space constraints to the motion of the motor-vehicle (2), and ◦ compute, based on the received data, a planned lateral trajectory and a planned longitudinal trajectory for the motor-vehicle (2) during the low-speed manoeuvre performed in automated driving; and - a Motion Controller (13) designed to: ◦ receive from the Trajectory Planner and Controller (12) data representative of the planned lateral and longitudinal trajectories, and ◦ compute commands for the Electric Power Steering (5) based on the planned lateral trajectory, and for the Braking System (6) and the Powertrain (7) based on the planned longitudinal trajectory; the Driving Path Planner (11) is designed to compute the planned driving path as an obstacle-free driving corridor within which the motor-vehicle (2) may be driven and made up of a series of planned driving path segments each defined by start and end waypoints and an orientation ( ^^ _^^) thereof referenced in an inertial reference frame; the MPC-based Trajectory Planner and Controller (12) comprises: - an MPC-based Lateral Trajectory Planner and Controller (12a) designed to compute the planned lateral trajectory as a series of steering requests (δ) referenced in a motor-vehicle reference frame; and - an MPC-based Longitudinal Trajectory Planner and Controller (12b) designed to compute the planned longitudinal trajectory as a series of longitudinal acceleration requests (a); the Lateral Trajectory Planner and Controller (12a) is further designed to compute the planned lateral trajectory based on a linearized Model which exhibits a representation singularity whenever the relative orientation of a couple of successive driving path segments of the planned driving path is equal to or higher than a given amount; and the Lateral Trajectory Planner and Controller (12a) is further designed to dynamically modify orientation ( ^^ _{^^ ^^ ^^}) of the motor-vehicle reference frame relative to the inertial reference frame along the planned driving path to cause relative orientations of all of the couples of successive driving path segments of the planned driving path to be lower than the given amount; the Lateral Trajectory Planner and Controller (12a) is further designed to compute in the inertial reference frame rotation angles ( ^^ _{^^ ^^ ^^}) of which the motor- vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path; characterised in that the Lateral Trajectory Planner and Controller (12a) is further designed to compute the rotation angles ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path by: - computing an equivalent driving path made up of a series of equivalent driving path segments, wherein: ◦ an equivalent driving path segment is computed to extend from a start waypoint of a planned driving path segment and an end waypoint of either the same or a different planned driving path segment along the planned driving path; ◦ an equivalent driving path segment is defined by start and end waypoints of the planned driving path segment(s) between which the equivalent driving path segment extends and by an equivalent orientation in the inertial reference frame; and ◦ the end waypoint of an equivalent driving path segment is computed as the end waypoint of the last planned driving path segment that causes the equivalent orientation of the equivalent driving path segment relative to the equivalent orientation of the preceding equivalent driving path segment to be lower than the given amount; and: - computing the rotation angles ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame along the planned driving path based on the computed equivalent driving path. 2. The automotive electronic dynamics control system (1) of claim 1, wherein the Lateral Trajectory Planner and Controller (12a) is further designed to compute a rotation angle ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame in a planned driving path segment (ik) based on: - the computed equivalent orientation ( ^^_{0 ^^} of the equivalent driving path segment extending between planned driving path segments including the planned driving path segment (i_k); and - the computed equivalent orientation(s) ( ^^_{1 ^^} , ^^_{2 ^^}) of one or more of the next equivalent driving path segments. 3. The automotive electronic dynamics control system (1) of claim 2, wherein the Lateral Trajectory Planner and Controller (12a) is further designed to compute a rotation angle ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame in a planned driving path segment (i_k) based on: - the computed equivalent orientation ( ^^_{1 ^^} , ^^_{2 ^^}) of the next two equivalent driving path segment. 4. The automotive electronic dynamics control system (1) of claim 3, wherein the Lateral Trajectory Planner and Controller (12a) is further designed to compute a rotation angle ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame in a planned driving path segment (ik) as: ( ^^ ^^ _{2 ^^} − ^^_{1 ^^}) ^^ _^^ ( ^^_{1 ^^} − ^^ _{^^ ^^ ^^ ^^−} ) ^^ _{^^ ^^ ^^ ^^ ^^} = ^^ + ₁ + ^^ ^^ _{^^ ^^ ^^ 1,0 ^^−1 ^^} ^^_{1,0 ^^} where: k denotes the time computation step; ^^ _{^^ ^^ ^^} denotes the computed rotation angle; ^^_{0 ^^} denotes the equivalent orientation of the equivalent driving path segment extending between planned driving path segments including the planned driving path segment; ^^_{1 ^^}denotes the equivalent orientation of the first next equivalent driving path segment; ^^_{2 ^^} denotes the equivalent orientation of the second next equivalent driving path segment; W is a weighting factor, dk is the Euclidean distance between the current motor-vehicle position and the end waypoint of the planned driving path segment containing the current motor- vehicle position; and dk denotes the length of the equivalent driving path segment subtending the planned driving path segment. 5. The automotive electronic dynamics control system (1) of claim 4, wherein the Lateral Trajectory Planner and Controller (12a) is further designed to compute a rotation angle ( ^^ _{^^ ^^ ^^}) of which the motor-vehicle reference frame is to be rotated relative to the inertial reference frame in a planned driving path segment (ik) as:

where ^^ _{^^ ^^ ^^} denotes a saturation rotation angle relative to ^^_{0 ^^}. 6. The automotive electronic dynamics control system (1) of any one of the preceding claims, wherein the end waypoint of an equivalent driving path segment is computed as the end waypoint of the last planned driving path segment that causes the following condition to be satisfied: |θref − θnew| ≤ θth where: ^^ _{^^ ^^ ^^} denotes an equivalent orientation of an equivalent driving path segment; ^^ _{^^ ^^ ^^} denotes an equivalent orientation of a preceding equivalent driving path segment; and ^^ _^^ℎ denotes an equivalent orientation threshold.