CN103770115A - 一种家蚕吐丝轨迹和速度的控制方法 - Google Patents

Abstract

本发明公开一种家蚕吐丝轨迹和速度的控制方法。该方法依靠机械手臂硬件平台和机器人控制技术、人工智能等组成的控制系统来实现。其目的是通过研制模拟家蚕吐丝轨迹的机器人系统，来模拟家蚕吐丝行为，最终让家蚕按照人工设计的轨迹和速度吐丝，从而获取便于归类对比分析的蚕丝素材组。

Description

一种家蚕吐丝轨迹和速度的控制方法

技术领域

本发明属于机器人技术和人工智能领域，其实质是利用该领域的相关技术应用于家蚕吐丝行为解析研究中，进一步来模拟和控制家蚕吐丝轨迹和速度，以便获得蚕丝素材组。

背景技术

家蚕吐丝速度对于不同家蚕个体是不同的，并且随着环境条件和家蚕的吐丝阶段而改变。同样，家蚕吐丝轨迹也被认为对蚕丝理化性能有直接影响。考虑到家蚕吐丝行为（例如，轨迹和速度等）与蚕丝理化性能之间的关联性，以实现蚕丝理化性能的提升或特意化为目标，再加上各种化学或物理上的处理方法，可以开发各种新的功能性材料，拓宽蚕丝的新用途。然而以往的成果仅仅只着眼于家蚕吐丝行为或其他研究，并未考虑如何在已知家蚕吐丝行为研究基础上去模拟和仿真家蚕吐丝行为系统，最终让家蚕按照人工设计的轨迹和速度吐丝。

发明内容

本发明所要解决的技术问题是一种控制家蚕可以按照人工设计的轨迹和速度吐丝的方法，其是将人工智能理论和机器人技术相结合，在家蚕吐丝行为数据解析的基础上，设计和发明一种方法来控制家蚕吐丝轨迹和速度，使得蚕丝按照人工设计的轨迹和速度吐丝，便于得到不同组合的家蚕吐丝轨迹和速度的蚕丝素材组，从而方便研究者对蚕丝理化性能进行研究。

本发明技术解决方案如下：

本发明是在解析出家蚕的吐丝行为和位姿等特性的基础上，设计和发明一种机械手臂控制系统来控制家蚕吐丝轨迹和速度，其系统结构包括：机械手臂、机械手臂驱动控制器、家蚕吐丝平台。其中机械手臂能够与家蚕灵活固定，驱动方式可以采取电机传动等形式，家蚕吐丝平台为二维平面或三维立体平台。

一种家蚕吐丝轨迹和速度的控制方法，其实现方法在空间坐标系下，给定家蚕的初始位置、运动轨迹和速度，计算出家蚕吐丝轨迹路径的控制设定点，从而对电机进行伺服控制，实现对机械手臂末端点处的空间轨迹点的有效控制和规划。从而实现对家蚕吐丝行为（轨迹和速度等）的控制，并使得家蚕按照人工设计的轨迹和速度吐丝。

所述控制方法实现的具体步骤如下：

（1）搭建全局直角坐标系，无论是家蚕吐丝轨迹的初始位置还是家蚕运动轨迹中各个路径的控制设定点均相对该全局直角坐标系而言；同时，还建立关节空间坐标系，该坐标系作用于控制机械臂的操作；

（2）根据给定的家蚕吐丝轨迹，利用matlab将吐丝轨迹抽样成离散点，该离散点即是家蚕在全局直角坐标系中吐丝的运动控制设定点；

（3）假定机械臂的关节驱动器是在关节空间坐标系中给定的，得到的机械臂末端控制设定点是在全局坐标系中约束的，因此还需要把运动学逆问题反解求出（该方法是已知的，是机械臂运动轨迹规划熟知方法），也就是把直角坐标系中的控制设定点映射到所建立的关节空间中，即求出机械臂末端控制家蚕吐丝过程中的各关节变量，从而才能够更好对机械臂进行操作；

步骤2和步骤3的解释：由于对机械臂进行控制首先要得出关节空间坐标系中的一些离散的控制点，因此为了得到与给定曲线轨迹十分接近的离散点，需采用某种函数逼近的方法把在全局直角坐标系中的轨迹离散化，离散化的轨迹控制点是相对曲线轨迹所在的全局直角坐标系而得出的，对机械臂的操作是在机械臂关节空间坐标系，所以需要用逆运动学计算把直角坐标系中的离散化的轨迹控制点映射到满足关节路径约束的参数化路径控制点。

