曲线插值拟合算法拆解为三个子类算法的优缺点

摘要：运动规划是联通机器人自主导航系统中的重要模块之一，相关算法研究成果层出不穷，本文将曲线插补拟合算法拆解为三个子类算法：基于插补的规划算法、基于特殊曲线的规划算法及基于优化的规划算法加速度路程公式推导，并沿时间次序概述相关算法的发展历程，最后从模型复杂度、实时性、环境适应能力及路径曲线质量等方面剖析了上述三类算法的异同点。

#

01

序言

#

在前文《机器人图规划算法研究现况浅析》中，结合图1.1对“运动规划是哪些？要解决哪些问题？”这两个问题有详尽论述，并概述了运动规划与运动控制模型、环境感知这两个部份之间的关系。

#

图1.1运动规划示意图（图片来源：/）

从上述可知，要进行运动规划，须要有两个前提条件：地图和定位。机器人须要先借助传感获取环境信息，建立环境地图并估算自身在环境地图中的位姿（见《机器人环境感知研究现况浅析》），接着依照自身在地图中的位置，明晰起始点和终点后，才可以步入到运动规划阶段，最后将生成的轨迹转化为运动控制命令发送给运动控制模型（见《常见联通机器人运动学模型总结》），驱动机器人运动。

#

如图1.2所示，运动规划的研究主要是对多目标多变量多约束耦合的规划模型优化求解。对于具有非完整约束的联通机器人而言（见《两轮差速驱动机器人运动模型及应用剖析》），在分布有障碍物的环境中求解最优路径是NP-hard问题，即对于任意场景难以保证在方程时间内求得最优解，因而大部份规划算法追求次优解或局部最优[1]。

#

众多学者针对不同应用场景和需求，设计、改进了十分多的运动规划算法，其中常见的运动规划算法主要包括四类：图规划算法、空间取样算法、曲线插补拟合算法和仿生智能算法，通过规划模型求解得到最佳轨迹曲线，包含无碰撞顺滑的路径曲线和平滑的速率曲线，并输入到控制器驱动机器人运动。

本文将对曲线插补拟合算法领域的研究进展及成果进行分类论述，主要概述同宗算法的发展过程，不对具体算法展开剖析，在后续系列文章会选购部份精典常用算法深入探讨。

图1.2运动规划通用模型

#

02 #

曲线插补拟合算法研究剖析

上述大部份《图规划算法》和《空间取样算法》生成的路径存在折点、急弯等曲率不连续的情况，影响了机器人运动平稳性，因而须要综合考虑模型硬约束与实际规划软需求，以提高路径平滑度。曲线插补拟合算法在曲线平滑控制及优化方面有明显的优势，根据曲线生成方法及其种类可分为：基于插补的规划算法、基于特殊曲线的规划算法及基于优化的规划算法三类，该类算法在手动驾驶等领域有着广泛的应用。 #

2.1

基于插补的规划算法 #

第一类是采用方程曲线来描述机器人运动轨迹，这是由于方程曲线灵活多变，且能容易满足曲率连续要求，并才能容易导入速率曲线，因而特别适宜描述机器人运动轨迹，根据方程构造方法主要分为：插补方程曲线、贝塞尔曲线、样条曲线，具体如下： #

Chang等人提出QPMI(and)算法可创建无碰撞、无龙格现象的曲率连续曲线，并使用Pan算法监测曲线碰撞点位置，并通过降低子锚点以重规划无碰撞路径，但在复杂拥挤环境下的碰撞检查及重规划成本较高[2]。 #

等人基于公路完善座标系，将无人车运动分解为横向、横向运动，并分别构建路程-时间的五次方程等式，考虑循迹约束、舒适度等诱因及其常年运动模式（换道、合并等）设计优化目标函数，结合始末运动状态，输出最佳运动控制量[3]。 #

图2.1基于座标系规划的方程轨迹[3]

#

Xu等人进一步提出路径、速度前馈循环迭代优化框架，如图2.2所示，将三次方程（红色）或五次方程（蓝色）应用于公路状态网格，综合静态、动态成本函数，选择最佳轨迹，并以该最佳轨迹（包含路径、曲线）为年率，使用单纯形法循环迭代优化路径曲线（先）及速率曲线（后），比未使用该迭代优化框架的算法性能提高10%，规划时间降低50%[4]。

#

图2.2方程路径曲线[4]

#

等人提出与文献[3,4]相像的运动规划方式，作者基于座标系纵轴、纵轴分别用两个五次方程曲线联接相邻状态，并剖析得到该曲线及其曲率变化具有对称性，得到简约的优化模型，极大提高路径曲线生成效率，并基于该曲线构建速率优化模型，最终在仿真中验证了无人车在循迹、变道等场景的良好性能[5]。 #

