(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 20221019140 6.7 (22)申请日 2022.02.28 (65)同一申请的已公布的文献号 申请公布号 CN 114568108 A (43)申请公布日 2022.06.03 (73)专利权人 清华大学深圳国际研究生院 地址 518055 广东省深圳市南 山区西丽 街 道深圳大 学城清华校区A栋二楼 (72)发明人 张凯　安昊　董宇涵　 (74)专利代理 机构 深圳新创友知识产权代理有 限公司 4 4223 专利代理师 孟学英 (51)Int.Cl. A01D 34/00(2006.01) G06T 7/136(2017.01) G06T 7/13(2017.01)G06T 5/30(2006.01) G06V 10/762(2022.01) G06V 10/82(2022.01) G06V 10/56(2022.01) G06K 9/62(2022.01) G06N 3/04(2006.01) G06N 3/08(2006.01) (56)对比文件 CN 109673242 A,2019.04.26 US 20210 64043 A1,2021.0 3.04 US 2020042009 A1,2020.02.0 6 CN 107509443 A,2017.12.26 CN 208242 224 U,2018.12.18 CN 213424010 U,2021.0 6.11 CN 205912478 U,2017.02.01 CN 111413966 A,2020.07.14 审查员 梅婷 (54)发明名称 一种无人割草机轨迹跟踪控制方法及计算 机可读存 储介质 (57)摘要 本发明提供一种无人割草机轨迹跟踪控制 方法及计算机可读存储介质， 方法中包括： 无人 割草机运行前需进行系统模型的建立和全区域 覆盖的路径 规划， 通过最优化求解得到当前时刻 的最优控制序列， 将其输入到模 型预测控制器执 行控制指令， 基于当前时刻的状态 量观测值反馈 未来时刻的状态量， 如此循环， 实现全局轨迹跟 踪控制； 加入适用于无人割草机的轨迹重规划 层， 在满足无人割草机的约束条件下， 基于图像 信息处理获取无人割草机作业草坪边界的准确 信息， 设计基于草坪边界信息的模 型预测控制器 输入量， 同时避 开草坪中障碍物， 减少跟踪偏差， 将重规划的局部路径输入跟踪控制层进行跟踪 控制， 提高无人割草机轨迹跟踪控制的精度、 效 率和安全性。 权利要求书2页 说明书11页 附图6页 CN 114568108 B 2022.11.11 CN 114568108 B 1.一种无 人割草机 轨迹跟踪控制方法， 其特 征在于， 包括如下步骤： S1： 根据无人割草机运行的实际工作环境规划全工作区域覆盖的参考路径轨迹并对所 述参考路径轨 迹进行离 散化获取参 考轨迹点； S2： 基于所述无人割草机的所述实际工作环境和状态建立系统模型， 所述系统模型包 括运动学模型、 动力学模型以及轮胎驱动模型； S3： 基于所述无人割草机的所述系统模型搭建模型预测控制器， 所述模型预测控制器 控制无人割草机的线速度、 航向角； S4： 根据相对定位及绝对定位信息， 在所述模型预测控制器的基础上设计轨迹重规划 层， 通过所述轨迹重规划层得到局部规划路径信息； S5： 通过机载摄像机获取所述无人割草机行驶过程中的图像， 通过阈值分割提取所述 实际工作环境的草坪边界线， 确定所述无人割草机的质心距离所述草坪边界线的最短距 离， 通过调整 所述无人割草机的线速度与航向角来控制所述最短距离不小于预先设定的安 全阈值； S6： 基于所述图像识别出所述实际工作环境中障碍物类型， 通过预先设计的惩罚函数 进行避障； S7： 处理所述轨迹重规划层中所述局部规划路径信息， 将所述轨迹重规划层和所述模 型预测控制器的跟踪控制层对接； S8： 结合所述系统模型、 所述模型预测控制器的目标函数和约束条件进行最优化求解， 输出当前时刻的最优控制序列， 将所述最优控制序列输入到所述模型预测控制器的所述跟 踪控制层中执行控制指令； 基于当前时刻的状态量估计未来时刻无法直接观测的状态量， 再次进行最优化求解， 得到下一时刻所述模型预测控制器的所述跟踪控制层所需的控制变 量， 如此循环。 2.如权利要求1所述的无人割草机轨迹跟踪控制方法， 其特征在于， 步骤S1中利用路径 最短的优化 准则规划所述 参考路径轨 迹， 采用方法为螺 旋式规划法或折线往复式规划法。 3.如权利要求2所述的无人割草机轨迹跟踪控制方法， 其特征在于， 步骤S3 中对所述无 人割草机的系统模型逐步进行线性化、 离散化后可得到适用于所述模型预测控制器实时求 解的离散线性时变系统状态空间， 通过设计所述模型预测控制器的目标函数， 求解二次规 划问题， 获取最优控制增量序列并将序列作为实际的控制增量作用于所述模型预测控制器 中从而控制所述无 人割草机的线速度和航向角。 4.如权利要求3所述的无人割草机轨迹跟踪控制方法， 其特征在于， 通过所述轨迹重规 划层得到局部规划路径信息包括： 所述轨迹重规划层根据传感器获得的障碍物信息并结合所述参考路径轨迹重新规划 出绕开障碍物的局部规划路径信息； 将所述局部规划路径信息 输入到所述跟踪控制层中； 根据所述无人割草机的所述实 际工作环境分别用于草地边缘检测、 草地内避 障、 避障 后重回参 考轨迹。 5.如权利要求4所述的无人割草机轨迹跟踪控制方法， 其特征在于， 步骤S5 中通过所述 机载摄像机获取所述无人割草机行驶过程中的图像， 通过阈值分割提取草坪边界线信息包 括：权　利　要　求　书 1/2 页 2 CN 114568108 B 2利用所述机载摄像机获取所述无人割草机行驶过程中的所述图像， 通过对所述图像进 行阈值分割， 利用聚类算法对色彩进行分类并对绿色进行提取和 二值化， 再经过可变结构 元素膨胀腐蚀运 算， 最后获取草 坪区域与非草 坪区域的所述 草坪边界线。 6.如权利要求5所述的无 人割草机 轨迹跟踪控制方法， 其特 征在于， 还 包括： 通过寻找极值 点将所述 草坪边界线拟合成草 坪边界直线， 具体步骤如下： 提取所述草坪边界线的边缘点坐标： 对所述图像从左到右、 从上至下进行扫描， 每碰到 第一行第一个 像素为1的像素点， 进行记录； 找出极小值点： 对记录的边界点进行比较， 选择对应图像坐标系下横坐标最小的多个 极值点； 回归所述草坪边界直线： 采用最小二乘法将所述极值点进行回归， 输出所述草坪边界 直线。 7.如权利要求5所述的无人割草机轨迹跟踪控制方法， 其特征在于， 步骤S6 中基于所述 图像识别出 所述实际工作环境中障碍物类型包括： 利用目标识别算法对所述实际工作环境的草坪上的物体进行聚类识别， 经过特征提取 后输入到深度卷积神经网络分类 器中进行训练； 调整训练参数， 输出 特征物体标签得到所述障碍物类型。 8.如权利要求7所述的无人割草机轨迹跟踪控制方法， 其特征在于， 所述惩罚函数表示 为： 其中， vx,vy分别为无人割草机在给定坐标系下的x轴向和y轴向速度； xi,yi分别为参考 路径轨迹的目标点在给定坐标系下的x,y轴坐标； x0,y0分别为所述无人割草机的质心在给 定坐标系下的x,y轴坐标； α 为防止分母为0的正数。 9.如权利要求8所述的无人割草机轨迹跟踪控制方法， 其特征在于， 步骤S7中利用多项 式曲线拟合的方式对重新规划的预测时域内的离散点进行处理将所述轨迹重规划层和所 述模型预测控制器的跟踪控制层对接 。 10.一种计算机可读存储介质， 所述计算机可读存储介质存储有计算机程序， 其特征在 于， 所述计算机程序被处 理器执行时实现如权利要求1 ‑9任一所述方法的步骤。权　利　要　求　书 2/2 页 3 CN 114568108 B 3