CN114839985A - 一种牵引式无人平台自主对接方法及系统 - Google Patents

一种牵引式无人平台自主对接方法及系统 Download PDF

Info

Publication number
CN114839985A
CN114839985A CN202210447649.2A CN202210447649A CN114839985A CN 114839985 A CN114839985 A CN 114839985A CN 202210447649 A CN202210447649 A CN 202210447649A CN 114839985 A CN114839985 A CN 114839985A
Authority
CN
China
Prior art keywords
platform
cargo
traction
distance
included angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210447649.2A
Other languages
English (en)
Inventor
龚建伟
贾鹏
臧政
王羽纯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huidong Planet Beijing Technology Co ltd
Beijing Institute of Technology BIT
Original Assignee
Huidong Planet Beijing Technology Co ltd
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huidong Planet Beijing Technology Co ltd, Beijing Institute of Technology BIT filed Critical Huidong Planet Beijing Technology Co ltd
Priority to CN202210447649.2A priority Critical patent/CN114839985A/zh
Publication of CN114839985A publication Critical patent/CN114839985A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Electromagnetism (AREA)
  • Optics & Photonics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种牵引式无人平台自主对接方法及系统，属于自动驾驶领域。该方法包括：S1：通过SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；S2：根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；S3：采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；S4：当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离小于距离阈值，切换为近距离精定位，通过激光雷达与视觉二维码检测融合的方法进行载货平台的精确定位。通过对路径进行多次规划与重规划，实现牵引平台与载货平台的自主对接。

Description

