CN110716561A - 用于轨迹跟踪的扫地机前视位姿控制方法和系统 - Google Patents

Abstract

本发明涉及一种用于轨迹跟踪的扫地机前视位姿控制方法和系统，其中控制方法包括：从目标轨迹中选择前视目标点，计算所述前视目标点与扫地机位姿的向量差，得到位姿误差；将所述位姿误差传递给PID控制器，生成控制量；所述控制量包括X轴控制量、Y轴控制量和方向角控制量；将所述X轴控制量、所述Y轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。本发明结合了结合了扫地机的工作场景，提高了扫地机的轨迹跟踪精确度，并解决了在扫地机大幅度转向过程中跟踪容易产生误差的问题。

Description

用于轨迹跟踪的扫地机前视位姿控制方法和系统

技术领域

本发明涉及扫地机，具体涉及一种用于轨迹跟踪的扫地机前视位姿控制方法和系统。

背景技术

扫地机应当在保证安全的情况下，尽可能地覆盖工作环境中的空闲区域。它主要用于家庭和办公环境的清扫，而这样的环境中存在很多狭小的角落。所以通常采用可以原地转动的差速轮进行驱动。扫地机在使用过程中还涉及轨迹跟踪的问题。

在现有技术中，轨迹跟踪的方法主要有以下两种：

专利CN201811134109.9公开了一种基于Pure Pursuit改良的智能车辆路径跟踪方法。轨迹跟踪问题是无人驾驶技术需要解决的一个基础问题，在无人汽车领域得到了比较充分的研究。Pure Pursuit(纯追踪法)是一种无人汽车中的常用的轨迹跟踪方法，但是由于汽车这种前向加转舵的驱动方式需要较为开阔的工作空间，所以Pure Pursuit跟踪方法并不完全适用于扫地机。

Pure Pursuit方法的关键在于前视距离。用于Pure Pursuit方法的控制器总是在汽车前进的方向上，从目标路径中选择一个到汽车当前驱动轴位置有一定距离的点。这个距离就是前视距离，所对应的点就是下一时刻汽车的目标位置。根据几何关系和汽车的运动模型，计算汽车的转向角控制量。但是Pure Pursuit方法的转向角控制量的计算过程与位移控制量之间存在耦合关系，以至于它很难实现被控对象的原地转动。当需要大幅度转向时会出现较大的跟踪误差。

专利CN201611146847.6公开了一种差速驱动式AGV的完整控制系统和自动循迹方法。应用于工厂和自动化物流的自动导航车(AGV，Automated Guided Vehicle)通常采用和扫地机类似的差速驱动方式。PID控制器是这类机器进行轨迹跟踪的主要方法。在工厂或者物流仓库中，通常使用有明显色差的色带或者磁条来铺设AGV的运动轨迹。所以它们可以使用一些特定的传感器来识别预定轨迹，进而实现反馈控制。

在传统的AGV使用场景中，我们可以通过传感器直接测量机器人相对于预设轨迹之间的偏差。而且不需要考虑已经经过的轨迹。而在扫地机的应用场景中，单纯的从空间上计算机器人到各个路径点之间的距离来计算扫地机与目标轨迹之间的相对关系是不够的，这是因为如果路径中存在交叉点或者有两个路径点距离很近就会产生误判。

对于扫地机的工作环境而言，我们不可能预先铺设运动轨迹，因此不能通过传感器直接测量偏差量。但是可以通过一些全局定位的技术手段，比如惯导定位、雷达定位、视觉定位，确定扫地机在全局坐标系下的位姿，从而计算扫地机与目标轨迹之间的相对关系，完成控制。这有助于降低转向过程中的跟踪误差。

发明内容

针对现有技术的不足，本发明公开了一种用于轨迹跟踪的扫地机前视位姿控制方法和系统。

本发明所采用的技术方案如下：

一种用于轨迹跟踪的扫地机前视位姿控制方法，包括：

从目标轨迹中选择前视目标点，计算所述前视目标点与扫地机位姿的向量差，得到位姿误差；

将所述位姿误差传递给PID控制器，生成控制量；所述控制量包括X轴控制量、Y轴控制量和方向角控制量；

