空间三维大尺度运动学仿真系统及方法与流程

本发明涉及三维半物理仿真技术领域，尤其涉及一种空间三维大尺度运动学仿真系统及方法。

背景技术：

随着空间技术的发展，空间飞行器需要执行的任务逐渐变得复杂，为了完成这些越来越复杂的任务，研究空间环境中各个飞行器之间的运动关系和控制策略非常必要，但是空间飞行器在轨运行过程中直接进行实验时，成本和风险较高。另一方面，随着人工智能技术的发展，空间飞行器的控制决策技术也将进入智能化时代，但目前人工智能技术的研究往往依赖于大量数据集，空间环境的特殊性导致在该环境下数据集的获取难度极大，并且在空间环境中很难直接进行人工智能相关算法的验证。在这种情况下，一套合理的模拟空间飞行器之间的相对运动关系的地面仿真系统对研究空间环境中飞行器的运动及其控制以及研究空间飞行器的人工智能算法有重要的作用。

目前已经存在一些模拟多空间飞行器相互运动的地面仿真系统，如2016年5月4日公开的中国专利cn105539890a提出了一种模拟空间机械臂捕获目标飞行器的地面三维空间微重力的装置与方法，在该专利中，任务飞行器与目标飞行器安装在两个固定基座的机械臂末端，以模拟微重力环境下任务飞行器的机械臂捕获目标飞行器过程中的运动学。但是这种方式由于固定基座的限制，地面仿真系统往往只能在地面一定范围内对空间中任务飞行器和目标飞行器之间的相对运动的仿真，动作空间较小，只能针对小范围运动进行仿真，很难直接仿真空间中的三维大范围运动。

技术实现要素：

鉴于背景技术中存在的问题，本发明的目的在于提供一种空间三维大尺度运动学仿真系统及方法，其能够在有限的地面实验空间中实现大尺度三维空间内的飞行器间的相对运动的模拟，增加仿真系统的灵活性，降低问题的复杂程度，降低整体仿真系统的构建成本。

为了实现上述目的，在第一方面，本发明提供了一种空间三维大尺度运动学仿真系统，其包括实验场地、目标飞行器模拟机构、任务飞行器模拟机构；目标飞行器模拟机构包括目标飞行器和第一工业机械臂，第一工业机械臂的基座固定于实验场地上，第一工业机械臂的末端连接于目标飞行器；任务飞行器模拟机构包括任务飞行器、第二工业机械臂和全方位移动平台，第二工业机械臂的基座固定于全方位移动平台上，第二工业机械臂的末端连接于任务飞行器，全方位移动平台设置在实验场地上并能够在实验场地上移动。

在一实施例中，空间三维大尺度运动学仿真系统还包括：定位系统，设置在全方位移动平台上，用于实时获得全方位移动平台相对第一工业机械臂的基座的相对位姿。

