第二章第二节(下) 参考线代码实践&问题
必要准确
1.全局路径
prescan打开testemplanner文件,并通过prescan打开matlab,
matlab自动配置环境,matlab中打开testemplanner文件夹,运行其中的init文件,实现初始化;打开slx文件
carsim打开DynamicModel
测试一下:先仿真下regenerate,车就可以跑起来了;再运行textctrl.m文件,发现实际轨迹符合规划轨迹,所以模型+环境没有问题
现在把全局路径的模块放进去,一般要配合定位和地图实现,我们这里用另一个方法直接导入全局路径
回到prescan开始造道路-inherited path definition-按esc退出,得到路径,这个路径就是我们要的全局路径(一般的路径要听过导航+定位算出)
这个路径没办法直接导入matlab,可以让车在prescan中安这个路径走,然后采集定位信息,就可以导入到matlab中了:重新选一辆车放道路上 ,线变绿且有结点;再设置车速、动力学模型,build之后regenerate,设置一下这个新车x-y的采集频率,注释掉旧车,300s差不多,然后运行采路径 在工作区得到文件global_path_x_original和global_path_y_original,然后提取数据中的只xy无时间的路径 并保存
在init文件中导入全局路径
这样刚刚采集数据的车可以删掉,刚刚被注释的车又要用了,并在其中导入全局路径
后面的编程要先看后完全自己写 不要看
问题
这里的输入是frenet坐标系下的,后面还要转换把-----不不不就是自然坐标系下的,只是这几个参数是路径规划出的路线,这个路线在自然坐标系下的坐标,后面应该要以他为s轴建立frenet坐标系(但prescan导出的就两个变量xy,参考线有4个参数,所以后面还会根据xy算?
感觉x_set应该是动态变化的,这个是只考虑一个规划周期内可能涉及到的投影点?
输出不存在的点就需要用NAN,而输入的向量在输之前也需要NAN一下
首次计算匹配点的时候,起名为pre??这个不就是该周期下的输出吗 后面好像没有用到哎–在最终版是init,感觉相关的几个参数都可以直接合成一个了
匹配点match_point_x感觉都不太需要用i存储,这是针对一个周期下的一个点 但是因为有多个点,所有就需要存储了
%通过匹配点找投影点–其实在基于路径找参考线的过程中可以不用找投影点,直接匹配点就行 那为啥还要这样做----为了严谨
这个好像还是没有改哎:下面写的有点问题,只适用于轨迹没有变化的情况:因为我们用上周期匹配点的下标在这个周期内找其匹配点,只用在上个周期保存匹配点编号信息+frenet_path信息(用以查找);这个对于参考线模块是没有问题的因为全局轨迹是定的,但不能作为通用模块
is_first_run = 0这个其实可以不等于0吧 感觉任何数字都行
代码好几块都重复感觉可以重复的部分内嵌函数
我的算法是先针对任意点,再判断是否第一次运行,老王是反的,这个有影响吗?而且我这样写了之后isnan只用判断一次
老王是判断x_set不是nan之后,是else,我是直接end,感觉也可以啊
老王increase_count = 0;这个在一开始就定义了一次,但我没有 也觉得不需要吧,因为第一次执行肯定在if 距离<min距离,那么还是会初始化的
老王init_match_point_index_set(i) = j是在判断if 距离<min距离的时候就写了,我是在>50次之后
老王判断遍历方向的时候没有找当前车位置与上一个匹配点连接的向量,应该是写错了!不然就是不需要判断,因为老王这样写,车速一直向前,当前无论车前/后移动,对结果都没有影响
老王vector_err = vector_d’vector_match_point_directionvector_match_point_direction;加了括号应该更严谨
在判断遍历方向的时候,我是直接判断处理,没有重复写句子了
注意算法我自己写的,和老王有点不一样,可能不对
算法模板搞好之后在slx中调一下输入输出
输入的话车的xy本来模型里就有,参考线的xy之前也从prescan中导出了,现在还缺参考线的heading和kappa,这两个将从xy计算得到
老王的dx=diff(path_x)会导致少一个量