CN115575923A - 一种地面机器人静止判断方法、系统、装置以及存储介质 - Google Patents

一种地面机器人静止判断方法、系统、装置以及存储介质 Download PDF

Info

Publication number
CN115575923A
CN115575923A CN202211572435.4A CN202211572435A CN115575923A CN 115575923 A CN115575923 A CN 115575923A CN 202211572435 A CN202211572435 A CN 202211572435A CN 115575923 A CN115575923 A CN 115575923A
Authority
CN
China
Prior art keywords
robot
static
stillness
calculating
laser radar
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.)
Granted
Application number
CN202211572435.4A
Other languages
English (en)
Other versions
CN115575923B (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.)
Kilo X Robotics Co ltd
Original Assignee
Kilo X Robotics Co ltd
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 Kilo X Robotics Co ltd filed Critical Kilo X Robotics Co ltd
Priority to CN202211572435.4A priority Critical patent/CN115575923B/zh
Publication of CN115575923A publication Critical patent/CN115575923A/zh
Application granted granted Critical
Publication of CN115575923B publication Critical patent/CN115575923B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种地面机器人静止判断方法、系统、装置以及存储介质，该方法如下：S1、在激光雷达采集的一帧点云数据中提取合适的点；S2、根据提取点之间的深度差，计算激光雷达的静止量
Figure 36629DEST_PATH_IMAGE001
；S3、根据静止量
Figure 707782DEST_PATH_IMAGE001
和IMU数据，得到区分机器人状态的阈值
Figure 5908DEST_PATH_IMAGE002
；S4、根据IMU数据窗口采样，计算激光雷达的静止量
Figure 78906DEST_PATH_IMAGE003
；S5、综合激光雷达与IMU数据，得到机器人静止概率
Figure 109179DEST_PATH_IMAGE004
。本发明能够非常精准有效的判断机器人是否处于静止状态，便于机器人在静止状态时不更新自身位置，节省了大部分算力。

Description

