第10课 - 笛卡尔空间控制

Python

import numpy as np
def fk(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(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])]])
def dyn(q,qd,tau):
    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)*9.81*np.cos(q[0])+3*0.4*9.81*np.cos(q[0]+q[1]),3*0.4*9.81*np.cos(q[0]+q[1])])
    return np.linalg.solve(M,tau-C@qd-G)
x_des=np.array([1.2,0.8]); q=np.array([0.0,0.0]); qd=np.zeros(2); dt=0.001
print("笛卡尔空间控制仿真"); print("="*60)
for method in ['transpose','inverse','ctc']:
    q_t=q.copy(); qd_t=qd.copy()
    for i in range(int(4.0/dt)):
        x=fk(q_t); J=jac(q_t); ex=x_des-x; ev=-J@qd_t; F=200*ex+40*ev
        if method=='transpose': tau=J.T@F
        elif method=='inverse': tau=J.T@np.linalg.solve(J@J.T+0.01*np.eye(2),F)
        else:
            Jinv=np.linalg.pinv(J); aq=Jinv@(200*ex+40*ev)
            c2=np.cos(q_t[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_t[1]); C=np.array([[-h*qd_t[1],-h*(qd_t[0]+qd_t[1])],[h*qd_t[0],0]])
            G=np.array([(5*0.5+3*1)*9.81*np.cos(q_t[0])+3*0.4*9.81*np.cos(q_t[0]+q_t[1]),3*0.4*9.81*np.cos(q_t[0]+q_t[1])])
            tau=M@aq+C@qd_t+G
        if method!='ctc':
            G2=np.array([(5*0.5+3*1)*9.81*np.cos(q_t[0])+3*0.4*9.81*np.cos(q_t[0]+q_t[1]),3*0.4*9.81*np.cos(q_t[0]+q_t[1])])
            tau+=G2
        qdd=dyn(q_t,qd_t,tau); qd_t+=qdd*dt; q_t+=qd_t*dt
    err=np.linalg.norm(fk(q_t)-x_des)
    print(f"{method:10s}: 位置误差={err:.6f}m")

运行结果

🔬 工程实践要点

动力学实施注意事项

参数辨识：实际机器人的质量、质心、惯量参数难以精确获取，需要通过运动激励和最小二乘辨识
实时计算：逆动力学需要1kHz计算，Newton-Euler递推比Lagrange更高效（O(n) vs O(n³)）
摩擦补偿：关节摩擦（Coulomb + 粘性）是最大的未建模项，需要专门的摩擦模型
柔性关节：谐波减速器的柔性会引入谐振，需要弹性关节模型
重力补偿精度：5%的重力补偿误差可能导致数毫米的位置偏差

📖 延伸阅读

🔬 扩展实验：参数灵敏度分析

Python

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}%")

参数灵敏度分析帮助确定哪些参数需要精确辨识。连杆长度对惯性矩阵影响最大。

💡 知识图谱

本课在知识体系中的位置

本课程按"运动学→动力学→力控制→感知规划→实战项目"组织，每课都建立在前面知识基础上。理解本课后，你将：

深入理解机械臂系统的核心原理
独立实现和验证关键算法
为后续课程打下坚实基础
将理论应用到实际工程项目

🎯 学习目标回顾

📋 本课小结

📚 动力学理论基础

牛顿-欧拉递推的物理意义

牛顿-欧拉法的第一步递推（正向）从基座向末端传播运动量：

每个连杆的速度和加速度由前一个连杆的运动加上当前关节的运动确定
科里奥利加速度和离心加速度是运动耦合的体现
重力加速度作为基座加速度加入递推

第二步递推（反向）从末端向基座传播力：

每个连杆的惯性力等于质量乘加速度
关节力矩由力矩平衡方程确定
相邻连杆间的力传递遵循牛顿第三定律

拉格朗日法的优势

虽然拉格朗日法推导复杂，但它有独特优势：

自动获得惯性矩阵M(q)的解析表达式
斜对称性Ṁ-2C可以直接验证
便于设计基于能量的控制器
适合符号推导和参数辨识

关节摩擦模型

实际关节的摩擦力远比简单阻尼模型复杂，常用模型包括：

Coulomb摩擦：F = μ·N·sign(ω)，与速度方向有关
粘性摩擦：F = b·ω，与速度成正比
Stribeck效应：低速时摩擦力下降
LuGre模型：统一的动态摩擦模型

摩擦补偿是高精度运动控制的必要环节。

🏭 工业应用实例

本课知识在实际工程中的应用

本课涉及的技术在以下场景中有重要应用：

操作空间控制、笛卡尔刚度调节、多任务优先级控制、零空间控制——核心算法的直接应用
不同应用场景对算法精度、速度和鲁棒性有不同要求
工业实践中通常需要在理想模型和实际约束之间做权衡
从实验室仿真到工业部署需要大量的工程化工作

理解理论与实践的差距，是成为优秀机器人工程师的关键。

❓ 常见问题解答

Q1: 本课的算法在实际机器人上能直接使用吗？

仿真验证的算法通常需要以下适配才能部署到实际机器人：参数标定、实时性优化、异常处理、安全保护。仿真与现实的差距（sim-to-real gap）是机器人领域的重要挑战。

Q2: Python实现的效率够用吗？

Python适合算法验证和原型开发，但生产级控制器通常用C++实现。Python的NumPy/SciPy计算效率约为C++的1/10到1/100，对于1kHz控制频率可能不够。可以使用Cython、Numba或直接C++重写来加速。

Q3: 如何从仿真过渡到实际机器人？

推荐步骤：(1) Python仿真验证算法正确性；(2) 添加传感器噪声和延迟模型测试鲁棒性；(3) 用C++重写核心计算模块；(4) 在低速度下实际测试；(5) 逐步提高速度和负载。安全永远是第一位的。

🐛 调试技巧

本课代码的常见问题和调试方法

数值不稳定：检查除零、sqrt负数、矩阵奇异等情况，添加小的正则化项
IK不收敛：尝试不同初始值，增大阻尼系数，检查目标可达性
仿真发散：减小时间步长，检查控制器增益，添加速度限幅
结果异常：用简单情况验证（如θ=0、θ=π/2等），对比解析解
性能问题：避免在循环中创建数组，预分配内存，向量化计算

📖 术语表

本课关键术语

术语	英文	定义
正运动学	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	确定相机与机器人坐标系的变换

笛卡尔空间控制

🌐 从关节到笛卡尔

📐 笛卡尔控制律

💻 笛卡尔空间控制器仿真