本发明属于机器人，具体涉及蜘蛛机械手及其行走路径控制方法及系统。
背景技术：
1、在制造业的快速发展背景下，自动化和智能化技术正在改变着各行各业的工作模式。工业机器人是面向工业领域的多关节机械手或多自由度的机器装置，它能自动执行工作，是靠自身动力和控制能力来实现各种功能的一种机器。它可以接受人类指挥，也可以按照预先编排的程序运行，现代的工业机器人还可以根据人工智能技术制定的行走路径移动完成搬运转移的生产流程。蜘蛛机械手是一种仿生机器人，其灵感来源于蜘蛛的八足结构。如现有技术中公告号为cn213057689u的实用新型公开了一种食品装箱装置及其蜘蛛机械手，再如，公布告为cn215158857u的实用新型公开了一种蜘蛛手收板机，这种蜘蛛机械手一般由多个完全相同的主动臂连接的从动臂组成，每个主动臂都由一个电机驱动，可以实现多方向的灵活运动，进而带动多个从动臂同一连接的承载盘上的抓取机构在上料区域拾取目标物。由于其独特的结构特点，蜘蛛手并联机器人在空间狭小的环境下也能实现准确的操作。
2、然而，现有技术中，并未有对于蜘蛛机械手行走路径在承接所要拾取的目标物移动过程中保持受力平衡的限定条件下，实现平稳移动所拾取的目标物的蜘蛛机械手及其行走路径控制方法和系统。
技术实现思路
1、本发明针对上述缺陷，提供蜘蛛机械手及其行走路径控制方法和系统。本发明提供的方法仅仅需要监测动平台中心点的实时位置坐标这一个变量，通过s2-s3步骤的构建相关臂的点位置坐标，利用从动臂长度l2恒定的关系，求解得到实时关节角度变量，进一步通过s4步骤求解在平衡方程限定条件下构建最优实时关节角度变量求解模型，无需多个传感器检测各个驱动臂末端ai、各个从动臂末端bi以及各个关节点ci的最优实时位置以及各个关节点相对于定平台所在平面x1y1的实时偏折角度，即可求解最优实时行走轨迹，避免了多数据计算所带来的计算冗繁，进而所需芯片计算容量大且输出结果缓慢，动作执行出现时延的缺陷。整个模型轻量化设计，且提高了计算结果的准确性。
2、本发明提供如下技术方案：蜘蛛机械手行走路径控制方法，所述蜘蛛机械手用于在上料区域拾取目标物并按照预定轨迹移动至下料区域目标位置下料，包括以下步骤：
3、s1、实时监测蜘蛛机械手各个从动臂机构末端形成的动平台中心点位置；
4、s2、构建动平台实时空间运动学矢量等式，求解各个关节点与各个驱动臂起始端形成的定平台所在平面实时夹角；
5、s3、根据所述s2步骤求解结果，构建各个关节点在基准坐标系内的实时关节角度变量矩阵以及关节位置变量矩阵；
6、s4、构建蜘蛛机械手动态移动力矩平衡方程，并在平衡方程限定条件下构建最优实时关节角度变量求解模型；
7、s5、根据求解得到的在基准坐标系内的确定动平台中心点、各个驱动臂起始端、各个从动臂机构末端最优实时坐标，以及各个关节点最优实时角度和坐标所组成的蜘蛛机械手最优实时行走轨迹，并以此控制蜘蛛机械手实时动态移动目标物。
8、进一步地，所述s2步骤包括以下步骤：
9、s21、以动平台笛卡尔坐标系o2-x2y2z2为基准坐标系，构建各个从动臂机构末端bi基准坐标系内的实时坐标计算模型：
10、
11、
12、zbi＝z2；
13、s22、构建计算各个驱动臂末端所在关节点ci在基准坐标系的实时坐标计算模型：
14、
15、
16、
17、其中，r为第i个驱动臂起始端ai形成的定平台的半径，r为所述动平台的半径；i＝1,2,3；l1为每个所述驱动臂的长度；θi为第i个驱动臂末端bi所在的第i个关节点ci与定平台所在平面实时夹角；为第i个驱动臂起始端ai与定平台坐标系x轴的夹角，ybi和分别为第i个从动臂机构末端bi在基准坐标系内的x轴实时坐标、y轴实时坐标和z轴实时坐标；和分别为第i个驱动臂末端所在关节点ci在基准坐标系内的x轴实时坐标、y轴实时坐标和z轴实时坐标；x2、y2和z2为动平台中心点在基准坐标系内x轴实时坐标、y轴实时坐标和z轴实时坐标；
18、s23、构建动平台实时空间运动学矢量等式：
19、其中，l2为每个所述从动臂机构的长度；
20、s24、令为θi的换元未知数，构建关于的一元二次方程：其中，αi为二次项系数，βi为一次项系数，γi为常数项；由于因此，求解得到
21、
22、βi＝-4z2l1；
23、
24、进一步地，所述s3步骤包括以下步骤：
25、s31、根据所述s2步骤计算得到的第i个关节点与第i个驱动臂起始端形成的定平台所在平面实时夹角θi，得到各个关节点在基准坐标系内的实时关节角度变量矩阵θ＝[θ1θ2 θ3]t；i＝1,2,3，
26、s32、带入至所述s22步骤中计算各个驱动臂末端所在关节点ci在基准坐标系的实时坐标公式中，即可得到第i个关节点ci在基准坐标系内的关节位置变量矩阵qi，进而进一步构建关节位置变量矩阵q＝[q1 q2 q3]t。
27、进一步地，所述s4步骤包括以下步骤：
28、s41、根据每个从动臂机构的惯性矩ibi构建从动臂机构惯性矩矩阵ib：
29、
30、s42、计算由于各个驱动臂与各个从动臂机构在运动过程中的自身重力扭矩ts：
31、ts＝com(ma,mb,mc)×[cosθ1 cosθ2 cosθ3]；其中，com(ma,mb,mc)为所述蜘蛛机械手的质心系数；
32、
33、s43、计算当蜘蛛机械手拾取目标物后的总重量mtotal：
34、mtotal＝md+mp+6(1-r)mb；
35、其中，md为从动平台的质量，mp为蜘蛛机械手拾取的目标物重量；
36、s44、根据s41-s43步骤计算结果，构建蜘蛛机械手动态移动力矩平衡方程：
37、
38、其中，m(θ)为正定质量矩阵；为哥氏力系数矩阵；为关节速度变量矩阵，为关节加速度变量矩阵，f(θ)为重力矢量矩阵；
39、s45、在平衡方程限定条件下构建最优实时关节角度变量求解模型：
40、
41、其中，e为所述蜘蛛机械手单次搬运目标物所需能量，ek为所述蜘蛛机械手单次搬运目标物过程中运动做功能；es为所述蜘蛛机械手单次搬运目标物过程中克服目标物及自身重量做功能；
42、s46、根据所述s45步骤求解得到的最优关节角度变量θi,best得到最优实时关节角度变量矩阵θbest＝[θ1,best θ2,best θ3,best]t，将其代入至所述s22步骤中，得到各个驱动臂末端所在关节点ci在基准坐标系的最优实时坐标再将其代入至所述s23步骤中构建的动平台实时空间运动学矢量等式中，得到动平台中心点在基准坐标系内最优实时坐标(x2,best,y2,best,z2,best)；；
43、s47、构建动平台中心点终止移动判定阈值计算函数：
44、
45、判断所述动平台中心点终止移动判定阈值计算函数结果f(x2,best,y2,best,z2,best)是否小于0.10，若是，则输出求解得到的最优关节角度变量θi,best；否则，重复所述s41-s46步骤。
46、进一步地，所述s44步骤中的正定质量矩阵m(θ)计算公式如下：
47、m(θ)＝ib+mtotaljtj；
48、所述哥氏力系数矩阵计算公式如下：
49、
50、所述重力质量矩阵g(q)计算公式如下：
51、g(θ)＝-jtmtotal[0 0 g]t-ts；
52、其中，j为表达动平台中心点移动速度和各个关节点的关节角度变化速度之间关系的雅可比矩阵，为动平台中心点实时速度矩阵，x2为动平台中心点实时位置矩阵，x2＝[x2 y2 z2]t；g为重力加速度。
53、进一步地，所述s41步骤中，每个从动臂机构的惯性矩ibi的计算公式如下：
54、
55、其中，ma为每个驱动臂的质量，mc为每个关节点ci处的球形接头的质量，mb为每个从动臂机构的质量；im为驱动所述驱动臂移动的步进伺服电机的惯性矩，im＝3.85×10-5m4～4.05×10-5m4。
56、进一步地，所述s45步骤中的所述蜘蛛机械手单次搬运目标物过程中运动做功能ek计算公式如下：
57、
58、其中，ek,d为所述动平台在单次搬运目标物过程中运动做功能，ek,bi为第i个从动臂机构在单次搬运目标物过程中运动做功能，ek,ai为第i个驱动臂在单次搬运目标物过程中运动做功能；
59、和分别为所述动平台中心点在基准坐标系内x轴实时移动速率、y轴实时移动速率和z轴实时移动速率；
60、和分别为第i个从动臂机构末端bi在基准坐标系内的x轴实时移动速率、y轴实时移动速率和z轴实时移动速率；
61、
62、其中，im为驱动所述驱动臂移动的步进伺服电机的惯性矩，im＝3.85×10-5m4～4.05×10-5m4。
63、进一步地，所述s45步骤中的所述蜘蛛机械手单次搬运目标物过程中克服目标物及自身重量做功能es计算公式如下：
64、
65、其中，es,d为所述动平台在单次搬运目标物过程中克服自身重量做功能，es,p为在单次搬运目标物过程中克服目标物重量做功能，es,bi为第i个从动臂机构在单次搬运目标物过程中克服自身重量做功能，es,ai为第i个驱动臂在单次搬运目标物过程中克服自身重量做功能；
66、es,d＝mdgz2；es,p＝mpgz2；
67、
68、
69、本发明还提供一种采用如上方法的蜘蛛机械手行走路径控制系统，包括动平台位置监测模块、关节点夹角计算模块、关节点位置计算模块、力矩平衡最优关节点角度计算模块和路径动态规划模块；
70、所述动平台位置监测模块，用于实时监测蜘蛛机械手各个从动臂机构末端形成的动平台中心点位置；
71、所述关节点夹角计算模块，用于构建动平台实时空间运动学矢量等式，求解各个关节点ci与各个驱动臂起始端形成的定平台所在平面实时夹角；
72、所述关节点位置计算模块，用于根据所述关节点夹角计算模块的求解结果，构建各个关节点在基准坐标系内的实时关节角度变量矩以及关节位置变量矩阵。
73、所述力矩平衡最优关节点角度计算模块，用于构建蜘蛛机械手动态移动力矩平衡方程，并在平衡方程限定条件下构建最优实时关节角度变量求解模型；
74、所述路径动态规划模块，用于根据求解得到的在基准坐标系内的确定动平台中心点、各个驱动臂起始端、各个从动臂机构末端最优实时坐标，以及各个关节点最优实时角度和坐标所组成的蜘蛛机械手最优实时行走轨迹，并以此控制蜘蛛机械手实时动态移动目标物。
75、本发明还提供一种采用如上所述方法的蜘蛛机械手，包括三个铝型材横梁、三个六边形固定块、三个步进伺服电机、三个驱动臂、与三个驱动臂末端连接的三个并联从动臂机构，三个从动臂机构的从动臂末端与同一承载盘连接；每个步进伺服电机的输出端与相应的驱动臂起始端ai固定连接，并使相应的驱动臂起始端ai位于所在铝型材横梁的中点所在竖直方向正下方，三个驱动臂起始端ai形成半径为r的定平台；每个驱动臂末端均与相应的从动臂机构的从动臂起始端相连接形成关节点ci，每个从动臂机构的从动臂末端bi与同一承载盘连接形成半径为r的动平台；
76、每个从动臂机构为由两个相互平行的第一从动臂和第二从动臂组成；每个所述铝型材横梁上在齿轮箱侧还设置有相应的步进伺服电机的手动开关，手动开关上设置有com端引脚、no端引脚和nc端引脚。
77、本发明的有益效果为：
78、1、本发明提供的蜘蛛机械手能够按照求解得到的最优实时行走轨迹控制方法，以最优路径抓取左侧无序来料有序放入右侧包装盒内，从而完成分拣、理料、包装及废品剔除等复杂功能。本发明提供的蜘蛛机械手行走轨迹控制方法与单空间(笛卡尔空间)轨迹规划相比，结合了关节空间的轨迹规划算法(即构建蜘蛛机械手动态移动力矩平衡方程，并在平衡方程限定条件下求解最优实时关节角度变量进而规划蜘蛛机械手的各个端点和关节点行走轨迹的控制方法)可以获得平滑的轨迹，并且在相同的控制器(传统pid)下可以有效地改善蜘蛛机械手行走轨迹曲线的不连续性和轨迹突变。
79、2、本技术提供的蜘蛛机械手行走轨迹控制方法，通过s4步骤构建蜘蛛机械手动态移动力矩平衡方程，强调了并联机器人各关节之间的强耦合关系和动力学特性。在高速运行场合，机器人所受惯性力、科氏力和重力增大，由此产生的控制力非线性变化不容忽视，此外还有外部力矩扰动的影响，运动学控制无法保证最佳的控制性能与运动学控制不同，动力学控制的设计考虑了机器人的动力学特性，利用逆动力学求解机器人所需的驱动力或者力矩，驱动机器人按照期望轨迹运动。
80、3、本发明提供的方法通过s4步骤构建蜘蛛机械手动态移动力矩平衡方程，并在平衡方程限定条件下构建最优实时关节角度变量求解模型，避免了目标物的重心相对于动平台坐标系z轴出现偏移所导致的动平台相对于其初始x2y2平面(相对于大地坐标系为0度偏折)出现一定角度的倾斜，进而避免了蜘蛛机械手被控制按照最终求解得到的最优实时行走轨迹行走过程中倾斜所导致的所携带的目标物受力不稳所出现的坠落现象发生，进而避免了蜘蛛机械手在搬运携带目标物至目的地过程中的损毁所导致的生产成本增加。
81、4、本发明提供的方法在求解最优实时行走轨迹时，是通过仅仅只需要实时监测各个从动臂末端形成的动平台中心点o2位置坐标(x2,y2,z2)，然后再经过s2步骤假设各个关节点ci在基准坐标系内的实时关节角度变量θi已知的情况下，构建各个关节点ci以及从动臂末端bi在以动平台笛卡尔坐标系o2-x2y2z2的基准坐标系三维空间内的实时坐标计算模型，最终构建一元二次方程求解得到不含有未知数，仅以r、r、l1、l2这些蜘蛛机械手已知物理参数以及实时监测的各个从动臂末端形成的动平台中心点位置坐标(x2,y2,z2)表达并得到具体数值的第i个驱动臂末端bi所在的第i个关节点ci与定平台所在x1y1平面实时夹角θi，再进一步构建在拾取目标物后，使动平台中心点始终保持受力均衡限定条件下的最优实时夹角θi的求解模型，经过动平台中心点终止移动判定阈值计算函数f(x2,best,y2,best,z2,best)值的求解，判定其是否小于可以输出求解得到的最优关节角度变量θi,best的限定值0.10，进而判断是否需要重复迭代求解最优关节角度变量直至符合要求，最终，利用求解结果进一步计算各个驱动臂末端ai、各个从动臂末端bi以及各个关节点ci的最优实时位置，形成最优实时行走轨迹。
82、本发明提供的方法仅仅需要监测动平台中心点的实时位置坐标这一个变量，通过s2-s3步骤的构建相关臂的点位置坐标，利用从动臂长度l2恒定的关系，求解得到实时关节角度变量，进一步通过s4步骤求解在平衡方程限定条件下构建最优实时关节角度变量求解模型，无需多个传感器检测各个驱动臂末端ai、各个从动臂末端bi以及各个关节点ci的最优实时位置以及各个关节点相对于定平台所在平面x1y1的实时偏折角度，即可求解最优实时行走轨迹，避免了多数据计算所带来的计算冗繁，进而所需芯片计算容量大且输出结果缓慢，动作执行出现时延的缺陷。整个模型轻量化设计，且提高了计算结果的准确性。
1.蜘蛛机械手行走路径控制方法，所述蜘蛛机械手用于在上料区域拾取目标物并按照预定轨迹移动至下料区域目标位置下料，其特征在于，包括以下步骤：
2.根据权利要求1所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s2步骤包括以下步骤：
3.根据权利要求2所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s3步骤包括以下步骤：
4.根据权利要求3所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s4步骤包括以下步骤：
5.根据权利要求4所述的一种蜘蛛机械手行走路径控制方法，其特征在于，所述s44步骤中的正定质量矩阵m(θ)计算公式如下：
6.根据权利要求4所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s41步骤中，每个从动臂机构的惯性矩ibi的计算公式如下：
7.根据权利要求4所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s45步骤中的所述蜘蛛机械手单次搬运目标物过程中运动做功能ek计算公式如下：
8.根据权利要求4所述的蜘蛛机械手行走路径控制方法，其特征在于，所述s45步骤中的所述蜘蛛机械手单次搬运目标物过程中克服目标物及自身重量做功能es计算公式如下：
9.采用如权利要求1-8任一方法的蜘蛛机械手行走路径控制系统，其特征在于，包括动平台位置监测模块、关节点夹角计算模块、关节点位置计算模块、力矩平衡最优关节点角度计算模块和路径动态规划模块；
10.采用如权利要求1-8任一所述方法的蜘蛛机械手，其特征在于，包括三个铝型材横梁(1)、三个六边形固定块(2)、三个步进伺服电机(3)、三个驱动臂(4)、与三个驱动臂(4)末端连接的三个并联从动臂机构(5)，三个从动臂机构(5)的从动臂末端与同一承载盘(6)连接；每个步进伺服电机(3)的输出端与相应的驱动臂(4)起始端ai固定连接，并使相应的驱动臂起始端ai位于所在铝型材横梁(1)的中点所在竖直方向正下方，三个驱动臂起始端ai形成半径为r的定平台；每个驱动臂(4)末端均与相应的从动臂机构(5)的从动臂起始端相连接形成关节点ci，每个从动臂机构(5)的从动臂末端bi与同一承载盘(6)连接形成半径为r的动平台；
