CN112257011B - 一种基于BAS的Stewart机构实时正解算方法 - Google Patents

Abstract

本发明公开了一种基于BAS的Stewart机构实时正解算方法。首先，针对六自由度并联系统，设计Stewart平台模型，确定机构基本参数，其次，根据实时系统的控制要求，设计推导反解算数学模型，在此基础上设计基于支链长度解算绝对误差评价函数的BAS正解算模型。通过本发明方法对具有确定参数的Stewart机构进行正解算验证，取得效果能够满足实时高精度控制要求。

Description

一种基于BAS的Stewart机构实时正解算方法

技术领域

本发明属于系统自动控制领域，具体涉及一种基于BAS的Stewart机构实时正解算方法，针对并联机构正解算的强非线性特征，实现良好的解算精度。

背景技术

Stewart(六自由度并联机构)平台位置控制分为两种：即基于铰点空间的开环控制及基于工作空间的闭环控制，后者可以消除各个支链耦合作用引起的姿态位置误差，因此控制精度要优于前者。基于工作空间的控制方式需要运动平台的真实位置姿态作为反馈输入，正解算是最为常用的方法。由于正解算的需要通过六个支链来解算平台的位置和姿态，其解算过程存在强非线性且容易出现多组解，传统的正解算方法难以满足实时精确的解算要求。因此需要设计新的解算方法以满足复杂的控制要求。

近些年来，一些较为复杂的算法被应用到正解算中，如迭代优化法、解析法、同伦法、神经网络法等方法。这些方法能够满足复杂解算要求，迭代优化法依赖于初始值的设定，如果初值设置不准确则会引起发散，解析法不依赖于初值，但是需求解多次一元方程，其数学推导过程复杂、求解速度低，同伦法不依赖于初值且不需要化简代数方程，但求解过程很慢，神经网络法不依赖于数学模型及初始值，但是需要经过大量样本的预先训练学习，其解算精度及应用受限。随着生物启发式算法的出现，如天牛须寻优算法(BAS)，该方法不依赖系统数学模型，有很强的全局寻优能力，在一定程度上克服了迭代优化法等方法的不足并在复杂系统的最优化过程中取得了广泛的应用。其的仿生寻优原理如下：甲虫可以根据食物气味的强弱来觅食，即使它不知道目标在哪里。它有两个长的触须，即左，右触须，如果左触须接收到的气味强度大于右触须，那么下一步就是飞到左边，否则飞到右边。根据这个原理，甲虫最终可以找到食物。目前基于全局寻优的BAS算法在复杂系统参数实时辨识及控制并在一定程度上取得了较好的效果，但目前在Stewart平台的正解算中还没有被提出与应用。

提高正解算的实时计算精度也可以在硬件上进行改进，例如采用高性能CPU及GPU的计算机，这种方案在一定程度上可以提高算法的更新迭代速度，却增加了硬件的复杂性，提高了成本，不利于在实践中进行推广应用。

发明内容

为解决上述问题，本发明公开了一种基于BAS的Stewart机构实时正解算方法，其解算精度及解算时间可以满足精确及实时控制需求。

为达到上述目的，本发明的技术方案如下：

一种基于BAS的Stewart机构实时正解算方法，包括下列步骤：

步骤1)，对于Stewart机构，动平台正解算姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示。其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移。首先设计k次迭代正解算性能评价函数f(x)，其过程表达式：

其中，||·||为求模符号。l_i为通过电机编码器计算的支链长度，l'_i为由x(k)＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T在k次迭代反解算的支链长度。

步骤2)，初始化所有参数值。设定初始触角长度d⁰＝0.2及移动步距δ⁰＝0.2。天牛的初始搜寻位置设置为:x(0)＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T＝[0,0,0,0,0,0]^T。

步骤3)，计算步骤2)的评价函数f(x)。

步骤4)，定义搜寻方向。通过随机函数设定搜寻方向：

其中，n为搜寻方向，rnd为随机向量函数，||·||为求模符号。

步骤5)，计算左，右移动方向。甲虫不知道食物在哪里，它使用触须来寻找下一个移动方向，如果左触须接收到的食物的气味强度大于右天线，那么下一步就是向左飞，否则向右飞，故左，右触须的位置分别是：

x_l＝x+dn

x_r＝x-dn

其中，x_l为左触须位置，x_r为右触须位置。d为触须长度。

步骤6)，分别计算左右触须的评价函数f(x_l)及f(x_r)。

步骤7)，计算天牛下一步位置：

其中，δ为移动步距，sgn为符号函数。

步骤8)，计算步骤7)的评价函数

步骤9)，更新天牛新的全局位置，触须长度及移动步距：

更新位置：若

更新触须长度：d＝0.99d+0.01；

更新移动步距：δ＝0.99δ。

