[發明專利]一種基于變質心的機器人越障控制方法和系統

有效

• 申請號: 202111207023.6
• 申請日: 2021-10-18
• 公開（公告）號: CN113641180B
• 公開（公告）日: 2022-01-11
• 發明（設計）人: 陶永;高赫;溫宇方;蘭江波;劉海濤;韓棟明
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京中知法苑知識產權代理有限公司 11226
• 代理人: 陳俊由;李明

1.一種基于變質心的機器人越障控制方法，其特征在于，包括如下步驟：

S1：建立擺臂式履帶機器人變結構質心運動學模型，計算障礙相對于機器人本體的障礙估算高度，機械臂末端觸碰障礙頂部，獲取機器人姿態信息，基于機器人姿態信息計算障礙高度以獲得障礙相對于機器人本體的障礙估算高度；

機器人在機器人坐標系下質心坐標為：

其中，為整機質心在機器人坐標系下的坐標，、分別為整機和機械臂質量，分別為主履帶、前擺臂、機械臂大臂、機械臂小臂和機械臂末端質量，分別為底盤、前擺臂質心距離機器人坐標系原點的距離，分別表示機械臂大臂、機械臂小臂、機械臂末端連桿質心到起始關節的距離，表示機械臂大臂、機械臂小臂連桿長度，為前擺臂與機器人坐標系軸的夾角，為各連桿與機器人坐標系X軸之間的角度，為機械臂底座中心距機器人坐標系原點方向和方向的偏移量；

S2：基于障礙估算高度和動作序列規則生成動作控制序列，所述動作控制序列包括一個或多個子動作；

S3：基于子動作與變結構質心運動學模型預測質心變化狀態，根據變結構質心運動學模型計算在子動作和子動作控制量下質心變化后的坐標；

S4：基于質心約束和越障質心需求評價質心變化狀態是否滿足越障需求，若滿足，執行S6，否則執行S5；

S5：基于預測質心變化狀態、質心約束和越障質心需求修正子動作，返回步驟S3；

S6：輸出子動作至擺臂式履帶機器人控制器，根據子動作控制機器人越障。

2.根據權利要求1所述的一種基于變質心的機器人越障控制方法，其特征在于，所述S1具體包括：

機器人質心在機器人坐標系中的坐標與在世界坐標系的坐標轉換公式為：

其中：表示機器人質心坐標在機器人坐標系與世界坐標系的坐標轉換，為機器人坐標系與世界坐標系之間的齊次變換矩陣，為整機質心在機器人坐標系下的坐標，為中間參數，為繞固定世界坐標系機器人的俯仰角，為坐標系變換的平動系數，

分別為主履帶、前擺臂、機械臂大臂、機械臂小臂和機械臂末端質量，分別為底盤和機械臂質量，分別為底盤、前擺臂質心距離機器人坐標系原點的距離，表示機械臂大臂、機械臂小臂、機械臂末端連桿質心到關節2、3、4的距離，表示機械臂大臂、機械臂小臂連桿長度，為機械臂底座中心距機器人坐標系原點方向和方向的偏移量。

3.根據權利要求1所述的一種基于變質心的機器人越障控制方法，其特征在于，所述機械臂末端觸碰障礙頂部，獲取機器人姿態信息，基于機器人姿態信息計算障礙高度以獲得障礙相對于機器人本體的障礙估算高度，具體包括：

建立機械臂載荷運動學模型；

建立機械臂末端位姿與機器人車體的位姿變換矩陣；

確定機器人姿態，基于機器人姿態計算障礙與地面的相對高度。

4.根據權利要求3所述的一種基于變質心的機器人越障控制方法，其特征在于，所述機械臂末端位姿與機器人車體的位姿變換矩陣為：

其中，為機械臂末端相對底座的平移矩陣；為機械臂末端相對底座的旋轉矩陣；為底座坐標系0到末端坐標系4的齊次變換矩陣，為底座坐標系0到大臂坐標系1的齊次變換矩陣，為大臂坐標系1到小臂坐標系2的齊次變換矩陣，為小臂坐標系2到腕部坐標系3的齊次變換矩陣，為腕部坐標系3到末端坐標系4的齊次變換矩陣。

5.根據權利要求3所述的一種基于變質心的機器人越障控制方法，其特征在于，若擺臂式履帶機器人姿態為水平，障礙與機器人移動平臺的相對高度為：，其中，為機械臂末端相對底座的平移矩陣中Z方向的數據，h為機械臂底座中心距機器人坐標系原點y方向的偏移量。

6.根據權利要求3所述的一種基于變質心的機器人越障控制方法，其特征在于，若擺臂式履帶機器人姿態為傾斜，障礙與機器人移動平臺的相對高度為：，其中，為固定世界坐標系機器人的俯仰角，為機械臂末端相對底座的平移矩陣中Z方向的數據，h為機械臂底座中心距機器人坐標系原點y方向的偏移量，為底盤質心距離機器人坐標系原點的距離。