机器人运动学与动力学建模基础及实践

1. 机器人空间描述与坐标变换基础

在机器人学中，准确描述物体在三维空间中的位置和姿态是一切运动控制的基础。刚体的空间描述主要包含位置和方位两个要素，而坐标变换则是实现不同坐标系间数据转换的关键数学工具。

1.1 位置描述与坐标系建立

位置描述需要选定参考坐标系，通常采用右手直角坐标系。在基坐标系{A}中，点P的位置可以用3×1的位置矢量ᴬP表示：

ᴬP = [px, py, pz]ᵀ

其中px、py、pz分别表示点P在坐标系{A}的三个坐标轴上的投影。这种表示方法直观清晰，但在实际应用中需要注意：

重要提示：在不同坐标系间进行位置描述转换时，必须明确参考坐标系。忽略参考系会导致严重的定位错误，这是机器人控制中常见的坐标混淆问题。

1.2 方位描述的三大数学工具

刚体的方位描述比位置复杂得多，主要有三种数学表示方法：

1.2.1 旋转矩阵

旋转矩阵是一个3×3的正交矩阵，表示坐标系{B}相对于{A}的方位：

ᴬ[R]ʙ = [ᴬxʙ ᴬyʙ ᴬzʙ]

其中ᴬxʙ、ᴬyʙ、ᴬzʙ是{B}的坐标轴单位向量在{A}中的表示。旋转矩阵具有以下特性：

• 正交性：RᵀR = I
• 行列式为1：det(R) = 1
• 可逆性：R⁻¹ = Rᵀ

1.2.2 欧拉角

欧拉角通过三次连续旋转来描述方位，常见的有ZYX欧拉角（横滚-俯仰-偏航）：
R = Rz(ψ)Ry(θ)Rx(φ)

优点：

• 直观易理解
• 占用存储空间小（仅3个参数）

缺点：

• 存在万向节死锁问题
• 不适用于插值和连续旋转

1.2.3 四元数

四元数q = [q0, q1, q2, q3] = [cos(θ/2), n·sin(θ/2)]，用旋转轴n和旋转角度θ表示方位。

优势：

• 避免万向节死锁
• 计算效率高
• 适合插值运算

实际应用建议：

• 存储用四元数
• 显示用欧拉角
• 计算用旋转矩阵

1.3 位姿描述与齐次变换

刚体的完整位姿（位置+方位）可以用4×4齐次变换矩阵表示：

ᴬ[T]ʙ = [ ᴬ[R]ʙ ᴬPʙᴼʀɪɢ ]
[ 0 1 ]

其中ᴬPʙᴼʀɪɢ是{B}原点在{A}中的位置。齐次变换的优势在于：

• 统一表示平移和旋转
• 可通过矩阵连乘实现复杂变换
• 简化运动学方程推导

2. 机器人运动学建模与实践

2.1 D-H参数法详解

Denavit-Hartenberg(D-H)参数法是建立机器人运动学模型的标准方法，它通过四个参数描述相邻连杆间的几何关系：

1. 连杆长度aᵢ：沿xᵢ轴，zᵢ₋₁到zᵢ的距离
2. 连杆转角αᵢ：绕xᵢ轴，zᵢ₋₁到zᵢ的转角
3. 连杆偏距dᵢ：沿zᵢ₋₁轴，xᵢ₋₁到xᵢ的距离
4. 关节角θᵢ：绕zᵢ₋₁轴，xᵢ₋₁到xᵢ的转角

对于旋转关节，θᵢ是变量；对于平移关节，dᵢ是变量。

2.1.1 标准D-H参数建模步骤

1. 确定各关节轴并标注z轴
2. 建立基坐标系（通常设在关节1）
3. 确定x轴方向（zᵢ和zᵢ₊₁的公垂线）
4. 确定y轴完成右手系
5. 测量并记录四个D-H参数
6. 为末端执行器添加工具坐标系

实践经验：在建立D-H坐标系时，常犯的错误是x轴方向确定不当。一个检查技巧是确保xᵢ垂直于zᵢ₋₁且指向下一个关节。

2.2 正运动学方程推导

相邻连杆间的变换矩阵ⁱ⁻¹[T]ᵢ可根据D-H参数计算：

ⁱ⁻¹[T]ᵢ = Rot(z,θᵢ)Trans(z,dᵢ)Trans(x,aᵢ)Rot(x,αᵢ)

= [cosθᵢ -sinθᵢcosαᵢ sinθᵢsinαᵢ aᵢcosθᵢ]
[sinθᵢ cosθᵢcosαᵢ -cosθᵢsinαᵢ aᵢsinθᵢ]
[0 sinαᵢ cosαᵢ dᵢ ]
[0 0 0 1 ]

机械臂的正运动学方程即为各连杆变换矩阵的连乘：

⁰[T]ⁿ = ⁰[T]¹ ¹[T]² ... ⁿ⁻¹[T]ⁿ

2.3 逆运动学求解策略

逆运动学求解通常比正运动学复杂得多，主要有两种方法：

2.3.1 解析法

适用于具有特定几何特性的机械臂（如6自由度机械臂满足Pieper准则）。典型步骤：

1. 分离变量：利用矩阵等式两边对应元素相等
2. 建立三角函数方程
3. 使用代数或几何方法求解

以PUMA 560机械臂为例，其逆解可通过以下步骤求得：

