CN111459160A - 一种开放道路无人洗扫车大规模轨迹平滑方法 - Google Patents

Abstract

本发明公开了一种开放道路无人洗扫车大规模轨迹平滑方法，连接车道中心线，获取大规模原始指引线轨迹；将原始指引线轨迹按照固定距离线性插值，建立无人洗扫车的运动学模型，并设置轨迹点属性；将带有轨迹点属性的指引线轨迹分段，并开辟平滑器线程；根据开放道路无人洗扫车的任务场景需求，设置平滑处理的目标函数；构建几何学约束及运动学模型相关约束；在平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分求解；从而得到各个平滑处理后轨迹点坐标，组成平滑后的轨迹，使无人洗扫机按照该轨迹进行运动，完成整个平滑处理过程。能保证无人洗扫车按照设定轨迹稳定运行，并且平滑处理效率较高，大大缩短平滑优化时间。

Description

一种开放道路无人洗扫车大规模轨迹平滑方法

技术领域

本发明涉及一种大规模轨迹平滑方法，具体是一种开放道路无人洗扫车大规模轨迹平滑方法。

背景技术

无人洗扫车是一种无需人工干预而能够感知其周边环境，识别障碍物及各类标识，对各类开放道路自主进行喷洒及清扫作业的车辆。它将根据环境及任务需求，实时规划路径，并自动跟随，主要工作在城市道路非机动车道及部分机动车道等开放道路中。

在开放道路中，无人洗扫车主要由两类规划路径的方式：(1)获取车道中心线，依次连接作为指引线；(2)通过规划算法规划合理路径。洗扫车单次任务路径较长，可能为十几公里甚至更长，为了减少制图的复杂性，每条车道取的路径点较少，一般隔几米取一个点，但总体路径规模仍较大。通过以上两种方式获取的路径，在道路弯曲处由于点的稀疏性、地图本身属性(如直角弯等)、高精地图制图误差等造成弯折角度过大或连续弯折情况频繁。

无人洗扫车采用阿克曼轮模型，与汽车一致，与差速轮模型及全向轮模型相比，带有典型的角度约束，不适用在小角度转弯或连续转弯场景。且体型较大，两侧附带喷洒水及旋转清扫装置，宽度约为普通非机动车道一半以上，当出现连续弯折或弯折角度过大路径时，不利于保持整车控制精度，影响前进速度及运行效率。

因此，开放道路的无人洗扫车需要平滑的路径，并且必须在考虑其本身所拥有的各项参数(尺寸、速度、角度、加速度、角速度)，运动学模型(阿克曼模型)以及其他约束(可行的曲率，车道尺寸)的同时生成平滑的轨迹。

此外，由于任务路径距离较长，规模较大，故需要保证平滑效率符合无人扫地车任务需求。因此，需要采用满足车辆本身约束，路径点几何约束、安全性高且计算效率满足需求的平滑方式。

目前主流的轨迹平滑方式有：

(1)基于插值的轨迹平滑：

多项式插值、贝塞尔曲线、三次样条、B样条、NURBS曲线。

优点：易于计算；曲线可串联以获取所需要的形状；适合于局部规划

缺点：高阶曲线的系数难以控制；高阶曲线很耗时，不适合高速使用

(2)使用特殊曲线进行轨迹平滑：

杜宾斯曲线、回旋线、内摆线；这几种方式计算效率较高，杜宾斯曲线可以确保最短轨迹，但曲率连续性差，在直线和圆弧的过渡点会发生跳动；回旋线很容易获得曲率连续性，常用作过渡曲线，但其Fresne积分比较难计算；摆线方式易于计算，可生成所需的角度，但无法保证曲率连续性，不适合无人清扫车。

(3)基于优化的平滑方法：线性优化、凸优化、非线性优化等。

优点：可以考虑各种约束，容易结合其他方法进行使用

缺点：优化比较耗时，对设立的约束正确性要求高

基于插值的平滑方法以及使用特殊曲线的平滑方法，更多的会考虑轨迹点本身的性质，对于局部轨迹平滑有着较好的效果，但对于具体的某一类或某一个机器人而言，并不能充分结合具体车辆的运动学模型进行轨迹平滑，而基于优化的平滑方法，则可以通过设立目标函数及约束，很好的结合机器人的运动学模型建立多种合理约束，使得平滑后的每个轨迹点都满足运动学约束。但是其存在优化比较耗时的缺点，因此如何研究一种轨迹平滑方法，能将多种平滑方法结合，使其具有更好的优点，并且其缺点更少，是本行业亟需解决的技术问题。

