將門好聲音 | IROS 2019:軟體微型機器人3D任意路徑跟隨控制

點此回顧 IROS2019 群星閃耀·精英之夜

論文作者是來自將門機器人主題社群、中國科學院深圳先進技術研究院黃晨陽，本次他將分享其團隊發表在今年IROS的工作——微型機器人的路徑跟隨控制任務。如果你也想與廣大群友分享自己的研究工作、文章觀點、出坑經驗，點擊「閱讀原文」或聯繫將門小姐姐！只要內容合適，我"門"送你頭條出道！

由外部磁場驅動的不受束縛的軟體微型機器人因其軟而靈活的身體結構，在微裝配、微創診斷、靶向送藥等方面都有著巨大的應用前景。然而，高效精準地操控軟體微型機器人，使其在實際應用中具備自主性和可重複性，閉環控制是必要且具有挑戰的。

關於作者：黃晨陽，中國科學院深圳先進技術研究院三年級碩士生，導師為徐天添副研究員。他的研究方向微型軟體機器人的運動控制、路徑規劃。最新一作論文發表於IEEE RA-L (with IROS Option)，併入圍IROS ICROS Best Application Paper Award的評選。

https://ieeexplore.ieee.org/abstract/document/8772184/ 

這篇論文聚焦於微型機器人最基礎的控制任務之一：路徑跟隨控制任務。微型機器人導航研究主要分為自主決策、路徑規劃及跟隨控制，路徑跟隨控制即為導航任務中最底層和最基礎的任務。已有的關於移動機器人路徑跟隨控制工作有純追蹤導引控制（Point to point）、基於Frenet坐標系的鏈式控制及基於預瞄駕駛員模型的自主轉向控制等。然而已有的報導中關於控制器對路徑曲率突變的響應情況及運動模型不確定的問題研究還很少。作者提出了一種基於向量場的路徑跟隨控制算法並實現了毫米級磁驅動軟體微型機器人3D任意路徑的跟隨控制。基於該控制算法設計的具有友好交互系統中，用戶可以非常便攜地通過3D繪圖滑鼠繪製任意曲率的3D參考路徑。文章的實驗部分驗證了該控制算法在曲率突變和運動模型不確定的跟隨路徑情況下的魯棒性和準確性。此外，該方法在未來的微型機器人遠程外科手術中具有非常重要的意義，外科手術醫生可以很方便地遠程繪製路徑，從而讓微型機器人自動完成路徑跟隨任務。

在流體中運動的微尺度機器人處於低雷諾數環境，機器人慣性作用可被忽略,即如果想要在低雷諾數環境中產生有效位移，非對稱的運動(non-reciprocalmotion)是一種有效的方法。本文設計的磁性軟體微型機器人能在外界旋轉磁場作用下在黏性液體中做非對稱地螺旋運動從而產生位移。機器人的磁化剖面和旋轉磁場激勵下的形變可以表示為兩部分的總和：Top面的磁化向量和Side 面的磁化向量。根據磁彈性力學理論，具有磁化剖面沿長軸變化的柔性帶材，在均勻磁場中經歷了分布力矩，其計算公式為：

當機器人放置在均勻旋轉磁場時，由Top磁化分量與外界磁場相互作用引起磁矩導致軟體機器人以半徑R的圓周旋轉運動，而由Side磁化分量與外界磁場相互作用引起軟體機器人以傾斜角φ螺旋運動。最終，軟體微型機器人形變成半徑為R、傾斜角φ的螺旋推進模型。

其中，A、E和I分別為機器人橫截面積、楊氏模量和截面慣性矩。G為材料的剪切模量，Jl為扭轉剛度。l為機器人螺旋運動的旋轉軸。τl為機器人受到的沿旋轉軸方向的力矩。在螺旋推進模型中，非流體作用力F和扭矩T與機器人推進速度和角速度的關係可以表示為:

3D亥姆霍茲線圈由三對中軸線相互垂直的線圈對組成，可通過編程產生任意方向的均勻磁場。針對給定的旋轉磁場中軸線方向向量(np)或中軸線方向角向量（ΩB, 俯仰角和偏航角），磁驅動系統通過磁場生成算法可以產生期望的均勻磁場，從而為微型機器人提供期望方向的推進力。

在不考慮受力過程的運動模型中，上述微型機器人運動模型可以簡化為欠驅動的水下無人機（AUV）。微型機器人驅動力由外界旋轉驅動磁場提供，即控制輸入為旋轉驅動磁場旋轉中軸線方向角向量ΩB(ψ,θ)（俯仰角ψ和偏航角θ）。然而在重力等非流體力造成的側滑漂移，微型機器人的前進方向n的方向角向量與驅動場角向量並不一致。在本文中將採用「offline training」的方法對這兩個向量進行標定。關鍵點序列ζ(n)可以用來描述用戶用3D滑鼠繪製的三維任意路徑，而不需要複雜參數方程描述。路徑點序列中每個相鄰的點連接成直線來近似路徑上的一個子段。三維任意路徑跟隨任務可以看作是多個直線子段路徑跟隨任務的組合，是一個迭代過程。

