本发明涉及港口自动化，尤其是涉及一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法。
背景技术：
1、在自动化港口操作中，使用岸桥进行集装箱的装卸作业是一项高频、高难度且高度机械化的任务。主要目标是将集装箱快速且准确地移至目标位置，并尽可能减少摆动。目前，实际应用的吊车系统多依赖人工操作，但这种方式效率低下，减摆效果不佳，误操作率高，安全性低，事故率高。因此，随着自动控制技术的发展，自动减摆控制成为国内外研究的焦点之一，也是智能化港口的关键技术之一。
2、然而，吊车系统的控制自由度低于其控制对象，是典型的欠驱动系统，且易受外界干扰如摩擦力、风力等的影响。吊车系统的状态间存在强烈的耦合和显著的非线性特点，增加了设计减摆控制方法的难度。当前，已有多种轨迹规划方法有效利用欠驱动系统的动力学耦合效应调整运动状态，成功减轻摆动。但是，快速、平滑的岸桥全局轨迹规划及避障技术，目前尚缺乏足够的研究和关注。
技术实现思路
1、本发明的目的是为了提供一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，结合模型预测控制算法和minimum jerk方法，构建跨越长距离、由多个轨迹段组成的轨迹，将搜索与优化相结合，既保证了运动的平滑性，又充分满足了动态约束和性能指标的要求。
2、本发明的目的可以通过以下技术方案来实现：
3、一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，包括以下步骤：
4、s1，基于拉格朗日方程构建多绳岸桥吊载系统的动力学模型；
5、s2，采用线性时变模型预测控制算法生成无碰撞的轨迹，并在滚动优化过程中，利用量子粒子群算法求解非线性二次规划问题；
6、s3，利用minimum jerk方法将离散轨迹点拟合为平滑轨迹，并将约束条件融入代价函数，将非线性二次规划问题转化为无约束优化问题，以寻找闭式解；同时，在规划过程中，持续监测规划轨迹点与障碍物之间的距离，结合该距离并考虑安全余量，实现有效避障。
7、所述多绳岸桥吊载系统包括多个台车、主梁、钢丝绳、吊具和集装箱，其中，每一个台车均在主梁上进行水平方向的移动，并通过一根钢丝绳与吊具连接，通过吊具控制集装箱移动。
8、所述步骤s1中，对多绳岸桥吊载系统做如下假设：
9、1)将吊具和集装箱视为一个整体的有效载荷，假设有效载荷的质量分布均匀，当台车在主梁的轨道上运行时，有效载荷不会发生旋转；
10、2)忽略钢丝绳的柔性和弹性，将其视为质量忽略不计的刚体；假设同一侧悬挂的钢丝绳位于同一平面上并且均匀分布。
11、所述步骤s1具体为：基于拉格朗日方程构建多绳岸桥吊载系统的动力学模型，并将其表示为矩阵形式：
12、
13、其中：q＝[x,l,θ1]t表示系统状态变量，m(q)＝ntm1n表示惯性矩阵，表示科里奥利矩阵，g(q)＝ntg1表示重力矩阵，u＝ntu1表示驱动力矩阵，其具体形式如下：
14、
15、
16、其中，si＝sinθi，ci＝cosθi，si+j＝sin(θi+θj)，ci+j＝cos(θi+θj)，si-j＝sin(θi-θj)，ci-j＝cos(θi-θj)，i,j＝1,2,3，θ1，θ2是两侧钢丝绳与y轴方向所成的夹角，θ3是负载与y轴方向所成的夹角；mt是台车质量，mp是负载的质量，g是重力加速度,x是台车在x轴方向的位移，l是钢丝绳的长度；钢丝绳在台车上的间距为2a，钢丝绳在吊具上的间距为2c(a＞c)，b是负载重心与悬挂中心之间的距离；fx为台车驱动力，fl为钢丝绳拉力，分别为函数g(l,θ1)对θ1的一阶偏导和二阶偏导，分别为函数h(l,θ1)对θ1的一阶偏导和二阶偏导。
17、对于多绳岸桥吊载系统，θ1，θ2，θ3中，只有一个变量是独立的；为了建立它们之间的关系，假设θ2＝g(l,θ1)和θ3＝h(l,θ1)，对其进行微分，得到：为函数h(l,θ1)对l的一阶偏导，
18、所述步骤s2中，采用线性时变模型预测控制算法生成无碰撞的轨迹的过程如下：
19、假设目标状态量和输出量分别为ηg，yg，控制量为ug，在目标点，参考系统需满足以下条件：
20、
21、yg＝cηg
22、其中，c表示状态量与输出量之间的转换矩阵；
23、在目标点，对ηg进行泰勒展开，仅保留一阶项并忽略高阶项，得到：
24、
25、岸桥的线性误差模型定义如下：
26、
27、其中，a为f相对于η的雅可比矩阵，b为f相对于u的雅可比矩阵；
28、采用近似方法对岸桥的线性误差模型进行离散化：
29、ak＝ik+ta,bk＝tb,ck＝c
30、其中，t表示采样时间，i为单位矩阵；
31、得到离散线性误差模型：
32、
33、并将其表示为状态空间形式：
34、
35、其中，在竖线“|”之后的t表示当前时刻，k-1|t表示在当前t时刻下的k-1步状态量或控制量，由此得到以下新的状态空间表达式：
36、
37、其中，m为状态量维度，n为控制量维度；
38、未来的系统输出采用矩阵形式表示：
39、y(t)＝ψkξ(t|t)+θkδu(t)
40、y(t)＝[γ(t+1|t)t,…,γ(t+np|t)t]t
41、
42、δu(t)＝[δu(t|t)t,…,δu(t+nc|t)t]t
43、其中，np是预测时域，nc是控制时域；
44、系统的控制增量δu(t)未知的，为了实现系统对参考信号的快速且平稳的规划，定义优化目标函数如下：
45、
46、其中，ξd(k+i|t)代表上次迭代中得到的当前参考状态，ud(k+i-1|t)代表下次迭代将求解的参考控制信号，qp表示状态加权矩阵，rp表示控制加权矩阵；
47、同时，在规划阶段引入硬件约束：
48、ξmin(t+j)≤ξ(t+j)≤ξmax(t+j)
49、δξmin(t+j)≤δξ(t+j)≤δξmax(t+j)
50、其中，j＝0,1…nc-1。
51、所述步骤s2中，利用量子粒子群算法求解非线性二次规划问题具体为：
52、设定量子粒子群算法的目标函数：
53、
54、s.t.ξmin(t+j)≤ξ(t+j)≤ξmax(t+j)
55、δξmin(t+j)≤δξ(t+j)≤δξmax(t+j)
56、其中，g(t)＝[2e(t)tqpθk 0]，e(t)＝ξd(t|t)-ηg；
57、将h(t)和g(t)作为参数传递到量子粒子群优化算法，将量子粒子群算法的目标函数作为适应度函数，在每个控制周期结束后，获得一系列控制时域内的控制输入增量：
58、
59、其中，为t时刻的控制增量；
60、根据模型预测控制的基本原则，将控制序列中的首个元素作为实际的控制输入增量施加于系统，即
61、
62、系统持续执行该控制量直至下一个时间点，当到下一个时间点时，系统依据最新的状态信息，预测未来新的一段时间内的输出，并通过优化流程计算出一系列新的控制增量，直到控制任务完全完成。
63、所述步骤s3中，利用minimum jerk方法将离散轨迹点拟合为平滑轨迹具体为：采用通过n+1个离散点的n条五次多项式拟合离散轨迹：
64、
65、其中，n表示轨迹段数，pi＝[ki0,ki1,ki2,ki3,ki4,ki5]t，i∈[1,n]表示参数向量，j∈[0,5]表示多项式参数。
66、所述步骤s3中，代价函数定义如下：
67、
68、其中，q是hessian矩阵；选择最小加加速度项作为优化项，确保生成的轨迹平滑并避免推力不连续性；
69、约束条件设置如下：
70、
71、pi(ti)＝pi+1(ti)，
72、其中，pi(ti)表示第i段多项式在ti时刻的位置，pi+1(ti)表示第i+1段多项式在ti时刻的位置；
73、将含有等式约束的优化问题建模为二次规划问题，具体表现为：
74、min f(p)
75、s.t.aipi＝dii∈[1,n]
76、其中，矩阵ai编码了第i段多项式的等式约束，向量di包含第i段多项式两个端点的位置、速度、加速度信息；由于连续性约束，向量di中的终点信息与相邻向量di+1的起点信息重复；用一个映射矩阵来关联同一个变量的这两个实例：
77、d＝md′
78、其中，m6n×3(n+1)为映射矩阵；
79、消除重复变量之后，在向量d'中重新排列变量，即，将已知的变量上移，未知的变量下移，以便分别处理：
80、
81、其中，c为置换矩阵，df是已知变量向量，dp是未知变量向量。
82、所述步骤s3中，采用两阶段避障策略，其中，
83、轨迹规划阶段的避障策略通过在量子粒子群优化算法的目标函数中引入避障函数实现，其中，所述避障函数设置为：
84、
85、其中，jobs,i表示在采样时刻i的避障函数，sobs是惩罚函数的权重，其中，vx,vy分别表示水平和垂直方向的速度，(xi,yi)表示障碍物的左上角坐标，当前位置坐标为(xk,yk)，其中，xk＝x+lsinθ3和yk＝l，d代表安全距离，ε为预设正数；
86、轨迹生成阶段的避障策略为：当一个优化轨迹与障碍物相交时，分别设置其两个端点的坐标为(xk,yk)，(xk+1,yk+1)，在这两点间插入一个轨迹点，把原轨迹分为两段，每段分配相同的时间t/2，设置中间点的坐标为((xk+xk+1)/2,min(yk,yk+1))；基于码头起重机的工作模式，所述中间点必然无碰撞；利用中间点作为新的轨迹点重新优化轨迹。
87、与现有技术相比，本发明具有以下有益效果：
88、一、本发明利用模型预测控制设计轨迹规划器。模型预测控制在每个控制周期内通过选择最佳控制输入进行优化，以最小化状态误差并满足多种约束。这一动态优化过程确保系统在遵循硬件限制的同时，能够实现快速地轨迹规划。同时，该策略可以根据系统的即时反应在线调整，允许使用简化的模型，以减少计算负担并提高反应速度。
89、二、基于模型预测控制方法生成离散轨迹，采用minimum jerk方法设计轨迹生成器。当构建跨越长距离、由多段多项式组成的轨迹时，该方法能有效地与模型预测控制算法协同工作。minimum jerk方法通过将模型预测控制生成的离散轨迹点拟合为平滑轨迹，并将约束条件整合进代价函数中，将优化问题转化为无约束优化问题，并求解闭式解，以此提高计算效率。同时，基于等时间间隔采样的离散点能够在一定程度上规避minimum jerk方法遇到的主要难题：时间分配。该方法通过结合搜索与优化，不仅保证了运动的平滑性，也满足了动态约束和性能指标的要求。
90、三、在实际场景中，考虑到船舶甲板上可能存在大量堆叠的集装箱以及周围环境的复杂性，将避障能力融入轨迹规划过程变得至关重要。本发明把一种综合速度和惩罚函数的避障函数整合到模型预测控制的目标函数中，旨在提高避障性能。在优化轨迹后，如果发现特定轨迹段与障碍物相交，就在两个端点之间加入一个额外轨迹点，将该段一分为二。通过使用附加轨迹点重新优化多项式，并重复这一过程，直至得到一个安全无碰撞的轨迹。此策略不仅能在有限的优化迭代次数内找到解决方案，还降低了每次迭代的计算复杂度。
1.一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述多绳岸桥吊载系统包括多个台车、主梁、钢丝绳、吊具和集装箱，其中，每一个台车均在主梁上进行水平方向的移动，并通过一根钢丝绳与吊具连接，通过吊具控制集装箱移动。
3.根据权利要求2所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s1中，对多绳岸桥吊载系统做如下假设：
4.根据权利要求1所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s1具体为：基于拉格朗日方程构建多绳岸桥吊载系统的动力学模型，并将其表示为矩阵形式：
5.根据权利要求4所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，对于多绳岸桥吊载系统，θ1，θ2，θ3中，只有一个变量是独立的；为了建立它们之间的关系，假设θ2＝g(l,θ1)和θ3＝h(l,θ1)，对其进行微分，得到：为函数h(l,θ1)对l的一阶偏导，
6.根据权利要求1所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s2中，采用线性时变模型预测控制算法生成无碰撞的轨迹的过程如下：
7.根据权利要求6所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s2中，利用量子粒子群算法求解非线性二次规划问题具体为：
8.根据权利要求7所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s3中，利用minimum jerk方法将离散轨迹点拟合为平滑轨迹具体为：采用通过n+1个离散点的n条五次多项式拟合离散轨迹：
9.根据权利要求8所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s3中，代价函数定义如下：
10.根据权利要求7所述的一种融合模型预测控制的岸桥动态避障及平滑轨迹规划方法，其特征在于，所述步骤s3中，采用两阶段避障策略，其中，
