工程科学学报 Chinese Journal of Engineering 空间机械臂在轨插、拔孔操作基于力位姿跟踪指数型阻抗控制 曾晨东陈力 Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator ZENG Chen-dong.CHEN Li 引用本文： 曾晨东，陈力.空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制.工程科学学报，2022,44(2：254-264.doi: 10.13374-issn2095-9389.2020.07.31.002 ZENG Chen-dong,CHEN Li.Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator[J].Chinese Journal of Engineering,2022,44(2):254-264.doi:10.13374/j.issn2095-9389.2020.07.31.002 在线阅读View online:https::/doi.org10.13374.issn2095-9389.2020.07.31.002 您可能感兴趣的其他文章 Articles you may be interested in 螺旋桨清洗机器人超灵巧机械臂设计 Ultra-smart manipulator design for propeller-cleaning robots 工程科学学报.2017,39(6：924htps:/ldoi.org10.13374.issn2095-9389.2017.06.016 基于EtherCAT.总线的七自由度机械臂的隐蔽攻击技术 Covert attack technology of EtherCAT based 7 degrees of freedom manipulator 工程科学学报.2020,42(12：1653htps:1doi.org10.13374.issn2095-9389.2019.12.07.002 受限空间不同照度环境下VDT作业视觉疲劳 Visual fatigue of VDT operation under different illumination conditions in confined space 工程科学学报.2020.42(12：1605htps:/doi.org10.13374.issn2095-9389.2020.08.20.002 基于多变量混沌时间序列的航班运行风险预测模型 Flight operation risk prediction model based on the multivariate chaotic time series 工程科学学报.2020,42(12：1664htps:/1doi.0rg10.13374j.issn2095-9389.2019.12.09.002 基于非线性模型预测控制的自动泊车路径跟踪 Path tracking of automatic parking based on nonlinear model predictive control 工程科学学报.2019,41(7)：947 https:loi.org10.13374.issn2095-9389.2019.07.014 高速公路绿篱修剪机器人手臂避障路径规划 Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator 工程科学学报.2019,41(1)：134htps:/1doi.org/10.13374j.issn2095-9389.2019.01.015
空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 曾晨东 陈力 Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator ZENG Chen-dong, CHEN Li 引用本文: 曾晨东, 陈力. 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制[J]. 工程科学学报, 2022, 44(2): 254-264. doi: 10.13374/j.issn2095-9389.2020.07.31.002 ZENG Chen-dong, CHEN Li. Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator[J]. Chinese Journal of Engineering, 2022, 44(2): 254-264. doi: 10.13374/j.issn2095-9389.2020.07.31.002 在线阅读 View online: https://doi.org/10.13374/j.issn2095-9389.2020.07.31.002 您可能感兴趣的其他文章 Articles you may be interested in 螺旋桨清洗机器人超灵巧机械臂设计 Ultra-smart manipulator design for propeller-cleaning robots 工程科学学报. 2017, 39(6): 924 https://doi.org/10.13374/j.issn2095-9389.2017.06.016 基于EtherCAT总线的七自由度机械臂的隐蔽攻击技术 Covert attack technology of EtherCAT based 7 degrees of freedom manipulator 工程科学学报. 2020, 42(12): 1653 https://doi.org/10.13374/j.issn2095-9389.2019.12.07.002 受限空间不同照度环境下VDT作业视觉疲劳 Visual fatigue of VDT operation under different illumination conditions in confined space 工程科学学报. 2020, 42(12): 1605 https://doi.org/10.13374/j.issn2095-9389.2020.08.20.002 基于多变量混沌时间序列的航班运行风险预测模型 Flight operation risk prediction model based on the multivariate chaotic time series 工程科学学报. 2020, 42(12): 1664 https://doi.org/10.13374/j.issn2095-9389.2019.12.09.002 基于非线性模型预测控制的自动泊车路径跟踪 Path tracking of automatic parking based on nonlinear model predictive control 工程科学学报. 2019, 41(7): 947 https://doi.org/10.13374/j.issn2095-9389.2019.07.014 高速公路绿篱修剪机器人手臂避障路径规划 Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator 工程科学学报. 2019, 41(1): 134 https://doi.org/10.13374/j.issn2095-9389.2019.01.015
工程科学学报.第44卷.第2期：254-264.2022年2月 Chinese Journal of Engineering,Vol.44,No.2:254-264,February 2022 https://doi.org/10.13374/j.issn2095-9389.2020.07.31.002;http://cje.ustb.edu.cn 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型 阻抗控制 曾晨东，陈力四 福州大学机械工程及自动化学院，福州350116 ☒通信作者.E-mail:chnle@fzu.edu.cn 摘要讨论了空间机械臂在轨插、拔孔操作的阻抗控制问题.为此，结合系统动量守恒关系，空间机械臂替换部件末端输 出插、拔孔主动力与孔内所受摩擦阻力作用关系，以及第二类拉格朗日方程，推导得到了载体位置、姿态均不受控制情况下， 空间机械臂在轨插、拔孔操作过程系统动力学方程.同时，根据相关操作控制系统设计需要，利用系统位置几何关系分析、建 立了空间机械臂替换部件末端相对基联坐标系的相对运动雅可比关系.之后，由空间机械臂替换部件末端位姿与末端输出 插、拔孔主动力之间的动态关系并结合阻抗控制原理，建立了二阶线性阻抗模型.在上述工作基础上，针对空间机械臂在轨 插、拔孔操作过程同时存在运动学与动力学不确定性的情况，设计了空间机械臂替换部件末端力位姿跟踪指数型阻抗控制 策略：并通过李雅普诺夫理论，证明了控制系统的稳定性.提到的控制策略具有结构简单、收敛速度快、稳定性好的特点.系 统数值仿真，验证了上述控制策略的有效性 关键词空间机械臂：在轨操作：插、拔孔操作：力/位姿：指数型阻抗控制 分类号TP242 Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator ZENG Chen-dong,CHEN L School of Mechanical Engineering and Automation,Fuzhou University,Fuzhou 350116,China Corresponding author,E-mail:chnle@fzu.edu.cn ABSTRACT With the developments in space technology and exploration,the space manipulator has become a better choice than astronauts for performing long-time and high-precision operation tasks,such as orbit assembly,orbit maintenance,and orbit refueling.In carrying out the above orbit service tasks,the space manipulator must perform insertion and extraction operations.By considering the impedance control,a dynamic relationship can be established between the pose and output force during insertion and extraction tasks.In this paper,the impedance control problems associated with the insertion and extraction operation of the space manipulator were discussed.By combining the conservation of the momentum of the system,relationship between the driving forces of insertion and extraction at the end of the replacement parts,friction resistance in the holes,and second Lagrange equation,we derived dynamic equations for the space manipulator during the orbit insertion and extraction operation when the position and attitude of the carrier were not controlled.In addition,based on the design requirements of the related operation and control systems,we established the Jacobian relation of the relative motion between the end of the replacement parts and the basic coordinate system by performing a geometric relation analysis of the system position.Then,we established a second-order linear impedance control model based on the dynamic 收稿日期：2020-07-31 基金项目：国家自然科学基金资助项目(1132273)：福建省工业机器人基础部件技术重大研发平台资助项目(2014H21010011)
空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型 阻抗控制 曾晨东，陈 力苣 福州大学机械工程及自动化学院，福州 350116 苣通信作者， E-mail: chnle@fzu.edu.cn 摘 要 讨论了空间机械臂在轨插、拔孔操作的阻抗控制问题. 为此，结合系统动量守恒关系，空间机械臂替换部件末端输 出插、拔孔主动力与孔内所受摩擦阻力作用关系，以及第二类拉格朗日方程，推导得到了载体位置、姿态均不受控制情况下， 空间机械臂在轨插、拔孔操作过程系统动力学方程. 同时，根据相关操作控制系统设计需要，利用系统位置几何关系分析、建 立了空间机械臂替换部件末端相对基联坐标系的相对运动雅可比关系. 之后，由空间机械臂替换部件末端位姿与末端输出 插、拔孔主动力之间的动态关系并结合阻抗控制原理，建立了二阶线性阻抗模型. 在上述工作基础上，针对空间机械臂在轨 插、拔孔操作过程同时存在运动学与动力学不确定性的情况，设计了空间机械臂替换部件末端力/位姿跟踪指数型阻抗控制 策略；并通过李雅普诺夫理论，证明了控制系统的稳定性. 提到的控制策略具有结构简单、收敛速度快、稳定性好的特点. 系 统数值仿真，验证了上述控制策略的有效性. 关键词 空间机械臂；在轨操作；插、拔孔操作；力/位姿；指数型阻抗控制 分类号 TP242 Exponential impedance control based on force/pose tracking for orbit insertion and extraction operation by space manipulator ZENG Chen-dong，CHEN Li苣 School of Mechanical Engineering and Automation, Fuzhou University, Fuzhou 350116, China 苣 Corresponding author, E-mail: chnle@fzu.edu.cn ABSTRACT With the developments in space technology and exploration, the space manipulator has become a better choice than astronauts for performing long-time and high-precision operation tasks, such as orbit assembly, orbit maintenance, and orbit refueling. In carrying out the above orbit service tasks, the space manipulator must perform insertion and extraction operations. By considering the impedance control, a dynamic relationship can be established between the pose and output force during insertion and extraction tasks. In this paper, the impedance control problems associated with the insertion and extraction operation of the space manipulator were discussed. By combining the conservation of the momentum of the system, relationship between the driving forces of insertion and extraction at the end of the replacement parts, friction resistance in the holes, and second Lagrange equation, we derived dynamic equations for the space manipulator during the orbit insertion and extraction operation when the position and attitude of the carrier were not controlled. In addition, based on the design requirements of the related operation and control systems, we established the Jacobian relation of the relative motion between the end of the replacement parts and the basic coordinate system by performing a geometric relation analysis of the system position. Then, we established a second-order linear impedance control model based on the dynamic 收稿日期: 2020−07−31 基金项目: 国家自然科学基金资助项目（11372273）；福建省工业机器人基础部件技术重大研发平台资助项目（2014H21010011） 工程科学学报，第 44 卷，第 2 期：254−264，2022 年 2 月 Chinese Journal of Engineering, Vol. 44, No. 2: 254−264, February 2022 https://doi.org/10.13374/j.issn2095-9389.2020.07.31.002; http://cje.ustb.edu.cn
曾晨东等：空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 255· relationship between the pose and driving force of the end of the replacement parts and the impedance control principle.Based on the above work,to address the uncertainty of the kinematics and dynamics of the orbit insertion and extraction operation,performed by the space manipulator,we designed an exponential impedance control strategy on the basis of force/pose tracking,and confirmed the stability of the control system based on the Lyapunov theory.The proposed control strategy has a simple structure,fast convergence speed,and good stability.As such,it is suitable for situations with limited computing and storage capacities,such as the space station computer.The numerical simulation results of this system verify the effectiveness of the proposed control strategy. KEY WORDS space manipulator;orbit operation;insertion and extraction operation;force/pose:exponential impedance control 空间站作为一种在近地轨道长时间运行的载 提出对空间机械臂在轨插、拔孔操作的阻抗控制 人航天器，其运行过程中部件的插拔、旋拧、搬运 问题进行研究.实际上，空间机械臂在轨执行插、 等复杂精密的太空操作任务大部分依赖于宇航员 拔孔操作过程中，由于操作空间有限，为避免发生 出舱执行而在宇航员出舱执行此类太空操作 较大碰撞，造成机械臂、替换部件及孔的损坏，替 任务时，由于操作空间有限、操作时间较长、精度 换部件末端跟踪期望位姿及输出力要求非常精 要求较高，其存在成本高、效率低、风险大等问 细，一般的，末端位置精度应优于1mm,姿态精度 题.为此，利用空间机械臂代替宇航员完成此类太 应优于0.5°，输出力精度应优于1~2N,且孔内的 空任务具有重要的实际意义5-] 摩擦阻力发生突变，即出现卡阻现象时，为使插、 到目前为止，世界各航天大国有近20座空 拔孔操作顺利完成，需采取相关策略.由此，对空 间机械臂正在或计划应用于执行一系列复杂 间机械臂在轨执行插、拔孔操作的研究具有一定 精密的在轨操作任务，其中，加拿大航天局的 的难度.考虑到Hogan2o1提出的阻抗控制可以通 CANADARM29,欧洲航天局与俄罗斯空间局的 过对机械臂阻抗参数进行调整，建立末端位姿和 ERAIO,日本航天局的JEMRMS,美国航天局的 输出力之间的动态关系.故本文尝试结合阻抗控 FREND2I等空间机械臂系统都已应用到空间站 制原理，对空间机械臂在轨插、拔孔操作的阻抗控 上，同时，还在不断提出的空间机械臂应用计划 制问题进行研究 有：美国的凤凰计划)、德国的轨道服务任务 对于空间机械臂控制策略的研究，Nanos和 等.针对空间机械臂执行太空操作任务方面的研 Papadopoulos2提出了一种柔性关节空间机械臂 究，程靖和陈力讨论了双臂捕获自旋卫星过程 末端跟踪的反馈线性化控制策略，付晓东和陈 的动力学演化模拟，以及捕获操作后其不稳定闭 力1设计了一种全柔性空间机械臂运动振动一体 链混合体系统的镇定控制问题.Yoshida等I基于 化输入受限重复学习控制策略.Wang等)研究 动量守恒定律研究了空间机械臂捕获卫星的碰撞 了空间机械臂捕获非合作卫星后组合体系统的解 动力学及运动学问题.Huang等m针对空间机械 耦策略及协调控制方案，提出了基于四次Bezier 臂捕获卫星后质量特性与反作用轮结构发生变化 曲线和特定约束条件下的自适应粒子群优化算 的问题，提出了一种改进的状态依赖Riccati方程 法.Luo等2]考虑了混合体系统的不可测状态、未 最优控制器：谢立敏和陈力⑧讨论了在空间机械 知惯性特性和外部干扰，提出了一种基于有限时 臂关节控制输入力矩幅值受限且系统存在不确 间收敛的鲁棒无惯性预定性能控制策略.上述控 定参数的复杂情况下，载体位置与姿态均不受控 制策略虽可对空间机械臂进行有效控制，但存在 的漂浮基空间机械臂系统的智能控制问题 结构较复杂、计算量较大等特点，不适用于空间机 Gasbarri和Piscull介绍了空间机械臂捕获卫星 械臂在轨插、拔孔操作.考虑到指数型跟踪控制 前后的两种控制策略，实现了对柔性构件的柔性 结构简单，可以在保证系统性能的基础上，加快收 激励补偿 敛速度，从而有效减少控制率的更新次数，节约系 综合分析上述研究成果可知，大多数学者关 统资源，优化控制成本基于此，本文提出了基 注于空间机械臂与非合作卫星的捕获对接操作， 于力/位姿跟踪指数型阻抗控制策略 并未对空间站运行过程中部件的插拔、旋拧、搬 本文研究了空间机械臂在轨插、拔孔操作的 运等复杂精密的太空操作任务进行相关研究.同 阻抗控制问题.首先，在建立载体位置、姿态均不 时，考虑到空间机械臂在轨部件替换及在轨燃料 受控的空间机械臂系统动力学方程的基础上，利 加注过程中，必然涉及插、拔孔操作.为此，本文 用系统位置几何关系分析、建立了替换部件末端
relationship between the pose and driving force of the end of the replacement parts and the impedance control principle. Based on the above work, to address the uncertainty of the kinematics and dynamics of the orbit insertion and extraction operation, performed by the space manipulator, we designed an exponential impedance control strategy on the basis of force/pose tracking, and confirmed the stability of the control system based on the Lyapunov theory. The proposed control strategy has a simple structure, fast convergence speed, and good stability. As such, it is suitable for situations with limited computing and storage capacities, such as the space station computer. The numerical simulation results of this system verify the effectiveness of the proposed control strategy. KEY WORDS space manipulator；orbit operation；insertion and extraction operation；force/pose；exponential impedance control 空间站作为一种在近地轨道长时间运行的载 人航天器，其运行过程中部件的插拔、旋拧、搬运 等复杂精密的太空操作任务大部分依赖于宇航员 出舱执行[1−4] . 而在宇航员出舱执行此类太空操作 任务时，由于操作空间有限、操作时间较长、精度 要求较高，其存在成本高、效率低、风险大等问 题. 为此，利用空间机械臂代替宇航员完成此类太 空任务具有重要的实际意义[5−8] . 到目前为止，世界各航天大国有近 20 座空 间机械臂正在或计划应用于执行一系列复杂 精密的在轨操作任务. 其中 ，加拿大航天局的 CANADARM2[9] ，欧洲航天局与俄罗斯空间局的 ERA[10] ，日本航天局的 JEMRMS[11] ，美国航天局的 FREND[12] 等空间机械臂系统都已应用到空间站 上. 同时，还在不断提出的空间机械臂应用计划 有：美国的凤凰计划[13]、德国的轨道服务任务[14] 等. 针对空间机械臂执行太空操作任务方面的研 究，程靖和陈力[15] 讨论了双臂捕获自旋卫星过程 的动力学演化模拟，以及捕获操作后其不稳定闭 链混合体系统的镇定控制问题. Yoshida 等[16] 基于 动量守恒定律研究了空间机械臂捕获卫星的碰撞 动力学及运动学问题. Huang 等[17] 针对空间机械 臂捕获卫星后质量特性与反作用轮结构发生变化 的问题，提出了一种改进的状态依赖 Riccati 方程 最优控制器；谢立敏和陈力[18] 讨论了在空间机械 臂关节控制输入力矩幅值受限且系统存在不确 定参数的复杂情况下，载体位置与姿态均不受控 的 漂 浮 基 空 间 机 械 臂 系 统 的 智 能 控 制 问 题 . Gasbarri 和 Pisculli[19] 介绍了空间机械臂捕获卫星 前后的两种控制策略，实现了对柔性构件的柔性 激励补偿. 综合分析上述研究成果可知，大多数学者关 注于空间机械臂与非合作卫星的捕获对接操作， 并未对空间站运行过程中部件的插拔、旋拧、搬 运等复杂精密的太空操作任务进行相关研究. 同 时，考虑到空间机械臂在轨部件替换及在轨燃料 加注过程中，必然涉及插、拔孔操作. 为此，本文 提出对空间机械臂在轨插、拔孔操作的阻抗控制 问题进行研究. 实际上，空间机械臂在轨执行插、 拔孔操作过程中，由于操作空间有限，为避免发生 较大碰撞，造成机械臂、替换部件及孔的损坏，替 换部件末端跟踪期望位姿及输出力要求非常精 细，一般的，末端位置精度应优于 1 mm，姿态精度 应优于 0.5°，输出力精度应优于 1～2 N，且孔内的 摩擦阻力发生突变，即出现卡阻现象时，为使插、 拔孔操作顺利完成，需采取相关策略. 由此，对空 间机械臂在轨执行插、拔孔操作的研究具有一定 的难度. 考虑到 Hogan[20] 提出的阻抗控制可以通 过对机械臂阻抗参数进行调整，建立末端位姿和 输出力之间的动态关系. 故本文尝试结合阻抗控 制原理，对空间机械臂在轨插、拔孔操作的阻抗控 制问题进行研究. 对于空间机械臂控制策略的研究，Nanos 和 Papadopoulos[21] 提出了一种柔性关节空间机械臂 末端跟踪的反馈线性化控制策略. 付晓东和陈 力[22] 设计了一种全柔性空间机械臂运动振动一体 化输入受限重复学习控制策略. Wang 等[23] 研究 了空间机械臂捕获非合作卫星后组合体系统的解 耦策略及协调控制方案，提出了基于四次 Bézier 曲线和特定约束条件下的自适应粒子群优化算 法. Luo 等[24] 考虑了混合体系统的不可测状态、未 知惯性特性和外部干扰，提出了一种基于有限时 间收敛的鲁棒无惯性预定性能控制策略. 上述控 制策略虽可对空间机械臂进行有效控制，但存在 结构较复杂、计算量较大等特点，不适用于空间机 械臂在轨插、拔孔操作. 考虑到指数型跟踪控制 结构简单，可以在保证系统性能的基础上，加快收 敛速度，从而有效减少控制率的更新次数，节约系 统资源，优化控制成本[25] . 基于此，本文提出了基 于力/位姿跟踪指数型阻抗控制策略. 本文研究了空间机械臂在轨插、拔孔操作的 阻抗控制问题. 首先，在建立载体位置、姿态均不 受控的空间机械臂系统动力学方程的基础上，利 用系统位置几何关系分析、建立了替换部件末端 曾晨东等： 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 · 255 ·
256 工程科学学报，第44卷，第2期 在基联坐标系下的运动雅可比关系.结合阻抗控 51为侧面孔轴线到载体底部的距离；52为正面孔轴 制原理，建立了二阶线性阻抗模型.然后，设计了 线到载体右侧的距离；F为替换部件末端输出力 基于力位姿跟踪指数型阻抗控制策略.控制策略 及力矩；F、F2为插、拔孔操作过程孔内摩擦阻力 分为位姿跟踪控制内环和阻抗控制外环.最后，为 及力矩 保证插孔操作过程的精确控制，采用分段控制方 在系统惯性参考坐标系XOY下，根据各分体 案将插孔操作细分为两个阶段.将上述力位姿跟 位置满足的几何关系及系统总质心的定义，可得： 踪指数型阻抗控制策略应用于空间机械臂在轨 ro =rD+Nooeo +Nole1+No2e2+No3e3 插、拔孔操作，并对操作过程仿真分析，校验所提 r1=rp+Nioeo Nue1+Ni2e2+Ni3e3 (1) 控制策略的正确性和有效性 r2=rD+N2oeo+N2ie1 N22e2 N23e3 r3 rD+N30e0+N31e1 N32e2 N33e3 1运动学、动力学及阻抗模型分析建立 rp rp+Npoeo Npie1 Np2e2 Np3e3 式中，e(i=1,2,3)为x(i=1,2,3)方向上的单位矢量， 11运动学、动力学模型分析建立 N0j、NjN2Nj、N(i=0,1,2,3)为系统惯性参数 空间机械臂在轨插、拔孔操作模型如图1所 的组合函数 示，其由自由漂浮的载体G0,机械臂杆G、G2、 若忽略太空的微重力影响，空间机械臂系统 G3及替换部件Gp组成，取OD0、ODii=1,2,3)、 满足动量守恒定律，假设系统初始动量为零，则： ODP分别为载体、机械臂杆、替换部件的质心；Oo、 0(i=1,2,3)分别为载体、关节铰几何中心，其中 ∑mi+mp=Mm=0 (2) O0与0D0重合；x0为00到01的连线，xi=1,2,3)为机 械臂杆G的对称轴；XOY、x0Oo0、xO(i=1,2,3) 结合式(1)、式(2)，根据系统在惯性坐标系中 分别为系统惯性参考坐标系、载体质心坐标系、关 的几何位置关系，可得到系统总动能表达式： 节中心坐标系；XOY为系统基联坐标系，X方向 (3) 与载体水平面平行，Y方向与载体竖直平面平行 02 式中，w0=%、w1=o+A、w2=0+0+凸、w=0+ d1+d2+d3 03 忽略太空微弱重力梯度，可知系统势能U为 61 零，因而拉格朗日函数为： O, G L=T-U 根据第二类拉格朗日方程： d (aL aL =0 (5) 图1空间机械臂在轨插、拔孔操作模型 Fig.I Model of space manipulator orbit insertion and extraction 式中，1为时间，q=[o,,2,T为系统广义坐标， operation Q∈R4x为系统广义力.因此，可得到欠驱动形式 定义载体质量、转动惯量和质心O到O1的距 的空间机械臂系统动力学模型为： 离分别为mo、0、do;各臂杆质量、转动惯量、长度 分别为m、、s(i=1,2,3):替换部件Gp的质量、转 M(Qj+C(q.q=0. (6) 动惯量、长度分别为p、p、S;关节铰中心O,到臂 式中：M(g)eR4x4为系统对称、正定的惯性矩阵， 杆G质心的距离为d(i=1,2,3):载体姿态角、关节 C(q,9)9为包含科氏力、离心力列向量；te∈R3x1为 转角、替换部件末端转角分别为o、0,(i=1,2,3)、m; 关节电机输出力矩 系统总质量为M,总质心为D,矢径为D;载体质心 对式(6)进行如下分块： 矢径为ro;各臂杆质心矢径为r:(i=1,2,3):替换部 :]L第]+]L多]=[] 件末端P点在惯性坐标系下的矢径为P,在基联坐 7 标系下的矢径为rp:h为侧面孔的深度；为正面 式中：M1I∈RIxI、M13∈Rx3、M31eR3xI、M33∈R3x3 孔的深度；g1为侧面孔的直径；g2为正面孔的直径； 分别为矩阵M(q)的子矩阵；C11eRx、C13eRlx3
在基联坐标系下的运动雅可比关系. 结合阻抗控 制原理，建立了二阶线性阻抗模型. 然后，设计了 基于力/位姿跟踪指数型阻抗控制策略. 控制策略 分为位姿跟踪控制内环和阻抗控制外环. 最后，为 保证插孔操作过程的精确控制，采用分段控制方 案将插孔操作细分为两个阶段. 将上述力/位姿跟 踪指数型阻抗控制策略应用于空间机械臂在轨 插、拔孔操作，并对操作过程仿真分析，校验所提 控制策略的正确性和有效性. 1 运动学、动力学及阻抗模型分析建立 1.1 运动学、动力学模型分析建立 G0 G1 G2 G3 GP OD0 ODi(i = 1,2,3) ODP O0 Oi(i = 1,2,3) O0 OD0 x0 O0 O1 xi(i = 1,2,3) Gi XOY x0O0y0 xiOiyi(i = 1,2,3) X ′ O1Y ′ X ′ Y ′ 空间机械臂在轨插、拔孔操作模型如图 1 所 示 ，其由自由漂浮的载体 ，机械臂杆 、 、 及 替 换 部 件 组 成 . 取 、 、 分别为载体、机械臂杆、替换部件的质心； 、 分别为载体、关节铰几何中心，其中 与 重合； 为 到 的连线， 为机 械臂杆 的对称轴； 、 、 分别为系统惯性参考坐标系、载体质心坐标系、关 节中心坐标系； 为系统基联坐标系， 方向 与载体水平面平行， 方向与载体竖直平面平行. G1 G2 G3 G0 O2 O1 O3 GP O Y X x0 x1 x2 x3 P Ff1 FP r0 r1 r2 r3 rP θ0 θ1 θ2 θ3 Ff2 s2 OD1 OD2 OD3 ODP X' Y' r ′ P s1 g1 h2 h1 g2 D rD O0 (OD0) 图 1 空间机械臂在轨插、拔孔操作模型 Fig.1 Model of space manipulator orbit insertion and extraction operation O0 O1 m0 I0 d0 mi Ii ςi(i = 1,2,3) GP mP IP ςP Oi Gi di(i = 1,2,3) θ0 θi(i = 1,2,3) θP M D rD r0 ri(i = 1,2,3) P rP r ′ P h1 h2 g1 g2 定义载体质量、转动惯量和质心 到 的距 离分别为 、 、 ；各臂杆质量、转动惯量、长度 分别为 、 、 ；替换部件 的质量、转 动惯量、长度分别为 、 、 ；关节铰中心 到臂 杆 质心的距离为 ；载体姿态角、关节 转角、替换部件末端转角分别为 、 、 ； 系统总质量为 ，总质心为 ，矢径为 ；载体质心 矢径为 ；各臂杆质心矢径为 ；替换部 件末端 点在惯性坐标系下的矢径为 ，在基联坐 标系下的矢径为 ； 为侧面孔的深度； 为正面 孔的深度； 为侧面孔的直径； 为正面孔的直径； s1 s2 FP Ff1 Ff2 为侧面孔轴线到载体底部的距离； 为正面孔轴 线到载体右侧的距离； 为替换部件末端输出力 及力矩； 、 为插、拔孔操作过程孔内摩擦阻力 及力矩. 在系统惯性参考坐标系 XOY 下，根据各分体 位置满足的几何关系及系统总质心的定义，可得： r0 = rD +N00e0 +N01e1 +N02e2 +N03e3 r1 = rD +N10e0 + N11e1 + N12e2 + N13e3 r2 = rD +N20e0 + N21e1 + N22e2 + N23e3 r3 = rD +N30e0 + N31e1 + N32e2 + N33e3 rP = rD +NP0e0 + NP1e1 + NP2e2 + NP3e3 （1） ei(i = 1,2,3) xi(i = 1,2,3) N0 j N1 j N2 j N3 j NPi(i = 0,1,2,3) 式中， 为 方向上的单位矢量， 、 、 、 、 为系统惯性参数 的组合函数. 若忽略太空的微重力影响，空间机械臂系统 满足动量守恒定律，假设系统初始动量为零，则： ∑ 3 i=0 mir˙i +mPr˙P = Mr˙D = 0 （2） 结合式 (1)、式 (2)，根据系统在惯性坐标系中 的几何位置关系，可得到系统总动能表达式： T = ∑ 3 i=0 ( 1 2 Iiω 2 i + 1 2 mir˙ 2 i ) + 1 2 IPω 2 3 + 1 2 mPr˙ 2 P （3） ω0 = θ˙ 0 ω1 = θ˙ 0 +θ˙ 1 ω2 = θ˙ 0 +θ˙ 1 +θ˙ 2 ω3 = θ˙ 0+ θ˙ 1 +θ˙ 2 +θ˙ 3 式中， 、 、 、 . 忽略太空微弱重力梯度，可知系统势能 U 为 零，因而拉格朗日函数为： L = T −U = ∑ 3 i=0 ( 1 2 Iiω 2 i + 1 2 mir˙ 2 i ) + 1 2 IPω 2 3+ 1 2 mPr˙ 2 P（4） 根据第二类拉格朗日方程： d dt ( ∂L ∂q˙ ) − ∂L ∂q = Q （5） t q = [θ0, θ1, θ2, θ3] T Q ∈ R 4×1 式中， 为时间， 为系统广义坐标， 为系统广义力. 因此，可得到欠驱动形式 的空间机械臂系统动力学模型为： M(q) q¨ +C(q, q˙) q˙ = [ 0,τc T ]T （6） M(q) ∈ R 4×4 C(q, q˙)q˙ τc ∈ R 3×1 式中： 为系统对称、正定的惯性矩阵， 为包含科氏力、离心力列向量； 为 关节电机输出力矩. 对式 (6) 进行如下分块： [ M11 M13 M31 M33 ] [ θ¨ 0 q¨θ ] + [ C11 C13 C31 C33 ] [ θ˙ 0 q˙θ ] = [ 0 τc ] （7） M11 ∈ R 1×1 M13 ∈ R 1×3 M31 ∈ R 3×1 M33 ∈ R 3×3 M(q) C11 ∈ R 1×1 C13 ∈ R 1×3 式中： 、 、 、 分别为矩阵 的子矩阵； 、 、 · 256 · 工程科学学报，第 44 卷，第 2 期
曾晨东等：空间机械臂在轨插、拔孔操作基于力位姿跟踪指数型阻抗控制 257· C31∈R3x1、C33∈R3x3分别为矩阵C(g)的子矩阵； TPe=Jf(Fp-Fe) (11) qe=[01,02,3]T.式(7乘起来后所得第一行的首次 结合上述内容，并定义Ar为q变换到X的过 积分为系统的动量矩守恒关系，而系统的动量守恒 程，则可得到如图2所示的阻抗控制流程. 关系已在系统动力学方程的推导中耦合进去了 考虑到空间机械臂在轨插、拔孔操作中，载体 位置、姿态均不受控，因此需要研究替换部件末端 在基联坐标系内的轨迹运动情况.为建立替换部 件末端P点相对于载体G0的位姿关系，将P点相对 于O的矢径。向基联坐标系XOY内投影，并定义 S3P=S3+Sp,得到： =S1 cos01+52cos(01+02)+53P cos(01+02+03) yp=S1 sin01+s2sin(01+0)+S3p sin(01+02+03) 8) 又可知，在基联坐标系XO1Y内，臂杆G3与替 换部件姿态保持一致，故有r=,。=1++ 结合式(8)对时间求导，得到替换部件末端P点相 内内店 Joint torque control 对运动Jacobian关系： [,,的J=J01,2,d3T (9) Space manipulator 式中，JreR3x3为相对运动Jacobian矩阵 1.2阻抗模型分析建立 阻抗控制作为机械臂操作控制中非常有效的 一种控制方法，目标是通过调整机械臂阻抗参数 图2阻抗控制流程 来保持末端执行器的位姿、末端执行器与环境之 Fig.2 Impedance control process 间接触力的理想动态关系.阻抗控制的具体实现 由此，在空间机械臂在轨插、拔孔操作过程 是通过阻抗关系模型将力和位姿容纳到同一个框 中，若开启阻抗控制，则式(6)表示的载体位置、姿 架，对力和位姿的动态关系进行调整 态均不受控的空间机械臂系统动力学模型可写为： 考虑到空间机械臂在轨插、拔孔操作对替换 M(q)q+C(q.)q=[0.reT+[0.TpeT (12) 部件末端输出力和位姿的要求，将阻抗控制应用 于空间机械臂在轨插、拔孔操作不失为一种有益 2基于力/位姿跟踪指数型阻抗控制策略 尝试.一般的，替换部件末端阻抗关系的数学模型 设计 可表现为二阶微分方程形式，环境模型可近似为 二阶非线性函数形式： 2.1指数型跟踪控制内环设计 考虑空间机械臂在轨插、拔孔操作过程，系统 Mp(Xd-X)+Bp(Xd-X)+Kp(Xd-X)=Fp (10) 存在动力学不确定及扰动，则式(6)可写为： Be(X-Xe)+Ke(X-Xe)=Fe M(q)q+C(q.)q+rd=[0.reT] (13) 式中：X=、x=%'分别为替 式中，r表示系统动力学不确定及扰动项 换部件末端实际位姿、期望位姿；MpeR3x3、 其满足如下结构特性： Bp∈R3x3、Kp∈R3x3分别为机械臂惯量矩阵、阻尼 特性1M(q以、C(q,)满足一致有界性： 矩阵、刚度矩阵；Fp∈Rx1、Fe∈Rx1分别为替换部 Mm≤IM(q)l≤MM,IlC(g,q)l≤Cm llall 件末端输出力及力矩、接触力及力矩；Be∈R3x3、 其中，Mm、MM、Cm为正常数 Ke∈R3x3分别为环境阻尼矩阵、刚度矩阵 特性2矩阵M(g)-2C(q,)满足斜对称性，适 根据式(10)可计算出替换部件末端输出力及 当选取C(q,),对于任意的z∈Rx1: 力矩Fp与替换部件末端接触力及力矩F。的误差， 得到如下表达式： 22M(g)z-2C(g,)z=0
C31 ∈ R 3×1 C33 ∈ R 3×3 C(q) qθ = [θ1, θ2, θ3] T 、 分别为矩阵 的子矩阵 ； . 式 (7) 乘起来后所得第一行的首次 积分为系统的动量矩守恒关系，而系统的动量守恒 关系已在系统动力学方程的推导中耦合进去了. P G0 P O1 r ′ P X ′ O1Y ′ ς3P = ς3 + ςP 考虑到空间机械臂在轨插、拔孔操作中，载体 位置、姿态均不受控，因此需要研究替换部件末端 在基联坐标系内的轨迹运动情况. 为建立替换部 件末端 点相对于载体 的位姿关系，将 点相对 于 的矢径 向基联坐标系 内投影，并定义 ，得到： { x ′ P = ς1 cos θ1 +ς2 cos(θ1 +θ2)+ς3P cos(θ1 +θ2 +θ3) y ′ P = ς1 sinθ1 +ς2 sin(θ1 +θ2)+ς3P sin(θ1 +θ2 +θ3) （8） X ′ O1Y ′ G3 θP ′ = θ3 θ˙ ′ P = θ˙ 1 +θ˙ 2 +θ˙ 3 t P 又可知，在基联坐标系 内，臂杆 与替 换部件姿态保持一致，故有 ， . 结合式 (8) 对时间 求导，得到替换部件末端 点相 对运动 Jacobian 关系： [ ˙x ′ P , y˙ ′ P , θ˙ ′ P ] T = Jr[θ˙ 1, θ˙ 2, θ˙ 3] T （9） Jr ∈ R 式中， 3×3 为相对运动 Jacobian 矩阵. 1.2 阻抗模型分析建立 阻抗控制作为机械臂操作控制中非常有效的 一种控制方法，目标是通过调整机械臂阻抗参数 来保持末端执行器的位姿、末端执行器与环境之 间接触力的理想动态关系. 阻抗控制的具体实现 是通过阻抗关系模型将力和位姿容纳到同一个框 架，对力和位姿的动态关系进行调整. 考虑到空间机械臂在轨插、拔孔操作对替换 部件末端输出力和位姿的要求，将阻抗控制应用 于空间机械臂在轨插、拔孔操作不失为一种有益 尝试. 一般的，替换部件末端阻抗关系的数学模型 可表现为二阶微分方程形式，环境模型可近似为 二阶非线性函数形式： MP ( X¨ d − X¨ ) + BP ( X˙ d − X˙ ) + KP (Xd − X) = FP Be ( X˙ − X˙ e ) + Ke (X− Xe) = Fe （10） X = [ x ′ P , y ′ P , θ′ P ]T Xd = [ x ′ Pd, y ′ Pd, θ′ Pd]T MP ∈ R 3×3 BP ∈ R 3×3 KP ∈ R 3×3 FP ∈ R 3×1 Fe ∈ R 3×1 Be ∈ R 3×3 Ke ∈ R 3×3 式中： 、 分别为替 换 部 件 末 端 实 际 位 姿 、 期 望 位 姿 ； 、 、 分别为机械臂惯量矩阵、阻尼 矩阵、刚度矩阵； 、 分别为替换部 件末端输出力及力矩、接触力及力矩； 、 分别为环境阻尼矩阵、刚度矩阵. FP Fe 根据式 (10) 可计算出替换部件末端输出力及 力矩 与替换部件末端接触力及力矩 的误差， 得到如下表达式： τPe = J T r (FP − Fe) （11） 结合上述内容，并定义 Ar为 qθ 变换到 X 的过 程，则可得到如图 2 所示的阻抗控制流程. Joint torque control Space manipulator + + + + + + + + + − − − − Xd ·· X ·· qθ ·· qθ · qθ Fe Mp Fp Jr Jr J r T Ar X Bp Kp Xd · X · Xd Jr · 图 2 阻抗控制流程 Fig.2 Impedance control process 由此，在空间机械臂在轨插、拔孔操作过程 中，若开启阻抗控制，则式 (6) 表示的载体位置、姿 态均不受控的空间机械臂系统动力学模型可写为： M(q) q¨ +C(q, q˙) q˙ = [ 0,τc T ]T + [ 0,τPe T ]T （12） 2 基于力/位姿跟踪指数型阻抗控制策略 设计 2.1 指数型跟踪控制内环设计 考虑空间机械臂在轨插、拔孔操作过程，系统 存在动力学不确定及扰动，则式 (6) 可写为： M(q) q¨ +C(q, q˙) q˙ +τd = [ 0,τc T ]T （13） 式中， τd 表示系统动力学不确定及扰动项. 其满足如下结构特性： 特性 1 M(q)、C(q, q˙) 满足一致有界性： Mm ⩽ ∥M(q)∥ ⩽ MM，∥C(q, q˙)∥ ⩽ Cm ∥q˙∥ 其中， Mm、MM、Cm为正常数. M(q)−2C(q, q˙) C(q, q˙) z ∈ R 4×1 特性 2 矩阵 满足斜对称性，适 当选取 ，对于任意的 ： 1 2 z TM˙ (q)z− z TC(q, q˙)z = 0 曾晨东等： 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 · 257 ·