CN113175324A - 一种凿岩台车大臂耦合运动串级复合pid控制方法 - Google Patents
一种凿岩台车大臂耦合运动串级复合pid控制方法 Download PDFInfo
- Publication number
- CN113175324A CN113175324A CN202110458520.7A CN202110458520A CN113175324A CN 113175324 A CN113175324 A CN 113175324A CN 202110458520 A CN202110458520 A CN 202110458520A CN 113175324 A CN113175324 A CN 113175324A
- Authority
- CN
- China
- Prior art keywords
- oil cylinder
- speed
- pid
- large arm
- motion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 166
- 238000000034 method Methods 0.000 title claims abstract description 46
- 239000002131 composite material Substances 0.000 title claims abstract description 10
- 230000008878 coupling Effects 0.000 claims abstract description 20
- 238000010168 coupling process Methods 0.000 claims abstract description 20
- 238000005859 coupling reaction Methods 0.000 claims abstract description 20
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 239000011435 rock Substances 0.000 claims abstract description 13
- 238000001824 photoionisation detection Methods 0.000 claims description 25
- 150000001875 compounds Chemical class 0.000 claims description 15
- 238000005553 drilling Methods 0.000 claims description 11
- 230000004069 differentiation Effects 0.000 claims description 5
- 230000010354 integration Effects 0.000 claims description 5
- 239000011295 pitch Substances 0.000 description 30
- 230000001276 controlling effect Effects 0.000 description 16
- 230000008569 process Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E21—EARTH OR ROCK DRILLING; MINING
- E21C—MINING OR QUARRYING
- E21C35/00—Details of, or accessories for, machines for slitting or completely freeing the mineral from the seam, not provided for in groups E21C25/00 - E21C33/00, E21C37/00 or E21C39/00
- E21C35/24—Remote control specially adapted for machines for slitting or completely freeing the mineral
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/14—Programme-controlled manipulators characterised by positioning means for manipulator elements fluid
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
-
- E—FIXED CONSTRUCTIONS
- E21—EARTH OR ROCK DRILLING; MINING
- E21C—MINING OR QUARRYING
- E21C31/00—Driving means incorporated in machines for slitting or completely freeing the mineral from the seam
- E21C31/08—Driving means incorporated in machines for slitting or completely freeing the mineral from the seam for adjusting parts of the machines
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F15—FLUID-PRESSURE ACTUATORS; HYDRAULICS OR PNEUMATICS IN GENERAL
- F15B—SYSTEMS ACTING BY MEANS OF FLUIDS IN GENERAL; FLUID-PRESSURE ACTUATORS, e.g. SERVOMOTORS; DETAILS OF FLUID-PRESSURE SYSTEMS, NOT OTHERWISE PROVIDED FOR
- F15B11/00—Servomotor systems without provision for follow-up action; Circuits therefor
- F15B11/16—Servomotor systems without provision for follow-up action; Circuits therefor with two or more servomotors
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F15—FLUID-PRESSURE ACTUATORS; HYDRAULICS OR PNEUMATICS IN GENERAL
- F15B—SYSTEMS ACTING BY MEANS OF FLUIDS IN GENERAL; FLUID-PRESSURE ACTUATORS, e.g. SERVOMOTORS; DETAILS OF FLUID-PRESSURE SYSTEMS, NOT OTHERWISE PROVIDED FOR
- F15B21/00—Common features of fluid actuator systems; Fluid-pressure actuator systems or details thereof, not covered by any other group of this subclass
- F15B21/08—Servomotor systems incorporating electrically operated control means
- F15B21/087—Control strategy, e.g. with block diagram
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Geology (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geochemistry & Mineralogy (AREA)
- Life Sciences & Earth Sciences (AREA)
- Robotics (AREA)
- Physics & Mathematics (AREA)
- Fluid Mechanics (AREA)
- General Engineering & Computer Science (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Operation Control Of Excavators (AREA)
Abstract
本发明提出了一种凿岩台车大臂耦合运动串级复合PID控制方法,用以解决现有常规的PID控制方法达不到大臂耦合运动控制需求的问题。本发明控制外环包括至少两个位置环,控制内环包括至少两个速度环,位置环输出至少两个油缸运动速度,相应地油缸运动速度进行叠加计算,分别得到叠加后的设定运动速度,将设定运动速度作为速度环的输入,根据速度环的输出得到控制大臂油缸运动的比例阀开度值。本发明位置环的反馈角度值为编码器数值,速度环的反馈速度利用编码器角度和油缸结构三角函数关系计算;有效解决了凿岩机大臂因耦合运动导致大臂左右摆动和上下俯仰两关节无法同时控制的问题,实现大臂双关节联动,降低大臂运动控制时间,提高工作效率。
Description
技术领域
本发明涉及隧道施工装备的技术领域,尤其涉及一种凿岩台车大臂耦合运动串级复合PID控制方法。
背景技术
在三臂凿岩台车机械臂运动控制系统中,通过两个并联油缸共同控制大臂左右摆动和大臂上下俯仰关节的运动。大臂左右摆动和大臂上下俯仰两个关节可以同时运动,也可以单独运动。在传统的手动模式下,操作手只能通过手柄分别控制大臂左右摆动或大臂上下俯仰运动;而在自动模式下,为了让大臂尽快到达目标位置,需要寻求一种控制方法,实现自动控制大臂左右摆动和大臂上下俯仰的同时运动。因为大臂耦合运动,常规的单级PID已经达不到大臂耦合运动控制需求,需要采用一种凿岩台车大臂耦合运动串级复合PID控制方法。
专利申请号为202010585584.9为的中国发明,公开了了基于模糊神经网络PID控制器的阀位串级控制方法,解决了阀位控制过程复杂多变,难以建立精确数学模型,致使传统PID控制控制效果不佳以及单回路控制难以消除外界扰动的难题,能够对阀位控制过程进行动态实时控制,提升了控制过程的快速性、准确性和鲁棒性,利于调节阀稳定连续地工作。但是,此方法是单环串级PID,并没有涉及到两个串级PID的复合控制,不能直接应用到大臂左右摆动和大臂上下俯仰的油缸控制中。
发明内容
针对现有常规的PID控制方法达不到大臂耦合运动控制需求的技术问题,本发明提出一种凿岩台车大臂耦合运动串级复合PID控制方法,让大臂左右摆动和大臂上下俯仰关节同时运动,分别达到目标角度,减少大臂耦合运动时间,提高了运动控制精度和工作效率。
为了达到上述目的,本发明的技术方案是这样实现的:一种凿岩台车大臂耦合运动串级复合PID控制方法,控制外环包括至少两个位置环,控制内环包括至少两个速度环,每个位置环输出至少两个油缸运动速度,相应地油缸运动速度进行叠加计算,分别得到叠加后的设定运动速度,将设定运动速度作为速度环的输入,根据速度环的输出得到控制大臂油缸运动的比例阀开度值。
本发明的步骤如下:
步骤一:根据需要的大臂左右摆动和上下俯仰的目标角度,分别计算大臂的左右摆动和上下俯仰的关节角度误差;
步骤二:将大臂的左右摆动和上下俯仰的关节角度误差输入相应的位置环PID,位置环PID均输出油缸运动速度;
步骤三:将油缸运动速度进行相应的叠加分别得到油缸的设定运动速度;
步骤四:根据油缸的设定运动速度,计算油缸的速度误差,将速度误差输入相应的速度环PID得到控制大臂的油缸的比例阀开度增量;
步骤五:根据比例阀开度增量计算油缸的比例阀开度,利用比例阀开度控制油缸的多个方向的运动速度,从而控制大臂的关节的耦合转动。
优选地,所述位置环的数量设有两个,速度环的数量设有两个,每个位置环的位置环PID均输出两个油缸运动速度;两个速度环的速度环PID分别输出两个油缸的比例阀开度增量。
优选地,所述两个油缸为并联连接的左油缸和右油缸,左油缸和右油缸均可单独控制大臂左右摆动关节或大臂上下俯仰关节;左油缸和右油缸均可以让大臂左右摆动关节和大臂上下俯仰关节同时运动,从而实现大臂的耦合运动。
优选地,所述步骤一中计算关节角度误差时,位置环PID的反馈角度值为编码器检测大臂的左右摆动关节或上下俯仰关节的角度;所述步骤四中计算油缸的速度误差时,速度环PID的反馈运动速度为利用编码器检测的角度和油缸结构三角函数计算的油缸反馈速度。
优选地,所述大臂的左右摆动关节为左右摆动转动副,大臂的上下俯仰关节为上下俯仰转动副,所述编码器包括左右摆动编码器和上下俯仰编码器,左右摆动编码器与左右摆动转动副同轴连接,上下俯仰编码器与上下俯仰转动副同轴连接,左右摆动编码器用于实时记录大左右摆动转动副的当前角度,上下俯仰编码器用于实时记录上下俯仰转动副的当前角度;所述左油缸和右油缸对称布置在大臂的两侧。
所述两个位置环PID为系数相同的位置式PID,所述两个速度环PID为系数相同的增量式PID。
所述两个位置环PID为左右摆动位置环PID和上下俯仰位置环PID,且左右摆动位置环PID输出的左油缸运动速度vL1(k)和右油缸运动速度vR1(k)相反,上下俯仰位置环PID输出的左油缸运动速度vL2(k)和右油缸运动速度vR2(k)相等。
所述大臂的左右摆动和上下俯仰的关节角度误差的计算方法为:
eθ1(k)=θ1-θ3
eθ2(k)=θ2-θ4;
其中,θ1为第k次大臂的左右摆动目标角度,θ2为第k次大臂的上下俯仰目标角度,θ3为左右摆动编码器检测的角度,θ4为上下俯仰编码器检测的角度,eθ1(k)为第k次左右摆动的关节角度误差,eθ2(k)为第k次上下俯仰的关节角度误差;
当关节角度误差eθ1(k)输入左右摆动位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
vR1(k)=-vL1(k)
当关节角度误差eθ2(k)输入上下俯仰位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
vR2(k)=vL2(k)
其中,vL1(k)和vR1(k)为第k次左右摆动位置环PID输出的左油缸运动速度和右油缸运动速度;vL2(k)和vR2(k)为第k次上下俯仰位置环PID输出的左油缸运动速度和右油缸运动速度;eθ1(k-1)为第k-1次左右摆动的关节角度误差,eθ2(k-1)为第k-1次上下俯仰的关节角度误差;eθ1(i)为第i次左右摆动的关节角度误差,eθ2(i)为第i次上下俯仰的关节角度误差,i=0,1,,...,k;KP1、KI1、KD1分别为位置环PID的比例调节系数、积分调节系数和微分调节系数。
所述油缸结构三角函数计算油缸反馈速度的方法为:
V'L=K'(θ3-θ3last)/Δt,
V'R=K'(θ4-θ4last)/Δt;
其中,K'为三角关系计算系数,θ3为第k次大臂左右摆动编码器检测的数值,θ4为第k次大臂上下俯仰编码器检测的数值,θ3last为第k-1次大臂左右摆动编码器检测的数值,θ4last为第k-1次大臂上下俯仰编码器检测的数值,Δt为第k次与第k-1次大臂运动的时间差值。
所述油缸的设定运动速度通过调节系数K1和K2调节位置环PID输出的两个油缸运动速度,即
VL=K1vL1(k)+K2vL2(k)
VR=K1vR1(k)+K2vR2(k);
其中,VL为大臂的左油缸设定运动速度,VR为大臂的右油缸设定运动速度,vL1(k)和vR1(k)分别为左右摆动位置环PID第k次输出的左油缸运动速度和右油缸运动速度,vL2(k)和vR2(k)分别为上下俯仰位置环PID第k次输出的左油缸运动速度和右油缸运动速度。
所述步骤四中速度误差的计算方法为:
eVL(k)=VL-V'L
eVR(k)=VR-V'R;
其中,eVL(k)为第k次左油缸的速度误差,eVR(k)为第k次右油缸的速度误差。
所述两个速度环PID计算比例阀开度增量的方法为:
ΔdL(k)=KP2[eVL(k)-eVL(k-1)]+KI2eVL(k)+KD2[eVL(k)-2eVL(k-1)+eVL(k-2)]
ΔdR(k)=KP2[eVR(k)-eVR(k-1)]+KI2eVR(k)+KD2[eVR(k)-2eVR(k-1)+eVR(k-2)]
其中,ΔdL(k)为第k次左油缸的速度环PID输出的左油缸控制比例阀开度增量,ΔdR(k)为第k次右油缸的速度环PID输出的右油缸控制比例阀开度增量;eVL(k-1)为第k-1次的左油缸的速度误差,eVL(k-2)为第k-2次的左油缸的速度误差;eVR(k)为第k次右油缸的速度误差,eVR(k-1)为第k-1次的右油缸的速度误差,eVR(k-2)为第k-2次的右油缸的速度误差;KP2、KI2、KD2分别为两个速度环PID的比例调节系数、积分调节系数和微分调节系数。
所述计算油缸的比例阀开度的方法为:
dL(k)=ΔdL(k)+dL(k-1)
dR(k)=ΔdR(k)+dR(k-1);
其中,dL(k)和dR(k)分别为第k次控制大臂的左油缸和右油缸的比例阀开度;dL(k-1)和dR(k-1)分别为第k-1次控制大臂的左油缸和右油缸的比例阀开度。
当左油缸缩短长度l,同时右油缸伸长长度l时,大臂只绕左右摆动转动副向左转动;反之,当左油缸伸长长度l,同时右油缸缩短长度l时,大臂只绕左右摆动转动副向右转动;
当左油缸和右油缸都伸长长度l时,大臂只绕上下俯仰转动副向上转动;反之,当左油缸和右油缸同时都缩短长度l时,大臂只绕上下俯仰转动副向下转动;
若左油缸的比例阀开度大于右油缸的比例阀开度,则左油缸运动速度大于右油缸,大臂在绕左右摆动转动副向右转动的同时也绕上下俯仰转动副向上转动,从而实现大臂的耦合运动。
与现有技术相比,本发明的有益效果:包括两个位置环PID作为控制外环,两个速度环PID作为控制内环;通过每一个位置环可得到两个输出量:大臂左油缸运动速度,大臂右油缸运动速度;然后将位置环PID的速度输出量进行叠加计算,作为两个速度环PID的输入,得到控制大臂左、右油缸运动的比例阀开度值,通过位置环输出速度的叠加可以准确的控制比例阀的设定开度,进而更好的控制油缸运动;本发明位置环PID的反馈角度值为编码器数值,速度环的反馈速度利用编码器角度和油缸结构三角函数关系计算求出油缸的运动反馈速度,这样可以省去测量油缸运动速度的传感器,降低设备生产成本。本发明能够有效解决凿岩机大臂因耦合运动导致大臂左右摆动和上下俯仰两关节无法同时控制的问题,从而使凿岩台车实现大臂双关节联动,降低大臂运动控制时间,提高了工作效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明的大臂结构的俯视图。
图2为本发明的大臂结构的主视图。
图3为本发明的控制流程示意图。
图中,1为大臂基座,2为大臂左右摆动转动副,3为大臂左右摆动编码器,4为大臂左油缸,5为大臂右油缸,6为大臂,7为大臂上下俯仰转动副,8为大臂上下俯仰编码器。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例1,一种凿岩台车大臂耦合运动串级复合PID控制方法,控制外环包括至少两个位置环,每个位置环中设有一个位置环PID,控制内环包括至少两个速度环,每个速度环中设有一个速度环PID,因为无论是控制大臂上下俯仰,还是控制大臂左右摆动,大臂左油缸和大臂右油缸必须要同时运动才能完成,所以每个位置环都需要输出两个油缸的运动速度,当大臂同时进行左右摆动和上下俯仰复合运动的时候,就需要把相应地油缸运动速度进行叠加计算,分别得到叠加后的设定运动速度,将设定运动速度作为速度环的输入,位置环叠加后得到的大臂的油缸运动速度分别作为速度环PID的输入,根据速度环的输出得到控制大臂油缸运动的比例阀开度值,利用比例阀开度值控制比例阀的开度,从而控制油缸的运动速度。设定运动速度是叠加后的油缸运动速度,设定运动速度作为速度环的输入,速度环的输入就是速度值。通过比例阀开度值控制两个油缸的不同方向的摆动和伸缩的运动速度,从而控制大臂左右和俯仰运动的耦合。
实施例2,一种凿岩台车大臂耦合运动串级复合PID控制方法,具体的步骤如下:
步骤一:根据需要的大臂左右摆动和上下俯仰的目标角度,分别计算大臂的左右摆动和上下俯仰的关节角度误差。
如图3所示,位置环的数量设有两个,速度环的数量设有两个,每个位置环的位置环PID均输出两个油缸运动速度,即大臂的左油缸运动速度和右油缸运动速度;两个速度环的速度环PID分别输出两个油缸的比例阀开度增量,即左油缸的比例阀开度增量和右油缸的比例阀开度增量。
两个位置环PID的输出速度公式如下:
其中,vL1(k)、vR1(k)分别为第k次大臂左右摆动关节的位置环PID输出的左油缸速度和右油缸速度;eθ1(k)为第k次大臂左右摆动关节角度误差,eθ1(k-1)为第k-1次的关节角度误差;vL2(k)、vR2(k)分别为第k次大臂上下俯仰关节的位置环PID输出的左油缸速度和右油缸速度;eθ2(k)为第k次大臂上下俯仰关节角度误差,eθ2(k-1)为第k-1次的关节角度误差;eθ1(i)为第i次大臂上下俯仰关节角度误差,eθ2(i)为第i次的大臂上下俯仰关节角度误差,i=0,1,,...,k,i=0时,eθ1(0)=0,eθ2(0)=0;KP1、KI1、KD1分别为位置环PID的比例调节系数、积分调节系数和微分调节系数,三个系数值需要根据实际情况进行整定。
两个速度环PID公式分别为:
ΔdL(k)=KP2[eVL(k)-eVL(k-1)]+KI2eVL(k)+KD2[eVL(k)-2eVL(k-1)+eVL(k-2)]
ΔdR(k)=KP2[eVR(k)-eVR(k-1)]+KI2eVR(k)+KD2[eVR(k)-2eVR(k-1)+eVR(k-2)];
其中,ΔdL(k)为第k次大臂左油缸速度环PID输出的左油缸控制比例阀开度增量;ΔdR(k)为第k次大臂右油缸速度环PID输出的右油缸控制比例阀开度增量;eVL(k)为第k次的左油缸速度误差,eVL(k-1)为第k-1次的左油缸速度误差,eVL(k-2)为第k-2次的左油缸速度误差;eVR(k)为第k次的右油缸速度误差,eVR(k-1)为第k-1次的右油缸速度误差,eVR(k-2)为第k-2次的右油缸速度误差;当k=0时,eVL(0)=0,eVR(0)=0;KP2、KI2、KD2分别为速度环PID的比例调节系数、积分调节系数和微分调节系数,三个系数值需要根据实际情况进行整定。
大臂耦合运动由两个并联油缸控制。两个油缸为并联连接的左油缸4和右油缸5,左油缸4和右油缸5均可单独控制大臂左右摆动关节或大臂上下俯仰关节;左油缸4和右油缸5均可以让大臂左右摆动关节和大臂上下俯仰关节同时运动,从而实现大臂的耦合运动。
如图1所示,所述大臂的左右摆动关节为左右摆动转动副2,大臂的上下俯仰关节为上下俯仰转动副7,所述编码器包括左右摆动编码器3和上下俯仰编码器8,左右摆动编码器3与左右摆动转动副2同轴连接,如图2所示,上下俯仰编码器8与上下俯仰转动副7同轴连接,左右摆动编码器3用于实时记录大左右摆动转动副2的当前角度,上下俯仰编码器8用于实时记录上下俯仰转动副7的当前角度;所述左油缸4和右油缸5对称布置在大臂6的两侧,对称布置可以让大臂俯仰时左右油缸运动速度相同,大臂左右摆动时,左右油缸运动速度刚好相反。大臂基座1与大臂左右摆动转动副2铰接在一起,形成一个转动副,大臂基座1与左油缸4、右油缸5通过万向节铰接在一起,使左右油缸可以绕基座上下摆动;大臂6与大臂基座1通过左右摆动转动副2、上下俯仰转动副7组成的万向节铰接在一起,使左右油缸可以绕大臂进行上下摆动。通过左右摆动编码器3、上下俯仰编码器8的编码器数值和油缸结构三角函数关系可分别计算出左油缸4和右油缸5的运动速度。
当左油缸4缩短长度l,同时右油缸5伸长长度l时,大臂6只绕左右摆动转动副2向左转动;反之,当左油缸4伸长长度l,同时右油缸5缩短长度l时,大臂6只绕左右摆动转动副2向右转动;当左油缸4和右油缸5都伸长长度l时,大臂6只绕上下俯仰转动副7向上转动;反之,当左油缸4和右油缸5同时都缩短长度l时,大臂6只绕上下俯仰转动副7向下转动。
左油缸4或右油缸5的运动速度快慢是由控制油缸运动的比例阀的开度决定的,比例阀的开度越大,油缸运动速度越快,反之越慢。若左油缸4的比例阀开度大于右油缸5的比例阀开度,则左油缸4运动速度大于右油缸5,大臂6在绕左右摆动转动副2向右转动的同时也绕上下俯仰转动副7向上转动,从而实现大臂油缸的耦合运动。
所述两个位置环PID为系数相同的位置式PID,所述两个速度环PID为系数相同的增量式PID。
计算关节角度误差时,位置环PID的反馈角度值为编码器检测大臂的左右摆动关节或上下俯仰关节的角度。在大臂自动模式下,第k次同时下发的大臂左右摆动目标角度θ1和大臂上下俯仰目标角度θ2,通过读取大臂的左右摆动编码器3、上下俯仰摆动编码器8检测角度的数值,求出误差值eθ1(k)、eθ2(k)。即大臂的左右摆动和上下俯仰的关节角度误差的计算方法为:
其中,θ1为第k次大臂的左右摆动目标角度,θ2为第k次大臂的上下俯仰目标角度,θ3为左右摆动编码器3检测的角度,θ4为上下俯仰编码器8检测的角度,eθ1(k)为第k次大臂左右摆动即左右摆动转动副2的关节角度误差,eθ2(k)为第k次大臂上下俯仰时上下俯仰转动副7的关节角度误差。
步骤二:将大臂的左右摆动和上下俯仰的关节角度误差输入相应的位置环PID,位置环PID均输出油缸运动速度。
所述两个位置环PID为左右摆动位置环PID和上下俯仰位置环PID,且左右摆动位置环PID输出的左油缸运动速度vL1(k)和右油缸运动速度vR1(k)相反,上下俯仰位置环PID输出的左油缸运动速度vL2(k)和右油缸运动速度vR2(k)相等,是由左右油缸对称布置决定的。
当输入为大臂左右摆动目标角度θ1时,经过左右摆动位置环PID输出为左油缸速度vL1(k)和右油缸速度VR1。当关节角度误差eθ1(k)输入左右摆动位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
当输入为大臂上下俯仰目标角度θ2时,经过位置环PID输出左油缸速度vL2(k)、右油缸速度vR2(k),左右油缸速度相同。当关节角度误差eθ1(k)输入上下俯仰位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
其中,vL1(k)和vR1(k)为第k次左右摆动位置环PID输出的左油缸运动速度和右油缸运动速度;vL2(k)和vR2(k)为第k次上下俯仰位置环PID输出的左油缸运动速度和右油缸运动速度;eθ1(k-1)为第k-1次左右摆动的关节角度误差,eθ2(k-1)为第k-1次上下俯仰的关节角度误差;eθ1(i)为第i次大臂左右摆动的关节角度误差,eθ2(i)为第i次上下俯仰的关节角度误差;KP1、KI1、KD1分别为位置环PID的比例调节系数、积分调节系数和微分调节系数。
步骤三:将油缸运动速度进行相应的叠加分别得到油缸的设定运动速度。
因大臂左右摆动和大臂上下俯仰运动是耦合在一起,所以两个位置环PID输出两组大臂左油缸和右油缸运动速度,需要对输出结果进行速度叠加。其中,左右摆动位置环PID的输出速度对大臂左右摆动关节起作用,上下俯仰位置环PID的输出对大臂上下俯仰关节起作用。
为了调节大臂左右摆动和上下俯仰关节运动速度的快慢程度,位置环PID输出量有调节系数K1和K2,默认情况下设定K1=K2=1.0,通过调节系数K1、K2将两个位置环PID输出的4个油缸运动速度进行叠加可以调节大臂左油缸设定速度VL即左油缸4的设定速度和大臂右油缸设定速度VR即右油缸5的设定速度,即调节大臂左右摆动关节相对大臂上下俯仰关节到达目标角度的快慢程度。若设定K1>1则大臂左右摆动输出的两个速度均按K1比例增大,分别为K1vL1(k)和;同理调节K2也是如此。油缸的设定运动速度通过调节系数K1和K2调节位置环PID输出的两个油缸运动速度,经相应速度叠加后即:
其中,VL为大臂的左油缸设定运动速度,VR为大臂的右油缸设定运动速度,vL1(k)和vR1(k)分别为左右摆动位置环PID第k次输出的左油缸运动速度和右油缸运动速度,vL2(k)和vR2(k)分别为上下俯仰位置环PID第k次输出的左油缸运动速度和右油缸运动速度。
若K1=1.2,K2=1.0则对大臂左右摆动关节调节快慢程度大于大臂上下俯仰关节;反之,如果K1=1.0,K2=1.2则对大臂左右摆动关节调节快慢程度小于大臂上下俯仰关节,通过调节调节系数K1,K2,从而实现对大臂上下俯仰和左右摆动相对运动速度调节。
步骤四:根据油缸的设定运动速度,计算油缸的速度误差,将速度误差输入相应的速度环PID得到控制大臂的油缸的比例阀开度增量。
速度环PID的反馈速度不是通过传感器直接测量油缸速度,而是通过大臂左右摆动编码器、上下俯仰编码器角度和油缸结构三角函数计算出左油缸、右油缸的运动反馈速度,这样可以省去测量油缸运动速度的传感器,降低设备生产成本。即计算油缸的速度误差时,速度环PID的反馈运动速度为利用编码器检测的角度和油缸结构三角函数计算的油缸反馈速度。
通过读取左右摆动编码器3、上下俯仰摆动编码器8在第k-1次的数值θ3last、θ4last和第k次的数值θ3、θ4,经过油缸结构三角函数计算求出在第k次大臂左油缸和大臂右油缸的速度V'L、V'R。油缸结构三角函数计算油缸反馈速度的方法为:
V'L=K'(θ3-θ3last)/Δt,
V'R=K'(θ4-θ4last)/Δt;
其中,K'为三角关系计算系数,其值通过大臂、油缸构成的空间三角形及空间坐标变换计算得到,θ3为第k次大臂左右摆动编码器检测的数值,θ4为第k次大臂上下俯仰编码器检测的数值,θ3last为第k-1次大臂左右摆动编码器检测的数值,θ4last为第k-1次大臂上下俯仰编码器检测的数值,Δt为第k次与第k-1次大臂运动的时间差值。
根据左油缸设定速度VL、右油缸设定速度VR可计算出第k次与k-1次大臂运动的速度差值,即速度误差的计算方法为:
eVL(k)=VL-V'L
eVR(k)=VR-V'R;
其中,eVL(k)为第k次左油缸的速度误差,eVR(k)为第k次右油缸的速度误差。
把eVL(k)、eVR(k)分别带入两个速度环PID可以计算得到控制左油缸4的比例阀开度增量值ΔdL(k)、右油缸5的比例阀开度增量值ΔdR(k)。两个速度环PID计算比例阀开度增量的方法为:
ΔdL(k)=KP2[eVL(k)-eVL(k-1)]+KI2eVL(k)+KD2[eVL(k)-2eVL(k-1)+eVL(k-2)]
ΔdR(k)=KP2[eVR(k)-eVR(k-1)]+KI2eVR(k)+KD2[eVR(k)-2eVR(k-1)+eVR(k-2)]
其中,ΔdL(k)为第k次左油缸的速度环PID输出的左油缸控制比例阀开度增量,ΔdR(k)为第k次右油缸的速度环PID输出的右油缸控制比例阀开度增量;eVL(k-1)为第k-1次的左油缸的速度误差,eVL(k-2)为第k-2次的左油缸的速度误差;eVR(k)为第k次右油缸的速度误差,eVR(k-1)为第k-1次的右油缸的速度误差,eVR(k-2)为第k-2次的右油缸的速度误差;KP2、KI2、KD2分别为两个速度环PID的比例调节系数、积分调节系数和微分调节系数。
步骤五:根据比例阀开度增量计算油缸的比例阀开度,利用比例阀开度控制油缸的多个方向的运动速度,从而控制大臂的关节的耦合转动。
所述计算油缸的比例阀开度的方法是计算左油缸4和右油缸5的控制比例阀开度计算如下:
其中,dL(k)和dR(k)分别为第k次控制大臂的左油缸和右油缸的比例阀开度;dL(k-1)和dR(k-1)分别为第k-1次控制大臂的左油缸和右油缸的比例阀开度。
通过速度环PID实现对大臂的左油缸4或右油缸5的耦合运动控制,使大臂6可以同时绕左右摆动转动副2和上下俯仰转动副7转动,降低大臂运动控制时间,提高控制系统稳定性和工作效率。在本实施例中,速度环PID控制为内环、位置环PID控制为外环共同构成凿岩台车大臂耦合运动串级复合PID控制。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (15)
1.一种凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,控制外环包括至少两个位置环,控制内环包括至少两个速度环,每个位置环输出至少两个油缸运动速度,相应地油缸运动速度进行叠加计算,分别得到叠加后的设定运动速度,将设定运动速度作为速度环的输入,根据速度环的输出得到控制大臂油缸运动的比例阀开度值。
2.根据权利要求1所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,其步骤如下:
步骤一:根据需要的大臂左右摆动和上下俯仰的目标角度,分别计算大臂的左右摆动和上下俯仰的关节角度误差;
步骤二:将大臂的左右摆动和上下俯仰的关节角度误差输入相应的位置环PID,位置环PID均输出油缸运动速度;
步骤三:将油缸运动速度进行相应的叠加分别得到油缸的设定运动速度;
步骤四:根据油缸的设定运动速度,计算油缸的速度误差,将速度误差输入相应的速度环PID得到控制大臂的油缸的比例阀开度增量;
步骤五:根据比例阀开度增量计算油缸的比例阀开度,利用比例阀开度控制油缸的多个方向的运动速度,从而控制大臂的关节的耦合转动。
3.根据权利要求2所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述位置环的数量设有两个,速度环的数量设有两个,每个位置环的位置环PID均输出两个油缸运动速度;两个速度环的速度环PID分别输出两个油缸的比例阀开度增量。
4.根据权利要求3所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述两个油缸为并联连接的左油缸(4)和右油缸(5),左油缸(4)和右油缸(5)均可单独控制大臂左右摆动关节或大臂上下俯仰关节;左油缸(4)和右油缸(5)均可以让大臂左右摆动关节和大臂上下俯仰关节同时运动,从而实现大臂的耦合运动。
5.根据权利要求1或3所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述步骤一中计算关节角度误差时,位置环PID的反馈角度值为编码器检测大臂的左右摆动关节或上下俯仰关节的角度;所述步骤四中计算油缸的速度误差时,速度环PID的反馈运动速度为利用编码器检测的角度和油缸结构三角函数计算的油缸反馈速度。
6.根据权利要求5所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述大臂的左右摆动关节为左右摆动转动副(2),大臂的上下俯仰关节为上下俯仰转动副(7),所述编码器包括左右摆动编码器(3)和上下俯仰编码器(8),左右摆动编码器(3)与左右摆动转动副(2)同轴连接,上下俯仰编码器(8)与上下俯仰转动副(7)同轴连接,左右摆动编码器(3)用于实时记录大左右摆动转动副(2)的当前角度,上下俯仰编码器(8)用于实时记录上下俯仰转动副(7)的当前角度;所述左油缸(4)和右油缸(5)对称布置在大臂(6)的两侧。
7.根据权利要求3、4或6所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述两个位置环PID为系数相同的位置式PID,所述两个速度环PID为系数相同的增量式PID。
8.根据权利要求7所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述两个位置环PID为左右摆动位置环PID和上下俯仰位置环PID,且左右摆动位置环PID输出的左油缸运动速度vL1(k)和右油缸运动速度vR1(k)相反,上下俯仰位置环PID输出的左油缸运动速度vL2(k)和右油缸运动速度vR2(k)相等。
9.根据权利要求8所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述大臂的左右摆动和上下俯仰的关节角度误差的计算方法为:
其中,θ1为第k次大臂的左右摆动目标角度,θ2为第k次大臂的上下俯仰目标角度,θ3为左右摆动编码器(3)检测的角度,θ4为上下俯仰编码器(8)检测的角度,eθ1(k)为第k次左右摆动的关节角度误差,eθ2(k)为第k次上下俯仰的关节角度误差;
当关节角度误差eθ1(k)输入左右摆动位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
vR1(k)=-vL1(k)
当关节角度误差eθ2(k)输入上下俯仰位置环PID得到的左油缸运动速度和右油缸运动速度分别为:
vR2(k)=vL2(k)
其中,vL1(k)和vR1(k)为第k次左右摆动位置环PID输出的左油缸运动速度和右油缸运动速度;vL2(k)和vR2(k)为第k次上下俯仰位置环PID输出的左油缸运动速度和右油缸运动速度;eθ1(k-1)为第k-1次左右摆动的关节角度误差,eθ2(k-1)为第k-1次上下俯仰的关节角度误差;eθ1(i)为第i次左右摆动的关节角度误差,eθ2(i)为第i次上下俯仰的关节角度误差,i=0,1,,...,k;KP1、KI1、KD1分别为位置环PID的比例调节系数、积分调节系数和微分调节系数。
10.根据权利要求6、8或9述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述油缸结构三角函数计算油缸反馈速度的方法为:
V'L=K'(θ3-θ3last)/Δt,
V'R=K'(θ4-θ4last)/Δt;
其中,K'为三角关系计算系数,θ3为第k次大臂左右摆动编码器检测的数值,θ4为第k次大臂上下俯仰编码器检测的数值,θ3last为第k-1次大臂左右摆动编码器检测的数值,θ4last为第k-1次大臂上下俯仰编码器检测的数值,Δt为第k次与第k-1次大臂运动的时间差值。
13.根据权利要求12所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,所述两个速度环PID计算比例阀开度增量的方法为:
ΔdL(k)=KP2[eVL(k)-eVL(k-1)]+KI2eVL(k)+KD2[eVL(k)-2eVL(k-1)+eVL(k-2)]
ΔdR(k)=KP2[eVR(k)-eVR(k-1)]+KI2eVR(k)+KD2[eVR(k)-2eVR(k-1)+eVR(k-2)]
其中,ΔdL(k)为第k次左油缸的速度环PID输出的左油缸控制比例阀开度增量,ΔdR(k)为第k次右油缸的速度环PID输出的右油缸控制比例阀开度增量;eVL(k-1)为第k-1次的左油缸的速度误差,eVL(k-2)为第k-2次的左油缸的速度误差;eVR(k)为第k次右油缸的速度误差,eVR(k-1)为第k-1次的右油缸的速度误差,eVR(k-2)为第k-2次的右油缸的速度误差;KP2、KI2、KD2分别为两个速度环PID的比例调节系数、积分调节系数和微分调节系数。
15.根据权利要求6、8、9、11-14中任意一项所述的凿岩台车大臂耦合运动串级复合PID控制方法,其特征在于,当左油缸(4)缩短长度l,同时右油缸(5)伸长长度l时,大臂(6)只绕左右摆动转动副(2)向左转动;反之,当左油缸(4)伸长长度l,同时右油缸(5)缩短长度l时,大臂(6)只绕左右摆动转动副(2)向右转动;
当左油缸(4)和右油缸(5)都伸长长度l时,大臂(6)只绕上下俯仰转动副(7)向上转动;反之,当左油缸(4)和右油缸(5)同时都缩短长度l时,大臂(6)只绕上下俯仰转动副(7)向下转动;
若左油缸(4)的比例阀开度大于右油缸(5)的比例阀开度,则左油缸(4)运动速度大于右油缸(5),大臂(6)在绕左右摆动转动副(2)向右转动的同时也绕上下俯仰转动副(7)向上转动,从而实现大臂的耦合运动。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110458520.7A CN113175324B (zh) | 2021-04-27 | 2021-04-27 | 一种凿岩台车大臂耦合运动串级复合pid控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110458520.7A CN113175324B (zh) | 2021-04-27 | 2021-04-27 | 一种凿岩台车大臂耦合运动串级复合pid控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113175324A true CN113175324A (zh) | 2021-07-27 |
CN113175324B CN113175324B (zh) | 2022-03-11 |
Family
ID=76926411
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110458520.7A Active CN113175324B (zh) | 2021-04-27 | 2021-04-27 | 一种凿岩台车大臂耦合运动串级复合pid控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113175324B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114183179A (zh) * | 2021-12-06 | 2022-03-15 | 中铁工程装备集团有限公司 | 一种锚杆台车自动化控制系统及锚杆台车 |
CN115870966A (zh) * | 2021-09-28 | 2023-03-31 | 中国科学院沈阳自动化研究所 | 一种基于d-s证据合成理论的速度协调方法 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6272826A (ja) * | 1985-09-24 | 1987-04-03 | Komatsu Ltd | パワ−シヨベルにおける作業機の制御装置 |
CA2141984A1 (en) * | 1995-02-07 | 1996-08-08 | Herbert A. Smith | Continuous control system for a mining or tunnelling machine |
JP2000158371A (ja) * | 1998-11-25 | 2000-06-13 | Ohbayashi Corp | マニピュレータの制御方法および制御装置 |
CN102707730A (zh) * | 2012-04-05 | 2012-10-03 | 大连理工大学 | 高空作业车操作平台轨迹控制装置 |
CN104747189A (zh) * | 2014-12-31 | 2015-07-01 | 太重煤机有限公司 | 一种运用于采煤机调高的比例控制系统 |
CN105945954A (zh) * | 2016-06-21 | 2016-09-21 | 天津大学 | 一种五自由度混联机器人的双闭环控制方法 |
CN106368996A (zh) * | 2016-11-10 | 2017-02-01 | 武汉华海通用电气有限公司 | 一种基于pid算法的油缸同步方法 |
US20170220006A1 (en) * | 2016-02-01 | 2017-08-03 | Varian Semiconductor Equipment Associates, Inc. | Proportional integral derivative control incorporating multiple actuators |
CN209388132U (zh) * | 2019-01-26 | 2019-09-13 | 中铁工程装备集团隧道设备制造有限公司 | 一种三臂三篮钢拱架安装机电控系统 |
CN110985035A (zh) * | 2019-12-30 | 2020-04-10 | 中铁工程装备集团有限公司 | 一种串联式柔臂tbm刀盘掘进控制方法 |
CN112091975A (zh) * | 2020-08-28 | 2020-12-18 | 无锡卡尔曼导航技术有限公司 | 一种自走式光伏清洁机器人的机械臂运动控制方法 |
-
2021
- 2021-04-27 CN CN202110458520.7A patent/CN113175324B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6272826A (ja) * | 1985-09-24 | 1987-04-03 | Komatsu Ltd | パワ−シヨベルにおける作業機の制御装置 |
CA2141984A1 (en) * | 1995-02-07 | 1996-08-08 | Herbert A. Smith | Continuous control system for a mining or tunnelling machine |
JP2000158371A (ja) * | 1998-11-25 | 2000-06-13 | Ohbayashi Corp | マニピュレータの制御方法および制御装置 |
CN102707730A (zh) * | 2012-04-05 | 2012-10-03 | 大连理工大学 | 高空作业车操作平台轨迹控制装置 |
CN104747189A (zh) * | 2014-12-31 | 2015-07-01 | 太重煤机有限公司 | 一种运用于采煤机调高的比例控制系统 |
US20170220006A1 (en) * | 2016-02-01 | 2017-08-03 | Varian Semiconductor Equipment Associates, Inc. | Proportional integral derivative control incorporating multiple actuators |
CN105945954A (zh) * | 2016-06-21 | 2016-09-21 | 天津大学 | 一种五自由度混联机器人的双闭环控制方法 |
CN106368996A (zh) * | 2016-11-10 | 2017-02-01 | 武汉华海通用电气有限公司 | 一种基于pid算法的油缸同步方法 |
CN209388132U (zh) * | 2019-01-26 | 2019-09-13 | 中铁工程装备集团隧道设备制造有限公司 | 一种三臂三篮钢拱架安装机电控系统 |
CN110985035A (zh) * | 2019-12-30 | 2020-04-10 | 中铁工程装备集团有限公司 | 一种串联式柔臂tbm刀盘掘进控制方法 |
CN112091975A (zh) * | 2020-08-28 | 2020-12-18 | 无锡卡尔曼导航技术有限公司 | 一种自走式光伏清洁机器人的机械臂运动控制方法 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115870966A (zh) * | 2021-09-28 | 2023-03-31 | 中国科学院沈阳自动化研究所 | 一种基于d-s证据合成理论的速度协调方法 |
CN114183179A (zh) * | 2021-12-06 | 2022-03-15 | 中铁工程装备集团有限公司 | 一种锚杆台车自动化控制系统及锚杆台车 |
Also Published As
Publication number | Publication date |
---|---|
CN113175324B (zh) | 2022-03-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113175324B (zh) | 一种凿岩台车大臂耦合运动串级复合pid控制方法 | |
CN103556945B (zh) | 一种高造斜率井眼轨迹控制方法 | |
CN111946340B (zh) | 基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统 | |
CN102278065B (zh) | 旋转导向工具及其偏置机构以及对该偏置机构的控制方法 | |
CN105739513B (zh) | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 | |
CN107223220A (zh) | 云台控制方法、装置及云台 | |
CN115290289B (zh) | 一种提高大迎角尾撑试验系统控制精度的优化方法 | |
CN109176517A (zh) | 基于末端名义点约束的串联工业机器人连杆参数标定方法 | |
CN109720609A (zh) | 一种悬吊式微重力模拟装置及控制方法 | |
CN106625629B (zh) | 隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置及方法 | |
US8452454B2 (en) | Method, apparatus and computer program for adjusting hydraulically operated boom | |
WO2022001760A1 (zh) | 一种基于5g技术的远程可监控多轴协同智能控制器 | |
CN110441026B (zh) | 一种提高探针自动跟踪测量控制稳定性的方法 | |
CN113942017A (zh) | 罐体焊接点位姿规划方法、焊接工作站、设备及介质 | |
CN108673502A (zh) | 一种基于未知模型动态观测器的机械臂运动控制方法 | |
CN109032072A (zh) | 一种双并联杆钻铆设备的法矢调姿及其偏移补偿方法 | |
CN113110027B (zh) | 一种轨道式斗轮取料机位置切换抗干扰控制方法 | |
CN102910533A (zh) | 一种基于吊机的空间角度测量方法 | |
CN207216927U (zh) | 板球系统 | |
JP7316743B2 (ja) | 回転テーブル | |
CN1292144C (zh) | 水平螺旋钻机钻头控向装置 | |
CN211916897U (zh) | 一种用于隧道巡检机器人的三维稳像平台 | |
CN116733441B (zh) | 一种隧道打孔机器人及其智能目标识别与自动化打孔方法 | |
Parshin et al. | Robotization of slip form for monolithic construction of tall buildings | |
CN113530439B (zh) | 一种自动调整工具面定向控制系统和方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |