刘帅等：四足机器人软硬地面稳定过渡的腿部主动变刚度调节策略 423· 通过在腾空阶段规划落足点位置实现对前向 摆髋关节角度、前摆髋关节角度、膝关节角度和 速度控制.即： 等效摆角，y是第i条腿的虚拟弹簧腿与自身小腿 _还+Kp使-知) (5) 之间的夹角.则由安装在侧摆关节处的角度传 2 感器测量得到，p2和pa可根据数学几何关系推出 式中，为虚拟弹簧腿期望落足点位置，x、分别 以第①条腿为例，如式(6)、(7)所示： 是机身的实际水平速度和期望水平速度，K,为速 L2+rn1()2-L22 度反馈增益，T、是着地时间 12=01(t)-arccos (6) 2L1n(0) 这里以单腿①为例，研究腾空腿关节力矩的 L12+L22-r1(02 作用，单腿示意图见图3所示 p13=π-arccos (7) 2L1L2 Fuselage in9n Hip 2126144* 四条腿的运动状态以足-地接触力作为判定标 :个 Virtual spring 准，引入有限状态机(Finite state machine,FSM)对各 lee 状态进行监控和条件转移.基于位置控制的思想， r (t) 利用多项式函数对腿部的目标摆动角进行规划，通 La即 过逆运动学求解得到对应的各个期望关节角 Calf Foot (Desired joint angle,pp,此数据与实时的关节角 (Actual joint angle,pa)进行比较，得到的误差输入比 例微分(Proportion differentiation,PD)控制器并进行 图3单腿示意图 及时处理，换算得到的力矩值作为电机参数，驱动机 Fig.3 Diagram of one leg 器人关节运动.使其中一对对角腿（这里以腿①、 图3中，l、t2、tB分别是第i条腿(i取1、2、 ④为例)以一定的角度触地.落足点远离或接近中 3、4中的一个值)的侧摆髋关节、前摆髋关节和膝 性点，机器人得到的净向前加速度，使整体加速或者 关节的驱动力矩；L1、L2、n()分别为大腿(Thigh)、 减速，以便达到期望的水平速度.同时，另外一对对 小腿(Calf)和单腿①的虚拟弹簧腿(Virtual spring 角腿②、③摆动到合适的位置，为下一周期的着地 leg)的长度；9、2、p3、()分别是第i条腿的侧 状态做好准备，依次循环往复.腾空相流程见图4. FSM PA Joint torque Decoupled control- Inverse kinematics PD controller Robot Sensors Velocities,forces,etc 图4四足机器人腾空相控制 Fig.4 Control of the quadruped robot in flight phase 3四足机器人着地相控制 存在于对角着地的情况，还有外部难以避免的额 外干扰、机器人自身结构的非对称性又或是腾 3.1跳跃高度控制 空相摆动腿所产生的反作用力无法得到及时补 机器人在着地过程中会进行跳跃高度与身体 偿等.机体姿态偏移角的合理控制是实现机器人 姿态的控制.足端与地面的非弹性碰撞以及各关 持续稳定运动的前提条件.在腾空相，机器人的 节运动阻尼等因素均会导致系统能量的损失.为 角动量守恒，为了解决在对角步态下产生的翻转 了保证四足机器人持续稳定运动，需要及时对其 进行能量补充.在忽略空气阻力影响的前提下，当 问题，应选择在着地时采用一定的手段对姿态偏 机身下落到最低点时利用势能与动能之间的能量 差进行纠正.通常的解决办法是在足端着地时利 用腿部的侧摆髋关节和前摆髋关节对机身施加相 关系，计算得到相应的系统能量补偿值，这部分能 量是通过对虚拟弹簧腿刚度的修正实现 对应的反作用力用以纠正姿态角偏差，即常规姿 态反馈控制(Conventional attitude feedback control,. 3.2姿态控制 cAFC): 3.2.1常规姿态反馈控制 影响四足机器人机身姿态偏差的因素不仅仅 Tpitch=-Kp_pitch (Opitch-Opitch desire-Kv pitchopitch (8)通过在腾空阶段规划落足点位置实现对前向 速度控制. 即： xf,d = xT˙ s 2 +Kp (x˙ − x˙d) （5） 式中，xf,d 为虚拟弹簧腿期望落足点位置，x˙、x˙d 分别 是机身的实际水平速度和期望水平速度， Kp 为速 度反馈增益，Ts 是着地时间. 这里以单腿①为例，研究腾空腿关节力矩的 作用，单腿示意图见图 3 所示. θpitch θ1 (t) r1 (t) Fuselage cleg1 kleg1 Fleg1 L1 L2 Lap Thigh Hip Virtual spring leg Calf Foot φ13 φ12 τ13 γ1 τ11, φ11 τ12 图 3    单腿示意图 Fig.3    Diagram of one leg τi1 τi2 τi3 L1 L2 r1 (t) φi1 φi2 φi3 θi(t) 图 3 中 ， 、 、 分别是第 i 条腿 (i 取 1、2、 3、4 中的一个值) 的侧摆髋关节、前摆髋关节和膝 关节的驱动力矩； 、 、 分别为大腿 (Thigh)、 小腿 (Calf) 和单腿①的虚拟弹簧腿 (Virtual spring leg) 的长度； 、 、 、 分别是第 i 条腿的侧 γi φi1 φi2 φi3 摆髋关节角度、前摆髋关节角度、膝关节角度和 等效摆角， 是第 i 条腿的虚拟弹簧腿与自身小腿 之间的夹角. 则由安装在侧摆关节处的角度传 感器测量得到， 和 可根据数学几何关系推出. 以第①条腿为例，如式 (6)、(7) 所示： φ12 = θ1 (t)−arccos( L1 2 +r1(t) 2 − L2 2 2L1r1 (t) ) −θpitch （6） φ13 = π −arccos( L1 2 + L2 2 −r1(t) 2 2L1L2 ) （7） 四条腿的运动状态以足−地接触力作为判定标 准，引入有限状态机 (Finite state machine，FSM) 对各 状态进行监控和条件转移. 基于位置控制的思想， 利用多项式函数对腿部的目标摆动角进行规划，通 过 逆 运 动 学 求 解 得 到 对 应 的 各 个 期 望 关 节 角 (Desired  joint  angle, φD)，此数据与实时的关节角 (Actual joint angle，φA) 进行比较，得到的误差输入比 例微分 (Proportion differentiation，PD) 控制器并进行 及时处理，换算得到的力矩值作为电机参数，驱动机 器人关节运动. 使其中一对对角腿 (这里以腿①、 ④为例) 以一定的角度触地. 落足点远离或接近中 性点，机器人得到的净向前加速度，使整体加速或者 减速，以便达到期望的水平速度. 同时，另外一对对 角腿②、③摆动到合适的位置，为下一周期的着地 状态做好准备，依次循环往复. 腾空相流程见图 4. FSM Decoupled control Inverse kinematics PD controller Robot Joint torque Velocities, forces, etc Sensors xf,d xd · x · + − + φD − φA 图 4    四足机器人腾空相控制 Fig.4    Control of the quadruped robot in flight phase 3    四足机器人着地相控制 3.1    跳跃高度控制 机器人在着地过程中会进行跳跃高度与身体 姿态的控制. 足端与地面的非弹性碰撞以及各关 节运动阻尼等因素均会导致系统能量的损失. 为 了保证四足机器人持续稳定运动，需要及时对其 进行能量补充. 在忽略空气阻力影响的前提下，当 机身下落到最低点时利用势能与动能之间的能量 关系，计算得到相应的系统能量补偿值，这部分能 量是通过对虚拟弹簧腿刚度的修正实现. 3.2    姿态控制 3.2.1    常规姿态反馈控制 影响四足机器人机身姿态偏差的因素不仅仅 存在于对角着地的情况，还有外部难以避免的额 外干扰、机器人自身结构的非对称性又或是腾 空相摆动腿所产生的反作用力无法得到及时补 偿等. 机体姿态偏移角的合理控制是实现机器人 持续稳定运动的前提条件. 在腾空相，机器人的 角动量守恒，为了解决在对角步态下产生的翻转 问题，应选择在着地时采用一定的手段对姿态偏 差进行纠正. 通常的解决办法是在足端着地时利 用腿部的侧摆髋关节和前摆髋关节对机身施加相 对应的反作用力用以纠正姿态角偏差，即常规姿 态反馈控制 (Conventional attitude feedback control， cAFC)： τpitch = −Kp_pitch ( θpitch −θpitch_desire) −Kv_pitchθ˙ pitch（8） 刘    帅等： 四足机器人软硬地面稳定过渡的腿部主动变刚度调节策略 · 423 ·