[发明专利]一种自动驾驶编组车自动对接方法

权利要求书

1.一种自动驾驶编组车自动对接方法，其特征在于，包括：

S1、获取目标车辆标记；

S2、根据目标车辆标记的位置和姿态计算跟随车与目标车辆的相对位置和目标相对航向角；

S3、根据所得到的相对位置、目标相对航向角，利用Stanley算法确定跟随车基本转向角控制率和最近路径点。

S4、根据所得到的基本转向角控制率和最近路径点控制跟随车与目标车辆对接。

2.根据权利要求1所述的一种自动驾驶编组车自动对接方法，其特征在于，所述目标车辆标记为ArUco标记，固定于目标车辆尾部。

3.根据权利要求2所述的一种自动驾驶编组车自动对接方法，其特征在于，所述ArUco标记的创建方法为：

加载设定的预字典；

创建指定编号和尺寸的ArUco标记。

4.根据权利要求3所述的一种自动驾驶编组车自动对接方法，其特征在于，所述步骤S2中目标车辆标记的姿态计算方法为：

识别目标车辆标记的编号和对应的四个角；

根据目标车辆标记的真实边长以及跟随车辆相机标定的参数计算目标车辆的姿态，得到旋转向量rvec和平移向量tvec；

将得到的旋转向量rvec转换为旋转矩阵R；

将得到的平移向量tvec转换为平移矩阵T；

根据旋转矩阵R和平移矩阵T计算目标车辆标记与跟随车的相对位置，得到跟随车与目标车辆标记位置的水平距离x₀、垂直距离y₀以及目标车辆相对航向角δ。

5.根据权利要求4述的一种自动驾驶编组车自动对接方法，其特征在于，所述旋转矩阵表示为：

其中，r为空间B中各轴方向单位向量在空间A中各轴的分量，空间A为ArUco标记坐标中心的坐标系，其平行于地面和ArUco标记方向为空间A的x轴方向，垂直于地面和平行于ArUco标记方向为A空间y轴方向，平行于地面垂直于ArUco标记方向为空间A的z轴方向；空间B为跟随车辆上的摄像头的相机坐标空间。

6.据权利要求5述的一种自动驾驶编组车自动对接方法，其特征在于，所述目标车辆标记与跟随车的相对位置关系满足如下公式：

其中，T＝[t₁,t₂,t₃]，表示目标车辆标记在跟随车辆上的摄像头的相机坐标，t₁为标记在相机坐标中的x轴位置，t₂为标记在相机坐标中的y轴位置，t₃为标记在相机坐标中的z轴位置；(x₀,y₀)为标识中心在空间C中的坐标；δ为标识绕成像面垂直方向的轴形成的夹角为目标车辆相对航向角，空间C为跟随车辆上的摄像头光心为原点的坐标系，摄像头光轴为空间C的y轴方向，同时垂直光轴和平行于地面方向为空间C的x轴方向。

7.根据权利要求6述的一种自动驾驶编组车自动对接方法，其特征在于，所述跟随车辆相机标定的过程为：

利用跟随车摄像头拍摄任意角度和位置的多张棋盘图；

将拍摄的棋盘图像进行灰度化，找到每张图片中的棋盘图角点；

提取图像中的亚像素角点并储存；

进行相机标定，得到内参数矩阵和畸变系数。

8.根据权利要求7述的一种自动驾驶编组车自动对接方法，其特征在于，所述跟随车辆与最近路径点的距离表示为：

其中，k为跟随车的目标路径的斜率，目标路径为穿过ArUco标记中心并与标记垂直的直线。

9.根据权利要求8述的一种自动驾驶编组车自动对接方法，其特征在于，所述基本转向角控制率表示为：

其中，L为跟随车的前视距离，e车辆与最近路径点的距离。

10.根据权利要求9述的一种自动驾驶编组车自动对接方法，其特征在于，所述跟随车辆与目标车辆对接完成所要满足的条件为：

其中，x₀、y₀标识中心在空间C中的坐标，θ为转向角控制率。