[发明专利]一种面向SRS仿人手臂的自运动角计算方法

摘要

一种面向SRS仿人手臂的自运动角计算方法，它包括如下步骤步骤一建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、腕关节坐标系；步骤二由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动角；步骤三由步骤二的自运动角和工具坐标系位姿计算关节空间运动角度；步骤四将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系位姿的运动。本发明给出了一种冗余度机械臂的逆运动学求法，和传统的计算方法相比，算法简单，计算速度快，可以得到解析解，在降低了求解难度的同时对关节运动做了一定的优化。

主权项

一种面向SRS仿人手臂的自运动角计算方法，其特征在于：它包括如下步骤：步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、腕关节坐标系；步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动角；步骤三：由步骤二给定工具坐标系位姿时的自运动角和工具坐标系位姿计算关节空间运动角度；步骤四：将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系位姿的运动；其中，步骤二中所述PID算法具体过程如下：1)由当前状态的自运动角θn和给定的执行末端点的位姿确定各个关节运动角度；2)将得到的关节运动角度，计算腕关节姿态矩阵的Z轴与腕关节中心点矢量构成的平面与当前状态下的大臂小臂平面之间的夹角αn；3)新的自运动角θn+1由如下公式：θn+1＝θn+P*αn+I*(θn–θ0)求出；式中符号说明如下：θn+1为新的自运动角，θn为上一次计算得到的自运动角，θ0为初始状态的自运动角，P、I为可调节参数；4)在内存中存储自运动角θn+1；该算法需给定初始状态时的自运动角θ0，由初始状态时的自运动角θ0按照上面1)‑4)所述递推。