基于轨迹位移与转向角映射函数的泊车横纵向协调控制方法和系统与流程

本发明涉及智能泊车，更具体地，涉及一种基于轨迹位移与转向角映射函数的泊车横纵向协调控制方法和系统。

背景技术：

1、apa(全自动泊车辅助)功能是自动驾驶辅助系统(adas)中一项非常有意义的功能。通过控制车辆的加减速度和转向角度自动停放车辆，使用avm(环视)和uss(超声波雷达)感知泊车环境，使用imu和车轮传感器估计车辆姿态(位置和行驶方向)，并根据驾驶员的选择自动或手动设置目标泊车位；然后系统进行自动泊车轨迹计算，并通过精确的车辆定位与车辆控制系统使车辆沿定义的泊车轨迹进行全自动泊车，直至到达最终目标泊车位。

2、目前泊车控制的主流方案是依据规划的轨迹采用pid、lqr、mpc等控制算法解算横纵向控制指令分别驱动车辆横纵向运动跟踪目标轨迹，专利cn202210955651.0即基于该种方法，该种方法存在如下技术缺陷：

3、由于线控底盘横纵向指令响应存在延迟且延迟时间不固定等特点，所以此方案存在横纵向控制协调性差等缺陷，导致车辆启动停止时存在原地调整转向的问题。

技术实现思路

1、本发明的目的之一是提供一种基于轨迹位移与转向角映射函数的泊车横纵向协调控制方法，解决当车辆启动停止时存在原地调整转向的问题；本发明的目的之二是提供一种基于轨迹位移与转向角映射函数的泊车横纵向协调控制系统。

2、为解决上述技术问题，本发明的技术方案如下：

3、本发明的第一方面提供一种基于轨迹位移与转向角映射函数的泊车横纵向协调控制方法，包括以下步骤：

4、s1：建立轨迹位移与转向角的映射函数；

5、s2：选取车辆位移作为纵向控制指令，车辆转向角作为横向控制指令；

6、s3：根据当前时刻的车辆转向角判断当前时刻的车辆转向阶段，所述车辆转向阶段包括启动/停止阶段和固定圆弧阶段；

7、s4：根据当前时刻的期望位移和当前时刻的实际位移，计算下一时刻的纵向控制指令；

8、s5：当当前时刻的车辆转向阶段为启动/停止阶段时，根据所述下一时刻的纵向控制指令和所述轨迹位移与转向角的映射函数，得到下一时刻的横向控制指令；

9、当当前时刻的车辆转向阶段为固定圆弧阶段时，横向控制指令固定；

10、s6：重复步骤s3至s5，直至车辆泊车完成。

11、进一步的，步骤s1中建立轨迹位移与转向角的映射函数，包括：

12、在t时刻车辆转向角δ(t)如下：

13、

14、式中，α为转向固定角速度，ωmax表示最大转向角速度，t1表示转向加速时间，t2表示转向匀速时间，t表示开始转向到结束转向的时间；

15、在t时刻车辆纵向速度角v(t)如下：

16、

17、式中，a表示固定纵向加速度，vmax表示纵向最大速度；

18、根据公式1和公式2，得到在t时刻车辆相对泊车车位坐标系的航向角θ(t)为：

19、

20、式中，k表示车辆转向角和前轮倾角的转换系数，l表示车辆轴距；

21、根据公式2，得到在t时刻车辆的位移s(t)为：

22、s(t)＝∫0tv(t)dt   公式4

23、根据公式3和公式4，得到轨迹位移与转向角的映射函数δ(s)。

24、进一步的，步骤s3中根据当前时刻的车辆转向角判断当前时刻的车辆转向阶段，包括：

25、将车辆泊车轨迹中任一曲线分为启动阶段变曲率圆弧ab、固定曲率圆弧bc和停止阶段变曲率圆弧cd；

26、首先进入启动阶段变曲率圆弧ab，此时所述车辆转向阶段为启动阶段；

27、当车辆转向阶段为启动阶段时且下一时刻的横向控制指令大于等于b点的目标转向角，此时所述车辆转向阶段转为固定圆弧阶段；

28、当车辆转向阶段为固定圆弧阶段时且车辆航向角大于等于c点的目标航向角，此时所述车辆转向阶段转为停止阶段；

29、当车辆转向阶段为停止阶段时且下一时刻的横向控制指令大于等于d点的目标转向角，当前曲线跟踪完成。

30、进一步的，所述当车辆转向阶段为固定圆弧阶段时且车辆航向角大于等于c点的目标航向角，此时所述车辆转向阶段转为停止阶段，还包括：

31、所述当车辆转向阶段为固定圆弧阶段时且车辆航向角加上转向延迟补偿角度大于等于c点的目标航向角，此时所述车辆转向阶段转为停止阶段。

32、进一步的，所述转向延迟补偿角度，包括：

33、测试实车底盘在不同速度下的转向延迟时间，根据实车底盘标定不同车速下的转向延迟时间得到转向延迟时间函数tδ(v)；

34、依据当前时刻的实际车速vact和转向延迟时间函数tδ(v)得到当前转向延迟时间tδ，计算得到转向延迟补偿角度δθ。

35、进一步的，所述计算得到转向延迟补偿角度δθ，包括：

36、

37、式中，r表示车辆当前转弯半径，δact表示当前转向角。

38、进一步的，步骤s4中根据当前时刻的期望位移和当前时刻的实际位移，计算下一时刻的纵向控制指令，包括：

39、snext＝star-sact

40、式中，snext为下一时刻的纵向控制指令，star为当前时刻的期望位移，sact为当前时刻的实际位移。

41、进一步的，步骤s5中根据所述下一时刻的纵向控制指令和所述轨迹位移与转向角的映射函数，得到下一时刻的横向控制指令，包括：

42、根据下一时刻的纵向控制指令反向查找所述轨迹位移与转向角的映射函数，得到下一时刻的横向控制指令。

43、进一步的，步骤s5中当当前时刻的车辆转向阶段为固定圆弧阶段时，横向控制指令固定，包括固定横向控制指令为期望横向控制指令。

44、本发明的第二方面提供一种基于轨迹位移与转向角映射函数的泊车横纵向协调控制系统，包括：

45、映射函数建立模块，所述映射函数建立模块建立轨迹位移与转向角的映射函数；

46、选取模块，所述选取模块选取车辆位移作为纵向控制指令，车辆转向角作为横向控制指令；

47、转向阶段判断模块，所述转向阶段判断模块根据当前时刻的车辆转向角判断当前时刻的车辆转向阶段，所述车辆转向阶段包括启动/停止阶段和固定圆弧阶段；

48、纵向控制指令计算模块，所述纵向控制指令计算模块根据当前时刻的期望位移和当前时刻的实际位移，计算下一时刻的纵向控制指令；

49、横向控制指令计算模块，所述横向控制指令计算模块当当前时刻的车辆转向阶段为启动/停止阶段时，根据所述下一时刻的纵向控制指令和所述轨迹位移与转向角的映射函数，得到下一时刻的横向控制指令；当当前时刻的车辆转向阶段为固定圆弧阶段时，横向控制指令固定；

50、重复模块，所述重复模块重复转向阶段判断模块至横向控制指令计算模块的步骤，直至车辆泊车完成。

51、与现有技术相比，本发明技术方案的有益效果是：

52、本发明考虑线控底盘的横纵向控制特性，本发明建立了轨迹位移与转向角度之间的映射关系，优化了横纵向控制的协调性，解决了当车辆启动停止时存在原地调整转向的问题。