[发明专利]多关节布料臂架末端轨迹控制方法、系统及存储介质

权利要求书

1.一种多关节布料臂架末端轨迹控制方法，其特征在于：包括：

S1.由各个安装在所述布料臂架上的倾角传感器采集各个关节的角度值；

S2.基于运动学模型，利用所述采集的各个关节的角度值计算所述布料臂架末端的实际位置；

S3.计算所述布料臂架末端的实际位置与期望轨迹位置的偏差；

S4.将所述计算的偏差转换成期望末端速度；

S5.任意选择所述布料臂架中的两个关节进行逆运动学求解，得到通过所述两个关节达到所述期望末端速度的速度方案；

S6.利用所述得到的速度方案驱动所述两个关节；

S7.执行S1。

2.根据权利要求1所述的多关节布料臂架末端轨迹控制方法，其特征在于：所述布料臂架包括有6节臂及6个关节；6节臂包括第一节臂、第二节臂、第三节臂、第四节臂、第五节臂和第六节臂；6个关节包括第一关节、第二关节、第三关节、第四关节、第五关节和第六关节；

第一关节、第一节臂、第二关节、第二节臂、第三关节、第三节臂、第四关节、第四节臂、第五关节、第五节臂、第六关节、第六节臂依次连接。

3.根据权利要求2所述的多关节布料臂架末端轨迹控制方法，其特征在于：所述各个安装在所述布料臂架上的倾角传感器采集各个关节的角度值包括采集的第一关节的角度值θ₁、第二关节的角度值θ₂、第三关节的角度值θ₃、第四关节的角度值θ₄、第五关节的角度值θ₅、第六关节的角度值θ₆。

4.根据权利要求3所述的多关节布料臂架末端轨迹控制方法，其特征在于：

所述S2基于运动学模型，利用所述采集的各个关节的角度值计算所述布料臂架末端的实际位置(x_e，y_e)包括：

x_e＝L₁cosθ₁-L₂cos(θ₁+θ₂)+L₃cos(θ₁+θ₂+θ₃)-L₄cos(θ₁+θ₂+θ₃+θ₄)+L₅cos(θ₁+θ₂+θ₃+θ₄+θ₅)-L₆cos(θ₁+θ₂+θ₃+θ₄+θ₅+θ₆)；

y_e＝L₁sinθ₁-L₂sin(θ₁+θ₂)+L₃sin(θ₁+θ₂+θ₃)-L₄sin(θ₁+θ₂+θ₃+θ₄)+L₅sin(θ₁+θ₂+θ₃+θ₄+θ₅)-L₆sin(θ₁+θ₂+θ₃+θ₄+θ₅+θ₆)；

其中，L₁、L₂、L₃、L₄、L₅、L₆分别表示第一节臂、第二节臂、第三节臂、第四节臂、第五节臂和第六节臂的长度。