简单设目标关节角会导致加速度冲击和振动，需要生成平滑的时间函数。
import numpy as np
def cubic(q0,qf,v0=0,vf=0,T=1.0):
a0=q0; a1=v0; a2=(3*(qf-q0)-(2*v0+vf)*T)/T**2; a3=(-2*(qf-q0)+(v0+vf)*T)/T**3
return a0,a1,a2,a3
def quintic(q0,qf,v0=0,vf=0,a0=0,af=0,T=1.0):
a0c=q0; a1=v0; a2c=a0/2
a3=(20*(qf-q0)-(8*vf+12*v0)*T-(3*a0-af)*T**2)/(2*T**3)
a4=(-30*(qf-q0)+(14*vf+16*v0)*T+(3*a0-2*af)*T**2)/(2*T**4)
a5=(12*(qf-q0)-6*(vf+v0)*T+(af-a0)*T**2)/(2*T**5)
return a0c,a1,a2c,a3,a4,a5
def eval_p(c,t): return sum(ci*t**i for i,ci in enumerate(c))
def eval_pd(c,t): return sum(i*ci*t**(i-1) for i in range(1,len(c)))
def eval_pdd(c,t): return sum(i*(i-1)*ci*t**(i-2) for i in range(2,len(c)))
q0,qf=0.0,np.pi/2; T=2.0; dt=0.01; t=np.arange(0,T+dt,dt)
cc=cubic(q0,qf,T=T); qc=quintic(q0,qf,T=T)
print("轨迹规划对比"); print("="*60)
print(f"起点:{np.degrees(q0):.0f}° 终点:{np.degrees(qf):.0f}° 周期:{T}s")
qd_c=[eval_pd(cc,ti) for ti in t]; qdd_c=[eval_pdd(cc,ti) for ti in t]
qd_q=[eval_pd(qc,ti) for ti in t]; qdd_q=[eval_pdd(qc,ti) for ti in t]
print(f"三次: max|qd|={max(abs(v) for v in qd_c):.4f} max|qdd|={max(abs(a) for a in qdd_c):.4f} 加速度跳变={abs(qdd_c[0]):.4f}")
print(f"五次: max|qd|={max(abs(v) for v in qd_q):.4f} max|qdd|={max(abs(a) for a in qdd_q):.4f} 加速度跳变={abs(qdd_q[0]):.4f}")
# 梯形速度规划
v_max=2.0; a_max=3.0; D=abs(qf-q0); t_acc=v_max/a_max; d_acc=0.5*a_max*t_acc**2
if 2*d_acc>D: t_acc=np.sqrt(D/a_max); t_const=0; v_act=a_max*t_acc
else: t_const=(D-2*d_acc)/v_max; v_act=v_max
T_trap=2*t_acc+t_const
print(f"\n梯形速度规划: Vmax={v_max}, Amax={a_max}")
print(f" 加速时间:{t_acc:.3f}s 匀速时间:{t_const:.3f}s 总时间:{T_trap:.3f}s")import numpy as np
print("动力学参数灵敏度")
L1,L2,m1,m2,lc1,lc2,I1,I2,g=1.,.8,5.,3.,.5,.4,.5,.2,9.81
base_c2=np.cos(np.pi/6); M11=m1*lc1**2+I1+m2*(L1**2+lc2**2+2*L1*lc2*base_c2)+I2
for param,delta in [("m1",0.1),("m2",0.1),("L1",0.01),("lc1",0.01)]:
vals={k:v for k,v in zip(["m1","m2","L1","lc1"],[m1,m2,L1,lc1])}
vals[param]*=(1+delta); c2=base_c2
M11p=vals["m1"]*0.25+0.5+vals["m2"]*(vals["L1"]**2+0.16+2*vals["L1"]*0.4*c2)+0.2
print(f" {param}+{delta:.0%}: M11变化{(M11p-M11)/M11*100:.2f}%")参数灵敏度分析帮助确定哪些参数需要精确辨识。连杆长度对惯性矩阵影响最大。
本课程按"运动学→动力学→力控制→感知规划→实战项目"组织，每课都建立在前面知识基础上。理解本课后，你将：
完成本课后，你应该能够：
完成练习后，继续下一课的学习。本课内容将在后续课程中被反复使用和扩展。
牛顿-欧拉法的第一步递推（正向）从基座向末端传播运动量：
第二步递推（反向）从末端向基座传播力：
虽然拉格朗日法推导复杂，但它有独特优势：
实际关节的摩擦力远比简单阻尼模型复杂，常用模型包括：
摩擦补偿是高精度运动控制的必要环节。
本课涉及的技术在以下场景中有重要应用：
理解理论与实践的差距，是成为优秀机器人工程师的关键。
仿真验证的算法通常需要以下适配才能部署到实际机器人：参数标定、实时性优化、异常处理、安全保护。仿真与现实的差距（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 | 确定相机与机器人坐标系的变换 |
掌握多项式插值和梯形规划！