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

有效

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

本發明公開了基于雙閉環三維路徑跟蹤的微機器人控制方法，涉及微納機器人控制技術領域，該方法包括：輸入期望跟蹤路徑，通過攝像機獲取磁性微機器人的當前位姿信息進而計算得到質心位置、實際軸線方向、期望跟蹤路徑上距離質心最近的期望位置點坐標以及此點的切線方向；根據實際軸線方向和切線方向以及擾動補償計算出兩點的水平距離、垂直距離、方向角誤差、俯仰角誤差；根據設計的位置閉環控制器求得所需的旋轉磁場，電流閉環磁場控制器根據旋轉磁場和反饋的線圈輸出電流閉環控制亥姆霍茲線圈產生期望磁場，使磁性微機器人完成對期望跟蹤路徑的跟蹤。該方法采用位置和電流反饋的雙閉環控制能夠精準的完成對三維路徑的跟蹤。

技術領域

本發明涉及微納機器人控制技術領域，尤其是基于雙閉環三維路徑跟蹤的微機器人控制方法。

背景技術

機器人的小型化為復雜和危險環境下的多種應用提供了解決方案，電磁驅動的磁性微機器人可以應用于醫學環境，在不傷害人體的情況下磁性微機器人可以用于攜帶運輸藥物實現微創治療。為了執行這樣的任務磁性微機器人應該能實現對期望路徑的跟蹤。現有磁性微機器人的路徑跟蹤控制大多應用于平面范圍，而且多采用單閉環路徑跟蹤即通過攝像機的視覺反饋獲取磁性微機器在平面內的位姿信息建立位置閉環實現平面范圍內的跟蹤，然而這其中存在一定的響應延遲和適應性低的問題。一方面在高頻環境下由于磁場發生裝置的響應延遲問題，所使用的驅動裝置往往不能及時準確的建立所需磁場造成響應速度慢。另一方面對復雜的人體環境我們更希望的是磁性微機器人能夠完成對三維路徑的跟蹤，在平面路徑跟蹤控制領域上述方法可行，但應用于三維環境下的路徑跟蹤上述的控制方法便失去作用。

發明內容

本發明人針對上述問題及技術需求，提出了基于雙閉環三維路徑跟蹤的微機器人控制方法，實現對給定的三維期望跟蹤路徑準確及時的跟蹤，克服在高頻條件下磁驅裝置響應速度慢的問題，采用位置反饋和電流反饋的雙閉環控制提高了三維路徑跟蹤的準確性和快速性。

本發明的技術方案如下：

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

其進一步的技術方案為，線圈支架包括上下相對設置的底座、支架和中空擋板，每個底座沿軸線方向上均設有三個三角斜塊，軸向上的一對三角斜塊的斜面平行設置且用于放置帶梯形探頭的一級鐵芯，支架設置在底座之間起到支撐作用，中空擋板設置在支架的中部并與底座平行，中空擋板將底座圍成的空間分成上層區域和下層區域，每對帶梯形探頭的一級鐵芯及其相應的亥姆霍茲線圈中的一個設置在中空擋板上且位于上層區域、另一個位于下層區域，中空擋板的中空面積至少與磁性微機器人的工作空間面積相同。

其進一步的技術方案為，一級鐵芯為DT4-E材料制成的圓柱鐵芯，直徑50mm、厚30mm，亥姆霍茲線圈的匝數為190，梯形探頭的末端為邊長35mm的正方形，前端為長16mm、寬2mm的矩形，工作空間為半徑16mm的球形空間，磁性微機器人為螺旋形狀。

基于雙閉環三維路徑跟蹤的微機器人控制方法，控制方法包括：