本發(fā)明涉及上肢康復(fù)，特別涉及一種上肢姿態(tài)計算方法及系統(tǒng)。

背景技術(shù)：

1、用于小臂驅(qū)動的上肢康復(fù)機器人，即康復(fù)機器人僅驅(qū)動患者小臂，通過機械臂帶動小臂、小臂帶動大臂進行康復(fù)運動；該種康復(fù)機器人機械結(jié)構(gòu)動力部分及供電部分離使用者距離更遠，更加安全和舒適。

2、現(xiàn)有技術(shù)當(dāng)中，上肢康復(fù)機器人在實際使用中，需要對患者的康復(fù)效果進行評估、上肢運動空間范圍進行限位，效果評估和空間限位需要獲得患者的上肢姿態(tài)。對于僅驅(qū)動小臂的上肢康復(fù)機器人而言，肘關(guān)節(jié)位置獲取較為容易，肩關(guān)節(jié)中心獲取是有一定難度的，現(xiàn)有技術(shù)及常規(guī)方法僅能夠粗略的確認(rèn)肩關(guān)節(jié)中心位置，忽略了患者使用過程中坐姿變化導(dǎo)致的肩關(guān)節(jié)中心偏移。

技術(shù)實現(xiàn)思路

1、基于此，本發(fā)明的目的是提供一種上肢姿態(tài)計算方法及系統(tǒng)，以解決上述現(xiàn)有技術(shù)當(dāng)中的不足。

2、第一方面，本發(fā)明提供一種上肢姿態(tài)計算方法，應(yīng)用于上肢康復(fù)機器人，所述方法包括：

3、實時獲取小臂運動半徑、上肢康復(fù)機器人的末端中心點坐標(biāo)以及末端中心點至肘關(guān)節(jié)的肘關(guān)節(jié)距離；

4、基于所述末端中心點坐標(biāo)、所述小臂運動半徑、垂直所述末端中心點的運動軌跡的法向量、所述肘關(guān)節(jié)距離以及小臂方向向量計算肘關(guān)節(jié)坐標(biāo)；

5、根據(jù)所述肘關(guān)節(jié)坐標(biāo)以及所述肘關(guān)節(jié)距離計算肩關(guān)節(jié)坐標(biāo)，并基于所述肘關(guān)節(jié)坐標(biāo)以及所述肩關(guān)節(jié)坐標(biāo)得到上肢姿態(tài)數(shù)據(jù)；

6、基于所述上肢姿態(tài)數(shù)據(jù)將所述肩關(guān)節(jié)坐標(biāo)、所述肘關(guān)節(jié)坐標(biāo)、所述末端中心點坐標(biāo)存儲為數(shù)據(jù)組，根據(jù)所述肘關(guān)節(jié)坐標(biāo)對所述數(shù)據(jù)組進行剔除以及補錄，并對所述數(shù)據(jù)組進行處理以得到第一數(shù)據(jù)組；

7、判斷新獲取的肘關(guān)節(jié)坐標(biāo)是否在所述第一數(shù)據(jù)組中，若否，則將所述新獲取的肘關(guān)節(jié)坐標(biāo)存儲至第二數(shù)據(jù)組中；

8、求取所述第一數(shù)據(jù)組中所有肩關(guān)節(jié)點位置的第一平均值，并將所述第一平均值作為肩關(guān)節(jié)中心點；

9、求取所述第二數(shù)據(jù)組中所有肩關(guān)節(jié)點位置的第二平均值，并比對所述第二平均值與所述肩關(guān)節(jié)中心點，若所述第二平均值與所述肩關(guān)節(jié)中心點的偏差大于預(yù)設(shè)值，則更新所述肩關(guān)節(jié)中心點。

