0 专栏介绍
🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解
🚀详情：运动规划实战进阶：轨迹优化篇
1 安全走廊建模的动机
在轨迹优化算法中，通常约束机器人或自车不与障碍物发生碰撞，最朴素的办法是建立自身与所有障碍物的联系。然而，机器人并没有在每一刻都与每个障碍物发生碰撞的可能性。
如图所示，考虑到从 A A A到 B B B的任务，如果自动驾驶车辆选择通过上面的路径行驶，则在底部可能很少与障碍物 O 1 O_1 O1或 O 2 O_2 O2发生碰撞；如果选择下面的路径，则可能不会与 O 5 O_5 O5发生碰撞。因此，同时避免与 O 1 O_1 O1、 O 2 O_2 O2和 O 5 O_5 O5发生碰撞会导致冗余约束。如果预先给定了引导路径，可以减少碰撞回避约束的规模。
进一步考虑，如果沿着选定路径构建一条安全走廊来将机器人与周围障碍物隔离开来，则与环境相关的约束规模将完全独立于障碍物数量，与环境复杂度无关——这对于优化建模非常关键。
2 矩形增量膨胀算法
矩形增量膨胀算法的核心原理是：第 i i i个局部框最初由一个安全矩形构成——考虑机器人的几何尺寸以及安全边界等。接着依次在每条边上扩展直到检测到碰撞，扩展顺序和方向如图( b b b)所示。
具体地，每次扩展尝试都会通过恒定大小 将当前安全区域的相关边向外推，并增量地对扩展区域进行碰撞检测，如图( c c c)所示。若没有发生碰撞，则将扩展区域合并到安全区域中；否则它将被拒绝，同时在该方向上不会进行进一步的扩展。算法流程如下所示
3 算法仿真
3.1 C++实现
bool TrajectoryOptimizer::GenerateBox(double time, double &x, double &y, double radius, AABox2d &result) const
{
double ri = radius;
AABox2d bound({-ri, -ri}, {ri, ri});
if (CheckCollision(time, x, y, bound))
{
// initial condition not satisfied, involute to find feasible box
int inc = 4;
double real_x, real_y;
do
{
int iter = inc / 4;
uint8_t edge = inc % 4;
real_x = x;
real_y = y;
if (edge == 0)
{
real_x = x - iter * 0.05;
}
else if (edge == 1)
{
real_x = x + iter * 0.05;
}
else if (edge == 2)
{
real_y = y - iter * 0.05;
}
else if (edge == 3)
{
real_y = y + iter * 0.05;
}
inc++;
} while (CheckCollision(time, real_x, real_y, bound) && inc < config_.corridor_max_iter);
if (inc > config_.corridor_max_iter)
{
return false;
}
x = real_x;
y = real_y;
}
int inc = 4;
std::bitset<4> blocked;
double incremental[4] = {0.0};
double step = radius * 0.2;
do
{
int iter = inc / 4;
uint8_t edge = inc % 4;
inc++;
if (blocked[edge])
continue;
incremental[edge] = iter * step;
AABox2d test({-ri - incremental[0], -ri - incremental[2]},
{ri + incremental[1], ri + incremental[3]});
if (CheckCollision(time, x, y, test) || incremental[edge] >= config_.corridor_incremental_limit)
{
incremental[edge] -= step;
blocked[edge] = true;
}
} while (!blocked.all() && inc < config_.corridor_max_iter);
if (inc > config_.corridor_max_iter)
{
return false;
}
result = {{x - incremental[0], y - incremental[2]},
{x + incremental[1], y + incremental[3]}};
return true;
}
3.2 Python实现
def decompose(self, way_points: list) -> list:
step = 1
prune_waypoints, decomp_polygons = [], []
nums = len(way_points)
for i in range(nums - 1):
x, y = way_points[i]
if len(decomp_polygons) > 0 and decomp_polygons[-1].isInside(np.array([[x], [y]])):
continue
# adjustment for initial (x, y)
if self.isRectCollision(
x - self.radius, y - self.radius, x + self.radius, y + self.radius
):
inc = 4
while inc < self.max_iter:
it = inc / 4
edge = inc % 4
inc += 1
real_x, real_y = x, y
if edge == 0:
real_x = int(x - it * step)
elif edge == 1:
real_y = int(y - it * step)
elif edge == 2:
real_x = int(x + it * step)
else:
real_y = int(y + it * step)
# collision detection
if not self.isRectCollision(
real_x - self.radius, real_y - self.radius,
real_x + self.radius + 1, real_y + self.radius + 1
):
break
if inc > self.max_iter:
return [], []
else:
x, y = real_x, real_y
# main loop for construction in (rx, ry)
inc = 4
block, increment = np.zeros(4, dtype=np.bool_), np.zeros(4, dtype=np.int32)
while inc < self.max_iter and not block.all():
it = inc / 4
edge = inc % 4
inc += 1
if block[edge]:
continue
increment[edge] = int(it * step)
if increment[edge] >= self.safe_range or self.isCollision(edge, x, y, increment):
# increment[edge] -= step
block[edge] = True
if inc > self.max_iter:
return [], []
else:
prune_waypoints.append((x, y))
decomp_polygons.append(Polygon([
Hyperplane(n=np.array([[-1], [0]]), d=np.array([[x - increment[0]], [y]])),
Hyperplane(n=np.array([[1], [0]]), d=np.array([[x + increment[2]], [y]])),
Hyperplane(n=np.array([[0], [-1]]), d=np.array([[x], [y - increment[1]]])),
Hyperplane(n=np.array([[0], [1]]), d=np.array([[x], [y + increment[3]]]))
]))
prune_waypoints.append(way_points[-1])
return decomp_polygons, prune_waypoints
🔥 更多精彩专栏：