CN115291527B - 一种随动控制方法、系统、设备及存储介质 - Google Patents

Abstract

本发明涉及随动控制技术领域，具体地说，涉及一种随动控制方法、系统、设备及存储介质，首先通过采集前倾角α、横滚角β、航向角δ，计算出理论俯仰角ζ₁和理论回转角θ₁，然后根据理论俯仰角ζ₁和理论回转角θ₁，并结合俯仰编码器和回转编码器的反馈值，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o），最后转化为与俯仰驱动器和回转驱动器对应的数据格式，并通过CAN总线传输至相应的驱动器，驱动器跟随行进引起的变动实时调整俯仰角和回转角，在减小控制难度的同时，解决了随动系统的振动和抖动改变俯仰角和回转角的问题。

Description

一种随动控制方法、系统、设备及存储介质

技术领域

本发明涉及随动控制技术领域，具体地说，涉及一种随动控制方法、系统、设备及存储介质。

背景技术

随动系统servo system是一种反馈控制系统。在这类系统中，它的参考输入不是时间的解析函数，如何变化事先并不知道是随着时间任意变化，控制系统的任务是在各种情况下保证输出以一定精度跟随参考输入的变化而变化。根据其任务的性质分析可知快速跟踪和准确定位是随动系统的两个非常重要的性能指标。要满足相关的性能指标，首要要有精准的检测元器件和良好的执行元件，另外随动控制是完全依靠伺服系统对目标信号的响应，当加速度过大而超过系统的响应能力，会出现随动系统的振动或抖动等现象，控制难度较大。控制算法是随动控制系统的核心灵魂，因此亟需一种准确、高效的控制算法，以解决随动系统的振动和抖动、以及控制难度较大的问题。

发明内容

本发明针对上述随动系统的振动和抖动、以及控制难度较大的问题，提出一种随动控制方法、系统、设备及存储介质，首先通过采集前倾角α、横滚角β、航向角δ计算出理论俯仰角ζ₁和理论回转角θ₁，然后根据理论俯仰角ζ₁和理论回转角θ₁，以及俯仰编码器反馈的实际俯仰角

和回转编码器反馈的实际回转角

，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o），最后转化为与俯仰驱动器和回转驱动器对应的数据格式，并通过CAN总线传输至相应的驱动器，驱动器跟随行进引起的变动实时调整俯仰角和回转角，在减小控制难度的同时，解决了随动系统的振动和抖动改变俯仰角和回转角的问题。

本发明具体实现内容如下：

一种随动控制方法，包括以下步骤：

步骤1：将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ，以及目标俯仰角ζ、目标回转角θ输入至随动控制系统中，计算出理论俯仰角ζ₁和理论回转角θ₁；

步骤2：根据从俯仰编码器获取的实际俯仰角

和从回转编码器获取的实际回转角

，以及步骤1中计算出的理论俯仰角ζ₁和理论回转角θ₁，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o）；

步骤3：将俯仰方向的输出加速度a_fy（o）、俯仰方向的输出速度V_fy（o）转化为与俯仰驱动器对应的数据格式，将回转方向的输出加速度a_hz（o）、回转方向的输出速度V_hz（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^,,,）下的俯仰角和回转角，得到理论俯仰角ζ₁和理论回转角θ₁。

为了更好地实现本发明，进一步地，所述步骤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：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的三次方与比例系数K_fy的乘积、平衡系数D_fy，计算出俯仰方向的目标速度V_fy；根据理论回转角θ₁与回转编码器获取的实际回转角

差的三次方与比例系数K_hz的乘积、平衡系数D_hz，计算出回转方向的目标速度V_hz；

步骤22：判断计算出的俯仰方向的目标速度V_fy与设定的俯仰最大速度V_max（fy）的大小关系、回转方向的目标速度V_hz与设定的回转最大速度V_max（hz）的大小关系，若俯仰方向的目标速度V_fy大于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于设定的俯仰最大速度V_max（fy），若俯仰方向的目标速度V_fy小于或等于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于俯仰方向的目标速度V_fy；若回转方向的目标速度V_hz大于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz（o）等于设定的回转最大速度V_max（hz），若回转方向的目标速度V_hz小于或等于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz(o)等于回转方向的目标速度V_hz；

