CN111060131B

CN111060131B - 一种基于激光雷达的机器人精确姿态矫正方法及装置

Info

Publication number: CN111060131B
Application number: CN201911180747.9A
Authority: CN
Inventors: 彭倍; 王龙; 曾博才; 包杨川; 何明
Original assignee: Sichuan Artigent Robotics Equipment Co ltd
Current assignee: Sichuan Artigent Robotics Equipment Co ltd
Priority date: 2019-11-27
Filing date: 2019-11-27
Publication date: 2021-08-10
Anticipated expiration: 2039-11-27
Also published as: CN111060131A

Abstract

本发明涉及一种基于激光雷达的机器人精确姿态矫正方法及装置，其中方法包括：采集机器人周围环境的点云信息数据，所述周围环境包括平整墙面；将所述点云信息数据转换成二维笛卡尔坐标数据；从所述二维笛卡尔坐标数据提取参考点云的坐标数据；对所述参考点云的坐标数据进行线性拟合，得到拟合直线段；根据所述拟合直线段的斜率得到机器人当前姿态的角度值；将所述机器人当前姿态的角度值与标准设定值进行比较，得到比较差值，基于所述比较差值利用PID控制算法计算得到矫正值，基于所述矫正值对机器人进行姿态矫正。本发明利用激光雷达采集点云数据并进行处理，能够准确得到机器人当前姿态，利用PID控制算法能够精确快速的矫正机器人姿态。

Description

一种基于激光雷达的机器人精确姿态矫正方法及装置

技术领域

本发明涉及机器人姿态矫正领域，特别涉及一种基于激光雷达的机器人精确姿态矫正方法及装置。

背景技术

目前室内自主导航机器人大多采用码盘，陀螺仪，IMU(Inertial measurementunit，惯性测量单元)，激光雷达等传感器采集相应信息通过融合算法来估算机器人实时的姿态。码盘类似于汽车的里程计，通过计算轮毂转动的圈数来推算机器人运动的路程，但机器人在运动过程中难免受到打滑，轮胎磨损等因素的影响导致码盘检测误差，并且会随着机器人运动逐渐累加，无法有效消除误差积累，用来估算机器人位置姿态的效果不是特别好。陀螺仪和IMU由于电子器件的电气特性会出现零漂(零漂是输入为0的时候的输出)的现象，并且会随着时间误差逐渐累积，同样无法有效消除误差积累，从而不能准确估算机器人的姿态。

激光雷达是通过激光扫描周围环境信息，经过处理得到点云信息之后通过粒子滤波算法与先验地图进行匹配，最后估计出置信度最高的机器人姿态，雷达有很强的适应性，不受光照的影响，基于激光雷达的定位算法在周围定位特征较为丰富的情况下有较优秀的定位效果。但是在某些特殊场合下对机器人的姿态精度要求较高，特别是当机器人周围环境存在平整墙面，并对机器人在墙面附近有较高精度角度定位要求的时候，如果将传统算法的允许误差调到一个较小的水平时，机器人会在目标位置不停旋转来精确微调姿态以到达目标姿态，这样就会存在机器人姿态微调过慢的问题。

发明内容

本发明的目的在于克服现有技术中所存在的不能准确估算机器人姿态和在特定应用场景中调整机器人姿态速度慢的不足，提供一种基于激光雷达的机器人精确姿态矫正方法及装置。

为了实现上述发明目的，本发明提供了以下技术方案：

一种基于激光雷达的机器人精确姿态矫正方法，包括以下步骤：

步骤一：采集机器人周围环境的点云信息数据，所述周围环境包括平整墙面；

步骤二：将所述点云信息数据转换成二维笛卡尔坐标数据，从所述二维笛卡尔坐标数据提取参考点云的坐标数据；

步骤三：对所述参考点云的坐标数据进行线性拟合，得到拟合直线段，根据所述拟合直线段的斜率得到机器人当前姿态的角度值；

步骤四：将所述机器人当前姿态的角度值与标准设定值进行比较，得到比较差值；

步骤五：基于所述比较差值利用PID控制算法计算得到矫正值，基于所述矫正值对机器人进行姿态矫正。

本发明所述的一种基于激光雷达的机器人精确姿态矫正方法，主要应用在机器人周围环境有平整墙面，且对机器人在墙面附近有较高精度角度定位要求的场景。本发明在这样特定的应用场景中，利用激光雷达采集周围环境的点云信息进行处理后，能够准确得到机器人的姿态角度；利用PID控制算法对机器人当前的姿态角度进行调控，能够快速精确的矫正机器人姿态。

