一种柔性悬挂刀锯的树障清理空中机器人飞行控制方法及装置

本发明涉及航空飞行器与机器人控制方法及装置，特别是涉及柔性悬挂刀锯的树障清理空中机器人飞行控制方法及装置。

背景技术：

1、输电线路是电力网络的重要组成部分，常常受到天气、植物、动物和人为等因素影响。其中，高压输电线路通道附近的树木经常超过安全距离，造成线路通过树木向地面放电，造成停电、火灾等安全事故，给社会生产生活带来巨大不便，一直是一个棘手的问题。因此，电力部门每年都要消耗大量的人力和物力进行树障清理，然而，完全以传统的人工作业模式来承担电网的运维工作，所需人员甚多，当前电力部门相关工作人员实际数量远低于需求。且在复杂的地形条件下，人工作业人员需要承担极大的安全风险，比如山区和大面积湖区输电线路的运维，在恶劣天气环境(寒冬、酷暑等)以及在夜间的运维工作。在这些恶劣环境下人工承担运维工作任务，工作难度大大提高，由此而带来的所需成本也成倍增加。因此，电力部门亟需一种自动化工具代替人工作业来克服上述困难，以确保电网系统更加长久可靠地运行，以此来保障工业生产和日常生活中电能的长期稳定供给。

2、树障清理无人机带来了一种全新的工作方式，改变了长期以来完全依靠人工清障的作业模式，可以做到精准切割树障，提高了作业安全性和作业效率，避免了人工作业带来的人身伤亡事故风险，同时也带来对物种及植物的保护。在无人机物理交互的应用领域，传统无人机所注重的位置和姿态控制方式已经不能满足多旋翼与环境进行物理交互接触作业的应用需求，在与物理环境对象的交互过程中，多旋翼与交互对象需要进行直接的物理交互接触，接触过程两者之间会有交互力的产生，如何控制这个交互力也是无人机的一个挑战，交互力控制不当会对无人机产生损伤。刚性无人机具有响应速度快，刚度高和动态性能良好等特点，非常符合工业机器人高位置精度的使用需求。但刚性无人机也带来了低适应性和低安全性等缺点。对于刚性无人机来说，其追求的高刚度使得无人机在接触作业碰撞的时候会产生非常大的冲击力，对无人机本体会产生较大的失控风险。

3、在位置姿态控制方法上，虽然柔性悬挂给飞行器作业时带来了缓冲和减振，但是由于柔性悬挂的弹性会产生周期摆动、非刚性带来的参数摄动、通道耦合干扰，这对无人机的控制器抗扰动的性能要求更高。传统的pid结构简单，但是pid参数的调整需要依赖经验并且对扰动抑制能力有限，很难到达较好的效果，lqr控制精度较高，能实现最优指标，但是抗扰性不足。自抗扰控制器(adrc)主要由跟踪微分器、非线性状态误差反馈控制率以及状态扩张观测器三部分构成，adrc控制方法由于扩张状态观测器对耦合项的观测量直接线性补偿到状态误差反馈控制率输出中，因此抗扰能力还有待加强。传统滑模控制(slidingmode contral，smc)采用线性滑模表面。当系统的状态达到滑模表面时，系统的响应对系统内部参数的不确定性和外部干扰不敏感，这使得smc控制器具有良好的鲁棒性。然而，传统smc的缺点是系统状态收敛到平衡点是渐近的，不能达到有限时间收敛、控制结构简单，但是存在抖振，对干扰的抑制能力也较弱。近年来，终端滑模控制(terminal sliding modecontral，tsmc)在实际应用中表现出了一些优异的性能，如系统状态误差可以在有限时间内收敛到零，且相比于线性滑模面，其控制器增益可大为减小，并且具有比传统滑模控制更高的稳态精度。然而，当一般的tsmc方法接近系统的平衡状态时，控制器的输出将出现奇异。

技术实现思路

1、发明目的：本发明的目的是提供一种可以实现良好控制能力和接触作业控制能力的柔性悬挂刀锯的树障清理空中机器人飞行控制方法，本发明的另一个目的是提供一种该控制方法所使用的装置。

2、技术方案：本发明的一种柔性悬挂刀锯的树障清理空中机器人飞行控制方法，包括以下步骤：

3、s1、根据柔性悬挂刀锯的树障清理空中机器人的柔性悬挂方式，确定柔性悬挂刀锯的树障清理空中机器人结构；

4、s2、根据柔性悬挂刀锯的树障清理空中机器人结构，推导空中机器人的柔性悬挂的弹簧-阻尼系数；

