[发明专利]一种开放道路无人洗扫车大规模轨迹平滑方法

权利要求书

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)；

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

构建几何学约束为：

设相邻三个轨迹点中，处于中间的轨迹点在平滑前和平滑后，都能满足在另外两个轨迹点连线的垂线上，因此在原始轨迹中，根据相邻三点之间的关系，得到每一个点的垂线方程，即: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进行自动微分，输入设定的目标函数、运动学约束、几何学约束，开始求解；

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