CN113558522A - 应用于清洁机器人的行进控制方法以及清洁机器人 - Google Patents

Abstract

本申请公开了应用于清洁机器人的行进控制方法以及清洁机器人，清洁机器人包括激光测距传感器、控制芯片、里程计和陀螺仪，方法包括：里程计被配置为获取清洁机器人在第一采样周期内的行进距离信息；陀螺仪被配置为获取清洁机器人在第二采样周期内的偏转角度信息；控制芯片被配置为根据行进距离信息和偏转角度信息，获取清洁机器人在第一位姿后的目标位姿，以及确定目标位姿对应的测距激光的第一测距信息，根据目标位姿与第一测距信息，对第一位姿下激光测距传感器的测距结果进行修正，得到第二测距信息。本申请综合利用了里程计和陀螺仪的数据，来修正激光测距传感器的测距结果，有助于得到更可靠的测距信息。

Description

应用于清洁机器人的行进控制方法以及清洁机器人

技术领域

本申请涉及清洁机器人技术领域，尤其涉及一种应用于清洁机器人的行进控制方法，以及相应的一种清洁机器人和计算机可读存储介质。

背景技术

2D激光测距传感器广泛应用于扫地机器人。其精确的测距信息是激光即时定位与地图构建(simultaneous localization and mapping，SLAM)算法的前提。在实际应用中，激光测距传感器固定在扫地机器人上，由于扫地机器人自身在运动，导致测距结果与事实产生误差，该过程称为畸变。如图1所示，图1示出了激光测距传感器测距误差产生的原理示意图，一帧激光数据(通常包括多束激光)周期内，假定在发射某束激光时，扫地机器人在世界坐标系w下的真实位姿可由SLAM技术确切得到，记为(x₀,y₀,θ₀)，测得的激光反射点P₀在扫地机器人自身的坐标系b下的距离与角度信息记为(L₀,β₀)；在第i束激光测量时，扫地机器人实际已经运动到(x_i,y_i,θ_i)，而其给出的测量信息为(L_i,β_i)。在后续激光数据处理时，若把(L_i,β_i)当作(x₀,y₀,θ₀)下的测量数据，则会引入误差。在扫地机器人中，往往采用低成本的激光测距传感器，其扫描频率小，一般为5Hz～10Hz，在扫地机器人快速平移或旋转时，引入的误差明显。

一种解决办法是求出扫地机器人在世界坐标系w下的(x_i,y_i,θ_i)，然后通过几何运算，计算第i束激光击中点P_i在世界坐标系w下的坐标，然后计算出P_i在扫地机器人(x₀,y₀,θ₀)位姿下的激光测距传感器的极坐标(L_i',β_i')。

难点是如何计算(x_i,y_i,θ_i)，目前的方案有：

1、用高采样频率(比如，100Hz～1000Hz)的惯性测量单元(Inertial measurementunit，IMU)测量加速度，并通过二次积分，求出相对位移

测量角速度，通过积分计算出角度相对变化Δθ_i，进而得出(x_i,y_i,θ_i)＝(x₁+Δx_i,y₁+Δy_i,θ₁+Δθ_i)。其缺点在于：一般的IMU线加速度精度差，二次积分后的精度依然很差，使得ΔP_i计算不准，导致修正去畸变效果不理想。

2、用低采样频率(比如，20Hz～50Hz)的里程计直接测量扫地机器人的线速度与角速度。一帧激光数据内相邻激光点的时间间隔(比如，10Hz，0.9°分辨率激光测距传感器的相邻采样点间隔为0.25ms)，往往小于里程计的更新周期(20ms～50ms)。假定扫地机器人在一帧数据周期内做匀速运动，用测量到的扫地机器人的线速度与角速度(υ₀,ω₀)推算出一帧内第i束激光在其发射时，扫地机器人在世界坐标系w下位置偏移

和角度偏移Δθ_i，最后计算出(x_i,y_i,θ_i)＝(x₁+Δx_i,y₁+Δy_i,θ₁+Δθ_i)。其缺点在于：里程计的角度测量较差，且一个里程计周期内(20ms～50ms)内做匀速运动的假设较弱，导致修正去畸变效果不理想。

基于此，对于激光测距传感器，需要修正得到更可靠的测距信息。

发明内容

本申请实施例提供一种应用于清洁机器人的行进控制方法，以及相应的一种清洁机器人和计算机可读存储介质，用以解决现有技术中的如下技术问题：通过现有的清洁机器人的激光测距传感器得到的测距信息不够可靠。

本申请实施例采用下述技术方案：

一种应用于清洁机器人的行进控制方法，所述清洁机器人包括激光测距传感器、控制芯片、里程计和陀螺仪，其特征在于，所述方法包括：

所述里程计被配置为获取所述清洁机器人在第一采样周期内的行进距离信息；

所述陀螺仪被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息；

所述控制芯片被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

可选地，所述控制芯片被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在所述第一位姿之后的多个后续位姿构成的后续位姿序列，根据所述后续位姿序列，获取所述清洁机器人的所述目标位姿。

可选地，所述第一采样周期被配置为大于所述第二采样周期；

所述控制芯片被配置为根据所述第二采样周期，获取所述清洁机器人的所述目标位姿。

可选地，所述控制芯片被配置为通过拓展卡尔曼滤波(Extended Kalman Filter，EKF)或者无迹卡尔曼滤波(Unscented Kalman Filter，UKF)，对所述行进距离信息和所述偏转角度信息进行数据融合处理，以便获取所述目标位姿。

可选地，所述控制芯片被配置为获取指定的状态获取方程和协方差方程，通过将所述行进距离信息和所述偏转角度信息代入所述状态获取方程和所述协方差方程进行计算，获取出状态和协方差，根据所述状态和协方差，计算增益，通过根据所述增益迭代更新所述状态和协方差，获取所述后续位姿序列。

可选地，所述后续位姿序列内相邻位姿之间的间隔时间等于所述第二采样周期。

可选地，所述第一采样周期被配置为20ms～50ms；和/或，

所述第二采样周期被配置为1ms～10ms。

可选地，所述控制芯片被配置为若所述后续位姿与所述第一测距信息之间不具有时间同步性，则通过在所述后续位姿序列中进行位姿插值，得到中间位姿，将所述中间位姿与所述第一测距信息进行时间同步性的匹配，根据匹配成功的中间位姿和所述第一测距信息，确定所述第二测距信息。

可选地，所述控制芯片被配置为若所述后续位姿与所述第一测距信息之间不具有时间同步性，则以所述清洁机器人在不同的所述后续位姿之间进行匀速运动为条件，进行所述位姿插值。

一种清洁机器人，包括激光测距传感器、控制芯片、里程计和陀螺仪，所述里程计，被配置为获取所述清洁机器人在第一采样周期内的行进距离信息；

所述陀螺仪，被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息；

所述控制芯片，被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述的应用于清洁机器人的行进控制方法。

本申请实施例采用的上述至少一个技术方案能够达到以下有益效果：综合利用了里程计采集的行进距离信息和陀螺仪采集的偏转角度信息，来修正激光测距传感器的测距结果，有助于得到更可靠的测距信息；通过卡尔曼滤波对行进距离信息和偏转角度信息进行融合，获取清洁机器人的位姿，并且可以通过假设匀速运动来插值位姿，来提高位姿获取的准确性；采用高频的偏转角度信息来获取清洁机器人高频的位姿序列，提高了假设匀速运动的可靠性，有助于更准确地修正得到更可靠的测距信息。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为激光测距传感器测距偏差产生的原理示意图；

图2为本申请的一些实施例提供的一种应用于清洁机器人的行进控制方法的流程示意图；

图3为本申请的一些实施例提供的一种应用场景下，图2的方法的原理示意图；

图4为本申请的一些实施例提供的一种应用场景下，图2的方法采用的数据融合流程示意图；

图5为本申请的一些实施例提供的一种应用场景下，图2的方法的详细流程示意图；

图6为本申请的一些实施例提供的一种清洁机器人的结构示意图。

具体实施方式

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

