申请/专利权人：西安电子科技大学

申请日：2020-07-15

公开（公告）日：2024-07-05

公开（公告）号：CN111965662B

主分类号：G01S17/58

分类号：G01S17/58;G01S17/42;G01S17/89;G01C21/20

优先权：

专利状态码：有效-授权

法律状态：2024.07.05#授权;2020.12.08#实质审查的生效;2020.11.20#公开

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

主权项：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]时间内小车状态为匀变速直线运动；其中，δii＝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，将激光雷达的位姿信息转换为小车的位姿信息。

全文数据：

权利要求：

百度查询： 西安电子科技大学 一种室内小车测速和定位方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD