[發明專利]平面4R欠驅動機械臂位姿控制方法

在審

• 申請號: 201710054336.X
• 申請日: 2017-01-22
• 公開（公告）號: CN106584465A
• 公開（公告）日: 2017-04-26
• 發明（設計）人: 余躍慶;毛冬冬
• 申請（專利權）人: 北京工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J9/06;B25J17/02
• 代理公司: 北京思海天達知識產權代理有限公司11203
• 代理人: 沈波

1.平面4R欠驅動機械臂位姿控制方法，平面4R欠驅動機械臂的結構包括欠驅動機械臂的第四桿(1)、欠驅動機械臂的第三桿(3)、欠驅動機械臂的第二桿(5)、欠驅動機械臂的第一桿(7)、第四編碼器(2)、第三編碼器(4)、第二編碼器(6)、第一編碼器(8)、基座(9)、第一主動關節伺服控制端(10)、第二主動關節伺服控制端(11)、第三主動關節伺服控制端(12)、被動關節(13)；被動關節(13)未設伺服控制端，處于完全自由狀態。伺服控制端由驅動電機和減速器組成，伺服控制端由相應編碼器控制，驅動電機和減速器連接，減速器的輸出軸和各主動關節連接。

欠驅動機械臂的第四桿(1)、欠驅動機械臂的第三桿(3)、欠驅動機械臂的第二桿(5)、欠驅動機械臂的第一桿(7)依次連接組成平面4R欠驅動機械臂的主體結構，第四編碼器(2)、第三編碼器(4)、第二編碼器(6)、第一編碼器(8)依次設置在欠驅動機械臂的第四桿(1)、欠驅動機械臂的第三桿(3)、欠驅動機械臂的第二桿(5)、欠驅動機械臂的第一桿(7)的末端，第一主動關節伺服控制端(10)、第二主動關節伺服控制端(11)、第三主動關節伺服控制端(12)分別設置在第一編碼器(8)、第二編碼器(6)、第三編碼器(4)的底部，被動關節(13)設置在第四編碼器(2)的底部；第一主動關節伺服控制端(10)安裝在基座(9)上。

其特征在于：

S1構建ADAMS虛擬樣機模型；

由于ADAMS對復雜機械模型的建模能力相對薄弱，選擇利用SolidWorks三維設計軟件建立水平欠驅動機械手的三維機械模型，將其保存為Parasolid格式，然后導入到ADAMS中。在ADAMS軟件中對每個零件進行屬性編輯，定義各零件的質量、材料、各關節摩擦因數物理屬性；然后在基座9與大地之間添加固定約束，在四個關節上添加轉動副，最后在前三個關節的轉動副上添加轉矩來代替電機的輸入轉矩，在第四個轉動副處不添加轉矩表示該關節是自由關節；考慮到各個關節的摩擦力對機械臂運動的影響，在關節處添加摩擦阻尼器達到更加接近真實的物理屬性。經過該過程建立的虛擬樣機具有和欠驅動試驗臺相同的物理屬性，用來實現平面4R欠驅動機械臂的動力學控制問題的仿真分析。

S2確定ADAMS機械系統的輸入和輸出變量，導出控制系統；

ADAMS實時獲取控制系統輸出的關節轉矩的數值，并同時將各關節的角度和末端點的軌跡誤差反饋到控制系統中的模糊控制器中，從而構成閉環控制。各個變量的物理意義，q1、q2、q3分別表示第一主動關節的驅動轉矩、第二主動關節的驅動轉矩、第三主動關節的驅動轉矩；a1、a2、a3、a4分別表示第一關節的關節角度、第二關節的關節角度、第三關節的關節角度、第四關節的關節角度，并且a1、a2、a3、a4以相對坐標的方式度量；d表示機械臂末端點p到期望軌跡S上任一點的法向距離；aa表示被動桿即欠驅動機械臂的第四桿的姿態角度，且aa＝a1+a2+a3+a4。

根據關節型機器人的控制原理，在ADAMS中定義三個主動關節的輸入轉矩(q1、q2、q3)為系統的輸入變量，存放MATLAB控制系統輸出的轉矩值；定義四個關節角度(a1、a2、a3、a4)、末桿姿態角偏差(aa)和末端點軌跡偏差(d)為輸出變量，作為系統的反饋輸入到系統中。在構建完成的欠驅動機械臂虛擬樣機中添加輸入輸出變量，然后導出欠驅動控制系統的機械子系統。

S3制定模糊規則，設計模糊控制器；

對于欠驅動系統，由于主、被動桿在不同的位形下具有不同的耦合規律，所以在制定模糊規則時需要考慮運動過程中主、被動桿的位形。分別對位置控制和軌跡跟蹤控制進行說明不同位形下的控制規則。

主動桿和被動桿之間的相對位形共有四種基本情況，當a4為銳角并且為負值時，這種情況為位形a)；當a4為銳角并且為正值時，這種情況為位形b)；當a4為鈍角并且為正值時，這種情況為位形c)；當a4為鈍角并且為負值時，這種情況為位形d)。進行姿態控制時，相對位置為a)和b)時，被動桿和主動桿轉向相反，記為Flag＝1；相對位置為c)和d)時，被動桿和主動桿轉向相同，記為Flag＝2。進行軌跡跟蹤控制時，相對位置為a)、c)和d)時，末端點運動趨勢與主動桿轉向一致，記為Flag＝2；相對位置為b)時，末端點運動趨勢與主動桿轉向相反，記為Flag＝1。

根據在實驗臺的手動經驗總結出模糊規則，模糊規則的形式采用“If...Then...”的形式，具體如表1和表2所示。其中，{NB、NS、ZO、PS、PB}為五個模糊子集，分別表示“負大”、“負小”、“零”、“正小”、“正大”。為了精確地表達動力學耦合規律，采用高斯函數作為隸屬度函數。

表1 FLC1、FLC2模糊控制規則

表2 FLC3模糊控制規則

然后打開MATLAB中的模糊控制工具箱，首先編輯軌跡控制器FLC1和FLC2，并把上述模糊控制規則輸入到模糊控制器中，然后把該模糊控制器的.fis文件保存到電腦，模糊控制器FLC3的設計同上。

S4構建控制系統框圖；

把步驟2中的ADAMS的工作路徑添加到MATLAB的工作路徑中，在MATLAB的命令窗口中輸入控制系統的文件名，然后再輸入adams_sys，打開步驟2中導出的控制系統。在Simulink中新建一個控制框圖，把adams_sub復制進來，依次添加Fuzzy Logic Controller模塊、增益模塊、顯示模塊、延時模塊，根據控制策略設計出聯合仿真控制系統框圖。