CN114488810A - 机器人末端姿态跟踪预测控制方法及系统和机器人 - Google Patents

Abstract

本发明提供了一种机器人末端姿态跟踪预测控制方法及系统和机器人，包括：建立机器人运动学模型；根据机器人末端运动约束建立状态空间模型；对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围采样时刻状态向量的预测；构建性能指标函数,建立约束优化问题；根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制。本发明通过控制两驱动轮速度以及电动推杆速度进而实现除草机器人的轨迹姿态跟踪控制。本发明的轨迹姿态跟踪控制方法能够实现对除草机器人在崎岖山路下目标轨迹和姿态的稳定跟踪，且跟踪精度良好。

Description

机器人末端姿态跟踪预测控制方法及系统和机器人

技术领域

本发明涉及农业自主机器人控制技术领域，具体地，涉及一种机器人末端姿态跟踪预测控制方法及系统和机器人。

背景技术

在农业果园生产中，杂草由于与果树争夺养分、阳光等，是影响果树生长以及果园产量品质的重要因素之一，当杂草丛生时，果树由于抑制物质的作用生长受到阻碍，容易导致产量下降，质量受损。目前传统果园中常见的除草方式主要有人工除草、化学除草以及机械除草。人工除草效率低、难度高而且危险性较大；化学除草会给果园带来严重的农药污染残留问题；机械除草效率有所提升，但在果园崎岖的山路环境下，传统大田除草机具通过性较差，同时对果树株间的杂草清除性较差。随着部分果园除草机具的出现，果园主要以人工操控除草机械作业为主，但随着果园矮砧密植栽培模式的发展，当前果树种植密度大、分散性大、地形复杂多变等特点对人工和大型机械除草作业都提出了更高的要求，也因此对研发果园自主除草机器人提出了迫切需求，丘陵山区复杂多样的地形地貌对农机具的行走控制稳定性也提出了更高的要求。

自主移动机器人作为一类非线性动力学系统，其非完整约束的特点使得要实现精确控制具备较大的难度。目前主要用于非线性系统控制的方法有滑模控制、Back-Stepping控制、模型预测控制以及智能控制方法等。滑模控制能动态根据当前状态不断进行变化，使得系统沿着预定的“滑动模态”的状态轨迹运动，但在控制过程中会存在“抖振”。Back-Stepping控制是将复杂系统分解成数量不超过系统阶数的子系统，再为每个子系统设计稳定的控制器，同时将子系统的状态坐标作为虚拟控制变量，每个子系统与一个镇定函数相关联，从而最终获得系统的控制器设计，整个设计过程非常复杂繁琐。模型预测控制主要由预测模型、滚动优化、反馈控制三部分组成。适用于具有约束和不确定性的多输入多输出系统，其通过求解每个采样时刻下一个有限时域内的约束优化问题从而得到最优控制序列并取第一个元素作为控制器输出，但预测模型的构建以及约束优化问题的求解在非线性系统中较为困难。智能控制方法不依赖具体确定的数学模型，因此在非线性的控制系统中比较有效，目前主要用于移动机器人控制的方法主要有模糊控制和神经网络控制两种，模糊控制需要专家经验建立模糊规则，当模糊规则不理想时，控制效果会受到直接影响；神经网络控制结构构造缺乏系统化方法，需要进行较长时间训练完成控制任务。

专利文献CN109769419A(申请号：CN201711132807.0)公开了一种智能除草机器人，包括小车及设置在小车上的GPS定位装置、用于识别杂草的摄像机以及除草装置，除草装置包括安装在小车前端用于切除杂草的刀头及用于支撑刀头的支撑臂；小车还设有用于将切除掉的杂草烧掉的烧草装置；小车内设有自动化控制模块，自动化控制模块包括GPS定位模块、图像识别模块、切除草模块以及烧草模块；GPS定位模块、图像识别模块、切除草模块以及烧草模块分别用于控制GPS定位装置、摄像机、除草装置以及烧草装置；自动化控制模块包括控制板以及触摸显示屏。但该发明的轨迹姿态跟踪控制方法在崎岖山路下目标轨迹和姿态的稳定跟踪精度不够，不能适应。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种机器人末端姿态跟踪预测控制方法及系统和机器人。

根据本发明提供的一种机器人末端姿态跟踪预测控制方法，包括：

步骤S1：建立机器人运动学模型；

步骤S2：根据机器人末端运动约束建立状态空间模型；

步骤S3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

步骤S4：确定遗传算法参数；

步骤S5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

步骤S6：以控制向量为决策变量，构建性能指标函数,建立约束优化问题；

步骤S7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复步骤S5-S7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

优选地，在所述步骤S1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述步骤S2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述步骤S3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

优选地，在所述步骤S4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述步骤S5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

优选地，在所述步骤S6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述步骤S7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

根据本发明提供的一种机器人末端姿态跟踪预测控制系统，包括：

模块M1：建立机器人运动学模型；

模块M2：根据机器人末端运动约束建立状态空间模型；

模块M3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

模块M4：确定遗传算法参数；

模块M5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

模块M6：以控制向量为决策变量，构建性能指标函数，建立约束优化问题；

模块M7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复调用模块M5-M7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

优选地，在所述模块M1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述模块M2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述模块M3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

优选地，在所述模块M4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述模块M5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

优选地，在所述模块M6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述模块M7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

根据本发明提供的一种机器人，包括所述的机器人末端姿态跟踪预测控制系统，执行实现任一项所述的机器人末端姿态跟踪预测控制方法的步骤；

电动推杆(1)右端与车身固连；电动推杆(1)左端与末端固连；末端与车身通过轴承铰接位置，末端能够绕铰接位置旋转；

左侧驱动轮(21)为履带主动轮，通过齿轮连接与车身履带啮合；

右侧驱动轮(22)为履带主动轮，通过齿轮连接与车身履带啮合。

优选地，所述机器人运动方式为左侧驱动轮(21)、右侧驱动轮(22)差速驱动方式；

所述机器人末端由电动推杆(1)驱动运动。

与现有技术相比，本发明具有如下的有益效果：

1、本发明的轨迹姿态跟踪控制方法能够实现对除草机器人在崎岖山路下目标轨迹和姿态的稳定跟踪，且跟踪精度良好；

2、本发明实时预测求解迅速，适用性广，控制精度高，鲁棒性较好，实现了在崎岖山路下对自主除草机器人的本体和末端姿态跟踪预测控制方法。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明实施例除草机器人立体图；

图2为本发明实施例除草机器人除草末端装置示意图；

图3为本发明实施例除草机器人俯视示意图；

图中：

1为电动推杆；

a为推杆右端与车身固连处；

b为推杆左端与除草末端固连处；

c为除草末端与车身通过轴承铰接位置处；

21为左侧驱动轮；

22为右侧驱动轮；

d₁为车身与电动推杆1连接处a和c之间的距离；

d₂为c和b之间的距离；

s为电动推杆的长度；

E为除草机器人在坐标系中的位置；

L为左侧驱动轮和右侧驱动轮之间的距离；

θ为除草机器人朝向与x方向夹角。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1：

根据本发明提供的一种机器人末端姿态跟踪预测控制方法，如图1-图3所示，包括：

步骤S1：建立机器人运动学模型；

步骤S2：根据机器人末端运动约束建立状态空间模型；

步骤S3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

步骤S4：确定遗传算法参数；

步骤S5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

步骤S6：以控制向量为决策变量，构建性能指标函数，建立约束优化问题；

步骤S7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复步骤S5-S7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

具体地，在所述步骤S1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述步骤S2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述步骤S3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

具体地，在所述步骤S4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述步骤S5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

具体地，在所述步骤S6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述步骤S7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

本领域技术人员可以将本发明提供的一种机器人末端姿态跟踪预测控制方法，理解为机器人末端姿态跟踪预测控制系统的具体实施方式，即所述机器人末端姿态跟踪预测控制系统可以通过执行所述机器人末端姿态跟踪预测控制方法的步骤流程予以实现。

根据本发明提供的一种机器人末端姿态跟踪预测控制系统，包括：

模块M1：建立机器人运动学模型；

模块M2：根据机器人末端运动约束建立状态空间模型；

模块M3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

模块M4：确定遗传算法参数；

模块M5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

模块M6：以控制向量为决策变量，构建性能指标函数，建立约束优化问题；

模块M7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复调用模块M5-M7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

具体地，在所述模块M1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述模块M2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述模块M3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

具体地，在所述模块M4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述模块M5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

具体地，在所述模块M6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述模块M7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

根据本发明提供的一种机器人，包括所述的机器人末端姿态跟踪预测控制系统，执行实现任一项所述的机器人末端姿态跟踪预测控制方法的步骤；

电动推杆(1)右端与车身固连；电动推杆(1)左端与末端固连；末端与车身通过轴承铰接位置，末端能够绕铰接位置旋转；

左侧驱动轮(21)为履带主动轮，通过齿轮连接与车身履带啮合；

右侧驱动轮(22)为履带主动轮，通过齿轮连接与车身履带啮合。

具体地，所述机器人运动方式为左侧驱动轮(21)、右侧驱动轮(22)差速驱动方式；

所述机器人末端由电动推杆(1)驱动运动。

实施例2：

实施例2为实施例1的优选例，以更为具体地对本发明进行说明。

本发明的目的是提供一种崎岖山路下除草机器人的本体和末端姿态跟踪预测控制方法，发明以模型预测控制为基础，为了解决模型预测控制中非线性预测模型约束优化问题求解这一难点，本发明利用智能优化算法-遗传算法进行优化问题的求解。本发明实时预测求解迅速，适用性广，控制精度高，鲁棒性较好，实现了在崎岖山路下对自主除草机器人的本体和末端姿态跟踪预测控制方法。

本发明提供的一种崎岖山路下除草机器人的本体和末端姿态跟踪预测控制方法包括以下步骤：

S1：根据除草机器人本体模型和双电机差速驱动模型，建立机器人运动学模型。

S2：结合除草机器人末端运动约束，对整体建立运动学方程，以除草机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量

机器人本体位姿和末端位姿为状态变量

建立状态空间模型

x为图3坐标系中除草机器人x方向坐标，y为图3坐标系中除草机器人y方向坐标，θ为图3坐标系中除草机器人朝向与x方向夹角，s为电动推杆(1)长度，

为图2中除草末端与电动推杆连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵。

S3：确定采样时间间隔T，根据S2中状态空间模型进行离散化，

其中I_5×5为维数5×5的单位矩阵。记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)；记(I_5×5+TA(k))为A_k，TB(k)为B_k，则得到以采样时刻k表示的离散状态方程η(k+1)＝A_k·η(k)+B_k·u(k)。

S4：为了之后S7中约束优化问题的求解，根据对系统的要求以及除草机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]，编码方式，种群数量m，终止条件，最大遗传代数n；其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值。

S5：在当前采样时刻k确定除草机器人当前状态η_c(k)和目标状态η_g(k)，以S3中所得到的离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，一定控制时长范围N_c＜N_p，各个采样时刻状态向量的预测H(k)＝M·η(k)+N·U(k)；其中N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵。

S6：以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题。

S7：将性能指标函数ξ(k)，作为适应度函数计算个体适应度，随机产生数量为S4中所述种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足S4中设定的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制。

S8：在下一采样时刻重复步骤S5-S7，实时得到当前时刻下的控制器输出，当除草机器人完成所有给定的目标状态时结束。

图1-3中，1为电动推杆；具体安装细节见图2，在图2中，推杆右端a处与车身固连；左端b处与除草末端固连；c处为除草末端与车身通过轴承铰接位置，即除草末端可绕c处旋转。

21为左侧驱动轮；其为履带主动轮，通过齿轮连接与车身履带啮合；

22为右侧驱动轮；其为履带主动轮，通过齿轮连接与车身履带啮合；

d₁为车身与电动推杆1连接处a和c之间的距离；

d₂为c和b之间的距离；

s为电动推杆的长度；

E代表除草机器人在坐标系中的位置；

L为左侧驱动轮和右侧驱动轮之间的距离；

θ为除草机器人朝向与x方向夹角。

所述机器人运动方式为左侧驱动轮21、右侧驱动轮22差速驱动方式。

所述机器人除草末端由电动推杆1驱动运动。

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维。

所述的遗传算法参数可以根据控制性能要求设定。

可以通过传感器计算获得该时刻机器人的状态变量η_c(k)。

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时可自由选择。

性能指标函数ξ(k)可以根据对控制性能的要求进行自由设定。

实施例3：

实施例3为实施例1的优选例，以更为具体地对本发明进行说明。

本发明提供的一种崎岖山路下除草机器人的本体和末端姿态跟踪预测控制方法包括以下步骤：

可选的，在本发明实施例中，使用GPS和IMU来获取除草机器人当前的姿态，而目标姿态则事先给出设定。

S1中，结合附图3，两履带轮的间距为L，E点坐标为[x，y，θ]，左右驱动轮转速分别为[ω_L，ω_R]，驱动轮半径为r，根据差速驱动运动模型，得到运动学方程如下：

在步骤S2中，除草末端由电动推杆1控制，如附图2所示，除草末端倾角

由电动推杆(1)行程s控制，约束关系如下所示：

结合步骤1中的运动学方程，加入电动推杆速度v为控制变量，得到状态空间模型

中有

A(t)＝0

在步骤S3中，对2中状态方程进行离散化之后得到的离散状态方程η(k+1)＝A_k·η(k)+B_k·u(k)中，采样时间间隔T。

A_k＝I_5×5

其中θ_k、s_k、

分别表示时刻k状态变量η(k)中θ、s、

的值，可选的，在步骤S4中，遗传算法参数可以设定为：控制量的取值范围u(k)∈[u_min，u_max]，采用二进制编码方式对每个控制量进行编码，确定编码长度o，满足(000···00)_o＝u_min，(111…11)o＝u_max；确定控制量种群数量；设定遗传算法最大代数n。设定终止条件为达到最大代数或ξ(k)小于设定值value。

可选的，在步骤S5中，预测模型根据步骤3中离散状态方程推导H(k)＝M·η(k)+N·U(k)，设预测长度为N_p，控制长度为N_c＜N_p，得到预测输出向量如下所示。

可选的，在步骤S6中，建立的性能指标函数如下所示，考虑预测输出和参考输出的差值以及预测控制输出的变化值，其中Q和R为二次项系数矩阵，设为对角矩阵，并给定Q中每个对角元素为q，R中每个对角元素为r，即：

Q＝q·I_5×5

R＝r·I_3×3

I_5×5为维数5×5的单位矩阵，I_3×3为维数3×3的单位矩阵。

可选的，在步骤S7中，使用轮盘赌法进行选择，不进行变异和交叉，得到控制器输出。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种机器人末端姿态跟踪预测控制方法，其特征在于，包括：

步骤S1：建立机器人运动学模型；

步骤S2：根据机器人末端运动约束建立状态空间模型；

步骤S3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

步骤S4：确定遗传算法参数；

步骤S5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

步骤S6：以控制向量为决策变量，构建性能指标函数，建立约束优化问题；

步骤S7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复步骤S5-S7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

2.根据权利要求1所述的机器人末端姿态跟踪预测控制方法，其特征在于：

在所述步骤S1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述步骤S2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述步骤S3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

3.根据权利要求1所述的机器人末端姿态跟踪预测控制方法，其特征在于：

在所述步骤S4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述步骤S5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

4.根据权利要求1所述的机器人末端姿态跟踪预测控制方法，其特征在于：

在所述步骤S6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述步骤S7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

5.一种机器人末端姿态跟踪预测控制系统，其特征在于，包括：

模块M1：建立机器人运动学模型；

模块M2：根据机器人末端运动约束建立状态空间模型；

模块M3：对状态空间模型进行离散化，得到以采样时刻表示的离散状态方程；

模块M4：确定遗传算法参数；

模块M5：以离散状态方程为预测模型，计算在当前时刻下对未来预设时域范围各个采样时刻状态向量的预测；

模块M6：以控制向量为决策变量，构建性能指标函数，建立约束优化问题；

模块M7：根据性能指标函数计算个体适应度，取使得适应度函数最小的个体第一个控制量作为输出进行控制；

进入下一采样时刻，重复调用模块M5-M7，得到下一采样时刻下的控制器输出，当机器人完成所有预设的目标状态时结束。

6.根据权利要求5所述的机器人末端姿态跟踪预测控制系统，其特征在于：

在所述模块M1中：

根据机器人本体模型和双电机差速驱动模型，建立机器人运动学模型；

在所述模块M2中：