将所述X轴控制量、所述Y轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。

其进一步的技术方案为，从目标轨迹中选择前视目标点的方法是：

记录上一个控制周期所选择的前视目标点；

在当前控制周期中，从上一个控制周期所选择的前视目标点开始依次计算所述目标轨迹中的各个路径点到扫地机当前位置的欧式距离；

第一个到扫地机当前位置的欧式距离大于前视距离的点作为当前控制周期的前视目标点。

其进一步的技术方案为，当目标轨迹更新后，以新的目标轨迹的第一个路径点作为上一个控制周期的前视目标点。

其进一步的技术方案为，位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

其进一步的技术方案为，对所述线速度进行限幅，实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。

其进一步的技术方案为，对所述角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

一种用于轨迹跟踪的扫地机前视位姿控制系统，包括

前视目标点选择器，用于从目标轨迹中选择前视目标点，计算所述前视目标点与扫地机位姿的向量差，得到位姿误差；

PID控制器，接收所述位姿误差，并生成控制量；所述控制量包括X轴控制量、Y轴控制量和方向角控制量；

指令生成器，将所述X轴控制量、所述Y轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。

其进一步的技术方案为，所述PID控制器为三个独立的PID控制器；位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

其进一步的技术方案为，还包括线速度限幅模块，对指令生成器所合成的线速度进行限幅，生成实际线速度；实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。

其进一步的技术方案为，还包括角速度限幅模块，对指令生成器所生成的角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

本发明的有益效果如下：

为了解决跟踪路径点误判的问题，本发明结合了前视距离的概念，从空间和时间上区分路径点。为了精准的跟踪目标轨迹中需要大幅度转向的区段，本发明又充分利用差速驱动的运动模型，将转角控制量与位移控制量解耦，使用PID控制器分别控制转角和位移，二者在扫地机的工作场景中结合，使得扫地机的轨迹跟踪问题得到了很好的解决。

进一步的，本发明还在大幅度转向过程中，适当地约束被控对象的速度，可以提供足够长的时间进行转向，这是通过引入了一个速度衰减因子来实现这一功能，这主要得益于转角控制量与位移控制量的解耦。

附图说明

图1为实施例1的流程示意图。

图2为实施例2中选择前视目标点的方法流程示意图。

图3为实施例4的流程示意图。

图4为实施例9的框架结构示意图。

具体实施方式

下面结合附图，说明本发明的具体实施方式。在扫地机轨迹跟踪的过程中，设定目标轨迹和扫地机位姿两个概念如下：

目标轨迹由一系列前后相连的路径点构成，令P表示目标轨迹，p_k表示目标轨迹中第k个路径点，那么一条由n个路径点构成的路径可以形式化表示为：

P＝{p₀,p₁,...,p_n}

扫地机从路径点p₀开始到路径点p_n，先后依次经过各个路径点。路径点p_k由扫地机工作空间下的全局坐标来描述，即：

其中，x_k和y_k分别为路径点p_k在扫地机工作空间下的X轴和Y轴坐标值。

扫地机位姿由全局坐标和扫地机的方向角两个部分构成，可以用一个有三个元素的向量来表示：

其中，x和y分别为扫地机的X轴和Y轴坐标值。θ则是扫地机的方向角，当扫地机朝向X轴的正方向时θ＝0，从X轴正方向开始逆时针旋转为正，顺时针为负。

根据以上的前提，详述本发明的各个实施例。

实施例1。

图1为实施例1的流程示意图。如图1所示，实施例1具体包括：

S101.从目标轨迹P中选择前视目标点(x′,y′,θ′)^T。计算当前控制周期的前视目标点(x′,y′,θ′)^T与扫地机位姿(x,y,θ)的向量差，得到位姿误差(d_x,d_y,d_θ)^T：

S102.将位姿误差(d_x,d_y,d_θ)^T作为输入量传递给PID控制器，生成控制量(u_x,u_y,u_θ)^T；控制量包括X轴控制量u_x、Y轴控制量u_y和方向角控制量u_θ；

S103.根据运动学模型，每个控制周期下的控制量(u_x,u_y,u_θ)^T与扫地机角速度v和线速度ω关系：

