一种变几何桁架模型化定位方法
【专利摘要】本发明公开了一种基于变几何桁架微分运动学模型定位方法，步骤1，在操作空间范围内，初始化参数θ0，L0，X0；步骤2，设计主动杆长度微分变量dL；步骤3，根据变几何桁架结构约束方程获取微分映射表达式步骤4，计算依赖于角度的微分仿射类似的得到关于节点Q的仿射步骤5，根据微分链式法则确定结构末端中心的微分映射步骤6，提取闭合速度雅克比矩阵，建立变几何桁架结构微分运动学方程，离散化结构微分运动学系统；步骤7，参考机械臂平台的测量装置提供的数据进行量测节点过程跟踪，获取该变几何桁架结构的量测模型。步骤8，通过设定间隙误差函数，综合该结构微分模型与量测模型，实现目标期望位姿误差对消的精确定位。
【专利说明】
一种变几何桁架模型化定位方法
技术领域
[0001] 本发明属于天基空间技术领域，特别涉及一种变几何桁架模型化定位方法。
【背景技术】
[0002] 空间平台机械臂具有多体、大柔度固有特点。在天基背景下，利用该桁架变几何桁 架结构内在微分映射关系，可以获得一个闭合形式的速度雅克比矩阵。通过变几何桁架微 分运动学输入输出关系实现了从运动学向微分运动学的传递，探讨一类三重八面体变几何 桁架的微分运动学模型化方法。参考机械臂平台的测量装置提供的数据进行量测节点过程 跟踪，获取该变几何桁架结构的量测模型，通过设定间隙误差函数，综合该结构微分模型与 量测模型，实现目标期望位姿误差对消的精确定位。
[0003] 经对现有技术的文献检索发现，变几何桁架在三维空间内可以通过操控驱动杆长 达到自身构型的任意弯曲，使得变几何桁架结构能够实现预定方位的定位。基于空间机械 臂的在轨空间卫星平台操作方案多用于合作目标、GPS、相机和雷达组成而成。如美国NASA 在2005年发射验证了DART(Demonstration of Autonomous Rendezvous Technology)、轨 道快车、欧空局的ATV(Transfer Autonomous Vehicle)、日本工程试验卫星（ETS-7)等。但 是，这种基于变几何桁架结构系统存在操作精度不够精确问题，安装于卫星上，可能导致空 间机械臂操作平台对合作目标超近距离跟踪丢失。
【发明内容】
[0004] 本发明目的是提供一种基于变几何桁架微分运动学模型定位方法。
[0005] 本发明的技术方案是，一种变几何桁架模型化定位方法，包括以下步骤:步骤1，在 变几何桁架主动杆长度可达的操作范围内，对输入的参数进行初始化，参数包括 9q、Lq和X〇， θ〇表示对应于角度参数集合? = {θι ,02,03}的初始状态，Lo表示对应于主动杆长参数集合L ={Li，L2，L3}的初始状态，Ll= I I A2B2 I I 表不 A2B2 的长度，L2= I I B2C2 I I 表不 B2C2 的长度，L3= I IA2C2II表不A2C2的长度，
[0006] Xo表示对应于操作臂移动平台的中心位置向量参数集合X= {Xi I i = l，…，n}的初 始状态，η依赖非对称单模块个数，
[0007] 初始化参数的具体关系如图2所示；
[0008] 通过对变几何桁架结构进行平移、旋转、缩放、仿射变换以保持平台局部特征的不 变性；
[0009] 步骤2,根据空间量测平台规划的数据设定一条期望路径，设定变几何桁架结构最 邻近连杆伸展距离和次邻近连杆伸展距离误差，其计算公式：
[0010] dL = Lk-Lk-I , Lmin^i Lk^;Lmax J
[0011] 其中，Lmin为连杆L最大收缩距离，Lmax为连杆L最大伸展距离；
[0012] 步骤3，根据变几何桁架结构约束方程限制，采用Newton-Raphson方法，通过变几 何桁架结构正向运动学关系计算Θ与L的微分雅克比矩阵，获取微分映射表达：
_，其计 算过程为：
[0013] (1)基于变几何桁架结构基本约束方程：
[0014] Fi( Θ ) = ci+C2+AciC2-2AsiS2+B = 0
[0015] F2( Θ ) =C2+C3+AC2C3-2AS2S3+C = 0
[0016] F3( θ ) =C3+C1+AC3C1-2AS3S1+D = 0
[0017] (2)利用Newton-Raphson迭代过程表达雅克比矩阵：
[0018]
[0019]
[0020] (3)计算?k+1=?k+S?k，直到I |S?k| |<ε，对于该变几何桁架结构的雅克比矩阵 为
[0021]
[0022]
[0023]
[0024]
[0025] Kd = -2Asi ,Ke = -2As2 ,Kf = -2As3
[0026] 步骤4,如图2所示，根据步骤1定义的角度参数{0^02,03}，进一步计算节点Q1,
[0027]
[0028]
[0029]
[0030] 其中，色表示变几何桁架结构的底端平面中心与末端平面中心连线的中间法向 量，爲的定义隐含了变几何桁架结构具有一定的中心对称性，
[0031]
[0032]
[0033]
[0034]
[0035]
[0036]其中，^是向量A基于惯性坐标系Ο-xyz对应x轴方向的矢量，心是向量良.基于 惯性坐标系Ο-xyz对应y轴方向的矢量，?)Λ;是向量艮基于惯性坐标系Ο-xyz对应z轴方向的矢 量；
[0037]步骤5,根据隐函数存在性定理确定结构末端中心与主动杆长参数集合L= ILllL2, L3}的微分映：
：十算过程：
[0038] 为了确定变几何桁架结构末端中心位姿X，根据变几何桁架结构几何对称关系，得 到如下末端中心位姿的闭合方程：
[0039]
[0040]
[0041 ]其中，/，= ·2ρ,7?·, + .V，如果雅克比矩阵是闭合解析的，变几何桁架结构微分运动学 模型满足
[0042] X =J(0,L) L
[0043] 其中，J(0，L)是解析的速度雅克比矩阵。事实上，根据隐函数存在定理，我们得到 函数关系
[0044]
[0045] 由以上方程求解出的雅克比矩阵，实现了变几何桁架结构运动学向微分运动学的 空间传递，即，通过速度雅克比矩阵揭示了操作速度空间与连杆速度空间之间转化的物理 意义；
[0046] 步骤6,为了克服模型不确定影响，利用隐函数存在定理提取闭合速度雅克比矩 阵，将变几何结构结构运动学模型转化为其微分运动学模型，计算变换公式如下，
[0047]
[0048]
[0049 ]其中，u =[ L1，L2，L3 ]τ是输入控制变量，
[0050] 离散化结构微分运动学系统的计算如下：
[0051]
[0052]
[0053] 步骤7,参考机械臂平台的测量装置提供的数据进行量测节点过程跟踪，获取该变 几何桁架结构的量测模型，
[0054] 首先，参考机械臂平台的测量装置提供的数据，设定测量节点位姿
[0055] Q1 = Q1(P)，Q2 = Q2(p)，Q3 = Q3(p)，
[0056] 进行量测节点过程跟踪，获取该变几何桁架结构的量测模型，
[0057] Yk = C(P)Uk
[0058] ek = r-Yk
[0059]其中，C(p)是关于与Uk无关的节点测量函数，Uk是输入控制变量，ek是参考输入与 量测节点信息的误差，r是参考输入；
[0060] 步骤8,通过设定间隙误差函数，综合该结构微分模型与量测模型，实现目标期望 位姿误差对消的精确定位，过程是，
[0061] 利用变几何桁架结构节点特征，设定节点间隙误差函数，
[0062] wk = d(uk)
[0063] 综合该结构微分模型与量测模型，
[0064]
[0065] 为了实现目标期望位姿误差对消的精确定位，设计反馈控制器，
[0066] Uk= OXk
[0067] 利用设计的反馈控制器，实现变几何桁架结构末端位姿精确定位。
[0068]本发明对近距空间卫星目标的变几何桁架结构微分运动学模型状态位姿进行定 位。利用该桁架变几何结构内在微分映射关系，获得一个闭合形式的速度雅克比矩阵。提出 一类三重八面体变几何桁架的微分运动学模型化方法。参考机械臂平台的测量装置提供的 数据进行量测节点过程跟踪，获取该变几何桁架结构的量测模型，通过设定间隙误差函数， 综合该结构微分模型与量测模型，实现目标期望位姿误差对消的精确定位。
[0069] 本发明利用计算机视觉技术，使得单平台的空间机器人可以对目标的进行相对姿 态的测量，提高测量精度，最终实现对空间卫星平台识别、跟踪和维护。为地面的宇航工作 人员提供多方面的技术支持。利用多个三重八面体构成的变几何桁架组合机械臂对空间站 可进行废旧卫星回收、卫星捕获任务，通过机械臂平台不仅可检测卫星损坏部件，还可通过 机械臂路径规划的平台数据实时跟踪合作目标对完成空间任务具有重要作用。此外，通过 变几何桁架结构机械臂微分运动学建模分析，进一步提出该结构精确定位方法，提高空间 非合作目标测量的精度，便于后续空间机械臂运动稳定性和主动控制策略等问题提供重要 的理论支撑。
[0070] 本发明针对空间复杂背景下，具有一定的适应太空作业环境的能力，且高度自适 应能力的空间变几何桁架机械臂，提供并实现了满足一定实时性要求的卫星目标相对位姿 的精确定位。采用此类机械臂协助或代替宇航员完成一些太空作业(在轨维护任务:普查太 空垃圾、废旧卫星回收、卫星捕获）。本发明有助于推动载人航天空间科学研究与应用的发 展。
【附图说明】
[0071] 图1为本发明流程图；
[0072] 图2为本发明基于变几何桁架结构链式模块与其结构单元解析图；
[0073]图3为本发明基于变几何桁架结构模块的正向、逆向运动学分析路径图；
[0074] 图4为本发明基于变几何桁架结构主动杆速度与操作速度之间的空间映射；
[0075] 图5为本发明基于变几何桁架结构微分运动学模型在XOY剖面其状态变量相对运 动轨迹的跟踪结果；
[0076] 图6为本发明基于变几何桁架结构微分运动学模型在YOZ剖面其状态变量相对运 动轨迹的跟踪结果；
[0077] 图7为本发明基于变几何桁架结构微分运动学模型在XOZ剖面其状态变量相对运 动轨迹的跟踪结果；
[0078] 图8为本发明变几何桁架结构MATLAB仿真环境微分模型系统仿真流程图。
【具体实施方式】
[0079] 下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行 实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施 例。
[0080] 如图1所示，本实施例提供的基于变几何桁架微分运动学模型精确定位方法，经过 MTLAB仿真数据和半物理仿真图像数据，共两个方面的数据进行实际测试，实施步骤如下： [0081 ]步骤1，首先分别对输入的参数进行初始化。
[0082]利用主要参数0〇儿(^()，0()表示对应于角度参数集合? = {01，02，03}的初始状态儿〇 表示对应于主动杆长参数集合L={Li，L2，L3}的初始状态，Li= I IA2B2I I表示A2B2的长度，L2 =I Ib2C2I I表示B2C2的长度，L3= IIA2C2II表示A2C2的长度，Xo表示对应于操作臂移动平台的 中心位置向量参数集合X=(X 1Ii = Ir^n)的初始状态，η依赖非对称单模块个数，初始化 参数的具体关系如图2所示。对变几何桁架结构进行平移、旋转、缩放、仿射变换以保持平台 局部特征的不变性。
[0083]步骤2,根据空间平台提供的数据规划一条期望路径，设定变几何桁架结构最邻近 连杆伸展距离和次邻近连杆伸展距离误差。
[0084]其计算过程为：
[0085] dL = Lk-Lk-i, Lmin^ Lk^ Lmax,
[0086] 其中，Lmin为连杆L最大收缩距离，Lmax为连杆L最大伸展距离。
[0087] 步骤3，根据变几何桁架结构约束方程限制，利用了Newton-Raphson方法，通过变 几何桁架结构正向运动学关系，计算Θ与L的微分雅克比矩阵，其计算过程为：
[0088] Newton-Raphson方法采用隐函数存在定理，需从变几何桁架结构正向运动学关系 出发，利用Newton-Raphson迭代过程，分析二面角Θ与变连杆长度L之间的隐含关系。求解 过程如下：
[0089] (1)基于变几何桁架结构基本约束方程：
[0090] Fi( Θ ) = ci+C2+AciC2-2AsiS2+B = 0 [0091 ] F2( θ ) =C2+C3+AC2C3-2AS2S3+C = 0
[0092] F3( Θ ) = c3+ci+Ac3Ci_2As3Si+D = 0
[0093] (2)利用Newton-Raphson迭代过程表达雅克比矩阵：
[0094] J50k = -Fi( 0k)
[0095]
[0096] (3)计算?k+1=?k+S?k，直到I |S?k| |<ε，对于该变几何桁架结构的雅克比矩阵 为
[0097]
[0098] 步骤4,如图2所示，根据步骤1定义的角度参数{0^02,03}，进一步计算节点Q1,
[0099] Qi=(Nsi,0iy+Nci,0)T
[0100]
[0101]
[0102] 其中，色表示变几何桁架结构的底端平面中心与末端平面中心连线的中间法向 量，色的定义隐含了变几何桁架结构具有一定的中心对称性，
[0103]
[0104] 分别计算依赖于角度的主动节点&微分仿射
[0105]
[0106] 计算依赖于角度的中间法向量咚微分仿射
[0107]
[0108] 其中，是向量色基于惯性坐标系Ο-xyz对应X轴方向的矢量為2是向量％基于惯 性坐标系Ο-xyz对应y轴方向的矢量，色,是向量色基于惯性坐标系Ο-xyz对应z轴方向的矢 量；
[0109] 基于Newton-Raphson法，利用微分链式准则，输入一系列变几何桁架结构参数，主 要包括变几何桁架结构电机的内参数、变几何桁架结构末端位姿等，求出变几何桁架结构 中间参数化:与民关于二面角的Θ最佳估计值。
[0110] 步骤5,根据隐函数存在性定理，确定结构末端中心与主动杆长参数集合L= IL1, fY L2,L3}的微分映射-7-，描述机械臂平台移动速度的输入输出关系，从而实现了变几何桁架 c L 结构运动学向微分运动学的传递空间重构。
[0111] 为了确定变几何桁架结构末端中心位姿X，根据变几何桁架结构几何对称关系得 到如下末端中心位姿的闭合方程：
[0112] '
[0113]
[0114] 其中，= 如果雅克比矩阵是闭合解析的，变几何桁架结构微分运动学 模型满足
[0115]
[0116] 其中，J(?，L)是解析的速度雅克比矩阵。事实上，根据隐函数存在定理，我们得到 函数关系
[0117]
[0118] 由以上方程求解出的雅克比矩阵实现了变几何桁架结构运动学向微分运动学的 空间传递。即，通过速度雅克比矩阵揭示了操作速度空间与连杆速度空间之间转化的物理 意义。
[0119] 步骤6,为了克服模型不确定影响，利用隐函数存在定理提取闭合速度雅克比矩 阵，将变.几何格Φ结构运动学模型转化为其微分运动学模型，计算变换公式如下，
[0120]
[0121]
[0122] 其中，u =[ L1，L2，L3 ]τ是输入控制变量。
[0123] 离散化变几何桁架结构微分运动学系统，计算如下：
[0124]
[0125]
[0126] 所述步骤7,参考机械臂平台的测量装置提供的数据，设定测量节点位姿
[0127] Q1 = Q1(P)，Q2 = Q2(p)，Q3 = Q3(p)，
[0128] 进行量测节点过程跟踪，获取该变几何桁架结构的量测模型。
[0129] Yk = C(P)Uk
[0130] ek = r-Yk
[0131] 其中，C(p)是关于与Uk无关的节点测量函数，Uk是输入控制变量，r是参考输入，ek 是参考输入与量测节点信息的误差。
[0132] 所述步骤8,利用变几何桁架结构节点特征，设定节点间隙误差函数，
[0133] wk = d(uk)
[0134] 综合该结构微分樽型与量测模型，
[0135]
[0136] 为了实现目标期望位姿误差对消的精确定位，设计反馈控制器，
[0137] Uk= OXk
[0138] 利用设计的反馈控制器，实现变几何桁架结构末端位姿精确定位。
[0139] 本实施例提供的基于变几何桁架结构机械臂微分运动学模型，进一步研究该结构 精确定位方法。本方法利用该桁架变几何桁架结构内在微分映射关系建立多个未知变量的 微分链式法则，进而获得一个闭合形式的速度雅克比矩阵。最终建立一类三重八面体变几 何桁架的微分运动学模型。本方法参考机械臂平台的测量装置提供的数据进行量测节点过 程跟踪，获取该变几何桁架结构的量测模型，通过设定间隙误差函数，综合该结构微分模型 与量测模型，实现目标期望位姿误差对消的精确定位。另外本方法研究具有一定的适应太 空作业环境的能力，且高度自适应能力的空间变几何桁架机械臂，提供并实现了满足一定 实时性要求的卫星目标相对位姿的精确定位是有必要的。这类空间机械臂平台不仅可检测 修复卫星损坏部件，还可基于机械臂路径规划的平台数据实时跟踪合作目标，可实现空间 卫星平台识别、跟踪和维护。这项发明也便于后续空间机械臂运动稳定性和主动控制策略 等问题提供重要的理论支撑。因此在卫星维修、太空垃圾清除、空间态势感知等领域中均可 有广泛的应用。
【主权项】
1. 一种变几何桁架模型化定位方法，其特征在于，包括以下步骤： 步骤1，在变几何桁架主动杆长度可达的操作范围内，对输入的参数进行初始化，参数 包括90、L〇和X〇，0Q表示对应于角度参数集合? = { 01，02，03}的初始状态，L〇表示对应于主动 杆长参数集合1^={1^1，：12丄3}的初始状态儿1=||4282||表示4282的长度儿2=||：82〇2||表示 B2C2的长度，L3=| IA2C2I |表不A2C2的长度， Xo表示对应于操作臂移动平台的中心位置向量参数集合X= {Xi I i = l，…，n}的初始状 态，n依赖非对称单模块个数； 通过对变几何桁架结构进行平移、旋转、缩放、仿射变换以保持平台局部特征的不变 性； 步骤2,根据空间量测平台规划的数据设定一条期望路径，设定变几何桁架结构最邻近 连杆伸展距离和次邻近连杆伸展距离误差，其计算公式： dL - Lk-Lk-l, Lmin ^ Lk ^ Lmax j 其中，Lmin为连杆L最大收缩距离，Lmax为连杆L最大伸展距离；步骤3,根据变几何桁架结构约束方程限制，采用Newton-Raphson方法，通过变几何桁 架结构正向运动学关系计算?与1的微分雅克比矩阵，获取微分映射表达式，其计算过 程为： (1) 基于变几何桁架结构基本约束方程： Fl( 0 ) =Cl+C2+AciC2-2AsiS2+B = 0 F2( ? ) =C2+C3+AC2C3-2AS2S3+C = 0 F3( ? ) =C3+C1+AC3C1-2AS3S1+D = 0 (2) 利用Newton-Raphson迭代过程表达雅克比矩阵： J8 0k = -Fi( @k)(3) 计算?k+i=?k+5?k，直到I |5?k| |<e，对于该变几何桁架结构的雅克比矩阵为其中，Kl = KbSI+KeCI，K2 = KcSI+KfCI，K3 = KAS2+KDC2， K4 = KcS2+KfC2 ,K5 = KaS3+KdC3 ,K6 = KbS3+KeC3 Ka=-(1+Aci) ,Kb = -(1+Ac2) ,Kc = -(1+Ac3), Kd = -2Asi, Ke = -2As2 , Kf=~2As3 步骤4,根据步骤1定义的角度参数{01,02,03}，进一步计算节点Qi， Qi= (Nsi,0iy+Nci,0)T (g2 = 0.5(2A^2,2O2v -M'2,2〇2__ + ^Nc2 f Q3 = 0.5^2^3,203,, - Afe3,2(^3j, - I 其中，A表示变几何桁架结构的底端平面中心与末端平面中心连线的中间法向量，％ 的定义隐含了变几何桁架结构具有一定的中心对称性，计算依赖于角度的主动节点&微分仿射计算依赖于角度的中间法向量A微分仿射其中，心是向量巧基于惯性坐标系〇-xyz对应x轴方向的矢量，色2是向量色基于惯性坐 标系O-xyz对应y轴方向的矢量，堯3是向量巧基于惯性坐标系〇-XyZ对应z轴方向的矢量； 步骤5,根据隐函数存在性定理确定结构末端中心与主动杆长参数集合LilL^Ls，!^} 的微分映射_.，计算过程： 为了确定变几何桁架结构末端中心位姿X，根据变几何桁架结构几何对称关系，得到如 下末端中心位姿的闭合方程： 'X = pL } = / '((-). L ) r 其中，+5，如果雅克比矩阵是闭合解析的，变几何桁架结构微分运动学模型满 足 l = j{0,L)i 其中，J(?，L)是解析的速度雅克比矩阵。事实上，根据隐函数存在定理，我们得到函数 关系由以上方程求解出的雅克比矩阵，实现了变几何桁架结构运动学向微分运动学的空间 传递，即，通过速度雅克比矩阵揭示了操作速度空间与连杆速度空间之间转化的物理意义； 步骤6,为了克服模型不确定影响，利用隐函数存在定理提取闭合速度雅克比矩阵，将 变几何结构结构运动学模型转化为其微分运动学模型，计算变换公式如下，其中，u = [ U，L2，L3 ]T是输入控制变量， 离散化结构微分运动学系统的计算如下： Xk = Juk步骤7，参考机械臂平台的测量装置提供的数据进行量测节点过程跟踪，获取该变几何 桁架结构的量测模型， 首先，参考机械臂平台的测量装置提供的数据，设定测量节点位姿 Ql = Ql(P)，Q2 = Q2(P)，Q3 = Q3(P)， 进行量测节点过程跟踪，获取该变几何桁架结构的量测模型， Yk=C(p)uk ek = r-Yk 其中，C(p)是关于与uk无关的节点测量函数，uk是输入控制变量，ek是参考输入与量测 节点信息的误差，r是参考输入； 步骤8,通过设定间隙误差函数，综合该结构微分模型与量测模型，实现目标期望位姿 误差对消的精确定位，过程是， 利用变几何桁架结构节点特征，设定节点间隙误差函数， wk = d(uk) 综合该结构微分模型与量测模型，为了实现目标期望位姿误差对消的精确定位，设计反馈控制器， Uk= ?Xk 利用设计的反馈控制器，实现变几何桁架结构末端位姿精确定位。
【文档编号】G06F17/50GK106055818SQ201610406881
【公开日】2016年10月26日
【申请日】2016年6月12日
【发明人】敬忠良, 徐启敏, 胡士强, 杨永胜
【申请人】上海交通大学