一种无杆牵引构型下舰载机甲板调运轨迹规划方法

本发明属于舰载武器装备保障，涉及一种无杆牵引构型下舰载机甲板调运轨迹规划方法。

背景技术：

1、传统甲板调运轨迹依赖于人工进行规划。当使用牵引车对舰载机进行牵引时，由牵引车与舰载机构成的牵引系统本质上为一多体系统，在复杂的甲板环境下，人工方法难以可靠地高效生成调运路径。近年来，基于优化的方法由于其对各类障碍的统一描述与处理能力，在轨迹规划领域获得了广泛的关注。然而甲板通常密集分布着各类障碍(如设备、人员、舰岛等)，且障碍通常不具备规则外形。若使用传统的“1对1”避障描述列式将导致两方面难点：一方面，难以实现对不规则外形障碍的描述；另一方面，由于非线性运动方程的存在导致问题已经具有较高的非凸性，当进一步考虑了较多的障碍时，则难以保证计算收敛性与计算效率。为此，面向优化框架下无杆牵引构型下舰载机甲板调运轨迹规划问题的可靠求解，有必要发展一种高效的避障约束施加方法。

技术实现思路

1、为解决上述技术问题，本发明提出一种无杆牵引构型下舰载机甲板调运轨迹规划方法。本发明通过对牵引系统进行运动学建模并进行相关约束分析，从而在最优控制问题框架下求解调运轨迹。为实现高效调运过程中对复杂避障条件的高效处理，引入通道思想，将传统“1对1”的避障描述方法转化为“限制在通道内运动”的描述方法。具体地，首先根据所研究牵引系统的运动特性，构造一种改进的混合a*算法，并采用该算法生成一条粗糙的运动路径。基于该路径，一方面，进行通道的构造以简化避障约束的列式；另一方面，通过重采样实现决策变量的高质量初始化以提高求解速度与鲁棒性。

2、为达到上述目的，本发明采用的技术方案为：

3、一种无杆牵引构型下舰载机甲板调运轨迹规划方法，第一，根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题；第二，根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张；第三，根据设定的调运需求，在前述形成的栅格地图上，利用改进的混合a*算法生成一条粗糙调运路径；第四，结合牵引系统机械约束，根据时间最优原则，估算前述粗糙调运路径所需的转运时间；第五，根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙路径进行重采样；第六，根据重采样信息进行通道构建，对前述构造的最优控制问题中的避障条件进行转化；第七，根据通道约束构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹。

4、步骤1：根据无杆牵引系统运动学方程、机械约束以及调运需求，初步构建最优控制问题

5、步骤1-1：建立无杆牵引系统运动学方程

6、考虑如图2中所示的无杆牵引系统。其中，mt(xt,yt)和ma(xa,ya)分别为牵引车和舰载机的后轴中心点，h为舰载机与牵引车的铰接点。lt和la分别为牵引车和舰载机的轴距，lh为点mt与铰接点h间的距离。θt和θa分别为牵引车和舰载机的朝向角。φt和φa分别为牵引车和舰载机的前轮转向角，则舰载机与牵引车之间夹角为φa＝θt-θa。记牵引车前轮转向角对应的角速度为ωt。记牵引车与舰载机的速度分别表示为vt与va，由运动学关系，得到二者关系为根据上述变量及参数定义，牵引系统的运动学方程可以写作如下微分方程形式

7、

8、在公式(1)中，xa、ya、θa、θt、vt和φt为系统的状态变量，将其记为s，即s＝[xa,ya,θa,θt,vt,φt]t；at和ωt为系统的控制变量，将其记为c，即c＝[at,ωt]t。方程中表示变量(·)对于时间的导数。采用动态方程f表示上述运动方程，则公式(1)可表示成如下形式

9、

10、同时，考虑到实际机械约束，对系统中部分状态/控制变量施加如下箱型约束条件

11、

12、其中(·)min和(·)max分别表示变量(·)的下限和上限。

13、步骤1-2：将调运需求描述为边界条件

14、调运位置及姿态需求对应了最优控制问题中的边界条件。记调运过程起始时间为t＝0，记调运过程结束时间为tf(tf为需要优化的决策变量)。调运起始端和结束端的边界条件可以分别表示为

15、

16、

17、其中，(·)0和(·)f分别表示变量(·)在起始时刻和终端时刻的构型约束。

18、步骤1-3：避障约束描述

19、在调运过程中舰载机需要满足避障条件，将整个甲板所表示区域记为γ，将障碍物区域记为γobs，则在甲板的可行区域γfree可以表示为γfree＝γ\γobs，定义g为牵引系统的从状态空间到构型空间的映射函数，则避碰约束可以描述为

20、

21、步骤1-4：建立目标函数

22、建立目标函数j如下

23、