发明内容

针对上述现有技术存在的问题，本发明提供一种开放道路无人洗扫车大规模轨迹平滑方法，能使平滑后的轨迹与原始轨迹尽可能相似，从而保证无人洗扫车按照设定轨迹稳定运行，并且平滑处理效率较高，大大缩短平滑优化时间。

为了实现上述目的，本发明采用的技术方案是：一种开放道路无人洗扫车大规模轨迹平滑方法，具体步骤为：

步骤一、获取原始指引线轨迹：连接车道中心线，获取大规模原始指引线轨迹；

步骤二、将步骤一获取的大规模原始指引线轨迹按照固定距离线性插值，获取插值指引线；

步骤三、建立无人洗扫车的运动学模型，根据运动学模型获取无人洗扫车本身参数限制，并设置轨迹点属性；

步骤四、将带有轨迹点属性的指引线轨迹分段，并开辟平滑器线程；

步骤五、根据开放道路无人洗扫车的任务场景需求，设置平滑处理的目标函数；

步骤六、分析步骤二得到的插值指引线轨迹，构建几何学约束；然后分析步骤三建立的无人洗扫车运动学模型，构建运动学模型相关约束；

步骤七、设置迭代次数，在步骤四建立的平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，输入设定的目标函数、运动学约束、几何学约束，开始求解；

步骤八、将步骤七的求解得到的各个平滑处理后轨迹点坐标，组成平滑后的轨迹，使无人洗扫机按照该轨迹进行运动，完成整个平滑处理过程。

进一步，所述步骤二具体为：获取大规模原始指引线轨迹后，通过线性插值的方式，获得步长相等的新轨迹；其中设定插值步长为0.4，设定原始轨迹相邻点距离在1.0～2.0之间。

进一步，所述步骤三具体为：设定无人洗扫车运动为阿克曼运动学模型，在惯性坐标系下建立阿克曼运动学模型，设(X_r,Y_r)，(X_f,Y_f)分别为车辆后轴和前轴轴心坐标，θ为车体横摆角，δ_f为前轮偏角，v_r和v_f分别为车辆后、前轴中心速度，l为轴距；R为后轮转向半径，P为车辆的瞬时转动中心，M,N分别为车辆后轴、前轴轴心；并确定车辆瞬时转向半径与道路曲率半径相同；

在后轴行驶轴心(X_r,Y_r)处，速度为：

前、后轴运动学约束为：

根据式(1)、(2)联合后得到：

根据前后轮几何关系得到：

联合上式，得到横摆角速度为：

综上，能得到无人洗扫车的运行学模型为：

上式中

为后轮X方向速度，

为后轮Y方向速度，

为前轮偏转角速度；将前轮偏转角δ_f作为轨迹点的其中一个属性，代表无人洗扫车在当前轨迹点的偏转角；

设轨迹点的坐标(x_i,y_i),以及当前点的角度，用以下公式进行计算：

因此能得到轨迹点的三个属性，x,y,θ；

综上，确定轨迹点的固有属性共四个，分别为x,y,θ,δ，对每个点进行初始化；

获取无人洗扫车实际运行时的参数限制，包括车辆轴距l；最大速度限制v_max，最大加速度限制a_max；最大前轮偏转角δ_max，最大角速度w_max。

进一步，所述步骤四具体为：设定分段距离对轨迹分段，采用多线程处理的方式，对其中一个轨迹段平滑处理时对后续轨迹进行获取，使无人洗扫机在获取第一段平滑的结果后即开始运动；

搭建非线性优化平滑器：

变量初始化：

对轨迹中的每一个点，确定其分成两类属性，一类为x，一类为g，其中x代表每个点的四个固有属性；g为每个点所要满足的约束个数；设轨迹中轨迹点的个数为ne，因此得到：

x矩阵的大小为n_x×1，其中n_x＝4×ne+1；

g矩阵的大小为n_g×1，其中n_g＝4×ne+1；

对x矩阵进行初始化，使用之前保存的每个点的原始属性，依次进行初始化；首先对第一个值传入原始轨迹总长度：x_i＝path_length i＝0，接下来继续初始化：

