人形机器人与全身控制

1. 人形机器人运动学与浮动基座动力学

浮动基座动力学方程

与固定基座机械臂不同，基座6自由度是"浮动的"。接触力$\mathbf{F}_c$由地面反力提供，必须满足摩擦锥约束。

2. ZMP零力矩点与双足步行控制

ZMP稳定性判据

ZMP是地面上重力和惯性力的合力矩水平分量为零的点。若ZMP在脚掌支撑多边形内，机器人不翻倒——就像你判断支架是否倾覆。

def compute_zmp(com, com_acc, gravity=-9.81):
    """线性倒立摆简化ZMP计算"""
    z = com[2]
    x_zmp = com[0] - (z / gravity) * com_acc[0]
    y_zmp = com[1] - (z / gravity) * com_acc[1]
    return np.array([x_zmp, y_zmp])

def is_stable(zmp, support_polygon):
    from shapely.geometry import Point, Polygon
    return Polygon(support_polygon).contains(Point(zmp))

3. 双足步行控制——从ZMP到全身运动

步行周期

双足步行是一个周期性离散事件系统：单支撑相(SSP)→双支撑相(DSP)→单支撑相→...。每个相位的动力学约束不同，就像四冲程发动机在不同冲程的控制策略不同。

ZMP轨迹规划

def generate_zmp_trajectory(footsteps, step_duration, com_height):
    """根据足部落点生成ZMP参考轨迹"""
    zmp_x, zmp_y = [], []
    for i, step in enumerate(footsteps):
        t = i * step_duration
        zmp_x.extend([step[0]] * int(step_duration / dt))
        zmp_y.extend([step[1]] * int(step_duration / dt))
    return np.array(zmp_x), np.array(zmp_y)

# 由ZMP反推CoM轨迹（预览控制）
from scipy.linalg import solve_toeplitz
com_x = preview_control(zmp_x, com_height, dt, preview_horizon=50)

4. 全身控制(WBC)——同时满足多个任务的优化框架

WBC的核心思想

人形机器人有30+个自由度，但要同时满足多个任务：保持平衡（质心不动）、手臂到达目标位置、避开关节限位、最小化能量消耗...WBC将这些任务组织为带优先级的多目标优化问题。

WBC的数学框架

如果无法同时满足所有任务（比如平衡 + 够物超出工作空间），WBC按优先级逐级放松：

• 最高优先级：物理约束（动力学方程、摩擦锥、关节限位）
• 高优先级：安全约束（质心在支撑多边形内、ZMP稳定）
• 中优先级：任务目标（手到达目标、身体朝向）
• 低优先级：姿态优化（最小化关节力矩、最小化能量）

class WholeBodyController:
    """带任务优先级的分层QP求解器"""
    
    def __init__(self, robot_model):
        self.model = robot_model
        self.n_dof = robot_model.n_dof
        self.n_contacts = 0
        
    def solve(self, q, dq, tasks, contact_points):
        """求解带优先级的QP: x = [ddq, F_c, tau]"""
        n = self.n_dof + 3 * self.n_contacts + self.n_dof
        
        # 第一层：物理约束（硬约束）
        M = self.model.mass_matrix(q)
        h = self.model.bias_forces(q, dq)
        J_c = self.model.contact_jacobian(q, contact_points)
        S = np.eye(self.n_dof)
        
        # 摩擦锥约束: |F_tan| <= mu * F_norm, F_norm >= 0
        mu = 0.5
        cone_constraints = []
        for i in range(self.n_contacts):
            # 线性化4面摩擦锥
            C_i = self._linear_friction_cone(mu)
            idx = self.n_dof + 3 * i
            # C_i * F_c[i] <= 0
            cone_constraints.append((C_i, idx))
        
        # 第二层：平衡任务
        # 第三层：任务目标（分层QP）
        for priority, task_list in tasks.items():
            for task in task_list:
                J_task = self.model.task_jacobian(q, task.frame)
                b = task.desired_accel - self.model.jacobian_derivative(q, dq, task.frame)
                
                if priority == 'hard':
                    A_eq = J_task; b_eq = b  # 硬约束：必须满足
                else:
                    H += task.weight * J_task.T @ J_task  # 软约束：最小化误差
                    g += -task.weight * J_task.T @ b
        
        # 调用QP求解器
        from qpsolvers import solve_qp
        x_opt = solve_qp(H, g, C, l, u, A_eq, b_eq, solver='osqp')
        
        ddq = x_opt[:self.n_dof]
        F_c = x_opt[self.n_dof:self.n_dof + 3*self.n_contacts].reshape(-1, 3)
        tau = x_opt[-self.n_dof:]
        return ddq, F_c, tau

