CN111965662B - 一种室内小车测速和定位方法 - Google Patents

一种室内小车测速和定位方法 Download PDF

Info

Publication number
CN111965662B
CN111965662B CN202010682404.9A CN202010682404A CN111965662B CN 111965662 B CN111965662 B CN 111965662B CN 202010682404 A CN202010682404 A CN 202010682404A CN 111965662 B CN111965662 B CN 111965662B
Authority
CN
China
Prior art keywords
trolley
information
laser radar
speed
point cloud
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
CN202010682404.9A
Other languages
English (en)
Other versions
CN111965662A (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.)
Xidian University
Original Assignee
Xidian 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 Xidian University filed Critical Xidian University
Priority to CN202010682404.9A priority Critical patent/CN111965662B/zh
Publication of CN111965662A publication Critical patent/CN111965662A/zh
Application granted granted Critical
Publication of CN111965662B publication Critical patent/CN111965662B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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/89—Lidar systems specially adapted for specific applications for mapping or imaging

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明属于室内定位技术领域，公开了一种室内小车测速和定位方法，1)获取小车静止时的基本环境信息以及相对位置信息；2)获取小车运动时的点云数据，并计算时间戳；3)根据获取的信息判断小车的运动状态；4)根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度等信息；5)根据姿态角度等信息，计算激光雷达在世界坐标系下的位姿信息；6)重复执行2)～5)直至运动停止；7)根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。本发明实现了获得小车的位姿信息，具有精确度高、所需计算量小、成本低廉的特点，可用于室内环境下，小车高精度的测速和定位。

Description

