qq_35635374的博客

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对局部轨迹规划导读做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

运行程序后，将输出曲线上的点坐标，这些点可以用于绘制B样条曲线。以下是一个完整的B样条曲线的C++实现示例，基于搜索结果中的信息。该代码实现了B样条曲线的计算，包括基函数的递归计算和曲线点的生成。曲线点生成：getPoint函数计算给定参数u处的曲线点，generateCurvePoints函数生成一系列曲线点。以下代码代码实现了B样条曲线的计算，包括基函数的递归计算和曲线点的生成。输出结果：程序生成并输出曲线上的点，可以用于绘图或其他处理。B样条曲线是先生成B样条曲线，然后再B样条曲线上取点。

在移动过程中，为了保证路径无碰撞，需要进行碰撞检测，当移动后的点发生碰撞时，不对该点移动操作。在定义了平滑度后，可以发现通过移动P点的位置，平滑度会发生改变，那么当平滑度最小时，P的位置在哪个地方，或者说P点朝哪个方向移动多少距离？这是一个最小值问题。依次选取5个路径点定义平滑度与路径点的关系，通过移动第3个点的位置，求解出使5个路径点平滑度最小的值，梯度方向为第3个点的移动方向，如下图所示。（2）Pa为P−2 与P的中点，Pb 为P−1与P1的中点，Pc为P2 与P的中点。4、平滑度的定义不唯一；

对于一个 二次型优化问题：一般情况下，分别用 梯度下降法（GD） 和 共轭方向法（CG），有如下图的收敛轨迹：显然可以看出，梯度下降法需要经过多次迭代，而共轭方向法只需要经过两次迭代。

算法入口在 planning\tasks\optimizers\path_time_heuristic\path_time_heuristic_optimizer.cc。速度规划这块，可以参考一下EM Planner，保留了EM的思想，DP+QP。1、S-T图离散，动态规划（代价最小的有向图搜索）

Voronoi图是在收到栅格地图的回调函数处理生成的，Voronoi图就是根据栅格地图障碍物代价生成的一系列原理障碍物的蓝色点路径，蓝色点路径和之前路径探索得到的路径是不完全吻合和，【防盗标记–盒子君hzj】使用蓝色点路径优化原有路径可以实现车辆也能很好地通过狭窄路段，使路径远离障碍物。voronoi 场表明到最近的 voronoi 边缘和最近的障碍物的距离。在stage 1的子节点扩张的过程中，路径会有一些额外的不必要控制动作（即steering），所以算法的第二个部分就是对生成点曲线进行平滑处理。

窗口滑动的方式，并对每个窗口内的固定数量路径点进行平滑。避免分段拼接的折角，避免全局路径分段。Savitzky-Golay数字滤波器实现路径平滑，具体参考我以下的博客。路径的多项式拟合算法，具体参考我以下的博客。

3次样条曲线(Cubic Spline), 是一种通过一系列三次多项式来逼近给定数据点集的方法。每个多项式定义在两个相邻的数据点之间，且满足在数据点处及其一阶导数和二阶导数都连续的条件。

最小二乘估计具有良好的统计性质，如在一定条件下，它是无偏的、一致的，并且在所有线性无偏估计中具有最小方差（即它是BLUE，Best Linear Unbiased Estimator）。最小二乘法的数学意义在于它提供了一种系统的方法来最小化观测值与模型预测值之间的误差，从而得到模型参数的最优估计。在统计学和机器学习中，通过最小化残差平方和，可以找到最佳的模型参数，使得模型对数据的拟合最优。使用最小二乘的思想，为了使拟合效果最好，需要使残差的平方和最小，则需要使得所有拟合系数的一阶偏导为零。

最小二乘估计具有良好的统计性质，如在一定条件下，它是无偏的、一致的，并且在所有线性无偏估计中具有最小方差（即它是BLUE，Best Linear Unbiased Estimator）。最小二乘法的数学意义在于它提供了一种系统的方法来最小化观测值与模型预测值之间的误差，从而得到模型参数的最优估计。在方差分析（ANOVA）中，残差平方和是总平方和（Total Sum of Squares, TSS）的一部分，用于分析模型中不同因素对因变量的影响。具体来说，它最小化的是误差的平方和，即残差平方和（RSS）。

其中，t i 必须单调递增，i = 0 , 1 , . . . , n。给定n + 1 n+1n+1个点序列( t i , p i )，利用。

给定n + 1个点序列( t i , p i )，利用分段五次多项式插值，使得拼接而成的分段多项式p(t)经过所有点序列。其中，t i必须单调递增，i = 0 , 1 , . . . , n。

给定n + 1个点序列( t i , p i )，利用分段七次多项式插值，使得拼接而成的分段多项式p(t)经过所有点序列。其中，t i必须单调递增，i = 0 , 1 , . . . , n。

一阶贝塞尔曲线二阶贝塞尔曲线三阶贝塞尔曲线n阶贝塞尔曲线的一般性定义。

三次螺旋线是由一个曲率k关于弧长s的多项式定义的.假设我们有一个用3次螺旋线表达的曲线, 它的始末状态分别是。

本文先对基于曲线实现速度规划、基于逻辑实现速度规划做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容1.介绍Double S规划也叫S形曲线规划，S轨迹也称作七段轨迹曲线，因此主要由七段函数构成。，Point to Point的直线速度规划2.图例位置曲线速度曲线加速度曲线加加速度曲线标记q0:起点q1:终点v0:初始速度v1:终点速度a0:初始加速度a1:终点加速度j0:初始加加速度j1:终止加加速度。

起点和目标点状态约束包括位置、速度、加速度。最大速度限制v {lim}，最大加速度 a {lim}，最大加加速度 j {lim}给定起点和目标点的状态约束与速度限制值，拟合生成两点之间的运动轨迹，该运动轨迹满足速度平滑且小于速度约束条件。，也可体现在运动总时间T和运动总长度与速度限制值的关系上（非归一化表达）T取等号时，归一化变量u处达到极值，即T取等号以达到时间最短的速度规划。无论上上面那种方法，都可得到连续平滑的归一化7次多项式。不同的起点和目标点的状态约束，解出来的系数是不一样的。

给定起点和目标点的状态约束与速度限制值，拟合生成两点之间的运动轨迹，该运动轨迹满足速度平滑且小于速度约束条件。起点和目标点状态约束包括位置、速度、加速度。最大速度限制v {lim}，最大加速度 a {lim}，也可体现在运动总时间T和运动总长度与速度限制值的关系上（非归一化表达）T取等号时，归一化变量u处达到极值，即T取等号以达到时间最短的速度规划。无论上上面那种方法，都可得到连续平滑的归一化5次多项式。不同的起点和目标点的状态约束，解出来的系数是不一样的。根据速度限制值与速度曲线表达式，

给定起点和目标点的状态约束与速度限制值，拟合生成两点之间的运动轨迹，该运动轨迹满足速度平滑且小于速度约束提奥健。，也可体现在运动总时间T和运动总长度与速度限制值的关系上（非归一化表达）T取等号时，归一化变量u处达到极值，即T取等号以达到时间最短的速度规划。无论上上面那种方法，都可得到连续平滑的速度曲线表达式和位置曲线表达式。不同的起点和目标点的状态约束，解出来的系数是不一样的。并使得，3次多项式轨迹的最大速度 v{max}根据速度限制值与速度曲线表达式，设：插补总长度L 、总运动时间T。

他认为最近的过去态势，在某种程度上会持续到最近的未来，所以将较大的权数放在最近的数据。1.St是yt和 St − 1的加权算数平均数，随着a取值的大小变化，决定yt和 St − 1对St的影响程度，当a取1时，St = yt；3.尽管St包含有全期数据的影响，但实际计算时，仅需要两个数值，即yt和 St − 1，再加上一个常数a，这就使指数滑动平均具逐期递推性质，从而给预测带来了极大的方便。它们的基本思想都是：预测值是以前观测值的加权和，且对不同的数据给予不同的权，新数据给较大的权，旧数据给较小的权。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对相关计算机图形学计算的库介绍做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容Polyscope被设计为一个轻量级的通用三维可视化库，不开源。主要用户界面显示、用于可视化界面,使用这个库成为项目的一部分，即可以实现rviz的可视化效果（类似于嵌套在项目中的代码实现rviz）

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**路径点线性插值方法**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容线性插值是用一系列首尾相连的线段依次连接相邻各点线性插值的特点是计算简便，但光滑性很差。如果用线性插值拟合一条光滑曲线，对每一段线段，原曲线在该段内二阶导数绝对值的最大值越大，拟合的误差越大。一次多项式就是直线拟合算法。常用的又线性差值算法和双线性差值算法选择哪种插值方法取决于具体的应用场景和需求。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对常用的路径抽稀算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容在处理矢量化数据时，记录中往往会有很多重复数据，对进一步数据处理带来诸多不便。多余的数据一方面浪费了较多的存储空间，另一方面造成所要表达的图形不光滑或不符合标准。因此要通过某种规则，在保证矢量曲线形状不变的情况下， 最大限度地减少数据点个数，这个过程称为抽稀。

