6-UPRU并联机器人动力学建模及基本动力学参数确定

Dynamics Modeling and Base Dynamics Parameter Determination of 6-UPRU Parallel Manipulators

摘要

为克服并联机器人动力学模型应用难题，以6-UPRU型并联机器人作为研究对象，对其精确动力学建模及基本动力学参数确定问题进行了研究。首先对机器人的各运动构件进行详细的运动学分析；然后基于牛顿-欧拉法推导机器人的精确动力学模型，并结合提出的模型线性化运算规则，将其转化为关于动力学参数的线性表达形式；随后，以最小化观测矩阵条件数为优化目标，设计了一条满足物理约束的五阶傅里叶级数形式的激励轨迹；进一步，通过对观测矩阵进行QR分解，成功提取了基本动力学参数，将动力学参数从29个减少至17个，有效解决了参数冗余问题；最后，通过SimMechanics和ADAMS仿真平台下的力拟合实验以及样机基本动力学参数辨识实验，验证了理论模型的正确性。

Abstract

To address the challenges in applying the dynamics model of parallel manipulators， the 6-UPRU parallel manipulators were focused on to investigate the precise dynamics modeling and base dynamics parameter determination. Firstly， a detailed kinematics analysis of the manipulator's moving components was conducted. Subsequently， the precise dynamics model was derived using the Newton-Euler method， and through the proposed model linearization rules， which was transformed into a linear form with respect to the dynamics parameters. Then， a fifth-order Fourier series-based exciting trajectory satisfying physical constraints was designed， with the goal of minimizing the condition number of the observation matrix. Furthermore， by performing QR decomposition on the observation matrix， the base dynamics parameters were successfully extracted， reducing the number of dynamics parameters from 29 to 17， effectively addressing the issue of parameter redundancy. Finally， the correctness of the theoretical model was validated through force-fitting experiments conducted on the SimMechanics and ADAMS simulation platforms and the prototype's base dynamics parameter identification experiments.

Graphical abstract

关键词

并联机器人 / 动力学建模 / 基本动力学参数 / 牛顿-欧拉法

Key words

parallel manipulator / dynamics modeling / base dynamics parameter / Newton-Euler method

Author summay

倪涛，男，1978年生，教授、博士研究生导师。研究方向为移动机器人、环境感知。发表论文20余篇。E-mail：[e-mail]。

0 引言

并联机器人动力学建模方法主要包括拉格朗日法、虚功原理法以及牛顿-欧拉法等。拉格朗日法为了避免引入拉格朗日乘子带来的复杂度，需要对动力学模型进行简化。尽管简化后的模型便于分析，但不可避免地会牺牲一定的模型精度^［1-2］。虚功原理避免了对约束力和力矩的计算，能够降低动力学计算的成本。KLEIN等^［3］利用虚功原理构建了Gough-Stewart并联平台的动力学模型，并设计了PD轨迹控制器。但是利用虚功原理构建的动力学模型不能同时满足结构设计和控制系统设计的要求^［4］。牛顿-欧拉法作为一种基于矢量力学的递归求解方法为并联机器人精确动力学模型的构建提供了一条清晰且直观的途径。ZHANG等^［5］和YANG等^［6］均采用牛顿-欧拉法构建了基于Stewart平台的卫星天线支撑机构的动力学模型，并对其振动特性进行了分析。KHANBABAYI等^［7］成功地将牛顿-欧拉法应用于基于Stewart平台的踝关节康复装置，构建了显式动力学模型，并设计了相应的鲁棒控制器。

并非所有动力学参数均对动力学模型具有实际贡献，去除冗余项后的动力学参数被称为基本动力学参数。获取基本动力学参数的方法主要分为符号法与数值法两类。符号法仅适用于只包含转动关节的普通并联及垂直机械臂^［8］，而对于一般并联机器人，需针对特定情况逐一分析处理，导致算法难以实现自动化^［9］。数值法具有普适性，通过对观测矩阵进行奇异值分解（SVD）或QR分解^［10］，可有效消除冗余项，且算法实现过程简便。YANG等^［11］采用SVD法求解出7自由度Baxter机械臂的基本动力学参数，并实现了参数辨识。TADESE等^［12］采用数值法得到Indy7协作机器人机械手的基本动力学参数，提高了动力学模型的精度。目前，针对串联机器人的基本动力学参数的确定研究已经非常成熟，但在并联机器人方面的相关研究较少。

现有关于并联六自由度平台动力学模型的研究大多基于简化结构展开，针对6-UPRU构型平台的研究相对较少，或直接将其简化为6-UPS构型进行处理。此类简化虽降低了计算复杂度，却不可避免地引入了建模误差，导致模型精度下降。此外，动力学参数在实际计算过程中难以直接获取，通常需通过参数辨识方法确定。然而，动力学参数的冗余现象不仅显著增加了计算成本，而且它引发的观测矩阵病态问题更会导致参数辨识过程难以收敛，从而严重制约动力学模型的实际应用效果。

为了弥补上述研究工作的不足，本文采用牛顿-欧拉法构建了6-UPRU并联机器人的精确动力学模型，并提出了一种用于线性化并联机器人动力学模型的运算规则，在此基础上推导了动力学模型关于动力学参数的线性表达式。进一步，通过设计优化的五阶傅里叶级数形式激励轨迹，并结合观测矩阵QR分解法成功提取了基本动力学参数，构建了相应的简化动力学模型。最后，基于SimMechanics模型与ADAMS模型的力拟合仿真实验以及基本动力学参数辨识实验，验证了完整动力学模型及基于基本动力学参数的简化动力学模型的准确性。

1 机器人动力学建模

如图1所示，6-UPRU并联机器人由基座B、6个UPRU分支以及动平台p组成。其中，基座坐标系\{B\}的原点\mathit{O}位于半径为r_{\mathrm{b}}的下铰点所在圆的圆心处，动平台坐标系\{p\}的原点\mathit{o}位于半径为r_{\mathrm{p}}的上铰点所在圆的圆心处。如图2所示，UPRU分支{\mathit{L}}_i(i=\mathrm{1,2},\cdots ,\mathrm{6})自下而上依次包括：虎克铰U_{\mathrm{b}i}、电动缸{\mathit{L}}_{i\mathrm{4}}（质量为m_{\mathrm{4}}，质心{\mathit{c}}_{i\mathrm{4}}距下铰点距离为l_{\mathrm{c}\mathrm{4}}）、移动副P_{\mathrm{a}i}（P）、丝杠推杆{\mathit{L}}_{i\mathrm{3}}（质量为m_{\mathrm{3}}，质心{\mathit{c}}_{i\mathrm{3}}距上铰点距离为l_{\mathrm{c}\mathrm{3}}）、拉压力传感器S_{\mathrm{a}i}、连杆{\mathit{L}}_{i\mathrm{2}}（质量为m_{\mathrm{2}}，质心{\mathit{c}}_{i\mathrm{2}}距上铰点距离为l_{\mathrm{c}\mathrm{2}}）、转动副{\mathit{r}}_{\mathrm{a}i}、连杆{\mathit{L}}_{i\mathrm{1}}（质量为m_{\mathrm{1}}，质心{\mathit{c}}_{i\mathrm{1}}距上铰点距离为l_{\mathrm{c}\mathrm{1}}）以及虎克铰U_{\mathrm{p}i}。动平台的质量为m_{\mathrm{p}}，其质心{\mathit{o}}_{\mathrm{c}}在坐标系\{p\}中的矢量表示为{\mathit{r}}_{\mathrm{c}}^{\{p\}}。虎克铰U_{\mathrm{b}i}中，与基座固连的转动副的单位向量为{\mathit{r}}_{\mathrm{b}i\mathrm{1}}，与连杆固连的转动副的单位向量为{\mathit{r}}_{\mathrm{b}i\mathrm{2}}，铰点在坐标系\{B\}中的矢量坐标为{\mathit{B}}_i。虎克铰U_{\mathrm{p}i}中，与动平台固连的转动副的单位向量为{\mathit{r}}_{\mathrm{p}i\mathrm{1}}，与连杆固连的转动副的单位向量为{\mathit{r}}_{\mathrm{p}i\mathrm{2}}，铰点在坐标系\{B\}中的矢量坐标为{\mathit{b}}_i，在坐标系\{p\}中的矢量坐标为{\mathit{b}}_i^{\{p\}}。

