(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111566416.6 (22)申请日 2021.12.20 (71)申请人 燕山大学 地址 066004 河北省秦皇岛市海港区河北 大街西段438号 (72)发明人 张立杰　吴焕春　戴闯　许磊涛　 (74)专利代理 机构 北京孚睿湾知识产权代理事 务所(普通 合伙) 11474 代理人 王冬杰 (51)Int.Cl. G06F 30/17(2020.01) G06F 30/27(2020.01) B25J 9/16(2006.01) (54)发明名称 改进的Stewart并联机构正运动学参数求解 方法 (57)摘要 本发明涉及一种改进的Stewart并联机构正 运动学参数求解方法， 其包括以下步骤， 步骤一： 搭建Stewart并联机构平台， 根据平台的实际参 数确定平台铰点的初始坐标； 步骤二： 根据给定 随机位姿计算出训练样本； 步骤三： 利用SSA优化 BP神经网络得到位置正解； 步骤四： 应用NAd1法 求解并联机构平台正运动学的结果。 本发明提出 了NAd1法与SSA算 法优化神经网络 结合的混合算 法， 有效解决了Stewart并联机构平台正运动学 求解问题； 本发明的应用有效减小了Stewart并 联机构平台正运动学计算结果的误差， 并能解决 局部最优问题； 提高了算法整体迭代精度， 减少 了迭代次数。 权利要求书4页 说明书11页 附图6页 CN 114329825 A 2022.04.12 CN 114329825 A 1.一种改进的Stewart并联机构正运动学参数求解方法， 其特征在于， 其包括以下步 骤： 步骤1： 搭建Stewart并联机构平台， 根据Stew art并联机构平台的实际参数进一步确定 平台铰点的初始坐标； 步骤2： 根据给定随机位姿计算出训练样本； 步骤21： 根据动平台位姿， 确定 旋转矩阵； 动平台的位姿公式如下式所示： P＝[xp yp zp α β γ]T 式中： P表示动平台的位姿， 即位置和姿态； xp表示动平台的水平横向位移； yp表示动平 台的水平纵向位移； zp表示动平台的竖直方向位移； α表示动坐标系绕静坐标系的x轴旋转 角度； β 表 示动坐标系绕静坐标系的y轴旋转角度； γ表 示动坐标系绕静坐标系的z轴旋转角 度； 根据动平台的位姿， 则旋转矩阵如下式所示： R( α, β,γ)＝Rot(z,γ)Rot(y, β )Rot(x, α ) 式中： R表示关于γ、 β 、 α 的旋转矩阵； Rot(z,γ)表示动坐标系绕静坐标系的z轴旋转角 度的计算结果； Rot(y, β )表示动坐标系绕静坐标系的y轴旋转角度的计算结果； Rot(x, α )表 示动坐标系绕静坐标系的x轴旋转角度的计算结果； x、 y、 z分别表示静坐标系的三个坐标 轴； 步骤22： 确定并联机构的广义 坐标； 根据旋转矩阵得到第s个支腿的长度公式为： 式中： s表示支腿的编号； ls表示第s个支腿的长度； Op为动坐标系原点在静坐标系中的 位置； As表示第s个支腿上铰点的空间坐标； Bs表示第s个支腿上铰点的空间坐标； 则Stewar t并联机构的广义 坐标计算结果如下式所示： q＝(l1,…,l6)T 式中： q表示Stewart并联机构的广义坐标； l1，…， l6分别表示第1个到第6个支腿的长 度； 步骤23： 确定训练样本； 重复步骤21和步骤22， 给定随机动平台的位姿计算出对应的Stewart并联机构的广义 坐标， 保存随机动平台位姿与对应的广义 坐标作为训练样本； 步骤3： 利用S SA优化BP神经网络得到位置正 解， 其具体步骤如下： 步骤31： 获取步骤23计算得到的训练样本， 分为训练集和测试集； 初始化麻雀种群位置 与适应度， 设置最大迭代 步数N、 种群大小n、 发现者数量PD、 感应危险的麻雀数量SD、 安全值 ST、 预警值R2； 确定初始参数后开始循环； 步骤32： 构建迭代关系； 首先， 构造发现者的位置的迭代关系， 计算过程如下式所示； 权　利　要　求　书 1/4 页 2 CN 114329825 A 2式中： i表示个体编号； j表示位置编号； t表示迭代次数， 初始值为0； 表示t+1次迭代 发现者的位置； 表示t次迭代发现者的位置； α 为(0,1]之间均匀随机数； N表示设定的最 大 迭代次数； Q是服从正态分布的随机数； L是单位行向量； R2表示预警值； ST表示安全值； exp 表示以自然常数 e为底的指数函数； 其次， 构造加入者 位置的迭代关系， 计算过程如下式所示； 式中： 表示t+1次迭代加入者的位置； Y'worst表示适应度最低的加入者位置； 表示t 次迭代加入者的位置； 表示t+1迭代下加入者所占据的最优位置； n表示种群大小； A+为随 机包含1和‑1两个元素的行向量； otherw ise表示其他情况； 最后， 构建麻雀种群位置迭代关系， 计算过程如下式所示； 式中： 表示t+1次迭代麻雀种群的位置； 表示第t次迭代适应度最高的麻雀位置； β 为服从正态分布随机数； 表示t次迭代麻雀种群的位置； 表示第t次迭代适应度最 高 的麻雀位置； 表示第t次迭代适应度最低的麻雀位置； K是[ ‑1,1]之间的随机数； fi是第 i个体适应度值； fbest是个体最优适应度值； fworst是个体最差适应度值； ε是为常数； 步骤33： 重复执行步骤32， 达到最大迭代步数， 结束循环， 输出神经网络最优权值和阈 值， 保存训练好的BP神经网络； 步骤34： 调用保存好的神经网络训练得到预测值， 作 为Stewart并联机构平台的正解结 果， 输出得出初值位姿P0； 步骤4： 应用NAd1法求 解Stewar t并联机构平台正 运动学的结果， 具体步骤如下： 步骤41： 获取步骤34预测得到的初始位姿P0； 给定杆长允许误差 ε， 最大迭代步数N； 设置 迭代次数k ＝0； 步骤42： 构建位姿迭代关系， 具体 计算过程如下 所示： 输入Stewar t并联机构平台的初始杆长qs； 构造Stewar t并联机构平台正 解方程F(Pk)； F(Pk)＝q(Pk)‑qs 式中： Pk表示第k次迭代的正解； k表示位姿迭代次数； F(Pk)表示根据 位姿Pk计算出的正 解方程； q(Pk)表示根据位姿Pk计算求出的杆长； qs表示并联机构平台第s个杆长初始值； 根据位姿Pk计算迭代的中间变量yk； 式中： yk表示第k次迭代的中间变量； F'(Pk)表示根据位姿Pk计算出正 解方程的导数； 根据迭代的中间变量yk计算位姿Pk+1；权　利　要　求　书 2/4 页 3 CN 114329825 A 3