CN112099494B - 一种面向全地形车及其自动检测楼梯与攀爬方法 - Google Patents

一种面向全地形车及其自动检测楼梯与攀爬方法 Download PDF

Info

Publication number
CN112099494B
CN112099494B CN202010910982.3A CN202010910982A CN112099494B CN 112099494 B CN112099494 B CN 112099494B CN 202010910982 A CN202010910982 A CN 202010910982A CN 112099494 B CN112099494 B CN 112099494B
Authority
CN
China
Prior art keywords
vehicle
stairs
stair
point cloud
climbing
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.)
Active
Application number
CN202010910982.3A
Other languages
English (en)
Other versions
CN112099494A (zh
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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN202010910982.3A priority Critical patent/CN112099494B/zh
Publication of CN112099494A publication Critical patent/CN112099494A/zh
Application granted granted Critical
Publication of CN112099494B publication Critical patent/CN112099494B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a 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/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic 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/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)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种面向全地形车及其自动检测楼梯与攀爬方法，方法具体为：对获取的单帧或多帧点云数据降采样以及过滤地面点处理，进行分割得到不同的点云团簇并将其拟合成一个平面，并提取拟合优度和平面坡度特征，判断所述点云团簇是否为楼梯，从获取代表楼梯的点云团簇提取出表征楼梯结构的平行线，并根据平行线获取楼梯与全地形车的相对位置关系，需要攀爬通过楼梯时，将车辆航向调整到与楼梯垂直；根据获取车辆实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态，根据横滚角引导车辆攀爬楼梯；本发明对楼梯不是由台阶平面检测出来的，而是由坡度和平行边缘几何特征来检测，本发明中方法可以从单帧激光雷达数据中实时检测和定位楼梯。

Description

