逆解计算偏置机械臂关节角度值的方法、装置及计算设备与流程

本发明实施例涉及机器人控制技术领域，特别是涉及一种逆解计算偏置机械臂关节角度值的方法、装置及计算设备。

背景技术：

近年来，随着工程技术的发展，六自由度工业机器人已经成熟应用到工业生产中，该种机器人在有限的空间范围内可以达到任意位姿，能够胜任一定的抓取、焊接、涂胶等工作。然而，工业机器人的应用环境严格控制，一般无障碍物且在围栏内工作，这是因为对于一个末端位姿，六自由度机器人只有有限数量的解，无法避开障碍物。然而随着服务型机器人进入家庭的意愿愈发强烈，七自由度这种拟人结构的机械臂应用越来越广泛。

本发明的发明人在实现本发明的过程中，发现：这种七自由度冗余结构的逆解数量有无数个，造成了逆运动学过程异常复杂，臂角法较好的解决了该类问题，然而，臂角法仅适用于无偏置(srs)构型的机械臂，对于有偏置的构型并不能求解，本发明提供了一种改进的臂角法，使其能够求解偏置构型的七自由度机械臂。

技术实现要素：

鉴于上述问题，提出了本发明实施例以便提供一种克服上述问题或者至少部分地解决上述问题的基于改进臂角法的逆解计算偏置机械臂关节角度值方法、装置及计算设备。

为解决上述技术问题，本发明实施例采用的一个技术方案是：提供一种方法，所述方法包括：获取所述机械臂的末端位姿矩阵t和臂角确定与所述机械臂的肘关节相对应的空间圆方程；根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节为在所述机械臂之中位置比所述肘关节更靠前以及转动会影响所述肘关节的位置的其它关节，所述前端次级关节为在所述肘关节与前端位置关节之间，并且转动对所述肘关节的位置无影响的其它关节；根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，所述后端关节为比所述肘关节更靠近所述机械臂末端的其它关节。

可选的，所述根据所述末端位姿矩阵t和臂角确定与所述机械臂的肘关节相对应的空间圆方程的步骤包括：获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值pe_rp、空间圆心c点的位置坐标值pc以及空间圆的半径r；根据所述肘关节在参考平面的位置坐标值pe_rp、所述c点的位置坐标值pc以及所述空间圆的半径r，得到空间圆方程。

可选的，所述前端位置关节包括第一关节和第二关节，当所述臂角为零度时，所述第一关节在所述参考平面的角度值q1_rp计算公式为：q1_rp＝atan2(pw(2),pw(1))，其中，pw为末端w的位置坐标值；当臂角为零度时，所述第二关节在所述参考平面的角度值q2_rp的计算公式为

q2_rp＝pi-(atan2(la2,la1)+acos((lse^2+lsw^2-lew^2)/

(2*lse*lsw))-atan2(pw(3)-ps(3),sqrt((pw(1)-ps(1))^2+(pw(2)-ps(2))^2)))

其中，pi为圆周率∏，la1为os的长度，la2为se的长度，lse为se的长度，lsw为sw的长度，lew为ew的长度，pw为腕点w的位置坐标值，ps为点s的位置坐标值。

可选的，所述肘关节在参考平面的位置坐标值pe_rp计算公式为：

pe_rp＝[-la1*cos(q1_rp)*cos(q2_rp)+la2*cos(q1_rp)*sin(q2_rp)；

la2*sin(q1_rp)*sin(q2_rp)-la1*cos(q2_rp)*sin(q1_rp)；；

-la1*sin(q2_rp)-la2*cos(q2_rp)]

所述c点的位置坐标值pc计算公式为：

pc＝ps+(lsw-lcw)*(pw-ps)/lsw；

所述空间圆的半径r的计算公式为：

r＝lew*sin(acos((lew^2+lsw^2-lse^2)/(2*lew*lsw)))。

可选的，所述空间圆方程为：

circle＝[pc(1)+r*cos(phi)*u(1)+r*sin(phi)*v(1)；

pc(2)+r*cos(phi)*u(2)+r*sin(phi)*v(2)；，其中，u为向量ce’，

pc(3)+r*cos(phi)*u(3)+r*sin(phi)*v(3)]

v为向量co叉乘向量u，phi为臂角

可选的，所述根据所述空间圆方程、所述臂角和末端位姿矩阵t，求解得到所述前端位置关节、前端次级关节和肘关节的角度值的步骤包括：根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；根据所述末端位姿矩阵t，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；根据所述前端位置关节的角度，计算所述前端次级关节的位置；根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。

可选的，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值包括：根据末端位姿矩阵t，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。

可选的，所述后端关节包括第五关节、第六关节和第七关节，所述第五关节的角度值q5、第六关节的角度值q6和第七关节的角度值q7的计算公式分别为：

为解决上述技术问题，本发明实施例采用的另一个技术方案是：提供一种逆解计算偏置机械臂关节角度值装置，所述装置包括：获取模块，用于获取所述机械臂的末端位姿矩阵t和臂角确定模块，用于确定与所述机械臂的肘关节相对应的空间圆方程；

第一计算模块，用于根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节为在所述机械臂之中位置比所述肘关节更靠前以及转动会影响所述肘关节的位置的其它关节，所述前端次级关节为在所述肘关节与前端位置关节之间，并且转动对所述肘关节的位置无影响的其它关节；第二计算模块，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，所述后端关节为比所述肘关节更靠近所述机械臂末端的其它关节。

可选的，所述确定模块包括：角度值获取单元，用于获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；第一求解单元，用于根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值pe_rp、空间圆心c点的位置坐标值pc以及空间圆的半径r；第二求解单元，用于根据所述肘关节在参考平面的位置坐标值pe_rp、所述c点的位置坐标值pc以及所述空间圆的半径r，得到空间圆方程。

可选的，所述前端位置关节包括第一关节和第二关节，所述角度值获取单元具体用于：当所述臂角为零度时，所述第一关节在所述参考平面的角度值q1_rp计算公式为：q1_rp＝atan2(pw(2),pw(1))，其中，pw为末端w的位置坐标值；当臂角为零度时，所述第二关节在所述参考平面的角度值q2_rp的计算公式为

q2_rp＝pi-(atan2(la2,la1)+acos((lse^2+lsw^2-lew^2)/

(2*lse*lsw))-atan2(pw(3)-ps(3),sqrt((pw(1)-ps(1))^2+(pw(2)-ps(2))^2)))

其中，pi为圆周率∏，la1为os的长度，la2为se的长度，lse为se的长度，lsw为sw的长度，lew为ew的长度，pw为腕点w的位置坐标值，ps为点s的位置坐标值。

可选的，所述肘关节在参考平面的位置坐标值pe_rp计算公式为：

pe_rp＝[-la1*cos(q1_rp)*cos(q2_rp)+la2*cos(q1_rp)*sin(q2_rp)；

la2*sin(q1_rp)*sin(q2_rp)-la1*cos(q2_rp)*sin(q1_rp)；；

-la1*sin(q2_rp)-la2*cos(q2_rp)]

所述c点的位置坐标值pc计算公式为：

pc＝ps+(lsw-lcw)*(pw-ps)/lsw；

所述空间圆的半径r的计算公式为：

r＝lew*sin(acos((lew^2+lsw^2-lse^2)/(2*lew*lsw)))。

可选的，所述空间圆方程为：

circle＝[pc(1)+r*cos(phi)*u(1)+r*sin(phi)*v(1)；

pc(2)+r*cos(phi)*u(2)+r*sin(phi)*v(2)；，其中，u为向量ce’，

pc(3)+r*cos(phi)*u(3)+r*sin(phi)*v(3)]

v为向量co叉乘向量u，phi为臂角

可选的，所述第一计算模块包括：第三求解单元，用于根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；第一建立单元，用于根据所述末端位姿矩阵t，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；第四求解单元，用于根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；第五求解单元，用于根据所述前端位置关节的角度，计算所述前端次级关节的位置；第六求解单元，用于根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。

可选的，所述第二计算模块包括：第二构建单元，用于根据末端位姿矩阵t，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；第七求解单元，用于根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。

可选的，所述后端关节包括第五关节、第六关节和第七关节，所述第五关节的角度值q5、第六关节的角度值q6和第七关节的角度值q7的计算公式分别为：

为解决上述技术问题，本发明实施例采用的另一个技术方案是：还提供一种计算设备，包括处理器、存储器、通信接口和通信总线，所述处理器、所述存储器和所述通信接口通过所述通信总线完成相互间的通信；

所述存储器用于存放至少一可执行指令，所述可执行指令使所述处理器执行所述逆解计算偏置机械臂关节角度值方法对应的操作。

为解决上述技术问题，本发明实施例采用的又一个技术方案是：还提供一种计算机存储介质，所述存储介质中存储有至少一可执行指令，所述可执行指令使处理器执行所述的逆解计算偏置机械臂关节角度值方法对应的操作。

为解决上述技术问题，本发明实施例采用的再一个技术方案是：提供一种机械臂，机械臂包括机械本体和上述计算设备。

为解决上述技术问题，本发明实施例采用的还一个技术方案是：提供一种机器人，包括上述机械臂。

本发明实施例的有益效果是：区别于现有技术的情况，本发明实施例基于改进的臂角法，确定与机械臂的肘关节相对应的空间圆方程；根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，实现了七自由度肩部偏置机械臂逆解方法求解。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施例。

附图说明

通过阅读下文优选实施例的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施例的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1是本本发明实施例七自由度偏置机械臂的结构示意图；

图2是本发明实施例逆解计算偏置机械臂关节角度值方法的流程图；

图3是本发明方法实施例中步骤s20的细化流程示意图；

图4是本发明方法实施例中步骤s30的细化流程示意图；

图5是本发明方法实施例中步骤s40的细化流程示意图；

图6是本发明实施例的逆解计算偏置机械臂关节角度值装置的功能框图；

图7是本发明计算设备实施例的结构示意图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

机械臂指是多个关节组成的，通过关节之间的联动，实现机械臂的转动的机械设备。请参阅图1，图1是本发明实施例七自由度偏置机械臂的结构示意图。七自由度偏置机械臂包括前端位置关节11、前端次级关节12、肘关节13和后端关节14，前端位置关节11为在机械臂之中位置比肘关节13更靠前以及转动会影响肘关节13的位置的其它关节，前端次级关节12为在肘关节13与前端位置关节11之间，并且转动对肘关节13的位置无影响的其它关节。前端位置关节11和前端次级关节12组成七自由度偏置机械臂的肩关节。后端关节14为比肘关节13更靠近机械臂末端的其它关节，后端关节14为七自由度偏置机械臂的腕关节。具体的，前端位置关节11包括第一关节111和第二关节112，前端次级关节12为第三关节，肘关节13为第四关节，后端关节14包括第五关节141、第六关节142和第七关节143。

第一关节111和第二关节112的旋转轴交于一点为原点o，第三关节03的转轴中点为肩点s，肘关节13的转轴中点为肘点e，腕关节组567的旋转轴交于一点为腕点w，由肩点s、肘点e和腕点w确定的平面为臂角平面。由向量sw与七自由度偏置机械臂基坐标z轴向量确定的平面为参考平面，向量sw为肩点s指向腕点w的向量。参考平面与臂角平面绕向量sw右手定则的夹角为臂角。srs构型机械臂的肩点s与原点o重合，由于七自由度偏置机械臂的原点o、肩点s、肘点e和腕点w大部分情况无法构成平面，在本发明实施例中，定义当第二关节112与第四关节04平行，且第三关节03角度为零度时，该关节角度下，原点o、肩点s、肘点e和腕点w在同一平面，该平面即为参考平面，原点o、肘点e和腕点w确定的平面为臂角平面。当臂角为零度时，臂角平面与参考平面重合，当臂角不为零度时，s’不在oe’w平面上，且定义oe’w为臂角平面，臂角为定义os长度为los＝a1，se长度lse＝a2，ew长度lew＝a3，各点在x轴、y轴以及z轴相应坐标值为px＝[px(1)；px(2)；px(3)]。