优选的，所述步骤一中，所述点云信息数据包括雷达极限扫描角度数据、雷达扫描角度增益数据和点云距离雷达原点的距离数据。

优选的，所述步骤二中，将所述点云信息数据转换成二维笛卡尔坐标数据，包括：

将所述点云信息数据中的雷达扫描角度增益数据和点云距离雷达原点的距离数据，转换成二维笛卡尔坐标数据(x[i]，y[i])：

其中，Ranges为点云距离雷达原点的距离，AngleIncrement为雷达扫描角度增益，由于采集的点云信息数据是一个数组，i为数组下标。

优选的，所述步骤二中，从所述二维笛卡尔坐标数据提取参考点云的坐标数据，包括根据所述的平整墙面设定参考范围，根据设定好的参考范围采用裁剪法从所述二维笛卡尔坐标数据提取出参考点云的坐标数据。

优选的，所述步骤三中，采用最小二乘法对参考点云的坐标数据进行线性拟合，得到拟合直线段。

优选的，所述步骤五中，采用增量式PID控制算法计算得到矫正值。

本发明还公开了一种基于激光雷达的机器人精确姿态矫正的装置，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行前述的一种基于激光雷达的机器人精确姿态矫正方法。

与现有技术相比，本发明的有益效果：

本发明应用在一种特定的场景中，即在机器人周围环境有平整墙面，且对机器人在墙面附近有较高精度角度定位要求，本发明利用激光雷达采集周围环境的点云信息进行处理后，能够准确得到机器人的姿态角度，利用PID控制算法对机器人当前的姿态角度进行调控，能够快速精确的矫正机器人姿态。

附图说明：

图1是本发明实施例1所述的一种基于激光雷达的机器人精确姿态矫正方法的方法流程图。

图2是本发明实施例1所述的一种基于激光雷达的机器人精确姿态矫正方法的机器人周围环境的模拟图。

图3是本发明实施例1所述的一种基于激光雷达的机器人精确姿态矫正方法的将点云信息数据转换成二维笛卡尔坐标数据的结果模拟图。

图4是本发明实施例1所述的一种基于激光雷达的机器人精确姿态矫正方法的从二维笛卡尔坐标数据提取参考点云的坐标数据的结果模拟图。

图5是本发明实施例2所述的一种基于激光雷达的机器人精确姿态矫正装置的装置结构示意图。

图中标记：310-电子设备，311-处理器，312-存储器，313-输入输出接口，314-电源。

具体实施方式

下面结合试验例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围。

实施例1

如图1所示，一种基于激光雷达的机器人精确姿态矫正方法，包括以下步骤：

本发明中所要解决的机器人姿态矫正问题，机器人是应用在周围环境有平整墙面的场景中，具体的，本实施例中采用激光雷达采集机器人周围环境的点云信息数据，包括有雷达极限扫描角度、雷达扫描角度增益、以及点云距离雷达原点的距离等数据。

步骤二：将所述点云信息数据转换成二维笛卡尔坐标数据；

将激光雷达采集的点云信息数据处理成在二维笛卡尔坐标系下的(x,y)坐标。具体的，本实施例中将采集到的点云信息数据中的雷达扫描角度增益数据和点云距离雷达原点的距离数据转换成二维笛卡尔坐标数据(x[i]，y[i])：

步骤三：从所述二维笛卡尔坐标数据提取参考点云的坐标数据；

本发明中使用的机器人其工作时需要环境中的平整墙面作为参考来进行前行，因此，本实施例中，根据所述平整墙面设定一个参考范围，根据设定好的参考范围采用裁剪法从所述二维笛卡尔坐标数据提取处参考点云的坐标数据。

具体的，如图2-4所示，实验环境中，首先要确保机器人工作环境中存在平整墙面，根据平整墙面来设定裁剪角度范围，就能够保证裁剪出的数据为直线段的一部分。因此，所裁剪出的参考点云的坐标数据为激光雷达扫描平整墙面上的部分点云的坐标数据。

步骤四：对所述参考点云的坐标数据进行线性拟合，得到拟合直线段；

本实施例中，优选的，采用最小二乘法对参考点云的坐标数据进行线性拟合，得到拟合直线段，包括：

设定拟合直线段的直线方程为：

y＝c₁x+c₀ (1)

将n组离散的点云坐标数据带入公式(1)得：

上述式(2)可以转换成：