步骤23：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_fy的乘积，计算出俯仰方向的目标加速度a_fy；根据理论回转角θ₁与回转编码器获取的实际回转角

差的二次方与平衡系数K_hz的乘积，计算出回转方向的目标加速度a_hz；

步骤24：判断计算出的俯仰方向的目标加速度a_fy与设定的俯仰最大加速度a_max（fy）的大小关系、回转方向的目标加速度a_hz与设定的回转最大加速度a_max（hz）的大小关系，若俯仰方向的输出速度V_fy(o)等于设定的最大速度V_max（fy），则俯仰方向的输出加速度a_fy（o）为0，若俯仰方向的输出速度V_fy（o）小于设定的最大速度V_max（fy），且计算出俯仰方向的目标加速度a_fy小于设定的最大加速度a_max（fy），则俯仰方向的输出加速度a_fy（o）为计算出的俯仰方向的目标加速度a_fy，若俯仰方向的目标加速度a_fy大于设定的最大加速度a_max（fy），则根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_fy、迭代基数η的n次方的乘积，计算出新的俯仰方向的目标加速度a_fy’，直到目标加速度a_fy’小于等于a_max（fy），输出俯仰方向的输出加速度a_f（o）等于目标加速度a_fy’；若回转方向的输出速度V_hz(o)等于设定的回转最大速度V_max（hz），则回转方向的输出加速度a_hz（o）为0，若回转方向的输出速度V_hz（o）小于设定的回转最大速度V_max（hz），且回转方向的目标加速度a_hz小于设定的回转最大加速度a_max（hz），则回转方向的输出加速度a_hz（o）等于计算出的回转方向的目标加速度a_hz，若回转方向的目标加速度a_hz大于设定的回转最大加速度a_max（hz），则根据理论回转角θ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_hz、迭代基数η的n次方，计算出新的回转方向的目标加速度a_hz’，直到目标加速度a_hz’小于回转最大加速度等于a_max（hz），输出回转方向的输出加速度a_hz（o）等于目标加速度a_hz’。

基于上述提出的随动控制方法，为了更好地实现本发明，进一步地，提出一种随动控制系统，与控制计算机连接，包括俯仰驱动器、俯仰编码器、回转驱动器、回转编码器、双轴倾角仪、寻北仪；

所述双轴倾角仪通过CAN总线与控制计算机连接，用于获 取前倾角α、横滚角β；

所述寻北仪通过CAN总线与控制计算机连接，用于获取航向角δ；

所述俯仰编码器通过CAN总线与控制计算机连接，用于获取实际俯仰角

；

所述回转编码器通过CAN总线与控制计算机连接，用于获取实际回转角

；

所述俯仰驱动器通过CAN总线与控制计算机、俯仰编码器连接，用于接收转化后的俯仰方向的输出加速度a_fy、俯仰方向的输出速度V_fy；

所述回转驱动器通过CAN总线与控制计算机、回转编码器连接，用于接收转化后的回转方向的输出加速度a_fy、回转方向的输出速度V_fy。

为了更好地实现本发明，进一步地，提出一种电子设备，包括存储器、处理器，所述存储器用于存储计算机程序；

所述处理器，用于执行所述计算机程序时实现上述的随动控制方法的步骤。

为了更好地实现本发明，进一步地，提出一种计算机存储介质，存储有一个或多个计算机程序，所述一个或多个计算机程序被一个或多个处理器执行时，致使所述一个或多个处理器执行上述的随动控制方法的步骤。

本发明具有以下有益效果：

（1）本发明通过理论俯仰角ζ₁和理论回转角θ₁，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o），最后转化为与俯仰驱动器和回转驱动器对应的数据格式，并通过CAN总线传输至相应的驱动器实时调整俯仰角和回转角，在减小控制难度的同时，解决了随动系统的振动和抖动改变俯仰角和回转角的问题。

（2）本发明通过理论角与实际角差的三次方，以三次曲线的形式对速度进行控制，同时通过理论理论角与实际角差的二次方，以二次曲线的形式对加速度进行控制，提高了系统响应速度，且速度变化更加平滑。

（3）本发明对加速度的控制，不是简单进行比较大小，然后机械赋值操作，而是按相关的物理学规律进行插值迭代计算，按进行插值，周期循环执行上述的算法，使得加速度变化更加平稳，最大限度的避免突变，从而使得运动更加平稳。

