[發明專利]一種步行機器人坡地崎嶇地形的步態生成與姿態控制方法

有效

• 申請號: 202210526150.0
• 申請日: 2022-05-16
• 公開（公告）號: CN114995476B
• 公開（公告）日: 2022-11-29
• 發明（設計）人: 金波;唐煊;王謙
• 申請（專利權）人: 浙江大學
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 杭州天勤知識產權代理有限公司 33224
• 代理人: 沈金龍

1.一種步行機器人坡地崎嶇地形的步態生成與姿態控制方法，其特征在于，所述步行機器人包括機身和至少6條腿，將機身一周的所有腿中相互間隔的腿分為同一組，共可分為兩組，兩組腿各稱為一組主支撐腿，每條腿有支撐相和擺動相兩種狀態；同一組的各條腿均處于支撐相時，該組各條腿的足端所構成的平面為主支撐面，

定義機身坐標系{X_B,Y_B,Z_B}、世界坐標系{X_W,Y_W,Z_W}和主支撐面坐標系{X_S,Y_S,Z_S}，機身坐標系原點O_B位于機身形心位置，過機身坐標系原點作主支撐面的垂線，垂足為主支撐面坐標系原點O_S，Z_S方向為主支撐面法向，X_S的方向與X_W沿重力方向在主支撐面上的投影軸方向一致，將O_B到主支撐面的垂直距離定義為機身高度H，

所述步態生成與姿態控制方法包括以下步驟：

(1)人為給出行走速度和行走方向指令后，將處于支撐相的一組腿稱為第一組腿，另一組腿稱為第二組腿，分析處于支撐相的第一組腿所構成的主支撐面信息，規劃機身坐標系相對于世界坐標系的移動速度指令和姿態角指令E_e，用來區分哪一組腿支撐的標識符LR置0，處于支撐相的第一組腿的關節角度使用支撐相的步態規劃，第二組腿的關節角度使用擺動相的步態規劃；

(2)將E_e經過PID運算后得到的值作為姿態角速度指令將和共同輸入速度逆運動學進行計算，得到關節角速度指令經過積分得到關節角度指令θ_i，i為腿的編號，作為處于支撐相的第一組腿的關節驅動量；將第二組腿的足端初始位置作為擺動軌跡的起始位置，采用六次插值方式規劃擺動腿足端軌跡，經過腿部幾何逆解，得到關節角度指令θ_i，i為腿的編號，作為處于擺動的第二組腿的關節驅動量；

(3)第二組腿在擺動的過程中，陸續觸地，在觸地的瞬間進入支撐相步態規劃，將觸地瞬間的關節角速度作為積分初值，通過速度逆運動學方式得出關節角速度，積分得到關節驅動量，與原先支撐的第一組腿一起，共同支撐機身；

(4)在第二組腿都觸地之后，分析第二組腿所構成的主支撐面信息，重新規劃機身坐標系相對于世界坐標系的移動速度指令和姿態角指令E_e，標識符LR置1，第二組腿繼續支撐，第一組腿開始擺動，規劃方法同步驟(2)；

(5)第一組腿在擺動的過程中，陸續觸地，在觸地的瞬間進入支撐相步態規劃，規劃方法同步驟(3)；

(6)在第一組腿都觸地之后，回到步驟(1)，完成一個步態周期，重復步驟(1)至(5)的過程，步態進行循環，步行機器人在腿的不斷支撐與擺動交替的過程中實現坡地崎嶇地形上的連續行走，

步驟(2)中，在機器人行走過程中姿態角的實時控制時，引入機身上陀螺儀IMU的實時數據作為反饋，與規劃的姿態角軌跡E_e作差，采用PID控制策略，將偏差值作為姿態角速度指令引入機器人速度逆運動學模塊進行計算，

機器人速度逆運動學模塊中的機器人速度逆運動學是指已知機身相對于世界坐標系的運動速度指令，包括三個方向的移動速度和三個方向的轉動速度然后得出支撐足端相對于機身的速度，再通過雅克比矩陣求逆得出關節角速度，積分可得關節角度θ_i，從而計算執行器所需的位移量，驅動執行器，完成支撐相步態，

關節角度指令θ_i的具體求解算法如下：

足端在世界坐標系下的移動速度，

足端在機身坐標系下的移動速度，

機身坐標系相對于世界坐標系的旋轉變換矩陣，

機身坐標系相對于世界坐標系的旋轉速度變換矩陣，

機身坐標系相對于世界坐標系的移動速度指令，

其中，

Rot(z,α)表示繞世界坐標系z軸旋轉α角，Rot(y,β)表示繞世界坐標系y 軸旋轉β角，Rot(x,γ)表示繞世界坐標系x軸旋轉γ角，

α：偏航角，β：俯仰角，γ：橫滾角，s和c分別為三角函數sin和cos的簡寫，

J_i：i號腿的速度雅克比矩陣，i號腿的各個關節角速度，L₀：根關節與髖關節之間的連桿長度，L₁：髖關節與膝關節之間的連桿長度，L₂：膝關節與足端之間的連桿長度，θ_i0：i號腿的根關節旋轉角度，θ_i1：i號腿的髖關節旋轉角度，θ_i2：i號腿的膝關節旋轉角度，

ω_x、ω_y、ω_z分別是機身坐標系相對于世界坐標系的X_W、Y_W、Z_W三軸的旋轉角速度，即為期望姿態角與IMU反饋的實際姿態角之間的偏差信號經過PID運算后得到的姿態角速度指令

θ_i：i號腿計算執行器位移所需的三個關節角度值，θ_{i_initial}：單條腿在由擺動到觸地成為支撐腿的時刻的三個關節角度的初始值，t：該腿在進入支撐相到結束支撐相的整個支撐時長。