[发明专利]卫星平台与机械臂协同仿真模拟器

摘要

本发明提供一种卫星平台与机械臂协同仿真模拟器，包括：GNC模拟器、机械臂模拟器、动力学仿真机、星务遥测模拟器和综合接口箱。可实现卫星平台与机械臂之间的耦合仿真，可分析机械臂运动与平台控制之间的相互影响关系，所得结果较模拟器独立仿真具有更好的可信度；另外，该模拟器中卫星平台和机械臂分系统的相对独立，模拟器间的数据格式与实际卫星系统中的一致，因而可以接入真实的星上计算机，不仅可以验证系统算法的有效性，也可以验证系统设计的可靠性，具有可信度高、可扩展性好、灵活性强的优点。

主权项

一种卫星平台与机械臂协同仿真模拟器，其特征在于，包括：GNC模拟器、机械臂模拟器、动力学仿真机、星务遥测模拟器和综合接口箱；按照飞行任务流程，卫星平台设置了姿态捕获、对地定向、逼近停靠、机械臂操作和快速稳定5个任务模式，GNC模拟器预存了每个任务模式的转入条件、姿态和轨道控制算法、每个任务模式下所使用的执行机构列表，执行机构包括飞轮、磁力矩计和推力器，同时也预存了所有执行机构的安装位置、控制力和控制力矩大小信息；所述的GNC模拟器，用于模拟星载GNC系统的功能；GNC模拟器具体实现如下：(11)GNC模拟器启动后，首先进入姿态捕获任务模式；在仿真过程中，GNC模拟器采用独立线程监控CAN总线，随时接收由星务遥测模拟器发送的GNC指令；收到GNC指令后，根据指令中包含的任务模式字获取预存的任务模式转入条件，并判断转入条件是否满足；如果满足转入条件，则在下一控制周期开始切换到对应的任务模式，并载入任务模式对应的姿态和轨道控制算法；如果不满足转入条件，则丢弃收到的遥控指令；(12)GNC模拟器利用模拟器内部时间作为基准进行周期控制，等待时间达到预定控制时间时，开始一个控制周期的仿真；在GNC模拟器的每一控制周期内，首先通过CAN总线采集动力学仿真机发送的卫星状态信息，所述卫星状态信息包括姿态敏感器、交会测量敏感器、机械臂关节状态敏感器的测量信息；所述机械臂关节状态包括关节角度和角速度；(13)根据当前控制周期的任务模式，确定姿态和轨道控制算法，以卫星状态信息为输入，进行卫星平台姿态和轨道控制计算，求解所需要的控制力和控制力矩；(14)根据当前控制周期的任务模式，确定卫星执行机构列表，获得执行机构列表中包含的飞轮、磁力矩器和推力器的安装位置、控制力和控制力矩，进行执行机构分配求解，确定需要执行机构列表中各执行机构的开关时间及控制力和力矩大小，将步骤(13)产生的推力和力矩指令转化为飞轮、磁力矩器和推力器的开/关指令和动作电流，即形成执行机构指令，并发送至CAN总线；(15)按照卫星遥测数据格式，生成GNC状态信息发送至CAN总线，完成本控制周期的仿真，等待内部始终到达下一控制周期预定的控制时间后，开始下一周期的仿真；所述的机械臂模拟器，用于模拟机械臂任务规划系统的功能；按照飞行任务流程，实际系统设置了多个预先设定的机械臂关节期望动作序列，称为机械臂任务模式；机械臂模拟器预存了机械臂任务模式信息。机械臂模拟器具体实现如下：(21)在机械臂模拟器每个控制周期，首先从任务模式列表读取下一模式开始时间和任务模式字，如果时间到达模式开始时间，则切换机械臂任务模式至对应的任务模式；(22)然后从CAN总线接收由动力学仿真机发送的卫星平台位置和姿态、当前机械臂关节状态测量值；根据当前机械臂关节角和角速度，利用机械臂运动学关系，计算得到机械臂位于机械臂末端的手爪在卫星平台坐标系中的相对位置、姿态、速度和角速度；(23)根据卫星平台的位置和姿态、手爪在卫星平台坐标系中的相对位置和姿态，通过坐标变换，计算当前时刻太阳、手爪以及卫星平台之间的相对位置关系，确定手爪的光照条件；然后根据安装于机械臂手爪的手眼相机的视场角范围、手爪的光照条件，基于Open Scene Graph模拟手眼相机对安装于平台上的手柄成像进行模拟，生成手眼相机模拟图像；利用图像识别算法，对所成图像进行处理，求解得到手爪相对于手柄的相对位置测量值；(24)机械臂模拟器采用独立线程监控CAN总线，随时接收由星务遥测模拟器发送的机械臂动作指令，并记录指令中包含的任务模式字和任务开始时间，形成任务模式列表；根据当前的机械臂任务模式，从预存的机械臂关节期望动作序列读取本控制周期结束时的期望关节状态；将期望关节状态与当前机械臂的关节状态测量值作差值，得到本控制周期的机械臂关节状态的变化量；将控制周期时间内均匀取若干个时间点，从当前机械臂关节状态测量值到期望关节状态进行插值，得到各时间点的期望关节状态，形成当前控制周期的期望关节状态序列，并发送至CAN总线；所述动力学仿真机，用于实现卫星平台与机械臂复合动力学计算，轨道动力学、姿态敏感器、交会敏感器和机械臂关节状态敏感器测量仿真，以及机械臂控制过程仿真功能；具体实现如下：(31)动力学仿真机启动后，首先通过以太网与综合接口箱建立连接，之后开始监控以太网，接收由综合接口箱发送的数据；(32)接收综合接口箱转发的执行机构指令后，计算飞轮、推力器和磁力矩器的输出力和力矩；(33)接收综合接口箱转发的本周期机械臂期望关节状态序列后，利用机械臂关节角度测量结果求得误差信息，采用PD控制算法，计算机械臂各关节的控制力矩；(34)考虑6×6阶摄动项、大气阻力摄动、光压摄动因素计算卫星所受的环境力和力矩，与执行机构输出力和力矩合成，得到卫星所受的外力和外力矩，进行卫星平台轨道动力学计算，生成卫星平台轨道参数；(35)采用拉格朗日方法建立平台和机械臂复合动力学模型，将卫星所受的外力和外力矩及机械臂各关节控制力矩作为输入，采用8阶龙格‑库塔方法进行数值积分求解。由于卫星的姿态控制周期较大，为了提高复合动力学计算的精度，对每个控制周期内分为100步作为动力学计算的积分步长，得到控制周期内各时间点的卫星平台姿态、机械臂关节角和角速度参数；(36)将卫星平台轨道参数、卫星平台姿态、机械臂关节角和角速度参数输入至轨道动力学、姿态敏感器、交会敏感器和机械臂关节状态敏感器测量模型，计算得到卫星状态信息，按照通讯格式的要求组成数据帧并通过以太网发送至综合接口箱；所述星务遥测模拟器，用于为仿真模拟器提供用户输入输出界面，实现地面指令的输入和发送，并完成遥测数据的打包存储；具体实现如下：(41)星务遥测模拟器通过用户界面设置遥控指令输入，当收到新指令后，首先完成遥控指令解析，分析指令的内容和参数，并将指令按照卫星实际遥控指令格式进行打包；(42)如果是GNC指令，则通过CAN总线发送至GNC模拟器；如果是机械臂动作指令，则通过CAN总线发送至机械臂模拟器；(43)在仿真过程中，星务遥测模拟器通过轮询方式检测CAN总线的数据，由于CAN总线具有广播特性，因此GNC模拟器、机械臂模拟器和综合接口箱发送的信息都被星务遥测模拟器收到，包括执行机构指令、GNC状态信息、卫星状态信息和期望关节状态序列；星务遥测模拟器收到上述数据后，从中挑选构建遥测数据包需要的数据，并按照星上遥测数据格式进行重新打包，形成遥测数据包存储在星务遥测模拟器硬盘中，用于仿真后查阅；所述综合接口箱，用于完成所述的动力学仿真机的数据格式转换；它一端连接于CAN总线，通过CAN总线与GNC模拟器、机械臂模拟器及星务遥测模拟器进行数据交换；另一端通过以太网连接于动力学仿真机；综合接口箱启动后，首先监听以太网端口，等待动力学仿真机的连接，建立连接后，通过两个线程分别监听CAN总线和以太网的数据，当收到以太网数据，则按照CAN总线协议进行打包，并发送至CAN总线；当收到CAN总线数据，则按照以太网协议进行打包，通过以太网发送至动力学仿真机。