（4）把步骤（3）得到的一系列关节空间中的控制设定点转换为电机的给定值，对电机进行伺服控制，也就是对机械臂进行控制操作，从而实现控制家蚕运动轨迹和速度。

使用发明方法来控制家蚕吐丝轨迹和速度，可以使蚕丝按照人工设计的轨迹和速度吐丝，便于得到不同组合的家蚕吐丝轨迹和速度的蚕丝素材组，从而方便研究者对蚕丝理化性能进行研究。

附图说明

图1是采用多关节机械手臂控制家蚕吐丝轨迹和速度的结构示意图；

图2是采用直角坐标系机械手臂控制家蚕吐丝轨迹和速度的结构示意图；

图1中：1，2，3—多关节机械手臂的活动关节点，实际关节点数不局限于3，数量可>=1，4—机械手臂与家蚕固定终端，5—家蚕吐丝平台，6—控制系统的全局坐标系。

图2中：1，2，3—分别对应直角坐标系机械手臂的X，Y，Z旋转轴，且可以分别在对应的X，Y，Z方向平行移动。4—机械手臂与家蚕固定终端，5—家蚕吐丝平台，6—控制系统的全局坐标系。

具体实施方式

本发明采用的系统结构包括：机械手臂、机械手臂驱动控制、家蚕吐丝平台。其中机械手臂能够与家蚕灵活固定，驱动方式可以采取电机传动等形式，家蚕吐丝平台为二维平面或三维立体平台。

具体实施方式如下：

（1）搭建直角坐标系和关节空间坐标系，家蚕的初始位置和家蚕的运动轨迹是针对直角坐标系而言，机械臂的关节变量是在关节空间坐标系中给定。

（2）已知直角坐标系下家蚕吐丝的运动轨迹，利用等间隔曲线函数离散化方法(该方法通常有等间隔法、弦长法、等误差法等)将该运动轨迹离散成一系列点，假如为(p_xi,p_yi,p_zi),i＝1,2,3...。其中，p_xi、p_yi、p_zi分别表示一系列点在直角坐标系下x、y、z轴的坐标值；i表示一系列点的个数，从1开始计数，增量为1。

（3）在已知家蚕吐丝离散位置坐标点的情况下，运用机器人反向运动学求解机械臂的各关节变量，首先说明下面将使用到的各符号变量：a_n：机械臂连杆的长度；α_n：连杆扭转角；d_n：相邻两连杆的距离；θ_n：相邻两连杆的夹角；n：机械臂的自由度数；其中，机械臂类型不同，关节变量不同，旋转关节θ_n为关节变量，而滑动关节d_n为关节变量，其余的都为机械臂的连杆参数。具有n个自由度的机器人运动学方程一般为：

⁰T_n＝⁰T₁ ¹T₂ ²T₃...^n-1T_n＝A₁A₂A₃...A_n   （1）

其中T_n表示机械臂末端连杆相对于关节空间坐标系的位姿，其可以用公式(2)表示；它是由三个平移分量p_x、p_y、p_z(确定空间位置)和三个旋转矢量n(接近矢量)、o(方位矢量)、a(法向矢量)，这三个矢量确定姿态。A_n(n＝1,2,...)表示相邻两个关节坐标系间的变换矩阵，可以用公式(3)表示，其中机械臂各个关节的坐标系都是根据Denavit和Harenberg(即D-H)法(机器人运动建模的标准方法)建立的，

$T_n=\left[\begin{array}{cccc}n& o& a& p\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

$A_n=\left[\begin{array}{cccc}c{\mathrm{\θ}}_n& -s{\mathrm{\θ}}_{n}c{\mathrm{\α}}_n& s{\mathrm{\θ}}_{n}s{\mathrm{\α}}_n& a_{n}c{\mathrm{\θ}}_n\\ s{\mathrm{\θ}}_n& c{\mathrm{\θ}}_{n}c{\mathrm{\α}}_n& -c{\mathrm{\θ}}_{n}s{\mathrm{\α}}_n& a_{n}s{\mathrm{\θ}}_n\\ 0& s{\mathrm{\α}}_n& c{\mathrm{\α}}_n& d_n\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

其中，c代表cosθ，s代表sinθ。

（4）假设机械臂关节数为3，实际关节点数不局限于3，数量可>=1，且为旋转关节，则由公式(1)(2)可得：

$T_3^0=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]=A_1{A}_2{A}_3$

（5）联立公式(3)(4),列方程求出其运动过程中机械臂的关节变量θ₁,θ₂,θ₃，其中a_i、α_i及d_i是固定值，其根据机械臂的类型不同而不同；另外，(p_xi,p_yi,p_zi)为已知的家蚕吐丝位置坐标点，其中i＝1,2,...n。

（6）把得到的一系列关节空间中的关节变量转换为电机的给定值。

（7）对电机进行伺服控制从而实现家蚕运动轨迹和速度的控制方法。

以几种常见的机械臂为例：

参见图1，对多关节机械手臂，与基座相连的关节点能够绕基座旋转，其余关节点能够实现水平和垂直方向转动，从而使得臂末端点能够在可到达范围内的任意空间坐标点移动。

参见图2，对直角坐标机械手臂，可通过分别控制机械手臂的X、Y、Z三个方向的自由度在各坐标长度范围内运动。

其中图1和图2中的机械手臂为常见的两种类型，实际使用时不局限于这两种结构。

对圆柱坐标机械手臂，以环绕极地主体做轴承旋转执行动作，使得机械手臂能够在水平方向和俯仰角垂直方向旋转运动。

对极坐标机械手臂由两个旋转轴支撑，一个绕着基座转动，能够以线性运动收入伸出，另一个绕基座做出垂直线性的转动。

对球型坐标机械手臂，具有两个旋转运动和一个直线运动关节,并按球坐标形式运动。

Claims (5)

1.一种家蚕吐丝轨迹和速度的控制方法，其特征在于：所述方法是利用机械手臂的末端固定家蚕，给定家蚕初始位置和家蚕吐丝轨迹、速度以及目标位置后，计算出家蚕吐丝轨迹路径的控制设定点，通过对电机的伺服控制，实现机械手臂带动家蚕按照预先设定好的运动轨迹和速度运动吐丝；

所述控制方法实现的具体步骤如下： 

（1）搭建全局直角坐标系，无论是家蚕吐丝轨迹的初始位置还是家蚕运动轨迹中各个路径的控制设定点均相对该全局直角坐标系而言；同时，还建立关节空间坐标系，该坐标系作用于控制机械臂的操作；

（2）根据给定的家蚕吐丝轨迹，利用matlab将吐丝轨迹曲线抽样成离散点，该离散点即是家蚕在全局直角坐标系中吐丝的运动控制设定点；

（3）假定机械臂的关节驱动器是在关节空间坐标系中给定的，得到的机械臂末端控制设定点是在全局坐标系中约束的，因此还需要把运动学逆问题反解求出,也就是把直角坐标系中的控制设定点映射到所建立的关节空间中，即求出机械臂末端控制家蚕吐丝过程中的各关节变量，从而才能够更好对机械臂进行操作；

（4）把步骤（3）得到的一系列关节空间中的控制设定点转换为电机的给定值，对电机进行伺服控制，也就是对机械臂进行控制操作，从而实现控制家蚕运动轨迹和速度。

2.根据权利要求1所述的控制家蚕吐丝轨迹和速度的方法，其特征在于：所述机械手臂的结构形式可以为多关节机械手臂、直角坐标系机械手臂、球坐标机械手臂、极坐标机械手臂或柱坐标机械手臂；

采用多关节机械手臂，与基座相连的关节点能够绕基座旋转，其余关节点能够实现水平和垂直方向转动，从而使得机械手臂的末端能够在可到达范围内的任意空间坐标点移动；

采用直角坐标机械手臂，是通过分别控制机械手臂的X、Y、Z三个方向的自由度在各坐标长度范围内运动；

采用圆柱坐标机械手臂，是以环绕极地主体做轴承旋转执行动作，使得机械手臂能够在水平方向和俯仰角垂直方向旋转运动； 

采用极坐标机械手臂，是由两个旋转轴支撑，一个绕着基座转动，能够以线性运动伸入伸出，另一个绕基座做出垂直线性的转动；

采用球型坐标机械手臂，具有两个旋转运动和一个直线运动关节，并按球坐标形式动作运动。

3.根据权利要求2所述的控制家蚕吐丝轨迹和速度的方法，其特征在于：所述机械手臂的自由度个数>=1，机械手臂可到达所要求的运动空间范围内任意点。

4.根据权利要求2所述的一种控制家蚕吐丝轨迹和速度的方法，其特征在于：机械手臂的运动速度通过控制系统调控，能够实现匀速或变速的运动。 

5.根据权利要求1-4之任一项所述的控制家蚕吐丝轨迹和速度的方法，其特征在于：所述运动轨迹可为二维平面或三维空间的直线或曲线的任意运动轨迹。

Images