1.1 运动学分析

坐标系\{p\}相对于坐标系\{B\}的位姿表示为

\mathit{q}=\left[\begin{array}{c}\mathit{o}\\ \mathit{\theta }\end{array}\right]={[\begin{array}{cccccc}x& y& z& \alpha & \beta & \gamma \end{array}]}^{\mathrm{T}}（1）

其中， o 表示位置矢量，\mathit{o}={(x,y,z)}^{\mathrm{T}};\mathit{\theta }表示Z-Y-X欧拉角，\mathit{\theta }={(\alpha ,\beta ,\gamma )}^{\mathrm{T}}。旋转矩阵可以根据下式求得：

{\mathit{R}}_{\{p\}}^{\{B\}}=\mathit{R}\left(z,\alpha \right)\mathit{R}\left(y,\beta \right)\mathit{R}\left(x,\gamma \right)（2）

平台线速度表示为\mathit{v}，角速度表示为\mathit{\omega }，则平台广义速度\mathit{V}可以表示为

\mathit{V}=\left[\begin{array}{c}\mathit{v}\\ \mathit{\omega }\end{array}\right]=\left[\begin{array}{cc}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathit{J}}_{\omega }\end{array}\right]\left[\begin{array}{c}\dot{\mathit{o}}\\ \dot{\mathit{\theta }}\end{array}\right]={\mathit{J}}_q\dot{\mathit{q}}（3）

其中，{\mathit{I}}_{\mathrm{3}\times \mathrm{3}}表示三维单位矩阵，角速度变化矩阵

{\mathit{J}}_{\omega }=\left[\begin{array}{ccc}\mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\alpha & \mathrm{c}\mathrm{o}\mathrm{s}\alpha \mathrm{c}\mathrm{o}\mathrm{s}\beta \\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\alpha & \mathrm{s}\mathrm{i}\mathrm{n}\alpha \mathrm{c}\mathrm{o}\mathrm{s}\beta \\ \mathrm{1}& \mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\beta \end{array}\right]（4）

动平台原点的线加速度表示为\mathit{a}，角速度表示为\mathit{\epsilon }，则广义加速度 A 可由式（3）求导得到：

\mathit{A}=\left[\begin{array}{c}\mathit{a}\\ \mathit{\epsilon }\end{array}\right]={\dot{\mathit{J}}}_q^{}\dot{\mathit{q}}+{\mathit{J}}_q\ddot{\mathit{q}}（5）

动平台铰点在\{B\}中的坐标可表示为

\left.\begin{array}{l}{\mathit{b}}_i=\mathit{o}+{\mathit{e}}_i\\ {\mathit{e}}_i={\mathit{R}}_{\{p\}}^{\{B\}}{\mathit{b}}_i^{\{p\}}\end{array}\right\}（6）

可求得驱动分支i整体矢量{\mathit{L}}_i和单位向量{\mathit{n}}_i分别为

\left.\begin{array}{l}{\mathit{L}}_i={\mathit{b}}_i-{\mathit{B}}_i\\ {\mathit{n}}_i={\mathit{L}}_i/l_i\end{array}\right\}（7）

分支i的速度为

{\mathit{v}}_i=({\dot{l}}_{\mathrm{1}}^{},{\dot{l}}_{\mathrm{2}}^{},\cdots ,{\dot{l}}_{\mathrm{6}}^{}{)}^{\mathrm{T}}=\mathit{J}\mathit{V}（8）

\mathit{J}=\left[\begin{array}{cc}\begin{array}{l}{\mathit{n}}_{\mathrm{1}}^{\mathrm{T}}\\ {\mathit{n}}_{\mathrm{2}}^{\mathrm{T}}\end{array}& \begin{array}{l}({\mathit{e}}_{\mathrm{1}}\times {\mathit{n}}_{\mathrm{1}}{)}^{\mathrm{T}}\\ ({\mathit{e}}_{\mathrm{2}}\times {\mathit{n}}_{\mathrm{2}}{)}^{\mathrm{T}}\end{array}\\ ⋮& ⋮\\ {\mathit{n}}_{\mathrm{6}}^{\mathrm{T}}& ({\mathit{e}}_{\mathrm{6}}\times {\mathit{n}}_{\mathrm{6}}{)}^{\mathrm{T}}\end{array}\right]（9）

分支加速度可由式（8）求导得到：

{\mathit{a}}_i=({\ddot{l}}_{\mathrm{1}}^{},{\ddot{l}}_{\mathrm{2}}^{},\cdots ,{\ddot{l}}_{\mathrm{6}}^{}{)}^{\mathrm{T}}=\dot{\mathit{J}}\mathit{V}+\mathit{J}\mathit{A}（10）

\dot{\mathit{J}}=\left[\begin{array}{cc}\begin{array}{l}{\dot{\mathit{n}}}_{\mathrm{1}}^{\mathrm{T}}\\ {\dot{\mathit{n}}}_{\mathrm{2}}^{\mathrm{T}}\end{array}& \begin{array}{l}(\mathit{\omega }\times {\mathit{e}}_{\mathrm{1}}\times {\mathit{n}}_{\mathrm{1}}+{\mathit{e}}_{\mathrm{1}}\times {\dot{\mathit{n}}}_{\mathrm{1}}^{}{)}^{\mathrm{T}}\\ (\mathit{\omega }\times {\mathit{e}}_{\mathrm{2}}\times {\mathit{n}}_{\mathrm{2}}+{\mathit{e}}_{\mathrm{2}}\times {\dot{\mathit{n}}}_{\mathrm{2}}^{}{)}^{\mathrm{T}}\end{array}\\ ⋮& ⋮\\ {\dot{\mathit{n}}}_{\mathrm{6}}^{\mathrm{T}}& (\mathit{\omega }\times {\mathit{e}}_{\mathrm{6}}\times {\mathit{n}}_{\mathrm{6}}+{\mathit{e}}_{\mathrm{6}}\times {\dot{\mathit{n}}}_{\mathrm{6}}^{}{)}^{\mathrm{T}}\end{array}\right]（11）

连杆{\mathit{L}}_{i\mathrm{2}}、{\mathit{L}}_{i\mathrm{3}}和{\mathit{L}}_{i\mathrm{4}}没有相对转动，因此具有相同的角速度{\mathit{\omega }}_{\mathrm{b}i}和角加速度{\mathit{\epsilon }}_{\mathrm{b}i}。{\mathit{\omega }}_{\mathrm{b}i}可表示为

\left.\begin{array}{l}{\mathit{\omega }}_{\mathrm{b}i}={\dot{\theta }}_{\mathrm{b}i\mathrm{1}}^{}{\mathit{r}}_{\mathrm{b}i\mathrm{1}}+{\dot{\theta }}_{\mathrm{b}i\mathrm{2}}^{}{\mathit{r}}_{\mathrm{b}i\mathrm{2}}\\ {\mathit{r}}_{\mathrm{b}i\mathrm{2}}=\frac{{\mathit{r}}_{\mathrm{b}i\mathrm{1}}\times {\mathit{n}}_i}{\left|{\mathit{r}}_{\mathrm{b}i\mathrm{1}}\times {\mathit{n}}_i\right|}\end{array}\right\}（12）

