阅读说明：本技术 一种杆状作物收割机器人控制方法 (Control method of rod-shaped crop harvesting robot ) 是由 陈刚 李超 李涛 王卓麟 张筱婕 于 2020-08-17 设计创作，主要内容包括：本发明涉及一种杆状作物收割机器人控制方法,属于农业自动化领域。包括杆状作物的自主识别、机器人控制方法、设计夹持机构、设计捆缚机构、设计运送机构和多电机检测故障诊断。设计了一种机器人的整体硬件结构,在保持成本低廉的前提前下,解决了杆状作物的收割问题,通过对算法的优化和多传感器的融合使系统更稳定且精准。设计了一种卷积神经网络对目标进行识别,并使用简单的识别算法进行识别以提高实时性,并对两种算法得到的数据进行数据融合,得到一致性解释。本发明使用一种对超声波数据进行优化的算法,在低成本的超声波上通过算法优化得到更为准确的数据。并使用一种自适应加权融合估计算法,优化数据得到对环境的一致性解释。(The invention relates to a control method of a rod-shaped crop harvesting robot, belonging to the field of agricultural automation. The method comprises the steps of autonomous identification of the rod-shaped crops, a robot control method, design of a clamping mechanism, design of a binding mechanism, design of a conveying mechanism and multi-motor detection fault diagnosis. The whole hardware structure of the robot is designed, the harvesting problem of the rod-shaped crops is solved in advance before the cost is kept low, and the system is more stable and accurate through the optimization of an algorithm and the fusion of multiple sensors. A convolutional neural network is designed to identify a target, a simple identification algorithm is used for identifying so as to improve the real-time performance, and data obtained by the two algorithms are subjected to data fusion to obtain a consistency explanation. The invention uses an algorithm for optimizing ultrasonic data, and obtains more accurate data through algorithm optimization on the ultrasonic with low cost. And a self-adaptive weighting fusion estimation algorithm is used, and the data is optimized to obtain the consistency explanation of the environment.)

一种杆状作物收割机器人控制方法

技术领域

本发明属于农业自动化领域，涉及一种杆状作物收割机器人控制方法。

背景技术

农业机器人根本上还是属于机器人的范畴，不仅仅是机械结构和控制算法的简单融合，而且还涉及了多传感器融合技术，信息处理技术，自动控制技术、计算机技术、多系统控制等诸多技术。是一个综合交叉学科。机器人系统的一般结构包括机械结构、感知系统、控制系统三部分；包含驱动系统、机械结构系统、感受系统、机器人-环境交互系统、人机交互系统、控制系统六大子系统。

农业机器人与工业机器人的应用是存在较大区别的及农业机器人的特点：

(1)工业机器人操作的对象一般比较确定、规则、可知性较大，应用环境也是确定的，具备机械性持续重复操作的可能性，且操作的目标物体一般不宜损伤。而农业机器人操作的对象一般更不规则，应用环境也更复杂多变，不具备机械性持续重复操作的条件，且操作的目标物容易被机械机构损伤，且这种损伤是不可逆的。所以相对于工业机器人，农业机器人一定程度上更为复杂。

(2)工业机器人可以机械重复操作，在一定程度上说，其不需要传感器做支撑。但农业机器人由于环境的复杂性，和目标位置的不确定性，就必须具备一定的感知能力。所以仅从这两方面说，农业机器人想做到像工业机器人一样普及和大范围投入使用是更加困难的。

(3)工业机器人的操作人员一般是工科出身的技术人员或者是经过系统技术培训的人员，是有一定的机电操作基础的人员，而农业机器人所应用的领域就注定了操作人员是农民，他们在农业种植方面具备丰富的经验，但却缺乏相应的技术操作经验，所以这就需要在研发农业机器人的时候更多的要考虑到人机交互的问题，人机交互要更简洁易懂。

一方面是技术的复杂度所造成的壁垒；另一方面是其投入和产出不协调导致的经济壁垒；最后还有对人机交互较高的需求。所以本发明需要在前两方面做出协调平衡，在最后一方面深入研究。在保证功能的基础上，需要尽可能的降低硬件成本，在功能与成本之上还要完善人机交互。

发明内容

有鉴于此，本发明的目的在于提供一种杆状作物收割机器人控制方法。

