本文经自动驾驶之心公众号授权转载，转载请联系出处。

本项目代码：

github.com/liangwq/robot_motion_planing

轨迹约束中的软硬约束

前面的几篇文章已经介绍了，轨迹约束的本质就是在做带约束的轨迹拟合。输入就是waypoint点list，约束条件有两种硬约束和软约束。所谓硬约束对应到数学形式就是代价函数，硬约束对应的就是最优化秋季的约束条件部分。对应到物理意义就是，为了获得机器人可行走的安全的轨迹有：

1. 把轨迹通过代价函数推离障碍物的方式
2. 给出障碍物之间的可行走凸包走廊，通过硬约束让机器人轨迹必须在凸包走廊行走

上图展示的是软硬约束下Bezier曲线拟合的求解的数学框架，以及如何把各种的约束条件转成数学求解的代价函数（软约束）或者是求解的约束条件（软约束）。image.png

上面是对常用的代价函数约束的几种表示方式的举例。image.png

Bezier曲线拟合轨迹

前面已经一篇文章介绍过贝赛尔曲线拟合的各种优点：

• 端点插值。贝塞尔曲线始终从第一个控制点开始，结束于最后一个控制点，并且不会经过任何其他控制点。
• 凸包。贝塞尔曲线 ( ) 由一组控制点 完全限制在由所有这些控制点定义的凸包内。
• 速度曲线。贝塞尔曲线 ( ) 的导数曲线 ′( ) 被称为速度曲线，它也是一个由控制点定义的贝塞尔曲线，其中控制点为 ∙ ( +1− )，其中 是阶数。
• 固定时间间隔。贝塞尔曲线始终在 [0,1] 上定义。

Fig1.一段轨迹用bezier曲线拟合image.png

上面的两个表达式对应的代码实现如下：

def bernstein_poly(n, i, t):
    """
    Bernstein polynom.
    :param n: (int) polynom degree
    :param i: (int)
    :param t: (float)
    :return: (float)
    """
    return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)

def bezier(t, control_points):
    """
    Return one point on the bezier curve.
    :param t: (float) number in [0, 1]
    :param control_points: (numpy array)
    :return: (numpy array) Coordinates of the point
    """
    n = len(control_points) - 1
    return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)