一种基于多相机环视信息的铰链式车辆轨迹规划方法

本发明涉及自动驾驶，具体涉及一种基于多相机环视信息的铰链式车辆轨迹规划方法。

背景技术：

1、多相机环视系统分布于铰链式车辆的各级车体上，包括非刚性连接系统和刚性连接系统两类，可以对车身周围环境进行全向无盲区感知，提供丰富的多视角图像，因此广泛用于车辆的轨迹规划。车辆通过匹配多视角图像与语义地图进行位姿估计，并以搜索的形式求解车辆轨迹。但目前基于多视角图像的车辆轨迹规划方法通常匹配图像的几何特征，存在参数依赖性强、鲁棒性差的问题，因此在刚性连接系统提供的动态、随机的多视角图像下，针对非刚性系统的车辆轨迹规划方法无法适应。

2、目前，针对非刚性系统的车辆轨迹规划方法通常采用双层启发式搜索的方式，以提高求解速度，增强实时性。双层启发式搜索方法包含两层搜索方法，第一层次得出的粗解用于启发第二层次的快速搜索，目前包括a*-最优控制，状态晶格-最优控制，状态晶格-图搜索，强化学习-晶格外搜索等方式，但实时性差。另一种搜索方法是通过确定性策略和搜索方法切换以提高实时性，但适应场景有限。

3、因此，目前亟需一种车辆轨迹规划方法，能够适应非刚性连接的多相机环绕系统，提高轨迹规划的可行性与实时性。

技术实现思路

1、有鉴于此，本发明提供了一种基于多相机环视信息的铰链式车辆轨迹规划方法，能够提高规划的可行性、实时性和跟踪精度。

2、为实现上述发明目的，本发明的技术方案为：

3、一种基于多相机环视信息的铰链式车辆轨迹规划方法，步骤包括：

4、s0、在铰链式车辆的各级车体位姿的时空关系组成车辆的状态可行域内进行搜索，离线获取通行策略集。

5、s1、在铰链式车辆的各级车体上搭载多个相机，组成多相环视系统，采集车辆的环视图像。

6、根据环视图像对各相机间相对位姿进行估计，结合各相机间相对位姿将各相机对其自身视场环境的独立语义认知转换到鸟瞰视角下的地面坐标系，组成行驶环境的鸟瞰环视语义认知图。

7、s2、建立鸟瞰环视语义认知图和地面的语义地图之间的全局映射，构建车辆位姿预测网络，采用车辆位姿预测网络计算车辆位姿。

8、s3、根据鸟瞰环视语义认知图获取行驶环境的准静态表征。

9、s4、以车辆位姿作为起始点，融合行驶环境的准静态表征和通行策略集，获取策略链，根据策略链获取车辆的预测轨迹。

10、进一步的，还包括s5：按照预测轨迹对车辆进行跟踪并计算跟踪精度，根据跟踪精度确定是否更新通行策略集，更新方式为：当跟踪精度不低于期望，不更新通行策略集，并返回s1继续进行车辆轨迹规划，直至车辆行驶至目的地；当跟踪精度低于期望，采用车辆的真实轨迹替换预测轨迹，并返回s1继续进行车辆轨迹规划，直至车辆行驶至目的地。

11、进一步的，对各相机间相对位姿进行估计的方式为：采用图神经网络对各相机采集的环视图像进行特征关联计算，获取各相机之间的相对位姿；采用区域共视传递机制对相对位姿进行回环优化，得到相对位姿的初步估计结果；采用卡尔曼滤波方法将相机的相对位姿的初步估计结果与各级车体之间的相对位姿的历史数据融合，得到相机的相对位姿估计结果。

12、进一步的，车辆位姿预测网络的构建方式为：

13、根据鸟瞰空间语义认知图建立鸟瞰语义特征编码器，获取每个环视图像在鸟瞰视角下对应的环视特征；根据语义地图建立语义地图编码器，获取行驶环境的语义特征；建立融合自注意力机制和交叉注意力机制的transformer模型，以语义特征和环视特征作为输入量，两个输入量的匹配特征作为目标输出对其进行训练，得到反映鸟瞰环视语义认知图和语义地图的全局映射关系的transformer模型；采用该transformer模型输出的匹配特征作为输入量，采用牵引车位姿作为输出量，对神经网络进行训练，得到车辆位姿预测网络。

