[发明专利]一种基于扫刷误差的二自由度动力模型沿边清扫控制方法

权利要求书

1.一种基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述方法包括：

获取车辆作业速度范围并建立扫刷误差模型；

根据所述车辆作业速度范围以及所述扫刷误差模型制作离线的线性二次调节器控制表；

获取清扫车的状态信息以及贴边清扫的运动轨迹；

根据所述清扫车的状态信息以及贴边清扫的运动轨迹计算匹配点以及投影点；

根据所述投影点以及当前车身位姿计算误差变化量；

根据车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。

2.根据权利要求1所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述误差变化量为根据所述投影点以及当前车身位姿计算出的车辆前轮扫刷中心到所述投影点的横向误差的误差变化量和根据所述投影点以及当前车身位姿计算出的车辆质心到所述投影点的航向误差的误差变化量。

3.根据权利要求1所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述建立扫刷误差模型具体包括：

获取车辆质心到所述投影点的横向误差以及车辆质心到所述投影点的航向误差；

建立以车辆质心为控制点的二自由度模型；

根据前扫刷误差公式获取车辆的前轮扫刷中心到所述投影点的横向误差；

联立所述二自由度模型，推导得所述扫刷误差模型。

4.根据权利要求2所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，根据所述投影点以及当前车身位姿计算误差变化量之后，所述方法还包括：

判断所述横向误差的误差变化量是否异常，并判断所述航向误差的误差变化量是否异常。

5.根据权利要求4所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述判断所述横向误差的误差变化量是否异常具体包括：

设定一第一阈值，如若所述横向误差的误差变化量小于所设定的第一阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；

如若所述横向误差的误差变化量大于所设定的第一阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。

6.根据权利要求4所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述判断所述航向误差的误差变化量是否异常具体包括：

设定一第二阈值，如若所述航向误差的误差变化量小于所设定的第二阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；

如若所述航向误差的误差变化量大于所设定的第二阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。

7.根据权利要求5或6所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述的认为定位程序异常，误差异常计数器加一方法之后，所述方法还包括：

判断所述误差异常计数器的数值是否异常，同时设定一第三阈值，如若所述误差异常计数器的数值超过所设定的第三阈值，则控制车辆退出清扫作业模式，停车进入保护模式；

如若所述误差异常计数器的数值不超过所设定的第三阈值，则根据计算出的所述当前车辆位姿计算当前帧的横向误差及其导数和航向误差及其导数，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。

8.根据权利要求7所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量的方法之后，所述方法还包括：

计算前馈控制量，并获得当前前轮最优控制量，并将所述当前前轮最优控制量作为控制模块输出给底盘执行，所述当前前轮最优控制量为所述反馈控制量与所述前馈控制量之和。