其中，states_j代表原始点的参数值，x_i为平滑后的点的参数值，使用states_j为x_i赋初值。

进一步，所述步骤五具体为：目标函数包含两个部分：

①平滑后指引线轨迹尽可能贴合原始规划轨迹：

其中

为平滑前轨迹点坐标，x_i，y_i为平滑后轨迹；

②相邻轨迹点的无人洗扫车前轮偏转角变化之和尽可能小：

则目标函数为：

f_min＝ad₁+bd₂ (11)

其中a，b分别为以上两部分的权重，根据具体任务场景需求进行配置；

其中目标函数的第一项为总轨迹长度，设每一段平滑前后的长度差的平方为l_i，和为l_sum，则：

l_i＝(states_i→x-x_i+1)²+(states_i→y-x_ne+i+1)i∈[0,ne-1] (12)

第二项为洗扫车相邻点前轮偏转角度变化，设平滑后相邻点的前轮偏转角度度差平方为δ_i，和为δ_sum，则：

δ_i＝(x_3ne+i-x_3ne+i-1)² i∈[2,ne] (14)

设平滑权重分别为a,b，则得到目标函数为：

f_min＝a×l_sum+b×θ_sum (16)。

进一步，所述步骤六具体为：

构建几何学约束为：

设相邻三个轨迹点中，处于中间的轨迹点在平滑前和平滑后，都能满足在另外两个轨迹点连线的垂线上，因此在原始轨迹中，根据相邻三点之间的关系，得到每一个点的垂线方程，即:ax+by+c＝0；

根据每一点的前后点关系，求得每个点的a,b,c值，也把这个值保存为点的属性之一；

则约束方程为：

g_i＝(states_j→a)×x_j+(states_j→b)×x_ne+j+c j,i∈[2,ne-1] (17)

为了保证平滑后的轨迹点与平滑前相比，顺序一致，因此，另一个约束为点的顺序约束；使用向量的方式来表示：设三个顺序轨迹点为A、B、C，为保证轨迹点在平滑处理后顺序不变，则建立

向量和

向量，满足：

则两个向量顺序相同，由此可得约束方程为：

g_i＝(x_j+1-x_j)×(x_j+2-x_j+1)+(x_ne+j+1-x_ne+i)×x_ne+j+2-x_ne+j+1 (18)

其中i∈[ne+1,2ne-2]，j∈[1,ne-2]；

构建运动学模型相关约束，具体为：

根据洗扫车的转向运动学模型：

可得相邻轨迹点之间的关系：

式(19)等式两端同乘以Δt可得：

dx为连续两轨迹点x方向位移，dy为连续两轨迹点y方向位移，θ为当前点的角度值，δ为当前点洗扫车的前轮偏转角，d_δ为当前点到下一点偏转角角度变化量，ds为当前点到下一点的位移；

综上，得到两个关于运动学模型的约束方程：

g_i＝(x_j+1-x_j)×sin(x_2ne+j)-(x_ne+j+1-x_ne+j)×cos(x_2ne+j) (21)

其中，i∈[2ne+1,3ne-1]，j∈[1,ne-1]，上式为运动学模型的第一个约束；

其中i∈[3ne+1,4ne-1],j∈[1,ne-1]；

综上，共设置四个约束，分别为：点的垂直约束、点的顺序约束、机器人运动学x,y约束，机器人运动学角度约束；

设定各个约束的上下限及点属性的上下限：

变量x:

x_i∈(-∞,+∞)，其中i∈[0,3ne]；

x_i∈(-δ_max,+δ_max)，其中i∈[3ne+1,4ne]

对于约束g：

点的垂直约束：g_i＝0，其中i∈[0,ne]；

点的顺序约束：g_i∈(0,+∞)，其中i∈[ne+1,2ne]；

运动学x,y约束：g_i＝0，其中i∈[2ne+1,3ne]；

运动学角度约束：g_i＝0，i∈[3ne+1,4ne]。

进一步，所述步骤七具体为：将变量矩阵x，约束矩阵g以及目标函数f输入平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，根据设定的迭代次数进行求解，求解完成后获得每一个轨迹点对应的x,y值。

与现有技术相比，本发明具有以下优点：

(1)构建无人洗扫车运动学约束作为非线性优化中的约束条件，保证平滑后的指引线轨迹点满足机器人运动学约束；