5、s3、根据柔性悬挂刀锯的树障清理空中机器人结构，及空中机器人的柔性悬挂的弹簧-阻尼系数，推导出柔性悬挂刀锯的树障清理空中机器人拉格朗日动力学模型；

6、s4、根据柔性悬挂刀锯的树障清理空中机器人拉格朗日动力学模型，推导出接触作业时的接触作业数学模型；

7、s5、根据柔性悬挂刀锯的树障清理空中机器人拉格朗日动力学模型，设计位姿非奇异终端滑模自抗扰飞行控制器，位姿非奇异终端滑模自抗扰飞行控制器将空中机器人的拉格朗日动力学模型解耦成为六个通道分别进行控制。

8、进一步的，空中机器人包括机体、柔性悬挂和刀锯，步骤s1中柔性悬挂的方式为：

9、(a)机腹悬挂，悬挂刀锯通过弹簧-阻尼器和空中机器人的机腹柔性连接；

10、(b)对称安装，柔性结构由四个对称斜置安装的弹簧-阻尼器组成，第i个柔性悬挂结构相对于机体坐标系的三轴姿态角为：其中，φf为柔性悬挂结构的滚转角，ψf为柔性弹簧-阻尼器的航向角，i为柔性弹簧-阻尼器的序号。

11、进一步的，步骤s2中柔性悬挂结构的三维弹性系数ckb在机体坐标系和三维阻尼系数cξb在机体坐标系计算为：

12、

13、其中，ck是单个柔性悬挂结构的弹性系数，cξ是单个柔性悬挂结构的阻尼系数，t为转置；

14、柔性悬挂结构的弹性系数ck计算方法为：

15、

16、其中，gk是弹簧的刚度系数，dk是弹簧的线径，nk是弹簧的圈数，dk是弹簧的外径；

17、柔性悬挂结构的阻尼系数cξ计算方法为：

18、

19、其中，wξ是阻尼器能够承受的最大外部冲击力，vξ是阻尼器最大的缓冲速度。

20、进一步的，步骤s3中柔性悬挂刀锯的树障清理空中机器人的拉格朗日动力学方程为：

21、

22、其中，是系统状态量，为的导数，为拉格朗日函数，e为总动能，u为系统的总势能，d是耗散函数，γ是外部作用力；是机体在世界坐标系的位置，是刀锯在世界坐标系的位置，是机体在世界坐标系的姿态角；

23、总动能e写为：

24、

25、其中，tb为世界坐标系欧拉角速度转换为机体角速度的矩阵，mb是机体的质量，ms是刀锯的质量，jb是机体的转动惯量，js是刀锯的转动惯量，为机体在世界坐标系的速度，为刀锯在世界坐标系的速度，为机体在世界坐标系的欧拉角速度，是刀锯在世界坐标系的欧拉角速度，i3为三维单位矩阵；

26、总势能u为：

27、

28、其中，g＝[00g]，g为重力加速度，δpke为柔性悬挂结构在世界坐标系的位移量，为机体坐标系相对于世界坐标系的旋转矩阵；

29、耗散函数为：

30、

31、其中，为在机体坐标系的刀锯距离柔性悬挂结构位置，为世界坐标系相对于机体坐标系的旋转矩阵；

32、拉格朗日函数写为：

33、

34、通过对q，求导可得：

35、

36、将上面结果带入拉格朗日方程：

37、

38、定义

39、

40、拉格朗日方程整理为：

41、

42、拉格朗日动力学模型写为：

43、

44、其中，是的二阶导数。

45、进一步的，步骤s4中空中机器人在工作时，切割扰动力fc为：

46、

47、其中，pc是切割时刀锯的功率，vc是切割的速度，α为切割的角度；

48、切割扰动力fc产生的切割扰动力矩τc为：

49、τc＝rf×fc

50、其中，切割点的位置为rc＝(rx,ry,0)t，rx为切割点距离刀锯坐标系的x轴距离，ry为切割点距离刀锯坐标系的y轴距离；

51、接触扰动力fθ与无人机的俯仰角θ有关，如果无人机在空中缓慢与树障接触，忽略与树障的碰撞，无人机和树障接触达到平衡时，接触扰动力fθ只与无人机的俯仰姿态与推力ft有关，可得：

52、

53、产生的接触力矩为：

54、τθ＝rc×fθ

55、综合以上两部分扰动，得到最终的作业扰动力和力矩为：

56、

57、其中，fext为作业扰动力，τext为作业扰动力矩。

58、进一步的，步骤s5中设计位姿非奇异终端滑模自抗扰飞行控制器ntsm-adrc，位姿非奇异终端滑模自抗扰飞行控制器将空中机器人的动力学模型解耦成为六个通道分别进行控制，解耦后的空中机器人六自由度模型表示为：

