Vehicle maneuvering

[0001] A vehicle can be equipped with electronic and electro-mechanical components, e.g., computing devices, networks, sensors, and controllers. A vehicle computer can acquire data regarding the vehicle's environment and can operate the vehicle or at least some components thereof based on the acquired data. Vehicle sensors can provide data concerning routes to be traveled and objects to be maneuvered around in the vehicle's environment. Operation of the vehicle can include acquiring accurate and timely data regarding objects in a vehicle's environment while the vehicle is being operated.

[0007] The disclosure describes techniques for a host vehicle to perform a maneuver that involves coordinating with one or more target vehicles. Examples of maneuvers include merging or changing into a lane in which the target vehicles are traveling. The techniques may be used to select whether to enter the lane in front of or behind a target vehicle and control the speed of the host vehicle accordingly. A computer of the host vehicle is programmed to receive a command for the host vehicle to perform a maneuver, detect a target vehicle, minimize a cost function, determine a host control input for the host vehicle based on the minimization of the cost function, and actuate the host vehicle to perform the maneuver according to the host control input. For example, the host control input may be an acceleration or deceleration of the host vehicle. The cost function includes a host cost and a target cost. The host cost is a cost associated with a host kinematic state of the host vehicle, and the target cost is a cost associated with a target kinematic state of the target vehicle. The cost function may include greater costs associated with greater acceleration or deceleration of the host vehicle and target vehicle, thereby encouraging minimization of the acceleration or deceleration of the vehicles, as well as other factors.

[0008] The computer does not actuate the target vehicle, only the host vehicle, and may not even be in communication with the target vehicle. But by including the target cost in the cost function along with the host cost, more efficient coordination of the maneuver between the host vehicle and the target vehicle may be achieved. For example, the techniques herein may be able to mitigate “soft gridlock,” a situation in which the host vehicle and target vehicle both attempt to give the other one the right of way. The inventors have run simulations of a scenario in which the host vehicle merges into a lane in which the target vehicle is traveling, with the acceleration of the host vehicle controlled according to the techniques described herein, i.e., with a cost function including the host cost and the target cost, as well as with the acceleration of the host vehicle controlled according to a similar cost function without the target cost. When the cost function does not include the target cost, the simulation found that a range of initial positions of the target vehicle that result in soft gridlock is approximately 15 meters long. When the cost function includes the target cost, the simulation found that the range of initial positions of the target vehicle that result in soft gridlock is only 2.75 centimeters.

[0009] A computer includes a processor and a memory, and the memory stores instructions executable by the processor to receive a command for a host vehicle to perform a maneuver, detect a target vehicle, minimize a cost function, determine a host control input for the host vehicle based on the minimization of the cost function, and actuate the host vehicle to perform the maneuver according to the host control input. The cost function includes a host cost and a target cost. The host cost is a cost associated with a host kinematic state of the host vehicle, and the target cost is a cost associated with a target kinematic state of the target vehicle.

[0010] In an example, the host control input may include a host acceleration of the host vehicle.

[0011] In an example, the instructions may further include instructions to determine a target control input for the target vehicle based on the minimization of the cost function. In a further example, the instructions may not include instructions to actuate the target vehicle.

[0012] In another further example, the instructions may further include instructions to execute a feedback loop, the feedback loop including the minimization of the cost function. In a yet further example, the feedback loop may include a determination of a difference between the target control input and a target observed behavior of the target vehicle. In a still yet further example, the target control input may include a target predicted acceleration of the target vehicle, and the target observed behavior may include a target observed acceleration of the target vehicle.

[0013] In an example, the maneuver may be the host vehicle entering a lane. In a further example, the target vehicle may be in the lane.

[0014] In another further example, the instructions may further include instructions to select between the host vehicle entering the lane in front of the target vehicle and the host vehicle entering the lane behind the target vehicle by minimizing the cost function.

[0015] In an example, the cost function may include a summation of the host cost through a completion of the maneuver and a summation of the target cost through the completion of the maneuver.

[0016] In an example, the host cost may include a host acceleration cost associated with an acceleration of the host vehicle. In a further example, the target cost may include a target acceleration cost associated with an acceleration of the target vehicle.

[0017] In an example, the host cost may include a speed cost associated with a speed of the host vehicle.

[0018] In an example, the host cost may include a jerk cost associated with a change in acceleration of the host vehicle.

[0019] In an example, the instructions to minimize the cost function may include instructions to minimize the cost function subject to a constraint to prevent intersecting a virtual barrier of the target vehicle.

[0020] In an example, the instructions to minimize the cost function may include instructions to perform mixed-integer quadratic programming.

[0021] In an example, the target vehicle may be a first target vehicle, the target cost may be a first target cost, the target kinematic state may be a first target kinematic state, the instructions may include instructions to detect a second target vehicle, the cost function may include a second target cost, and the second target cost may be associated with a second target kinematic state of the second target vehicle. In a further example, the maneuver may be the host vehicle entering a lane, the first target vehicle and the second target vehicle may be in the lane, the first target vehicle may be ahead of the second target vehicle, and the instructions may further include instructions to select among (1) the host vehicle entering the lane in front of the first target vehicle, (2) the host vehicle entering the lane behind the first target vehicle and ahead of the second target vehicle, and (3) the host vehicle entering the lane behind the second target vehicle, by minimizing the cost function.

