一种基于动力学的胶囊手术机器人闭环运动控制方法

本发明涉及一种基于动力学的胶囊闭环运动方法。

背景技术：

1、自从胶囊内窥镜机器人问世以来，在胃肠道检查和治疗领域已经成为关键的技术。目前的无线胶囊内窥镜主要运用外部控制能量场，如磁场、电场和超声波等方法。特别是磁场供能方法，由于对磁介质影响微小、响应速度快、能够实现无线控制以及对细胞无害等优势，使其在主动控制无线胶囊内窥镜方面具备显著的优越性。目前胶囊的控制系统设计主要有两种方法：一种是设计关键点，映射到轨迹路径上然后找到一群关键点的最优解，对应着本专利中的开环设计；另一种是设计跟踪轨迹，通过胶囊的输入输出信息，计算出当前时刻控制器的控制输入，从而完成控制，对应着本专利中的闭环设计。相比于开环设计，闭环设计方法有着更好的实时响应速度，从而可以达到更高的控制精度

2、在设计控制器时，系统先规划出了一条期望的轨迹。通过相机实时采集期望轨迹与胶囊之间的距离，将距离误差传入带有电磁铁的机械臂，机械臂直接控制电流内环进行调节。

3、在胶囊手术机器人的控制方法中，一般控制方法是基于准确模型或者部分基于准确模型，根据物理模型动力学建模后得出相关控制器表达式，然后将关键点映射到需求路径上，通过动力学控制机械臂上的电磁铁，从而带动胶囊。

技术实现思路

1、为了克服现有技术的不足，本发明提供一种基于动力学的胶囊手术机器人闭环运动控制方法，区别于轴控制方法，将距离误差传入带有电磁铁的机械臂，在笛卡尔空间进行控制，并且控制其电流内环，在力矩控制下通过机械臂动力学对胶囊进行了控制器设计。此外，实验在平台上进行了实验证明，达到了相应的预期效果。

2、为了解决上述技术问题提出的技术方案如下：

3、一种基于动力学的胶囊手术机器人闭环运动控制方法，包括以下步骤：

4、步骤1，分析胶囊机器人原理图；

5、步骤2，求取胶囊机器人位置以及其轨迹规划，过程如下:

6、2.1记胶囊中心位置以及角度：

7、pc＝[x,y,z]                        ⑴

8、r＝rz(β)rx(-α)ry(-γ)                    ⑵

9、其中，x,y,z分别表示胶囊机器人中心三维坐标的三个坐标轴；rx,ry,rz分别表示绕着ωx,ωy,ωz旋转；路径从pc1移动到pc2，伴随着姿态从r0旋转到r1，r为旋转姿态，r0，r1为其中的姿态，设λ(t)为在运动时间t完成的运动段的比例，t为总时间，为了实现均匀运动,设计公式：

10、

11、2.2笛卡尔坐标胶囊规划轨迹表达式：

12、

13、rot(n,θ)表示绕着θ轴旋转n度的矩阵，r(t)为在t时刻的旋转姿态，p(t)为在t时刻的胶囊位置，p0，p1为其中的位置；

14、2.3四元素坐标胶囊规划轨迹表达式：

15、由于笛卡尔坐标规划轨迹时容易出现万向节锁问题，在笛卡尔表达式的基础上，修改为四元素表达式

16、

17、其中δp＝(p2-p1)，n为恒定增加的常数，如果已知在整个路径执行过程中r1保持不变，就可以预先计算许多生成r(t)所需的中间表达式，从而在实时计算中减轻计算负荷；

18、步骤3，求取机械臂末端磁铁位置

19、在动态建模中，系统中的机械臂被视为一个由刚性连杆通过旋转关节连接在一起的机械系统，该模型预测由惯性、重力、科氏力和离心力效应产生的扭矩，一个具有6个自由度的刚性机器人用以下形式描述：

20、

21、其中，

22、

23、其中，x,表示任务空间的位置，速度以及加速度向量，m(q)∈rn×n表示一个正定的惯性矩阵，c(q)∈rn×n表示科里奥利力以及离心力，g(q)∈rn表示重力矩阵，τf表示摩檫力向量，表示内部未建模误差，j◇表示伪逆雅可比矩阵，tf表示伪逆雅可比矩阵的转置与摩檫力向量的乘积，表示伪逆雅可比矩阵的转置与内部未建模误差的乘积，d表示内部以及摩擦未建模误差；

24、mo(q)，go(q)以及d在特定的时刻内值是确定的，同理得在k时刻，以下公式成立：

25、

26、其中，表示任务空间的位置，速度以及加速度向量表示一个正定的惯性矩阵，表示科里奥利力以及离心力，表示重力矩阵，表示摩檫力向量，表示未建模误差，表示伪逆雅可比矩阵；

