內力為零的約束條件，計算出唯一的期望接觸力。內環(huán)通過測量實際足 端接觸力和期望接觸力之間的誤差計算關節(jié)驅動力矩。進一步，如果前向關節(jié)個數(shù)大于2， 也加入適當約束條件，以保證關節(jié)驅動力矩的唯一性。
[0067] 由于本體的高度在徑向平面內可控，所以機器人在法向平面內的投影可以簡化為 一個一維線性倒立擺（見圖3)。當步態(tài)周期固定，機器人本體側向期望速度為0的情況下， 根據(jù)一維倒立擺的運動學和動力學模型，每個支撐期結束時刻機器人的前向速度只與此支 撐期開始時刻機器人本體的初始速度、姿態(tài)相關。
[0068] 為此，本發(fā)明通過計算、選擇法向平面內擺動腿落足點的位置，在保證機器人穩(wěn)定 的前提下，調整每個支撐期開始時刻機器人本體的速度、姿態(tài)，進而控制本體在支撐期結束 時刻法向平面內的速度，與徑向平面內機器人本體的速度進行合成，完成對機器人速度的 控制。
[0069] 在上述步驟Sl和步驟S2中，于徑向平面內進行建模與控制時，包括以下過程：
[0070] 四足機器人在徑向平面內的投影可以簡化為一個七連桿結構，如圖2所示（假設 左前腿和右后腿作為支撐腿）。（X，Y)為本體質心在慣性坐標系下的位置，L。為機器人本 體長度，d為本體的寬度，L 1, L2, L3分別為腿部踝關節(jié)、膝關節(jié)、髖關節(jié)的長度，Θ θ2, θ3 為左前腿踝關節(jié)角、膝關節(jié)角、髖關節(jié)角，θ4, θ5, θ6為右后腿踝關節(jié)角、膝關節(jié)角、髖關節(jié) 角，β為本體俯仰角，〇、A、B、C、D分別為本體坐標系原點（即本體質心）、右后腿髖部與本 體連接點、左前腿髖部與本體連接點、右后腿足端點、左前腿足端點，F(xiàn) Ml，F(xiàn)_，F(xiàn)m2, 分別 為C點和D點受到的接觸力。C點和D點的位置坐標為：
[0071 ] Xc= (X_ (L 〇/2) cos β ) _L3cos ( θ 6- β ) +L2Cos ( θ 5- Θ 6+ β ) -L1Cos ( θ 4- Θ 5+ θ 6- β )
[0072] (I)
[0073] Yc= (Y- (L 0/2) sin β ) -L3sin ( θ 6- β ) -L2sin ( θ 5- Θ 6+ β ) -L1Sin ( θ 4- Θ 5+ θ 6- β )
[0074] XD= (X+ (L 〇/2) cos β ) +L3Cos ( θ 3- β ) -L2Cos ( θ 2- θ 3+ β ) +L1Cos ( θ 丄―θ 2+ θ 3- β )
[0075] (2)
[0076] Yd= (Υ+ (L 0/2) sin β ) -L3Sin ( θ 3- β ) -L2Sin ( θ 2- θ 3+ β ) -L1Sin ( θ 丄-θ 2+ θ 3- β )
[0077] 基于拉格朗日方程，七連桿結構的動力學模型為：
[0078]
[0079] (3)
[0080]
[0081] 其中，Cll= [χ，γ，β] T，q2= [XC，YC，0dl，XD，YD，0 d2]T，F(xiàn)e= [Fxel，F(xiàn)yel，F(xiàn)xe2，F(xiàn)ye2] T，τ 為對應于θ p·· Θ 6的關節(jié)驅動力矩，M M2為慣性矩陣，C C2為哥氏力矩陣，N N2為重力 項，J1,1為雅克比矩陣。
[0082] 公式⑶中1為3*4雅克比矩陣，不能直接求取逆矩陣，本發(fā)明出于柔順性的考 慮，加入支撐腿足端點之間內力為〇的約束條件，即：
[0083]
(4)
[0084] 根據(jù)式（3)和（4)，七連桿系統(tǒng)的動力學模型變?yōu)椋?br>[0085]
[0086] (5)
[0087]
[0088] 其中，氣為4*4雅克比矩陣。
[0089] 根據(jù)七連桿系統(tǒng)的動力學模型，本發(fā)明所采用的基于力內環(huán)的雙環(huán)PID控制器：
[0090] 外環(huán)控制器： CN 105137969 A ~P 6/8 頁
[0091]
(6)
[0092] 其中qldS q !的期望軌跡，F(xiàn) ed為支撐腿足端期望接觸力。
[0093] 內環(huán)控制器：
[0094]
(7)
[0095] 在上述步驟Sl和步驟S2中，于法向平面內建模與控制時，包括以下過程：
[0096] 由于機器人本體的高度在徑向平面內可控，所以四足機器人在法向平面內的投影 可以簡化為一個線性倒立擺（如圖3所示）。線性倒立擺的運動模型如下：
[0097]
[0098] (8)
[0099]
[0100] 從公式（8)中可以看出，一個支撐期結束時刻機器人本體的速度在法向平面上的 投影只是和此支撐期開始時刻本體的位置和速度相關。本發(fā)明中，在法向平面內的控制策 略是通過調整擺動腿落足點的位置，從而改變下一個支撐期開始時刻本體的位置和速度， 進而控制支撐期結束時刻本體的速度。在一個步態(tài)周期內的具體計算過程如下（假設當 前時刻為t。，下一個支撐期開始時刻為h，支撐期時間為T，支撐腿足端點連線與機器人前 進方向的夾角為α，當前支撐腿為左前腿和右后腿，tftfT時間內支撐腿為右前腿和左后 腿，ti+T-ti+ST時間內支撐腿為左前腿和右后腿，流程圖如圖4所示，具體解算過程如圖5 所示）：
[0101] 第一步：計算ti+T時刻本體在徑向平面和法向平面內的期望速度

