本發(fā)明涉及一種連續(xù)型機器人重建領(lǐng)域，更具體地說是一種基于末端姿態(tài)的連續(xù)型機器人的形狀追蹤方法。

背景技術(shù)：

連續(xù)型連續(xù)型機器人由于能夠在很好的工作在復雜和封閉的環(huán)境中，因此成為微創(chuàng)外科手術(shù)的一個優(yōu)良選擇。然而，目前連續(xù)型機器人實時的位置與形狀信息不能夠很好的被估測，特別是當末端執(zhí)行器受到外力或負載時。在外科手術(shù)中，連續(xù)型機器人必然會與人體組織產(chǎn)生干涉，組織也會影響連續(xù)型機器人的位置與形狀，因此為了避免對人體組織造成傷害，實時的位置與形狀信息的檢測尤為重要。

先前的研究人員針對連續(xù)型機器人的實時位置與形狀檢測主要分為兩種方法。一種為基于機器人的動力學模型，如利用cosserat介質(zhì)理論和靜力學模型獲得機器人形狀的方法；基于瑞利-里茲法預測形狀變形的方法等。該方法的缺點在于需提前計算出施加在連續(xù)型機器人的外部負載或外力。另一種則是基于特定的傳感器技術(shù)，如基于醫(yī)學圖像法；電磁追蹤法；光纖光柵法等。該方法的缺點在于需要導線或引線將固定在機器人上的各個傳感器連接起來，機器人的形狀較為復雜。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是為了解決現(xiàn)有技術(shù)中存在的缺點，而提出的一種基于末端姿態(tài)的連續(xù)型機器人的形狀追蹤方法。

為了實現(xiàn)上述目的，本發(fā)明采用了如下技術(shù)方案：

一種基于末端姿態(tài)的連續(xù)型機器人的形狀追蹤方法，包括機器人驅(qū)動系統(tǒng)、磁追蹤系統(tǒng)、形狀重建算法和電腦顯示界面，具體步驟如下：

s1機器人驅(qū)動系統(tǒng)用來控制連續(xù)型機器人的前后移動，以及連續(xù)型機器人的左右和上下彎曲運動；

s2磁追蹤系統(tǒng)則是由多個磁傳感器組成的磁傳感器陣列，在機器人的末端放置一個小磁鐵，當機器人在磁傳感器陣列上方運動時，小磁鐵的磁場可以被磁傳感器陣列進行獲取得到，通過各個磁傳感器獲得的磁場強度，我們便可以得到機器人的末端的三維的位置信息以及二維的方位信息；

s3形狀重建算法根據(jù)機器人末端的位置姿態(tài)信息，利用曲線擬合算法和擬合曲線，我們便可以得到機器人實時的形狀信息；

s4電腦顯示界面，通過所建立地圖映射環(huán)境，機器人末端姿態(tài)信息將會實時顯示在電腦屏幕上，同時連續(xù)型機器人的實時形狀將會疊置顯示在地圖環(huán)境中。

優(yōu)選地，s2中磁傳感器為地磁傳感器，型號為hmc5883l。

優(yōu)選地，s3中的擬合曲線為貝賽爾曲線。

優(yōu)選地，s4中通過opengl建立地圖環(huán)境。

本發(fā)明是基于一種線驅(qū)動連續(xù)型手術(shù)機器人，包括步進電機、絲杠、第一伺服電機、柔性機械臂、機器人基座、牽引繩、第二伺服電機、導軌，柔性機械臂設置在機器人基座前端，第一伺服電機、第二伺服電機上下對稱設置在機器人基座上，柔性機械臂包括基椎體、多個中間椎體、末端椎體，每兩個相鄰的椎體可以構(gòu)成一個球關(guān)節(jié)，每個椎體上設有中空的中央腔和位于中央腔外部均布的導孔，兩對牽引繩通過導孔將各個椎體連接起來，每一對牽引繩的末端固定在同一個伺服電機上，手術(shù)器械可以通過中央腔到達機器人末端，機器人基座固定在導軌上，步進電機通過帶動絲杠而使得機器人基座前后移動；而兩個伺服電機則是通過牽引繩控制柔性機械臂的彎曲運動，第一伺服電機控制柔性機械臂的左右彎曲運動，第二伺服電機控制柔性機械臂的上下彎曲運動。

其中，柔性機械臂的長度可以通過改變中間椎體的數(shù)量進行調(diào)節(jié)。

