CN113156948B - 一种两轮自平衡机器人的地面与障碍物区分识别方法 - Google Patents

Abstract

本发明公开了一种两轮自平衡机器人的地面与障碍物区分识别方法，包括：接收惯性测量单元上传的IMU数据以及2D激光雷达上传的激光雷达数据；对接收的IMU数据和激光雷达数据进行时间戳同步处理；根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角；通过俯仰角对时间戳同步处理后的激光雷达数据进行修正。本发明克服了因两轮自平衡机器人晃动导致的误识别缺陷。

Description

一种两轮自平衡机器人的地面与障碍物区分识别方法

技术领域

本申请属于移动机器人导航技术领域，具体涉及一种两轮自平衡机器人的地面与障碍物区分识别方法。

背景技术

两轮自平衡机器人在运动时和四轮机器人有很大的区别，两轮自平衡机器人首先要保证自身的平衡性，然后才是导航和避障。通常实际应用中使用的两轮自平衡机器人因尺寸较大，建图和定位需要考虑大范围的场景，因此需要使用安装于机器人“头部”的3D激光雷达完成；而导航和避障需要近距离的信息，所以使用安装于机器人“底部”的2D激光雷达完成。

机器人在导航的过程中，会维护一张较小范围的局部地图，当障碍物“进入”局部地图的范围内，机器人识别到障碍物后会自动规避。但实际使用时发现，机器人在运动过程中无法一直保持水平运动，会出现前后轻微的晃动，导致2D激光雷达在晃动过程中照射到地面，因此存在将地面误识别为障碍物，影响机器人的正常运动的情况。传统识别地面的方法是使用3D激光雷达对地面进行聚类，聚类的本质是找到最近的点，只要点与点之间的欧几里得距离在一定半径范围内就认为这些点属于一个类，从而将地面和非地面分别开来，但是点云信息一来数据量庞大，二来不能直接应用于机器人的导航过程。因此在机器人的运动中还是主要依靠2D激光雷达完成。

发明内容

本申请的目的在于提供一种两轮自平衡机器人的地面与障碍物区分识别方法，克服了因两轮自平衡机器人晃动导致的误识别缺陷。

为实现上述目的，本申请所采取的技术方案为：

一种两轮自平衡机器人的地面与障碍物区分识别方法，所述两轮自平衡机器人上安装有惯性测量单元和2D激光雷达，所述两轮自平衡机器人的地面与障碍物区分识别方法，包括：

步骤1、接收惯性测量单元上传的IMU数据以及2D激光雷达上传的激光雷达数据；

步骤2、对接收的IMU数据和激光雷达数据进行时间戳同步处理；

步骤3、根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角；

步骤4、通过俯仰角对时间戳同步处理后的激光雷达数据进行修正，包括：

步骤4.1、获取两轮自平衡机器人的轮子的半径为r，两轮圆心连线的中点为A点，过A点做线段AE垂直于地面交于点E，则AE＝r，取2D激光雷达的中心位置为点C，获取两轮自平衡机器人平衡状态下A点到C点的距离为AC，获取两轮自平衡机器人平衡状态下A点和C点之间的水平高度差值为AB、水平间距为BC，且获取的俯仰角为θ，则计算得到两轮自平衡机器人运动过程中C点产生位移后的C′点与地面的实际距离为C′D如下：

AF＝AC′×cos(90°-θ-α)＝AC×sin(θ+α)

式中，α为线段AC和线段BC之间的夹角，AF为两轮自平衡机器人运动过程中的A点和C′点之间的水平高度差值，D点为过C′点做垂直于地面的线段与地面的交点；

步骤4.2、取中心位置位移至C′点后的2D激光雷达的照射面与地面的交线为GM，其中点G为2D激光雷达0°角照射方向与地面的交点，点M为2D激光雷达β°角照射方向与地面的交点，则线段C′G为2D激光雷达的正方向，计算β°角下2D激光雷达到达地面的距离C′M以及2D激光雷达的照射距离DM如下：

步骤4.3、取β°角下2D激光雷达到达地面的距离C′M为计算值C_C′M，取时间戳同步处理后的激光雷达数据为实际测量值M_C′M；

若

则表示β°角下2D激光雷达的照射对象为地面，设置修正后的照射距离D′M′＝inf；否则表示β°角下2D激光雷达的照射对象为障碍物，设置修正后的照射距离D′M′＝DM，其中Δk为判断阈值。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，β的初始值为-45，且β的取值范围为-45～45。

作为优选，所述根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角，包括：

获取IMU数据为四元数信息[w x y z]，满足w²+x²+y²+z²＝1；

根据四元数信息转换为欧拉角的公式如下：

其中，roll，pitch，yaw分别为两轮自平衡机器人绕x轴，y轴，z轴的旋转角度，并且pitch角即为所需的俯仰角。