为了实现上述目的，在第二方面，本发明提供了一种空间三维大尺度运动学仿真方法，其基于第一方面所述的空间三维大尺度运动学仿真系统进行仿真，其包括：建立目标飞行器固接坐标系ft，ft的原点为目标飞行器的质心，建立目标飞行器轨道坐标系fw，fw的原点为目标飞行器的质心，建立任务飞行器固接坐标系fs，建立实验场地坐标系fe、第一工业机械臂基座坐标系ftb和第二工业机械臂基座坐标系fsb，其中，不考虑目标飞行器沿空间轨道的平动，只考虑空间中目标飞行器与任务飞行器间的相对运动关系；在坐标系ft、fw、fs下，基于给定目标飞行器的初始角速度向量值并依据空间动力学的计算得出目标飞行器在空间环境下的定点运动轨迹t2(t)，其中，t2(t)是目标飞行器绕质心的转动轨迹；在坐标系ft、fw、fs下，针对任务飞行器的控制策略，控制任务飞行器相对目标飞行器运动，根据任务飞行器的飞行路线直接得出任务飞行器在空间环境下的的运动轨迹t1(t)，其中，t1(t)为任务飞行器的固接坐标系fs相对轨道坐标系fw的相对运动轨迹，t1(t)由一系列密集的离散点表示；将任务飞行器对目标飞行器在空间上的相对运动进行在实验场地的运动规划：将任务飞行器与目标飞行器间的相对运动划分为第一虚拟运动和第二虚拟运动，第一虚拟运动为ft绕fw的原点的定点运动，第二虚拟运动为fs相对于fw的空间六自由度任意运动，其中，第二虚拟运动为fs相对于fsb、fsb相对于fe、fe相对于ftb、ftb相对于fw的连续运动链实现，将连续运动链虚拟为超冗余机械臂，超冗余机械臂的基座坐标系为fw，超冗余机械臂的末端为任务飞行器的固接坐标系fs；依据给定的任务飞行器的运动轨迹t1(t)，基于虚拟的超冗余机械臂的运动学模型进行运动规划和轨迹规划，计算得出超冗余机械臂各关节的运动轨迹，即，第二工业机械臂的各关节运动轨迹i∈[1,ns]，其中，ns是第二工业机械臂的关节数量；全方位移动平台的各自由度的运动轨迹i∈[x,y,θ]；以及第一工业机械臂的各关节运动轨迹i∈[1,ntt]，其中，ntt是第一工业机械臂的关节数量；根据第一工业机械臂的各关节运动轨迹tratti(t)计算得出第一工业机械臂的末端fw相对基座ftb的运动轨迹trat(t)＝[xt(t)yt(t)zt(t)rxt(t)ryt(t)rzt(t)]^t；将第一工业机械臂末端fw的运动轨迹trat(t)与目标飞行器绕质心的运动轨迹t2(t)进行融合，得到第一工业机械臂的末端ft在ftb下的运动轨迹，然后基于ft在ftb下的运动轨迹，求得第一工业机械臂的最终关节合运动的运动轨迹i∈[1,nt]，其中，nt是第一工业机械臂的关节数量；根据对各关节的轨迹规划结果，进行任务飞行器相对目标飞行器的相对运动的仿真。

在一实施例中，将第一工业机械臂末端fw的运动轨迹trat(t)与目标飞行器绕质心的运动轨迹t2(t)进行融合的具体步骤包括：建立正运动学方程ptm＝x(qtm)，逆运动学方程qtm＝y(ptm)，其中，ptm表示第一工业机械臂末端在ftb下的位置姿态向量，qtm表示第一工业机械臂在关节空间中各个关节的角度向量；将超冗余机械臂的末端fw相对基座ftb规划所得的运动轨迹trat(t)用齐次变换矩阵表示为将目标飞行器绕质心的运动轨迹t2(t)扩充为齐次变换矩阵根据可以计算出第一工业机械臂末端ft在ftb下的齐次变换矩阵然后基于通过第一工业机械臂的逆运动学方程qtm＝y(ptm)求解各关节的位置，通过差分方法计算各关节的速度和加速度，最终得到第一工业机械臂合运动规划结果

在一实施例中，在执行最后一步过程中，地面仿真系统仿真空间中任务飞行器抵近目标飞行器的方法还包括：全方位移动平台依据实测数据轨迹i∈[x,y,θ]，建立pid反馈控制系统，以跟随全方位移动平台各自由度的期望的运动轨迹i∈[x,y,θ]。

本发明的有益效果如下：通过本发明的空间三维大尺度运动学仿真方法，通过第二机械臂与全方位移动平台的结合，突破了地面实验环境的限制，实现了较大范围内的运动学仿真，增加了仿真系统的灵活性；此外，超冗余机械臂的建立，将多个执行机构的复杂协同规划问题整合成一个超冗余机械臂的规划问题，降低了问题的复杂程度；再者，通过定义轨道坐标系及抽象出的虚拟的超冗余机械臂和一个定点运动结构，将现有技术中的多个系统分别进行运动规划的问题转化为两个独立的虚拟运动机构的协同规划问题，降低了问题的复杂程度。

附图说明

