SYSTEMS AND METHODS FOR USING R-FUNCTIONS AND SEMI-ANALYTIC GEOMETRY FOR LANE KEEPING IN TRAJECTORY PLANNING

System, methods, and other embodiments described herein relate to lane keeping in a vehicle. In one embodiment, a method includes determining lane boundaries according to at least the sensor data and a map. The method includes defining a reference system over a lane defined by the lane boundaries. The method includes evaluating vehicle boundary points within the reference system as a cost in optimizing a trajectory of the vehicle and using an R-function that defines geometric relationships between the vehicle boundary points and the reference system. The method includes providing an indicator about the trajectory to control the vehicle.