【技术实现步骤摘要】
本专利技术涉及轨迹跟踪，具体而言，涉及一种基于轨迹跟踪的倒车循迹控制方法及系统。
技术介绍
1、当驾驶车辆进入狭窄路段，前方无法前进且无法掉头时，对于一般驾驶员来说，倒车原路返回是比较困难的，为提高此场景下的行驶安全性，目前已有车辆上配置倒车循迹的功能，辅助驾驶员在一定距离内实现倒车原路返回，从而避免发生交通事故。
2、现有的倒车循迹控制方法，通过存储正向行驶时横向及纵向参数例如正向转角数组、正向转速数组和正向行驶距离，形成相应的拟合函数，并将之反向读取来控制车辆后退。该方法虽然可以实现倒车循迹功能，但当正向转角或转速数组存储区域满时，待存储值从数组第一个元素起，依次覆盖已有值，从而实现总是存储最近行驶过的一段距离的转角值及转速值，导致无法判别数组内相邻存储点之间位置的顺序关系，则由此进行的后续数据处理，无法保证数据的连续性。此外，该方法不参考车辆正向行驶轨迹，仅通过驱动电机行驶距离与电动助力转向系统转角、转速的对应关系记录，倒车循迹时无法做到对轨迹的闭环跟踪，存在与正向行驶的轨迹发生偏差从而导致交通事故的风险。
技术实现思路
1、本专利技术提供一种基于轨迹跟踪的倒车循迹控制方法及系统，用以克服现有技术中存在的至少一个技术问题。
2、第一方面，本专利技术实施例提供一种基于轨迹跟踪的倒车循迹控制方法，包括：正向轨迹存储阶段和倒车循迹阶段；其中：
3、所述正向轨迹存储阶段包括：
4、实时获取车辆姿态坐标和行驶里程；所述车辆姿态坐标包括车辆
5、判断所述行驶里程是否大于等于预设存储间隔，若是，将所述行驶里程清零，并存储所述车辆位置坐标；若否，继续实时获取所述车辆姿态坐标和所述行驶里程；
6、所述倒车循迹阶段包括：
7、依次读取正向轨迹存储阶段存储的所述车辆位置坐标，根据存储的所述车辆位置坐标、当前车辆位置坐标，计算循迹总里程；
8、判断所述循迹总里程是否小于预设循迹里程，若是，将所述循迹总里程作为最终循迹里程，若否，将所述预设循迹里程作为最终循迹里程；
9、实时获取车辆姿态坐标和行驶里程，根据所述行驶里程和所述最终循迹里程计算剩余行驶里程，并循迹剩余行驶里程；
10、获取初始目标轨迹点，计算当前车辆位置坐标与所述初始目标轨迹点之间的距离；
11、判断所述当前车辆位置坐标与所述初始目标轨迹点之间的距离是否大于预瞄距离，若是，将所述初始目标轨迹点作为最终目标轨迹点；否则，更新初始目标轨迹点，并判断所述当前车辆位置坐标与更新后的初始目标轨迹点之间的距离是否大于预瞄距离；
12、根据所述最终目标轨迹点计算方向盘转角控制值；
13、判断剩余行驶里程是否小于预设距离，若是，结束倒车循迹；若否，实时获取车辆姿态坐标和行驶里程，根据所述行驶里程和所述最终循迹里程计算剩余行驶里程，并循迹剩余行驶里程。
14、可选地，在所述正向轨迹存储阶段之前，还包括：
15、检测车辆的当前档位和正向行驶车速，若所述当前档位为前进挡、且正向行驶车速小于预设车速，则进入正向轨迹存储阶段；否则，将已存储的车辆位置坐标清空。
16、可选地，存储所述车辆位置坐标前，判断存储空间是否已满，若已满，根据先入先出策略，将第一个地址内存储的所述车辆位置坐标移除，并将要存储的所述车辆位置坐标存入末位地址内；否则将存储地址加1，并存入要存储的所述车辆位置坐标。
17、可选地，所述根据存储的所述车辆位置坐标、当前车辆位置坐标，计算循迹总里程，具体为：
18、利用两点间距离公式，对存储的所述车辆位置坐标执行累加计算，再加上当前车辆位置坐标与末位地址内车辆位置坐标之间的距离，循迹总里程为其中，store_x表示存储轨迹的x坐标，store_y表示存储轨迹的y坐标，n表示存储轨迹坐标的采样点数，x、y表示车辆位置坐标。
19、可选地，所述获取初始目标轨迹点，计算当前车辆位置坐标与所述初始目标轨迹点之间的距离，具体为：
20、读取末位地址内存储的车辆位置坐标，作为初始目标轨迹点；
21、获取当前车辆位置坐标；
22、计算当前车辆位置坐标与所述初始目标轨迹点之间的距离其中，x_ta、y_ta表示初始目标轨迹点的坐标，x、y表示当前车辆位置坐标。
23、可选地，所述更新初始目标轨迹点，具体为：
24、判断当前地址是否为所述存储空间的第一个地址，若是，将当前地址内存储的车辆位置坐标作为初始目标轨迹点；否则，读取前一个地址内存储的车辆位置坐标，并将前一个地址内存储的车辆位置坐标，作为初始目标轨迹点。
25、可选地，在车辆坐标系下，车辆后轴中心坐标为所述车辆位置坐标；所述根据所述最终目标轨迹点计算方向盘转角控制值，具体为：
26、设定所述车辆后轴中心经过所述最终目标轨迹点，则所述车辆后轴中心转弯半径为其中，x_tb、y_tb表示最终目标轨迹点的坐标，θ表示车辆航向角，x、y表示车辆位置坐标；
27、设车辆轴距为l，方向盘转角到车轮转角的传动比为μ，则方向盘转角控制值为
28、第二方面，本专利技术实施例提供一种基于轨迹跟踪的倒车循迹控制系统，包括：
29、数据获取模块，用于实时获取车辆姿态坐标和行驶里程；所述车辆姿态坐标包括车辆位置坐标；
30、判断模块，用于判断所述行驶里程是否大于等于预设存储间隔，若是，将所述行驶里程清零，并存储所述车辆位置坐标；若否，继续实时获取所述车辆姿态坐标和所述行驶里程；
31、计算模块，用于依次读取正向轨迹存储阶段存储的所述车辆位置坐标，根据存储的所述车辆位置坐标、当前车辆位置坐标，计算循迹总里程；
32、所述判断模块还用于判断所述循迹总里程是否小于预设循迹里程，若是，将所述循迹总里程作为最终循迹里程，若否，将所述预设循迹里程作为最终循迹里程；
33、所述计算模块还用于根据所述行驶里程和所述最终循迹里程计算剩余行驶里程；
34、轨迹点获取模块，用于获取初始目标轨迹点，计算当前车辆位置坐标与所述初始目标轨迹点之间的距离；
35、所述判断模块还用于判断所述当前车辆位置坐标与所述初始目标轨迹点之间的距离是否大于预瞄距离，若是，将所述初始目标轨迹点作为最终目标轨迹点；否则，更新初始目标轨迹点，并判断所述当前车辆位置坐标与更新后的初始目标轨迹点之间的距离是否大于预瞄距离；
36、转角控制值确定模块，用于根据所述最终目标轨迹点计算方向盘转角控制值；
37、所述判断模块还用于判断剩余行驶里程是否小于预设距离，若是，结束倒车循迹；若否，实时获取车辆姿态坐标和行驶里程，根据所述行驶里程和所述最终循迹里程计算剩余行驶里程，并循迹剩余行驶里程。
38、可选地，还包括：
39、检测模块，用于检测车辆的当前档位和正向行驶车速，若所述当本文档来自技高网...
【技术保护点】
1.一种基于轨迹跟踪的倒车循迹控制方法，其特征在于，包括：正向轨迹存储阶段和倒车循迹阶段；其中：
2.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，在所述正向轨迹存储阶段之前，还包括：
3.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，存储所述车辆位置坐标前，判断存储空间是否已满，若已满，根据先入先出策略，将第一个地址内存储的所述车辆位置坐标移除，并将要存储的所述车辆位置坐标存入末位地址内；否则将存储地址加1，并存入要存储的所述车辆位置坐标。
4.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，所述根据存储的所述车辆位置坐标、当前车辆位置坐标，计算循迹总里程，具体为：
5.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，所述获取初始目标轨迹点，计算当前车辆位置坐标与所述初始目标轨迹点之间的距离，具体为：
6.根据权利要求5所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，所述更新初始目标轨迹点，具体为：
7.根据权利要求6所述的基于轨迹跟踪的
8.一种基于轨迹跟踪的倒车循迹控制系统，其特征在于，包括：
9.根据权利要求8述的基于轨迹跟踪的倒车循迹控制系统，其特征在于，还包括：
10.根据权利要求8述的基于轨迹跟踪的倒车循迹控制系统，其特征在于，所述判断模块还用于判断存储空间是否已满，若已满，根据先入先出策略将首个存储的车辆位置坐标移除，并将要存储的车辆位置坐标存入末位地址内；否则将存储地址加1，并存入要存储的车辆位置坐标。
...【技术特征摘要】
1.一种基于轨迹跟踪的倒车循迹控制方法，其特征在于，包括：正向轨迹存储阶段和倒车循迹阶段；其中：
2.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，在所述正向轨迹存储阶段之前，还包括：
3.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，存储所述车辆位置坐标前，判断存储空间是否已满，若已满，根据先入先出策略，将第一个地址内存储的所述车辆位置坐标移除，并将要存储的所述车辆位置坐标存入末位地址内；否则将存储地址加1，并存入要存储的所述车辆位置坐标。
4.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，所述根据存储的所述车辆位置坐标、当前车辆位置坐标，计算循迹总里程，具体为：
5.根据权利要求1所述的基于轨迹跟踪的倒车循迹控制方法，其特征在于，所述获取初始目标轨迹点，计算当前车...
【专利技术属性】
技术研发人员：吕英超，吕荣华，
申请(专利权)人：苏州优达斯汽车科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。