为达到上述目的，本发明提供如下技术方案：

一种杆状作物收割机器人控制方法，包括杆状作物的自主识别、机器人控制方法、设计夹持机构、设计捆缚机构、设计运送机构和多电机检测故障诊断。

可选的，所述杆状作物的自主识别为三个部分：

第一部分：

(1)采集目标杆状作物的图像，构建杆状作物的数据集，对数据集进行初步的处理和标注，按照3：1：1的比例准备训练集、验证集和测试集；

(2)选择包括10个卷积层和5个全连接层的神经网络，通过卷积运算对目标进行层次逐步升高的特征提取；采用迁移学习的方法，将卷积神经网络的最后一层去掉，在不同的数据集上重新训练模型的最后一层，通俗的说就是重新训练模型的最后一层来识别不同的高级特征，减少对不同杆状作物的训练时间；

(3)使用模拟退火算法，调整学习率，使训练刚开始时有较大的学习率，随着时间的推移降低学习率，以保证模型能够找到最优值；

第二部分：

设计一种简单的识别算法，以弥补在成本受限情况下，深度卷积神经网络识别算法无法做到实时检测的缺陷；采用自适应光照的色块分割方法，降低阳光的影响；

第三部分：

(1)一种针对超声波测距的滤波处理算法：

a、连续采集8个点；

b、记录能成功采集到的点数m；

c、若m>2，使用冒泡法对m个点进行排序，否则数据有误调a继续执行循环；

d、去除相邻一对值的两两之差，并记录其中差值最大的一组位置

e、最大组若位于前半部分进行下一步，否则数据有误调a继续执行循环；

f、取该位置后半部分的值为有效值；

g、对有效值取平均值为最终值；

h、结束或返回a继续循环；

(2)一种多传感器融合的算法：

采用分布式的多传感器融合体系结构，逐一将传感器数据纳入到系统中进行逐步融合，得到最终的结果；采用多传感器融合算法，对两种数据进行融合；然后将融合后的数据看作一种数据，继续加入其他数据进行融合，直至输出最终结果；

自适应加权融合估计算法：基于总均方误差最小的前提下，依据每个传感器实时测量到的数据，动态为各传感器分配所对应的最优加权因子W_i(i＝1,2,...n)，其中传感器的方差越大，对应所分配的权值越小，以求得最终的估计值贴近真实值Z；假设：采用n个传感器测量某事物的同一特性参数，σ₁,σ₂,σ₃,...,σ_n是n个传感器的方差，测量的理论值为Z，每个传感器的检测数据为Z_i，i＝1,2,...n，各检测数据彼此相互独立且是Z的无偏估计，估计后的参数就是最终所求的融合结果；其中和W_i必须满足：

由上式推导出总均方误差σ²为：

根据多元函数求极值理论，求得在总均方误差σ²最小的条件，各传感器所对应的权值为W_i(i＝1,2,...n)，当方差越小时，其对应的权值越大；最小总均方误差σ²为：

其对应的每个传感器的加权因子W_i为：

通过(1.5)式对多个传感器相应权值W_i的计算，代入式(1.7)，进而得到最终融合结果并对融合结果进行评估，判断其是否可信；

和Z_t分别是融合后的传感器的检测数据和理论最优值，假设

服从高斯分布，那么

是其概率分布曲线且可作为各传感器的特性函数，σ_i是其方差，引用e用以反映

和理论最优值之间的偏差；

设置可信上限η，取可信系数δ的值为：

当δ_it＝1，融合结果

得到理论最优值Z_t的支持，融合结果可信，反之，δ_ij＝0，融合结果被理论最优值Z_t否定，融合结果不可信；若可信则采纳数据融合结果，若不可信则考虑使用单一传感器的数据，同理按一定顺序对单一传感器数据进行评估，找到偏差最小的传感器，将其作为最优检测数据使用。

可选的，所述机器人控制方法为：

一种状态反馈优化控制器设计，所设计的控制器是一种通用型控制器，并非只针对静态目标，亦适用于动态目标物；

机器人的动态模型为：

通过机器人的感知系统观测目标的状态和位置，设计状态观测器为：

局部误差为

e_i＝x_i-ω_i1 (2.3)

