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

权利要求书

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为最大角速度，ω为所生成的角速度。