其中，T为控制周期。进一步的可以写出扫地机的角速度和线速度控制量：

将X轴控制量u_x、Y轴控制量u_y合成线速度指令：

根据方向角控制量u_θ生成角速度指令：

实施例2。

实施例2具体包括：

S201.从目标轨迹P中选择前视目标点(x′,y′,θ′)^T。计算当前控制周期的前视目标点(x′,y′,θ′)^T与扫地机位姿(x,y,θ)的向量差，得到位姿误差(d_x,d_y,d_θ)^T；

图2为实施例2中选择前视目标点的方法流程示意图。如图2所示，具体的，选择前视目标点的方法包括：

S2011.记录上一个控制周期所选择的前视目标点p_k-1。如果在选择前视目标点的时候发生了目标轨迹更新事件，则以新的目标轨迹的第一个路径点p₀作为上个控制周期的前视目标点p_k-1；

S2012.在当前控制周期中，从前视目标点p_k-1开始依次计算扫地机目标轨迹中各个路径点到扫地机当前位置的欧式距离：

其中，j为路径点序号，x_j和y_j分别为第j路径点的X轴和Y轴坐标值，x和y分别为扫地机的X轴和Y轴坐标值。

S2013.选取当前控制周期的前视目标点；当前控制周期的前视目标点为第一个到扫地机当前位置的欧式距离大于前视距离l_d的点p′＝(x′,y′,θ′)^T。

S102.将位姿误差(d_x,d_y,d_θ)^T作为输入量传递给PID控制器，生成控制量(u_x,u_y,u_θ)^T；控制量包括X轴控制量u_x、Y轴控制量u_y和方向角控制量u_θ；

S103.将X轴控制量u_x、Y轴控制量u_y合成线速度指令：

根据方向角控制量u_θ生成角速度指令：

实施例2中增加了前视目标点的选择方法，是从上一控制周期的前视目标点开始(包含该点)，依次向后搜索路径点，选择第一个到扫地机欧式距离超过前视距离的点

实施例3。

实施例3具体包括：

S301.从目标轨迹P中选择前视目标点(x′,y′,θ′)^T。计算当前控制周期的前视目标点(x′,y′,θ′)^T与扫地机位姿(x,y,θ)的向量差得到位姿误差(d_x,d_y,d_θ)^T；

S302.将位姿误差(d_x,d_y,d_θ)^T中的三个分量分别作为输入量传递给三个独立的PID控制器，三个独立的PID控制器分别生成控制量(u_x,u_y,u_θ)^T的三个分量，包括X轴控制量u_x、Y轴控制量u_y和方向角控制量u_θ；

差速驱动的小车运动学模型如下所示，其中，v为扫地机的线速度，ω是扫地机的角速度。

从其运动学模型中，可以看出方向角的变化量只与角速度相关。而X轴与Y轴是正交关系的，所以我们可以分别为X轴、Y轴和方向角设计三个独立的控制通道。那么k时刻的位姿可以表示为：

其中，x(k)，y(k)，θ(k)分别为第k时刻的扫地机位姿，u_x,u_y,u_θ分别是X轴、Y轴和方向角控制器输出的控制量。

S303.将X轴控制量u_x、Y轴控制量u_y合成线速度指令：

根据方向角控制量u_θ生成角速度指令：

实施例3实现了位姿的三通道解耦控制。根据差速驱动的小车运动学模型，为扫地机的X轴、Y轴和方向角分别设计一个PID控制器，进行独立控制。并将X轴、Y轴的控制器输出合成为线速度控制量，方向角控制器的输出直接生成角速度控制量。

实施例4。

图3为实施例4的流程示意图。如图3所示，实施例4具体包括：

S401.从目标轨迹P中选择前视目标点(x′,y′,θ′)^T。计算当前控制周期的前视目标点(x′,y′,θ′)^T与扫地机位姿(x,y,θ)的向量差得到位姿误差(d_x,d_y,d_θ)^T；

S402.将位姿误差(d_x,d_y,d_θ)^T作为输入量传递给PID控制器，生成控制量(u_x,u_y,u_θ)^T；控制量包括X轴控制量u_x、Y轴控制量u_y和方向角控制量u_θ；