图2为本发明实施例逆解计算偏置机械臂关节角度值方法的流程图。如图2所示，该方法包括以下步骤：

步骤s10：获取所述机械臂的末端位姿矩阵t和臂角

其中，在本步骤中，所述机械臂的末端位姿矩阵t表示成齐次矩阵形式，如式(1)所示：

其中，rxy表示姿态信息，px、py、pz表示位置信息。

其中，臂角为根据末端姿态的当前角度计算出的oe’w平面与参考平面的夹角。

步骤s20：确定与所述机械臂的肘关节相对应的空间圆方程。

如图3所示，为本发明方法实施例中步骤s20的细化流程示意图，所述步骤s20包括：

步骤s201：获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；

具体地，在本步骤中，当所述臂角为零度时，所述第一关节在所述参考平面的角度值q1_rp计算公式为式(2)所示：

q1_rp＝atan2(pw(2),pw(1))(2)

其中，pw为末端w的位置坐标值；

当臂角为零度时，所述第二关节在所述参考平面的角度值q2_rp的计算公式为式(3)所示：

q2_rp＝pi-(atan2(la2,la1)+acos((lse^2+lsw^2-lew^2)/

(2*lse*lsw))-atan2(pw(3)-ps(3),sqrt((pw(1)-ps(1))^2+(pw(2)-ps(2))^2)))(3)

其中，pi为圆周率∏，la1为os的长度，la2为se的长度，lse为se的长度，lsw为sw的长度，lew为ew的长度，pw为腕点w的位置坐标值，ps为点s的位置坐标值。

根据机器人正运动学可知，第四关节04的位置坐标仅与第一关节111和第二关节112的实际角度值有关，即第四关节04的位置坐标为式(4)所示：

其中，q1和q2分别为第一关节111和第二关节112的角度值，为第四关节04的位置坐标值。

步骤s202：根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值pe_rp、空间圆心c点的位置坐标值pc以及空间圆的半径r；

具体地，在本步骤中，肘点e在参考平面的位置坐标值pe_rp计算公式如式(5)所示：

pe_rp＝[-la1*cos(q1_rp)*cos(q2_rp)+la2*cos(q1_rp)*sin(q2_rp)；

la2*sin(q1_rp)*sin(q2_rp)-la1*cos(q2_rp)*sin(q1_rp)；

-la1*sin(q2_rp)-la2*cos(q2_rp)](5)

c点的位置坐标值pc计算公式如式(6)所示：

pc＝ps+(lsw-lcw)*(pw-ps)/lsw(6)

所述空间圆是指肘点e沿ow旋转形成的圆，所述空间圆的半径r的计算公式如式(7)所示：

r＝lew*sin(acos((lew^2+lsw^2-lse^2)/(2*lew*lsw)))(7)。

步骤s203：根据所述肘关节在参考平面的位置坐标值pe_rp、所述c点的位置坐标值pc以及所述空间圆的半径r，得到空间圆方程。

在本步骤中，根据式(5)、式(6)和式(7)，得到空间圆关于臂角的方程如式(8)所示：

circle＝[pc(1)+r*cos(phi)*u(1)+r*sin(phi)*v(1)；

pc(2)+r*cos(phi)*u(2)+r*sin(phi)*v(2)；

pc(3)+r*cos(phi)*u(3)+r*sin(phi)*v(3)](8)

其中，u(1)、u(2)和u(3)表示向量u在原点坐标系中三个坐标方向的分量；同理，v(1)、v(2)和v(3)也是表示向量v在原点坐标系中三个坐标方向的分量，参照图1所示，u为向量ce’，v为向量co叉乘向量u，phi为臂角

步骤s30：根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值。