利用式(2.1)和(2.3)，得局部跟踪误差e_i的动态模型为

性能函数式为

控制目标为：对于机器人，设计控制器u_i使：

1)局部跟踪误差e_i动态模型(2.4)渐近稳定；

2)性能函数V_i(x_i,ω_i1,u_i)(2.5)达到最小值；

定义由局部跟踪误差e_i和待跟踪状态ω_i1组成的增广系统状态

由式(2.2)和(2.4)得增广系统动态模型为

其中

基于增广系统动态模型(2.7)，性能函数(2.5)写为

其中

式(2.9)对时间t求导得下列贝尔曼方程

其中，令V_i ^*为贝尔曼方程(2.11)的最优解，则由最优性必要条件得最优控制器为

将式(2.12)带入贝尔曼方程(2.11)得如下跟踪HamiltonJacobiBellman，即HJB方程

若式(2.13)存在一个连续半正定解V_i ^*，则由式(2.12)得到的最优控制器使性能函数(2.9)收敛到最小值V_i ^*(X_i(0))；若式(2.13)存在一个连续半正定解V_i ^*，则由式(2.12)得到的最优控制器

使局部跟踪误差系统(2.4)渐近稳定；

HJB方程为：

初始化：给定一个可行的控制器

步骤1：策略评估，对于给定的利用下式解出

步骤2：策略改进，利用下式更新控制器：

步骤3：令

返回步骤1，直到

收敛到最小值，

由式(2.15)得到的控制器

使V_i ^j+1≤V_i ^j，其中V_i ^j满足式(2.14)；性能函数最终收敛到其最小值，即

由式(2.15)得到的控制器

使增广系统(2.7)渐近稳定，即使局部跟踪误差系统(2.4)渐进稳定。

可选的，所述设计夹持机构为：

a)夹持机构的前半部分为软体材质，最末端为球形结构；

b)夹持机构的后半部分为硬质材质，内部贴有橡胶垫，以防止机构对杆状作物的损伤；

c)前半部分与后半部分的衔接处以及后半部分的弯折处均为可旋转轴；其中两者都是受力转轴，即只有在受到一定力的情况下才会转动，其中后者力大于前者力；

d)整个夹持机构都被设计为上下可调的，以适应不同高度的杆状作物的收割。

可选的，所述设计捆缚机构为：

a)两侧各个有一个辅助转轮，右侧为1号转轮，为主动轮，带有主动旋转机构，收紧捆缚绳，左侧为从动轮，不能主动旋转，带有急停机构，能停止其转动；

b)在夹持机构上有辅助捆缚的结构，捆缚机构收紧是在夹持机构前半部分的辅助下向外收紧；

c)在夹持机构的合紧位置，即捆缚绳在夹持机构上缠绕的位置带有加热器，在捆缚绳收紧后加热熔断捆缚绳。

可选的，所述设计多电机检测故障诊断为：

1)编码器的故障分析

避免由于编码器的严重故障或完全失灵而导致故障，进而提高系统的鲁棒性和安全性；

2)多电机检测故障诊断系统策略

基于主元分析法，利用数控系统中的编码器信号与速度信号之间的上述相关性，将反映此相关性的数据协方差矩阵向其变化最大的方向投影，并通过此投影的长度来监测编码器的工作情况；

3)基于主元分析法和可信度判断的多电机检测故障诊断算法

系统中有m个电机需要诊断，首先是采集信息，包括编码器信号和速度信号，为了表征其相关性，对数据进行实时的处理，对位置信号做前向差分处理，表示为q∈R^m和w∈R^m，并假设采集n次数据后进入诊断过程，根据实际经验取n＞＞2m；得到数据矩阵F∈R^2mn：

其中，q_i和w_i，i＝1,...,n为第i次采样值；利用概率理论中的协方差矩阵(M∈R^2m)表征两列信号的相关性，其为对称正定矩阵；

由矩阵理论得，对于任意2m×2m正定对称的矩阵M，其特征值为λ₁≥λ₂≥...λ_2m，a_i为对应λ_i的特征矢量；F矩阵的最大的变化方向即为特征矢量a_i的方向；设L为最大投影方向上的单位矢量a_i张成的一条直线，并设每个转轴上，即捆缚机构中的辅助转轮1的转轴、带动机器人履带转动的转轴、带动切割机构转动的转轴上的测量或差分值得到的向量v＝<F>，运算<F>为数据矩阵F各列的期望；协方差矩阵向此子空间的投影定义为