式中：{\dot{\theta }}_{\mathrm{b}i\mathrm{1}}^{}、{\dot{\theta }}_{\mathrm{b}i\mathrm{2}}^{}分别为绕轴{\mathit{r}}_{\mathrm{b}i\mathrm{1}}和{\mathit{r}}_{\mathrm{b}i\mathrm{2}}的角速度。

式（7）第一个公式两边关于时间求导可得

{\dot{\mathit{b}}}_i^{}-{\dot{l}}_i^{}{\mathit{n}}_i={\mathit{\omega }}_{\mathrm{b}i}\times l_i{\mathit{n}}_i（13）

式（13）两边用{\mathit{r}}_{\mathrm{b}\mathrm{c}\mathrm{i}}叉乘，整理可得

{\mathit{\omega }}_{\mathrm{b}i}=\frac{{\mathit{r}}_{\mathrm{b}\mathrm{c}i}\times ({\dot{\mathit{b}}}_i^{}-{\dot{l}}_i^{}{\mathit{n}}_i)}{l_i{\mathit{n}}_i\cdot {\mathit{r}}_{\mathrm{b}\mathrm{c}i}}（14）

其中，{\mathit{r}}_{\mathrm{b}\mathrm{c}\mathrm{i}}={\mathit{r}}_{\mathrm{b}i\mathrm{1}}\times {\mathit{r}}_{\mathrm{b}i\mathrm{2}}，{\dot{\mathit{b}}}_i^{}表示{\mathit{b}}_i点速度，{\dot{\mathit{b}}}_i^{}=\mathit{v}+\mathit{\omega }\times {\mathit{e}}_i。

式（14）两边分别点乘{\mathit{r}}_{\mathrm{b}i\mathrm{1}}和{\mathit{r}}_{\mathrm{b}i\mathrm{2}}，得

\left.\begin{array}{l}{\dot{\theta }}_{\mathrm{b}i\mathrm{1}}^{}={\mathit{\omega }}_{\mathrm{b}i}\cdot {\mathit{r}}_{\mathrm{b}i\mathrm{1}}=\frac{-({\dot{\mathit{b}}}_i^{}-{\dot{l}}_i^{}{\mathit{n}}_i)\cdot {\mathit{r}}_{\mathrm{b}i\mathrm{2}}}{l_i{\mathit{n}}_i\cdot {\mathit{r}}_{\mathrm{b}\mathrm{c}i}}\\ {\dot{\theta }}_{\mathrm{b}i\mathrm{2}}^{}={\mathit{\omega }}_{\mathrm{b}i}\cdot {\mathit{r}}_{\mathrm{b}i\mathrm{2}}=\frac{({\dot{\mathit{b}}}_i^{}-{\dot{l}}_i^{}{\mathit{n}}_i)\cdot {\mathit{r}}_{\mathrm{b}i\mathrm{1}}}{l_i{\mathit{n}}_i\cdot {\mathit{r}}_{\mathrm{b}\mathrm{c}i}}\end{array}\right\}（15）

对式（14）两边关于时间求导，整理得到

{\mathit{\epsilon }}_{\mathrm{b}i}=\frac{\mathrm{1}}{l_i{\mathit{n}}_i\cdot {\mathit{r}}_{\mathrm{b}\mathrm{c}i}}[{\mathit{r}}_{\mathrm{b}\mathrm{c}i}\times ({\ddot{\mathit{b}}}_i^{}-{\ddot{l}}_i^{}{\mathit{n}}_i-{\dot{l}}_i^{}{\mathit{\omega }}_{bi}\times {\mathit{n}}_i)-{\dot{\theta }}_{\mathrm{b}i\mathrm{1}}^{}{\mathit{r}}_{\mathrm{b}i\mathrm{2}}\times({\dot{\mathit{b}}}_i^{}-{\dot{l}}_i^{}{\mathit{n}}_i)-({\dot{l}}_i^{}{\mathit{n}}_i+l_i{\mathit{\omega }}_{bi}\times {\mathit{n}}_i)\cdot {\mathit{r}}_{\mathrm{b}\mathrm{c}i}{\mathit{\omega }}_{\mathrm{b}i}]（16）

连杆{\mathit{L}}_{i\mathrm{1}}的角速度可以表示为

\left.\begin{array}{l} {\mathit{\omega }}_{\mathrm{p}i}=\mathit{\omega }-{\dot{\theta }}_{\mathrm{p}i\mathrm{1}}^{}{\mathit{r}}_{\mathrm{p}i\mathrm{1}}-{\dot{\theta }}_{\mathrm{p}i\mathrm{2}}^{}{\mathit{r}}_{\mathrm{p}i\mathrm{2}}\\ {\mathit{r}}_{\mathrm{p}i\mathrm{2}}=\frac{{\mathit{r}}_{\mathrm{p}i\mathrm{1}}\times {\mathit{n}}_i}{\left|{\mathit{r}}_{\mathrm{p}i\mathrm{1}}\times {\mathit{n}}_i\right|}\end{array}\right\}（17）

式中：{\dot{\theta }}_{\mathrm{p}i\mathrm{1}}^{}、{\dot{\theta }}_{\mathrm{p}i\mathrm{2}}^{}分别为绕轴{\mathit{r}}_{\mathrm{p}i\mathrm{1}}和{\mathit{r}}_{\mathrm{p}i\mathrm{2}}的角速度。

由连杆{\mathit{L}}_{i\mathrm{2}}和连杆{\mathit{L}}_{i\mathrm{1}}在杆的法线方向上的角速度相等可以得到

{\mathit{\omega }}_{\mathrm{p}i}-({\mathit{\omega }}_{\mathrm{p}i}\cdot {\mathit{n}}_i){\mathit{n}}_i={\mathit{\omega }}_{\mathrm{b}i}-({\mathit{\omega }}_{\mathrm{b}i}\cdot {\mathit{n}}_i){\mathit{n}}_i（18）

式（18）两边点乘{\mathit{r}}_{\mathrm{p}i\mathrm{2}}\times {\mathit{n}}_i，整理可得

\left.\begin{array}{l}{\dot{\theta }}_{\mathrm{p}i\mathrm{1}}^{}=\frac{(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot ({\mathit{r}}_{\mathrm{p}i\mathrm{2}}\times {\mathit{n}}_i)}{{\mathit{r}}_{\mathrm{p}\mathrm{c}i}\cdot {\mathit{n}}_i}\\ {\mathit{r}}_{\mathrm{p}\mathrm{c}i}={\mathit{r}}_{\mathrm{p}i\mathrm{1}}\times {\mathit{r}}_{\mathrm{p}i\mathrm{2}}\end{array}\right\}（19）

式（18）两边点乘{\mathit{r}}_{\mathrm{p}i\mathrm{2}}，整理可得