24、其中p和q分别为状态变量和控制变量进行惩罚的对角矩阵，w为权重系数。通过调节p、q以及w来调节相应指标在目标函数中的权重，来保证满足实际调度中的耗时、调运速度、调运稳定性等需求。

25、步骤1-5：构建最优控制问题，并设定数值求解中离散方式

26、通过前面的描述，能够获得无杆牵引系统在生成调运轨迹规划问题描述为如下的最优控制问题

27、

28、最优控制问题数值求解过程中，需要将时间域进行离散，并在节点上施加相关约束条件。不妨考虑将时间域划分为nocp个小的时间间隔，对应于(nocp+1)个离散节点。时间节点满足如下关系，0＝ρ0＜ρ1＜ρ2＜...＜ρk＜...＜ρocp＝1，其中tk＝ρktf表示优化后第k个时间节点对应的实际时间。

29、步骤2：根据甲板环境建立栅格地图，利用双特征圆对牵引系统进行表示，并对栅格地图中障碍栅格进行相应扩张

30、步骤2-1：栅格地图的构建

31、为了方便对环境进行信息表示，构建甲板对应的栅格地图。设定栅格地图的分辨率为dres，则每个栅格表示dres×dres区域内的信息。若栅格内包含障碍物，则将该栅格视作障碍物栅格；否则，是为自由栅格。对于一个横坐标上下限分别为xmax和xmin，纵坐标的上下限分别为ymax和ymin的区域，可获得一个由imax×jmax个栅格构成的栅格地图，其中x方向栅格数目为imax＝ceil(xmax-xmin)/dres，y方向栅格数目为jmax＝ceil(xmax-xmin)/dres，ceil表示向上取整。甲板地图如图3(a)所示，转换后的栅格地图如图3(b)所示。

32、步骤2-2：舰载机的双特征圆表示及扩张的栅格地图的构建

33、首先根据图4中舰载机长方形轮廓的长度l以及宽度w设置特征圆的半径为根据特征圆半径r及栅格地图分辨率dres，将栅格地图中障碍物向外扩张的栅格数设置为ne＝(ceil(r)/dres)+λ，其中λ为额外添加的一个安全余量。最终获得如图3(c)所示的扩张后的栅格地图。此时可以将系统的两个特征圆缩聚至对应特征圆的圆心，从而简化路径获取过程中对系统轮廓的表示。若按照图4所示将两个特征圆圆的圆心分别表示为maf(xaf,yaf)和mar(xar,yar)，lr表示mar至机尾距离，可以分别得到两个圆心的坐标位置为

34、{maf(xaf,yaf)|xaf＝xa+(3l/4-lr)cosθa,yaf＝ya+(3l/4-lr)sinθa}  (9)

35、{mar(xar,yar)|xar＝xa+(l/4-lr)cosθa,yaf＝ya+(l/4-lr)sinθa}  (10)

36、步骤3：根据设定的调运需求，在前述形成的栅格地图上，利用改进的混合a*算法生成一条粗糙调运路径

37、步骤3-1：设定混合a*算法中的避障方式

38、在本专利中若无特别说明，使用(·)h表示实施混合a*算法过程中涉及的对应变量(·)。

39、根据步骤2-2中介绍的栅格地图获取方式，将栅格地图中的障碍物栅格记为γoe，根据公式(9)和(10)，只要通过混合a*算法获得的舰载机位置对应的特征圆圆心满足且就可以保证混合a*算法获得的路径满足碰撞避免需求。

40、为防止发生jackknife现象，在无杆牵引系统的构型下还需要保证φa不能够超过其上下限，因此在混合a*算法中将或视作违约行为，同样判定为发生“碰撞”，以保证成功搜索出符合运动学约束的路径。

41、步骤3-2：设定混合a*算法中的节点扩展方式

42、混合a*算法中，设定牵引车扩展过程中的控制量为图5中的(vtmax,φtmax)，(vtmax,0)，(vtmax,-φtmax)三种向前扩展模式以及(vtmin,φtmax)，(vtmin,0)，(vtmin,-φtmax)三种向后扩展模式。若使用(·)c和(·)p区分混合a*算法中一次扩展结束时获得的当前节点及其父节点相关变量，则可以根据父节点位置和姿态信息以及扩展模式根据运动学公式(1)采用数值积分的方式获得当前节点位置和姿态信息

43、步骤3-3：设定代价函数的形式

44、混合a*算法的设计中，需要设定代价函数fa来引导扩展方向，设置fa＝gac+hac。gac为搜索至当前节点的代价，将其设置为其中μv、μd、μs分别为对应于行驶距离、变向操作以及转向操作进行相应惩罚的参数。hac为混合a*算法中的启发式函数，表示当前点位置至终点的预估代价，将其设置为其中lrs表示不考虑碰撞情况下当前节点至终点的reed-shepp曲线长度，l2dha表示只考虑二维空间中的位置信息时利用a*算法所获得的当前节点至终点的路径长度。