59、

60、其中，为0刀锯的树障清理空中机器人机体的x轴加速度，为机体的y轴加速度，为机体的z轴加速度，为机体的滚转角加速度，为机体的俯仰角加速度，为机体的航向角加速度，ux、uy、uz为机体的x、y、z轴位置控制量，uφ、uθ、uψ分别为机体的滚转、俯仰、航向通道控制量，jb,x、jb,y、jb,z分别为机体x、y、z轴的惯性张量，js,x、js,y、js,z分别为刀锯x、y、z轴的惯性张量，是机体x轴通道所受干扰项，是机体y轴通道所受干扰项，是机体z轴通道所受干扰项，是机体滚转通道所受干扰项，是机体俯仰通道所受干扰项，是机体航向通道所受干扰项；

61、设计扩张状态观测器，以实现对不确定性影响的估计与补偿；

62、分别对解耦后的六个通道设计相应的扩张状态观测器，估计出相应通道中扰动值，并引入相应的补偿，实现对扰动的抑制；

63、位姿控制转换的关系为：

64、

65、其中，uz为空中机器人的高度控制量，φd为空中机器人的期望滚转角，θd为空中机器人的期望俯仰角。

66、进一步的，针对x轴位置通道，设计的扩张状态观测器为：

67、

68、其中，为xi的观测状态值，为x轴位置的观测角速度，为x轴位置的观测角加速度，为x轴位置的观测扰动，e为x轴位置观测值和实际值的误差，ε>0，α1、α2和α3为正整数，多项式s3+α1s2+α2s+α3满足hurwitz条件；

69、进一步的，采用非奇异终端滑模控制实现x轴位置控制，定义误差变量：

70、

71、其中，ex为x轴位置的误差值；和分别为x轴位置误差的一阶导数和二阶导数值，xd为x轴位置通道的期望值，和分别为x轴位置通道期望值的一阶导数和二阶导数值，x1为x轴位置通道的实际角度，为x轴位置通道的实际角速度；

72、设计非奇异终端滑模面为：

73、

74、其中，βx>0，βx、px、qx为可调参数；sx为滑模面；

75、选取指数趋近率，非奇异终端滑模控制率设计为：

76、

77、其中，ωx为x轴位置通道的等速趋近系数，kx为x轴位置通道的指数趋近系数；

78、为了降低抖振，采用双曲正切函数代替sgn(·)

79、

80、其他通道与x轴位置通道计算同理；

81、因此，得到所有通道的控制律为：

82、

83、其中，分别为x、y、z轴位置通道期望值的二阶导，分别为滚转、俯仰、航向通道期望值的二阶导，分别为x、y、z轴位置通道期望值的内外部扰动，分别为滚转、俯仰、航向通道期望值的内外部扰动，βx、βy、βz、qx、qy、qz、px、py、pz是x、y、z轴位置通道滑模面的可调参数，βφ、βθ、βψ、qφ、qθ、qψ、pφ、pθ、pψ分别为滚转、俯仰、航向通道滑模面的可调参数，ω、ωy、ωz分别为x、y、z轴位置通道的等速趋近系数，ωφ、ωθ、ωψ分别为滚转、俯仰、航向通道的等速趋近系数，kx、ky、kz分别为x、y、z轴位置通道的指数趋近系数，kφ、kθ、kψ分别为滚转、俯仰、航向通道的指数趋近系数。

84、有益效果：与现有技术相比，本发明的显著优点为：(1)提出了一种新型的柔性悬挂刀锯的树障清理空中机器人构型，该无人机能够从树障上方切割树枝，具有更高的效率；由于刀锯和无人机设有柔性悬挂机构，能有效保证无人机旋翼与树障顶端的安全距离，缓冲接触作业时对无人机的冲击；(2)针对柔性悬挂刀锯的树障清理空中机器人非刚性的问题，对柔性悬挂的弹性-阻尼系统进行了建模，对整体进行了拉格朗日动力学建模，对控制器的研究提供了较好的参考意义；(3)针对柔性悬挂刀锯的树障清理空中机器人具有系统非刚性、参数不确定性、易受内外部扰动影响等特点，对柔性悬挂刀锯的树障清理空中机器人进行位姿非奇异终端滑模自抗扰控制器设计，不仅能克服柔性悬挂刀锯的树障清理空中机器人本身的系统非刚性、参数不确定性对控制器带来的影响，还能对飞行过程中机体受到的未知外部扰动起到良好的抑制作用，提高了空中机器人姿态控制器的鲁棒性和可靠性。