{\dot{\theta }}_{\mathrm{p}i\mathrm{2}}^{}=(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot {\mathit{r}}_{\mathrm{p}i\mathrm{2}}（20）

对式（17）第一个公式两边关于时间求导，可以求得连杆{\mathit{L}}_{i\mathrm{1}}角加速度

{\mathit{\epsilon }}_{\mathrm{p}i}=\mathit{\epsilon }-{\epsilon }_{\mathrm{p}i\mathrm{1}}{\mathit{r}}_{\mathrm{p}i\mathrm{1}}-{\epsilon }_{\mathrm{p}i\mathrm{2}}{\mathit{r}}_{\mathrm{p}i\mathrm{2}}+\mathit{\omega }\times {\mathit{\omega }}_{\mathrm{p}i}+{\dot{\theta }}_{\mathrm{p}i\mathrm{1}}^{}{\dot{\theta }}_{\mathrm{p}i\mathrm{2}}^{}{\mathit{r}}_{\mathrm{p}\mathrm{c}i}（21）

\begin{array}{c}{\epsilon }_{\mathrm{p}i\mathrm{1}}=\frac{(\mathit{\epsilon }-{\epsilon }_{\mathrm{b}i})\cdot ({\mathit{r}}_{\mathrm{p}i\mathrm{2}}\times {\mathit{n}}_i)+{\dot{\theta }}_{\mathrm{p}i\mathrm{2}}^{}{\mathit{\omega }}_{\mathrm{p}i}\cdot {\mathit{n}}_i}{{\mathit{r}}_{\mathrm{p}\mathrm{c}i}\cdot {\mathit{n}}_i}-\\ \frac{({\mathit{\omega }}_{\mathrm{b}i}\cdot {\mathit{r}}_{\mathrm{p}i\mathrm{2}})(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot {\mathit{n}}_i}{{\mathit{r}}_{\mathrm{p}\mathrm{c}i}\cdot {\mathit{n}}_i}-\end{array}

\frac{({\mathit{r}}_{\mathrm{p}i\mathrm{1}}\cdot {\mathit{n}}_i)(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot ({\mathit{r}}_{\mathrm{p}i\mathrm{2}}\times {\mathit{n}}_i)(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot {\mathit{r}}_{\mathrm{p}i\mathrm{2}}}{({\mathit{r}}_{\mathrm{p}\mathrm{c}i}\cdot {\mathit{n}}_i{)}^{\mathrm{2}}}（22）

{\epsilon }_{\mathrm{p}i\mathrm{2}}=(\mathit{\epsilon }-{\mathit{\epsilon }}_{\mathrm{b}i})\cdot {\mathit{r}}_{\mathrm{p}i\mathrm{2}}+(\mathit{\omega }-{\mathit{\omega }}_{\mathrm{b}i})\cdot ({\mathit{\omega }}_{\mathrm{p}i}\times {\mathit{r}}_{\mathrm{p}i\mathrm{2}})

连杆{\mathit{L}}_{i\mathrm{1}}、{\mathit{L}}_{i\mathrm{2}}、{\mathit{L}}_{i\mathrm{3}}和{\mathit{L}}_{i\mathrm{4}}的质心加速度可通过对其质心位置矢量求二阶导数得到：

\left.\begin{array}{l}{\mathit{a}}_{i\mathrm{1}}={\mathit{a}}_{\mathrm{p}i}-l_{\mathrm{c}\mathrm{1}}{\mathit{b}}_{\mathrm{p}i}\\ {\mathit{a}}_{i\mathrm{2}}={\mathit{a}}_{\mathrm{b}i}-l_{\mathrm{c}\mathrm{2}}{\mathit{b}}_{\mathrm{b}i}\\ {\mathit{a}}_{i\mathrm{3}}={\mathit{a}}_{\mathrm{b}i}-l_{\mathrm{c}\mathrm{3}}{\mathit{b}}_{\mathrm{b}i}\\ {\mathit{a}}_{i\mathrm{4}}=l_{\mathrm{c}\mathrm{4}}{\mathit{b}}_{\mathrm{b}i}\end{array}\right\}（23）

\left.\begin{array}{l}{\mathit{a}}_{\mathrm{p}i}={\ddot{l}}_i^{}{\mathit{n}}_i+\mathrm{2}{\dot{l}}_i^{}{\mathit{\omega }}_{\mathrm{p}i}\times {\mathit{n}}_i+l_i{\mathit{b}}_{\mathrm{p}i}\\ {\mathit{b}}_{\mathrm{p}i}={\mathit{\epsilon }}_{\mathrm{p}i}\times {\mathit{n}}_i+{\mathit{\omega }}_{\mathrm{p}i}\times ({\mathit{\omega }}_{\mathrm{p}i}\times {\mathit{n}}_i)\\ {\mathit{a}}_{\mathrm{b}i}={\ddot{l}}_i^{}{\mathit{n}}_i+\mathrm{2}{\dot{l}}_i^{}{\mathit{\omega }}_{\mathrm{b}i}\times {\mathit{n}}_i+l_i{\mathit{b}}_{\mathrm{b}i}\\ {\mathit{b}}_{\mathrm{b}i}={\mathit{\epsilon }}_{\mathrm{b}i}\times {\mathit{n}}_i+{\mathit{\omega }}_{\mathrm{b}i}\times ({\mathit{\omega }}_{\mathrm{b}i}\times {\mathit{n}}_i)\end{array}\right\}（24）

同理，可求得动平台质心{\mathit{o}}_{\mathrm{c}}的加速度{\mathit{a}}_{\mathrm{c}}为

\left.\begin{array}{l} {\mathit{a}}_c=\mathit{a}+\mathit{\epsilon }\times {\mathit{r}}_c+\mathit{\omega }\times (\mathit{\omega }\times {\mathit{r}}_c)\\ {\mathit{r}}_c={\mathit{R}}_p^B{\mathit{r}}_c^p\end{array}\right\}（25）

1.2 动力学分析

分支连杆通常可以视为轴对称结构^［13］，因此连杆{\mathit{L}}_{ik}的转动惯量矩阵可以表示为

{\mathit{I}}_{ik}=I_{\mathrm{a}k}{\mathit{n}}_i{\mathit{n}}_i^{\mathrm{T}}+I_{\mathrm{n}k}({\mathit{I}}_{\mathrm{3}\times \mathrm{3}}-{\mathit{n}}_i{\mathit{n}}_i^{\mathrm{T}})（26）

式中：I_{\mathrm{a}k}为第k个连杆{\mathit{L}}_{ik}的轴向主惯性矩；I_{\mathrm{n}k}为第k个连杆{\mathit{L}}_{ik}的径向主惯性矩。

图3展示了连杆{\mathit{L}}_{i\mathrm{1}}的受力分布情况，利用欧拉方程对连杆{\mathit{L}}_{i\mathrm{1}}进行分析可得

{\mathit{I}}_{i\mathrm{1}}{\mathit{\epsilon }}_{\mathrm{p}i}+{\mathit{\omega }}_{\mathrm{p}i}\times {\mathit{I}}_{i\mathrm{1}}{\mathit{\omega }}_{\mathrm{p}i}+(s_{\mathrm{1}}-l_{\mathrm{c}\mathrm{1}}){\mathit{n}}_i\times m_{\mathrm{1}}{\mathit{a}}_{i\mathrm{1}}=

s_{\mathrm{1}}{\mathit{n}}_i\times {\mathit{F}}_{\mathrm{p}i}^{\mathrm{n}}+\left(s_{\mathrm{1}}-l_{\mathrm{c}\mathrm{1}}\right){\mathit{n}}_i\times m_{\mathrm{1}}\mathit{g}+m_{\mathrm{p}i}{\mathit{r}}_{\mathrm{p}\mathrm{c}i}+{\mathit{M}}_{i\mathrm{1}}（27）

式中：s_{\mathrm{1}}为连杆{\mathit{L}}_{i\mathrm{1}}的长度；{\mathit{F}}_{\mathrm{p}i}^{\mathrm{a}}、{\mathit{F}}_{\mathrm{p}i}^{\mathrm{n}}分别为连杆{\mathit{L}}_{i\mathrm{1}}上端受到的轴向分力和径向分力；m_{\mathrm{p}i}为上端所受力矩大小；{\mathit{M}}_{i\mathrm{1}}为下端所受力矩，其方向与连杆方向{\mathit{n}}_i垂直。

式（27）两边点乘{\mathit{n}}_i，整理可得

m_{\mathrm{p}\mathrm{i}}=(I_{\mathrm{a}\mathrm{1}}{\mathit{\epsilon }}_{\mathrm{p}i}\cdot {\mathit{n}}_i)/({\mathit{r}}_{\mathrm{p}\mathrm{c}i}\cdot {\mathit{n}}_i)（28）

图4展示了分支整体的受力分布情况。基于欧拉方程，对分支整体进行动力学分析如下：

l_i{\mathit{n}}_i\times {\mathit{F}}_{\mathrm{p}i}^{\mathrm{n}}+m_{\mathrm{p}i}{\mathit{r}}_{\mathrm{p}\mathrm{c}i}+m_{\mathrm{b}i}{\mathit{r}}_{\mathrm{b}\mathrm{c}i}={\mathit{N}}_i（29）

\begin{array}{c}{\mathit{N}}_i={\mathit{I}}_{i\mathrm{1}}{\mathit{\epsilon }}_{\mathrm{p}i}+({\mathit{I}}_{i\mathrm{2}}+{\mathit{I}}_{i\mathrm{3}}+{\mathit{I}}_{i\mathrm{4}}){\mathit{\epsilon }}_{\mathrm{b}i}-({\mathit{I}}_{i\mathrm{2}}+{\mathit{I}}_{i\mathrm{3}}+{\mathit{I}}_{i\mathrm{4}}){\mathit{\omega }}_{\mathrm{b}i}\times \\ {\mathit{\omega }}_{\mathrm{b}i}-{\mathit{I}}_{i\mathrm{1}}{\mathit{\omega }}_{\mathrm{p}i}\times {\mathit{\omega }}_{\mathrm{p}i}+m_{\mathrm{1}}(l_i-l_{\mathrm{c}\mathrm{1}}){\mathit{n}}_i\times ({\mathit{a}}_{i\mathrm{1}}-\mathit{g})+\\ m_{\mathrm{2}}(l_i-l_{\mathrm{c}\mathrm{2}}){\mathit{n}}_i\times ({\mathit{a}}_{i\mathrm{2}}-\mathit{g})+m_{\mathrm{3}}(l_i-l_{\mathrm{c}\mathrm{3}}){\mathit{n}}_i\times ({\mathit{a}}_{i\mathrm{3}}-\mathit{g})+\end{array}m_{\mathrm{4}}{l}_{\mathrm{c}\mathrm{4}}{\mathit{n}}_i\times ({\mathit{a}}_{i\mathrm{4}}-\mathit{g})（30）

式中：m_{\mathrm{b}i}为连杆{\mathit{L}}_{i\mathrm{4}}受到的反作用力矩； g 为重力加速度。

式（29）两边点乘{\mathit{n}}_i，整理可得

m_{\mathrm{b}i}=(I_{\mathrm{a}\mathrm{2}}+I_{\mathrm{a}\mathrm{3}}+I_{\mathrm{a}\mathrm{4}})({\mathit{\epsilon }}_{\mathrm{b}i}\cdot {\mathit{n}}_i)/({\mathit{r}}_{\mathrm{b}\mathrm{c}i}\cdot {\mathit{n}}_i)（31）

式（29）两边叉乘{\mathit{n}}_i，整理可得

{\mathit{F}}_{\mathrm{p}i}^{\mathrm{n}}=({\mathit{N}}_i-m_{\mathrm{p}i}{\mathit{r}}_{\mathrm{p}\mathrm{c}i}-m_{\mathrm{b}i}{\mathit{r}}_{\mathrm{b}\mathrm{c}i})\times {\mathit{n}}_i/l_i（32）

对连杆{\mathit{L}}_{i\mathrm{1}}和{\mathit{L}}_{i\mathrm{2}}整体进行受力分析，沿杆方向根据牛顿方程可得到

f_{\mathrm{p}i}^{\mathrm{a}}=[m_{\mathrm{1}}({\mathit{a}}_{i\mathrm{1}}-\mathit{g})+m_{\mathrm{2}}({\mathit{a}}_{i\mathrm{2}}-\mathit{g})]\cdot {\mathit{n}}_i-f_{\mathrm{s}i}^{\mathrm{a}}（33）

其中，f_{\mathrm{s}i}^{\mathrm{a}}表示连杆{\mathit{L}}_{i\mathrm{2}}和{\mathit{L}}_{i\mathrm{3}}之间接触力的大小（即力传感器受力大小），f_{\mathrm{p}i}^{\mathrm{a}}表示{\mathit{F}}_{\mathrm{p}i}^{\mathrm{a}}的大小。

动平台的受力分布情况如图5所示，采用牛顿-欧拉法对动平台进行分析：

\left.\begin{array}{l}{\mathit{F}}_{\mathrm{e}}={\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}{f}_{\mathrm{p}i}^{\mathrm{a}}{\mathit{n}}_i+{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}{\mathit{F}}_{\mathrm{p}i}^{\mathrm{n}}+m_{\mathrm{p}}({\mathit{a}}_{\mathrm{c}}-\mathit{g})\\ {\mathit{M}}_{\mathrm{e}}+{\mathit{r}}_{\mathrm{f}}\times {\mathit{F}}_{\mathrm{e}}={\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}[{\mathit{e}}_i\times (f_{pi}^a{\mathit{n}}_i+{\mathit{F}}_{pi}^n)]+\\ {\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}{m}_{\mathrm{p}i}{\mathit{r}}_{\mathrm{p}\mathrm{c}i}+{\mathit{I}}_{\mathrm{c}}\mathit{\alpha }-{\mathit{I}}_{\mathrm{c}}\mathit{\omega }\times \mathit{\omega }+{\mathit{r}}_{\mathrm{c}}\times m_{\mathrm{p}}({\mathit{a}}_{\mathrm{c}}-\mathit{g})\end{array}\right\}（34）