5. 质心动量动力学——从30自由度到6自由度的降维

为什么需要降维

30+自由度的全身动力学计算量巨大(>1ms)。但对平衡控制而言，最关键的状态就是质心(CoM)位置/速度和角动量——6个自由度的信息。质心动量动力学将全身动力学的质心部分抽取出来。

质心动力学方程

$c$是质心位置，$L$是绕质心的角动量，$f_i$是第$i$个接触点的力。这个方程的美妙之处：你不需要知道所有30个关节在做什么——只要知道脚底力的合力和合力矩，就能判断机器人是否稳定。

线性倒立摆模型(LIPM)——双足步行最基本的分析工具

class LinearInvertedPendulum:
    """线性倒立摆——双足步行稳定性分析的基本工具"""
    
    def __init__(self, com_height=0.9, g=9.81):
        self.z = com_height  # 恒定质心高度（关键假设）
        self.g = g
        self.omega = np.sqrt(g / com_height)  # 自然频率
    
    def dynamics(self, x, zmp):
        """LIPM动力学: x_ddot = omega^2 * (x - zmp)"""
        x_ddot = self.omega**2 * (x - zmp)
        return x_ddot
    
    def compute_zmp(self, com, com_acc):
        """LIPM下的ZMP计算: zmp = com - (z/g)*com_acc"""
        return com - (self.z / self.g) * com_acc
    
    def analytical_solution(self, x0, v0, zmp, t):
        """LIPM的解析解（双曲函数形式）"""
        omega = self.omega
        # x(t) = zmp + (x0-zmp)*cosh(omega*t) + (v0/omega)*sinh(omega*t)
        x_t = zmp + (x0 - zmp) * np.cosh(omega * t) + (v0 / omega) * np.sinh(omega * t)
        return x_t

预览控制——由ZMP轨迹反推CoM轨迹

你设计出了脚步序列→ZMP参考轨迹（左脚支撑→右脚支撑→...）。但机器人执行的是CoM轨迹。预览控制解决的是：给定未来N步的ZMP目标，当前CoM应该怎么动？

def preview_control(zmp_ref, com_height, dt, preview_horizon=50):
    """预览控制：ZMP轨迹 → CoM轨迹 + 质心加速度"""
    g = 9.81
    omega = np.sqrt(g / com_height)
    
    # 离散化LIPM: x[k+1] = A x[k] + B zmp[k]
    # 位置/速度/加速度 三重状态
    A = np.array([
        [1, dt, dt**2/2],
        [0, 1, dt],
        [0, 0, 1]
    ])
    B = np.array([[dt**3/6], [dt**2/2], [dt]])
    
    # 最优增益 - 通过求解Riccati方程得到
    Qe = 1.0  # ZMP跟踪误差权重
    R = 1e-6  # 控制输入惩罚
    
    # 求解稳态Riccati方程 P = A'PA - A'PB(R+B'PB)^(-1)B'PA + Q
    P = scipy.linalg.solve_discrete_are(A, B, 
        np.diag([1, 0, 0]), np.array([[R]]))
    
    # 最优增益
    K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
    
    # 前向计算：用ZMP参考和预览信息递推CoM
    x = np.array([0.0, 0.0, 0.0])  # [com_pos, com_vel, com_acc]
    com_traj = [x[0]]
    
    for k in range(len(zmp_ref)):
        # 预览控制: u[k] = -K*x[k] + sum(f[j]*zmp_ref[k+j])
        u = -K @ x
        for j in range(min(preview_horizon, len(zmp_ref) - k)):
            u += preview_gain[j] * zmp_ref[k + j]
        
        x = A @ x + B.flatten() * u
        com_traj.append(x[0])
    
    return np.array(com_traj)

6. 脚步规划——从A到B的落脚点优化

脚步规划的三个层次

class FootstepPlanner:
    """基于A*的脚步规划器"""
    
    def __init__(self, step_length=0.3, step_width=0.15, max_turn=0.5):
        self.step_length = step_length
        self.step_width = step_width
        self.max_turn = max_turn  # 最大偏转角 (rad)
    
    def plan(self, start_pose, goal_pose, terrain_map):
        """A*搜索脚步序列"""
        import heapq
        
        start_state = FootstepState(start_pose, support='left')
        goal_state = None
        
        open_set = [(self._heuristic(start_pose, goal_pose), start_state)]
        closed_set = set()
        
        while open_set:
            _, current = heapq.heappop(open_set)
            
            if self._goal_reached(current.pose, goal_pose):
                return self._reconstruct_path(current)
            
            for next_state in self._expand(current, terrain_map):
                if next_state not in closed_set:
                    cost = current.cost + self._step_cost(current, next_state)
                    heapq.heappush(open_set, (cost + self._heuristic(next_state.pose, goal_pose), next_state))
        
        return None  # 无解
    
    def _expand(self, state, terrain):
        """生成可能的下一步落脚点"""
        successors = []
        new_support = 'right' if state.support == 'left' else 'left'
        
        # 离散化候选脚步：不同步长+偏转角组合
        for sl in np.linspace(0.15, self.step_length, 4):
            for turn in np.linspace(-self.max_turn, self.max_turn, 5):
                for sw in [-self.step_width, 0, self.step_width]:
                    # 计算新的落脚点在世界坐标系下的位姿
                    dx = sl * np.cos(state.pose[2] + turn)
                    dy = sl * np.sin(state.pose[2] + turn) + sw * (-1 if new_support == 'left' else 1)
                    new_pose = (state.pose[0] + dx, state.pose[1] + dy, state.pose[2] + turn)
                    
                    if terrain.is_valid(new_pose):
                        successors.append(FootstepState(new_pose, new_support, state))
        
        return successors

7. 推搡恢复策略——当机器人被推一把

恢复策略的分级响应

class PushRecoveryController:
    """推搡检测与分级恢复"""
    
    def __init__(self, robot_model):
        self.model = robot_model
        self.disturbance_history = []
    
    def detect_push(self, imu_reading, ft_sensors):
        """检测外部推搡——通过IMU加速度突变 + 脚底力异常"""
        # 预期加速度 = 根据当前关节运动计算出的惯性力
        expected_acc = self.model.compute_expected_acceleration()
        actual_acc = imu_reading.linear_acceleration
        
        # 外部力 = 实际加速度 - 预期加速度
        external_acc = actual_acc - expected_acc
        external_force = self.model.total_mass * external_acc
        
        return external_force
    
    def select_strategy(self, external_force, current_zmp, support_polygon):
        """根据扰动大小选择恢复策略"""
        # 计算ZMP到支撑多边形边缘的最小距离
        margin = compute_margin(current_zmp, support_polygon)
        disturbance_magnitude = np.linalg.norm(external_force)
        
        if margin < 0.02:  # ZMP已接近边缘
            if disturbance_magnitude > 50:  # 大力推搡 (N)
                return 'footstep'  # 迈步策略
            return 'hip'  # 髋部策略
        
        if disturbance_magnitude > 20:
            return 'ankle'  # 踝关节策略
        
        return 'normal'  # 正常控制
    
    def ankle_strategy(self, external_force):
        """踝关节策略：用脚踝力矩将ZMP拉回中心"""
        # 需要的地面反力矩 = 外部扰动力矩的相反方向
        ankle_torque = -np.cross(external_force[:3], np.array([0,0,self.model.com_height]))
        return ankle_torque
    
    def hip_strategy(self, external_force):
        """髋部策略：摆动上身产生反向角动量"""
        # 上身加速摆动 → 产生角动量变化 → 抵消扰动
        required_angular_acc = external_force[:3] / self.model.upper_body_inertia
        return required_angular_acc

8. 液压驱动与安全规范——机械工程师的核心领域

液压驱动在人形机器人中的应用

大型人形机器人（如Boston Dynamics Atlas, Tesla Optimus腿部）使用液压系统：

• 功率密度极高：同等重量下液压功率是电机的3-5倍
• 自然柔顺性：油液可压缩→被动冲击吸收→安全的力控制
• 你的优势：流量/压力/管路设计的经验直接适用

液压伺服阀的数学模型

$Q$流量，$C_d$流量系数，$A_v$阀口面积（控制输入），$P_s$供油压力，$P_L$负载压力。这个方程你已经在液压系统设计中见过无数次。

机器人安全标准体系