一种地面机器人静止判断方法、系统、装置以及存储介质
技术领域
本发明涉及一种地面机器人静止判断技术，尤其涉及一种地面机器人静止判断方法、系统、装置以及存储介质，属于机器人技术领域。
背景技术
目前，前沿通用型机器人行业普遍采用基于激光雷达为主要传感器构建的地图进行3D空间定位，当机器人在超大型场景下工作时，由于机器人端硬件算力的限制，超大型场景庞大的点云数据量往往导致机器人系统难以加载与处理，限制了机器人的工作能力。
如在变电站巡检中应用时，巡检机器人的行为包括在全站范围巡检，以及在充电房充电；在上述过程中，机器人还需要实时计算雷达在点云地图当中的位置，而实时计算自身位置需要处理庞大的数据量，对于专注于巡检领域的机器人来说算力消耗较大。为此，本发明考虑可以在机器人处于静止状态的时候，通过让自身位置不更新来节省大部分算力，这就需要非常精准有效判断出地面机器人的静止状态。
因此，亟待研究出一种机器人静止判断方法，以便非常精准有效的控制自身位置不更新的时机。
发明内容
针对上述现存的技术问题，本发明的目的在于实现非常精准有效的判断机器人是否处于静止状态的目的，从而便于机器人在静止状态时不更新自身位置，进而节省大部分算力，提供一种地面机器人静止判断方法、系统、装置以及存储介质。
为实现上述目的，本发明提供一种地面机器人静止判断方法，包括如下步骤：
S1、在激光雷达采集的一帧点云数据中提取合适的点；
S2、根据提取点之间的深度差，计算激光雷达的静止量
Figure 50755DEST_PATH_IMAGE001
；
S3、根据静止量
Figure 90255DEST_PATH_IMAGE001
和IMU数据，得到区分机器人状态的阈值
Figure 955443DEST_PATH_IMAGE002
；
S4、根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 422196DEST_PATH_IMAGE003
；
S5、综合激光雷达与IMU数据，得到机器人静止概率
Figure 271204DEST_PATH_IMAGE004
。
本发明方法进一步的，所述S1包括如下步骤：
S01、采集16线激光雷达的点云数据；
S02、选择一帧第0-N 线的点云数据，其中0＜N＜5；
S03、记每一线的点为：
Figure 52078DEST_PATH_IMAGE005
，并提取点
Figure 568510DEST_PATH_IMAGE006
，
Figure 675006DEST_PATH_IMAGE007
。
本发明方法具体的，所述S02中的N为3，即选取一帧第0-3 线的点云数据。
本发明方法更进一步的，所述S2包括如下步骤：
S201、将提取点投影到XOY水平坐标系；
S202、设定提取点之间的深度差，公式为：
Figure 11309DEST_PATH_IMAGE008
；
S203、计算断层的大小，公式为：
Figure 595875DEST_PATH_IMAGE009
；
S204、得到激光雷达的静止量
Figure 966813DEST_PATH_IMAGE010
，公式为：
Figure 978631DEST_PATH_IMAGE011
，
式中，g函数一般采用二次方程。
本发明方法进一步的，所述S3包括如下步骤：
S301、根据静止量
Figure 67810DEST_PATH_IMAGE010
的数值，结合最新的单帧IMU数据的加速度和角速度信息，将 当前机器人的状态区分为八种情况，以及高、低角速度，高、低加速度，高、低静止量
Figure 190487DEST_PATH_IMAGE010
的情 况；
S302、设计区分八种情况的不同阈值
Figure 415932DEST_PATH_IMAGE002
，以及区分高、低角速度，高、低加速度，高、 低静止量
Figure 598652DEST_PATH_IMAGE010
的不同固定阀值。
本发明方法进一步的，所述S4包括如下步骤：
S401、选取时间窗口等于激光雷达数据周期
Figure 175126DEST_PATH_IMAGE012
内的IMU数据；
S402、计算IMU数据的角速度的分布平均值
Figure 101494DEST_PATH_IMAGE013
和方差
Figure 447025DEST_PATH_IMAGE014
，设定根据角速度得 到的机器人静止权重，公式为：
Figure 800646DEST_PATH_IMAGE015
；
S403、计算IMU数据的加速度的分布平均值
Figure 598837DEST_PATH_IMAGE016
和方差
Figure 63317DEST_PATH_IMAGE017
，依照方差超过一定 阈值
Figure 263354DEST_PATH_IMAGE018
的情况，设定根据加速度得到的机器人静止权重，公式为：
当
Figure 53455DEST_PATH_IMAGE019
时，
Figure 338943DEST_PATH_IMAGE020
,
当
Figure 872693DEST_PATH_IMAGE021
时，
Figure 927236DEST_PATH_IMAGE022
；
S404、计算激光雷达的静止量
Figure 622660DEST_PATH_IMAGE023
，公式为：
Figure 395444DEST_PATH_IMAGE024
。
本发明方法进一步的，所述S5包括如下步骤：
设计激光雷达与IMU数据融合情况下的机器人静止概率，公式为：
Figure 467305DEST_PATH_IMAGE025
，
式中，
Figure 110776DEST_PATH_IMAGE026
函数根据实测数据进行拟合得到。
本发明还提供一种地面机器人静止判断系统，包括：点云数据提取模块，第一静止量计算模块，状态阈值计算模块，第二静止量计算模块，以及静止概率计算模块：
点云数据提取模块，用于在激光雷达采集的一帧点云数据中提取合适的点；
第一静止量计算模块，用于根据提取点之间的深度差，计算激光雷达的静止量
Figure 242680DEST_PATH_IMAGE027
；
状态阈值计算模块，用于根据静止量
Figure 237181DEST_PATH_IMAGE028
和IMU数据，得到区分机器人状态的阈值
Figure 112733DEST_PATH_IMAGE029
；
第二静止量计算模块，用于根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 876290DEST_PATH_IMAGE030
；
静止概率计算模块，用于综合激光雷达与IMU数据，得到机器人静止概率
Figure 179095DEST_PATH_IMAGE031
。
本发明系统进一步的，所述的点云数据提取模块包括：数据采集单元，线路选择单元，以及点提取单元；
数据采集单元，用于采集16线激光雷达的点云数据；
线路选择单元，用于选择一帧第0-N 线的点云数据，其中0＜N＜5；
点提取单元，用于记每一线的点为：
Figure 660892DEST_PATH_IMAGE032
，并提取点
Figure 74556DEST_PATH_IMAGE033
与
Figure 223777DEST_PATH_IMAGE034
。
本发明系统进一步的，所述的第一静止量计算模块包括投影坐标单元，深度差计 算单元，断层计算单元，以及静止量
Figure 697484DEST_PATH_IMAGE035
计算单元；
投影坐标单元，用于将提取点投影到XOY水平坐标系；
深度差计算单元，用于设定提取点之间的深度差，公式为：
Figure 666577DEST_PATH_IMAGE036
；
断层计算单元：用于计算断层的大小，公式为：
Figure 883932DEST_PATH_IMAGE037
；
静止量
Figure 622081DEST_PATH_IMAGE038
计算单元，用于得到激光雷达的静止量
Figure 266689DEST_PATH_IMAGE039
，公式为：
Figure 723078DEST_PATH_IMAGE040
；
式中，g函数一般采用二次方程。
本发明系统进一步的，所述的第二静止量计算模块包括：IMU窗口选取单元，角速 度计算单元，加速度计算单元，以及静止量
Figure 478544DEST_PATH_IMAGE041
计算单元；
IMU窗口选取单元，用于选取时间窗口等于激光雷达数据周期
Figure 336779DEST_PATH_IMAGE042
内的IMU数据；
角速度计算单元，用于计算IMU数据的角速度的分布平均值
Figure 886709DEST_PATH_IMAGE043
和方差
Figure 830394DEST_PATH_IMAGE044
， 设定根据角速度得到的机器人静止权重，公式为：
Figure 389551DEST_PATH_IMAGE045
；
加速度计算单元，用于计算IMU数据的加速度的分布平均值
Figure 102292DEST_PATH_IMAGE046
和方差
Figure 291965DEST_PATH_IMAGE047
， 依照方差超过一定阈值
Figure 277543DEST_PATH_IMAGE048
的情况，设定根据加速度得到的机器人静止权重，公式为：
当
Figure 109232DEST_PATH_IMAGE049
时，
Figure 207638DEST_PATH_IMAGE050
,
当
Figure 364950DEST_PATH_IMAGE051
时，
Figure 17649DEST_PATH_IMAGE052
；
静止量
Figure 918608DEST_PATH_IMAGE053
计算单元，用于计算激光雷达的静止量
Figure 605942DEST_PATH_IMAGE054
，公式为：
Figure 934155DEST_PATH_IMAGE055
。
本发明又提供一种地面机器人静止判断装置，包括：至少一个处理器，以及与所述的至少一个处理器通信连接的存储器；
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的地面机器人静止判断方法。
本发明再提供一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述的地面机器人静止判断方法步骤。
综上，本发明可以非常精准有效判断机器人是否处于静止状态，这样一来，当机器人处于静止状态的时候，不再实时更新自身位置，避免了时刻处理超大型场景庞大的点云数据量，从而节省了大部分算力，提升了机器人的工作性能。
相比现有技术，本发明的技术优势在于：
1、本发明能够根据激光雷达下方的点云数据的深度关系来估算机器人的静止量。
2、本发明能够根据当前的IMU数据，并结合激光雷达的数据，将机器人的运动情况区分为不同的情况。
3、本发明能够根据IMU数据的分布关系来估算机器人的静止量。
4、本发明能够根据点云数据的深度关系估算的机器人静止量，以及根据IMU数据的分布关系估算机器人静止量，得到机器人静止概率，且静止状态的判断准确有效。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明方法的全部流程图；
图2为本发明系统的模块原理框图；
图3为本发明装置的硬件组成框图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、模块和/或单元的存在，但并不排除一个或多个其它特征、整体、步骤、操作、模块、单元和/或其集合的存在或添加。
还应当理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
一、地面机器人静止判断方法实施例：
如图1所示，本发明提供一种地面机器人静止判断方法，包括如下实施步骤：
S1、在激光雷达采集的一帧点云数据中提取合适的点，具体实施步骤如下：
S01、采集16线激光雷达的点云数据。
S02、选择一帧第0-N 线的点云数据，其中0＜N＜5。根据经验， N优选3，即选取激光雷达一帧点云数据中的第0-3 线的点云数据，因为这几线比较容易与地面相交。
S03、记每一线的点为：
Figure 74149DEST_PATH_IMAGE056
，且每一线数据里面包含大约2400个三维 点，并提取其中的点
Figure 513221DEST_PATH_IMAGE057
与
Figure 55061DEST_PATH_IMAGE058
，总共约120个点。
并且，选用点
Figure 554175DEST_PATH_IMAGE059
与
Figure 181465DEST_PATH_IMAGE058
的原因在于：在激光雷达按列扫描过程中，这两 段数据存在近似0.1秒的时间间隔，相当于激光雷达的数据采集周期；而激光雷达一帧点云 数据是在一定时间间隔
Figure 627490DEST_PATH_IMAGE060
内采集的，如果机器人携带激光雷达在
Figure 23837DEST_PATH_IMAGE060
内发生运动，那么这约 120个点将会产生一定的断层，用于后续根据断层的大小得到激光雷达的静止量
Figure 693852DEST_PATH_IMAGE061
。
S2、根据提取点之间的深度差，计算激光雷达的静止量
Figure 808439DEST_PATH_IMAGE062
，具体步骤如下：
S201、将上步提取的点投影到XOY水平坐标系。
S202、设定提取点之间的深度差，公式为：
Figure 854892DEST_PATH_IMAGE063
。
S203、计算断层的大小，公式为：
Figure 105745DEST_PATH_IMAGE064
。
S204、根据断层的大小，得到激光雷达的静止量
Figure 946662DEST_PATH_IMAGE065
，公式为：
Figure 548545DEST_PATH_IMAGE066
。
并且，为了让U更加有区分度，g函数一般采用二次方程，例如一元二次函数ax2+bx+c。
S3、根据静止量
Figure 398689DEST_PATH_IMAGE067
和IMU数据，得到区分机器人状态的阈值
Figure 504048DEST_PATH_IMAGE068
，具体实施步骤如 下：
S301、根据静止量
Figure 515867DEST_PATH_IMAGE067
数值的大小，结合最新的单帧IMU数据的加速度和角速度信 息，将当前机器人的状态区分为八种情况，以及高、低角速度，高、低加速度，高、低静止量
Figure 605045DEST_PATH_IMAGE069
的情况。
S302、由于数据的高低是通过和一个固定的阈值比较得到，所以对于八种情况为 后续的流程设计套不同的阈值
Figure 727722DEST_PATH_IMAGE070
，以及为区分高、低角速度，高、低加速度，高、低静止量
Figure 218746DEST_PATH_IMAGE067
设计不同的固定阀值。
S4、根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 401466DEST_PATH_IMAGE071
，具体实施步骤如下：
S401、首先进行IMU数据的取样，选取时间窗口等于激光雷达数据周期
Figure 712362DEST_PATH_IMAGE072
内的IMU 数据。
S402、计算IMU数据的角速度的分布平均值
Figure 638729DEST_PATH_IMAGE073
和方差
Figure 249839DEST_PATH_IMAGE074
，设定根据角速度得 到的机器人静止权重，公式为：
Figure 603460DEST_PATH_IMAGE075
。
S403、计算IMU数据的加速度的分布平均值
Figure 729548DEST_PATH_IMAGE076
和方差
Figure 256344DEST_PATH_IMAGE077
。由于当物体静止或 者匀速运动的时候， IMU数据的加速度的方差都几乎为0，所以仅惩罚当方差超过一定阈值
Figure 456381DEST_PATH_IMAGE078
的情况。因此，依照方差超过一定阈值
Figure 980904DEST_PATH_IMAGE079
的情况，设定根据加速度得到的机器人静止 权重，公式为：
当
Figure 266392DEST_PATH_IMAGE080
时，
Figure 534562DEST_PATH_IMAGE081
,
当
Figure 589106DEST_PATH_IMAGE082
时，
Figure 550108DEST_PATH_IMAGE083
。
S404、计算激光雷达的静止量
Figure 322892DEST_PATH_IMAGE084
，公式为：
Figure 129174DEST_PATH_IMAGE085
。
S5、综合激光雷达与IMU数据，得到机器人静止概率
Figure 303804DEST_PATH_IMAGE086
，具体步骤如下：
设计激光雷达与IMU数据融合情况下的机器人静止概率，公式为：
Figure 170128DEST_PATH_IMAGE087
。
式中， k函数可以采用二元二次函数，具体参数需要根据实测数据进行拟合得到。
二、地面机器人静止判断系统实施例：
如图2所示，本发明还提供一种地面机器人静止判断系统，包括：点云数据提取模块，第一静止量计算模块，状态阈值计算模块，第二静止量计算模块，以及静止概率计算模块。
点云数据提取模块，用于在激光雷达采集的一帧点云数据中提取合适的点。
第一静止量计算模块，用于根据提取点之间的深度差，计算激光雷达的静止量
Figure 633471DEST_PATH_IMAGE088
。
状态阈值计算模块，用于根据静止量
Figure 243444DEST_PATH_IMAGE062
和IMU数据，得到区分机器人状态的阈值
Figure 69317DEST_PATH_IMAGE089
。
第二静止量计算模块，用于根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 372123DEST_PATH_IMAGE090
。
静止概率计算模块，用于综合激光雷达与IMU数据，得到机器人静止概率
Figure 853920DEST_PATH_IMAGE086
。
本发明系统进一步实施时，所述的点云数据提取模块包括：数据采集单元，线路选择单元，以及点提取单元。
数据采集单元，用于采集16线激光雷达的点云数据。
线路选择单元，用于选择一帧第0-N 线的点云数据，其中0＜N＜5。
点提取单元，用于记每一线的点为：
Figure 267583DEST_PATH_IMAGE091
，并提取点
Figure 151226DEST_PATH_IMAGE092
与
Figure 624932DEST_PATH_IMAGE093
。
本发明系统进一步实施时，所述的第一静止量计算模块包括投影坐标单元，深度 差计算单元，断层计算单元，以及静止量
Figure 797288DEST_PATH_IMAGE067
计算单元。
投影坐标单元，用于将提取点投影到XOY水平坐标系。
深度差计算单元，用于设定提取点之间的深度差，公式为：
Figure 749063DEST_PATH_IMAGE094
。
断层计算单元：用于计算断层的大小，公式为：
Figure 752791DEST_PATH_IMAGE095
。
静止量
Figure 397399DEST_PATH_IMAGE067
计算单元，用于得到激光雷达的静止量
Figure 650526DEST_PATH_IMAGE067
，公式为：
Figure 405993DEST_PATH_IMAGE096
。
本发明系统进一步实施时，所述的第二静止量计算模块包括：IMU窗口选取单元， 角速度计算单元，加速度计算单元，以及静止量
Figure 998648DEST_PATH_IMAGE090
计算单元。
IMU窗口选取单元，用于选取时间窗口等于激光雷达数据周期
Figure 814157DEST_PATH_IMAGE097
内的IMU数据。
角速度计算单元，用于计算IMU数据的角速度的分布平均值
Figure 961105DEST_PATH_IMAGE098
和方差
Figure 520262DEST_PATH_IMAGE099
，设 定根据角速度得到的机器人静止权重，公式为：
Figure 233003DEST_PATH_IMAGE100
。
加速度计算单元，用于计算IMU数据的加速度的分布平均值
Figure 953834DEST_PATH_IMAGE101
和方差
Figure 181553DEST_PATH_IMAGE102
，依 照方差超过一定阈值
Figure 278822DEST_PATH_IMAGE103
的情况，设定根据加速度得到的机器人静止权重，公式为：
当
Figure 111649DEST_PATH_IMAGE104
时，
Figure 3382DEST_PATH_IMAGE105
,
当
Figure 921659DEST_PATH_IMAGE106
时，
Figure 822619DEST_PATH_IMAGE107
。
静止量
Figure 244373DEST_PATH_IMAGE108
计算单元，用于计算激光雷达的静止量
Figure 572586DEST_PATH_IMAGE090
，公式为：
Figure 915843DEST_PATH_IMAGE109
。
三、地面机器人静止判断装置实施例：
如图3所示，本发明又提供一种地面机器人静止判断装置，包括：至少一个处理器，以及与所述的至少一个处理器通信连接的存储器。
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的地面机器人静止判断方法。
四、计算机可读存储介质实施例：
本发明再提供一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利上述的地面机器人静止判断方法步骤。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换本发明各实施例技术方案，并不使相应技术方案的本质脱离本方案的范围。

