基于移动机器人的运动学半物理系统的规划方法

2022-01-20宋立滨张继文

宇航学报 2021年11期

刘琛,刘宇,宋立滨,张继文

(1.清华大学机械工程系,北京 100084；2.北京跟踪与通信技术研究所,北京 100094)

0 引言

目前,空间技术已经取得了较大发展,但在轨服务、空间对接等多飞行器之间的复杂任务仍然是一个非常大的挑战。由于空间环境的特殊性以及空间飞行器高昂的成本,在进行相关技术的研究和验证时,往往需要先在地面进行充分的实验[1]。利用半物理(Hardware-in-the-loop,HIL)系统进行空间技术的验证是一种较为常见的方式[2-3],如文献[4]中对航天器逼近阶段的导航研究和半物理仿真验证。半物理仿真系统可以结合实际硬件测试和计算机仿真,在地面搭建代仿真空间对象的等效硬件系统,并且实现完整的控制闭环,模拟真实情况下的地面控制和空间操作过程,对研究空间对接、在轨服务等卫星间的相关技术有非常重要的作用[5]。

目前国内外在半物理系统的研究和搭建方面均有一些成果。德国宇航中心开发了一套EPOS(European proximit operations simulator)系统[6-7],该系统基于两台机械臂和一个直线导轨搭建而成,用于研究在轨条件下的卫星之间的交会对接任务。加拿大航天局为了研究交会对接中的接触动力学过程,搭建了主要由两台固定基座的机械臂组成的STVF(SPDM task verification facility)系统[8-9],此外,还对利用空间操作臂抓捕自由翻滚的卫星的全过程进行了仿真[10]。文献[11]中同样通过基于两台机械臂的半物理系统对抓捕受损卫星的过程进行研究。在国内方面,哈工大通过力补偿方式构建了用于模拟捕获目标卫星的微重力地面验证系统[12],并利用该系统对空间操作臂抓捕自由漂浮卫星进行研究。此外,空间技术研究院[13]、中国科学院[14]等单位也对半物理系统进行了相关的研究,但从结构上来看,均是以两台固定基座的机械臂为基础进行搭建。

为了满足对空间飞行器之间的大范围相对运动进行运动学仿真的需求,在保留现有半物理系统操作能力的同时扩大半物理系统的工作空间十分必要。目前这种被广泛采用的基于两台固定基座的机械臂的结构,保证了飞行器之间的接触动力学仿真的可靠性,但同时严重限制了半物理系统工作空间的大小,在仿真较大范围的相对运动时存在一定不足。部分半物理系统通过将机械臂安装在直线导轨上的方式扩展工作空间,但由于直线导轨自身特点,在继续扩展其他方向平移自由度时面临结构复杂、成本过高等一系列问题。

文献[15]对半物理仿真系统的基本问题和解决方案进行了分析和总结,对半物理系统的设计和调试过程提供了指导意义。在此基础上,结合移动机器人在平面运动的灵活性,如图1所示的基于移动机器人的半物理系统的工作空间可以被极大扩展,且平面内的运动具有较高灵活性,避免了直线导轨的复杂结构设计,同时,移动机器人的引入增加了半物理系统的平移关节,增加了系统的冗余性,可以满足复杂的大范围的相对运动轨迹的需求,在仿真较大范围内的空间飞行器之间的相对运动时具有较大优势。

图1 基于移动机器人的半物理系统整体框架Fig.1 The framework of the mobile-robots-based hardware-in-the-loop simulation system

但是,基于移动机器人的半物理系统也存在明显的缺点：移动机器人的运动精度明显低于机械臂等执行机构的精度,与直线导轨相比也存在一定差距,将移动机器人引入半物理系统后将面临整体系统精度下降的问题。

针对上述问题,本文采取通过外部测量设备实时测量和机械臂运动补偿的方式,在离线规划过程中,借鉴了超冗余机械臂的规划方法,提前为机械臂留出实时运动补偿的余量,并且保证其末端的灵活运动能力,从而完成整个半物理系统的实时运动补偿控制。

1 问题描述

基于移动机器人的半物理系统可以将移动机器人与机械臂等执行机构相结合,通过两个机械臂末端的相对运动模拟空间中飞行器之间的相对运动,并且通过动力学等效的方式模拟复杂任务中的飞行器之间的接触动力学过程。这些任务对飞行器各自的运动精度提出了较高的要求。但是移动机器人的运动存在较多不确定性,精度较差,需要通过外部测量设备实时测量和机械臂等精度较高的执行机构实时运动补偿的方式提高其精度,因此在规划过程中需要保证机械臂基本实时补偿的运动能力。

