[发明专利]一种绳驱机械臂三维臂型测量方法及装置

权利要求书

1.一种绳驱机械臂三维臂型测量方法，其特征在于，包括如下步骤：

S1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；

S2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；

S3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θ_i以及关节的旋转角

S4、根据异面直线夹角θ_i以及旋转角确定每个关节的十字轴方向角α_i和β_i；

S5、根据每个关节的十字轴方向角α_i和β_i，从根部往末端迭代得到所述绳驱机械臂的臂型。

2.如权利要求1所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤S2包括：

第i节关节上的两个靶球的坐标分别为和靶球在中轴线的投影距离为d，靶球坐标投影到中轴线的坐标分别对应为和靶球与中轴线构成的平面法向量为n_i，第i节坐标系原点位于第i节与第i-1节的旋转轴中心O_i处，中轴线投影到xoy平面的投影直线与x轴的夹角为与之间的夹角为θ_i；

通过三个约束关系来求解每一个靶球投影点的坐标，如下：

O_i与和在同一个平面上，按照公式(1)计算得到该平面的法向量n_i：

n_i＝O_iP_i¹×P_i¹P_i² (1)

由n_i垂直于向量得到第一个约束关系：

根据投影关系有垂直于得到第二个约束关系：

根据投影距离d得到第三个约束关系：

综合以上三个约束关系求解得到的坐标，同理，求得的坐标。

3.如权利要求1或2所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤S3包括：

将第i节关节的坐标结合第i-1节关节的和坐标，计算夹角θ_i：

根据坐标的逐步递推，得到坐标系变换矩阵，用对应的关节坐标系表示每个关节上的点的坐标；坐标变换公式如下所示：

其中，p_i表示第i个坐标系的点，p_i相对于第i个坐标系的原点Oi的位置关系与第一个关节上的点p₁相对于O₁的位置关系相同，表示从第i-1个坐标系到第i个坐标系的变换矩阵；

根据公式(6)将前面计算得到的每个关节上的靶球投影坐标换算成当前坐标系下的坐标，再将该坐标投影到对应关节的xoy平面上，进而得到投影直线与x轴之间的夹角即关节的旋转角。

4.如权利要求1至2任一项所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤S4包括：

计算x方向以及y方向的旋转矩阵以及平移矩阵T₀：

将旋转矩阵以及平移矩阵T₀相乘得到关节的变换矩阵T_i：

即，将式(7)～(9)代入式(10)得到关节的变换矩阵T_i：

通过式(12)表示的坐标点的对应关系计算得到α_i和β_i：