(2)对指引线轨迹中的点搭建合理的几何学约束，保证平滑后相邻轨迹点满足一定的几何约束，消除不满足几何条件的可能；

(3)对原始指引线轨迹按照一定步长进行线性插值，使插值后相邻点距离相同，有利于求解，得到的平滑点距离更加合理；

(4)使用IPOPT线性优化库，可以求解大规模NLP问题，并结合先进的casadi自动微分工具，使用C++语言搭建完整优化框架，提升优化效率；

(5)采用轨迹分段结合多线程，洗扫车可以边走边平滑，大大提升系统整体效率。

附图说明

图1是本发明的整体流程图；

图2是本发明中多线程流程图；

图3是本发明中无人洗扫车的阿克曼运动模型示意图；

图4是本发明中无人洗扫车的前轮转向示意图；

图5是本发明中轨迹点属性示意图；

图6是本发明中轨迹点的垂直约束示意图；

图7是本发明中轨迹点的顺序约束示意图；

图8是采用本发明的平滑轨迹与原始轨迹的对比图；

其中，(a)直线部分；(b)微弯曲部分；(c)单向转弯1；(d)单向转弯2；

图9是采用本发明的平滑轨迹与原始轨迹的局部放大对比图；

其中，(a)连续直角转弯部分；(b)U型弯道部分；

图10是本发明的平滑轨迹、原始轨迹和贝塞尔平滑器的平滑轨迹的对比图；

其中，(a)单向转弯部分；(b)连续直角转弯部分；(c)U型弯道部分；(d)微弯曲部分。

具体实施方式

下面将对本发明作进一步说明。

如图1所示，本发明具体步骤为：

步骤一、获取原始指引线轨迹：连接车道中心线，获取大规模原始指引线轨迹；

步骤二、将步骤一获取的大规模原始指引线轨迹按照固定距离线性插值，获取插值指引线；具体为：

获取大规模原始指引线轨迹后，通过线性插值的方式，获得步长相等的新轨迹；其中插值步长为0.4，原始轨迹相邻点距离在1.0～2.0之间；经过线性插值后，轨迹点增多，更为密集，整条线特征也变得更多，有利于进行接下来的平滑任务；

步骤三、建立无人洗扫车的运动学模型，根据运动学模型获取无人洗扫车本身参数限制，并设置轨迹点属性；具体为：

设定无人洗扫车运动为阿克曼运动学模型，在惯性坐标系下建立阿克曼运动学模型，设(X_r,Y_r)，(X_f,Y_f)分别为车辆后轴和前轴轴心坐标，θ为车体横摆角，δ_f为前轮偏角，v_r和v_f分别为车辆后、前轴中心速度，l为轴距；R为后轮转向半径，P为车辆的瞬时转动中心，M,N分别为车辆后轴、前轴轴心；并确定车辆瞬时转向半径与道路曲率半径相同；

在后轴行驶轴心(X_r,Y_r)处，速度为：

前、后轴运动学约束为：

根据式(1)、(2)联合后得到：

根据前后轮几何关系得到：

联合上式，得到横摆角速度为：

综上，能得到无人洗扫车的运行学模型为：

上式中

为后轮X方向速度，

为后轮Y方向速度，

为前轮偏转角速度；将前轮偏转角δ_f作为轨迹点的其中一个属性，代表无人洗扫车在当前轨迹点的偏转角；

设轨迹点的坐标(x_i,y_i),以及当前点的角度，可以用以下公式进行计算：

因此能得到轨迹点的三个属性，x,y,θ；

综上，确定轨迹点的固有属性共四个，分别为x,y,θ,δ，对每个点进行初始化；

获取无人洗扫车实际运行时的参数限制，包括车辆轴距l；最大速度限制v_max，最大加速度限制a_max；最大前轮偏转角δ_max，最大角速度w_max。

步骤四、将带有轨迹点属性的指引线轨迹分段，并开辟平滑器线程；具体为：

设定分段距离对轨迹分段，采用多线程处理的方式，对其中一个轨迹段平滑处理时对后续轨迹进行获取，使无人洗扫机在获取第一段平滑的结果后即开始运动；

搭建非线性优化平滑器：

变量初始化：

