[發(fā)明專利]一種輪式雙足機(jī)器人姿態(tài)控制方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110811070.5
• 申請(qǐng)日: 2021-07-19
• 公開(kāi)（公告）號(hào): CN113485398A
• 公開(kāi)（公告）日: 2021-10-08
• 發(fā)明（設(shè)計(jì)）人: 黃強(qiáng);韓連強(qiáng);陳學(xué)超;余張國(guó);趙凌萱;邱雪健
• 申請(qǐng)（專利權(quán)）人: 北京理工大學(xué)
• 主分類號(hào): G05D1/08
• 分類號(hào): G05D1/08
• 代理公司: 南京智造力知識(shí)產(chǎn)權(quán)代理有限公司 32382
• 代理人: 汪芬

1.一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，分別構(gòu)造姿態(tài)控制器和平衡控制器；所述姿態(tài)控制器包括姿態(tài)解算和全身動(dòng)力學(xué)補(bǔ)償控制，姿態(tài)解算接收給定上身軀干的參考姿態(tài)矩陣R_ref、軀干相對(duì)輪軸的參考位置p_ref以及狀態(tài)估計(jì)得到的機(jī)器人狀態(tài)；姿態(tài)解算進(jìn)行姿態(tài)解算后經(jīng)全身動(dòng)力學(xué)補(bǔ)償控制單元得到姿態(tài)控制量τ_leg，即機(jī)器人腿部各關(guān)節(jié)驅(qū)動(dòng)力矩，從而控制身體姿態(tài)；

所述平衡控制器接收給定輪式雙足機(jī)器人移動(dòng)的參考速度v_ref、狀態(tài)估計(jì)得到的機(jī)器人質(zhì)心相對(duì)輪軸的位置d_com以及質(zhì)心相對(duì)世界坐標(biāo)系的實(shí)際速度v_com，形成反饋控制率得到平衡調(diào)節(jié)控制量τ_wheel，即輪子的驅(qū)動(dòng)力矩，從而調(diào)整機(jī)器人的運(yùn)動(dòng)平衡。

2.根據(jù)權(quán)利要求1所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，平衡控制器的構(gòu)造方法為：

S1、將當(dāng)前機(jī)器人狀態(tài)下上身簡(jiǎn)化為質(zhì)心剛體，底部為輪子剛體，兩個(gè)剛體之間通過(guò)無(wú)質(zhì)量的結(jié)構(gòu)連接形成輪-倒立質(zhì)心擺模型；

S2、設(shè)平衡控制器的優(yōu)化變量ε，建立控制目標(biāo)函數(shù)及優(yōu)化的約束條件；

S3、在每個(gè)控制周期內(nèi)，根據(jù)機(jī)器人狀態(tài)得到輪-倒立質(zhì)心擺狀態(tài)，經(jīng)過(guò)優(yōu)化計(jì)算最終優(yōu)化量ε中的力矩值，即平衡控制器的調(diào)節(jié)量τ_wheel。

3.根據(jù)權(quán)利要求2所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，輪-倒立質(zhì)心擺模型的動(dòng)力學(xué)方程表示為：

其中，ξ_WIP是輪-倒立質(zhì)心擺模型的狀態(tài)變量，分別是狀態(tài)變量一階微分和二階微分，M(ξ_WIP)是由狀態(tài)變量決定的系統(tǒng)慣量矩陣，是由狀態(tài)變量及其一階微分決定的科氏力、重力項(xiàng)矩陣，B是驅(qū)動(dòng)力矩選擇矩陣，J_wheel是輪子邊緣與地面接觸點(diǎn)相對(duì)世界坐標(biāo)系的雅克比矩陣，(·)^T是矩陣中的轉(zhuǎn)置運(yùn)算，λ_ext是該接觸點(diǎn)的外力。因此整個(gè)系統(tǒng)的輸入為輪子的驅(qū)動(dòng)力矩τ_wheel。

4.根據(jù)權(quán)利要求2所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，設(shè)平衡控制器的優(yōu)化變量為目標(biāo)函數(shù)表示為：

其中，w₁,w₂,w₃均是優(yōu)化權(quán)重系數(shù)，β是距離調(diào)節(jié)增益系數(shù)，J_com是質(zhì)心相對(duì)世界坐標(biāo)系的雅克比矩陣而是其一階微分，是輪子驅(qū)動(dòng)力矩的上一次調(diào)節(jié)量。

5.根據(jù)權(quán)利要求2所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，平衡控制器的約束包括動(dòng)力學(xué)方程約束、輪子驅(qū)動(dòng)能力約束和接觸點(diǎn)的摩擦錐約束。

6.根據(jù)權(quán)利要求5所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，動(dòng)力學(xué)約束為：M(ξ_WIP)是由狀態(tài)變量決定的系統(tǒng)慣量矩陣，是由狀態(tài)變量及其一階微分決定的科氏力、重力項(xiàng)矩陣，B是驅(qū)動(dòng)力矩選擇矩陣，J_wheel是輪子邊緣與地面接觸點(diǎn)相對(duì)世界坐標(biāo)系的雅克比矩陣；

輪子驅(qū)動(dòng)約束：τ_min和τ_max分別是電機(jī)輸出的最小力矩值與最大力矩值；

接觸點(diǎn)摩擦錐約束：其中μ為摩擦因數(shù)。

7.根據(jù)權(quán)利要求1所述的一種輪式雙足機(jī)器人姿態(tài)控制方法，其特征在于，姿態(tài)控制器的構(gòu)造方法為：

S1、設(shè)全身動(dòng)力學(xué)模型的系統(tǒng)狀態(tài)變量χ_WBD，使用拉格朗日動(dòng)力學(xué)構(gòu)建全身動(dòng)力學(xué)方程；

S2、將上身的位置和姿態(tài)參考加速度輸入PD控制器，由PD控制器輸入的六個(gè)量和角加速度參考值組成因此得到全身動(dòng)力學(xué)補(bǔ)償控制器；是系統(tǒng)狀態(tài)變量參考值的二階微分；

S3、為了增加驅(qū)動(dòng)關(guān)節(jié)的姿態(tài)控制精度，在前饋力矩基礎(chǔ)上增加關(guān)節(jié)層的PD控制器得到最終的姿態(tài)控制器控制量τ_leg。