一种空间机器人动力学参数辨识的激励轨迹优化方法
【专利摘要】本发明公开了一种空间机器人动力学参数辨识的激励轨迹优化方法，可以实现空间机器人动力学参数辨识的激励轨迹优化，同时满足参数辨识的PE条件和机械臂关节运动约束，提高空间机器人参数辨识的收敛速度和准确性。自由漂浮空间机器人的非完整特性决定了参数辨识模型回归矩阵A(k)不仅含有与机械臂关节运动轨迹相关的关节位置和关节速度，还含有与待辨识动力学参数间接相关的基座位置、姿态、速度和角速度，而这些量必须根据系统的动力学模型进行求解，因此在激励轨迹离线优化时需要用到待辨识动力学参数的先验信息。
【专利说明】一种空间机器人动力学参数辨识的激励轨迹优化方法 【技术领域】
[0001 ]本发明属于空间机器人领域，涉及一种空间机器人动力学参数辨识的激励轨迹优 化方法。 【【背景技术】】
[0002] 空间机器人完成在轨服务任务时一般采用基于模型的控制方法，这类方法的控制 精度与空间机器人动力学参数的准确程度紧密相关。在进行空间机器人设计和加工时能够 通过CAD软件计算其动力学参数，也能够通过地面测试得到其各个部件精确的动力学参数。 然而空间机器人在轨捕获目标后整个系统的动力学参数会发生相应改变，为了保证后续操 作中控制系统的稳定性并提高控制精度，需要对捕获目标后的空间机器人动力学参数进行 辨识并利用辨识得到的动力学参数更新控制器的相关参数。
[0003] 动力学参数辨识的准确性和快速性不仅和选用的参数辨识模型以及估计方法有 关，也与参数辨识的激励轨迹有很大关系。为了加快参数辨识的收敛速度并提高参数估计 的准确性，需要合理选择空间机器人参数辨识的激励轨迹，以保证用于辨识的测量数据满 足持续激励(PE)条件。
[0004] 地面工业机器人一般是通过离线设计激励轨迹来满足动力学参数辨识所需的PE 条件，但自由漂浮空间机器人由于其非完整约束特性，导致动力学参数辨识模型的回归矩 阵中含有和待辨识动力学参数相关的状态量(基座的位置、姿态、速度和角速度），这些状态 量与关节运动轨迹相关，无法预先单独设计，需要根据系统的动力学模型进行解算。因此不 同于地面工业机器人，空间机器人的激励轨迹优化需要用到待辨识参数的先验信息。
[0005] 虽然一些空间机器人动力学参数辨识方面的文献提到了激励轨迹需要满足PE条 件，但多数文献仅是在完成参数辨识后验证参数辨识模型回归矩阵的条件数是否足够小， 目前已发表的文献中只有很少几篇涉及空间机器人动力学参数辨识的激励轨迹优化问题。 文献"Modeling and experimental design for the on-orbit inertial parameter identification of free-flying space robots"中采用机械臂运动对空间机器人基座的 扰动幅度来衡量激励轨迹的激励程度并根据该指标优化激励轨迹，然而该指标的提出更多 是出于一种直观感觉，缺乏准确的理论支撑。文献"Parameter identification methods for free-floating space robots with direct torque sensing"米用B样条参数化表不 空间机器人的机械臂运动轨迹并根据设计的优化准则优化激励轨迹，然而其轨迹优化的目 的是为了更好地激励空间机器人的柔性模态来研究空间机器人的柔性附件和燃料晃动对 参数辨识的影响，而并非通过优化使激励轨迹满足参数辨识所需的PE条件。 【
【发明内容】
】
[0006] 本发明的目的在于针对自由漂浮空间机器人动力学参数辨识问题，提供一种空间 机器人动力学参数辨识的激励轨迹优化方法，用于在机械臂关节运动约束范围内提高空间 机器人动力学参数辨识的收敛速度和准确性，保证用于参数辨识的测量信息满足持续激励 (PE)条件且不违背机械臂的关节运动约束。
[0007] 为达到上述目的，本发明采用以下技术方案予以实现：
[0008] 一种空间机器人动力学参数辨识的激励轨迹优化方法，包括以下步骤：
[0009] 1)建立自由漂浮空间机器人的参数辨识模型
[0010] 空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由 η个旋转关节连接而成，所有构件均视为刚体;基座航天器上安装有测量基座位置、姿态、线 速度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器；
[0011] 空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为：
[0016]空间机器人的线动量Ρ和角动量L用矩阵形式统一表示为：
[0022] jTi= [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0,…，0] (8)
[0023] Jri = [ki,k2, ··· ,ki,0, ··· ,0] (9)
[0024] r〇g = rg-r〇, rgi = r i-rg, r〇i = n-r〇 (10)
[0025] 设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理得到：
[0027] 式中：
[0028] in= [ Ixx, _Ixy , _Ixz , Iyy , _Iyz , Izz]T
[0032] 构造一个以x=[l/mnnanT inT]T为未知量的线性回归方程组进行求解;假设在每 个采样点获取一组测量值，当完成第k次采样后，线性回归方程组能够表示如下：
[0033] A(k)x = Y(k) (12)
[0034] 式中：
[0038]待辨识的动力学参数X可采用最小二乘算法进行估计：
[0040] 2)确定激励轨迹优化准则
[0041]选用回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：
[0043] 式中：〇max(A(k))表不矩阵A(k)的最大奇异值，〇min(A(k))表不矩阵A(k)的最小奇 异值；
[0044] 3)空间机器人关节轨迹参数化
[0045] 空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：
[0047]式中：cof为傅立叶级数的基频，φι()为关节位置偏移量;机械臂各关节采用相同的 基频来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2Ν+1个待定系 数；
[0048]对式（18)关于时间求一阶导数和二阶导数能够得到关节i的角速度和角加速度如 下所示：
[0051 ] 4)激励轨迹优化问题求解
[0052] 激励轨迹优化问题描述为如下形式：
[0053] <})*(t)=argmin(J) (21)
[0054] 约束条件为：
[0056]式（21)和式（22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的 fmincon函数求得该非线性优化问题的解，即步骤3)中待定系数《丨，踔和Φ1()的值。
[0057]与现有技术相比，本发明具有以下有益效果：
[0058] 本发明可以实现空间机器人动力学参数辨识的激励轨迹优化，同时满足参数辨识 的ΡΕ条件和机械臂关节运动约束，提高空间机器人参数辨识的收敛速度和准确性。自由漂 浮空间机器人的非完整特性决定了参数辨识模型回归矩阵A(k)不仅含有与机械臂关节运 动轨迹相关的关节位置和关节速度，还含有与待辨识动力学参数间接相关的基座位置、姿 态、速度和角速度，而这些量必须根据系统的动力学模型进行求解，因此在激励轨迹离线优 化时需要用到待辨识动力学参数的先验信息。但仿真结果表明该方法对先验误差具有较强 的鲁棒性，在先验值和真实值具有50 %误差的情况下，通过优化方法得到的激励轨迹仍可 以满足PE条件，因此该方法具有很好的工程适用性。 【【附图说明】】
[0059] 图1为空间机器人模型图；
[0060]图2为最优激励轨迹的关节角度变化曲线图；
[0061] 图3为最优激励轨迹的关节角速度变化曲线图；
[0062] 图4为对照组激励轨迹的关节角度变化曲线图；
[0063] 图5为对照组激励轨迹的关节角速度变化曲线图；
[0064] 图6为质量辨识结果图；
[0065]图7为转动惯量辨识结果图；
[0066]图8为质心位置辨识结果图；
[0067]图9为回归矩阵条件数图；
[0068]图10为捕获目标后的空间机器人系统图；
[0069]图11为激励轨迹优化求解流程图。
[0070] 其中：1-连杆1; 2-连杆2; 3-连杆3;4-连杆4; 5-连杆5;6-连杆6;7-连杆7;8-基座。 【【具体实施方式】】
[0071]下面结合附图对本发明做进一步详细描述：
[0072] 参见图1-图11，本发明空间机器人动力学参数辨识的激励轨迹优化方法，其特征 在于，包括以下步骤：
[0073] 步骤1:建立自由漂浮空间机器人的参数辨识模型
[0074] 空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由 η个旋转关节连接而成，所有构件均视为刚体。基座航天器上安装有测量基座位置、姿态、线 速度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器。
[0075] 空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为：
[0080]空间机器人的线动量ρ和角动量L用矩阵形式统一表示为：
[0086] jTi= [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0,…，0] (8)
[0087] Jri= [ki,k2, ··· ,ki,0, ··· ,0] (9)
[0088] r〇g = rg-r〇, rgi = r i-rg, r〇i = n-r〇 (10)
[0089] 设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理可得：
[0096] (11)是一个含有10个未知数，6个独立方程的线性方程组，因此仅靠一组测量数据 无法得到确定的解，需要利用多组测量数据，构造一个以x=[l/m nnanT inT]T为未知量的线 性回归方程组进行求解。假设在每个采样点获取一组测量值，当完成第k次采样后，线性回 归方程组可以表示如下：
[0097] A(k)x = Y(k) (12)
[0098] 式中：
[0102]待辨识的动力学参数X可采用最小二乘算法进行估计：
[0104]步骤2:确定激励轨迹优化准则
[0105]线性回归方程组A(k)x = Y(k)中系数矩阵A(k)的条件数可以度量方程组的解X对 于误差的敏感性。如果A(k)的条件数较大，Y(k)的微小改变就会引起X较大的改变，从而降 低数值稳定性。
[0106]在参数辨识中，经常采用辨识模型的回归矩阵条件数来衡量参数辨识的激励水 平，为了保证参数辨识的准确性和收敛速度，需要尽量减小回归矩阵的条件数。本发明选用 回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：
[0108] 式中：〇max(A(k))表不矩阵A(k)的最大奇异值，〇min(A(k))表不矩阵A(k)的最小奇 异值。
[0109] 步骤3:空间机器人关节轨迹参数化
[0110] 空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：
[0112] 式中：cof为傅立叶级数的基频，φι()为关节位置偏移量。机械臂各关节采用相同的 基频来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2Ν+1个待定系 数，根据步骤4中的方法确定尽和Φ 1()的值能够保证空间机器人的运动满足参数辨识的 ΡΕ条件。
[0113] 对式（18)关于时间求一阶导数和二阶导数可以得到关节i的角速度和角加速度如 下所示：
[0116] 步骤4:激励轨迹优化问题求解
[0117] 空间机器人机械臂的关节运动不仅要满足参数辨识的激励要求，还需要满足关节 运动范围、关节角速度、关节角加速度的约束。因此激励轨迹优化问题可以描述为如下形 式：
[0118] <})*(t)=argmin(J) (21)
[0119] 约束条件为：
[0121] 式（21)和式（22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的 fmincon函数可求得该非线性优化问题的解，即步骤3中待定系数α丨，6；和φ ι()的值。
[0122] 本发明的原理：
[0123] 针对图1所示的空间机器人模型，采用本发明得到的最优激励轨迹如图2和图3所 示。空间机器人(含目标）的动力学参数如表1所示，7自由度机械臂的Denavit-Hatenberg (DH)参数如表2所示，关节运动约束如表3所示。
[0124] 表1空间机器人动力学参数
[0127] 表2机械臂DH参数
[0128]
[0129]表3机械臂关节运动约束
[0131] 图2和图3所示的最优激励轨迹根据动力学参数先验值计算的回归矩阵条件数为 1.6912。为了说明本发明的效果，选取图4和图5所示的对照组激励轨迹，该激励轨迹由频率 为0.1Hz和0.5Hz的两种正弦谐波组成，满足机械臂关节角度和关节角速度的初始及终端约 束条件，对照组激励轨迹根据参数先验值计算的回归矩阵条件数为510.6878。
[0132] 控制空间机器人分别跟踪最优激励轨迹和对照组激励轨迹，根据两组采样数据分 别完成对空间机器人动力学参数的辨识，辨识结果如图6至图8所示。从仿真结果可以看出， 最优激励轨迹对应的参数辨识结果收敛速度更快、准确性更高。虽然仿真中的最优激励轨 迹是根据待辨识参数的先验值求得的，但从图9所示的回归矩阵条件数仿真结果可以看出， 本发明的激励轨迹设计方法对于先验误差具有较强的鲁棒性，在参数先验值仅为真实值一 半的情况下，设计的激励轨迹仍具有较小的回归矩阵条件数，能够满足PE条件，从而保证了 参数辨识的快速性和准确性。
[0133] 图10是捕获目标后空间机器人系统的结构模型，其中：
[0134] ΣΙ:惯性坐标系，原点为〇1;
[0135] Σ β :空间机器人基座的体坐标系，原点(^位于基座质心；
[0136] m〇，πη :分别表示基座和连杆i的质量，
[0137] I。，Ii e R3X3:分别表示基座和连杆i绕各自质心的惯性张量；
[0138] ki e R3:表示关节i旋转方向的单位向量；
[0139] riGR3:连杆i质心的位置向量；
[0140] rg e R3:空间机器人系统质心〇g的位置向量；
[0141] r〇GR3:空间机器人基座质心〇b的位置向量；
[0142] PleR3:关节i的位置向量；
[0143] pe e R3:机械臂末端执行器的位置向量；
[0144] b〇eR3:从基座质心仏指向关节1的位置矢量；
[0145] ai，bieR3:分别为从关节i指向连杆i质心，从连杆i质心指向关节i+Ι的位置矢量；
[0146] ViGR3:连杆i质心的线速度；
[0147] ?1^护:连杆丨的角速度；
[0148] R/ e/?3x3:表示从坐标系Σ^Ι」坐标系Σ」的旋转矩阵；
[0149] VQeR3:基座的线速度；
[0150] co〇eR3:基座的角速度；
[0151] veeR3:机械臂末端执行器的线速度；
[0152] ω e e R3:机械臂末端执行器的角速度；
[0?53] Φ eRn:机械臂关节角向量；
[0154] E，0:分别为单位矩阵和零矩阵；
[0155]此外，定义向量1=|^，7,2]1'的叉乘算子？为：
[0157] 图11为激励轨迹优化求解流程，其中虚线框内的部分体现了自由漂浮空间机器人 激励轨迹优化设计的特殊之处。
[0158] 不同于固定基座机器人，自由漂浮空间机器人动力学参数辨识模型的回归矩阵中 不仅包含由机械臂关节运动轨迹直接决定的关节角度和关节角速度，还包含与待辨识动力 学参数相关的基座位置、姿态、线速度和角速度。其中，基座的线速度和角速度需要将关节 角速度和空间机器人的动力学参数(包括待辨识动力学参数的先验值)代入式(4)进行求 解，基座的位置和姿态则需要对基座线速度和角速度进行数值积分获得，因此在激励轨迹 优化设计过程中不可避免地要用到待辨识动力学参数的先验信息。
[0159] 图11中JdPJ1+1分别为经过i次和i+Ι次迭代优化后得到的激励轨迹所对应的目标 函数值，即回归矩阵A(k)的谱条件数。
[0160]以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按 照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书 的保护范围之内。
【主权项】
1. 一种空间机器人动力学参数辨识的激励轨迹优化方法，其特征在于，包括以下步骤： 1)建立自由漂浮空间机器人的参数辨识模型 空间机器人由一个串联机械臂和一个作为基座的航天器平台组成，其中机械臂由η个 旋转关节连接而成，所有构件均视为刚体;基座航天器上安装有测量基座位置、姿态、线速 度和角速度的敏感器，机械臂各关节都安装有测量关节位置和角速度的敏感器； 空间机器人末端执行器的线速度和角速度用矩阵形式统一表示为： e =/s0 +/』 （1) l_oe」 [_两」 式中： JS = 〇 E ，m (2) = ΓΑ,χ：(^-/?1) k2x(pe-p2) -· Λ"χ( W1 < h … < 」 空间机器人的线动量P和角动量L用矩阵形式统一表示为： 細: 卜]+卜1 [ ο ] 式中： ^^(L+m^ + I^R^ (5) Μ ~= Σ(，Λ,· +册 ΛΑ)Μ3χη (6) Μ JTco = 0) />1 jTi = [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0，.··，0] (8) jRi=[ki，k2，."，ki，0，.·_，0] (9) r〇g=rg-r〇, rgi = r i-rg, r〇i = r i~r〇 (10) 设系统初始动量为零，从式(4)中分离出待辨识的动力学参数，化简整理得到：式中： ?η [ IXX , - Ixy , - Ixz , Iyy , - Iyz , Izz ] ? 〇 ο ' 〇 Η, 〇 "?m. ?: 〇 _ 〇 ο Ο 、"ω"ζ_ ^Γ~\ η -1 . Η = ΣΙ-?ην? ^=Σ,-Ιο (?ω·+Γ/χ ηι^ )+^χρ" 构造一个以X = [l/msΒ?/ i/；f为未知量的线性回归方程组进行求解;假设在每个 采样点获取一组测量值，当完成第k次采样后，线性回归方程组能够表示如下： A(k)x = Y(k) (12) 式中：待辨识的动力学参数X可采用最小二乘算法进行估计： x = (ai (k)A(k)) 'a1 {k)\(k) (16) 2) 确定激励轨迹优化准则 选用回归矩阵A(k)的谱条件数作为激励轨迹的优化准则，如下所示：(17) 式中：〇max(A(k))表不矩阵A(k)的最大奇异值，〇min(A(k))表不矩阵A(k)的最小奇异值； 3) 空间机器人关节轨迹参数化 空间机器人机械臂第i个关节的关节位置使用有限傅里叶级数表示如下：(18) 式中：C0f为傅立叶级数的基频，φι()为关节位置偏移量;机械臂各关节采用相同的基频 来保证激励轨迹的周期性，空间机器人每个关节的参数化运动轨迹含有2Ν+1个待定系数； 对式（18)关于时间求一阶导数和二阶导数能够得到关节i的角速度和角加速度如下所 示： 4(，) = cos(r"/") + ,，; sin(%")) (19) /=1 x ' φ： (/) = 〇jt y^i^/co^(〇Jt^-(A]lsin {(〇. //j) (20) 1=1 、 4)激励轨迹优化问题求解 激励轨迹优化问题描述为如下形式： <i>*(t)=argmin(J) (21) 约束条件为： Mr:、=i' 4 (，，卜 〇 於（，") = 〇, 》:(，,) = 0 Vi < ' _ (22) ΦΓη<Φ,{?<ΦΓ Vi e[v tf]j <Γ<φ,{?)<φ- Vie[v tf],i 式（21)和式（22)为含多约束的非线性优化问题，采用MATLAB优化工具箱中的fmincon 函数求得该非线性优化问题的解，即步骤3)中待定系数α丨，贫和Φ1()的值。
【文档编号】B25J9/16GK106064377SQ201610389042
【公开日】2016年11月2日
【申请日】2016年6月2日 公开号201610389042.8, CN 106064377 A, CN 106064377A, CN 201610389042, CN-A-106064377, CN106064377 A, CN106064377A, CN201610389042, CN201610389042.8
【发明人】罗建军, 薛爽爽, 袁建平, 朱战霞, 马卫华, 王明明, 方群, 唐歌实, 胡松杰, 王保丰
【申请人】西北工业大学