1、哈尔滨工业大学工学硕士学位论文- 34 -第四章 工程实现FOGSS 是一个精密的机电系统装置。通过工程设计工作，基本完成了前阶段预期期间的工程要求，同时发现了一些技术上的问题。本章将就硬件配置、控制系统综合、软件设计展开讨论。4.1 硬件组成硬件系统的基本框图如图 4-1 所示。系统由从结构上由两大块组成：外框架回路（组件名称上标注 1）和内框架回路（组件名称上标注 2） ，内环对外环有耦合作用，为了设计方便，通过光电编码器对环架进行了解耦。这样，每个环架实现了单入单出，并且环架的控制律的设计是类似的。在硬件设计上，作了以下考虑：1. 采用工业 PC 和运动控制卡 GO400 组成控制计算机
2、系统。这样可以避免设计繁杂的硬件电路，并且可以使用 PC 机功能齐全的软件系统，便于软件编程和系统调试。2. 采用 Compumotor 公司的直接驱动方式的无刷直流电机作为执行器。直接驱动电机的精度较高，而且可以实现传动链的简化和结构的简化，避免复杂的传动机构带来的机械加工误差。3. 系统的两级安全保护：为防止控制作用处理失误造成相机和整个框架受损，设置两级限位装置对系统进行保护。第一级为光电型行程开关，当系统超出工作范围时由软件进行故障处理，第二级为机械式挡块，阻止框架连续旋转而绞断电机信号传输电缆。4. 云台框架初始化的硬件保证：为了尽可能的减小稳定系统两框架之间的耦合，需要每次启动系统
3、自动校零。由于基座伺服回路的位置传感器为增量式光电编码器，因而在硬件设计上采用光电行程开关帮助云台系统回到理想的初始位置（相机 LOS 的水平位置） 。第 4 章 工程实现- 35 -图 4-1 FOGSS 的结构Figure 4-1 the structure of FOGSS图中IPC：研华工控机；GO400：运动控制卡；BDDM1、BDDM2：Compumotor 公司的无刷直接驱动电机DM1030B（外环）和 DM1015B（内环） ；FOG1、FOG2：KVH 公司数字输出开环光纤陀螺 Ecore 2000；Encoder1、Encoder2：电机内嵌的增量式光电编码器；Frame1
4、、Frame2：云台的外框架和内框架；4.2 数字 PID 控制控制系统综合是系统设计的核心部分。需要考虑的控制律和运动控制有： 稳定回路的控制 基座伺服回路控制 方位自动校零 俯仰自动校零IPCBDDM2Encoder2Frame2GO-400Motion controllerBDDM1Encoder1Frame1FOG1FOG2哈尔滨工业大学工学硕士学位论文- 36 - 基座伺服回路与稳定回路的相互切换由于稳定回路是系统的核心部分，我们仅仅讨论稳定回路设计。4.2.1 外环稳定回路控制器设计有了前面的模型处理工作之后，我们就可以进行控制器设计了。设计掌握的基本原则是：结构简单，调试方便；下
5、面，我们具体的就每个回路分别讨论。控制系统综合是针对简化模型的，由第二章的系统分析，我们可以得到稳定回路的简化模型如下图所示。此时摩擦力矩先假定为一个干扰。 Gc(z) ZOH mKfTJs1sT1z s)(t)(tc )(t)(tn图 4-2 稳定回路控制系统综合用框图Figure 4-2 simplified diagram of stabilization loop used for control system synthesis1. PID 控制器设计设计框图如图 4-2 所示。名义对象Gp(s) （41）Jskm其 z 传函为Gp(z)Z zoh（ Gp(s)）= = （42）)1
6、(zJTksm1Ks式中 K = 0.8208 （43）Jkm考虑光纤陀螺的一拍延迟，则整个系统的开环 z 传函为第 4 章 工程实现- 37 -Go(z) = （44）12zKTs采用 w 变换 18，z 或 w （45）Tss21s21zGo(w) = （46）)21(2wTKssGo(jv) = （47）)21(jvTjss校正前系统的对数频率特性如图 4-3 所示，图 4-3 校正前的系统的对数频率特性Figure 4-3 frequency characteristics before control system synthesis系统对斜坡角度率信号响应是有差的，为了消除位置误差
7、和提高增益，采用迟后校正，为了调试方便，设计中采用基本 II 型系统，故可取校正函数： 0 0 0 ) 1 2 3 哈尔滨工业大学工学硕士学位论文- 38 -Gc(w) = （48）Kc)1(取 w110rad/s，K c= 244，此时带宽约为 20rad/s，在调试时应在20线调整这两个参数。校正后的对数频率特性如图 4-4 所示，图 4-4 校正后系统的开环对数频率特性Figure 4-4 frequency characteristics after control system synthesis将校正函数变换到 z 域，Gc(z) = （49）1.236.5系统对指令角速度的阶跃响
8、应和斜坡响应如图 4-5 所示，在仿真中考虑了执行器饱和对系统的影响。 0 0 0 第 4 章 工程实现- 39 -图 4-5 校正后系统对角速度信号( )的阶跃响应sc/20Figure 4-5 step response to angle velocity input after control system synthesis2. 外环轴干扰力矩的影响1）干扰力矩的作用形式干扰力矩是影响平台稳定精度的主要因素。在外环，干扰力矩的主要形式是基座和外框架轴之间的摩擦耦合。其作用规律与基座的摇摆角速度有关。设基座的摇摆规律为（410）tbmsin为了定量的分析摩擦力矩对系统的影响，忽略动静摩擦
9、的差异，不考虑摩擦力矩的 Stribeck 效应，将摩擦力矩 建模为)(tTf= M im sign( t) ( t+) （411）)(tTf其 Fourier 展开级数为 （412）)(tf 1sin4imt上式表明，摩擦力矩 的频谱取决于基座摇摆角速度的频谱。)(tTf0 1 05 ) ) % 哈尔滨工业大学工学硕士学位论文- 40 -因此，分析载体的振动频谱对于确定系统稳定回路的低频带宽有重要的指导意义。2）系统对常值干扰力矩的响应校正后系统以角速度为输出的开环传函可表示为：Gco(w) = = G1(w) （413）Kc)1()2(2wTss2Kc设常值干扰力矩为=M/w （414）)
10、(tTf设干扰力矩幅值为 M=1N.m，由图 4-2 可得 (w)= = G (w)M(w)= M(w) （415）)(1Jco(2KGJicG (w)M(w)= M(w) /)12ic（416）根据终值定理，（） =0 )(lim0w（417）（） = M=0.0010rad = 0.06deg （418）)(li0wcJK13）系统对周期性干扰力矩的响应设干扰力矩为 =M sgn(vt) )(tTf（419）由频率响应的基本概念，系统稳态时的输出应为 (t)M sinvt + phase(G (jv) )(jG（420）第 4 章 工程实现- 41 - (t)M sinvt + phase
11、(G (jv) )(jG（421）忽略高频小参数，只考虑系统的第一个转折频率，可得 (t) =M sinvt + phase(G (jv) （422）212)(1(wKvJcc (t)M sinvt + phase(G (jv) （423）212)(1(vJcc由以上分析可以得出 系统的刚度与开环增益和转子的转动惯量成正比。 系统在常值干扰力矩下，稳态时有一个微小的转角。 在干扰力矩作用下，系统无长期角速度漂移。当平台的基座振动角速度分别为 0.2Hz，1Hz ，10Hz 时，假定摩擦力矩最大为 1N.m，则输出角速度曲线如图 4-6 4-8 所示，输出角度曲线如图 4-9 4-11 所示：图
12、 4-6 0.2Hz 时的输出角速度Figure 4-6 angle velocity output when 0.2Hz 2 4 6 8 1 哈尔滨工业大学工学硕士学位论文- 42 -图 4-7 1Hz 时的输出角速度 Figure 4-7 angle velocity output when 1Hz图 4-8 10Hz 时的输出角速度 Figure 4-8 angle velocity output when 10Hz0 5 4 6 8 1 ) 0 5 01 2 3 4 5 ) ) 第 4 章 工程实现- 43 -图 4-9 0.2Hz 时的输出角度Figure 4-9 angle out
13、put when 0.2Hz图 4-10 1Hz 时的输出角度 Figure 4-10 angle output when 1Hz图 4-11 10Hz 时的输出角度Figure 4-11 angle output when 10Hz从仿真结果可以看到，利用基本型控制系统，平台在不同基座摇摆角速度作用下的性能非常接近，这说明，将稳定回路设计为基本型伺服系统，对基座的摩擦力矩干扰频率的变化抑制有很好的适应性。3. 陀螺仪噪声对平台系统的影响设线性系统的输入随机信号为 (t)，输出为 y(t)，从 (s)到 y(s)的闭环传函为 C(s)，则系统的输出谱密度和输入谱密度的关系由下式给出 19： 0
14、 1 0 哈尔滨工业大学工学硕士学位论文- 44 - y(v) (v) （424）2jC输出均方误差为（425）陀螺仪输出的噪声可以近似等效为一个白噪声，其统计特性表示为：R() 2() （426）谱密度为(v) 2 （427）在白噪声作用下，系统的性能采用等效噪声带宽来表征。考虑系统的输出角速度低频模型，Gco(w) （428）21)(Kc其闭环传函为C(w) （429）cccK12所以白噪声作用下输出角速度均方误差为：2 2 （430）1wc其等效角速率噪声带宽为： b （431）21Kc当 w1 很小或很大时，系统的噪声都很大，若w1 （432）c此时等效噪声带宽最小，为dvRyy)()
15、0(2第 4 章 工程实现- 45 - b （433）2Kc设计中，陀螺仪输出噪声方差为 25.910 8 (rad/s)2 （434）设计的第一个转折频率为 w110rad/s，开环增益为 Kc200，30 25.538110 8 (rad/s)2 （435）0.0024rad/s0.135deg/s = 485.4deg/h （436）2现在分析噪声对轴角度输出的影响。类似的，从 (s)到位置角度的传函为H(w) （437）1cccKw12输出角度均方误差为（其中 BW 为光纤陀螺带宽）（438）dvjBW22)(我们对系统噪声影响做了仿真，其结果如图 4-12 所示。图 4-12 FOG
16、 噪声输入时的角度输出响应曲线（3600 秒）Figure 4-12 FOG output angle response with noise input可以看到，仿真与计算结果是一致的。综合以上计算分析和仿真，可以得İ 0 05 1 哈尔滨工业大学工学硕士学位论文- 46 -出如下结论： 陀螺仪的噪声影响平台的短期精度和长期漂移指标。 在短的时间内，陀螺仪噪声将引起平台输出轴的颤振。 在长时间内，陀螺噪声将使输出轴的角度振荡发散。4.2.2 内环稳定回路控制器设计内环 PID 控制器设计与外环相比，存在以下特有问题：1. 质量不平衡偏心干扰力矩平台质量不平衡引起的偏心力矩对系统也有一定的影响
17、，设平台内环转动部分的质量为 m，质心偏离支承的距离为 r，m 则当基座的摇摆角速度如式（4-10）所示，则表达式可：Md= mgr mr2 （439）tsin可以看到，偏心力矩分为两部分，一部分为常值干扰力矩，另一部分为周期干扰力矩，它们在内环轴输出产生的转角如式（4-18）和（4-21 ）所示。2. 转动惯量的变动由表 2-1 可以看到，由于负载的变化，引起了外环和内环转动惯量的 改变，外环转动惯量的波动是相当小的，为 1/6，对于这么小的变动，利用 PID 方法设计后的系统对该变化有足够的适应性。而内环则变化幅度为 47，所以设计后的系统应该检验系统增益在该范围变化后的性能。若不行，就不
18、能采用单独的 PID 设计。以下采用仿真的方法校验了系统的性能。3. PID 控制器检验类似的可设计内环 PID 控制器为Gci(w) = （440）Kci)1(其中 Kci162，w 1=10rad/s （441）变换到 z 域后为Gci(z) = （442）4.5.7第 4 章 工程实现- 47 -当转动惯量分别为 1.6kg.m2 和 0.851kg.m2 时的干扰响应和指令角速度响应如图 4-134-16 所示（设基座摇摆角速度为 0.2Hz，摩擦干扰力矩最大为 0.4N.m） 。图 4-13 0.2Hz 时的干扰力矩作用下的输出角度响应（惯量为 1.6kg.m2）Figure 4-1
19、3 output angle response to input friction disturbance when0.2Hz (with moment of inertia 1.6kg.m2)图 4-14 指令角速度的阶跃响应 ( r=20deg/s) （惯量为 1.6kg.m2）Figure 4-14 step response to input command angle velocity (with moment of inertia being 1.6kg.m2)0 1 05 ) ) 0 5 8 6 4 2 0 ) e 哈尔滨工业大学工学硕士学位论文- 48 -图 4-15 0.2H
20、z 时的干扰力矩输出角度响应（惯量为 0.851kg.m2）Figure 4-15 output angle response to input friction disturbance when0.2Hz (with moment of inertia being 0.851kg.m2)图 4-16 指令角速度的阶跃响应 ( r=20deg/s) （惯量为 0.851kg.m2）Figure 4-13 step response to input command angle velocity(with moment of inertia 0.851kg.m2)由上面可以看到，增益变化后系统性
21、能并没有太大的变化，这也可以从 bode 图上可以看出：采用典型 型系统设计时，系统的相频特性在剪切频率附近变化缓慢，故当增益在较大范围内上升时，系统的 1 05 ) ) 0 5 8 6 4 2 0 ) e 第 4 章 工程实现- 49 -相位裕度变化不大，因而转动惯量变化对系统性能的影响不大。4.2.3 A/D 量化噪声的影响FOG 的输出角速度经过 A/D 量化后编程数字量，由于有限字长将导致量化误差。设 q 为量化阶，即 LSB，其数值为：q0.00305 。量化误差s/因量化方法的不同而互异。该误差可等效的作为 FOG 的输出噪声在系统回路中放大，从而出现在输出端。由数字控制理论得知：
22、截尾量化噪声的统计特性为 均值： 0.0015 （443）T2qs/方差： 2.3610 10 (rad/s)2 （444）2T1舍入量化噪声的统计特性为均值：0 （445）R方差： 2.3610 10 (rad/s)2 （446）12q可以看到，它比 FOG 本体产生的噪声小两个数量级，在设计中可以忽略。4.3 软件设计设计中软件采用 Borland C+语言实现 2021。对于一个高精度的机电系统，如何在确保设备的安全性的前提下顺利调试是一个相当重要的问题。因此，除了在硬件上做一些必要的安全处理措施之外，还在软件规划中做了一些必要的故障诊断和处理。1 软件所需实现的主要任务哈尔滨工业大学工
23、学硕士学位论文- 50 -1） 系统自检和故障诊断和处理FOGSS 是一个精密的机电设备，为了使系统维护方便，除了硬件上做一些措施外，在软件里做了一些必要的容错处理。这些包括： 运动控制卡、FOG、电机驱动器的自检。在启动系统时，必须检测元器件是否正常工作，否则有可能因元器件故障而使系统受损。 系统故障报警和处理。当有故障发生时，软件能检测出该故障，并诊断其故障类型，从而做出必要的处理。主要的故障有：a) 电机过速b) 电机过载c) 云台撞到限位开关d) 驱动器工作不正常 事故急停处理：当一旦有事故发生时，系统应该立即关闭，力求将事故对系统的破坏降低到最低程度。2） 光纤陀螺仪角速率采集程序通
24、过串口中断处理程序，实时采集和组合 FOG 的数据。3） 自动校零为了使系统在基座伺服回路工作时有一个合适的参考基准，并为两个框架解耦，系统工作时应该自动校零，找到机械原点(Home)，以此作为系统工作的参照点。4） 基座电锁定控制正常工作情况下，由于稳定回路工作时系统的稳定裕度较低，为了避免环架在载体大机动时受损，系统处于基座电锁定状态，此时陀螺仪不起作用，电机伺服回路由光电编码器来闭环。5） 陀螺仪温度漂移补偿利用测试和实验得来的模型，降低温度对 FOG 的漂移的影响。6） 平台稳定当需要稳定时，通过指令将环架切换到稳定状态，使环架的运动与基座的角运动隔离。2. 软件流程软件主要包括三大块
25、：1） 主程序：包括初始化，安装中断向量，系统自检，接收命令等。第 4 章 工程实现- 51 -2） 串口中断服务程序：读取 FOG 传输到串口缓冲区的数据，组成台体相对惯性空间的角速度，并由此计算稳定调节控制律，送给执行电机，使台体相对惯性空间保持稳定。3） 定时器中断服务程序：采集光电编码器的数据，组成台体相对基座的角速度和角位移，由此计算基座伺服控制律，送给执行电机，使平台自动校零或相对基座电锁定。以下是程序流程。开 始无 故 障 ？ Yes 故 障 处 理关 机 ， 退 出 程 序自 动 校 零 状态 ？ NoNo刷 新 伺 服 数 据自 动 校 零返 回关 串 口 中 断开 中 断Y
26、es基 座 锁 定图 4-17 定时器中断服务程序流程Figure 4-17 flowchart of Timer ISR哈尔滨工业大学工学硕士学位论文- 52 -开 始元 件 自 检 与 复 位无 故 障 ？设 定 PID参 数设 定 定 时 器 定 时 时间 为 1ms复 位 轴 运 动 信 息系 统 预 热YesNo 故 障 处 理打 开 伺 服请 零 DAC无 故 障 ？YesNo故 障 处 理归 零 完 毕 ？YesNo 自 动 归 零读 入 切 换 指 令退 出退 出启 动 光 纤 陀 螺 补 偿给 伺 服 电 机 上 电稳 定 指 令进 入 稳 定 状 态关 机 指 令关 伺 服
27、基 座 伺 服 指 令基 座 伺 服有 故 障故 障 处 理急 停 指 令急 停 处 理退 出 稳 定 状 态有 切 换 指 令YesNo退 出退 出图 4-18 主程序工作流程Figure 4-18 flowchart of main procedure第 4 章 工程实现- 53 -开 始无 故 障 ？ Yes 故 障 处 理关 机 ， 退 出 程 序校 验 错 误NoYes 返 回No传 输 完 毕YesNo 返 回接 收 数 据组 成 光 纤 陀 螺 数 据光 纤 陀 螺 温 漂 补 偿返 回关 串 口 中 断开 中 断开 始关 定 时 器 中 断开 串 口 中 断有 切 换 指 令N
28、oYes 返 回 主 程 序组 成 稳 定 控 制 律刷 新 DAC图 4-19 稳定回路和串口中断服务程序流程Figure 4-19 flowchart of stabilization procedure and serial port ISR4.4 本章小结本章从工程的角度比较完整的讨论了 FOGSS 的数字 PI 控制系统综合和软硬件实现。控制系统综合所用的模型必须是简化的模型，这样可以抓住关键，并且所设计的控制器实现简单，从而在调试起来也比较方便，由此可以发现系统中的一些不确定问题。控制系统综合后，对误差源影响进行了一些解析计算和仿真估计。在软件和硬件处理中则是考虑了系统的容错设计，从而为系统的调试提供安全的保障。实际运行与理论上的计算是一致的。后来通过测试表明，外环的库仑摩擦哈尔滨工业大学工学硕士学位论文- 54 -力矩幅值大小为 1.5N.m，由此引起的稳定误差在 1.5mrad（ ） 。在调试中09.由于内环的摩擦力矩比较大，所以采用了混和式稳定系统配置方式。