（4）本发明与传统的5次样条曲线算法比较，计算更为简单，且运算周期更短。

附图说明

图1为随动平台结构示意图；

图2为随动控制系统连接框图；

图3为坐标系位置关系示意图；

图4为随动控制系统运行流程示意图；

图5为本发明实施例中回转正弦波测试结果示意图；

图6为本发明实施例中俯仰正弦波测试结果示意图；

图7为本发明实施例中回转随动结果示意图；

图8为本发明实施例中俯仰随动结果示意图；

其中，1、随动平台，2、回转平台，3、俯仰数字缸，4、回转数字缸，5、传感器组件。

具体实施方式

为了更清楚地说明本发明实施例的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，应当理解，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例，因此不应被看作是对保护范围的限定。基于本发明中的实施例，本领域普通技术工作人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“设置”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；也可以是直接相连，也可以是通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

实施例1：

本实施例提出一种随动控制方法，如图4所示，包括以下步骤：

步骤1：将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ，以及目标俯仰角ζ、目标回转角θ输入至随动控制系统中，计算出理论俯仰角ζ₁和理论回转角θ₁；

步骤2：根据从俯仰编码器获取的实际俯仰角

和从回转编码器获取的实际回转角

，以及步骤1中计算出的理论俯仰角ζ₁和理论回转角θ₁，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o）；

步骤3：将俯仰方向的输出加速度a_fy（o）、俯仰方向的输出速度V_fy（o）转化为与俯仰驱动器对应的数据格式，将回转方向的输出加速度a_hz（o）、回转方向的输出速度V_hz（o）转化为与回转驱动器对应的数据格式，并通过CAN总线传输至相应的驱动器。

工作原理：本实施例首先通过采集前倾角α、横滚角β、航向角δ，以及目标俯仰角ζ、目标回转角θ，计算出理论俯仰角ζ₁和理论回转角θ₁，然后根据俯仰编码器反馈的实际俯仰角

和回转编码器反馈的实际回转角

，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（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相同，故不再赘述。

实施例3：

本实施例在上述实施例1-2任一项的基础上，对步骤2的具体步骤进行说明。

工作原理：所述步骤2具体包括以下步骤：

步骤21：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的三次方与比例系数K_fy的乘积、平衡系数D_fy，计算出俯仰方向的目标速度V_fy；根据理论回转角θ₁与回转编码器获取的实际回转角

差的三次方与比例系数K_hz的乘积、平衡系数D_hz，计算出回转方向的目标速度V_hz；

步骤22：判断计算出的俯仰方向的目标速度V_fy与设定的俯仰最大速度V_max（fy）的大小关系、回转方向的目标速度V_hz与设定的回转最大速度V_max（hz）的大小关系，若俯仰方向的目标速度V_fy大于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于设定的俯仰最大速度V_max（fy），若俯仰方向的目标速度V_fy小于或等于设定的俯仰最大速度_Vmax（fy），则俯仰方向的输出速度V_fy（o）等于俯仰方向的目标速度V_fy；若回转方向的目标速度V_hz大于设定的回转最大速度_Vmax（hz），则回转方向的输出速度V_hz（o）等于设定的回转最大速度V_max（hz），若回转方向的目标速度V_hz小于或等于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz(o)等于回转方向的目标速度V_hz；

步骤23：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_fy的乘积，计算出俯仰方向的目标加速度a_fy；根据理论回转角θ₁与回转编码器获取的实际回转角

差的二次方与平衡系数K_hz的乘积，计算出回转方向的目标加速度a_hz；