本文先对轨迹平滑问题做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容（1）适合给移动机器人运动的路径【满足运动学，高阶平滑】（2）高阶的运动学不能够突变的【至少三阶的加速度不能突变，最好四阶的jerk和五阶的snap也不能突变】（3）在转弯的地方不会停下来（4）加减速均衡，节省能量（5）节省时间的。

全局路径由一系列路径点构成，这些路径点包含空间位置信息，也可以包含姿态信息，但不需要与时间相关。轨迹则在路径的基础上加入了时间约束，轨迹点是一种特殊形式的路径点，它在路径点的基础上加入了时间信息。本文先对路径插值与抽稀篇做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容在选择插值、拟合或逼近方法时，需要考虑路径的用途、所需的精度以及计算资源的限制。对于需要高精度的应用场景，如精密制造或医疗手术，插值方法可能是首选。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对算术滤波平滑的方式做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对使用滤波器进行滤波平滑做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容（1）阶数越高，整定的参数越多，一般我用到三阶就差不多了（三阶已经有四个参数了）（2）如何选择高通、带通、低通滤波器主要看信号的特性。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**基于地图的平滑算法–佛洛依德路径平滑算法（FLOYD）**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

系列文章目录提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加TODO:写完再整理文章目录系列文章目录前言（1）双圆弧轨迹插值拟合算法一、双圆弧轨迹插值拟合算法的应用场景二、双圆弧轨迹插值拟合算法的实现流程三、实现工具（2）正余弦曲线插值拟合算法一、正余弦曲线插值拟合算法--路径规划二、正余弦曲线插值拟合算法--速度规划（3）基于三次、五次多项式曲线拟合算法的路径插值及平滑1、直线插值、二次样条插值、三次样条插值生成路径效果对比2、三次样条插值的路径生成实现1.人为数学推导过程（代数的

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**使用优化理论的方式**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对基于曲线实现速度规划、基于逻辑实现速度规划做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容1.介绍Double S规划也叫S形曲线规划，S轨迹也称作七段轨迹曲线，因此主要由七段函数构成。，Point to Point的直线速度规划2.图例位置曲线速度曲线加速度曲线加加速度曲线标记q0:起点q1:终点v0:初始速度v1:终点速度。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对分段路径优化的策略&非线性优化做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对线性加权轨迹代价损失评分，轨迹多选一方法–DWA_planner、Open_Planner、lexicographic_planner做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容1、工程常用方法：线性加权法–如上面的计算路径长度和路径平均曲率两个目标的代价线性加权法并没有用到优化的知识–从多选优2、学术常用方法：利用了优化理论的NSGA、Pareto最前沿。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**【多选一】OpenPlanner规划算法**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容规划执行最优路径，同时避免障碍，触发行为例如在交通信号灯处停车将高清矢量地图和定位数据作为先验信息输入，在全局中，指定一个起点，一个终点，通过全局规划得到一条全局静态路径，全局路径规划在车辆启动或重启的时候被确定，一旦分配了全局路径，本地马上启动运动规划器。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容open_planner更适合用于无人驾驶车道线场景，【防盗标记–盒子君hzj】lexicographic_planner更适合于农业的一垄一垄林场和仓库的场景，或者数原论文中阿无人船场景【属于replan局部规划的范畴，因为全局是给定路线的】1.优点。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**regular state lattice in unstructured environment（结合了全局采样和图搜索的思想）**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容regular state lattice的每个采样网格中都是用曲线连接起来的，改进了RRT的折线问题，规划出来的path更加平滑。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对基于控制的方法实现轨迹避障做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容更粗暴，算法理论更地一级的时方形绕障碍–匹配适合场景实现效果就行感知传感器用超声波雷达、毫米波雷达、激光雷达 、视觉等等距离传感器（1）DWA_planning是move_base默认的局部规划器（2）该算法思想比较简单，且ROS已经封装成以一个功能包，【防盗标记–盒子君hzj】比较容易部署与移植。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对应激式BUG局部避障算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容更粗暴，算法理论更地一级的时方形绕障碍–匹配适合场景实现效果就行感知传感器用超声波雷达、毫米波雷达、激光雷达 、视觉等等距离传感器。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对感知局部规划做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对常用的优化方法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容。