如图4所示，为本发明方法实施例中步骤s30的细化流程示意图。具体地，步骤s30包括：

步骤s301：根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；

具体地，在本步骤中，当已知臂角时，根据空间圆关于臂角的方程可计算出第四关节04的位置pe。

步骤s302：根据所述末端位姿矩阵t，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；

所述第一变换矩阵为：

步骤s303：根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；

具体地，在本步骤中，通过正运动学可知，p4仅是关于q1、q2的函数，因此通过求解方程组即可求得第一关节111的角度值q1和第二关节112的角度值q2。

步骤s304：根据所述前端位置关节的角度，计算所述前端次级关节的位置；

具体地，在本步骤中，当求得q1、q2后，根据即求得s点的位置ps。根据正运动学，且联合可求解第三关节03的角度值q3。

步骤s305：根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。

具体地，在本步骤中，根据ps、pe、pw三点坐标，再结合余弦定理，即求得第四关节04的角度值q4。

步骤s40：根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值。

如图5所示，为本发明方法实施例中步骤s40的细化流程示意图。具体地，步骤s40包括：

s401：根据末端位姿矩阵t，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；

所述第二变换矩阵为

s402：根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。

根据第二变换矩阵得到：第五关节141、第六关节142和第七关节143的角度值q5、q6、q7的计算公式分别如式(9)、式(10)和式(11)所示：

根据以上步骤即可求得机械臂7个关节的角度值。

在一些实施例中，所述方法还包括：

当得到关节的角度值q1、q2、q3、q4、q5、q6和q7之后，还可以判断各个关节的角度值是否超出其自身的角度极限，若超出，则返回不能够满足当前臂角的告警提示，若没有超出，则输出所述机械臂7个关节的角度值q1、q2、q3、q4、q5、q6和q7。

根据本发明实施例以上方法，发明人在matlab环境中设计了实验进行验证，设计连杆参数为la1＝0.0478、la2＝0.2157、la3＝0.2160。验证方法为随机获取一组关节角度，通过正运动学求得末端位姿及该组角度的臂角值，再将正运动学输出的末端位姿及臂角值输入到该发明提出的逆运动学方法，并查看数据是否匹配，若数据完全相同，则说明方法正确，实验数据如下：

7个关节的角度值矩阵为：

q＝[1.6421；1.6518；1.2257；-0.8096；-0.1709；-1.3410；0.3157]，

根据正运动学求得末端位姿矩阵为：

臂角为：5.2401。

再根据本发明实施例提供基于改进臂角法的七自由度肩部偏置机械臂逆解方法，反解得到7个关节的角度值矩阵为：

ik_q＝[1.6421011534；1.6517788477；1.2257063646；-0.80964077553；-0.17094599210；-1.3409678505；0.31572216493]。

由此可见，在本发明实施例中，基于改进的臂角法，根据末端位姿矩阵t和臂角确定与机械臂的肘关节相对应的空间圆方程；根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，实现了七自由度肩部偏置机械臂逆解方法求解。并且实验结果显示，通过本发明基于改进的臂角法的七自由度肩部偏置机械臂逆解方法求解的结果与预先设计的实验数据一致，说明本发明实施例基于改进臂角法的七自由度肩部偏置机械臂逆解方法正确。

参照图6所示，图6是本发明实施例的逆解计算偏置机械臂关节角度值装置的功能框图。所述逆解计算偏置机械臂关节角度值装置10包括但不限于：获取模块10、确定模块20、第一计算模块30、第二计算模块40和输出模块50。参照图7所示，本发明所称的模块是指一种能够被计算设备的处理器602执行并且能够完成固定功能的一系列计算机程序指令段，其存储在存储器606中。

获取模块10，用于获取所述机械臂的末端位姿矩阵t和臂角

确定模块20，用于确定与所述机械臂的肘关节相对应的空间圆方程；

第一计算模块30，用于根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节为在所述机械臂之中位置比所述肘关节更靠前以及转动会影响所述肘关节的位置的其它关节，所述前端次级关节为在所述肘关节与前端位置关节之间，并且转动对所述肘关节的位置无影响的其它关节；