本申请提供的两轮自平衡机器人的地面与障碍物区分识别方法，结合机器人的IMU数据，获取机器人的晃动幅度，并根据晃动幅度修正所采集的激光雷达数据，从而分辨出激光雷达的照射对象为地面或障碍物，大大降低了激光雷达识别错误率，保证机器人的正常运动。

附图说明

图1为本申请的两轮自平衡机器人的地面与障碍物区分识别方法的流程图；

图2为本申请计算两轮自平衡机器人在运动过程中2D激光雷达的中心位置与地面的距离的示意图；

图3为本申请计算2D激光雷达的照射距离的示意图；

图4为本申请两轮自平衡机器人的地面与障碍物区分识别方法在一实施例中的流程图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

其中一个实施例中，提供一种两轮自平衡机器人的地面与障碍物区分识别方法，克服现有技术中因两轮自平衡机器人运动过程中的晃动而导致2D激光雷达误检的情况，显著提高激光雷达识别准确率，保证两轮自平衡机器人的正常运动。

容易理解的是，要保证两轮自平衡机器人的正常运行，其必不可少的需具备左右两轮、驱动单元、惯性测量单元和2D激光雷达等部件，关于两轮自平衡机器人的具体结构不作为本实施例的重点，不展开描述，本申请的方法应用对象为可支持正常行驶的两轮自平衡机器人即可。

具体的，如图1所示，本实施例的两轮自平衡机器人的地面与障碍物区分识别方法，包括：

步骤1、接收惯性测量单元上传的IMU数据以及2D激光雷达上传的激光雷达数据。本实施例中2D激光雷达安装在两轮的中垂线上，但其前后位置不做限制，应该理解的是，这里的前后指两轮自平衡机器人前进方向为前，后退方向为后，并在前后确定的情况下描述左右方向。

步骤2、对接收的IMU数据和激光雷达数据进行时间戳同步处理。进行时间戳同步处理是为了保证IMU数据和激光雷达数据的对应性，避免因数据存在时间差扩大修正后数据的误差。并且本实施例中时间戳同步处理基于现有技术实现即可，不作为本实施例的重点。

步骤3、根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角。在后续的数据修正中，本实施例结合IMU数据解算的欧拉角，确定两轮自平衡机器人当前运动状态下的晃动幅度，以提高数据修正的准确性。在一个实施例中，提供的解算过程如下：

本实施例中获取的IMU数据为四元数信息[w x y z]，满足w²+x²+y²+z²＝1。

根据四元数信息转换为欧拉角的公式如下：

其中，roll，pitch，yaw分别为两轮自平衡机器人绕x轴，y轴，z轴的旋转角度，并且pitch角即为所需的俯仰角。

步骤4、通过俯仰角对时间戳同步处理后的激光雷达数据进行修正，包括：

步骤4.1、如图2所示，获取两轮自平衡机器人的轮子的半径为r，两轮圆心连线的中点为A点，过A点做线段AE垂直于地面交于点E，则AE＝r，取2D激光雷达的中心位置为点C，获取两轮自平衡机器人平衡状态下A点到C点的距离为AC，获取两轮自平衡机器人平衡状态下A点和C点之间的水平高度差值为AB、水平间距为BC，且获取的俯仰角为θ，则计算得到两轮自平衡机器人运动过程中C点产生位移后的C′点与地面的实际距离为C′D如下：

AF＝AC′×cos(90°-θ-α)＝AC×sin(θ+α)

式中，α为线段AC和线段BC之间的夹角，AF为两轮自平衡机器人运动过程中的A点和C′点之间的水平高度差值，D点为过C′点做垂直于地面的线段与地面的交点。由于两轮自平衡机器人在晃动过程中围绕轮子中心进行圆周晃动，而晃动过程中2D激光雷达的中心位置与A点的距离是不变的，因此本实施例中的AC′＝AC。容易理解的是，这里的C′点为C点在机器人运动过程中绕A点进行圆周运动的曲线上的任意一点。

步骤4.2、如图3所示，取中心位置位移至C′点后的2D激光雷达的照射面与地面的交线为GM，其中点G为2D激光雷达0°角照射方向与地面的交点，点M为2D激光雷达β°角照射方向与地面的交点，则线段C′G为2D激光雷达的正方向，计算β°角下2D激光雷达到达地面的距离C′M以及2D激光雷达的照射距离DM如下：

步骤4.3、取β°角下2D激光雷达到达地面的距离C′M为计算值C_C′M，取时间戳同步处理后的激光雷达数据为实际测量值M_C′M。

若

则表示β°角下2D激光雷达的照射对象为地面，设置修正后的照射距离D′M′＝inf(inf表无穷)；否则表示β°角下2D激光雷达的照射对象为障碍物，设置修正后的照射距离D′M′＝DM，其中Δk为判断阈值，根据判断阈值确定当前角度下的照射对象，该阈值通常可根据多次试验得到，例如Δk＝0.05。

