v-rep内提供简单的建模，但是复杂的则需要导入，简单的导入obj等格式这里就略过，这里说一下导入URDF模型，摘录：URDF（Universal Robot Description Format）——通用机器人描述格式，它是ROS等里边使用的一种机器人的描述文件，包含的内容有：连杆、关节，运动学和动力学参数、可视化模型、碰撞检测模型等。 我这里使用SolidWorks软件...

本发明属于塔吊防摇控制领域，针对突变风载、机械冲击载荷等引起的吊重大摆角非零初始状态的防摇控制问题，建立了“小车-吊重”非线性时变数学模型并采用pd(比例微分)控制律快速抑制吊重的摆角，可显著提高起吊作业效率和定位精度。

“小车-吊重”防摇控制是塔吊工程机械领域备受关注的一个问题，从动力学控制角度来看，这是一个典型的欠驱动点位控制问题。由于突变风载、机械冲击载荷多种因素的影响，在吊重(含吊具)到达指定位置后将不可避免地产生残余振荡；若振荡不能快速消除则直接影响到塔吊的起吊作业效率和定位精度，甚至产生生产安全事故。塔吊防摇技术可大致分为“机械式防摇”和“电子防摇”两种，其中“电子防摇”更具灵活性，具有较好的发展前景。

在“电子防摇“控制算法方面，国内外同行做了大量的理论研究和产品试制工作并取得了较好效果，譬如梯形图速度规划法，s曲线速度规划法，zvd输入整形法、lqr最优控制以及fuzzy模糊控制等方法。然而需要指出的是，上述控制算法多基于以下2点理论假设：

(1)吊重为零初始状态(初始摆角和角速度均为零)，忽略非零初始值引起的自由振荡。

(2)假设摆角小于10度，将非线性防摇控制系统简化为线性模型来处理，即假设等效质量，等效刚度，等效阻尼均为时不变系数。考虑到在塔吊作业现场经常会出现由于突变风载、振动引起的非零初始状态甚至可能出现摆角大于10°的恶劣工况。此时，如果单纯利用上述控制方法，由于防摇系统本身的阻尼系数较小，其非零初始状态引起的摇摆振荡将很难消除、防摇效果大打折扣。

针对当前技术非零初始状态引起的摇摆振荡将很难消除的难题，本发明在此提供了一种时变非线性小车-吊重防摇控制装置及防摇控制方法，实现快速、稳定地抑制大摆角非零初始值引起的振荡，具体方案如下：

一种时变非线性小车-吊重防摇控制装置，包括plc控制器、imu惯性测量传感器、升降电机光电编码器、小车驱动电机、机械传动系统；

所述imu惯性测量传感器安装在吊具上用于实时获取吊重的摆角θ，该imu惯性测量传感器含有卡尔曼滤波并与plc控制器相连；

升降电机光电编码器，与plc控制器相连，用于采集小车吊重的吊绳长度l；

小车驱动电机，与plc控制器相连，且小车驱动电机通过所述机械传动系统驱动小车前进或后退，小车运行速度v由小车驱动电机的光电编码器采集；

plc控制器根据小车运行速度v、吊重摆角θ建立大摆角时变非线性防摇系统数学模型，之后进行大摆角时变非线性防摇系统数值迭代求解，以得到大摆角防摇系统的时间响应曲线；

plc控制器以大摆角防摇系统的时间响应曲线为依据，通过改变输出脉冲的频率和方向来调节小车驱动电机的转速，并由机械传动系统带动小车前进和后退。

进一步的，plc控制器根据小车运行速度v、吊重摆角θ建立大摆角时变非线性防摇系统数学模型具体如下：

小车速度v和吊重摆角θ为广义坐标；以v为控制系统的输入，以θ为控制系统的输出建立运动学微分方程：

式(1)中，l、v、g0分别表示吊绳的长度，小车速度和重力加速度；

将大摆角θ的三角函数用泰勒级数展开转化为代数运算：

将式(2)、式(3)式带入式(1)，整理得：

根据比例微分pd反馈控制律，小车调节速度表达为

式(5)中，kp、kd分别为比例系统和微分系数；

将(5)式求导数并带入(4)式，整理得：

式中，θ0、ω0分别为零时刻的吊重摆角与角速度，m、b、k分别为防摇控制系统的等效质量、等效阻尼和等效刚度，

进一步的，大摆角时变非线性防摇系统数值迭代求解的步骤如下：

对于式(6)-式(7)所描述的时变非线性动力学微分方程进行数值计算得到pd控制的仿真曲线具体如下：

第一步，将(6)式分解为2个一阶微分方程：

第二步，由隆格-库塔方法得到吊重摆角θ与角速度ω随时间t变化的递推式:

式中，θn+1，ωn+1，θn，ωn分别表示第n+1时刻和第n时刻吊重的摆角和角速度，h为步长，令tn＝nh，上式中的2组迭代系数的表达式为：

