Stewart并联机构以其刚度大、承载力强等特点弥补了串联机器人的不足，在工业领域具有广阔的应用前景。根据执行器的分布不同大致分为3-3、5-4、6-5和6-6等构型^[1]。并联机构运动学作为机器人领域的重点问题得到了国内外学者的广泛研究，实时精确的运动学正解算法对于并联机构的控制影响显著。

Stewart并联机构位置正解需要求解一组强耦合非线性方程组，尚无公认正解解法能兼顾计算的高效与精确，而逆解相对容易。传统正解方法分为数值法和解析法，数值法^[2-3]核心为迭代计算，计算量较大，计算的结果对初值敏感，迭代初值选取不当则结果可能发散；解析法通过建立约束方程^[4-5]、消元^[6-7]和几何分析^[8]等方法建立了几种特定结构并联平台的封闭解，该类算法无需初值且能避免位置奇异，但是公式推导复杂，仅适用于特定结构的并联平台，具有局限性。

近年来，学者们开始借鉴智能算法解决位置正解问题，以此弥补传统位置正解算法缺陷，神经网络无需推导输入输出的显性表达式，但是得到精确解需要大量样本数据进行学习训练^[9]。神经网络算法中，BP神经网络研究最为普遍，不能同时满足求解的精确性和快速性，众多学者基于该算法进行改进^[10-13]。此外，Morell等^[14]使用支持向量回归的方法求解，思路新颖，但该方法模型训练时间过长。刘伟锐等^[15]基于PSO算法，吴小勇等^[16]、谢志江等^[17]改进了蚁群算法应用于正解问题，未能验证求解耗时。Mahmoodi等^[18]通过安装旋转传感器实时采集位姿数据，摆脱了平台构型的限制，通用性高，但该方法精确度依赖于传感器灵敏度，且信号处理占用了大量运算资源。

为了克服以上算法思路的不足，本文提出一种混合算法，结合非线性系统线性化的思路和计算机的计算流程，使用速度迭代减少了总迭代次数，缩短了计算时间，最后通过试验对比验证了本文算法的精确性与实时性。

1 并联机构正解的混合算法

不同于以往学者孤立地求解单个点的位置正解，本文算法融合传统算法与智能算法的优点，基于轨迹运动过程实时求解。由于计算机解算位置正解时以极短的采样周期对运动数据进行采样，之后对此进行解算，根据这一特性将计算过程离散化，利用上一采样点数据进行速度迭代得出当前时刻的位置正解以节约运算资源。

混合算法由3个部分组成：BP神经网络、雅克比速度迭代和牛顿迭代法。计算机采样点序列为k，其中，初始采样点(k=1)的位置正解由BP神经网络和牛顿迭代法处理得出，如图 1所示。其中，BP神经网络仅在位姿初始值未知时执行一次，用于提供后续计算的初始估计值。若位姿初始值已知，则不启用BP神经网络。k=2和之后的位置正解过程如图 2，其中速度迭代的作用是通过线性化计算减少总迭代次数。

算法可分为4个步骤：

1) 建立运动学模型：根据各执行器初始状态时的上下连接点坐标建立运动学反解方程，在此基础上建立非线性方程组作为运动学正解求解公式。

2) 神经网络训练：获取若干组工作空间内的位姿与对应的执行器长度，交换数据集映射关系作为BP神经网络的训练样本，使用训练完毕的神经网络输出估计值作为后续计算的初始值。

3) 速度迭代：依据运动的线性近似求得位姿变化速度，与前一采样周期累加得到速度迭代的结果，并更新雅可比矩阵。

4) 速度迭代的结果代入误差判断公式，若不满足精度要求，则继续牛顿迭代直至满足精度要求。

2 运动学模型

Stewart平台存在多种构型，本文以6-6构型平台为例建立坐标系并分析其运动学问题，同样的正解思路对其他构型的并联机构也可移植应用。

2.1 坐标系建立与运动学反解

6-6构型Stewart平台由6个执行器连接基座和上平台，动坐标系与上平台固连，静坐标系建立在基座中心位置。运动学反解的基本思路是：根据动静坐标系以及欧拉角旋转变换公式推导出动坐标系中的点到静坐标系的位姿转换矩阵，求出6个执行器上铰点在静坐标系中的坐标，根据空间向量二范数公式求出对应执行器的长度，得到上平台位姿反解公式^[19]。根据本文并联机构机械参数，为方便矩阵换算，将静坐标系建立在各执行器伸长量为零时对应的动坐标系位置，与该位置动坐标系重合。平台坐标系如图 3。