S403.将X轴控制量u_x、Y轴控制量u_y合成线速度指令：

根据方向角控制量u_θ生成角速度指令：

S404.对线速度和角速度进行限幅。在大幅度转向过程中，需要适当地约束扫地机的线速度，为转向提供足够长的时间，以达到更精准地跟踪目标轨迹的目的。为此，本实施例设计了一个线速度衰减因子β，根据角度误差限制扫地机的线速度。

为了防止控制器输出过大的控制量，导致系统发生意外，在使用时还需要设定最大的线速度v_max和最大角速度ω_max。

实际输出的线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。当角度误差的绝对值|d_θ|＝0时，不对线速度进行限制，有

角度误差越大，限制就越强，有|d_θ|→∞时，

实际输出的角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)，

其中，ω_max为最大角速度，ω为所生成的角速度。

其中，

是根据角度误差限制之后的扫地机线速度。在实施例4中，根据角度偏差限制线速度控制量。通过线速度衰减因子和指数函数，对合成后的X轴、Y轴的控制量进行限制。

实施例5。

实施例5具体包括：

S501.从目标轨迹P中选择前视目标点(x′,y′,θ′)^T。计算当前控制周期的前视目标点(x′,y′,θ′)^T与扫地机位姿(x,y,θ)的向量差得到位姿误差(d_x,d_y,d_θ)^T：

具体的，选择前视目标点的方法包括：

S5011.记录上一个控制周期所选择的前视目标点p_k-1；如果在选择前视目标点的时候发生了目标轨迹更新事件，则以新的目标轨迹的第一个路径点p₀作为上个控制周期的前视目标点p_k-1；

S5012.在当前控制周期中，从前视目标点p_k-1开始依次计算扫地机目标轨迹中各个路径点到扫地机当前位置的欧式距离：

S5013.选取当前控制周期的前视目标点；当前控制周期的前视目标点为第一个到扫地机当前位置的欧式距离大于前视距离l_d的点p′＝(x′,y′,θ′)^T。

S502.将位姿误差(d_x,d_y,d_θ)^T中的三个分量分别作为输入量传递给三个独立的PID控制器，三个独立的PID控制器分别生成控制量(u_x,u_y,u_θ)^T的三个分量，包括X轴控制量u_x、Y轴控制量u_y和方向角控制量u_θ；

S503.将X轴控制量u_x、Y轴控制量u_y合成线速度指令：

根据方向角控制量u_θ生成角速度指令：

S504.设计线速度衰减因子β，根据角度误差限制扫地机的线速度。

为了防止控制器输出过大的控制量，导致系统发生意外，在使用时还需要设定最大的线速度v_max和角速度ω_max。

实际输出的线速度控制量v_cmd为

实际输出的角速度控制量ω_cmd：

ω_cmd＝min(ω_max,ω)

实施例5中各个具体的推理和计算过程见实施例1～实施例4。实施例5作为优选实施例。

实施例6

实施例6是用于轨迹跟踪的扫地机前视位姿控制系统，包括

前视目标点选择器，用于从目标轨迹中选择前视目标点，计算前视目标点与扫地机位姿的向量差，得到位姿误差；

其中，选择前视目标点的方法是：

记录上一个控制周期所选择的前视目标点；

在当前控制周期中，从上一个控制周期所选择的前视目标点开始依次计算目标轨迹中的各个路径点到扫地机当前位置的欧式距离；

第一个到扫地机当前位置的欧式距离大于前视距离的点作为当前控制周期的前视目标点。

PID控制器，接收位姿误差，并生成控制量；控制量包括X轴控制量、Y轴控制量和方向角控制量；

指令生成器，将X轴控制量、Y轴控制量合成线速度指令；根据方向角控制量生成角速度指令。

实施例7。

实施例7是在实施例6的基础上，进一步的，PID控制器为三个独立的PID控制器；位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

实施例8

实施例8是在实施例6的基础上，进一步的，还包括线速度限幅模块和/或角速度限幅模块。

线速度限幅模块，用于对指令生成器所合成的线速度进行限幅，生成实际线速度；实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。