Han等人综合局部栅格图中的全局路径及障碍物影响以确定控制点，生成无碰撞的四阶贝塞尔曲线作为局部路径，并使用PID控制器按照曲线[6]。

如图2.3所示，González等人针对单岔路、连续岔路场景使用三阶贝塞尔曲线过渡，并提出曲率评估权重算法调节控制点以保证曲线曲率（起始点、终止点及中间段）连续且满足最二锅头率约束，并被集成到RITS手动驾驶系统[7]。 #

图2.3贝塞尔曲线[7]

#

等人针对类车机器人提出B样条曲线与RRT相结合的方式，借助B样条曲线平滑性好的特性以减少搜索维度，快速生成适用于履带机器人的曲率连续曲线[8]。

#

图2.4B样条曲线[8]

#

NURBS曲线是特殊的B样条曲线，等人针对联通机器人在三维环境下的运动规划问题，采用NURBS曲线生成平滑的路径[9]。 #

2.2 #

基于特殊曲线的规划算法

第二类算法是采用特殊曲线描述规划的轨迹，特殊曲线规划主要包含了直线-弧形曲线和曲线两种。 #

其中直线-弧形曲线是指针对类车机器人运动约束问题，在给定两个位置矢量，仅使用弧形（满足最小曲率）和直线段合成最短平滑的路径，曲线限定机器人仅能前向运动。如图2.5所示，R&S曲线将其拓展为单向运动，因为其简约性而多被后续研究集成应用到其他算法，如A*[10]使用R&S曲线条件改善路径质量，CL-RRT[11]使用距离提高取样点生成“良率”。

图2.5曲线[10] #

曲线的曲率随弦长线性变化，在轨迹平滑中极具优势，如Kim等人提出了一种基于曲线的联通机器人运动规划算法，除了减短了路径宽度，也增加了曲率变化量[12]。

2.3 #

基于优化的规划算法 #

第三类算法是主要是将多目标多变量多约束耦合的规划模型转化为有权重的惩罚函数，并优化求解，一般用于初始路径的平滑。

#

Ren等人针对势场法存在振荡等问题，将改进牛顿法优化方式应用于连续势场导航模型，并考虑了机器人非全向约束和联通障碍物影响，极大提高了系统性能，但存在优化估算成本较高的问题[13]。

等人针对高维运动规划问题提出CHOMP（）算法（图2.6），先创建起始位置到中止位置的初始路径，针对成本函数使用梯度增长法对轨迹进行优化，获得平滑、无碰撞的轨迹，但存在容易深陷局部最小值的问题，为此引入了使用MonteCarlo算法施加扰动，以重新启动优化过程，但是，引入了随机性，进而增加了优化结果的确定性。 #

图2.6CHOMP[14] #

与之相仿，等人提出的STOMP()算法不须要目标函数梯度信息，因而可提升机器人运动规划性能，通过生成带有噪音的轨迹探求初始轨迹周围的空间，以形成较低成本的轨迹。其随机性也克服了基于梯度方式存在的局部最小值问题[14]。 #

等人使用A*生成的初始路径，并以路径曲率最小化为目标，使用共轭梯度法优化得到平滑路径，并将存在碰撞情况的对应原始路径点作为锚点重新优化，以保证路径安全可靠[10]。 #

等人提出一种局部连续的轨迹规划方式，通过结合公路边沿及障碍物将工作空间转化为凸空间，并引入连续可微的伪距离建立伪梯度场，综合模型硬约束及舒适性等要求，转化为带约束的非线性规划问题，并使用序列二次规划算法求解[15]。 #

、R?smann、等人将“橡皮筋受内、外力作用形成形变”的原理应用于路径平滑处理，EB（Bands）[16]算法使用气泡带算法生成不连续的初始路径，再使用“橡皮筋”形变模型优化路径形状，生成无碰撞的平滑路径。TEB（TimedBand）[17]将静态、动态约束（速率、加速度等）和机器人状态使用稀疏图描述（强扩充特点），并转化为加权的单目标优化问题，并使用大规模最小二加法求解，生成符合约束最低成本的轨迹（见图2.7），具有极强的动态循迹和重规划能力，并应用于多种类型的履带机器人。DT-TEB（TEBin）[18]将RRT算法、图分别与TEB相结合，实时生成局部拓扑空间中全局最优轨迹，并延长了生成轨迹的宽度，提升循迹性能。CES()[19]算法针对类车机器人运动规划模型，将气泡带初始路径形状“拉伸”优化及给定路径下速率优化问题构造为两个凸优化问题，并迭代优化路径曲线-速率曲线，实时生成平滑轨迹。

图2.7TEB算法[17] #

03 #

曲线配准拟合算法剖析 #

