基于逐步几何计算的受力笼中物体姿态估计方法及其在机器人柔顺装配中的应用

《IEEE Robotics and Automation Letters》：Estimation of the Caged Object's Posture under Forces Using Stepwise Geometric Calculations

为解决精密装配中力控需求高、错位导致变形等问题，研究人员开展了笼中物体在受力下姿态估计的研究。通过将物体建模为长方体组合、笼结构表示为点集，提出基于几何计算的步进式姿态估计算法，实现了毫米级间隙下的快速姿态模拟。结果表明该方法与理论解一致，计算时间低于0.6秒，为机器人柔顺装配提供了新思路。

　　

在机器人装配领域，针孔装配过程往往需要极高的精度要求。传统方法依赖精确对齐或复杂的柔顺机制，但即使微小错位也可能导致部件变形或内部应力集中。特别是在使用低自由度机械臂时，缺乏主动力控能力使得装配过程更加困难。有没有一种方法能够在不依赖力控的情况下，自动校正相对位姿，同时避免错位引起的变形呢？

日本东京大学的研究团队提出了一种创新解决方案——利用笼约束（Caging）技术来实现物体的柔顺连接。笼约束是一种特殊的抓取方式，通过几何约束将物体包围在机械手指或类似装置中，仅需位置控制而无需力控。当物体被笼约束时，其位置和姿态并未被完全固定，而是在一定范围内可调。这项研究的关键创新在于：当笼中物体与其他物体接触时，如何准确预测其在反作用力下的姿态变化。

研究团队开发了一套基于几何计算的步进式算法，将三维笼中物体在受力下的姿态变化近似为围绕接触点的旋转运动。该方法将物体表示为长方体元素的组合，笼结构建模为点集，通过计算点与平面之间的几何交集来预测物体运动轨迹。特别值得注意的是，该方法即使在存在微小间隙的情况下也能稳定计算，避免了传统网格离散化方法可能出现的收敛问题。

主要技术方法包括三个核心部分：首先，通过平移运动计算初始接触点T₁；其次，基于旋转运动依次确定后续接触点T₂和T₃；最后，引入考虑表面法向量与外力夹角的滑移判断机制，准确模拟物体在笼中的复杂运动行为。

物体表示与约束条件

研究将物体（Obj）近似为多个长方体碰撞体（Col_i）的组合，每个碰撞体具有宽度w_i、高度h_i和深度d_i等参数。笼结构被表示为多个点C_i，外力f作用于物体上的点^op_f。该方法假设物体的姿态变化足够小（各轴欧拉角在45度以内），从而简化计算复杂度。

基于几何变换的姿态近似

根据接触点数量的不同，研究将物体运动分为三种情况：无接触点时物体沿外力方向平移；单接触点时物体绕接触点旋转；双接触点时物体绕两接触点连线旋转。旋转方向由力矩方向决定，通过向量叉积计算旋转轴方向向量a₁和a₂。

稳定状态判定与滑移处理

当存在三个及以上接触点时，算法通过检查所有可能的旋转轴是否与笼结构发生干涉来判断稳定性。对于滑移情况，根据接触点表面法向量n_i与外力f的夹角α_i进行分类：当α_i小于阈值θ_th时为无滑移点，反之为滑移点。针对不同数量的线性无关法向量，分别采用旋转或平移运动来模拟物体行为。

仿真框架实现

研究团队在MATLAB中实现了完整的仿真系统，包含重力下的初始姿态确定和受力下的姿态变化计算两个主要步骤。通过算法1的平移运动计算和算法2的旋转运动计算，准确捕捉了物体与笼结构的接触过程。

方法验证

通过简单物体的测试案例，验证了算法的准确性。如图7所示，物体在重力作用下的运动轨迹与几何理论解完全一致：平移运动使物体沿z轴移动-5毫米，旋转运动分别产生28.96度和51.32度的角度变化，与理论计算值吻合。

笼约束物体验证

研究进一步将方法应用于模块化轨道结构ManipuRailer的连接机制。通过设置0.5毫米的间隙创建笼约束条件，模拟了轨道结构在俯仰和偏航方向上的姿态校正过程。

在俯仰角实验中，当轨道滑动机构初始俯仰角θ_roller=0.1度时，轨道结构的最终俯仰角θ_rail趋近于0度，变化量-0.560度与理论值Δθ_dif=0.626度基本一致。计算时间仅0.554秒，证明了方法的实用性。

在偏航角实验中，虽然物体行为受到滑移的显著影响，但仿真结果仍显示偏航角φ_rail从0.1度减小到0.014度，趋近于零。同时，滚转角ψ_rail变化为-0.44度，表明该方法能够捕捉多方向耦合的姿态变化，这是传统二维几何计算无法实现的。

这项研究的意义在于为机器人柔顺装配提供了一种全新的理论基础和实用工具。通过几何计算替代复杂的力学仿真，该方法能够在秒级时间内完成毫米级间隙下的姿态预测，极大降低了计算成本。特别值得关注的是，该方法不依赖主动力控制，使得低自由度机械臂也能实现精确装配，具有重要的工程应用价值。

未来研究方向包括将方法扩展到更复杂的几何形状，以及在实际机器人系统上进行实验验证。随着机器人技术在精密制造、空间操作等领域的深入应用，这种基于几何计算的笼约束姿态估计方法有望成为机器人柔顺操作的重要工具。