Claims (13)

1.一种地面机器人静止判断方法，其特征在于，包括如下步骤：
S1、在激光雷达采集的一帧点云数据中提取合适的点；
S2、根据提取点之间的深度差，计算激光雷达的静止量
Figure 557221DEST_PATH_IMAGE001
；
S3、根据静止量
Figure 331142DEST_PATH_IMAGE002
和IMU数据，得到区分机器人状态的阈值
Figure 993067DEST_PATH_IMAGE003
；
S4、根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 663083DEST_PATH_IMAGE004
；
S5、综合激光雷达与IMU数据，得到机器人静止概率
Figure 777669DEST_PATH_IMAGE005
。
2.根据权利要求1所述的一种地面机器人静止判断方法，其特征在于，所述S1包括如下步骤：
S01、采集16线激光雷达的点云数据；
S02、选择一帧第0-N 线的点云数据，其中0＜N＜5；
S03、记每一线的点为：
Figure 558544DEST_PATH_IMAGE006
，并提取点
Figure 74976DEST_PATH_IMAGE007
与
Figure 915893DEST_PATH_IMAGE008
。
3.根据权利要求2所述的一种地面机器人静止判断方法，其特征在于，所述S02中的N为3。
4.根据权利要求2所述的一种地面机器人静止判断方法，其特征在于，所述S2包括如下步骤：
S201、将提取点投影到XOY水平坐标系；
S202、设定提取点之间的深度差，公式为：
Figure 517775DEST_PATH_IMAGE009
；
S203、计算断层的大小，公式为：
Figure 102340DEST_PATH_IMAGE010
；
S204、得到激光雷达的静止量
Figure 676541DEST_PATH_IMAGE011
，公式为：
Figure 62261DEST_PATH_IMAGE012
。
5.根据权利要求1所述的一种地面机器人静止判断方法，其特征在于，所述S3包括如下步骤：
S301、根据静止量
Figure 885861DEST_PATH_IMAGE001
的数值，结合最新的单帧IMU数据的加速度和角速度信息，将当前机 器人的状态区分为八种情况，以及高、低角速度，高、低加速度，高、低静止量
Figure 274117DEST_PATH_IMAGE011
的情况；
S302、设计区分八种情况的不同阈值
Figure 499561DEST_PATH_IMAGE013
，以及区分高、低角速度，高、低加速度，高、低静 止量
Figure 682281DEST_PATH_IMAGE001
的不同固定阀值。
6.根据权利要求1所述的一种地面机器人静止判断方法，其特征在于，所述S4包括如下步骤：
S401、选取时间窗口等于激光雷达数据周期
Figure 393842DEST_PATH_IMAGE014
内的IMU数据；
S402、计算IMU数据的角速度的分布平均值
Figure 851368DEST_PATH_IMAGE015
和方差
Figure 931320DEST_PATH_IMAGE016
，设定根据角速度得到的 机器人静止权重，公式为：
Figure 550520DEST_PATH_IMAGE017
；
S403、计算IMU数据的加速度的分布平均值
Figure 348712DEST_PATH_IMAGE018
和方差
Figure 813191DEST_PATH_IMAGE019
，依照方差超过一定阈值
Figure 13228DEST_PATH_IMAGE020
的情况，设定根据加速度得到的机器人静止权重，公式为：
当
Figure 537750DEST_PATH_IMAGE021
时，
Figure 557659DEST_PATH_IMAGE022
,
当
Figure 91408DEST_PATH_IMAGE023
时，
Figure 145952DEST_PATH_IMAGE024
；
S404、计算激光雷达的静止量
Figure 841376DEST_PATH_IMAGE025
，公式为：
Figure 614160DEST_PATH_IMAGE026
。
7.根据权利要求1所述的一种地面机器人静止判断方法，其特征在于，所述S5包括如下步骤：
设计激光雷达与IMU数据融合情况下的机器人静止概率，公式为：
Figure 686021DEST_PATH_IMAGE027
。
8.一种地面机器人静止判断系统，其特征在于，包括：点云数据提取模块，第一静止量计算模块，状态阈值计算模块，第二静止量计算模块，以及静止概率计算模块：
点云数据提取模块，用于在激光雷达采集的一帧点云数据中提取合适的点；
第一静止量计算模块，用于根据提取点之间的深度差，计算激光雷达的静止量
Figure 595071DEST_PATH_IMAGE028
；
状态阈值计算模块，用于根据静止量
Figure 726975DEST_PATH_IMAGE029
和IMU数据，得到区分机器人状态的阈值
Figure 987055DEST_PATH_IMAGE030
；
第二静止量计算模块，用于根据IMU 数据窗口采样，计算激光雷达的静止量
Figure 862607DEST_PATH_IMAGE031
；
静止概率计算模块，用于综合激光雷达与IMU数据，得到机器人静止概率
Figure 891743DEST_PATH_IMAGE032
。
9.根据权利要求8所述的一种地面机器人静止判断系统，其特征在于，所述的点云数据提取模块包括：数据采集单元，线路选择单元，以及点提取单元；
数据采集单元，用于采集16线激光雷达的点云数据；
线路选择单元，用于选择一帧第0-N 线的点云数据，其中0＜N＜5；
点提取单元，用于记每一线的点为：
Figure 928969DEST_PATH_IMAGE033
，并提取点
Figure 676345DEST_PATH_IMAGE034
与
Figure 90009DEST_PATH_IMAGE035
。
10.根据权利要求8所述的一种地面机器人静止判断系统，其特征在于，所述的第一静 止量计算模块包括投影坐标单元，深度差计算单元，断层计算单元，以及静止量
Figure 973652DEST_PATH_IMAGE036
计算单 元；
投影坐标单元，用于将提取点投影到XOY水平坐标系；
深度差计算单元，用于设定提取点之间的深度差，公式为：
Figure 47959DEST_PATH_IMAGE037
；
断层计算单元：用于计算断层的大小，公式为：
Figure 17052DEST_PATH_IMAGE010
；
静止量
Figure 234407DEST_PATH_IMAGE038
计算单元，用于得到激光雷达的静止量
Figure 972556DEST_PATH_IMAGE039
，公式为：
Figure 617164DEST_PATH_IMAGE040
。
11.根据权利要求8所述的一种地面机器人静止判断系统，其特征在于，所述的第二静 止量计算模块包括：IMU窗口选取单元，角速度计算单元，加速度计算单元，以及静止量
Figure 73553DEST_PATH_IMAGE041
计 算单元；
IMU窗口选取单元，用于选取时间窗口等于激光雷达数据周期
Figure 563440DEST_PATH_IMAGE042
内的IMU数据；
角速度计算单元，用于计算IMU数据的角速度的分布平均值
Figure 421675DEST_PATH_IMAGE043
和方差
Figure 237184DEST_PATH_IMAGE044
，设定 根据角速度得到的机器人静止权重，公式为：
Figure 915290DEST_PATH_IMAGE045
；
加速度计算单元，用于计算IMU数据的加速度的分布平均值
Figure 474447DEST_PATH_IMAGE046
和方差
Figure 187189DEST_PATH_IMAGE047
，依照方 差超过一定阈值
Figure 235916DEST_PATH_IMAGE048
的情况，设定根据加速度得到的机器人静止权重，公式为：
当
Figure 666897DEST_PATH_IMAGE049
时，
Figure 764166DEST_PATH_IMAGE050
,
当
Figure 331414DEST_PATH_IMAGE051
时，
Figure 754305DEST_PATH_IMAGE052
；
静止量
Figure 407003DEST_PATH_IMAGE053
计算模块，用于计算激光雷达的静止量
Figure 307963DEST_PATH_IMAGE053
，公式为：
Figure 995296DEST_PATH_IMAGE054
。
12.一种地面机器人静止判断装置，其特征在于，包括：至少一个处理器，以及与所述的至少一个处理器通信连接的存储器；
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至7中任一所述的地面机器人静止判断方法。
13.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-7任一所述的地面机器人静止判断方法步骤。
CN202211572435.4A 2022-12-08 2022-12-08 一种地面机器人静止判断方法、系统、装置以及存储介质 Active CN115575923B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211572435.4A CN115575923B (zh) 2022-12-08 2022-12-08 一种地面机器人静止判断方法、系统、装置以及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211572435.4A CN115575923B (zh) 2022-12-08 2022-12-08 一种地面机器人静止判断方法、系统、装置以及存储介质

