一种小型工业机械手的学习控制方法与流程

本发明属于机械手自动控制领域，具体涉及一种小型工业机械手的学习控制方法。

背景技术：

在轻、手工业的生产中，存在着大量高重复性、高强度、低技术含量的体力劳动。随着近年来我国生育率的降低和老龄化趋势的加深，我国劳动力人口总数下降，人力成本逐年上升，“机器替人”对企业带来的经济效益将越来越明显。工业机械手在轻、手工业行业生产过程中的作用将不可忽视。

在汽车制造、电子工业等行业中，工业机械手及其配套控制方法已经被大范围推广使用且日趋成熟；但是，现有的成熟机械手系统大多使用abb、松下等国外知名公司生产的高精度机械手，设备引进与维护成本都很高。轻、手工业生产过程对机械手系统精度往往没有过高的要求，而现有成熟技术的高昂成本却是很多中小型企业难以负担的。

近年来，绝大多数关于工业机械手控制方法的研究都是以市面上常见的高端工业机械手作为被控对象，这一类机械手的主要特点是各旋转关节均由高精度伺服电机驱动，且机械手本身结构精密、模型精确。在轻、手工业生产过程中，一类由舵机(低成本小型伺服电机)驱动的小型工业机械手即可满足实际需求，且这种小型工业机械手相比于高端工业机械手具有明显的成本优势，更适合在生产中投入使用。然而对于这一类舵机驱动的小型工业机械手，目前还鲜有成熟有效的控制方法。

技术实现要素：

由于现有技术难以解决舵机驱动的小型工业机械手的自动控制问题，本发明提出了一种小型工业机械手的学习控制方法，用于实现小型工业机械手的自动控制，使其自动完成重复性工作。

为了实现上述的目的，本发明所采取的技术方案如下：

小型工业机械手的学习控制方法，方法包含以下步骤：

(1)针对目标重复性任务，通过离线轨迹规划或人工轨迹示教，获取抓手的期望运行轨迹，规定单次任务中抓手运行轨迹与期望轨迹之间的最大偏差；

(2)根据步骤(1)中得到的抓手期望运行轨迹以及当前所采用的机械手的模型参数，计算得到机械手各关节角的期望变化轨迹；

(3)向各关节的舵机输出初始控制信号，在机械手运行过程中，实时测量抓手的实际位置以及各个关节角的实际大小，根据实际关节角大小与期望关节角大小之差实时修正控制信号；

(4)记录机械手运行过程中控制器实际输出的控制信号，并在本次任务结束后将其设定为下一次任务的初始控制信号；

(5)利用步骤(3)中测量得到的单次任务中抓手的实际运行轨迹以及各个关节角的大小变化情况，使用参数估计方法获取机械手模型参数代替原有的模型参数，机械手回复至初始位置；

(6)重复执行步骤(2)至(5)，直至单次任务过程中抓手的运行轨迹与期望轨迹之间的偏差小于步骤(1)中最大偏差。

步骤(3)中实时修正控制信号的过程如下式所示：

un+1(k)＝un(k)+kp[qd(k)-qn(k)]

式中，k为单次任务中的运行时间，u为控制器输出信号，qn(k)为第n次任务中k时刻测得的机械手各关节角度，qd(k)为关节角的期望变化轨迹qd在k时刻的值；矩阵kp为学习控制的增益；un(k)为第n+1次执行任务时的初始控制信号在k时刻的值；当n＝0时，u0可以人为设定或默认为0。

步骤(5)中的参数估计方法使用卡尔曼滤波算法；第n次任务中实际抓手轨迹yn与机械手模型参数之间的函数关系表示为下式：

式中，φ(k+1)机械手模型参数在k+1时刻的估计值；c为观测矩阵，通过已知的机械手模型参数以及步骤(3)中测量得到的实际关节角轨迹xn计算得到；ω(k)与v(k)是相应维数的独立分布正态白噪声在k时刻的值；单次任务过程中ω(k)与v(k)的均值分别为q和r，协方差阵分别为q和r；

