[發(fā)明專利]一種實(shí)時(shí)交互式四足機(jī)器人多步態(tài)動(dòng)力學(xué)建模方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201710325053.4
• 申請(qǐng)日: 2017-05-10
• 公開(kāi)（公告）號(hào): CN107256284B
• 公開(kāi)（公告）日: 2022-06-21
• 發(fā)明（設(shè)計(jì)）人: 許鵬;蘇波;江磊;姚其昌;黨睿娜;許威;蔣云峰;慕林棟;降晨星;鄧秦丹;康祖銘;楊超寧
• 申請(qǐng)（專利權(quán)）人: 中國(guó)北方車輛研究所
• 主分類號(hào): G06F30/17
• 分類號(hào): G06F30/17
• 代理公司: 中國(guó)兵器工業(yè)集團(tuán)公司專利中心 11011
• 代理人: 周恒

1.一種實(shí)時(shí)交互式四足機(jī)器人多步態(tài)動(dòng)力學(xué)建模方法，其特征在于：

構(gòu)建四足機(jī)器人的多步態(tài)動(dòng)力學(xué)系統(tǒng)、四足機(jī)器人和地形的圖形可視化模型；

所述多步態(tài)動(dòng)力學(xué)系統(tǒng)與所述四足機(jī)器人和地形的圖形可視化模型之間采用多線程通信進(jìn)行信息傳輸；

所述構(gòu)建四足機(jī)器人的多步態(tài)動(dòng)力學(xué)系統(tǒng)，包括：

構(gòu)建四足機(jī)器人單腿模型；

基于Vortex搭建四足機(jī)器人的動(dòng)力學(xué)模型及地形動(dòng)力學(xué)模型；

建立關(guān)節(jié)控制器，執(zhí)行多步態(tài)控制模型產(chǎn)生的期望動(dòng)力學(xué)行為；

所述建立關(guān)節(jié)控制器，執(zhí)行多步態(tài)控制模型產(chǎn)生的期望動(dòng)力學(xué)行為，包括：

采用計(jì)算機(jī)時(shí)鐘頻率保證動(dòng)力學(xué)解算的實(shí)時(shí)性，用于精確定時(shí)，并采用多線程技術(shù)進(jìn)行通信；

在動(dòng)力學(xué)系統(tǒng)采用基于c++面向?qū)ο蠓绞降亩嗖綉B(tài)控制算法；

構(gòu)建關(guān)節(jié)控制器，執(zhí)行動(dòng)力學(xué)系統(tǒng)產(chǎn)生的機(jī)器人動(dòng)作；

所述構(gòu)建關(guān)節(jié)控制器為：

其中：關(guān)節(jié)控制器表示根據(jù)關(guān)節(jié)的位置和前饋力信息構(gòu)建關(guān)節(jié)的驅(qū)動(dòng)信號(hào)，θ_d為由單腿逆運(yùn)動(dòng)學(xué)模型計(jì)算期望的關(guān)節(jié)角度，θ為由動(dòng)力學(xué)模型實(shí)時(shí)采集關(guān)節(jié)的位置信號(hào)，利用Vortex動(dòng)力學(xué)引擎實(shí)時(shí)迭代解算獲得實(shí)際關(guān)節(jié)角度；

根據(jù)動(dòng)力學(xué)系統(tǒng)采樣頻率，和分別表示實(shí)時(shí)計(jì)算關(guān)節(jié)的期望速度與實(shí)際速度，k_p為關(guān)節(jié)控制器比例因子；k_v為關(guān)節(jié)控制器微分因子，將關(guān)節(jié)解耦進(jìn)行獨(dú)立控制，根據(jù)腿部各個(gè)關(guān)節(jié)的動(dòng)力學(xué)模型，以及關(guān)節(jié)的期望位置與實(shí)際位置，構(gòu)建關(guān)節(jié)PD控制器；τ_ff為期望足端力F_d向關(guān)節(jié)的映射前饋力矩，表達(dá)式為τ_ff＝-J^TF_d J為關(guān)節(jié)力雅可比矩陣，u為控制器輸入；

