CN104260095B

CN104260095B - 一种医疗机械臂的控制方法及装置

Info

Publication number: CN104260095B
Application number: CN201410489915.3A
Authority: CN
Inventors: 丁大民; 邓琛; 王朝斌
Original assignee: Shanghai University of Engineering Science
Current assignee: Shanghai University of Engineering Science
Priority date: 2014-09-23
Filing date: 2014-09-23
Publication date: 2016-04-27
Anticipated expiration: 2034-09-23
Also published as: CN104260095A

Abstract

本发明涉及一种医疗机械臂的控制方法和装置，该方法包括以下步骤：A.初步定位，获取目标点位置和医疗机械臂末端位置，结合医疗机械臂各定位关节的当前定位夹角，计算各定位夹角的目标值，并驱动医疗机械臂各定位关节动作；B.精确调整，根据医疗机械臂末端和目标点的相对距离，进行微调定位；C.竖直矫正，控制医疗机械臂竖直向下运动至目标点位置。控制装置包括角度传感器、位置传感器、驱动电机和控制模块，控制模块的输入端与角度传感器和位置传感器连接，输出端与驱动电机的输入端连接，驱动电机的输出端与医疗机械臂连接。与现有技术相比，本发明具有定位精准等优点。

Description

一种医疗机械臂的控制方法及装置

技术领域

本发明涉及医疗器械领域，尤其是涉及一种医疗机械臂的控制方法及装置。

背景技术

在机械臂轨迹运动控制中，机械臂运动的速度和精度都是极重要的指标，尤其是在医疗器械行业相关机械臂产品中，精度的要求非常高，例如手术机器人。目前国产的医疗机械臂种类稀少，并且对电机要求也非常高，比如大量采用功率密度高，寿命短的超声波电机。在供电方面采用高性能的二相驱动电源，整个系统对环境温度、运行中温度、负载特性、预压力要求高。

在精度控制方面，市场上的医疗机械臂采用单反馈甚至无反馈开环控制系统驱动电机运动，精度方面有待提高。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种定位精准的医疗机械臂的控制方法及装置。

本发明的目的可以通过以下技术方案来实现：

一种医疗机械臂的控制方法，该方法包括以下步骤：

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.控制医疗机械臂竖直向下运动靠近目标点。

所述步骤A3或步骤A5中依次为从医疗机械臂根部至末端。

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

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

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

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

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

一种医疗机械臂的控制装置，该装置用于控制医疗机械臂的运动，所述控制装置包括角度传感器、位置传感器、驱动电机和控制模块，所述控制模块的输入端与角度传感器和位置传感器连接，输出端与驱动电机的输入端连接，所述驱动电机的输出端与医疗机械臂连接，所述角度传感器设在医疗机械臂上，所述位置传感器设在医疗机械臂末端。

所述医疗机械臂包括定位关节、调整关节和竖直运动组件，所述驱动电机包括定位关节电机、调整关节电机和竖直运动电机，所述定位关节电机、调整关节电机和竖直运动电机分别与定位关节、调整关节和竖直运动组件连接。

所述角度传感器设在定位关节和调整关节上。

所述定位关节和调整关节的数量均至少为两个。

所述角度传感器的个数为定位关节和调整关节的数目之和，所述定位关节电机和调整关节电机的数目分别与定位关节和调整关节的数目相同。

所述控制模块包括依次连接的单片机、PWM单元和电机驱动电路，所述电机驱动电路与驱动电机连接，所述单片机与角度传感器和位置传感器连接。

与现有技术相比，本发明具有以下优点：

1)角度反馈和位置反馈双反馈机制，提高精确度。

2)因为是一次性计算好所有关节角度的变化，因此可以一次性供给限定数量的脉冲，抗干扰。

3)分次调节，先进行初步定位，在进行精确定位可以增加定位的精度。

4)保证医疗机械臂末端的竖直高度不低于目标点的竖直高度，可以保证不伤到病人。

