1.一种用于欠驱动起重机系统的安全滤波器，其特征在于，所述滤波器通过时变约束方法对起重机系统进行层级实时控制；其中：所述安全滤波器包括动力分析模块、状态约束模块、时变截断分析模块、高阶控制障碍函数计算模块、分层二次规划计算模块和状态测量模块；

所述动力分析模块根据欧拉-拉格朗日方程构建起重机动力数学模型，即：





其中：表示起重机系统的广义位形坐标,为广义速度，是输入矢量且是输入矩阵，其中m≤n且Bn(q)是零矩阵，而Bc(q)是一个m阶可逆矩阵；

所述状态约束模块根据时变约束函数构建起重机系统的吊载时变约束条件：





其中：sn(qn,t)用来描述非并置坐标qn上的时变约束，sc(qc,t)用来描述并置坐标qc上的时变约束；

所述时变截断分析模块通过如下公式对起重机系统的吊载时变约束条件修正构建起重机系统的第二吊载时变约束条件：



其中：χ(τ):＝(min{τ-1,0})3+1，恒正标量函数δj(·)源自于所考虑时变约束的性质；

根据所述起重机动力数学模型和所述起重机系统的第二吊载时变约束条件，构建高阶控制障碍函数计算模块；

所述分层二次规划模块通过获取状态测量模块中负载信息按照二次规划算法向起重机系统输出层级安全控制信号。

2.根据权利要求1所述的一种用于欠驱动起重机系统的安全滤波器，其特征在于，根据所述起重机动力数学模型和所述第二起重机系统的吊载时变约束条件构建高阶控制障碍函数计算模块：





其中：αj,i>0为控制增益，









其中：矩阵和

3.根据权利要求1所述的一种用于欠驱动起重机系统的安全滤波器，其特征在于，所述分层二次规划模块通过获取状态测量模块中负载与起重机信息按照二次规划算法向起重机系统输出层级安全控制信号，所述层级安全控制信号为：



其中：和u0为标称控制律，ρ≥0,αj,i>0,i∈{1,2},j∈{n,c}和正定矩阵是用户可调节的控制增益。

技术领域

[0001]本发明属于非线性欠驱动机电系统自动控制的技术领域，尤其涉及一种用于欠驱动起重机系统的安全滤波器。

背景技术

[0002]起重机系统是一种广泛存在的欠驱动系统，被应用于生产生活的各个领域，如汽车生产车间、钢厂、港口、轮船上等。各式起重机，例如桥式起重机、吊杆式起重机、飞行吊运系统等，在组成上，它们具有统一特点，通常由可移动平台、吊绳、负载三个部分组成；且在数学上，均可由欠驱动欧拉-拉格朗日方程描述。因负载通过吊绳悬挂于各式可移动平台上，由于系统本身具有的欠驱动与耦合特性，可移动平台在运行过程中会引起负载的往复摆动。在起重机的欧拉-拉格朗日方程描述中，可移动平台以及吊绳长度的运动自由度被统称为并置(或可驱动的)位形，而描述负载摆动的角度量被称为非并置的(或无驱动的)。国内外的研究者针对各类起重机系统提出了许多方法用于解决最短路径搜索、最优轨迹生成等运动规划问题，镇定、跟踪、鲁棒自适应等控制问题。

[0003]在现有流行的轨迹规划与跟踪控制框架中，防止起重机系统与环境碰撞、约束摆角等位形约束的处理往往通过离线轨迹规划的方式解决。通过模型预测控制方法等进行重规划的方式在自动驾驶等领域可以获得良好的性能表现，但对于装有计算能力有限的工控机或微型处理器的起重机系统来说，因过于计算量庞大而难以实时运行，且因需要持续求解数值优化问题，运行效果并不稳定。因此，针对起重机系统发展能够实时运行、轻量化计算的在线轨迹规划算法或反馈控制算法，可提升起重机系统在时变环境中运行的自主性、鲁棒性、智能化水平显得尤为关键。如何从控制层面，通过解析构造的形式设计状态反馈控制律，实现以起重机系统为代表的一类欠驱动系统的时变位形约束显得十分关键，具有重要意义。