[0022] A method includes receiving a command for a host vehicle to perform a maneuver, detecting a target vehicle, minimizing a cost function, determining a host control input for the host vehicle based on the minimization of the cost function, and actuating the host vehicle to perform the maneuver according to the host control input. The cost function includes a host cost and a target cost. The host cost is a cost associated with a host kinematic state of the host vehicle, and the target cost is a cost associated with a target kinematic state of the target vehicle.

[0023] With reference to the Figures, wherein like numerals indicate like parts throughout the several views, a computer 105 includes a processor and a memory, and the memory stores instructions executable by the processor to receive a command for a host vehicle 100 to perform a maneuver, detect a target vehicle 200, minimize a cost function, determine a host control input for the host vehicle 100 based on the minimization of the cost function, and actuate the host vehicle 100 to perform the maneuver according to the host control input. The cost function includes a host cost and a target cost. The host cost is a cost associated with a host kinematic state of the host vehicle 100, and the target cost is a cost associated with a target kinematic state of the target vehicle 200.

[0024] With reference to FIG. 1, the host vehicle 100 may be any passenger or commercial automobile such as a car, a truck, a sport utility vehicle, a crossover, a van, a minivan, a taxi, a bus, etc.

[0025] The computer 105 is a microprocessor-based computing device, e.g., a generic computing device including a processor and a memory, an electronic controller or the like, a field-programmable gate array (FPGA), an application-specific integrated circuit (ASIC), a combination of the foregoing, etc. Typically, a hardware description language such as VHDL (VHSIC (Very High Speed Integrated Circuit) Hardware Description Language) is used in electronic design automation to describe digital and mixed-signal systems such as FPGA and ASIC. For example, an ASIC is manufactured based on VHDL programming provided pre-manufacturing, whereas logical components inside an FPGA may be configured based on VHDL programming, e.g., stored in a memory electrically connected to the FPGA circuit. The computer 105 can thus include a processor, a memory, etc. The memory of the computer 105 can include media for storing instructions executable by the processor as well as for electronically storing data and/or databases, and/or the computer 105 can include structures such as the foregoing by which programming is provided. The computer 105 can be multiple computers coupled together.

[0026] The computer 105 may transmit and receive data through a communications network 110 such as a controller area network (CAN) bus, Ethernet, WiFi, Local Interconnect Network (LIN), onboard diagnostics connector (OBD-II), and/or by any other wired or wireless communications network. The computer 105 may be communicatively coupled to sensors 115, a propulsion system 120, a brake system 125, a steering system 130, a user interface 135, a transceiver 140, and other components via the communications network 110.

[0027] The sensors 115 may provide data about operation of the vehicle, for example, wheel speed, wheel orientation, and engine and transmission data (e.g., temperature, fuel consumption, etc.). The sensors 115 may detect the location and/or orientation of the host vehicle 100. For example, the sensors 115 may include global positioning system (GPS) sensors; accelerometers such as piezo-electric or microelectromechanical systems (MEMS); gyroscopes such as rate, ring laser, or fiber-optic gyroscopes; inertial measurements units (IMU); and magnetometers. The sensors 115 may detect the external world, e.g., objects and/or characteristics of surroundings of the host vehicle 100, such as the target vehicles 200, road lane markings, traffic lights and/or signs, pedestrians, etc. For example, the sensors 115 may include radar sensors, ultrasonic sensors, scanning laser range finders, light detection and ranging (lidar) devices, and image processing sensors such as cameras.

[0028] The propulsion system 120 of the vehicle generates energy and translates the energy into motion of the host vehicle 100. The propulsion system 120 may be a conventional vehicle propulsion subsystem, for example, a conventional powertrain including an internal-combustion engine coupled to a transmission that transfers rotational motion to wheels; an electric powertrain including batteries, an electric motor, and a transmission that transfers rotational motion to the wheels; a hybrid powertrain including elements of the conventional powertrain and the electric powertrain; or any other type of propulsion. The propulsion system 120 can include an electronic control unit (ECU) or the like that is in communication with and receives input from the computer 105 and/or a human operator. The human operator may control the propulsion system 120 via, e.g., an accelerator pedal and/or a gear-shift lever.

[0029] The brake system 125 is typically a conventional vehicle braking subsystem and resists the motion of the host vehicle 100 to thereby slow and/or stop the host vehicle 100. The brake system 125 may include friction brakes such as disc brakes, drum brakes, band brakes, etc.; regenerative brakes; any other suitable type of brakes; or a combination. The brake system 125 can include an electronic control unit (ECU) or the like that is in communication with and receives input from the computer 105 and/or a human operator. The human operator may control the brake system 125 via, e.g., a brake pedal.

[0030] The steering system 130 is typically a conventional vehicle steering subsystem and controls the turning of the wheels. The steering system 130 may be a rack-and-pinion system with electric power-assisted steering, a steer-by-wire system, as both are known, or any other suitable system. The steering system 130 can include an electronic control unit (ECU) or the like that is in communication with and receives input from the computer 105 and/or a human operator. The human operator may control the steering system 130 via, e.g., a steering wheel.

[0031] The user interface 135 presents information to and receives information from an operator of the host vehicle 100. The user interface 135 may be located, e.g., on an instrument panel in a passenger cabin of the host vehicle 100, or wherever may be readily seen by the operator. The user interface 135 may include dials, digital readouts, screens, speakers, and so on for providing information to the operator, e.g., human-machine interface (HMI) elements such as are known. The user interface 135 may include buttons, knobs, keypads, microphone, and so on for receiving information from the operator.

[0032] The transceiver 140 may be adapted to transmit signals wirelessly through any suitable wireless communication protocol, such as cellular, Bluetooth®, Bluetooth® Low Energy (BLE), ultra-wideband (UWB), WiFi, IEEE 802.11a/b/g/p, cellular-V2X (CV2X), Dedicated Short-Range Communications (DSRC), other RF (radio frequency) communications, etc. The transceiver 140 may be adapted to communicate with a remote server, that is, a server distinct and spaced from the host vehicle 100. The remote server may be located outside the host vehicle 100. For example, the remote server may be associated with one of the target vehicles 200 (e.g., V2V communications), an infrastructure component (e.g., V2I communications), an emergency responder, a mobile device associated with the owner of the host vehicle 100, etc. The transceiver 140 may be one device or may include a separate transmitter and receiver.

[0033] With reference to FIGS. 2 and 3, the computer 105 is programmed to receive a command for the host vehicle 100 to perform a maneuver. For example, the operator may provide an input via the user interface 135 instructing the computer 105 to perform the maneuver autonomously or semi-autonomously. For another example, the computer 105 may determine to perform the maneuver by executing an autonomous-operation algorithm. The maneuver may involve turn-taking between the host vehicle 100 and one or more of the target vehicles 200. For example, the maneuver may be for the host vehicle 100 to enter a target lane 210 in which the target vehicles 200 are traveling, as shown in FIGS. 2 and 3.

[0034] With reference to FIG. 2, the maneuver may be for the host vehicle 100 to enter the target lane 210, e.g., to merge from an onramp 205 into the target lane 210 of a limited-access road 215. One or more target vehicles 200 may be in the target lane 210, e.g., a first target vehicle 200a ahead of a second target vehicle 200b. To complete the maneuver, the vehicle will do one of the following: (1) merge into the target lane 210 in front of the target vehicle(s) 200, (2) merge into the target lane 210 behind the target vehicle(s) 200, or (3) if applicable, merge into the target lane 210 in a gap between consecutive target vehicles 200, e.g., behind the first target vehicle 200a and in front of the second target vehicle 200b.

[0035] With reference to FIG. 3, the maneuver may be for the host vehicle 100 to enter the target lane 210, e.g., to change from an originating lane 220 to a target lane 210 of a multiple-lane road 225. One or more target vehicles 200 may be in the target lane 210, e.g., the first target vehicle 200a ahead of the second target vehicle 200b. To complete the maneuver, the vehicle will do one of the following: (1) change into the target lane 210 in front of the target vehicle(s) 200, (2) change into the target lane 210 behind the target vehicle(s) 200, or (3) if applicable, change into the target lane 210 in a gap between consecutive target vehicles 200, e.g., behind the first target vehicle 200a and in front of the second target vehicle 200b.

[0036] The computer 105 is programmed to detect the target vehicles 200, e.g., based on data from the sensors 115. For example, the computer 105 may receive data from radar, ultrasonic sensors, and/or lidar indicating objects in the environment, and the computer 105 may classify the objects as target vehicles 200 based on speed and size, e.g., length. Alternatively or additionally, the computer 105 may receive data from a camera, and the computer 105 may classify the target vehicles 200 as target vehicles 200 using conventional object-recognition techniques, e.g., a convolutional neural network programmed to accept images as input and output an identified type of object.

[0037] The computer 105 is programmed to identify alternative final ordinal positions of the host vehicle 100 relative to the target vehicles 200, e.g., at least two final ordinal positions. A final ordinal position is a position of the host vehicle 100 at the completion of the maneuver relative to the target vehicles 200 within an ordering of the host vehicle 100 and the target vehicles 200. The final ordinal position is “final” in that it is a position of the host vehicle 100 at the completion of the maneuver. The final ordinal position is “ordinal” in that it is a position within an ordering of the host vehicle 100 and the target vehicles 200, i.e., a position indicating whether the host vehicle 100 is ahead or behind one of the target vehicles 200 but not a distance between the host vehicle 100 and the target vehicle 200. For example, if one target vehicle 200 is in the target lane 210, the final ordinal positions are in front of the target vehicle 200 and behind the target vehicle 200. For another example, if two target vehicles 200 are in the target lane 210 as shown in FIGS. 2 and 3, the final ordinal positions are (1) in front of the first target vehicle 200a, (2) behind the first target vehicle 200a and ahead of the second target vehicle 200b, and (3) behind the second target vehicle 200b.

