[發明專利]一種基于多傳感器感知的艾灸機器人運動控制方法

有效

• 申請號: 202011557937.0
• 申請日: 2020-12-24
• 公開（公告）號: CN112618346B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 楊海濱;鄧旭高;羅永華;周星;高萌;陳思敏;陳元浩
• 申請（專利權）人: 佛山冠灣智能科技有限公司
• 主分類號: A61B18/04
• 分類號: A61B18/04;A61H39/06
• 代理公司: 廣州嘉權專利商標事務所有限公司 44205
• 代理人: 譚志鵬

1.一種基于多傳感器感知的艾灸機器人運動控制方法，其特征在于：

其采用關節機器人（100），所述關節機器人（100）設置有執行末端和機器人控制器，在所述執行末端設置有溫度傳感器（600）和多個距離傳感器，艾灸機器人運動控制方法具體包括如下步驟：

步驟一：靜態建模，控制所述關節機器人（100）的執行末端在人體表面上方執行任意手法軌跡的運動，同時多個所述距離傳感器掃描人體表面，并測量出執行末端與人體體表之間的距離值，機器人控制器根據所述距離傳感器采集的距離數值擬合出體表的靜態曲面模型并生成執行末端的主動運動軌跡程序；

步驟二：把灸頭裝設于執行末端上，根據所述主動運動軌跡程序，機器人控制器控制關節機器人（100）上的執行末端運動；

步驟三：動態實時補償，多個距離傳感器實時測量執行末端與人體體表的距離值，并計算測量值和設定施灸的高度值之間的誤差△Eh；溫度傳感器（600）實時監測人體體表穴位的溫度，并計算測量值和設定的施灸溫度值之間的誤差△Et，機器人控制器采用閉環控制算法擬合出執行末端的隨動運動軌跡；

步驟四：機器人控制器把主動運動軌跡和隨動運動軌跡采用多項式擬合在一起，最終形成執行末端的疊加運動軌跡；

在步驟一和步驟三中，執行末端與人體體表之間的距離值的具體測量方法如下：所述距離傳感器的數量為四個，四個距離傳感器呈十字對稱設置于執行末端同一平面上，四個距離傳感器包括相對設置的第一距離傳感器（200）和第三距離傳感器（300）、相對設置的第二距離傳感器（400）和第四距離傳感器（500），第一距離傳感器（200）、第二距離傳感器（400）、第三距離傳感器（300）和第四距離傳感器（500）分別獲取到距離數據d1、d2、d3、d4，第一距離傳感器（200）和第三距離傳感器（300）的軸線所組成的平面與關節機器人（100）B角所處的平面重合，其中第二距離傳感器（400）和第四距離傳感器（500）的軸線所組成的平面與關節機器人（100）C角所處的平面重合，所述第一距離傳感器（200）與第三距離傳感器（300）之間距離、第二距離傳感器（400）與第四距離傳感器（500）之間的距離均為L，使用三角余切公式：tanθ= (d3-d1)/L 可計算出關節機器人（100）姿態的B角補償值θ，同理使用d2、d4、L可計算出關節機器人（100）姿態的C角補償值α，使用d1、d2、d3、d4、θ和α的值，結合三角函數推導出來的公式得出當前執行末端的實際高度值，即是出執行末端與人體體表之間的距離值：

H=(d1+d2+d3+d4)/4)*COS(ATAN(SQRT(TAN(90-θ)*TAN(90-θ)+TAN(90-α)*TAN(90-α)))*π/180)；

在步驟三中，距離傳感器測量計算得出的實際距離值H減去設定的距離值Hset，得到距離補償值△Eh，即H-Hset=△Eh；關節機器人（100）控制器實時計算△Eh、θ、α ，并根據其值進行實時的位置補償。

2.根據權利要求1所述的一種基于多傳感器感知的艾灸機器人運動控制方法，其特征在于：

在步驟一中，關節機器人（100）的執行末端循環執行手法軌跡運動多次。

3.根據權利要求1所述的一種基于多傳感器感知的艾灸機器人運動控制方法，其特征在于：

在步驟一中，多個距離傳感器測量并計算得出的多組實際高度值H，機器人控制器將多組的高度值H擬合成曲線，并采用樣條曲線插值的方式不斷擬合優化，得出人體表面的曲線模型。

4.根據權利要求3所述的一種基于多傳感器感知的艾灸機器人運動控制方法，其特征在于：

在步驟一中，采用三次樣條曲線插值的方式生成執行末端的主動運動軌跡，并對主動運動軌跡進行平滑處理，得出人體體表的靜態曲面模型和關節機器人（100）的主動運動程序。

5.根據權利要求1所述的一種基于多傳感器感知的艾灸機器人運動控制方法，其特征在于：

在所述執行末端安裝有力矩傳感器，力矩傳感器用于監控執行末端是否與障礙物發生碰撞。