步骤24：判断计算出的俯仰方向的目标加速度a_fy与设定的俯仰最大加速度a_max（fy）的大小关系、回转方向的目标加速度a_hz与设定的回转最大加速度a_max（hz）的大小关系，若俯仰方向的输出速度V_fy(o)等于设定的最大速度V_max（fy），则俯仰方向的输出加速度a_fy（o）为0，若俯仰方向的输出速度V_fy（o）小于设定的最大速度V_max（fy），且计算出俯仰方向的目标加速度a_fy小于设定的最大加速度a_max（fy），则俯仰方向的输出加速度a_fy（o）为计算出的俯仰方向的目标加速度a_fy，若俯仰方向的目标加速度a_fy大于设定的最大加速度a_max（fy），则根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_fy、迭代基数η的n次方的乘积，计算出新的俯仰方向的目标加速度a_fy’，直到目标加速度a_fy’小于等于a_max（fy），输出俯仰方向的输出加速度a_f（o）等于目标加速度a_fy’；若回转方向的输出速度V_hz(o)等于设定的回转最大速度V_max（hz），则回转方向的输出加速度a_hz（o）为0，若回转方向的输出速度V_hz（o）小于设定的回转最大速度V_max（hz），且回转方向的目标加速度a_hz小于设定的回转最大加速度a_max（hz），则回转方向的输出加速度a_hz（o）等于计算出的回转方向的目标加速度a_hz，若回转方向的目标加速度a_hz大于设定的回转最大加速度a_max（hz），则根据理论回转角θ₁与从俯仰编码器获取的实际俯仰角

差的二次方与平衡系数K_hz、迭代基数η的n次方，计算出新的回转方向的目标加速度a_hz’，直到目标加速度a_hz’小于回转最大加速度等于a_max（hz），输出回转方向的输出加速度a_hz（o）等于目标加速度a_hz’。

本实施例的其他部分与上述实施例1-2任一项相同，故不再赘述。

实施例4：

本实施例在上述实施例1-3任一项的基础上，如图2所示，提出一种随动控制系统，与控制计算机连接，包括俯仰驱动器、俯仰编码器、回转驱动器、回转编码器、双轴倾角仪、寻北仪；

所述双轴倾角仪通过CAN总线与控制计算机连接，用于获 取前倾角α、横滚角β；

所述寻北仪通过CAN总线与控制计算机连接，用于获取航向角δ；

所述俯仰编码器通过CAN总线与控制计算机连接，用于获取实际俯仰角

；

所述回转编码器通过CAN总线与控制计算机连接，用于获取实际回转角

；

所述俯仰驱动器通过CAN总线与控制计算机、俯仰编码器连接，用于接收转化后的俯仰方向的输出加速度a_fy、俯仰方向的输出速度V_fy；

所述回转驱动器通过CAN总线与控制计算机、回转编码器连接，用于接收转化后的回转方向的输出加速度a_fy、回转方向的输出速度V_fy。

本实施例的其他部分与上述实施例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上任意一点A的模为R，那么：

所以：

在原坐标系下的俯仰角ζ，回转角为θ，故在新坐标系的俯仰角ζ₁，回转角，与正北方向的回转角为θ₁；

2.位移控制算法

根据随动控制的特点，要求系统既要快速响应跟随到目标位置，又要运行平稳，不能出现抖动、爬行等现象，这个系统研究带来较大的难度，进一步研究发现，在随动过程中加速时，惯性动态力矩或力作用在进给驱动结构上，如果进给驱动加速指令不平滑，施加在驱动电机上的力就会产生冲击，并产生高阶震荡，这些高频分量将激励进给驱动的结构动态响应，引起不希望的振动。另外，在系统中适用了基于步进电机的数字液压缸，由于步进电机属于开环控制，自身没有反馈，因此调整更加困难。为了获得快速响应和平稳的运行速度，提出基于物理规律的加速度和速度同时控制的算法模型，具体算法如下：

---③

将其对时间t求导后得：

那么当

，时需按进行迭代计算：

其中，v为最终的输出速度；v_max为系统设定的最大速度；K为现场调试确定出的比例系数；Ф为目标角度，即控制量解算算法解算的理论角度；

为编码器反馈的角度；D为平衡系数；a_max为系统设定的最大加速度；a_x为系统计算的目标加速度；η为迭代基数取值范围为（0.9，1.0），n为迭代次数，取值为自然数集。

计算步骤：首先使用上述公式③计算目标速度，然后利用公式④计算对应的加速度，但如果计算的结果

，需要利用公式⑤进行迭代计算，直到计算结果

结束迭代，输出

和V。

3.两种算法的结合应用