数据在此方向上的分散程度定义为：

其中，表示数据v向u_i'张成的子空间上的投影，算子<·>表示求取数据的期望，∑_vv＝<vv^T>＝F表示数据矩阵的协方差矩阵，即得到分散程度最大值；

将所测数据矩阵期望v沿M的最大特征值对应的特征向量方向投影，并通过对此投影结果求解范数，得到最大的变化量；此变化量便对应主元分析法中的主元，用以表征各列信号之间的相关性；致此得到故障判断的第一个标准；

其次引入检测值与理论最优值的关系，是用可信度作为第二个标准进行评价其是否为故障；主元分析法是将编码信号和速度信号作为信息依据，使用速度信号作为信息依据；

V_i和V_t分别是第i个编码器检测到的速度数据和速度的理论最优值，V_i服从高斯分布，是其概率分布曲线且作为各传感器的特性函数，v_i为V_i的一次观测值，σ_i是其方差，引用Δa_it用以反映v_i和理论最优值之间的偏差；

假设最优数据集有M个数据，则由Δa_ij组成的置信矩阵T_m为：

设置可信上限η_it，取可信系数δ_it的值为：

当δ_it＝1，表示第i个传感器更接近理论最优值，传感器更可信，反之，δ_ij＝0，则第i个传感器原理理论最优值，传感器更不可信；对N个传感器进行评价后，参数精度明显高于单个传感器；通过关系矩阵R_m可找到可信度最高的传感器，测量的数据定义为最优测量值，在多个相同传感器的情况下可用最优测量值代替被剔除的异常数据，最后进行动态加权融合估计；关系矩阵R_m如下

即关系矩阵R_m的第i列之和为第i个传感器的可信度；传感器可信度的计算公式为：

至此得到故障判断的第二个标准；根据系统噪声统计特点，包括期望和方差设置可信度阈值ξ_i和诊断阈值ML，根据数据分析结果与此值的比较，判断是否满足下面的条件：

如果符合上述条件，即输出超过设定的阈值，说明故障发生，实时记录时刻并采取相应的故障应对测试或进行与预警；反之则表明编码器正常。

本发明的有益效果在于：

1、设计了一种机器人的整体硬件结构，在保持成本低廉的前提前下，解决了杆状作物的收割问题，通过对算法的优化和多传感器的融合使系统更稳定且精准。

2、设计了一种卷积神经网络对目标进行识别，并使用简单的识别算法进行识别以提高实时性，并对两种算法得到的数据进行数据融合，得到一致性解释

3、本发明使用一种对超声波数据进行优化的算法，在低成本的超声波上通过算法优化得到更为准确的数据。并使用一种自适应加权融合估计算法，优化数据得到对环境的一致性解释。

4、设计了一种通用的状态反馈优化控制器。本控制器对静态或者动态目标进行观测，并通过观测结果的反馈完成设计了一种优良且稳定的控制器。

5、设计了一种自主的本身不耗能的用于杆状作物夹持的夹持机构，设计了一种杆状作物捆缚的捆缚机构，设计了一种用于捆缚完成后杆状作物的运送机构。

6、由于本发明中设计的硬件系统中包含多个电机，且有切割机构，考虑到安全问题，应用了一种多电机检测故障诊断算法，以增加整个系统的安全系数。

7、本发明具备一定的参照价值，不仅仅对于农作物，对于较高的杆状杂草等都可以在此发明的基础上改进完成设计。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为卷积神经网络设计流程图；

图2为简单识别算法设计流程图；

图3为超声波算法设计流程图；

图4为机器人硬件系统及各部分设计图缩略图；

图5为机器人内部硬件设计图；

图6为机器人外部整体侧视图；

图7为机器人外部整体俯视图；

图8为捆缚机构结构图；

图9为切割机构结构图；

图10为本发明中分布式多传感器融合过程图；

图11为本发明中多传感器融合算法流程图；

图12为多电机检测故障诊断系统流程图；