一种室内小车测速和定位方法
技术领域
本发明属于室内定位技术领域，尤其涉及一种室内小车测速和定位方法。
背景技术
随着人工智能浪潮的兴起，移动机器人已经开始走进千家万户，但是因为机器人在室内的移动需要精准的地图信息，现有的商用室外定位方案并不适用，所以机器人在室内未知环境下的定位问题得到广泛的重视。现阶段，机器人的室内定位已经发展得较为成熟，涌现了很多方案提供商和开源方案，但由于受到硬件设备精度、噪声以及本身存在的局限性，这些现有技术的方案的定位精度往往不够高，无法达到毫米级以下。
目前在室内未知环境下机器人定位的主流技术是激光雷达SLAM技术。现有的基于2D激光雷达的定位，大都利用相邻帧间的点云位姿解算变换，误差较大；另外，视觉SLAM受室内环境的影响较大，光线的微弱改变就会影响到建图以及定位精度；并且通过摄像头直接或间接获得的距离信息噪声大，难以与2D激光雷达的精度相比较，同时，由于单目以及双目摄像头获得深度信息需要大量计算，影响到SLAM的实时性。遇到要求更高的室内定位场景时，不能高精度的定位，从而造成不能大规模推广应用的问题。这些问题严重影响了室内移动小车进一步的发展。
专利申请号202010148743.9，专利名称为：一种多2D激光雷达融合建图与定位方法及***；该申请案通过旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据，处理扫描墙角，找出角点位姿，将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波，对雷达数据进行时间戳处理，进行匹配建图，每隔一段距离或一定时间记录一次雷达数据帧，存储激光点数据及当前位姿作为站点，每隔一段距离搜索一次附近站点，搜索最近站点进行一次激光数据匹配，根据匹配度p判断是否需要进行位姿修正。该方法存在的不足之处是利用相邻帧间的点云位姿解算变换，误差较大。
专利申请号201910554218.4，专利名称为：基于单线激光雷达与双目相机数据融合的机器人路径规划方法；该申请案以2D激光雷达的测量数据为基准，采用2D激光雷达的测量数据修正双目相机的测量数据；完成双目相机和激光雷达的数据融合后，对双目相机进行SLAM构建，使SLAM具有稠密点云，再将修正后的点云作为机器人路径规划的点云图，从而完成机器人的路径规划。该方法存在的不足之处是需要用到双目相机、视觉里程计等，且需要对数据进行融合，修正等操作，依赖设备多，计算量大，无法适用于精度要求更高的室内定位场景。
通过上述分析，现有技术存在的问题及缺陷为：现有的室内移动机器人的测速和定位方法，依赖的设备较多，且精度不高，成本高昂。
解决以上问题及缺陷的难度为：减少辅助设备会减少可以利用的信息，大大提高室内定位的难度和精度,但是增加辅助设备，又会提高成本；由于对点云数据经常分帧进行处理，忽略了一帧时间内小车的运动变化，这样势必会限制精度的提高，因此基于现有的算法，难以实现高精度的室内定位。
解决以上问题及缺陷的意义为：减少辅助设备将减少室内定位机器人的体积，便于使用者进行操作；高精度的室内定位有利于开展更多服务，适用于对定位精度要求更高的场合；降低成本会促进室内定位相关产品的普及。
发明内容
针对现有技术存在的问题，本发明提供了一种室内小车测速和定位方法。
本发明是这样实现的，一种室内小车测速和定位方法，所述室内小车测速和定位方法包括：
获取小车静止时的基本环境信息以及相对位置信息；
获取小车运动时的点云数据，并计算时间戳；
根据获取的信息判断小车的运动状态；
根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
根据姿态角度等信息，计算激光雷达在世界坐标系下的位姿信息；
重复执行获取小车运动时的点云数据，并计算时间戳；根据获取的信息判断小车的运动状态；根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度等信息；根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息直至运动停止；
根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
进一步，所述室内小车测速和定位方法还包括：
1)令小车靠墙静止，安装在小车上方的2D激光雷达旋转一周，得到点云数据，包括距离和角度，在激光雷达启动时刻，将激光束所指的方向作为x轴，激光雷达中心作为原点，建立世界坐标系，在世界坐标系下用直线拟合扫到每面墙的点云数据，得到墙的直线方程，进而建立环境地图；
2)建立小车坐标系ocxcyc和激光雷达坐标系olxlyl，根据步骤(1)得到的点云数据，假设距离最短时，对应的角度为α，进而得到雷达坐标系和小车坐标系的旋转角度差，位移差通过卷尺测量，建立激光雷达坐标系和小车坐标系的转换矩阵Hl，c；
3)获取小车运动时的点云数据(xi，yi)，根据激光雷达旋转角速度，得到每个点云数据(xi，yi)对应的时间戳ti；
4)根据步骤(3)获得的点云数据，判断扫到每一面墙的起始数据点，以及转弯起始点和转弯结束点，具体如下：
4a)：将步骤(3)获得的点云数据(xi，yi)，按<1>式计算两两之间倾斜角度差的绝对值θ2，i，
当θ2，i＞ε1∩θ2，i-1＜ε1时，其中ε1是预设的与两面墙夹角有关的阈值，则将i作为扫到的另一面墙的起始数据点；
4b)：在步骤4a)获得的倾斜角度差的绝对值基础上，计算θ3，i＝θ2，i-θ2，i-1，若以下条件同时成立：
A).θ3，i＞ε4∩θ3，i＜ε5；
B).θ3，i-1＜ε4；
其中ε4、ε5表示预设的与小车转弯有关的阈值，则将i作为转弯起始点。若以下条件同时成立：
A).θ3，i＞-ε5∩θ3，i＜-ε4；
B).θ3，i-1＞-ε4；
则将i作为转弯结束点，对于转弯起始点之后和转弯结束点之前的点，标记小车状态为匀速转弯；
(5)根据步骤(3)获取的点云数据分段判断其他运动状态，包括静止，匀速直线运动，匀加速直线运动，匀减速直线运动，具体步骤如下：
5a)：根据4a)获得的每一面墙的起始数据点，对于匀速转弯运动状态之外的点，在依次判断的两面墙上，各取连续的四个点{X1，X2，X3，X4}、{X5，X6，X7，X8}，代入匀变速计算公式，得出一组状态量，即速度，加速度，和运动方向(v1，a1，θ1)，重复上述过程，得出另一组状态量(v2，a2，θ2)，假设该运算过程涉及[tp，tq]时间内的点云数据，对比两组状态量，得到运动状态，具体规则如下：
A)若v1＜δ1∩v2＜δ1成立，则标记[tp，tq]时间内小车状态为静止；
B)若|v1-v2|＜δ2∩a1＜δ3∩a2＜δ3成立，则标记[tp，tq]时间内小车状态为匀速直线运动；
C)若|a1-a2|＜δ4成立，则标记[tp，tq]时间内小车状态为匀变速直线运动；
其中，δi(i＝1，2，3，4)是预设的接近于0的阈值。
5b)：重复步骤5a)，依次得到两个运动状态R和Q，假设该运算过程涉及[tp，tw]时间内的点云数据，若R＝Q，则运动状态不变，若R≠Q，则根据tp时刻激光雷达的位置，运动状态，依次算出下一时刻雷达应输出的点云数据，并与实际输出的点云作比较，如果相差较大，则状态发生改变，进而得到精确的状态改变点tv。对于[tp，tv]之间的点，标记状态为R；
(6)根据步骤(4)和步骤(5)标记状态的不同，将点云数据进行不同处理，具体如下：
当标记状态为匀速直线运动时，将对应时间内的点云数据代入匀速计算算法；
当标记状态为匀变速直线运动时，将对应时间内的点云数据代入匀变速计算算法；
当标记状态为匀速转弯运动时，将对应时间内的点云数据代入转弯计算算法；
(7)根据步骤(6)获得的姿态角度等信息，获取激光雷达在世界坐标系下的位姿信息，具体步骤如下：
7a)：根据步骤(6)得到的每段时间内激光雷达的姿态角度信息、速度信息，得到对应每个时刻ti的姿态角度信息、速度信息；
7b)：根据步骤7a)获得的每个时刻激光雷达的姿态角度信息、速度信息，结合航位推算法，计算激光雷达在世界坐标系下，每个时刻ti的位姿信息，包括激光雷达位置、小车运动方向、小车速度；
(8)让小车继续运动获取点云数据，重复执行步骤(3)-步骤(7)，若步骤5b)中运动状态不一致，则从状态改变点tv开始取点计算；
(9)根据步骤1获得的转换矩阵Hl，c，将激光雷达的位姿信息转换为小车的位姿信息。
进一步，步骤(6)中所述匀速计算算法，具体如下：
当判断状态为匀速直线运动时，将{X1，X2，X3}、{X5，X6，X7}分别依次代入公式<2>，得到两组直线的系数{a1，b1，a1ΔX+b1Δy}，{a2，b2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设激光雷达相邻两个采样点的时间间隔为其中，ωji是激光雷达旋转角速度(ωji的单位是°/s)，(Δx，Δy)是Δt时间内小车匀速运动的距离：
将公式<3>代入{a1Δx+b1Δy、a2Δx+b2Δy}，得到速度v和运动方向θ。
进一步，步骤5a)、步骤(6)中所述匀变速计算算法，具体如下：
当判断状态为匀变速时，将{X1，X2，X3，X4}、{X5，X6，X7，X8}分别依次代入公式<4>，得到两组直线的系数{a1，b1，a1xchu，1+b1ychu，1，a1ΔX+b1Δy}，{a2，b2，a1xchu，2+b1ychu，2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设每次取一面墙上连续四点时，对应时刻t1、t2、t3、t4，时间间隔是[t1，t2]内小车的位移距离，(Δx、Δy)是匀加速直线运动时，相等时间间隔Δt内前后位移的变化量，根据匀加速的位移公式：
其中，v0是t1时刻的小车初速度，g是加速度，θ是运动方向，v0，g，θ均是未知参数，将公式<5>代入{a1，b1，a1xchu，1+b1ychu，1，a1ΔX+b1Δy}，{a2，b2，a1xchu，2+b1ychu，2，a2Δx+b2Δy}，得到v0，g，θ。
进一步，步骤(6)中所述转弯计算算法，具体如下：
当判断状态为转弯时，根据步骤7b)得到转弯前一时刻，激光雷达在世界坐标系的位置A(xa，ya)，小车运动方向θ，记转弯过程中激光雷达产生的第一个点云数据为C(x′，y′)，转换到世界坐标系下为
其中，s是小车转弯角速度，r是转弯半径，
转弯过程中激光雷达产生的第二个点云数据为D(x″，y″)，转换到世界坐标系下为
设通过步骤(1)获取的世界系中C点和D点所在墙的方程为
cX+dY+1＝0<8>
分别将公式<6>和公式<7>代入公式<8>，就得到含有两个未知参数的非线性方程组，进而得到s和r。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
获取小车静止时的基本环境信息以及相对位置信息；
获取小车运动时的点云数据，并计算时间戳；
根据获取的信息判断小车的运动状态；
根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息；
重复执行获取小车运动时的点云数据，并计算时间戳；根据获取的信息判断小车的运动状态；根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息直至运动停止；
根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
获取小车静止时的基本环境信息以及相对位置信息；
获取小车运动时的点云数据，并计算时间戳；
根据获取的信息判断小车的运动状态；
根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息；
重复执行获取小车运动时的点云数据，并计算时间戳；根据获取的信息判断小车的运动状态；根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息直至运动停止；
根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
本发明的另一目的在于提供一种运行所述室内小车测速和定位方法的室内小车测速和***，所述室内小车测速和定位***包括：
信息获取模块，用于获取小车静止时的基本环境信息以及相对位置信息；
时间戳计算模块，用于获取小车运动时的点云数据，并计算时间戳；
运动状态判断模块，用于根据获取的信息判断小车的运动状态；
激光雷达信息获取模块，用于根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
第一位姿信息计算模块，用于根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息；
第二位姿信息计算模块，用于根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
本发明的另一目的在于提供一种室内定位***，所述室内定位***安装有所述的室内小车测速和定位***。
本发明的另一目的在于提供一种机器人，所述机器人安装有所述的室内小车测速和定位***。
结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明的小车处于不同的运动状态时，必然造成激光雷达获取的点云数据的细微变化，根据这些细微变化和运动状态建立方程组，能够得到小车的速度、加速度等信息，实现定位。
本发明根据运动状态的不同将其分为静止状态、匀速直线运动状态、匀加速直线运动状态、匀减速直线运动状态、转弯状态，既符合实际小车的运动情况，又更加便于获得更高的测速和定位精度；只需2D激光雷达即可实现精确测量小车的速度，并在此基础上实现高精度的定位。成本低廉，泛用性强；方法简单高效，并可实现实时性的测速和定位。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的室内小车测速和定位方法流程图。
图2是本发明实施例提供的室内小车测速和定位***的结构示意图；
图2中：1、信息获取模块；2、时间戳计算模块；3、运动状态判断模块；4、激光雷达信息获取模块；5、第一位姿信息计算模块；6、第二位姿信息计算模块。
图3是本发明实施例提供的室内小车测速和定位方法实现流程图。
图4是本发明实施例提供的初始时刻让小车靠墙静止的示意图；
图5是本发明实施例提供的转弯状态的示意图。
图6是本发明实施例提供的根据获得的点云数据，定位和建图效果图。
图7是本发明实施例提供的对匀速直线、匀速转弯、匀加速直线运动部分各取20个定位点测量定位误差，结果图。
图8是本发明实施例提供的对于匀速直线部分，取10个点测量速度误差和运动方向误差，结果图。
图9是本发明实施例提供的对于匀加速直线运动部分，取10个点测量速度误差、加速度误差和运动方向误差，结果图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种室内小车测速和定位方法，下面结合附图对本发明作详细的描述。
如图1所示，本发明提供的室内小车测速和定位方法包括以下步骤：
S101：获取小车静止时的基本环境信息以及相对位置信息；
S102：获取小车运动时的点云数据，并计算时间戳；
S103：根据S102获取的信息判断小车的运动状态；
S104：根据S103判断的运动状态，将S102获得的数据进行不同处理，得到激光雷达的姿态角度等信息；
S105：根据姿态角度等信息，计算激光雷达在世界坐标系下的位姿信息；
S106：重复执行S102～S105直至运动停止；
S107：根据S101获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
本发明提供的室内小车测速和定位方法业内的普通技术人员还可以采用其他的步骤实施，图1的本发明提供的室内小车测速和定位方法仅仅是一个具体实施例而已。
如图2所示，本发明提供的室内小车测速和定位***包括：
信息获取模块1，用于获取小车静止时的基本环境信息以及相对位置信息；
时间戳计算模块2，用于获取小车运动时的点云数据，并计算时间戳；
运动状态判断模块3，用于根据获取的信息判断小车的运动状态；
激光雷达信息获取模块4，用于根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度等信息；
第一位姿信息计算模块5，用于根据姿态角度等信息，计算激光雷达在世界坐标系下的位姿信息；
第二位姿信息计算模块6，用于根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
下面结合附图对本发明的技术方案作进一步的描述。
如图3所示，本发明提供的室内小车测速和定位方法包括以下步骤：
步骤一，将激光雷达固定在小车上，并获取小车静止时的点云数据。
将单线激光雷达固定在小车上，小车在空旷的室内运动，先让小车靠任意一面墙静止，如图4，获取激光雷达旋转一周的点云数据。
空旷是指其他物体不对雷达成像造成干扰；固定采用螺丝固定和焊接固定，牢固程度要不受平台移动影响的方式，且使经过固定的激光雷达和小车中心之间的距离和位姿这些相对位置信息不再发生改变；单线激光雷达输出的数据没有少点、多点的现象，数据准确度在毫米级以下。
步骤二，对小车静止时获取的数据进行处理，建立环境地图，获取每面墙的直线方程，以及激光雷达和小车的相对位置信息。
在激光雷达启动时刻，将激光束所指的方向作为x轴，激光雷达中心作为原点，建立世界坐标系，在世界坐标系下用直线拟合扫到每面墙的点云数据，得到每面墙的直线方程，进而建立环境地图；
建立小车坐标系ocxcyc和激光雷达坐标系olxlyl，小车坐标系以小车中心作为原点，小车正前方作为y轴，雷达坐标系时时以雷达中心作为原点，激光雷达启动时激光束所指的方向作为x轴，根据小车静止时得到的点云数据，假设距离最短时，对应的角度为α，此时激光束垂直于墙壁，根据这一关系，得到雷达坐标系和小车坐标系的旋转角度差，位移差通过卷尺测量，进而建立激光雷达坐标系和小车坐标系的转换矩阵Hl，c；
步骤三，让小车运动，小车的运动状态可在匀速直线运动、匀变速直线运动、匀速转弯、静止之间变换，获取小车运动时的点云数据(xi，yi)。
步骤四，根据小车运动时获得的点云数据，判断扫到每一面墙的起始数据点，以及转弯起始点和转弯结束点，具体如下：
4.1)：根据激光雷达旋转角速度，得到每个点云数据(xi，yi)对应的时间戳ti；
4.2)：将小车运动时获得的点云数据(xi，yi)，按<1>式计算两两之间倾斜角度差的绝对值θ2，i，
当θ2，i＞ε1∩θ2，i-1＜ε1时，其中ε1是预设的与两面墙夹角有关的阈值，则将i作为扫到的另一面墙的起始数据点；
4.3)：在4.2)获得的倾斜角度差的绝对值基础上，计算θ3，i＝θ2，i-θ2，i-1，若以下条件同时成立：
A).θ3，i＞ε4∩θ3，i＜ε5；
B).θ3，i-1＜ε4；
其中ε4、ε5均是预设的与小车转弯有关的阈值，则将i作为转弯起始点。若以下条件同时成立：
A).θ3，i＞-ε5∩θ3，i＜-ε4；
B).θ3，i-1＞-ε4；
则将i作为转弯结束点，对于转弯起始点之后和转弯结束点之前的点，标记小车状态为匀速转弯运动；
步骤五，分段判断小车匀速转弯外的其他运动状态，包括静止，匀速直线运动，匀加速直线运动，匀减速直线运动，具体步骤如下：
5.1)：根据4.1)获得的每一面墙的起始数据点，对于匀速转弯运动状态之外的点，在依次判断的两面墙上，各取连续的四个点{X1，X2，X3，X4}、{X5，X6，X7，X8}，代入匀变速计算公式，得出一组状态量，即速度，加速度，和运动方向(v1，a1，θ1)，重复上述过程，得出另一组状态量(v2，a2，θ2)，假设该运算过程涉及[tp，tq]时间内的点云数据，对比两组状态量，得到运动状态，具体规则如下：
A)若v1＜δ1∩v2＜δ1成立，则标记[tp，tq]时间内小车状态为静止；
B)若|v1-v2|＜δ2∩a1＜δ3∩a2＜δ3成立，则标记[tp，tq]时间内小车状态为匀速直线运动；
C)若|a1-a2|＜δ4成立，则标记[tp，tq]时间内小车状态为匀变速直线运动；
其中，δi(i＝1，2，3，4)是预设的接近于0的阈值。
5.2)：重复步骤5.1)，依次得到两个运动状态R和Q，假设该运算过程涉及[tp，tw]时间内的点云数据，若R＝Q，则运动状态不变，若R≠Q，则根据tp时刻激光雷达的位置，运动状态，依次算出下一时刻雷达应输出的点云数据，并与实际输出的点云作比较，如果相差较大，则状态发生改变，进而得到精确的状态改变点tv。对于[tp，tv]之间的点，标记状态为R；
步骤六，根据标记状态的不同，将点云数据进行不同处理，具体如下：
当标记状态为匀速直线运动时，将对应时间内的点云数据代入匀速计算算法，即：
将{X1，X2，X3}、{X5，X6，X7}分别依次代入公式<2>，得到两组直线的系数{a1，b1，a1Δx+b1Δy}，{a2，b2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设激光雷达旋转β°的时间间隔为 其中，ωji是激光雷达旋转角速度(ωji的单位是°/s)，(Δx，Δy)是Δt时间内小车匀速运动的距离：
将公式<3>代入{a1Δx+b1Δy、a2Δx+b2Δy}，得到速度v和运动方向θ。
当标记状态为匀变速直线运动时，将对应时间内的点云数据代入匀变速计算算法，即：
将{X1，X2，X3，x4}、{X5，X6，X7，X8}分别依次代入公式<4>，得到两组直线的系数：
{a1，b1，a1Xchu，1+b1ychu，1，a1Δx+b1Δy}，{a2，b2，a1Xchu，2+b1ychu，2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设取一面墙上连续四点时，对应时刻t1、t2、t3、t4，时间间隔(xchu，ychu)是[t1，t2]时间内小车的位移距离，(Δx、Δy)是匀加速直线运动时，相等时间间隔Δt前后位移的变化量，根据匀加速的位移公式：
其中，v0是t1时刻的小车初速度，g是加速度，θ是运动方向，v0，g，θ均是未知参数，将公式<5>代入{a1，b1，a1Xchu，1+b1ychu，1，a1Δx+b1Δy}，{a2，b2，a1xchu，2+b1ychu，2，a2Δx+b2Δy}，得到v0，g，θ。
当标记状态为匀速转弯运动时，将对应时间内的点云数据代入转弯计算算法，即：
当判断状态为转弯时，根据7.2)得到转弯前一时刻，激光雷达在世界坐标系的位置A(xa，ya)，小车运动方向θ，记转弯过程中激光雷达产生的第一个点云数据为C(x′，y′)，转换到世界坐标系下为：
其中，s是小车转弯角速度，r是转弯半径，转弯过程中激光雷达产生的第二个点云数据为D(x″，y″)，转换到世界坐标系下为：
设通过步骤二获取的世界系中C点和D点所在墙的方程为：
cX+dY+1＝0<8>
分别将公式<6>和公式<7>代入公式<8>，就得到含有两个未知参数的非线性方程组，进而得到s和r。
步骤七，获取激光雷达在世界坐标系下的位姿信息，具体步骤如下：
7.1)：根据步骤六得到的每段时间内激光雷达的姿态角度信息、速度信息，得到对应每个时刻ti的姿态角度信息、速度信息；
小车匀速直线运动时，每个时刻的γ、θ均不变，加速度，g＝0m/s^2；
小车匀变速直线运动时，每个时刻的g、θ均不变，而速度可根据匀变速的速度公式v＝v0+gt得到，其中，t是该运动过程中每个时刻到t1时刻的时间差；
小车匀速转弯运动时，每个时刻的速度是运动方向可根据θ＝θ+st递推得到，其中，t是该运动过程中每个时刻到转弯前一时刻的时间差。
7.2)：根据7.1)获得的每个时刻激光雷达的姿态角度信息、速度信息，结合航位推算法，计算激光雷达在世界坐标系下，每个时刻ti的位姿信息，包括激光雷达位置、小车运动方向、小车速度；
步骤八，让小车继续运动获取点云数据，重复执行步骤三-步骤七，若5.2)中运动状态不一致，则从状态改变点tv开始取点计算；
步骤九，根据步骤二获得的转换矩阵Hl，c，将激光雷达的位姿信息转换为小车的位姿信息。
下面结合实验对本发明的技术效果作详细的描述。
本发明依照需求设计了一个室内定位实验，令小车在12m*10m的室内运动，雷达转速为3600°/s，小车坐标系和激光雷达坐标系的旋转角度差为0°，位移差为(0，0)米。首先令小车做匀速直线运动，从室内长度的1/2，室内宽度的1/4处开始出发，小车速度为3m/s，运动方向与水平向右方向的夹角为150°；接着速度保持不变，作匀速转弯运动，小车转弯角速度为360°/s；最后作匀加速直线运动，加速度为1m/s^2，。
根据获得的点云数据，定位和建图效果如下，外周的长方形是建立的初始地图，中间的点是定位结果，激光雷达每输出一个数据，图5中都有一个预测的小车位置与之对应。
对匀速直线、匀速转弯、匀加速直线运动部分各取20个定位点测量定位误差，结果图6如下，从上图可见，整体定位误差不超过1毫米。
对于匀速直线部分，取10个点测量速度误差和运动方向误差，结果图7如下，从上图可见，速度误差和运动方向误差都非常小，与理论上的数值基本一致。
对于匀加速直线运动部分，取10个点测量速度误差、加速度误差和运动方向误差，结果图8如下，从上图可见，受累积误差的影响，匀加速直线运动部分的误差比匀速直线运动部分要大，但误差整体保持在低水平。
对于匀速转弯运动，计算得到的小车转弯角速度误差为6.132*10-13°/s,转弯半径误差为9.85906816*10-13米，因此，根据小车转弯角速度和转弯半径计算得到的速度误差和运动方向误差整体保持在低水平。
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行***，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种室内小车测速和定位方法，其特征在于，所述室内小车测速和定位方法包括：
获取小车静止时的基本环境信息以及相对位置信息；
获取小车运动时的点云数据，并计算时间戳；
根据获取的信息判断小车的运动状态；
根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
根据姿态角度等信息，计算激光雷达在世界坐标系下的位姿信息；
重复执行获取小车运动时的点云数据，并计算时间戳；根据获取的信息判断小车的运动状态；根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度等信息；根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息直至运动停止；
根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息；
所述室内小车测速和定位方法还包括：
第一步，小车靠墙静止，安装在小车上方的2D激光雷达旋转一周，得到点云数据，包括距离和角度，在激光雷达启动时刻，将激光束所指的方向作为x轴，激光雷达中心作为原点，建立世界坐标系，在世界坐标系下用直线拟合扫到每面墙的点云数据，得到墙的直线方程，建立环境地图；
第二步，建立小车坐标系ocxcyc和激光雷达坐标系olxlyl，根据第一步得到的点云数据，假设距离最短时，对应的角度为α，进而得到雷达坐标系和小车坐标系的旋转角度差，位移差通过卷尺测量，建立激光雷达坐标系和小车坐标系的转换矩阵Hl，c；
第三步，获取小车运动时的点云数据(xi，yi)，根据激光雷达旋转角速度，得到每个点云数据(xi，yi)对应的时间戳ti；
第四步，根据第三步获得的点云数据，判断扫到每一面墙的起始数据点，以及转弯起始点和转弯结束点，具体如下：
1)：将第三步获得的点云数据(xi，yi)，计算两两之间倾斜角度差的绝对值θ2，i：
当θ2，i＞ε1∩θ2，i-1＜ε1时，其中ε1是预设的与两面墙夹角有关的阈值，则将i作为扫到的另一面墙的起始数据点；
2)：在1)获得的倾斜角度差的绝对值基础上，计算θ3，i＝θ2，i-θ2，i-1，若以下条件同时成立：
A)θ3，i＞ε4∩θ3，i＜ε5；
B)θ3，i-1＜ε4；
其中ε4、ε5表示预设的与小车转弯有关的阈值，则将i作为转弯起始点，若以下条件同时成立：
A)θ3，i＞-ε5∩θ3，i＜-ε4；
B)θ3，I-1＞-ε4；
则将i作为转弯结束点，对于转弯起始点之后和转弯结束点之前的点，标记小车状态为匀速转弯；
第五步，根据第三步获取的点云数据分段判断其他运动状态，包括静止，匀速直线运动，匀加速直线运动，匀减速直线运动，具体步骤如下：
1)：根据获得的每一面墙的起始数据点，对于匀速转弯运动状态之外的点，在依次判断的两面墙上，各取连续的四个点{X1，X2，X3，X4}、{X5，X6，X7，X8}，代入匀变速计算公式，得出一组状态量，即速度，加速度，和运动方向(v1，a1，θ1)，重复上述过程，得出另一组状态量(v2，a2，θ2)，假设该运算过程涉及[tp，tq]时间内的点云数据，对比两组状态量，得到运动状态，具体规则如下：
A)若v1＜δ1∩v2＜δ1成立，则标记[tp，tq]时间内小车状态为静止；
B)若|v1-v2|＜δ2∩a1＜δ3∩a2＜δ3成立，则标记[tp，tq]时间内小车状态为匀速直线运动；
C)若|a1-a2|＜δ4成立，则标记[tp，tq]时间内小车状态为匀变速直线运动；其中，δi(i＝1，2，3，4)是预设的接近于0的阈值；
2)：重复1)，依次得到两个运动状态R和Q，该运算过程涉及[tp，tw]时间内的点云数据，若R＝Q，则运动状态不变，若R≠Q，则根据tp时刻激光雷达的位置，运动状态，依次算出下一时刻雷达应输出的点云数据，并与实际输出的点云作比较，如果相差较大，则状态发生改变，进而得到精确的状态改变点tv，对于[tp,tv]之间的点，标记状态为R；
第六步，根据第四步和第五步标记状态的不同，将点云数据进行不同处理，具体如下：
当标记状态为匀速直线运动时，将对应时间内的点云数据代入匀速计算算法；
当标记状态为匀变速直线运动时，将对应时间内的点云数据代入匀变速计算算法；
当标记状态为匀速转弯运动时，将对应时间内的点云数据代入转弯计算算法；
第七步，根据第六步获得的姿态角度等信息，获取激光雷达在世界坐标系下的位姿信息，具体步骤如下：
1)：根据第六步得到的每段时间内激光雷达的姿态角度信息、速度信息，得到对应每个时刻ti的姿态角度信息、速度信息；
2)：根据1)获得的每个时刻激光雷达的姿态角度信息、速度信息，结合航位推算法，计算激光雷达在世界坐标系下，每个时刻ti的位姿信息，包括激光雷达位置、小车运动方向、小车速度；
第八步，让小车继续运动获取点云数据，重复执行第三步-第七步，若步骤
5b)中运动状态不一致，则从状态改变点tv开始取点计算；
第九步，根据第一步获得的转换矩阵Hl，c，将激光雷达的位姿信息转换为小车的位姿信息。
2.如权利要求1所述的室内小车测速和定位方法，其特征在于，所述第六步匀速计算算法，具体如下：当判断状态为匀速直线运动时，将{X1，X2，X3}、{X5，X6，X7}分别依次代入公式，得到两组直线的系数{a1，b1，α1Δx+b1Δy}，{a2，b2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设激光雷达相邻两个采样点的时间间隔为其中，ωji是激光雷达旋转角速度，ωji的单位是°/s，(Δx，Δy)是Δt时间内小车匀速运动的距离：
将公式代入{a1Δx+b1Δy、a2Δx+b2Δy}，得到速度v和运动方向θ。
3.如权利要求1所述的室内小车测速和定位方法，其特征在于，所述第五步和第六步的匀变速计算算法，具体如下：
当判断状态为匀变速时，将{X1，X2，X3，X4}、{X5，X6，X7，X8}分别依次代入公式，得到两组直线的系数{α1，b1，a1xchu，1+b1ychu，1，a1Δx+b1Δy}，{a2，b2，a1xchu，2+b1ychu，2，a2Δx+b2Δy}：
其中，a和b是表示一面墙的直线系数，设每次取一面墙上连续四点时，对应时刻t1、t2、t3、t4，时间间隔是[t1，t2]内小车的位移距离，(Δx、Δy)是匀加速直线运动时，相等时间间隔Δt内前后位移的变化量，根据匀加速的位移公式：
其中，v0是t1时刻的小车初速度，g是加速度，θ是运动方向，v0，g，θ均是未知参数，将公式代入{a1，b1，a1xchu，1+b1ychu，1，a1Δx+b1Δy}，{a2，b2，a1xchu，2+b1ychu，2，a2Δx+b2Δy}，得到v0，g，θ。
4.如权利要求1所述的室内小车测速和定位方法，其特征在于，所述第六步中转弯计算算法，具体如下：
当判断状态为转弯时，根据得到转弯前一时刻，激光雷达在世界坐标系的位置A(xa，ya)，小车运动方向θ，记转弯过程中激光雷达产生的第一个点云数据为C(x′，y′)，转换到世界坐标系下为：
其中，s是小车转弯角速度，r是转弯半径，转弯过程中激光雷达产生的第二个点云数据为D(x″，y″)，转换到世界坐标系下为：
设通过获取的世界系中C点和D点所在墙的方程为：
cX+dY+1＝0；
分别将公式和公式代入公式cX+dY+1＝0，就得到含有两个未知参数的非线性方程组，进而得到s和r。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如如权利要求1～4任意一项所述室内小车测速和定位方法的步骤。
6.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1～4任意一项所述室内小车测速和定位方法的步骤。
7.一种运行权利要求1～4任意一项所述室内小车测速和定位方法的室内小车测速和***，其特征在于，所述室内小车测速和定位***包括：
信息获取模块，用于获取小车静止时的基本环境信息以及相对位置信息；
时间戳计算模块，用于获取小车运动时的点云数据，并计算时间戳；
运动状态判断模块，用于根据获取的信息判断小车的运动状态；
激光雷达信息获取模块，用于根据判断的运动状态，将获得的数据进行不同处理，得到激光雷达的姿态角度信息；
第一位姿信息计算模块，用于根据姿态角度信息，计算激光雷达在世界坐标系下的位姿信息；
第二位姿信息计算模块，用于根据获得的基本信息，将激光雷达的位姿信息转换为小车的位姿信息。
8.一种室内定位***，其特征在于，所述室内定位***安装有权利要求7所述的室内小车测速和定位***。
9.一种机器人，其特征在于，所述机器人安装有权利要求7所述的室内小车测速和定位***。
CN202010682404.9A 2020-07-15 2020-07-15 一种室内小车测速和定位方法 Active CN111965662B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010682404.9A CN111965662B (zh) 2020-07-15 2020-07-15 一种室内小车测速和定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010682404.9A CN111965662B (zh) 2020-07-15 2020-07-15 一种室内小车测速和定位方法

Publications (2)

Publication Number Publication Date
CN111965662A CN111965662A (zh) 2020-11-20
CN111965662B true CN111965662B (zh) 2024-07-05

Family

ID=73360681

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010682404.9A Active CN111965662B (zh) 2020-07-15 2020-07-15 一种室内小车测速和定位方法

Country Status (1)

Country Link
CN (1) CN111965662B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112731354B (zh) * 2020-12-24 2024-04-05 芜湖哈特机器人产业技术研究院有限公司 Agv上激光雷达位姿的自标定方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110554396A (zh) * 2019-10-21 2019-12-10 深圳市元征科技股份有限公司 一种室内场景下激光雷达建图方法、装置、设备及介质
CN111240331A (zh) * 2020-01-17 2020-06-05 仲恺农业工程学院 基于激光雷达和里程计slam的智能小车定位导航方法及***

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105759829A (zh) * 2016-04-12 2016-07-13 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及***
CN109709801B (zh) * 2018-12-11 2024-02-02 智灵飞（北京）科技有限公司 一种基于激光雷达的室内无人机定位***及方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110554396A (zh) * 2019-10-21 2019-12-10 深圳市元征科技股份有限公司 一种室内场景下激光雷达建图方法、装置、设备及介质
CN111240331A (zh) * 2020-01-17 2020-06-05 仲恺农业工程学院 基于激光雷达和里程计slam的智能小车定位导航方法及***

Also Published As

Publication number Publication date
CN111965662A (zh) 2020-11-20

Similar Documents

Publication Publication Date Title
CN111123280B (zh) 激光雷达的定位方法、装置、***、电子设备及存储介质
CN111366908B (zh) 一种激光雷达转台及其测量装置和测量方法
CN109269422B (zh) 一种点激光位移传感器测量误差校对的实验方法及装置
Fang et al. A Real‐Time 3D Perception and Reconstruction System Based on a 2D Laser Scanner
CN114745529B (zh) 一种投影仪单tof梯形校正的方法和投影仪
CN111443337B (zh) 一种基于手眼标定的雷达-imu标定方法
CN111965662B (zh) 一种室内小车测速和定位方法
CN111982091A (zh) 一种基于同步imu的激光点云畸变纠正方法
Tang et al. LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots
CN111486867B (zh) 一种视觉和惯性混合跟踪组件安装参数的标定装置及方法
CN116429084A (zh) 一种动态环境3d同步定位与建图方法
Wang et al. Automatic reading system for analog instruments based on computer vision and inspection robot for power plant
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
CN115179323A (zh) 基于远心视觉约束的机器末位姿测量装置及精度提升方法
Peng et al. A novel algorithm based on nonlinear optimization for parameters calibration of wheeled robot mobile chasses
CN111521996A (zh) 一种激光雷达的安装标定方法
CN109443326B (zh) 一种工程机械定位方法及***
WO2022237375A1 (zh) 用于定位装置和里程计的标定方法、程序产品和标定装置
Ye et al. Robust and efficient vehicles motion estimation with low-cost multi-camera and odometer-gyroscope
Kim et al. Rover mast calibration, exact camera pointing, and camera handoff for visual target tracking
CN114299477A (zh) 一种车辆视觉定位方法、***、设备及可读存储介质
CN113377104A (zh) 基于差速模型的机器人位置控制方法、装置
JPS6132690B2 (zh)
CN112222798B (zh) 一种大型部件自动安装方法及装置
Wang et al. Distance overestimation error correction method (DOEC) of time of flight camera based on pinhole model

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