式中：{\mathit{F}}_{\mathrm{e}}、{\mathit{M}}_{\mathrm{e}}分别为动平台在{\mathit{O}}_{\mathrm{f}}点受到的外力和外力矩；{\mathit{I}}_{\mathrm{c}}={\mathit{R}}_{\{p\}}^{\{B\}}{\mathit{I}}_{\mathrm{c}}^{\{p\}}({\mathit{R}}_{\{p\}}^{\{B\}}{)}^{\mathrm{T}}表示动平台在\{B\}中的转动惯量；{\mathit{I}}_{\mathrm{c}}^{\{p\}}表示动平台在\{p\}中的转动惯量。

1.3 关于动力学参数的线性模型

将得到的相关量代入式（34），并对动力学参数进行提取，最终整理得到以下线性模型：

{\mathit{J}}^{\mathrm{T}}\mathit{\tau }+\left[\begin{array}{c}{\mathit{F}}_{\mathrm{e}}\\ {\mathit{M}}_{\mathrm{e}}+{\mathit{r}}_{\mathrm{f}}\times {\mathit{F}}_{\mathrm{e}}\end{array}\right]=\mathit{\Psi }\mathit{X}（35）

{\mathit{\tau }=\begin{array}{ccc}[f_{\mathrm{s}\mathrm{1}}^{\mathrm{a}}{f}_{\mathrm{s}\mathrm{2}}^{\mathrm{a}}& \cdots & f_{\mathrm{s}\mathrm{6}}^{\mathrm{a}}\end{array}]}^{\mathrm{T}}