图1是在轨道坐标系fw下的根据本发明的空间三维大尺度运动学仿真系统的示意图。

图2是将图1中的任务飞行器与目标飞行器间的连续运动链虚拟成超冗余机械臂的示意图，其中，全方位移动平台与实验场地坐标系之间建立了虚拟机械臂。

其中，附图标记说明如下：

1实验场地32第二工业机械臂

2目标飞行器模拟机构33全方位移动平台

21目标飞行器4定位系统

22第一工业机械臂r超冗余机械臂

3任务飞行器模拟机构m虚拟机械臂

31任务飞行器

具体实施方式

下面参照附图详细说明根据本发明的空间三维大尺度运动学仿真系统及方法。

参照图1和图2，根据本发明的空间三维大尺度运动学仿真系统包括实验场地1、目标飞行器模拟机构2、任务飞行器模拟机构3以及定位系统4。

目标飞行器模拟机构2包括目标飞行器21和第一工业机械臂22，第一工业机械臂22的基座固定于实验场地1上，第一工业机械臂22的末端连接于目标飞行器21。第一工业机械臂22通过调整末端位置姿态来调整目标飞行器21的运动。需要补充的是，第一工业机械臂22主要由关节和连杆组成，关节由电机、谐波减速器、绝对位置传感器、关节力矩传感器以及关节控制器等组成。

在一实施例中，目标飞行器21通过连接法兰连接于第一工业机械臂22。

在一实施例中，第一工业机械臂22为六轴机械臂，当然不限于此，第一工业机械臂22可以根据实际情况增加或减少自由度，以更好地满足使用需求。

任务飞行器模拟机构3包括任务飞行器31、第二工业机械臂32和全方位移动平台33，第二工业机械臂32的基座固定于全方位移动平台33上，第二工业机械臂32的末端连接于任务飞行器31，全方位移动平台33设置在实验场地1上并能够在实验场地1上移动。

在一实施例中，任务飞行器31通过连接法兰连接于第二工业机械臂32。

在一实施例中，第二工业机械臂32为六轴机械臂，同样地，第二工业机械臂32也可根据实际情况增加或减少自由度，以更好地满足使用需求。需要补充的是，第二工业机械臂32主要由关节和连杆组成，关节由电机、谐波减速器、绝对位置传感器、关节力矩传感器以及关节控制器等组成。

全方位移动平台33为全向移动agv，例如基于多组麦克纳姆轮的全向移动小车或者基于多组舵轮的全向移动小车。全方位移动平台33在实验场地1中运动，可以实现x、y的平动和z轴的转动，同时第二工业机械臂32调整末端的任务飞行器31的位姿，与全方位移动平台33共同控制任务飞行器31的运动。全方位移动平台33相比于现有技术而言，大幅度增加了任务飞行器31的运动范围，避免使用导轨对运动机构产生的约束，满足空间大尺度三维运动模拟的要求；此外，使得仿真系统可以增加被模拟对象的数量，例如可以设置两个任务飞行器来追踪目标飞行器，从而增加了仿真系统的灵活性。

