一种变负载高转速翼伞驱动控制方法、设备和存储介质与流程

本发明一种变负载高转速翼伞驱动控制方法，属于航天器返回减速着陆设计领域。

背景技术：

1、火箭助推器、火箭整流罩完成任务后脱离本体，脱离后如果处于自由降落状态，落地速度大，落点分布广，将对地面人员设施的安全造成巨大威胁。采用翼伞方式可以帮助火箭助推器、火箭整流罩以可控的速度和轨迹降落在指定的安全区域。操纵翼伞的伺服驱动装置通过电机旋转控制翼伞操纵绳，根据操纵指令精准收放。在高空状态下，操纵绳一端连接翼伞，另一端连接电机及其固连的助推器、整流罩。由于风力等自然环境影响和助推器整流罩不定姿态影响，操纵绳将有0～6000n不可预测的拉力变化，同时操纵指令将根据翼伞回收状态实时计算，指令值不可预测但必须保证按规定速度尽快到达，同时不可对电机、电池、驱动器以及传动机构造成损伤。

技术实现思路

0、技术实现要素：

1、本发明解决的技术问题：克服现有技术的不足，提供一种变负载高转速翼伞驱动控制方法，能够在克服这种拉力变化下，保证操纵指令的正确执行，同时在翼伞驱动控制系统出现问题的情况下，进入保护流程，保证系统其他设备的安全，最终实现翼伞回收任务顺利完成。

2、本发明采用的技术方案为：

3、本发明公开了一种变负载高转速翼伞驱动控制方法，包括：

4、s1、接收上位机发送的操纵绳位置指令cmd，设置反转标志位为0；获取当前指令速度cmdspeed、实时速度realspeed和电机角度；

5、s2、根据当前指令速度cmdspeed、实时速度realspeed和电机角度，判断是否存在电机失速或卡滞；若是，则进行错误处理，退出；若否，则进入步骤s3；

6、s3、根据所述操纵绳位置指令cmd，得到电机位置指令goalpos；

7、s4、根据电机位置指令goalpos和电机实际位置realpos，得到电机转动角度needtogo；

8、s5、获得电机当前状态和电机设定角度disforspeed；

9、s6、根据电机当前状态、电机设定角度disforspeed和所述电机转动角度needtogo，确定电机下一时刻的运动状态；

10、s7、根据电机下一时刻的运动状态，计算下一时刻的电机运动状态下所能到达的最大速度speedfinal；

11、s8、根据所述电机下一时刻的运动状态和当前指令速度cmdspeed，计算下一时刻的速度指令scmdspeed和剩余距离；

12、s9、根据剩余距离，判断电机当前位置是否到达电机位置指令goalpos，若是，则停止，退出；若否，则进入步骤s10；

13、s10、根据电机当前状态、剩余距离、反转标志位、当前指令速度cmdspeed和下一时刻的电机运动状态下所能到达的最大速度speedfinal，控制电机的运动状态；

14、s11、当电机的运动状态为正向减速或反向减速，如果速度指令scmdspeed为0且反转标志位为1时，则进入步骤s4；否则，则进入步骤s9。

15、进一步地，在上述控制方法中，步骤s10中，根据电机当前状态、剩余距离、反转标志位、当前指令速度cmdspeed和下一时刻的电机运动状态下所能到达的最大速度speedfinal，控制电机的运动状态，具体方法为：

16、当电机当前状态为正向加速时，判断最大速度speedfinal是否小于最大目标速度speedgoal；若是，计算下一时刻的速度指令scmdspeed，当下一时刻的速度指令scmdspeed等于最大速度speedfinal时，则电机的下一运动状态为正向减速；否则，计算下一时刻的速度指令scmdspeed，当下一时刻的速度指令scmdspeed等于最大速度speedgoal时，电机的下一运动状态为正向匀速；

17、当电机当前状态为正向匀速时，计算下一时刻的速度指令scmdspeed；如果剩余距离小于等于电机设定角度disforspeed，下一时刻运动状态为正向减速；

18、当电机当前状态为正向减速时，计算下一时刻的速度指令scmdspeed；如果反转标志位为0，则当速度指令scmdspeed到达设定速度，则以设定速度匀速运动；如果反转标志位为1，则判断下一时刻的速度指令scmdspeed为0时，下一时刻运动状态为反向加速；

19、当电机当前状态为反向加速时，判断最大速度speedfinal是否小于最大目标速度speedgoal；若是，计算下一时刻的速度指令scmdspeed，当下一时刻的速度指令scmdspeed等于最大速度speedfinal时，则电机的下一运动状态为反向减速；否则，计算下一时刻的速度指令scmdspeed，当下一时刻的速度指令scmdspeed等于最大速度speedgoal时，电机的下一状态为反向匀速；

20、当电机当前状态为反向匀速时，计算下一时刻的速度指令scmdspeed；如果剩余距离小于等于电机设定角度disforspeed，下一时刻状态为反向减速；

21、当电机当前状态为反向减速时，计算下一时刻的速度指令scmdspeed；如果反转标志位为0，则当速度指令scmdspeed到达设定速度，则以设定速度匀速运动；如果反转标志位为1，则判断下一时刻的速度指令scmdspeed为0时，下一时刻状态为正向加速。

22、进一步地，在上述控制方法中，计算下一时刻的速度指令scmdspeed，具体方法为：

23、当电机当前状态为正向加速时，scmdspeed＝cmdspeed+accgoal*ts；