[0038] The computer 105 is programmed to minimize a cost function, as will be described below, subject to a plurality of constraints. A first constraint is selecting exactly one final ordinal position. The selection of the final ordinal position may be represented by a set of binary variables b. Each binary variable in the set may represent one of the final ordinal positions and may be zero when that final ordinal position is selected and one otherwise. The constraint may thus be that the sum of the binary variables equals one less than the number of binary variables, i.e., one less than the number of alternative final ordinal positions. For example, in a situation with two target vehicles 200, there are three alternative final ordinal positions, as described above, and the first constraint may be as follows:

[0039] $b_{1+}+b_{1-2}+b_{2-}=2$
in which the subscript 1+ indicates the final ordinal position in front of the first target vehicle 200a, the subscript 1-2 indicates the final ordinal position between the first target vehicle 200a and the second target vehicle 200b, and the subscript 2-indicates the final ordinal position behind the second target vehicle 200b. Continuing this example, if the final ordinal position is between the first target vehicle 200a and the second target vehicle 200b, then the binary variable b_1-2 equals zero, and the binary variables b₁₊ and b₂₋ equal one, thus satisfying the first constraint.

[0040] The computer 105 is programmed to determine a host kinematic state of the host vehicle 100 and target kinematic states of the target vehicles 200. For the purposes of this disclosure, a “kinematic state” is defined as a mathematical description of the position and/or motion of an entity. The kinematic states can include some combination of a position, a heading, a velocity vector, a scalar speed, and/or a yaw rate. For example, the kinematic states may include position, heading, and scalar speed. The computer 105 may determine the host kinematic state based on data from the sensors 115, e.g., from a GPS sensor, wheel speed sensors, IMUs, etc. The computer 105 may determine the target kinematic states based on data from the sensors 115, e.g., positions and velocities of the target vehicles 200 over time from radar, ultrasonics sensors, and/or lidar.

[0041] The techniques below determine a host control input and one or more target control inputs as a result of minimizing the cost function. For the purposes of this disclosure, a “control input” is defined as one or more values that control operation of a component of a vehicle. For example, the components of the host vehicle 100 may include the propulsion system 120 and the brake system 125, and the host control input may include a host acceleration of the host vehicle 100. Actuating the propulsion system 120 may include, when the host acceleration is positive, setting the throttle to the host acceleration. Actuating the brake system 125 may include, when the host acceleration is negative, engaging the brake system 125 with a brake force resulting in a negative acceleration equal to the host acceleration. Similarly, the target control inputs may include respective target accelerations of the respective target vehicles 200. The control inputs may further include, e.g., speed of the respective vehicle, steering angle, etc. As described below, the host control input is used for actuating the host vehicle 100, but the target control inputs are not used for actuating the target vehicles 200.

[0042] A second set of constraints may be that values of the control inputs are within predetermined ranges. The predetermined ranges may be chosen based on the physical capabilities of the host vehicle 100, e.g., the propulsion system 120, the brake system 125, and/or the steering system 130, as well as on the comfort of the occupants of the host vehicle 100. For example, the second set of constraint may include that the host acceleration is within a predetermined acceleration range, the target accelerations are within the predetermined acceleration range, a host jerk of the host vehicle 100 is within a predetermined jerk range, and target jerks of the target vehicles 200 are within the predetermined jerk range. Jerk is a rate of change of acceleration. The upper value of the predetermined acceleration range may be chosen based on the capabilities of the propulsion system 120, the lower value of the predetermined acceleration range may be chosen based on the capabilities of the brake system 125, and the predetermined jerk range may be chosen according to the comfort of the occupants. For example, in the situation with two target vehicles 200, the second set of constraint may include the following formulas:

[0043] $a_{\min }\le a_H\left(i\right)\le a_{\max }$ $a_{\min }\le a_{T1}\left(i\right)\le a_{\max }$ $a_{\min }\le a_{T2}\left(i\right)\le a_{\max }$ $\Delta a_{\min }\le \Delta a_H\left(i\right)\le \Delta a_{\max }$ $\Delta a_{\min }\le \Delta a_{T1}\left(i\right)\le \Delta a_{\max }$ $\Delta a_{\min }\le \Delta a_{T2}\left(i\right)\le \Delta a_{\max }$
in which i is an index of time steps.

[0044] Because the computer 105 does not actuate the target vehicle 200, the target control input can deviate from a target observed behavior of the target vehicle 200. The target control input is determined along with the host control input by minimizing the cost function as described below. The target observed behavior is the target kinematic state as indicated by the data from the sensors 115, as described above. The computer 105 is programmed to determine a difference between the target control input and the target observed behavior. For example, the target control input may include a target predicted acceleration of the target vehicle 200, the target observed behavior may include a target observed acceleration of the target vehicle 200, and the computer 105 may determine a difference between the target observed acceleration and the target predicted acceleration, as given in the following equation:

[0045] ${\hat{w}}_T=a_T^{meas}-a_T$
in which the superscript meas indicates an observed value. In the situation of multiple target vehicles 200, the difference may be determined for each target vehicle 200.

[0046] The computer 105 is programmed to determine virtual barriers 230, 235 for the target vehicles 200, e.g., a front virtual barrier 230 for each target vehicle 200 and a rear virtual barrier 235 for each target vehicle 200. The virtual barriers 230, 235 may be chosen to be spaced from the respective target vehicles 200 such that the host vehicle 100 should not approach closer to the target vehicles 200 than the virtual barriers 230, 235 to maintain distance from the target vehicles 200. The virtual barriers 230, 235 may be, e.g., control barrier functions. The virtual barriers 230, 235 may be defined to provide a predetermined minimum distance between the host vehicle 100 and the respective target vehicle 200. The virtual boundary may be defined in reference frame relative to the host vehicle 100 as a difference between a distance from the host vehicle 100 to the target vehicle 200 and the predetermined minimum distance plus possibly a distance headway of the target vehicle 200. The distance headway may be a product of a time headway and a speed of the target vehicle 200. The time headway is an estimated time for the host vehicle 100 to reach the target vehicle 200 in the longitudinal direction, which may be set at a predetermined numerical value based on typical speeds for the maneuver. For example, the front virtual barrier 230 may be given by the following equation:

[0047] $h_f=x_T-d_m-k_v{v}_{\mathrm{Tf}}$ ${\dot{v}}_{\mathrm{Tf}}=\omega \left(v_T-v_{\mathrm{Tf}}\right)$
in which h_f is the front virtual barrier 230, x_T is a longitudinal distance between a reference point on the host vehicle 100 and a reference point on the target vehicle 200, d_m is the predetermined minimum distance, k_v is the time headway, v_T is the speed of the target vehicle 200, v_Tf is a filtered speed of the target vehicle 200, and ω is the reciprocal of the time constant T of the filter, i.e., ω=1/T. The speed and acceleration of the front virtual barrier 230 may be given by the following equations:

[0048] ${\dot{h}}_f=v_H-v_T-\omega k_v\left(v_T-v_{Tf}\right)$ ${\ddot{h}}_f=a_H-\left(1+\omega k_v\right)a_T+{\omega }^2{k}_v\left(v_T-v_{Tf}\right)-\left(1+\omega k_v\right){\hat{w}}_T$
in which v_H is the speed of the host vehicle 100 and a_H is the acceleration of the host vehicle 100. The rear virtual barrier 235 may be similarly defined in the following equations:

[0049] $h_f=-x_T-d_m-k_v{v}_{Hf}$ ${\dot{v}}_{Hf}=\omega \left(v_H-v_{Hf}\right)$ ${\dot{h}}_r=v_T-v_H-\omega k_v\left(v_H-v_{Hf}\right)$ ${\ddot{h}}_r=a_T-\left(1+\omega k_v\right)a_H+{\omega }^2{k}_v\left(v_H-v_{Hf}\right)+{\hat{w}}_T$
in which v_Hf is a filtered speed of the host vehicle 100.

[0050] A third set of constraints on minimizing the cost function may be to prevent intersecting the virtual barriers 230, 235 of the target vehicles 200, i.e., to prevent the host vehicle 100 from moving closer to the target vehicles 200 than the virtual barriers 230, 235. The third set of constraints may include a constraint for each front virtual barrier 230 and each rear virtual barrier 235. For example, in a situation with two target vehicles 200, the third set of constraint may include four constraints. Each constraint may include a term for the acceleration of the virtual barrier 230, 235, a term for the speed of the virtual barrier 230, 235, a term for the position of the virtual barrier 230, 235, a slack variable, and a term for the selection of the final ordinal position; e.g., the constraint may be a sum of the foregoing terms. The acceleration term may depend on the host acceleration, the respective target acceleration, and the difference between the target observed acceleration and the target predicted acceleration. A slack variable is a variable summed to an inequality constraint to turn the constraint into an equality, and whether the constraint is satisfied is indicated by whether the slack variable is positive or negative. The term for the final ordinal position may be a product of the binary variable b implicated by that constraint and a large value M. The large value M is chosen so that that constraint is satisfied when the binary variable b is one. Thus, different constraints in the third set are actually constraining depending on the selection of the final ordinal position. For the example with two target vehicles 200, the third set of constraints may be as follows:

[0051] ${\ddot{h}}_{T1f}\left(a_H\left(i\right),a_{T1}\left(i\right),{\hat{w}}_{T1}\left(i\right)\right)+l_1{\dot{h}}_{T1f}\left(i\right)+l_0{h}_{T1f}\left(i\right)+s_{T1f}\left(i\right)+M{b}_{1+}\ge 0$ ${\ddot{h}}_{T1r}\left(a_H\left(i\right),a_{T1}\left(i\right),{\hat{w}}_{T1}\left(i\right)\right)+l_1{\dot{h}}_{T1r}\left(i\right)+l_0{h}_{T1r}\left(i\right)+s_{T1r}\left(i\right)+M{b}_{1-2}\ge 0$ ${\ddot{h}}_{T2f}\left(a_H\left(i\right),a_{T2}\left(i\right),{\hat{w}}_{T2}\left(i\right)\right)+l_1{\dot{h}}_{T2f}\left(i\right)+l_0{h}_{T2f}\left(i\right)+s_{T2f}\left(i\right)+M{b}_{1-2}\ge 0$ ${\ddot{h}}_{T2r}\left(a_H\left(i\right),a_{T2}\left(i\right),{\hat{w}}_{T2}\left(i\right)\right)+l_1{\dot{h}}_{T2r}\left(i\right)+l_0{h}_{T2r}\left(i\right)+s_{T2r}\left(i\right)+M{b}_{2-}\ge 0$
in which the subscript H indicates the host vehicle 100, the subscript T1 indicates the first target vehicle 200a, the subscript T2 indicates the second target vehicle 200b, the subscript f indicates the front virtual barrier 230 of the respective target vehicle 200, ŵ is the difference between the target control input and the target observed behavior of the respective target vehicle 200, l₀ and l₁ are decay constants, and s indicates a slack variable.

[0052] The computer 105 is programmed to minimize the cost function J. e.g., by determining a selection of the final ordinal position, the host control input, and/or the target control input(s) that minimize the cost function J, subject to the constraints described above. The output of the minimization may include values for the host control input, e.g., the host acceleration; the target control input for each target vehicle 200, e.g., the target accelerations; and the selection of the final ordinal position of the host vehicle 100. For situations with one target vehicle 200, the selection of the final ordinal position may be one of (1) the host vehicle 100 entering the target lane 210 in front of the target vehicle 200 and (2) the host vehicle 100 entering the target lane 210 behind the target vehicle 200. For situations with two target vehicles 200, the selection of the final ordinal position may be one of (1) the host vehicle 100 entering the target lane 210 in front of the first target vehicle 200a, (2) the host vehicle 100 entering the target lane 210 behind the first target vehicle 200a and ahead of the second target vehicle 200b, and (3) the host vehicle 100 entering the target lane 210 behind the second target vehicle 200b. The selection of the final ordinal position may be represented in the cost function J by the binary variables b, as described above. The minimization may be represented by the following formula:

[0053] $\underset{a_H,a_{T1},a_{T2},s_{T1f},s_{T1r},s_{T2f},s_{T2r},b_{1+},b_{1-2},b_2-}{\min }J$

[0054] The cost function J includes the host cost J_H and the target cost J_T for each target vehicle 200. For example, the cost function J may include a sum of the host cost J_H and the target costs J_T. e.g., for a situation with two target vehicles 200, a sum of the host cost J_H, a first target cost J_T1, and a second target cost J_T2. The cost function J may further include a cost associated with the slack variables s. The cost associated with the slack variables may include the slack variables from the third set of constraints, e.g., a sum of the squares of the slack variables, summed over the time steps through the completion of the maneuver, weighted by a slack variable weight. For example, the following equation may give the cost function J for a situation with two target vehicles 200:

[0055] $J=J_H+J_{T1}+J_{T2}+H_s\sum _{i=0}^{N-1}\left(s_{T1f}^2\left(i\right)+s_{T1r}^2\left(i\right)+s_{T2f}^2\left(i\right)+s_{T2r}^2\left(i\right)\right)$
in which H_s is the slack variable weight, i is an index of time steps, and N is a prediction horizon measured in a number of time steps. The time horizon N may be a constant value, i.e., the computer 105 calculates the cost function J for the time horizon N from the current time step i. The time horizon N may be chosen to encompass the likely completion of the maneuver.

[0056] The host cost is a cost associated with the host kinematic state of the host vehicle 100. The host cost may include a host acceleration cost associated with an acceleration of the host vehicle 100, a host speed cost associated with a speed of the host vehicle 100, and/or a host jerk cost associated with a jerk of the host vehicle 100. The host cost may include a sum of the host acceleration cost, host speed cost, and/or host jerk cost. The host acceleration cost may be a product of an acceleration weight and an acceleration term, e.g., a square of the host acceleration. The host speed cost may be a product of a speed weight and a speed term, e.g., a square of the speed. The host jerk cost may be a product of a jerk weight and a jerk term, e.g., a square of the jerk. The weights may be chosen to make the terms commensurable for summing. The host cost may be a summation through a completion of the maneuver, e.g., as the values for the acceleration, speed, and jerk of the host vehicle 100 change over the time steps i. For example, the host cost may be given by the following formula:

[0057] $J_H=\sum _{i=1}^{N-1}\left(W_v{v_H\left(i\right)-v_d}^2+W_a{a_H\left(i\right)}^2+W_{\Delta a}{\Delta a_H\left(i\right)}^2\right)$
in which W_v is the speed weight, v_d is a recommended speed for the maneuver, W_a is the acceleration weight, and W_Δa is the jerk weight. The recommended speed v_d may be a value stored in memory associated with the maneuver or may be included in the command to perform the maneuver.