执行器上铰点A_i(i=1, 2, …, 6)在动坐标系中的矢量为：

$ {\mathit{\boldsymbol{A}}_i} = {[{A_{ix}}\quad {A_{iy}}\quad {A_{iz}}]^{\rm{T}}} $

(1)

执行器下铰点B_i(i=1, 2, …, 6)在静坐标系中的矢量为：

$ {\mathit{\boldsymbol{B}}_i} = {[{B_{ix}}\quad {B_{iy}}\quad {B_{iz}}]^{\rm{T}}} $

(2)

动坐标系位姿表示为：

$ \mathit{\boldsymbol{P}} = {[{\mathit{\boldsymbol{\theta }}^{\rm{T}}}|{\mathit{\boldsymbol{O}}_p}^{\rm{T}}]^{\rm{T}}} = {\left[ {\begin{array}{*{20}{l}} \alpha &\beta &\gamma &x&y&z \end{array}} \right]^{\rm{T}}} $

(3)

式中：O_p为动坐标系原点在静坐标系中的位置矢量；θ为动坐标系绕x、y、z轴的欧拉角矢量；A_iB_i由编号为i的执行器连接。动坐标系相对静坐标系的旋转矩阵为：

$ \mathit{\boldsymbol{R}} = \left[ {\begin{array}{*{20}{c}} {{\rm{c}}\beta {\rm{c}}\gamma }&{ - {\rm{c}}\beta {\rm{s}}\gamma }&{{\rm{s}}\beta }\\ {{\rm{s}}\alpha {\rm{s}}\beta {\rm{c}}\gamma + {\rm{c}}\alpha {\rm{s}}\gamma }&{ - {\rm{s}}\alpha {\rm{s}}\beta {\rm{c}}\gamma + {\rm{c}}\alpha {\rm{c}}\gamma }&{ - {\rm{s}}\alpha {\rm{c}}\beta }\\ { - {\rm{c}}\alpha {\rm{s}}\beta {\rm{c}}\gamma + {\rm{s}}\alpha {\rm{s}}\gamma }&{{\rm{c}}\alpha {\rm{s}}\beta {\rm{s}}\gamma + {\rm{s}}\alpha {\rm{c}}\gamma }&{{\rm{c}}\alpha {\rm{s}}\beta } \end{array}} \right] $

(4)

式中s表示sin函数，c表示cos函数。上平台运动时，上铰点在动坐标系中的向量可变换为该点在静坐标系中的向量，各执行器长度，即运动学反解公式可表示为：

$ {L_i} = \sqrt {\left\| {\mathit{\boldsymbol{R}}{\mathit{\boldsymbol{A}}_i} + {\mathit{\boldsymbol{O}}_p} - {\mathit{\boldsymbol{B}}_i}} \right\|} = {f_i}(\mathit{\boldsymbol{P}}) $

(5)

式中L_i为第i个执行器的长度，i=1, 2，…，6。

2.2 运动学正解 2.2.1 BP神经网络

BP神经网络对非线性映射有很好的逼近能力，能实现从平台杆长变量空间到工作变量空间的映射，从而求得运动学正解^[13]，6-6构型平台所用神经网络基本结构如图 4所示。

本文BP神经网络用于提供初始估计值，精度要求不高，为节约神经网络训练时间，采用三层神经网络。输入向量为执行器长度，输出向量为正解位姿向量，输入层、输出层各由6个神经元组成。根据下式确定隐藏层节点数：

$ N = \sqrt {io} $

(6)

式中：i、o分别为输入层、输出层节点数。

训练神经网络需要合适的训练样本。根据平台的运动空间和执行器硬件参数，为保证神经网络的鲁棒性，在6个自由度上随机取值，由式(5)得到平台反解数据的映射为{α β γ x y z}→{L₁ L₂ L₃ L₄ L₅ L₆}，再将2组数据集映射方向逆转即得到位置正解的神经网络学习样本。

轨迹运动经采样后得到离散点序列，实时计算位置正解时计算机以采样周期T采集执行器长度，求解对应时刻位置正解。

由于样本数量有限，神经网络输出结果一般不能达到精确度要求，需要用牛顿迭代法得出轨迹运动初始采样点的位置正解，作为后续速度迭代的初始基准值。

2.2.2 线性近似与速度迭代

由于计算机采样周期足够短，且轨迹运动时执行器速度不会产生较大突变，执行器瞬时速度计算公式为：

$ {\dot L_i}(k - 1) \approx \frac{{{L_i}(k) - {L_i}(k - 1)}}{T} $