基于移动机器人的半物理系统具有较多冗余自由度,对于任意相对目标位姿,均存在对应的构型子空间,因此可以额外设计性能指标,在每一个构型子空间中根据性能指标计算最符合要求的构型,从而保证整个仿真过程的顺利进行。

2 算法框架

为了实现在规划过程中优化基于移动机器人的半物理系统的操作能力、避关节极限性能,为后续的运动补偿环节创造条件,本文采取了一种基于超冗余机械臂的规划方法,通过一套完整的方法简化当前问题,提出的规划方法框架如图2所示。

图2 算法框架Fig.2 The framework of the proposed algorithm

具体步骤如下：

1)通过运动学等效的方式,将基于移动机器人的半物理系统等效为一个串联超冗余机械臂。

2)根据该超冗余机械臂的物理参数建立其运动学模型。

3)建立性能指标,并将该性能指标引入运动学模型中。

4)针对稀疏位姿点,利用五次多项式插值及球面线性插值等方式进行插值计算每个时刻的操作空间位姿及速度。

5)根据带性能指标的运动学公式计算该时刻的关节空间构型及速度。

6)循环计算整条轨迹。

7)输出规划结果。

3 运动学模型的等效与建立

本文中的规划问题均基于整体系统等效而成的超冗余机械臂,该超冗余机械臂的构建方法如下：以飞行器1为超冗余机械臂的基座,飞行器2为超冗余机械臂的末端,将移动机器人之间的相对运动等效为两个平移关节和一个转动关节的运动。此时,半物理系统被抽象为一个具有多自由度的超冗余机械臂,具有多个平移和转动关节,给定的空间飞行器之间的大范围相对运动轨迹即为超冗余机械臂的末端相对于基座的运动轨迹。等效过程示意图如图3所示。

图3 超冗余机械臂等效图(仅用于表示等效过程,不代表真实关节类型及数量)Fig.3 The equivalent process of the super redundant manipulator (only used to represent the equivalent process,not the real type and number of joints)

由于该机械臂高度冗余,对于任意给定的操作空间下的末端位姿,均有对应的无穷组关节构型。这些关节构型构成了该超冗余机械臂构型空间的一个子空间,在该子空间内可以根据特定目的设计性能指标以寻找对应的最优构型[16]。

此时,根据Denavit-Hartenberg方法建立超冗余机械臂各关节连杆的坐标系,根据该超冗余机械臂关节尺寸的参数,可以确定其D-H参数表,进而根据D-H参数表可以建立超冗余机械臂的运动学模型：

(1)

(2)

其中:J+(θ)为当前关节构型的雅可比矩阵的伪逆;(I-J+(θ)J(θ))为当前关节构型的雅可比矩阵的零空间;φ为任意优化向量;(I-J+(θ)J(θ))φ被称为当前构型的超冗余机械臂的自运动。

在对超冗余机械臂求解过程中,考虑到机械臂需要对移动平台的运动误差进行补偿,在对超冗余机械臂进行规划时需要在保证末端位姿准确的情况下,使超冗余机械臂的操作灵活性尽可能高、各关节角度尽可能远离关节极限,因此这里选定可操作度ω[17]与避关节极限系数h为性能指标,其定义分别为：

(3)

(4)

可操作度是机械臂的末端可操作度椭球的体积,当可操作度越接近零表示当前构型的末端可操作度椭球的某一个轴长度接近于零,该机械臂即将丧失沿该轴方向的运动能力,即奇异位形。计算可操作度的矢量梯度[18]可以得到：

(5)

(6)

其中:A=J(θ)JT(θ)=(aij)n×n,θi为超冗余机械臂的第i个关节角度。计算避关节极限系数的矢量梯度可以得到：

(7)

(8)

(9)

其中:a,b为调节系数,J和J+分别为当前关节构型对应的雅可比矩阵以及雅可比矩阵的伪逆。对公式进行离散化可以得到：

(10)

进一步整理可以得到:

θt+1-θt=J+(Pt+1-Pt)+α(I-J+J)·

(11)

其中:α为时间调节因子。此时即可根据上述公式并且结合当前关节构型计算下一时刻的构型,并且可以一定程度上保证超冗余机械臂的远离关节极限性能和可操作度性能。

4 轨迹规划方法

