[發明專利]一種醫療機械臂的控制方法及裝置

有效

• 申請號: 201410489915.3
• 申請日: 2014-09-23
• 公開（公告）號: CN104260095B
• 公開（公告）日: 2016-04-27
• 發明（設計）人: 丁大民;鄧琛;王朝斌
• 申請（專利權）人: 上海工程技術大學
• 主分類號: B25J9/18
• 分類號: B25J9/18
• 代理公司: 上海科盛知識產權代理有限公司 31225
• 代理人: 趙繼明

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單元和電機驅動電路，所述電機驅動電路與 驅動電機連接，所述單片機與角度傳感器和位置傳感器連接。