	US 12,227,203 B2
	Roadmodel manifold for 2D trajectory planner
Maximilian Doemling, Landshut (DE); Qianshan Li, Shanghai (CN); and Wenxin Tian, Shanghai (CN)
Assigned to Bayerische Motoren Werke Aktiengesellschaft, Munich (DE)
Appl. No. 17/596,528
Filed by Bayerische Motoren Werke Aktiengesellschaft, Munich (DE)
PCT Filed Jun. 14, 2019, PCT No. PCT/CN2019/091214 § 371(c)(1), (2) Date Dec. 13, 2021, PCT Pub. No. WO2020/248210, PCT Pub. Date Dec. 17, 2020.
Prior Publication US 2022/0242444 A1, Aug. 4, 2022
Int. Cl. B60W 60/00 (2020.01); G01C 21/34 (2006.01)

CPC B60W 60/001 (2020.02) [G01C 21/34 (2013.01); B60W 2510/18 (2013.01); B60W 2510/20 (2013.01); B60W 2520/10 (2013.01); B60W 2520/14 (2013.01); B60W 2520/16 (2013.01); B60W 2520/18 (2013.01); B60W 2552/15 (2020.02)]

9 Claims

1. A computer-implemented method for planning a trajectory for a vehicle, the method comprising:

obtaining a pre-defined roadmodel 2-dimensional (2D) manifold in 6D space, wherein the roadmodel 2D manifold in 6D space represents objects in a roadmodel in 6D (x, y, z, R, P, Y) space, wherein x and y respectively represent positions of objects on a horizontal plane, and z, R, P and Y respectively represent a height, roll angle, pitch angle and yaw angle of the objects in a real world;

obtaining a current position (x₀, y₀, z₀) of the vehicle and a target position of the vehicle on the horizontal plane (x₁, y₁);

obtaining a piece of the roadmodel 2D manifold in 6D space covering the current position and the target position;

flattening a piece of the roadmodel represented by the piece of the roadmodel 2D manifold in 6D space onto the horizontal plane while maintaining a distance between any two points from 3D space to the horizontal plane;

planning a trajectory for the vehicle from the current position to the target position in the flattened roadmodel on the horizontal plane; and

planning a velocity, throttle, brake and steering of the vehicle based on the planned trajectory;

wherein planning the trajectory for the vehicle from the current position to the target position in the flattened roadmodel on the horizontal plane comprises:

planning the trajectory for the vehicle from the current position to the target position based on the flattened roadmodel as if the roadmodel were originally on the horizontal plane; and

planning a delta odometry (Δx, Δy, θ) for each of a plurality of sections of the planned trajectory;

wherein planning the velocity, throttle, brake and steering of the vehicle based on the planned trajectory comprises:

converting the delta odometry (Δx, Δy, θ) for each of the plurality of sections of the planned trajectory into the 6D space; and

planning the velocity, throttle, brake and steering of the vehicle for each section by taking the height, roll angle, pitch angle and yaw angle information into consideration; and

wherein the vehicle comprises an odometer for providing odometry information, and wherein the odometry information provided by the odometer is comparable to the delta odometry (Δx, Δy, θ) for each section to determine whether the vehicle deviates from the planned trajectory.