第二计算模块40，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，所述后端关节为比所述肘关节更靠近所述机械臂末端的其它关节。

在其中一个实施例中，所述确定模块20包括：

角度值获取单元201，用于获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；

第一求解单元202，用于根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值pe_rp、空间圆心c点的位置坐标值pc以及空间圆的半径r；

第二求解单元203，用于根据所述肘关节在参考平面的位置坐标值pe_rp、所述c点的位置坐标值pc以及所述空间圆的半径r，得到空间圆方程。

在其中一个实施例中，所述第一计算模块30包括：

第三求解单元301，用于根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；

第一构建单元302，用于根据所述末端位姿矩阵t，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；

第四求解单元303，用于根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；

第五求解单元304，用于根据所述前端位置关节的角度，计算所述前端次级关节的位置；

第六求解单元305，用于根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。

在其中一个实施例中，所述第二计算模块40包括：

第二构建单元401，用于根据末端位姿矩阵t，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；

第七求解单元402，用于根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。

在其中一个实施例中，所述装置还包括输出模块50，所述输出模块50用于当臂角满足当前角度变化需求时，输出所述机械臂7个关节的角度值q1、q2、q3、q4、q5、q6和q7。

关于上述各个模块的具体结构和工作原理可参照方法实施例中相应步骤的描述，此处不再赘述。

本申请实施例提供了一种非易失性计算机存储介质，所述计算机存储介质存储有至少一可执行指令，该计算机可执行指令可执行上述任意方法实施例中的逆解计算偏置机械臂关节角度值方法。

图7为本发明计算设备实施例的结构示意图，本发明具体实施例并不对计算设备的具体实现做限定。

如图7所示，该计算设备可以包括：处理器(processor)602、通信接口(communicationsinterface)604、存储器(memory)606、以及通信总线608。

其中：

处理器602、通信接口604、以及存储器606通过通信总线608完成相互间的通信。

通信接口604，用于与其它设备比如客户端或其它服务器等的网元通信。

处理器602，用于执行程序610，具体可以执行基于改进的臂角法的七自由度肩部偏置机械臂逆解方法实施例中的相关步骤。

具体地，程序610可以包括程序代码，该程序代码包括计算机操作指令。

处理器602可能是中央处理器cpu，或者是特定集成电路asic(applicationspecificintegratedcircuit)，或者是被配置成实施本发明实施例的一个或多个集成电路。计算设备包括的一个或多个处理器，可以是同一类型的处理器，如一个或多个cpu；也可以是不同类型的处理器，如一个或多个cpu以及一个或多个asic。

存储器606，用于存放程序610。存储器606可能包含高速ram存储器，也可能还包括非易失性存储器(non-volatilememory)，例如至少一个磁盘存储器。

结合图1所示，程序610具体可以用于使得处理器602执行以下操作：

获取所述机械臂的末端位姿矩阵t和臂角

确定与所述机械臂的肘关节相对应的空间圆方程；

根据所述空间圆方程、所述臂角和所述末端位姿矩阵t，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节为在所述机械臂之中位置比所述肘关节更靠前以及转动会影响所述肘关节的位置的其它关节，所述前端次级关节为在所述肘关节与前端位置关节之间，并且转动对所述肘关节的位置无影响的其它关节；

根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值，所述后端关节为比所述肘关节更靠近所述机械臂末端的其它关节。

在一种可选的方式中，程序610具体可以进一步用于使得处理器602执行以下操作：确定与所述机械臂的肘关节相对应的空间圆方程的步骤包括：

获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；

根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值pe_rp、空间圆心c点的位置坐标值pc以及空间圆的半径r；

根据所述肘关节在参考平面的位置坐标值pe_rp、所述c点的位置坐标值pc以及所述空间圆的半径r，得到空间圆方程。

在一种可选的方式中，所述前端位置关节包括第一关节和第二关节，