Publications (2)

Publication Number Publication Date
CN115575923A true CN115575923A (zh) 2023-01-06
CN115575923B CN115575923B (zh) 2023-03-28

Family

ID=84590740

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211572435.4A Active CN115575923B (zh) 2022-12-08 2022-12-08 一种地面机器人静止判断方法、系统、装置以及存储介质

Country Status (1)

Country Link
CN (1) CN115575923B (zh)

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014125808A1 (ja) * 2013-02-12 2014-08-21 旭化成エレクトロニクス株式会社 静止判定装置、姿勢角検出装置、携帯機器、静止判定方法およびプログラム
US20170186169A1 (en) * 2015-12-29 2017-06-29 Texas Instruments Incorporated Stationary-vehicle structure from motion
CN108664841A (zh) * 2017-03-27 2018-10-16 郑州宇通客车股份有限公司 一种基于激光点云的动静态目标物识别方法及装置
CN108983213A (zh) * 2018-09-07 2018-12-11 百度在线网络技术（北京）有限公司 障碍物静止状态的确定方法、装置、设备及存储介质
WO2019060524A1 (en) * 2017-09-22 2019-03-28 Waymo Llc DETECTION OF THE MOTION OF AN AUTONOMOUS VEHICLE USING RADAR TECHNOLOGY
CN110018489A (zh) * 2019-04-25 2019-07-16 上海蔚来汽车有限公司 基于激光雷达的目标追踪方法、装置及控制器和存储介质
CN110916577A (zh) * 2019-12-17 2020-03-27 小狗电器互联网科技（北京）股份有限公司 一种机器人静止状态的判断方法、装置及机器人
WO2020237495A1 (zh) * 2019-05-28 2020-12-03 深圳市大疆创新科技有限公司 地面可移动平台与其运动信息检测方法、系统
CN112835063A (zh) * 2021-01-08 2021-05-25 北京京东乾石科技有限公司 物体动静属性的确定方法、装置、设备及存储介质
WO2022037370A1 (zh) * 2020-08-18 2022-02-24 华为技术有限公司 一种运动估计方法及装置
CN114820697A (zh) * 2021-01-27 2022-07-29 英飞凌科技股份有限公司 使用静止状态模型的交互多模型跟踪算法
CN114910069A (zh) * 2022-06-01 2022-08-16 深圳市边界智控科技有限公司 一种无人机的融合定位初始化系统及方法
CN115131514A (zh) * 2022-07-12 2022-09-30 烟台艾睿光电科技有限公司 一种同时定位建图的方法、装置、系统及存储介质
CN115272408A (zh) * 2022-06-28 2022-11-01 广州小马智行科技有限公司 车辆静止检测方法、装置、计算机设备和存储介质

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014125808A1 (ja) * 2013-02-12 2014-08-21 旭化成エレクトロニクス株式会社 静止判定装置、姿勢角検出装置、携帯機器、静止判定方法およびプログラム
US20170186169A1 (en) * 2015-12-29 2017-06-29 Texas Instruments Incorporated Stationary-vehicle structure from motion
CN108664841A (zh) * 2017-03-27 2018-10-16 郑州宇通客车股份有限公司 一种基于激光点云的动静态目标物识别方法及装置
WO2019060524A1 (en) * 2017-09-22 2019-03-28 Waymo Llc DETECTION OF THE MOTION OF AN AUTONOMOUS VEHICLE USING RADAR TECHNOLOGY
CN108983213A (zh) * 2018-09-07 2018-12-11 百度在线网络技术（北京）有限公司 障碍物静止状态的确定方法、装置、设备及存储介质
CN110018489A (zh) * 2019-04-25 2019-07-16 上海蔚来汽车有限公司 基于激光雷达的目标追踪方法、装置及控制器和存储介质
WO2020237495A1 (zh) * 2019-05-28 2020-12-03 深圳市大疆创新科技有限公司 地面可移动平台与其运动信息检测方法、系统
CN110916577A (zh) * 2019-12-17 2020-03-27 小狗电器互联网科技（北京）股份有限公司 一种机器人静止状态的判断方法、装置及机器人
WO2022037370A1 (zh) * 2020-08-18 2022-02-24 华为技术有限公司 一种运动估计方法及装置
CN112835063A (zh) * 2021-01-08 2021-05-25 北京京东乾石科技有限公司 物体动静属性的确定方法、装置、设备及存储介质
CN114820697A (zh) * 2021-01-27 2022-07-29 英飞凌科技股份有限公司 使用静止状态模型的交互多模型跟踪算法
CN114910069A (zh) * 2022-06-01 2022-08-16 深圳市边界智控科技有限公司 一种无人机的融合定位初始化系统及方法
CN115272408A (zh) * 2022-06-28 2022-11-01 广州小马智行科技有限公司 车辆静止检测方法、装置、计算机设备和存储介质
CN115131514A (zh) * 2022-07-12 2022-09-30 烟台艾睿光电科技有限公司 一种同时定位建图的方法、装置、系统及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
YUNHUI LI等: "A Spatiotemporal Calibration Algorithm for IMU–LiDAR Navigation System Based on Similarity of Motion Trajectories", 《SENSORS》 *
王皓: "室内移动机器人定位与导航方法研究", 《中国优秀硕士学位论文全文数据库信息科技辑(月刊)》 *

