·52 分钟
具身智能运动控制：让机器人灵活运动
具身智能运动控制强化学习路径规划机器人学AI
运动控制概述
运动控制是具身智能的核心能力之一，决定了智能体如何执行物理动作。关键挑战包括：
- 高维控制：多关节协调
- 非线性动力学：复杂物理约束
- 实时性：毫秒级决策
- 安全性：避免碰撞和伤害
运动学基础
1. 正运动学
python
class ForwardKinematics:
def __init__(self, robot_model):
self.robot = robot_model
self.dh_params = robot_model.dh_parameters
def compute(self, joint_angles):
"""计算正运动学"""
T = np.eye(4) # 齐次变换矩阵
for i, (a, alpha, d, theta) in enumerate(self.dh_params):
# DH 参数变换
theta_i = joint_angles[i] + theta
# 单关节变换矩阵
Ti = np.array([
[np.cos(theta_i), -np.sin(theta_i)*np.cos(alpha),
np.sin(theta_i)*np.sin(alpha), a*np.cos(theta_i)],
[np.sin(theta_i), np.cos(theta_i)*np.cos(alpha),
-np.cos(theta_i)*np.sin(alpha), a*np.sin(theta_i)],
[0, np.sin(alpha), np.cos(alpha), d],
[0, 0, 0, 1]
])
T = T @ Ti
return T
def get_end_effector_pose(self, joint_angles):
"""获取末端执行器位姿"""
T = self.compute(joint_angles)
position = T[:3, 3]
rotation = Rotation.from_matrix(T[:3, :3])
return {
'position': position,
'orientation': rotation.as_euler('xyz')
}
2. 逆运动学
python
class InverseKinematics:
def __init__(self, robot_model):
self.robot = robot_model
self.fk = ForwardKinematics(robot_model)
def solve_numerical(self, target_pose, initial_guess, max_iter=100, tol=1e-6):
"""数值法求解逆运动学"""
q = initial_guess.copy()
for _ in range(max_iter):
# 当前位姿
current_pose = self.fk.get_end_effector_pose(q)
# 位姿误差
pos_error = target_pose['position'] - current_pose['position']
orient_error = self.compute_orientation_error(
target_pose['orientation'],
current_pose['orientation']
)
error = np.concatenate([pos_error, orient_error])
# 检查收敛
if np.linalg.norm(error) < tol:
return q, True
# 雅可比矩阵
J = self.compute_jacobian(q)
# 伪逆法更新
dq = np.linalg.pinv(J) @ error
q = q + dq
return q, False
def solve_analytical(self, target_pose):
"""解析法求解逆运动学（特定构型）"""
# 以 6-DOF 机器人为例
p = target_pose['position']
R = Rotation.from_euler('xyz', target_pose['orientation']).as_matrix()
# 计算腕部位置
d6 = self.robot.d6
pw = p - d6 * R[:, 2]
# 求解前三个关节（位置）
q1 = np.arctan2(pw[1], pw[0])
# 使用几何法求解 q2, q3
r = np.sqrt(pw[0]**2 + pw[1]**2)
s = pw[2] - self.robot.d1
D = (r**2 + s**2 - self.robot.a2**2 - self.robot.a3**2) / (2 * self.robot.a2 * self.robot.a3)
q3 = np.arctan2(np.sqrt(1 - D**2), D)
q2 = np.arctan2(s, r) - np.arctan2(
self.robot.a3 * np.sin(q3),
self.robot.a2 + self.robot.a3 * np.cos(q3)
)
# 求解后三个关节（姿态）
R03 = self.fk.compute_rotation([q1, q2, q3, 0, 0, 0])
R36 = R03.T @ R
q4 = np.arctan2(R36[1, 2], R36[0, 2])
q5 = np.arccos(R36[2, 2])
q6 = np.arctan2(R36[2, 1], R36[2, 0])
return np.array([q1, q2, q3, q4, q5, q6])
def compute_jacobian(self, q):
"""计算雅可比矩阵"""
n = len(q)
J = np.zeros((6, n))
T = np.eye(4)
z = np.array([0, 0, 1])
p_n = self.fk.compute(q)[:3, 3]
for i in range(n):
# 关节轴
z_i = T[:3, 2]
p_i = T[:3, 3]
if self.robot.joint_types[i] == 'revolute':
# 旋转关节
J[:3, i] = np.cross(z_i, p_n - p_i)
J[3:, i] = z_i
else:
# 移动关节
J[:3, i] = z_i
J[3:, i] = np.zeros(3)
# 更新变换矩阵
Ti = self.fk.compute_single_joint(q[i], i)
T = T @ Ti
return J
动力学控制
1. 动力学模型
python
class DynamicsModel:
def __init__(self, robot_model):
self.robot = robot_model
self.num_joints = robot_model.num_joints
def compute_dynamics(self, q, qd, qdd):
"""计算动力学：M(q)qdd + C(q,qd)qd + G(q) = tau"""
# 惯性矩阵 M(q)
M = self.compute_inertia_matrix(q)
# 科氏力和离心力 C(q,qd)qd
C = self.compute_coriolis(q, qd)
# 重力项 G(q)
G = self.compute_gravity(q)
# 计算力矩
tau = M @ qdd + C @ qd + G
return tau
def compute_inertia_matrix(self, q):
"""计算惯性矩阵"""
M = np.zeros((self.num_joints, self.num_joints))
for i in range(self.num_joints):
for j in range(self.num_joints):
# 使用牛顿-欧拉递推
M[i, j] = self.inertia_element(q, i, j)
return M
def compute_coriolis(self, q, qd):
"""计算科氏力和离心力"""
C = np.zeros(self.num_joints)
for i in range(self.num_joints):
for j in range(self.num_joints):
for k in range(self.num_joints):
# Christoffel 符号
c_ijk = self.christoffel(q, i, j, k)
C[i] += c_ijk * qd[j] * qd[k]
return C
def compute_gravity(self, q):
"""计算重力项"""
G = np.zeros(self.num_joints)
for i in range(self.num_joints):
G[i] = self.gravity_term(q, i)
return G
2. 计算力矩控制
python
class ComputedTorqueController:
def __init__(self, robot_model, dynamics_model):
self.robot = robot_model
self.dynamics = dynamics_model
# PID 增益
self.Kp = np.diag([100, 100, 100, 50, 50, 50])
self.Kd = np.diag([20, 20, 20, 10, 10, 10])
self.Ki = np.diag([1, 1, 1, 0.5, 0.5, 0.5])
self.integral_error = np.zeros(6)
def compute(self, q_desired, qd_desired, qdd_desired,
q_actual, qd_actual, dt):
"""计算控制力矩"""
# 跟踪误差
e = q_desired - q_actual
ed = qd_desired - qd_actual
# 积分项
self.integral_error += e * dt
# 期望加速度（PD + 前馈）
qdd_command = (
qdd_desired +
self.Kd @ ed +
self.Kp @ e +
self.Ki @ self.integral_error
)
# 计算动力学补偿
M = self.dynamics.compute_inertia_matrix(q_actual)
C = self.dynamics.compute_coriolis(q_actual, qd_actual)
G = self.dynamics.compute_gravity(q_actual)
# 计算力矩
tau = M @ qdd_command + C @ qd_actual + G
return tau
强化学习控制
1. 策略网络
python
class RLController:
def __init__(self, state_dim, action_dim):
self.policy = PolicyNetwork(state_dim, action_dim)
self.value = ValueNetwork(state_dim)
self.optimizer = torch.optim.Adam(self.policy.parameters())
def select_action(self, state):
"""选择动作"""
state_tensor = torch.FloatTensor(state)
# 策略前向传播
action_mean, action_std = self.policy(state_tensor)
# 采样动作
dist = torch.distributions.Normal(action_mean, action_std)
action = dist.sample()
log_prob = dist.log_prob(action)
return action.detach().numpy(), log_prob
def update(self, trajectories):
"""更新策略"""
states, actions, rewards, next_states, dones = trajectories
# 计算优势函数
values = self.value(states)
next_values = self.value(next_states)
advantages = self.compute_advantages(rewards, values, next_values, dones)
# 策略梯度
action_means, action_stds = self.policy(states)
dist = torch.distributions.Normal(action_means, action_stds)
log_probs = dist.log_prob(actions)
# PPO 损失
ratio = torch.exp(log_probs - self.old_log_probs)
surr1 = ratio * advantages
surr2 = torch.clamp(ratio, 1 - 0.2, 1 + 0.2) * advantages
policy_loss = -torch.min(surr1, surr2).mean()
# 价值损失
value_loss = F.mse_loss(values, rewards + 0.99 * next_values * (1 - dones))
# 更新
loss = policy_loss + 0.5 * value_loss
self.optimizer.zero_grad()
loss.backward()
self.optimizer.step()
2. 奖励设计
python
class RewardDesigner:
def __init__(self, task_type):
self.task_type = task_type
def compute_reward(self, state, action, next_state, goal):
"""计算奖励"""
if self.task_type == 'reaching':
return self.reaching_reward(state, action, next_state, goal)
elif self.task_type == 'manipulation':
return self.manipulation_reward(state, action, next_state, goal)
elif self.task_type == 'locomotion':
return self.locomotion_reward(state, action, next_state)
def reaching_reward(self, state, action, next_state, goal):
"""到达任务奖励"""
# 距离奖励
current_dist = np.linalg.norm(state['ee_pos'] - goal)
next_dist = np.linalg.norm(next_state['ee_pos'] - goal)
distance_reward = current_dist - next_dist
# 到达奖励
reach_reward = 10.0 if next_dist < 0.05 else 0.0
# 动作惩罚（鼓励平滑动作）
action_penalty = -0.01 * np.linalg.norm(action)
# 碰撞惩罚
collision_penalty = -1.0 if next_state['collision'] else 0.0
total_reward = (
distance_reward +
reach_reward +
action_penalty +
collision_penalty
)
return total_reward
def manipulation_reward(self, state, action, next_state, goal):
"""操作任务奖励"""
# 物体位置奖励
object_dist = np.linalg.norm(
next_state['object_pos'] - goal['object_pos']
)
object_reward = -object_dist
# 抓取奖励
grasp_reward = 5.0 if next_state['grasped'] else 0.0
# 稳定性奖励
stability_reward = -np.linalg.norm(
next_state['object_velocity']
) if next_state['grasped'] else 0.0
return object_reward + grasp_reward + stability_reward
路径规划
1. 运动规划
python
class MotionPlanner:
def __init__(self, robot, environment):
self.robot = robot
self.env = environment
def plan_rrt(self, start, goal, max_iter=1000):
"""RRT 路径规划"""
tree = RRTTree(start)
for _ in range(max_iter):
# 随机采样
if random.random() < 0.1:
sample = goal
else:
sample = self.random_sample()
# 最近节点
nearest = tree.nearest(sample)
# 扩展
new_node = self.extend(nearest, sample)
if new_node and self.is_collision_free(nearest, new_node):
tree.add_node(new_node, parent=nearest)
# 检查是否到达目标
if self.reached_goal(new_node, goal):
path = tree.get_path(new_node)
return self.smooth_path(path)
return None
def plan_prm(self, start, goal, num_samples=1000):
"""PRM 路径规划"""
roadmap = PRMGraph()
# 采样阶段
for _ in range(num_samples):
sample = self.random_sample()
if self.is_valid(sample):
roadmap.add_node(sample)
# 连接阶段
for node in roadmap.nodes:
neighbors = roadmap.find_neighbors(node, radius=1.0)
for neighbor in neighbors:
if self.is_collision_free(node, neighbor):
roadmap.add_edge(node, neighbor)
# 搜索阶段
path = roadmap.search(start, goal)
return path
def smooth_path(self, path):
"""路径平滑"""
smoothed = [path[0]]
i = 0
while i < len(path) - 1:
# 尝试直线连接
j = len(path) - 1
while j > i + 1:
if self.is_collision_free(path[i], path[j]):
smoothed.append(path[j])
i = j
break
j -= 1
else:
smoothed.append(path[i + 1])
i += 1
return smoothed
2. 轨迹优化
python
class TrajectoryOptimizer:
def __init__(self, robot, dynamics):
self.robot = robot
self.dynamics = dynamics
def optimize(self, waypoints, duration, num_samples=100):
"""轨迹优化"""
# 初始轨迹（线性插值）
t_samples = np.linspace(0, duration, num_samples)
trajectory = self.interpolate_waypoints(waypoints, t_samples)
# 优化目标
def objective(x):
# x 包含所有时间点的关节位置
q = x.reshape(num_samples, self.robot.num_joints)
# 平滑性代价
smoothness = self.smoothness_cost(q, t_samples)
# 动力学可行性代价
dynamics_cost = self.dynamics_feasibility(q, t_samples)
# 障碍物避让代价
obstacle_cost = self.obstacle_cost(q)
return smoothness + dynamics_cost + obstacle_cost
# 约束
constraints = self.build_constraints(waypoints, t_samples)
# 优化
result = minimize(
objective,
trajectory.flatten(),
constraints=constraints,
method='SLSQP'
)
return result.x.reshape(num_samples, self.robot.num_joints)
def smoothness_cost(self, q, t):
"""平滑性代价"""
# 最小化加加速度（jerk）
dt = t[1] - t[0]
qd = np.gradient(q, dt, axis=0)
qdd = np.gradient(qd, dt, axis=0)
jerk = np.gradient(qdd, dt, axis=0)
return np.sum(jerk**2) * dt
def dynamics_feasibility(self, q, t):
"""动力学可行性代价"""
dt = t[1] - t[0]
qd = np.gradient(q, dt, axis=0)
qdd = np.gradient(qd, dt, axis=0)
cost = 0
for i in range(len(t)):
tau = self.dynamics.compute_dynamics(q[i], qd[i], qdd[i])
# 力矩约束
tau_max = self.robot.max_torques
cost += np.sum(np.maximum(0, np.abs(tau) - tau_max)**2)
return cost
步态规划（双足机器人）
1. ZMP 稳定性
python
class ZMPPlanner:
def __init__(self, robot):
self.robot = robot
self.gravity = 9.81
def compute_zmp(self, q, qd, qdd):
"""计算零力矩点（ZMP）"""
# 使用逆动力学计算地面反力
total_force = np.zeros(3)
total_torque = np.zeros(3)
com_position = np.zeros(3)
for i in range(self.robot.num_links):
# 连杆质心位置
link_com = self.robot.get_link_com(i, q)
# 连杆加速度
link_acc = self.robot.get_link_acceleration(i, q, qd, qdd)
# 连杆质量
mass = self.robot.link_masses[i]
# 惯性力
force = mass * (link_acc + np.array([0, 0, self.gravity]))
# 力矩
torque = np.cross(link_com, force)
total_force += force
total_torque += torque
com_position += mass * link_com
# 质心位置
com_position /= self.robot.total_mass
# ZMP 计算
zmp_x = -total_torque[1] / total_force[2]
zmp_y = total_torque[0] / total_force[2]
return np.array([zmp_x, zmp_y, 0])
def is_stable(self, zmp, support_polygon):
"""检查 ZMP 是否在支撑多边形内"""
return self.point_in_polygon(zmp[:2], support_polygon)
2. 步态生成
python
class GaitGenerator:
def __init__(self, robot):
self.robot = robot
self.zmp_planner = ZMPPlanner(robot)
def generate_walk_gait(self, step_length, step_duration):
"""生成行走步态"""
# 步态参数
double_support_duration = step_duration * 0.2
single_support_duration = step_duration * 0.8
# 生成参考轨迹
trajectories = {
'left_foot': [],
'right_foot': [],
'com': [],
'zmp': []
}
t = 0
while t < 2 * step_duration:
# 支撑腿判定
if t % (2 * step_duration) < step_duration:
support_leg = 'left'
swing_leg = 'right'
else:
support_leg = 'right'
swing_leg = 'left'
# 摆动腿轨迹
swing_trajectory = self.swing_foot_trajectory(
swing_leg, step_length, t, step_duration
)
# 质心轨迹
com_trajectory = self.com_trajectory(
t, step_duration, double_support_duration
)
# ZMP 轨迹
zmp_trajectory = self.zmp_trajectory(
support_leg, t, step_duration, double_support_duration
)
trajectories[swing_leg].append(swing_trajectory)
trajectories['com'].append(com_trajectory)
trajectories['zmp'].append(zmp_trajectory)
t += 0.01 # 100Hz
return trajectories
def swing_foot_trajectory(self, leg, step_length, t, duration):
"""摆动腿轨迹（贝塞尔曲线）"""
# 起点和终点
if leg == 'left':
start = np.array([0, 0.1, 0])
end = np.array([step_length, 0.1, 0])
else:
start = np.array([0, -0.1, 0])
end = np.array([step_length, -0.1, 0])
# 控制点（抬腿高度）
height = 0.05
mid = (start + end) / 2 + np.array([0, 0, height])
# 贝塞尔插值
s = t / duration
position = (1-s)**2 * start + 2*(1-s)*s * mid + s**2 * end
return position
全身控制
1. 全身控制器
python
class WholeBodyController:
def __init__(self, robot):
self.robot = robot
self.num_joints = robot.num_joints
self.num_contacts = robot.num_contacts
def compute(self, desired_acceleration, contact_forces):
"""全身控制"""
# 构建优化问题
# 最小化: ||M*qdd + h - J^T*f - tau||^2
# 约束: 接触约束、摩擦锥、力矩限制
M = self.robot.get_mass_matrix()
h = self.robot.get_bias_forces()
J_c = self.robot.get_contact_jacobian()
# 决策变量: [qdd, f, tau]
n_vars = self.num_joints + 3 * self.num_contacts + self.num_joints
# 目标函数
H = np.zeros((n_vars, n_vars))
g = np.zeros(n_vars)
# 动力学一致性
H[:self.num_joints, :self.num_joints] = M.T @ M
g[:self.num_joints] = M.T @ h
# 二次成本
H += np.eye(n_vars) * 1e-6 # 正则化
# 约束
A_eq, b_eq = self.equality_constraints(desired_acceleration)
A_ineq, b_ineq = self.inequality_constraints()
# 求解 QP
solution = self.solve_qp(H, g, A_eq, b_eq, A_ineq, b_ineq)
# 提取关节力矩
tau = solution[2*self.num_joints + 3*self.num_contacts:]
return tau
def equality_constraints(self, desired_qdd):
"""等式约束（动力学一致性）"""
M = self.robot.get_mass_matrix()
h = self.robot.get_bias_forces()
J_c = self.robot.get_contact_jacobian()
# M*qdd + h = J_c^T*f + tau
A = np.zeros((self.num_joints, self.n_vars))
A[:, :self.num_joints] = M
A[:, 2*self.num_joints:2*self.num_joints+3*self.num_contacts] = -J_c.T
A[:, 2*self.num_joints+3*self.num_contacts:] = -np.eye(self.num_joints)
b = -h
return A, b
def inequality_constraints(self):
"""不等式约束（摩擦锥、力矩限制）"""
# 摩擦锥约束
# |f_tangent| <= mu * f_normal
# 力矩约束
# tau_min <= tau <= tau_max
# 构建约束矩阵...
return A_ineq, b_ineq
总结
具身智能运动控制的关键技术：
- 运动学：正/逆运动学求解
- 动力学：力矩计算和控制
- 强化学习：从试错中学习控制策略
- 路径规划：避障和轨迹优化
- 步态规划：双足/多足运动
- 全身控制：多任务协调
随着计算能力的提升和算法的进步，机器人的运动能力将越来越接近人类水平。
延伸阅读：