[0058] The target cost is a cost associated with the target kinematic state of each target vehicle 200. The target cost may include a target acceleration cost associated with an acceleration of the target vehicle 200 and/or a target jerk cost associated with a jerk of the target vehicle 200. The target cost may lack a target speed cost associated with a speed of the target vehicle 200 if the host vehicle 100 is not in communication with the target vehicle 200. Alternatively, the target cost may include a target speed cost if the host vehicle 100 receives a recommended or intended speed from the target vehicle 200, e.g., via the transceiver 140. The target cost may include a sum of the target acceleration cost and the host jerk cost, as well as the target speed cost if present. The target acceleration cost, the target jerk cost, and the target speed cost may be defined in the same manner as the respective terms for the host cost above. The weights may be the same as used for the host cost. The target cost may be a summation through a completion of the maneuver by the host vehicle 100, e.g., as the values for the acceleration, speed, and jerk of the target vehicle 200 change. For example, the target cost may be given by the following formula:

[0059] $J_T=\sum _{i=1}^{N-1}\left(W_a{a_T\left(i\right)}^2+W_{\Delta a}{\Delta a_T\left(i\right)}^2\right)$

[0060] The computer 105 is programmed to minimize the cost function J subject to the constraints using any suitable optimization algorithm. For example, the computer 105 may perform quadratic programming. Quadratic programming means solving optimization problems formulated as quadratic functions. More specifically, the computer 105 may perform mixed-integer quadratic programming (MIQP), quadratic programming in which some decision variables are discrete and some are not discrete, e.g., are continuous. For this application, the binary variables are discrete, and the control inputs and the slack variables are treated as continuous.

[0061] FIG. 4 is a block diagram illustrating an example feedback loop 400. The memory of the computer 105 stores executable instructions for executing the feedback loop 400, i.e., performing the steps of the feedback loop 400, and/or programming can be implemented in structures such as mentioned above. The feedback comes from a difference between the target control input, which is a prediction of how the target vehicle 200 will behave, and a target observed behavior of how the target vehicle 200 actually behaves. The feedback loop 400 may execute until the host vehicle 200 completes the maneuver.

[0062] In a block 405, the computer 105 determines the host control input u_H and the target control input u_T by minimizing the cost function as described above. The computer 105 receives the host kinematic state and the target kinematic state at a time step i, along with the difference ŵ_T between the target control input and the target observed behavior, e.g., between a target predicted acceleration ar and a target observed acceleration

[0063] $a_T^{meas},$
for a previous time step i−1. In the first iteration of the feedback loop 400, the computer 105 may use zero for the difference.

[0064] In a block 410, the computer 105 actuates the host vehicle 100 according to the host control input u_H, e.g., actuates at least one component of the host vehicle 100 such as the propulsion system 120 and/or the brake system 125, as described above. The actuation results in the host kinematic state used in the next time step i+1.

[0065] In a block 415, the target vehicle 200 actuates independently of the host vehicle 100, resulting in the target observed behavior. The computer 105 does not include instructions to actuate the target vehicle 200.

[0066] In a summation block 420, the computer 105 determines the difference between the target control input and the target observed behavior for the current time step i. For example, the computer 105 determines the difference ŵ_T between the target observed acceleration

[0067] $a_T^{meas}$
and the target predicted acceleration ar, as described above.

[0068] In a delay block 425, the computer 105 delays the difference from the time step i to be used in the block 405 in the time step i+1, i.e., the next iteration of the feedback loop 400. The length of the delay Δt may be the length of time of the time step i. The time steps i may correspond to computation steps, e.g., approximately 50 milliseconds.

[0069] FIG. 5 is a flowchart illustrating an example process 500 for controlling the host vehicle 100 to perform the maneuver. The memory of the computer 105 stores executable instructions for performing the steps of the process 500 and/or programming can be implemented in structures such as mentioned above. As a general overview of the process 500, the computer 105 receives the command for the host vehicle 100 to perform the maneuver, receives data from the sensors 115 detecting at least one target vehicle 200, determines the host kinematic state and target kinematic state, determines the host control input and the target control input, and actuates the host vehicle 100 to perform the maneuver according to the host control input. The computer 105 repeats these steps until the maneuver is complete.

[0070] The process 500 begins in a block 505, in which the computer 105 receives the command for a host vehicle 100 to perform the maneuver, as described above.

[0071] Next, in a block 510, the computer 105 receives data from the sensors 115 and detects the target vehicles 200, as described above.

[0072] Next, in a block 515, the computer 105 determines the host kinematic state and target kinematic states, as described above.

[0073] Next, in a block 520, the computer 105 determines the host control input and the target control inputs based on the minimization of the cost function, as described above.

[0074] Next, in a block 525, the computer 105 actuates the host vehicle 100 to perform the maneuver according to the host control input. The instructions do not include instructions to actuate the target vehicle 200.