其中，\mathit{\tau }表示力传感器值， X 表示动力学参数向量，其维数为\mathrm{29}\times \mathrm{1}，如表1所示；矩阵 Ψ 的表达式如下：

\mathit{\Psi }=\left[\begin{array}{cc}\begin{array}{cc}{\mathit{\Psi }}_{\mathrm{f}\mathrm{1}}& {\mathit{\Psi }}_{\mathrm{f}\mathrm{2}}\end{array}& \begin{array}{cc}\cdots & {\mathit{\Psi }}_{\mathrm{f}\mathrm{17}}\end{array}\\ \begin{array}{cc}{\mathit{\Psi }}_{\mathrm{m}\mathrm{1}}& {\mathit{\Psi }}_{\mathrm{m}\mathrm{2}}\end{array}& \begin{array}{cc}\cdots & {\mathit{\Psi }}_{\mathrm{m}\mathrm{17}}\end{array}\end{array}\right]（36）

\left.\begin{array}{l}{\mathit{\Psi }}_{fk}=\left\{\begin{array}{l}\begin{array}{l}{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}{\mathit{\psi }}_{ik}k=\mathrm{1,2},\cdots ,\mathrm{13}\\ \mathit{a}-\mathit{g}k=\mathrm{14}\\ (\widehat{\mathit{\epsilon }}+\widehat{\mathit{\omega }}\widehat{\mathit{\omega }}){\mathit{R}}_{\{p\}}^{\{B\}}k=\mathrm{15}\end{array}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{6}}k=\mathrm{16}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{6}}k=\mathrm{17}\end{array}\right.\\ {\mathit{\Psi }}_{\mathrm{m}k}=\left\{\begin{array}{l}\begin{array}{l}{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}({\widehat{\mathit{e}}}_i{\mathit{\psi }}_{i\mathrm{1}}+\frac{{\mathit{\epsilon }}_{\mathrm{p}i}\bullet {\mathit{n}}_i}{{\mathit{r}}_{\mathrm{p}\mathrm{c}i}\bullet {\mathit{n}}_i}{\mathit{r}}_{\mathrm{p}\mathrm{c}i})k=\mathrm{1}\\ {\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{6}}}{\widehat{\mathit{e}}}_i{\mathit{\psi }}_{ik}k=\mathrm{2,3},\cdots ,\mathrm{13}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{1}}k=\mathrm{14}\\ \left(\widehat{\mathit{g}}-\widehat{\mathit{a}}\right){\mathit{R}}_{\{p\}}^{\{B\}}k=\mathrm{15}\end{array}\\ \begin{array}{l}{\mathit{R}}_{\{p\}}^{\{B\}}\widetilde{(({\mathit{R}}_{\{p\}}^{\{B\}}{)}^{\mathrm{T}}\mathit{\epsilon })}+\widehat{\mathit{\omega }}{\mathit{R}}_{\{p\}}^{\{B\}}\widetilde{(({\mathit{R}}_{\{p\}}^{\{B\}}{)}^{\mathrm{T}}\mathit{\omega })}\\ k=\mathrm{16}\end{array}\\ \begin{array}{l}\mathit{\epsilon }{\stackrel{̿}{\mathit{E}}}_{\mathrm{3}\times \mathrm{3}}-{\mathit{R}}_{\{p\}}^{\{B\}}\widetilde{(({\mathit{R}}_{\{p\}}^{\{B\}}{)}^{\mathrm{T}}\mathit{\epsilon })}-\widehat{\mathit{\omega }}{\mathit{R}}_{\{p\}}^{\{B\}}\widetilde{(({\mathit{R}}_{\{p\}}^{\{B\}}{)}^{\mathrm{T}}\mathit{\omega })}\\ k=\mathrm{17}\end{array}\end{array}\right.\end{array}\right\}（37）

其中，{\mathit{\psi }}_{ik}对应的表达式如表2所示。令\mathit{\omega }表示任意三维向量，\mathit{I}表示三阶对称矩阵；\widehat{\mathit{\omega }}表示向量的斜对称矩阵；线性化运算符定义如下：

\left.\begin{array}{l}\tilde{\mathit{\omega }}=\left[\begin{array}{cc}\begin{array}{ccc}{\mathit{\omega }}_x& {\mathit{\omega }}_y& {\mathit{\omega }}_z\\ \mathrm{0}& {\mathit{\omega }}_x& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& {\mathit{\omega }}_x\end{array}& \begin{array}{ccc}\mathrm{0}& \mathrm{0}& \mathrm{0}\\ {\mathit{\omega }}_y& {\mathit{\omega }}_z& \mathrm{0}\\ \mathrm{0}& {\mathit{\omega }}_y& {\mathit{\omega }}_z\end{array}\end{array}\right]\\ {\overline{\mathit{I}}=\begin{array}{cc}[\begin{array}{ccc}{\mathit{I}}_{\mathrm{11}}& {\mathit{I}}_{\mathrm{12}}& {\mathit{I}}_{\mathrm{13}}\end{array}& \begin{array}{ccc}{\mathit{I}}_{\mathrm{22}}& {\mathit{I}}_{\mathrm{23}}& {\mathit{I}}_{\mathrm{33}}\end{array}\end{array}]}^{\mathrm{T}}\\ \stackrel{̿}{\mathit{I}}=\begin{array}{cc}\begin{array}{ccc}[{\mathit{I}}_{\mathrm{11}}& \mathrm{2}{\mathit{I}}_{\mathrm{12}}& \mathrm{2}{\mathit{I}}_{\mathrm{13}}\end{array}& \begin{array}{ccc}{\mathit{I}}_{\mathrm{22}}& \mathrm{2}{\mathit{I}}_{\mathrm{23}}& {\mathit{I}}_{\mathrm{33}}]\end{array}\end{array}\end{array}\right\}（38）

因此，可以证明以下用于线性化动力学模型的运算规则是成立的：

\left.\begin{array}{l}\mathit{I}\mathit{\omega }=\tilde{\mathit{\omega }}\overline{\mathit{I}}\\ {\mathit{\omega }}^{\mathrm{T}}\mathit{I}\mathit{\omega }=\stackrel{̿}{\mathit{I}}\overline{\mathit{\omega }{\mathit{\omega }}^{\mathrm{T}}}\end{array}\right\}（39）

2 基本动力学参数求解

为了便于求解动力学参数，假设动平台不受外力和外力矩的作用，平台动力学方程简化为

\mathit{\tau }={\mathit{J}}^{-\mathrm{T}}\mathit{\Psi }\mathit{X}=\mathit{k}\mathit{X}（40）

式中： k 为动态系数矩阵。

为了消除动力学参数中的冗余项，需在平台执行一定轨迹运动过程中采集多组不同姿态下的 k 和 τ，并构建如下超定方程组：

{\mathit{T}}_N={\mathit{K}}_N\mathit{X}（41）

式中：{\mathit{T}}_N为N组力传感器值组成的\mathrm{6}N\times \mathrm{1}维向量，{\mathit{T}}_N=({\mathit{\tau }}_{\mathrm{1}}^{\mathrm{T}},{\mathit{\tau }}_{\mathrm{2}}^{\mathrm{T}},\cdots ,{\mathit{\tau }}_N^{\mathrm{T}}{)}^{\mathrm{T}}；{\mathit{K}}_N为\mathrm{6}N\times \mathrm{29}阶观测矩阵，{\mathit{K}}_N=[{\mathit{k}}_{\mathrm{1}}^{\mathrm{T}}{\mathit{k}}_{\mathrm{2}}^{\mathrm{T}}\cdots {\mathit{k}}_N^{\mathrm{T}}{]}^{\mathrm{T}}。

2.1 激励轨迹的优化

