一种基于末端姿态的连续型机器人的形状追踪方法与流程

本发明涉及一种连续型机器人重建领域，更具体地说是一种基于末端姿态的连续型机器人的形状追踪方法。

背景技术：

连续型连续型机器人由于能够在很好的工作在复杂和封闭的环境中，因此成为微创外科手术的一个优良选择。然而，目前连续型机器人实时的位置与形状信息不能够很好的被估测，特别是当末端执行器受到外力或负载时。在外科手术中，连续型机器人必然会与人体组织产生干涉，组织也会影响连续型机器人的位置与形状，因此为了避免对人体组织造成伤害，实时的位置与形状信息的检测尤为重要。

先前的研究人员针对连续型机器人的实时位置与形状检测主要分为两种方法。一种为基于机器人的动力学模型，如利用cosserat介质理论和静力学模型获得机器人形状的方法；基于瑞利-里兹法预测形状变形的方法等。该方法的缺点在于需提前计算出施加在连续型机器人的外部负载或外力。另一种则是基于特定的传感器技术，如基于医学图像法；电磁追踪法；光纤光栅法等。该方法的缺点在于需要导线或引线将固定在机器人上的各个传感器连接起来，机器人的形状较为复杂。

技术实现要素：

本发明的目的是为了解决现有技术中存在的缺点，而提出的一种基于末端姿态的连续型机器人的形状追踪方法。

为了实现上述目的，本发明采用了如下技术方案：

一种基于末端姿态的连续型机器人的形状追踪方法，包括机器人驱动系统、磁追踪系统、形状重建算法和电脑显示界面，具体步骤如下：

s1机器人驱动系统用来控制连续型机器人的前后移动，以及连续型机器人的左右和上下弯曲运动；

s2磁追踪系统则是由多个磁传感器组成的磁传感器阵列，在机器人的末端放置一个小磁铁，当机器人在磁传感器阵列上方运动时，小磁铁的磁场可以被磁传感器阵列进行获取得到，通过各个磁传感器获得的磁场强度，我们便可以得到机器人的末端的三维的位置信息以及二维的方位信息；

s3形状重建算法根据机器人末端的位置姿态信息，利用曲线拟合算法和拟合曲线，我们便可以得到机器人实时的形状信息；

s4电脑显示界面，通过所建立地图映射环境，机器人末端姿态信息将会实时显示在电脑屏幕上，同时连续型机器人的实时形状将会叠置显示在地图环境中。

优选地，s2中磁传感器为地磁传感器，型号为hmc5883l。

优选地，s3中的拟合曲线为贝赛尔曲线。

优选地，s4中通过opengl建立地图环境。

本发明是基于一种线驱动连续型手术机器人，包括步进电机、丝杠、第一伺服电机、柔性机械臂、机器人基座、牵引绳、第二伺服电机、导轨，柔性机械臂设置在机器人基座前端，第一伺服电机、第二伺服电机上下对称设置在机器人基座上，柔性机械臂包括基椎体、多个中间椎体、末端椎体，每两个相邻的椎体可以构成一个球关节，每个椎体上设有中空的中央腔和位于中央腔外部均布的导孔，两对牵引绳通过导孔将各个椎体连接起来，每一对牵引绳的末端固定在同一个伺服电机上，手术器械可以通过中央腔到达机器人末端，机器人基座固定在导轨上，步进电机通过带动丝杠而使得机器人基座前后移动；而两个伺服电机则是通过牵引绳控制柔性机械臂的弯曲运动，第一伺服电机控制柔性机械臂的左右弯曲运动，第二伺服电机控制柔性机械臂的上下弯曲运动。

其中，柔性机械臂的长度可以通过改变中间椎体的数量进行调节。

本发明相对于基于运动学模型的方法，该发明不需要提前预测外部载荷或者外力；相对于图像和视觉的方法，此发明在没有视线的环境下依然可以保持优秀的工作能力；相对于其他基于传感器的方法，这个发明不需要将导线或者传感器放置于机器人中，所以几乎不需要改进机器人。

附图说明

图1是机器人系统整体图；

图2是系统工作流程图；

图3是磁追踪系统；

图4是机器人拟合曲线。

图5是贝塞尔曲线。

其中，a-机器人驱动系统，b-连续型机器人，c-磁传感器阵列，d-电脑显示界面；1-小永磁铁，2-磁传感器，3-拟合曲线，4-贝赛尔曲线。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

参照图1-5，一种基于末端姿态的连续型机器人的形状追踪方法，

一种基于末端姿态的连续型机器人的形状追踪方法，包括机器人驱动系统、磁追踪系统、形状重建算法和电脑显示界面，具体步骤如下：

s1机器人驱动系统用来控制连续型机器人的前后移动，以及连续型机器人的左右和上下弯曲运动；

s2磁追踪系统则是由多个磁传感器组成的磁传感器阵列，在机器人的末端放置一个小磁铁，当机器人在磁传感器阵列上方运动时，小磁铁的磁场可以被磁传感器阵列进行获取得到，通过各个磁传感器获得的磁场强度，我们便可以得到机器人的末端的三维的位置信息以及二维的方位信息；

s3形状重建算法根据机器人末端的位置姿态信息，利用曲线拟合算法和拟合曲线，我们便可以得到机器人实时的形状信息；

s4电脑显示界面，通过所建立地图映射环境，机器人末端姿态信息将会实时显示在电脑屏幕上，同时连续型机器人的实时形状将会叠置显示在地图环境中。

s2中磁追踪系统设计

利用多个磁传感器组成一个磁传感器阵列。当小磁铁在传感器阵列上方移动时，小磁铁的磁场可以被传感器阵列检测到，通过定位算法可以获得小磁铁的三维的位置和二维取向。

定义第个传感器的误差方程为，如下所示：

（1）

其中,是第个传感器所测量的磁场。是估计的磁场，其可以如下表示：

（2）

其中，k是磁铁的磁矩模数。是磁铁的位置坐标，是磁矩的单位方向向量，即，。是第个传感器的位置坐标。

因此，将最优化，便可估计出磁铁的位置，方向。

s3中形状重建算法

形状重建算法使用曲线拟合方法来实时模拟连续型机器人的形状。如图4所示，通过机器人始端的位置与方向和磁追踪系统得到的连续型机器人的末端的位置与方向，进而利用曲线拟合算法重建得到连续型机器人的实时形状。

在下面的部分中，将会介绍基于形状重建算法的贝塞尔曲线。

图5展示了贝塞尔曲线，是起点，是终点，和是控制点，

与图1中机器人的结构相比，即使基椎体的位置，是末端椎体。磁追踪系统提供的末端椎体的5d的姿态信息，即为和。

是起点，是终点，和是控制点。和分别表示和的长度。是从到的方向向量，是到的方向向量。

（4）

，和，的关系如下所示：

（5）

因此，通过解出两个未知量（，），就可以确定曲线。定义估计误差函数为，则：

（6）

其中，是机器人的长度，是到的曲线长度。通过最小化即可以估计得到，两个参数。

通过以上两个系统，我们就可以建立起连续型连续型机器人的形状跟踪方案，在电脑显示的的结果下，我们可以实时的观测到机器人的末端姿态与形状。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。