为了保证超冗余机械臂的末端运动按照给定的相对运动轨迹运动,这里采取在超冗余机械臂的操作空间内直接进行规划的方式。通常情况下,给定的相对运动轨迹为一系列较为稀疏的路径点,每个路径点均包含时间、位姿、速度及加速度。为了实现操作空间内的较为平滑的运动,需要对路径点之间进行插值以计算密集的插值点信息。路径点之间的位置信息采用对每个方向的坐标分别进行五次多项式插值,姿态信息采用姿态四元数的球面线性插值(Spherical linear interpolation,SLERP)[19]与直线插值相结合的方式,插值方式为等时间间隔插值。该规划过程具体如下：

对于任意相邻路径点,将相邻路径点之间的各方向位置坐标分别进行五次多项式插值[20]。五次多项式插值的表示形式为：

S(t)=At5+Bt4+Ct3+Dt2+Et+F

(12)

其中:S(t)为插值得到的关于时间的五次多项式,A,B,C,D,E,F分别为五次多项式的各阶系数。根据插值结果可以得到相邻路径点之间的任意插值点的位置向量[px,py,pz]T和速度向量[vx,vy,vz]T。

相邻路径点之间任意一个插值点的姿态四元数可以根据如下公式计算：

qk=

(13)

其中:q表示姿态四元数,Ω=arccos(qt·qt+1)为相邻路径点姿态四元数的夹角,·表示四元数乘法。θthreshold为直线插值与球面圆弧插值的临界角度。根据四元数运算规律,对于任意四元数q,有:

(14)

此时,经过整理可以得到当前插值点的转动速度：

(15)

其中:q*为当前姿态四元数的共轭,[ωx,ωy,ωz]T为机械臂末端在笛卡尔坐标系下的沿各个方向的转动角速度。

图4 在各构型子空间中规划的示意图Fig.4 The schematic diagram of the planning in each configuration subspace

5 实验系统搭建及结果分析

为了对上述规划方法进行验证和分析,本文设计并搭建了一套基于移动机器人的半物理系统并基于该系统对本文提出的方法展开详细分析与验证。该基于移动机器人的半物理系统整体结构示意图如图5所示。

图5 基于移动机器人的半物理系统设计图Fig.5 The overall design of the mobile robots based hardware-in-the-loop simulation system

该半物理系统主要分为三个主要组成部分：动作捕捉子系统、飞行器1子系统和飞行器2子系统。动作捕捉子系统可以提供实时精确测量结果,其原理为通过一定数量固定的高速相机捕捉粘贴于实验环境中物体上的标志点位置,并且根据标志点相对位置关系解算出试验场地内目标物体的实时精确位置姿态[21],同时测量频率可以达到120 Hz,基本满足实时性要求。该系统主要用来实时监测半物理系统中的移动机器人的运动,提供实时精确外部测量数据,结合控制及运动补偿部分改善移动机器人的运动精度。飞行器1子系统主要包括移动机器人、并联机构及飞行器1模型,飞行器2子系统主要包括移动机器人、机械臂及飞行器2模型。移动机器人基于麦克纳姆轮设计,具有完整非约束的三个自由度,可以在平面内保证灵活性的自由运动。并联机构为Stewart构型,用来小范围调整飞行器1的位姿,机械臂为七轴冗余机械臂,可以在一定范围内对飞行器2的位姿进行调整。

在上述整体系统框架下,基于ROS搭建了该基于移动机械臂的半物理系统,如图6、图7所示。图8所示为动作捕捉系统软件中的高速相机布置以及测量得到的移动机器人刚体位姿。各个子系统以及集控台之间通过ROS的多机通信框架实现信息交互。子系统上均搭载微型计算机完成低层次的规划和控制,保证控制频率,微型计算机和集控台之间通信,通过集控台协调实现整体系统的控制。整体系统的轨迹规划过程在集控台上离线完成。该半物理系统的工作空间不会受到系统本身的限制,但由于实验场地的限制,实际半物理系统的仿真空间为所用实验场地的可用空间。

图6 飞行器1子系统实物图Fig.6 The subsystem for the aircraft 1

图7 飞行器2子系统实物图Fig.7 The subsystem for the aircraft 2

图8 动作捕捉系统及测量到的移动机器人位姿Fig.8 The motion capture system and the measured pose of the mobile robots

针对上述基于移动机器人的半物理系统,根据第3节中的运动学等效方式将其等效为超冗余机械臂,移动机器人之间的平面内的运动等效为两个平移和一个旋转共三个自由度,由于并联机构的六自由度运动的耦合性及一般情况下仅小范围调整飞行器1的俯仰角度的功能,这里仅将并联机构等效为超冗余机械臂中的一个转动关节。此时,根据上述等效过程得到的超冗余机械臂的物理参数,建立该超冗余机械臂各连杆坐标系,如图9所示,以及D-H参数表,见表1。

表1 超冗余机械臂D-H参数表Table 1 D-H parameters table for super redundant manipulator

图9 超冗余机械臂的连杆坐标系Fig.9 Joint coordinate system of the super redundant manipulator

为了直观地对上述规划算法进行性能分析,这里给定一条典型路径用来通过实例验证算法性能。该路径分为两个阶段,第一阶段为飞行器2相对于飞行器1做直线运动,第二阶段飞行器2以飞行器1周围一点为圆心进行绕飞,第一阶段持续时间为10 s,第二阶段持续时间为10 s。其定义分别如下：

第一阶段的平移部分轨迹为:

姿态四元数为[0,0,0.707,0.707]T并且保持不变。第二阶段的平移部分轨迹为:

初始姿态四元数为[0,0,0.707,0.707]T,最终姿态四元数为[0,0,0,1]T,第二阶段全过程中机械臂末端匀速转动。

对上述轨迹进行等时间间隔均匀采点并根据轨迹公式进一步计算生成目标轨迹点,轨迹点之间的时间间隔为1 s。对生成的路径点使用上述规划及优化逆解方法进行求解和规划,可以得到如图10所示的各关节规划结果。图中优化前表示的数据为式(1)中的优化向量φ为零向量时获得的规划结果,优化后所表示的数据为根据式(10)中的方法计算得到的数据。由于关节2、3、4的运动由移动平台完成,并且未列入优化向量中,规划结果几乎完全相同,因此并未直接画出关节2、3、4三个关节的角度。从关节1及关节6的数据可以看出,相比较于未经过优化的方法,经过优化的规划方法可以有效避免关节角度超限,同时对于其他原本远离关节极限的关节,在原有位置的基础上基本保持不变。因此,规划结果验证了该方法为后续控制过程中为移动机器人的运动误差提供运动补偿预留了足够余量的结论。

图10 各关节的规划结果Fig.10 The planning results of every joint

在实际搭建出来的半物理系统上执行上述规划结果,在实验过程中,移动机器人、机械臂及并联机构均完全根据规划结果通过自身控制其进行运动,并未通过机械臂和并联机构对移动机器人的运动进行误差补偿。移动机器人通过实时获取动作捕捉系统的测量数据并进行闭环控制,其运动曲线如图11所示,可以看出,移动机器人的各个方向的运动误差范围约为±10 mm。实验过程中的半物理系统如图12所示。

图11 飞行器1子系统中的移动机器人运动曲线(上)和飞行器2子系统中的移动机器人运动曲线(下)Fig.11 The motion curve of the mobile robot in aircraft 1 subsystem (upper)and the motion curve of the mobile robot in aircraft 2 subsystem (lower)

图12 运行过程中的半物理仿真系统Fig.12 The hardware-in-the-loop simulation system during the experiment

利用动作捕捉系统对实验过程中两个子系统的移动平台位姿进行测量,并且结合机械臂及并联机构高精度编码器的测量数据,经过计算后可以得到两个飞行器模型之间的相对位姿数据,将测量数据与规划结果、期望位姿点进行比对,结果如图13所示。期望路径点表示对本文实验所用的典型路径直接采样生成的目标轨迹点,规划结果表示本文算法规划得到的结果对应的超冗余机械臂的末端相对于基座的相对位姿数据,实际运动表示使用本系统进行仿真的过程中飞行器2模型相对于飞行器1模型的相对位姿数据。可以看出,在本文提出的规划算法的基础上,该半物理系统可以连续、平稳地再现飞行器模型之间的相对运动,两个飞行器模型之间的相对运动与期望的运动存在一定误差,各方向的位置误差范围约为±15 mm,姿态角偏差范围约为±0.5°。对整体系统的运动误差来源进行分析可以发现,移动机器人的误差为当前系统运动误差的主要来源,另外,动作捕捉系统的测量坐标系的X-Y平面与实际实验场地的地面并未完全重合,导致移动机器人以及飞行器模型的运动出现了竖直方向上的运动偏差。因此,在当前情况下使用该半物理系统进行半物理仿真时,运动误差较大,不能满足精度要求,需要在后续过程中通过机械臂和并联机构对移动机器人的误差进行实时补偿,以提高运动精度,这也验证了本文提出的算法的重要性。