激励轨迹的优劣对动力学参数求解的精度具有显著影响，为减少轨迹优化过程中的变量数量并充分激励系统的动力学特性，采用五阶傅里叶级数^［14］表示动平台原点的激励轨迹：

q_i(t)=q_{i\mathrm{0}}+{\displaystyle \sum _{j=\mathrm{1}}^{\mathrm{5}}}[\frac{a_{ij}}{k{\omega }_{\mathrm{f}}}\mathrm{s}\mathrm{i}\mathrm{n}({\omega }_{f}jt)-\frac{b_{ij}}{k{\omega }_{\mathrm{f}}}\mathrm{c}\mathrm{o}\mathrm{s}({\omega }_{f}jt)]（42）

式中：(q_{\mathrm{1}}\left(t\right),q_{\mathrm{2}}\left(t\right),\cdots ,q_{\mathrm{6}}{\left(t\right))}^{\mathrm{T}}为t时刻动平台位姿；{\omega }_{\mathrm{f}}为基础频率；a_{ij}、b_{ij}分别为正弦和余弦的幅值；q_{i\mathrm{0}}为动平台的初始位姿。

研究表明^［15］，观测矩阵{\mathit{K}}_N的条件数可用来评估激励轨迹的质量。为求解优化轨迹，在保证运动连续性及物理约束条件下，以最小化观测矩阵的条件数为优化目标，建立如下优化问题：

（43）

式中：t_{\mathrm{0}}、t_{\mathrm{e}\mathrm{n}\mathrm{d}}分别为一个周期的起始和终止时刻；l_{\mathrm{m}\mathrm{i}\mathrm{n}}、l_{\mathrm{m}\mathrm{a}\mathrm{x}}、{\dot{l}}_{\mathrm{m}\mathrm{a}\mathrm{x}}^{}和{\ddot{l}}_{\mathrm{m}\mathrm{a}\mathrm{x}}^{}分别为分支允许的最小长度、最大长度、最大速度和最大加速度；cond（·）为计算矩阵条件数函数。

2.2 观测矩阵QR分解法

在优化后的激励轨迹下，采集并计算得到观测矩阵{\mathit{K}}_N。经数值检验可知，{\mathit{K}}_N为非列满秩矩阵，表明其对应的动力学参数 X 中存在耦合项，无法直接作为平台的基本动力学参数。为提取基本动力学参数，对{\mathit{K}}_N进行QR分解：

{\mathit{K}}_N\mathit{P}=[\begin{array}{cc}{\mathit{Q}}_{\mathrm{b}}& {\mathit{Q}}_{\mathrm{d}}\end{array}]\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{b}}& {\mathit{R}}_{\mathrm{d}}\\ \mathrm{0}& \mathrm{0}\end{array}\right]=

[\begin{array}{cc}{\mathit{Q}}_{\mathrm{b}}{\mathit{R}}_{\mathrm{b}}& {\mathit{Q}}_{\mathrm{b}}{\mathit{R}}_{\mathrm{d}}\end{array}]=[\begin{array}{cc}{\mathit{K}}_{\mathrm{b}N}& {\mathit{K}}_{\mathrm{d}N}\end{array}]（44）

式中：\mathit{Q}为正交矩阵，\mathit{Q}=[{\mathit{Q}}_{\mathrm{b}}{\mathit{Q}}_{\mathrm{d}}]；\mathit{P}为置换矩阵；{\mathit{R}}_{\mathrm{b}}为n_{\mathrm{b}}\times n_{\mathrm{b}}阶上三角矩阵。

因此基本观测矩阵{\mathit{K}}_{\mathrm{b}N}的列秩为n_{\mathrm{b}}，即基本动力学参数的个数为n_{\mathrm{b}}。

基本动力学参数可以由如下矩阵变换得到：

{\mathit{K}}_N\mathit{X}={\mathit{K}}_N\mathit{P}{\mathit{P}}^{\mathrm{T}}\mathit{X}={\mathit{K}}_{\mathrm{b}N}\left[\begin{array}{cc}\mathit{E}& {\mathit{R}}_{\mathrm{b}}^{-\mathrm{1}}{\mathit{R}}_{\mathrm{d}}\end{array}\right]{\mathit{P}}^{\mathrm{T}}\mathit{X}={\mathit{K}}_{\mathrm{b}N}{\mathit{X}}_{\mathrm{b}}（45）

式中：{\mathit{X}}_{\mathrm{b}}为基本动力学参数，其维数为n_{\mathrm{b}}\times \mathrm{1}。

根据{\mathit{K}}_N到{\mathit{K}}_{\mathrm{b}N}的矩阵变换，最终可得到对应的动态系数矩阵{\mathit{k}}_{\mathrm{b}}。

3 模型验证与参数辨识

3.1 仿真实验平台

根据所研究的真实样机的实际构型布局以及初步的CAD模型参数，利用MATLAB软件中的Simulink/Simscape/SimMechanics模块构建了对应的仿真模型，如图6所示。仿真模型的整体逻辑块如图7a所示，分支的逻辑块如图7b所示。

3.2 基本动力学参数确定

为了简化激励轨迹的优化过程，本文选定周期为10 s（即{\omega }_{\mathrm{f}}=\mathrm{0.2}\mathrm{\pi }），并将电动缸伸长一半时平台的位姿作为初始位姿q_{i\mathrm{0}}，以10 ms的时间间隔采集一个周期的数据，构建观测矩阵{\mathit{K}}_N。随后，利用MATLAB中的fmincon函数迭代求解式（43）所述的优化问题，得到动平台相对于初始位姿的激励轨迹增量，如图8a所示。最终，通过逆运动学计算得到对应的连杆长度激励轨迹增量，如图8b所示。

根据优化激励轨迹对应的{\mathit{K}}_N，按照2.2节方法进行QR分解。最终得到6-UPRU并联机器人的基本动力学参数{\mathit{X}}_{\mathrm{b}}，其维数为\mathrm{17}\times \mathrm{1}，如表3所示。表3中，{\mathit{r}}_{\mathrm{c}}^{\{p\}}=(r_{\mathrm{c}x}^{\{p\}},r_{\mathrm{c}y}^{\{p\}},r_{\mathrm{c}z}^{\{p\}}{)}^{\mathrm{T}}，I_{\mathrm{c}mn}^{\{p\}}表示{\mathit{I}}_{\mathrm{c}}^{\{p\}}的第m行第n列元素。其对应的基本动态系数矩阵如下：

{\mathit{k}}_{\mathrm{b}}={\mathit{J}}^{-\mathrm{T}}{\mathit{\Psi }}_{\mathrm{b}}（46）

{\mathit{\Psi }}_{\mathrm{b}}=\left[\begin{array}{l}{\mathit{\Psi }}_{\mathrm{f}\mathrm{1}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{3}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{4}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{5}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{6}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{10}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{11}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{12}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{15}}{\mathit{\Psi }}_{\mathrm{f}\mathrm{16}}\\ {\mathit{\Psi }}_{\mathrm{m}\mathrm{1}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{3}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{4}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{5}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{6}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{10}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{11}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{12}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{15}}{\mathit{\Psi }}_{\mathrm{m}\mathrm{16}}\end{array}\right]

因此，在不考虑外力和外力矩作用的情况下， 6-UPRU并联机器人的动力学模型可简化为如下线性形式：

\mathit{\tau }={\mathit{J}}^{-\mathrm{T}}{\mathit{\Psi }}_{\mathrm{b}}{\mathit{X}}_{\mathrm{b}}={\mathit{k}}_{\mathrm{b}}{\mathit{X}}_{\mathrm{b}}（47）

3.3 模型验证

