📚 课程目录第3章运动学基础

3、运动学基础：刚体运动描述、正运动学与逆运动学、雅可比矩阵及其在速度控制中的应用

运动学是机器人学与控制系统的基石，它研究物体在空间中的位置、姿态、速度与加速度随时间的变化关系，而不涉及产生这些运动的力与力矩。对于运动控制工程师而言，理解刚体运动描述、正逆运动学求解以及雅可比矩阵的构建，是设计高性能轨迹规划、伺服控制与力控算法的前提。本章将系统梳理这些核心概念，并结合工业机器人、AGV（自动导引车）与并联平台等典型场景，剖析从数学建模到工程落地的完整链路。

上图展示了运动学四大模块之间的逻辑关系：刚体运动描述为所有计算提供数学语言，正运动学与逆运动学构成位置层面的双向映射，而雅可比矩阵则进一步将这种映射扩展到速度与力层面。下面我们逐一深入。

3.1 刚体运动描述：位置、姿态与齐次变换

3.1.1 核心原理：从矢量到变换矩阵

刚体在空间中的完整位姿由位置（平移）和姿态（旋转）两部分组成。位置通常用3×1的列向量 p 表示，而姿态则用3×3的旋转矩阵 R 描述。旋转矩阵的列向量是刚体坐标系各轴在参考坐标系中的方向余弦，满足正交性（R^TR = I）和行列式为+1的约束。为了统一处理平移与旋转，工程中广泛采用4×4齐次变换矩阵：

T = [ R   p ]
    [ 0   1 ]

其中左上角3×3为旋转矩阵，右上角3×1为平移矢量，底行为[0 0 0 1]。齐次变换矩阵允许将多次连续变换通过矩阵乘法串联，例如 T₀₃ = T₀₁ · T₁₂ · T₂₃，极大简化了运动学推导。

除了旋转矩阵，工程中还常用欧拉角（如ZYX、ZYZ）、四元数和轴角表示姿态。欧拉角直观但存在万向锁问题；四元数无奇异且插值平滑，适合姿态规划；旋转矩阵适合计算但冗余。实际控制器中，常混合使用：内部计算用旋转矩阵或四元数，人机交互用欧拉角。

经验提醒：在编写运动学库时，务必明确旋转的约定（内旋/外旋、左乘/右乘）。同一组欧拉角在不同约定下对应的姿态完全不同，这是团队协作中最常见的bug来源。

3.1.2 判断方法：选择适合的姿态表示

表示方法	优点	缺点	典型应用
旋转矩阵 R	无奇异，计算稳定	9个参数冗余，有约束	运动学推导、坐标变换
欧拉角 (α,β,γ)	直观，3个参数	万向锁，插值非线性	示教器显示、简单姿态设定
四元数 (x,y,z,w)	无奇异，插值平滑	不直观，需归一化	姿态规划、SLAM、惯性导航
轴角 (axis, θ)	物理意义明确	有奇异（θ=0）	旋转增量计算

3.1.3 案例拆解：六轴机器人基座到末端的位姿链

以典型六轴关节机器人为例，从基座坐标系{0}到末端坐标系{6}的位姿可表示为：

T_06 = T_01(q1) · T_12(q2) · T_23(q3) · T_34(q4) · T_45(q5) · T_56(q6)

每个 T_i-1,i 由关节变量 q_i 和连杆几何参数（DH参数）决定。例如，当 q1=0°, q2=90°, q3=0°, q4=0°, q5=0°, q6=0° 时，末端位置可通过代入DH参数表计算得到。实际调试中，工程师常通过手动示教几个关键位姿，对比理论计算值与编码器反馈值，验证运动学模型的正确性。

风险提示：DH参数存在标准型和改进型两种约定，两者在坐标系建立上不同，导致变换矩阵的乘序和参数含义有差异。混用两种约定会导致末端位置偏差数厘米甚至更大，务必在项目文档中明确标注。

3.2 正运动学与逆运动学

3.2.1 核心原理：双向映射

正运动学（FK）：给定关节角度矢量 q = [q1, q2, ..., qn]^T，求解末端位姿 T。这是一个确定性的映射，计算量小，常用于仿真、可视化以及逆运动学的迭代初值。

逆运动学（IK）：给定期望的末端位姿 T_des，求解满足条件的关节角 q。这是运动控制中最关键也最复杂的问题，因为：

多解性：同一末端位姿可能对应多组关节角（如肘上/肘下、腕部翻转）；
奇异性：在特定位形下，雅可比矩阵降秩，导致关节速度趋于无穷；
冗余性：对于7自由度或更高自由度的机器人，逆解有无穷多组，需要附加优化准则（如最小关节变化、避障）。

求解方法分为两类：解析法（通过代数或几何方法直接求解，适用于满足Pieper准则的6轴机器人）和数值迭代法（如牛顿-拉夫森法、雅可比伪逆法，适用于任意结构）。

3.2.2 操作流程：数值逆运动学求解步骤

给定目标位姿 T_des，设置初始关节角 q₀（通常取上一控制周期的解）；
计算当前正运动学 T_cur = FK(q_cur)；
计算位姿误差 e = log(T_des · T_cur^-1)（李代数映射，包含位置和姿态误差）；
求解关节增量 Δq = J^† · e，其中 J^† 为雅可比伪逆；
更新 q_cur += Δq，并检查误差范数是否小于阈值（如1e-6）；
若迭代次数超过上限或误差发散，则切换初始值重新尝试，或报告奇异。

  工程要点：数值IK的收敛速度与初值密切相关。在连续轨迹控制中，上一周期的解就是极佳的初值，通常1-3次迭代即可收敛。但在大范围跳转或奇异点附近，需要增加阻尼最小二乘（DLS）策略，避免关节速度爆炸。

3.2.3 实战场景：SCARA机器人的平面定位

SCARA机器人具有3个旋转关节（θ1, θ2, θ3）和1个垂直移动关节（d4），其逆运动学可解析求解。设末端在XY平面的目标位置为 (x, y)，则：

cosθ2 = (x² + y² - L1² - L2²) / (2·L1·L2)
θ2 = ±acos(cosθ2)   // 两种解：肘左/肘右
θ1 = atan2(y, x) - atan2(L2·sinθ2, L1 + L2·cosθ2)

实际控制器中，会根据当前关节位置选择最接近的一组解，避免关节大幅跳动。例如，若当前 θ2 为 +30°，而另一组解为 -30°，则选择 +30° 对应的 θ1。这种“最短路径”策略在码垛、分拣等场景中可显著减少循环时间。

3.3 雅可比矩阵及其在速度控制中的应用

3.3.1 核心原理：速度映射与力映射

雅可比矩阵 J(q) 是关节速度到末端空间速度的线性映射：

[ v ]   [ Jv ]
[ ω ] = [ Jω ] · q̇

其中 v 为末端线速度（3×1），ω 为末端角速度（3×1），J 为6×n矩阵（n为关节数）。雅可比矩阵的每一列对应一个关节的单位速度对末端速度的贡献。其关键性质包括：

奇异位形：当 det(J·J^T) = 0 时，机器人失去某个方向的运动能力，此时关节速度理论上趋于无穷；
力映射：关节力矩 τ = J^T · F，其中 F 为末端广义力（力和力矩）。这是力控和阻抗控制的基础；
可操作度：w = sqrt(det(J·J^T)) 用于衡量机器人远离奇异的程度，常用于轨迹优化。

3.3.2 判断方法：奇异位形的识别与规避

奇异类型	典型表现	识别方法	规避策略
边界奇异	机械臂完全展开或折叠	检查关节角是否达到限位	规划时预留安全边界
内部奇异	腕部中心与肩部中心重合	计算 det(J·J^T) 接近0	调整路径点或增加冗余自由度
耦合奇异	多个关节速度同时趋于无穷	分析J的奇异值分解	使用阻尼最小二乘（DLS）

3.3.3 案例拆解：六轴机器人直线轨迹的速度规划

假设要求机器人末端以恒定线速度 v = 0.5 m/s 沿直线从点A运动到点B。控制流程如下：

在笛卡尔空间插补得到一系列中间位姿 T_i；
对每个 T_i 求解逆运动学得到关节角 q_i；
计算雅可比矩阵 J(q_i)，求解关节速度 q̇_i = J^-1 · [v; ω]（若J为方阵且非奇异）；
将 q̇_i 作为速度前馈，叠加到伺服环的PID输出上。

在某次实际调试中，当机械臂经过腕部奇异位形附近时，计算出的 q̇₄ 和 q̇₆ 突然增大到额定值的5倍以上，导致伺服驱动器过流报警。通过引入DLS策略（在J^TJ中加入阻尼项λ²I），关节速度被限制在安全范围内，轨迹精度损失约0.3mm，但系统运行平稳。

风险提示：雅可比矩阵的数值计算对关节角误差敏感。在低速重载场景下，减速器弹性变形会导致实际关节角与编码器读数偏差，进而影响雅可比精度。此时建议在关节端增加力矩传感器，或采用基于模型的补偿。

3.3.4 总结复盘：运动学基础的核心要点

位姿描述是语言：统一使用齐次变换矩阵，明确旋转约定，避免团队协作中的歧义；
正运动学是基础：务必通过实测数据验证DH参数，任何微小的连杆长度误差都会在末端放大；
逆运动学是核心：优先使用解析法（如果存在），数值法作为通用后备；
雅可比是桥梁：它连接了位置层与速度层，也是奇异分析和力控的入口；
工程落地：所有算法必须在实时系统中验证，注意计算耗时、数值稳定性和异常处理。

运动学基础是运动控制算法的“内功”，后续的轨迹规划、动力学控制、视觉伺服等高级主题都建立在这一章的知识之上。建议读者在仿真环境（如MATLAB Robotics Toolbox、CoppeliaSim）中亲手搭建一个六轴机器人模型，完成从DH参数到雅可比矩阵的全流程实现，这将极大加深对本章内容的理解。