一种牵引式无人平台自主对接方法及系统
技术领域
本发明涉及自动驾驶技术领域，特别是涉及一种牵引式无人平台自主对接方法及系统。
背景技术
近年来自动驾驶技术迅速发展并逐步应用到物流运输、矿山运输及港口运输等领域。牵引式车辆是交通运输领域常用的运输工具，其由牵引平台和载货平台组成，并在载货平台的前端设置有对接机构。
目前，无人车牵引平台与载货平台的自主对接过程主要是基于全球卫星定位系统的固定位置对接方法，即将载货平台停放到标定好的预设位置，牵引平台基于实时动态差分定位技术，并结合其他车载传感器（摄像头、激光雷达、UWB等）获取车辆周围的环境信息，控制牵引平台移动到预设位置，实现牵引平台与载货平台的对接。现有方法主要存在以下缺点：
（1）现有方法主要适用于具有良好卫星信号的场景，在卫星拒止环境下，实时动态差分定位方法失效，对牵引平台的定位定向精度降低，导致无人平台的自主对接成功率降低；
（2）现有方法假设牵引平台与载货平台的距离较近（小于车载传感器的感知距离），然而在现实应用中，两者的距离可能是任意的，从而限制了自主对接系统的应用范围；
（3）现有方法需要将载货平台精确停放到标定好的预设位置，当载货平台停放的位置有偏差时会降低牵引平台与载货平台的自主对接成功率，制约了自主对接方法在实际生产中的应用。
发明内容
本发明的目的是提供一种牵引式无人平台自主对接方法及系统，用以解决卫星拒止环境下，具有自动驾驶功能的牵引式无人车的高精度定位问题，实现牵引平台与载货平台的自主对接功能，且不受自主对接距离以及载货平台停靠位置偏差的限制。
为实现上述目的，本发明提供了如下方案：
一种牵引式无人平台自主对接方法，包括：
S1：当牵引平台与载货平台的距离大于距离阀值时，基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；当牵引平台与载货平台的距离不大于距离阀值时，执行步骤S4；
S2：根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；
S3：采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
S4：当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值，通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角；牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点；载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点；
S5：根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
S6：当所述第一精确距离以及所述第一夹角满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向，否则使用自主泊车规划算法进行路径，调整牵引平台与载货平台间的相对位置；当所述第一精确距离以及所述第一夹角不满足第一预设条件时，使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件；
S7：通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角；
S8：根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度要求，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
可选地，当所述第一精确距离以及所述第一夹角不满足第一预设条件时，根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划。
可选地，所述第一精确距离D的计算公式如下：
Figure 386679DEST_PATH_IMAGE002
Figure 19524DEST_PATH_IMAGE004
Figure 321323DEST_PATH_IMAGE006
；
所述第一夹角γ的计算公式如下：
Figure 166657DEST_PATH_IMAGE008
；
其中，Dl、Dr分别为激光雷达距离载货平台对接面左、右边的距离，w为激光雷达测量的载货平台对接面的宽度，α为激光雷达和载货平台对接面左边的连线与载货平台对接面的夹角，β为激光雷达和载货平台对接面右边的连线与载货平台对接面的夹角，DY为激光雷达测量的在Y轴方向上距离载货平台对接平面的距离。
可选地，所述第一预设条件为：
Figure 424463DEST_PATH_IMAGE010
。
可选地，所述第二预设条件为：
Figure 228209DEST_PATH_IMAGE012
；δ为第二夹角，d为第二精确距离。
本发明还提供了一种牵引式无人平台自主对接系统，包括：
信息获取模块，用于当牵引平台与载货平台的距离大于距离阀值时，基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；
第一路径规划模块，用于根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；
控制模块，用于采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
第一精确距离和第一夹角计算模块，用于当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值，通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角；牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点；载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点；
第二路径规划和控制模块，用于根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
定位定向模块，用于当所述第一精确距离以及所述第一夹角满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向，否则使用自主泊车规划算法进行路径，调整牵引平台与载货平台间的相对位置；以及用于当所述第一精确距离以及所述第一夹角不满足第一预设条件时，使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件；
第二精确距离和第二夹角计算模块，用于通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角；
第三路径规划和控制模块，用于根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度要求，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
可选地，所述第一精确距离D的计算公式如下：
Figure 17304DEST_PATH_IMAGE002
Figure 100002_DEST_PATH_IMAGE013
Figure 100002_DEST_PATH_IMAGE014
；
所述第一夹角γ的计算公式如下：
Figure 338433DEST_PATH_IMAGE008
；
其中，Dl、Dr分别为激光雷达距离载货平台对接面左、右边的距离，w为激光雷达测量的载货平台对接面的宽度，α为激光雷达和载货平台对接面左边的连线与载货平台对接面的夹角，β为激光雷达和载货平台对接面右边的连线与载货平台对接面的夹角，DY为激光雷达测量的在Y轴方向上距离载货平台对接平面的距离。
可选地，所述第一预设条件为：
Figure 27909DEST_PATH_IMAGE010
。
可选地，所述第二预设条件为：
Figure 100002_DEST_PATH_IMAGE015
；δ为第二夹角，d为第二精确距离。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
1）本发明使用多源信息综合定位方法，解决了卫星拒止环境下牵引式无人车的高精度定位问题，实现牵引平台与载货平台的自主对接功能；
2）本发明提出了远距离粗定位与近距离精定位结合的综合定位方法，能够实现牵引平台与载货平台的远距离自主对接；
3）本发明提出了激光雷达与视觉二维码检测融合的近距离精定位方法，避免了相机无法感知到二维码或载货平台停靠位置偏差导致的对接失败；
4）本发明所提出的牵引式无人平台自主对接方法，保证了牵引平台与载货平台的自主对接成功率，减少了人员的人工干预，提升了交通运输的效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例牵引平台和载货平台自主对接示意图；
图2为本发明实施例牵引平台和载货平台对接面的布置图；
图3为本发明实施例牵引式无人平台自主对接方法的流程图；
图4为本发明实施例牵引式无人平台自主对接方法的具体流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种牵引式无人平台自主对接方法及系统，用以解决卫星拒止环境下，具有自动驾驶功能的牵引式无人车的高精度定位问题，实现牵引平台与载货平台的自主对接功能，且不受自主对接距离以及载货平台停靠位置偏差的限制。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1所示，考虑到载货平台庞大，不易控制，把载货平台作为基准车辆平台，牵引平台向载货平台对准对接。本发明应用了多源信息高精度定位技术，采用了远距离粗定位和近距离精定位相结合的定位方案。在卫星拒止环境下，远距离粗定位结合车载激光雷达、惯性导航信息进行多源信息综合定位，粗定位定向信息引导牵引平台向载货平台靠拢，当牵引平台与载货平台的距离小于10m时，切换为近距离精定位，通过将激光雷达与视觉二维码检测融合，进行精确定位。通过对路径进行多次规划与重规划，实现牵引平台与载货平台的自主对接。
图2显示了牵引平台与载货平台对接面上的硬件布置，其中（a）为牵引平台对接面的布置图，（b）为载货平台对接面的布置图。牵引平台对接面1的宽和高与载货平台对接面5的宽和高对应相等，分别用w、h表示，两条虚线分别表示牵引平台对接面1与载货平台对接面5的横、纵向对称轴。在图2(a)中，相机2安装在牵引平台对接面1的中心，在近距离精定位时用于检测载货平台对接面5上的二维码6，实现对载货平台的精准定位；激光雷达3安装在牵引平台顶部，且位于牵引平台对接面的纵向对称轴上，用于感知周围的环境信息并实现对车辆的精确定位；两个销柱4对称布置在牵引平台对接面1的横向对称轴上，距离牵引平台对接面1的中心点3w/8，两个销柱4分别与载货平台对接面5上的销孔7连接，用于引导牵引平台与载货平台的精准对接。在图2(b)中，二维码6布置在载货平台对接面5的中心，使用视觉二维码检测的方式实现对车辆的精准定位；两个销孔7对称布置在载货平台对接平面5的横向对称轴上，距离载货平台对接面5的中心点3w/8，两个销孔7分别与载货平台对接面1上的销柱4连接，用于引导牵引平台与载货平台进行精准对接。
如图3-4所示，本发明提供的牵引式无人平台自主对接方法，包括以下步骤：
S1：当牵引平台与载货平台的距离大于距离阀值时（称其为远距离），基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；当牵引平台与载货平台的距离不大于距离阀值时，执行步骤S4。
S2：根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划。
当牵引平台与载货平台的直线距离大于10米，使用现有的激光雷达与惯性测量单元（IMU）紧耦合的激光SLAM，输出车辆的粗定位定向信息和环境感知信息，并根据粗定位定向信息和环境感知信息，使用现有的A*算法规划出从牵引平台当前位置到载货平台的路径。
S3：采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台。
S4：当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值（称其为近距离），通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角。设牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点，根据右手定则可确定Z轴正方向；同理，设载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点，根据右手定则可确定z轴正方向。
由于激光SLAM在非结构化场景中难以保持较高的精度稳定性，牵引平台与载货平台距离较近会导致激光雷达失效，故单独的激光雷达无法实现对载货平台的精定位。由于视觉二维码检测方法存在二维码不在相机感受野的情况，导致视觉二维码定位定向失效，故单独的视觉二维码检测方法无法实现对载货平台的精定位。针对上述问题，本发明提出了激光雷达与视觉二维码融合的精定位定向方法。
当牵引平台与载货平台的距离不大于10米，使用激光SLAM来感知车辆周围的环境信息，使用激光雷达信息进行定位定向。将激光雷达距离载货平台对接面左、右边的距离分别用Dl、Dr表示，激光雷达测量的在Y轴方向上距离载货平台对接平面的距离用DY表示，激光雷达测量的载货平台对接面的宽度用w表示；在水平面上，设激光雷达和载货平台对接面左、右边的连线与载货平台对接面的夹角分别为α、β，牵引平台纵轴Y与载货平台纵轴y的夹角为γ，牵引平台对接装置与载货平台对接装置的距离为D；则α、β和D可分别用下式表示：
Figure 566338DEST_PATH_IMAGE002
Figure 872423DEST_PATH_IMAGE013
Figure 747976DEST_PATH_IMAGE014
则γ可表示为：
Figure 324582DEST_PATH_IMAGE008
。
S5：根据所述第一精确距离D以及所述第一夹角γ，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台。
S6：当所述第一精确距离D以及所述第一夹角γ满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向，否则使用自主泊车规划算法进行路径重规划，最终实现对二维码的检测。
若牵引平台与载货平台的距离D、夹角γ不满足第一预设条件使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件。第一预设条件如下：
Figure 892966DEST_PATH_IMAGE010
。
S7：通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角。
S8：根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度条件，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
使用现有的视觉二维码定位定向方法计算牵引平台对接装置与载货平台对接装置间的距离d及牵引平台纵轴Y与载货平台纵轴y之间的夹角δ，基于d和δ，采用A*算法重新进行路径规划，并采用模型预测控制算法调整牵引平台的位姿，为无人平台的自动对接提供良好的位姿。若
Figure 100002_DEST_PATH_IMAGE016
，则满足牵引平台与载货平台的自主对接条件，可实现自主对接，否则使用自主泊车规划算法进行路径重规划。
考虑到卫星拒止环境及系统对不同场景的适应能力，本发明采用了多源信息综合定位方法，在非结构化场景中能够输出稳定精确的全局定位结果，并且在卫星信号受到干扰或缺失的情况下仍可以长时间保持定位精度，保证了无人平台在不同场景下的自主定位精度。
考虑到现实应用中存在牵引平台与载货平台对接距离较远的情况，本发明将定位系统划分为远距离粗定位和近距离精定位两个阶段；粗定位信息引导牵引平台向载货平台靠拢，达到预设条件后，切换为近距离精定位，通过精确的定位定向方法，实现无人车的远距离自主对接。
考虑到载货平台庞大，不易控制，把载货平台作为基准车辆平台（可以以任意角度停放），使用激光雷达与视觉二维码检测融合的精定位方法对载货平台进行定位定向，实现载货平台的高精度定位。
本发明还提供了一种牵引式无人平台自主对接系统，包括：
信息获取模块，用于当牵引平台与载货平台的距离大于距离阀值时，基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；
第一路径规划模块，用于根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；
控制模块，用于采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
第一精确距离和第一夹角计算模块，用于当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值，通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角；牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点；载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点；
第二路径规划和控制模块，用于根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台，否则使用自主泊车规划算法进行路径，调整牵引平台与载货平台间的相对位置；以及用于当所述第一精确距离以及所述第一夹角不满足第一预设条件时，使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件；
定位定向模块，用于当所述第一精确距离以及所述第一夹角满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向；
第二精确距离和第二夹角计算模块，用于通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角；
第三路径规划和控制模块，用于根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度要求，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种牵引式无人平台自主对接方法，其特征在于，包括：
S1：当牵引平台与载货平台的距离大于距离阀值时，基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；当牵引平台与载货平台的距离不大于距离阀值时，执行步骤S4；
S2：根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；
S3：采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
S4：当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值，通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角；牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点；载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点；
S5：根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
S6：当所述第一精确距离以及所述第一夹角满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向，否则使用自主泊车规划算法进行路径，调整牵引平台与载货平台间的相对位置；当所述第一精确距离以及所述第一夹角不满足第一预设条件时，使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件；
S7：通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角；
S8：根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度要求，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
2.根据权利要求1所述的牵引式无人平台自主对接方法，其特征在于，当所述第一精确距离以及所述第一夹角不满足第一预设条件时，根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划。
3.根据权利要求1所述的牵引式无人平台自主对接方法，其特征在于，所述第一精确距离D的计算公式如下：
Figure 993527DEST_PATH_IMAGE002
Figure 989296DEST_PATH_IMAGE004
Figure 237874DEST_PATH_IMAGE006
；
所述第一夹角γ的计算公式如下：
Figure 960980DEST_PATH_IMAGE008
；
其中，Dl、Dr分别为激光雷达距离载货平台对接面左、右边的距离，w为激光雷达测量的载货平台对接面的宽度，α为激光雷达和载货平台对接面左边的连线与载货平台对接面的夹角，β为激光雷达和载货平台对接面右边的连线与载货平台对接面的夹角，DY为激光雷达测量的在Y轴方向上距离载货平台对接平面的距离。
4.根据权利要求3所述的牵引式无人平台自主对接方法，其特征在于，所述第一预设条件为：
Figure 821357DEST_PATH_IMAGE010
。
5.根据权利要求1所述的牵引式无人平台自主对接方法，其特征在于，所述第二预设条件为：
Figure 519186DEST_PATH_IMAGE012
；δ为第二夹角，d为第二精确距离。
6.一种牵引式无人平台自主对接系统，其特征在于，包括：
信息获取模块，用于当牵引平台与载货平台的距离大于距离阀值时，基于SLAM获取载货平台的粗定位定向信息、环境感知信息以及牵引平台与载货平台之间的距离；所述SLAM由牵引平台上设置的激光雷达与惯性测量单元紧耦合生成；
第一路径规划模块，用于根据载货平台的粗定位定向信息和环境感知信息，采用A*算法进行路径规划；
控制模块，用于采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
第一精确距离和第一夹角计算模块，用于当所述牵引平台按照规划好的路径靠近所述载货平台过程中，若牵引平台与载货平台之间的距离不大于距离阈值，通过所述激光雷达的测量参数计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第一精确距离以及牵引平台纵轴Y与载货平台纵轴y的第一夹角；牵引平台的前进方向为Y轴正方向，水平向右为X轴正方向，牵引平台对接面上的对接点O为坐标系原点；载货平台的前进方向为y轴正方向，水平向右为x轴正方向，载货平台对接面上的对接点o为坐标系原点；
第二路径规划和控制模块，用于根据所述第一精确距离以及所述第一夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；
定位定向模块，用于当所述第一精确距离以及所述第一夹角满足第一预设条件时，若牵引平台对接面上的相机检测到载货平台对接面上的二维码，则通过牵引平台对接面上的相机检测载货平台对接面上的二维码进行定位定向，否则使用自主泊车规划算法进行路径，调整牵引平台与载货平台间的相对位置；以及用于当所述第一精确距离以及所述第一夹角不满足第一预设条件时，使用A*算法进行路径规划，调整牵引平台与载货平台间的相对位置，直到满足第一预设条件；
第二精确距离和第二夹角计算模块，用于通过视觉二维码定位定向方法计算当前时刻牵引平台的对接装置与载货平台的对接装置之间的第二精确距离以及牵引平台纵轴Y与载货平台纵轴y之间的第二夹角；
第三路径规划和控制模块，用于根据所述第二精确距离以及所述第二夹角，采用A*算法进行路径规划，并采用模型预测控制算法控制所述牵引平台按照规划好的路径靠近所述载货平台；当所述第二精确距离以及所述第二夹角满足第二预设条件时，牵引平台与载货平台满足自主对接的定位精度要求，否则使用自主泊车规划算法进行路径重规划，调整牵引平台与载货平台间的相对位置，直到第二精确距离以及第二夹角满足第二预设条件。
7.根据权利要求6所述的牵引式无人平台自主对接系统，其特征在于，所述第一精确距离D的计算公式如下：
Figure DEST_PATH_IMAGE013
Figure DEST_PATH_IMAGE014
Figure DEST_PATH_IMAGE015
；
所述第一夹角γ的计算公式如下：
Figure 301066DEST_PATH_IMAGE008
；
其中，Dl、Dr分别为激光雷达距离载货平台对接面左、右边的距离，w为激光雷达测量的载货平台对接面的宽度，α为激光雷达和载货平台对接面左边的连线与载货平台对接面的夹角，β为激光雷达和载货平台对接面右边的连线与载货平台对接面的夹角，DY为激光雷达测量的在Y轴方向上距离载货平台对接平面的距离。
8.根据权利要求7所述的牵引式无人平台自主对接系统，其特征在于，所述第一预设条件为：
Figure 562283DEST_PATH_IMAGE010
。
9.根据权利要求7所述的牵引式无人平台自主对接系统，其特征在于，所述第二预设条件为：
Figure DEST_PATH_IMAGE016
；δ为第二夹角，d为第二精确距离。
CN202210447649.2A 2022-04-27 2022-04-27 一种牵引式无人平台自主对接方法及系统 Pending CN114839985A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210447649.2A CN114839985A (zh) 2022-04-27 2022-04-27 一种牵引式无人平台自主对接方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210447649.2A CN114839985A (zh) 2022-04-27 2022-04-27 一种牵引式无人平台自主对接方法及系统