• 利用腕部位置解出前三个关节
• 利用末端姿态解出后三个关节

2.3.2 数值迭代法

当解析解不可得时，可采用数值方法。最常用的是牛顿-拉夫森法：

1. 初始化关节角度θ₀
2. 计算当前末端位姿f(θₖ)
3. 计算误差e = x_d - f(θₖ)
4. 使用雅可比矩阵更新：θₖ₊₁ = θₖ + J⁺e
5. 重复直到误差小于阈值

避坑指南：数值法对初始值敏感，不良初始值可能导致收敛到局部最优或无法收敛。实践中常结合解析法提供好的初始值。

3. 机器人动力学建模

3.1 拉格朗日动力学方程

拉格朗日法基于能量观点建立动力学方程，避免了牛顿-欧拉法中的内力分析。基本步骤：

1. 计算系统动能K = Σ(½mᵢvᵢᵀvᵢ + ½ωᵢᵀIᵢωᵢ)
2. 计算系统势能P = Σmᵢghᵢ
3. 构建拉格朗日函数L = K - P
4. 应用拉格朗日方程：d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = τᵢ

最终得到的动力学方程形式为：
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ

其中：

• M(q)：n×n质量矩阵（对称正定）
• C(q,q̇)：科里奥利和向心力项
• G(q)：重力项
• τ：关节力矩向量

3.2 动力学参数辨识

实际应用中，精确的动力学参数难以直接测量，通常通过辨识获得。实验设计要点：

1. 设计激励轨迹：应充分激发所有动力学模态
2. 数据采集：记录关节位置、速度和驱动力矩
3. 参数回归：使用最小二乘法等估计参数

常用激励轨迹有：

• 有限傅里叶级数
• 正弦扫频信号
• 随机信号

4. 速度运动学与雅可比矩阵

4.1 雅可比矩阵定义

雅可比矩阵建立了关节速度与末端执行器速度之间的映射关系：

v = J(q)q̇

其中：

• v = [v, ω]ᵀ：末端线速度和角速度
• J(q)：几何雅可比矩阵
• q̇：关节速度向量

对于n自由度机械臂，雅可比矩阵是6×n的矩阵，前3行对应线速度，后3行对应角速度。

4.2 雅可比矩阵构造方法

1. 对于平移关节i：Jᵢ = [zᵢ₋₁, 0]ᵀ
2. 对于旋转关节i：Jᵢ = [zᵢ₋₁ × (p - pᵢ₋₁), zᵢ₋₁]ᵀ

其中：

• zᵢ₋₁：关节i的旋转轴在基坐标系下的表示
• p：末端位置
• pᵢ₋₁：关节i的位置

4.3 奇异位形分析

当雅可比矩阵秩亏时，机械臂处于奇异位形，表现为：

• 某些末端运动方向不可达
• 关节速度趋于无穷大
• 控制精度下降

常见奇异类型：

1. 边界奇异：机械臂完全展开或折叠
2. 内部奇异：由于机构几何特性导致

奇异规避策略：

• 轨迹重规划
• 优化机械结构
• 使用阻尼最小二乘法求逆

5. 机器人控制基础

5.1 关节空间控制

5.1.1 PID控制

u(t) = Kₚe(t) + Kᵢ∫e(t)dt + Kₚde(t)/dt

参数整定技巧：

• 先调Kₚ使系统稳定
• 再调Kᵢ消除稳态误差
• 最后调Kₑ抑制超调

5.1.2 计算力矩控制

基于动力学模型的前馈控制：
τ = M(q)q̈_d + C(q,q̇)q̇_d + G(q) + Kₚe + Kᵢ∫edt

5.2 操作空间控制

将期望轨迹转换到关节空间：
q̈_d = J⁺(ẍ_d - J̇q̇) + (I - J⁺J)q̈₀

其中：

• J⁺：雅可比伪逆
• (I - J⁺J)q̈₀：零空间优化项

5.3 阻抗控制

实现力/位混合控制：
MₑΔẍ + DₑΔẋ + KₑΔx = Fₑₓₜ

参数选择原则：

• Mₑ：影响惯性特性
• Dₑ：决定阻尼特性
• Kₑ：调节刚度特性

6. 实践技巧与常见问题

6.1 运动学建模验证

建立运动学模型后，必须进行验证：

1. 一致性检查：特殊位形下验证
2. 数值验证：与商业软件对比
3. 实验验证：实际测量末端位姿

6.2 动力学仿真要点

进行动力学仿真时需注意：

• 积分步长选择（通常1ms-10ms）
• 摩擦模型选择（库仑+粘滞）
• 执行器饱和限制

6.3 实时控制实现

实现高精度实时控制的关键：

1. 硬实时操作系统（如Xenomai）
2. 高精度时钟（1kHz以上）
3. 优化代码执行时间

6.4 常见故障排查

1. 奇异位形附近振动：

   • 增加阻尼项
   • 轨迹优化

2. 轨迹跟踪误差大：

   • 检查动力学参数
   • 调整控制增益
   • 验证传感器校准

3. 关节力矩波动：

   • 检查传动间隙
   • 优化摩擦补偿
   • 验证电源稳定性

在实际机器人项目中，我深刻体会到理论建模与实际系统的差距。一个经验是：永远预留30%的性能余量应对未建模动态。另外，建立完善的调试日志系统能大幅缩短开发周期——记录所有关节状态、控制指令和传感器读数，这对分析偶发问题至关重要。