利用视觉反馈实时调整机械臂运动。
import numpy as np
f=500; cx,cy=320,240
def proj(p3d,T):
pc=T[:3,:3].T@(p3d-T[:3,3]); Z=max(pc[2],.01)
return np.array([f*pc[0]/Z+cx,f*pc[1]/Z+cy])
def img_jac(u,v,Z):
return np.array([[-f/Z,0,u*f/Z,u*v/f,-(f**2+v**2)/f,v],[0,-f/Z,v*f/Z,(f**2+u**2)/f,-u*v/f,-u]])
pts3d=np.array([[.3,.1,0],[.3,-.1,0],[.3,-.1,.2],[.3,.1,.2]])
Td=np.eye(4); Td[:3,3]=[0,0,.1]
df=np.concatenate([proj(p,Td) for p in pts3d])
Ti=np.eye(4); Ti[:3,3]=[.2,.1,.15]
T=Ti.copy(); lam=.5; dt=.01
print("视觉伺服(IBVS)"); print("="*60)
for it in range(500):
feats=np.concatenate([proj(p,T) for p in pts3d]); err=np.linalg.norm(feats-df)
if err<1: break
L=np.zeros((8,6))
for j,p in enumerate(pts3d):
pc=T[:3,:3].T@(p-T[:3,3]); Z=max(pc[2],.1)
L[j*2:(j+1)*2]=img_jac(feats[j*2],feats[j*2+1],Z)
vc=-lam*np.linalg.pinv(L)@(feats-df)
T[:3,3]+=vc[:3]*dt
dR=np.array([[0,-vc[5],vc[4]],[vc[5],0,-vc[3]],[-vc[4],vc[3],0]])*dt
T[:3,:3]=T[:3,:3]@(np.eye(3)+dR)
U,_,Vt=np.linalg.svd(T[:3,:3]); T[:3,:3]=U@Vt
print(f"迭代:{it+1} 位置误差:{np.linalg.norm(T[:3,3]-Td[:3,3])*1000:.1f}mm 图像误差:{err:.1f}px")import numpy as np
np.random.seed(42)
print("感知系统噪声鲁棒性")
for sigma in [0.5,1.0,2.0,5.0]:
K=np.array([[500,0,320],[0,500,240],[0,0,1]]); n=8
pts3d=np.random.rand(n,3)*0.2; R=np.eye(3); t=np.array([0,0,1.])
pc=(R@pts3d.T).T+t; p2d=(K@pc.T).T; p2d=p2d[:,:2]/p2d[:,2:3]
noisy=p2d+np.random.randn(*p2d.shape)*sigma
proj_err=np.mean(np.linalg.norm(noisy-p2d,axis=1))
print(f" σ={sigma:.1f}px: 2D误差={proj_err:.2f}px, 3D误差≈{proj_err/500*1000:.1f}mm")感知鲁棒性取决于特征质量、观测数量和噪声水平。工程中常用RANSAC等鲁棒估计方法。
本课程按"运动学→动力学→力控制→感知规划→实战项目"组织，每课都建立在前面知识基础上。理解本课后，你将：
完成本课后，你应该能够：
完成练习后，继续下一课的学习。本课内容将在后续课程中被反复使用和扩展。
针孔相机模型将3D世界投影到2D图像：
其中K是内参矩阵（焦距、主点、畸变），[R|t]是外参（位姿）。从2D恢复3D是欠约束问题，需要额外的约束（多个视角、已知3D模型、深度传感器）。
PRM和RRT是两类经典的采样式规划算法：
采样式方法的成功取决于构型空间的维度和障碍物的几何复杂度。
深度学习极大地改变了机器人感知：
挑战：标注数据需求大、泛化能力有限、实时性要求高。合成数据+域适应是当前的解决方案。
本课涉及的技术在以下场景中有重要应用：
理解理论与实践的差距，是成为优秀机器人工程师的关键。
仿真验证的算法通常需要以下适配才能部署到实际机器人：参数标定、实时性优化、异常处理、安全保护。仿真与现实的差距（sim-to-real gap）是机器人领域的重要挑战。
Python适合算法验证和原型开发，但生产级控制器通常用C++实现。Python的NumPy/SciPy计算效率约为C++的1/10到1/100，对于1kHz控制频率可能不够。可以使用Cython、Numba或直接C++重写来加速。
推荐步骤：(1) Python仿真验证算法正确性；(2) 添加传感器噪声和延迟模型测试鲁棒性；(3) 用C++重写核心计算模块；(4) 在低速度下实际测试；(5) 逐步提高速度和负载。安全永远是第一位的。
| 术语 | 英文 | 定义 |
|---|---|---|
| 正运动学 | Forward Kinematics | 已知关节角求末端位姿 |
| 逆运动学 | Inverse Kinematics | 已知末端位姿求关节角 |
| 雅可比矩阵 | Jacobian Matrix | 关节速度到末端速度的映射 |
| 奇异位形 | Singularity | 末端失去某些方向运动能力的位形 |
| 工作空间 | Workspace | 末端可达的空间区域 |
| 自由度 | Degrees of Freedom | 独立运动变量的数量 |
| 齐次变换 | Homogeneous Transform | 4x4矩阵表示位姿 |
| 阻抗控制 | Impedance Control | 控制力与位移的动态关系 |
| 导纳控制 | Admittance Control | 输入力输出位移修正 |
| 轨迹规划 | Trajectory Planning | 生成平滑的运动时间函数 |
| 力封闭 | Force Closure | 接触力可抵抗任意外力 |
| 碰撞检测 | Collision Detection | 判断几何体是否相交 |
| 路径规划 | Path Planning | 在障碍物间找到安全路径 |
| 视觉伺服 | Visual Servoing | 基于视觉反馈的运动控制 |
| 手眼标定 | Hand-Eye Calibration | 确定相机与机器人坐标系的变换 |
视觉伺服是感知与控制的桥梁，它将第19课手眼标定获得的视觉信息，与第10课笛卡尔空间控制结合，形成闭环控制系统。视觉伺服的性能取决于：
在智能分拣工作站（第30课毕业项目）中，视觉伺服是实现看到即抓到的核心技术。
掌握基于图像反馈的实时控制！
在实际机器人上实验时，请始终遵循以下安全规则：