Also Published As

Publication number Publication date
CN115575923B (zh) 2023-03-28

Similar Documents

Publication Publication Date Title
CN109506642B (zh) 一种机器人多相机视觉惯性实时定位方法及装置
US20190086923A1 (en) Method and apparatus for generating obstacle motion information for autonomous vehicle
EP3581890A2 (en) Method and device for positioning
CN112837352B (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
CN112034431B (zh) 一种雷达和rtk的外参标定方法及装置
CN111522034B (zh) 基于惯性导航的定位方法、设备及装置
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
JP2023036796A (ja) レーンラインと特徴点による測位方法及び測位装置、電子機器、記憶媒体、コンピュータプログラム、ならびに自動運転車両
CN114217665A (zh) 一种相机和激光雷达时间同步方法、装置及存储介质
CN115077541A (zh) 自动驾驶车辆的定位方法、装置及电子设备、存储介质
CN111623773A (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN115457152A (zh) 外参标定方法、装置、电子设备及存储介质
CN115575923B (zh) 一种地面机器人静止判断方法、系统、装置以及存储介质
CN113219439A (zh) 目标主干点云提取方法、装置、设备及计算机存储介质
CN116772858A (zh) 车辆定位方法、装置、定位设备及存储介质
CN115792985A (zh) 一种车辆定位方法、装置、电子设备、存储介质及车辆
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN111696159B (zh) 激光里程计的特征存储方法、电子设备和存储介质
CN114911235A (zh) 一种机器人防跌落方法、机器人及存储介质
CN117292118B (zh) 雷达引导光电跟踪坐标补偿方法、装置、电子设备及介质
CN113804194B (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN114593735B (zh) 一种位姿预测方法及装置
CN114612544B (zh) 图像处理方法、装置、设备和存储介质
CN115128655B (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
GR01 Patent grant
GR01 Patent grant