圖4 基於雙目視覺的3D路徑跟隨控制框圖

設計了一種基於雙目視覺3D路徑跟隨控制算法，能實現微型機器人3D任意路徑跟隨控制。通過雙目視覺測量的方法實時測量微型機器人當前位置和前進方向。外環控制器為路徑跟隨控制器，輸出為下一時刻微型機器人最優的前進方向向量。內環控制為轉向控制器，輸出為下一時刻驅動磁場的旋轉面中軸線方向角向量。

 

3.3 外環導航控制器

圖5 外環路徑跟隨控制器及係數M對控制性能影響仿真結果

 

外環控制器輸入為期望路徑和機器人當前位置坐標，在這個算法中，期望路徑通過一系列的關鍵點序列表示，每個相鄰點構成的線段為子段路徑。對於距離機器人最近的子段定義為激活段。整個路徑跟隨控制可以看做為迭代的過程，它最核心目的即最小化狀態向量q，即距離誤差和方向角誤差。

這裡作者設計了一種的基於向量場的控制率來最小化狀態向量q。可以將理想的前進方n*定義為兩向量加權求和獲得的合向量，即距離向量（平行於向量d）和激活段方向向量（垂直於向量d）；這裡的權係數由距離誤差值||d||和控制係數M組成。

作者通過李雅普諾夫穩定性理論證明了控制器的穩定性。從數學關係可以看出，距離路徑近時，下一步前進方向趨向於沿著路徑段方向運動；距離路徑遠時，趨向於靠近路徑的方向運動。

轉向控制器目的是尋找一個合適的磁驅動方向角向量，使得機器人從當前前進方向n轉向到最優前進方向n*,即最小化前進方向與最優前進方向的角向量差。通過一個PI控制，我們很容易得到下一時刻機器人的前進角向量，需要注意，因為重力等因素，前進角向量與實際驅動磁場中軸線角向量並不一致。通過「offline training」得到的標定表得到的映射關係，將前進角向量轉化為磁驅動轉向角向量。下圖描述了通過離線標定來確定機器人前進方向角與磁驅動方向角之間的映射關係。

圖6 離線標定用於確定前進方向角與磁驅動方向角映射

雙目視覺系統作為反饋迴路，用於實時定位和估計前進方向，考慮到機器人在給定旋轉頻率下的周期性運動，本文通滑窗濾波和最小二乘的方法求得較為準確的位置和前進方向。

圖7 雙目視覺測量及濾波算法

 

 

實驗中測試了不用控制增益M對系統性能影響，可以明顯發現，小的M響應速度快，但穩定性差；大的M穩定性好，但響應速度慢。具有自適應的M值控制器將是未來研究工作的一部分。

實驗最後一部分是路徑跟隨控制的實驗，通過滑鼠任意繪製的一組3D路徑，微型機器人能在非常小的誤差下實現路徑跟隨運動。任意參考路徑「S」、「I」、「A」、「T」的距離誤差的均方根分別為0.377mm、0.367mm、0.337mm、0.318mm。可以看出，跟隨控制的均方根誤差小於身體長的5%。

https://ieeexplore.ieee.org/abstract/document/8772184/media#media

圖10 跟隨控制實驗結果

-The End-

將門是一家以專注於發掘、加速及投資技術驅動型創業公司的新型創投機構，旗下涵蓋將門創新服務、將門技術社群以及將門創投基金。將門成立於2015年底，創始團隊由微軟創投在中國的創始團隊原班人馬構建而成，曾為微軟優選和深度孵化了126家創新的技術型創業公司。

將門創新服務專注於使創新的技術落地於真正的應用場景，激活和實現全新的商業價值，服務於行業領先企業和技術創新型創業公司。

將門技術社群專注於幫助技術創新型的創業公司提供來自產、學、研、創領域的核心技術專家的技術分享和學習內容，使創新成為持續的核心競爭力。

將門創投基金專注於投資通過技術創新激活商業場景，實現商業價值的初創企業，關注技術領域包括機器智能、物聯網、自然人機互動、企業計算。在三年的時間裡，將門創投基金已經投資了包括量化派、碼隆科技、禾賽科技、寬拓科技、杉數科技、迪英加科技等數十家具有高成長潛力的技術型創業公司。

如果您是技術領域的初創企業，不僅想獲得投資，還希望獲得一系列持續性、有價值的投後服務，歡迎發送或者推薦項目給我「門」: [e-mail]

    

 

將門創投

讓創新獲得認可！

微信：thejiangmen

[e-mail]