对轨迹中的每一个点，确定其分成两类属性，一类为x，一类为g，其中x代表每个点的四个固有属性；g为每个点所要满足的约束个数；设轨迹中轨迹点的个数为ne，因此得到：

x矩阵的大小为n_x×1，其中n_x＝4×ne+1(多余的1代表第一个值，表示轨迹总长度，用来进行校验)；

g矩阵的大小为n_g×1，其中n_g＝4×ne+1(多余的1代表第一个值，在这里赋值为0)；

对x矩阵进行初始化，使用之前保存的每个点的原始属性，依次进行初始化；首先对第一个值传入原始轨迹总长度：x_i＝path_length i＝0，接下来继续初始化：

其中，states_j代表原始点的参数值，x_i为平滑后的点的参数值，使用states_j为x_i赋初值。

步骤五、根据开放道路无人洗扫车的任务场景需求，设置平滑处理的目标函数；具体为：

目标函数包含两个部分：

①平滑后指引线轨迹尽可能贴合原始规划轨迹：

其中

为平滑前轨迹点坐标，x_i，y_i为平滑后轨迹；

②相邻轨迹点的无人洗扫车前轮偏转角变化之和尽可能小：

则目标函数为：

f_min＝ad₁+bd₂ (11)

其中a，b分别为以上两部分的权重，根据具体任务场景需求进行配置；

其中目标函数的第一项为总轨迹长度，为便于计算，设每一段平滑前后的长度差的平方为l_i，和为l_sum，则：

l_i＝(states_i→x-x_i+1)²+(states_i→y-x_ne+i+1)i∈[0,ne-1] (12)

第二项为洗扫车相邻点前轮偏转角度变化，为便于计算，设平滑后相邻点的前轮偏转角度度差平方为δ_i，和为δ_sum，则：

δ_i＝(x_3ne+i-x_3ne+i-1)² i∈[2,ne] (14)

设平滑权重分别为a,b，则得到目标函数为：

f_min＝a×l_sum+b×θ_sum (16)

步骤六、分析步骤二得到的插值指引线轨迹，构建几何学约束；然后分析步骤三建立的无人洗扫车运动学模型，构建运动学模型相关约束；并设定各个约束的上下限及点属性的上下限；

构建几何学约束，具体为：

设相邻三个轨迹点中，处于中间的轨迹点在平滑前和平滑后，都能满足在另外两个轨迹点连线的垂线上(即轨迹点设为A、B、C，B点满足A点和C点连线的垂线上)，因此在原始轨迹中，根据相邻三点之间的关系，得到每一个点的垂线方程，即:ax+by+c＝0；

根据每一点的前后点关系，求得每个点的a,b,c值，也把这个值保存为点的属性之一；

则约束方程为：

g_i＝(states_j→a)×x_j+(states_j→b)×x_ne+j+c j,i∈[2,ne-1] (17)

为了保证平滑后的轨迹点与平滑前相比，顺序一致，因此，另一个约束为点的顺序约束；为使用向量的方式来表示：设三个顺序轨迹点为A、B、C，为保证轨迹点在平滑处理后顺序不变，则建立

向量和

向量，满足：

则两个向量顺序相同，由此可得约束方程为：

g_i＝(x_j+1-x_j)×(x_j+2-x_j+1)+(x_ne+j+1-x_ne+i)×x_ne+j+2-x_ne+j+1 (18)

其中i∈[ne+1,2ne-2]，j∈[1,ne-2]。

构建运动学模型相关约束，具体为：

根据洗扫车的转向运动学模型：

可得相邻轨迹点之间的关系：

式(19)等式两端同乘以Δt可得：

dx为连续两轨迹点x方向位移，dy为连续两轨迹点y方向位移，θ为当前点的角度值，δ为当前点洗扫车的前轮偏转角，d_δ为当前点到下一点偏转角角度变化量，ds为当前点到下一点的位移；

综上，得到两个关于运动学模型的约束方程：

g_i＝(x_j+1-x_j)×sin(x_2ne+j)-(x_ne+j+1-x_ne+j)×cos(x_2ne+j) (21)

其中，i∈[2ne+1,3ne-1]，j∈[1,ne-1]，上式为运动学模型的第一个约束；

其中i∈[3ne+1,4ne-1],j∈[1,ne-1]。

综上，共设置四个约束，分别为：点的垂直约束、点的顺序约束、机器人运动学x,y约束，机器人运动学角度约束；

