[發明專利]基于雙閉環三維路徑跟蹤的微機器人控制方法

有效

• 申請號: 202011118436.2
• 申請日: 2020-10-19
• 公開（公告）號: CN112180736B
• 公開（公告）日: 2021-08-03
• 發明（設計）人: 樊啟高;陳偉;謝林柏;朱一昕;楊國鋒;李岳陽;畢愷韜;黃文濤;羅海馳;趙正青
• 申請（專利權）人: 江南大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 無錫華源專利商標事務所(普通合伙) 32228
• 代理人: 聶啟新

1.基于雙閉環三維路徑跟蹤的微機器人控制方法，其特征在于，控制方法基于磁驅裝置，所述磁驅裝置包括電磁線圈模組、直流電流源模組、PWM逆變電路、電流傳感器、上位機、兩個攝像機，所述電磁線圈模組包括六個帶梯形探頭的一級鐵芯、設置在所述一級鐵芯上的亥姆霍茲線圈以及線圈支架，每對中帶梯形探頭的兩個一級鐵芯及其相應的兩個亥姆霍茲線圈均平行設置，所述線圈支架用于固定三對帶梯形探頭的一級鐵芯和三對亥姆霍茲線圈，所述三對帶梯形探頭的一級鐵芯沿軸線方向兩兩正交，所述三對亥姆霍茲線圈沿軸線方向兩兩正交，三對梯形探頭內側形成的區域作為磁性微機器人的工作空間，每個直流電流源通過所述PWM逆變電路后為一對或一個所述亥姆霍茲線圈提供交流電，所述上位機分別連接所述PWM逆變電路、電流傳感器和攝像機，所述電流傳感器用于檢測所述亥姆霍茲線圈的輸出電流，兩個所述攝像機設置在所述線圈支架外側并且正交分布，所述上位機發送控制信號到所述PWM逆變電路輸出頻率和幅值可調的交流電，所述亥姆霍茲線圈產生旋轉磁場進而控制所述磁性微機器人沿著所述旋轉磁場的軸線方向進行三維運動，所述攝像機獲取所述磁性微機器人的位置信息并傳送至所述上位機實現閉環控制所述磁性微機器人的三維運動；

所述控制方法包括：

在所述上位機中輸入期望跟蹤路徑得到期望運動方向，所述攝像機獲取所述磁性微機器人的當前位姿信息并反饋給所述上位機得到所述磁性微機器人的質心三維坐標和實際軸線方向；

在所述期望跟蹤路徑上尋找與質心距離最小的期望位置點，建立期望跟蹤路徑曲線坐標系獲取所述期望位置點三維坐標、期望跟蹤路徑曲線的曲率和撓率；

將所述實際軸線方向與所述期望運動方向、所述質心三維坐標與所述期望位置點三維坐標分別進行處理得到方向誤差和位置誤差，所述方向誤差包括俯仰角誤差和方向角誤差，所述位置誤差包括兩者的水平距離和垂直距離；

根據所述曲率、撓率、方向誤差和位置誤差對所述期望跟蹤路徑的三維運動學建模得到狀態空間方程；

通過所述狀態空間方程設計位置閉環控制器得到所需旋轉磁場，將所述旋轉磁場輸入至建立的電流閉環磁場控制器輸出期望磁場，實現對所述磁性微機器人的雙閉環三維運動控制；

重新執行所述攝像機獲取所述磁性微機器人的當前位姿信息并反饋給所述上位機得到所述磁性微機器人的質心三維坐標和實際軸線方向的步驟，直至完成整個所述期望跟蹤路徑的跟蹤。

2.根據權利要求1所述的基于雙閉環三維路徑跟蹤的微機器人控制方法，其特征在于，所述攝像機獲取所述磁性微機器人的當前位姿信息并反饋給所述上位機得到所述磁性微機器人的質心三維坐標和實際軸線方向，包括：

利用側面攝像機和頂部攝像機對所述磁性微機器人的當前位姿信息進行檢測，把頂部攝像機坐標系固定為世界坐標系，通過下式計算所述磁性微機器人在所述世界坐標系中的實際軸線方向：

X_B＝^cth_t×^cth_s＝^cth_t×(^ctR_cs^csh_s)

其中，^cth_t表示頂部攝像機平面的法向量，^csh_s表示側面攝像機平面的法向量，^cth_s表示所述側面攝像機平面的法向量到所述頂部攝像機平面的變換，^ctR_cs表示側面攝像機坐標系到頂部攝像機坐標系的旋轉矩陣，X_B表示所述磁性微機器人在所述世界坐標系中的實際軸線方向；

通過下式計算所述磁性微機器人的質心在所述世界坐標系中的三維坐標：

G＝^ctR_cs^csG+^ctt_cs

其中，^csG表示質心在所述側面攝像機平面的坐標，^ctt_cs表示側面攝像機坐標系到頂部攝像機坐標系的平移矩陣，G表示質心在所述世界坐標系中的三維坐標。