图13为系统整体流程图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明针对杆状作物的收割问题，搭建了一种杆状作物收割机器人平台，集合了感知、运动控制、夹持、收割、运送等子系统。使机器人能够根据所给出的目标杆状作物，自主的完成作物识别、机器人运动和对作物的收割任务，将人从复杂恶劣的收割环境中解放出来。同时，为了保证系统的安全稳定运行，设计了一种电机检测故障诊断方法。

图1为卷积神经网络设计流程图；图2为简单识别算法设计流程图；图3为超声波算法设计流程图；图4为机器人硬件系统及各部分设计图缩略图；图5为机器人内部硬件设计图；图6为机器人外部整体侧视图；图7为机器人外部整体俯视图；图8为捆缚机构结构图；图9为切割机构结构图；图10为本发明中分布式多传感器融合过程图；图11为本发明中多传感器融合算法流程图；图12为多电机检测故障诊断系统流程图；图13为系统整体流程图。

为了达到上述目的。本发明包括以下几个技术环节：

一、杆状作物的自主识别

自主识别分为三个部分：

第一部分

设计一种基于深度卷积神经网络的针对某种确定杆状作物的识别算法，通过对超参数的调节和优化得到一个最优的识别模型。

(1)采集目标杆状作物的图像，构建杆状作物的数据集，对数据集进行初步的处理和标注，按照3：1：1的比例准备训练集、验证集和测试集。

(2)由于识别物体较为简单，且并非对单个作物进行识别，而是对集中作物进行集中识别，所以不需要非常多层的神经网络，故选择包括10个卷积层和5个全连接层的神经网络，通过卷积运算对目标进行层次逐步升高的特征提取。考虑到杆状作物的特征重合点是非常多的，且卷积神经网络中最后几层通常专门针对输入数据；另一方面，前面的层则更为通用，主要在一个大类中找到简单的模型。故我们采用迁移学习的方法，将卷积神经网络的最后一层去掉，在不同的数据集上重新训练模型的最后一层，通俗的说就是重新训练模型的最后一层来识别不同的高级特征。可以大大的减少对不同杆状作物的训练时间，而且对于数据量的要求也会相对更小。

(3)学习率对于神经网络是非常重要的，他控制着基于损失梯度调整神经网络权值的速度，学习率越小那么梯度下降就越慢，收敛的就会越慢，直观上说，训练的速度就会耗费更长的时间和算力；但是如果梯度过大又会容易跨过最优值或者在最优值周围波动，导致无法收敛到最优；所以在本发明中使用模拟退火算法，来自主的调整学习率，使训练刚开始时有较大的学习率，可以快速的下降，但随着时间的推移降低学习率，以保证模型能够找到最优值，即保证了训练的速度，也避免了局部最优的问题。

第二部分

设计一种简单的识别算法，以弥补在成本受限情况下，深度卷积神经网络识别算法无法做到实时检测的缺陷。采用色块分割算法，由于杆状作物颜色比较单一，且所处环境也较为单一，所以采用色块分割也能达到预想的效果。同时根据光线的变化，为了避免光照造成的干扰和噪声，采用自适应光照的色块分割方法，尽量把阳光的影响降到最低。

第三部分

是本发明中使用的一种超声波滤波的方法和多传感器融合的算法，以此得到杆状目标作物的三维空间位置信息。本发明为了降低成本采用价格相对低廉的超声波传感器，超声波测距会有不定期产生错误数据的特性，这是传感器本身的误差，本发明对超声波的数据进行两次处理，增加超声波测距的精确性。

(1)提出一种针对超声波测距的滤波处理算法：

a、连续采集8个点；

b、记录能成功采集到的点数m；

c、若m>2，使用冒泡法对m个点进行排序，否则数据有误调a继续执行循环；

d、去除相邻一对值的两两之差，并记录其中差值最大的一组位置

e、最大组若位于前半部分进行下一步，否则数据有误调a继续执行循环；

f、取该位置后半部分的值为有效值；

g、对有效值取平均值为最终值；

h、结束或返回a继续循环；

(2)一种多传感器融合的算法：

本发明采用分布式的多传感器融合体系结构，逐一将传感器数据纳入到系统中进行逐步融合，得到最终的结果。为此本发明采用了如下的多传感器融合算法，对两种数据进行融合。然后将融合后的数据看作一种数据，继续加入其他数据进行融合，直至输出最终结果。