本申请提到的清洁机器人包括扫地机器人，还可以包括其他一些具有清洁功能的机器人，比如，拖地机器人、玻璃清洁机器人等。

清洁机器人包括激光测距传感器、控制芯片、里程计和陀螺仪等部件，背景技术中提到了IMU，IMU中包含陀螺仪，主要利用陀螺仪测量偏转角度。图2为本申请的一些实施例提供的一种应用于清洁机器人的行进控制方法的流程示意图。

图2中的流程包括以下步骤：

S200：所述里程计被配置为获取所述清洁机器人在第一采样周期内的行进距离信息。

S202：所述陀螺仪被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息。

在本申请的一些实施例中，第一采样周期大于第二采样周期，也即，里程计相应的数据采样频率小于陀螺仪的数据采样频率。第一采样周期比如被配置为20ms～50ms；第二采样周期比如被配置为1ms～10ms。

S204：所述控制芯片被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

在本申请的一些实施例中，对于背景技术，假定第一位姿为(x₀,y₀,θ₀)，则目标位姿可以包括(x_i,y_i,θ_i)，第一测距信息可以包括(L_i,β_i)，第二测距信息比如为(L_i',β_i')。

在本申请的一些实施例中，对行进距离信息和偏转角度信息进行融合处理，利用融合的信息修正测距结果，而不是简单地分别单独用这两种信息修正测距结果再结合。

通过图2的方法，综合利用了行进距离信息和偏转角度信息，来修正激光测距传感器的测距结果，有助于得到更可靠的测距信息。

基于图2的方法，本申请的一些实施例还提供了该方法的一些具体实施方案，以及扩展方案，下面继续说明。

在本申请的一些实施例中，控制芯片被配置为根据行进距离信息和偏转角度信息，获取清洁机器人在所述第一位姿之后的多个后续位姿构成的后续位姿序列，根据后续位姿序列，获取清洁机器人的目标位姿。后续位姿、目标位姿可以通过估计获得，或者，若后续位姿、目标位姿已经确切发生，则可以直接使用，如此可靠性更好，下面一些实施例主要以通过估计获得后续位姿、目标位姿为例，进行说明。更直观地，参见图3，图3为本申请的一些实施例提供的一种应用场景下，图2的方法的原理示意图。

在图3中，清洁机器人的位姿是以世界坐标系w进行表示的，测距信息是以机器人坐标系b进行表示的，示例性地示出了第一位姿(x₀,y₀,θ₀)，以及至少包含三个后续位姿((x₁,y₁,θ₁)、(x_i,y_i,θ_i)、(x₂,y₂,θ₂))的后续位姿序列。可以对高频率的陀螺仪获取的偏转角度信息与低频率的里程计获取的行进距离信息，进行数据融合，在已知的真实位姿(x₀,y₀,θ₀)基础上，获取出与陀螺仪同频率的后续位姿序列，根据所要修正的激光束的测距结果，在后续位姿序列中尝试匹配相应的(x_i,y_i,θ_i)，在后续位姿序列频率较高的情况下，后续位姿序列中的位姿密集，因此更容易匹配成功，若未匹配成功，可以进一步地在后续位姿序列中插值位姿，使得后续位姿序列中的位姿更为密集，如此，容易匹配成功，比如，初始的后续位姿序列可以包含(x₁,y₁,θ₁)和(x₂,y₂,θ₂)，而不包含(x_i,y_i,θ_i)，通过在(x₁,y₁,θ₁)和(x₂,y₂,θ₂)之间插值位姿，才***了(x_i,y_i,θ_i)。

在上例中，是获取了与陀螺仪同频率的后续位姿序列，即后续位姿序列内相邻位姿之间的间隔时间等于第二采样周期。在实际应用中，以第二采样周期作为参照目标，还可以往高或者往低调整后续位姿序列的频率，而未必要保持与陀螺仪同频率。

在本申请的一些实施例中，控制芯片被配置为通过EKF或者UKF，对行进距离信息和偏转角度信息进行数据融合处理，并基于数据融合处理，进一步地获取目标位姿。下面以EKF为例具体说明。参见图4，图4为本申请的一些实施例提供的一种应用场景下，图2的方法采用的数据融合流程示意图。

