[發明專利]一種受約束的機械臂零力控制系統及其控制方法

有效

• 申請號: 202111497390.4
• 申請日: 2021-12-09
• 公開（公告）號: CN114145848B
• 公開（公告）日: 2023-08-25
• 發明（設計）人: 程敏;楊輝;陳云川;袁文;申登偉
• 申請（專利權）人: 佗道醫療科技有限公司
• 主分類號: A61B34/30
• 分類號: A61B34/30;A61B34/00
• 代理公司: 南京寧慧知識產權代理事務所(普通合伙) 32766
• 代理人: 唐紹焜

1.一種受約束的機械臂零力控制系統，其特征在于：包括：

動力學補償控制器，用于根據關節自身重力、所受摩擦力控制各關節電機輸出補償力矩；

速度限制控制器，根據笛卡爾空間及關節空間的速度限制分別計算得到笛卡爾空間及關節空間的速度限制力矩，并將笛卡爾空間的速度限制力矩映射到關節空間得到根據笛卡爾空間的速度限制對參與運動的每個關節提供的反向力矩，通過該反向力矩與關節空間的速度限制力矩之間的加權計算得到為各關節電機輸出的速度限制力矩；

位置限制控制器，根據笛卡爾空間及關節空間的位置限制分別計算得到笛卡爾空間及關節空間的位置限制力矩，并將笛卡爾空間的位置限制力矩映射到關節空間得到根據笛卡爾空間的位置限制對參與運動的每個關節提供的反向力矩，通過該反向力矩與關節空間的位置限制力矩之間的加權計算得到為各關節電機輸出的位置限制力矩；

機械臂各關節電機在前述三個控制器的控制下輸出合力矩。

2.根據權利要求1所述的機械臂零力控制系統，其特征在于：所述動力學補償控制器根據關節自身重力、所受摩擦力控制各關節電機輸出補償力矩具體如下：

通過牛頓歐拉動力學或拉格朗日動力學計算各個關節所需的克服關節重力所需的補償力矩Mg和克服摩擦力所需的補償力矩Mf：

Mg＝g(q)

其中，g(q)表示關節重力矩，表示關節的轉動速度，表示關節轉動的粘滯摩擦力矩，表示關節轉動的庫倫摩擦力矩。

3.根據權利要求1所述的機械臂零力控制系統，其特征在于：所述速度限制控制器為各關節提供速度限制力矩具體如下：

假定笛卡爾空間速度為其中，分別表示機械臂的笛卡爾空間速度和角速度；

當分別表示設定的機械臂的笛卡爾空間速度最大值和角速度最大值；則提供笛卡爾空間限制力矩將笛卡爾空間力矩映射到關節空間得到根據笛卡爾空間的速度限制對參與運動的每個關節提供的反向力矩其中K₁表示笛卡爾空間速度限制權重，J表示雅可比矩陣；

假定某個關節實時速度為θ，當|θ|θ_max，提供關節空間限制力矩其中，θ_max表示設定的關節速度最大值，k₁表示關節空間速度限制權重；

據此得到速度限制力矩為

4.根據權利要求1所述的機械臂零力控制系統，其特征在于：所述位置限制控制器為各關節提供位置限制力矩具體如下：

假設在笛卡爾空間觸發限制的邊界位置為當機械臂在笛卡爾空間的實際位置且機械臂的運動方向依然有加大其在笛卡爾空間的實際位置超出邊界位置的趨勢時，則提供笛卡爾空間限制力矩將笛卡爾空間力矩映射到關節空間根據笛卡爾空間的位置限制對參與運動的每個關節提供的反向力矩其中，P_limx、P_limy、P_limz分別表示笛卡爾空間X、Y、Z軸方向上的限制邊界，(P_x,P_y,P_z)分別表示機械臂在笛卡爾空間的實際位置坐標，K₂表示笛卡爾空間位置限制權重；J表示雅可比矩陣；

假定某個關節實時位置為p，當某個關節的實時位置|p|p_lim且運動方向依然有加大其在關節空間內的實時位置超出其位置邊界的趨勢時，提供關節空間限制力矩其中，p_lim表示在關節空間內某個關節的位置邊界，k₂表示關節空間位置限制權重；

據此得到用于位置限制的合力矩為

5.根據權利要求1所述的機械臂零力控制系統，其特征在于：還包括力矩限制控制器，用于根據前述三個控制器控制各關節電機輸出力矩的合力矩及對應關節電機的最大允許輸出力矩控制各關節電機輸出力矩。