当所述臂角为零度时，所述第一关节在所述参考平面的角度值q1_rp计算公式为：q1_rp＝atan2(pw(2),pw(1))，其中，pw为末端w的位置坐标值；

当臂角为零度时，所述第二关节在所述参考平面的角度值q2_rp的计算公式为

q2_rp＝pi-(atan2(la2,la1)+acos((lse^2+lsw^2-lew^2)/

(2*lse*lsw))-atan2(pw(3)-ps(3),sqrt((pw(1)-ps(1))^2+(pw(2)-ps(2))^2)))

其中，pi为圆周率∏，la1为os的长度，la2为se的长度，lse为se的长度，lsw为sw的长度，lew为ew的长度，pw为腕点w的位置坐标值，ps为点s的位置坐标值。

在一种可选的方式中，所述肘关节在参考平面的位置坐标值pe_rp计算公式为：

pe_rp＝[-la1*cos(q1_rp)*cos(q2_rp)+la2*cos(q1_rp)*sin(q2_rp)；

la2*sin(q1_rp)*sin(q2_rp)-la1*cos(q2_rp)*sin(q1_rp)；

-la1*sin(q2_rp)-la2*cos(q2_rp)]；

所述c点的位置坐标值pc计算公式为：

pc＝ps+(lsw-lcw)*(pw-ps)/lsw；

所述空间圆的半径r的计算公式为：

r＝lew*sin(acos((lew^2+lsw^2-lse^2)/(2*lew*lsw)))。

在一种可选的方式中，所述空间圆方程为：

circle＝[pc(1)+r*cos(phi)*u(1)+r*sin(phi)*v(1)；

pc(2)+r*cos(phi)*u(2)+r*sin(phi)*v(2)；

pc(3)+r*cos(phi)*u(3)+r*sin(phi)*v(3)]，其中，u为向量ce’，v为向量co叉乘向量u，phi为臂角

在一种可选的方式中，程序610具体可以进一步用于使得处理器602执行以下操作：所述根据所述空间圆方程、所述臂角和末端位姿矩阵t，求解得到所述前端位置关节、前端次级关节和肘关节的角度值的步骤包括：

根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；

根据所述末端位姿矩阵t，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；

根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；

根据所述前端位置关节的角度，计算所述前端次级关节的位置；

根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。

在一种可选的方式中，程序610具体可以进一步用于使得处理器602执行以下操作：根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵t，求解得到所述机械臂的后端关节的角度值包括：

根据末端位姿矩阵t，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；

根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。

在一种可选的方式中，所述后端关节包括第五关节、第六关节和第七关节，所述第五关节的角度值q5、第六关节的角度值q6和第七关节的角度值q7的计算公式分别为：

在一种可选的方式中，程序610具体可以进一步用于使得处理器602执行以下操作：当臂角满足当前角度变化需求时，输出所述机械臂7个关节的角度值。

本发明还提供了一种机械臂，机械臂包括机械臂本体和上述计算设备。对于计算设备具体实现可以参考上述实施例，此处不再一一赘述。

本发明还提供了一种机器人，包括一种机械臂。所述机械臂与上述实施例中机械臂的结构与功能一致，对于机械臂的结构与功能可参阅上述实施例，此处不再一一赘述。

在此提供的算法和显示不与任何特定计算机、虚拟系统或者其它设备固有相关。各种通用系统也可以与基于在此的示教一起使用。根据上面的描述，构造这类系统所要求的结构是显而易见的。此外，本发明也不针对任何特定编程语言。应当明白，可以利用各种编程语言实现在此描述的本发明的内容，并且上面对特定语言所做的描述是为了披露本发明的最佳实施例。

在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

类似地，应当理解，为了精简本公开并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施例的权利要求书由此明确地并入该具体实施例，其中每个权利要求本身都作为本发明的单独实施例。

本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。

此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。

本发明的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(dsp)来实现根据本发明实施例的逆解计算偏置机械臂关节角度值装置中的一些或者全部部件的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如，计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。

应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。