所述關(guān)節(jié)PD控制器為：

式中：μ_pd表示關(guān)節(jié)PD控制器輸入。

2.如權(quán)利要求1所述的多步態(tài)動(dòng)力學(xué)建模方法，其特征在于：所述構(gòu)建四足機(jī)器人單腿模型包括：

構(gòu)建單腿運(yùn)動(dòng)學(xué)模型，包括腿部正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)；

根據(jù)單腿運(yùn)動(dòng)學(xué)模型，計(jì)算腿部關(guān)節(jié)力雅可比矩陣J；

采用拉格朗日法構(gòu)建無(wú)外力作用下的關(guān)節(jié)動(dòng)力學(xué)模型；

將足端力向關(guān)節(jié)映射的力矩融入關(guān)節(jié)動(dòng)力學(xué)模型；

所述腿部正運(yùn)動(dòng)學(xué)公式為：

腿部逆運(yùn)動(dòng)學(xué)公式為：

θ₁＝atan2(p_y,p_x)

腿部正運(yùn)動(dòng)學(xué)是獲得足部坐標(biāo)系相對(duì)于髖關(guān)節(jié)坐標(biāo)系的齊次變換矩陣T，足部坐標(biāo)系指關(guān)節(jié)旋轉(zhuǎn)軸z4所在的坐標(biāo)系，髖關(guān)節(jié)坐標(biāo)系指的關(guān)節(jié)旋轉(zhuǎn)軸z1所在的坐標(biāo)系；

腿部逆運(yùn)動(dòng)學(xué)是由足部相對(duì)髖關(guān)節(jié)坐標(biāo)系的位置p＝(p_x,p_y,p_z)求解關(guān)節(jié)角度；

式中：a_i表示腿部關(guān)節(jié)連桿長(zhǎng)度，s_i＝sinθ_i，c_i＝cosθ_i，s_ij＝sin(θ_i+θ_j)，c_ij＝cos(θ_i+θ_j),θ_i表示第i關(guān)節(jié)的關(guān)節(jié)角度，i表示關(guān)節(jié)的序號(hào)，i＝1、2、3；k_fh為前后腿關(guān)于角度符號(hào)的配置因子，前腿對(duì)應(yīng)的k_fh＝1，后腿對(duì)應(yīng)的k_fh＝-1；a₁表示腿部關(guān)節(jié)自上而下連桿1的長(zhǎng)度,a₂表示腿部關(guān)節(jié)連桿2的長(zhǎng)度,a₃表示腿部關(guān)節(jié)連桿3的長(zhǎng)度；c₁＝cosθ₁,s₁＝sinθ₁,c₂₃＝cos(θ₂+θ₃),s₂₃＝sin(θ₂+θ₃)；θ₁、θ₂、θ₃分別表示第1、2、3關(guān)節(jié)的關(guān)節(jié)角度；p_x,p_y,p_z分別表示足部相對(duì)髖關(guān)節(jié)坐標(biāo)系x、y、z軸對(duì)應(yīng)的位置；

所述關(guān)節(jié)動(dòng)力學(xué)模型為：

足端力向關(guān)節(jié)映射的力矩融入關(guān)節(jié)動(dòng)力學(xué)模型為：

τ_f＝J^TF

其中：q為關(guān)節(jié)角度，M為關(guān)節(jié)質(zhì)量矩陣，C為哥氏力矩陣，G為重力矩陣，τ為關(guān)節(jié)驅(qū)動(dòng)力矩，F(xiàn)為足端力，τ_f為足端力向關(guān)節(jié)映射的力矩；分別表示關(guān)節(jié)角加速度、關(guān)節(jié)角速度，J為雅克比矩陣。