[发明专利]五自由度机械臂关节变量确定方法

权利要求书

1.一种五自由度机械臂关节变量确定方法，其特征在于，包括如下步骤：

(10)机械臂正逆运动学模型建立：根据结构设计确定五自由度机械臂的DH参数，并根据机器人学基础知识，建立五自由度机械臂的正逆运动学求解模型；

(20)机械臂关节量获取：将给定的五自由度机械臂末端在笛卡尔空间下的期望位姿代入五自由度机械臂逆运动学计算公式，求出5个机械臂关节量；

(30)实际可达位姿获取：将所述5个机械臂关节量代入五自由度机械臂正解计算公式，求出五自由度机械臂末端在笛卡尔空间下的实际可达位姿；

(40)位姿误差计算：计算所述实际可达位姿与期望位姿之间的位姿误差；

(50)收敛性判断：根据位姿误差是否低于位姿误差阈值，判断是否收敛；如是，则跳转至(70)关节变量输出步骤；

(60)迭代控制：用根据自由度缺省方向上的自由度缺省偏差量计算得到的控制量，修正期望位姿，跳转至(20)机械臂关节量获取步骤；

(70)关节变量输出：输出5个机械臂关节量。

2.根据权利要求1所述的关节变量确定方法，其特征在于，所述(60)迭代控制步骤包括：

(61)自由度缺省偏差量计算：计算五自由度机械臂期望位姿与实际可达位姿在自由度缺省方向上的自由度缺省偏差量；

(62)自由度缺省控制量计算：将自由度缺省偏差量输入到线性控制器调节迭代步长，计算得到五自由度机械臂末端期望位姿与实际可达位姿在自由度缺省方向上的控制量；

(63)期望位姿修正：根据所述控制量，在自由度缺省方向上将期望位姿修正为新期望位姿；以所述新期望位姿作为五自由度机械臂末端在笛卡尔空间下的期望位姿，跳转至步骤(20)获取机械臂关节量。

3.根据权利要求2所述的关节变量确定方法，其特征在于，所述(63)期望位姿修正步骤包括：

(631)控制量齐次变换矩阵构建：缺省自由度若为平移自由度，则构建旋转矩阵为单位矩阵的齐次变换矩阵；若为转动自由度，则构建平移矩阵为0的位置向量的齐次变换矩阵；

(632)期望位姿右乘：将期望位姿右乘齐次变换矩阵修正得到新期望位姿，跳转至步骤(20)获取机械臂关节量。