直接在关节角度空间进行控制，是最基本的控制方法。
import numpy as np
def dyn_2r(q,qd,tau,L1=1.0,L2=0.8,m1=5.0,m2=3.0,g=9.81):
c2,s2=np.cos(q[1]),np.sin(q[1])
M11=m1*0.25+0.5+m2*(1+0.16+0.64*c2)+0.2; M12=m2*(0.16+0.32*c2)+0.2; M22=m2*0.16+0.2
M=np.array([[M11,M12],[M12,M22]]); h=m2*L1*0.4*s2
C=np.array([[-h*qd[1],-h*(qd[0]+qd[1])],[h*qd[0],0]])
G=np.array([(m1*0.5+m2*L1)*g*np.cos(q[0])+m2*0.4*g*np.cos(q[0]+q[1]),m2*0.4*g*np.cos(q[0]+q[1])])
return np.linalg.solve(M,tau-C@qd-G)
def sim(ctrl,q0,qd0,q_des,dt=0.001,T=3.0):
q=np.array(q0,dtype=float); qd=np.array(qd0,dtype=float)
for i in range(int(T/dt)):
tau=ctrl(q,qd,q_des); qdd=dyn_2r(q,qd,tau); qd+=qdd*dt; q+=qd*dt
return np.linalg.norm(q-np.array(q_des))
# PD控制器
def pd(q,qd,qd_des,Kp=100,Kd=20):
e=np.array(qd_des)-q; return Kp*e+Kd*(-qd)
# 重力补偿PD
def gpd(q,qd,qd_des,Kp=100,Kd=20,g=9.81):
e=np.array(qd_des)-q; tau_pd=Kp*e+Kd*(-qd)
G1=(5*0.5+3*1)*g*np.cos(q[0])+3*0.4*g*np.cos(q[0]+q[1]); G2=3*0.4*g*np.cos(q[0]+q[1])
return tau_pd+np.array([G1,G2])
# CTC
def ctc(q,qd,qd_des,Kp=100,Kd=20,g=9.81):
e=np.array(qd_des)-q; v=Kp*e+Kd*(-qd)
c2=np.cos(q[1]); M=np.array([[2.45+0.64*c2,0.36+0.32*c2],[0.36+0.32*c2,0.36]])
h=0.32*np.sin(q[1]); C=np.array([[-h*qd[1],-h*(qd[0]+qd[1])],[h*qd[0],0]])
G=np.array([(5*0.5+3*1)*g*np.cos(q[0])+3*0.4*g*np.cos(q[0]+q[1]),3*0.4*g*np.cos(q[0]+q[1])])
return M@v+C@qd+G
q_des=[np.pi/4,np.pi/6]
print("关节空间控制仿真对比"); print("="*60)
for name,ctrl in [("PD",pd),("重力补偿PD",gpd),("CTC",ctc)]:
err=sim(ctrl,[0,0],[0,0],q_des)
print(f"{name:12s}: 稳态误差={err:.6f} rad ({np.degrees(err):.3f}°)")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 | 确定相机与机器人坐标系的变换 |
掌握PD、重力补偿PD、CTC三种方法！