[發明專利]一種繩驅機械臂三維臂型測量方法及裝置

有效

• 申請號: 202010032836.5
• 申請日: 2020-01-13
• 公開（公告）號: CN111238367B
• 公開（公告）日: 2021-04-02
• 發明（設計）人: 孟得山;王學謙;梁斌;黃少平;米奕霖;徐文福
• 申請（專利權）人: 清華大學深圳國際研究生院
• 主分類號: G01B11/00
• 分類號: G01B11/00;G01B11/24;G01B11/26
• 代理公司: 深圳新創友知識產權代理有限公司 44223
• 代理人: 王震宇

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：