对于2D激光雷达而言其具有较大的扫描角度，但是由于受限于其在机器人上的安装位置，导致部分角度下视野受机器人结构遮挡，因此本实施例中β°角的取值范围与2D激光雷达在机器人上的安装位置有关，通常为了保证具有较优的识别效果，需保证β取值的绝对值不小于45，本实施例中优选设置β的初始值为-45，且β的取值范围为-45～45，本实施例在数据修正中以β每次加1对各个照射角度下的照射距离进行修正。

需要说明的是，为了保证本实施例区分识别方法的正常应用，本实施例中两轮自平衡机器人的平衡状态应理解为俯仰角的绝对值为0～2.86之间，并且考虑到机器人通常在0.8～2米之间进行障碍物识别，以保证机器人的正常行驶，因此本实施例设置在俯仰角的值在2.86～7.125之间时进行地面与障碍物区分识别，其余角度下不进行区分识别。

其中修正后的照射距离D′M′可用于机器人的避障运动或导航，在避障运动或导航中根据D′M′的返回值即可确定当前照射为地面还是障碍物，使机器人基于正确的反馈实现运动，保证机器人的运动精度以及正常性。

为了便于对本申请方法的理解，以下提供一种具体的应用实例，如图4所示。

1)启动两轮自平衡机器人底盘，实时上发IMU数据；

2)工控机接收底盘上发的IMU数据，按照ros环境中标准IMU数据格式解析，数据类型为sensor_msgs::IMU，话题为imu_data；

3)启动2D激光雷达，发布原始激光数据，数据类型为sensor_msgs::LaserScan，话题为scan；

4)使用一个新的节点同时订阅激光雷达话题scan和IMU话题imu_data，并做时间戳同步，保证信息的一致性，并在回调函数中通过IMU数据中的四元数解算欧拉角，获取俯仰角pitch信息；

5)在pitch大于2.86且小于7.125时，遍历当前帧雷达信息，初始β值为-45，以β++循环通过俯仰角信息对激光雷达数据进行修正，直至β值大于45，发布修正后的新的雷达话题new_scan；在pitch为其他取值情况下，直接发布原始雷达数据scan；

5)计算机订阅new_scan话题或scan话题后，利用修正后的雷达数据实现机器人的导航和避障。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (3)

1.一种两轮自平衡机器人的地面与障碍物区分识别方法，所述两轮自平衡机器人上安装有惯性测量单元和2D激光雷达，其特征在于，所述两轮自平衡机器人的地面与障碍物区分识别方法，包括：

步骤1、接收惯性测量单元上传的IMU数据以及2D激光雷达上传的激光雷达数据；

步骤2、对接收的IMU数据和激光雷达数据进行时间戳同步处理；

步骤3、根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角；

步骤4、通过俯仰角对时间戳同步处理后的激光雷达数据进行修正，包括：

步骤4.1、获取两轮自平衡机器人的轮子的半径为r，两轮圆心连线的中点为A点，过A点做线段AE垂直于地面交于点E，则AE＝r，取2D激光雷达的中心位置为点C，获取两轮自平衡机器人平衡状态下A点到C点的距离为AC，获取两轮自平衡机器人平衡状态下A点和C点之间的水平高度差值为AB、水平间距为BC，且获取的俯仰角为θ，则计算得到两轮自平衡机器人运动过程中C点产生位移后的C′点与地面的实际距离为C′D如下：

AF＝AC′×cos(90°-θ-α)＝AC×sin(θ+α)

式中，α为线段AC和线段BC之间的夹角，AF为两轮自平衡机器人运动过程中的A点和C′点之间的水平高度差值，D点为过C′点做垂直于地面的线段与地面的交点；

步骤4.2、取中心位置位移至C′点后的2D激光雷达的照射面与地面的交线为GM，其中点G为2D激光雷达0°角照射方向与地面的交点，点M为2D激光雷达β°角照射方向与地面的交点，则线段C′G为2D激光雷达的正方向，计算β°角下2D激光雷达到达地面的距离C′M以及2D激光雷达的照射距离DM如下：

步骤4.3、取β°角下2D激光雷达到达地面的距离C′M为计算值C_C′M，取时间戳同步处理后的激光雷达数据为实际测量值M_C′M；

若

则表示β°角下2D激光雷达的照射对象为地面，设置修正后的照射距离D′M′＝inf；否则表示β°角下2D激光雷达的照射对象为障碍物，设置修正后的照射距离D′M′＝DM，其中Δk为判断阈值。

2.如权利要求1所述的两轮自平衡机器人的地面与障碍物区分识别方法，其特征在于，β的初始值为-45，且β的取值范围为-45～45。

3.如权利要求1所述的两轮自平衡机器人的地面与障碍物区分识别方法，其特征在于，所述根据时间戳同步处理后的IMU数据解算欧拉角，并从欧拉角中获取俯仰角，包括：

获取IMU数据为四元数信息[W x y z]，满足w²+x²+y²+z²＝1；

根据四元数信息转换为欧拉角的公式如下：

其中，roll，pitch，yaw分别为两轮自平衡机器人绕x轴，y轴，z轴的旋转角度，并且pitch角即为所需的俯仰角。

Images