一种面向全地形车及其自动检测楼梯与攀爬方法
技术领域
本发明属于自动驾驶和机器人领域，具体涉及一种面向全地形车及其自动检测楼梯与攀爬方法。
背景技术
近年来，自动驾驶和机器人技术得到了广泛应用，然而，在L5级别的自动驾驶技术成熟之前，将这些技术应用于特定场景中的一些轮式机器人，显得更加重要和有意义，全地形车辆 (ATV)广泛应用于城市搜救，ATV工作的非结构化、复杂场景中通常包含许多特殊的障碍，如楼梯；因此，自动检测、定位和通过楼梯是一种十分重要的能力。机器人遥控爬楼梯已经被证明是低效和不可靠的，因此，许多关于机器人自主爬楼梯检测和导航的研究已经展开。为了能在有楼梯的环境中自主安全移动，ATV必须:(1)找到楼梯并定位；(2)判断是否需要走楼梯；(3)如有需要，安全快速爬上楼梯，但是，目前的大多数方法只能提供部分的解决方案。比如有的只关注楼梯检测，有的只关注控制方面，有些方法探索了楼梯检测与导航两方面的研究，但仅限于室内环境或其他特定场景。
发明内容
为了解决现有技术的问题，本发明提供一种面向全地形车及其自动检测楼梯与攀爬方法，基于激光雷达采集关于环境的单帧或多帧点云数据，获取基于点云数据中点云团簇的几何特征，如坡度和平行线等，识别和定位楼梯，基于ATV位于楼梯上时的横滚角和偏航角的耦合关系，根据实时获取的三轴姿态，控制并稳定车辆姿态，从而进行楼梯攀爬，结果证明了本发明算法的鲁棒性和准确性，并证明了该方法是安全可行的。
为了达到上述目的，本发明采用的技术方案为：一种面向全地形车的楼梯自动检测与攀爬方法，包括以下步骤：
步骤1，楼梯自动检测：获取单帧或多帧点云数据，对所述单帧或多帧点云数据进行降采样以及过滤地面点处理，再进行分割得到不同的点云团簇，将所述一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述拟合优度和平面坡度特征判断所述点云团簇是否代表楼梯，获取代表楼梯的点云团簇；
步骤2，定位楼梯并调整车辆航向与楼梯对齐：从步骤1所获取代表楼梯的点云团簇中提取出表征楼梯结构的平行线，并根据所述平行线获取楼梯与全地形车的相对位置关系，当车辆与楼梯的距离小于设定阈值，且需要攀爬通过楼梯时，将车辆航向调整到与楼梯横向垂直；
步骤3，自动攀爬楼梯：获取车辆实时车辆三轴姿态数据，根据所述实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态，根据位姿状态数据中横滚角的大小判断车辆朝向是否与楼梯横向保持垂直，并据此引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角大小判断车辆是否完成楼梯攀爬，如果俯仰角大于设定阈值，说明没有完成攀爬任务，则需要继续攀爬；如果俯仰角小于阈值，则说明攀爬任务已经完成，停止攀爬动作。
所述步骤1中，基于激光雷达获取单帧或多帧点云数据，将激光雷达一次采集的所有点云数据记作：
Pori＝{pi＝(xi，yi，zi)|i＝1，…，N} (8)
上式中N为点数，经过降噪、范围过滤和地面滤波以及降采样预处理后的点云数据表示为：
Pprocessed＝Pre_processing(Pori) (9)
式中Pre_processing(·)为对点云数据进行预处理过程。
采用基于KD-tree优化的DBSCAN算法对预处理后的点云进行分割，Pobjs是一组点云团簇，每个点云团簇代表真实环境中的一个对象：
Pobjs＝{Pi|i＝1，…，Nc}＝DBSCAN(Pprocessed) (10)
上式中Nc为点云团簇的个数，pi表示第i个点云团簇，将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断所述点云团簇是否为楼梯。
若某点云团簇为楼梯，则点云团簇内的点分布于倾斜面上，汇聚成多条相互平行的线，所述具有平行关系的线与线之间由小圆弧连接，考虑到激光雷达水平安装的扫描特性，直线分布的点对应楼梯台阶的垂直面，小圆弧对应楼梯台阶的水平面，将某个团簇中的点拟合到一个平面中，通过平面的倾角信息和拟合优度来判断该簇描述的对象是否是楼梯。
对于楼梯的点簇Pi，簇中的点大致分布在坡度为si的斜面αi上，平面αi的描述方程如下：
Figure BDA0002663257770000031
上式中
Figure BDA0002663257770000032
均表示平面αi的参数，由此得到平面αi的切线si为：
Figure BDA0002663257770000033
楼梯斜面的坡度si∈[smin，smax]，与此同时，合成平面αi对点簇Pi的拟合优度应满足gi∈ [gmin，gmax]。
步骤2中，将点云团簇投影到水平面，对所述水平面的进行点拟合获取m条直线，并得到所述直线的直线方程，基于所述直线方程计算得到该直线在激光雷达坐标系下到坐标原点的距离以及从x轴到所述直线的法向量的旋转角度。
所述步骤3中，楼梯攀爬过程中全地形车辆的相对航向估计方法为：
假设楼梯为一个斜坡，当车辆在楼梯上且车辆前进方向向与楼梯横向垂直时，俯仰角θ等于楼梯的倾斜角α，即：
θ＝α＝atan(s) (13)
当在楼梯上的车辆的航向改变
Figure BDA0002663257770000034
时，滚转角变为
Figure BDA0002663257770000035
两者关系为：
Figure BDA0002663257770000036
通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角判断车辆是否完成楼梯攀爬。
一种面向全地形车的楼梯自动检测与攀爬系统，包括楼梯自动检测模块、车辆航向与楼梯对齐模块以及自动攀爬楼梯模块；其中，楼梯自动检测模块对采集到的点云数据进行预处理，对预处理后的点云数据进行分割；将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断该团簇是否为楼梯；车辆航向与楼梯对齐模块将属于楼梯的点簇投影到水平面上，计算楼梯与激光雷达坐标系的原点的距离以及相对于x轴的旋转角度，当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直；自动攀爬楼梯模块通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯，根据车辆俯仰角大小判断车辆是否完成楼梯攀爬。
一种全地形车，包括车载计算机、车载多线激光雷达和车载惯性测量单元，所述车载计算机 包括至少一个处理器和存储器，所述存储器与处理器通过I/O接口连接，车载多线激光雷达和车载惯性测量单元均连接处理器的输入端，存储器用于存储计算机程序，所述处理器执行所述计算机程序时实现本发明所述面向全地形车的楼梯自动检测与攀爬方法，车载惯性测量单元用于获取实时车辆三轴姿态数据，车载多线激光雷达用于实时测得所述单帧或多帧点云数据，处理器在执行步骤1时所述单帧或多帧点云数据，获取处理器在执行步骤3时获取所述车辆实时车辆三轴姿态数据。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现本发明所述面向全地形车的楼梯自动检测与攀爬方法的步骤。
与现有技术相比，本发明提供了从稀疏的单帧激光雷达数据中快速、实时地识别和定位楼梯的方法，基于激光雷达采集关于环境的单帧或多帧点云数据，获取基于点云数据中点云团簇的几何特征，如坡度和平行线等，自动识别和定位楼梯，基于ATV位于楼梯上时的横滚角和偏航角的耦合关系，根据实时获取的三轴姿态，控制并稳定车辆姿态，从而进行楼梯攀爬，实验结果证明了本发明算法的鲁棒性和准确性，并证明了该方法是安全可行的能实现全地形车辆自主、快速、安全地检测、定位与攀爬楼梯。
附图说明
图1为自动楼梯检测与攀爬方法流程图。
图2为不同楼梯结构与物体的仿真实验示意图。
图3为楼梯攀爬实验场景示意图。
图4为楼梯攀爬实验结果。
图5为不同车辆与楼梯距离时的定位结果。
具体实施方式
下面结合附图对本发明做进一步说明。
本发明提出的方法基于激光雷达数据实现全地形车的自动楼梯检测和攀爬方法，该方法主要包括几个部分：预处理、分割、识别、定位以及自动攀爬。对激光雷达采集到的单帧或多帧点云数据进行降采样和过滤地面点预处理，然后将预处理之后的点云分割为不同的团簇，根据拟合平面的坡度和平行边缘特征判断该团簇是否为楼梯，计算楼梯团簇在激光雷达坐标系下相对于原点的距离d与旋转角度γ，最后基于车载惯性测量单元测得的实时车辆三轴姿态数据实现自动攀爬楼梯。
基于激光雷达数据实现全地形车的自动楼梯检测和攀爬方法如下：
步骤1，对激光雷达采集到的单帧或多帧点云数据进行降采样和过滤地面点预处理，然后将预处理之后的点云分割为不同的团簇，将分割后每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据这些特征判断该团簇是否为楼梯；激光雷达传感器的结构特点和几何光学原理，点云数据具有远稀疏、近密集的特点，并会在地面或水平面上形成圆弧，为了提高分割的效率，滤除所述圆弧和远距离的稀疏点，并对点云进行降采样。
步骤2，定位楼梯并调整车辆航向与楼梯对齐：对代表楼梯的点云团簇进行处理，提取出表征楼梯结构的平行线，并根据这些平行线计算出楼梯与全地形车的相对位置关系，包括车辆朝向与楼梯走向之间的夹角和车辆到楼梯的距离。当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直；
步骤3，自动攀爬楼梯：根据车载惯性测量单元测得的实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态：根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角大小判断车辆是否完成楼梯攀爬。
下面进行具体描述：
本发明示例利用VLP-16激光雷达采集环境数据，将激光雷达一次采集的所有点云数据记作：
Pori＝{pi＝(xi，yi，zi)|i＝1，…，N} (15)
上式中N为点数。
S11，对采集到的点云数据Pori进行预处理：
由于激光雷达传感器的结构特点和几何光学原理，点云数据具有远稀疏、近密集的特点，并会在地面或水平面上形成圆弧。为了提高后续分割的效率，必须滤除这些圆弧和远距离的稀疏点，并对点云进行降采样。经过降噪、范围过滤和地面滤波和降采样预处理后的点云数据表示为：
Pprocessed＝Pre_processing(Pori) (16)
式中Pre_processing(.)为对点云数据进行预处理过程。
S12，对预处理后的点云数据Pprocessed进行分割：
DBSCAN是一种基于图的分割方法，DBSCAN的主要特点和优点是不需要预先指定点云中的对象个数，并且对噪声具有鲁棒性。这里采用基于KD-tree优化的DBSCAN算法对预处理后的点云进行分割，Pobjs是一组点簇，每个簇通常代表真实环境中的一个对象：
Pobjs＝{pi＝(xi，yi，zi)|i＝1，…，Nc}＝DBSCAN(Pprocessed) (17)
上式中Nc为簇的个数，pi表示第i个簇，
S13，将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度以及平面坡度特征，根据这些特征判断该团簇是否为楼梯：
若某团簇为楼梯，则簇内的点基本分布于倾斜面上，汇聚成多条平行线，线与线之间由小圆弧连接。考虑到激光雷达水平安装的扫描特性，直线分布的点对应楼梯台阶的垂直面，小圆弧对应楼梯台阶的水平面。因此，只要将某个团簇中的点拟合到一个平面中，就可以通过平面的倾角信息和拟合优度来判断该簇描述的对象是否是楼梯。对于楼梯的点簇Pi，簇中的点大致分布在坡度为si的斜面αi上。平面αi的描述方程如下：
Figure BDA0002663257770000071
上式中
Figure BDA0002663257770000072
均表示平面αi的参数，由此得到平面αi的切线si为：
Figure BDA0002663257770000073
楼梯斜面的坡度si∈[smin，smax]，与此同时，合成平面αi对点簇Pi的拟合优度应满足gi∈ [gmin，gmax]，确认点簇所表述的物体由是一个楼梯后，把这个团簇重命名为Pstair。
S2，将属于楼梯的点簇Pstair投影到水平面上，并将其记录为P′stair，计算楼梯与激光雷达坐标系的原点的距离d以及相对于x轴的旋转角度γ，当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直：
a、将P′stair上的点拟合成m条直线。第k条直线的方程为:
Figure BDA0002663257770000081
其中
Figure BDA0002663257770000082
是第k条直线的参数。
b、基于直线方程计算激光雷达坐标系下直线k到坐标原点的距离dk以及从x轴到直线k 的法向量的旋转角度γk：
Figure BDA0002663257770000083
Figure BDA0002663257770000084
c、所有直线与原点距离的最大最小值分别为：dmax和dmin，则楼梯与激光雷达坐标系的原点的距离d以及相对于x轴的旋转角度γ为：
Figure BDA0002663257770000085
当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直；
S3，通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯，根据车辆俯仰角大小判断车辆是否完成楼梯攀爬：
a、楼梯攀爬的任务是控制车辆平稳快速地通过楼梯，保持航向是核心问题。本发明采用对车辆横滚角的测量来解决这一问题。为了简化问题，假设楼梯为一个斜坡，当车辆在楼梯上且与楼梯垂直时，俯仰角θ等于楼梯的倾斜角α，即：
θ＝α＝atan(s) (24)
b、当在楼梯上的车辆的航向改变
Figure BDA0002663257770000086
时，滚转角变为
Figure BDA0002663257770000087
两者关系为：
Figure BDA0002663257770000088
通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角大小判断车辆是否完成楼梯攀爬。
下面通过以下不同情形对上述方法的效果进行说明：
图1为自主移动ATV遂行自动楼梯检测与攀爬方法的过程：具体实施过程如上文所述。。
图2使用Gazebo 7.16.0在Ubuntu 16.04系统中创建不同的模型：1-6级的普通楼梯，3种特殊楼梯，以及5种物体作为负样本，图2中(a)普通楼梯(b)圆台楼梯(c)不规则楼梯①(d)不规则楼梯②(e-i)为负样本。
表1所示为在图2的仿真环境中对不同楼梯结构与物体的楼梯检测结果，表中“总帧数”为激光雷达采集的样本帧数，“识别帧数”为检测到楼梯的帧数，“识别率”为检测正确率。当正常楼梯的台阶数大于2时对楼梯的检测精度高于90％。普通6级楼梯和特殊6级楼梯的检出率几乎相同。只有圆形台阶的楼梯的检测正确率稍微低一些。对给定的5种目标的误检率为 0。
表1
Figure BDA0002663257770000091
表2所示为在实际场景中楼梯检测结果，所有数据集均由VLP16激光雷达以10Hz的速率采集，表中“总帧数”为激光雷达采集的样本帧数，“识别帧数”为检测到楼梯的帧数，“平均耗时”为平均每帧所消耗的检测时间，“识别正确率”为检测正确率。数据集在三个场景下进行采集：1)带楼梯的普通场景，大概是高1.5米，长20多米。在数据采集过程中，楼梯上有行人，这增加了我们检测的难度；2)有一个只有三级的低矮的楼梯，上面有两根立柱；3)楼梯有5级台阶，在楼梯的两侧有两个花坛。数据的采集在不同距离下进行，且采集过程中，车辆处于运动状态。由图中结果可以看出本发明提出的楼梯检测方法平均准确率达到了96.4％。
表2
场景 总帧数 识别帧数 平均耗时 识别正确率
场景1 288 288 44.50ms 100％
场景2 376 368 43.05ms 98.87％
场景3 226 202 46.42ms 89.38％
总计 890 858 43.69ms 96.40％
表3为三个场景下楼梯检测过程的平均时间成本。平均而言，检测总时间消耗为43.69ms，远远小于激光雷达的扫描时间(100ms)。说明该算法可以为自主全地形车的楼梯导航提供实时的楼梯检测和定位。
表3
过程 预处理 分割 识别 总计
耗时 38.99ms 2.70ms 2.00ms 43.69ms
图5为不同距离时定位实验结果，x轴表示在全地形车距离楼梯2m、3m、4m、5m、6m的定位结果，y轴表示定位误差，左侧为距离定位结果，右侧为方位估计结果。从表中可以看出，距离估计的绝对误差随着距离的增加而减小。当距离大于3m时，误差小于0.1m。而当距离为2m时，估计误差为0.15m，估计方差较大。其主要原因是激光雷达盲区造成的点簇严重变形。因为盲区的存在，激光雷达距离楼梯越近，点云团簇的形变越大。这就是误差随距离的减小而增大的原因。方位估计的绝对误差小于0.01rad，且随着距离的增加有增大的趋势。楼梯定位精度一般达到楼梯导航的要求。
图3为楼梯攀爬实验场景，实验场景中有三个楼梯，Hero-X的目标是从START位置出发，通过stair 1，到达GOAL位置后停止。
图4为楼梯攀爬实验结果，实验过程分为五个部分：攀爬前的自由导航、对准与靠近、攀爬、攀爬后的自由导航以及到达目标点。图中部的两个曲线图显示了本发明所述方法的楼梯定位结果。每个点表示一个定位结果。不同的颜色代表不同的楼梯，黑线表示楼梯位置的真值。黄色的点(stair 4)是楼梯攀爬过程中的误检。其中(a)-(e)图为五个实验过程的可视化检测结果(左) 和真实场景(右)对比。由图可知Hero-X检测到了它周围的所有楼梯，判断并选出了它必须通过的楼梯，最后安全地攀上楼梯，并到达目标点。它只对横亘在其必经之路上的楼梯进行了攀爬，忽略了其他的楼梯。其次，由于图中每个点代表一个检测结果，从点的稠密程度和聚集程度可以推断出，当stair与Hero-X的距离小于8m时，对于stair 1和stair 3的检测是准确、鲁棒、实时的。图中还体现出了两个不足:在楼梯攀爬过程中，检测结果会出现假阳性；算法无法检测到下行楼梯(攀上stair 1之后，便再检测不到了)。
本发明提供一种面向全地形车的楼梯自动检测与攀爬系统，包括楼梯自动检测模块、车辆航向与楼梯对齐模块以及自动攀爬楼梯模块；其中，楼梯自动检测模块对采集到的点云数据进行预处理，对预处理后的点云数据进行分割；将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断该团簇是否为楼梯；车辆航向与楼梯对齐模块将属于楼梯的点簇投影到水平面上，计算楼梯与激光雷达坐标系的原点的距离d以及相对于 x轴的旋转角度γ，当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直；自动攀爬楼梯模块通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯，根据车辆俯仰角大小判断车辆是否完成楼梯攀爬。
楼梯自动检测模块在能够执行本发明所述方法的S11至S13，车辆航向与楼梯对齐模块执行本发明所述方法的S2，自动攀爬楼梯模块能够执行本发明所述方法的S3。
本发明还提供一种全地形车，包括车载计算机、车载多线激光雷达和车载惯性测量单元，所述车载计算机 包括至少一个处理器和存储器，所述存储器与处理器通过I/O接口连接，车载多线激光雷达和车载惯性测量单元均连接处理器的输入端，存储器用于存储计算机程序，所述处理器执行所述计算机程序时实现本发明所述面向全地形车的楼梯自动检测与攀爬方法，车载惯性测量单元用于获取实时车辆三轴姿态数据，车载多线激光雷达用于实时测得所述单帧或多帧点云数据，处理器在执行步骤1时所述单帧或多帧点云数据，获取处理器在执行步骤3时获取所述车辆实时车辆三轴姿态数据。
本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。本发明面向全地形车的楼梯自动检测与攀爬方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。
基于这样的理解，在示例性实施例中，还提供了一种计算机可读存储介质，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于该计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。其中，所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器 (例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘 (SSD))等。
在示例性实施例中，还提供计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述面向全地形车的楼梯自动检测与攀爬方法的步骤。处理器可能是中央处理单元(CentralProcessingUnit， CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor、DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray， FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (10)

1.一种面向全地形车的楼梯自动检测与攀爬方法，其特征在于，包括以下步骤：
步骤1，楼梯自动检测：获取单帧或多帧点云数据，对所述单帧或多帧点云数据进行降采样以及过滤地面点处理，再进行分割得到不同的点云团簇，将一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述拟合优度和平面坡度特征判断所述点云团簇是否代表楼梯，获取代表楼梯的点云团簇；
步骤2，定位楼梯并调整车辆航向与楼梯对齐：从步骤1所获取代表楼梯的点云团簇中提取出表征楼梯结构的平行线，并根据所述平行线获取楼梯与全地形车的相对位置关系，当车辆与楼梯的距离小于设定阈值，且需要攀爬通过楼梯时，将车辆航向调整到与楼梯横向垂直；
步骤3，自动攀爬楼梯：获取实时车辆三轴姿态数据，根据所述实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态，根据实时车辆三轴姿态数据中横滚角的大小判断车辆朝向是否与楼梯横向保持垂直，并据此引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角大小判断车辆是否完成楼梯攀爬，如果俯仰角大于设定阈值，说明没有完成攀爬任务，则需要继续攀爬；如果俯仰角小于阈值，则说明攀爬任务已经完成，停止攀爬动作。
2.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，所述步骤1中，基于激光雷达获取单帧或多帧点云数据，将激光雷达一次采集的所有点云数据记作：
Pori＝{pi＝(xi，yi，zi)|i＝1，…，N} (1)
上式中N为点数，经过降噪、范围过滤和地面滤波以及降采样预处理后的点云数据表示为：
Pprocessed＝Pre_processing(Pori) (2)式中Pre_processing(·)为对点云数据进行预处理过程。
3.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，采用基于KD-tree优化的DBSCAN算法对预处理后的点云进行分割，Pobjs是一组点云团簇，每个点云团簇代表真实环境中的一个对象：
Pobjs＝{Pi|i＝1，…，Nc}＝DBSCAN(Pprocessed) (3)
上式中Nc为点云团簇的个数，Pi 表示第i个点云团簇，将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断所述点云团簇是否为楼梯。
4.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，若某点云团簇为楼梯，则点云团簇内的点分布于倾斜面上，汇聚成多条相互平行的线，所述具有平行关系的线与线之间由小圆弧连接，考虑到激光雷达水平安装的扫描特性，直线分布的点对应楼梯台阶的垂直面，小圆弧对应楼梯台阶的水平面，将某个团簇中的点拟合到一个平面中，通过平面坡度和拟合优度来判断该簇描述的对象是否是楼梯。
5.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，对于楼梯的点簇Pi，簇中的点分布在坡度为si的斜面αi上，斜面αi的描述方程如下：
Figure FDA0003154147280000021
上式中
Figure FDA0003154147280000022
均表示平面αi的参数，由此得到斜面αi的坡度si为：
Figure FDA0003154147280000023
楼梯斜面的坡度si∈[smin，smax]，与此同时，合成斜面αi对点簇Pi的拟合优度应满足gi∈[gmin，gmax]。
6.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，步骤2中，将点云团簇投影到水平面，对所述水平面的进行点拟合获取m条直线，并得到所述直线的直线方程，基于所述直线方程计算得到该直线在激光雷达坐标系下到坐标原点的距离以及从x轴到所述直线的法向量的旋转角度。
7.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，所述步骤3中，楼梯攀爬过程中全地形车辆的相对航向估计方法为：
假设楼梯为一个斜坡，当车辆在楼梯上且车辆前进方向向与楼梯横向垂直时，俯仰角θ等于楼梯的倾斜角α，即：
θ＝α＝atan (s) (6)
当在楼梯上的车辆的航向改变
Figure FDA0003154147280000031
时，滚转角变为
Figure FDA0003154147280000032
两者关系为：
Figure FDA0003154147280000033
通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角判断车辆是否完成楼梯攀爬。
8.一种面向全地形车的楼梯自动检测与攀爬系统，其特征在于，包括楼梯自动检测模块、车辆航向与楼梯对齐模块以及自动攀爬楼梯模块；其中，楼梯自动检测模块对采集到的点云数据进行预处理，对预处理后的点云数据进行分割；将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断该团簇是否为楼梯；车辆航向与楼梯对齐模块将属于楼梯的点簇投影到水平面上，计算楼梯与激光雷达坐标系的原点的距离以及相对于x轴的旋转角度，当车辆与楼梯相距较近，且需要攀爬通过楼梯时，将车辆航向自动调整到与楼梯垂直；自动攀爬楼梯模块通过对车辆横滚角的测量实现对车辆航向与楼梯相对角度的估计，根据横滚角的大小判断车辆朝向是否与楼梯保持垂直，并据此引导车辆以垂直方向攀爬楼梯，根据车辆俯仰角大小判断车辆是否完成楼梯攀爬。
9.一种全地形车，包括车载计算机、车载多线激光雷达和车载惯性测量单元，其特征在于，所述车载计算机包括至少一个处理器和存储器，所述存储器与处理器通过I/O接口连接，车载多线激光雷达和车载惯性测量单元均连接处理器的输入端，存储器用于存储计算机程序，所述处理器执行所述计算机程序时实现权利要求1-7中任意一项所述面向全地形车的楼梯自动检测与攀爬方法，车载惯性测量单元用于获取实时车辆三轴姿态数据，车载多线激光雷达用于实时测得所述单帧或多帧点云数据，处理器在执行步骤1时获取所述单帧或多帧点云数据，处理器在执行步骤3时获取所述实时车辆三轴姿态数据。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述面向全地形车的楼梯自动检测与攀爬方法的步骤。
CN202010910982.3A 2020-09-02 2020-09-02 一种面向全地形车及其自动检测楼梯与攀爬方法 Active CN112099494B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010910982.3A CN112099494B (zh) 2020-09-02 2020-09-02 一种面向全地形车及其自动检测楼梯与攀爬方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010910982.3A CN112099494B (zh) 2020-09-02 2020-09-02 一种面向全地形车及其自动检测楼梯与攀爬方法

Publications (2)

Publication Number Publication Date
CN112099494A CN112099494A (zh) 2020-12-18
CN112099494B true CN112099494B (zh) 2021-09-07

Family

ID=73757557

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010910982.3A Active CN112099494B (zh) 2020-09-02 2020-09-02 一种面向全地形车及其自动检测楼梯与攀爬方法

Country Status (1)

Country Link
CN (1) CN112099494B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112904306A (zh) * 2021-01-18 2021-06-04 深圳市普渡科技有限公司 斜坡感知方法、装置、机器人和存储介质
CN113340310B (zh) * 2021-07-08 2024-03-15 深圳市人工智能与机器人研究院 一种移动机器人台阶地形识别定位方法及相关装置
CN113791400B (zh) * 2021-09-07 2023-10-13 大连理工大学 一种基于激光雷达的楼梯参数自主检测方法
CN115063431B (zh) * 2022-08-19 2022-11-11 山东远盾网络技术股份有限公司 一种基于图像处理的汽车零部件质量溯源方法

Family Cites Families (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2112963B1 (en) * 2006-10-06 2018-05-30 iRobot Defense Holdings, Inc. Robotic vehicle with tracks and flippers
CN100538723C (zh) * 2007-10-26 2009-09-09 浙江工业大学 多视觉传感器信息融合的内河船舶自动识别系统
CN204521268U (zh) * 2015-03-11 2015-08-05 西安交通大学 一种自动贴合楼梯表面式履带型自动上楼机
CN108267752B (zh) * 2016-12-15 2022-01-18 苏州宝时得电动工具有限公司 自移动设备的工作区域的分区方法、装置和电子设备
KR101911619B1 (ko) * 2017-02-17 2018-10-24 경남대학교 산학협력단 기울기 센서를 이용한 캐터필러 화물 운반 장치
JP2020520008A (ja) * 2017-05-09 2020-07-02 ブレーン コーポレーションＢｒａｉｎ Ｃｏｒｐｏｒａｔｉｏｎ ロボットの動作制御のためのシステムおよび方法
CN106980321B (zh) * 2017-05-18 2019-04-16 中国科学院合肥物质科学研究院 一种六足履式无人地面车辆的上台阶步态规划系统及其方法
CN107831484B (zh) * 2017-09-29 2021-11-19 深圳市行者机器人技术有限公司 一种基于激光雷达的阶梯数据获取方法及装置
CN108312150A (zh) * 2017-12-31 2018-07-24 天津木牛流马科技发展股份有限公司 楼宇服务机器人
KR101882483B1 (ko) * 2018-01-24 2018-07-27 엘아이지넥스원 주식회사 무인 수상정의 장애물 탐지 장치 및 방법
CN108876799B (zh) * 2018-06-12 2021-07-02 杭州视氪科技有限公司 一种基于双目相机的实时台阶检测方法
CN109434795B (zh) * 2018-10-29 2024-10-18 华中科技大学 一种具有目标跟踪和自动避障的载物爬楼机器人
CN109696663B (zh) * 2019-02-21 2021-02-09 北京大学 一种车载三维激光雷达标定方法和系统
CN110319813B (zh) * 2019-07-16 2021-02-02 河海大学 一种测量不同斜坡坡度角的测度尺及测量方法
CN110441791B (zh) * 2019-08-14 2023-07-04 深圳无境智能机器人有限公司 一种基于前倾2d激光雷达的地面障碍物检测方法
CN111077541B (zh) * 2019-12-30 2022-03-22 深圳优地科技有限公司 一种障碍物识别方法、障碍物识别装置及移动机器人

Also Published As

Publication number Publication date
CN112099494A (zh) 2020-12-18

Similar Documents

Publication Publication Date Title
CN112099494B (zh) 一种面向全地形车及其自动检测楼梯与攀爬方法
CN111337941B (zh) 一种基于稀疏激光雷达数据的动态障碍物追踪方法
CN111291708B (zh) 融合深度相机的变电站巡检机器人障碍物检测识别方法
CN110687904A (zh) 一种巡线机器人视觉导航巡检和避障方法
CN111461023A (zh) 基于三维激光雷达的四足机器人自主跟随领航员的方法
CN109434251B (zh) 一种基于粒子滤波的焊缝图像跟踪方法
CN108280840B (zh) 一种基于三维激光雷达的道路实时分割方法
CN112883878B (zh) 一种基于三维格网的变电站场景下的点云自动分类方法
CN111368607A (zh) 一种机器人、障碍物的检测方法及检测装置
CN113409252B (zh) 一种架空输电线路巡检机器人障碍物检测方法
CN112945196B (zh) 一种基于点云数据的露天矿台阶线提取和边坡监测的方法
CN114266960A (zh) 一种点云信息与深度学习相结合的障碍物检测方法
CN114926356B (zh) 一种针对降雪影响的LiDAR点云无监督去噪方法
CN114863376A (zh) 一种基于车载激光点云数据的道路标线分割方法及系统
CN113029185A (zh) 众包式高精度地图更新中道路标线变化检测方法及系统
CN115082665A (zh) 可提高作业精度的agv小车视觉自检方法、装置及系统
CN117746165A (zh) 一种轮式挖掘机轮胎种类的识别方法及装置
CN108520255B (zh) 一种红外弱小目标检测方法及装置
CN117629215A (zh) 一种基于单线激光雷达点云配准的底盘充电回桩方法
CN115291240B (zh) 矿区场景内卸载点后方挡墙感知识别的检测方法及系统
CN110231035A (zh) 攀爬移动机器人路径引导方法
CN110322508A (zh) 一种基于计算机视觉的辅助定位方法
CN114821553A (zh) 作业机械的贴边控制方法、装置、作业机械与电子设备
CN112729289A (zh) 应用于自动导引车的定位方法、装置、设备和存储介质
Mijit et al. LR-Seg: A Ground Segmentation Method for Low-Resolution LiDAR Point Clouds

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
GR01 Patent grant
GR01 Patent grant