在一实施例中，如图1所示，定位系统4设置于全方位移动平台33上。当然，定位系统4还可以设置在其它位置，可根据具体需要进行设置。定位系统4用于实时监测全方位移动平台33相对于第一工业机械臂22的基座ftb的相对位姿，然后根据相对位姿计算出全方位移动平台33的实时运动轨迹i∈[x,y,θ]，其中，为全方位移动平台33在某一时刻的i轴方向的实时角度，为全方位移动平台33在某一时刻的i轴方向的实时角速度，为全方位移动平台33在某一时刻的i轴方向的实时角加速度，之后建立pid反馈控制系统，以使全方位移动平台33不断调节位姿直至调整至后述的期望运动轨迹i∈[x,y,θ]，其中，qvi(t)为全方位移动平台33在某一时刻的i轴方向的期望角度，为全方位移动平台33在某一时刻的i轴方向的期望角速度，为全方位移动平台33在某一时刻的i轴方向的期望角加速度，由此，提高了全方位移动平台33的定位精度。需要说明的是，期望运动轨迹travi(t)中的各个数据是在实验场地下的基于t1(t)计算得出的理论数据。定位系统4可以为相机定位系统、高精度激光雷达定位系统或室内gps或高精度运动捕捉系统等。以高精度激光雷达为例对定位系统的定位过程进行具体说明：在第一工业机械臂22固定安装在实验场地1的情况下，将安装有高精度激光雷达的全方位移动平台33放入实验场地1中并控制全方位移动平台33运动，使用激光雷达对周围环境进行扫描得到周围环境的点云图，同时根据全方位移动平台33的自身里程计信息利用slam算法(如gmapping、cartographer等)对周围环境建图，直至得到具有较高精度的全局环境地图；在仿真过程中，激光雷达对当前位置的周围环境进行扫描，生成当前位置的局部点云地图，将该局部点云地图与全局环境地图进行匹配，将匹配得到的位置作为当前全方位移动平台33的实时位置(即，上文所述的全方位移动平台33相对于第一工业机械臂22的基座ftb的相对位姿)。采用高精度定位系统解决了全方位移动平台33的空间定位问题，从而在不采用大行程导轨等机构的情况下，精确测出全方位移动平台33的空间位姿，满足了运动模拟范围和模拟精度。

下面具体说明本发明的空间三维大尺度运动学仿真方法。

根据本发明的空间三维大尺度运动学仿真方法，其基于上述的空间三维大尺度运动学仿真系统进行仿真，其包括：建立目标飞行器21固接坐标系ft，ft的原点为目标飞行器21的质心，建立目标飞行器21轨道坐标系fw，取fw的原点为目标飞行器21的质心，fw的各轴指向空间轨道的切线和法线方向，建立任务飞行器31固接坐标系fs，建立实验场地1坐标系fe、第一工业机械臂22基座坐标系ftb(ftb与fe重合)和第二工业机械臂32基座坐标系fsb，其中，不考虑目标飞行器21沿空间轨道的平动，只考虑空间中目标飞行器21与任务飞行器31间的相对运动关系。需要说明的是，由于不考虑目标飞行器21沿空间轨道的平动，则fw的原点静止不动。

在坐标系ft、fw、fs下，基于给定目标飞行器21的初始角速度向量值并依据空间动力学的计算得出目标飞行器21在空间环境下的定点运动轨迹t2(t)，其中，t2(t)是目标飞行器21绕质心的转动轨迹。具体而言，依据空间动力学的计算结果得出目标飞行器21的运动轨迹分为目标飞行器21质心的平动轨迹tt(t)及目标飞行器21绕质心的转动轨迹tr(t)，由于仿真过程中不考虑目标飞行器21沿空间轨道的平动，因此，t2(t)＝tr(t)，空间动力学计算过程为：角速度向量其中ω＝cωs，c为与目标飞行器惯性参数有关的常数。ωs为自转角速度，且为常数，ωn为横向角速度，为常数，为角速度初始偏转角，为常数。依据需要给定初始值ωx0,ωy0,ωz0，可以计算ωs,ωn,三个常数为：ωs＝ωz0；对上述角速度向量进行积分可以得到偏转角度：其中，r0,y0,p0为根据需要设定的初始角度。由此对于任意时刻目标飞行器21的转动角度及速度，可以根据上述公式求得。对于得到的欧拉角r,p,y可以根据公式直接转换为齐次变换矩阵。定点运动轨迹t2(t)可以通过系列密集的离散点描述，其中任意一个离散点包含该点对应的时间和位置。

