空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制

工程科学学报 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 ·

258 工程科学学报，第44卷，第2期 由于空间机械臂系统动力学方程是欠驱动、 Mg(q)=[O.reT]"-f(q.q)-rd (23) 不完全能控的，这将不利于控制系统的设计.在 此，考虑采用增广变量的方法，即虚拟扩展系统的 式中，f(q,)=-M(q)9+C(q,)g 引入新的状态变量： 控制输入及输出，以解决这一问题.为此，对系统 输出的X加以扩展，定义Y=[o,x,p,I,Ya= x(=h0.r]-f(q.-x(Odt-hM:(q) [,ap,a,则可导出新的运动雅可比关系： (24) ǐ=J9 (14) 式中，为正实数，为积分变量.由此可得： 式中，J=￡01 产=-lx+lrd (25) E为1×1单位矩阵 对于式(25)中的系统动力学不确定及扰动项 由于系统存在运动学不确定，Y与q之间的运 rd,定义相关估计率： 动学关系可以表示为： a=(1l2r+2)/(1l2) (26) 卫=J(q)9+u (15) 式中，y2=2X,2为正实数，为x的估计值 式中，u=△J(q)9 设计相关观测器： 根据式(15)又可引入如下表达式： =-238+1/l22+l32 (27) 立v=J(q)g+kYe (16) 式中，为正实数. 式中，Y。=Y-Y表示Y与之间的误差，k为正实数 定理2对于系统动力学不确定及扰动项x,其 结合式(15)、式(16)可知，Y满足： 可以通过式(2l)进行估计，且估计误差a=td-d 立e=-kYe+u (17) 是全局指数型稳定的 对于式(17)中的不确定项私，定义相关估计率： 证明根据式(25)及式(27)，定义龙=X-￠为观 i=(k1k2立e+y1）/k2 (18) 测器误差，对其求导得： =X-(-l2l3求+1/l22+l3y2)= 式中，y1=k2Ye,k2为正实数 X-(-l23求+文+l23X)=-l23r (28) 设计相关观测器： 由此可得： 立。=-kk3立e+1/k21+k3y1 (19) ()=(0)e-1 (29) 式中，k3为正实数 上式表明，在观测器中，对于任意初始状态值 定理1对于系统运动学不确定项私，其可以通 (O),x①)都满足全局指数型稳定 过式(18)进行估计，且估计误差=u-是全局指 结合式(25)、式(26)、式(29)，估计误差可 数型稳定的 写成： 证明根据式(17)及式(19)，定义立=Y-立为 id=Td-id=+x(+h)/= 观测器误差，对其求导得： =(0)e-hlst (30) 立e=立e-(-k2k3立e+1/k21+k3i)= 通过上式可知，所设计的估计率可以保证：的 卫e-(-k2k3e+立e+k2k3Ye)=-k2k3立e (20) 全局指数型稳定，证毕 由此可得： 为实现轨迹跟踪，定义状态变量： 立e()=立e(0)e-k2ksr (21) a1=[J (q)]*(kc1s-i) (31) 上式表明，在观测器中，对于任意初始状态值 式中，ke为正实数，[J(q为J(q)的伪逆，s=Ya-Y. 卫(O),立()都满足全局指数型稳定 引入新的状态变量： 结合式(17)、式(18)、式(21)，估计误差可写成： z1=9-[J(q]t立a-1 (32) i=u-i Ye kiYe-(k2Ye+kik2Ye)/ 结合定理1、定理2，设计如下控制力矩： k2=k1立e=k立e0)e-kst (22) [O.TeT]=-ke2zI+[J()]s+id-M(q) 通过上式可知，所设计的估计率可以保证估 计误差的全局指数型稳定，证毕 aUga-Ugra-an+sor (33) 考虑式(13)中存在系统动力学不确定及扰动， 引入广义惯性矩阵M(g)=M(q)g,对其求导得： 式中，k2为正实数，参数6的作用在于使式(33)乘

