[发明专利]一种用于火箭垂直度全自动调整的控制系统及控制方法

权利要求书

1.一种用于火箭垂直度全自动调整方法，其特征在于，该方法基于一种用于火箭垂直度全自动调整的控制系统实现，该控制系统包括箭体垂直度测量系统、电气控制系统和支撑臂机构；所述的箭体垂直度测量系统包括水平测量仪和自动计算模块；所述的电气控制系统包括发射台电控上位机、支撑臂控制器和人机交互系统；所述的支撑臂机构为火箭垂直度全自动调整的执行机构，包括伺服电机、位移传感器、力传感器和支撑臂，安装于发射平台上；

所述的水平测量仪设置于箭体上，与所述的自动计算模块连接，所述的水平测量仪用于测量水平数据，实时提供水平数据，并将测量的水平数据通过信号电缆传输至自动计算模块，所述的自动计算模块将水平测量仪测量的水平数据自动解算为箭体垂直度参数；

所述的自动计算模块与发射台电控上位机连接，发射台电控上位机与支撑臂控制器连接，支撑臂控制器与人机交互系统连接；其中，自动计算模块将解算的箭体垂直度参数的数据传输至发射台电控上位机，再由发射台电控上位机将数据传输至支撑臂控制器，支撑臂控制器自动对数据进行解算，将结果显示至人机交互系统中，并将接收的数据与系统设定的垂直度要求进行比对，根据比对结果发出相应的控制指令，如满足垂直度要求，则不进行调整；如不满足要求，则需进行垂直度调整；人机交互系统用于显示当前的火箭垂直度参数与火箭安全发射的垂直度要求范围的比对结果，也用于对支撑臂控制器的参数进行调整；

所述的支撑臂机构包括多个支撑臂和多个伺服电机，每个支撑臂与一个伺服电机连接，伺服电机为支撑臂进行升降动作的动力源，支撑臂设置于发射平台上，箭体下端，支撑臂内置位移传感器和力传感器，分别用于实时监测支撑臂的运动量和实时监测各支撑臂的载荷情况；所述的支撑臂控制器与支撑臂机构的伺服电机连接，如需进行垂直度调整，支撑臂控制器则控制各个伺服电机转动，从而通过伺服电机的转动带动支撑臂按照指令做出相应的升降动作，从而完成火箭垂直度全自动调整工作；

所述的火箭垂直度全自动调整方法，包括以下步骤：

S1：箭体起竖到位，处于空载状态，置于箭体上的水平测量仪与自动计算模块已接通良好，处于工作状态，电气控制系统与支撑臂机构通讯良好，处于工作状态；

S2：通过水平测量仪实时检测箭体水平数据，并实时向自动计算模块发送；自动计算模块对水平数据进行解算及坐标系转换，得出垂直度数值传输至支撑臂控制器；

S3：支撑臂控制器将实时解算的垂直度数值与预存的火箭安全发射的垂直度要求范围进行比对并判断，将比对结果发送至人机交互系统；支撑臂控制器依据比对结果进行判断是否需要进行调整动作，如果对比结果不满足火箭安全发射的垂直度要求范围，则需进行全自动火箭垂直度调整；

S4：全自动火箭垂直度调整具体方式为：支撑臂控制器分别计算出各支撑臂各自的期望轨迹，由各支撑臂的期望轨迹和位移传感器的高度值之差组成闭环进行PID控制；在调整过程中求出每个时刻期望平面的运动方程，从而计算出各个伺服电机的期望输出；

S4.1：利用水平测量仪及自动计算模块，解算出4个支撑臂的当前相对高度值，假设发射平台为刚体，则期望平面由最高的三个支撑臂确定；

S4.2：计算步骤S4.1中确定的期望平面与各支撑臂当前位置的相对高度值，即，最高的三个支撑臂的平均相对高度值和各支撑臂相应的期望相对高度值；

S4.3：根据步骤S4.2中计算的各支撑臂的期望相对高度值及伺服电机启停运动的最小时间，计算各支撑臂的期望运动速度，运动轨迹为S曲线，以使运动过程的起停更平稳，提高支撑臂的控制精度，即单位时间内的运动距离，有利于避免各支撑臂载荷不均的情况发生；

S4.4：根据步骤S4.2中计算的各支撑臂的期望相对高度值和步骤S4.3中计算的期望运动速度，得出各支撑臂期望的运动规划轨迹，其与各支撑臂实际运动轨迹之间的差值，反应了各支撑臂跟踪期望平面的好坏；

S4.5：在调整过程中，利用位移传感器实时监测各支撑臂的运动位移，实时计算当前各支撑臂的平均平面高度，当前时刻下各支撑臂的实际高度与平均平面的高度差值，反应了运动过程中平台保持在同一平面的运动性能，即垂直度调整过程中箭体稳定性的保持能力；

S4.6：调用PID算法进行差值调节，作为支撑臂控制器对垂直度调整过程中实时的差值修正，根据当前时刻的实际运动与期望运动的高度差值，控制器实时分配予不同位置的支撑臂的伺服电机以不同的调整量及调整速度，直至火箭垂直度满足安全可靠发射的要求。