步骤10)，如果达到最大迭代步数或计算精度满足要求，则结束循环并输出最终参数，否则返回步骤4)并开始下一个循环。

步骤11)，由位置及姿态计算支链长度为反解算，其计算方法如下：Stewart平台由动平台，静平台，六个支链等组成。随动坐标系O_n-XYZ与动平台固连，大地坐标系O_b-XYZ与静平台固连。动平台相对于静平台的姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示。其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移。动平台相对于静平台的旋转矩阵为：

其中，sα,sβ,sγ分别表示sin(α),sin(β),sin(γ)，cα,cβ,cγ分别表示cos(α),cos(β),cos(γ).R_α,R_β,R_γ计算方法为:

反解算(由位置及姿态计算支链长度)公式为：

其中，l_i是静平台铰点点B_i到动平台铰点A_i的向量，t是动平台坐标系原点O_n在O_b-XYZ系中的坐标，p_i向量O_n到A_i在O_n-XYZ系中表示，b_i是静平台坐标系原点O_b到B_i在O_b-XYZ系中的表示。支链的长度计算公式为：

作为本发明一种基于BAS的Stewart机构实时正解算方法进一步的优化方案，所述步骤1)的补充说明如下：

1.1)，由支链长度解算动平台姿态即为正解算，此方法将正解算转化为六维向量寻优问题，由于实际的六个姿态x＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T为完全未知，且量纲、数量级均存在较大差异，在迭代寻优过程中加入如下约束条件：

若：l'_i>l_max或l'_i<l_min，即表示当前正解算值超过最大解算范围，舍去当前正解算值，取前一次迭代正解算值替代当前值做后续计算。其中，l_max为最大支链长度，l_min为最小支链长度。

本发明的有益效果是：

1.克服了常规正解算方法依赖复杂数学模型，本文所述控制策略不依赖于复杂的数学模型，可以适用并应用于常规控制系统的Stewart正解算；

2.常规正解算迭代方法的收敛快慢取决于初始值的选取，且初始值选取不当，亦会引起发散，本文所述正解算方法不依赖于初始值设定，且不会发散。，克服了因初始值的随机选取导致系统的振荡或发散；

3.常规迭代法进行正解算，其收敛精度严重依赖于解算时间，收敛精度越高，则所需解算时间越长，难以应用到实时控制中，本方法克服解算精度对时间的严重依赖等缺陷，其解算精度及解算时间可以满足精确及实时控制需求。

附图说明

图1是本发明方法的算法设计流程图。

图2是本发明方法的Stewart原理图。

图3是本发明方法正解算姿态误差曲线。

图4是本发明方法正解算位置误差曲线。

图5是运用本发明方法正计算时间消耗曲线。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1所示，本发明公开了一种基于BAS的Stewart机构实时正解算方法，包括以下步骤：

步骤1)，对于Stewart机构，动平台正解算姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示。其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移。首先设计k次迭代正解算性能评价函数f(x)，其过程表达式：

其中，||·||为求模符号，l_i为通过电机编码器计算的支链长度，l'_i为由x＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T在k次迭代反解算的支链长度。

由支链长度解算动平台姿态即为正解算，此方法将正解算转化为六维向量寻优问题，由于实际的六个姿态x＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T为完全未知，且量纲、数量级均存在较大差异，在迭代寻优过程中加入如下约束条件：

若：l'_i>l_max或l'_i<l_min，即表示当前正解算值超过最大解算范围，舍去当前正解算值，取前一次迭代正解算值替代当前值做后续计算。其中，l_max为最大支链长度，l_min为最小支链长度。

步骤2)，初始化所有参数值。设定初始触角长度d⁰＝0.2及移动步距δ⁰＝0.2。天牛的初始搜寻位置设置为:x⁰＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T＝[0,0,0,0,0,0]^T。

步骤3)，计算步骤2)的评价函数f(x)。

步骤4)，定义搜寻方向。通过随机函数设定搜寻方向：

其中，n为搜寻方向，rnd为随机向量函数。

步骤5)，计算左，右移动方向。甲虫不知道食物在哪里，它使用触须来寻找下一个移动方向，如果左触须接收到的食物的气味强度大于右天线，那么下一步就是向左飞，否则向右飞，故左，右触须的位置分别是：

其中，x_l为左触须位置，x_r为右触须位置。d为触须长度。

步骤6)，分别计算左右触须的评价函数f(x_l)及f(x_r)。

步骤7)，计算天牛下一步位置：

其中，δ为移动步距，sgn为符号函数。

步骤8)，计算步骤7)的评价函数

步骤9)，更新天牛新的全局位置，触须长度及移动步距：

更新位置：若

更新触须长度：d＝0.99d+0.01；

更新移动步距：δ＝0.99δ。

步骤10)，如果达到最大迭代步数或计算精度满足要求，则结束循环并输出最终参数，否则返回步骤4)并开始下一个循环。

