一种Delta并联机器人轨迹控制方法及系统与流程

本发明涉及机器人轨迹控制
技术领域：
，特别是涉及一种delta并联机器人轨迹控制方法及系统。
背景技术：
：并联机器人具有运动速度快、机械机构重量轻、柔性强等优点，与串联机器人互补。目前并联机器人尚未广泛应用在于其并联机构设计难、运动学求解难、轨迹规划复杂、轨迹跟踪不易控制等问题。delta并联机器人的三个关节相互耦合，控制对象具有非线性特性，其控制一直为研究的难点。控制策略的好坏，直接影响轨迹跟踪的质量，影响并联机器人的速速和精度。delta并联机器人的控制可以分为两类控制，一类为运动学控制，忽略机器人的向心力、科氏力和各种扰动，直接通过轨迹规划给定的旋转角度，控制伺服电机旋转相应的角度。主要应用在控制要求较低的低速运行的并联机器人中。另一类为动力学控制，在高速运动下，如果忽略机器人向心力、科氏力和各种扰动，将造成机器人精度下降，机器关节抖振等问题，因此设计好动力学控制方法，对提高机器人系统的动态响应性，控制高速运动下的各种力及干扰，实现机器人高速运动下的控制精度有着重要的意义。目前应用到并联机器人中的控制策略有pid控制、计算力矩控制和滑模变结构控制等。这些方法对控制对象的模型要求较高，且操作条件和运行环境是确定的。其中经典pid控制具有良好的鲁棒性和可靠性，并易于实现，在单输入、单输出应用场合占据着重要地位。但是delta并联机器人输出三个关节相互耦合，控制为非线性特性，因此pid控制难以保证高精度、高速轨迹跟踪且鲁棒性较差。虽然也提出了一些改进方法，但是都过多地依赖模型，存在计算复杂，不易实现，不能满足高速并联机器人高精度、高速控制的要求。技术实现要素：本发明的目的是提供一种delta并联机器人轨迹控制方法及系统，以提高delta并联机器人轨迹跟踪时的鲁棒性。为实现上述目的，本发明提供了如下方案：一种delta并联机器人轨迹控制方法，包括：获取delta并联机器人上一时刻主动臂电机的控制电压与输出角度；设计线性扩张状态观测器；所述线性扩张状态观测器的输入为所述上一时刻主动臂电机的控制电压与输出角度，输出为当前时刻主动臂的计算角度和总扰动；根据目标角度和所述计算角度得到所述线性扩张状态观测器的线性控制率；根据所述总扰动和所述线性控制率得到当前时刻主动臂电机的控制电压；根据所述当前时刻主动臂电机的控制电压控制所述delta并联机器人主动臂的运动轨迹。可选地，所述线性扩张状态观测器为：其中，e为当前时刻主动臂的计算角度和上一时刻主动臂的输出角度的偏差，z1为当前时刻主动臂的计算角度，y为上一时刻主动臂的输出角度，为z1的导数，z2为当前时刻主动臂的计算角度的微分，f1(e)＝z1-y，是z2的导数，z3为总扰动，f2(e)＝z2-y，b0为不确定控制增益的近似值，u′为上一时刻主动臂电机的控制电压，其中uo为线性控制率，是z3的导数，f3(e)＝z3-y，β1、β2和β3为线性扩张状态观测器参数，[β1β2β3]t＝l，其中t为转置，l为观测器增益。可选地，所述线性控制率的计算公式为：其中，kp为比例系数，γ为目标角度，kd为微分系数，为γ的导数。可选地，所述当前时刻主动臂电机的控制电压的计算公式为：其中，u为当前时刻主动臂电机的控制电压，s为复频域中的时间。一种delta并联机器人轨迹控制系统，包括：获取模块，用于获取delta并联机器人上一时刻主动臂电机的控制电压与输出角度；设计模块，用于设计线性扩张状态观测器；所述线性扩张状态观测器的输入为所述上一时刻主动臂电机的控制电压与输出角度，输出为当前时刻主动臂的计算角度和总扰动；线性控制率获取模块，用于根据目标角度和所述计算角度得到所述线性扩张状态观测器的线性控制率；控制电压获取模块，用于根据所述总扰动和所述线性控制率得到当前时刻主动臂电机的控制电压；轨迹控制模块，用于根据所述当前时刻主动臂电机的控制电压控制所述delta并联机器人主动臂的运动轨迹。可选地，所述线性扩张状态观测器为：其中，e为当前时刻主动臂的计算角度和上一时刻主动臂的输出角度的偏差，z1为当前时刻主动臂的计算角度，y为上一时刻主动臂的输出角度，为z1的导数，z2为当前时刻主动臂的计算角度的微分，f1(e)＝z1-y，是z2的导数，z3为总扰动，f2(e)＝z2-y，b0为不确定控制增益的近似值，u′为上一时刻主动臂电机的控制电压，其中uo为线性控制率，是z3的导数，f3(e)＝z3-y，β1、β2和β3为线性扩张状态观测器参数，[β1β2β3]t＝l，其中t为转置，l为观测器增益。可选地，所述线性控制率的计算公式为：其中，kp为比例系数，γ为目标角度，kd为微分系数，为γ的导数。可选地，所述当前时刻主动臂电机的控制电压的计算公式为：其中，u为当前时刻主动臂电机的控制电压，s为复频域中的时间。根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明公开了一种delta并联机器人轨迹控制方法，包括：获取delta并联机器人上一时刻主动臂电机的控制电压与输出角度；设计线性扩张状态观测器；所述线性扩张状态观测器的输入为所述上一时刻主动臂电机的控制电压与输出角度，输出为当前时刻主动臂的计算角度和总扰动；根据目标角度和所述计算角度得到所述线性扩张状态观测器的线性控制率；根据所述总扰动和所述线性控制率得到当前时刻主动臂电机的控制电压；根据所述当前时刻主动臂电机的控制电压控制所述delta并联机器人主动臂的运动轨迹。本发明设计了线性扩张状态观测器，即将线性自抗扰控制策略应用到delta并联机器人动力学控制中，提高了delta并联机器人轨迹跟踪时的鲁棒性。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1为本发明实施例1提供的delta并联机器人轨迹控制方法流程图；图2为本发明实施例1提供的delta并联机器人的结构简图；图3为本发明实施例1提供的delta并联机器人的控制结构图；图4为本发明实施例1提供的基于线性自抗扰控制的delta并联机器人控制结构图；图5为本发明实施例1提供的单闭环结构图；图6为本发明实施例1提供的线性自抗扰控制策略下的单臂控制器结构图；图7为本发明实施例1提供的线性自抗扰控制策略下的控制器结构总图；图8为本发明实施例1提供的实验一的结果图；图9为本发明实施例1提供的实验二的扰动输入波形图；图10为为本发明实施例1提供的实验二的给定轨迹与跟踪轨迹对比图；图11为本发明实施例1提供的实验三的扰动输入波形图；图12为本发明实施例1提供的实验三的给定轨迹与跟踪轨迹对比图；图13为本发明实施例1提供的实验三的给定轨迹与跟踪轨迹对比图的侧视图。具体实施方式下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。本发明的目的是提供一种机器人轨迹控制方法及系统，以提高机器人轨迹跟踪时的鲁棒性。为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。实施例1图1为本发明实施例1提供的delta并联机器人轨迹控制方法流程图，如图1所示，方法包括：步骤101：获取delta并联机器人上一时刻主动臂电机的控制电压与输出角度。步骤102：设计线性扩张状态观测器；所述线性扩张状态观测器的输入为所述上一时刻主动臂电机的控制电压与输出角度，输出为当前时刻主动臂的计算角度和总扰动。步骤103：根据目标角度和所述计算角度得到所述线性扩张状态观测器的线性控制率。步骤104：根据所述总扰动和所述线性控制率得到当前时刻主动臂电机的控制电压。步骤105：根据所述当前时刻主动臂电机的控制电压控制所述delta并联机器人主动臂的运动轨迹。在本实施例中，线性扩张状态观测器为：其中，e为当前时刻主动臂的计算角度和上一时刻主动臂的输出角度的偏差，z1为当前时刻主动臂的计算角度，y为上一时刻主动臂的输出角度，为z1的导数，z2为当前时刻主动臂的计算角度的微分，f1(e)＝z1-y，是z2的导数，z3为总扰动，f2(e)＝z2-y，b0为不确定控制增益的近似值，u′为上一时刻主动臂电机的控制电压，其中uo为线性控制率，是z3的导数，f3(e)＝z3-y，β1、β2和β3为线性扩张状态观测器参数，[β1β2β3]t＝l，其中t为转置，l为观测器增益。在本实施例中，线性控制率的计算公式为：其中，kp为比例系数，γ为目标角度，kd为微分系数，为γ的导数。在本实施例中，当前时刻主动臂电机的控制电压的计算公式为：其中，u为当前时刻主动臂电机的控制电压，s为复频域中的时间。以下对本发明的原理进行说明：图2为本发明实施例1提供的delta并联机器人的结构简图，如图2所示，delta并联机器人由动平台(即正三角形δb1b2b3)、静平台(即正三角形δa1a2a3)、三个主动臂a1c1、a2c2、a3c3及三个从动臂c1b1、c2b2、c3b3组成。a1、a2和a3为旋转关节，b1、b2、b3、c1、c2和c3为球关节。o为静平台的中心，p为动平台的中心，r为静平台的外接圆半径，r为动平台的外接圆半径。delta并联机器人机械结构较复杂，三个关节之间相互耦合，图3为本发明实施例1提供的delta并联机器人的控制结构图。如图3所示，g11(s)为从动臂1传递函数，g12(s)为从动臂1和从动臂2耦合传递函数，g13(s)为从动臂1和从动臂3耦合传递函数，g21(s)为从动臂2和从动臂1耦合传递函数，g22(s)为从动臂2传递函数，g23为从动臂2和从动臂3耦合传递函数，g31(s)为从动臂3和从动臂1耦合传递函数，g32(s)为从动臂3和从动臂2耦合传递函数，g33(s)为从动臂3传递函数。x1(s)、x2(s)和x3(s)为机器人的三个输入，y1(s)、y2(s)和y3(s)为机器人的输出，当任意一个输入xi(s)改变时，yi(s)均改变，且改变另2个输入xi(s)。所以要实现delta高速并联机器人轨迹跟踪控制，需要三个关节同时输入，且相互解耦控制。本发明采用的线性自抗扰控制策略(ladrc)具有强鲁棒性，因为在动力学建模时，省略了很多不定因素，在实际控制中，这些都是控制的不定因素。而线性自抗扰控制策略不依赖于动力学模型的建立，从而规避了并联机器人动力学模型的误差问题。其控制思想为单独对各关节进行控制，将各关节之间的耦合参数、建模时省略因素以及实际运行中的扰动都统一处理为干扰进行补偿，最终实现delta并联机器人高精度轨迹跟踪的控制。图4为本发明实施例1提供的基于线性自抗扰控制的delta并联机器人控制结构图。具体控制策略如下：获取delta并联机器人动力学方程：其中，m(θ)为delta并联机器人的惯性矩阵，m(θ)∈rmxn，rmxn为行数为m，列数为n的实数矩阵，θ为delta并联机器人主动臂的计算角度，θ∈rk，rk为行数为k的实数矩阵，为θ的二阶导数，为向心力与科氏力，为θ的一阶导数，g(θ)为重力，g(θ)∈rn，τ为伺服输入。在公式(4)中加入摩擦力和扰动，可得：其中，为摩擦力，τd为扰动。公式(5)进一步转换为：其中，m＝m(θ)。公式进一步转换为：其中，a2＝m-1g，为对象已知建模动态，为对象未知建模动态及外扰之总和，ω为角速度，t＝ωθ，b为不确定控制增益，u为系统输入，即当前时刻主动臂电机的控制电压，令x1＝θ，x3＝f(·)，则公式(7)所示二阶对象的状态方程为：其中，f(·)为自抗扰控制器里的非线性函数，b0为b的近似值。根据公式(8)和z1＝θ，z3＝f(·)设计线性扩张状态观测器(linearextendedstateobserver，leso)：其中，e为当前时刻主动臂的计算角度和上一时刻主动臂的输出角度的偏差，y为上一时刻主动臂的输出角度，f1(e)＝z1-y，f2(e)＝z2-y，b0为不确定控制增益的近似值，u′为上一时刻主动臂电机的控制电压，其中uo为线性控制率，f3(e)＝z3-y，β1、β2和β3为线性扩张状态观测器参数，[β1β2β3]t＝l，其中t为转置，l为观测器增益。线性控制率uo的计算公式为：其中，kp为比例系数，γ为目标角度，kd为微分系数，为γ的导数。由公式(9)可得：其中，u为当前时刻主动臂电机的控制电压，s为复频域中的时间。再根据公式(10)可得出：将根据公式(9)得出的z1、z2和z3代入公式(11)可得出最终的控制器：以下对本控制器的稳定性进行分析：图5为本发明实施例1提供的单闭环结构图，其中：g1(s)＝kp+kds(13)其中g1(s)、g2(s)、h(s)为中间参数，用于简化控制器，由图5可得控制器的闭环传递函数gb(s)为：定理1：当被控对象模型精确已知时，delta高速并联机器人使用本发明具有稳定性。线性自抗扰控制策略的微分跟踪器不影响系统稳定性，仅影响系统零点，合理选择(或预设)b0、kp、kd、β1、β2和β3可使系统稳定。证明如下：若被控对象模型精确已知，其传递函数(获取)为：将公式(13)、(14)、(15)和(17)代入公式(16)可得：公式(18)的闭环特征方程(即分母)为：令：d0＝b0，d1＝b0(β1+kd+a1)，d2＝b0(β2+kp+kdβ1+a1β1+a1kd+a2)d3＝[b0a1(β2+kp+kdβ1)+b0a2(β1+kd)+k0(kpβ1+kdβ2+β3)]d4＝[b0a2(β2+kp+kdβ1)+k0(kpβ2+kdβ3)]d5＝k0kpβ3由劳斯判据可得：s5d0d2d4s4d1d3d5s3b31b32s2b41b42s1b51s0b61b42＝d5，b51＝d5证明完毕。证明在外界干扰下整个系统稳定。说明delta并联机器人使用本发明时稳定性好。定理2：当被控对象模型未知时(delta高速并联机器人高速运行时，关节之间的耦合，建模时并联机器人不能省略转动惯量、关节之间的摩擦力的作用，否则会使系统在运行过程中发生抖震，最终破坏系统的稳定性和轨迹跟踪的性能。故线性自抗扰控制策略在设计时，这些考虑的因素为系统的模型参数未知)，delta高速并联机器人使用本发明具有稳定性。证明如下：设对象的标称模型为gn，则实际对象g(s)＝gn(1+δg(s))，其中δg为标称模型的摄动量，且满足其中为乘法范数有界不确定性。根据闭环特征方程可得：根据鲁棒稳定判据，对任意ω，满足式(4-27)不等式：b0+g2(s)gn(s)h(s)+g2(s)gn(s)h(s)δg(s)＝0(21)根据公式(21)和式(4-27)推导出式(22)，可使系统稳定。根据公式(20)推导出δg(s)的表达式，再代入可得出式(22)。根据鲁棒稳定判据，对任意ω，公式(22)都成立。所以是乘法范数有界不确定性。进而说明系统是稳定的。可见，合理选择(或预设)b0、kp、kd、β1、β2和β3可使系统稳定，且具有一定鲁棒性。由上面的稳定性证明可得，线性自抗扰控制策略能够实现delta高速并联机器人的轨迹跟踪的动力学控制，且在高速运行时，由于对象模型参数未知中没有出现代表不定因素的参数，所以不影响系统的稳定性。图6为本发明实施例1提供的线性自抗扰控制策略下的单臂控制器结构图，由图6可知，线性自抗扰控制器只需设置好β1、β2和β3。线性扩张状态观测器公式(9)稳定条件是β1、β2和β3均大于零，且β1β2＞β3。由图3可知，x1(s)、x2(s)、x3(s)为系统的三个输入，y1(s)、y2(s)、y3(s)为系统的输出，当任意一个输入xi(s)改变，yi(s)输出均改变，且改变另2个输入xi(s)。delta高速并联机器人为三个输入、三个输出，相互之间量耦合，可通过图5构建公式(23)：综合公式(7)可得公式(24)：其中，为等效综合扰动，为三个轴之间的耦合项，为高速运动时不确定项，为现场各种扰动项的综合函数。图7为本发明实施例1提供的线性自抗扰控制策略下的控制器结构总图，是基于图6构建线性自抗扰控制策略下的控制器结构总图。本实施例还进行了仿真实验设计对本发明进一步验证：实验中的标称参数如表1所示。表1参数数值伺服电机额定输出功率750w伺服电机额定转速3000rpm伺服电机额定转矩2.39n·m伺服电机转动惯量1.59*10-4kg·m2减速比20:01主动臂质量2.35kg从动臂质量0.9kg主动臂400mm从动臂长度1000mm分别设置了圆形给定、8字形给定等轨迹，对每一种轨迹，首先验证无扰动情况，接着验证扰动情况，控制中为了更好地验证控制效果，去除了机器人的角度限制、时间(速度)限制、空间限制。实验一：圆形轨迹输入，输入中不含扰动输入设定delta高速并联机器人的跟踪轨迹为半径为250mm的圆周。关节1、关节2、关节3运动的给定轨迹分别为：采用线性自抗扰控制策略控制，线性扩张状态观测器参数β1＝1，β2＝65000，β3＝600，得到轨迹跟踪控制仿真曲线如图8所示。实验二：圆形轨迹输入，仿真中5秒后加入三角波扰动为验证控制器针对圆形轨迹输入的鲁棒性，在机器人运行到5s时加入一幅值为500mm，周期0.1hz的三角波外部干扰，实验结果如图10所示。可知，由于加入的扰动幅值较大，但采用线性自抗扰控制策略控制，在三角波扰动加入后，三维轨迹图(图9)波动也不明显。验证了采用线性自抗扰能够实现三个关节的解耦控制，针对三角波扰动具有较强的鲁棒性。实验三：8字形轨迹输入，5秒后输入三角波扰动设定delta高速并联机器人的8字形跟踪轨迹。采用线性自抗扰控制策略控制，线性扩张状态观测器参数β1＝1，β2＝65000，β3＝600。关节1、关节2、关节3运动的给定轨迹分别为：为验证控制器针对8字形轨迹输入的鲁棒性，在机器人运行到5s时加入三角波外部干扰，如图11所示。输入幅值为50mm，周期0.1hz的三角波外部干扰实验结果如图12-13所示。采用线性自抗扰控制控制策略，在图12-13中，三维轨迹图波动不明显，所控制的8字形轨迹略有振动。验证了采用线性自抗扰控制策略能够实现三个关节的解耦控制，针对8字形输入轨迹，三角波扰动下具有较强的鲁棒性。综合实验一至实验三，实验中通过圆形轨迹跟踪控制、圆形轨迹跟踪抗干扰控制及8字形轨迹跟踪抗干扰控制，验证了线性自抗扰控制策略可以实现三个关节的解耦控制，能够实现给定轨迹精确实时跟踪，并具有良好的鲁棒性。实施例2本实施例提供了一种delta并联机器人轨迹控制系统，包括：获取模块，用于获取delta并联机器人上一时刻主动臂电机的控制电压与输出角度。设计模块，用于设计线性扩张状态观测器；所述线性扩张状态观测器的输入为所述上一时刻主动臂电机的控制电压与输出角度，输出为当前时刻主动臂的计算角度和总扰动。线性控制率获取模块，用于根据目标角度和所述计算角度得到所述线性扩张状态观测器的线性控制率。控制电压获取模块，用于根据所述总扰动和所述线性控制率得到当前时刻主动臂电机的控制电压。轨迹控制模块，用于根据所述当前时刻主动臂电机的控制电压控制所述delta并联机器人主动臂的运动轨迹。在本实施例中，所述线性扩张状态观测器为：其中，e为当前时刻主动臂的计算角度和上一时刻主动臂的输出角度的偏差，z1为当前时刻主动臂的计算角度，y为上一时刻主动臂的输出角度，为z1的导数，z2为当前时刻主动臂的计算角度的微分，f1(e)＝z1-y，是z2的导数，z3为总扰动，f2(e)＝z2-y，b0为不确定控制增益的近似值，u′为上一时刻主动臂电机的控制电压，其中uo为线性控制率，是z3的导数，f3(e)＝z3-y，β1、β2和β3为线性扩张状态观测器参数，[β1β2β3]t＝l，其中t为转置，l为观测器增益。在本实施例中，所述线性控制率的计算公式为：其中，kp为比例系数，γ为目标角度，kd为微分系数，为γ的导数。在本实施例中，所述当前时刻主动臂电机的控制电压的计算公式为：其中，u为当前时刻主动臂电机的控制电压，s为复频域中的时间。根据本发明提供的具体实施例，本发明公开了以下技术效果：针对delta高速并联机器人的运动要求、非线性特征及建模的省略，本发明将线性自抗扰控制策略应用到delta高速并联机器人动力学控制中，并通过稳定性分析，线性自抗扰控制策略能够实现delta高速并联机器人的轨迹跟踪的动力学控制，且在高速运行时，模型的不定因素不会影响系统的稳定性。即本发明轨迹跟踪效果好，具有很强的鲁棒性。本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。当前第1页12