进一步的，根据式(10)-式(12)的迭代算法利用c语言编程得到大摆角防摇系统的时间响应曲线。

同时本发明还提供了一种基于上述防摇控制装置的防摇控制方法，步骤如下：

s1、根据小车运行速度v、吊重摆角θ建立大摆角时变非线性防摇系统数学模型；

s2、进行大摆角时变非线性防摇系统数值迭代求解，得到大摆角防摇系统的时间响应曲线；

s3、以防摇系统的时间响应曲线为依据，plc控制器通过改变输出脉冲的频率和方向来调节小车驱动电机的转速，并通过机械传动系统带动小车前进和后退。

进一步的，建立大摆角时变非线性防摇系统数学模型具体如下：

小车速度v和吊重摆角θ为广义坐标；以v为控制系统的输入，以θ为控制系统的输出建立运动学微分方程：

式(1)中，l、v、g0分别表示吊绳的长度，小车速度和重力加速度；

将大摆角θ的三角函数用泰勒级数展开转化为代数运算：

将式(2)、式(3)式带入式(1)，整理得：

根据比例微分pd反馈控制律，小车调节速度表达为

式(5)中，kp、kd分别为比例系统和微分系数；

将(5)式求导数并带入(4)式，整理得：

式中，θ0、ω0分别为零时刻的吊重摆角与角速度，m、b、k分别为防摇控制系统的等效质量、等效阻尼和等效刚度，

进一步的，大摆角时变非线性防摇系统数值迭代求解的步骤如下：

对于式(6)-式(7)所描述的时变非线性动力学微分方程进行数值计算得到pd控制的仿真曲线具体如下：

第一步，将(6)式分解为2个一阶微分方程：

第二步，由隆格-库塔方法得到吊重摆角θ与角速度ω随时间t变化的递推式:

式中，θn+1，ωn+1，θn，ωn分别表示第n+1时刻和第n时刻吊重的摆角和角速度，h为步长，令tn＝nh，上式中的2组迭代系数的表达式为：

进一步的，根据式(10)-式(12)的迭代算法利用c语言编程得到大摆角防摇系统的时间响应曲线。

(1)充分考虑非零初始状态和大摆角的边界条件，建立更贴近实际工况的“小车—吊重”时变非线性防摇数学模型；并给出时变非线性防摇数学模型的数值求解方法。

(2)将高精度(分辨率0.01度)含有卡尔曼滤波的imu(惯性测量单元)传感器安装在吊具上，用来实时感知吊重摆角的变换。与传统的通过光学测量吊重摆角的方法相比，imu惯性测量方法成本更低，安装更便捷。

(3)利用plc控制器构建闭环反馈控制系统，针对时变非线性系统控制对稳定性要求高的特点，采用pd(比例微分)控制律实时调节小车运行速度达到模拟有经验塔吊司机“跟车操作”的效果，实现快速、稳定地抑制大摆角非零初始值引起的振荡。

(4)利用上述方法将吊重的大摆角抑制在可接收范围之后(理想状态是角度与角速度均为零，具体数值按照不同塔吊的防摇指标确定)，即可平滑切换到梯形图速度规划法，s曲线速度规划法以及zvd输入整形等控制律。

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种时变非线性小车-吊重防摇控制装置的框图；

图2为大摆角防摇控制系统时间响应曲线；

图3为吊重大摆角防摇相轨迹图。

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。

为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。

如图1所示，小车通过缆绳起吊重物，在小车加减速惯性运动和风载的影响下，吊重摆角θ必然发生振荡。如果不采用合适的防摇控制方法将很难在起吊作业结束后让摆角θ快速归零，并直接影响到定位精度和作业效率。为此，提出以下闭环控制措施：

(1)安装在吊具上的imu惯性测量传感器实时获取吊重的摆角θ的变化，通过can总线或者zigbee无线传输的方式传递给plc控制器构成闭环反馈控制系统；与传统的通过光学测量吊重摆角的方法相比，imu惯性测量方法成本更低，安装更便捷。

(2)为搭建完整的大摆角plc防摇控制闭环控制系统，小车运行速度v由小车驱动电机(伺服电机)的光电编码器采集，吊绳长度l由升降电机光电编码器采集。

(3)plc控制器通过改变输出脉冲的频率和方向来调节伺服电机的转速，机械传动系统带动小车前进和后退。防摇控制最终目的：通过调节小车的运行速度v来快速抑制突变风载和机械冲击引起的非零大摆角振荡，使得吊重振幅a(a＝lθ)在指定的时间内收敛于规定值。

本发明进行防摇控制的原理如下：

一、大摆角时变非线性防摇系统数学模型建立

如图1所示，以小车速度v和吊重摆角θ为广义坐标；以v为控制系统的输入，以θ为控制系统的输出建立运动学微分方程：