在本实施例的应用环境中，首先将双轴倾角仪测得前倾角α、横滚角β、以及寻北仪测得航向角δ，以及操作者输入的目标角度俯仰和回转代入控制量解算算法中，计算出俯仰方向和回转方向的两个实际控制角度，然后再将两个方向的控制量传入位移控制算法，并结合俯仰编码器和回转编码器的实际角度，计算电机的输出加速度a_x和速度V_x，最后通调用软件的接口转化模块程序，转化成驱动器可识别的数据格式，通过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：将从双轴倾角仪获取的前倾角α、横滚角β、从寻北仪获取的航向角δ，以及目标俯仰角ζ、目标回转角θ输入至随动控制系统中，计算出的理论俯仰角ζ₁和理论回转角θ₁；

步骤2：根据从俯仰编码器获取的实际俯仰角Υ_fy和从回转编码器获取的实际回转角Υ_hz，以及步骤1中计算出的理论俯仰角ζ₁和理论回转角θ₁，计算出俯仰方向的输出加速度a_fy（o）、回转方向的输出加速度a_hz（o）、俯仰方向的输出速度V_fy（o）、回转方向的输出速度V_hz（o）；

步骤3：将俯仰方向的输出加速度a_fy（o）、俯仰方向的输出速度V_fy（o）转化为与俯仰驱动器对应的数据格式，将回转方向的输出加速度a_hz（o）、回转方向的输出速度V_hz（o）转化为与回转驱动器对应的数据格式，并通过CAN总线传输至相应的驱动器；

所述步骤2具体包括以下步骤：

步骤21：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的三次方与比例系数K_fy的乘积、平衡系数D_fy，计算出俯仰方向的目标速度V_fy；根据理论回转角θ₁与回转编码器获取的实际回转角Υ_hz差的三次方与比例系数K_hz的乘积、平衡系数D_hz，计算出回转方向的目标速度V_hz；

步骤22：判断计算出的俯仰方向的目标速度V_fy与设定的俯仰最大速度V_max（fy）的大小关系、回转方向的目标速度V_hz与设定的回转最大速度V_max（hz）的大小关系，若俯仰方向的目标速度V_fy大于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于设定的俯仰最大速度V_max（fy），若俯仰方向的目标速度V_fy小于或等于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于俯仰方向的目标速度V_fy；若回转方向的目标速度V_hz大于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz（o）等于设定的回转最大速度V_max（hz），若回转方向的目标速度V_hz小于或等于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz(o)等于回转方向的目标速度V_hz；

步骤23：根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的二次方与平衡系数K_fy的乘积，计算出俯仰方向的目标加速度a_fy；根据理论回转角θ₁与回转编码器获取的实际回转角Υ_hz差的二次方与平衡系数K_hz的乘积，计算出回转方向的目标加速度a_hz；