10、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：通過肘關(guān)節(jié)坐標(biāo)以及肘關(guān)節(jié)距離計算肩關(guān)節(jié)坐標(biāo)，通過肘關(guān)節(jié)坐標(biāo)以及肩關(guān)節(jié)坐標(biāo)得到上肢姿態(tài)數(shù)據(jù)，通過上肢姿態(tài)數(shù)據(jù)得到數(shù)據(jù)組，并對數(shù)據(jù)組進行處理得到第一數(shù)據(jù)組，而通過比對第二平均值與所述肩關(guān)節(jié)中心點，能夠判斷肩關(guān)節(jié)中心是否發(fā)生偏移，并更新肩關(guān)節(jié)中心點，從而能夠準(zhǔn)確的確認(rèn)肩關(guān)節(jié)中心點位置。

11、進一步的，所述實時獲取小臂運動半徑、上肢康復(fù)機器人的末端中心點坐標(biāo)以及末端中心點至肘關(guān)節(jié)的肘關(guān)節(jié)距離的步驟包括：

12、獲取使用者大臂長度、小臂長度以及小臂運動半徑、末端中心點坐標(biāo)以及末端中心點至肘關(guān)節(jié)的肘關(guān)節(jié)距離；

13、基于上肢康復(fù)機器人的姿態(tài)傳感器轉(zhuǎn)換計算得到末端中心點的法向量以及小臂方向向量。

14、進一步的，所述肘關(guān)節(jié)坐標(biāo)的計算表達式為：

15、；

16、；

17、；

18、······

19、；

20、式中，、、、分別表示初始狀態(tài)下肘關(guān)節(jié)坐標(biāo)、第一次采用時刻下肘關(guān)節(jié)坐標(biāo)、第二次采用時刻下肘關(guān)節(jié)坐標(biāo)、第t次采用時刻下肘關(guān)節(jié)坐標(biāo)，、、、分別表示初始狀態(tài)下末端中心點坐標(biāo)、第一次采用時刻下末端中心點坐標(biāo)、第二次采用時刻下末端中心點坐標(biāo)、第t次采用時刻下末端中心點坐標(biāo)，表示小臂運動半徑，、、、分別表示初始狀態(tài)下垂直末端平面的法向量、第一次采用時刻下垂直末端平面的法向量、第二次采用時刻下垂直末端平面的法向量、第t次采用時刻下垂直末端平面的法向量，表示肘關(guān)節(jié)距離，、、、分別表示初始狀態(tài)下平行于人體小臂方向向量、第一次采用時刻下平行于人體小臂方向向量、第二次采用時刻下平行于人體小臂方向向量、第t次采用時刻下平行于人體小臂方向向量。

21、進一步的，所述肩關(guān)節(jié)坐標(biāo)計算表達式為：

22、；

23、；

24、······

25、；

26、式中，、、分別表示第一次采用時刻下肩關(guān)節(jié)坐標(biāo)、第二次采用時刻下肩關(guān)節(jié)坐標(biāo)、第t次采用時刻下肩關(guān)節(jié)坐標(biāo)，、、、分別表示初始狀態(tài)下肘關(guān)節(jié)坐標(biāo)、第一次采用時刻下肘關(guān)節(jié)坐標(biāo)、第二次采用時刻下肘關(guān)節(jié)坐標(biāo)、第t次采用時刻下肘關(guān)節(jié)坐標(biāo)，表示肘關(guān)節(jié)移動距離。

27、進一步的，所述基于所述上肢姿態(tài)數(shù)據(jù)將所述肩關(guān)節(jié)坐標(biāo)、所述肘關(guān)節(jié)坐標(biāo)、所述末端中心點坐標(biāo)存儲為數(shù)據(jù)組的步驟之前，所述方法還包括：

28、求解所述肩關(guān)節(jié)坐標(biāo)的運動曲線解，并將肩關(guān)節(jié)坐標(biāo)點、肘關(guān)節(jié)坐標(biāo)點以及末端中心點存為數(shù)組；

29、若所述運動曲線的解舍棄，則數(shù)據(jù)均不做記錄。