27、步骤4，建立末端磁铁以及胶囊机器人的闭环控制；

28、步骤5，闭环胶囊机器人移动实验：对胶囊内窥镜机器人系统中胶囊机器人移动进行测试实验。

29、进一步，所述步骤4的过程如下：

30、4.1胶囊机器人控制关系

31、当磁力矩的大小足够使胶囊启动时，胶囊将与激励磁体同步旋转，由于磁力和磁力矩的共同作用，胶囊的方向将保持与激励磁体一致，并移动至激励装置下方，因此，运动胶囊的激励模型描述为：

32、

33、其中以及代表着胶囊在t时刻的位姿代表着机械臂末端磁铁在t时刻的位姿由公式得当前胶囊的位姿是由之前胶囊的位姿以及当前机械臂末端磁铁位姿决定的；

34、4.2胶囊机器系统算法流程

35、在融合了定位轨迹规划以及机器人笛卡尔空间控制方法之后，基于胶囊机器人的闭环步骤如下：

36、4.2.1：旋转激励磁体一周，同时采样磁场；

37、4.2.2：计算一次旋转过程中相机采样的胶囊位置信息；

38、4.2.3：计算一次旋转过程中规划的胶囊位置信息；

39、4.2.4：无损卡尔曼滤波ukf驱动胶囊：

40、利用预测得到以及

41、4.2.5：跟新位置信息,

42、4.2.6：返回

43、4.3胶囊机器系统控制算法

44、已知胶囊的位姿胶囊的规划位姿可以由以下位置优化函数进行优化：

45、e(k)＝min||(pc(k)-pt(k))||+min||(α(k)-αt(k))+||+min||(β(k)-βt(k))||+min||(λ(k)-λt(k))||

46、(10)

47、已控制算法采用迭代pid进行控制，控制算法为

48、

49、4.4改进的积分分离pid控制

50、在积分分离pid控制器中，ε选取不合适会导致最后静差的不同，为改进参数选取造成的问题，提出了一种新的积分分离pid控制形式；

51、

52、

53、经过实际系统整定参数，得到参数为kp＝18,ki＝1.2,kd＝200,参数整定的数量级与z-n整定的预期相符，式中，t为采样时间；β为积分项的开关系数，

54、再进一步，所述步骤1中，参数代表意义如下：胶囊机器人运动方向，cx,cy,cz：胶囊机器人指向，为胶囊x轴方向；胶囊平面与指向方向垂直的方向，为胶囊y轴方向；cx,cy叉乘的方向，为胶囊z轴方向；ωx,ωy,ωz：绕着x轴的旋转方向；绕着y轴的旋转方向；绕着z轴的旋转方向；γ：胶囊机器人运动方向与指向的夹角，pc：胶囊机器人的位置，pa：机械臂末端磁铁的位置，perr：机械臂末端磁铁与胶囊机器人位置的差值，θ：围绕的旋转角度，α,β：胶囊机器人其它两个运动方向。

55、本发明的技术构思为：采用了基于动力学的手术机器人改进的积分分离pid控制方法，该方法具有以下优点：1)不依赖关键点,根据输入信息的响应频率实时对控制轨迹进行修改，在轨迹全流程让胶囊跟随着机械臂末端电磁铁，从而达到高精度跟踪的目的。2)仅根据胶囊的上一次位置信息，机械臂末端电磁铁的当前信息，就可以输出胶囊的当前位置以及机械臂末端电磁铁的下一个位置。相比于传统的建模并且计算准确模型然后再设计控制器的方法，本发明专利有着使用简单的特点，更适用于实际工程应用中；计算数据信息少，从而实用性更大的优点。并且做了相关实验，精度得到了相应验证。

56、在对胶囊机器人建模之后，按照顺序(1)分析胶囊机器人原理图，(2)求取胶囊机器人位置以及其轨迹规划方法，(3)求取机械臂末端磁铁位置，(4)建立末端磁铁以及胶囊机器人的闭环控制。根据当前的胶囊位姿信息，之前胶囊位姿信息以及当前机械臂末端磁铁位置信息，将规划位置与当前胶囊位置的差值输入到闭环改进的积分分离pid控制中，实时修改输入参数，达到胶囊实时跟踪规划轨迹的效果。

57、本发明的有益效果为：通过新提出的基于动力学的胶囊手术机器人闭环运动控制方法，可以避免胶囊在跟踪规划轨迹时依赖关键点，照成仅在关键点处跟踪精度较好的问题，改进方案为通过动力学关系，实时将规划位置与当前胶囊位置的差值输入到闭环改进的积分分离pid控制中，从而达到实时跟踪的目的。