CN115291527B - 一种随动控制方法、***、设备及存储介质 - Google Patents
一种随动控制方法、***、设备及存储介质 Download PDFInfo
- Publication number
- CN115291527B CN115291527B CN202211211378.7A CN202211211378A CN115291527B CN 115291527 B CN115291527 B CN 115291527B CN 202211211378 A CN202211211378 A CN 202211211378A CN 115291527 B CN115291527 B CN 115291527B
- Authority
- CN
- China
- Prior art keywords
- pitch
- angle
- acceleration
- rotation
- max
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60P—VEHICLES ADAPTED FOR LOAD TRANSPORTATION OR TO TRANSPORT, TO CARRY, OR TO COMPRISE SPECIAL LOADS OR OBJECTS
- B60P1/00—Vehicles predominantly for transporting loads and modified to facilitate loading, consolidating the load, or unloading
- B60P1/04—Vehicles predominantly for transporting loads and modified to facilitate loading, consolidating the load, or unloading with a tipping movement of load-transporting element
- B60P1/16—Vehicles predominantly for transporting loads and modified to facilitate loading, consolidating the load, or unloading with a tipping movement of load-transporting element actuated by fluid-operated mechanisms
Landscapes
- Engineering & Computer Science (AREA)
- Software Systems (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Health & Medical Sciences (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及随动控制技术领域,具体地说,涉及一种随动控制方法、***、设备及存储介质,首先通过采集前倾角α、横滚角β、航向角δ,计算出理论俯仰角ζ1和理论回转角θ1,然后根据理论俯仰角ζ1和理论回转角θ1,并结合俯仰编码器和回转编码器的反馈值,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o),最后转化为与俯仰驱动器和回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器,驱动器跟随行进引起的变动实时调整俯仰角和回转角,在减小控制难度的同时,解决了随动***的振动和抖动改变俯仰角和回转角的问题。
Description
技术领域
本发明涉及随动控制技术领域,具体地说,涉及一种随动控制方法、***、设备及存储介质。
背景技术
随动***servo system是一种反馈控制***。在这类***中,它的参考输入不是时间的解析函数,如何变化事先并不知道是随着时间任意变化,控制***的任务是在各种情况下保证输出以一定精度跟随参考输入的变化而变化。根据其任务的性质分析可知快速跟踪和准确定位是随动***的两个非常重要的性能指标。要满足相关的性能指标,首要要有精准的检测元器件和良好的执行元件,另外随动控制是完全依靠伺服***对目标信号的响应,当加速度过大而超过***的响应能力,会出现随动***的振动或抖动等现象,控制难度较大。控制算法是随动控制***的核心灵魂,因此亟需一种准确、高效的控制算法,以解决随动***的振动和抖动、以及控制难度较大的问题。
发明内容
本发明针对上述随动***的振动和抖动、以及控制难度较大的问题,提出一种随动控制方法、***、设备及存储介质,首先通过采集前倾角α、横滚角β、航向角δ计算出理论俯仰角ζ1和理论回转角θ1,然后根据理论俯仰角ζ1和理论回转角θ1,以及俯仰编码器反馈的实际俯仰角和回转编码器反馈的实际回转角,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o),最后转化为与俯仰驱动器和回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器,驱动器跟随行进引起的变动实时调整俯仰角和回转角,在减小控制难度的同时,解决了随动***的振动和抖动改变俯仰角和回转角的问题。
本发明具体实现内容如下:
一种随动控制方法,包括以下步骤:
步骤1:将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ,以及目标俯仰角ζ、目标回转角θ输入至随动控制***中,计算出理论俯仰角ζ1和理论回转角θ1;
步骤2:根据从俯仰编码器获取的实际俯仰角和从回转编码器获取的实际回转角,以及步骤1中计算出的理论俯仰角ζ1和理论回转角θ1,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o);
步骤3:将俯仰方向的输出加速度afy(o)、俯仰方向的输出速度Vfy(o)转化为与俯仰驱动器对应的数据格式,将回转方向的输出加速度ahz(o)、回转方向的输出速度Vhz(o)转化为与回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器。
为了更好地实现本发明,进一步地,所述步骤1具体包括以下步骤:
步骤11:以随动控制***轴线的运动方向,且平行于随动控制***底架平台的方向为X轴,以与随动控制***轴线的运动方向垂直的方向为Y轴、以垂直于X轴和Y轴构成的平面的方向为Z轴,建立随动控制***的坐标系O(x,y,z);
步骤12:从双轴倾角仪获取前倾角α、横滚角β,从寻北仪获取航向角δ,根据设定的坐标系O(x,y,z)得到变化后的旋转坐标系0’’’’(x,,,,,y,,,,,z,,,,);
步骤13:获取随动控制***在设定的坐标系O(x,y,z)下的俯仰角ζ、回转角θ,计算出旋转坐标系0’’’(x,,,,y,,,,z,,,)下的俯仰角和回转角,得到理论俯仰角ζ1和理论回转角θ1。
为了更好地实现本发明,进一步地,所述步骤12的具体操作为:首先将横滚角β、航向角δ赋值为0,得到第一次变换后的坐标系O,(x,,y,,z,),其次将前倾角α、航向角δ赋值为0,得到第二次变换后的坐标系O’’(x’’,y’’,z’’),然后将前倾角α、横滚角β赋值为0,得到第三次变换后的坐标系0’’’(x,,,,y,,,,z,,,),最后根据第一次变换后的坐标系O,(x,,y,,z,)、第二次变换后的坐标系O’’(x’’,y’’,z’’)、第三次变换后的坐标系0’’’(x,,,,y,,,,z,,,),通过公式计算出最终的旋转坐标系0’’’’(x,,,,,y,,,,,z,,,,),其中,ROT(Y,α)为绕y轴旋转a角度的旋转矩阵,ROT(X,β)为绕x轴旋转a角度的旋转矩阵,ROT(Z,δ)为绕z轴旋转δ角度的旋转矩阵,(x,y,z)为设定坐标系,T为向量转置。
为了更好地实现本发明,进一步地,所述步骤2具体包括以下步骤:
步骤21:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的三次方与比例系数Kfy的乘积、平衡系数Dfy,计算出俯仰方向的目标速度Vfy;根据理论回转角θ1与回转编码器获取的实际回转角差的三次方与比例系数Khz的乘积、平衡系数Dhz,计算出回转方向的目标速度Vhz;
步骤22:判断计算出的俯仰方向的目标速度Vfy与设定的俯仰最大速度Vmax(fy)的大小关系、回转方向的目标速度Vhz与设定的回转最大速度Vmax(hz)的大小关系,若俯仰方向的目标速度Vfy大于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于设定的俯仰最大速度Vmax(fy),若俯仰方向的目标速度Vfy小于或等于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于俯仰方向的目标速度Vfy;若回转方向的目标速度Vhz大于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),若回转方向的目标速度Vhz小于或等于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于回转方向的目标速度Vhz;
步骤23:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Kfy的乘积,计算出俯仰方向的目标加速度afy;根据理论回转角θ1与回转编码器获取的实际回转角差的二次方与平衡系数Khz的乘积,计算出回转方向的目标加速度ahz;
步骤24:判断计算出的俯仰方向的目标加速度afy与设定的俯仰最大加速度amax(fy)的大小关系、回转方向的目标加速度ahz与设定的回转最大加速度amax(hz)的大小关系,若俯仰方向的输出速度Vfy(o)等于设定的最大速度Vmax(fy),则俯仰方向的输出加速度afy(o)为0,若俯仰方向的输出速度Vfy(o)小于设定的最大速度Vmax(fy),且计算出俯仰方向的目标加速度afy小于设定的最大加速度amax(fy),则俯仰方向的输出加速度afy(o)为计算出的俯仰方向的目标加速度afy,若俯仰方向的目标加速度afy大于设定的最大加速度amax(fy),则根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Kfy、迭代基数η的n次方的乘积,计算出新的俯仰方向的目标加速度afy’,直到目标加速度afy’小于等于amax(fy),输出俯仰方向的输出加速度af(o)等于目标加速度afy’;若回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),则回转方向的输出加速度ahz(o)为0,若回转方向的输出速度Vhz(o)小于设定的回转最大速度Vmax(hz),且回转方向的目标加速度ahz小于设定的回转最大加速度amax(hz),则回转方向的输出加速度ahz(o)等于计算出的回转方向的目标加速度ahz,若回转方向的目标加速度ahz大于设定的回转最大加速度amax(hz),则根据理论回转角θ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Khz、迭代基数η的n次方,计算出新的回转方向的目标加速度ahz’,直到目标加速度ahz’小于回转最大加速度等于amax(hz),输出回转方向的输出加速度ahz(o)等于目标加速度ahz’。
基于上述提出的随动控制方法,为了更好地实现本发明,进一步地,提出一种随动控制***,与控制计算机连接,包括俯仰驱动器、俯仰编码器、回转驱动器、回转编码器、双轴倾角仪、寻北仪;
所述双轴倾角仪通过CAN总线与控制计算机连接,用于获 取前倾角α、横滚角β;
所述寻北仪通过CAN总线与控制计算机连接,用于获取航向角δ;
所述俯仰驱动器通过CAN总线与控制计算机、俯仰编码器连接,用于接收转化后的俯仰方向的输出加速度afy、俯仰方向的输出速度Vfy;
所述回转驱动器通过CAN总线与控制计算机、回转编码器连接,用于接收转化后的回转方向的输出加速度afy、回转方向的输出速度Vfy。
为了更好地实现本发明,进一步地,提出一种电子设备,包括存储器、处理器,所述存储器用于存储计算机程序;
所述处理器,用于执行所述计算机程序时实现上述的随动控制方法的步骤。
为了更好地实现本发明,进一步地,提出一种计算机存储介质,存储有一个或多个计算机程序,所述一个或多个计算机程序被一个或多个处理器执行时,致使所述一个或多个处理器执行上述的随动控制方法的步骤。
本发明具有以下有益效果:
(1)本发明通过理论俯仰角ζ1和理论回转角θ1,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o),最后转化为与俯仰驱动器和回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器实时调整俯仰角和回转角,在减小控制难度的同时,解决了随动***的振动和抖动改变俯仰角和回转角的问题。
(2)本发明通过理论角与实际角差的三次方,以三次曲线的形式对速度进行控制,同时通过理论理论角与实际角差的二次方,以二次曲线的形式对加速度进行控制,提高了***响应速度,且速度变化更加平滑。
(3)本发明对加速度的控制,不是简单进行比较大小,然后机械赋值操作,而是按相关的物理学规律进行插值迭代计算,按进行插值,周期循环执行上述的算法,使得加速度变化更加平稳,最大限度的避免突变,从而使得运动更加平稳。
(4)本发明与传统的5次样条曲线算法比较,计算更为简单,且运算周期更短。
附图说明
图1为随动平台结构示意图;
图2为随动控制***连接框图;
图3为坐标系位置关系示意图;
图4为随动控制***运行流程示意图;
图5为本发明实施例中回转正弦波测试结果示意图;
图6为本发明实施例中俯仰正弦波测试结果示意图;
图7为本发明实施例中回转随动结果示意图;
图8为本发明实施例中俯仰随动结果示意图;
其中,1、随动平台,2、回转平台,3、俯仰数字缸,4、回转数字缸,5、传感器组件。
具体实施方式
为了更清楚地说明本发明实施例的技术方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,应当理解,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例,因此不应被看作是对保护范围的限定。基于本发明中的实施例,本领域普通技术工作人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
在本发明的描述中,需要说明的是,除非另有明确的规定和限定,术语“设置”、“相连”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;也可以是直接相连,也可以是通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以具体情况理解上述术语在本发明中的具体含义。
实施例1:
本实施例提出一种随动控制方法,如图4所示,包括以下步骤:
步骤1:将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ,以及目标俯仰角ζ、目标回转角θ输入至随动控制***中,计算出理论俯仰角ζ1和理论回转角θ1;
步骤2:根据从俯仰编码器获取的实际俯仰角和从回转编码器获取的实际回转角,以及步骤1中计算出的理论俯仰角ζ1和理论回转角θ1,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o);
步骤3:将俯仰方向的输出加速度afy(o)、俯仰方向的输出速度Vfy(o)转化为与俯仰驱动器对应的数据格式,将回转方向的输出加速度ahz(o)、回转方向的输出速度Vhz(o)转化为与回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器。
工作原理:本实施例首先通过采集前倾角α、横滚角β、航向角δ,以及目标俯仰角ζ、目标回转角θ,计算出理论俯仰角ζ1和理论回转角θ1,然后根据俯仰编码器反馈的实际俯仰角和回转编码器反馈的实际回转角,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o),最后转化为与俯仰驱动器和回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器,驱动器跟随行进引起的变动实时调整俯仰角和回转角,在减小控制难度的同时,解决了随动***的振动和抖动改变俯仰角和回转角的问题。
实施例2:
本实施例在上述实施例1的基础上,对步骤1的步骤进行说明。
工作原理:所述步骤1具体包括以下步骤:
步骤11:以随动控制***轴线的运动方向,且平行于随动控制***底架平台的方向为X轴,以与随动控制***轴线的运动方向垂直的方向为Y轴、以垂直于X轴和Y轴构成的平面的方向为Z轴,建立随动控制***的坐标系O(x,y,z);
步骤12:从双轴倾角仪获取前倾角α、横滚角β,从寻北仪获取航向角δ,根据设定的坐标系O(x,y,z)得到变化后的旋转坐标系0’’’’(x,,,,,y,,,,,z,,,,);
步骤13:获取随动控制***在设定的坐标系O(x,y,z)下的俯仰角ζ、回转角θ,计算出旋转坐标系0’’’(x,,,,y,,,,z,,,)下的俯仰角和回转角,得到理论俯仰角ζ1和理论回转角θ1。
本实施例的其他部分与上述实施例1相同,故不再赘述。
实施例3:
本实施例在上述实施例1-2任一项的基础上,对步骤2的具体步骤进行说明。
工作原理:所述步骤2具体包括以下步骤:
步骤21:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的三次方与比例系数Kfy的乘积、平衡系数Dfy,计算出俯仰方向的目标速度Vfy;根据理论回转角θ1与回转编码器获取的实际回转角差的三次方与比例系数Khz的乘积、平衡系数Dhz,计算出回转方向的目标速度Vhz;
步骤22:判断计算出的俯仰方向的目标速度Vfy与设定的俯仰最大速度Vmax(fy)的大小关系、回转方向的目标速度Vhz与设定的回转最大速度Vmax(hz)的大小关系,若俯仰方向的目标速度Vfy大于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于设定的俯仰最大速度Vmax(fy),若俯仰方向的目标速度Vfy小于或等于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于俯仰方向的目标速度Vfy;若回转方向的目标速度Vhz大于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),若回转方向的目标速度Vhz小于或等于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于回转方向的目标速度Vhz;
步骤23:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Kfy的乘积,计算出俯仰方向的目标加速度afy;根据理论回转角θ1与回转编码器获取的实际回转角差的二次方与平衡系数Khz的乘积,计算出回转方向的目标加速度ahz;
步骤24:判断计算出的俯仰方向的目标加速度afy与设定的俯仰最大加速度amax(fy)的大小关系、回转方向的目标加速度ahz与设定的回转最大加速度amax(hz)的大小关系,若俯仰方向的输出速度Vfy(o)等于设定的最大速度Vmax(fy),则俯仰方向的输出加速度afy(o)为0,若俯仰方向的输出速度Vfy(o)小于设定的最大速度Vmax(fy),且计算出俯仰方向的目标加速度afy小于设定的最大加速度amax(fy),则俯仰方向的输出加速度afy(o)为计算出的俯仰方向的目标加速度afy,若俯仰方向的目标加速度afy大于设定的最大加速度amax(fy),则根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Kfy、迭代基数η的n次方的乘积,计算出新的俯仰方向的目标加速度afy’,直到目标加速度afy’小于等于amax(fy),输出俯仰方向的输出加速度af(o)等于目标加速度afy’;若回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),则回转方向的输出加速度ahz(o)为0,若回转方向的输出速度Vhz(o)小于设定的回转最大速度Vmax(hz),且回转方向的目标加速度ahz小于设定的回转最大加速度amax(hz),则回转方向的输出加速度ahz(o)等于计算出的回转方向的目标加速度ahz,若回转方向的目标加速度ahz大于设定的回转最大加速度amax(hz),则根据理论回转角θ1与从俯仰编码器获取的实际俯仰角差的二次方与平衡系数Khz、迭代基数η的n次方,计算出新的回转方向的目标加速度ahz’,直到目标加速度ahz’小于回转最大加速度等于amax(hz),输出回转方向的输出加速度ahz(o)等于目标加速度ahz’。
本实施例的其他部分与上述实施例1-2任一项相同,故不再赘述。
实施例4:
本实施例在上述实施例1-3任一项的基础上,如图2所示,提出一种随动控制***,与控制计算机连接,包括俯仰驱动器、俯仰编码器、回转驱动器、回转编码器、双轴倾角仪、寻北仪;
所述双轴倾角仪通过CAN总线与控制计算机连接,用于获 取前倾角α、横滚角β;
所述寻北仪通过CAN总线与控制计算机连接,用于获取航向角δ;
所述俯仰驱动器通过CAN总线与控制计算机、俯仰编码器连接,用于接收转化后的俯仰方向的输出加速度afy、俯仰方向的输出速度Vfy;
所述回转驱动器通过CAN总线与控制计算机、回转编码器连接,用于接收转化后的回转方向的输出加速度afy、回转方向的输出速度Vfy。
本实施例的其他部分与上述实施例1-3任一项相同,故不再赘述。
实施例5:
本实施例在上述实施例1-4任一项的基础上,如图1、图2、图3、图4所示,以地面特种装备车的随动***的核心控制算法为例,进行详细说明。
工作原理:本实施例提出的随动***主要由底盘、底架平台、回转平台2、随动平台1、俯仰数字缸3、回转数字缸4、传感器组件5以及控制计算机等组成,所述传感器组件5包括双轴倾角仪、寻北仪。回转和俯仰两个方向均通过俯仰数字缸3和回转数字缸4即步进电机带动液压缸的伸缩进行推动。随动运动时,在控制软件中手动输入俯仰角和回转角,控制软件实时收集各类传感器信号,周期性执行如下的算法,然后通过CAN总线,分别控制俯仰和回转两个方向的数字液压缸,由步进电机带动液压缸,准确及时的跟随因行进引起的变动,确保随动平台1的俯仰角和回转在大地坐标系中保持不变。
随动控制算法分为三大部分,一是因被测对象姿态发生变化,从而导致控制量发生变化的算法,二是根据控制量准确控制电机到位的位移控制算法,三是将两种算法有机的结合起来进行综合应用,从而构成随动控制算法,具体如下:
1. 控制量解算算法
1.1建立坐标系
为了建立数学模型,规定X轴沿着随动***轴线向前,Y轴在垂直于随动***轴线,平行于底架平台,Z轴垂直于X轴和Y轴构建的平面向上。***绕Y轴的旋转,称为前倾角,记为α,绕X轴的旋转,称为横滚角,记为β,绕Z轴的旋转,称为航向角,记为δ。理想状态下的坐标系较为O(x,y,z),变化后的坐标系为0,,,(x,,,,y,,,,z,,,)。为了检测相关的变化,在随动***的底架平台与***平台刚性连接上安装有双轴倾角传感器和寻北仪,用来检测随动***前倾角α、横滚角β以及航向角δ。
1.2直角坐标推算过过程:
当α≠0,β=0,δ=0时,坐标变换为:O,(x,,y,,z,),则根据坐标变换,可以得到:
当α=0,β≠0,δ=0时,坐标变换为:O,,(x,,,y,,,z,,),则根据坐标变换,可以得到:
当α=0,β=0,δ≠0时,坐标变换为:O,,(x,,,y,,,z,,),则根据坐标变换,可以得到:
如是可得:经过α,β,δ变换后,初始的坐标系O(x,y,z)变换为O(x,,,,y,,,,z,,,);
计算后得到:
1.3极坐标转化
由于该随动***最终是通过控制俯仰角和回转角,达到对准精度,因此假设在原坐标系下的俯仰角ζ,回转角为θ,故在新坐标系的俯仰角ζ1,回转角为θ1,假设随动平台1上任意一点A的模为R,那么:
所以:
在原坐标系下的俯仰角ζ,回转角为θ,故在新坐标系的俯仰角ζ1,回转角,与正北方向的回转角为θ1;
2.位移控制算法
根据随动控制的特点,要求***既要快速响应跟随到目标位置,又要运行平稳,不能出现抖动、爬行等现象,这个***研究带来较大的难度,进一步研究发现,在随动过程中加速时,惯性动态力矩或力作用在进给驱动结构上,如果进给驱动加速指令不平滑,施加在驱动电机上的力就会产生冲击,并产生高阶震荡,这些高频分量将激励进给驱动的结构动态响应,引起不希望的振动。另外,在***中适用了基于步进电机的数字液压缸,由于步进电机属于开环控制,自身没有反馈,因此调整更加困难。为了获得快速响应和平稳的运行速度,提出基于物理规律的加速度和速度同时控制的算法模型,具体算法如下:
将其对时间t求导后得:
其中,v为最终的输出速度;vmax为***设定的最大速度;K为现场调试确定出的比例系数;Ф为目标角度,即控制量解算算法解算的理论角度;为编码器反馈的角度;D为平衡系数;amax为***设定的最大加速度;ax为***计算的目标加速度;η为迭代基数取值范围为(0.9,1.0),n为迭代次数,取值为自然数集。
3.两种算法的结合应用
在本实施例的应用环境中,首先将双轴倾角仪测得前倾角α、横滚角β、以及寻北仪测得航向角δ,以及操作者输入的目标角度俯仰和回转代入控制量解算算法中,计算出俯仰方向和回转方向的两个实际控制角度,然后再将两个方向的控制量传入位移控制算法,并结合俯仰编码器和回转编码器的实际角度,计算电机的输出加速度ax和速度Vx,最后通调用软件的接口转化模块程序,转化成驱动器可识别的数据格式,通过CAN总线下发到相应的驱动器,然后再有驱动器去控制电机,使得电机稳定、快速的响应,并周期性0.1s的执行上述两种算法,具体流程图4所示,这两种控制算法可以分开应用,也可以综合起来应用,加上周期性刷新计算,从而形成随动控制算法。
本实施例***采用了三次曲线③式对速度进行控制,同时采用二次曲线④式和⑤式对加速度进行控制,不仅速度变化平滑,且提高了***响应速度;
本实施例对加速度的控制,不是简单进行比较大小,然后机械赋值操作,而是按相关的物理学规律进行插值迭代计算,当,按进行插值,第一次迭代n取1,第二次迭代n取2,第三次迭代n取3,直到满足要求为止,然后周期循环执行上述的算法,使得加速度变化更加平稳,最大限度的避免突变,从而使得运动更加平稳;
本实施例与传统的5次样条曲线算法比较,计算更为简单,使得运算周期更短;
本实施例控制量解算算法采用双轴传感器与基体固连,减少了变化和计算量;
本实施例将两种算法进行有效的组合,并循环调用,构成了整个随动算法。
如图5、图6所示,应用上述算法,完成***调试后,分别加载对两个方向的正弦波周期20S进行测试;
如图7、图8所示为测试过程车辆以20km左右的速度行驶的跟踪曲线,通过对实际路面的跟随数据统计分析,均满足相关总体指标要求,该***随动指标详见表1所示。
表1 精度完成情况
序号 | 项目 | 设计值(度) | 测试值(度) |
1 | 回转精度 | 1 | 0.44086 |
2 | 俯仰精度 | 1 | 0.862512 |
本实施例提出的两种算法可以应用在很多控制领域,控制量更新算法可以应用到需要空间解算的场合,如机器人控制、导弹随动发射***等;调节控制算法可以应用到电机控制领域、液压控制领域等,如数控机床、随动控制***等。总之,这两种控制算法可以分开应用,也可以综合起来应用,加上周期性刷新计算,从而形成随动控制算法。
本实施例的其他部分与上述实施例1-4任一项相同,故不再赘述。
以上所述,仅是本发明的较佳实施例,并非对本发明做任何形式上的限制,凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化,均落入本发明的保护范围之内。
Claims (6)
1.一种随动控制方法,其特征在于,包括以下步骤:
步骤1:将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ,以及目标俯仰角ζ、目标回转角θ输入至随动控制***中,计算出的理论俯仰角ζ1和理论回转角θ1;
步骤2:根据从俯仰编码器获取的实际俯仰角Υfy和从回转编码器获取的实际回转角Υhz,以及步骤1中计算出的理论俯仰角ζ1和理论回转角θ1,计算出俯仰方向的输出加速度afy(o)、回转方向的输出加速度ahz(o)、俯仰方向的输出速度Vfy(o)、回转方向的输出速度Vhz(o);
步骤3:将俯仰方向的输出加速度afy(o)、俯仰方向的输出速度Vfy(o)转化为与俯仰驱动器对应的数据格式,将回转方向的输出加速度ahz(o)、回转方向的输出速度Vhz(o)转化为与回转驱动器对应的数据格式,并通过CAN总线传输至相应的驱动器;
所述步骤2具体包括以下步骤:
步骤21:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的三次方与比例系数Kfy的乘积、平衡系数Dfy,计算出俯仰方向的目标速度Vfy;根据理论回转角θ1与回转编码器获取的实际回转角Υhz差的三次方与比例系数Khz的乘积、平衡系数Dhz,计算出回转方向的目标速度Vhz;
步骤22:判断计算出的俯仰方向的目标速度Vfy与设定的俯仰最大速度Vmax(fy)的大小关系、回转方向的目标速度Vhz与设定的回转最大速度Vmax(hz)的大小关系,若俯仰方向的目标速度Vfy大于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于设定的俯仰最大速度Vmax(fy),若俯仰方向的目标速度Vfy小于或等于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于俯仰方向的目标速度Vfy;若回转方向的目标速度Vhz大于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),若回转方向的目标速度Vhz小于或等于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于回转方向的目标速度Vhz;
步骤23:根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的二次方与平衡系数Kfy的乘积,计算出俯仰方向的目标加速度afy;根据理论回转角θ1与回转编码器获取的实际回转角Υhz差的二次方与平衡系数Khz的乘积,计算出回转方向的目标加速度ahz;
步骤24:判断计算出的俯仰方向的目标加速度afy与设定的俯仰最大加速度amax(fy)的大小关系、回转方向的目标加速度ahz与设定的回转最大加速度amax(hz)的大小关系,若俯仰方向的输出速度Vfy(o)等于设定的最大速度Vmax(fy),则俯仰方向的输出加速度afy(o)为0,若俯仰方向的输出速度Vfy(o)小于设定的最大速度Vmax(fy),且计算出俯仰方向的目标加速度afy小于设定的最大加速度amax(fy),则俯仰方向的输出加速度afy(o)为计算出的俯仰方向的目标加速度afy,若俯仰方向的目标加速度afy大于设定的最大加速度amax(fy),则根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的二次方与平衡系数Kfy、迭代基数η的n次方的乘积,计算出新的俯仰方向的目标加速度afy’,直到目标加速度afy’小于等于amax(fy),输出俯仰方向的输出加速度af(o)等于目标加速度afy’;若回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),则回转方向的输出加速度ahz(o)为0,若回转方向的输出速度Vhz(o)小于设定的回转最大速度Vmax(hz),且回转方向的目标加速度ahz小于设定的回转最大加速度amax(hz),则回转方向的输出加速度ahz(o)等于计算出的回转方向的目标加速度ahz,若回转方向的目标加速度ahz大于设定的回转最大加速度amax(hz),则根据理论回转角θ1与从俯仰编码器获取的实际俯仰角Υhz差的二次方与平衡系数Khz、迭代基数η的n次方,计算出新的回转方向的目标加速度ahz’,直到目标加速度ahz’小于回转最大加速度等于amax(hz),输出回转方向的输出加速度ahz(o)等于目标加速度ahz’。
2.如权利要求1所述的一种随动控制方法,其特征在于,所述步骤1具体包括以下步骤:
步骤11:以随动控制***轴线的运动方向,且平行于随动控制***底架平台的方向为X轴,以与随动控制***轴线的运动方向垂直的方向为Y轴、以垂直于X轴和Y轴构成的平面的方向为Z轴,建立随动控制***的坐标系O(x,y,z);
步骤12:从双轴倾角仪获取前倾角α、横滚角β,从寻北仪获取航向角δ,根据设定的坐标系O(x,y,z)得到变化后的旋转坐标系0’’’’(x,,,,,y,,,,,z,,,,);
步骤13:获取随动控制***在设定的坐标系O(x,y,z)下的俯仰角ζ、回转角θ,计算出旋转坐标系0’’’(x,,,,y,,,,z,,,)下的俯仰角和回转角,得到理论俯仰角ζ1和理论回转角θ1。
3.如权利要求2所述的一种随动控制方法,其特征在于,所述步骤12的具体操作为:首先将横滚角β、航向角δ赋值为0,得到第一次变换后的坐标系O,(x,,y,,z,),其次将前倾角α、航向角δ赋值为0,得到第二次变换后的坐标系O’’(x’’,y’’,z’’),然后将前倾角α、横滚角β赋值为0,得到第三次变换后的坐标系0’’’(x,,,,y,,,,z,,,),最后根据第一次变换后的坐标系O,(x,,y,,z,)、第二次变换后的坐标系O’’(x’’,y’’,z’’)、第三次变换后的坐标系0’’’(x,,,,y,,,,z,,,),通过公式ROT(X,β)(Z,δ) T计算出最终的旋转坐标系0’’’’(x,,,,,y,,,,,z,,,,),其中,ROT(Y,α)为绕y轴旋转α角度的旋转矩阵,ROT(X,β)为绕x轴旋转β角度的旋转矩阵,ROT(Z,δ)为绕z轴旋转δ角度的旋转矩阵,(x,y,z)T为向量转置。
4.一种随动控制***,连接控制计算机,其特征在于,包括俯仰驱动器、俯仰编码器、回转驱动器、回转编码器、双轴倾角仪、寻北仪;
所述双轴倾角仪通过CAN总线与控制计算机连接,用于获取前倾角α、横滚角β;
所述寻北仪通过CAN总线与控制计算机连接,用于获取航向角δ;
所述俯仰编码器通过CAN总线与控制计算机连接,用于获取实际俯仰角Υfy;
所述回转编码器通过CAN总线与控制计算机连接,用于获取实际回转角Υhz;
所述俯仰驱动器通过CAN总线与控制计算机、俯仰编码器连接,用于根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的三次方与比例系数Kfy的乘积、平衡系数Dfy,计算出俯仰方向的目标速度Vfy,判断计算出的俯仰方向的目标速度Vfy与设定的俯仰最大速度Vmax(fy)的大小关系,若俯仰方向的目标速度Vfy大于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于设定的俯仰最大速度Vmax(fy),若俯仰方向的目标速度Vfy小于或等于设定的俯仰最大速度Vmax(fy),则俯仰方向的输出速度Vfy(o)等于俯仰方向的目标速度Vfy;根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的二次方与平衡系数Kfy的乘积,计算出俯仰方向的目标加速度afy;判断计算出的俯仰方向的目标加速度afy与设定的俯仰最大加速度amax(fy)的大小关系,若俯仰方向的输出速度Vfy(o)等于设定的最大速度Vmax(fy),则俯仰方向的输出加速度afy(o)为0,若俯仰方向的输出速度Vfy(o)小于设定的最大速度Vmax(fy),且计算出俯仰方向的目标加速度afy小于设定的最大加速度amax(fy),则俯仰方向的输出加速度afy(o)为计算出的俯仰方向的目标加速度afy,若俯仰方向的目标加速度afy大于设定的最大加速度amax(fy),则根据理论俯仰角ζ1与从俯仰编码器获取的实际俯仰角Υfy差的二次方与平衡系数Kfy、迭代基数η的n次方的乘积,计算出新的俯仰方向的目标加速度afy’,直到目标加速度afy’小于等于amax(fy),输出俯仰方向的输出加速度af(o)等于目标加速度afy’;
所述回转驱动器通过CAN总线与控制计算机、回转编码器连接,用于根据理论回转角θ1与回转编码器获取的实际回转角Υhz差的三次方与比例系数Khz的乘积、平衡系数Dhz,计算出回转方向的目标速度Vhz,判断计算出的回转方向的目标速度Vhz与设定的回转最大速度Vmax(hz)的大小关系,若回转方向的目标速度Vhz大于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),若回转方向的目标速度Vhz小于或等于设定的回转最大速度Vmax(hz),则回转方向的输出速度Vhz(o)等于回转方向的目标速度Vhz;根据理论回转角θ1与回转编码器获取的实际回转角Υhz差的二次方与平衡系数Khz的乘积,计算出回转方向的目标加速度ahz;判断计算出的回转方向的目标加速度ahz与设定的回转最大加速度amax(hz)的大小关系,若回转方向的输出速度Vhz(o)等于设定的回转最大速度Vmax(hz),则回转方向的输出加速度ahz(o)为0,若回转方向的输出速度Vhz(o)小于设定的回转最大速度Vmax(hz),且回转方向的目标加速度ahz小于设定的回转最大加速度amax(hz),则回转方向的输出加速度ahz(o)等于计算出的回转方向的目标加速度ahz,若回转方向的目标加速度ahz大于设定的回转最大加速度amax(hz),则根据理论回转角θ1与从俯仰编码器获取的实际俯仰角Υhz差的二次方与平衡系数Khz、迭代基数η的n次方,计算出新的回转方向的目标加速度ahz’,直到目标加速度ahz’小于回转最大加速度等于amax(hz),输出回转方向的输出加速度ahz(o)等于目标加速度ahz’。
5.一种控制计算机,其特征在于,包括存储器、处理器,所述存储器用于存储计算机程序;
所述处理器,用于执行所述计算机程序时实现如权利要求1-3任一项所述的随动控制方法的步骤。
6.一种计算机存储介质,其特征在于,存储有一个或多个计算机程序,所述一个或多个计算机程序被一个或多个处理器执行时,致使所述一个或多个处理器执行如权利要求1-3中任一项所述的随动控制方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211211378.7A CN115291527B (zh) | 2022-09-30 | 2022-09-30 | 一种随动控制方法、***、设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211211378.7A CN115291527B (zh) | 2022-09-30 | 2022-09-30 | 一种随动控制方法、***、设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115291527A CN115291527A (zh) | 2022-11-04 |
CN115291527B true CN115291527B (zh) | 2022-12-20 |
Family
ID=83833330
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211211378.7A Active CN115291527B (zh) | 2022-09-30 | 2022-09-30 | 一种随动控制方法、***、设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115291527B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB8907661D0 (en) * | 1988-04-07 | 1989-05-17 | Sony Corp | A servo system for a motor |
CN101452293A (zh) * | 2007-11-30 | 2009-06-10 | 中国科学院沈阳自动化研究所 | 一种随动托轮测量控制方法及其专用装置 |
CN201358142Y (zh) * | 2009-02-25 | 2009-12-09 | 中国船舶重工集团公司第七一三研究所 | 伸缩臂起重机自动控制*** |
CN105700558A (zh) * | 2014-11-28 | 2016-06-22 | 西安博昱新能源有限公司 | 一种太阳能随动控制装置 |
CN110282578A (zh) * | 2019-06-18 | 2019-09-27 | 北京卫星环境工程研究所 | 航天器大型舱段总装用可升降式多功能支架车 |
CN110794878A (zh) * | 2019-11-19 | 2020-02-14 | 北京特种机械研究所 | 一种随动***俯仰角度跟踪控制方法 |
CN114104981A (zh) * | 2021-12-01 | 2022-03-01 | 山东港口渤海湾港集团有限公司 | 一种门机抓斗防摇*** |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6650078B1 (en) * | 2002-04-24 | 2003-11-18 | Rockwell Automation Technologies, Inc. | Method and system for controlling mechanical systems exhibiting discontinuity in inertial loading |
JP4553591B2 (ja) * | 2004-01-07 | 2010-09-29 | 株式会社アイチコーポレーション | 高所作業車の制御装置 |
DE102007039408A1 (de) * | 2007-05-16 | 2008-11-20 | Liebherr-Werk Nenzing Gmbh | Kransteuerung, Kran und Verfahren |
-
2022
- 2022-09-30 CN CN202211211378.7A patent/CN115291527B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB8907661D0 (en) * | 1988-04-07 | 1989-05-17 | Sony Corp | A servo system for a motor |
CN101452293A (zh) * | 2007-11-30 | 2009-06-10 | 中国科学院沈阳自动化研究所 | 一种随动托轮测量控制方法及其专用装置 |
CN201358142Y (zh) * | 2009-02-25 | 2009-12-09 | 中国船舶重工集团公司第七一三研究所 | 伸缩臂起重机自动控制*** |
CN105700558A (zh) * | 2014-11-28 | 2016-06-22 | 西安博昱新能源有限公司 | 一种太阳能随动控制装置 |
CN110282578A (zh) * | 2019-06-18 | 2019-09-27 | 北京卫星环境工程研究所 | 航天器大型舱段总装用可升降式多功能支架车 |
CN110794878A (zh) * | 2019-11-19 | 2020-02-14 | 北京特种机械研究所 | 一种随动***俯仰角度跟踪控制方法 |
CN114104981A (zh) * | 2021-12-01 | 2022-03-01 | 山东港口渤海湾港集团有限公司 | 一种门机抓斗防摇*** |
Non-Patent Citations (1)
Title |
---|
靶载有源设备转台伺服随动***设计;郭万禄等;《无线电工程》;20181019(第11期);第101-104页 * |
Also Published As
Publication number | Publication date |
---|---|
CN115291527A (zh) | 2022-11-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10507899B2 (en) | Motion control device and motion control method for ship | |
Lu et al. | Real-time simulation system for UAV based on Matlab/Simulink | |
CN111026160B (zh) | 一种四旋翼无人机轨迹跟踪控制方法 | |
CN103869704A (zh) | 基于扩展雅克比矩阵的空间机器人星臂协调控制方法 | |
CN108646556A (zh) | 输入饱和航天器无退绕姿态跟踪控制方法 | |
CN111258216A (zh) | 一种适用于四旋翼飞行器的滑模重复控制器 | |
Hancer et al. | Designing 3-DOF hardware-in-the-loop test platform controlling multirotor vehicles | |
CN111007877A (zh) | 一种四旋翼飞行器的全局鲁棒自适应轨迹跟踪控制方法 | |
CN112668107B (zh) | 基于立式转台的航天运载器试验方法、装置及存储介质 | |
Muhsen et al. | Optimized PID control of quadrotor system using extremum seeking algorithm | |
CN115291527B (zh) | 一种随动控制方法、***、设备及存储介质 | |
CN112182737A (zh) | 一种基于模态法的并行化高精度颤振计算方法 | |
Elajrami et al. | Drone Control using the Coupling of the PID Controller and Genetic Algorithm | |
Hussein et al. | Autopilot design for a quadcopter | |
CN111399397B (zh) | 机器人的控制方法、控制器及控制*** | |
Saleem et al. | Application and comparison of kernel functions for linear parameter varying model approximation of nonlinear systems | |
CN112925222A (zh) | 一种基于ros的无人艇运动控制仿真方法及装置 | |
CN115576334B (zh) | 一种欠驱动水下航行器编队控制方法及*** | |
CN113189956B (zh) | 两轴两框架光电稳像平台的伺服***在线调试方法 | |
CN113865485B (zh) | 离轴非球面元件用六自由度调整平台精度优化方法及*** | |
CN108803681B (zh) | 一种无刷电机增稳云台控制方法及*** | |
Kuantama et al. | SS symmetry | |
Abdulkareem et al. | The UKF Based Approach to Improving Attitude and Position of Quadcopter Through Autonomous and Non-Autonomous Flight | |
CN116400721A (zh) | 一种分布式倾转旋翼无人机控制分配方法及装置 | |
Likhachev et al. | Calibration of model parameters in navigation devices based on fiber-optic gyroscopes |
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 |