第一步，进行图像捕捉，将捕捉到的图像分别进行处理，一方面直接对图像使用色块分割算法进行处理，得出目标在二维平面中的位置，另一方面将图像送入训练好的卷积神经网络模型进行识别，同样输出目标在二维平面中的位置结果，然后应用本发明中提出的融合算法对数据进行初步融合。第二步，使用超声波进行测距，使用本发明中设计的超声波滤波算法对超声波得到的数据进行处理，得到处理后的数据，然后与前面融合后的更精确的二维平面位置数据进一步融合得到目标在三维空间中的位置。

自适应加权融合估计算法：基于总均方误差最小的前提下，依据每个传感器实时测量到的数据，动态为各传感器分配所对应的最优加权因子W_i(i＝1,2,...n)，其中传感器的方差越大，对应所分配的权值越小，以求得最终的估计值

贴近真实值Z。假设：采用n个传感器测量某事物的同一特性参数，σ₁,σ₂,σ₃,...,σ_n是n个传感器的方差，测量的理论值为Z，每个传感器的检测数据为Z_i(i＝1,2,...n)，各检测数据彼此相互独立且是Z的无偏估计，估计后的参数就是最终所求的融合结果。其中

和W_i必须满足：

由上式推导出总均方误差σ²为：

根据多元函数求极值理论，求得在总均方误差σ²最小的条件，各传感器所对应的权值为W_i(i＝1,2,...n)，当方差越小时，其对应的权值越大。最小总均方误差σ²为：

其对应的每个传感器的加权因子W_i为：

通过(1.5)式对多个传感器相应权值W_i的计算，代入式(1.7)，进而可得到最终融合结果

并对融合结果进行评估，判断其是否可信。

和Z_t分别是融合后的传感器的检测数据和理论最优值，假设服从高斯分布，那么

是其概率分布曲线且可作为各传感器的特性函数，σ_i是其方差，引用e用以反映

和理论最优值之间的偏差。

设置可信上限η，取可信系数δ的值为：

当δ_it＝1，融合结果

得到理论最优值Z_t的支持，融合结果可信，反之，δ_ij＝0，融合结果

被理论最优值Z_t否定，融合结果不可信。若可信则采纳数据融合结果，若不可信则考虑使用单一传感器的数据，同理按一定顺序对单一传感器数据进行评估，找到偏差最小的传感器，将其作为最优检测数据使用。

二、机器人控制算法

一种状态反馈优化控制器设计，本发明所设计的控制器是一种通用型控制器，并非只针对静态目标，亦适用于动态目标物。

机器人的动态模型为：

通过机器人的感知系统可以观测目标的状态和位置，设计状态观测器为：

那么局部误差为

e_i＝x_i-ω_i1 (2.3)

利用式(2.1)和(2.3)，可得局部跟踪误差e_i的动态模型为

于是性能函数式

因此，控制目标为：对于机器人，设计控制器u_i使：1)局部跟踪误差e_i动态模型(2.4)渐近稳定；2)性能函数V_i(x_i,ω_i1,u_i)(2.5)达到最小值。

定义由局部跟踪误差e_i和待跟踪状态ω_i1组成的增广系统状态

由式(2.2)和(2.4)可得增广系统动态模型为

其中

基于增广系统动态模型(2.7)，性能函数(2.5)可写为

其中

式(2.9)对时间t求导得下列贝尔曼方程

其中。令V_i ^*为贝尔曼方程(2.11)的最优解，则由最优性必要条件

可得最优控制器

为

将式(2.12)带入贝尔曼方程(2.11)得如下跟踪HJB(HamiltonJacobiBellman)方程

若式(2.13)存在一个连续半正定解V_i ^*，则由式(2.12)得到的最优控制器可使性能函数(2.9)收敛到最小值V_i ^*(X_i(0))。若式(2.13)存在一个连续半正定解V_i ^*，则由式(2.12)得到的最优控制器可使局部跟踪误差系统(2.4)渐近稳定。