本發(fā)明相對于基于運動學模型的方法，該發(fā)明不需要提前預測外部載荷或者外力；相對于圖像和視覺的方法，此發(fā)明在沒有視線的環(huán)境下依然可以保持優(yōu)秀的工作能力；相對于其他基于傳感器的方法，這個發(fā)明不需要將導線或者傳感器放置于機器人中，所以幾乎不需要改進機器人。

附圖說明

圖1是機器人系統(tǒng)整體圖；

圖2是系統(tǒng)工作流程圖；

圖3是磁追蹤系統(tǒng)；

圖4是機器人擬合曲線。

圖5是貝塞爾曲線。

其中，a-機器人驅(qū)動系統(tǒng)，b-連續(xù)型機器人，c-磁傳感器陣列，d-電腦顯示界面；1-小永磁鐵，2-磁傳感器，3-擬合曲線，4-貝賽爾曲線。

具體實施方式

下面將結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而不是全部的實施例。

參照圖1-5，一種基于末端姿態(tài)的連續(xù)型機器人的形狀追蹤方法，

一種基于末端姿態(tài)的連續(xù)型機器人的形狀追蹤方法，包括機器人驅(qū)動系統(tǒng)、磁追蹤系統(tǒng)、形狀重建算法和電腦顯示界面，具體步驟如下：

s1機器人驅(qū)動系統(tǒng)用來控制連續(xù)型機器人的前后移動，以及連續(xù)型機器人的左右和上下彎曲運動；

s2磁追蹤系統(tǒng)則是由多個磁傳感器組成的磁傳感器陣列，在機器人的末端放置一個小磁鐵，當機器人在磁傳感器陣列上方運動時，小磁鐵的磁場可以被磁傳感器陣列進行獲取得到，通過各個磁傳感器獲得的磁場強度，我們便可以得到機器人的末端的三維的位置信息以及二維的方位信息；

s3形狀重建算法根據(jù)機器人末端的位置姿態(tài)信息，利用曲線擬合算法和擬合曲線，我們便可以得到機器人實時的形狀信息；

s4電腦顯示界面，通過所建立地圖映射環(huán)境，機器人末端姿態(tài)信息將會實時顯示在電腦屏幕上，同時連續(xù)型機器人的實時形狀將會疊置顯示在地圖環(huán)境中。

s2中磁追蹤系統(tǒng)設計

利用多個磁傳感器組成一個磁傳感器陣列。當小磁鐵在傳感器陣列上方移動時，小磁鐵的磁場可以被傳感器陣列檢測到，通過定位算法可以獲得小磁鐵的三維的位置和二維取向。

定義第個傳感器的誤差方程為，如下所示：

（1）

其中,是第個傳感器所測量的磁場。是估計的磁場，其可以如下表示：

（2）

其中，k是磁鐵的磁矩模數(shù)。是磁鐵的位置坐標，是磁矩的單位方向向量，即，。是第個傳感器的位置坐標。

因此，將最優(yōu)化，便可估計出磁鐵的位置，方向。

s3中形狀重建算法

形狀重建算法使用曲線擬合方法來實時模擬連續(xù)型機器人的形狀。如圖4所示，通過機器人始端的位置與方向和磁追蹤系統(tǒng)得到的連續(xù)型機器人的末端的位置與方向，進而利用曲線擬合算法重建得到連續(xù)型機器人的實時形狀。

在下面的部分中，將會介紹基于形狀重建算法的貝塞爾曲線。

圖5展示了貝塞爾曲線，是起點，是終點，和是控制點，

與圖1中機器人的結(jié)構(gòu)相比，即使基椎體的位置，是末端椎體。磁追蹤系統(tǒng)提供的末端椎體的5d的姿態(tài)信息，即為和。

是起點，是終點，和是控制點。和分別表示和的長度。是從到的方向向量，是到的方向向量。

（4）

，和，的關(guān)系如下所示：

（5）

因此，通過解出兩個未知量（，），就可以確定曲線。定義估計誤差函數(shù)為，則：

（6）

其中，是機器人的長度，是到的曲線長度。通過最小化即可以估計得到，兩個參數(shù)。

通過以上兩個系統(tǒng)，我們就可以建立起連續(xù)型連續(xù)型機器人的形狀跟蹤方案，在電腦顯示的的結(jié)果下，我們可以實時的觀測到機器人的末端姿態(tài)與形狀。

以上所述，僅為本發(fā)明較佳的具體實施方式，但本發(fā)明的保護范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，根據(jù)本發(fā)明的技術(shù)方案及其發(fā)明構(gòu)思加以等同替換或改變，都應涵蓋在本發(fā)明的保護范圍之內(nèi)。