上述三类曲线配准拟合算法各有各的特性，接出来对比剖析这三类规划算法的异同点： #

3.1

基于插补的规划算法 #

主要优点：便于估算，曲线形态灵活多变；

#

主要缺点：曲率连续性不保证

主要优点：估算成本低，控制点可形成期望的曲线，曲线间可互相联接； #

主要缺点：高阶曲线无法调节，全局路径点影响整条曲线，控制点较难设置

主要优点：估算成本低，曲率连续便于保证，控制点容易控制曲线形状； #

主要缺点：无法平衡曲线形状和连续性需求

#

3.2 #

基于特殊曲线的规划算法 #

主要优点：便于估算，且为最短路径；

#

主要缺点：路径的曲率不保证连续

主要优点：路径曲率变化线性，曲率连续性容易保证；

#

主要缺点：算法中的积分无法估算 #

3.3

#

基于优化的规划算法

主要优点：各种软硬约束便于综合处理，曲线连续性便于保证； #

主要缺点：优化变量越多，历时越久，且易深陷局部最小值。 #

总体而言，基于配准拟合算法早已才能在众多场景下的规划生成一条无碰撞路径，且应用较为广泛，例如TEB算法就被作为ROSstack中local的算法之一，曲线或R&S曲线常被应用于手动泊车领域。随着众多学者的改进升级，基于配准拟合算法的实时性和动态适应性日渐提高，但多数算法仍存在优化估算容易深陷局部最小值、计算复杂等问题，但随着硬件估算能力的提高，相关算法早已被应用于实际。

#

04

推论与展望

本文将曲线插补拟合算法拆解为三个子类算法：基于配准的规划算法、基于特殊曲线的规划算法及基于优化的规划算法，并沿时间先后次序概述了相关类型算法的发展改进历程，接着从模型复杂度、计算实时性、动态环境适应能力及路径曲线质量等方面剖析了上述三类算法的异同点，并指出存在的相关问题。 #

运动规划算法种类繁杂，应用场景各不相同，而本文仅概述剖析了四类运动规划算法之一的曲线插补拟合算法，后续会剖析其他类型算法。

（文章仅笔者个人剖析，有误请见谅，感谢！）

参考资料 #

[1]REIFJ,WANGH.Theofthetwo--path;oftheThirdonFound

[2]CHANGS-R,HUHU-Y.A-freeG2path-using[J].of,2014,11(12):194. #

[3]M,S,J,etal.fortime-using[J].Theof,2012,31(3):346-59. #

[4]XUW,WEIJ,DOLANJM,etal.Areal-timewithfor;ofthe2012IEEEonand,F,2012[C].IEEE.

#

[5]JP,A.forthebestpathandin[J].and,2023,114(93-105. #

[6]HANL,H,NEJADHTN,etal.curvebasedpathforinurban;ofthe2010IEEE,F,2010[C].IEEE. #

[7]GONZáLEZD,PéREZJ,R,etal.withinurban;ofthe17thIEEEon(ITSC),F,2014[C].IEEE. #

[8]M,SIMICM,JAZARR.B-[J].IEEEon,2023,17(2):406-19.

[9]H,A,B,etal.NURBsandbyanrobotin3D;oftheThe4thIEEEonCyberin,and,F,2014[C].IEEE.

[10]D,THRUNS,M,etal.Pathforinsemi-[J].Theof,2010,29(5):485-501.

[11]Y,TEOJ,FIOREG,etal.Real-timewithtourban[J].IEEEon,2009,17(5):1105-18.

[12]KIMY,PARKJB,SONW,etal.turnforbasedoncurve[J].,2023,53(24):1574-6.

#

[13]RENJ,KA,PATELRV.'stofield-basedforin[J].,2008,26(1):117-27. #

[14]M,S,E,etal.STOMP:for;ofthe2011IEEEonand,F,2011[C].IEEE. #

[15]J,P,DANGT,etal.for—Alocal,;ofthe2014IEEE,F,2014[C].IEEE.

#

[16]S,O.bands:pathand;ofthe[1993]IEEEonand,F,1993[C].IEEE.

[17]R?SMANNC,W,W?SCHT,etal.of;ofthe2012;7thon,F,2012[C].VDE.

[18]R?SMANNC,F,T.andin[J].and,2023,88(142-53. #

[19]ZHUZ,E,M.Atoforwithcar-like;ofthe202354thIEEEonand(CDC),F,2023[C].IEEE.of,F,1998[C].

#

福利放送 #

笔者为男子伴们整理了刊物论文版式原文PDF，便捷收藏和回味

#

链接：/s/aLQ #

提取码：v1sz #

若链接失效加速度路程公式推导，可在后台回复本文标题或发送短信： #