正规方程为：

求解得到c₁和c₀，即得到拟合直线段。

步骤五：根据所述拟合直线段的斜率得到机器人当前状态的角度值；

在上述得到的拟合直线段的直线方程中，c₁为所述拟合直线段的斜率，根据直线斜率与直线倾斜角的关系，计算得出直线倾斜角，从而得到机器人当前姿态的角度值。

步骤六：将所述机器人当前姿态的角度值与标准设定值进行比较，得到比较差值，基于所述比较差值利用PID控制算法计算得到矫正值，基于所述矫正值对机器人进行姿态矫正。

本实施例，将所述机器人当前状态的角度值与标准设定值进行比较，得到比较差值，计算公式为式(5)：

e(k)＝r(k)-c(k) (5)

式(5)中，r(k)为标准设定值，c(k)为当前姿态的角度值，e(k)为比较差值；

基于上述计算得到的比较差值，采用增量式PID控制算法计算得到矫正值，根据位置式PID公式：

式(6)中，K_p为比例系数，K_i为积分系数，K_d为微分系数，u(k)为第k个采样时刻的输出量；

由式(6)可以得到第k-1个采样时刻的输出量为：

将式(6)与式(7)相减，就可以得到增量式PID控制算法公式(8)与式(9)为：

Δu(k)＝u(k)-u(k-1) (8)

Δu(k)＝K_p(e(k)-e(k-1))+K_ie(k)+K_d(e(k)-2e(k-1)+e(k-2)) (9)

式(8)与(9)中，Δu(k)为采用增量式PID控制算法计算得到的矫正值。

利用上述得到矫正值来控制机器人旋转，使机器人自动调整姿态。

假定当前机器人旋转角速度为1rad/s，利用PID控制算法计算出来的矫正值为2rad/s，那么机器人的旋转角速度会调整到3rad/s。当机器人的旋转角速度太快时，通过PID控制算法能够使机器人旋转角速度降低，这样通过PID控制算法来调控机器人姿态角度能够快速并且准确的使机器人调整到目标状态。

实施例2

如图5所示，一种基于激光雷达的机器人精确姿态矫正的装置，即电子设备310(例如具备程序执行功能的计算机服务器)，其包括至少一个处理器311，电源314，以及与所述至少一个处理器311通信连接的存储器312和输入输出接口313；所述存储器312存储有可被所述至少一个处理器311执行的指令，所述指令被所述至少一个处理器311执行，以使所述至少一个处理器311能够执行前述实施例1所公开的方法；所述输入输出接口313可以包括显示器、键盘、鼠标、以及USB接口，用于输入输出数据；电源314用于为电子设备310提供电能。

本领域技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。

当本发明上述集成的单元以软件功能单元的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、磁碟或者光盘等各种可以存储程序代码的介质。

以上实施例仅用以说明本发明而并非限制本发明所描述的技术方案，尽管本说明书参照上述的各个实施例对本发明已进行了详细的说明，但本发明不局限于上述具体实施方式，因此任何对本发明进行修改或等同替换；而一切不脱离发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。

Claims

1.一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，包括以下步骤：

步骤二：将所述点云信息数据转换成二维笛卡尔坐标数据，采用裁剪法从所述二维笛卡尔坐标数据提取参考点云的坐标数据；

2.根据权利要求1所述的一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，所述步骤一中，所述点云信息数据包括雷达极限扫描角度数据、雷达扫描角度增益数据和点云距离雷达原点的距离数据。

3.根据权利要求2所述的一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，所述步骤二中，将所述点云信息数据转换成二维笛卡尔坐标数据，包括：

其中，Ranges为点云距离雷达原点的距离，AngleIncrement为雷达扫描角度增益，i为点云信息数据数组的下标。

4.根据权利要求1所述的一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，所述步骤二中，从所述二维笛卡尔坐标数据提取参考点云的坐标数据，包括根据所述的平整墙面设定参考范围，根据设定好的参考范围采用裁剪法从所述二维笛卡尔坐标数据提取出参考点云的坐标数据。

5.根据权利要求1所述的一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，所述步骤三中，采用最小二乘法对参考点云的坐标数据进行线性拟合，得到拟合直线段。

6.根据权利要求1所述的一种基于激光雷达的机器人精确姿态矫正方法，其特征在于，所述步骤五中，采用增量式PID控制算法计算得到矫正值。

7.一种基于激光雷达的机器人精确姿态矫正的装置，其特征在于，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1至6中任一项所述的方法。