角速度限幅模块，用于对指令生成器所生成的角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

实施例9。

图4为实施例9的框架结构示意图。如图4所示，实施例9包括

前视目标点选择器，用于从目标轨迹中选择前视目标点，计算前视目标点与扫地机位姿的向量差，得到位姿误差；

其中，选择前视目标点的方法是：

记录上一个控制周期所选择的前视目标点；

在当前控制周期中，从上一个控制周期所选择的前视目标点开始依次计算目标轨迹中的各个路径点到扫地机当前位置的欧式距离；

第一个到扫地机当前位置的欧式距离大于前视距离的点作为当前控制周期的前视目标点。

PID控制器，PID控制器为三个独立的PID控制器；位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

指令生成器，将X轴控制量、Y轴控制量合成线速度指令；根据方向角控制量生成角速度指令。

还包括线速度限幅模块和/或角速度限幅模块。

线速度限幅模块，用于对指令生成器所合成的线速度进行限幅，生成实际线速度；实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。

角速度限幅模块，用于对指令生成器所生成的角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

以上描述是对本发明的解释，不是对发明的限定，本发明所限定的范围参见权利要求，在不违背本发明的基本结构的情况下，本发明可以作任何形式的修改。

Claims (10)

1.一种用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，包括：

从目标轨迹中选择前视目标点，计算所述前视目标点与扫地机位姿的向量差，得到位姿误差；

将所述位姿误差传递给PID控制器，生成控制量；所述控制量包括X轴控制量、Y轴控制量和方向角控制量；

将所述X轴控制量、所述Y轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。

2.根据权利要求1所述的用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，从目标轨迹中选择前视目标点的方法是：

记录上一个控制周期所选择的前视目标点；

在当前控制周期中，从上一个控制周期所选择的前视目标点开始依次计算所述目标轨迹中的各个路径点到扫地机当前位置的欧式距离；

第一个到扫地机当前位置的欧式距离大于前视距离的点作为当前控制周期的前视目标点。

3.根据权利要求2所述的用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，当目标轨迹更新后，以新的目标轨迹的第一个路径点作为上一个控制周期的前视目标点。

4.根据权利要求1所述的用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

5.根据权利要求1所述的用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，对所述线速度进行限幅，实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，是根据角度误差d_θ限制之后的扫地机线速度。

6.根据权利要求1所述的用于轨迹跟踪的扫地机前视位姿控制方法，其特征在于，对所述角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

7.一种用于轨迹跟踪的扫地机前视位姿控制系统，其特征在于，包括

前视目标点选择器，用于从目标轨迹中选择前视目标点，计算所述前视目标点与扫地机位姿的向量差，得到位姿误差；

PID控制器，接收所述位姿误差，并生成控制量；所述控制量包括X轴控制量、Y轴控制量和方向角控制量；

指令生成器，将所述X轴控制量、所述Y轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。

8.根据权利要求7所述的用于轨迹跟踪的扫地机前视位姿控制系统，其特征在于，所述PID控制器为三个独立的PID控制器；位姿误差包括X轴坐标误差、Y轴坐标误差和方向角度误差；X轴坐标误差、Y轴坐标误差和方向角度误差分别传递给三个独立的PID控制器，三个独立的PID控制器分别生成X轴控制量、Y轴控制量和方向角控制量。

9.根据权利要求7所述的用于轨迹跟踪的扫地机前视位姿控制系统，其特征在于，还包括线速度限幅模块，对指令生成器所合成的线速度进行限幅，生成实际线速度；实际线速度控制量v_cmd为：

其中，β为线速度衰减因子，v_max为最大线速度，v为所合成的线速度，d_θ是位姿误差中的角度误差，

是根据角度误差d_θ限制之后的扫地机线速度。

10.根据权利要求7所述的用于轨迹跟踪的扫地机前视位姿控制系统，其特征在于，还包括角速度限幅模块，对指令生成器所生成的角速度进行限幅，实际角速度控制量ω_cmd为：

ω_cmd＝min(ω_max,ω)

其中，ω_max为最大角速度，ω为所生成的角速度。

Images