lattice planner调试记录
前情提要
AEB仿真效果
前期把apollo中的lattice规划器的大体实现了,收获颇丰,docker搭了,protobuf也取代了老旧的数据结构,自己的码能也肉眼提升,另外为某厂开发的AEB也到了验收阶段,CNACP全满通过。再接再厉。
感知的小伙伴们目标级障碍物输出了,控制的小伙伴们优化了车辆控制,大哥搞定了框架,我得以继续完善,目标不变: 让车在封闭的园区内像人一样自由稳定的行驶 。
1.单车道nudge的实现
车辆的避障行为分为制动避障与转向避障,即下图中的A和B、C、D:
上图中的B、C、D是理想的nudge表现。此阶段并未考虑换道,因为目前仅使用的单车道地图,对于增大偏移量实现换道的效果并不建议,因为这种做法直接抹杀了Lane的概念,一方面无法扩展交通规则,另一方面跳过了HD map中的车道信息,在建立OSQP的bounds时存在大量风险。
- 目标 :
规划结果与实车表现同B、C、D吻合,实现车道内的平滑绕行 - 现象 :
当车辆行驶到车道某位置附近时,突然往右避让幅度超大,基本要撞上右侧车了。但是nudge的值为常值并无改动。
- 问题排查 :
通过调整FLAGS_nudge_buffer
值来改变偏移程度,无法起作用,然后注意到该车是倾斜的,正常停的车是没有这个现象的,基本锁定是碰撞检测环节出问题,要么没算对,要么障碍物尺寸不对!碰撞检测的源码:
bool CollisionChecker::InCollision(
const DiscretizedTrajectory& discretized_trajectory) {
CHECK_LE(discretized_trajectory.NumOfPoints(),
predicted_bounding_rectangles_.size());
const auto& vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
double ego_length = vehicle_config.vehicle_param().length();
double ego_width = vehicle_config.vehicle_param().width();
for (size_t i = 0; i < discretized_trajectory.NumOfPoints(); ++i) {
const auto& trajectory_point =
discretized_trajectory.TrajectoryPointAt(static_cast<std::uint32_t>(i));
double ego_theta = trajectory_point.path_point().theta();
Box2d ego_box(
{trajectory_point.path_point().x(), trajectory_point.path_point().y()},
ego_theta, ego_length, ego_width);
// 车辆center和车辆的几何中心不重合,所以box需要校正一下
double shift_distance =
ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
Vec2d shift_vec{shift_distance * std::cos(ego_theta),
shift_distance * std::sin(ego_theta)};
ego_box.Shift(shift_vec);
for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
if (ego_box.HasOverlap(obstacle_box)) {
return true;
}
}
}
return false;
}
方法很简单,核对一下自车box数据没问题后,断点查看ego_box.HasOverlap()
入参障碍物的Box尺寸参数,发现果然不对,尺寸比真实尺寸大了很多…变长了。
让我们进入它!在函数CollisionChecker::BuildPredictedEnvironment()
中发现对障碍物做了膨胀处理,如下:相当于把障碍物增长2×FLAGS_lon_collision_buffer
米,增宽2×FLAGS_lat_collision_buffer
米。
while (relative_time < FLAGS_trajectory_time_length) {
std::vector<Box2d> predicted_env;
for (const Obstacle* obstacle : obstacles_considered) {
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
Box2d box = obstacle->GetBoundingBox(point);
box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
predicted_env.push_back(std::move(box));
}
predicted_bounding_rectangles_.push_back(std::move(predicted_env));
relative_time += FLAGS_trajectory_time_resolution;
}
把经过膨胀后的障碍物对车道的占据情况显示出来,在碰撞检测阶段,真实的障碍物如右图所示,很明显车当然过不去了,即使强行通过也会再更近的时候由于突破bounds导致osqp无解进而规划失败…
障碍物膨胀不难理解,为了安全,人在开车的过程中也会适当的远离附近车辆,但是这个膨胀方法显然太过死板了,或许百度内部早就优化了,只是开源没释放?所以暂时做了些许改动: 静止物体膨胀程度缩小、动态障碍物适当膨胀 。后期必然是需要继续完善的,进一步可借鉴物体危险程度势场方法。
while (relative_time < FLAGS_trajectory_time_length) {
std::vector<Box2d> predicted_env;
for (const Obstacle* obstacle : obstacles_considered) {
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
Box2d box = obstacle->GetBoundingBox(point);
if(obstacle->IsStatic()){
box.LongitudinalExtend(2.0 * 0.1);
box.LateralExtend(2.0 * 0.05);
} else {
box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
}
predicted_env.push_back(std::move(box));
}
predicted_bounding_rectangles_.push_back(std::move(predicted_env));
relative_time += FLAGS_trajectory_time_resolution;
}
2.SL图建立-bug描述与解决
我一直在告诫自己,是我错了是我错了,应该是我错了…但是,把障碍物尺寸更新到bounds里,怎么也不是光start_s吧,源码:
auto start_iter = std::lower_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
// 这里应该是sl_boundary.end_s()吧?
auto end_iter = std::upper_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
按照源码逻辑,SL图实际如下左图:
仅采用障碍物SL中的start_s进行采样的话,必然导致车辆轨迹过早的回归,如果理解没出错的话,是apollo这里出错了吧,issue已提交,希望是自己被打脸,为了实现右图的效果,没错,我改了代码,在查询上边界时sl_boundary.start_s()
改为sl_boundary.end_s()
。
然后,然后就搞定了…然后让目标车横停、竖停、斜着停,丝滑~
nudge实车测试
3.狭窄路段通行过程中规划失败
在通过狭窄路段时,车辆正常行驶均正常,但在即将驶出时,出现无轨迹现象,排查后发现由OSQP求解失败导致。
OSQP求解失败最常见原因之前记录过,大致:边界以及轨迹已经规划好了,但是控制误差太大导致车辆行驶过程中驶出了边界,导致求解器失败。这个问题控制的小伙伴优化后已经解决。
此次问题,bounds为[-1.072, -0.635],自车横向d=-0.8并未出现越界行现象:
为了确定约束问题,重新对二次规划建模进行了整理:
A为Cloum矩阵进行CSC压缩后,需要满足等式与不等式约束:
认为车辆在驶离狭窄路段时,车辆驶出角度过大导致运动约束无法满足。
解决方案
通过调整目标函数即P与Q,来调整目标轨迹的形状,使得车辆能够平滑的通过狭窄路段,实现nudge,同时可扩大对control精度的宽容度。P与Q的作用效果如下图所示:
通过适当的调节P和Q来调控a、b,以达到nudge的范围容忍度调节,当然,如果control足够精确,完全可以让车辆尽量近的贴近障碍物进行nudge。代码实现:
P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset +
2.0 * FLAGS_weight_lateral_obstacle_distance;
和
q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
(d_bounds[i].first + d_bounds[i].second);
最终实现效果:车辆更贴近外边界进行nudge,不会出现较大的转向控制。
下一个议题
未完待续
后续:
丰富障碍物的处理,包括:bounding box的膨胀优化,逐步完成lattice planner的全功能。
一直在获得很多大佬的指导,希望自己继续深入,每天进步一点点