[发明专利]一种医疗机械臂的控制方法及装置

权利要求书

1.一种医疗机械臂的控制方法，其特征在于，该方法包括以下步骤：

A.初步定位，获取目标点位置和医疗机械臂末端位置，结合医疗机械臂各定 位关节的当前定位夹角，计算各定位夹角的目标值，并驱动医疗机械臂各定位关节 动作，

B.精确调整，根据医疗机械臂末端和目标点的相对距离，进行微调定位，

C.竖直矫正，控制医疗机械臂竖直向下运动至目标点位置，

所述步骤A具体包括步骤：

A1.获取状态信息，所述状态信息包括目标点位置(x₁,y₁,z₁)、医疗机械臂末 端位置(x₀,y₀,z₀)和各定位关节的当前定位夹角，

A2.设定医疗机械臂末端目标位置(x₁,y₁,z)，并计算各定位夹角的目标值，

A3.计算各定位夹角依次转动至目标值的过程中z坐标的最小值z_min，并判断 z_min与z₁的大小，若z₁大于z_min，则执行步骤A4，反之，则执行步骤A5，

A4.控制医疗机械臂竖直向上移动大于(z₁-z_min)的距离，

A5.依次控制各定位关节转动至目标定位夹角，

A6.控制医疗机械臂竖直向下运动靠近目标点。

2.根据权利要求1所述的一种医疗机械臂的控制方法，其特征在于，所述步 骤A3或步骤A5中依次为从医疗机械臂根部至末端。

3.根据权利要求1所述的一种医疗机械臂的控制方法，其特征在于，所述步 骤B具体包括步骤：

B1.获取医疗机械臂末端位置(x₂,y₂,z₂)和各调整关节的当前调整夹角，计算 机械臂末端位置与目标点位置的水平距离并判断距离d 与阈值ε的大小，若d大于ε，则执行步骤B2，反之，则执行步骤C；

B2.设定医疗机械臂末端目标位置(x₁,y₁,z)，并计算各调整夹角的目标值；

B3.依次控制各调整关节转动至目标调整夹角；

B4.控制医疗机械臂竖直向下运动靠近目标点，并执行步骤B1。

4.一种实现如权利要求1所述的医疗机械臂的控制方法的装置，该装置用于 控制医疗机械臂的运动，其特征在于，该装置包括角度传感器、位置传感器、驱动 电机和控制模块，所述控制模块的输入端与角度传感器和位置传感器连接，输出端 与驱动电机的输入端连接，所述驱动电机的输出端与医疗机械臂连接，所述角度传 感器设在医疗机械臂上，所述位置传感器设在医疗机械臂末端。

5.根据权利要求4所述的一种医疗机械臂的控制装置，其特征在于，所述医 疗机械臂包括定位关节、调整关节和竖直运动组件，所述驱动电机包括定位关节电 机、调整关节电机和竖直运动电机，所述定位关节电机、调整关节电机和竖直运动 电机分别与定位关节、调整关节和竖直运动组件连接。

6.根据权利要求5所述的一种医疗机械臂的控制装置，其特征在于，所述定 位关节和调整关节的数量均至少为两个。

7.根据权利要求6所述的一种医疗机械臂的控制装置，其特征在于，所述角 度传感器设在定位关节和调整关节上，所述角度传感器的个数为定位关节和调整关 节的数目之和。

8.根据权利要求5所述的一种医疗机械臂的控制装置，其特征在于，所述定 位关节电机和调整关节电机的数目分别与定位关节和调整关节的数目相同。

9.根据权利要求4所述的一种医疗机械臂的控制装置，其特征在于，所述控 制模块包括依次连接的单片机、PWM单元和电机驱动电路，所述电机驱动电路与 驱动电机连接，所述单片机与角度传感器和位置传感器连接。