X Y = [θ0, x ′ P , y ′ P , θ′ P ] T Yd = [θ0, x ′ Pd, y ′ Pd, θ′ Pd] T 由于空间机械臂系统动力学方程是欠驱动、 不完全能控的，这将不利于控制系统的设计. 在 此，考虑采用增广变量的方法，即虚拟扩展系统的 控制输入及输出，以解决这一问题. 为此，对系统 输 出 的 加 以 扩 展 ， 定 义 ， ，则可导出新的运动雅可比关系： Y˙ = Jq˙ （14） J = [ E 0 0 Jr ] 式中， ，E 为 1×1 单位矩阵. 由于系统存在运动学不确定， Y 与 q 之间的运 动学关系可以表示为： Y˙ = J (q) q˙ +u （15） 式中， u = ∆J (q) q˙. 根据式 (15) 又可引入如下表达式： Y˙ v = J (q) q˙ +k1Ye （16） 式中， Ye = Y −Yv表示 Yv与 Y 之间的误差， k1 为正实数. 结合式 (15)、式 (16) 可知， Ye满足： Y˙ e = −k1Ye +u （17） 对于式 (17) 中的不确定项 u ，定义相关估计率： uˆ = ( k1k2Yˆ e + y˙1 ) /k2 （18） 式中， y1 = k2Ye，k2 为正实数. 设计相关观测器： ˙Yˆ e = −k2k3Yˆ e +1/k2 y˙1 +k3 y1 （19） 式中， k3 为正实数. u u˜ = u−uˆ 定理 1 对于系统运动学不确定项 ，其可以通 过式 (18) 进行估计，且估计误差 是全局指 数型稳定的. Y˜ e = Ye −Yˆ 证明 根据式 (17) 及式 (19)，定义 e为 观测器误差，对其求导得： ˙Y˜ e = Y˙ e −(−k2k3Yˆ e +1/k2 y˙1 +k3 y1) = Y˙ e −(−k2k3Yˆ e +Y˙ e +k2k3Ye)=−k2k3Y˜ e （20） 由此可得： Y˜ e (t) = Y˜ e(0)e−k2k3t （21） Yˆ e (0) Y˜ e (t) 上式表明，在观测器中，对于任意初始状态值 ， 都满足全局指数型稳定. 结合式 (17)、式 (18)、式 (21)，估计误差 u˜可写成： u˜ = u−uˆ = Y˙ e + k1Ye −(k2Y˙ e +k1k2Yˆ e)/ k2 = k1Y˜ e = k1Y˜ e(0)e−k2k3t （22） u˜ 通过上式可知，所设计的估计率可以保证估 计误差 的全局指数型稳定，证毕. Mg (q) = M(q) q˙ 考虑式 (13) 中存在系统动力学不确定及扰动， 引入广义惯性矩阵 ，对其求导得： M˙ g (q) = [ 0,τc T ]T − f (q, q˙)−τd （23） f (q, q˙) = −M˙ 式中， (q) q˙ +C(q, q˙) q˙. 引入新的状态变量： χ(t) = l1 w t 0 [[ 0,τc T ]T − f (q, q˙)−χ(ℓ) ] dℓ−l1Mg (q) （24） 式中， l1 为正实数， ℓ 为积分变量. 由此可得： χ˙ = −l1χ+l1τd （25） τd 对于式 (25) 中的系统动力学不确定及扰动项 ，定义相关估计率： τˆd = ( l1l2χˆ + y˙2 ) / (l1l2) （26） 式中， y2 = l2χ，l2 为正实数， χˆ为 χ 的估计值. 设计相关观测器： ˙χˆ = −l2l3χˆ +1/l2y˙2 +l3y2 （27） 式中， l3 为正实数. τd τ˜d = τd −τˆd 定理 2 对于系统动力学不确定及扰动项 ，其 可以通过式 (21) 进行估计，且估计误差 是全局指数型稳定的. 证明 根据式 (25) 及式 (27)，定义 χ˜ = χ−χˆ为观 测器误差，对其求导得： ˙χ˜=χ˙ −(−l2l3χˆ +1/l2 y˙2 +l3 y2) = χ˙ −(−l2l3χˆ +χ˙ +l2l3χ) = −l2l3χ˜ （28） 由此可得： χ˜ (t) = χ˜(0)e−l2l3t （29） χˆ (0) χ˜ (t) 上式表明，在观测器中，对于任意初始状态值 ， 都满足全局指数型稳定. 结 合 式 (25)、 式 (26)、 式 (29)，估计误差 τ˜d 可 写成： τ˜d = τd −τˆd = χ˙ + l1χ−(l2χ˙ +l1l2χˆ)/l2 = l1χ˜ = l1χ˜(0)e−l2l3t （30） 通过上式可知，所设计的估计率可以保证 τ˜d 的 全局指数型稳定，证毕. 为实现轨迹跟踪，定义状态变量： α1 = [ J (q) ]+ (kc1ε−uˆ) （31） kc1 [ J (q) ]+ 式中， 为正实数， 为 J (q) 的伪逆， ε = Yd −Y. 引入新的状态变量： z1 = q˙ − [ J (q) ]+Y˙ d −α1 （32） 结合定理 1、定理 2，设计如下控制力矩： [ 0,τc T ]T = −kc2 z1 + [ J(q) ]T ε+τˆd − M(q)    d ([ J(q) ]+ ) dt Y˙ d − [ J(q) ]+Y¨ d −α˙ 1    +[δ,0] T （33） 式中， kc2 为正实数，参数 δ 的作用在于使式 (33) 乘 · 258 · 工程科学学报，第 44 卷，第 2 期