设定各个约束的上下限及点属性的上下限：

变量x:

x_i∈(-∞,+∞)，其中i∈[0,3ne]；

x_i∈(-δ_max,+δ_max)，其中i∈[3ne+1,4ne]

对于约束g：

点的垂直约束：g_i＝0，其中i∈[0,ne]。

点的顺序约束：g_i∈(0,+∞)，其中i∈[ne+1,2ne]。

运动学x,y约束：g_i＝0，其中i∈[2ne+1,3ne]。

运动学角度约束：g_i＝0，i∈[3ne+1,4ne]；

步骤七、设置迭代次数，在步骤四建立的平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，输入设定的目标函数、运动学约束、几何学约束，开始求解；

Ipopt(Interior Point Optimizer)是用于大规模非线性优化的开源软件包。它可以用来解决一般形式的非线性编程问题，以下为NLP问题原理：

上式为NLP问题模型，其中，x∈Rⁿ是最优化变量(可能具有上下限，x^L∈(RU{-∞})ⁿ，x^U∈(RU{+∞})ⁿ)，f:Rⁿ→R是目标函数，g:Rⁿ→R^m是一般的非线性约束。函数f(x)和g(x)可以是线性或非线性的，也可以是凸的或非凸的(但应该连续两次可微)。约束g(x)具有上下限g^L∈(RU{-∞})^m，g^U∈(RU{+∞})^m。

IPOPT主要使用内点法进行优化求解，需要对函数进行一次及二次微分，因此这里用到一个自动微分工具Casadi，可以快速求出需要的雅克比矩阵及海森矩阵，相比于其他自动微分工具，Casadi基于动态框架搭配独有的SX、MX矩阵，在求导中性能更加优异。

具体求解为：将变量矩阵x，约束矩阵g以及目标函数f输入平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，根据设定的迭代次数进行求解，求解完成后获得每一个点对应的x,y值；

步骤八、将步骤七的求解得到的各个平滑处理后轨迹点坐标，组成平滑后的轨迹，使无人洗扫机按照该轨迹进行运动，完成整个平滑处理过程。

对本发明的平滑结果进行验证：

将本发明的平滑结果与传统的贝塞尔曲线平滑方法的平滑结果进行对比，评价本发明设计的平滑方法对无人洗扫车指引线轨迹的平滑效果。

(1)本发明的平滑结果(即IPOPT平滑器平滑)：

将多组无人洗扫车实际场景下的原始指引线，采用本发明的平滑方法进行整体平滑：

如图所示，图8(a)为原始轨迹(即origin path)为长直线部分的平滑结果，图8(b)为有轻微弯曲的长直线部分，由箭头轨迹的平滑结果可以看到(1)本发明的平滑器会在直线部分贴近原始轨迹；(2)为了保证转向角不改变过大，在微弯曲部分，会根据约束条件和目标函数，得到更为平滑的轨迹。

图8(c)和(d)分别为单向转弯轨迹图，从图中可发现，经过平滑后，在原本存在弯折的单向弯道处，可获得一条连续且满足约束的轨迹，它充分考虑到无人洗扫车各项参数及运动学模型，减少车辆前轮转向角连续变化。

图9(a)为连续直角弯，存在角度变化过急的拐点，且两个直角距离较近，经过本发明的方式进行平滑后，得到箭头轨迹；图9(b)为开放道路中可以进行车辆掉头的U型弯，蓝色原始轨迹曲率较大，经过平滑后，曲率减小，相邻点之间偏转角变化更加合理。

(2)贝塞尔平滑器(即bazier平滑器)对比

贝塞尔平滑器是使用贝塞尔曲线，设置控制点将一段轨迹使用贝塞尔曲线进行拟合，本发明的实验采用以6个点为一组进行平滑，与本发明的平滑器效果进行对比；

如图10可知，贝塞尔曲线在直线上贴近原始轨迹，与本发明平滑效果类似；但在弯曲的轨迹中，贝塞尔曲线会将弯曲严重的轨迹点进行平滑，且由于每次只选择连续的6个点，导致轨迹不够连续；而本发明的平滑器可以根据运动学模型约束及目标函数，考虑更多的轨迹点分布，效果更好。

由实际结果可见，本发明基于非线性优化的方式进行平滑，可以得到更好的平滑效果。

时间效率对比：