[0102]
[0103] (9)
[0104]
[0105] vd(t)、V(t)為t時刻本體前向期望速度、實際速度，上標〇表示按照落足前 (后）支撐腿足端連線角度分解徑向平面和法向平面。
[0106] 第二步：基于公式⑶，根據(jù)OT)計算?,+ i時刻線性倒立擺初始位置：
[0107]
[0108] 其中，U ) =「）sin ?
[0109] 第三步：計算本體χ方向速度軌跡、徑向平面內速度軌跡、法向平面內速度軌跡：
[0110] (in
[0111]
[0112]
[0113] Vs⑴的控制方法在上述徑向平面內建模與控制過程中已有闡述。
[0114] 第四步：計算€時刻擺動腿操作空間落足位置：
[0115] 右前腿落足點操作空間位置：
[0119] (16)
[0116] p，
[0117] Π 5>[0118] 左后腿落足點操作空間位置：
[0121] 第五步：計算V2T時刻本體在徑向平面和法向平面內的期望速度
[0120] (17)

[0122]
[0123]
[0124]
[0125] 第六步：基于公式（8)，根據(jù)巧(彳+2D計算（#時刻線性倒立擺初始位置：
[0126] (現(xiàn)
[0127]
[0128] 第七步：計算本體X方向速度軌跡、徑向平面內速度軌跡、法向平面內速度軌跡：
[0129]
)
[0132] Vs⑴的控制方法與第三步相同。
[0133] 第八步：計算「+T時刻擺動腿操作空間落足位置：
[0134] 左前腿落足點操作空間位置：
[0135]
[0136] 判
[0138]
[0137] 右后腿落足點操作空間位置：
[0139] (26)
[0140] 以上僅是本發(fā)明的優(yōu)選實施方式，本發(fā)明的保護范圍并不僅局限于上述實施例， 凡屬于本發(fā)明思路下的技術方案均屬于本發(fā)明的保護范圍。應當指出，對于本技術領域的 普通技術人員來說，在不脫離本發(fā)明原理前提下的若干改進和潤飾，應視為本發(fā)明的保護 范圍。
【主權項】
1. 一種基于支撐線運動分解的四足機器人trot步態(tài)本體位姿控制方法，其特征在于， 步驟為： 51 :將機器人分別投影到徑向平面和法向平面上；所述徑向平面為通過支撐腿足端點 連線并且與水平面垂直的平面；所述法向平面為通過本體質心并且垂直于徑向平面的平 面； 52 :將機器人本體的期望速度使用與步驟Sl相同的方法投影到徑向平面和法向平面 上，得到兩個投影平面上的本體期望速度； 53 :分別建立簡化模型的運動學和動力學方程，通過采用控制器使得兩個投影平面上 的本體速度跟蹤各自的期望軌跡，再進行速度合成，完成對機器人本體的速度和姿態(tài)的控 制。2. 根據(jù)權利要求1所述的基于支撐線運動分解的四足機器人trot步態(tài)本體位姿控制 方法，其特征在于，所述步驟Sl和S2中，在所述徑向平面上的投影簡化為平面七連桿模型； 通過調整關節(jié)驅動力矩使得平面七連桿的質心位置、姿態(tài)跟蹤期望軌跡。3. 根據(jù)權利要求2所述的基于支撐線運動分解的四足機器人trot步態(tài)本體位姿控制 方法，其特征在于，所述步驟Sl和S2中，在所述法向平面上的投影簡化為一維線形倒立擺 模型。4. 根據(jù)權利要求1或2或3所述的基于支撐線運動分解的四足機器人trot步態(tài)本體 位姿控制方法，其特征在于，所述步驟S3中，通過計算、選擇法向平面內擺動腿落足點的位 置，調整每個支撐期開始時刻機器人本體的速度、姿態(tài)，控制本體在支撐期結束時刻法向平 面內的速度，與徑向平面內機器人本體的速度進行合成，完成對機器人速度的控制。5. 根據(jù)權利要求1或2或3所述的基于支撐線運動分解的四足機器人trot步態(tài)本體 位姿控制方法，其特征在于，采用基于足端接觸力控制的雙環(huán)PID控制器，外環(huán)利用本體質 心和姿態(tài)誤差計算期望的足端接觸力，且加入支撐腿足端之間內力為零的約束條件，計算 出唯一的期望接觸力；內環(huán)通過測量實際足端接觸力和期望接觸力之間的誤差計算關節(jié)驅 動力矩。
【專利摘要】一種基于支撐線運動分解的四足機器人trot步態(tài)本體位姿控制方法，其步驟為：S1：將機器人分別投影到徑向平面和法向平面上；所述徑向平面為通過支撐腿足端點連線并且與水平面垂直的平面；所述法向平面為通過本體質心并且垂直于徑向平面的平面；S2：將機器人本體的期望速度使用與步驟S1相同的方法投影到徑向平面和法向平面上，得到兩個投影平面上的本體期望速度；S3：分別建立簡化模型的運動學和動力學方程，通過采用控制器使得兩個投影平面上的本體速度跟蹤各自的期望軌跡，再進行速度合成，完成對機器人本體的速度和姿態(tài)的控制。本發(fā)明具有通用性強、控制效果好、控制精度高等優(yōu)點。
【IPC分類】G05D1/02
【公開號】CN105137969
【申請?zhí)枴緾N201510465226
【發(fā)明人】郎琳, 韋慶, 安宏雷, 馬宏緒, 王劍, 王建文, 饒錦輝, 張獻鵬, 許可, 許佳奇
【申請人】中國人民解放軍國防科學技術大學
【公開日】2015年12月9日
【申請日】2015年7月31日