为了验证构建的6-UPRU并联机器人动力学模型及其基本动力学参数的正确性，本节基于SimMechanics模型与ADAMS模型分别进行仿真实验，并开展基本动力学参数的辨识实验。

3.3.1 动力学模型验证

为了验证所构建的动力学模型的准确性，首先在MATLAB的SimMechanics模型中对六个驱动分支施加正弦形式的伸长量激励：

∆l_i=A_i\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{2}\mathrm{\pi }t/T_i)（48）

其中，A_i和T_i分别表示第i个驱动分支的幅值和周期，且各分支的参数均不相同。在每个仿真周期内，分别计算完整动力学模型和简化模型对力传感器的理论解析值，并记录对应力传感器的测量值，结果如图9所示。通过误差统计分析，两种动力学模型的解析值相对于SimMechanics模型测量值的均方根误差均小于10{}^{-\mathrm{4}} N。

为了进一步验证所提出动力学模型的准确性，在ADAMS软件中搭建了相应的虚拟样机模型，如图10所示。对驱动分支施加新的正弦轨迹激励进行仿真实验，结果如图11所示。实验结果表明，完整动力学模型与简化模型的理论解析值与ADAMS仿真测量值在动态响应中均呈现出较高的一致性，其均方根误差稳定在10{}^{-\mathrm{3}}数量级，进一步证明了所提出的完整动力学模型以及简化动力学模型的准确性。

3.3.2 基本动力学参数辨识实验

本文所研究的6-UPRU并联机器人的真实物理样机如图12所示。主要硬件包括倍福CX5130控制器、INTERFACE 1210型力传感器、松下MINAS A6B系列伺服电机及6-UPRU并联装置，电气设备集成于电控柜中。

基于2.1节所述的激励轨迹优化方法，以基本动态系数矩阵{\mathit{k}}_{\mathrm{b}}构成的观测矩阵{\mathit{K}}_{\mathrm{b}N}的条件数最小化为优化目标，优化得到机器人的激励轨迹。在优化的激励轨迹下进行实验，采用10 ms的采样周期采集数据，计算获得观测矩阵{\mathit{K}}_{\mathrm{b}N}和测量力向量{\mathit{T}}_N。为抑制随机噪声干扰，重复实验10次并取测量均值。经数值检验，观测矩阵{\mathit{K}}_{\mathrm{b}N}为列满秩矩阵，满足线性系统参数辨识条件，故可采用最小二乘法对基本动力学参数进行辨识。进一步，利用辨识出的基本动力学参数，根据式（47）对运动过程中的力传感器值进行拟合，结果见图13。

基于基本动力学参数辨识值对力传感器实测数据进行拟合，结果表明，拟合值和测量值之间的均方根误差为14.55 N，平均绝对误差为11.52 N，该误差显著小于力传感器实测力值范围。因此，本文构建的6-UPRU并联机器人动力学模型及其基本动力学参数的准确性得到了充分验证。

4 结论

通过对UPRU分支进行精确的运动学分析，并基于牛顿-欧拉法构建了精确的动力学模型，有效避免了以往研究中因构型简化而引入的建模误差。针对并联机器人动力学模型高度耦合性导致的线性化困难问题，提出了模型线性化运算规则，成功构建了动力学模型关于动力学参数的线性表达形式。在此基础上，以最小化观测矩阵条件数为优化目标，设计了满足物理约束的激励轨迹，并通过观测矩阵的QR分解，得到了基本动力学参数及对应的简化动力学模型，将动力学参数从29个减少至17个，成功解决了参数冗余问题。最后，通过SimMechanics和ADAMS两个仿真平台在不同正弦轨迹下的力拟合仿真实验以及基本动力学参数辨识实验验证了理论模型的正确性。

参考文献

[1] XIN Guiyang， DENG Hua， ZHONG Guoliang. Closed-form Dynamics of a 3-DOF Spatial Parallel Manipulator by Combining the Lagrangian Formulation with the Virtual Work Principle［J］. Nonlinear Dynamics， 2016，86：1329-1347.

[2] ZHOU Zhou， GOSSELIN C. Simplified Inverse Dynamic Models of Parallel Robots Based on a Lagrangian Approach［J］. Meccanica， 2024，59（4）：657-680.

[3] KLEIN D， GAMARRA D F T. Dynamic Modeling Using the Principle of Virtual Work and Decentralized PD Control of a Stewart-gough Platform［C］∥2022 Latin American Robotics Symposium （LARS）， 2022 Brazilian Symposium on Robotics （SBR）， and 2022 Workshop on Robotics in Education （WRE）.2022：1-6.

[4] CHEN Zhengsheng， XU Lingming， ZHANG Weizhong， et al. Closed-form Dynamic Modeling and Performance Analysis of an Over-constrained 2PUR-PSR Parallel Manipulator with Parasitic Motions［J］. Nonlinear Dynamics， 2019，96：517-534.

[5] ZHANG Guoxing， HE Jianliang， GUO Jinwei， et al. Dynamic Modeling and Vibration Characteristics Analysis of Parallel Antenna［J］. Mechanical Sciences， 2022，13（2）：1019-1029.

[6] YANG Songlin， YANG Haibo， ZHANG Xiang， et al. Design of Active Vibration Isolation System for Satellite Optical Load Based on Stewart Platform［C］∥Journal of Physics：Conference Series. IOP Publishing， 2024，2746（1）：012008.

[7] KHANBABAYI E， NOORANI M R S. Design Computed Torque Control for Stewart Platform with Uncertainty to the Rehabilitation of Patients with Leg Disabilities［J］. Computer Methods in Biomechanics and Biomedical Engineering， 2024，27（8）：1028-1041.

[8] MAYEDA H， YOSHIDA K， OSUKA K. Base Parameters of Manipulator Dynamic Models［C］∥Proceedings. 1988 IEEE International Conference on Robotics and Automation. IEEE， 1988：1367-1372.

[9] TAN Chao， ZHAO Huan， DING Han. Identification of Dynamic Parameters of Closed-chain Industrial Robots Considering Motor Couplings ［J］. Computers and Electrical Engineering， 2022，99：107740.

[10] GAUTIER M. Numerical Calculation of the Base Inertial Parameters of Robots［J］. Journal of Robotic Systems， 1991，8（4）：485-506.

[11] YANG Chenguang， JIANG Yiming， HE Wei， et al. Adaptive Parameter Estimation and Control Design for Robot Manipulators with Finite-time Convergence［J］. IEEE Transactions on Industrial Electronics， 2018，65（10）：8112-8123.

[12] TADESE M， PICO N， SEO S， et al. A Two-step Method for Dynamic Parameter Identification of Indy7 Collaborative Robot Manipulator［J］. Sensors， 2022，22（24）：9708.

[13] HARIB K， SRINIVASAN K. Kinematic and Dynamic Analysis of Stewart Platform-based Machine Tool Structures［J］. Robotica， 2003，21（5）：541-554.

[14] 郭菲， 李永泉， 李玉昆， 等. 电液驱动3-UPS/S并联稳定平台的动力学参数辨识［J］. 中国机械工程， 2016，27（21）：2862-2868.

[15] GUO Fei， LI Yongquan， LI Yukun， et al. Identification of Dynamics Parameters for an Electro-Hydraulic 3-UPS/S Parallel Stabilized Platform［J］. China Mechanical Engineering， 2016，27（21）：2862-2868.

[16] DANAEI B， ARIAN A， TALE Masouleh M， et al. Dynamic Modeling and Base Inertial Parameters Determination of a 2-DOF Spherical Parallel Mechanism［J］. Multibody System Dynamics， 2017， 41：367-390.

基金资助

吉林省科技发展计划(20200401130GX)