AI机器人控制与感知完全教程
1. AI机器人技术概述与架构
AI机器人是人工智能与物理世界交互的核心载体，融合了感知、决策、控制等多个技术栈。现代机器人系统通常采用分层架构：
应用层 → 任务规划、人机交互、场景理解
决策层 → 行为决策、任务调度、异常处理
感知层 → 视觉感知、触觉感知、力觉感知、SLAM
控制层 → 运动控制、力控制、轨迹规划
执行层 → 电机驱动、传感器采集、通信总线
常见的机器人类型包括：
- 工业机械臂：固定在工作台上，执行焊接、装配、搬运等任务
- 移动机器人（AGV/AMR）：在仓库、工厂中自主移动
- 人形机器人：双足行走，具备类人操作能力
- 协作机器人（Cobot）：设计为与人类安全共处
- 无人机（UAV）：空中作业，用于巡检、物流等
开发框架方面，ROS2（Robot Operating System 2）是事实上的标准中间件：
# ROS2节点示例（Python）
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, JointState
from geometry_msgs.msg import Twist
class RobotController(Node):
def __init__(self):
super().__init__('robot_controller')
# 订阅关节状态
self.joint_sub = self.create_subscription(
JointState, '/joint_states', self.joint_callback, 10
)
# 发布速度指令
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
# 定时器控制循环
self.timer = self.create_timer(0.01, self.control_loop) # 100Hz
self.current_joint_state = None
self.get_logger().info('机器人控制器已启动')
def joint_callback(self, msg):
self.current_joint_state = msg
def control_loop(self):
if self.current_joint_state is None:
return
cmd = Twist()
cmd.linear.x = 0.5 # 前进速度 m/s
cmd.angular.z = 0.0 # 角速度 rad/s
self.cmd_pub.publish(cmd)
def main():
rclpy.init()
controller = RobotController()
rclpy.spin(controller)
controller.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
2. 机器人感知系统（视觉/触觉/力觉）
2.1 视觉感知
机器人视觉是最主要的感知通道，核心技术包括物体检测、姿态估计、深度感知等。
import numpy as np
class HandEyeCalibration:
"""手眼标定：求解相机与机器人末端执行器之间的变换关系"""
def __init__(self):
self.calibration_data = []
def add_measurement(self, T_gripper_base, T_target_camera):
"""
T_gripper_base: 夹爪在基座坐标系下的4x4变换矩阵
T_target_camera: 标定板在相机坐标系下的4x4变换矩阵
"""
self.calibration_data.append((T_gripper_base, T_target_camera))
def solve_ax_xb(self):
"""
求解 AX = XB 问题（手眼标定核心方程）
X: 相机在夹爪坐标系下的变换（eye-in-hand配置）
"""
n = len(self.calibration_data)
if n < 2:
raise ValueError("至少需要2组标定数据")
# 收集旋转和平移方程
A_rots, B_rots = [], []
A_trans, B_trans = [], []
for i in range(n - 1):
A = np.linalg.inv(self.calibration_data[i+1][0]) @ self.calibration_data[i][0]
B = self.calibration_data[i+1][1] @ np.linalg.inv(self.calibration_data[i][1])
A_rots.append(A[:3, :3])
B_rots.append(B[:3, :3])
A_trans.append(A[:3, 3])
B_trans.append(B[:3, 3])
# 使用Tsai-Lenz方法求解旋转
R_X = self._solve_rotation(A_rots, B_rots)
# 求解平移
t_X = self._solve_translation(A_rots, A_trans, B_trans, R_X)
T_camera_gripper = np.eye(4)
T_camera_gripper[:3, :3] = R_X
T_camera_gripper[:3, 3] = t_X
return T_camera_gripper
def _solve_rotation(self, A_rots, B_rots):
"""使用四元数方法求解旋转"""
from scipy.spatial.transform import Rotation
# 构建 Kronecker 积方程组
M = np.zeros((3 * len(A_rots), 4))
for i, (Ra, Rb) in enumerate(zip(A_rots, B_rots)):
qa = Rotation.from_matrix(Ra).as_quat() # [x,y,z,w]
qb = Rotation.from_matrix(Rb).as_quat()
# 构建四元数乘法矩阵
M[3*i:3*i+3] = self._quat_multiply_matrix(qa)[:, :3] - \
self._quat_multiply_matrix(qb)[:, :3]
# SVD求解
_, _, Vt = np.linalg.svd(M)
q_X = Vt[-1]
q_X = q_X / np.linalg.norm(q_X)
return Rotation.from_quat(np.append(q_X, 0)).as_matrix()[:3, :3]
def _quat_multiply_matrix(self, q):
"""四元数左乘矩阵"""
x, y, z, w = q
return np.array([
[w, -z, y, x],
[z, w, -x, y],
[-y, x, w, z],
[-x, -y, -z, w]
])
def _solve_translation(self, A_rots, A_trans, B_trans, R_X):
"""求解平移向量"""
C = []
d = []
for i in range(len(A_rots)):
C.append(A_rots[i] - np.eye(3))
d.append(R_X @ B_trans[i] - A_trans[i])
C = np.vstack(C)
d = np.concatenate(d)
t_X, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
return t_X
2.2 触觉感知与力觉感知
触觉传感器使机器人能够感知接触力、物体纹理和形状。力觉传感器则用于精确的力控制。
import numpy as np
from collections import deque
class ForceTorqueSensor:
"""六维力/力矩传感器"""
def __init__(self, bias=None, noise_std=0.1):
self.bias = bias if bias is not None else np.zeros(6)
self.noise_std = noise_std
self.filter_window = 10
self.readings = deque(maxlen=self.filter_window)
def read_raw(self, true_force):
"""读取原始力数据（含噪声和偏置）"""
noise = np.random.randn(6) * self.noise_std
return true_force + self.bias + noise
def read_filtered(self, true_force):
"""中值滤波后的力数据"""
raw = self.read_raw(true_force)
self.readings.append(raw)
if len(self.readings) < 3:
return raw
return np.median(list(self.readings), axis=0)
def calibrate(self, zero_force_readings):
"""零点标定"""
self.bias = np.mean(zero_force_readings, axis=0)
def detect_contact(self, force, threshold=2.0):
"""接触检测"""
force_magnitude = np.linalg.norm(force[:3])
return force_magnitude > threshold
class TactileProcessor:
"""触觉数据处理"""
def __init__(self, sensor_size=(16, 16)):
self.sensor_size = sensor_size
self.baseline = None
def set_baseline(self, tactile_image):
"""设置基线（无接触状态）"""
self.baseline = tactile_image.copy()
def compute_contact_map(self, tactile_image):
"""计算接触图"""
if self.baseline is None:
self.baseline = np.zeros(self.sensor_size)
diff = tactile_image - self.baseline
contact_map = np.maximum(diff, 0)
return contact_map
def estimate_contact_force(self, contact_map, calibration_factor=0.1):
"""估计接触力"""
total_force = np.sum(contact_map) * calibration_factor
return total_force
def estimate_contact_center(self, contact_map):
"""估计接触中心位置"""
if np.sum(contact_map) < 1e-6:
return None
rows, cols = np.meshgrid(
np.arange(self.sensor_size[0]),
np.arange(self.sensor_size[1]),
indexing='ij'
)
center_r = np.sum(rows * contact_map) / np.sum(contact_map)
center_c = np.sum(cols * contact_map) / np.sum(contact_map)
return (center_r, center_c)
def estimate_slip(self, prev_contact, curr_contact, prev_center, curr_center):
"""滑移检测"""
if prev_center is None or curr_center is None:
return False, 0.0
displacement = np.sqrt(
(curr_center[0] - prev_center[0])**2 +
(curr_center[1] - prev_center[1])**2
)
# 接触面积变化
prev_area = np.sum(prev_contact > 0.1)
curr_area = np.sum(curr_contact > 0.1)
area_change = abs(curr_area - prev_area) / max(prev_area, 1)
is_slipping = displacement > 1.0 or area_change > 0.3
return is_slipping, displacement
3. SLAM与自主导航
SLAM（Simultaneous Localization and Mapping）使机器人能够在未知环境中同时定位自身和构建地图。
3.1 基于图优化的SLAM
import numpy as np
from scipy.optimize import least_squares
class PoseGraphSLAM:
"""基于位姿图的SLAM"""
def __init__(self):
self.poses = [] # 位姿节点 [(x, y, theta), ...]
self.odometry_edges = [] # 里程计边 [(i, j, measurement, information)]
self.loop_edges = [] # 回环检测边
def add_pose(self, pose):
self.poses.append(np.array(pose))
def add_odometry(self, from_idx, to_idx, measurement, information_matrix):
"""
添加里程计约束
measurement: (dx, dy, dtheta) 相对位移
information_matrix: 3x3信息矩阵（协方差的逆）
"""
self.odometry_edges.append((from_idx, to_idx,
np.array(measurement),
np.array(information_matrix)))
def add_loop_closure(self, from_idx, to_idx, measurement, information_matrix):
"""添加回环约束"""
self.loop_edges.append((from_idx, to_idx,
np.array(measurement),
np.array(information_matrix)))
def optimize(self, max_iterations=100):
"""使用Levenberg-Marquardt优化位姿图"""
n_poses = len(self.poses)
if n_poses < 2:
return
# 将所有位姿展平为一维向量
x0 = np.concatenate(self.poses)
def residuals(x):
poses = x.reshape(-1, 3)
all_residuals = []
# 里程计残差
for (i, j, meas, info) in self.odometry_edges:
pred = self._compute_relative_pose(poses[i], poses[j])
error = pred - meas
# 信息矩阵加权
L = np.linalg.cholesky(info)
weighted_error = L @ error
all_residuals.extend(weighted_error)
# 回环残差
for (i, j, meas, info) in self.loop_edges:
pred = self._compute_relative_pose(poses[i], poses[j])
error = pred - meas
L = np.linalg.cholesky(info)
weighted_error = L @ error
all_residuals.extend(weighted_error)
return np.array(all_residuals)
# 优化
result = least_squares(residuals, x0, method='lm', max_nfev=max_iterations)
# 更新位姿
optimized_poses = result.x.reshape(-1, 3)
self.poses = [p for p in optimized_poses]
return result.cost
def _compute_relative_pose(self, pose1, pose2):
"""计算两个位姿之间的相对变换"""
dx = pose2[0] - pose1[0]
dy = pose2[1] - pose1[1]
dtheta = pose2[2] - pose1[2]
# 转换到pose1的局部坐标系
c, s = np.cos(pose1[2]), np.sin(pose1[2])
local_dx = c * dx + s * dy
local_dy = -s * dx + c * dy
return np.array([local_dx, local_dy, dtheta])
class SimpleOccupancyGrid:
"""占用栅格地图"""
def __init__(self, width=100, height=100, resolution=0.1):
self.width = width
self.height = height
self.resolution = resolution # 每格代表的米数
self.grid = np.full((height, width), 0.5) # 0.5表示未知
self.log_odds = np.zeros((height, width))
def update(self, robot_pose, scan_points, max_range=10.0):
"""用激光扫描更新地图"""
rx, ry, rtheta = robot_pose
# 机器人位置对应的栅格坐标
gx = int(rx / self.resolution) + self.width // 2
gy = int(ry / self.resolution) + self.height // 2
for point in scan_points:
# 计算终点栅格坐标
angle = rtheta + point[1]
dist = min(point[0], max_range)
ex = int((rx + dist * np.cos(angle)) / self.resolution) + self.width // 2
ey = int((ry + dist * np.sin(angle)) / self.resolution) + self.height // 2
# Bresenham光线追踪
cells = self._bresenham(gx, gy, ex, ey)
for i, (cx, cy) in enumerate(cells):
if not (0 <= cx < self.width and 0 <= cy < self.height):
continue
if i == len(cells) - 1 and dist < max_range:
# 终点：占用
self.log_odds[cy, cx] += 0.85
else:
# 光线路径：空闲
self.log_odds[cy, cx] -= 0.4
# 转换为概率
self.grid = 1.0 / (1.0 + np.exp(-self.log_odds))
def _bresenham(self, x0, y0, x1, y1):
"""Bresenham直线算法"""
cells = []
dx = abs(x1 - x0)
dy = abs(y1 - y0)
sx = 1 if x1 > x0 else -1
sy = 1 if y1 > y0 else -1
err = dx - dy
while True:
cells.append((x0, y0))
if x0 == x1 and y0 == y1:
break
e2 = 2 * err
if e2 > -dy:
err -= dy
x0 += sx
if e2 < dx:
err += dx
y0 += sy
return cells
def is_occupied(self, x, y):
"""检查某点是否被占用"""
gx = int(x / self.resolution) + self.width // 2
gy = int(y / self.resolution) + self.height // 2
if 0 <= gx < self.width and 0 <= gy < self.height:
return self.grid[gy, gx] > 0.65
return True # 未知区域视为占用
3.2 自主导航
import numpy as np
import heapq
class DWAPlanner:
"""DWA（Dynamic Window Approach）局部规划器"""
def __init__(self, max_speed=1.0, max_yaw_rate=1.0,
max_accel=0.5, max_delta_yaw_rate=1.0,
v_resolution=0.05, yaw_resolution=0.05,
dt=0.1, predict_time=2.0):
self.max_speed = max_speed
self.max_yaw_rate = max_yaw_rate
self.max_accel = max_accel
self.max_delta_yaw_rate = max_delta_yaw_rate
self.v_resolution = v_resolution
self.yaw_resolution = yaw_resolution
self.dt = dt
self.predict_time = predict_time
# 评价函数权重
self.alpha = 0.15 # 航向评价权重
self.beta = 0.2 # 障碍物距离评价权重
self.gamma = 0.1 # 速度评价权重
def plan(self, robot_state, goal, obstacles):
"""
robot_state: (x, y, theta, v, yaw_rate)
goal: (gx, gy)
obstacles: [(ox, oy), ...]
"""
x, y, theta, v, yaw_rate = robot_state
# 动态窗口
vs = self._compute_dynamic_window(v, yaw_rate)
best_score = -float('inf')
best_control = (0, 0)
best_trajectory = None
# 搜索最优控制量
for v_candidate in np.arange(vs[0], vs[1], self.v_resolution):
for yaw_candidate in np.arange(vs[2], vs[3], self.yaw_resolution):
trajectory = self._predict_trajectory(
robot_state, v_candidate, yaw_candidate
)
# 计算评价分数
heading_score = self._heading_score(trajectory, goal)
clearance_score = self._clearance_score(trajectory, obstacles)
velocity_score = v_candidate / self.max_speed
total_score = (self.alpha * heading_score +
self.beta * clearance_score +
self.gamma * velocity_score)
if total_score > best_score:
best_score = total_score
best_control = (v_candidate, yaw_candidate)
best_trajectory = trajectory
return best_control, best_trajectory
def _compute_dynamic_window(self, current_v, current_yaw_rate):
"""计算动态窗口"""
v_min = max(0, current_v - self.max_accel * self.dt)
v_max = min(self.max_speed, current_v + self.max_accel * self.dt)
yaw_min = max(-self.max_yaw_rate,
current_yaw_rate - self.max_delta_yaw_rate * self.dt)
yaw_max = min(self.max_yaw_rate,
current_yaw_rate + self.max_delta_yaw_rate * self.dt)
return (v_min, v_max, yaw_min, yaw_max)
def _predict_trajectory(self, state, v, yaw_rate):
"""预测轨迹"""
trajectory = [state[:3]]
x, y, theta = state[0], state[1], state[2]
steps = int(self.predict_time / self.dt)
for _ in range(steps):
theta += yaw_rate * self.dt
x += v * np.cos(theta) * self.dt
y += v * np.sin(theta) * self.dt
trajectory.append((x, y, theta))
return trajectory
def _heading_score(self, trajectory, goal):
"""航向评价：末端朝向与目标方向的一致性"""
end_x, end_y, end_theta = trajectory[-1]
dx = goal[0] - end_x
dy = goal[1] - end_y
goal_angle = np.arctan2(dy, dx)
angle_diff = abs(end_theta - goal_angle)
angle_diff = min(angle_diff, 2 * np.pi - angle_diff)
return np.pi - angle_diff
def _clearance_score(self, trajectory, obstacles):
"""障碍物距离评价"""
min_dist = float('inf')
for (x, y, _) in trajectory:
for (ox, oy) in obstacles:
dist = np.sqrt((x - ox)**2 + (y - oy)**2)
min_dist = min(min_dist, dist)
return min_dist
4. 机械臂运动学与控制
4.1 正运动学（D-H参数法）
import numpy as np
class DHRobot:
"""基于D-H参数的机械臂运动学"""
def __init__(self, dh_params):
"""
dh_params: [(a, alpha, d, theta_offset), ...]
a: 连杆长度
alpha: 连杆扭角
d: 连杆偏距
theta_offset: 关节角偏移
"""
self.dh_params = dh_params
self.n_joints = len(dh_params)
def forward_kinematics(self, joint_angles):
"""正运动学：从关节角计算末端位姿"""
T = np.eye(4)
transforms = [T.copy()]
for i, (a, alpha, d, offset) in enumerate(self.dh_params):
theta = joint_angles[i] + offset
# 标准D-H变换矩阵
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
Ti = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
T = T @ Ti
transforms.append(T.copy())
return T, transforms
def jacobian(self, joint_angles):
"""计算几何雅可比矩阵"""
T, transforms = self.forward_kinematics(joint_angles)
p_e = T[:3, 3] # 末端位置
J = np.zeros((6, self.n_joints))
for i in range(self.n_joints):
z_i = transforms[i][:3, 2] # 关节轴方向
p_i = transforms[i][:3, 3] # 关节位置
# 旋转关节
J[:3, i] = np.cross(z_i, p_e - p_i) # 线速度
J[3:, i] = z_i # 角速度
return J
def inverse_kinematics_numerical(self, target_pose, initial_angles=None,
max_iter=1000, tol=1e-4):
"""数值法逆运动学"""
if initial_angles is None:
q = np.zeros(self.n_joints)
else:
q = np.array(initial_angles, dtype=float)
for iteration in range(max_iter):
T, _ = self.forward_kinematics(q)
# 位姿误差
pos_error = target_pose[:3, 3] - T[:3, 3]
# 旋转误差（使用对数映射）
R_error = target_pose[:3, :3] @ T[:3, :3].T
# 罗德里格斯公式提取旋转轴角
angle = np.arccos(np.clip((np.trace(R_error) - 1) / 2, -1, 1))
if abs(angle) < 1e-10:
rot_error = np.zeros(3)
else:
axis = np.array([
R_error[2, 1] - R_error[1, 2],
R_error[0, 2] - R_error[2, 0],
R_error[1, 0] - R_error[0, 1]
]) / (2 * np.sin(angle))
rot_error = axis * angle
error = np.concatenate([pos_error, rot_error])
if np.linalg.norm(error) < tol:
print(f"IK收敛于第{iteration+1}次迭代")
return q
# 阻尼最小二乘法（DLS）
J = self.jacobian(q)
damping = 0.01
delta_q = J.T @ np.linalg.solve(
J @ J.T + damping**2 * np.eye(6), error
)
q += delta_q
print(f"IK未收敛，最终误差: {np.linalg.norm(error):.6f}")
return q
# 创建6轴机械臂（类UR5参数）
dh_params = [
(0, -np.pi/2, 0.0892, 0), # 关节1
(-0.425, 0, 0, 0), # 关节2
(-0.392, 0, 0, 0), # 关节3
(0, -np.pi/2, 0.1093, 0), # 关节4
(0, np.pi/2, 0, 0), # 关节5
(0, 0, 0.0823, 0), # 关节6
]
robot = DHRobot(dh_params)
# 正运动学示例
joint_angles = [0, -np.pi/4, np.pi/2, 0, np.pi/4, 0]
T, _ = robot.forward_kinematics(joint_angles)
print(f"末端位置: {T[:3, 3]}")
print(f"末端旋转矩阵:\n{T[:3, :3]}")
4.2 轨迹规划
import numpy as np
class TrajectoryPlanner:
"""关节空间轨迹规划"""
@staticmethod
def cubic_polynomial(q0, qf, v0, vf, T, dt=0.01):
"""三次多项式插值"""
# 系数求解
a0 = q0
a1 = v0
a2 = (3*(qf - q0) - (2*v0 + vf)*T) / T**2
a3 = (2*(q0 - qf) + (v0 + vf)*T) / T**3
t = np.arange(0, T + dt, dt)
q = a0 + a1*t + a2*t**2 + a3*t**3
v = a1 + 2*a2*t + 3*a3*t**2
a = 2*a2 + 6*a3*t
return t, q, v, a
@staticmethod
def quintic_polynomial(q0, qf, v0, vf, a0, af, T, dt=0.01):
"""五次多项式插值（保证加速度连续）"""
h = qf - q0
# 五次多项式系数
a_0 = q0
a_1 = v0
a_2 = a0 / 2
a_3 = (20*h - (8*vf + 12*v0)*T - (3*a0 - af)*T**2) / (2*T**3)
a_4 = (-30*h + (14*vf + 16*v0)*T + (3*a0 - 2*af)*T**2) / (2*T**4)
a_5 = (12*h - 6*(vf + v0)*T + (af - a0)*T**2) / (2*T**5)
t = np.arange(0, T + dt, dt)
q = a_0 + a_1*t + a_2*t**2 + a_3*t**3 + a_4*t**4 + a_5*t**5
v = a_1 + 2*a_2*t + 3*a_3*t**2 + 4*a_4*t**3 + 5*a_5*t**4
a = 2*a_2 + 6*a_3*t + 12*a_4*t**2 + 20*a_5*t**3
return t, q, v, a
@staticmethod
def trapezoidal_velocity_profile(q0, qf, v_max, a_max, dt=0.01):
"""梯形速度规划"""
direction = 1 if qf > q0 else -1
dist = abs(qf - q0)
# 判断是否能达到最大速度
t_accel = v_max / a_max
d_accel = 0.5 * a_max * t_accel**2
if 2 * d_accel > dist:
# 无法达到最大速度，三角形速度规划
t_accel = np.sqrt(dist / a_max)
t_total = 2 * t_accel
t_const = 0
v_peak = a_max * t_accel
else:
d_const = dist - 2 * d_accel
t_const = d_const / v_max
t_total = 2 * t_accel + t_const
v_peak = v_max
t = np.arange(0, t_total + dt, dt)
q = np.zeros_like(t)
v = np.zeros_like(t)
for i, ti in enumerate(t):
if ti <= t_accel:
v[i] = a_max * ti
q[i] = 0.5 * a_max * ti**2
elif ti <= t_accel + t_const:
v[i] = v_peak
q[i] = d_accel + v_peak * (ti - t_accel)
else:
t_decel = ti - t_accel - t_const
v[i] = v_peak - a_max * t_decel
q[i] = dist - 0.5 * a_max * (t_total - ti)**2
q = q0 + direction * q
v = direction * v
return t, q, v
class CartesianTrajectoryPlanner:
"""笛卡尔空间轨迹规划"""
@staticmethod
def linear_interpolation(T_start, T_end, n_points=100):
"""直线插值"""
trajectories = []
for i in range(n_points + 1):
s = i / n_points
# 位置线性插值
pos = (1 - s) * T_start[:3, 3] + s * T_end[:3, 3]
# 旋转球面线性插值（SLERP）
R = TrajectoryPlanner._slerp(T_start[:3, :3], T_end[:3, :3], s)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = pos
trajectories.append(T)
return trajectories
@staticmethod
def _slerp(R1, R2, t):
"""旋转矩阵的球面线性插值"""
from scipy.spatial.transform import Rotation, Slerp
rots = Rotation.from_matrix([R1, R2])
slerp = Slerp([0, 1], rots)
interp_rot = slerp(t)
return interp_rot.as_matrix()
5. 抓取规划与力控
5.1 抓取规划
import numpy as np
class GraspPlanner:
"""抓取规划器"""
def __init__(self, gripper_width=0.08, gripper_force=50.0):
self.gripper_width = gripper_width # 最大开口宽度(m)
self.gripper_force = gripper_force # 最大抓取力(N)
def generate_grasp_candidates(self, object_pose, object_size, n_candidates=16):
"""
生成候选抓取姿态
object_pose: (x, y, z, roll, pitch, yaw)
object_size: (length, width, height)
"""
x, y, z = object_pose[:3]
candidates = []
for i in range(n_candidates):
angle = 2 * np.pi * i / n_candidates
# 抓取点在物体表面
grasp_x = x + object_size[0]/2 * np.cos(angle)
grasp_y = y + object_size[1]/2 * np.sin(angle)
grasp_z = z + object_size[2] / 2 # 从上方抓取
# 夹爪朝向物体中心
approach_angle = np.arctan2(y - grasp_y, x - grasp_x)
grasp_pose = {
'position': np.array([grasp_x, grasp_y, grasp_z]),
'orientation': self._angle_to_rotation(approach_angle),
'width': min(self.gripper_width,
min(object_size[0], object_size[1])),
'score': 0.0
}
candidates.append(grasp_pose)
return candidates
def score_grasp(self, grasp_pose, object_pose, obstacles=None):
"""评估抓取质量"""
score = 0.0
# 1. 抓取方向与物体主轴的对齐度
obj_center = np.array(object_pose[:3])
grasp_pos = grasp_pose['position']
direction = obj_center - grasp_pos
direction = direction / (np.linalg.norm(direction) + 1e-6)
# 偏好从上方抓取
vertical_score = abs(direction[2]) # z方向分量
score += 0.3 * vertical_score
# 2. 抓取稳定性（对称抓取得分更高）
symmetry_score = 1.0 # 简化
score += 0.3 * symmetry_score
# 3. 障碍物碰撞惩罚
if obstacles:
min_dist = float('inf')
for obs in obstacles:
dist = np.linalg.norm(grasp_pos - obs[:3])
min_dist = min(min_dist, dist)
collision_score = min(1.0, min_dist / 0.1)
score += 0.4 * collision_score
else:
score += 0.4
grasp_pose['score'] = score
return score
def select_best_grasp(self, candidates):
"""选择最优抓取"""
if not candidates:
return None
return max(candidates, key=lambda g: g['score'])
def _angle_to_rotation(self, angle):
"""将绕z轴旋转角转换为旋转矩阵"""
c, s = np.cos(angle), np.sin(angle)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])
class ImpedanceController:
"""阻抗控制器：柔顺力控"""
def __init__(self, mass=1.0, damping=10.0, stiffness=100.0):
"""
M * (x_ddot - x_ddot_d) + D * (x_dot - x_dot_d) + K * (x - x_d) = F_ext
"""
self.M = mass
self.D = damping
self.K = stiffness
def compute_force(self, current_pos, desired_pos,
current_vel, desired_vel, external_force):
"""
计算阻抗控制输出力
"""
pos_error = current_pos - desired_pos
vel_error = current_vel - desired_vel
# 阻抗控制律
F_command = external_force - (
self.K * pos_error + self.D * vel_error
)
return F_command
def admittance_control(self, measured_force, desired_pos, dt=0.001):
"""
导纳控制：根据力反馈调整期望位置
"""
# 计算虚拟加速度
acceleration = (measured_force - self.K * 0) / self.M
# 简化：直接计算位置修正
position_correction = measured_force / self.K
new_desired_pos = desired_pos + position_correction
return new_desired_pos
class HybridForcePositionController:
"""力/位混合控制器"""
def __init__(self, selection_matrix=None):
"""
selection_matrix: 对角矩阵，1表示力控方向，0表示位置控方向
"""
if selection_matrix is None:
self.S = np.diag([0, 0, 1, 0, 0, 0]) # z方向力控
else:
self.S = selection_matrix
self.position_pid = {'kp': 500, 'ki': 10, 'kd': 50}
self.force_pid = {'kp': 0.01, 'ki': 0.001, 'kd': 0.0005}
def compute(self, desired_wrench, measured_wrench,
desired_pose, current_pose, dt=0.001):
"""
计算混合力/位控制输出
desired_wrench: 期望力/力矩 [Fx, Fy, Fz, Tx, Ty, Tz]
"""
# 位置控制部分（S的对角线为0的方向）
pos_error = desired_pose[:3] - current_pose[:3]
I_minus_S = np.eye(6) - self.S
# 力控制部分（S的对角线为1的方向）
force_error = desired_wrench - measured_wrench
# 合并输出
position_contribution = I_minus_S[:3, :3] @ pos_error * self.position_pid['kp']
force_contribution = self.S[:3, :3] @ force_error[:3] * self.force_pid['kp']
output_wrench = position_contribution + force_contribution
return output_wrench
6. 强化学习机器人控制
6.1 基于PPO的机器人控制
import numpy as np
import torch
import torch.nn as nn
import torch.optim as optim
from torch.distributions import Normal
class PolicyNetwork(nn.Module):
"""策略网络（Actor）"""
def __init__(self, state_dim, action_dim, hidden_dim=256):
super().__init__()
self.network = nn.Sequential(
nn.Linear(state_dim, hidden_dim),
nn.ReLU(),
nn.Linear(hidden_dim, hidden_dim),
nn.ReLU(),
)
self.mean_head = nn.Linear(hidden_dim, action_dim)
self.log_std = nn.Parameter(torch.zeros(action_dim))
def forward(self, state):
features = self.network(state)
mean = self.mean_head(features)
std = torch.exp(self.log_std).expand_as(mean)
return mean, std
def get_action(self, state):
mean, std = self.forward(state)
dist = Normal(mean, std)
action = dist.sample()
log_prob = dist.log_prob(action).sum(dim=-1)
return action, log_prob
def evaluate(self, state, action):
mean, std = self.forward(state)
dist = Normal(mean, std)
log_prob = dist.log_prob(action).sum(dim=-1)
entropy = dist.entropy().sum(dim=-1)
return log_prob, entropy
class ValueNetwork(nn.Module):
"""价值网络（Critic）"""
def __init__(self, state_dim, hidden_dim=256):
super().__init__()
self.network = nn.Sequential(
nn.Linear(state_dim, hidden_dim),
nn.ReLU(),
nn.Linear(hidden_dim, hidden_dim),
nn.ReLU(),
nn.Linear(hidden_dim, 1),
)
def forward(self, state):
return self.network(state)
class PPOAgent:
"""PPO强化学习智能体"""
def __init__(self, state_dim, action_dim, lr=3e-4, gamma=0.99,
clip_epsilon=0.2, epochs=10, batch_size=64):
self.policy = PolicyNetwork(state_dim, action_dim)
self.value = ValueNetwork(state_dim)
self.policy_optimizer = optim.Adam(self.policy.parameters(), lr=lr)
self.value_optimizer = optim.Adam(self.value.parameters(), lr=lr)
self.gamma = gamma
self.clip_epsilon = clip_epsilon
self.epochs = epochs
self.batch_size = batch_size
def select_action(self, state):
state_tensor = torch.FloatTensor(state).unsqueeze(0)
with torch.no_grad():
action, log_prob = self.policy.get_action(state_tensor)
return action.squeeze(0).numpy(), log_prob.item()
def update(self, states, actions, old_log_probs, returns, advantages):
states = torch.FloatTensor(states)
actions = torch.FloatTensor(actions)
old_log_probs = torch.FloatTensor(old_log_probs)
returns = torch.FloatTensor(returns)
advantages = torch.FloatTensor(advantages)
for _ in range(self.epochs):
# 随机打乱数据
indices = np.random.permutation(len(states))
for start in range(0, len(states), self.batch_size):
end = start + self.batch_size
batch_idx = indices[start:end]
batch_states = states[batch_idx]
batch_actions = actions[batch_idx]
batch_old_log_probs = old_log_probs[batch_idx]
batch_returns = returns[batch_idx]
batch_advantages = advantages[batch_idx]
# 计算新的log概率
new_log_probs, entropy = self.policy.evaluate(
batch_states, batch_actions
)
# PPO裁剪目标
ratio = torch.exp(new_log_probs - batch_old_log_probs)
surr1 = ratio * batch_advantages
surr2 = torch.clamp(ratio, 1 - self.clip_epsilon,
1 + self.clip_epsilon) * batch_advantages
policy_loss = -torch.min(surr1, surr2).mean() - 0.01 * entropy.mean()
# 价值函数损失
values = self.value(batch_states).squeeze()
value_loss = nn.MSELoss()(values, batch_returns)
# 更新
self.policy_optimizer.zero_grad()
policy_loss.backward()
nn.utils.clip_grad_norm_(self.policy.parameters(), 0.5)
self.policy_optimizer.step()
self.value_optimizer.zero_grad()
value_loss.backward()
nn.utils.clip_grad_norm_(self.value.parameters(), 0.5)
self.value_optimizer.step()
def compute_gae(self, rewards, values, dones, next_value, lam=0.95):
"""广义优势估计（GAE）"""
advantages = np.zeros_like(rewards)
last_gae = 0
for t in reversed(range(len(rewards))):
if t == len(rewards) - 1:
next_val = next_value
else:
next_val = values[t + 1]
delta = rewards[t] + self.gamma * next_val * (1 - dones[t]) - values[t]
advantages[t] = last_gae = delta + self.gamma * lam * (1 - dones[t]) * last_gae
returns = advantages + values
return advantages, returns
6.2 机器人强化学习环境封装
import numpy as np
class RobotReachEnv:
"""机器人到达任务环境（简化版）"""
def __init__(self, n_joints=6, target_range=0.5):
self.n_joints = n_joints
self.target_range = target_range
self.max_steps = 200
self.step_count = 0
# 关节限制
self.joint_limits = [(-np.pi, np.pi)] * n_joints
# 动作空间：关节角增量
self.action_dim = n_joints
# 状态空间：关节角 + 目标位置 + 末端位置
self.state_dim = n_joints + 6
def reset(self):
"""重置环境"""
self.step_count = 0
self.joint_angles = np.zeros(self.n_joints)
# 随机目标位置
self.target = np.random.uniform(-self.target_range, self.target_range, 3)
return self._get_observation()
def step(self, action):
"""执行一步"""
self.step_count += 1
# 应用动作（关节角增量）
action = np.clip(action, -0.1, 0.1)
self.joint_angles += action
# 关节限位
for i, (low, high) in enumerate(self.joint_limits):
self.joint_angles[i] = np.clip(self.joint_angles[i], low, high)
# 计算正运动学（简化）
end_effector_pos = self._forward_kinematics(self.joint_angles)
# 计算奖励
distance = np.linalg.norm(end_effector_pos - self.target)
reward = -distance # 距离奖励
# 到达奖励
if distance < 0.05:
reward += 10.0
# 关节运动惩罚（鼓励平滑运动）
reward -= 0.01 * np.sum(action**2)
# 终止条件
done = False
if distance < 0.05:
done = True
if self.step_count >= self.max_steps:
done = True
obs = self._get_observation()
info = {'distance': distance, 'end_effector': end_effector_pos}
return obs, reward, done, info
def _get_observation(self):
end_effector = self._forward_kinematics(self.joint_angles)
obs = np.concatenate([
self.joint_angles,
self.target,
end_effector
])
return obs.astype(np.float32)
def _forward_kinematics(self, joint_angles):
"""简化正运动学"""
pos = np.array([0.0, 0.0, 0.5]) # 基座位置
link_length = 0.3
cumulative_angle = 0
for i, angle in enumerate(joint_angles):
cumulative_angle += angle
pos[0] += link_length * np.cos(cumulative_angle)
pos[1] += link_length * np.sin(cumulative_angle)
pos[2] += 0.05 * np.sin(angle) # 简化的z轴运动
return pos
# 训练循环
def train_reach_task():
env = RobotReachEnv(n_joints=6)
agent = PPOAgent(
state_dim=env.state_dim,
action_dim=env.action_dim
)
n_episodes = 1000
max_steps = 200
for episode in range(n_episodes):
states, actions, rewards, log_probs, dones = [], [], [], [], []
state = env.reset()
for step in range(max_steps):
action, log_prob = agent.select_action(state)
next_state, reward, done, info = env.step(action)
states.append(state)
actions.append(action)
rewards.append(reward)
log_probs.append(log_prob)
dones.append(done)
state = next_state
if done:
break
# 计算GAE
values = [agent.value(torch.FloatTensor(s)).item() for s in states]
next_value = agent.value(torch.FloatTensor(next_state)).item() if not done else 0
advantages, returns = agent.compute_gae(
np.array(rewards), np.array(values), np.array(dones), next_value
)
# 更新
agent.update(states, actions, log_probs, returns, advantages)
if (episode + 1) % 50 == 0:
avg_reward = np.sum(rewards)
print(f"Episode {episode+1}, Total Reward: {avg_reward:.2f}, "
f"Final Distance: {info['distance']:.4f}")
# train_reach_task() # 取消注释以运行训练
7. 仿真环境（Isaac Sim/Gazebo/MuJoCo）
7.1 MuJoCo环境使用
import numpy as np
# MuJoCo XML模型定义（简化机械臂）
ARM_XML = """
<mujoco model="robot_arm">
<compiler angle="radian" />
<option timestep="0.001" gravity="0 0 -9.81" />
<default>
<joint damping="0.5" />
<geom rgba="0.5 0.5 0.5 1" />
</default>
<asset>
<texture name="grid" type="2d" builtin="checker" rgb1=".8 .8 .8" rgb2=".6 .6 .6" width="300" height="300"/>
<material name="grid" texture="grid" texrepeat="5 5" reflectance="0.1"/>
</asset>
<worldbody>
<geom name="floor" type="plane" size="1 1 0.01" material="grid"/>
<light pos="0 0 3" dir="0 0 -1" diffuse="0.8 0.8 0.8"/>
<body name="base" pos="0 0 0.1">
<geom type="cylinder" size="0.05 0.05" rgba="0.3 0.3 0.8 1"/>
<body name="link1" pos="0 0 0.1">
<joint name="joint1" type="hinge" axis="0 0 1" range="-3.14 3.14"/>
<geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.03" rgba="0.8 0.2 0.2 1"/>
<body name="link2" pos="0 0 0.3">
<joint name="joint2" type="hinge" axis="0 1 0" range="-1.57 1.57"/>
<geom type="capsule" fromto="0 0 0 0.3 0 0" size="0.03" rgba="0.2 0.8 0.2 1"/>
<body name="link3" pos="0.3 0 0">
<joint name="joint3" type="hinge" axis="0 1 0" range="-1.57 1.57"/>
<geom type="capsule" fromto="0 0 0 0.25 0 0" size="0.025" rgba="0.2 0.2 0.8 1"/>
<body name="gripper" pos="0.25 0 0">
<geom type="box" size="0.03 0.04 0.01" rgba="0.8 0.8 0.2 1"/>
<site name="grip_site" pos="0 0 0" size="0.01"/>
</body>
</body>
</body>
</body>
</body>
<!-- 目标物体 -->
<body name="target_object" pos="0.5 0.0 0.15">
<geom type="box" size="0.03 0.03 0.03" rgba="0.9 0.1 0.1 1" mass="0.1"/>
<freejoint name="object_joint"/>
</body>
<!-- 目标位置 -->
<body name="target_site" pos="0.4 0.3 0.2">
<site name="target" type="sphere" size="0.02" rgba="0 1 0 0.3"/>
</body>
</worldbody>
<actuator>
<motor name="act1" joint="joint1" ctrlrange="-50 50"/>
<motor name="act2" joint="joint2" ctrlrange="-50 50"/>
<motor name="act3" joint="joint3" ctrlrange="-50 50"/>
</actuator>
</mujoco>
"""
class MuJoCoRobotEnv:
"""MuJoCo机器人环境封装"""
def __init__(self):
try:
import mujoco
self.model = mujoco.MjModel.from_xml_string(ARM_XML)
self.data = mujoco.MjData(self.model)
self.renderer = None
except ImportError:
print("MuJoCo未安装，使用模拟模式")
self.model = None
self.data = None
def reset(self):
if self.data is None:
return np.zeros(9)
mujoco.mj_resetData(self.model, self.data)
# 随机化目标位置
self.data.qpos[-7:-4] = np.random.uniform(-0.3, 0.3, 3)
mujoco.mj_forward(self.model, self.data)
return self._get_obs()
def step(self, action):
if self.data is None:
return np.zeros(9), 0, False, {}
# 施加控制力矩
self.data.ctrl[:3] = np.clip(action, -50, 50)
mujoco.mj_step(self.model, self.data)
obs = self._get_obs()
reward = self._compute_reward()
done = self._check_done()
return obs, reward, done, {}
def _get_obs(self):
if self.data is None:
return np.zeros(9)
joint_pos = self.data.qpos[:3].copy()
joint_vel = self.data.qvel[:3].copy()
target_pos = self.data.qpos[-7:-4].copy()
return np.concatenate([joint_pos, joint_vel, target_pos])
def _compute_reward(self):
if self.data is None:
return 0
# 末端执行器位置
grip_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "grip_site")
grip_pos = self.data.site_xpos[grip_site_id].copy()
target_pos = self.data.qpos[-7:-4].copy()
distance = np.linalg.norm(grip_pos - target_pos)
reward = -distance
if distance < 0.05:
reward += 10.0
return reward
def _check_done(self):
if self.data is None:
return True
return self.data.time > 5.0
def render(self):
if self.data is None:
return None
if self.renderer is None:
import mujoco
self.renderer = mujoco.Renderer(self.model, height=480, width=640)
self.renderer.update_scene(self.data)
return self.renderer.render()
7.2 Gazebo仿真
# Gazebo ROS2 Launch配置示例
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# 启动Gazebo仿真
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]),
launch_arguments={'world': 'robot_arm_world.sdf'}.items()
)
# 机器人状态发布
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{
'robot_description': open('robot.urdf').read(),
'use_sim_time': True
}]
)
# 运动规划节点
move_group = Node(
package='moveit_servo',
executable='servo_node',
parameters=[{
'use_gazebo': True,
'status_topic': '/servo_server/status'
}]
)
return LaunchDescription([
gazebo,
robot_state_publisher,
move_group,
])
8. 人机协作与安全
协作机器人（Cobot）需要在与人类共享工作空间时保证安全。核心安全技术包括：
8.1 安全监控系统
import numpy as np
import time
class SafetyMonitor:
"""人机协作安全监控系统"""
def __init__(self, robot_workspace_bounds, safety_distances=None):
"""
robot_workspace_bounds: ((x_min, x_max), (y_min, y_max), (z_min, z_max))
safety_distances: 安全距离阈值
"""
self.bounds = robot_workspace_bounds
self.safety_distances = safety_distances or {
'emergency_stop': 0.3, # 紧急停止距离
'reduced_speed': 0.6, # 减速距离
'normal_operation': 1.0, # 正常操作距离
}
self.safety_state = 'NORMAL' # NORMAL, REDUCED, STOPPED
self.human_positions = []
def update_human_positions(self, positions):
"""更新人类位置（来自视觉检测）"""
self.human_positions = positions
def evaluate_safety(self, robot_end_effector, robot_joints_vel):
"""评估安全状态"""
if not self.human_positions:
self.safety_state = 'NORMAL'
return self.safety_state
# 计算最近人类距离
min_distance = float('inf')
closest_human = None
for human_pos in self.human_positions:
dist = np.linalg.norm(robot_end_effector - human_pos)
if dist < min_distance:
min_distance = dist
closest_human = human_pos
# 速度加权距离
robot_speed = np.linalg.norm(robot_joints_vel)
effective_distance = min_distance / (1 + robot_speed * 0.5)
# 判断安全等级
if effective_distance < self.safety_distances['emergency_stop']:
self.safety_state = 'STOPPED'
elif effective_distance < self.safety_distances['reduced_speed']:
self.safety_state = 'REDUCED'
else:
self.safety_state = 'NORMAL'
return {
'state': self.safety_state,
'min_distance': min_distance,
'effective_distance': effective_distance,
'closest_human': closest_human,
'robot_speed': robot_speed
}
def get_speed_limit(self):
"""根据安全状态返回速度限制"""
limits = {
'NORMAL': 1.0, # 全速
'REDUCED': 0.25, # 25%速度
'STOPPED': 0.0 # 停止
}
return limits[self.safety_state]
def compute_safe_trajectory(self, desired_trajectory, safety_eval):
"""修改轨迹以确保安全"""
speed_factor = self.get_speed_limit()
if speed_factor == 0:
return None # 必须停止
# 缩放速度
safe_trajectory = desired_trajectory.copy()
for i in range(len(safe_trajectory)):
if i > 0:
delta = safe_trajectory[i] - safe_trajectory[i-1]
safe_trajectory[i] = safe_trajectory[i-1] + delta * speed_factor
return safe_trajectory
class CollisionAvoidance:
"""碰撞避免模块"""
def __init__(self, min_distance=0.1):
self.min_distance = min_distance
def check_collision(self, robot_points, obstacle_points):
"""
robot_points: 机器人各连杆的采样点
obstacle_points: 障碍物点云
"""
for r_point in robot_points:
distances = np.linalg.norm(obstacle_points - r_point, axis=1)
if np.min(distances) < self.min_distance:
return True, np.min(distances)
return False, float('inf')
def compute_repulsive_force(self, robot_point, obstacles, influence_range=0.5):
"""计算人工势场的排斥力"""
force = np.zeros(3)
for obs in obstacles:
diff = robot_point - obs
dist = np.linalg.norm(diff)
if dist < influence_range and dist > 1e-6:
# 排斥力大小与距离成反比
magnitude = (1/dist - 1/influence_range) / dist**2
force += magnitude * (diff / dist)
return force
9. 多机器人协调
9.1 多机器人任务分配
import numpy as np
from scipy.optimize import linear_sum_assignment
class MultiRobotCoordinator:
"""多机器人协调控制器"""
def __init__(self, n_robots):
self.n_robots = n_robots
self.robot_states = [{'position': np.zeros(3), 'status': 'idle'}
for _ in range(n_robots)]
self.task_queue = []
self.assignments = {}
def add_task(self, task_id, position, priority=1, required_robots=1):
self.task_queue.append({
'id': task_id,
'position': np.array(position),
'priority': priority,
'required_robots': required_robots,
'status': 'pending'
})
def assign_tasks(self):
"""使用匈牙利算法进行任务分配"""
pending_tasks = [t for t in self.task_queue if t['status'] == 'pending']
idle_robots = [i for i, s in enumerate(self.robot_states)
if s['status'] == 'idle']
if not pending_tasks or not idle_robots:
return
# 构建代价矩阵
n = max(len(idle_robots), len(pending_tasks))
cost_matrix = np.full((n, n), 1e6)
for i, robot_idx in enumerate(idle_robots):
for j, task in enumerate(pending_tasks):
dist = np.linalg.norm(
self.robot_states[robot_idx]['position'] - task['position']
)
# 考虑任务优先级
cost_matrix[i, j] = dist / task['priority']
# 匈牙利算法
row_indices, col_indices = linear_sum_assignment(cost_matrix)
for r, c in zip(row_indices, col_indices):
if r < len(idle_robots) and c < len(pending_tasks):
robot_idx = idle_robots[r]
task = pending_tasks[c]
self.assignments[task['id']] = robot_idx
self.robot_states[robot_idx]['status'] = 'busy'
task['status'] = 'assigned'
def update_robot_state(self, robot_idx, position, status=None):
self.robot_states[robot_idx]['position'] = np.array(position)
if status:
self.robot_states[robot_idx]['status'] = status
def collision_free_path(self, robot_paths):
"""时空协调：避免机器人间碰撞"""
time_resolution = 0.1
safe_distance = 0.3
for i in range(len(robot_paths)):
for j in range(i + 1, len(robot_paths)):
path_i = robot_paths[i]
path_j = robot_paths[j]
# 检查时间重叠
min_len = min(len(path_i), len(path_j))
for t in range(min_len):
dist = np.linalg.norm(
np.array(path_i[t]) - np.array(path_j[t])
)
if dist < safe_distance:
# 冲突：让一个机器人等待
# 在路径j的t时刻添加等待
path_j.insert(t, path_j[t])
print(f"机器人{i}和{j}在时刻{t}存在碰撞风险，已协调避让")
return robot_paths
class FormationController:
"""编队控制器"""
def __init__(self, formation_offsets):
"""
formation_offsets: [(dx, dy, dz), ...] 各机器人相对编队中心的偏移
"""
self.offsets = [np.array(o) for o in formation_offsets]
self.kp = 2.0 # 位置增益
def compute_velocities(self, robot_positions, target_center, target_heading=0):
"""
计算各机器人的编队控制速度
"""
velocities = []
for i, (pos, offset) in enumerate(zip(robot_positions, self.offsets)):
# 旋转偏移量到目标朝向
c, s = np.cos(target_heading), np.sin(target_heading)
R = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
rotated_offset = R @ offset
# 期望位置
desired_pos = target_center + rotated_offset
# 位置误差
error = desired_pos - pos
velocity = self.kp * error
# 限速
speed = np.linalg.norm(velocity)
max_speed = 1.0
if speed > max_speed:
velocity = velocity / speed * max_speed
velocities.append(velocity)
return velocities
10. 实战案例：机械臂智能抓取系统
综合前述所有技术，构建一个完整的机械臂智能抓取系统：
import numpy as np
class IntelligentGraspSystem:
"""智能抓取系统：感知-规划-控制一体化"""
def __init__(self):
# 感知模块
self.object_detector = None
self.pose_estimator = None
self.force_sensor = ForceTorqueSensor()
# 规划模块
self.grasp_planner = GraspPlanner(gripper_width=0.08)
self.trajectory_planner = TrajectoryPlanner()
# 控制模块
self.arm_controller = None
self.impedance_ctrl = ImpedanceController(mass=1.0, damping=15.0, stiffness=200.0)
self.safety_monitor = SafetyMonitor(
robot_workspace_bounds=((-0.5, 0.5), (-0.5, 0.5), (0, 0.8))
)
# 状态
self.current_state = 'IDLE'
self.object_pose = None
self.grasp_target = None
def perceive(self, rgb_image, depth_image):
"""感知阶段：检测物体并估计位姿"""
# 模拟物体检测
detected_objects = [
{
'class': 'cube',
'bbox': [200, 150, 300, 250],
'position_3d': np.array([0.35, 0.1, 0.15]),
'size': (0.05, 0.05, 0.05),
'confidence': 0.95
}
]
if detected_objects:
obj = detected_objects[0]
self.object_pose = {
'position': obj['position_3d'],
'orientation': np.eye(3),
'size': obj['size']
}
return True
return False
def plan_grasp(self):
"""规划阶段：生成并评估抓取姿态"""
if self.object_pose is None:
return False
object_pose_tuple = (
self.object_pose['position'][0],
self.object_pose['position'][1],
self.object_pose['position'][2],
0, 0, 0
)
# 生成候选抓取
candidates = self.grasp_planner.generate_grasp_candidates(
object_pose_tuple, self.object_pose['size']
)
# 评估每个候选
for candidate in candidates:
self.grasp_planner.score_grasp(candidate, object_pose_tuple)
# 选择最优抓取
best_grasp = self.grasp_planner.select_best_grasp(candidates)
if best_grasp:
self.grasp_target = best_grasp
return True
return False
def plan_trajectory(self, robot_state):
"""轨迹规划"""
# 预抓取位置（物体上方）
pre_grasp_pos = self.grasp_target['position'].copy()
pre_grasp_pos[2] += 0.15 # 上方15cm
# 规划到预抓取位置的轨迹
t, q, v, a = self.trajectory_planner.quintic_polynomial(
q0=robot_state['joint_angles'][0],
qf=pre_grasp_pos[0],
v0=0, vf=0, a0=0, af=0,
T=2.0
)
return {'approach': (t, q, v, a), 'target': pre_grasp_pos}
def execute_grasp(self, robot_interface):
"""执行抓取（力控）"""
target_force = -5.0 # 向下的抓取力
max_force = 15.0
# 力控下降阶段
for step in range(200):
# 读取力传感器
force = self.force_sensor.read_filtered(np.array([0, 0, target_force, 0, 0, 0]))
# 检测接触
if self.force_sensor.detect_contact(force, threshold=3.0):
print(f"检测到接触，力: {np.linalg.norm(force[:3]):.2f}N")
break
# 阻抗控制
current_pos = robot_interface.get_end_effector_position()
desired_pos = self.grasp_target['position']
correction = self.impedance_ctrl.admittance_control(
force[:3], desired_pos
)
robot_interface.move_to(correction)
# 闭合夹爪
robot_interface.close_gripper(force=20.0)
# 检测是否成功抓取
grasp_force = self.force_sensor.read_filtered(np.zeros(6))
if np.linalg.norm(grasp_force[:3]) > 1.0:
print("抓取成功")
return True
else:
print("抓取失败")
return False
def run(self, rgb_image=None, depth_image=None, robot_interface=None):
"""主循环"""
print(f"当前状态: {self.current_state}")
if self.current_state == 'IDLE':
if self.perceive(rgb_image, depth_image):
self.current_state = 'PLANNING'
elif self.current_state == 'PLANNING':
if self.plan_grasp():
trajectory = self.plan_trajectory({'joint_angles': [0]*6})
self.current_state = 'EXECUTING'
elif self.current_state == 'EXECUTING':
# 安全检查
safety = self.safety_monitor.evaluate_safety(
robot_end_effector=np.zeros(3),
robot_joints_vel=np.zeros(6)
)
if safety['state'] == 'STOPPED':
print("安全事件：紧急停止")
robot_interface.emergency_stop()
return
success = self.execute_grasp(robot_interface)
if success:
self.current_state = 'COMPLETE'
else:
self.current_state = 'IDLE' # 重试
# 使用示例
system = IntelligentGraspSystem()
system.perceive(rgb_image=None, depth_image=None)
system.plan_grasp()
print(f"目标抓取位置: {system.grasp_target['position']}")
print(f"抓取得分: {system.grasp_target['score']:.3f}")
11. 工业应用场景
11.1 智能质检系统
import numpy as np
class QualityInspectionSystem:
"""基于AI的工业质检系统"""
def __init__(self):
self.defect_types = ['scratch', 'dent', 'discoloration', 'crack', 'missing_part']
self.thresholds = {
'scratch': {'min_area': 5, 'max_depth': 0.1},
'dent': {'min_area': 10, 'min_depth': 0.2},
'discoloration': {'min_area': 20, 'color_diff': 15},
'crack': {'min_length': 3, 'max_width': 0.5},
}
def inspect_surface(self, surface_image, point_cloud=None):
"""表面检测"""
defects = []
# 图像预处理
processed = self._preprocess(surface_image)
# 缺陷检测（模拟）
potential_defects = self._detect_anomalies(processed)
for defect in potential_defects:
# 缺陷分类
defect_type = self._classify_defect(defect, surface_image)
# 严重程度评估
severity = self._evaluate_severity(defect, defect_type)
defects.append({
'type': defect_type,
'location': defect['position'],
'size': defect['size'],
'severity': severity,
'pass': severity < 0.7
})
# 综合判定
overall_pass = all(d['pass'] for d in defects)
return {
'defects': defects,
'overall_pass': overall_pass,
'num_defects': len(defects),
'critical_defects': sum(1 for d in defects if d['severity'] > 0.8)
}
def _preprocess(self, image):
"""图像预处理"""
return image # 简化
def _detect_anomalies(self, image):
"""异常检测"""
return [] # 简化
def _classify_defect(self, defect, image):
"""缺陷分类"""
return 'scratch' # 简化
def _evaluate_severity(self, defect, defect_type):
"""评估缺陷严重程度"""
return 0.5 # 简化
class PickAndPlaceSystem:
"""工业拾放系统"""
def __init__(self, robot_arm, conveyor_speed=0.1):
self.robot = robot_arm
self.conveyor_speed = conveyor_speed
self.pick_offset = np.array([0, 0, 0.02]) # 抓取偏移
def track_object_on_conveyor(self, object_pose, camera_trigger_time):
"""跟踪传送带上的物体"""
current_time = time.time()
elapsed = current_time - camera_trigger_time
# 补偿传送带运动
compensated_pose = object_pose.copy()
compensated_pose[0] += self.conveyor_speed * elapsed
return compensated_pose
def compute_intercept_point(self, object_pose, object_velocity, robot_reach_time):
"""计算拦截点"""
intercept_point = object_pose + object_velocity * robot_reach_time
return intercept_point
def execute_dynamic_grasp(self, object_pose, place_pose):
"""动态抓取与放置"""
# 1. 预测物体到达抓取点的时间
object_velocity = np.array([self.conveyor_speed, 0, 0])
# 2. 规划拦截轨迹
robot_pose = self.robot.get_current_pose()[:3]
intercept_point = self.compute_intercept_point(
object_pose, object_velocity, robot_reach_time=0.5
)
# 3. 执行抓取
pre_grasp = intercept_point + self.pick_offset
# 使用时间最优轨迹
trajectory = self._time_optimal_trajectory(
robot_pose, pre_grasp, max_vel=2.0, max_accel=5.0
)
self.robot.execute_trajectory(trajectory)
# 4. 接近并抓取
self.robot.move_to(intercept_point, speed=0.3)
self.robot.close_gripper(force=30)
# 5. 抬起并放置
lift_pose = place_pose + np.array([0, 0, 0.1])
self.robot.move_to(lift_pose, speed=1.0)
self.robot.move_to(place_pose, speed=0.5)
self.robot.open_gripper()
return True
def _time_optimal_trajectory(self, start, end, max_vel, max_accel):
"""时间最优轨迹规划"""
distance = np.linalg.norm(end - start)
# 计算各段时间
t_accel = max_vel / max_accel
d_accel = 0.5 * max_accel * t_accel**2
if 2 * d_accel > distance:
t_accel = np.sqrt(distance / max_accel)
t_total = 2 * t_accel
else:
d_const = distance - 2 * d_accel
t_const = d_const / max_vel
t_total = 2 * t_accel + t_const
# 生成轨迹点
dt = 0.01
t_points = np.arange(0, t_total, dt)
trajectory = []
direction = (end - start) / distance
for t in t_points:
if t <= t_accel:
s = 0.5 * max_accel * t**2
elif t <= t_total - t_accel:
s = d_accel + max_vel * (t - t_accel)
else:
t_decel = t_total - t
s = distance - 0.5 * max_accel * t_decel**2
pos = start + direction * s
trajectory.append(pos)
return trajectory
11.2 协作装配系统
import numpy as np
class CollaborativeAssemblySystem:
"""人机协作装配系统"""
def __init__(self):
self.safety_monitor = SafetyMonitor(
robot_workspace_bounds=((-0.8, 0.8), (-0.8, 0.8), (0, 1.0))
)
self.assembly_steps = []
self.current_step = 0
def define_assembly_task(self, steps):
"""
定义装配任务
steps: [{'action': 'insert', 'part': 'shaft', 'target': pos, ...}, ...]
"""
self.assembly_steps = steps
def human_intent_recognition(self, human_hand_pose, human_gaze_target):
"""识别工人意图"""
# 基于手势和注视方向推断意图
intent = {
'action': 'handover', # 递送、指向、确认
'target_object': None,
'urgency': 'normal',
'confidence': 0.8
}
# 简化的意图识别逻辑
if human_gaze_target is not None:
intent['target_object'] = human_gaze_target
return intent
def adaptive_speed_control(self, human_proximity, task_complexity):
"""根据人类接近程度和任务复杂度自适应调整速度"""
base_speed = 1.0
# 距离因子
if human_proximity < 0.5:
distance_factor = human_proximity / 0.5
else:
distance_factor = 1.0
# 复杂度因子
complexity_factor = 1.0 / (1 + task_complexity * 0.5)
return base_speed * distance_factor * complexity_factor
def execute_collaborative_step(self, step, human_state):
"""执行协作装配步骤"""
action = step['action']
if action == 'handover':
# 递送零件给人
return self._handover_to_human(step, human_state)
elif action == 'insert':
# 精密插入（力控）
return self._precision_insert(step)
elif action == 'hold':
# 固定部件供人操作
return self._hold_for_human(step, human_state)
elif action == 'wait_human':
# 等待人完成操作
return self._wait_for_human(step, human_state)
return False
def _handover_to_human(self, step, human_state):
"""递送零件给人"""
# 计算安全递送点（人手前方）
handover_point = human_state['hand_position'] + np.array([0, 0, 0.1])
# 以安全速度移动到递送点
speed = self.adaptive_speed_control(
human_state['proximity'], task_complexity=0.3
)
print(f"移动到递送点，速度: {speed:.2f}m/s")
# robot.move_to(handover_point, speed=speed)
# 等待人接住
# while not detect_hand_contact():
# time.sleep(0.01)
return True
def _precision_insert(self, step):
"""力控精密插入"""
target_pos = step['target']
insertion_force_limit = 10.0 # N
# 使用螺旋搜索策略
search_radius = 0.002 # 2mm搜索半径
spiral_points = self._generate_spiral(search_radius, n_points=20)
for point in spiral_points:
attempt_pos = target_pos + np.array([point[0], point[1], 0])
# 力控下降
for z in np.linspace(0, -step.get('depth', 0.02), 20):
pos = attempt_pos + np.array([0, 0, z])
# force = read_force_sensor()
force = np.zeros(6)
# 检测插入成功（力突变）
if abs(force[2]) > insertion_force_limit:
print("插入完成")
return True
print("插入失败")
return False
def _hold_for_human(self, step, human_state):
"""固定部件供人操作"""
hold_force = step.get('hold_force', 20.0)
timeout = step.get('timeout', 30.0)
# 施加恒定力保持位置
print(f"保持位置，力: {hold_force}N，超时: {timeout}s")
# robot.apply_force(hold_force)
# 监控人类操作完成
start_time = time.time()
while time.time() - start_time < timeout:
# 检查人类是否完成（通过视觉或按钮信号）
# if human_done_signal():
# return True
pass
return True
def _wait_for_human(self, step, human_state):
"""等待人完成操作"""
timeout = step.get('timeout', 60.0)
expected_action = step.get('expected_action', None)
print(f"等待人类完成操作: {expected_action}，超时: {timeout}s")
# 机器人退到安全位置待命
safe_pos = np.array([0.0, -0.4, 0.5])
# robot.move_to(safe_pos, speed=0.5)
return True
def _generate_spiral(self, radius, n_points=20):
"""生成螺旋搜索点"""
points = []
for i in range(n_points):
angle = 2 * np.pi * i / n_points
r = radius * (i / n_points)
x = r * np.cos(angle)
y = r * np.sin(angle)
points.append((x, y))
return points
# === 应用示例 ===
if __name__ == '__main__':
# 示例1：正运动学
dh_params = [
(0, -np.pi/2, 0.0892, 0),
(-0.425, 0, 0, 0),
(-0.392, 0, 0, 0),
(0, -np.pi/2, 0.1093, 0),
(0, np.pi/2, 0, 0),
(0, 0, 0.0823, 0),
]
robot = DHRobot(dh_params)
joint_angles = [0, -np.pi/4, np.pi/2, 0, np.pi/4, 0]
T, _ = robot.forward_kinematics(joint_angles)
print(f"末端位置: {T[:3, 3]}")
# 示例2：轨迹规划
planner = TrajectoryPlanner()
t, q, v, a = planner.quintic_polynomial(
q0=0, qf=1.5, v0=0, vf=0, a0=0, af=0, T=3.0
)
print(f"轨迹规划完成，共{len(t)}个点")
# 示例3：安全监控
safety = SafetyMonitor(
robot_workspace_bounds=((-0.5, 0.5), (-0.5, 0.5), (0, 0.8))
)
safety.update_human_positions([np.array([0.3, 0.2, 0.5])])
result = safety.evaluate_safety(
robot_end_effector=np.array([0.25, 0.15, 0.4]),
robot_joints_vel=np.array([0.1, 0.2, 0.1, 0, 0, 0])
)
print(f"安全状态: {result['state']}, 最近距离: {result['min_distance']:.3f}m")
print("\n所有示例运行完成！")
