[发明专利]一种家蚕吐丝轨迹和速度的控制方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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