Publications (1)

Publication Number Publication Date
CN114839985A true CN114839985A (zh) 2022-08-02

Family

ID=82565799

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210447649.2A Pending CN114839985A (zh) 2022-04-27 2022-04-27 一种牵引式无人平台自主对接方法及系统

Country Status (1)

Country Link
CN (1) CN114839985A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168469A (zh) * 2023-11-03 2023-12-05 普达迪泰(成都)智造研究院有限公司 一种自由切换导航方式的组合导航方法及系统

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168469A (zh) * 2023-11-03 2023-12-05 普达迪泰(成都)智造研究院有限公司 一种自由切换导航方式的组合导航方法及系统

Similar Documents

Publication Publication Date Title
RU2720138C2 (ru) Способ автоматического подведения к погрузочно-разгрузочной площадке для применения в грузовых автомобилях большой грузоподъемности
US9499197B2 (en) System and method for vehicle steering control
EP1010047B1 (de) Verfahren zur andockpositionierung einer autonomen mobilen einheit unter verwendung eines leitstrahles
US20200247471A1 (en) Vehicle park assist systems
US10878709B2 (en) System, method, and computer readable medium for autonomous airport runway navigation
CN105353772A (zh) 一种无人机机动目标定位跟踪中的视觉伺服控制方法
CN108871209A (zh) 一种大尺寸工件移动测量机器人系统及方法
CN105607635A (zh) 自动导引车全景光学视觉导航控制系统及全向自动导引车
CN113791621B (zh) 一种自动驾驶牵引车与飞机对接方法及系统
KR101060988B1 (ko) 지그비의 수신신호세기를 이용한 지능형 이동로봇의 이동 대상물 추적 제어 장치 및 그 방법
CN113156998B (zh) 一种无人机飞控系统的控制方法
CN111487976B (zh) 一种倒车轨迹跟踪方法
WO2024109837A1 (zh) 基于异构无人系统相互观测的同时定位与地图构建方法
CN103852082A (zh) 一种星间测量和陀螺的姿态轨道一体化滤波估计方法
CN109387194A (zh) 一种移动机器人定位方法和定位系统
CN114839985A (zh) 一种牵引式无人平台自主对接方法及系统
AU2021209229A1 (en) Method and apparatus for positioning movable device, and movable device
CN102661749A (zh) 动力平板运输车精确对接控制系统
CN107300386A (zh) 一种基于仅测角导航的闭环凸优化最优交会制导方法
DE102020105334A1 (de) Verfahren für ein Steuern eines fahrerlosen Transportfahrzeugs und Steuerungssystem, das angepasst ist, um das Verfahren auszuführen
CN113478483A (zh) 一种基于不锈钢储罐移动机器人焊接方法及系统
CN107228663A (zh) 一种自动导引运输车的定位系统和方法
Vezinet et al. State of the art of image-aided navigation techniques for aircraft approach and landing
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
CN212796545U (zh) 一种自动导引运输车的导航系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination