CN114888810B - 一种末端仪器的防碰撞方法 - Google Patents

Abstract

本发明公开了一种末端仪器的防碰撞方法，末端仪器通过机械臂转动设置于转台上，包括步骤：建立末端各仪器的运动学模型，计算得到各仪器的受约束位姿；控制转台运动至基准仪器的基准点的正上方，并在转台运动过程中实时调整各仪器的关节位置，保持各仪器的受约束位姿固定。本发明同时对多个仪器关节进行控制，可以通过保持其相对位置来避免之间的碰撞，且实时计算补偿后的工具位姿，当位姿误差过大时，及时停止，以免对人员或设备的损伤，具有安全性保障。

Description

一种末端仪器的防碰撞方法

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种末端仪器的防碰撞方法。

背景技术

越来越多的装置被电子装置所替代，例如医疗机器人应用于医院、诊所的医疗或辅助医疗中，实现半自主或全自主的操作，它能够完成有益于人类健康的服务工作，如腹腔镜手术机器人系统在微创外科手术中的应用越来越广泛。

在人机协作、手术机器人等领域中，末端的仪器一般被用于定位、抓取、搬运等操作，且通常会设计冗余关节，以保证末端仪器操作的灵活性。当冗余关节存在时，可获得多种不同的关节位置组合来保证末端仪器保持固定位姿。这是零空间概念，即关节运动时，保证末端位姿固定的技术。在实际应用过程中，可以选择需要保持的仪器位置和姿态。

在机器人辅助手术的过程中，通常需要保证仪器位姿固定情况下，调节系统部件到特定位置来获取更大的手术工作空间，其中包含有与基准靶对准，即转台的旋转中心与保持位姿固定的基准仪器的基准点对准。通常在对准的过程中，需要保持基准仪器的位姿固定。但是基准仪器周围会存在其他仪器，当其他仪器保持与转台的随动时，基准仪器与其他仪器存在相对运动，这会增加仪器之间碰撞的风险。特别是当其他仪器在转台和基准仪器中间时，碰撞的可能性会大大增加。

发明内容

发明目的：本发明针对上述不足，提出一种末端仪器的防碰撞方法，以此避免各仪器之间的碰撞，且在此过程中可以避免对人员或设备的损伤，可以保证安全性。

技术方案：

一种末端仪器的防碰撞方法，末端仪器通过机械臂转动设置于转台上，包括步骤：

建立末端各仪器的运动学模型，计算得到各仪器的受约束位姿；

控制转台运动至基准仪器的基准点的正上方，并在转台运动过程中实时调整各仪器的关节位置，保持各仪器的受约束位姿固定。

所述计算得到各仪器的受约束位姿具体为：

确定各仪器需要保持固定的自由度，据此计算得到约束方程；

根据所述转台的当前关节位置及各仪器的当前关节位置计算得到各仪器的受约束位姿。

所述在转台运动过程中实时调整各仪器的关节位置具体为：

根据转台运动过程中转台各关节的实际位置更新所述约束方程，并结合所述各仪器的受约束位姿计算得到各仪器的关节目标位置，控制各仪器的关节运动。

所述控制转台运动至基准仪器的基准点的正上方具体为：

根据基准仪器的基准点位置计算得到转台的目标位置；

根据所述转台的当前位置和目标位置规划转台的运动轨迹，并据此控制所述转台运动。

还包括步骤：

在驱动各关节运动后，根据转台运动过程中各关节的实际位置及各仪器各关节的实际位置计算得到各仪器的实际位姿，并比对其与各仪器对应的受约束位姿之间的偏差，在其中任一偏差超出阈值，则停止转台各关节和各仪器各关节的驱动控制。

有益效果：

1、本发明同时对多个仪器关节进行控制，可以通过保持其相对位置来避免之间的碰撞。