45、步骤3-4：混合a*算法的终止条件与粗路径的获取

46、为保证混合a*算法的搜索效率同时保证舰载机终端位置约束精度，将混合a*算法的扩展终止条件设置为只考虑舰载机位置和朝向角信息落入考虑角度分辨率为dθ的三维空间的目标网格。若使用nh表示混合a*算法所获得的除起点外的节点数，通过实施混合a*算法最终可以获得nh+1个包含无杆牵引系统的位置和姿态信息，记为若其中任意第m个节点的位置和姿态信息使用表示，则4元组δm可以具体表示为

47、步骤4：结合牵引系统机械约束，根据时间最优原则，估算前述粗糙调运路径所需的转运时间

48、首先为δh中任意第k(k＝0,1,…,nh-1)个节点δk定义变量则当dk(k＞0)与dk-1符号相反时系统的行驶方向发生变化，此时将δk视为一个尖端节点。将尖端节点的数目记为nc，使用cuspj(0＜cuspj＜nh)记录第j(j＝1,2,…nc)个尖端节点在δh中的位置。定义列表其元素定义为

49、

50、由此，可以定义δh中第n(n＝1,2,...,nc+1)个片段为δseg,n，其由共(segn-segn-1+1)个节点组成。同时，根据通过舰载机位置信息以及几何关系获得的牵引车位置信息定义片段长度为当对应的时将第n个片段路径的行驶方向记为dirn＝1表示该片段路径向前行驶，当时记dirn＝-1表示该片段路径向后行驶。设定当dirn＝1时，a＝|atmin|；当dirn＝-1时，a＝|atmax|，并引入限制变量以及llim＝llim1+llim2。此时可以估计调运过程在任意第n个片段的运行时间为

51、

52、基于此，可以获得对终端时间tf的初步估计ts为

53、

54、步骤5：根据运动学方程及前述估算的转运时间，在离散点上对前述粗糙路径进行重采样

55、本专利中若无特别说明，使用(·)s表示重采样过程中涉及的对应变量(·)。使用(l＝0,1,…,nocp)表示通过重采样获得的对应于第l个采样时刻的相关变量(·),l。

56、根据2.3描述的求解方式，需要提供(nocp+1)个节点的初始解，对任意第l个采样点的预估时间为tl＝tsρl，根据{tseg,n}判断其所属的片段编号，由此可由插值计算得到假设在相邻的两个a*节点内，舰载机与牵引车行驶距离之比等于两者速度之比，并依据此假设通过插值点牵引车位置获得相应插值点舰载位置

57、此外通过向前差分的方式表示并根据运动学关系以及φt的上下限获得

58、

59、对再次进行向前差分并根据ωt上下限获得

60、

61、通过重采样可以获得nocp+1个带有无杆牵引系统的状态与控制信息的八元组信息将其记为

62、步骤6：根据重采样信息进行通道构建，对前述构造的最优控制问题中的避障条件进行转化

63、对任意第l个采样时刻位置(xaf,l,yaf,l)通过重采样预估为并根据公式(9)和(10)获得对应预估的特征圆的圆心对应于不同圆心位置构建通道的方式相似，因此只以(xaf,l,yaf,l)为例，讲解通道构建过程。首先将对应预估圆心点看作一个0宽0长的矩形并将信息记录为即且每次以相同步长δ按照上、左、下、右的顺序连续向四个方向扩张，若是新扩张区域(如图6所示)不包含障碍物栅格，则视为扩张成功并更新通道信息若是新扩张区域包含障碍物栅格，则视为扩张失败不记录此次扩张信息，并不再向这个方向进行扩张。直至四个方向均扩张失败获得最终的通道信息。此外为了避免无限扩张设置了一个扩张上限lmax，当任意方向扩张距离超过lmax时，将此次扩张视为失败。分别对nocp+1个离散时间节点重复上述通道构建过程可以获得nocp+1个如下约束

64、

65、步骤7：构建新的最优控制问题，并进行求解，生成牵引系统的最优控制量及相应轨迹根据步骤6获得的通道约束可以将最优控制问题重新描述为

66、

67、最后根据获得的高质量初始解ξs与ts，对最优控制问题进行求解，生成牵引系统的最优控制量及相应轨迹。

68、本发明的有益效果为：

69、本发明通过改进混合a*算法将牵引车的控制变量映射为舰载机的状态变量，同时只约束无杆牵引系统中舰载机终端位置，保证高效的为最优控制问题提供一个高质量同伦轨迹，并通过对同伦轨迹重采样获得一个高质量的初始解。根据重采样点通过构建安全通道的方式降低了面对无杆牵引系统调运任务最终构建的最优控制问题中避障约束的数量，使其只与采样点数目有关。结合高质量初解与新的避障约束，显著提高了构建的最优控制问题的求解效率，保证高效的生成一条严格满足舰载机终端位置要求的路径。