在坐标系ft、fw、fs下，针对任务飞行器31的控制策略，控制任务飞行器31相对目标飞行器21运动，根据任务飞行器(31)的飞行路线直接得出任务飞行器31的在空间环境下的运动轨迹t1(t)，其中，t1(t)为任务飞行器31的固接坐标系fs相对轨道坐标系fw的相对运动轨迹，该轨迹通过已知的一系列密集的离散点描述，任意一个离散点包含该点对应的时间及位置。补充的是，任务飞行器31的控制策略是待测的任务执行策略，控制策略具有多种实现形式，例如，通过地面遥操作的远程控制方式；再如，依据飞行器自带的传感器经感知环境并规划出的相应的运动，然后基于当前的控制策略，任务飞行器31将产生的空间轨迹，即，任务飞行器31的固接坐标系fs相对于轨道坐标系fw的轨迹t1(t)。

将任务飞行器31对目标飞行器21在空间上的相对运动进行在实验场地的仿真规划：将任务飞行器31与目标飞行器21间的相对运动划分为第一虚拟运动和第二虚拟运动，第一虚拟运动为ft绕fw的原点的定点运动，第二虚拟运动为fs相对于fw的空间六自由度任意运动，其中，第二虚拟运动为fs相对于fsb(fs相对于fsb由第二工业机械臂32的ns个关节实现)、fsb相对于fe(fsb相对于fe由下文所述虚拟机械臂m的三个关节实现)、fe相对于ftb(相互位姿固定不动)、ftb相对于fw(ftb相对于fw由第一工业机械臂22的nt个关节实现)的连续运动链实现，将连续运动链虚拟为超冗余机械臂r，超冗余机械臂r的基座坐标系为fw，超冗余机械臂r的末端为任务飞行器31的固接坐标系fs。需要说明的是，为了建立超冗余机械臂r，将全方位移动平台33与实验场地1之间构建一个虚拟机械臂m(如图2所示)，与第一工业机械臂22和第二工业机械臂32不同的是，虚拟机械臂m只有三个自由度，分别为在实验场地1上的x、y方向的平动和绕z方向的转动，在此可以将虚拟机械臂m的三个自由度抽象为三个关节。由此，超冗余机械臂r由第一工业机械臂22、虚拟机械臂m和第二工业机械臂32串联而成，目标飞行器21绕超冗余机械臂r的基座fw做定点运动。

依据前述给定的任务飞行器31的运动轨迹t1(t)，基于虚拟的超冗余机械臂r的运动学模型进行运动规划和轨迹规划，计算得出超冗余机械臂r各关节的运动轨迹，即，第二工业机械臂32的各关节运动轨迹i∈[1,ns]，其中，ns是第二工业机械臂32的关节数量，qsi(t)为第二工业机械臂32的某个关节在某一时刻的角度，为第二工业机械臂32的某个关节在某一时刻的角速度，为第二工业机械臂32的某个关节在某一时刻的角加速度；全方位移动平台33的各自由度的运动轨迹i∈[x,y,θ]；以及第一工业机械臂22的各关节运动轨迹i∈[1,ntt]，其中，ntt是第一工业机械臂22的关节数量。

根据第一工业机械臂22在构建的超冗余机械臂r的情况下，以fw为基座坐标系计算得到的各关节运动轨迹tratti(t)，进行计算并得出第一工业机械臂22的末端fw相对基座ftb的运动轨迹trat(t)＝[xt(t)yt(t)zt(t)rxt(t)ryt(t)rzt(t)]^t。

将第一工业机械臂22末端fw的运动轨迹trat(t)与目标飞行器21绕质心的运动轨迹t2(t)进行融合，得到第一工业机械臂22的末端ft在ftb下的运动轨迹，然后基于ft在ftb下的运动轨迹，求得第一工业机械臂22的最终关节合运动的关节运动轨迹i∈[1,nt]其中，nt是第一工业机械臂(22)的关节数量。需要说明的是，运动轨迹trat(t)是第一工业机械臂22的末端fw相对基座ftb的运动轨迹，是通过第二虚拟运动计算得出的结果，若想通过第一工业机械臂22对目标飞行器21进行空间上的仿真，还需要考虑第一虚拟运动，将第一虚拟运动与trat(t)进行合成，才能获得第一工业机械臂22的合运动的关节轨迹trati(t)。

根据对各关节的轨迹规划结果，进行任务飞行器31相对目标飞行器21的相对运动的仿真。