2、相比于基于位姿误差的逆雅可比行列式求解方法，本发明通过确定受约束或保持固定的位姿，建立包含转台关节和基准仪器关节、转台关节和其他仪器关节位置变量的约束方程，采用解析法或者数值法求解并控制各仪器的各关节位置，其精度不受位姿误差大小的影响；

3、本发明实时计算补偿后的工具位姿，当位姿误差过大时，及时停止，以免对人员或设备的损伤，具有安全性保障。

附图说明

图1为本发明示例的基准点对准前手术机器人的示意图；

图2为本发明的转台和末端仪器的俯视图；

图3为本发明的末端仪器的防碰撞方法的流程图；

图4为图3中计算各仪器的受约束位姿的流程图；

图5为图3中控制转台运动至目标位置的流程图；

图6为图3中转台运动过程中各仪器保持位姿的流程图；

图7为本发明示例的基准点对准后手术机器人的示意图。

其中，1为手术机器人，11为转台，12为机械臂，13为仪器，13A为基准仪器。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

如图1、2所示，在机器人辅助手术中，手术机器人1设置有转台11，在转台11上设置有若干机械臂12，机械臂12的末端设置有仪器13，机械臂12可以随着转台11转动，并带动末端仪器13的移动，同时各仪器13本身具有若干运动自由度，可以通过控制机械臂12的各自由度关节的位置和各仪器13本身的运动自由度调整各仪器13的位姿；其中，在转台运动过程中，保持基准点对准且位姿固定的为基准仪器，如图2中的13A所示。

图3为本发明的末端仪器的防碰撞方法的流程图，如图3所示，本发明包括如下步骤：

（1）确定基准仪器及其他仪器的约束位姿，流程图如图4所示，包括如下步骤：

（11）建立末端各仪器的运动学模型；

根据转台、基准仪器和其他仪器的连杆结构DH参数，建立基准仪器和其他仪器相对于参考坐标系的运动学模型T _a 和T _b ，可表示为：

其中，参考坐标系可以设置为手术机器人基坐标系或转台基坐标系；

u _xa 、u _ya 、u _za 是指基准仪器的运动学模型所在坐标系x轴在参考坐标系下的朝向；v _xa 、v _ya 、v _za 是指基准仪器的运动学模型所在坐标系y轴在参考坐标系下的朝向；w _xa 、w _ya 、w _za 是指基准仪器的运动学模型所在坐标系z轴在参考坐标系下的朝向；p _xa 、p _ya 、p _za 是指基准仪器的运动学模型所在坐标系的原点在参考坐标系下的坐标，也即基准仪器末端在参考坐标系下的坐标；u _xb 、u _yb 、u _zb 是指其他仪器的运动学模型所在坐标系x轴在参考坐标系下的朝向；v _xb 、v _yb 、v _zb 是指其他仪器的运动学模型所在坐标系y轴在参考坐标系下的朝向；w _xb 、w _yb 、w _zb 是指其他仪器的运动学模型所在坐标系z轴在参考坐标系下的朝向；p _xb 、p _yb 、p _zb 是指其他仪器的运动学模型所在坐标系的原点在参考坐标系下的坐标，也即其他仪器末端在参考坐标系下的坐标；

（12）确定约束方程：根据实际应用场景，确定基准仪器和其他仪器需要保持固定的自由度（最多为六个自由度，本发明以六个自由度为例），即得到包含转台和各仪器关节变量的约束方程，其中各仪器包含基准仪器和其他仪器，本发明中以保持基准仪器末端空间位姿为例，从而得到基准仪器末端和其他仪器末端的空间位姿[p _xa p _ya p _za q _za q _ya q _za ]和[p _xb p _yb p _zb q _xb q _yb q _zb ]作为约束条件，保持其不变；其中，q _xa 、q _ya 、q _za 是指基准仪器末端在参考坐标系下分别绕x、y、z轴旋转的角度，q _xb 、q _yb 、q _zb 是指其他仪器末端在参考坐标系下分别绕x、y、z轴旋转的角度；

