运动规划的本质是一个优化问题。
1.决策变量及目标函数的定义
既然是优化问题，那么首先就是要明确优化的对象是什么以及优化的目标是什么，也就是要明确决策变量和目标函数。
在运动规划中，决策变量通常是按照一定的时间间隔离散的一系列控制量u，优化的过程即是寻找使得目标函数最优的决策变量。对于路径规划问题来说，则不关注具体的控制量，而只关注曲线的几何形状，这时决策变量可能就退化为离散的路径点。
目标函数则是关于决策变量的函数，其将优化的目标转化为可量化的指标形成目标函数。
2.数学、计算机语言表示
要处理真实事件里的优化问题，我们首先需要知道如何用数学语言、计算机语言去描述与优化问题相关的机器人自身状态和环境状态，也就是上图中的State representation。
环境状态通常是无穷的，常常需要离散化为占据栅格来进行表示（用栅格被占据来对不可行的区域进行表示），对于一些形状规则的环境障碍物还可以用一些几何图形来表示；机器人自身状态通常用坐标来表示即可。
3.约束条件
3.1 动力学模型（内在约束）
因为规划通常是对未来一段时间的行为进行规划，所以我们需要知道我们的行为会对未来产生什么样的影响。要想知道我们的行为会对未来产生什么影响，我们就需要用到动力学模型去预测行为导致的状态改变。
图中的u就是我们的行为，也就是控制量，x则是机器人的状态，动力学模型的作用即是预测在xt状态下，采取了ut的行为，会将机器人的状态变为什么样（xt+1）。
因为我们的机器人运动必须满足其动力学模型，所以其动力学模型其实是优化问题的约束条件之一。
还有一个常见的约束条件就是不能与环境发生碰撞。
3.2 碰撞避免（环境约束）
碰撞避免的核心即是碰撞检测，碰撞检测主要有以下几种方式：
3.2.1 栅格法
栅格法将环境是否可行用栅格来表示，是否与环境发生碰撞则转换为是否与栅格发生重叠。
3.2.2 矩形与矩形碰撞
一般规则的物体碰撞都可以处理成矩形碰撞，实现的原理就是检测两个矩形是否重叠。
矩形1的参数是：左上角的坐标是(x1,y1)，宽度是w1，高度是h1;
矩形2的参数是：左上角的坐标是(x2,y2)，宽度是w2，高度是h2。
在检测时，数学上可以处理成比较中心点的坐标在x和y方向上的距离和宽度的关系。
即两个矩形中心点在x方向的距离的绝对值小于等于矩形宽度和的二分之一，同时y方向的距离的绝对值小于等于矩形高度和的二分之一。
数学表达式如下：
x方向：| (x1 + w1 / 2) – (x2 +w2/2) | < |(w1 + w2) / 2|
y方向：| (y1 + h1 / 2 ) – (y2 + h2/2) | < |(h1 + h2) / 2 |
3.2.3 圆形与圆形碰撞
计算两个圆心之间的距离是否小于两个圆的半径和。
假设
圆形1的左上角坐标是(x1,y1)，半径是r1，
圆形2的左上角的坐标是(x2,y2)，半径是r2。
数学表达式如下：
(x1 – x2)2 + (y1 – y2)2 < (r1 + r2)2
3.2.4 矩形与圆碰撞
我们会找到AABB上距离圆最近的一个点，如果圆到这一点的距离小于它的半径，那么就产生了碰撞。
难点在于获取AABB上的最近点P¯。下图展示了对于任意的AABB和圆我们如何计算该点：
首先我们要获取球心C¯与AABB中心B¯的矢量差D¯。接下来用AABB的半边长(half-extents)w和h¯来限制(clamp)矢量D¯。长方形的半边长是指长方形的中心到它的边的距离；简单的说就是它的尺寸除以2。这一过程返回的是一个总是位于AABB的边上的位置矢量（除非圆心在AABB内部）。
限制运算把一个值限制在给定范围内，并返回限制后的值。通常可以表示为：
float clamp(float value, float min, float max) {
return std::max(min, std::min(max, value));
}这个限制后矢量P¯就是AABB上距离圆最近的点。接下来我们需要做的就是计算一个新的差矢量D′¯，它是圆心C¯和P¯的差矢量。
既然我们已经有了矢量D′，我们就可以比较它的长度和圆的半径以判断是否发生了碰撞。
GLboolean CheckCollision(BallObject &one, GameObject &two) // AABB - Circle collision
{
// 获取圆的中心
glm::vec2 center(one.Position + one.Radius);
// 计算AABB的信息（中心、半边长）
glm::vec2 aabb_half_extents(two.Size.x / 2, two.Size.y / 2);
glm::vec2 aabb_center(
two.Position.x + aabb_half_extents.x,
two.Position.y + aabb_half_extents.y
);
// 获取两个中心的差矢量
glm::vec2 difference = center - aabb_center;
glm::vec2 clamped = glm::clamp(difference, -aabb_half_extents, aabb_half_extents);
// AABB_center加上clamped这样就得到了碰撞箱上距离圆最近的点closest
glm::vec2 closest = aabb_center + clamped;
// 获得圆心center和最近点closest的矢量并判断是否 length <= radius
difference = closest - center;
return glm::length(difference) < one.Radius;
}4.优化问题求解
优化问题的求解有很多种方式，这也对应于不同的运动规划算法。
4.1 传统的方法
传统方法则是采用凸优化、非线性优化的一些方法直接求解优化问题。但是这种方式往往需要一个比较合适的初始解，运算量可能也会比较大。需要对原始问题进行一些变换和处理才能够应用于实时的机器人系统。常用的有凸可行集（Convex Feasible Set, CFS）法等。
4.2 搜索的方式
搜索的方式则是通过在可行解集内采用Dijstra类的最短路算法算法或RRT类的随机搜索算法对可行解、最优解进行搜索。
4.2.1 Dijstra类
Dijstra类算法的主要思想是从起始节点出发进行节点扩展，将满足约束条件的节点放入一个集合中，每次选择代价值最小的节点添加到最短路中进行生长，算法的伪代码如下：
运动规划中的A算法、D算法、混合A*算法、状态晶格等都是属于Dijstra算法的变种，核心思想都是一致的。
4.2.2 RRT类
RRT类的算法则是通过在状态空间中随机采样一系列状态点，并基于这些采样点对已有的随机树进行扩展，逐步得到可行解，并在此基础上进行优化（RRT*）。
算法的伪代码如下：
4.3 候选生成优选方法
候选路径生成优选法是根据模型生成一系列候选路径，并基于评价函数选出最优解作为规划结果。常见的有动态窗口算法等。
下图是一个该类方法的流程图：
5.总结
运动规划的本质是一个优化问题。只是在不同的情况下，决策变量、目标函数、约束条件有所不同。
决策变量有时是离散的控制量、有时是离散的路径线段、有时是离散的路径点，不同的决策变量应用于不同的运动规划算法中。
目标函数有时应用于非线性优化中，有时体现在最短路的代价值中，有时体现在优选的指标中，不同的运动规划算法中对目标函数的应用方式不同。
约束条件在机器人内部体现为其运动学、动力学对机器人运动的约束，在外部体现为环境对机器人运动的约束。
