逆解计算偏置机械臂关节角度值的方法、装置及计算设备制造方法及图纸

本发明专利技术涉及机器人控制技术领域，尤其公开了一种逆解计算偏置机械臂关节角度值方法、装置及计算设备，其中，方法包括：获取所述机械臂的末端位姿矩阵T和臂角

【技术实现步骤摘要】
逆解计算偏置机械臂关节角度值的方法、装置及计算设备
本专利技术实施例涉及机器人控制
，特别是涉及一种逆解计算偏置机械臂关节角度值的方法、装置及计算设备。
技术介绍
近年来，随着工程技术的发展，六自由度工业机器人已经成熟应用到工业生产中，该种机器人在有限的空间范围内可以达到任意位姿，能够胜任一定的抓取、焊接、涂胶等工作。然而，工业机器人的应用环境严格控制，一般无障碍物且在围栏内工作，这是因为对于一个末端位姿，六自由度机器人只有有限数量的解，无法避开障碍物。然而随着服务型机器人进入家庭的意愿愈发强烈，七自由度这种拟人结构的机械臂应用越来越广泛。本专利技术的专利技术人在实现本专利技术的过程中，发现：这种七自由度冗余结构的逆解数量有无数个，造成了逆运动学过程异常复杂，臂角法较好的解决了该类问题，然而，臂角法仅适用于无偏置(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的位置坐标值；当臂本文档来自技高网...

【技术保护点】
1.一种逆解计算偏置机械臂关节角度值的方法，其特征在于，所述方法包括：获取所述机械臂的末端位姿矩阵T和臂角

【技术特征摘要】
1.一种逆解计算偏置机械臂关节角度值的方法，其特征在于，所述方法包括：获取所述机械臂的末端位姿矩阵T和臂角确定与所述机械臂的肘关节相对应的空间圆方程；根据所述空间圆方程、所述臂角和所述末端位姿矩阵T，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节为在所述机械臂之中位置比所述肘关节更靠前以及转动会影响所述肘关节的位置的其它关节，所述前端次级关节为在所述肘关节与前端位置关节之间，并且转动对所述肘关节的位置无影响的其它关节；根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵T，求解得到所述机械臂的后端关节的角度值，所述后端关节为比所述肘关节更靠近所述机械臂末端的其它关节。2.根据权利要求1所述的方法，其特征在于，所述确定与所述机械臂的肘关节相对应的空间圆方程的步骤包括：获取当所述机械臂的臂角为零度时，所述前端位置关节在机械臂的臂角的参考平面的角度值；根据所述前端位置关节在机械臂的臂角的参考平面的角度值，求解肘关节在所述参考平面的位置坐标值Pe_rp、空间圆心C点的位置坐标值Pc以及空间圆的半径R；根据所述肘关节在参考平面的位置坐标值Pe_rp、所述C点的位置坐标值Pc以及所述空间圆的半径R，得到空间圆方程。3.根据权利要求2所述的方法，其特征在于，所述前端位置关节包括第一关节和第二关节，当所述臂角为零度时，所述第一关节在所述参考平面的角度值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的位置坐标值。4.根据权利要求3所述的方法，其特征在于，所述肘关节在参考平面的位置坐标值Pe_rp计算公式为：所述C点的位置坐标值Pc计算公式为：Pc＝Ps+(Lsw-Lcw)*(Pw-Ps)/Lsw；所述空间圆的半径R的计算公式为：R＝Lew*sin(acos((Lew^2+Lsw^2-Lse^2)/(2*Lew*Lsw)))。5.根据权利要求4所述的方法，其特征在于，所述空间圆方程为：其中，u为向量CE’，v为向量CO叉乘向量u，Phi为臂角6.根据权利要求2-5中任意一项所述的方法，其特征在于，所述根据所述空间圆方程、所述臂角和末端位姿矩阵T，求解得到所述前端位置关节、前端次级关节和肘关节的角度值的步骤包括：根据所述空间圆方程和所述臂角计算所述机械臂的肘关节的位置；根据所述末端位姿矩阵T，建立所述机械臂的前端位置关节与肘关节之间的第一变换矩阵；根据所述第一变换矩阵，并且结合所述肘关节的位置，计算所述前端位置关节的角度；根据所述前端位置关节的角度，计算所述前端次级关节的位置；根据所述前端次级关节的位置、肘关节的位置以及腕关节的位置，计算所述肘关节的角度值。7.根据权利要求6所述的方法，其特征在于，根据所述前端位置关节、前端次级关节和肘关节的角度值，并且结合所述末端位姿矩阵T，求解得到所述机械臂的后端关节的角度值包括：根据末端位姿矩阵T，构建所述后端关节与所述前端位置关节、前端次级关节之间的第二变换矩阵；根据所述第二变换矩阵，并且结合所述前端位置关节、前端次级关节和肘关节的角度值，计算得到所述后端位置关节的角度值。8.根据权利要求7所述的方法，其特征在于，所述后端关节包括第五关节、第六关节和第七关节，所述第五关节的角度值q5、第六关节的角度值q6和第七关节的角度值q7的计算公式分别为：9.一种逆解计算偏置机械臂关节角度值装置，其特征在于，所述装置包括：获取模块，用于获取所述机械臂的末端位姿矩阵T和臂角确定模块，用于确定与所述机械臂的肘关节相对应的空间圆方程；第一计算模块，用于根据所述空间圆方程、所述臂角和所述末端位姿矩阵T，求解得到前端位置关节、前端次级关节和肘关节的角度值，其中，所述前端位置关节...

【专利技术属性】
技术研发人员：王天昊，徐慎华，
申请(专利权)人：深圳前海达闼云端智能科技有限公司，
类型：发明
国别省市：广东,44