由式(2.15)得到的控制器可以使V_i ^j+1≤V_i ^j，其中V_i ^j满足式(2.14)。因此，性能函数最终可收敛到其最小值，即由式(2.15)得到的控制器可使增广系统(2.7)渐近稳定，即使局部跟踪误差系统(2.4)渐进稳定。

三、夹持机构设计

设计一种夹持机构，可以在不损伤杆状作物的基础上对杆状蔬菜进行夹持。

其设计要点有以下几点：

a)夹持机构的前半部分为软体材质，最末端为球形结构。软体材质和球形结构一方面可以防止对杆状作物的损伤，另一方面在夹持后软体材质可以更有效的收紧杆状作物，使捆束的大小可以根据机器人的位置进行调整；

b)夹持机构的后半部分为硬质材质，但内部贴有橡胶垫，可以防止机构对杆状作物的损伤；

c)前半部分与后半部分的衔接处以及后半部分的弯折处均为可旋转轴。其中两者都是受力转轴(即只有在受到一定力的情况下才会转动)，其中后者力大于前者力；

d)整个夹持机构都被设计为上下可调的，以适应不同高度的杆状作物的收割

e)整个夹持机构的夹持过程不需要任何的控制操作，完全借助旋转轴，依靠杠杆原理和力的作用原理实现，即节能又能完成预想的工作。

四、捆缚机构设计

捆缚机构可以在杆状作物进入夹持范围后通过预设的困缚绳对杆状作物进行捆缚，两端各有一个转轮。

其设计要点有以下几点：

a)两侧各个有一个辅助转轮，右侧为1号转轮，为主动轮，带有主动旋转机构，可以收紧捆缚绳，左侧为从动轮，不能主动旋转，但是带有急停机构，可以停止其转动。

b)在夹持机构上有辅助捆缚的结构，捆缚机构收紧是在夹持机构前半部分的辅助下向外收紧。

c)在夹持机构的合紧位置(即捆缚绳在夹持机构上缠绕的位置)带有加热器，可以在捆缚绳收紧后加热熔断捆缚绳，既可以完成捆缚动作，又可以保持困缚绳的连续性。

五、运送机构设计

运送机构可以在杆状作物捆绑结束够，将完成捆缚的杆状作物送出夹持机构，使之进入下一次循环收割。

其设计要点有以下几点：

a)推杆被设计为一个旋转的结构，但是推杆内侧采用软体材料，为了防止对杆状作物造成损伤。

b)推杆被设计为一个具备上下可调节功能的结构，其目的是为了能够适应高度不一的杆状作物。

六、多电机检测故障诊断系统

本发明硬件设计中，使用了多个电机来完成一些必要的功能。机器人的移动依靠的是安装在机器人尾部的电机带动履带前进，通过差速来改变方向；夹持机构的1号辅助转轮为主动轮、切割机构，也分别依靠两个小型电机完成工作。本系统属于离散事件系统，每个子系统不具备连续性，但具有严格的顺序性，任何一个环节的错误就会导致之后环节的错误，对任何一个电机的测量错误都会导致整个系统的失控，故本发明使用了一种多电机检测故障诊断系统。

问题描述

实际的控制器一般只用到从编码器测量得到的转角反馈信号，而控制器算法所需要的速度反馈信号一般是通过位置信号的差分得到的。因为控制器对编码信号的这种强烈的依赖关系，所以对反馈信号的准确性和实时性有较高的要求。当编码器出现故障后，必然会导致系统的故障，无法完成作业任务。

编码器的故障分析

老化、外部撞击以及剧烈干扰的影响都会导致编码器的故障。丢码和完全失灵是主要的故障。通常对于后者，伺服驱动器会关闭输出，进入报警状态；对于丢码现象则很难进行准确诊断和保护，这一点制约了控制系统的高精度和高可靠性，但其对控制系统的鲁棒性意义重大。通过大量的实践经验我们发现编码器在完全失灵之前，通常会出现丢码现象。因此，只要能够及时、准确地捕捉此现象并进行诊断，采取对应措施，那么就可以避免由于编码器的严重故障或完全失灵而导致故障，进而提高系统的鲁棒性和安全性。

多电机检测故障诊断系统策略

