[发明专利]改进的Stewart并联机构正运动学参数求解方法

权利要求书

1.一种改进的Stewart并联机构正运动学参数求解方法，其特征在于，其包括以下步骤：

步骤1：搭建Stewart并联机构平台，根据Stewart并联机构平台的实际参数进一步确定平台铰点的初始坐标；

步骤2：根据给定随机位姿计算出训练样本；

步骤21：根据动平台位姿，确定旋转矩阵；

动平台的位姿公式如下式所示：

P＝[x_p y_p z_p α β γ]^T

式中：P表示动平台的位姿，即位置和姿态；x_p表示动平台的水平横向位移；y_p表示动平台的水平纵向位移；z_p表示动平台的竖直方向位移；α表示动坐标系绕静坐标系的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表示支腿的编号；l_s表示第s个支腿的长度；O_p为动坐标系原点在静坐标系中的位置；A_s表示第s个支腿上铰点的空间坐标；B_s表示第s个支腿上铰点的空间坐标；

则Stewart并联机构的广义坐标计算结果如下式所示：

q＝(l₁,…,l₆)^T

式中：q表示Stewart并联机构的广义坐标；l₁，…，l₆分别表示第1个到第6个支腿的长度；

步骤23：确定训练样本；

重复步骤21和步骤22，给定随机动平台的位姿计算出对应的Stewart并联机构的广义坐标，保存随机动平台位姿与对应的广义坐标作为训练样本；

步骤3：利用SSA优化BP神经网络得到位置正解，其具体步骤如下：

步骤31：获取步骤23计算得到的训练样本，分为训练集和测试集；初始化麻雀种群位置与适应度，设置最大迭代步数N、种群大小n、发现者数量PD、感应危险的麻雀数量SD、安全值ST、预警值R₂；确定初始参数后开始循环；

步骤32：构建迭代关系；

首先，构造发现者的位置的迭代关系，计算过程如下式所示；

式中：i表示个体编号；j表示位置编号；t表示迭代次数，初始值为0；表示t+1次迭代发现者的位置；表示t次迭代发现者的位置；α为(0,1]之间均匀随机数；N表示设定的最大迭代次数；Q是服从正态分布的随机数；L是单位行向量；R₂表示预警值；ST表示安全值；exp表示以自然常数e为底的指数函数；

其次，构造加入者位置的迭代关系，计算过程如下式所示；

式中：表示t+1次迭代加入者的位置；Y'_worst表示适应度最低的加入者位置；表示t次迭代加入者的位置；表示t+1迭代下加入者所占据的最优位置；n表示种群大小；A⁺为随机包含1和-1两个元素的行向量；otherwise表示其他情况；

最后，构建麻雀种群位置迭代关系，计算过程如下式所示；

式中：表示t+1次迭代麻雀种群的位置；表示第t次迭代适应度最高的麻雀位置；β为服从正态分布随机数；表示t次迭代麻雀种群的位置；表示第t次迭代适应度最高的麻雀位置；表示第t次迭代适应度最低的麻雀位置；K是[-1,1]之间的随机数；f_i是第i个体适应度值；f_best是个体最优适应度值；f_worst是个体最差适应度值；ε是为常数；

步骤33：重复执行步骤32，达到最大迭代步数，结束循环，输出神经网络最优权值和阈值，保存训练好的BP神经网络；

步骤34：调用保存好的神经网络训练得到预测值，作为Stewart并联机构平台的正解结果，输出得出初值位姿P₀；

步骤4：应用NAd1法求解Stewart并联机构平台正运动学的结果，具体步骤如下：

步骤41：获取步骤34预测得到的初始位姿P₀；给定杆长允许误差ε，最大迭代步数N；设置迭代次数k＝0；

步骤42：构建位姿迭代关系，具体计算过程如下所示：

输入Stewart并联机构平台的初始杆长q_s；构造Stewart并联机构平台正解方程F(P_k)；

F(P_k)＝q(P_k)-q_s

式中：P_k表示第k次迭代的正解；k表示位姿迭代次数；F(P_k)表示根据位姿P_k计算出的正解方程；q(P_k)表示根据位姿P_k计算求出的杆长；q_s表示并联机构平台第s个杆长初始值；

根据位姿P_k计算迭代的中间变量y_k；

式中：y_k表示第k次迭代的中间变量；F'(P_k)表示根据位姿P_k计算出正解方程的导数；

根据迭代的中间变量y_k计算位姿P_k+1；

式中：P_k+1表示第k+1次迭代的正解；I表示单位矩阵；F(y_k)表示根据y_k计算出正解方程；F'(y_k)表示根据y_k计算出正解方程的导数；

步骤43：根据P_k+1计算F(P_k+1)，进一步计算||F(P_k+1)||，以及杆长最大误差ΔL，其计算公式如下式所示；

ΔL＝||F(P_k+1)||^∞

式中：ΔL表示杆长最大误差；||F(P_k+1)||^∞表示取向量F(P_k+1)中的最大值；

步骤44：判断是否满足输出条件，输出计算得到的位姿P_k+1；否则转至步骤42进行下次迭代，并通过下式更新迭代次数；

k＝k+1；

所述输出条件包括ΔL是否小于ε，如果ΔL小于ε，则迭代停止，输出位姿P_k+1；如果ΔL大于ε，则判断迭代步数k+1是否大于最大迭代步数N，如果是则迭代停止，输出迭代失败；其中ε表示给定杆长允许误差。