[發明專利]一種異構主從系統的運動映射方法及系統

在審

• 申請號: 201711492587.2
• 申請日: 2017-12-30
• 公開（公告）號: CN108107765A
• 公開（公告）日: 2018-06-01
• 發明（設計）人: 李萌;張恩慧;其他發明人請求不公開姓名
• 申請（專利權）人: 北京工業大學
• 主分類號: G05B19/04
• 分類號: G05B19/04
• 代理公司: 北京路浩知識產權代理有限公司 11002
• 代理人: 王瑩;吳歡燕

1.一種異構主從系統的運動映射方法，其特征在于，包括：

S1、獲取主臂的各個連桿的方向數據，并確定所述方向數據對應的主臂姿態；

S2、基于單位對偶四元數，將所述主臂的各個連桿的方向數據作為輸入，采用正向運動學FK和反向運動學IK算法，將所述主臂姿態映射為從臂姿態。

2.根據權利要求1所述的方法，其特征在于，在S1之前，所述方法還包括：

構建從臂模型，使得從臂的各個連桿的方向和主臂的各個連桿的方向一致。

3.根據權利要求2所述的方法，其特征在于，所述構建從臂模型，使得從臂的各個連桿的方向和主臂的各個連桿的方向一致，包括：

在所述從臂模型中構建三自由度的腕關節對應主臂的腕部、構建單自由度的肘關節對應主臂的肘部、構建兩自由度的肩關節對應主臂的肩部；

依次用連桿連接所述肩關節、所述肘關節以及所述腕關節，并調整連桿的方向和主臂的各個連桿的方向一致。

4.根據權利要求3所述的方法，其特征在于，S2包括：

基于單位對偶四元數，將所述主臂的各個連桿的方向數據作為輸入，獲取主臂運動姿態對應的當前運動角度和期望運動角度；

基于所述主臂運動姿態對應的當前運動角度和期望運動角度，利用IK算法，依次計算從臂肩關節、肘關節以及腕關節設置的角度；

基于所述從臂肩關節、肘關節以及腕關節設置的角度，利用FK算法，確定所述從臂姿態，以實現將所述主臂姿態映射為從臂姿態。

5.根據權利要求4所述的方法，其特征在于，所述基于所述主臂運動姿態對應的當前運動角度和期望運動角度，利用IK算法，依次計算從臂肩關節、肘關節以及腕關節設置的角度，包括：

基于所述主臂運動姿態對應的當前運動角度和期望運動角度，利用IK算法，依次確定所述從臂肩關節、肘關節以及腕關節的軸方向向量；

基于所述從臂肩關節、肘關節以及腕關節的軸方向向量，依次計算所述從臂肩關節、肘關節以及腕關節的角增量。

6.根據權利要求4所述的方法，其特征在于，所述基于所述主臂運動姿態對應的當前運動角度和期望運動角度，利用IK算法，依次計算從臂肩關節、肘關節以及腕關節設置的角度，還包括：

在計算過程中對各個角度進行線性插值：

θ i = θ 0 + Σ i = 1 k Δ t · ω i , ]]>

其中，θi和ωi分別是是時間步長上關節的角度值和角速度，θ0是關節的初始角度值，Δt是一個步驟的時間間隔，k為步長。