《Mechanism and Machine Theory》：Dynamics-based forward kinematics estimation for Stewart platform using unscented Kalman filter with extended actuator observations
编辑推荐：
Stewart-Gough平台的正运动学问题涉及根据六条执行器（actuator）长度确定运动平台的位置和姿态，该问题本质上是高度非线性的，可能产生多达40个不同的解。虽然牛顿-拉夫逊（Newton-Raphson）等数值方法在连续跟踪中提供了精确的位姿（
Stewart-Gough平台的正运动学问题涉及根据六条执行器（actuator）长度确定运动平台的位置和姿态，该问题本质上是高度非线性的，可能产生多达40个不同的解。虽然牛顿-拉夫逊（Newton-Raphson）等数值方法在连续跟踪中提供了精确的位姿（pose）解，但通过数值微分获得速度和角速度会放大测量噪声和编码器量化误差。本文提出了一种基于动力学的前向运动学估计方法，使用无迹卡尔曼滤波器（Unscented Kalman Filter, UKF），并采用包含腿长度和腿速度的扩展执行器观测。速度级观测通过雅可比矩阵（Jacobian matrix）提供了平台旋量（platform twist）的直接可观测性，解决了传统仅使用长度测量时存在的可观测性缺陷。研究人员开发了一个完整的动力学模型，包含一个力-角速度耦合项，该项源于对球关节黏性阻尼（viscous damping）的系统辨识，将力预测均方根误差降低了82%。可观测性格拉姆矩阵（observability Gramian）分析从理论上确认，在扩展观测下，完整的12维状态变为完全可观测，而仅运动学方法在速度子空间中秩亏（rank-deficient）。研究人员进一步证明，直接速度可观测性使得简化的角加速度模型（˙ω≈0）能够使用，该模型在角速度精度上比完整的欧拉（Euler）旋转动力学高出5.3倍，因为测量更新主导了对惯性参数误差的预测步骤。与牛顿-拉夫逊法结合数值微分的对比研究表明，在干净条件下角速度估计精度提高了14倍（0.004对0.053 rad/s），在0.5 mm编码器量化条件下平移速度精度提高了3.1倍（0.80对2.49 cm/s），同时保持了相当的亚毫米位置精度（约1.3 mm均方根误差，Root Mean Square Error, RMSE）。参数敏感性分析证实了该方法在广泛的滤波器整定参数、质量和惯性失配达±20%、力扰动达30%以及编码器分辨率达0.5 mm范围内的鲁棒性。该方法在不增加超出标准编码器的额外传感器的情况下实现了亚毫米精度，为需要高保真速度信息的基于模型的控制应用提供了一种实用解决方案。
**论文解读**
**一、研究背景与问题**
Stewart-Gough平台（简称Stewart平台）是一种六自由度并联机械臂（parallel manipulator），由固定基座通过六根可变长度执行器连接运动平台构成。因其高刚度、高承载能力和高定位精度，该机构广泛应用于运动模拟器、精密定位系统、加工中心和空间应用等领域。许多先进应用采用基于模型的控制策略，需要实时准确地获取平台的运动学状态，包括速度和角速度信息，以实现鲁棒的轨迹跟踪。
Stewart平台运行中面临的一个根本性挑战是正运动学（forward kinematics）问题，即根据六根执行器的长度确定运动平台的位置和姿态。与具有直接解析解的逆运动学（inverse kinematics）问题不同，正运动学问题本质上是高度非线性的，对于一般构型可产生多达40个不同的解，因此在不附加额外约束或信息的情况下无法获得唯一的解析解。
现有方法主要包括数值方法和滤波方法两大类。牛顿-拉夫逊迭代等数值方法在连续跟踪中通过热启动（warm-starting）能够高效且精确地估计位置，但获取速度信息需要对求解的位姿序列进行数值微分，这从根本上放大了测量噪声。施加低通滤波器可以降低噪声，但会引入相位滞后（phase lag），在噪声衰减与时间精度之间形成了无法在微分框架内消除的固有权衡。当考虑编码器量化效应时，有限的位置分辨率导致微分信号呈阶梯状，速度保真度差。在滤波方法方面，已有研究采用扩展卡尔曼滤波器（Extended Kalman Filter, EKF）和无迹卡尔曼滤波器（UKF），利用运动的时间连续性在多个解中跟踪正确解分支。然而，现有的卡尔曼滤波方法面临一个显著局限性：当仅使用腿长度作为观测时，速度和角速度状态无法从测量中直接观测，滤波器必须完全依赖动力学模型推断这些状态，在模型存在不确定性时会导致估计漂移。部分研究者通过在运动平台上加装惯性测量单元（Inertial Measurement Unit, IMU）来解决可观测性问题，但这需要额外硬件，引入传感器融合复杂性，且在空间约束、成本或强磁场等环境因素下可能不切实际。
**二、研究内容与意义**
研究人员提出了一种基于动力学的正运动学估计方法，使用UKF结合扩展的执行器观测（同时包含腿长度和腿速度），在不增加额外传感器的情况下实现高精度估计。该方法的核心创新在于识别并利用了执行器速度观测通过雅可比矩阵（Jacobian matrix）提供的平台旋量（platform twist，即组合的线速度和角速度）的直接可观测性，从根本上解决了正运动学中速度估计问题。研究人员还开发了包含力-角速度耦合效应的完整动力学模型，并通过系统辨识量化了该耦合的贡献。该研究的理论意义在于揭示了速度级观测对正运动学可观测性结构的根本性改变，实践意义在于仅利用标准编码器硬件即可实现与IMU增强方法相当的估计精度，在保持硬件简洁性的同时为基于模型的控制提供了高保真速度信息。
**三、关键技术方法概述**
研究采用的主要技术方法包括以下几方面。首先，在仿真验证层面，使用MATLAB/Simscape Multibody搭建了高保真Stewart平台多体动力学模型，平台质量为0.1814 kg，名义腿长为30 cm，平台半径为18 cm，基座半径为24 cm，采样周期为25.25 ms。该Simscape模型捕捉了完整的刚体动力学、关节约束、执行器力和球关节阻尼效应。其次，在姿态表示方面，采用修正罗德里格斯参数（Modified Rodrigues Parameters, MRP）作为三参数最小姿态表示，避免了欧拉角的万向节锁（gimbal lock）奇异性，并通过影子集（shadow set）变换处理MRP在完整360度旋转处的奇异性。再次，在滤波框架方面，采用UKF进行非线性状态估计，利用2n+1个sigma点通过无迹变换（unscented transformation）捕获状态的均值和协方差，传播采用四阶龙格-库塔（RK4）积分。在观测模型方面，扩展观测向量包含六条腿长度和六条腿速度共12维测量，其中腿速度通过解析推导的雅可比矩阵与平台速度建立线性关系。在动力学建模方面，通过对比Simscape地面真值与标准动力学模型预测，对加速度残差进行回归分析，辨识出力-角速度线性耦合系数（cx = 7.32, cy = 7.37, cz ≈ 0），并将其纳入平移动力学方程。在可观测性分析方面，通过计算可观测性格拉姆矩阵（observability Gramian），从理论上比较了仅运动学观测与扩展观测两种配置下的系统可观测性。
**四、主要研究结果**
**（1）可观测性分析结果。** 研究人员通过可观测性格拉姆矩阵分析从理论上证明，在仅使用腿长度观测（仅运动学配置）时，观测雅可比矩阵对速度状态的偏导数为零块，系统在速度子空间中秩亏（rank-deficient），速度状态不可直接观测。而采用扩展观测（腿长度加腿速度）后，腿速度对平台速度的灵敏度恰好为雅可比矩阵J，该矩阵在非奇异构型下可逆，使得完整的12维状态变为完全可观测。这一理论分析为扩展观测方法的有效性提供了严格的数学基础。
**（2）力-角速度耦合辨识结果。** 通过系统辨识，研究人员发现了源于球关节黏性阻尼（viscous damping）的力-角速度耦合效应。回归分析揭示了加速度残差与角速度分量之间的线性关系，辨识出耦合系数cx = 7.32和cy = 7.37，cz近似为零。将该项建模为附加力项Fcoupling = c ⊙ ω纳入平移动力学方程后，力预测均方根误差从1.73 kg·cm/s2降至0.31 kg·cm/s2，建模误差降低了82%。
**（3）与牛顿-拉夫逊方法的对比结果。** 在干净测量条件下，两种方法的位置精度相当（约1.3 mm RMSE），但UKF在速度估计上优势显著：平移速度估计提高1.7倍，角速度估计提高14倍（0.004对0.053 rad/s）。在0.5 mm编码器量化条件下，UKF的优势进一步扩大：平移速度改善达3.1倍（0.80对2.49 cm/s），角速度改善达1.6倍。研究人员还评估了启停（pick-and-place）工作循环下的性能，结果表明UKF在各运动阶段均保持优势，尤其在驻留阶段能有效抑制数值微分产生的虚假速度。
**（4）角加速度模型对比结果。** 研究人员比较了简化模型（˙ω ≈ 0）与完整欧拉旋转动力学模型的性能。结果揭示了反直觉的发现：在扩展观测下，简化模型的角速度RMSE为0.004 rad/s，而完整欧拉模型因力矩计算误差经惯性矩阵求逆放大后，RMSE恶化至0.020 rad/s，简化模型精度高出5.3倍。该结果说明当存在直接速度观测时，测量更新主导了角速度估计，零加速度假设的预测误差反而小于完整动力学模型的参数误差。
**（5）鲁棒性与参数敏感性分析结果。** 参数敏感性分析表明，该方法对滤波器整定参数在宽范围内不敏感：速度过程噪声在[0.1, 10] (cm/s)2范围内位置RMSE变化小于20%。质量失配达±20%时位置RMSE仍低于0.25 mm，惯性张量失配几乎无影响（因˙ω ≈ 0消除了对惯性的依赖）。力扰动容忍度显著：白噪声、常值偏置和正弦扰动达30%时仍保持亚毫米精度。然而，基座与平台连接点位置标定误差对绝对位置精度影响显著：±5%的标定误差导致约19.5 mm的位置误差，但速度与角速度估计对该误差远不敏感。在±1~2%的现实标定精度下，位置误差约为4~8 mm，而角速度RMSE仍保持在0.0054~0.0085 rad/s。
**五、讨论与结论**
在方法比较方面，研究人员指出对于仅需要位姿信息的应用，牛顿-拉夫逊方法因其计算效率高（每步0.3 ms）仍具有竞争力；而对于需要速度信息的基于模型的控制应用，UKF方法在角速度上14倍的改进和避免相位滞后的优势使其成为更优选择。相较于IMU增强方法，该方法在无需额外硬件的情况下达到类似精度。
在实际实施考虑方面，腿速度观测质量是决定UKF优势的主要因素，需根据编码器分辨率正确设定测量噪声协方差R。力-角速度耦合的线性模型应理解为在仿真工况内的一阶近似，物理平台上可能需要更复杂的摩擦模型。简化角加速度假设在参考轨迹下是合理的，但对于高频运动，可能需要增大角速度过程噪声或提高采样频率。
研究结论部分指出：第一，腿速度观测方程通过雅可比矩阵建立了腿速度与平台速度状态之间的线性关系，为速度状态提供了直接可观测性，这是该方法的核心贡献。第二，识别并建模的力-角速度耦合效应将力预测误差降低了82%。第三，直接速度可观测性使得简化的角加速度模型（˙ω ≈ 0）能够以5.3倍的优势优于完整欧拉旋转动力学。第四，对比研究提供了细致评估：在干净条件下角速度改善14倍，在0.5 mm编码器量化下平移速度改善3.1倍，且优势随噪声水平增加而扩大。第五，参数敏感性分析确认了方法的鲁棒性，但连接点标定误差对绝对位置精度有关键影响，±1~2%的现实标定误差导致4~8 mm的位置误差，而速度级估计对此不敏感。该方法为需要速度与角速度信息的Stewart平台应用提供了一种实用且精确的实时正运动学估计方案，未来工作将集中于物理平台上的实验验证、非线性摩擦模型研究、在线运动学校准以及在真实工况下的性能评估。