在电机伺服控制系统中除了编码器信号作为反馈之外，一般电机驱动还提供其他的信号，即以模拟量形式输出的速度信号，对于驱动器实际转矩监视信号来自对编码器的滤波，一般也可以将速度监视信号切换为控制输入(期望速度信号)。实际系统中，这些信号受到较强干扰的影响以及测量的不精确性，通常不会被直接的应用于控制算法中，而是将这些冗余信号用来监控转速或转矩，或者闲置不用。而这些信号与编码器反馈位置信号之间具有高度相关性，如对编码器转角测量信号的前向差分即为转速信号，这就为传感器的故障诊断提供了解析冗余。

本发明基于主元分析法，利用数控系统中的编码器信号与速度信号之间的上述相关性，将反映此相关性的数据协方差矩阵向其变化最大的方向投影，并通过此投影的长度来监测编码器的工作情况。

基于主元分析法和可信度判断的多电机检测故障诊断算法

系统中有m个电机需要诊断，首先是采集信息，包括编码器信号和速度信号，为了表征其相关性，对数据进行实时的处理，对位置信号做前向差分处理，表示为q∈R^m和w∈R^m，并假设采集n次数据后进入诊断过程，根据实际经验取n＞＞2m。得到数据矩阵F∈R^2mn：

其中，q_i和w_i，i＝1,...,n为第i次采样值。利用概率理论中的协方差矩阵(M∈R^2m)表征两列信号的相关性，其为对称正定矩阵。

由矩阵理论得，对于任意2m×2m正定对称的矩阵M，其特征值为λ₁≥λ₂≥...λ_2m，a_i为对应λ_i的特征矢量。F矩阵的最大的变化方向即为特征矢量a_i的方向。设L为最大投影方向上的单位矢量a_i张成的一条直线，并设每个转轴(本发明中即为捆缚机构中的辅助转轮1的转轴、带动机器人履带转动的转轴、带动切割机构转动的转轴)上的测量或差分值得到的向量v＝<F>，运算<F>为数据矩阵F各列的期望。那么，协方差矩阵向此子空间的投影定义为因此，数据在此方向上的分散程度定义为：

其中，

表示数据v向u_i'张成的子空间上的投影，算子<·>表示求取数据的期望，∑_vv＝<vv^T>＝F表示数据矩阵的协方差矩阵。即得到分散程度最大值。

据以上推论，将所测数据矩阵期望v沿M的最大特征值对应的特征向量方向投影，并通过对此投影结果求解范数，得到最大的变化量。此变化量便对应主元分析法中的主元，用以表征各列信号之间的相关性。致此得到了故障判断的第一个标准。

其次引入检测值与理论最优值的关系，是用可信度作为第二个标准进行评价其是否为故障。主元分析法是将编码信号和速度信号作为信息依据，而这里只使用速度信号作为信息依据，一方面速度信号更为直观，更重要的是速度信号的理论最优值更容易得到。V_i和V_t分别是第i个编码器检测到的速度数据和速度的理论最优值，V_i服从高斯分布，是其概率分布曲线且作为各传感器的特性函数，v_i为V_i的一次观测值，σ_i是其方差，引用Δa_it用以反映v_i和理论最优值之间的偏差。

假设最优数据集有M个数据，则由Δa_ij组成的置信矩阵T_m为：

设置可信上限η_it，取可信系数δ_it的值为：

当δ_it＝1，表示第i个传感器更接近理论最优值，传感器更可信，反之，δ_ij＝0，则第i个传感器原理理论最优值，传感器更不可信。对N个传感器进行评价后，参数精度明显高于单个传感器。通过关系矩阵R_m可找到可信度最高的传感器，其测量的数据定义为最优测量值，在多个相同传感器的情况下可用最优测量值代替被剔除的异常数据，最后进行动态加权融合估计。关系矩阵R_m如下

即关系矩阵R_m的第i列之和为第i个传感器的可信度。传感器可信度的计算公式为：

至此得到了故障判断的第二个标准。与传统的故障诊断方法的应用过程类似，应用此诊断过程，根据系统噪声统计特点(包括期望和方差)设置可信度阈值ξ_i和诊断阈值ML，根据数据分析结果与此值的比较，判断是否满足下面的条件:

如果符合上述条件，即输出超过设定的阈值，说明故障发生，实时记录时刻并采取相应的故障应对测试或进行与预警。反之则表明编码器正常。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。