解析解仅适用于特殊构型，数值方法可处理任意构型。
import numpy as np
def fk_2r(q,L1=1.0,L2=0.8):
return np.array([L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L1*np.sin(q[0])+L2*np.sin(q[0]+q[1])])
def jac_2r(q,L1=1.0,L2=0.8):
return np.array([[-L1*np.sin(q[0])-L2*np.sin(q[0]+q[1]),-L2*np.sin(q[0]+q[1])],
[L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L2*np.cos(q[0]+q[1])]])
target=np.array([1.2,0.8]); q=np.array([0.5,0.3])
print("数值逆运动学求解"); print("="*55)
for method,lam in [('pinv',0),('damped',0.01),('transpose',0)]:
q_t=q.copy(); hist=[]
for i in range(300):
e=target-fk_2r(q_t); en=np.linalg.norm(e); hist.append(en)
if en<1e-6: break
J=jac_2r(q_t)
if method=='pinv': Jinv=np.linalg.pinv(J)
elif method=='damped': JJT=J@J.T; Jinv=J.T@np.linalg.inv(JJT+lam**2*np.eye(2))
else: Jinv=J.T/(np.linalg.norm(J)**2+1e-10)
q_t+=Jinv@e
pc=fk_2r(q_t); err=np.linalg.norm(pc-target)
print(f"{method:10s}: iter={i+1}, err={err:.2e}, θ=({np.degrees(q_t[0]):.1f}°,{np.degrees(q_t[1]):.1f}°)")| 方法 | 收敛速度 | 稳定性 | 奇异处理 |
|---|---|---|---|
| 伪逆法 | 快 | 差 | 差 |
| 阻尼最小二乘 | 较快 | 好 | 好 |
| 雅可比转置 | 慢 | 好 | 好 |
| Levenberg-Marquardt | 自适应 | 极好 | 好 |
import numpy as np
def fk(q): return np.array([np.cos(q[0])+.8*np.cos(q[0]+q[1]),np.sin(q[0])+.8*np.sin(q[0]+q[1])])
def jac(q): return np.array([[-np.sin(q[0])-.8*np.sin(q[0]+q[1]),-.8*np.sin(q[0]+q[1])],[np.cos(q[0])+.8*np.cos(q[0]+q[1]),.8*np.cos(q[0]+q[1])]])
target=np.array([1.2,0.8])
for lam in [0.001,0.01,0.1,0.5]:
q=np.array([1.,1.])
for i in range(200):
e=target-fk(q)
if np.linalg.norm(e)<1e-6: break
J=jac(q); JJT=J@J.T; Jinv=J.T@np.linalg.inv(JJT+lam**2*np.eye(2)); q+=Jinv@e
print(f"λ={lam:.3f}: iter={i+1}, err={np.linalg.norm(e):.2e}")阻尼系数λ的选择至关重要：太小接近伪逆（奇异不稳定），太大收敛慢。实践中常使用自适应阻尼。
本课程按"运动学→动力学→力控制→感知规划→实战项目"组织，每课都建立在前面知识基础上。理解本课后，你将：
完成本课后，你应该能够：
完成练习后，继续下一课的学习。本课内容将在后续课程中被反复使用和扩展。
齐次变换矩阵是描述三维空间中刚体位姿的数学工具。一个4×4的齐次变换矩阵T同时包含了旋转信息（3×3旋转矩阵R）和平移信息（3×1位置向量p）：
旋转矩阵R是正交矩阵（RᵀR=I，det(R)=1），有3个自由度；位置向量p有3个自由度。因此一个刚体在三维空间中有6个自由度。
旋转有多种等价表示：
DH参数法为每个连杆建立一个坐标系，建立规则如下：
当相邻z轴平行或相交时，需要特殊处理。
实际机器人的运动学参数与设计值有偏差，需要通过运动学标定来补偿。标定流程：
标定后定位精度可从毫米级提升到0.1mm级别。
本课涉及的技术在以下场景中有重要应用：
理解理论与实践的差距，是成为优秀机器人工程师的关键。
仿真验证的算法通常需要以下适配才能部署到实际机器人：参数标定、实时性优化、异常处理、安全保护。仿真与现实的差距（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 | 确定相机与机器人坐标系的变换 |
掌握伪逆、阻尼等数值方法，处理任意构型！