[0004]基于控制障碍函数-二次规划算法已被公认是一种适用于非线性系统、可高计算效率地处理复杂约束的方法，这种基于控制障碍函数的状态反馈控制律设计方法被特别称为安全性滤波器。目前该方法已被逐渐扩展用于高阶非线性系统(称为高阶控制障碍函数)，并于近期用于欠驱欧拉-拉格朗日系统的约束控制。现有结果通常关注于时不变约束，而没有进一步考虑时变位形约束问题，但考虑该问题在实际问题是必要的，特别是各类工业起重机系统执行动态避障或暂态性能预设等时变约束任务的情形。而且，它们都没有进一步验证基于二次规划算法所设计控制律的局部李普希兹连续性质，无法排除控制律存在切换的风险，因此对执行器存在潜在伤害。综上所述，采用控制障碍函数-二次规划技术设计具有李普希兹连续性质的安全滤波器实现欠驱动起重机实现时变非并置与并置约束，仍是亟需解决的问题。

发明内容

[0005]针对现有技术存在的问题，本发明提供一种用于欠驱动起重机系统的安全滤波器，该滤波器不仅能够保证欠驱动起重机系统中的非并置约束(例如摆角的预设暂态性能约束)得到优先满足，还能尽可能地实现并置约束(例如负载的动态避障)。

[0006]本发明的目的是通过以下技术方案实现：

[0007]一种用于欠驱动起重机系统的安全滤波器，所述滤波器通过时变约束方法对起重机系统进行实时控制；其中：所述安全滤波器包括动力分析模块、状态约束模块、时变截断分析模块、高阶控制障碍函数计算模块、分层二次规划计算模块和状态测量模块；

[0008]所述动力分析模块根据欧拉-拉格朗日方程构建起重机动力数学模型，即：

[0009]

[0010]

[0011]其中：表示起重机系统的广义位形坐标，后缀"n"和"c"分别表示非并置坐标(即起重机系统中的负载摆动的摆角)的与并置坐标(即起重机系统中的可移动平台的运动坐标和绳长的平动坐标)；为广义速度；是输入矢量且是输入矩阵，其中m≤n且Bn(q)是零矩阵，而Bc(q)是一个m阶可逆矩阵。

[0012]所述状态约束模块根据时变约束函数构建起重机系统的吊载时变约束条件：

[0013]

[0014]

[0015]其中：sn(qn,t)用来描述非并置坐标qn上的时变约束，sc(qc,t)用来描述并置坐标qc上的时变约束；

[0016]所述时变截断分析模块通过如下公式对起重机系统的吊载时变约束条件修正构建第二起重机系统的吊载时变约束条件：

[0017]

[0018]其中：χ(τ):＝(min{τ-1,0})3+1，恒正标量函数δj(·)源自于所考虑时变约束的性质；

[0019]进一步地，根据所述起重机动力数学模型和所述起重机系统的第二吊载时变约束条件，构建高阶控制障碍函数计算模块：

[0020]

[0021]

[0022]其中：αj,i>0为控制增益，

[0023]

[0024]

[0025]

[0026]

[0027]矩阵和

[0028]所述起重机系统分层二次规划模块通过获取状态测量模块中负载与起重机状态信息按照二次规划算法向起重机系统输出安全控制信号；所述安全控制信号为：

[0029]

[0030]其中：和u0为标称控制律，实现分层的惩罚因子ρ≥0和正定矩阵是用户可调节的控制增益。

[0031]本发明的有益效果

[0032]与现有技术相比，本发明所提出的可处理时变约束的安全滤波器方法的有益效果为：1、本发明可以采用半自动运行的，可用于保证人工操作时仍是安全的；

[0033]2、本发明在二次性能指标意义下是最优的，且控制律不存在切换，具有李普希兹连续性；3、本发明是可泛化的，由于在安全滤波器设计中采用的是矩阵分块表示形式因而与其具体动力学特性无关，可适用于广泛存在的起重机系统；

[0034]4、本发明能够实现负载摆角的暂态性能约束，以及地面动态障碍物的避障，有望被进一步应用于实际货物运输任务中，具有十分重要的现实意义。

附图说明

[0035]构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

[0036]图1为本发明的欠驱动起重机系统结构及其工作环境中的典型时变约束的示意图。

[0037]图2为本发明安全滤波器的系统组成框图。

[0038]图3为本发明所提方法与PID方法在位形变量θ,φ,l方面的实验效果对比图；

[0039]图4为本发明所提方法与PID方法在控制输入uφ,ul方面的实验效果对比图；

[0040]图5为本发明所提方法与PID方法在性能指标sn,sc方面的实验效果对比图；

具体实施方式

[0041]本实施例公开了一种欠驱动起重机可处理时变约束的安全滤波器方法,下面结合附图对本发明作进一步的描述。本发明所针对的欠驱动起重机系统如图1所示。具体实现过程是：

