1.一种胶囊内窥镜机器人系统中机械臂PID整定方法，其特征在于，所述方法包括以下步骤：步骤1，通过以胶囊机器人的中心为原点建立坐标系，并将磁铁置于胶囊中心位置，将其视作磁偶极子；

步骤2，建立胶囊机器人的动力学模型，初始化系统状态以及控制参数；

步骤3，根据步骤2得到的胶囊机器人动力学模型，设计控制器，过程如下：

3.1定义跟踪误差:

ek＝q‑qd                       (9)其中qd为胶囊机器人期望位姿矩阵；

增量式PID控制器形式:

式中， Kd＝KpTd，T为采样周期；k为采样序列，k＝1，2，…；ek‑1和ek采分别是k‑1和k时刻所得的偏差信号；

3.2Ziegler‑Nichols(Z‑N)扩充临界比例法整定PID初值；

令Ti＝∞，Td＝0，改变kp使其到达临界值kr，系统输出首次呈现震荡的增益值，Tr为响应的震荡周期；

将调节器选为纯比例调节器，形成闭环，逐渐改变比例系数，使系统对阶跃输入的响应达到临界振荡状态，将此时的比例系数记为Kr，临界振荡的周期记为Tr，根据Z‑N提供的经验公式，得Kp，Ki，Kd参数如下按照先P后I最后D的操作，将控制器整定参数调整到计算值上；

3.3PID归一参数整定P值

根据z‑n公式结论，Ti＝∞，Td＝0.12Tr，Kr＝40，Tr＝0.57，T＝0.015，其中由于Z‑N扩充临界比例法需要调节T，Ti，Td，Kp4个参数，简化为对一个参数Kp的整定；

控制器表达式

3.4积分分离的PID参数整定，过程如下：控制器形式

式中，T为采样时间；β为积，分项的开关系数；

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

参数整定的数量级与Z‑N整定的预期相符；

3.6通过原胶囊机器人动力学方程：

即求解出驱动力f为：

其中 为期望位姿矩阵qd的二阶导数， 为期望位姿矩阵qd的一阶导数，为位姿矩阵q的二阶导数，为位姿矩阵q的一阶导数，C1,G1,B1分别为变换后的胶囊机器人的惯性矩阵，科里奥利矩阵和重力矩阵；

3.7通过不同轨迹跟踪实验

对胶囊内窥镜机器人系统中机械臂进行测试实验。

2.如权利要求1所述的一种胶囊内窥镜机器人系统中机械臂PID整定方法，其特征在于，所述3.2中，当只用P调节器时，Kp＝40时系统进入震荡状态，则Kr＝40时；震荡周期Tr得为0.57s；所述3.4中，由z‑n扩充临界比例法查表可得Kp＝24，Ki＝1.263，Kd＝109.44；所述

3.5中，经过实际系统整定参数，可得参数为Kp＝18,Ki＝1.2,Kd＝200。

3.如权利要求1或2所述的一种胶囊内窥镜机器人系统中机械臂PID整定方法，其特征在于，所述步骤1按照以下步骤进行：

1.1记胶囊中心位置为：

[x,y,z]        (1)其中，x,y,z分别表示胶囊机器人中心三维坐标的三个坐标轴；

1.2根据机器人学中的坐标变换原理，将胶囊内的任意一点标记为其相应的坐标，如下所示：其中，胶囊中心绕z轴旋转角度为ψ，绕x轴旋转角度为θ，绕y轴旋转角度为φ，l为预设的长度；

因此胶囊机器人的动能和势能和写为：

其中 表示胶囊机器人的总动能， 则表示胶囊势能，R为胶囊机器人长度的一半，M为胶囊机器人质量。

4.如权利要求3所述的一种胶囊内窥镜机器人系统中机械臂PID整定方法，其特征在于，所述步骤2的过程如下：根据步骤1建立的胶囊机器人的位姿坐标矩阵如下：T

q＝[x y z ψ θ φ]                        (4)根据拉格朗日动力学方程得到胶囊机器人的动态模型方程：该方程中：

T

G＝[0 0 ‑Mg 0 0 0]           (8)其中，B,C,G分别为胶囊机器人的惯性矩阵，科里奥利矩阵和重力矩阵；为位姿矩阵q的二阶导数，为位姿矩阵的一阶导数，f为胶囊机器人受到的磁驱动力，驱动力与胶囊机器人内置永磁体和驱动永磁体的相对位置有关。