使用卡尔曼滤波法进行机械手模型参数估计的过程如下式所示：

式中，为k时刻对机械手模型参数的估计值，yn(k)为第n次任务中k时刻的实际抓手轨迹；c^t为c的转置；矩阵为初始时刻参数的估计值，实际计算时选取为本次任务开始时已知的机械手模型参数，p(0)为初始时刻机械手模型参数估计值的协方差阵，i为单位矩阵。

本发明中提出的控制方法能够有效解决一种舵机驱动的小型工业机械手的控制问题。本发明的一个优点在于使用了学习控制的方法，在执行重复性任务的过程中，随着执行次数的上升，机械手抓手的运行轨迹与期望轨迹之间的偏差会逐渐减小。本发明的另一个优点在于，该方法能够应用于机械手模型与实际对象存在较大偏差的环境，在学习控制的过程中，控制器能够自动更新机械手模型参数。相比于现有的机械手控制方法，本发明提出的方法在处理舵机驱动的小型工业机械手的控制问题，尤其是在处理机械手已知模型与实际模型存在较大偏差情况下的控制问题时，具有非常明显的优势。

附图说明

图1是本发明提出的控制方法的流程图。

图2是一种舵机驱动的具有5自由度的小型工业机械手，用于在仿真实验中验证本发明提出的控制方法。

图3是本发明中对机械手参数更新的效果图。

图4是本发明提出的控制方法应用于仿真系统时的控制效果图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步阐述和说明。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。

图1为本发明提出的一种小型工业机械手的学习控制方法的流程图。为了方便说明本发明的特点与优势，下面将结合仿真实验说明本发明的具体步骤。

仿真实验中使用一种具有5自由度的机械手作为被控对象。图2为该5自由度机械手的实际结构。该机械手的数学描述如(1)式所示。

(1)式中，下标n代表第n次执行该任务；k为单次任务中的运行时间；x5×1为系统状态变量，其物理意义为图2中机械手的5个关节角的角度；u5×1为控制器输出，物理意义为5个舵机的转速；δk为离散步长，物理意义为控制器输出的离散间隔；g(·)为机械手模型；y3×1为系统输出，其物理意义为机械手抓手位置的三维坐标。若无特殊说明，上述符号在后文中代表的意义不变。

本发明的具体实施步骤如下：

1、针对特定的重复性任务，通过离线轨迹规划或人工轨迹示教，获取抓手的期望运行轨迹，规定单次任务中抓手运行轨迹与期望轨迹之间的最大偏差。

记抓手期望运行轨迹为yd，三个分量代表抓手位置的三个坐标。

2、根据1中得到的抓手期望运行轨迹以及已知的机械手模型参数，计算得到机械手各关节角的期望变化轨迹。

记关节角的期望变化轨迹为qd，五个分量对应五个关节角。

机械手模型参数使用d-h表示法表示。表1为本实施例中机械手真实参数，表2为已知的具有偏差的模型参数。

由于该偏差的存在，即使机械手各个关节角达到了步骤2中计算得到的qd，机械手抓手的实际运行轨迹也将与步骤1中设定的期望运行轨迹yd存在较大偏差。

表1.机械手真实d-h参数表

表2.已知的具有偏差的机械手d-h参数表

3、向舵机输出初始控制信号，在机械手运行过程中，实时测量抓手的实际位置以及各个关节角的实际大小，根据实际关节角大小与期望关节角大小之差实时修正控制信号。

步骤3中使用了学习控制的方法实时修正控制信号。修正控制信号的过程如(2)式所示。式中，k为单次任务中的运行时间，u为控制器输出信号，qn(k)为第n次任务中k时刻测得的机械手各关节角度，qd(k)为关节角的期望变化轨迹qd在k时刻的值。矩阵kp为学习控制的增益，本实施例中选取为单位矩阵i5×5。un(k)为第n+1次执行任务时的初始控制信号在k时刻的值。当n＝0时，u0可以人为设定，或默认为0(本实施例中采用默认为0)。

un+1(k)＝un(k)+kp[qd(k)-qn(k)](2)

