目录
关键词：RTK循迹、EM Planner 、Lattic Planner
关键之处：
如何去找一个成本最低的规划曲线呢？开发者面临的是一个三维空间中的优化问题，包括路面的二维平面，也包括时间维度。这是一个N立方难度的问题，Apollo 2.5的解决方法是，把这个N立方级别的问题，分拆成两个N平方级别的问题。也就是在x-y维度上求解，进行路径规划；在路径规划的基础上，以规划出来的路径为s轴，在s-t维度上进行速度规划。
Apollo 1.0中开始开放的RTK，也就是循迹Planner；Apollo 1.5中开始开放的EM Planner，也就是基于动态规划Dynamic Programming和二次规划Quadratic Programming的路径规划器与速度规划器；以及Apollo 2.5中开始开放的Lattice Planner，一种路径和速度同时规划的规划器。
RTK
先记录，然后播放来进行循迹
核心代码:
#!/usr/bin/env python
###############################################################################
# Copyright 2017 The Apollo Authors. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
###############################################################################
"""
Generate Planning Path
"""
import argparse
import atexit
import logging
import os
import sys
import rospy
import scipy.signal as signal
from logger import Logger
from numpy import genfromtxt
from modules.canbus.proto import chassis_pb2
from modules.common.proto import pnc_point_pb2
from modules.common.proto import drive_state_pb2
from modules.control.proto import pad_msg_pb2
from modules.localization.proto import localization_pb2
from modules.planning.proto import planning_pb2
APOLLO_ROOT = os.path.join(os.path.dirname(__file__), '../../../')
SEARCH_INTERVAL = 1000
class RtkPlayer(object):
"""
rtk player class
"""
def __init__(self, record_file, speedmultiplier, completepath, replan):
"""Init player."""
self.firstvalid = False
self.logger = Logger.get_logger(tag="RtkPlayer")
self.logger.info("Load record file from: %s" % record_file)
try:
file_handler = open(record_file, 'r')
except:
self.logger.error("Cannot find file: " + record_file)
file_handler.close()
sys.exit(0)
self.data = genfromtxt(file_handler, delimiter=',', names=True)
file_handler.close()
self.localization = localization_pb2.LocalizationEstimate()
self.chassis = chassis_pb2.Chassis()
self.padmsg = pad_msg_pb2.PadMessage()
self.localization_received = False
self.chassis_received = False
self.planning_pub = rospy.Publisher(
'/apollo/planning', planning_pb2.ADCTrajectory, queue_size=1)
self.speedmultiplier = speedmultiplier / 100
self.terminating = False
self.sequence_num = 0
b, a = signal.butter(6, 0.05, 'low')
self.data['acceleration'] = signal.filtfilt(b, a,
self.data['acceleration'])
self.start = 0
self.end = 0
self.closestpoint = 0
self.automode = False
self.replan = (replan == 't')
self.completepath = (completepath == 't')
self.estop = False
self.logger.info("Planning Ready")
def localization_callback(self, data):
"""
New localization Received
"""
self.localization.CopyFrom(data)
self.carx = self.localization.pose.position.x
self.cary = self.localization.pose.position.y
self.carz = self.localization.pose.position.z
self.localization_received = True
def chassis_callback(self, data):
"""
New chassis Received
"""
self.chassis.CopyFrom(data)
self.automode = (self.chassis.driving_mode ==
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE)
self.chassis_received = True
def padmsg_callback(self, data):
"""
New message received
"""
if self.terminating == True:
self.logger.info("terminating when receive padmsg")
return
self.padmsg.CopyFrom(data)
def restart(self):
self.logger.info("before replan self.start=%s, self.closestpoint=%s" %
(self.start, self.closestpoint))
self.closestpoint = self.closest_dist()
self.start = max(self.closestpoint - 100, 0)
self.starttime = rospy.get_time()
self.end = min(self.start + 1000, len(self.data) - 1)
self.logger.info("finish replan at time %s, self.closestpoint=%s" %
(self.starttime, self.closestpoint))
def closest_dist(self):
shortest_dist_sqr = float('inf')
self.logger.info("before closest self.start=%s" % (self.start))
search_start = max(self.start - SEARCH_INTERVAL / 2, 0)
search_end = min(self.start + SEARCH_INTERVAL / 2, len(self.data))
start = self.start
for i in range(search_start, search_end):
dist_sqr = (self.carx - self.data['x'][i]) ** 2 + \
(self.cary - self.data['y'][i]) ** 2
if dist_sqr <= shortest_dist_sqr:
start = i
shortest_dist_sqr = dist_sqr
return start
def closest_time(self):
time_elapsed = rospy.get_time() - self.starttime
closest_time = self.start
time_diff = self.data['time'][closest_time] - \
self.data['time'][self.closestpoint]
while time_diff < time_elapsed and closest_time < (len(self.data) - 1):
closest_time = closest_time + 1
time_diff = self.data['time'][closest_time] - \
self.data['time'][self.closestpoint]
return closest_time
def publish_planningmsg(self):
"""
Generate New Path
"""
if not self.localization_received:
self.logger.warning(
"locaization not received yet when publish_planningmsg")
return
planningdata = planning_pb2.ADCTrajectory()
now = rospy.get_time()
planningdata.header.timestamp_sec = now
planningdata.header.module_name = "planning"
planningdata.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
self.logger.debug(
"publish_planningmsg: before adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
if self.replan or self.sequence_num <= 1 or not self.automode:
self.logger.info(
"trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s"
% (self.replan, self.sequence_num, self.automode))
self.restart()
else:
timepoint = self.closest_time()
distpoint = self.closest_dist()
self.start = max(min(timepoint, distpoint) - 100, 0)
self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1)
xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
if self.completepath:
self.start = 0
self.end = len(self.data) - 1
self.logger.debug(
"publish_planningmsg: after adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
for i in range(self.start, self.end):
adc_point = pnc_point_pb2.TrajectoryPoint()
adc_point.path_point.x = self.data['x'][i]
adc_point.path_point.y = self.data['y'][i]
adc_point.path_point.z = self.data['z'][i]
adc_point.v = self.data['speed'][i] * self.speedmultiplier
adc_point.a = self.data['acceleration'][i] * self.speedmultiplier
adc_point.path_point.kappa = self.data['curvature'][i]
adc_point.path_point.dkappa = self.data['curvature_change_rate'][i]
time_diff = self.data['time'][i] - \
self.data['time'][self.closestpoint]
adc_point.relative_time = time_diff / self.speedmultiplier - (
now - self.starttime)
adc_point.path_point.theta = self.data['theta'][i]
adc_point.path_point.s = self.data['s'][i]
planningdata.trajectory_point.extend([adc_point])
planningdata.estop.is_estop = self.estop
planningdata.total_path_length = self.data['s'][self.end] - \
self.data['s'][self.start]
planningdata.total_path_time = self.data['time'][self.end] - \
self.data['time'][self.start]
planningdata.gear = int(self.data['gear'][self.closest_time()])
planningdata.engage_advice.advice = \
drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
self.planning_pub.publish(planningdata)
self.logger.debug("Generated Planning Sequence: " +
str(self.sequence_num - 1))
def shutdown(self):
"""
shutdown rosnode
"""
self.terminating = True
self.logger.info("Shutting Down...")
rospy.sleep(0.2)
def quit(self, signum, frame):
"""
shutdown the keypress thread
"""
sys.exit(0)
def main():
"""
Main rosnode
"""
parser = argparse.ArgumentParser(
description='Generate Planning Trajectory from Data File')
parser.add_argument(
'-s',
'--speedmulti',
help='Speed multiplier in percentage (Default is 100) ',
type=float,
default='100')
parser.add_argument(
'-c', '--complete', help='Generate complete path (t/F)', default='F')
parser.add_argument(
'-r',
'--replan',
help='Always replan based on current position(t/F)',
default='F')
args = vars(parser.parse_args())
rospy.init_node('rtk_player', anonymous=True)
Logger.config(
log_file=os.path.join(APOLLO_ROOT, 'data/log/rtk_player.log'),
use_stdout=True,
log_level=logging.DEBUG)
record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv')
player = RtkPlayer(record_file, args['speedmulti'],
args['complete'].lower(), args['replan'].lower())
atexit.register(player.shutdown)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
player.chassis_callback)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
player.localization_callback)
rospy.Subscriber('/apollo/control/pad', pad_msg_pb2.PadMessage,
player.padmsg_callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
player.publish_planningmsg()
rate.sleep()
if __name__ == '__main__':
main()
EM planer
EM Planner可以通过动态规划进行路径和速度规划。比如说，路径规划会在范围内进行机动撒点，离散化解空间进行求解。撒点后，通过Cost Function一层一层的进行动态规划，从而对问题进行了有效的简化。然后再通过平滑的曲线连接各层采样点得到最终的路径。
EM Planner中的速度规划，也可以采用和路径规划相似的方法。对解空间进行离散化以后，然后通过动态规划进行求解。从图中可以看到，在将与主车轨迹有重叠的障碍物映射到ST图以后，速度规划可以通过加速减速匀速等操作，从ST图中搜索到一条可通行区间并生成速度规划。从最后生成出来的图里面，可以解读出来相对于每一个障碍物的纵向决策。
核心代码
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/planning/planner/em/em_planner.h"
#include <fstream>
#include <limits>
#include <utility>
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/common/util/string_tokenizer.h"
#include "modules/common/util/string_util.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/constraint_checker/constraint_checker.h"
#include "modules/planning/tasks/dp_poly_path/dp_poly_path_optimizer.h"
#include "modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h"
#include "modules/planning/tasks/path_decider/path_decider.h"
#include "modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.h"
#include "modules/planning/tasks/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
#include "modules/planning/tasks/speed_decider/speed_decider.h"
namespace apollo {
namespace planning {
using common::ErrorCode;
using common::SLPoint;
using common::SpeedPoint;
using common::Status;
using common::TrajectoryPoint;
using common::adapter::AdapterManager;
using common::math::Vec2d;
using common::time::Clock;
namespace {
constexpr double kPathOptimizationFallbackClost = 2e4;
constexpr double kSpeedOptimizationFallbackClost = 2e4;
constexpr double kStraightForwardLineCost = 10.0;
} // namespace
void EMPlanner::RegisterTasks() {
task_factory_.Register(DP_POLY_PATH_OPTIMIZER,
[]() -> Task* { return new DpPolyPathOptimizer(); });
task_factory_.Register(PATH_DECIDER,
[]() -> Task* { return new PathDecider(); });
task_factory_.Register(DP_ST_SPEED_OPTIMIZER,
[]() -> Task* { return new DpStSpeedOptimizer(); });
task_factory_.Register(SPEED_DECIDER,
[]() -> Task* { return new SpeedDecider(); });
task_factory_.Register(QP_SPLINE_ST_SPEED_OPTIMIZER, []() -> Task* {
return new QpSplineStSpeedOptimizer();
});
task_factory_.Register(POLY_ST_SPEED_OPTIMIZER,
[]() -> Task* { return new PolyStSpeedOptimizer(); });
}
Status EMPlanner::Init(const PlanningConfig& config) {
AINFO << "In EMPlanner::Init()";
RegisterTasks();
for (const auto task : config.em_planner_config().task()) {
tasks_.emplace_back(
task_factory_.CreateObject(static_cast<TaskType>(task)));
AINFO << "Created task:" << tasks_.back()->Name();
}
for (auto& task : tasks_) {
if (!task->Init(config)) {
std::string msg(
common::util::StrCat("Init task[", task->Name(), "] failed."));
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
}
return Status::OK();
}
void EMPlanner::RecordObstacleDebugInfo(
ReferenceLineInfo* reference_line_info) {
if (!FLAGS_enable_record_debug) {
ADEBUG << "Skip record debug info";
return;
}
auto ptr_debug = reference_line_info->mutable_debug();
const auto path_decision = reference_line_info->path_decision();
for (const auto path_obstacle : path_decision->path_obstacles().Items()) {
auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
obstacle_debug->set_id(path_obstacle->Id());
obstacle_debug->mutable_sl_boundary()->CopyFrom(
path_obstacle->PerceptionSLBoundary());
const auto& decider_tags = path_obstacle->decider_tags();
const auto& decisions = path_obstacle->decisions();
if (decider_tags.size() != decisions.size()) {
AERROR << "decider_tags size: " << decider_tags.size()
<< " different from decisions size:" << decisions.size();
}
for (size_t i = 0; i < decider_tags.size(); ++i) {
auto decision_tag = obstacle_debug->add_decision_tag();
decision_tag->set_decider_tag(decider_tags[i]);
decision_tag->mutable_decision()->CopyFrom(decisions[i]);
}
}
}
void EMPlanner::RecordDebugInfo(ReferenceLineInfo* reference_line_info,
const std::string& name,
const double time_diff_ms) {
if (!FLAGS_enable_record_debug) {
ADEBUG << "Skip record debug info";
return;
}
if (reference_line_info == nullptr) {
AERROR << "Reference line info is null.";
return;
}
auto ptr_latency_stats = reference_line_info->mutable_latency_stats();
auto ptr_stats = ptr_latency_stats->add_task_stats();
ptr_stats->set_name(name);
ptr_stats->set_time_ms(time_diff_ms);
}
Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame) {
bool has_drivable_reference_line = false;
bool disable_low_priority_path = false;
auto status =
Status(ErrorCode::PLANNING_ERROR, "reference line not drivable");
for (auto& reference_line_info : frame->reference_line_info()) {
if (disable_low_priority_path) {
reference_line_info.SetDrivable(false);
}
if (!reference_line_info.IsDrivable()) {
continue;
}
auto cur_status =
PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
if (cur_status.ok() && reference_line_info.IsDrivable()) {
has_drivable_reference_line = true;
if (FLAGS_prioritize_change_lane &&
reference_line_info.IsChangeLanePath() &&
reference_line_info.Cost() < kStraightForwardLineCost) {
disable_low_priority_path = true;
}
} else {
reference_line_info.SetDrivable(false);
}
}
return has_drivable_reference_line ? Status::OK() : status;
}
Status EMPlanner::PlanOnReferenceLine(
const TrajectoryPoint& planning_start_point, Frame* frame,
ReferenceLineInfo* reference_line_info) {
if (!reference_line_info->IsChangeLanePath()) {
reference_line_info->AddCost(kStraightForwardLineCost);
}
ADEBUG << "planning start point:" << planning_start_point.DebugString();
auto* heuristic_speed_data = reference_line_info->mutable_speed_data();
auto speed_profile =
GenerateInitSpeedProfile(planning_start_point, reference_line_info);
if (speed_profile.empty()) {
speed_profile = GenerateSpeedHotStart(planning_start_point);
ADEBUG << "Using dummy hot start for speed vector";
}
heuristic_speed_data->set_speed_vector(speed_profile);
auto ret = Status::OK();
for (auto& optimizer : tasks_) {
const double start_timestamp = Clock::NowInSeconds();
ret = optimizer->Execute(frame, reference_line_info);
if (!ret.ok()) {
AERROR << "Failed to run tasks[" << optimizer->Name()
<< "], Error message: " << ret.error_message();
break;
}
const double end_timestamp = Clock::NowInSeconds();
const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
ADEBUG << "after optimizer " << optimizer->Name() << ":"
<< reference_line_info->PathSpeedDebugString() << std::endl;
ADEBUG << optimizer->Name() << " time spend: " << time_diff_ms << " ms.";
RecordDebugInfo(reference_line_info, optimizer->Name(), time_diff_ms);
}
RecordObstacleDebugInfo(reference_line_info);
if (reference_line_info->path_data().Empty()) {
ADEBUG << "Path fallback.";
GenerateFallbackPathProfile(reference_line_info,
reference_line_info->mutable_path_data());
reference_line_info->AddCost(kPathOptimizationFallbackClost);
}
if (!ret.ok() || reference_line_info->speed_data().Empty()) {
ADEBUG << "Speed fallback.";
GenerateFallbackSpeedProfile(reference_line_info,
reference_line_info->mutable_speed_data());
reference_line_info->AddCost(kSpeedOptimizationFallbackClost);
}
DiscretizedTrajectory trajectory;
if (!reference_line_info->CombinePathAndSpeedProfile(
planning_start_point.relative_time(),
planning_start_point.path_point().s(), &trajectory)) {
std::string msg("Fail to aggregate planning trajectory.");
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// determine if there is a destination on reference line.
double dest_stop_s = -1.0;
for (const auto* path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
if (path_obstacle->LongitudinalDecision().has_stop() &&
path_obstacle->LongitudinalDecision().stop().reason_code() ==
STOP_REASON_DESTINATION) {
SLPoint dest_sl = GetStopSL(path_obstacle->LongitudinalDecision().stop(),
reference_line_info->reference_line());
dest_stop_s = dest_sl.s();
}
}
for (const auto* path_obstacle :
reference_line_info->path_decision()->path_obstacles().Items()) {
if (path_obstacle->obstacle()->IsVirtual()) {
continue;
}
if (!path_obstacle->obstacle()->IsStatic()) {
continue;
}
if (path_obstacle->LongitudinalDecision().has_stop()) {
bool add_stop_obstacle_cost = false;
if (dest_stop_s < 0.0) {
add_stop_obstacle_cost = true;
} else {
SLPoint stop_sl =
GetStopSL(path_obstacle->LongitudinalDecision().stop(),
reference_line_info->reference_line());
if (stop_sl.s() < dest_stop_s) {
add_stop_obstacle_cost = true;
}
}
if (add_stop_obstacle_cost) {
constexpr double kRefrenceLineStaticObsCost = 1e3;
reference_line_info->AddCost(kRefrenceLineStaticObsCost);
}
}
}
if (FLAGS_enable_trajectory_check) {
if (!ConstraintChecker::ValidTrajectory(trajectory)) {
std::string msg("Failed to validate current planning trajectory.");
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
}
reference_line_info->SetTrajectory(trajectory);
reference_line_info->SetDrivable(true);
return Status::OK();
}
std::vector<SpeedPoint> EMPlanner::GenerateInitSpeedProfile(
const TrajectoryPoint& planning_init_point,
const ReferenceLineInfo* reference_line_info) {
std::vector<SpeedPoint> speed_profile;
const auto* last_frame = FrameHistory::instance()->Latest();
if (!last_frame) {
AWARN << "last frame is empty";
return speed_profile;
}
const ReferenceLineInfo* last_reference_line_info =
last_frame->DriveReferenceLineInfo();
if (!last_reference_line_info) {
ADEBUG << "last reference line info is empty";
return speed_profile;
}
if (!reference_line_info->IsStartFrom(*last_reference_line_info)) {
ADEBUG << "Current reference line is not started previous drived line";
return speed_profile;
}
const auto& last_speed_vector =
last_reference_line_info->speed_data().speed_vector();
if (!last_speed_vector.empty()) {
const auto& last_init_point = last_frame->PlanningStartPoint().path_point();
Vec2d last_xy_point(last_init_point.x(), last_init_point.y());
SLPoint last_sl_point;
if (!last_reference_line_info->reference_line().XYToSL(last_xy_point,
&last_sl_point)) {
AERROR << "Fail to transfer xy to sl when init speed profile";
}
Vec2d xy_point(planning_init_point.path_point().x(),
planning_init_point.path_point().y());
SLPoint sl_point;
if (!last_reference_line_info->reference_line().XYToSL(xy_point,
&sl_point)) {
AERROR << "Fail to transfer xy to sl when init speed profile";
}
double s_diff = sl_point.s() - last_sl_point.s();
double start_time = 0.0;
double start_s = 0.0;
bool is_updated_start = false;
for (const auto& speed_point : last_speed_vector) {
if (speed_point.s() < s_diff) {
continue;
}
if (!is_updated_start) {
start_time = speed_point.t();
start_s = speed_point.s();
is_updated_start = true;
}
SpeedPoint refined_speed_point;
refined_speed_point.set_s(speed_point.s() - start_s);
refined_speed_point.set_t(speed_point.t() - start_time);
refined_speed_point.set_v(speed_point.v());
refined_speed_point.set_a(speed_point.a());
refined_speed_point.set_da(speed_point.da());
speed_profile.push_back(std::move(refined_speed_point));
}
}
return speed_profile;
}
// This is a dummy simple hot start, need refine later
std::vector<SpeedPoint> EMPlanner::GenerateSpeedHotStart(
const TrajectoryPoint& planning_init_point) {
std::vector<SpeedPoint> hot_start_speed_profile;
double s = 0.0;
double t = 0.0;
double v = common::math::Clamp(planning_init_point.v(), 5.0,
FLAGS_planning_upper_speed_limit);
while (t < FLAGS_trajectory_time_length) {
SpeedPoint speed_point;
speed_point.set_s(s);
speed_point.set_t(t);
speed_point.set_v(v);
hot_start_speed_profile.push_back(std::move(speed_point));
t += FLAGS_trajectory_time_min_interval;
s += v * FLAGS_trajectory_time_min_interval;
}
return hot_start_speed_profile;
}
void EMPlanner::GenerateFallbackPathProfile(
const ReferenceLineInfo* reference_line_info, PathData* path_data) {
auto adc_point = reference_line_info->AdcPlanningPoint();
double adc_s = reference_line_info->AdcSlBoundary().end_s();
const double max_s = 150.0;
const double unit_s = 1.0;
// projection of adc point onto reference line
const auto& adc_ref_point =
reference_line_info->reference_line().GetReferencePoint(adc_s);
DCHECK(adc_point.has_path_point());
const double dx = adc_point.path_point().x() - adc_ref_point.x();
const double dy = adc_point.path_point().y() - adc_ref_point.y();
std::vector<common::PathPoint> path_points;
for (double s = adc_s; s < max_s; s += unit_s) {
const auto& ref_point =
reference_line_info->reference_line().GetReferencePoint(adc_s);
common::PathPoint path_point = common::util::MakePathPoint(
ref_point.x() + dx, ref_point.y() + dy, 0.0, ref_point.heading(),
ref_point.kappa(), ref_point.dkappa(), 0.0);
path_point.set_s(s);
path_points.push_back(std::move(path_point));
}
path_data->SetDiscretizedPath(DiscretizedPath(std::move(path_points)));
}
void EMPlanner::GenerateFallbackSpeedProfile(
const ReferenceLineInfo* reference_line_info, SpeedData* speed_data) {
*speed_data = GenerateStopProfileFromPolynomial(
reference_line_info->AdcPlanningPoint().v(),
reference_line_info->AdcPlanningPoint().a());
if (speed_data->Empty()) {
*speed_data =
GenerateStopProfile(reference_line_info->AdcPlanningPoint().v(),
reference_line_info->AdcPlanningPoint().a());
}
}
SpeedData EMPlanner::GenerateStopProfile(const double init_speed,
const double init_acc) const {
AERROR << "Slowing down the car.";
SpeedData speed_data;
const double kFixedJerk = -1.0;
const double first_point_acc = std::fmin(0.0, init_acc);
const double max_t = 3.0;
const double unit_t = 0.02;
double pre_s = 0.0;
const double t_mid =
(FLAGS_slowdown_profile_deceleration - first_point_acc) / kFixedJerk;
const double s_mid = init_speed * t_mid +
0.5 * first_point_acc * t_mid * t_mid +
1.0 / 6.0 * kFixedJerk * t_mid * t_mid * t_mid;
const double v_mid =
init_speed + first_point_acc * t_mid + 0.5 * kFixedJerk * t_mid * t_mid;
for (double t = 0.0; t < max_t; t += unit_t) {
double s = 0.0;
double v = 0.0;
if (t <= t_mid) {
s = std::fmax(pre_s, init_speed * t + 0.5 * first_point_acc * t * t +
1.0 / 6.0 * kFixedJerk * t * t * t);
v = std::fmax(
0.0, init_speed + first_point_acc * t + 0.5 * kFixedJerk * t * t);
const double a = first_point_acc + kFixedJerk * t;
speed_data.AppendSpeedPoint(s, t, v, a, 0.0);
pre_s = s;
} else {
s = std::fmax(pre_s, s_mid + v_mid * (t - t_mid) +
0.5 * FLAGS_slowdown_profile_deceleration *
(t - t_mid) * (t - t_mid));
v = std::fmax(0.0,
v_mid + (t - t_mid) * FLAGS_slowdown_profile_deceleration);
speed_data.AppendSpeedPoint(s, t, v, FLAGS_slowdown_profile_deceleration,
0.0);
}
pre_s = s;
}
return speed_data;
}
SpeedData EMPlanner::GenerateStopProfileFromPolynomial(
const double init_speed, const double init_acc) const {
AERROR << "Slowing down the car with polynomial.";
constexpr double kMaxT = 4.0;
for (double t = 2.0; t <= kMaxT; t += 0.5) {
for (double s = 0.0; s < 50.0; s += 1.0) {
QuinticPolynomialCurve1d curve(0.0, init_speed, init_acc, s, 0.0, 0.0, t);
if (!IsValidProfile(curve)) {
continue;
}
constexpr double kUnitT = 0.02;
SpeedData speed_data;
for (double curve_t = 0.0; curve_t <= t; curve_t += kUnitT) {
const double curve_s = curve.Evaluate(0, curve_t);
const double curve_v = curve.Evaluate(1, curve_t);
const double curve_a = curve.Evaluate(2, curve_t);
const double curve_da = curve.Evaluate(3, curve_t);
speed_data.AppendSpeedPoint(curve_s, curve_t, curve_v, curve_a,
curve_da);
}
return speed_data;
}
}
return SpeedData();
}
bool EMPlanner::IsValidProfile(const QuinticPolynomialCurve1d& curve) const {
for (double evaluate_t = 0.1; evaluate_t <= curve.ParamLength();
evaluate_t += 0.2) {
const double v = curve.Evaluate(1, evaluate_t);
const double a = curve.Evaluate(2, evaluate_t);
constexpr double kEpsilon = 1e-3;
if (v < -kEpsilon || a < -5.0) {
return false;
}
}
return true;
}
SLPoint EMPlanner::GetStopSL(const ObjectStop& stop_decision,
const ReferenceLine& reference_line) const {
SLPoint sl_point;
reference_line.XYToSL(
{stop_decision.stop_point().x(), stop_decision.stop_point().y()},
&sl_point);
return sl_point;
}
} // namespace planning
} // namespace apollo
lattice planner
此算法在Autoware 和udacity中的课程也出现过。
Lattice Planner会根据起点和终点的状态，在位置空间和时间上同时进行撒点。撒点的起始状态和终止状态各有6个参数，包括了3个横向参数，即横向位置、横向位置的导数也就是Heading、Heading的导数；3个纵向参数，即纵向位置、纵向位置的一阶导数也就是速度、纵向位置的二阶导数（也就是加速度）。
起点的参数是车辆当时真实的状态，或者Stitch的状态来进行设计，终止状态则是撒点枚举的各个情况。在确定了终点和起点状态以后，再通过五阶或者四阶的多项式连接起始状态和终止状态，从而得到规划的横向和纵向轨迹。
核心代码：
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file
**/
#include "modules/planning/planner/lattice/lattice_planner.h"
#include <algorithm>
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/macro.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/math/path_matcher.h"
#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/constraint_checker/collision_checker.h"
#include "modules/planning/constraint_checker/constraint_checker.h"
#include "modules/planning/lattice/behavior/path_time_graph.h"
#include "modules/planning/lattice/behavior/prediction_querier.h"
#include "modules/planning/lattice/trajectory1d/lattice_trajectory1d.h"
#include "modules/planning/lattice/trajectory_generation/backup_trajectory_generator.h"
#include "modules/planning/lattice/trajectory_generation/trajectory1d_generator.h"
#include "modules/planning/lattice/trajectory_generation/trajectory_combiner.h"
#include "modules/planning/lattice/trajectory_generation/trajectory_evaluator.h"
namespace apollo {
namespace planning {
using apollo::common::ErrorCode;
using apollo::common::PathPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::adapter::AdapterManager;
using apollo::common::math::PathMatcher;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::time::Clock;
namespace {
std::vector<PathPoint> ToDiscretizedReferenceLine(
const std::vector<ReferencePoint>& ref_points) {
double s = 0.0;
std::vector<PathPoint> path_points;
for (const auto& ref_point : ref_points) {
PathPoint path_point;
path_point.set_x(ref_point.x());
path_point.set_y(ref_point.y());
path_point.set_theta(ref_point.heading());
path_point.set_kappa(ref_point.kappa());
path_point.set_dkappa(ref_point.dkappa());
if (!path_points.empty()) {
double dx = path_point.x() - path_points.back().x();
double dy = path_point.y() - path_points.back().y();
s += std::sqrt(dx * dx + dy * dy);
}
path_point.set_s(s);
path_points.push_back(std::move(path_point));
}
return path_points;
}
void ComputeInitFrenetState(const PathPoint& matched_point,
const TrajectoryPoint& cartesian_state,
std::array<double, 3>* ptr_s,
std::array<double, 3>* ptr_d) {
CartesianFrenetConverter::cartesian_to_frenet(
matched_point.s(), matched_point.x(), matched_point.y(),
matched_point.theta(), matched_point.kappa(), matched_point.dkappa(),
cartesian_state.path_point().x(), cartesian_state.path_point().y(),
cartesian_state.v(), cartesian_state.a(),
cartesian_state.path_point().theta(),
cartesian_state.path_point().kappa(), ptr_s, ptr_d);
}
} // namespace
Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame) {
std::size_t success_line_count = 0;
std::size_t index = 0;
for (auto& reference_line_info : frame->reference_line_info()) {
if (index != 0) {
reference_line_info.SetPriorityCost(
FLAGS_cost_non_priority_reference_line);
} else {
reference_line_info.SetPriorityCost(0.0);
}
auto status =
PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
if (status != Status::OK()) {
if (reference_line_info.IsChangeLanePath()) {
AERROR << "Planner failed to change lane to "
<< reference_line_info.Lanes().Id();
} else {
AERROR << "Planner failed to " << reference_line_info.Lanes().Id();
}
} else {
success_line_count += 1;
}
++index;
}
if (success_line_count > 0) {
return Status::OK();
}
return Status(ErrorCode::PLANNING_ERROR,
"Failed to plan on any reference line.");
}
Status LatticePlanner::PlanOnReferenceLine(
const TrajectoryPoint& planning_init_point, Frame* frame,
ReferenceLineInfo* reference_line_info) {
static std::size_t num_planning_cycles = 0;
static std::size_t num_planning_succeeded_cycles = 0;
double start_time = Clock::NowInSeconds();
double current_time = start_time;
ADEBUG << "Number of planning cycles: " << num_planning_cycles << " "
<< num_planning_succeeded_cycles;
++num_planning_cycles;
reference_line_info->set_is_on_reference_line();
// 1. obtain a reference line and transform it to the PathPoint format.
auto ptr_reference_line =
std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(
reference_line_info->reference_line().reference_points()));
// 2. compute the matched point of the init planning point on the reference
// line.
PathPoint matched_point = PathMatcher::MatchToPath(
*ptr_reference_line, planning_init_point.path_point().x(),
planning_init_point.path_point().y());
// 3. according to the matched point, compute the init state in Frenet frame.
std::array<double, 3> init_s;
std::array<double, 3> init_d;
ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);
ADEBUG << "ReferenceLine and Frenet Conversion Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(
frame->obstacles(), ptr_reference_line);
// 4. parse the decision and get the planning target.
auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(
ptr_prediction_querier->GetObstacles(),
*ptr_reference_line,
reference_line_info,
init_s[0], init_s[0] + FLAGS_decision_horizon,
0.0, FLAGS_trajectory_time_length);
PlanningTarget planning_target = reference_line_info->planning_target();
if (planning_target.has_stop_point()) {
ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()
<< "Current ego s: " << init_s[0];
}
ADEBUG << "Decision_Time = " << (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
// 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
Trajectory1dGenerator trajectory1d_generator(
init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
trajectory1d_generator.GenerateTrajectoryBundles(
planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
ADEBUG << "Trajectory_Generation_Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
// 6. first, evaluate the feasibility of the 1d trajectories according to
// dynamic constraints.
// second, evaluate the feasible longitudinal and lateral trajectory pairs
// and sort them according to the cost.
TrajectoryEvaluator trajectory_evaluator(
init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,
ptr_path_time_graph, ptr_reference_line);
ADEBUG << "Trajectory_Evaluator_Construction_Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
ADEBUG << "number of trajectory pairs = "
<< trajectory_evaluator.num_of_trajectory_pairs()
<< " number_lon_traj = " << lon_trajectory1d_bundle.size()
<< " number_lat_traj = " << lat_trajectory1d_bundle.size();
// Get instance of collision checker and constraint checker
CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],
*ptr_reference_line, reference_line_info,
ptr_path_time_graph);
// 7. always get the best pair of trajectories to combine; return the first
// collision-free trajectory.
std::size_t constraint_failure_count = 0;
std::size_t collision_failure_count = 0;
std::size_t combined_constraint_failure_count = 0;
std::size_t num_lattice_traj = 0;
while (trajectory_evaluator.has_more_trajectory_pairs()) {
double trajectory_pair_cost =
trajectory_evaluator.top_trajectory_pair_cost();
// For auto tuning
std::vector<double> trajectory_pair_cost_components;
if (FLAGS_enable_auto_tuning) {
trajectory_pair_cost_components =
trajectory_evaluator.top_trajectory_pair_component_cost();
ADEBUG << "TrajectoryPairComponentCost";
ADEBUG << "travel_cost = " << trajectory_pair_cost_components[0];
ADEBUG << "jerk_cost = " << trajectory_pair_cost_components[1];
ADEBUG << "obstacle_cost = " << trajectory_pair_cost_components[2];
ADEBUG << "lateral_cost = " << trajectory_pair_cost_components[3];
}
auto trajectory_pair = trajectory_evaluator.next_top_trajectory_pair();
// combine two 1d trajectories to one 2d trajectory
auto combined_trajectory = TrajectoryCombiner::Combine(
*ptr_reference_line, *trajectory_pair.first, *trajectory_pair.second,
planning_init_point.relative_time());
// check longitudinal and lateral acceleration
// considering trajectory curvatures
if (!ConstraintChecker::ValidTrajectory(combined_trajectory)) {
++combined_constraint_failure_count;
continue;
}
// check collision with other obstacles
if (collision_checker.InCollision(combined_trajectory)) {
++collision_failure_count;
continue;
}
// put combine trajectory into debug data
const auto& combined_trajectory_points =
combined_trajectory.trajectory_points();
num_lattice_traj += 1;
reference_line_info->SetTrajectory(combined_trajectory);
reference_line_info->SetCost(reference_line_info->PriorityCost() +
trajectory_pair_cost);
reference_line_info->SetDrivable(true);
// Auto Tuning
if (AdapterManager::GetLocalization() == nullptr) {
AERROR << "Auto tuning failed since no localization is available.";
} else if (FLAGS_enable_auto_tuning) {
// 1. Get future trajectory from localization
DiscretizedTrajectory future_trajectory = GetFutureTrajectory();
// 2. Map future trajectory to lon-lat trajectory pair
std::vector<common::SpeedPoint> lon_future_trajectory;
std::vector<common::FrenetFramePoint> lat_future_trajectory;
if (!MapFutureTrajectoryToSL(future_trajectory, *ptr_reference_line,
&lon_future_trajectory,
&lat_future_trajectory)) {
AERROR << "Auto tuning failed since no mapping "
<< "from future trajectory to lon-lat";
}
// 3. evaluate cost
std::vector<double> future_traj_component_cost;
trajectory_evaluator.EvaluateDiscreteTrajectory(
planning_target, lon_future_trajectory, lat_future_trajectory,
&future_traj_component_cost);
// 4. emit
planning_internal::PlanningData* ptr_debug =
reference_line_info->mutable_debug()->mutable_planning_data();
apollo::planning_internal::AutoTuningTrainingData auto_tuning_data;
for (double student_cost_component : trajectory_pair_cost_components) {
auto_tuning_data.mutable_student_component()->add_cost_component(
student_cost_component);
}
for (double teacher_cost_component : future_traj_component_cost) {
auto_tuning_data.mutable_teacher_component()->add_cost_component(
teacher_cost_component);
}
ptr_debug->mutable_auto_tuning_training_data()->CopyFrom(
auto_tuning_data);
}
// Print the chosen end condition and start condition
ADEBUG << "Starting Lon. State: s = " << init_s[0] << " ds = " << init_s[1]
<< " dds = " << init_s[2];
// cast
auto lattice_traj_ptr =
std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first);
if (!lattice_traj_ptr) {
ADEBUG << "Dynamically casting trajectory1d ptr. failed.";
}
if (lattice_traj_ptr->has_target_position()) {
ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position()
<< " ds = " << lattice_traj_ptr->target_velocity()
<< " t = " << lattice_traj_ptr->target_time();
}
ADEBUG << "InputPose";
ADEBUG << "XY: " << planning_init_point.ShortDebugString();
ADEBUG << "S: (" << init_s[0] << ", " << init_s[1] << "," << init_s[2]
<< ")";
ADEBUG << "L: (" << init_d[0] << ", " << init_d[1] << "," << init_d[2]
<< ")";
ADEBUG << "Reference_line_priority_cost = "
<< reference_line_info->PriorityCost();
ADEBUG << "Total_Trajectory_Cost = " << trajectory_pair_cost;
ADEBUG << "OutputTrajectory";
for (uint i = 0; i < 10; ++i) {
ADEBUG << combined_trajectory_points[i].ShortDebugString();
}
break;
/*
auto combined_trajectory_path =
ptr_debug->mutable_planning_data()->add_trajectory_path();
for (uint i = 0; i < combined_trajectory_points.size(); ++i) {
combined_trajectory_path->add_trajectory_point()->CopyFrom(
combined_trajectory_points[i]);
}
combined_trajectory_path->set_lattice_trajectory_cost(trajectory_pair_cost);
*/
}
ADEBUG << "Trajectory_Evaluation_Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
ADEBUG << "Step CombineTrajectory Succeeded";
ADEBUG << "1d trajectory not valid for constraint ["
<< constraint_failure_count << "] times";
ADEBUG << "Combined trajectory not valid for ["
<< combined_constraint_failure_count << "] times";
ADEBUG << "Trajectory not valid for collision [" << collision_failure_count
<< "] times";
ADEBUG << "Total_Lattice_Planning_Frame_Time = "
<< (Clock::NowInSeconds() - start_time) * 1000;
if (num_lattice_traj > 0) {
ADEBUG << "Planning succeeded";
num_planning_succeeded_cycles += 1;
reference_line_info->SetDrivable(true);
return Status::OK();
} else {
AERROR << "Planning failed";
if (FLAGS_enable_backup_trajectory) {
AERROR << "Use backup trajectory";
BackupTrajectoryGenerator backup_trajectory_generator(
init_s, init_d, planning_init_point.relative_time(),
std::make_shared<CollisionChecker>(collision_checker),
&trajectory1d_generator);
DiscretizedTrajectory trajectory =
backup_trajectory_generator.GenerateTrajectory(*ptr_reference_line);
reference_line_info->AddCost(FLAGS_backup_trajectory_cost);
reference_line_info->SetTrajectory(trajectory);
reference_line_info->SetDrivable(true);
return Status::OK();
} else {
reference_line_info->SetCost(std::numeric_limits<double>::infinity());
}
return Status(ErrorCode::PLANNING_ERROR, "No feasible trajectories");
}
}
DiscretizedTrajectory LatticePlanner::GetFutureTrajectory() const {
// localization
const auto& localization =
AdapterManager::GetLocalization()->GetLatestObserved();
ADEBUG << "Get localization:" << localization.DebugString();
std::vector<TrajectoryPoint> traj_pts;
for (const auto& traj_pt : localization.trajectory_point()) {
traj_pts.emplace_back(traj_pt);
}
DiscretizedTrajectory ret(traj_pts);
return ret;
}
bool LatticePlanner::MapFutureTrajectoryToSL(
const DiscretizedTrajectory& future_trajectory,
const std::vector<PathPoint>& discretized_reference_line,
std::vector<apollo::common::SpeedPoint>* st_points,
std::vector<apollo::common::FrenetFramePoint>* sl_points) {
if (0 == discretized_reference_line.size()) {
AERROR << "MapFutureTrajectoryToSL error";
return false;
}
for (const common::TrajectoryPoint& trajectory_point :
future_trajectory.trajectory_points()) {
const PathPoint& path_point = trajectory_point.path_point();
PathPoint matched_point = PathMatcher::MatchToPath(
discretized_reference_line, path_point.x(), path_point.y());
std::array<double, 3> pose_s;
std::array<double, 3> pose_d;
ComputeInitFrenetState(matched_point, trajectory_point, &pose_s, &pose_d);
apollo::common::SpeedPoint st_point;
apollo::common::FrenetFramePoint sl_point;
st_point.set_s(pose_s[0]);
st_point.set_t(trajectory_point.relative_time());
st_point.set_v(pose_s[1]);
st_point.set_a(pose_s[2]); // Not setting da
sl_point.set_s(pose_s[0]);
sl_point.set_l(pose_d[0]);
sl_point.set_dl(pose_d[0]);
sl_point.set_ddl(pose_d[0]);
st_points->emplace_back(std::move(st_point));
sl_points->emplace_back(std::move(sl_point));
}
return true;
}
} // namespace planning
} // namespace apollo
点评
百度技术真厉害，迭代速度很快啊！小公司该何去何从是个值得思考的问题。
打赏作者