具体地，基准仪器通常为内窥镜，其他仪器为其他手术器械。

（13）计算各仪器的受约束位姿：分别获取转台、基准仪器和其他仪器当前的各关节位置φ ₁₀、φ ₂₀和φ ₃₀，通过步骤（11）中建立的运动学模型求解基准仪器末端和其他仪器末端的受约束位姿[p _xa0 p _ya0 p _za0 q _xa0 q _ya0 q _za0]和[p _xb0 p _yb0 p _zb0 q _xb0 q _yb0 q _zb0]，并将基准仪器末端的受约束位姿设定为固定位姿[p _xf p _yf p _zf q _xf q _yf q _zf ]，具体为：

其中，f _xa ( )、f _ya ( )、f _za ( )、f _xb ( )、f _yb ( )、f _zb ( )分别表示根据各关节位置得到对应坐标位置的函数，g _xa ( )、g _ya ( )、g _za ( )、g _xb ( )、g _yb ( )、g _zb ( )分别表示根据各关节位置得到绕对应坐标轴旋转角度的函数；

（2）调节转台的位置，以使转台的旋转中心和基准仪器的基准点竖直地对准（即转台的旋转中心位于基准仪器的基准点正上方），具体流程如图5所示，包括步骤：

（21）计算当前转台的旋转中心位置[p _xc0 p _yc0]，根据设计参数计算步骤（13）中的固定位姿[p _xf p _yf p _zf q _xf q _yf q _zf ]中基准仪器的基准点（即RCM点）位置，据此计算得到转台的目标位置，以使转台的旋转中心运动至基准仪器的基准点的正上方；

（22）根据步骤（21）得到的目标位置规划转台运动轨迹，具体地，可以在转台的当前位置与目标位置之间进行直线插补得到运动轨迹点，具体插补值为[p _xct p _yct ]；

（23）根据步骤（22）的规划实时调整转台位置；

（3）在转台移动过程中，实时调整基准仪器各关节的目标位置φ _2n 和其他仪器各关节的目标位置φ _3n ，保证基准仪器和其他仪器的受约束位姿固定，具体流程如图6所示，包括步骤：

（31）更新转台运动后的约束方程：转台的轨迹发生移动时，记录转台运动后转台各关节的实际位置φ _1n ，并更新步骤（12）得到的约束方程，具体如下：

（32）求解各仪器各关节的目标位置：基于步骤（31）中更新后的约束方程求解得到基准仪器各关节的目标位置φ _2n 和其他仪器各关节的目标位置φ _3n ，具体为：

（33）驱动各仪器关节运动：根据步骤（32）得到的各仪器的关节目标位置，控制各关节运动，从而完成基准点对准，且在此过程中不会发生仪器之间的碰撞，如图7所示；

更进一步地，在驱动各仪器关节运动后，还包括以下步骤：

（331）计算各仪器的实际位姿：根据步骤（31）得到的转台运动后各关节的实际位置和步骤（32）得到的基准仪器各关节的目标位置φ _2n 和其他仪器各关节的目标位置φ _3n 计算得到基准仪器的实际位姿[p _xan p _yan p _zan q _xan q _yan q _zan ]和其他仪器的实际位姿[p _xbn p _ybn p _zbn q _xbn q _ybn q _zbn ]，可表示为：

（332）判断位姿误差：判断基准仪器的实际位姿[p _xan p _yan p _zan q _xan q _yan q _zan ]和其他仪器的实际位姿[p _xbn p _ybn p _zbn q _xbn q _ybn q _zbn ]与其对应受约束位姿[p _xa0 p _ya0 p _za0 q _xa0 q _ya0 q _za0]和[p _xb0 p _yb0 p _zb0 q _xb0 q _yb0 q _zb0]之间的偏差[p _xae p _yae p _zae q _xae q _yae q _zae ]和[p _xbe p _ybe p _zbe q _xbe q _ybe q _zbe ]：