结合机器人末端运动约束，建立运动学方程，以机器人左轮驱动速度ω_L，右轮驱动速度ω_R以及末端驱动速度v为控制量u(t)：

机器人本体位姿和末端位姿为状态变量η(t)为：

建立状态空间模型：

x为坐标系中机器人x方向坐标，y为坐标系中机器人y方向坐标，θ为坐标系中机器人朝向与x方向夹角，s为电动推杆(1)长度，

为末端与电动推杆(1)连接所形成的夹角，A(t)为状态空间方程状态变量系数矩阵，t为时间，B(t)为状态空间方程控制变量系数矩阵；

所述的状态空间模型控制量u(t)为3维，状态变量η(t)为5维；

在所述模块M3中：

采样时间间隔为T，状态空间模型进行离散化：

其中I_5×5为维数5×5的单位矩阵；η(t+T)为机器人本体位姿和末端位姿在t+T时刻的状态变量；

记当前采样时刻t为k，下一个采样时刻t+T为k+1，后续采样时刻t+iT为k+i，i＞0，为正整数，有：

η(k+1)＝(I_5×5+TA(k))η(k)+TB(k)·u(k)

记(I_5×5+TA(k))为A_k，TB(k)为B_k，得到以采样时刻k表示的离散状态方程：

η(k+1)＝A_k·η(k)+B_k·u(k)。

7.根据权利要求5所述的机器人末端姿态跟踪预测控制系统，其特征在于：

在所述模块M4中：

为了约束优化问题的求解，根据对系统的要求以及机器人自身的性能，确定使用的遗传算法参数，包括控制量取值范围u(k)∈[u_min，u_max]、编码方式、种群数量m、终止条件、最大遗传代数n；

其中u_min为

中各变量能取的最小值，u_max为

中各变量能取的最大值；

所述的遗传算法参数根据控制性能要求设定；

在所述模块M5中：

在当前采样时刻k确定机器人当前状态η_c(k)和目标状态η_g(k)，以离散状态方程为预测模型计算在当前采样时刻k对未来一定时刻范围N_p内，预设控制时长N_c，满足N_c＜N_p时各个采样时刻状态向量的预测：

H(k)＝M·η(k)+N·U(k)

其中，N_p，N_c为正整数，H(k)为当前采样时刻k下，之后各个预测时刻i状态变量值η(k+i)组成的列向量，记为

U(k)为包括当前采样时刻k下，之后小于控制时长范围各个时刻i控制变量u(k+i)组成的列向量，记为

M，N为系数矩阵；η(k)为机器人本体位姿和末端位姿在k时刻的状态变量；通过传感器计算获得预设时刻机器人的状态变量；

所述的预测时长N_p，控制时长N_c在满足N_c＜N_p时能够自由选择。

8.根据权利要求5所述的机器人末端姿态跟踪预测控制系统，其特征在于：

在所述模块M6中：

以控制向量U(k)为决策变量，构建性能指标函数ξ(k)，建立约束优化问题；

在所述模块M7中：

将性能指标函数ξ(k)作为适应度函数计算个体适应度，随机产生数量为种群数量m的不同个体U(k)，进行遗传算法的选择、交叉和变异操作，当计算过程中存在U(k)满足预设的终止条件时，取使得适应度函数ξ(k)最小的个体U(k)作为最终最佳个体，取个体U(k)第一个控制量u(k|k)作为输出进行控制；

性能指标函数ξ(k)根据对控制性能的要求进行自由设定。

9.一种机器人，其特征在于，包括权利要求5-8任一项所述的机器人末端姿态跟踪预测控制系统，执行实现权利要求1-4中任一项所述的机器人末端姿态跟踪预测控制方法的步骤；

电动推杆(1)右端与车身固连；电动推杆(1)左端与末端固连；末端与车身通过轴承铰接位置，末端能够绕铰接位置旋转；

左侧驱动轮(21)为履带主动轮，通过齿轮连接与车身履带啮合；

右侧驱动轮(22)为履带主动轮，通过齿轮连接与车身履带啮合。

10.根据权利要求9所述的一种机器人，其特征在于：

所述机器人运动方式为左侧驱动轮(21)、右侧驱动轮(22)差速驱动方式；

所述机器人末端由电动推杆(1)驱动运动。

Images