| CPC B60W 60/0015 (2020.02) [B60W 50/14 (2013.01); B60W 60/0027 (2020.02); B60W 2554/4026 (2020.02); B60W 2554/4046 (2020.02); B60W 2556/35 (2020.02); B60W 2556/65 (2020.02)] | 17 Claims |
|
1. A system for imitating target vehicle behavior in ego vehicle automated driving, the system comprising:
one or more sensors of the ego vehicle and capturing ego vehicle condition information, and one or more sensors of the target vehicle and capturing target vehicle condition information;
one or more actuators of a vehicle dynamics system disposed on the ego vehicle and selectively altering a state of the ego vehicle;
one or more control modules disposed in the ego vehicle, each of the one or more control modules having a processor, a memory, and one or more input/output (I/O) ports, the I/O ports in communication with the one or more sensors of the ego vehicle and the one or more sensors of the target vehicle and with the one or more actuators; the memory storing programmatic control logic, the processor executing the programmatic control logic, the programmatic control logic including a target vehicle imitating (TVI) application comprising:
a first control logic for estimating a target vehicle state, and for estimating a target vehicle trajectory, the ego vehicle condition information and the target vehicle condition information partially defining the target vehicle state and the target vehicle trajectory;
a second control logic for evaluating target vehicle safety and performance constraints;
a third control logic that selectively initiates an imitation mode of the ego vehicle based on a status of the target vehicle and the ego vehicle relative to the target vehicle safety and performance constraints, including:
control logic that optimizes a planned ego vehicle path by minimizing a value of a cost function of a model predictive control (MPC) algorithm and constraining commands to the one or more actuators of the ego vehicle's vehicle dynamics system to be within predefined actuator constraints, wherein minimizing the value of the cost function further comprises:
control logic that minimizes a value of a target vehicle tracking error, wherein the target vehicle tracking error comprises: a longitudinal motion error, a lateral motion error, and a yaw motion error, wherein each of the longitudinal, lateral, and yaw motion errors defines a difference between a predicted target vehicle motion and an actual target vehicle motion relative to the ego vehicle, wherein:
a lateral distance error ey, is defined as:
ėy=vy+vxψ−vxψd=vy+vxeψ;
a heading orientation error eψ is defined by:
eψ=ψ−ψd; ėψ=r−ψd;
ψd=vx/R=ρvx; and the cost function is defined as:
J(x(t),Ut)=Σk=1Np∥y1t+k,t∥Q2+Σk=1Np∥y2t+k,t−y2dt+k,t∥S2+Σk=0Np−1∥ut+k,t∥R2+Σk=0Np−1∥Δut+k,t∥T2, where vx is a velocity of the ego vehicle in a X-direction, vy is a velocity of the ego vehicle in a Y-direction, n is an nth time step, uk|t∈U k=t+1 . . . t+Nc−1, defines control input constraints, y2 and y2d denote predicted states of the ego vehicle and desired states from the target vehicle, respectively; ut denotes control actions for the vehicle dynamics system actuators, Δu denotes a change rate for control actions for the vehicle dynamic system actuators, Qt, St, Rt and Tt are weights for path tracking errors, target vehicle 12′ behavior tracking error, vehicle dynamics system actuator control actions and change rates of vehicle dynamics system actuator control actions, respectively;
a fourth control logic within the imitation mode, that engages the MPC algorithm that models the target vehicle and optimizes a planned ego vehicle path subject to actuator constraints; and
a fifth control logic that generates outputs to the one or more actuators to cause the ego vehicle to follow the planned ego vehicle path and imitate driving behavior of the target vehicle.
|