[0075] Next, in a decision block 530, the computer 105 determines whether the maneuver is complete. For example, the computer 105 may determine whether the host vehicle 100 is within a distance threshold from a center of the target lane 210. The distance may be chosen to be a typical deviation of a vehicle from a center of a lane while traveling straight in the lane. If the host vehicle 100 is farther from the center of the target lane 210 than the distance threshold, the process 500 returns to the block 505 to continue updating the host control input for controlling the host vehicle 100 to perform the maneuver. If the host vehicle 100 is closer to the center of the target lane 210 than the distance threshold, the process 500 ends.

[0076] In general, the computing systems and/or devices described may employ any of a number of computer operating systems, including, but by no means limited to, versions and/or varieties of the Ford Sync® application, AppLink/Smart Device Link middleware, the Microsoft Automotive® operating system, the Microsoft Windows® operating system, the Unix operating system (e.g., the Solaris® operating system distributed by Oracle Corporation of Redwood Shores, California), the AIX UNIX operating system distributed by International Business Machines of Armonk, New York, the Linux operating system, the Mac OSX and iOS operating systems distributed by Apple Inc. of Cupertino, California, the BlackBerry OS distributed by Blackberry, Ltd. of Waterloo, Canada, and the Android operating system developed by Google, Inc. and the Open Handset Alliance, or the QNX® CAR Platform for Infotainment offered by QNX Software Systems. Examples of computing devices include, without limitation, an on-board vehicle computer, a computer workstation, a server, a desktop, notebook, laptop, or handheld computer, or some other computing system and/or device.

[0077] Computing devices generally include computer-executable instructions, where the instructions may be executable by one or more computing devices such as those listed above. Computer executable instructions may be compiled or interpreted from computer programs created using a variety of programming languages and/or technologies, including, without limitation, and either alone or in combination, Java™, C, C++, Matlab, Simulink, Stateflow, Visual Basic, Java Script, Python, Perl, HTML, etc. Some of these applications may be compiled and executed on a virtual machine, such as the Java Virtual Machine, the Dalvik virtual machine, or the like. In general, a processor (e.g., a microprocessor) receives instructions, e.g., from a memory, a computer readable medium, etc., and executes these instructions, thereby performing one or more processes, including one or more of the processes described herein. Such instructions and other data may be stored and transmitted using a variety of computer readable media. A file in a computing device is generally a collection of data stored on a computer readable medium, such as a storage medium, a random access memory, etc.

[0078] A computer-readable medium (also referred to as a processor-readable medium) includes any non-transitory (e.g., tangible) medium that participates in providing data (e.g., instructions) that may be read by a computer (e.g., by a processor of a computer). Such a medium may take many forms, including, but not limited to, non-volatile media and volatile media. Instructions may be transmitted by one or more transmission media, including fiber optics, wires, wireless communication, including the internals that comprise a system bus coupled to a processor of a computer. Common forms of computer-readable media include, for example, RAM, a PROM, an EPROM, a FLASH-EEPROM, any other memory chip or cartridge, or any other medium from which a computer can read.

[0079] Databases, data repositories or other data stores described herein may include various kinds of mechanisms for storing, accessing, and retrieving various kinds of data, including a hierarchical database, a set of files in a file system, an application database in a proprietary format, a relational database management system (RDBMS), a nonrelational database (NoSQL), a graph database (GDB), etc. Each such data store is generally included within a computing device employing a computer operating system such as one of those mentioned above, and are accessed via a network in any one or more of a variety of manners. A file system may be accessible from a computer operating system, and may include files stored in various formats. An RDBMS generally employs the Structured Query Language (SQL) in addition to a language for creating, storing, editing, and executing stored procedures, such as the PL/SQL language mentioned above.

[0080] In some examples, system elements may be implemented as computer-readable instructions (e.g., software) on one or more computing devices (e.g., servers, personal computers, etc.), stored on computer readable media associated therewith (e.g., disks, memories, etc.). A computer program product may comprise such instructions stored on computer readable media for carrying out the functions described herein.

[0081] In the drawings, the same reference numbers indicate the same elements. Further, some or all of these elements could be changed. With regard to the media, processes, systems, methods, heuristics, etc. described herein, it should be understood that, although the steps of such processes, etc. have been described as occurring according to a certain ordered sequence, such processes could be practiced with the described steps performed in an order other than the order described herein. It further should be understood that certain steps could be performed simultaneously, that other steps could be added, or that certain steps described herein could be omitted.

[0082] All terms used in the claims are intended to be given their plain and ordinary meanings as understood by those skilled in the art unless an explicit indication to the contrary in made herein. In particular, use of the singular articles such as “a,” “the,” “said,” etc. should be read to recite one or more of the indicated elements unless a claim recites an explicit limitation to the contrary. The adjectives “first.” “second.” and “third” are used throughout this document as identifiers and are not intended to signify importance, order, or quantity. Use of “in response to” and “upon determining” indicates a causal relationship, not merely a temporal relationship.

[0083] The disclosure has been described in an illustrative manner, and it is to be understood that the terminology which has been used is intended to be in the nature of words of description rather than of limitation. Many modifications and variations of the present disclosure are possible in light of the above teachings, and the disclosure may be practiced otherwise than as specifically described.