[发明专利]一种变几何桁架模型化定位方法

摘要

本发明公开了一种基于变几何桁架微分运动学模型定位方法，步骤1，在操作空间范围内，初始化参数θ₀，L₀，X₀；步骤2，设计主动杆长度微分变量dL；步骤3，根据变几何桁架结构约束方程获取微分映射表达式步骤4，计算依赖于角度的微分仿射类似的得到关于节点Q的仿射步骤5，根据微分链式法则确定结构末端中心的微分映射步骤6，提取闭合速度雅克比矩阵，建立变几何桁架结构微分运动学方程，离散化结构微分运动学系统；步骤7，参考机械臂平台的测量装置提供的数据进行量测节点过程跟踪，获取该变几何桁架结构的量测模型。步骤8，通过设定间隙误差函数，综合该结构微分模型与量测模型，实现目标期望位姿误差对消的精确定位。

主权项

1.一种变几何桁架模型化定位方法，其特征在于，包括以下步骤：步骤1，在变几何桁架主动杆长度可达的操作范围内，对输入的参数进行初始化，参数包括θ₀、L₀和X₀，θ₀表示对应角度参数集合Θ＝{θ₁,θ₂,θ₃}的初始状态，L₀表示对应于主动杆长参数集合L＝{L₁,L₂,L₃}的初始状态，L₁＝||Q₁Q₂||表示Q₁Q₂的距离，L₂＝||Q₂Q₃||表示Q₂Q₃的距离，L₃＝||Q₃Q₁||表示Q₃Q₁的距离，X₀表示对应于操作臂移动平台的中心位置向量参数集合X＝{X_i|i＝1,L,n}的初始状态，n依赖非对称单模块个数，支撑杆长定义为N；通过对变几何桁架结构进行平移、旋转、缩放、仿射变换以保持该结构平台局部特征的不变性；步骤2，根据空间量测平台规划的数据设定一条期望路径，设定变几何桁架结构最邻近连杆伸展距离和次邻近连杆伸展距离误差，其计算公式：dL＝L_k‑L_k‑1,L_min≤L_k≤L_max，          (1)其中，L_min为连杆L最大收缩距离，L_max为连杆L最大伸展距离，最邻近连杆伸展距离L_k定义为：第k时刻主动连杆的实际伸展距离；步骤3，根据变几何桁架结构动态约束限制(1)，若采用Newton‑Raphson方法，将得到变几何桁架结构正向运动学的动态约束关系，并可进一步计算Θ与L的微分映射关系其计算过程为：(1)基于变几何桁架结构基本约束方程：F₁(Θ)＝c₁+c₂+Ac₁c₂‑2As₁s₂+B＝0                  (2)F₂(Θ)＝c₂+c₃+Ac₂c₃‑2As₂s₃+C＝0         (3)F₃(Θ)＝c₃+c₁+Ac₃c₁‑2As₃s₁+D＝0                (4)其中，c₁＝cosθ₁,c₂＝cosθ₂,c₃＝cosθ₃,s₁＝sinθ₁,s₂＝sinθ₂，s₃＝sinθ₃，(2)利用Newton‑Raphson迭代过程表达雅克比矩阵：JδΘ_k＝‑F_i(Θ_k)      (5)其中，(3)计算Θ_k+1＝Θ_k+δΘ_k，直到||δΘ_k||＜ε，0＜ε＜1，对于该变几何桁架结构的雅克比矩阵为其中，K₁＝K_Bs₁+K_Ec₁,K₂＝K_Cs₁+K_Fc,K₃＝K_As₂+K_Dc,K₄＝K_Cs₂+K_Fc₂,K₅＝K_As₃+K_Dc，K₆＝K_Bs₃+K_EcK_A＝‑(1+Ac₁),K_B＝‑(1+A₂c),K_C＝‑(1+Ac₃),K_D＝‑2As₁,K_E＝‑2As₂,K_F＝‑2As₃步骤4，根据步骤1定义的角度参数{θ₁,θ₂,θ₃}，进一步计算节点Q_i，其中，参数O_1y,O_2y,O_3y,O_2z,O_3z，一般设定为VGT结构底面的固定时，对应的底面边形中点相应坐标，即O_1y为B端底面边形B₁B₂中点O₁的y轴坐标值，O_2y为B端底面边形B₂B₃中点O₂的y轴坐标值，O_3y为B端底面边形B₃B₁中点O₃的y轴坐标值，O_2z为B端底面边形B₂B₃中点O₂的z轴坐标值，O_3z为B端底面边形B₃B₁中点O₃的z轴坐标值，表示变几何桁架结构的底端平面中心与末端平面中心连线的中间法向量，的定义隐含了变几何桁架结构具有一定的中心对称性，其中，VGT底面平台表达^BQ_i，i＝1,2,3，是对应节点Q_i，i＝1,2,3，U_1x,U_1y,U_1z分别表示为的x，y，z坐标值，计算依赖角度θ₁的主动节点Q₁微分仿射计算依赖角度Θ的中间法向量微分映射其中，为向量基于惯性坐标系O‑xyz对应x轴方向的矢量，为向量基于惯性坐标系O‑xyz对应y轴方向的矢量，为向量基于惯性坐标系O‑xyz对应z轴方向的矢量；步骤5，根据隐函数存在性定理确定结构末端中心与主动杆长参数集合L＝{L₁,L₂,L₃}的微分映射计算过程：为了确定变几何桁架结构末端中心位置向量根据变几何桁架结构几何对称关系，得到如下末端中心位姿的闭合方程：其中，S表示为节点处的结构间隙耗散距离，r表示为VGT结构底面中心与中间主动平面中心的距离，为VGT底面中心初始时刻向量表达，如果雅克比矩阵是闭合解析的，变几何桁架结构微分运动学模型满足其中，J(Θ,L)是解析的速度雅克比矩阵，根据隐函数存在定理，得到函数关系由以上方程求解出的雅克比矩阵，实现了变几何桁架结构运动学向微分运动学的空间传递，即，速度雅克比的物理意义揭示了操作速度空间与连杆速度空间之间内在转化关系，如图4；步骤6，为了克服模型不确定影响，利用隐函数存在定理提取闭合速度雅克比矩阵，将变几何结构结构运动学模型转化为其微分运动学模型，计算变换公式如下，其中，u＝[L₁,L₂,L₃]^T是输入控制变量,J表示为J(Θ,L)，离散化结构微分运动学系统的计算如下：X_k＝Ju_k        (17)其中，表示微分系统(15)‑(16)为表示第kT时刻离散化采样后的离散化系统，采样周期为T＝1s；步骤7，参考机械臂平台的测量装置提供的数据进行量测节点过程跟踪，获取该变几何桁架结构的量测模型，首先，参考机械臂平台的测量装置提供的数据p，设定测量节点位姿Q₁＝Q₁(p)，Q₂＝Q₂(p)，Q₃＝Q₃(p)，进行量测节点过程跟踪，获取该变几何桁架结构的量测模型，Y_k＝C(p)u_k       (19)e_k＝r_d‑Y_k        (20)其中，C(p)是关于与u_k无关的节点测量函数，u_k是输入控制变量，e_k是参考输入与量测节点信息的误差，r_d为参考输入；步骤8，通过设定间隙误差函数，综合该结构微分模型与量测模型，实现目标期望位姿误差对消的精确定位，过程为：利用变几何桁架结构节点特征，设定节点间隙误差函数，w_k＝d(u_k)        (21)综合该结构微分模型与量测模型，为了实现目标期望位姿误差对消的精确定位，设计反馈控制器，u_k＝ΦX_k        (23)其中，Φ为系统(22)的反馈增益，利用设计的反馈控制器，输入控制指令实现变几何桁架结构末端位姿精确定位。