4、记录机械手运行过程中控制器实际输出的控制信号，并在本次任务结束后将其设定为下一次任务的初始控制信号。该步骤即是使用(2)式中修正得到的新的控制信号un+1作为下一次任务中的初始控制信号。

5、利用步骤3中测量得到的单次任务中抓手的实际运行轨迹以及各个关节角的大小变化情况，使用参数估计方法获取机械手模型参数代替原有的模型参数。机械手回复至初始位置。

本发明中使用卡尔曼滤波算法作为参数估计方法。卡尔曼滤波算法是一类成熟的数据处理方法，在模型的参数估计领域有广泛应用。该算法基于上一时刻的状态估计值以及当前时刻的状态观测值计算得到当前时刻状态的估计值。

上文(1)式中，机械手抓手位置y表示为关节角x的函数。现根据步骤3中测量得到的第n次任务中的实际抓手轨迹yn与实际关节角轨迹xn，将xn作为已知值、将机械手模型参数作为变量，得到第n次任务中实际抓手轨迹yn与机械手模型参数之间的函数关系，如(3)式所示：

式中，φ(k+1)机械手模型参数在k+1时刻的估计值；观测矩阵c可以通过(1)式中g(·)的具体形式以及步骤3中测量得到的实际关节角轨迹xn直接计算得到；ω(k)与v(k)是相应维数的独立分布正态白噪声在k时刻的值；单次任务过程中ω(k)与v(k)的均值分别为q和r，协方差阵分别为q和r；假定上述噪声统计特性已知或可测，当其不可测时，需要根据经验指定一个估计值。

本实施例中，g(·)的具体形式如下文(4)式所示。(4)式中隐去了代表第n次任务的下标n以及代表时刻的k。式中，yn＝[y1y2y3]^t为机械手抓手的实际轨迹，其三个分量y1、y2、y3别代表抓手实际位置的三维坐标，ci代表cos(xi)，si代表sin(xi)，i代表关节编号，本实施例中i取值为1至5。由(4)式可计算得到(3)式中所需的观测矩阵c。

此时，(3)式中的噪声统计特性、观测方程c和实际观测值yn均已知。卡尔曼滤波方程如式(5)所示。式中，为k时刻对机械手模型参数的估计值，yn(k)为第n次任务中k时刻的实际抓手轨迹；c^t为c的转置；矩阵为初始时刻参数的估计值，实际计算时选取为本次任务开始时已知的机械手模型参数，p(0)为初始时刻机械手模型参数估计值的协方差阵，i为单位矩阵。基于p(0)的值，可以通过公式(5)中的倒数第2和3行公式推导出p(k|k)和p(k|k-1)的取值。基于的取值，可以通过公式(5)中的第1和3行公式推导出和的取值。

6、重复执行步骤2至5，直至单次任务过程中抓手的运行轨迹与期望轨迹之间的偏差满足步骤1中设定的要求。此后，可以以满足该要求的机械手模型参数得到机械手控制信号。

图3为重复执行步骤2至5时，机械手模型各个参数绝对误差的变化情况。横坐标为任务重复执行的次数，纵坐标为各个参数的绝对偏差，单位为分米(dm)。

图4为步骤2至5重复执行4次后，机械手抓手运行轨迹。横坐标为单次任务中的运行时间，单位为秒(s)，纵坐标分别为抓手位置在三维空间中的坐标，单位为分米(dm)。

本发明提出了一种小型工业机械手的学习控制方法。该方法使用了学习控制的方法，在解决机械手重复性工作的控制任务时，能够有效利用之前的执行过程中的控制器输出等信息。在完成重复性任务时，机械手只需经历较少的执行次数即可完成对期望轨迹的自动跟踪。另外，该方法使用了卡尔曼滤波算法在重复性任务的执行过程中对机械手模型进行在线更新。通过参数估计、模型更新，该方法能够很好地解决小型工业机械手模型不准确、参数出现变化的问题，在保证了控制效果的前提下，使得控制系统对于机械手模型精度的要求大大降低。