通过本发明的空间三维大尺度运动学仿真方法，通过第二机械臂32与全方位移动平台33的结合，突破了地面实验环境的限制，实现了较大范围内的运动学仿真，增加了仿真系统的灵活性；此外，超冗余机械臂r的建立，将多个执行机构的复杂协同规划问题整合成一个超冗余机械臂r的规划问题，降低了问题的复杂程度；再者，通过定义轨道坐标系fw及抽象出的虚拟的超冗余机械臂r和一个定点运动结构，将现有技术中的多个系统分别进行运动规划的问题转化为两个独立的虚拟运动机构(上文所述的第一虚拟运动和第二虚拟运动)的协同规划问题，降低了问题的复杂程度。

需要补充的是，建立超冗余机械臂r的具体步骤包括：将fsb相对fe抽象为虚拟机械臂m(如图2所示)，虚拟机械臂m的三个自由度虚拟为三个关节，进而可以将第一工业机械臂22、虚拟机械臂m和第二工业机械臂32串联形成超冗余机械臂r。

针对虚拟超冗余机械臂r的运动学模型及空间运动轨迹t1(t)，求得各个关节的运动轨迹的具体过程如下：根据虚拟超冗余机械臂r的雅克比矩阵，可以得到各关节空间速度与超冗余机械臂末端速度的关系：其中ja为雅克比矩阵。定义加权矩阵w＝diag[ω1,ω2,…,ωn]，其中，n＝ns+nv+nt，n为超冗余机械臂r的自由度数量，其中，h(q)为拟优化的目标函数，优化性能指标包括机械臂的运动灵活性、避障和关节角度极限等。在加权矩阵作用下，定义加权雅克比矩阵及加权关节角速度基于加权雅克比矩阵及加权关节角速度，有由此可以求得加权关节角速度其中，进而得到式(一)将带入到式(一)中，进一步得到轨迹t1(t)由一系列关于时间的离散点组成，相邻点之间的时间间隔为δt，超冗余机械臂末端速度根据计算得出，由此可以计算得出进而可以根据上一时刻的关节角速度计算当前时刻的关节角度根据求得的关节角度q(t)，针对任一单一关节，采用三次多项式插值的方式计算速度和加速度值，由此，可以最终得到超冗余机械臂的规划结果，即，第二工业机械臂32的各关节运动轨迹i∈[1,ns]；全方位移动平台33的各自由度的运动轨迹i∈[x,y,θ]；以及第一工业机械臂22的各关节运动轨迹i∈[1,ntt]。

将第一工业机械臂22末端fw的运动轨迹trat(t)与目标飞行器21绕质心的运动轨迹t2(t)进行融合的具体步骤包括：建立正运动学方程ptm＝x(qtm)，逆运动学方程qtm＝y(ptm)，其中，ptm表示第一工业机械臂22末端在ftb下的位置姿态向量，qtm表示第一工业机械臂22在关节空间中各个关节的角度向量；将超冗余机械臂的末端fw相对基座ftb规划所得的运动轨迹trat(t)用齐次变换矩阵表示为将目标飞行器21绕质心的运动轨迹t2(t)扩充为齐次变换矩阵根据可以计算出第一工业机械臂22末端ft在ftb下的齐次变换矩阵然后基于通过第一工业机械臂22的逆运动学方程qtm＝y(ptm)求解各关节的位置，通过差分方法计算各关节的速度和加速度，最终得到第一工业机械臂22的合运动的规划结果i∈[1,nt]。

第一工业机械臂22和第二工业机械臂32自身设有关节控制器，从而使得在仿真过程中，二者能够精确地跟随规划的运动轨迹(即，trasi(t)和trati(t))。全方位移动平台33可以通过高精度定位系统4，能够精确调节自身位姿，从而精确跟随规划的运动轨迹，具体地，全方位移动平台33依据实测数据轨迹i∈[x,y,θ]，建立pid反馈控制系统，以跟随全方位移动平台33各自由度的期望的运动轨迹