一种轮式移动平台的停靠控制方法、装置及设备与流程

本技术涉及自动驾驶车辆控制，具体涉及一种轮式移动平台的停靠控制方法、装置及电子设备。

背景技术：

1、在港口等作业场景中，轮式移动平台（如自动驾驶引导车）需要停靠至目标停车位，以完成轮胎吊、岸桥、轨道吊等装置下方的集装箱装卸，或在固定工位进行电池更换等作业。

2、在引导轮式移动平台时，现有自动驾驶系统通常采用全局定位体系，即通过获取轮式移动平台在地图坐标系中的绝对位姿来引导其停靠。然而，受卫星定位信号漂移、地图及传感器标定误差、时间同步误差等因素影响，全局定位精度通常为米级或分米级，而上述作业通常要求轮式移动平台停靠至目标停车位时，位置偏差在厘米级范围内。更为关键的是，全局定位的绝对位姿误差是相对于全局坐标系存在的，并不以目标停车位为基准进行修正。因此，即使轮式移动平台的绝对位姿足够精确，其与目标停车位之间仍可能残留较大的相对偏差。

3、因此，如何在现有全局定位体系下，进一步提高轮式移动平台停靠至目标停车位的停靠精度，是目前亟待解决的技术问题。

技术实现思路

1、本技术实施例提供一种轮式移动平台的停靠控制方法，该方法在现有全局定位体系下进一步提高轮式移动平台停靠至目标停车位的停靠精度。

2、本技术实施例提供一种轮式移动平台的停靠控制方法，包括：在轮式移动平台驶入包含目标停车位的观测区域后，获取轮式移动平台的相机采集的包含标定图案的连续图像帧，标定图案固定设置于与目标停车位具有固定空间关系的支撑结构上，观测区域为相机能够采集到标定图案的空间范围；

3、根据连续图像帧、相机相对于轮式移动平台的相机安装外参及轮式移动平台在全局坐标系下的移动平台全局位姿数据，获得标定图案在全局坐标系下的标定图案全局位姿数据；根据移动平台全局位姿数据、标定图案全局位姿数据及标定图案相对于目标停车位的预设的标定图案变换外参，获得移动平台与目标停车位之间的相对距离数据；当相对距离数据小于预设的距离阈值时，根据连续图像帧中的当前帧图像获得标定图案相对于相机的当前视觉位姿数据；根据当前视觉位姿数据和相机安装外参，获得轮式移动平台相对于标定图案的当前观测位姿数据；获取轮式移动平台的运动数据，并根据运动数据和标定图案全局位姿数据获得轮式移动平台相对于标定图案的当前预测位姿数据；根据当前观测位姿数据和当前预测位姿数据，获得轮式移动平台相对于标定图案的当前融合位姿数据；根据当前融合位姿数据和标定图案变换外参，获得轮式移动平台相对于目标停车位的当前停靠位姿数据；根据当前停靠位姿数据和预设的目标停靠位姿数据生成停靠控制指令，以控制轮式移动平台停靠于目标停车位。

4、可选地，根据连续图像帧、相机相对于轮式移动平台的相机安装外参及轮式移动平台在全局坐标系下的移动平台全局位姿数据，获得标定图案在全局坐标系下的标定图案全局位姿数据，包括：从连续图像帧中筛选出残差最小的目标图像帧，并获得标定图案相对于相机的初始视觉位姿数据；根据初始视觉位姿数据和相机相对于轮式移动平台的相机安装外参，获得轮式移动平台相对于标定图案的初始观测位姿数据；获取在目标图像帧对应的采集时刻轮式移动平台在全局坐标系下的移动平台全局位姿数据，并根据移动平台全局位姿数据和初始观测位姿数据，获得标定图案在全局坐标系下的标定图案全局位姿数据。

5、可选地，从连续图像帧中筛选出残差最小的目标图像帧，并获得标定图案相对于相机的初始视觉位姿数据，包括：获取连续图像帧中起始的预设数量帧图像，作为起始连续图像帧；对起始连续图像帧中的每一帧图像，提取标定图案的二维特征点；获取预先存储的标定图案上各特征点在标定图案坐标系下的三维坐标数据；根据二维特征点与三维坐标数据之间的对应关系，对每一帧图像进行多点透视位姿计算，获得每一帧图像对应的标定图案相对于相机的初始位姿数据；根据每一帧图像对应的初始位姿数据，将三维坐标数据重投影至每一帧图像的图像平面，计算二维特征点与重投影点之间的重投影误差；将重投影误差最小的一帧图像确定为目标图像帧，并将目标图像帧对应的初始位姿数据作为标定图案相对于相机的初始视觉位姿数据。

6、可选地，根据初始视觉位姿数据和相机相对于轮式移动平台的相机安装外参，获得轮式移动平台相对于标定图案的初始观测位姿数据，包括：获取相机相对于轮式移动平台的相机安装外参；根据初始视觉位姿数据和相机安装外参，获得标定图案相对于轮式移动平台的中间位姿数据；对中间位姿数据求逆，获得在目标图像帧对应的采集时刻轮式移动平台相对于标定图案的初始观测位姿数据。

7、可选地，根据移动平台全局位姿数据、标定图案全局位姿数据及标定图案相对于目标停车位的预设的标定图案变换外参，获得移动平台与目标停车位之间的相对距离，包括：根据标定图案全局位姿数据和标定图案相对于目标停车位的预设的标定图案变换外参，获得目标停车位在全局坐标系下的目标停车位全局位姿数据；根据移动平台全局位姿数据和目标停车位全局位姿数据，获得移动平台与目标停车位之间的相对距离数据。

8、可选地，根据连续图像帧中的当前帧图像获得标定图案相对于相机的当前视觉位姿数据，包括：获取连续图像帧中的当前帧图像；提取当前帧图像中标定图案的当前帧二维特征点；获取预先存储的标定图案上各特征点在标定图案坐标系下的三维坐标数据；根据当前帧二维特征点与三维坐标数据之间的对应关系，获得当前帧图像对应的标定图案相对于相机的当前视觉位姿数据。

9、可选地，获取轮式移动平台的运动数据，并根据运动数据和标定图案全局位姿数据获得轮式移动平台相对于标定图案的当前预测位姿数据，包括：获取在当前帧图像对应的采集时刻轮式移动平台在车体坐标系下的运动数据，运动数据包括轮式移动平台的线速度和角速度；根据运动数据，预测当前帧图像对应的采集时刻轮式移动平台在全局坐标系下的位姿数据，得到移动平台当前预测位姿数据；根据标定图案全局位姿数据，将移动平台当前预测位姿数据转换到标定图案的坐标系下，获得轮式移动平台相对于标定图案的当前预测位姿数据。

10、可选地，根据当前观测位姿数据和当前预测位姿数据，获得轮式移动平台相对于标定图案的当前融合位姿数据，包括：获取轮式移动平台在上一融合时刻的融合位姿数据及位姿误差状态数据，上一融合时刻为当前帧图像对应的采集时刻的前一融合输出时刻；根据运动数据，对位姿误差状态数据进行预测，得到当前帧图像对应的采集时刻的预测位姿误差状态数据；计算当前观测位姿数据与当前预测位姿数据之间的残差；根据残差对预测位姿误差状态数据进行修正，获得修正后的位姿误差状态数据；将修正后的位姿误差状态数据叠加至当前预测位姿数据，获得当前帧图像对应的采集时刻轮式移动平台相对于标定图案的当前融合位姿数据。

11、可选地，根据当前停靠位姿数据和预设的目标停靠位姿数据生成停靠控制指令，以控制轮式移动平台停靠于目标停车位，包括：计算当前停靠位姿数据与预设的目标停靠位姿数据之间的位姿偏差数据；根据位姿偏差数据，通过预设的控制算法，生成用于驱动轮式移动平台的速度控制量和/或转向控制量；基于速度控制量和/或转向控制量，输出对应的停靠控制指令，以控制轮式移动平台停靠于目标停车位。

12、本技术实施例还提供一种轮式移动平台的停靠控制装置，包括：全局位姿初始化单元，用于在轮式移动平台驶入包含目标停车位的观测区域后，获取轮式移动平台的相机采集的包含标定图案的连续图像帧，标定图案固定设置于与目标停车位具有固定空间关系的支撑结构上，观测区域为相机能够采集到标定图案的空间范围；根据连续图像帧、相机相对于轮式移动平台的相机安装外参及轮式移动平台在全局坐标系下的移动平台全局位姿数据，获得标定图案在全局坐标系下的标定图案全局位姿数据；停靠距离判断单元，用于根据移动平台全局位姿数据、标定图案全局位姿数据及标定图案相对于目标停车位的预设的标定图案变换外参，获得移动平台与目标停车位之间的相对距离数据；视觉观测单元，用于当相对距离数据小于预设的距离阈值时，根据连续图像帧中的当前帧图像获得标定图案相对于相机的当前视觉位姿数据；根据当前视觉位姿数据和相机安装外参，获得轮式移动平台相对于标定图案的当前观测位姿数据；运动预测单元，用于获取轮式移动平台的运动数据，并根据运动数据和标定图案全局位姿数据获得轮式移动平台相对于标定图案的当前预测位姿数据；融合估计单元，用于根据当前观测位姿数据和当前预测位姿数据，获得轮式移动平台相对于标定图案的当前融合位姿数据；分阶段停靠控制单元，用于根据当前融合位姿数据和标定图案变换外参，获得轮式移动平台相对于目标停车位的当前停靠位姿数据；根据当前停靠位姿数据和预设的目标停靠位姿数据生成停靠控制指令，以控制轮式移动平台停靠于目标停车位。

13、本技术还提供一种电子设备，电子设备包括处理器和存储器；存储器中存储有计算机程序，处理器运行计算机程序后，执行上述方法。

14、本技术还提供一种计算机存储介质，计算机存储介质存储有计算机程序，计算机程序被处理器运行后，执行上述方法。

15、与现有技术相比，本技术实施例具有以下优点：

16、本技术利用轮式移动平台上搭载的相机观测标定图案，并结合其自身的全局位姿数据，在作业过程中在线反推计算标定图案的全局位姿数据，从而彻底免除了对目标停车位进行预先场地勘察的步骤。在此基础上，系统实时获取轮式移动平台与目标停车位之间的相对距离，当该相对距离小于预设阈值时，切换至视觉精调阶段。在视觉精调阶段，系统根据当前帧图像计算标定图案相对于相机的视觉位姿数据，并结合相机安装外参、运动数据及前述标定图案全局位姿数据，计算出轮式移动平台相对于标定图案的融合位姿数据；进而结合标定图案变换外参，最终获得轮式移动平台相对于目标停车位的当前停靠位姿数据。

17、本技术通过设定相对距离阈值作为切换条件，有效避免了在轮式移动平台与目标停车位之间的相对距离较远时过早引入视觉位姿，从而规避了因标定图案成像过小或特征提取不稳定而引入的额外误差，保证了停靠控制全过程在不同距离阶段均能获取适宜的位姿依据。更为关键的是，本技术最终输出的当前停靠位姿数据直接以目标停车位为参照，真实反映了轮式移动平台与目标停车位之间的实际偏差。以该位姿数据作为控制依据，可使系统直接以消除轮式移动平台与目标停车位之间的实际偏差为控制目标，从而确保控制逻辑能够有效满足停靠作业的实际需求。由此，本技术在完全保留现有全局定位体系的前提下，有效克服了全局定位精度受限的制约，显著提高了轮式移动平台停靠至目标停车位的停靠精度。