(7)

式中：T为采样周期；k为采样点序列。

雅克比矩阵将关节空间速度映射到笛卡尔空间速度，可用于求解位姿变化速度。式(5)两端求导得到位姿变化速度与执行器速度的关系：

$ \left[\begin{array}{c} \dot{L}_{1} \\ \dot{L}_{2} \\ \dot{L}_{3} \\ \dot{L}_{4} \\ \dot{L}_{5} \\ \dot{L}_{6} \end{array}\right]=\left[\begin{array}{llllll} \frac{\partial f_{1}}{\partial \alpha} & \frac{\partial f_{1}}{\partial \beta} & \frac{\partial f_{1}}{\partial \gamma} & \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial z} \\ \frac{\partial f_{2}}{\partial \alpha} & \frac{\partial f_{2}}{\partial \beta} & \frac{\partial f_{2}}{\partial \gamma} & \frac{\partial f_{2}}{\partial x} & \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial z} \\ \frac{\partial f_{3}}{\partial \alpha} & \frac{\partial f_{3}}{\partial \beta} & \frac{\partial f_{3}}{\partial \gamma} & \frac{\partial f_{3}}{\partial x} & \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial z} \\ \frac{\partial f_{4}}{\partial \alpha} & \frac{\partial f_{4}}{\partial \beta} & \frac{\partial f_{4}}{\partial \gamma} & \frac{\partial f_{4}}{\partial x} & \frac{\partial f_{4}}{\partial y} & \frac{\partial f_{4}}{\partial z} \\ \frac{\partial f_{5}}{\partial \alpha} & \frac{\partial f_{5}}{\partial \beta} & \frac{\partial f_{5}}{\partial \gamma} & \frac{\partial f_{5}}{\partial x} & \frac{\partial f_{5}}{\partial y} & \frac{\partial f_{5}}{\partial z} \\ \frac{\partial f_{6}}{\partial \alpha} & \frac{\partial f_{6}}{\partial \beta} & \frac{\partial f_{6}}{\partial \gamma} & \frac{\partial f_{6}}{\partial x} & \frac{\partial f_{6}}{\partial y} & \frac{\partial f_{6}}{\partial z} \end{array}\right]\left[\begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \\ \dot{x} \\ \dot{y} \\ \dot{z} \end{array}\right]=\boldsymbol{J} \dot{\boldsymbol{P}} $

(8)

式中J为雅克比矩阵。

求解位置正解时，当前时刻与上一时刻执行器长度由计算机采集，为已知变量，式(7)代入式(8)等号左侧，可求得位姿变化速度$\mathit{\boldsymbol{\dot P}}$。得出当前时刻的位姿为：

$ \mathit{\boldsymbol{P}}(k) \approx \mathit{\boldsymbol{P}}(k - 1) + \mathit{\boldsymbol{\dot P}}(k - 1)T $

(9)

由于第k个点的正解精确度依赖上一周期的正解，且将采样间隔的执行器理想化为匀速运动，此时求得正解有微小偏差，为避免误差累积，式(9)得出的数据需进一步验证。

2.2.3 牛顿迭代法

牛顿迭代法求解精确，但收敛效果依赖初值选取。式(9)单次所求结果已经逼近真实值，不会造成迭代的离散。

设置允许的误差阈值为ε，将式(9)结果代入式(5)，以此求得的执行器长度与实际长度对比：

$ \sum\limits_{i = 1}^6 | {L_{iy}}(k) - {L_i}(k)| < \varepsilon $

(10)

式中L_iy为式(9)求得的位姿经反解得到的各执行器长度。

若式(9)结果使式(10)成立，则求得位置正解足够精确，以此次结果计算雅克比矩阵并作为下一采样周期的累加值；若不满足精度要求，则用牛顿迭代法校正。为表示方便，将式(3)重写为：

(11)

式中：q₁、q₂、q₃为欧拉角变量; q₄、q₅、q₆为位置变量。

迭代的目标函数为：

$ h_{i}\left(\begin{array}{llllll} q_{1} & q_{2} & q_{3} & q_{4} & q_{5} & q_{6} \end{array}\right)=\left\|f_{i}(\boldsymbol{P})\right\|^{2}-L_{i}{}^{2}=0 $

(12)

用向量符号为：

$ \boldsymbol{H}=\left[\begin{array}{llll} h_{1} & h_{2} & \cdots & h_{6} \end{array}\right]^{\mathrm{T}} $

(13)

将式(9)得到的解代入下式:

$ \boldsymbol{P}^{(n+1)}=\boldsymbol{P}^{(n)}-\boldsymbol{H}^{\prime}\left(\boldsymbol{P}^{(n)}\right)^{-1} \boldsymbol{H}\left(\boldsymbol{P}^{(n)}\right) $

(14)

其中：

$ \boldsymbol{H}^{\prime}=\left[\begin{array}{ccc} \frac{\partial h_{1}}{\partial q_{1}} & \cdots & \frac{\partial h_{1}}{\partial q_{6}} \\ \vdots & \cdots & \vdots \\ \frac{\partial h_{6}}{\partial q_{1}} & \cdots & \frac{\partial h_{6}}{\partial q_{6}} \end{array}\right] $

n为牛顿迭代的次数。P⁽⁰⁾=P(k)；n=0，1，…。

求得结果代入反解公式，若求得执行器长度与采集长度对比满足式(10)，则停止迭代，否则继续迭代直到对应反解满足精度要求^[20]。轨迹中的位置正解计算流程如图 5。

3 实例验证

本文以MOOG公司生产的MB-E-6DOF并联机构为验证平台，实物图如图 6。

3.1 BP神经网络训练

由Stewart并联机构机械参数得到执行器伸长量为零时各执行器铰点位置坐标见表 1，A为上铰点，B为下铰点，其中上铰点坐标相对于动坐标系，下铰点坐标相对静坐标系。

为使执行器远离正负限位，各执行器在开始运动前伸长量设置为16.23 cm。

由式(6)可得隐藏层含有6个神经元。设定上平台平移范围为±20 cm，旋转范围±20 rad，在该范围内随机生成1 100组映射，其中1 000组作为学习样本，其余100组作为测试样本。经实验可知，激励函数选为tansig时训练时间最短，误差下降曲线如图 7。测试样本输出误差如图 8。

由图 7可知，神经网络在经过220次迭代后达到输出误差要求。由图 8知，BP神经网络输出误差在6%以内，满足作为迭代初值的要求。在初始值未知的情况下可由BP神经网络输出正解估计值，再经牛顿迭代得到轨迹运动初始采样点正解的精确值。

该神经网络训练时间为5 s，在不同构型的并联平台移植时能节约大量时间。

3.2 算法对比

为验证混合算法的效果，本文从计算时间和迭代次数2个方面与牛顿迭代法进行对比。程序在Windows 7，Matlab 2012a环境下运行，计算机配置为Intel i3CPU，内存4 GB，采用Matlab自带的秒表计时器统计计算时间。

使平台1号、4号执行器长度按正弦函数变化，幅值5.08 cm，频率0.3 Hz，其余执行器长度不变，在轨迹运动某一时刻开始每隔10 ms采集各执行器实际长度，共采集4 666组执行器长度数据。设定误差阈值ε为0.001 cm，分别使用牛顿迭代法和混合算法解算位置正解，牛顿迭代法初始采样点迭代初值选取为BP神经网络的输出结果，其余点迭代初值选取为上一采样点经牛顿迭代的位置正解。每组数据解算时间和迭代次数如图 9。

由图 9可知，混合算法求取位置正解能有效缩短解算时间，减少迭代次数。允许误差上限相同时，新算法较牛顿迭代法减少耗时约50%。由于2种算法设定误差阈值相同，故计算精确度相同。

值得注意的是，混合算法在初始采样点发生了3次迭代，之后的采样点牛顿迭代次数为零，说明经速度迭代计算后得出的位置正解误差已经符合要求，且速度迭代的收敛速度更快。尽管如此，考虑到计算机传输数据失帧等特殊情况，为避免误差累积，混合算法中的牛顿迭代法部分需要保留。

平台机械参数已知，即位置反解公式已知，由本文算法可快速得出相应的雅克比矩阵，在不同构型的平台间移植简单易行。

4 结论

1) 本文针对Stewart平台位置正解问题提出了结合传统算法与智能算法的混合算法，以6-6 Stewart平台为例对算法进行了验证。在相同精度要求下，新算法较牛顿迭代法耗时更短，具有更强实时性。

2) 本文算法可应用于并联平台的多线程任务控制，为其他耗时较长的任务，如视觉处理、与其他设备的通信节约计算资源。

由于条件有限，未在更多构型的并联平台上验证，后续研究将在其他构型的并联平台上应用验证。由于姿态矩阵采用欧拉角方式描述，为防止并联平台失控，在轨迹规划阶段避免了奇异情况，该情况有待在后续工作中研究。