由于原始路径为开放道路无人洗扫车路径，整条路径长度约为5-10公里。本发明采用了分段平滑的方式，使用多线程的方式，优化时间效率，因此不需要平滑完整条轨迹才反馈；只需要一边平滑一边反馈即可，下面对比改进前后的反馈时间：

表1两种方式平滑时间对比

上表为每项数据统计20次求得平均值。可得整体平滑效率较低，随着轨迹点数量上升，时间消耗变长。而经过分段及多线程后，由于每次只平滑长度固定的一段距离，时间消耗较短，不随轨迹点规模增大而增长，有效利用计算资源，提升无人洗扫车系统效率。

Claims (7)

1.一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，具体步骤为：

步骤一、获取原始指引线轨迹：连接车道中心线，获取大规模原始指引线轨迹；

步骤二、将步骤一获取的大规模原始指引线轨迹按照固定距离线性插值，获取插值指引线；

步骤三、建立无人洗扫车的运动学模型，根据运动学模型获取无人洗扫车本身参数限制，并设置轨迹点属性；

步骤四、将带有轨迹点属性的指引线轨迹分段，并开辟平滑器线程；

步骤五、根据开放道路无人洗扫车的任务场景需求，设置平滑处理的目标函数；

步骤六、分析步骤二得到的插值指引线轨迹，构建几何学约束；然后分析步骤三建立的无人洗扫车运动学模型，构建运动学模型相关约束；

步骤七、设置迭代次数，在步骤四建立的平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，输入设定的目标函数、运动学约束、几何学约束，开始求解；

步骤八、将步骤七求解得到的各个平滑处理后轨迹点坐标，组成平滑后的轨迹，使无人洗扫机按照该轨迹进行运动，完成整个平滑处理过程。

2.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤二具体为：获取大规模原始指引线轨迹后，通过线性插值的方式，获得步长相等的新轨迹；其中设定插值步长为0.4，设定原始轨迹相邻点距离在1.0～2.0之间。

3.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤三具体为：设定无人洗扫车运动为阿克曼运动学模型，在惯性坐标系下建立阿克曼运动学模型，设(X_r,Y_r)，(X_f,Y_f)分别为车辆后轴和前轴轴心坐标，θ为车体横摆角，δ_f为前轮偏角，v_r和v_f分别为车辆后、前轴中心速度，l为轴距；R为后轮转向半径，P为车辆的瞬时转动中心，M,N分别为车辆后轴、前轴轴心；并确定车辆瞬时转向半径与道路曲率半径相同；

在后轴行驶轴心(X_r,Y_r)处，速度为：

前、后轴运动学约束为：

根据式(1)、(2)联合后得到：

根据前后轮几何关系得到：

联合上式，得到横摆角速度为：

综上，能得到无人洗扫车的运行学模型为：

上式中

为后轮X方向速度，

为后轮Y方向速度，

为前轮偏转角速度；将前轮偏转角δ_f作为轨迹点的其中一个属性，代表无人洗扫车在当前轨迹点的偏转角；

设轨迹点的坐标(x_i,y_i),以及当前点的角度，用以下公式进行计算：

因此能得到轨迹点的三个属性，x,y,θ；

综上，确定轨迹点的固有属性共四个，分别为x,y,θ,δ，对每个点进行初始化；

获取无人洗扫车实际运行时的参数限制，包括车辆轴距l；最大速度限制v_max，最大加速度限制a_max；最大前轮偏转角δ_max，最大角速度w_max。

4.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤四具体为：设定分段距离对轨迹分段，采用多线程处理的方式，对其中一个轨迹段平滑处理时对后续轨迹进行获取，使无人洗扫机在获取第一段平滑的结果后即开始运动；

搭建非线性优化平滑器：

变量初始化：

对轨迹中的每一个点，确定其分成两类属性，一类为x，一类为g，其中x代表每个点的四个固有属性；g为每个点所要满足的约束个数；设轨迹中轨迹点的个数为ne，因此得到：

x矩阵的大小为n_x×1，其中n_x＝4×ne+1；

g矩阵的大小为n_g×1，其中n_g＝4×ne+1；

对x矩阵进行初始化，使用之前保存的每个点的原始属性，依次进行初始化；首先对第一个值传入原始轨迹总长度：x_i＝path_length i＝0，接下来继续初始化：

其中，states_j代表原始点的参数值，x_i为平滑后的点的参数值，使用states_j为x_i赋初值。

5.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤五具体为：目标函数包含两个部分：

①平滑后指引线轨迹尽可能贴合原始规划轨迹：

其中x_i ^·，y_i ^·为平滑前轨迹点坐标，x_i，y_i为平滑后轨迹；

②相邻轨迹点的无人洗扫车前轮偏转角变化之和尽可能小：

则目标函数为：

f_min＝ad₁+bd₂ (11)

其中a，b分别为以上两部分的权重，根据具体任务场景需求进行配置；

其中目标函数的第一项为总轨迹长度，设每一段平滑前后的长度差的平方为l_i，和为l_sum，则：

l_i＝(states_i→x-x_i+1)²+(states_i→y-x_ne+i+1) i∈[0,ne-1] (12)

第二项为洗扫车相邻点前轮偏转角度变化，设平滑后相邻点的前轮偏转角度度差平方为δ_i，和为δ_sum，则：

δ_i＝(x_3ne+i-x_3ne+i-1)² i∈[2,ne] (14)

设平滑权重分别为a,b，则得到目标函数为：

f_min＝a×l_sum+b×θ_sum (16)。

6.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤六具体为：

构建几何学约束为：

设相邻三个轨迹点中，处于中间的轨迹点在平滑前和平滑后，都能满足在另外两个轨迹点连线的垂线上，因此在原始轨迹中，根据相邻三点之间的关系，得到每一个点的垂线方程，即:ax+by+c＝0；

根据每一点的前后点关系，求得每个点的a,b,c值，也把这个值保存为点的属性之一；

则约束方程为：

g_i＝(states_j→a)×x_j+(states_j→b)×x_ne+j+c j,i∈[2,ne-1] (17)

为了保证平滑后的轨迹点与平滑前相比，顺序一致，因此，另一个约束为点的顺序约束；使用向量的方式来表示：设三个顺序轨迹点为A、B、C，为保证轨迹点在平滑处理后顺序不变，则建立

向量和

向量，满足：

则两个向量顺序相同，由此可得约束方程为：

g_i＝(x_j+1-x_j)×(x_j+2-x_j+1)+(x_ne+j+1-x_ne+i)×x_ne+j+2-x_ne+j+1 (18)

其中i∈[ne+1,2ne-2]，j∈[1,ne-2]；

构建运动学模型相关约束，具体为：

根据洗扫车的转向运动学模型：

可得相邻轨迹点之间的关系：

式(19)等式两端同乘以Δt可得：

dx为连续两轨迹点x方向位移，dy为连续两轨迹点y方向位移，θ为当前点的角度值，δ为当前点洗扫车的前轮偏转角，d_δ为当前点到下一点偏转角角度变化量，ds为当前点到下一点的位移；

综上，得到两个关于运动学模型的约束方程：

g_i＝(x_j+1-x_j)×sin(x_2ne+j)-(x_ne+j+1-x_ne+j)×cos(x_2ne+j) (21)

其中，i∈[2ne+1,3ne-1]，j∈[1,ne-1]，上式为运动学模型的第一个约束；

其中i∈[3ne+1,4ne-1],j∈[1,ne-1]；

综上，共设置四个约束，分别为：点的垂直约束、点的顺序约束、机器人运动学x,y约束，机器人运动学角度约束；

设定各个约束的上下限及点属性的上下限：

变量x:

x_i∈(-∞,+∞)，其中i∈[0,3ne]；

x_i∈(-δ_max,+δ_max)，其中i∈[3ne+1,4ne]

对于约束g：

点的垂直约束：g_i＝0，其中i∈[0,ne]；

点的顺序约束：g_i∈(0,+∞)，其中i∈[ne+1,2ne]；

运动学x,y约束：g_i＝0，其中i∈[2ne+1,3ne]；

运动学角度约束：g_i＝0，i∈[3ne+1,4ne]。

7.根据权利要求1所述的一种开放道路无人洗扫车大规模轨迹平滑方法，其特征在于，所述步骤七具体为：将变量矩阵x，约束矩阵g以及目标函数f输入平滑器线程内采用IPOPT非线性优化方式，并结合自动微分工具Casadi进行自动微分，根据设定的迭代次数进行求解，求解完成后获得每一个轨迹点对应的x,y值。

Images