本发明属于智能车辆，具体涉及一种分布式驱动转向智能车辆分层轨迹规划方法及系统。
背景技术：
1、目前智能车辆多基于传统的驱动转向构型进行后装改制，车辆的机动性和操控性受到传统构型的限制，难以适应狭窄拥堵、障碍物密集的复杂非结构化场景。而分布式驱动转向构型由于其具有高灵活性和通过性，且有利于实现集成化、轻量化和模块化设计，将成为未来智能车辆的理想载体。
2、对于传统驱动转向构型车辆的轨迹规划，按照规划方式可以分为基于搜索的方法、基于采样的方法、基于曲线插值的方法、基于数值优化的方法和基于学习的方法。而当前分布式驱动转向车辆基本沿用传统构型的规划方法，未考虑自身的四个车轮能够独立控制驱动和转向的特点，没有发挥出其在狭窄拥堵、障碍物密集的复杂非结构化场景中高灵活性和通过性的优势。同时，在保证分布式驱动转向车辆轨迹的连续性、平滑性和安全性的基础上，也应该适当考虑轨迹的舒适性和高效性。
3、综上所述，在狭窄拥堵、障碍物密集的复杂非结构化场景中，如何使分布式驱动转向智能车辆生成连续、平滑、舒适、安全、高效的轨迹是一个亟待解决的问题。
技术实现思路
1、本发明公开了一种分布式驱动转向智能车辆分层轨迹规划方法及系统，上层利用改进混合a*算法搜索速度快、适用性好的优点，搜索并构造按模式切换点进行分段的初始轨迹，下层利用数值优化方法对四轮转向模式的初始轨迹段进行优化，提升轨迹的质量，从而可以有效解决背景技术中涉及的至少一项技术问题。
2、为实现上述目的，本发明的技术方案为：
3、一种分布式驱动转向智能车辆分层轨迹规划方法，包括以下步骤：
4、步骤s1，分析分布式驱动转向车辆的运动学模型，获取非结构化场景相关参数；
5、步骤s2，重新设计混合a*算法的代价函数与节点扩展方式，搜索出按模式切换点进行分段的粗略路径，并构造对应的初始轨迹以作为轨迹优化的初始解；
6、步骤s3，采用基于行车走廊的数值优化方法对初始轨迹进行优化，设计考虑轨迹平滑性、舒适性、安全性、高效性的目标函数与约束条件，构造轨迹优化最优控制问题，然后对轨迹优化最优控制问题进行求解得到最优轨迹。
7、作为本发明的一种优选改进，步骤s1中，分布式驱动转向车辆的运动学模型包括四轮转向模式、楔形转向模式和原地转向模式，其中：
8、四轮转向模式下车辆前后车轮转角大小相同、方向相反，并且左右两侧车轮在任意时刻状态相同，车辆位姿既有平移又有旋转；
9、楔形转向模式下车辆前后车轮转角大小和方向均相同，并且左右两侧车轮在任意时刻状态相同，车辆位姿只有平移没有旋转；
10、原地转向模式下车辆绕着中心点进行原地旋转，车辆位姿只有旋转没有平移。
11、作为本发明的一种优选改进，步骤s1中，非结构化场景相关参数包括车辆的几何参数和运动学参数、起始点位姿终止点位姿以及非结构化场景障碍物地图；车辆的几何参数包括前悬长lf、后悬长lr、轴距lm、总长l、总宽b，车辆的运动学参数包括最小速度vmin、最大速度vmax、最小加速度amin、最大加速度amax、最大转向角度δmax、最大转向角速度ωmax。
12、作为本发明的一种优选改进，步骤s2中的节点扩展方式包括四轮转向模式节点扩展、楔形转向模式节点扩展和原地转向模式节点扩展，算法在每一次迭代搜索中，父节点可在离散空间中搜索出三种不同模式的子节点其中，下标i表示父节点信息，下标i+1表示子节点信息。
13、作为本发明的一种优选改进，当拓展步长为l1、横摆角变化量为时，四轮转向模式直行子节点位姿为：
14、
15、式中，di+1＝1表示车辆处于前向行驶状态，di+1＝-1表示车辆处于倒车行驶状态；
16、四轮转向模式转向子节点位姿为：
17、
18、式中，r为转弯半径；
19、当拓展步长为l2、横摆角变化量为时，楔形转向模式子节点位姿为：
20、
21、式中，为离散化后的车轮转角；
22、当拓展步长为l3＝0、横摆角变化量为时，原地转向模式子节点位姿为：
23、xi+1＝xi
24、yi+1＝yi
25、
26、式中，为横摆角。
27、作为本发明的一种优选改进，步骤s2中，代价函数包括累计代价函数、启发代价函数、模式切换代价函数和安全代价函数；其中，累计代价函数为：
28、
29、式中，g1(ni+1)表示车辆处于四轮转向模式拓展子节点时的累计代价函数，g2(ni+1)表示车辆处于楔形转向模式拓展子节点时的累计代价函数，g3(ni+1)表示车辆处于原地转向模式拓展子节点时的累计代价函数；mi+1＝1表示车辆处于四轮转向模式，mi+1＝2表示车辆处于楔形转向模式，mi+1＝3表示车辆处于原地转向模式；
30、启发代价函数为：
31、h(ni+1)＝max{hnonholonomics(ni+1)，hholonomics(ni+1)}
32、式中，hnonholonomics(ni+1)表示符合车辆运动学约束但是忽略碰撞因素的估计路径的长度，防止探索路径从错误的方向到达目标节点；hholonomics(ni+1)表示考虑避障因素但不考虑运动学可行性的估计路径的长度，防止探索路径进入断路或者凹形障碍物里面；
33、模式切换代价函数为：
34、
35、式中，mr(ni+1)为恢复代价函数；mc(ni+1)为切换代价函数；
36、恢复代价函数mr(ni+1)表示为：
37、
38、式中，vr1、vr2、vr3分别表示四轮转向模式、楔形转向模式、原地转向模式恢复到初始状态的代价；
39、切换代价函数mc(ni+1)表示为：
40、
41、式中，vc1表示四轮转向模式与楔形转向模式相互切换时的代价；vc2表示四轮转向模式与原地转向模式相互切换时的代价；vc3表示楔形转向模式与原地转向模式相互切换时的代价。
42、安全代价函数为：
43、
44、式中，sj表示单个障碍物产生的安全代价，其表达式为：
45、
46、式中，krep表示障碍物斥力正比例系数；rj表示当前节点到第j个障碍物几何中心的欧几里得距离；ρ0表示障碍物影响距离。
47、作为本发明的一种优选改进，步骤4中，约束条件包括车辆运动学约束、两点边值约束和行车走廊约束，轨迹优化最优控制问题表示为：
48、
49、
50、式中，表示最优控制问题中需要最小化的目标函数；约束条件中的第一项与第二项为两点边值约束，[xstart，ustart]与[xfinal，ufinal]分别表示优化起始时刻t＝ts和优化终止时刻t＝tf对应的状态量和控制量；第三项至第五项为车辆运动学约束，表示车辆非线性运动学方程，[xmin，xmax]与[umin，umax]表示状态量与控制量允许的取值范围；最后一项为行车走廊约束，表示车辆状态到运动足迹的映射。
51、本发明还提供了一种分布式驱动转向智能车辆分层轨迹规划系统，包括：
52、环境感知模块，用于获取车辆周围环境信息与自身状态信息，同时采取相关算法进行处理得到障碍物信息与车辆状态信息，并输入到轨迹规划模块；
53、轨迹规划模块，用于根据环境感知模块输入的障碍物信息与车辆状态信息和驾驶任务确定的车辆起始位姿与终止位姿，采取分层轨迹规划方法进行规划得到车辆运动轨迹信息及对应车辆运动模式信息，并输入到运动控制模块；
54、运动控制模块，用于对轨迹规划模块输入的车辆运动轨迹信息及其不同轨迹段的运动模式，选取相应模式的运动控制器来控制车辆以实现轨迹跟踪。
55、作为本发明的一种优选改进，障碍物信息包括障碍物的类型和包络多边形顶点位置，车辆状态信息包括车辆的位置、横摆角、速度、加速度和角速度。
56、本发明的有益效果如下：
57、本发明设计了一套完整的面向复杂非结构化场景的分层轨迹规划算法理论，相比较完全的传统混合a*算法，路径曲线更加符合分布式驱动转向智能车辆的运动特点，提高了轨迹跟踪的安全性和高效性；相比直接使用数值优化算法，具有更快的计算速度与更高的优化成功率，最终实现了分布式驱动转向智能车辆能够在狭窄拥堵、障碍物密集的复杂非结构化场景中生成连续、平滑、舒适、安全、高效的轨迹。
1.一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，步骤s1中，分布式驱动转向车辆的运动学模型包括四轮转向模式、楔形转向模式和原地转向模式，其中：
3.根据权利要求1所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，步骤s1中，非结构化场景相关参数包括车辆的几何参数和运动学参数、起始点位姿终止点位姿以及非结构化场景障碍物地图；车辆的几何参数包括前悬长lf、后悬长lr、轴距lm、总长l、总宽b，车辆的运动学参数包括最小速度vmin、最大速度vmax、最小加速度amin、最大加速度amax、最大转向角度δmax、最大转向角速度ωmax。
4.根据权利要求3所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，步骤s2中的节点扩展方式包括四轮转向模式节点扩展、楔形转向模式节点扩展和原地转向模式节点扩展，算法在每一次迭代搜索中，父节点可在离散空间中搜索出三种不同模式的子节点其中，下标i表示父节点信息，下标i+1表示子节点信息。
5.根据权利要求4所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，当拓展步长为l1、横摆角变化量为时，四轮转向模式直行子节点位姿为：
6.根据权利要求5所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，步骤s2中，代价函数包括累计代价函数、启发代价函数、模式切换代价函数和安全代价函数，其中，累计代价函数为：
7.根据权利要求6所述的一种分布式驱动转向智能车辆分层轨迹规划方法，其特征在于，步骤4中，约束条件包括车辆运动学约束、两点边值约束和行车走廊约束，轨迹优化最优控制问题表示为：
8.一种实现权利要求1-7任意一项所述的方法的分布式驱动转向智能车辆分层轨迹规划系统，其特征在于，包括：
9.根据权利要求8所述的一种分布式驱动转向智能车辆分层轨迹规划系统，其特征在于，障碍物信息包括障碍物的类型和包络多边形顶点位置，车辆状态信息包括车辆的位置、横摆角、速度、加速度和角速度。