步骤11)，由位置及姿态计算支链长度为反解算，其计算方法如下：Stewart平台由动平台，静平台，六个支链等组成，如图2所示。随动坐标系O_n-XYZ与动平台固连，大地坐标系O_b-XYZ与静平台固连。动平台相对于静平台的姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示。其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移。动平台相对于静平台的旋转矩阵为：

其中，sα,sβ,sγ分别表示sin(α),sin(β),sin(γ)，cα,cβ,cγ分别表示cos(α),cos(β),cos(γ).R_α,R_β,R_γ计算方法为:

反解算(由位置及姿态计算支链长度)公式为：

其中，l_i是静平台铰点点B_i到动平台铰点A_i的向量，t是动平台坐标系原点O_n在O_b-XYZ系中的坐标，p_i向量O_n到A_i在O_n-XYZ系中表示，b_i是静平台坐标系原点O_b到B_i在O_b-XYZ系中的表示。支链的长度计算公式为：

本发明中用于仿真验证的Stewart参数如下：

A₁A₂＝0.15m；A₂A₃＝0.92m；B₁B₂＝0.25m；B₂B₃＝1.2m；l_min＝0.9m；l_max＝1.65m。

用于仿真的动平台位置及姿态时间序列为：

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。

Claims (1)

1.一种基于BAS的Stewart机构实时正解算方法，其特征在于：包括下列步骤：

步骤1)，对于Stewart机构，动平台正解算姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示，其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移，首先设计k次迭代正解算性能评价函数f(x)，其过程表达式：

其中，||·||为求模符号，l_i为通过电机编码器计算的支链长度，l_i'为由x(k)＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T在k次迭代反解算的支链长度，

由支链长度解算动平台姿态即为正解算，此方法将正解算转化为六维向量寻优问题，由于实际的六个姿态x＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T为完全未知，且量纲、数量级均存在差异，在迭代寻优过程中加入如下约束条件：

若：l′_i>l_max或l′_i<l_min，即表示当前正解算值超过最大解算范围，舍去当前正解算值，取前一次迭代正解算值替代当前值做后续计算，其中，l_max为最大支链长度，l_min为最小支链长度；

步骤2)，初始化所有参数值：设定初始触角长度d⁰＝0.2及移动步距δ⁰＝0.2，天牛的初始搜寻位置设置为：x(0)＝[α_m,β_m,γ_m,X_m,Y_m,Z_m]^T＝[0,0,0,0,0,0]^T，

步骤3)，计算步骤2)的评价函数f(x)，

步骤4)，定义搜寻方向，通过随机函数设定搜寻方向：

其中，n为搜寻方向，rnd为随机向量函数，||·||为求模符号，

步骤5)，计算左，右移动方向：甲虫不知道食物在哪里，它使用触须来寻找下一个移动方向，如果左触须接收到的食物的气味强度大于右天线，那么下一步就是向左飞，否则向右飞，故左，右触须的位置分别是：

x_l＝x+dn

x_r＝x-dn

其中，x_l为左触须位置，x_r为右触须位置，d为触须长度，

步骤6)，分别计算左右触须的评价函数f(x_l)及f(x_r)，

步骤7)，计算天牛下一步位置：

其中，δ为移动步距，sgn为符号函数，

步骤8)，计算步骤7)的评价函数

步骤9)，更新天牛新的全局位置，触须长度及移动步距：

更新位置：若

更新触须长度：d＝0.99d+0.01；

更新移动步距：δ＝0.99δ，

步骤10)，如果达到最大迭代步数或计算精度满足要求，则结束循环并输出最终参数，否则返回步骤4)并开始下一个循环，

步骤11)，由位置及姿态计算支链长度为反解算，其计算方法如下：Stewart平台由动平台，静平台，六个支链组成，随动坐标系O_n-XYZ与动平台固连，大地坐标系O_b-XYZ与静平台固连，动平台相对于静平台的姿态与位置用α_m,β_m,γ_m,X_m,Y_m,Z_m表示，其中α_m,β_m,γ_m分别表示动平台分别为绕x,y,z轴的转角，X_m,Y_m,Z_m分别表示动平台沿x,y,z轴的位移，动平台相对于静平台的旋转矩阵为：

其中，sα,sβ,sγ分别表示sin(α),sin(β),sin(γ)，cα,cβ,cγ分别表示cos(α),cos(β),cos(γ).R_α,R_β,R_γ计算方法为：

反解算即由位置及姿态计算支链长度，公式为：

其中，l_i是静平台铰点点B_i到动平台铰点A_i的向量，t是动平台坐标系原点O_n在O_b-XYZ系中的坐标，p_i向量O_n到A_i在O_n-XYZ系中表示，b_i是静平台坐标系原点O_b到B_i在O_b-XYZ系中的表示，支链的长度计算公式为：