在图4中，控制芯片被配置为获取指定的状态获取方程和协方差方程(具体比如采用EKF的相应方程)，通过将行进距离信息和偏转角度信息代入状态获取方程和协方差方程进行计算，获取出状态和协方差，根据状态和协方差，计算增益，通过根据增益迭代更新状态和协方差，获取后续位姿序列。

例如，采用6个维度构建EKF的状态变量x_k＝(X,Y,Yaw,V_x,V_y,V_Yaw)^T，其中，(X,Y,Yaw)为世界坐标系w下的平面位移和平面旋转角度，(V_x,V_y,V_Yaw)为里程计和陀螺仪提供的机器人坐标系b下的线速度和角速度。下面基于此说明EKF的预测过程、观察过程和更新过程。

预测过程的状态方程为：

其中，f(x_k-1)由牛顿运动学方程组成，w_k-1为高斯噪声，具体如下面方程所示：

预测过程的协方差为：

其中，F为f(x_k-1)的一阶雅可比矩阵，具体为：

Q为过程噪声协方差，是个定值，可以在初始化时设定。

EKF是单状态方程、多观察方程的***，观察方程为：

z_k＝h(x_k)+υ_k；

其中，里程计能直接观察到线速度，设置其噪声为均值为0协方差为R_odom的噪声V_odom，其观察方程为线性，为：

其中，观察矩阵h_odom为：

其中，IMU能直接观察到角速度，设置其噪声为均值为0协方差为R_imu的噪声V_imu，其观察方程为线性，为：

其中，观察矩阵h_imu为：

对于更新过程，增益计算、状态更新以及协方差传递过程如下所示：

其中，R为R_odom或R_imu，K为增益，由于观察方程都为线性方程，因此观察方程的雅可比矩阵H即为h_odom或h_imu，x_k记为融合后的状态变量。

在本申请的一些实施例中，控制芯片被配置为若后续位姿与第一测距信息之间不具有时间同步性，则通过在后续位姿序列中进行位姿插值，得到中间位姿，将中间位姿与第一测距信息进行时间同步性的匹配，根据匹配成功的中间位姿和第一测距信息，确定所述第二测距信息。进一步地，控制芯片被配置为若后续位姿与第一测距信息之间不具有时间同步性，则进行位姿插值。

位姿插值的具体方式是多样的。前面已经提到，在陀螺仪频率较高时，可以获取与陀螺仪频率相同的后续位姿序列，在这种情况下，可以以清洁机器人在不同的后续位姿之间进行匀速运动为条件，进行位姿插值，这里的高频率有助于提高这种匀速运动假设的可靠性。

以匀速运动为例，参照图5，图5为本申请的一些实施例提供的一种应用场景下，图2的方法的详细流程示意图。

在图5中，利用了里程计提供的线速度，以及IMU提供的角速度，通过UKF进行滤波融合，获取出与IMU同频率的后续位姿序列。

激光测距仪作为上述的激光测距传感器，当前所要匹配的是第i束激光，第一测距信息为(L_i,β_i)。

若第i束激光正好与获取的后续位姿s时间同步，则：

(x_i,y_i,θ_i)＝(x_s,y_s,θ_s)；

若第i束激光无时间同步的后续位姿，则进行进行位姿插值。假定第i束激光对应的时间为t，图中表示为t_i，位于获取的后续位姿(x₁,y₁,θ₁)和(x₂,y₂,θ₂)之间，这两个位姿对应的时间分别为s和e。

对于平移量，进行匀速运动插值：

对于旋转量，可以进行四元数球面线性插值：

对于旋转量，也可以直接使用角度插值：

在求解出第i束激光对应的插值位姿(x_i,y_i,θ_i)后，可通过几何运算求得点P_i在世界坐标系w下的坐标：

x_pi＝x_i+sin(β_i+(θ₀-θ_i))L_i；

y_pi＝y_i+cos(β_i+(θ₀-θ_i))L_i；

则得到(x₀,y₀,θ₀)坐标系下的极坐标：

按照上面的方案，将各束激光对应的测距结果进行修正，从而实现了激光测距去畸变，进而有助于更可靠地控制清洁机器人行进。

基于同样的思路，本申请的一些实施例还提供了一种清洁机器人，参见图4，图4为该清洁机器人示例性的结构示意图。

图4中的清洁机器人包括激光测距传感器、控制芯片、里程计和陀螺仪，所述里程计，被配置为获取所述清洁机器人在第一采样周期内的行进距离信息；

所述陀螺仪，被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息；

所述控制芯片，被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

基于同样的思路，本申请的一些实施例还提供了一种计算机可读存储介质，存储介质存储有计算机程序，计算机程序被处理器执行时实现上述的应用于清洁机器人的行进控制方法。

本申请中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种应用于清洁机器人的行进控制方法，其特征在于，所述清洁机器人包括激光测距传感器、控制芯片、里程计和陀螺仪，其特征在于，所述方法包括：

所述里程计被配置为获取所述清洁机器人在第一采样周期内的行进距离信息；

所述陀螺仪被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息；

所述控制芯片被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

2.如权利要求1所述的应用于清洁机器人的行进控制方法，其特征在于，所述控制芯片被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在所述第一位姿之后的多个后续位姿构成的后续位姿序列，根据所述后续位姿序列，获取所述清洁机器人的所述目标位姿。

3.如权利要求1所述的应用于清洁机器人的行进控制方法，其特征在于，所述第一采样周期被配置为大于所述第二采样周期；

所述控制芯片被配置为根据所述第二采样周期，获取所述清洁机器人的所述目标位姿。

4.如权利要求2所述的应用于清洁机器人的行进控制方法，其特征在于，所述控制芯片被配置为通过拓展卡尔曼滤波EKF或者无迹卡尔曼滤波UKF，对所述行进距离信息和所述偏转角度信息进行数据融合处理，以便获取所述目标位姿。

5.如权利要求2所述的应用于清洁机器人的行进控制方法，其特征在于，所述控制芯片被配置为获取指定的状态获取方程和协方差方程，通过将所述行进距离信息和所述偏转角度信息代入所述状态获取方程和所述协方差方程进行计算，获取出状态和协方差，根据所述状态和协方差，计算增益，通过根据所述增益迭代更新所述状态和协方差，获取所述后续位姿序列。

6.如权利要求2所述的应用于清洁机器人的行进控制方法，其特征在于，所述后续位姿序列内相邻位姿之间的间隔时间等于所述第二采样周期。

7.如权利要求2所述的应用于清洁机器人的行进控制方法，其特征在于，所述第一采样周期被配置为20ms～50ms；和/或，

所述第二采样周期被配置为1ms～10ms。

8.如权利要求2所述的应用于清洁机器人的行进控制方法，其特征在于，所述控制芯片被配置为若所述后续位姿与所述第一测距信息之间不具有时间同步性，则通过在所述后续位姿序列中进行位姿插值，得到中间位姿，将所述中间位姿与所述第一测距信息进行时间同步性的匹配，根据匹配成功的中间位姿和所述第一测距信息，确定所述第二测距信息。

9.如权利要求8所述的应用于清洁机器人的行进控制方法，其特征在于，所述控制芯片被配置为若所述后续位姿与所述第一测距信息之间不具有时间同步性，则以所述清洁机器人在不同的所述后续位姿之间进行匀速运动为条件，进行所述位姿插值。

10.一种清洁机器人，其特征在于，包括激光测距传感器、控制芯片、里程计和陀螺仪，其特征在于，所述里程计，被配置为获取所述清洁机器人在第一采样周期内的行进距离信息；

所述陀螺仪，被配置为获取所述清洁机器人在第二采样周期内的偏转角度信息；

所述控制芯片，被配置为根据所述行进距离信息和所述偏转角度信息，获取所述清洁机器人在第一位姿后的目标位姿，以及确定所述目标位姿对应的测距激光的第一测距信息，根据所述目标位姿与所述第一测距信息，对所述第一位姿下所述激光测距传感器的测距结果进行修正，得到第二测距信息。

Images