坐标轨迹计算_无人驾驶轨迹拟合和二次轨迹计算

在轨迹重规划算法中,将目标函数设定为有限时域内与参考点的距离偏差最小,所规划出的轨迹是以预测时域内离散的点给出的。随着预测时域的增加、这些局部参考轨迹点的数量也会随之增加。直接将这些点输入控制层,占用了过多的控制器输入接口,不利于控制器的规范化设计。另外,由于规划层与控制层的控制周期并不一致,控制层很难根据离散的参考轨迹点完成轨迹跟踪的任务。

综合上述因素,有必要对轨迹重规划算法所规划出的局部参考路径进行处理,实现规划层与控制层的顺利对接。曲线拟合是对离散点处理的主要方式,根据所采用曲线的不同,有样条曲线拟合、多项式拟合以及幂指数拟合等。由于车辆存在运动学约束,如车辆位置连续要求曲线是连续的,横摆角连续要求曲线是一阶连续,而加速度约束则要求曲线二阶连续。因此,本书选择5次多项式作为拟合曲线,形式如下∶

50264bb14cd9a0927fa0c2b6d60dd804.png

设规划层预测时域N,=16,拟合结果如图所示。图中,星号为轨迹重规划算法在预测时域内规划出的离散点,曲线为拟合后的轨迹曲线。

以拟合后各数据点残差的范数来表征拟合质量,对整个轨迹重规划过程中的201次规划数据进行统计,统计结果如图所示。从图中可以看出,车辆位置横坐标Y的拟合残差最大值为0.003,车体航向角g的拟合残差最大值不超过7.50×10³,具备非常高的拟合精度。

9755df6a6ef0f3ae9f6b1784d0364bb0.png

在用S函数实现基于MPC的轨迹规划器时首先需要确定S函数的输入与输出量,如图所示。基于MPC的轨迹规划器将CarSim的输出作为输入,CaSim输出的参数为:

9936c9ccdb5690358770ebf085cb206d.png

分别代表车辆在Y和X坐标方向上的速度、航向角及航向角速度、在Y和X坐标轴下的坐标等,共6个参数。基于MPC的轨迹规划器的输出是规划好的期望轨迹方程的参数,因为选择了5次曲线进行曲线拟合,所以这里的输出量为10个。

6da3ad7937dbe3e9577a4002afb7b15a.png

第1部分为主函数。通过此函数完成程序的初始化,并设置S函数模块的输入、输出和状态变量的个数等。sys变量在不同的阶段表示不同的意义。

function[sys,x0,str,ts]=MPC_TrajPlanner(t,x,u,flag)该程序功能∶用点质量模型设计规划期,能够规避障碍物8程序版本V1.0,Matlab版本∶R201la,采用S函数的标准形式,%状态量=【y_dot,x_dot,phi,,YX】,控制量为前轮偏角ay
switch flag,
case0flag=0表示处于初始化状态,此时用函数mdlIni-
tializesizes进行初始化
[sys,x0,str,ts]=mdlInitializeSizes; Initializa
  • 0
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值