[發明專利]控制柜、操作臺、控制器電控系統及機械臂運行方法

在審

• 申請號: 202010026418.5
• 申請日: 2020-01-10
• 公開（公告）號: CN111214198A
• 公開（公告）日: 2020-06-02
• 發明（設計）人: 鄔傳浪
• 申請（專利權）人: 重慶金山醫療技術研究院有限公司
• 主分類號: A61B1/04
• 分類號: A61B1/04;A61B1/00
• 代理公司: 重慶輝騰律師事務所 50215
• 代理人: 張小曉

1.一種膠囊控制柜電控系統，其特征是：包括供電模塊(110)；所述供電模塊(110)與嵌入式控制器(120)、網絡連接模塊(130)和驅動模塊連接；所述嵌入式控制器(120)的信號交互端與所述網絡連接模塊(130)的信號交互端連接；所述網絡連接模塊(130)的信號交互端與所述伺服驅動模塊的信號交互端連接。

2.如權利要求1所述的膠囊控制柜電控系統，其特征是：所述供電模塊(110)包括依次連接的供電電源(111)、總控制開關(112)、隔離變壓器(113)和濾波器(114)；所述濾波器(114)分別與第一穩壓電源(115)和第二穩壓電源(116)連接；所述第一穩壓電源(115)與所述嵌入式控制器(120)和網絡連接模塊(130)連接；所述第二穩壓電源(116)與所述驅動模塊連接。

3.如權利要求1所述的膠囊控制柜電控系統，其特征是：所述驅動模塊包括上五軸伺服驅動模塊(140)和下五軸伺服驅動模塊(150)；

所述網絡連接模塊(130)包括信號交互端彼此連接的第一耦合器(131)和第二耦合器(132)；

所述第一耦合器(131)的輸出端與所述上五軸伺服驅動模塊(140)的接收端連接；所述第二耦合器(132)的輸出端與所述下五軸伺服驅動模塊(150)的接收端連接；

所述供電模塊(110)同時與所述第一耦合器(131)和第二耦合器(132)連接。

4.如權利要求3所述的膠囊控制柜電控系統，其特征是：所述上五軸伺服驅動模塊(140)包括信號交互端彼此連接的上伺服驅動器(141)和上伺服驅動電機(142)。

5.如權利要求3所述的膠囊控制柜電控系統，其特征是：所述下五軸伺服驅動模塊(150)包括信號交互端彼此連接的下伺服驅動器(151)和下伺服驅動電機(152)。

6.一種膠囊操作臺電控系統，其特征是：包括狀態指示燈(210)、工控機(220)和顯示器(230)；所述工控機(220)和顯示器(230)的信號交互端彼此連接；所述工控機(220)的連接有左操作手柄(221)、右操作手柄(222)、控制按鈕(223)和鼠標鍵盤(224)。

7.一種膠囊控制器電控系統，其特征是：包括如權利要求1至5任一所述的膠囊控制柜電控系統和如權利要6所述的膠囊操作臺電控系統；所述供電模塊(110)與所述工控機(220)和顯示器(230)連接；所述嵌入式控制器(120)與所述狀態指示燈(210)連接；所述嵌入式控制器(120)采用EtherNET總線通信的方式與所述網絡連接模塊(130)連接；所述嵌入式控制器(120)與所述工控機(220)連接。

8.一種膠囊控制器機械臂軌跡運行方法，包括以下步驟：

1)根據機械臂末端的坐標值，確定控制臂的一個或多個關節所需旋轉的角度，規劃機械臂末端的運行軌跡；

2)通過對驅動機械臂的電機運轉角度和速度的控制，使機械臂對應的關節在特定時間內轉過相應的角度，從而使機械臂末端的運行軌跡符合步驟1)的規劃。

9.如權利要求8所述的膠囊控制器機械臂軌跡運行方法，其特征是：步驟1)中，通過以下公式確定關節所需角度：

式中，H為地面到旋轉基座端面的高度，a為基座的水平分量，b為基座的豎直分量，L1為俯仰臂兩軸心長度，L2為水平連接件長度，L3為擺臂兩軸心長度，H、a、b、L1、L2、L3均為常量；α為機械臂末端與Y軸的夾角；β為機械臂末端與Y軸的夾角；γ為機械臂相對Z軸的旋轉角度；x為機械臂末端X方向的坐標值；y為機械臂末端Y方向的坐標值；z為機械臂末端Z方向的坐標值。

10.如權利要求8或9所述的膠囊控制器機械臂軌跡運行方法，其特征是：還包括以下步驟：

3)實時讀取控制機械臂的關節的角度量，當機械臂末端實際運行軌跡與規劃軌跡發生偏差時，實時調整控制機械臂的電機的角度和速度，使機械臂末端實際運行軌跡趨近規劃軌跡。