运动学是机器人控制的基石，它建立了关节空间与任务空间之间的映射关系。对于轮足机械臂这样的复杂系统，运动学不仅决定了机器人能够到达的位置，更影响着运动的灵活性、奇异性规避以及多任务协调能力。本章将深入探讨正逆运动学的求解方法，重点关注工程实践中的数值稳定性、计算效率和冗余度利用，为后续的动力学控制和智能规划奠定基础。
轮足机械臂系统的运动学链通常包含三个主要部分：移动底盘、腿部机构和机械臂。每个部分都有其独特的运动学特性：
完整的运动学链可以表示为：
\[\mathbf{T}_{base}^{ee} = \mathbf{T}_{world}^{base} \cdot \mathbf{T}_{base}^{leg} \cdot \mathbf{T}_{leg}^{arm} \cdot \mathbf{T}_{arm}^{ee}\]其中$\mathbf{T}$表示齐次变换矩阵，下标表示坐标系，上标表示变换目标。
在实际工程中，坐标系的选择至关重要。不当的坐标系定义会导致控制算法复杂化，甚至引入不必要的数值问题。轮足机器人的坐标系层次通常包括：
坐标系间的相对关系需要实时更新，特别是在不平整地形上运动时。传感器融合算法（如扩展卡尔曼滤波）用于估计$\mathbf{T}_{world}^{base}$，而编码器直接提供关节角度用于计算其他变换。
尽管Product of Exponentials (PoE)方法在理论上更优雅，但DH参数因其标准化和直观性在工业界仍广泛使用。对于第$i$个关节，DH变换矩阵为：
\[\mathbf{A}_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}\]其中：
DH参数的选择并非唯一，存在标准DH（Standard DH）和修正DH（Modified DH）两种约定。标准DH将坐标系原点放在当前关节轴上，而修正DH则放在前一关节轴上。这种差异看似细微，但会影响雅可比矩阵的计算和奇异性分析。
DH参数标定的实践考虑：
实际机器人的DH参数与理论设计值存在偏差，原因包括：
参数标定通常采用以下方法：
标定误差模型： \(\Delta \mathbf{x} = \mathbf{J}_{error} \Delta \boldsymbol{\phi}\)
其中$\Delta \boldsymbol{\phi}$是参数误差向量，$\mathbf{J}_{error}$是误差雅可比矩阵。通过最小二乘法可以辨识参数偏差： \(\Delta \boldsymbol{\phi} = (\mathbf{J}_{error}^T \mathbf{J}_{error})^{-1} \mathbf{J}_{error}^T \Delta \mathbf{x}\)
轮足机械臂的正运动学需要处理几个特殊问题：
机械臂末端
|
[机械臂关节链]
|
腿部平台
/ \
左腿链 右腿链
| |
左轮 右轮
\ /
地面
浮动基座的处理：
传统固定基座机器人的运动学从基座开始，而轮足机器人的基座本身在运动。这需要引入额外的状态估计：
\[\mathbf{T}_{world}^{base}(t) = \begin{bmatrix} \mathbf{R}(t) & \mathbf{p}(t) \\ \mathbf{0} & 1 \end{bmatrix}\]其中$\mathbf{R}(t) \in SO(3)$是基座姿态，$\mathbf{p}(t) \in \mathbb{R}^3$是基座位置。这些量通过以下方式估计：
接触约束的建模：
当腿部与地面接触时，形成运动学闭链。设接触点位置为$\mathbf{c}_i$，约束方程为：
\[\mathbf{T}_{world}^{base} \cdot \mathbf{T}_{base}^{foot_i}(\mathbf{q}_{leg}) = \mathbf{T}_{world}^{contact_i}\]这引入了$3m$个约束（$m$是接触点数），减少了系统的有效自由度。在运动规划时必须确保约束的满足：
\[\mathbf{J}_{contact} \dot{\mathbf{q}} = \mathbf{0}\]模式切换的连续性：
轮足机器人在不同运动模式间切换（如从轮式到足式），运动学模型会发生突变。平滑过渡策略包括：
对于特定的机械臂构型（如球形腕、拟人臂），可以推导闭式解析解。以6自由度机械臂为例，常用的解析方法包括：
几何法：利用机械臂的几何特征分解问题
代数法：通过矩阵方程系统求解
解析解的优势在于计算速度快、精度高，但仅适用于特定构型。大多数冗余机械臂没有闭式解。
Pieper准则：
对于6自由度串联机械臂，存在闭式解的充分条件是满足Pieper准则之一：
大多数工业机器人采用球形腕设计正是为了保证解析解的存在性。对于满足Pieper准则的机械臂，求解步骤为：
步骤1：腕心位置求解 \(\mathbf{p}_{wrist} = \mathbf{p}_{desired} - d_6 \mathbf{R}_{desired} \mathbf{z}_0\)
其中$d_6$是腕心到末端的距离，$\mathbf{z}_0 = [0, 0, 1]^T$。
步骤2：前三关节求解
利用腕心位置的几何约束： \(|\mathbf{p}_{wrist}|^2 = (a_2 \cos\theta_2 + a_3)^2 + (a_2 \sin\theta_2 + d_4)^2\)
可得$\theta_2$的解： \(\cos\theta_2 = \frac{|\mathbf{p}_{wrist}|^2 - a_2^2 - a_3^2 - d_4^2}{2a_2\sqrt{a_3^2 + d_4^2}}\)
步骤3：后三关节求解
利用姿态约束： \(\mathbf{R}_3^6(\theta_4, \theta_5, \theta_6) = (\mathbf{R}_0^3)^T \mathbf{R}_{desired}\)
通过欧拉角分解可得$\theta_4, \theta_5, \theta_6$。
对于一般构型和冗余机械臂，数值方法是主要选择：
牛顿-拉夫逊法： \(\Delta\mathbf{q} = \mathbf{J}^{-1}(\mathbf{q}) \cdot \Delta\mathbf{x}\)
其中$\mathbf{J}$是雅可比矩阵，$\Delta\mathbf{x}$是笛卡尔空间误差，$\Delta\mathbf{q}$是关节空间增量。
阻尼最小二乘法（DLS）： \(\Delta\mathbf{q} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda^2\mathbf{I})^{-1} \cdot \Delta\mathbf{x}\)
阻尼因子$\lambda$提供数值稳定性，特别是在奇异点附近。自适应阻尼策略：
\[\lambda = \begin{cases} 0 & \text{if } \sigma_{min} > \epsilon \\ \lambda_0 \sqrt{1 - (\sigma_{min}/\epsilon)^2} & \text{otherwise} \end{cases}\]其中$\sigma_{min}$是雅可比矩阵的最小奇异值。
改进的数值方法：
其中$\mu$根据收敛情况动态调整：
BFGS拟牛顿法： 避免直接计算雅可比矩阵，通过迭代更新Hessian近似： \(\mathbf{H}_{k+1} = \mathbf{H}_k + \frac{\mathbf{y}\mathbf{y}^T}{\mathbf{y}^T\mathbf{s}} - \frac{\mathbf{H}_k\mathbf{s}\mathbf{s}^T\mathbf{H}_k}{\mathbf{s}^T\mathbf{H}_k\mathbf{s}}\)
信赖域方法： 限制每步的移动范围，提高鲁棒性： \(\min_{\Delta\mathbf{q}} ||\mathbf{J}\Delta\mathbf{q} + \mathbf{e}||^2 \quad \text{s.t.} \quad ||\Delta\mathbf{q}|| \leq \delta\)
收敛性分析：
数值方法的收敛速度取决于多个因素：
典型的收敛准则组合： \(||\mathbf{e}|| < \epsilon_{pos} \quad \text{且} \quad ||\Delta\mathbf{q}|| < \epsilon_{joint} \quad \text{且} \quad k < k_{max}\)
逆运动学通常有多个解（如肘部上下、腕部翻转等）。选择策略包括：
| 最小关节运动：$\min | \mathbf{q} - \mathbf{q}_{current} | $ |
多解的几何解释：
对于典型的6-DOF机械臂，最多可能有16个解：
总计：$2 \times 2 \times 2 \times 2 = 16$个可能解。
解的连续性管理：
在连续运动中，解的选择必须保持一致性，避免关节突变：
解分支跟踪算法：
1. 计算所有可能解 {q_1, q_2, ..., q_n}
2. 对每个解计算连续性度量：
c_i = exp(-||q_i - q_prev||/σ)
3. 选择得分最高的解：
q_selected = argmax(c_i · w_i)
其中w_i是任务相关权重
全局最优解搜索：
对于关键任务，可以通过优化方法寻找全局最优：
\[\mathbf{q}^* = \arg\min_{\mathbf{q}} \left( w_1 f_{joint}(\mathbf{q}) + w_2 f_{limit}(\mathbf{q}) + w_3 f_{manip}(\mathbf{q}) + w_4 f_{energy}(\mathbf{q}) \right)\]其中：
解的可达性预测：
在开始IK求解前，快速判断目标是否可达：
| 距离检查：$ | \mathbf{p}_{target} | \leq r_{max}$ |
雅可比矩阵建立了关节速度与末端速度的线性映射：
\[\begin{bmatrix} \mathbf{v} \\ \boldsymbol{\omega} \end{bmatrix} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}\]对于旋转关节$i$，雅可比列向量为： \(\mathbf{J}_i = \begin{bmatrix} \mathbf{z}_{i-1} \times (\mathbf{p}_n - \mathbf{p}_{i-1}) \\ \mathbf{z}_{i-1} \end{bmatrix}\)
对于移动关节$i$： \(\mathbf{J}_i = \begin{bmatrix} \mathbf{z}_{i-1} \\ \mathbf{0} \end{bmatrix}\)
奇异性发生在雅可比矩阵秩降低时，导致某些方向失去可控性。奇异性类型：
奇异性检测指标：
实践中的奇异性处理方法：
奇异性规避流程：
┌─────────────┐
│ 目标位姿 │
└──────┬──────┘
↓
┌─────────────┐
│ 奇异性检测 │ ← 计算条件数
└──────┬──────┘
↓
危险？ ──Yes──→ ┌─────────────┐
↓ │ 路径修正 │
No └─────────────┘
↓
┌─────────────┐
│ 正常IK求解 │
└─────────────┘
对于冗余机械臂（关节数>任务维度），存在不影响末端位姿的内部运动。零空间投影算子：
\[\mathbf{N} = \mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J}\]其中$\mathbf{J}^{\dagger}$是伪逆。完整的运动控制律：
\[\dot{\mathbf{q}} = \mathbf{J}^{\dagger}\dot{\mathbf{x}} + \mathbf{N}\dot{\mathbf{q}}_0\]第二项$\mathbf{N}\dot{\mathbf{q}}_0$在零空间内优化次要目标。
多任务场景下，按优先级分配自由度：
严格优先级： \(\dot{\mathbf{q}} = \sum_{i=1}^{n} \mathbf{N}_{i-1}\mathbf{J}_i^{\dagger}(\dot{\mathbf{x}}_i - \mathbf{J}_i\sum_{j=1}^{i-1}\mathbf{N}_{j-1}\mathbf{J}_j^{\dagger}\dot{\mathbf{x}}_j)\)
其中$\mathbf{N}i = \mathbf{N}{i-1} - \mathbf{J}i^{\dagger}\mathbf{J}_i\mathbf{N}{i-1}$是递归零空间。
软优先级（加权）： \(\min_{\dot{\mathbf{q}}} \sum_{i} w_i ||\mathbf{J}_i\dot{\mathbf{q}} - \dot{\mathbf{x}}_i||^2\)
通过二次规划(QP)求解，允许任务间的妥协。
零空间可用于优化各种次要目标：
避障： \(\dot{\mathbf{q}}_0 = k_{obs} \nabla_{\mathbf{q}} \sum_{i} \frac{1}{d_i^2}\) 其中$d_i$是到障碍物的距离。
关节极限规避： \(\dot{\mathbf{q}}_0 = -k_{lim} \nabla_{\mathbf{q}} H(\mathbf{q})\) 其中$H(\mathbf{q}) = \sum_{i} \frac{(q_i - \bar{q}i)^2}{(q{i,max} - q_{i,min})^2}$
操作性最大化： \(\dot{\mathbf{q}}_0 = k_{man} \nabla_{\mathbf{q}} \sqrt{\det(\mathbf{J}\mathbf{J}^T)}\)
可达空间：末端执行器能够到达的所有位置集合
灵巧空间：末端能以任意姿态到达的位置集合
轮足平台的独特优势在于可以通过移动底盘动态扩展工作空间：
\[W_{total} = W_{arm} \oplus W_{base}\]其中$\oplus$表示闵可夫斯基和。
机械臂设计阶段的参数优化：
目标函数：
设计变量：
轮足系统可以通过协调运动扩展工作空间：
静态工作空间 → 移动扩展 → 动态工作空间
2m³ +3m² 覆盖整个房间
规划策略：
Franka Emika Panda是协作机器人的典范，其7-DOF设计体现了冗余度在实际应用中的价值。
DH参数表（简化）： | Joint | a(m) | d(m) | α(rad) | θ range(rad) | |——-|——|——|——–|————–| | 1 | 0 | 0.333 | 0 | ±2.90 | | 2 | 0 | 0 | -π/2 | ±1.76 | | 3 | 0 | 0.316 | π/2 | ±2.90 | | 4 | 0.0825 | 0 | π/2 | ±3.07 | | 5 | -0.0825 | 0.384 | -π/2 | ±2.90 | | 6 | 0 | 0 | π/2 | ±1.66 | | 7 | 0.088 | 0.107 | π/2 | ±2.90 |
Panda利用冗余度实现多种功能：
Panda的逆运动学求解采用混合策略：
// 伪代码
Solution computeIK(Pose target, Config current) {
// 1. 快速解析解（忽略冗余度）
vector<Config> analytical_solutions = analyticalIK(target);
// 2. 冗余度优化
Config best = selectBest(analytical_solutions, current);
// 3. 零空间调整
best = optimizeNullspace(best, secondary_tasks);
// 4. 数值精修
best = numericalRefinement(best, target);
return best;
}
性能指标：
处理多个任务约束时，增广雅可比提供统一框架：
\[\begin{bmatrix} \dot{\mathbf{x}}_1 \\ \dot{\mathbf{x}}_2 \\ \vdots \end{bmatrix} = \begin{bmatrix} \mathbf{J}_1 \\ \mathbf{J}_2 \\ \vdots \end{bmatrix} \dot{\mathbf{q}}\]但直接求伪逆会导致任务冲突时的不可预测行为。
严格的优先级保证通过递归投影实现：
def hierarchical_ik(tasks, q_current):
q_dot = zeros(n_joints)
N = eye(n_joints) # 初始零空间为全空间
for task in tasks:
J_task = task.jacobian(q_current)
x_dot_task = task.desired_velocity()
# 投影到当前可用零空间
J_proj = J_task @ N
# 计算该任务的贡献
q_dot_task = pinv(J_proj) @ (x_dot_task - J_task @ q_dot)
q_dot += N @ q_dot_task
# 更新零空间
N = N @ (eye(n_joints) - pinv(J_proj) @ J_proj)
return q_dot
任务切换时的平滑过渡：
\[\mathbf{N}(t) = (1-s(t))\mathbf{N}_1 + s(t)\mathbf{N}_2\]其中$s(t)$是光滑过渡函数（如5次多项式）。
基于任务完成度和系统状态动态调整优先级：
\[w_i(t) = \begin{cases} w_{i,max} & \text{if } e_i > e_{critical} \\ w_{i,max} \cdot \frac{e_i}{e_{critical}} & \text{otherwise} \end{cases}\]问题：浮点误差累积导致运动学链末端偏差
# 错误示例
T = eye(4)
for i in range(n_joints):
T = T @ compute_transform(theta[i]) # 误差累积
解决：定期重新计算完整变换，使用高精度库
问题：接近奇异点时微小输入产生巨大输出
# 危险操作
q_dot = inv(J) @ x_dot # 当det(J)→0时爆炸
解决：使用阻尼伪逆或切换到缩减任务空间
问题：IK求解器在相似输入下返回不同解，导致关节跳变
解决：
问题：IK只考虑位置，导致不可实现的运动
解决：在IK中集成动力学约束： \(\dot{q}_{min} \leq \dot{\mathbf{q}} \leq \dot{q}_{max}\)
问题：目标超出工作空间时IK发散或振荡
解决：
问题：复杂IK算法无法满足控制周期要求
解决：
本章系统介绍了轮足机械臂的运动学理论与工程实践。关键要点包括：
掌握这些概念和方法是实现高性能机器人控制的前提。下一章将在运动学基础上引入动力学，探讨力/力矩的传递与控制。
题6.1 给定3-DOF平面机械臂，连杆长度分别为$l_1=0.5m$, $l_2=0.4m$, $l_3=0.3m$。计算末端到达点$(0.8, 0.6)$的所有可能关节配置。
Hint: 利用几何关系，先确定第三关节的可能位置
题6.2 推导2-DOF机械臂的雅可比矩阵，并分析其奇异构型。
Hint: 计算末端位置对关节角的偏导数
题6.3 某6-DOF机械臂采用解析IK，平均求解时间0.5ms。若切换到迭代方法（5次迭代收敛），每次迭代0.2ms，分析两种方法的适用场景。
Hint: 考虑精度要求、实时性、开发复杂度
题6.4 设计7-DOF机械臂的零空间运动策略，同时优化：(1)避开障碍物 (2)远离关节极限 (3)最大化操作度。给出统一的优化框架。
Hint: 构建加权目标函数，投影到零空间
题6.5 轮足机械臂需要抓取地面上1.5m范围内的物体。机械臂工作半径1.2m，设计基座运动策略使得：(1)最小化基座运动 (2)最大化抓取成功率。
Hint: 考虑工作空间交集和操作度分布
题6.6 某机械臂在装配任务中需要同时满足：位置精度0.1mm、力控制5N精度、避免奇异点（条件数<100）。设计一个统一的QP控制框架。
Hint: 将所有约束转化为线性不等式
题6.7（开放题） 下一代轮足机械臂将集成变刚度执行器。分析这对运动学控制的影响，提出相应的控制架构修改。
Hint: 考虑刚度对精度、稳定性、安全性的影响
题6.8（思考题） 讨论群机器人协作场景下的运动学协调问题。多个轮足机械臂如何协同搬运大型物体？
Hint: 考虑闭链约束、负载分配、通信延迟