式中，l，v，g0,分别表示吊绳的长度，小车速度和重力加速度。如果不考虑非零初始条件，采用传统的左右对称的梯形速度规划，s形速度规划或者zvd整形速度规划均可以在理论上保证在点位控制末端，吊重的摆角能在一个摆动周期内归零。但在室外塔吊的实际作业过程中，由于机械振动，风载等影响，吊重的初始摆角和角速度一般不为零。为此，将大摆角θ的三角函数用泰勒级数展开转化为代数运算，以便于后续plc或单片机控制器编程。

将(2)，(3)式带入(1)式，整理得：

根据比例微分pd反馈控制律，小车调节速度可表达为

式中，kp，kd分别为比例系统和微分系数。其物理解释：当吊重摆角θ＜0即吊重落在小车后方时，小车减速；当吊重摆角θ＞0时，吊重超前小车垂线位置则小车加速。由此可见pd控制律实际上是对有经验的塔吊司机“跟车操作”的电子模拟。比例系数kp越大，则调整幅度越大也越容易引起系统振荡，此时可通过适当增加微分系数kd来保证“跟车操作”的平顺性和稳定性。

将(5)式求导数并带入(4)式，整理得：

式中，θ0，ω0分别为零时刻的吊重摆角与角速度，在本发明中可做非零初始条件处理。m,b，k则分别为防摇控制系统的等效质量、等效阻尼和等效刚度：

(1)比例系数kp的引入相等于添加了系统阻尼，使得系统可以做阻尼衰减并趋向收敛稳定，而不是长时间的等幅振荡；微分系数kd的引入相当于增加了系统的等效质量，使得摆动频率减小，有利于吊重摆角振荡性减弱。

(2)等效质量m，等效阻尼b和等效刚度k均为摆角θ的函数，而θ又是一个随时间变化的变量。因此整个防摇摆动系统实际上是一个时变、非线性动力学系统。

二、大摆角时变非线性防摇系统数值迭代求解方法

对于(6)-(7)式所描述的时变非线性动力学微分方程，理论上不存在精确的解析解。因此，有必要对其进行数值计算得到pd控制的仿真曲线，以便为后续plc或者ipc编程控制提供理论参考。下面给出其数值计算步骤：

(1)第一步：将(6)式分解为2个一阶微分方程：

其物理解释：吊重角加速度可通过吊重摆角θ和角速度ω的代数运算得到。

(2)为了便于计算机编程迭代，由隆格-库塔(runge-kutta)方法得到吊重摆角θ与角速度ω随时间t变化的递推公式:

式中，θn+1，ωn+1，θn，ωn分别表示第n+1时刻和第n时刻吊重的摆角和角速度。h为步长(即迭代间隔时间)，令tn＝nh，上式中的2组迭代系数的表达式为：

根据式(10)-(12)的迭代算法即可利用c语言编程得到大摆角防摇系统的时间响应曲线。

下面给出一算例：缆绳l＝15m，在风载作用下吊重初始角度θ＝30°，角速度ω＝2°/s。显然在没有进行防摇控制之前，负载将做等幅振荡；现在进行pd控制，比例系统kp＝10，微分系数kd＝2，系统响应时间历程如图2所示：细线表示小车的运行速度v随时间t的变化(x轴为时间t，y轴为摆角θ)，粗线表示吊重的摆角θ随时间变化。可见：上升时间约为5s，峰值时间约为7s，最大超调量约为2.5度，调整时间约为10s。改变kp、kd系数，防摇控制时间响应曲线会随之改变，以便适应不同的塔吊防摇控制指标的要求。

图3是大摆角防摇控制系统的相轨迹曲线，从中可清楚地看出吊重从大摆角非零状态向理想零状态的收敛过程是稳定可控，这为后续plc编程中控制参数的确定提供了理论依据。

综上，本发明建立了大摆角非零初始状态“小车-吊重”的数学模型并给出了具体的数值迭代求解方法。充分考虑了系统的时变非线性的物理本质，这为plc控制器编程提供了动力学参考。结论可概括如下：

(1)大摆角非零初始状态的防摇控制系统其等效质量m，等效阻尼b和等效刚度k均为摆角θ的函数，而θ又是一个随时间变化的变量。因此整个防摇摆动系统实际上是一个时变、非线性动力学系统。

(2)针对于时变非线性防摇系统而言，pd控制策略类似模拟有经验司机的“跟车操作”，具有较好的鲁棒性。可以保证非线性时变大摆角防摇控制系统的稳定性。比例系数kp的引入相等于添加了系统阻尼，使得系统可以做阻尼衰减并趋向收敛稳定，而不是长时间的等幅振荡；微分系数kd的引入相当于增加了系统的等效质量，使得振荡频率减小，有利于系统稳定性的提高。

(3)利用本发明提出的方法将吊重的大摆角非零初始状态引起的自由振荡抑制在可接收范围之后(理想状态是角度与角速度均为零，具体数值按照不同塔吊的防摇指标确定)，即可平滑切换到梯形图速度规划法，s曲线速度规划法以及zvd输入整形等控制律。

以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。