[发明专利]用于轨迹跟踪的扫地机前视位姿控制方法和系统

说明书

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

技术领域

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

背景技术

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

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

专利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轴控制量合成线速度指令；根据所述方向角控制量生成角速度指令。