【路径生成--插值拟合方法】贝塞尔曲线

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对贝塞尔曲线、B样条曲线做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

样条曲线（spline curve），样条是一根富有弹性的细木条或塑料条

一、曲线拟合、插值、获取方法的的引入

1.两点直线插值–一阶曲线线性插值的方法

【正向理解】已知的A、B位置，通过在B样条中输入时间因子表明线段上任意一点

（1）先在平面上取两点，A和B，【防盗标记–盒子君hzj】用直线直接连起来

---

（2）用已知的A、B位置，来表明线段上任意一点
1）取两点的中点（例子）

2）表示偏向A一侧七分之一处的点

3）通用方法


从A向B慢慢移动，最一开始由于从A出发，A占完全的比重1，而B是0。随着点P缓慢向右推移，B的占比逐渐增大，A的占比逐渐减小。【防盗标记–盒子君hzj】最终到达B点的时候，A成为了0，而B占完全的比重1。我们将这个比重值抽象成一个值t
.
.

【反向理解】一阶曲线线性插值的方法

通过上述的线性权重进行任意点的插入，进而实现插值
一阶直线线性插值的方法还有很多
.
.

2.三点曲线插值–二维情况

两个点的插值再怎么变化也只能做出一条直线。因此，我们要引入一个新点C

我们先考虑比较简单的情况，A和B的比值保持不变，【防盗标记–盒子君hzj】因此还是t和1-t，这个比值，更多的时候我们将其称之为权重。同样的，C也需要一个权重（我们假设为）
随着t的改变，我们依然可以通过t获取曲线中的任意点

这种方法存在的问题
真正的B-样条曲线的规则没有这么简单，【防盗标记–盒子君hzj】它的插值规则是不确定的，所以C的权重也不好确定下来，而且C点的权重时非线性的

解决的办法：
B样条和贝塞尔曲线时复杂的曲线拟合方法
.
.

二、贝塞尔曲线、B样条曲线的目的

1.通用的目的

（1）（最重要）【获取位置、速度、加速度】B样条曲线的一些基本参数，通过这些参数就可以生成B样条曲线，最终只需要通过修改t值，【防盗标记–盒子君hzj】就可以表述任意曲线上的任意位置点，再通过对位置的求导可以获得任意点的速度和加速度

（2）贝塞尔曲线和B样条曲线可以理解成曲线插值、平滑并获取曲线中任意点的方法

2.对于机器人路径规划的目的

（1）【轨迹插值与平滑】B样条可以做轨迹插值与平滑

（2）【路径转轨迹控制输出】B样条可以做路径转轨迹的控制输出，提供控制所需要的位置、速度、和加速度信息

3.对于机械臂轨迹规划的目的

（1）可以规划并控制机械臂，因为B样条可以做轨迹规划

三、贝塞尔曲线理论知识

1.贝塞尔曲线示例图

（1）二维贝塞尔曲线示意图

（2）三维贝塞尔曲线示意图

（3）7次贝塞尔曲线示意图

2.贝塞尔曲线本质

把平面上相邻的点用直线连接起来，每条直线上从当前点到端点设定一个变量t表示走完全程的占空比，t=0表示停留在当前点，t=0.5表示当前点走完全程的一半，把每条直线变量t相等所在的点连接到一起时的折线比原来的折线少了一条直线，重复该过程【递归过程】，【防盗标记–盒子君hzj】直到进剩下一条直线，这是一个不断降阶的过程，随着变量t的递增，最后的直线斜率会不断变化，直到端点，因此贝塞尔曲线与全局的点都相关
.
.

3.【核心】贝塞尔曲线轨迹递归公式推导（数学归纳法）

（1）一阶贝塞尔曲线推导
第一条折线占空比相同的点坐标：

化简整理成递归形式

（2）二阶贝塞尔曲线推导
第二条折线占空比相同的点坐标：

化简整理成递归形式：

…

（3）高阶的贝塞尔可以通过不停的递归直到一阶，得到

第n条折线占空比相同的点坐标：

第n条折线贝塞尔曲线轨迹：

整理得到贝塞尔曲线轨迹递归公式：

其中，t∈[0,1]为一个时间因子，即可得到腿的末端点在摆动相过程中在任何时间的位置

4.贝塞尔曲线轨迹生成缺点

修改曲线的某一部分，该部分的控制点调整会影响到整条曲线

5.贝塞尔曲线理论和应用的桥梁

摆动相轨迹规划控制器中，使用四个控制点来决定足端贝塞尔曲线轨迹
四足机器人摆动相腿部足末端点的轨迹规划基于贝塞尔曲线(Bézier curve)进行，由贝塞尔曲线的计算本质可以知道，【防盗标记–盒子君hzj】绘制贝塞尔曲线即是对贝塞尔曲线的关键控制点进行确定
在我们所设计的摆动相轨迹规划控制器中，其使用四个控制点来决定足端贝塞尔曲线轨迹，分别是抬脚点、落脚点、中间段控制点 1、中间段控制点 2
轨迹曲线通过一定数量且满足机器人运动中速度和加速度要求的关键点，从而利用贝塞尔曲线生成腿部运动轨迹。在贝塞尔递推公式中，变量t是占空比的含义，在腾空控制中认为 变量t为一个时间因子，因为腾空相是基于周期进行调度的，【防盗标记–盒子君hzj】故可以将t替换为摆动相相位，即可得到腿的末端点在摆动相过程中在任何时间的位置

.

参考资料

贝塞尔曲线参考资料
https://zhuanlan.zhihu.com/p/136647181

https://blog.csdn.net/weixin_42284263/article/details/107093255