24、当电机当前状态为反向加速时，scmdspeed＝cmdspeed-accgoal*ts；

25、当电机当前状态为正向匀速时，scmdspeed＝speedgoal；

26、当电机当前状态为反向匀速时，scmdspeed＝speedgoal；

27、当电机当前状态为正向减速时，scmdspeed＝cmdspeed-accgoal*ts；

28、当电机当前状态为反向减速时，scmdspeed＝cmdspeed+accgoal*ts；

29、当电机当前状态为静止状态时，scmdspeed＝0，cmdspeed＝0；

30、其中，scmdspeed为下一时刻的速度指令，cmdspeed为当前指令速度，ts为系统采样周期，accgoal为系统要求的电机加速度，speedgoal为系统要求的最大目标速度。

31、进一步地，在上述控制方法中，步骤s6中，所述根据电机当前状态、电机设定角度disforspeed和所述电机转动角度needtogo，确定电机下一时刻的运动状态，具体方法为：

32、若电机转动角度needtogo大于零，则判断电机当前状态：

33、当电机当前状态为正向加速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则电机下一时刻的运动状态为正向减速，否则为正向加速；

34、当电机当前状态为正向匀速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则电机下一时刻的运动状态为正向减速，否则为正向匀速；

35、当电机当前状态为正向减速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则保持正向减速状态，否则，电机下一时刻的运动状态为正向加速；

36、当电机当前状态为反向加速时，则电机下一时刻的运动状态为反向减速，设置反转标志位为1；

37、当电机当前状态为反向匀速时，则电机下一时刻的运动状态为反向减速，设置反转标志位为1；

38、当电机当前状态为反向减速时，则电机下一时刻的运动状态为反向减速，设置反转标志位为1；

39、当电机当前状态为静止时，则电机下一时刻的运动状态为正向加速。

40、进一步地，在上述控制方法中，步骤s6中，根据电机当前状态、电机设定角度disforspeed和所述电机转动角度needtogo，确定电机下一时刻的运动状态，具体方法还包括：

41、若电机转动角度needtogo小于零，则判断电机当前状态：

42、当电机当前状态为正向加速时，则电机下一时刻的运动状态为正向减速，设置反转标志位为1；

43、当电机当前状态为正向匀速时，则电机下一时刻的运动状态为正向减速，设置反转标志位为1；

44、当电机当前状态为正向减速时，则电机下一时刻的运动状态为正向减速，设置反转标志位为1；

45、当电机当前状态为反向加速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则电机下一时刻的运动状态为反向减速；

46、当电机当前状态为反向匀速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则电机下一时刻的运动状态为反向减速；

47、当电机当前状态为反向减速时，如果电机转动角度needtogo小于电机设定角度disforspeed，则保持正向减速状态，否则，电机下一时刻的运动状态为反向加速；

48、当电机当前状态为静止时，则电机下一时刻的运动状态为反向加速。

49、进一步地，在上述控制方法中，步骤s7中，根据电机下一时刻的运动状态，计算下一时刻的电机运动状态下所能到达的最大速度speedfinal，具体方法为：

50、当speedfinal大于等于speedgoal时，将speedgoal的值赋给speedfinal；

51、

52、其中，accgoal为系统要求的电机加速度，needtogo为电机转动角度，cmdspeed为指令速度，speedgoal为最大目标速度。

53、进一步地，在上述控制方法中，步骤s2中，所述判断是否存在电机失速或卡滞；若是，则进行错误处理，具体为：

54、若实时速度realspeed大于1.2倍指令速度cmdspeed，则判定电机处于失速状态，关闭电机中上桥臂功率开关管，开启电机中下桥臂功率开关管；

55、若电机在时间t内转的角度小于电角度60°，则判定电机为卡滞状态，关闭电机中上桥臂功率开关管，开启电机中下桥臂功率开关管；其中，t＝2*sqrt(60/accgoal/p)，表征电机在accgoal下走过60°电角度所用时间的2倍，p为电机的极对数，accgoal为系统要求的电机加速度。

56、进一步地，在上述控制方法中，步骤s1中，根据所述操纵绳位置指令cmd，得到电机位置指令goalpos，具体为：

57、goalpos＝cmd*k

58、其中，goalpos为电机需要旋转的目标角度，k为将操纵绳位置指令转换为目标角度的比例系数。

59、本发明公开了一种计算机设备，包括：

60、存储器，用于存储至少一个处理器所执行的指令；

61、处理器，用于执行存储器中存储的指令执行上述的一种变负载高转速翼伞驱动控制方法。

62、本发明公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行上述的一种变负载高转速翼伞驱动控制方法。

63、本发明相对于现有技术的有益效果为：

64、(1)本发明采用比例控制参数实时调整技术方案，实现了电机在大范围负载变动情况下仍能保证电机转速实时跟随指令转速的技术效果，解决了翼伞操纵绳收放距离因电机控制不稳而无法准确控制翼伞姿态的技术问题；

65、(2)本发明采用电机转速轨迹规划依据上位机指令实时调整的技术方案，实现了既能快速响应上位机指令又能使电机转速平滑过渡到要求状态的技术效果，解决了电机响应上位机指令过于直接，导致电机运转压力过大，反而造成电机响应指令不及时不准确的技术问题；

66、(3)本发明采用实时监测电机过载、失速并进行保护的技术方案，实现了保证电机不会因外部不可控环境导致电机失控的技术效果，解决了因意外环境突变，导致对电机过载或卡滞，造成对电机本身、电池、电路的伤害技术问题。