若其中任一偏差超出阈值，则停止转台各关节、基准仪器各关节和其他仪器各关节的驱动控制；若均未超出阈值，则重复步骤（2）；本发明中，p _xae、p _yae、p _zae、p _xbe、p _ybe、p _zbe的阈值为0.1mm，q _xae、q _yae、q _zae、q _xbe、q _ybe、q _zbe的阈值为0.05rad。

本发明通过确定受约束或保持固定的位姿，建立包含转台关节和基准仪器关节、转台关节和其他仪器关节位置变量的约束方程，当转台关节发生运动时，通过即采用解析法或数值法求解基准仪器关节和其他仪器关节的目标位置。同时，计算关节运动后的实际位姿，判断与约束位姿偏差是否过大，以此来保证保持末端仪器位姿过程中的安全性。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换（如数量、形状、位置等），这些等同变换均属于本发明的保护范围。

Claims (3)

1.一种末端仪器的防碰撞方法，末端仪器通过机械臂转动设置于转台上，其特征在于：包括步骤：

根据转台、基准仪器和其他仪器的连杆结构DH参数，建立末端各仪器相对于参考坐标系的运动学模型，确定各仪器需要保持固定的自由度，据此计算得到包含转台和各仪器关节变量的约束方程，其中，各仪器包含基准仪器和其他仪器，从而得到基准仪器末端和其他仪器末端的空间位姿[p _xa p _ya p _za q _za q _ya q _za ]和[p _xb p _yb p _zb q _xb q _yb q _zb ]作为约束条件，保持其不变；其中，p _xa 、p _ya 、p _za 是指基准仪器的运动学模型所在坐标系的原点在参考坐标系下的坐标，q _xa 、q _ya 、q _za 是指基准仪器末端在参考坐标系下分别绕x、y、z轴旋转的角度；p _xb 、p _yb 、p _zb 是指其他仪器的运动学模型所在坐标系的原点在参考坐标系下的坐标，q _xb 、q _yb 、q _zb 是指其他仪器末端在参考坐标系下分别绕x、y、z轴旋转的角度；

根据所述转台的当前关节位置φ ₁₀及基准仪器末端和其他仪器的当前关节位置φ ₂₀、φ ₃₀计算得到基准仪器末端和其他仪器末端的受约束位姿[p _xa0 p _ya0 p _za0 q _xa0 q _ya0 q _za0]和[p _xb0 p _yb0 p _zb0 q _xb0 q _yb0 q _zb0]；

；

；

其中，f _xa ( )、f _ya ( )、f _za ( )、f _xb ( )、f _yb ( )、f _zb ( )分别表示根据各关节位置得到对应坐标位置的函数，g _xa ( )、g _ya ( )、g _za ( )、g _xb ( )、g _yb ( )、g _zb ( )分别表示根据各关节位置得到绕对应坐标轴旋转角度的函数；

控制转台运动至基准仪器的基准点的正上方，并在转台运动过程中根据转台运动过程中转台各关节的实际位置更新所述约束方程，并结合所述各仪器的受约束位姿计算得到各仪器的关节目标位置，以实时调整各仪器的关节位置，保持各仪器的受约束位姿固定。

2.根据权利要求1所述的末端仪器的防碰撞方法，其特征在于：所述控制转台运动至基准仪器的基准点的正上方具体为：

根据基准仪器的基准点位置计算得到转台的目标位置；

根据所述转台的当前位置和目标位置规划转台的运动轨迹，并据此控制所述转台运动。

3.根据权利要求1所述的末端仪器的防碰撞方法，其特征在于：还包括步骤：

在驱动各关节运动后，根据转台运动过程中各关节的实际位置及各仪器各关节的实际位置计算得到各仪器的实际位姿，并比对其与各仪器对应的受约束位姿之间的偏差，在其中任一偏差超出阈值，则停止转台各关节和各仪器各关节的驱动控制。