曾晨东等：空间机械臂在轨插、拔孔操作基于力位姿跟踪指数型阻抗控制 259 起来后所得第一行的右端满足恒为零，而式 体位置、姿态均不受控空间机械臂系统动力学模 (33)乘起来后的其余各行为空间机械臂的实际控 型可写为： 制输入. M()q+C(q.)q+Td=O.Te+[0.TpeT (40) 定理3若式(33)满足2k2k3-1>0,223-1>0, 2k1-1>0,2k2-1>0,则轨迹跟踪误差6满足全局 联立式(10)、式(11)可构成控制策略中的阻 抗控制外环部分，将阻抗关系式与指数型跟踪控 指数型收敛 制相结合，并根据替换部件末端输出力及力矩 证明根据ε及1的定义，结合式(15)及式 Fp与替换部件末端接触力及力矩Fe的误差，在线 (32),可得： 修正替换部件末端位姿，获得对替换部件末端输 e=立a-立=立a-J(q)a1+[J(q立a+-u= 出力及力矩的跟踪.由此，对应的控制力矩可变为： -ke1s-J(q)zI-i (34) [0,re'-=-ke2z1+ 定义Lyapunov函数： [J(g)]'s+id-M(q) v-oe+rMga+a+ (35) a([) dt a-U(qta-d 对上式求导，应用特性2并结合式(13)、式 (32)~式(34)，可得： [0,rpe+[6,0jT (41) V-6(+M()= 3仿真模拟分析 -kels s-ke2z1TzI-8 i-zITid+ii+idid (36) 为验证所提控制策略的正确性和有效性，采 用图1所示空间机械臂在轨插、拔孔操作模型进 根据Young's不等式可得： 行仿真模拟.在仿真模拟中，可将孔内摩擦阻力及 /-2eTi≤sTg+i (37) 力矩等效为替换部件末端接触力及力矩，当替换 -2zTra≤z1Tz1+td 部件末端输出力及力矩大于孔内摩擦阻力及力矩 同时，根据定理1、定理2的证明过程可得： 时，即可进行插、拔孔操作.同时，由于控制精度 (ii=-kakau i 的问题，若直接以孔内摩擦力及力矩作为替换部 \iaid=-blstaTd (38) 件末端接触力及力矩带入阻抗力学模型中，得到 结合式(36)~式(38)可得： 的替换部件末端输出力及力矩可能小于孔内摩擦 V≤-ke1s「8-kc2z1Tz1+ 力及力矩，导致插、拔孔操作无法进行.由此，考 ee+aa+a'a+话利- 虑引入期望输出力及力矩，并选取期望输出力及 力矩的数值略大于孔内摩擦力及力矩. kak3ii i-1l3idids-(2kel -1) 为保证插孔操作过程的精确控制，将插孔操 e-(2kky-1) 2 2 -(225-10 作分为两个阶段：第一阶段，准备阶段，关闭力位 td_2k2-)zM(gz≤-av 姿阻抗控制，替换部件末端到达孔的正上方；第二 2 Mm 2 (39) 阶段，实施阶段，开启力位姿阻抗控制，替换部件 式中，a=mim2ke1-1,2ke-》,2kk3-1,226-1. 末端沿期望轨迹克服孔内摩擦阻力及力矩完成插 Mm 孔操作.选取空间机械臂在轨插、拔孔操作模型 根据定理3，a>0.结合式(39)、特性1及V的定义 参数如下：m0=50kg,m1=4kg,m2=4kg,m3=2kg, 知：V()≤V(O)em,ls≤V2Voe-,lzll≤ mp=5kg;s0=1.5m,7=2m,s2=2m,s3=0.4m, V2VO)/MMe-",lal≤V2V而e-a, lall≤ sp=0.2m,d1=1m,d=1m,d=0.2m;lo= V2V(而e-at 35kgm2,1=2kgm2,2=2kgm2,3=1kgm2, 由此，1、元、满足全局指数型稳定，轨迹跟 p=5kgm2.选取系统的有界扰动ra=[cos(0,sin(), 踪误差ε满足全局指数型收敛，证毕 cos(t),sin(r)] 2.2阻抗控制外环设计 仿真模拟包括三组：摩擦阻力及力矩不突变 当开启阻抗控制时，结合阻抗控制原理，并考 下侧面插孔操作：摩擦阻力及力矩突变下侧面插 虑系统运动学不确定、动力学不确定及扰动，则载 孔操作：摩擦阻力及力矩不突变下正面拔孔操作

起 来 后 所 得 第 一 行 的 右 端 满 足 恒 为 零 ， 而 式 (33) 乘起来后的其余各行为空间机械臂的实际控 制输入. 2k2k3 −1 > 0 2l2l3 −1 > 0 2kc1 −1 > 0 2kc2 −1 > 0 ε 定理 3 若式 (33) 满足 ， ， ， ，则轨迹跟踪误差 满足全局 指数型收敛. 证 明 根 据 ε 及 α1 的 定 义 ， 结 合 式 (15) 及 式 (32)，可得： ε˙ = Y˙ d −Y˙ = Y˙ d − J(q) { z1 + [ J(q) ]+Y˙ d + α1 } −u = −kc1ε− J(q)z1 −u˜ （34） 定义 Lyapunov 函数： V = 1 2 ε T ε+ 1 2 z1 TM(q)z1 + 1 2 u˜ Tu˜ + 1 2 τ˜ T d τ˜d （35） 对上式求导 ，应用特性 2 并结合式 (13)、式 (32)～式 (34)，可得： V˙ =ε T ε˙ + 1 2 z1 TM˙ (q)z1 + z1 TM(q)z1 +u˜ T ˙u˜ +τ˜ T d ˙τ˜d = −kc1ε T ε−kc2 z1 T z1 −ε Tu˜ − z1 T τ˜d +u˜ T ˙u˜ +τ˜ T d ˙τ˜d （36） 根据 Young’s 不等式可得： { −2ε Tu˜ ⩽ ε T ε+u˜ Tu˜ −2z1 T τ˜d ⩽ z1 T z1 +τ˜ T d τ˜d （37） 同时，根据定理 1、定理 2 的证明过程可得：    u˜ T ˙u˜ = −k2k3u˜ Tu˜ τ˜ T d ˙τ˜d = −l2l3τ˜ T d τ˜d （38） 结合式 (36)～式 (38) 可得： V˙ ⩽ −kc1ε T ε−kc2 z1 T z1+ 1 2 ( ε T ε+u˜ Tu˜ + z1 T z1 +τ˜ T d τ˜d ) − k2k3u˜ T ˙u˜ −l2l3τ˜ T d τ˜d ⩽ −(2kc1 −1) ε Tε 2 −(2k2k3 −1) u˜ Tu˜ 2 −(2l2l3 −1) τ˜ T d τ˜d 2 − (2kc2 −1) Mm z1 TM(q)z1 2 ⩽ −aV （39） a = min{ 2kc1 −1, (2kc2 −1) Mm ,2k2k3 −1,2l2l3 −1 } a > 0 V V (t) ⩽ V (0) e − 1 2 at ∥ε∥ ⩽ √ 2V (0)e − 1 2 at ∥z1∥ ⩽ √ 2V (0) /MMe − 1 2 at ∥u˜∥ ⩽ √ 2V (0)e − 1 2 at ∥τ˜d∥ ⩽ √ 2V (0)e − 1 2 at 式 中 ， ， 根据定理 3， . 结合式 (39)、特性 1 及 的定义 知 ： ， ， ， ， . z1 u˜ τ˜d ε 由此， 、 、 满足全局指数型稳定，轨迹跟 踪误差 满足全局指数型收敛，证毕. 2.2    阻抗控制外环设计 当开启阻抗控制时，结合阻抗控制原理，并考 虑系统运动学不确定、动力学不确定及扰动，则载 体位置、姿态均不受控空间机械臂系统动力学模 型可写为： M(q) q¨ +C(q, q˙) q˙ +τd = [ 0,τc T ]T + [ 0,τPe T ]T （40） FP Fe 联立式 (10)、式 (11) 可构成控制策略中的阻 抗控制外环部分，将阻抗关系式与指数型跟踪控 制相结合，并根据替换部件末端输出力及力矩 与替换部件末端接触力及力矩 的误差，在线 修正替换部件末端位姿，获得对替换部件末端输 出力及力矩的跟踪. 由此，对应的控制力矩可变为： [ 0,τc T ]T = −kc2 z1+ [ J(q) ]T ε + τˆd − M(q)    − d ([ J(q) ]+ ) dt Y˙ d − [ J(q) ]+Y¨ d −α˙ 1    − [ 0,τPe T ]T +[δ,0] T （41） 3    仿真模拟分析 为验证所提控制策略的正确性和有效性，采 用图 1 所示空间机械臂在轨插、拔孔操作模型进 行仿真模拟. 在仿真模拟中，可将孔内摩擦阻力及 力矩等效为替换部件末端接触力及力矩，当替换 部件末端输出力及力矩大于孔内摩擦阻力及力矩 时，即可进行插、拔孔操作. 同时，由于控制精度 的问题，若直接以孔内摩擦力及力矩作为替换部 件末端接触力及力矩带入阻抗力学模型中，得到 的替换部件末端输出力及力矩可能小于孔内摩擦 力及力矩，导致插、拔孔操作无法进行. 由此，考 虑引入期望输出力及力矩，并选取期望输出力及 力矩的数值略大于孔内摩擦力及力矩. m0 = 50 kg m1 = 4 kg m2 = 4 kg m3 = 2 kg mP = 5 kg ς0 = 1.5 m ς1 = 2 m ς2 = 2 m ς3 = 0.4 m ςP = 0.2 m d1 = 1 m d2 = 1 m d3 = 0.2 m I0 = 35 kg·m2 I1 = 2 kg·m2 I2 = 2 kg·m2 I3 = 1 kg·m2 IP = τd = [cos(t),sin(t), cos(t),sin(t)] T 为保证插孔操作过程的精确控制，将插孔操 作分为两个阶段：第一阶段，准备阶段，关闭力/位 姿阻抗控制，替换部件末端到达孔的正上方；第二 阶段，实施阶段，开启力/位姿阻抗控制，替换部件 末端沿期望轨迹克服孔内摩擦阻力及力矩完成插 孔操作. 选取空间机械臂在轨插、拔孔操作模型 参数如下： ， ， ， ， ； ， ， ， ， ， ， ， ； ， ， ， ， 5 kg·m2 . 选取系统的有界扰动 . 仿真模拟包括三组：摩擦阻力及力矩不突变 下侧面插孔操作；摩擦阻力及力矩突变下侧面插 孔操作；摩擦阻力及力矩不突变下正面拔孔操作. 曾晨东等： 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 · 259 ·

260 工程科学学报，第44卷，第2期 同时，为了更好的说明本文所提指数型跟踪控制 0.4 0.2 --Desire trajectory (Exponential tracking control,ETC)的有效性，将滑 trajectory 模控制(Sliding mode control,.SMC)Pa应用于上述 0 SMC trajectory Start x4.55 0.2 position 空间机械臂在轨插、拔孔操作，并进行对比和分析. Porthole -0.4 position 31摩擦阻力及力矩不突变下侧面插孔操作 -0.6 x3.601 选取空间机械臂系统控制策略参数如下： -0.8 p:-0.9993 3.8 Position Kp=diag(340,340,340),Bp=diag(200,200,200),Mp= -1.0 xp:3.5■一■、 :-0.9997 above hole diag(1,1,1),1=9,l2=10,3=10,k1=0.01,k2=25, -1.2 :-1 Complete hole insertion k3=10,kc1=5,k2=5;假设在基联坐标系内，侧面 34 3.6 3.84.04.2 4.44.6 孔口坐标3.6m,-1m,侧面孔的深度h1=0.1m, x /m 侧面孔轴线到载体底部的距离s1=1m,插孔深度 图3替换部件末端插孔轨迹 用h表示；为简化插孔操作，假设插孔过程仅存在 Fig.3 Trajectory of the end of the replacement parts 沿插孔方向的摩擦阻力，摩擦力矩为0Nm,即 0.4 Start hole Fn =[18 N,0 N,ON.m]T insertion Complete t36.92 0.2 t16.9 hole insertion h:0.1 假设替换部件末端初始位置及姿态为： k:1.409e-06 0 r39.47 X=[4.55m,0m,-171.9]T t17.86 (42) h:0.1 0.2 h:7.661e-07 Complete 仿真总时间为36.92s,在0~10s,关闭阻抗控 Start hole hole insertion -0.4 制，调整替换部件末端位置和姿态，使其从初始位 insertion 置到达孔的上方.在10~36.92s,开启阻抗控制， -0.6 调整替换部件末端位置和姿态，对准孔口，使其沿 -0.8 -ETC depth --SMC depth 期望轨迹克服孔内摩擦阻力完成插孔操作.根据 -1.0 0 10 152025303540 上述分段控制方案，仿真全程内替换部件末端的 t/s 期望位姿如下： 图4替换部件末端插孔深度 X=[3.8m,-1m,-114.6](0<t≤10) Fig.4 Depth of the end of the replacement parts X3=[3.7-0.005(t-10)m,-1m,0](10<t≤36.92) 150 ---Desire output force (43) 100 一ETC output force 期望输出力如下： r12.28 t28 --SMC output force 50 F:18 Fp18.86 Fpd =[0 N,0 N,O N.m](0<t<10) (44) 0 c15.88 r30 Fa=[20N,0N,0NmT(10<t≤36.92) F18 F18.53 -50 仿真结果如图3~图6所示.图3为替换部件 -100 末端插孔轨迹，可知进入孔口前一时刻，孔轴线方 向的轨迹误差约为0.7mm,进人孔口后，由于孔的 -150 位置约束，误差为0mm.图4为替换部件末端插 -20 10 15 20 25303540 孔深度，其以孔口为参照，孔口外为负，孔口内为 正；另外，可知指数型跟踪控制相较于滑模控制， 图5替换部件末端输出力 其到达孔口的速度更快，插孔所用的时间更少 Fig.5 Output force of the end of the replacement parts 图5为替换部件末端输出力，可知在插孔操作的 diag(1,1,1),l1=9,2=10,3=10,k1=0.01,k2=25 整个过程，插孔方向的输出力大于摩擦阻力；另外， k3=10,k1=5,kc2=5;假设在基联坐标系内，侧面 可知指数型跟踪控制的力误差约为1.1N,小于滑 孔口坐标[3.6m,-1m,侧面孔的深度h1=0.1m, 模控制的1.5N.图6为替换部件末端姿态角变化 侧面孔轴线到载体底部的距离51=1m,插孔深度 情况，可知两种控制方法的跟踪误差都小于0.5° 用h表示：为简化插孔操作，假设插孔过程仅存在 3.2摩擦阻力及力矩突变下侧面插孔操作 沿插孔方向的摩擦阻力，摩擦力矩为0Nm,即 选取空间机械臂系统控制策略参数如下： Fa=[18N,0N,0Nm';考虑实际插孔过程摩擦阻 Kp=diag(340,340,340),Bp=diag(200,200,200),Mp= 力可能发生突变，即存在卡阻现象，可假设插孔深

同时，为了更好的说明本文所提指数型跟踪控制 (Exponential tracking control, ETC) 的有效性，将滑 模控制 (Sliding mode control, SMC)[26] 应用于上述 空间机械臂在轨插、拔孔操作，并进行对比和分析. 3.1    摩擦阻力及力矩不突变下侧面插孔操作 KP = diag(340,340,340) BP = diag(200,200,200) MP = diag(1,1,1) l1 = 9 l2 = 10 l3 = 10 k1 = 0.01 k2 = 25 k3 = 10 kc1 = 5 kc2 = 5 [3.6 m,−1 m] T h1 = 0.1 m s1 = 1 m h N·m Ff1 = [18 N,0 N,0 N·m] T 选取空间机械臂系统控制策略参数如下 ： ， ， ， ， ， ， ， ， ， ， ；假设在基联坐标系内，侧面 孔口坐标 ，侧面孔的深度 ， 侧面孔轴线到载体底部的距离 ，插孔深度 用 表示；为简化插孔操作，假设插孔过程仅存在 沿插孔方向的摩擦阻力，摩擦力矩为 0 ，即 . 假设替换部件末端初始位置及姿态为： X = [ 4.55 m,0 m,−171.9◦ ]T （42） 仿真总时间为 36.92 s，在 0～10 s，关闭阻抗控 制，调整替换部件末端位置和姿态，使其从初始位 置到达孔的上方. 在 10～36.92 s，开启阻抗控制， 调整替换部件末端位置和姿态，对准孔口，使其沿 期望轨迹克服孔内摩擦阻力完成插孔操作. 根据 上述分段控制方案，仿真全程内替换部件末端的 期望位姿如下：    X 1 d = [ 3.8 m,−1 m,−114.6 ◦ ]T (0 < t ⩽ 10) X 2 d = [ 3.7−0.005(t−10) m,−1 m,0 ◦ ]T (10 < t ⩽ 36.92) （43） 期望输出力如下：    F 1 Pd = [0 N,0 N,0 N·m] T (0 < t ⩽ 10) F 2 Pd = [20 N,0N,0 N·m] T (10 < t ⩽ 36.92) （44） 仿真结果如图 3～图 6 所示. 图 3 为替换部件 末端插孔轨迹，可知进入孔口前一时刻，孔轴线方 向的轨迹误差约为 0.7 mm，进入孔口后，由于孔的 位置约束，误差为 0 mm. 图 4 为替换部件末端插 孔深度，其以孔口为参照，孔口外为负，孔口内为 正；另外，可知指数型跟踪控制相较于滑模控制， 其到达孔口的速度更快，插孔所用的时间更少. 图 5 为替换部件末端输出力，可知在插孔操作的 整个过程，插孔方向的输出力大于摩擦阻力；另外， 可知指数型跟踪控制的力误差约为 1.1 N，小于滑 模控制的 1.5 N. 图 6 为替换部件末端姿态角变化 情况，可知两种控制方法的跟踪误差都小于 0.5°. 3.2    摩擦阻力及力矩突变下侧面插孔操作 KP = diag(340,340,340) BP = diag(200,200,200) MP = 选取空间机械臂系统控制策略参数如下 ： ， ， diag(1,1,1) l1 = 9 l2 = 10 l3 = 10 k1 = 0.01 k2 = 25 k3 = 10 kc1 = 5 kc2 = 5 [3.6 m,−1 m] T h1 = 0.1 m s1 = 1 m h N·m Ff1 = [18 N,0 N,0 N·m] T ， ， ， ， ， ， ， ， ；假设在基联坐标系内，侧面 孔口坐标 ，侧面孔的深度 ， 侧面孔轴线到载体底部的距离 ，插孔深度 用 表示；为简化插孔操作，假设插孔过程仅存在 沿插孔方向的摩擦阻力，摩擦力矩为 0 ，即 ；考虑实际插孔过程摩擦阻 力可能发生突变，即存在卡阻现象，可假设插孔深 3.2 3.4 3.6 3.8 4.0 4.2 4.4 4.6 −1.4 −1.2 −1.0 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 Desire trajectory ETC trajectory SMC trajectory Porthole position Position above hole Complete hole insertion y ′ P/m x ′ p : 3.601 y ′ p : −0.9993 x ′ p : 3.5 y ′ p : −1 y ′ p : −0.9997 x ′ p : 3.8 Start position x ′ p : 4.55 y ′ p : 0 x ′ P /m 图 3    替换部件末端插孔轨迹 Fig.3    Trajectory of the end of the replacement parts 0 5 10 15 20 25 30 35 40 −1.0 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 h/m ETC depth SMC depth Complete hole insertion Start hole insertion t: 17.86 h: 7.661e−07 t: 36.92 h: 0.1 t: 39.47 h: 0.1 Complete hole insertion Start hole insertion h: 1.409e−06 t: 16.9 t/s 图 4    替换部件末端插孔深度 Fig.4    Depth of the end of the replacement parts 10 15 20 25 30 35 40 −200 −150 −100 −50 0 50 100 150 FP/N Desire output force ETC output force SMC output force t: 12.28 t: 15.88 t: 28 t: 30 Fp : 18 Fp : 18 Fp : 18.86 Fp : 18.53 t/s 图 5    替换部件末端输出力 Fig.5    Output force of the end of the replacement parts · 260 · 工程科学学报，第 44 卷，第 2 期

曾晨东等：空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 261· 40 r22.15 度更快，插孔所用的时间更少.图9为替换部件 -0.0519 末端输出力，在插孔操作的整个过程，插孔方向的 上24.58 6。-0.06719 输出力大于摩擦阻力，且在摩擦阻力大小突变为 40 30N的情况下，输出力线性增加至大于30N.另 -80 t 8 外，可知指数型跟踪控制的力误差约为1.2N,小于 6。-114.6 -120 t9.026 滑模控制的1.5N.图10为替换部件末端姿态角 0。:-114.7 变化情况，可知两种控制方法的跟踪误差都小 160 ---Desire attitude angle d 二SMC attitude angle 于0.5° -200 10 152025303540 0.4 tis 0.2 图6替换部件末端姿态角 0 SMC trajectory Start Fig.6 Attitude angle of the end of the replacement parts x4.55 -0.2 position 0 度为0.06m时，摩擦阻力突变为30N,此时 -0.4 Porthole -0.6 position Fn [30 N,0 N,O N.m] -0.8 xp:3.601 假设替换部件末端初始位置及姿态为： g0.9993 xe3.8 Position -1.0 35 %0.9997 above hole X=[4.55m,0m,-171.9]T (45) -1.2 Complete hole insertion 仿真总时间为36.96s,在0~10s,关闭阻抗控 3.2 3.4 3.6 3.84.0 4.24.44.6 制，调整替换部件末端位置和姿态，使其从初始位 x /m 置到达孔的上方.在10~36.96S,开启阻抗控制， 图7替换部件末端插孔轨迹 调整替换部件末端位置和姿态.对准孔口，使其沿 Fig.7 Trajectory of the end of the replacement parts 期望轨迹克服孔内摩擦阻力完成插孔操作.特别 0.4 Continue hole insertion 的，在插孔深度为0.06m,即28.9s时，摩擦阻力发 0.2 Stop hole t:28.9 生突变，此时插孔操作停止，调整期望输出力，实 insertion h:0.06 量■ 际输出力线性增加，当其大于摩擦阻力，插孔操作 乐067F3621f96 Stop hole h:0.06 h:0.06 继续，同时期望输出力调整为设定值.根据上述分 0.2 Complete insertion Continue hole insertion hole insertion 段控制方案，仿真全程内替换部件末端的期望位 -0.4 姿如下： -0.6 X=[3.8m,-1m,-114.6](0<t≤10) 0.8 -ETC depth ---SMC depth X3=[3.7-0.005t-10)m,-1m,0](10<t≤36.96) -10 5 1015 20253035404550 (46) tis 期望输出力如下： 图8替换部件末端插孔深度 Fa=[0N,0N,0N,m]T(0<t≤10) Fig.8 Depth of the end of the replacement parts Fa=20N0N,0NmT(10<1≤28.9) 150 F=2.5t-19)N,0N,0NmF(28.9<t≤32.46) 100 Ff=[20N,0N,0N.m(32.46<t≤39.96) r12.28 r32.46 E36.21 02 (47) 50 F:18 0中tr3881 Fp:3 ■■ 仿真结果如图7~图10所示.图7为替换部件 0 t15.88 34.9 F18 F。18.78 F18.5 末端插孔轨迹，可知进入孔口前一时刻，孔轴线方 -50 向的轨迹误差约为0.7mm,进入孔口后，由于孔的 -100 位置约束，误差为0mm.图8为替换部件末端插 -150 ---Desire output force -ETC output force 孔深度，其以孔口为参照，孔口外为负，孔口内为 ---SMC output force 正，可知在28.9~32.46s,由于摩擦阻力发生突变， 20010 15 20 253035. 404550 tis 插孔操作停止，插孔深度保持不变；另外，可知指 图9替换部件末端输出力 数型跟踪控制相较于滑模控制，其到达孔口的速 Fig.9 Output force of the end of the replacement parts

Ff1 = [30 N,0 N,0 N·m] T 度 为 0.06  m 时 ， 摩 擦 阻 力 突 变 为 30  N， 此 时 . 假设替换部件末端初始位置及姿态为： X = [ 4.55 m,0 m,−171.9◦ ]T （45） 仿真总时间为 36.96 s，在 0～10 s，关闭阻抗控 制，调整替换部件末端位置和姿态，使其从初始位 置到达孔的上方. 在 10～36.96 s，开启阻抗控制， 调整替换部件末端位置和姿态，对准孔口，使其沿 期望轨迹克服孔内摩擦阻力完成插孔操作. 特别 的，在插孔深度为 0.06 m，即 28.9 s 时，摩擦阻力发 生突变，此时插孔操作停止，调整期望输出力，实 际输出力线性增加，当其大于摩擦阻力，插孔操作 继续，同时期望输出力调整为设定值. 根据上述分 段控制方案，仿真全程内替换部件末端的期望位 姿如下：    X 1 d = [ 3.8 m,−1 m,−114.6 ◦ ]T (0 < t ⩽ 10) X 2 d = [ 3.7−0.005(t−10) m,−1 m,0 ◦ ]T (10 < t ⩽ 36.96) （46） 期望输出力如下：    F 1 Pd = [0 N,0 N,0 N·m] T (0 < t ⩽ 10) F 2 Pd = [20 N,0 N,0 N·m] T (10 < t ⩽ 28.9) F 3 Pd = [2.5(t−19) N,0 N,0 N·m] T (28.9 < t ⩽ 32.46) F 4 Pd = [20 N,0 N,0 N·m] T (32.46 < t ⩽ 39.96) （47） 仿真结果如图 7～图 10 所示. 图 7 为替换部件 末端插孔轨迹，可知进入孔口前一时刻，孔轴线方 向的轨迹误差约为 0.7 mm，进入孔口后，由于孔的 位置约束，误差为 0 mm. 图 8 为替换部件末端插 孔深度，其以孔口为参照，孔口外为负，孔口内为 正，可知在 28.9～32.46 s，由于摩擦阻力发生突变， 插孔操作停止，插孔深度保持不变；另外，可知指 数型跟踪控制相较于滑模控制，其到达孔口的速 度更快，插孔所用的时间更少. 图 9 为替换部件 末端输出力，在插孔操作的整个过程，插孔方向的 输出力大于摩擦阻力，且在摩擦阻力大小突变为 30 N 的情况下，输出力线性增加至大于 30 N. 另 外，可知指数型跟踪控制的力误差约为 1.2 N，小于 滑模控制的 1.5 N. 图 10 为替换部件末端姿态角 变化情况，可知两种控制方法的跟踪误差都小 于 0.5°. 3.2 3.4 3.6 3.8 4.0 4.2 4.4 4.6 −1.4 −1.2 −1.0 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 Desire trajectory ETC trajectory SMC trajectory Start position Porthole position Position above hole Complete hole insertion y ′ P/m x ′ P /m x ′ p : 4.55 y ′ p : 0 x ′ p : 3.8 y ′ p : −0.9997 x ′ p : 3.601 y ′ p : −0.9993 x ′ p : 3.5 y ′ p : −1 图 7    替换部件末端插孔轨迹 Fig.7    Trajectory of the end of the replacement parts 0 5 10 15 20 25 30 35 40 45 50 t/s −1.0 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 h/m ETC depth SMC depth Stop hole insertion Stop hole insertion t: 28.9 h: 0.06 t: 30.67 h: 0.06 t: 36.21 h: 0.06 Continue hole insertion Complete hole insertion t: 36.96 h: 0.1 t: 43.6 h: 0.1 t: 32.46 h: 0.06 Continue hole insertion 图 8    替换部件末端插孔深度 Fig.8    Depth of the end of the replacement parts 10 15 20 25 30 35 40 45 50 −200 −150 −100 −50 0 50 100 150 Desire output force ETC output force SMC output force t/s t: 12.28 Fp : 18 t: 15.88 Fp : 18 t: 32.46 Fp : 30 t: 36.21 Fp : 30 t: 38.81 Fp : 18.5 t: 34.9 Fp : 18.78 FP/N 图 9    替换部件末端输出力 Fig.9    Output force of the end of the replacement parts −200 −160 −120 −80 −40 0 40 Desire attitude angle ETC attitude angle SMC attitude angle t: 8 t: 24.58 t: 22.15 t: 9.026 θ′P/(°) θ ′ p : −114.6 θ ′ p : −114.7 θ ′ p : −0.0519 θ ′ p : −0.06719 0 15 20 25 30 35 40 5 10 t/s 图 6    替换部件末端姿态角 Fig.6    Attitude angle of the end of the replacement parts 曾晨东等： 空间机械臂在轨插、拔孔操作基于力/位姿跟踪指数型阻抗控制 · 261 ·

262 工程科学学报，第44卷，第2期 0 -0.04 t22.15 6。:-0.0519 2 0.06 0.05 t24.58 Complete hole 8。-0.06718 -0.08 extraction -40 0.10 ---Desire trajectory -80t8.375 9。-114.6 二5 -0.12 Strat hole -120 t9.026 8。-114.7 -0.14 extration v:2 -160 ---Desire attitude angle -ETC attitude angle 0.16 e0.15 ---SMC attitude angle -200 0.1 0 5 101520253035404550 1.0 1.5 2.0 2.5 3.0 tis xj/m 图10替换部件末端姿态角 图11替换部件末端拔孔轨迹 Fig.10 Attitude angle of the end of the replacement parts Fig.11 Trajectory of the end of the replacement parts 3.3摩擦阻力及力矩不突变下正面拔孔操作 0.14 选取空间机械臂系统控制策略参数如下： 0.12 -ETC depth Complete ---SMC depth Kp=diag(400,400,400),Bp=diag(200,200,200),Mp= 后a6 hole extraction 0.10 diag(1,1,1),1h=9,2=10,l3=10,k1=0.01,k2=25, 0.08 品2 k3=10,ke1=5,k2=5假设在基联坐标系内，正面 是0.06 Complete 孔口坐标[2m,-0.05m,孔底坐标2m,-0.15m, 0.04 Strat hole hole extraction 正面孔的深度2=0.1m,正面孔轴线到载体右侧的 extration 0.02 11.42 距离s2=1.5m,拔孔深度用h表示；为简化拔孔操 左2 h:0 Strat hole 0 extration 作，假设拔孔过程仅考虑沿拔孔方向的摩擦阻力， 0.0 0 10 15 20 25 30 摩擦力矩为0Nm,即F2=[0N,-18N,0Nm. 替换部件末端初始位置及姿态为： 因12替换部件末端拔孔深度 X=[2m,-0.15m,-90]I (48) Fig.12 Depth of the end of the replacement parts 仿真总时间为21.46s,在0~11.12s,开启阻抗 不变；另外，可知指数型跟踪控制相较于滑模控 控制，使替换部件末端输出力线性增加至大于孔 制，其拔孔所用的时间更少.图13为替换部件末 内摩擦阻力.在1112~21.46s,期望输出力调整为 端输出力，可知在拔孔操作的整个过程，输出力大 设定值，替换部件末端沿期望轨迹克服孔内摩擦 于摩擦阻力：另外，可知指数型跟踪控制的力误差 阻力完成拔孔操作.根据上述分析，仿真全程内替 约为0.7N,小于滑模控制的1.5N.图14为替换部 换部件末端的期望位姿如下： 件末端姿态角变化情况，可知由于孔的位置约束， X=2.0m,-0.05m,-90](0<t<11.12) 其姿态角跟踪误差为0° X3=[2.0m,-0.15+0.005t-11.12)m,-90] (11.12<t<21.46) 40 (49) 30 ---Desire output force 期望输出力如下： 20 二SMC oupute Fa=0N,-1.8N,0NmT(0<t≤11.12) 10 Fa=[0N,-20N,0NmF(11.12<t≤21.46 至-0 t11.42 t18.64 50) Fe-18 Fm-18.48 -20 仿真结果如图11~图14所示.图11为替换 t17.25 -30 p-19.34 部件末端拔孔轨迹，可知由于孔的位置约束，孔轴 -40 线方向的轨迹误差为0mm.图12为替换部件末 -50 10 15 20 25 30 端拔孔深度，深度以孔底为参照，向孔外为正，向 s 孔内为负，可知在0~11.12s,由于拔孔方向的输 图13替换部件末端输出力 出力小于摩擦阻力，拔孔操作停止，拔孔深度保持 Fig.13 Output force of the end of the replacement parts

3.3    摩擦阻力及力矩不突变下正面拔孔操作 KP = diag(400,400,400) BP = diag(200,200,200) MP = diag(1,1,1) l1 = 9 l2 = 10 l3 = 10 k1 = 0.01 k2 = 25 k3 = 10 kc1 = 5 kc2 = 5 [2 m,−0.05 m] T [2 m,−0.15 m] T h2 = 0.1 m s2 = 1.5 m h N·m Ff2 = [0 N,−18 N,0 N·m] T 选取空间机械臂系统控制策略参数如下 ： ， ， ， ， ， ， ， ， ， ， ；假设在基联坐标系内，正面 孔口坐标 ，孔底坐标 ， 正面孔的深度 ，正面孔轴线到载体右侧的 距离 ，拔孔深度用 表示；为简化拔孔操 作，假设拔孔过程仅考虑沿拔孔方向的摩擦阻力， 摩擦力矩为 0 ，即 . 替换部件末端初始位置及姿态为： X = [ 2 m,−0.15 m,−90◦ ]T （48） 仿真总时间为 21.46 s，在 0～11.12 s，开启阻抗 控制，使替换部件末端输出力线性增加至大于孔 内摩擦阻力. 在 11.12～21.46 s，期望输出力调整为 设定值，替换部件末端沿期望轨迹克服孔内摩擦 阻力完成拔孔操作. 根据上述分析，仿真全程内替 换部件末端的期望位姿如下：    X 1 d = [ 2.0 m,−0.05 m,−90◦ ]T (0 < t < 11.12) X 2 d = [ 2.0 m,−0.15+0.005(t−11.12) m,−90◦ ]T (11.12 < t < 21.46) （49） 期望输出力如下：    F 1 Pd = [0 N,−1.8t N,0 N·m] T (0 < t ⩽ 11.12) F 2 Pd = [0 N,−20 N,0 N·m] T (11.12 < t ⩽ 21.46) （50） 仿真结果如图 11～图 14 所示. 图 11 为替换 部件末端拔孔轨迹，可知由于孔的位置约束，孔轴 线方向的轨迹误差为 0 mm. 图 12 为替换部件末 端拔孔深度，深度以孔底为参照，向孔外为正，向 孔内为负，可知在 0～11.12 s，由于拔孔方向的输 出力小于摩擦阻力，拔孔操作停止，拔孔深度保持 不变；另外，可知指数型跟踪控制相较于滑模控 制，其拔孔所用的时间更少. 图 13 为替换部件末 端输出力，可知在拔孔操作的整个过程，输出力大 于摩擦阻力；另外，可知指数型跟踪控制的力误差 约为 0.7 N，小于滑模控制的 1.5 N. 图 14 为替换部 件末端姿态角变化情况，可知由于孔的位置约束， 其姿态角跟踪误差为 0°. 0 5 10 15 20 25 30 35 40 45 50 −200 −160 −120 −80 −40 0 40 Desire attitude angle ETC attitude angle SMC attitude angle t: 22.15 θ ′ p : −0.0519 t: 24.58 θ ′ p : −0.06718 t: 8.375 θ ′ p : −114.6 t: 9.026 θ ′ p : −114.7 θ′P/(°) t/s 图 10    替换部件末端姿态角 Fig.10    Attitude angle of the end of the replacement parts 1.0 1.5 2.0 2.5 3.0 −0.18 −0.16 −0.14 −0.12 −0.10 −0.08 −0.06 −0.04 y ′ P/m x ′ P /m Desire trajectory ETC trajectory SMC trajectory x ′ p : 2 y ′ p : −0.05 x ′ p : 2 y ′ p : −0.15 Complete hole extraction Strat hole extration 图 11    替换部件末端拔孔轨迹 Fig.11    Trajectory of the end of the replacement parts 0 5 10 15 20 25 30 t/s −0.02 0 0.02 0.04 0.06 0.08 0.10 0.12 0.14 h/m ETC depth SMC depth Complete hole extraction Complete hole extraction Strat hole extration Strat hole extration t: 21.46 h: 0.1 t: 22.32 h: 0.1 t: 11.42 t: 11.12 h: 0 h: 0 图 12    替换部件末端拔孔深度 Fig.12    Depth of the end of the replacement parts 0 5 10 15 20 25 30 t/s −50 −40 −30 −20 −10 0 10 20 30 40 FP/N Desire output force ETC output force SMC output force t: 11.42 Fp : −18 t: 18.64 Fp : −18.48 t: 17.25 Fp : −19.34 t: 11.12 Fp : −18 图 13    替换部件末端输出力 Fig.13    Output force of the end of the replacement parts · 262 · 工程科学学报，第 44 卷，第 2 期