5)关节的运动顺序为从医疗机械臂的根部至末端，这样可以减少医疗机械臂的调整次数，更加高效。

附图说明

图1为本发明的结构示意图；

图2为实施例一结构示意图；

图3为实施例三结构示意图；

其中：1、医疗机械臂，2、角度传感器，3、位置传感器，4、驱动电机，5、控制模块，111、第一定位关节，112第二定位关节，121、第一调整关节，122、第二调整关节，131、导轨，14、手术刀片。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例一：

一种医疗机械臂的控制装置，该装置用于控制医疗机械臂1的运动，如图1所示，该装置包括角度传感器2、位置传感器3、驱动电机4和控制模块5，控制模块5的输入端与角度传感器2和位置传感器3连接，输出端与驱动电机4的输入端连接，驱动电机4的输出端与医疗机械臂1连接，其中，角度传感器2设在医疗机械臂1上，位置传感器3设在医疗机械臂1末端。

如图2所示，医疗机械臂1包括定位关节、调整关节和竖直运动组件13，驱动电机4包括定位关节电机、调整关节电机和竖直运动电机，定位关节电机、调整关节电机和竖直运动电机分别与定位关节、调整关节和竖直运动组件13连接。

如图2所示，竖直运动组件13设有导轨131，依此实现医疗机械臂1的竖直运动，定位关节包括第一定位关节111和第二定位关节，其中第一定位关节111实现医疗机械臂1末端在纸面内横向运动，第二定位关节112实现医疗机械臂1末端在垂直纸面方向上运动，调整关节也包括第一调整关节121和第二调整关节122，其结构和原理与定位关节类似，但结构更小，并靠近医疗机械臂1末端，因为其调整更加精确。

角度传感器2设在定位关节和调整关节上，每一个关节处均设有一个角度传感器2。

角度传感器2的个数为4个，定位关节电机和调整关节电机的数目分别为两个。

控制模块5包括依次连接的单片机、PWM单元和电机驱动电路，电机驱动电路与驱动电机4连接，单片机与角度传感器2和位置传感器3连接。

上述医疗机械臂的控制方法，该方法包括以下步骤：

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

A1.获取状态信息，以下单位均为毫米，状态信息包括目标点位置(20，30，30)、医疗机械臂1末端位置(30，30，35)和各定位关节的当前定位夹角；

A2.设定医疗机械臂1末端目标位置(20，30，z)，并计算各定位夹角的目标值；

A3.计算各定位夹角依次转动至目标值的过程中z坐标的最小值为25，因为25小于30，故执行步骤A4；

A4.控制医疗机械臂1竖直向上移动6的距离；

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

A6.控制医疗机械臂1竖直向下运动靠近目标点使z坐标为31。

步骤A3或步骤A5中依次为从医疗机械臂1根部至末端。

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

步骤B具体包括步骤：

B1.获取医疗机械臂1末端位置(20.2，30，6)和各调整关节的当前调整夹角，计算机械臂末端位置与目标点位置的水平距离并判断距离d与阈值ε(阈值ε为根据要求设定，本实施例中为0.01)的大小，此处d大于ε，故执行步骤B2；

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

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

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

B1.获取医疗机械臂1末端位置(20.01，30，6)和各调整关节的当前调整夹角，计算机械臂末端位置与目标点位置的水平距离并判断距离d与阈值ε的大小，此处d等于ε，达到要求，故执行步骤C。

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

实施例二：

与实施例一中相同之处不再叙述，不同之处为，实施例二中第一定位关节111和第一调整关节121为同一个关节，第二定位关节121和第二调整关节122为同一个关节。

实施例三：

与实施例一中相同之处不再叙述。

如图3所示，本实施例中竖直运动组件13为伸缩机构，不再由导轨131实现竖直运动。

Claims

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.依次控制各调整关节转动至目标调整夹角；

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

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

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

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

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

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