[0042]基于欠驱动起重机系统动力学模型与欠驱动起重机所受状态约束的函数模型，引入一种时变截断函数修正时变约束函数以克服时变项与奇异点集的影响，然后基于高阶控制障碍函数-二次规划算法构建安全滤波器，保证状态约束完成，整体系统框图如图2所示，即：所述滤波器通过时变约束方法对起重机系统进行实时控制；所述安全滤波器包括动力分析模块、状态约束模块、时变截断分析模块、高阶控制障碍函数计算模块、分层二次规划计算模块和状态测量模块；具体包括以下步骤：

[0043]第一步：构建欠驱动起重机系统动力学模型与欠驱动起重机所受状态约束的函数模型。

[0044]针对的起重机动力学方程系统由可移动平台、吊绳、负载三个部分组成，其中可移动平台类型包括但不局限于桅杆式、桥式、飞行器吊运等方式。具体的，考虑起重机动力学系统表示为如下欧拉-拉格朗日方程：

[0045]

[0046]其中：表示起重机系统的广义位形坐标,为广义速度，分别表示惯性矩阵、科里奥利矩阵，是重力向量。另外，是输入矢量且是输入矩阵，其中m≤n且Bn(q)是零矩阵，而Bc(q)是一个m阶可逆矩阵。利用矩阵B(q)的分块表达式，可对广义坐标以及位形空间进行如下划分：q:＝col(qn,qc),其中且后缀"n"和"c"分别表示非并置坐标(即起重机系统中的负载摆动的摆角)的与并置坐标(即起重机系统中的可移动平台的运动坐标和绳长的平动坐标)。起重机系统(1)可等价表为如下分块形式：

[0047]

[0048]其中在本实施例中，所考虑的是桅杆式起重机，当然，在其他实施例中，起重机可以是其他类型，例如桥式起重机、飞行吊运系统等。对于桅杆式起重机，系统位形变量分别为负载的摆角(rad)，吊杆的俯仰角(rad)和绳长(m)，并且控制输入分别表示φ关节上的转动力矩(N·m)和l关节上的平动力(N)。桅杆式起重机系统的动力学表达式为

[0049]

[0050]

[0051]其中：和G2:＝(mpPL+mbd)gCφ。为简便起见，对任意变量ζsinζ,cosζ被分别简写为Sζ,Cζ，取ζ＝θ,φ,φ-θ等。

[0052]欠驱动起重机所受时变约束类型是广泛存在的、允许存在特定奇异点集，至少包括常见的预设暂态性能约束、动态避障等时变约束任务。具体地，非并置与并置时状态约束集合的定义为

[0053]

[0054]其中：函数sj(·),j∈{n,c}满足如下性质：存在标量函数δj>0,和集合使得这里采用了如下的符号记法：对任意一个时变函数在任何时间t≥0，引入水平集和其中δ(t)可以是一个时间相关函数或常量；此外，称为奇异点集，表示所有梯度消失点的集合。

[0055]第二步：基于步骤一状态约束解析表达式，引入一种时变截断函数修正时变约束函数以克服时变项与奇异点集的影响。具体地，考虑到时变奇异点集引入时变光滑截断函数hj(·)修正时变约束函数sj(qj,t)：

[0056]

[0057]其中：χ(τ):＝(min{τ-1,0})3+1，恒正标量函数δj(·)源自于所考虑时变约束的性质。易知，hj各偏导表达式为

[0058]

[0059]

[0060]和

[0061]

[0062]

[0063]

[0064]其中：τj＝sj(qj,t)/δj(t)，

[0065]第三步：基于步骤一中的欠驱动起重机系统动力学模型、步骤二中修正后的时变约束函数的表达式，构建时变高阶控制障碍函数。具体地，定义

[0066]

[0067]以及相应集合其中αj,i>0为控制增益。为计算ψj,i的表达式，首先，分块形式可等价写为

[0068]

[0069]其中：分块矩阵都是可逆矩阵(由于M(q)是正定矩阵)。将(6)代入(2)中，连同扩张状态p的方程可得到开环系统表达式为：

[0070]

[0071]其中:都行满秩。此外，矩阵是对称正定的。求ψn,0,ψc,0沿(10)的时间导数，可以得到

[0072]

[0073]

[0074]其中

[0075]

[0076]

[0077]

[0078]

[0079]步骤四：根据状态的测量值，结合步骤三中高阶控制障碍函数，使用二次规划算法构建安全滤波器，生成起重机实际控制输入信号。具体地，