步骤24：判断计算出的俯仰方向的目标加速度a_fy与设定的俯仰最大加速度a_max（fy）的大小关系、回转方向的目标加速度a_hz与设定的回转最大加速度a_max（hz）的大小关系，若俯仰方向的输出速度V_fy(o)等于设定的最大速度V_max（fy），则俯仰方向的输出加速度a_fy（o）为0，若俯仰方向的输出速度V_fy（o）小于设定的最大速度V_max（fy），且计算出俯仰方向的目标加速度a_fy小于设定的最大加速度a_max（fy），则俯仰方向的输出加速度a_fy（o）为计算出的俯仰方向的目标加速度a_fy，若俯仰方向的目标加速度a_fy大于设定的最大加速度a_max（fy），则根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的二次方与平衡系数K_fy、迭代基数η的n次方的乘积，计算出新的俯仰方向的目标加速度a_fy’，直到目标加速度a_fy’小于等于a_max（fy），输出俯仰方向的输出加速度a_f（o）等于目标加速度a_fy’；若回转方向的输出速度V_hz(o)等于设定的回转最大速度V_max（hz），则回转方向的输出加速度a_hz（o）为0，若回转方向的输出速度V_hz（o）小于设定的回转最大速度V_max（hz），且回转方向的目标加速度a_hz小于设定的回转最大加速度a_max（hz），则回转方向的输出加速度a_hz（o）等于计算出的回转方向的目标加速度a_hz，若回转方向的目标加速度a_hz大于设定的回转最大加速度a_max（hz），则根据理论回转角θ₁与从俯仰编码器获取的实际俯仰角Υ_hz差的二次方与平衡系数K_hz、迭代基数η的n次方，计算出新的回转方向的目标加速度a_hz’，直到目标加速度a_hz’小于回转最大加速度等于a_max（hz），输出回转方向的输出加速度a_hz（o）等于目标加速度a_hz’。

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^,,,）下的俯仰角和回转角，得到理论俯仰角ζ₁和理论回转角θ₁。

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总线与控制计算机、俯仰编码器连接，用于根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的三次方与比例系数K_fy的乘积、平衡系数D_fy，计算出俯仰方向的目标速度V_fy，判断计算出的俯仰方向的目标速度V_fy与设定的俯仰最大速度V_max（fy）的大小关系，若俯仰方向的目标速度V_fy大于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于设定的俯仰最大速度V_max（fy），若俯仰方向的目标速度V_fy小于或等于设定的俯仰最大速度V_max（fy），则俯仰方向的输出速度V_fy（o）等于俯仰方向的目标速度V_fy；根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的二次方与平衡系数K_fy的乘积，计算出俯仰方向的目标加速度a_fy；判断计算出的俯仰方向的目标加速度a_fy与设定的俯仰最大加速度a_max（fy）的大小关系，若俯仰方向的输出速度V_fy(o)等于设定的最大速度V_max（fy），则俯仰方向的输出加速度a_fy（o）为0，若俯仰方向的输出速度V_fy（o）小于设定的最大速度V_max（fy），且计算出俯仰方向的目标加速度a_fy小于设定的最大加速度a_max（fy），则俯仰方向的输出加速度a_fy（o）为计算出的俯仰方向的目标加速度a_fy，若俯仰方向的目标加速度a_fy大于设定的最大加速度a_max（fy），则根据理论俯仰角ζ₁与从俯仰编码器获取的实际俯仰角Υ_fy差的二次方与平衡系数K_fy、迭代基数η的n次方的乘积，计算出新的俯仰方向的目标加速度a_fy’，直到目标加速度a_fy’小于等于a_max（fy），输出俯仰方向的输出加速度a_f（o）等于目标加速度a_fy’；

所述回转驱动器通过CAN总线与控制计算机、回转编码器连接，用于根据理论回转角θ₁与回转编码器获取的实际回转角Υ_hz差的三次方与比例系数K_hz的乘积、平衡系数D_hz，计算出回转方向的目标速度V_hz，判断计算出的回转方向的目标速度V_hz与设定的回转最大速度V_max（hz）的大小关系，若回转方向的目标速度V_hz大于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz（o）等于设定的回转最大速度V_max（hz），若回转方向的目标速度V_hz小于或等于设定的回转最大速度V_max（hz），则回转方向的输出速度V_hz(o)等于回转方向的目标速度V_hz；根据理论回转角θ₁与回转编码器获取的实际回转角Υ_hz差的二次方与平衡系数K_hz的乘积，计算出回转方向的目标加速度a_hz；判断计算出的回转方向的目标加速度a_hz与设定的回转最大加速度a_max（hz）的大小关系，若回转方向的输出速度V_hz(o)等于设定的回转最大速度V_max（hz），则回转方向的输出加速度a_hz（o）为0，若回转方向的输出速度V_hz（o）小于设定的回转最大速度V_max（hz），且回转方向的目标加速度a_hz小于设定的回转最大加速度a_max（hz），则回转方向的输出加速度a_hz（o）等于计算出的回转方向的目标加速度a_hz，若回转方向的目标加速度a_hz大于设定的回转最大加速度a_max（hz），则根据理论回转角θ₁与从俯仰编码器获取的实际俯仰角Υ_hz差的二次方与平衡系数K_hz、迭代基数η的n次方，计算出新的回转方向的目标加速度a_hz’，直到目标加速度a_hz’小于回转最大加速度等于a_max（hz），输出回转方向的输出加速度a_hz（o）等于目标加速度a_hz’。

5.一种控制计算机，其特征在于，包括存储器、处理器，所述存储器用于存储计算机程序；

所述处理器，用于执行所述计算机程序时实现如权利要求1-3任一项所述的随动控制方法的步骤。

6.一种计算机存储介质，其特征在于，存储有一个或多个计算机程序，所述一个或多个计算机程序被一个或多个处理器执行时，致使所述一个或多个处理器执行如权利要求1-3中任一项所述的随动控制方法的步骤。

Images