30、進一步的，所述并對所述數(shù)據(jù)組進行處理以得到第一數(shù)據(jù)組的步驟包括：

31、確認(rèn)所述數(shù)據(jù)組中是否僅保留一組肘關(guān)節(jié)坐標(biāo)，若是則得到所述第一數(shù)據(jù)組。

32、進一步的，所述若所述第二平均與所述肩關(guān)節(jié)中心點的偏差大于預(yù)設(shè)值的步驟包括：

33、若所述第二平均值與所述肩關(guān)節(jié)中心點的偏差大于20mm，且所述第二數(shù)據(jù)組大于200組時，則判斷所述肩關(guān)節(jié)中心點發(fā)生偏移。

34、第二方面，本發(fā)明還提供一種上肢姿態(tài)計算系統(tǒng)，用于實現(xiàn)上述的上肢姿態(tài)計算方法，所述系統(tǒng)包括：

35、實時獲取模塊，用于實時獲取小臂運動半徑、上肢康復(fù)機器人的末端中心點坐標(biāo)以及末端中心點至肘關(guān)節(jié)的肘關(guān)節(jié)距離；

36、第一計算模塊，用于基于所述末端中心點坐標(biāo)、所述小臂運動半徑、垂直所述末端中心點的運動軌跡的法向量、所述肘關(guān)節(jié)距離以及小臂方向向量計算肘關(guān)節(jié)坐標(biāo)；

37、第二計算模塊，用于根據(jù)所述肘關(guān)節(jié)坐標(biāo)以及所述肘關(guān)節(jié)距離計算肩關(guān)節(jié)坐標(biāo)，并基于所述肘關(guān)節(jié)坐標(biāo)以及所述肩關(guān)節(jié)坐標(biāo)得到上肢姿態(tài)數(shù)據(jù)；

38、存儲處理模塊，用于基于所述上肢姿態(tài)數(shù)據(jù)將所述肩關(guān)節(jié)坐標(biāo)、所述肘關(guān)節(jié)坐標(biāo)、所述末端中心點坐標(biāo)存儲為數(shù)據(jù)組，根據(jù)所述肘關(guān)節(jié)坐標(biāo)對所述數(shù)據(jù)組進行剔除以及補錄，并對所述數(shù)據(jù)組進行處理以得到第一數(shù)據(jù)組；

39、判斷存儲模塊，用于判斷新獲取的肘關(guān)節(jié)坐標(biāo)是否在所述第一數(shù)據(jù)組中，若否，則將所述新獲取的肘關(guān)節(jié)坐標(biāo)存儲至第二數(shù)據(jù)組中；

40、第一求取模塊，用于求取所述第一數(shù)據(jù)組中所有肩關(guān)節(jié)點位置的第一平均值，并將所述第一平均值作為肩關(guān)節(jié)中心點；

41、第二求取模塊，用于求取所述第二數(shù)據(jù)組中所有肩關(guān)節(jié)點位置的第二平均值，并比對所述第二平均值與所述肩關(guān)節(jié)中心點，若所述第二平均值與所述肩關(guān)節(jié)中心點的偏差大于預(yù)設(shè)值，則更新所述肩關(guān)節(jié)中心點。

42、進一步的，所述實時獲取模塊包括：

43、獲取單元，用于獲取使用者大臂長度、小臂長度以及小臂運動半徑、末端中心點坐標(biāo)以及末端中心點至肘關(guān)節(jié)的肘關(guān)節(jié)距離；

44、轉(zhuǎn)換計算單元，用于基于上肢康復(fù)機器人的姿態(tài)傳感器轉(zhuǎn)換計算得到末端中心點的法向量以及小臂方向向量。

45、進一步的，所述第二求取模塊包括：

46、判斷單元，用于若所述第二平均值與所述肩關(guān)節(jié)中心點的偏差大于20mm，且所述第二數(shù)據(jù)組大于200組時，則判斷所述肩關(guān)節(jié)中心點發(fā)生偏移。