[0080]在高阶控制障碍函数-二次规划控制框架下，设计能够“分层实现”并置与非并置时变位形约束的安全滤波器。为保证可行性并出于使控制律简洁的考虑，提出如下分层安全滤波器设计：

[0081]

[0082]其中:u0可以取为任意有界李普希兹连续的标称控制律，ρ≥0,αj,i>0,i∈{1,2},j∈{n,c}和正定矩阵是用户可调节的控制增益。

[0083]在该控制律中，非并置约束被认为是优化算法的“硬约束”的而并置约束是“软约束”的。还有，增益ρ＝0表示不考虑次要约束，ρ越大代表则“次要约束”越容易满足。(8)的等价解析表达式为

[0084]

[0085]其中和由(9)可知，本专利所提的分层安全滤波器是具有李普希兹连续性的。

[0086]实验结果

[0087]为验证本发明所设计控制器的有效性，可按上述步骤，在自主搭建的桅杆式起重机平台上进行测试。该系统主要系统参数如下：杆的质心到旋转轴O、顶端到O的距离分别为d＝0.05(m),PL＝0.70(m)，吊杆的转动惯量、质量和负载质量分别为

[0088]J＝0.40(kg·m2),mb＝5.00(kg),mp＝0.50(kg)，以及重力加速度g＝9.8(m/s2)。

[0089]接下里，引入桅杆起重机工作时面临的一个多时变约束场景：

[0090]约束一：负载摆角θ满足暂态性能约束：

[0091]

[0092]其中并且参数特别选取为和因为特别选择作为sn(θ,t)的截断值。

[0093]约束二：负载运动最低点不与地面目标发生碰撞：

[0094]sc(qc,t):＝PL sinφ-l-zsafe(t)≥0,

[0095]其中zsafe(t)表示预设的安全裕度，本发明具体设定为

[0096]

[0097]并且参数为zh＝-0.2(m),za＝0.4(m),zT＝10(s),zp＝20(s)。注意到这意味着偏导始终不消失，因此可选择任意正常数作为sc(θ,t)的截断值，在本文特别选择

[0098]为测试所提分层安全滤波器处理时变约束的能力，标称输入选择为常见的PID控制律：

[0099]

[0100]其中表示并置位形误差，而φd,ld分别表示期望的设定的值(具体依下文不同实验选择而定)。值得指出，该控制律中没有引入摆角θ的反馈，且θ的期望值θd选择为自然平衡点，即θd＝0。PID增益矩阵为Kp＝[20,0；0,4],经过充分仿真，所提安全滤波器的参数选择为Q＝[1,0；0,1],ρ＝0.005,αn,1＝αn,2＝10.

[0101]具体地，给定初始点与期望平衡点分别为其余初始状态值设置为t0＝0,在该设置下，图3描述了位形变量与控制输入随时间变化关系，控制输入uφ,ul的响应在图4中给出，衡量安全性的指标函数sn(t),sc(t)的响应见图4。在这些图中，PID控制律与PID+SF控制律的响应分别给出用来进行对比。

[0102]从图中可以看到，仅使用PID控制律时，一方面，由θ(t),sn(t)在[0,7](s)时间内的响应可知，摆角θ(t)无法满足约束一，因此消摆性能没有预先保证；另一方面，在t＝7s以后，PID控制律对时变地面目标没有反应且对t∈[9,11](s)都有sc(t)<0，这意味着约束二没有满足，即负载与地面目标会发生碰撞。

[0103]作为对比，当使用PID+SF时，由图5可以看到sn(t),sc(t)都保持大于零，这意味着上述两个时变约束都得到满足。对t∈[1,5](s)，|θ(t)|若干次变大并靠近边界θppc，SF中的项被激活，使得uφ,ul得以调整，进而使得|θ(t)|不超过边界θppc，且在t＝5(s)以后始终远离边界，这些结果说明了良好的快速消摆与抑制残余摆动的能力。另一方面，在t＝7(s)以后，随着zsafe(t)增大，sc(t)逐渐减小靠近零，SF中的项“+ρac”被激活，ul得到改变使得l(t)减小，从而保持sc(t)恒正，即约束二得到满足，负载不与地面时变目标发生碰撞。

[0104]综上所述，本发明提出的安全滤波器方法是局部李普希兹连续的，能够保证非并置约束(即此处的约束一)始终得到满足，这与理论上对闭环系统的前向不变性分析是一致的；并置约束(即此处的约束二)虽然没有理论上保证，但在实验中能够实现。