基于操纵杆位移信号的槽道式水下无人航行器控位方法与流程

本发明涉及非电变量的控制，尤其涉及基于操纵杆位移信号的槽道式水下无人航行器控位方法。

背景技术：

1、人工控位模式下，为实现水下无人航行器的水平面和垂直面控位功能，岸上操纵人员需根据情况采用相应的岸基设备对水下无人航行器的槽道式推进器进行操纵，操纵杆作为航海领域最常见的操纵设备之一，其可通过x轴和y轴双轴运动产生对应的位移量信号，进而操纵船舶的舵或推进器等执行机构。

2、为进行垂向控位和横向控位的功能，槽道式水下无人航行器需要配置艏艉两个侧向槽道式推进器和艏艉两个垂向槽道式推进器，通过驱动艏艉两个侧向槽道式推进器可产生使水下无人航行器左右平移的侧向力和使水下无人航行器偏航的转艏力矩，通过驱动艏艉两个垂向槽道式推进器可产生使水下无人航行器垂直移动的垂向力和使水下无人航行器纵摇的纵倾力矩。但对于前后槽道式推进器布置的水下无人航行器，由于操纵杆位移信号无法直观有效地给定各辅助推进器的指令转速，因此需要设计专门的算法对人工控位模式下的操纵杆信号进行处理，驱动水下无人航行器的槽道式辅助推进器，使得岸上操纵人员更为便利地对水下无人航行器进行人工控位。

技术实现思路

1、本发明所要解决的技术问题是提供基于操纵杆位移信号的槽道式水下无人航行器控位方法，有效地将操纵杆位移信号和槽道式水下无人航行器各辅助推进器转速进行匹配，为人工操纵槽道式水下无人航行器进行位置控制提供了便利。

2、本发明是通过以下技术方案予以实现：

3、基于操纵杆位移信号的槽道式水下无人航行器控位方法，其包括如下步骤：

4、s1：确定操纵杆的配置及功能，将各操纵杆的位移量与实现槽道式水下无人航行器控位的垂向力、纵倾力矩、侧向力及转艏力矩一一对应；

5、s2：遵循优先控制航向或纵倾的原则根据槽道式推进器推力公式计算槽道式水下无人航行器控位的垂向力、纵倾力矩、侧向力及转艏力矩，并确定槽道式水下无人航行器控位的垂向力最大值、纵倾力矩最大值、侧向力最大值及转艏力矩最大值；

6、s3：根据槽道式水下无人航行器控位的垂向力最大值及纵倾力矩最大值解算指令垂向力及指令纵倾力矩，根据槽道式水下无人航行器控位的侧向力最大值及转艏力矩最大值解算指令侧向力及指令转艏力矩；

7、s4：根据指令垂向力、指令纵倾力矩、指令侧向力及指令转艏力矩解算各辅助推进器指令转速；

8、s5：根据解算的各辅助推进器指令转速驱动槽道式推进器动作，实现槽道式水下无人航行器的人工控位。

9、优化的，步骤s1中操纵杆的配置为左操纵杆和右操纵杆，功能划分为：纵向操纵左操纵杆形成左杆纵向位移量，使槽道式水下无人航行器产生纵摇运动的纵倾力矩，纵向操纵右操纵杆形成右杆纵向位移量，使槽道式水下无人航行器产生垂直移动的垂向力，横向操纵左操纵杆形成左杆横向位移量，使槽道式水下无人航行器产生偏航的转艏力矩，横向操纵右操纵杆形成右杆横向位移量，使槽道式水下无人航行器产生左右平移的侧向力。

10、进一步，步骤s2中采用如下方法确定槽道式水下无人航行器控位的垂向力最大值、纵倾力矩最大值、侧向力最大值及转艏力矩最大值：

11、s21：根据艏艉辅助推进器形成的侧向力和转艏力矩公式式（1）计算槽道式水下无人航行器控位的侧向力及转艏力矩：

12、（1）；

13、其中：为艏侧推推力，为艉侧推推力，为艏侧推力臂，为艉侧推力臂，为槽道式水下无人航行器控位的侧向力，为槽道式水下无人航行器控位的转艏力矩；

14、s22：根据辅助推进器推力公式式（2）计算辅助推进器最大推力：

15、（2）；

16、其中：为二次项拟合系数，为一次项拟合系数，为辅助推进器最大转速；

17、s23：根据式（3）计算转艏力矩最大值：

18、（3）；

19、s24：根据式（4）计算槽道式水下无人航行器控位的侧向力最大值：

20、（4）；

21、s25：根据艏艉推进器形成的垂向力和纵倾力矩公式式（5）计算槽道式水下无人航行器控位的垂向力及纵倾力矩：

22、（5）；

23、其中：为艏垂推推力，为艉垂推推力，为艏垂推力臂，为艉垂推力臂，为槽道式水下无人航行器控位的垂向力，为槽道式水下无人航行器控位的纵倾力矩；

24、s26：根据式（6）计算纵倾力矩最大值：

25、（6）；

26、s27：根据式（7）计算槽道式水下无人航行器控位的垂向力最大值：

27、（7）。

28、进一步，步骤s3中按照如下方法解算指令侧向力、指令转艏力矩、指令垂向力及指令纵倾力矩：

29、s31：根据式（8）确定左操纵杆有效纵向位移量，根据式（9）确定右操纵杆有效纵向位移量，根据式（10）确定左操纵杆有效横向位移量，根据式（11）确定右操纵杆有效横向位移量：

30、（8）；

31、（9）；

32、（10）；

33、（11）；

34、其中：表示左操纵杆有效纵向位移量，表示右操纵杆有效纵向位移量，表示左操纵杆有效横向位移量，表示右操纵杆有效横向位移量，表示左操纵杆纵向位移量死区参数，表示右操纵杆纵向位移量死区参数，表示左操纵杆横向位移量死区参数，表示右操纵杆横向位移量死区参数，表示左操纵杆初始纵向位移量，表示右操纵杆初始纵向位移量，表示左操纵杆初始横向位移量，表示右操纵杆初始横向位移量；

35、s32：根据式（12）计算左操纵杆纵向位移量比例值，根据式（13）计算右操纵杆纵向位移量比例值，根据式（14）计算左操纵杆横向位移量比例值，根据式（15）计算右操纵杆横向位移量比例值：

36、（12）；

37、（13）；

38、（14）；

39、（15）；

40、其中：表示左操纵杆纵向位移量比例值，表示左操纵杆纵向位移量最大值，表示右操纵杆纵向位移量比例值，表示右操纵杆纵向位移量最大值，表示左操纵杆横向位移量比例值，表示左操纵杆横向位移量最大值，表示右操纵杆横向位移量比例值，表示右操纵杆横向位移量最大值；

41、s33：根据式（16）计算原始指令纵倾力矩，根据式（17）计算原始指令垂向力，根据式（18）计算原始指令转艏力矩，根据式（19）计算原始指令侧向力：

42、（16）；

43、（17）；

44、（18）；

45、（19）。

46、进一步，步骤s4中按照如下方法解算各辅助推进器指令转速：

47、s41：将解算的原始指令侧向力与侧向力最大值进行比较并按式（20）进行处理，得到处理后指令侧向力，将解算的原始指令转艏力矩与转艏力矩最大值进行比较并按式（21）进行处理，得到处理后指令转艏力矩，将解算的原始指令垂向力与垂向力最大值进行比较并按式（22）进行处理，得到处理后指令垂向力，将解算的原始指令纵倾力矩与纵倾力矩最大值进行比较并按式（23）进行处理，得到处理后指令纵倾力矩：

48、（20）；

49、（21）；

50、（22）；

51、（23）；

52、其中：表示符号函数；

53、s42：根据式（24）反向推算指令艏艉侧向推力，根据式（25）反向推算指令艏艉垂向推力：

54、（24）；

55、（25）；

56、其中：为指令艏侧向推力，为指令艉侧向推力，为指令艏垂向推力，为指令艉垂向推力；

57、s43：根据式（26）解算各辅助推进器指令转速：

58、（26）；

59、其中：为第个辅助推进器指令转速，为第个辅助推进器的指令推力。

60、发明的有益效果：

61、本发明提供的基于操纵杆位移信号的槽道式水下无人航行器控位方法，采用左右双操纵杆配置，将左右操纵杆位移信号与控制水下无人航行器动力定位的侧向力、转艏力矩、垂向力和纵倾力矩一一对应，进一步通过死区限制、优先控制水下无人航行器姿态，采用矩阵求逆法进行推力分配并采用最大推力限幅等方法，由操纵杆位移信号可直接推算出艏艉侧向及垂向辅助推进器的指令转速，大幅提升了岸上操纵人员对槽道式水下无人航行器进行动力定位人工操纵的便利性，保证了槽道式水下无人航行器的控位稳定性。