14、进一步的，车辆位姿的计算方式为：根据车辆位姿预测网络获取牵引车位姿，基于各级车体间的铰链角度和车辆运动学模型推演各级车体的位姿，得到车辆位姿。

15、进一步的，获取通行策略集的具体方式为：采用车路耦合模型获取不同行驶环境对应的各级车体位姿的时空关系，组成车辆的状态可行域；对行驶环境的环境参数进行随机采样，搜索该行驶环境下状态可行域内的车辆通行策略，得到通行策略初解；根据预设的策略评价指标对通行策略初解进行筛选，并计算通行策略间的相似度，根据相似度将通行策略初解进行聚类，得到通行策略集；其中，车路耦合模型的构建方式为：以牵引车的历史运行轨迹，及每个历史运行轨迹下各级车体之间的铰链角度、车辆的运动学模型推演牵引车位姿与各级拖车位姿的时空关系；根据行驶环境及时空关系之间的耦合关系建立车路耦合模型。

16、进一步的，s4的具体方式为：以车辆位姿作为起始点，对行驶环境的准静态表征和通行策略集进行序列化匹配，得到策略链；将策略链中各策略节点进行松弛得到对应的可达集合，在可达集合中采样，获取每个策略节点对应的位置，组成初始轨迹；对初始轨迹进行平滑，得到车辆的预测轨迹。

17、有益效果：

18、1、本发明提出一种基于多相机环视信息的铰链式车辆轨迹规划方法，根据环视图像估计相机相对位姿，结合各相机的独立语义认知，获取鸟瞰环视语义认知图，与语义地图建立规划时间内不变的全局匹配关系，基于匹配特征完成车辆位姿的高精度估计，通过提高车辆位姿估计精度和匹配特征的长时鲁棒性来提升环境感知和定位精度，为后续的轨迹规划提供高精度的待融合数据。本发明根据鸟瞰环视语义认知图和车辆位姿估计设计了车辆轨迹规划方法，将构建好的通行策略集与准静态表征和车辆位姿进行融合，完成轨迹规划，提高轨迹规划的实时性和全备性；基于预测轨迹进行跟踪，通过跟踪精度更新通行策略集，提高轨迹规划的可行性。

19、2、本发明基于鸟瞰环视语义认知图提取环视图像的环视特征，以环视特征作为对行驶环境进行环视感知的衡量指标，与各相机本身视角下的环视图像对应的环视特征相比，可以更好地区分不同物体的语义认知，完成物体识别。

20、3、本发明构建基于通行策略启发的轨迹搜索方法，提高轨迹规划的实时性和完备性，建立了轨迹规划与跟踪精度的更新机制，在跟踪精度低于期望时用真实轨迹替换预测轨迹，及时纠正轨迹规划，可行性和实时性更好。

21、4、本发明采用区域共视传递机制对相对位姿进行回环优化，结合车辆相对位姿的历史数据进行补偿，可以对非刚性连接的相机的相对位姿进行精准估计，为相机的独立语义认知的语义融合奠定基础。

22、5、本发明通过建立语义特征与环视特征的transformer模型获取二者的匹配特征，经匹配特征得到车辆位姿预测网络，与现有技术中采用几何特征匹配不同，本发明通过高层特征(如语义特征)的匹配实现环视图像的和语义地图的深度匹配，提高匹配特征的长时鲁棒性，进而提高定位精度。

23、6、本发明建立融合自注意力机制和交叉注意力机制的transformer模型，获取反映鸟瞰环视语义认知图和语义地图的全局映射关系的匹配特征，进而得到车辆位姿预测网络，能够更好地完成环视图像与语义地图的语义特征匹配，以更精确地估计车辆位姿。

24、7、本发明通过车辆位姿、行驶环境的准静态表征与通行策略集进行序列化匹配，在短时域内依据前一步匹配得出的策略不断更新行驶环境以确定下一步策略，逐步得到策略链以获取预测轨迹，规划精度更高。