CN112296998A - 一种混合式臂架末端姿态直线控制系统及方法 - Google Patents

Abstract

本发明涉及一种混合式臂架末端姿态直线控制系统及方法，包括控制器、转台编码器、转台、比例多路阀组、托架、伸缩臂编码器、伸缩臂、伸缩臂油缸、伸缩位移传感器、折臂油缸、折臂编码器、折臂、平台编码器、平台内外翻转机构、平台左右摆动机构、平台前后翻转机构、平台倾角传感器、俯仰油缸、控制柜、控制面板等。具体方法：1)相邻点递归差补逼近算法对轨迹坐标进行结算；2)判断是否在安全范围；3)臂架直线运动控制。本发明的优点在于可以智能实现混合式臂架末端姿态沿直线运动，满足特种环境下作业需求，简化操作流程，提高作业效率，保证作业安全。

Description

一种混合式臂架末端姿态直线控制系统及方法

技术领域

本发明涉及飞机除冰车、高空作业救援车等领域，具体是一种混合式臂架末端姿态直线控制系统及方法。

背景技术

混合式臂架系统被用于飞机除冰车、高空作业车、云梯救援车等领域，操作人员通过比例手柄或开关按钮独立控制臂架回转，大臂伸缩，折臂调节等动作，将作业平台投放到欲指定位置，进行相关功能作业。此种控制方法，臂架末端轨迹为圆弧，在特殊场合，例如飞机机翼除冰、高空作业车需要直线规避空中障碍等工况下，要求操作人员需掌握一定的专业技巧，不断通过肉眼观察末端姿态，反复独立控制臂架回转、大臂伸缩、折臂摆动，可以近似达到直线控制或规避障碍的效果，但是其控制效果差、效率低、操作要求高，不能满足快速作业需求，有必要提出一种混合式臂架末端姿态直线控制系统及方法。

发明内容

本发明专利的目的是提供一种混合式臂架末端姿态直线运动的高精度控制系统及方法，满足特定操作环境下，臂架末端保持直线运动状态，简化操作流程，提高作业效率，保证作业安全。

为实现以上目的，本发明采用如下系统方案：一种混合式臂架末端姿态直线控制系统，包括控制器、转台编码器、转台、比例多路阀组、托架、伸缩臂编码器、伸缩臂、伸缩臂油缸、伸缩位移传感器、折臂油缸、折臂编码器、折臂、平台编码器、平台内外翻转机构、平台上下翻转机构、平台左右摆动机构、平台倾角传感器、俯仰油缸、控制柜、控制面板，所述控制器安装在控制柜内，采集传感器与开关信号，通过算法控制转台回转、伸缩臂伸缩、折臂摆动、平台内外翻转、平台上下翻转、平台左右摆动，控制器设置有CAN通讯接口、开关量输入端口、模拟量输入端口、开关量输出端口、PWM输出端口、模拟量输出端口，其中输入端口与转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器、平台编码器、平台倾角传感器、控制面板上的开关及比例手柄相连，输出端通过比例多路阀组或直接与转台、伸缩臂油缸、折臂油缸、平台内外翻转机构、平台上下翻转翻转机构、平台左右摆动机构、俯仰油缸相连，CAN接口与面板上显示终端相连。

所述平台内外翻转机构安装在转台上，驱动平台围绕Y轴旋转，保持直线运动过程中调节平台与面-XY夹角β。

所述平台上下翻转机构安装在转台上，驱动平台围绕X轴旋转，调节平台与面-XZ夹角δ。

所述平台左右摆动机构安装在转台上，驱动平台围绕Z轴摆动，保持直线运动过程中调节平台侧面与面-YZ夹角γ。

所述伸缩臂编码器安装端面与托架固定，旋转轴与伸缩臂旋转轴相连，测量伸缩臂与托架的相对旋转角度α。

转台编码器安装端面与托架固定，旋转轴与转台旋转轴心相连，测量回转转台旋转角度φ。

所述伸缩位移传感器安装在伸缩油缸上，测量油缸伸缩位移，得到伸缩臂的长度l ₁。

所述控平台倾角传感器安装在平台侧面上，测量平台与面-YZ的夹角及与面-XY的夹角β。

所述折臂编码器安装端面与伸缩臂固定，旋转轴与折臂旋转轴心相连，测量折臂与伸缩臂之间的夹角θ。

所述平台编码器安装在平台左右摆动机构上，测量平台与面-YZ的夹角γ。

所述控制柜安装于转台上，用于安装控制器与指令开关，并起到防尘、防震、防水作用。

所述控制面板包括2个比例手柄、3个三位旋转复位开关、4个三位旋转自锁开关、1个自锁按钮、1个急停开关、1个电源开关、1个电源指示灯、1个车载终端。

所述比例手柄属于双轴比例手柄，分别控制伸缩臂上行、下行、伸展、回收及转台左转、转台右转、折臂上摆、折臂下摆。

所述三位旋转复位开关为平台内外翻转、平台左右摆动、平台上下翻转功能按钮，控制平台围绕X、Y、Z轴旋转。

所述三位旋转自锁开关包括左右直线、前后直线、上下直线、作业功能4个按钮，左右直线按钮控制臂架末端沿着Y轴做直线运动，前后直线控制臂架末端沿着X轴做直线运动，上下直线控制臂架末端沿着Z轴做直线运动，作业功能按钮根据对象不同控制作业模式不同，例如飞机除冰车的蒸汽发生系统控制、消防云梯车水泡控制等。

所述自锁按钮为直线运动基准按钮，记录当前末端状态，建立臂架末端沿着特定方向直线运动的坐标系基准，例如：飞机除冰车清除飞机机翼冰层时，需要建立适应沿机翼方向做左右、前后直线运动的空间基准坐标系。

所述车载终端为触摸显示屏，显示臂架状态及安全报警信息，在线修改系统内部参数，设置直线运动速度等。

一种混合式臂架末端姿态直线控制方法，通过建立的数学控制模型，得到任意时刻回转转台旋转角度φ、伸缩臂与托架的相对夹角ɑ，折臂与伸缩臂夹角θ，伸缩臂长度l ₁，折臂长度l ₂与末端初始状态空间坐标x ₀、y ₀、z ₀之间关系式，将直线离散成多个点，通过相邻点递归差补逼近的方法，控制器根据转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器反馈信号，不断控制转台回转、伸缩臂伸缩、折臂摆动，调整臂架姿态，到达预期状态。

在于上述系统方案基础上，本发明进一步提供一种混合式臂架末端姿态直线控制方法：通过建立的数学控制模型，得到任意时刻φ、ɑ、θ、l ₁、l ₂与初始状态x ₀、y ₀、z ₀之间关系后，将直线离散成多个点，通过相邻点递归差补逼近的方法，控制器根据转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器反馈信号，不断控制转台回转、伸缩臂伸缩、折臂摆动，调整臂架姿态，到达预期状态。

所述数学控制模型如下：

简化模型，建立直角坐标系，则混合式臂架末端空间坐标为(x、y、z)，几何分析获得空间坐标系方程组：

　　　　　　　　　　　 （1）。

其中φ为臂架在面-XY的投影与X轴的夹角，ɑ为伸缩臂与面-XY之间的夹角，θ为折臂与伸缩臂之间的夹角，l ₁为伸缩臂的长度，l ₂为折臂的长度。

混合式臂架末端保持直线运动，即保持x、y、z其中两个坐标变，令初始状态点为A₀，坐标为(x ₀、y ₀、z ₀)，通过空间坐标方程得到状态变量φ、ɑ、θ、l ₁、l ₂之间关系式。

（1）左右直线运动

左右直线运动时，x、z量坐标保持不变，即(x ₀、z ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

　　　　　　　　　　　（2）

　　　　　　 （3）。

（2）前后直线运动

前后直线运动时，量坐标保持y、z不变，即(y ₀、z ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

　　　　　　　　　　　　（4）

　　　　　　　　 （5）。

（3）上下直线运动

上下直线运动时，量坐标保持x、y不变，即(x ₀、y ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

（6）。

其中x ₀/cosφ=y ₀/sinφ为常数，公式(6)看作l ₁cosɑ与l ₂cos(ɑ+θ)两部分，l ₁cosɑ与ɑ、l ₁有关，而l ₂cos(ɑ+θ)只与ɑ、θ有关，因此只需要保证l ₁cosɑ与l ₂cos(ɑ+θ)值为定值，便可实现上下直线运动。令初始状态点俯仰角、伸缩臂长、折臂角分别为ɑ ₀、θ ₀、l ₁₀，则可以得到：

（7）

（8）。

以上计算分析可知，在直线运动过程中φ、ɑ、θ、l ₁、l ₂及A₀坐标(x ₀、y ₀、z ₀)存在一定数学关系。

所述相邻点递归差补逼近算法即提取两个相邻点，通过前一点预测后一点的状态，即预测φ、ɑ、θ、l ₁、l ₂之间关系，具体方法如下。

假设将运动过程离散成n个状态，可得到一个空间坐标点集(A₀，A₁，A₂，...... ，A_n)，记作A_n，每个点的坐标可表示为(x _n、y _n、z _n)； 时间集合(t₀，t₁，t₂，...... ，t_n)，记作T_n；臂架在面-XY的投影与X轴的夹角φ集合(φ ₀ 、φ ₁ 、φ ₂ 、......、φ _n)，记作φ _n；伸缩臂与面-XY之间的夹角ɑ集合(ɑ ₀ 、ɑ ₁ 、ɑ ₂ 、......、ɑ _n)记作ɑ _n；折臂与伸缩臂之间的夹角θ集合(θ ₀ 、θ ₁ 、 θ ₂ 、......、θ _n)，记作θ _n；伸缩臂的长度l ₁集合(l ₁₀ 、l ₁₁ 、l ₁₂ 、......、l _1n)，记作l _1n；l ₂为定值。

任取一个时刻t_m-1的所对应点A_m-1，其中m属于n，其对应的状态变量为φ _m-1，θ _m-1，l _1m-1，可通过传感器信号获得，作为已知量输入，而下一时刻t_m对应点A_m，基于公式(2)、(3、(4)、(5)、(7)、(8)计算得到φ _m 、ɑ _m 、θ _m 、l _1m值，利用闭环控制原理，臂架末端从t_m-1到t_m运动时，控制器利用传感器实时反馈信号φ、ɑ、θ、l ₁，计算差值Δφ=φ _m-φ、Δθ=θ _m-θ、Δl ₁=l _1m-l ₁、Δɑ=ɑ _m-ɑ，精确控制转台回转、伸缩臂伸缩、折臂摆动，实现在t_m时刻，状态变量达到φ _m 、ɑ _m 、θ _m 、l _1m。

(1)左右直线运动

左右直线运动时，转台回转为主动作，伸缩臂伸缩与折臂摆动为随动动作，通过控制器控制转台以角速度ω匀速回转，把时间集合看作等差数列Δt=t _m-t _m-1，Δt _m为常数，任意相邻两点的角度差Δφ=ω*Δt为定值，以φ _m-1为已知量输入，由公式φ _m=φ _m-1+Δφ预测得到下一时刻回转角度φ _m，再由公式(2)和(3)可得到θ _m 、l _1m；当转台实时回转角φ=φ _m时，同样的方法预测t_m+1时刻φ _m+1、θ _m+1、l _1m+1，以此类推。

(2)前后直线运动

前后直线运动时，伸缩臂伸缩为主动作，转台回转与折臂摆动为随动动作，通过控制器控制伸缩臂以速度v匀速伸缩，把时间集合看作等差数列，则任意相邻两点的长度差Δl ₁=v*Δt为定值，以l _1m-1为已知量输出，由公式l _1m=l _1m-1+Δl ₁预测得到下一时刻伸缩臂长度l _1m，再由公式(4)和(5)可得到θ _m 、φ _m；当伸缩臂实时长度l ₁=l _1m时，同样的方法预测t_m+1时刻φ _m+1、θ _m+1、l _1m+1，以此类推。

(3)上下直线运动

上下直线运动时，转台回转角φ不变，伸缩臂俯仰为主动作，伸缩臂伸与缩折臂摆动为随动动作，通过控制器控制伸缩臂以角速度ω _ɑ匀速俯仰，把时间集合看作等差数列，则任意相邻两点的俯仰角差Δɑ=ω _ɑ*Δt为定值，以ɑ _m-1为已知量输出，由公式ɑ _m=ɑ _m-1+Δɑ预测得到下一时刻俯仰角度ɑ _m，再由公式(6)或(7)可得到θ _m 、l _1m；当伸缩臂实时俯仰角ɑ=ɑ _m时，同样的方法预测t_m+1时刻θ _m+1、l _1m+1，以此类推。

所述控制系统程序架构包括系统自检、参数设置、臂架状态计算、手柄臂架控制、工作平台姿态调整、建立基准坐标系、直线运动控制等子系统程序模块，具体控制流程如下。

S1，系统上电初始化后，开始自检，检查控制器各端口及传感器工作状态，如果控制器或传感器异常，报警并自锁系统，禁止操作；如果系统正常执行S2模块。

S2，显示屏通过CAN总线向控制器发送数据包，设置并修改系统内部参数，控制器实时接收并保存，子程序运行完毕后后执行S3模块。

S3，先判断是否执行直线运动，若是，结束运行S3，执行S4模块，若不是，再检测比例手柄输入控制信号，判断是否控制伸缩臂上行、下行、伸展、回收及转台回转动作，子程序运行完毕后再执行S4模块。

S4，先判断是否执行直线运动，若是，结束运行S4，执行S5模块，若不是，再检测平台姿态调整输入控制信号，判断是否控制平台内外、左右、平台前后翻转动作，子程序运行完毕后执行S5模块。

S5，判断是否建立新的基准坐标系，若是清除上一次记录参数，并建立新的基准坐标系，若不是，基准坐标系保持上一次不变，程序运行完毕后执行S6模块。

S6，判断是否执行特定直线运动，若是，以计算建立基准坐标系下臂架末端姿态空间坐标，若不是，计算默认准坐标系下臂架末端姿态空间坐标，子程序运行完毕后执行S7模块。

S7，判断是否执行直线运动，若是，执行直线运动，若不是，子模块程序运行完毕，系统程序结束。

所述S1中，系统不正常，若不开启强制操作，结束程序，若开启强制操作，执行S2。

所述S3中，控制调整臂架末端姿态，当检测到末端位置不在安全范围内，判断是否强制操作，若是，继续执行动作，若不是，禁止操作。

所述S4中，控制调整平台姿态，当检测到平台不在安全范围内，判断是否强制操作，若是，继续执行动作，若不是，禁止操作。

所述S7中，同时执行上下、左右、前后其中两个及以上动作，指令按键互锁，系统禁止执行直线运动；臂架或平台达到其极限运动位置，系统停止执行直线运动；直线运动过程中平台始保持终初始位置状态；

所述执行直线运动控制程序，首先判断臂架系统是否达到预定状态，若是，停止相关动作，若不是，再对比预定状态差值，实时调节相关动作，使臂架系统达到预定状态。

附图说明

图1为本发明一种混合式臂架末端姿态直线控制系统的示意图。

图2为本发明一种混合式臂架末端姿态直线控制系统的操作面板图。

图3为本发明混合式臂架简化模型示意图。

图4为本发明平台简化模型示意图。

图5为本发明一种混合式臂架末端姿态直线控制系统的主程序流程控制图。

图6为本发明手柄臂架控制模块子程序逻辑控制图。

图7为本发明工作平台姿态调整模块子程序逻辑控制图。

图8为本发明直线运动控制模块子程序逻辑控制图。

图9为本发明左右直线运动控制模块。

图10为本发明上下直线运动控制模块。

图11为本发明前后直线运动控制模块。

图中：1、转台编码器 2、转台 3、托架 4、伸缩臂编码器 5、伸缩臂 6、折臂油缸 7、折臂编码器 8、折臂 9、平台编码器 10、平台上下翻转机构 11、平台 12、平台左右摆动机构 13、倾角传感器 14、平台内外翻转机构 15、伸缩臂油缸 16、俯仰油缸17、控制面板 18、控制柜 19、控制器 20、多路阀组 21、伸缩位移传感器 22、比例手柄 23、平台上下翻转旋钮 24、平台左右摆动旋钮 25、平台内外翻转旋钮 26、基准按钮 27、作业功能旋钮 28、前后直线运动旋钮 29、上下直线运动旋钮 30、左右直线运动旋钮 31、急停按钮 32、电源开关 33、电源指示灯 34、显示终端。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所述实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，一种混合式臂架末端姿态直线控制系统，其特征在于：包括控制器、转台编码器、转台、比例多路阀组、托架、伸缩臂编码器、伸缩臂、伸缩臂油缸、伸缩位移传感器、折臂油缸、折臂编码器、折臂、平台编码器、平台内外反转机构、平台左右反转机构、平台前后反转机构、平台倾角传感器、俯仰油缸、控制柜、控制面板，所述控制器安装在控制柜内，采集传感器与开关信号，通过算法控制转台回转、伸缩臂伸缩、折臂摆动、平台内外翻转、平台左右翻转、平台前后翻转，设置有CAN通讯接口、开关量输入端口、模拟量输入端口、开关量输出端口、PWM输出端口、模拟量输出端口，输入端口与转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器、平台编码器、平台倾角传感器、控制面板上的开关及比例手柄相连，输出端通过比例多路阀组或直接与转台、伸缩臂油缸、折臂油缸、平台内外翻转机构、平台左右摆动机构、平台左右翻转机构、俯仰油缸相连，CAN接口与面板上显示终端相连。

如图2所示，一种混合式臂架末端姿态直线控制系统的操作面板，包括2个比例手柄、3个三位旋转复位开关、4个三位旋转自锁开关、1个自锁按钮、1个急停开关、1个电源开关、1个电源指示灯、1个车载终端。

所述比例手柄属于双轴比例手柄，分别控制伸缩臂上行、下行、伸展、回收及转台左转、转台右转、折臂上摆、折臂下摆；三位旋转复位开关包括平台内外翻转、平台左右摆动、平台左右翻转功能按钮，控制平台围绕X、Y、Z轴旋转；三位旋转自锁开关包括左右直线、前后直线、上下直线、作业功能4个按钮，左右直线按钮控制臂架末端走沿着平台左右走直线，前后直线控制臂架末端走沿着前后走直线，上下直线控制臂架末端走沿着上下走直线，作业功能按钮根据对象不同控制作业模式不同，例如飞机除冰车的蒸汽发生系统控制、消防云梯车水泡控制等；自锁按钮为直线运动基准按钮，记录当前末端状态，建立臂架末端沿着特定方向直线运动的坐标系基准，例如：飞机除冰车清除飞机机翼冰层时，需要建立适应沿机翼方向做左右、前后直线运动的空间基准坐标系；车载终端为触摸显示屏，显示臂架状态及安全报警信息，在线修改系统内部参数，设置直线运动速度。

如图3所示，建立臂架系统简化数学控制模型，以伸缩臂回转轴心为原点O建立直角坐标系，折臂摆动中心为B点，端点为A点，臂架初始状态用OB₀A₀表示，臂架运动状态用OB_mA_m表示，用OB表示伸缩臂l ₁，AB表示为折叠臂l ₂，ɑ ₀表示初始状态伸缩臂的俯仰角，φ ₀表示初始状态伸缩臂与X轴的夹角，θ ₀表示初始状态伸缩臂与折叠臂的夹角。ɑ _n 表示伸缩臂的俯仰角，φ _n 表示伸缩臂与X轴的夹角，θ _n 表示伸缩臂与折叠臂的夹角。

如图4所示，建立平台简化数学控制模型，其中β表示平台与面-XY夹角，δ表示平台与面-YZ的夹角，γ表示平台与面-XZ的夹角。

基于上述一种混合式臂架末端姿态直线控制系统，本发明还提供一种散混合式臂架末端姿态直线控制方法。

本发明的系统程序采用模块化的设计思想，将程序设计成了若干个子系统模块，程序主构架逻辑控制方法如图5所示，系统程序包括系统自检模块、系统参数设置、手柄臂架控制、工作平台姿态调整、建立基准坐标系、臂架状态计算、直线运动控制等子系统程序模块，具体控制策略如下，

A1，系统自检模块控制方法，通过电源开关给系统上电，系统进入初始化开机自检，检查控制器各端口及传感器工作状态，如果控制器或传感器正常， 系统自检子模块运行结束，进入系统参数设置子模块程序；若控制器或传感器不正常，报警并自锁系统，若开启强制操作，系统自检子模块运行结束，进入系统参数设置子模块程序，否则主结束程序。

A2，系统参数设置模块控制方法，通过显示屏设置并修改系统内部参数，利用CAN总线向控制器发送数据包，控制器实时接收并保存。

A3，手柄臂架控制模块控制方法，通过比例手柄向控制器发送控制指令，系统则根据手柄臂架控制策略实施控制转台回转、伸缩架伸缩、折臂摆动，如图6所示流程图，先判断是否执行直线运动，执行则结束本模块程序，不执行则继续执行判断是否执行臂架左回转指令；若是执行臂架左回转指令，并判断是否在安全范围，在安全范围执行臂架左回转，不在安全范围时，在强制操作模式下执行臂架左回转，否则禁止臂架左回转，并进入下一步判断是否执行臂架右回转指令，若不是执行臂架左回转指令，则直接进入下一步判断是否执行臂架右回转指令；若是执行臂架右回转指令，并判断是否在安全范围，在安全范围执行臂架右回转，不在安全范围时，在强制操作模式下执行臂架右回转，否则禁止臂架右回转，并进入下一步判断是否执行臂架伸展指令，若不是执行臂架右回转指令，则直接进入下一步判断是否执行臂架臂架伸展指令；若是执行臂架伸展指令，并判断是否在安全范围，在安全范围执行臂架伸展，不在安全范围时，在强制操作模式下执行臂架伸展，否则禁止臂架伸展，并进入下一步判断是否执行臂架回收指令，若不是执行臂架伸展指令，则直接进入下一步判断是否执行臂架臂架回收指令；若是执行臂架回收指令，并判断是否在安全范围，在安全范围执行臂架回收，不在安全范围时，在强制操作模式下执行臂架回收，否则禁止臂架回收，并进入下一步判断是否执行臂架上行指令，若不是执行臂架回收指令，则直接进入下一步判断是否执行臂架臂架上行指令；若是执行臂架上行指令，并判断是否在安全范围，在安全范围执行臂架上行，不在安全范围时，在强制操作模式下执行臂架上行，否则禁止臂架上行，并进入下一步判断是否执行臂架下行指令，若不是执行臂架上行指令，则直接进入下一步判断是否执行臂架臂架下行指令；若是执行臂架下行指令，并判断是否在安全范围，在安全范围执行臂架下行，不在安全范围时，在强制操作模式下执行臂架下行，否则禁止臂架下行，并进入下一步判断是否执行折臂上摆指令，若不是执行臂架下行指令，则直接进入下一步判断是否执行折臂上摆指令；若是执行折臂上摆指令，并判断是否在安全范围，在安全范围执行折臂上摆，不在安全范围时，在强制操作模式下执行折臂上摆，否则禁止折臂上摆，并进入下一步判断是否执行折臂下摆指令，若不是执行折臂上摆指令，则直接进入下一步判断是否执行折臂下摆指令；若是执行折臂下摆指令，并判断是否在安全范围，在安全范围执行折臂下摆，不在安全范围时，在强制操作模式下执行折臂下摆，否则禁止折臂下摆，并结束手柄臂架控制子模块程序，若不是执行折臂下摆指令，则结束手柄臂架控制子模块程序。

A4，工作平台姿态调整模块控制方法，通过平台控制旋钮开关向控制器发送控制指令，系统则根据工作平台姿态调整控制策略实施控制平台内外翻转、左右摆动、上下翻转，如图7所示流程图，先判断是否执行直线运动，执行结束本模块程序，不执行则继续执行判断是否执行平台内翻指令；若是执行平台内翻指令，并判断是否在安全范围，在安全范围执行平台内翻，不在安全范围时，在强制操作模式下平台内翻，否则禁止平台内翻，并进入下一步判断是否执行平台外翻指令，若不是执行平台外翻指令，则直接进入下一步判断是否执行平台外翻指令；若是执行平台外翻指令，并判断是否在安全范围，在安全范围执行平台外翻，不在安全范围时，在强制操作模式下平台外翻，否则禁止平台外翻，并进入下一步判断是否执行平台左摆指令，若不是执行平台外翻指令，则直接进入下一步判断是否执行平台左摆指令；若是执行平台左摆指令，并判断是否在安全范围，在安全范围执行平台左摆，不在安全范围时，在强制操作模式下平台左摆，否则禁止平台左摆，并进入下一步判断是否执行平台右摆指令，若不是执行平台左摆指令，则直接进入下一步判断是否执行平台右摆指令；若是执行平台右摆指令，并判断是否在安全范围，在安全范围执行平台右摆，不在安全范围时，在强制操作模式下平台右摆，否则禁止平台右摆，并进入下一步判断是否执行平台右摆指令，若不是执行平台右摆指令，则直接进入下一步判断是否执行平台左摆指令；若是执行平台左摆指令，并判断是否在安全范围，在安全范围执行平台左摆，不在安全范围时，在强制操作模式下平台左摆，否则禁止平台左摆，并进入下一步判断是否执行平台上翻指令，若不是执行平台左摆指令，则直接进入下一步判断是否执行平台上翻指令；若是执行平台上翻指令，并判断是否在安全范围，在安全范围执行平台上翻，不在安全范围时，在强制操作模式下平台上翻，否则禁止平台上翻，并结束工作平台姿态调整模块程序，若不是执行平台下翻指令，则直接结束工作平台姿态调整模块程序。

A5，建立基准坐标系模块控制方法，需要在指定坐标系内直线运动时，首先将臂架运动到预定坐标位置，通过基准按钮指令，清除上一次记录参数，并建立新的基准坐标系，若在默认坐标系内运动，则以运动的最初状态时的伸缩臂方向为X轴建立坐标系。

A6，臂架状态计算模块控制方法，臂架末端姿态空间坐标计算方法分为基准坐标和默认坐标两种模式，需要判断是否执行特定直线运动，若是，以计算基准坐标系模式下臂架末端姿态空间坐标，若不是，计算默认准坐标系下臂架末端姿态空间坐标。

A7，直线运动控制模块控制方法，通过直线控制旋钮开关向控制器发送控制指令，系统则根据工作平台姿态调整控制策略实施控制左右直线运动、上下直线运动、前后直线运动，如图8所示流程图，首先判断是否在特定基准下做直线运动，若是，在基准模式下做直线运动，若不是在默认模式下做直线运动；系统上下、左右、前后直线运动只执行其中一种运动直线运动，若系统检测两种或以上运动，系统自锁，禁止运动，若系统正常，先判断是否左右直线运动，若是，进入左右直线运动控制，若不是，停止左右直线运动，判断是否上下直线运动，若是，进入上下直线运动控制，若不是，停止上下直线运动，最后判断是否前后直线运动，若是，进入前后直线运动控制，若不是，停止前后直线运动，并结束程序。

在所述A7中，系统进入左右直线运动时，如图9所示流程图，臂架与平台若处于极限位置，程序结束，臂架与平台不处于极限位置，若是右回转指令执行右回转，左回转指令执行左回转；判断伸缩臂是否达到预设长度，若是停止伸缩，若不是，大于伸缩长度，回收，若小于伸缩长度，伸长；下一步判断折臂是否达到预设夹角，若是停止折臂摆动，若不是，大于折臂夹角，折臂下摆，若小于折臂夹角，折臂上摆；进一步判断平台是否达到预设夹角，若是停止折臂摆动，若不是，大于折臂夹角，折臂下摆，若小于折臂夹角，折臂上摆；再进一步判断平台内外翻转预定角是否到达预定角度，若是停止平台内外反转，若不是，大于预定角度，平台内翻，小于预定角度，平台外翻；最后判断平台左右翻转预定角是否到达预定角度，若是停止平台左右摆动，若不是，大于预定角度，平台右摆，小于预定角度，平台左摆。

在所述A7中，系统进入上下直线运动时，如图10所示流程图，臂架与平台若处于极限位置，程序结束，臂架与平台不处于极限位置，若是臂架上行直线指令臂架上行，臂架下行直线指令执行臂架下行；判断折臂是否达到预设夹角，若是停止折臂摆动，若不是，大于折臂夹角，折臂下摆，若小于折臂夹角，折臂上摆；下一步判断平台内外翻转预定角是否到达预定角度，若是停止平台内外反转，若不是，大于预定角度，平台内翻，小于预定角度，平台外翻；最后判断平台左右翻转预定角是否到达预定角度，若是停止平台左右摆动，若不是，大于预定角度，平台右摆，小于预定角度，平台左摆。

在所述A7中，系统进入前后直线运动时，如图11所示流程图，臂架与平台若处于极限位置，程序结束，臂架与平台不处于极限位置，若是向前直线指令执行伸缩臂伸长，向后直线指令执行伸缩臂回收；判断转台回转角是否达到预设角度，若是转台回转，若不是，大于回转角，转台左回转，若小于回转角，转台右回转；下一步判断折臂是否达到预设夹角，若是停止折臂摆动，若不是，大于折臂夹角，折臂下摆，若小于折臂夹角，折臂上摆；进一步判断平台内外翻转预定角是否到达预定角度，若是停止平台内外反转，若不是，大于预定角度，平台内翻，小于预定角度，平台外翻；最后判断平台左右翻转预定角是否到达预定角度，若是停止平台左右摆动，若不是，大于预定角度，平台右摆，小于预定角度，平台左摆。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种混合式臂架末端姿态直线控制系统，其特征在于：包括控制器、转台编码器、转台、比例多路阀组、托架、伸缩臂编码器、伸缩臂、伸缩臂油缸、伸缩位移传感器、折臂油缸、折臂编码器、折臂、平台编码器、平台内外翻转机构、平台上下翻转翻转机构、平台左右摆动机构、平台倾角传感器、俯仰油缸、控制柜、控制面板，所述控制器安装在控制柜内，采集传感器与开关信号，通过算法控制转台回转、伸缩臂伸缩、折臂摆动、平台内外翻转、平台上下翻转、平台左右摆动，控制器设置有CAN通讯接口、开关量输入端口、模拟量输入端口、开关量输出端口、PWM输出端口、模拟量输出端口，其中输入端口与转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器、平台编码器、平台倾角传感器、控制面板上的开关及比例手柄相连，输出端通过比例多路阀组或直接与转台、伸缩臂油缸、折臂油缸、平台内外翻转机构、平台上下翻转翻转机构、平台左右摆动机构、俯仰油缸相连，CAN接口与面板上显示终端相连。

2.根据权利要求1所述一种混合式臂架末端姿态直线控制系统，其特征在于：所述平台内外翻转机构安装在转台上，驱动平台围绕Y轴旋转，保持直线运动过程中调节平台与面-XY夹角；驱动平台围绕X轴旋转，调节平台与面-XZ夹角；驱动平台围绕Z轴摆动，保持直线运动过程中调节平台侧面与面-YZ夹角；所述伸缩臂编码器安装端面与托架固定，旋转轴与伸缩臂旋转轴相连，测量伸缩臂与托架的相对夹角ɑ；所述转台编码器安装端面与托架固定，旋转轴与转台旋转轴心相连，测量回转转台旋转角度φ；所述伸缩位移传感器安装在伸缩油缸上，测量油缸伸缩位移，得到伸缩臂的长度l ₁；所述控平台倾角传感器安装在平台侧面上，测量平台与面-YZ的夹角δ及与面-XY的夹角β；所述折臂编码器安装端面与伸缩臂固定，旋转轴与折臂旋转轴心相连，测量折臂与伸缩臂之间的夹角θ；所述平台编码器安装在平台左右摆动机构上，测量平台与面-YZ的夹角γ。

3.根据权利要求1所述一种混合式臂架末端姿态直线控制系统，其特征在于：所述控制柜安装于转台上，用于安装控制器与指令开关，并起到防尘、防震、防水作用。

4.根据权利要求1或3所述一种混合式臂架末端姿态直线控制系统，其特征在于：所述控制面板包括2个比例手柄、3个三位旋转复位开关、4个三位旋转自锁开关、1个自锁按钮、1个急停开关、1个电源开关、1个电源指示灯、1个车载终端；所述比例手柄属于双轴比例手柄，分别控制伸缩臂上行、下行、伸展、回收及转台左转、转台右转、折臂上摆、折臂下摆；所述三位旋转复位开关为平台内外翻转、平台左右摆动、平台上下翻转功能按钮，控制平台围绕X、Y、Z轴旋转；所述三位旋转自锁开关包括左右直线、前后直线、上下直线、作业功能4个按钮，左右直线按钮控制臂架末端走沿着平台左右走直线，前后直线控制臂架末端走沿着前后走直线，上下直线控制臂架末端走沿着上下走直线，作业功能按钮根据对象不同控制作业模式不同，例如飞机除冰车的蒸汽发生系统控制、消防云梯车水泡控制等；所述自锁按钮为直线运动基准按钮，记录当前末端状态，建立臂架末端沿着特定方向直线运动的坐标系基准，例如：飞机除冰车清除飞机机翼冰层时，需要建立适应沿机翼方向做左右、前后直线运动的空间基准坐标系；所述车载终端为触摸显示屏，显示臂架状态及安全报警信息，在线修改系统内部参数，设置直线运动速度。

5.根据权利要求1所述一种混合式臂架末端姿态直线控制方法，其特征在于：通过建立的数学控制模型，得到任意时刻回转转台旋转角度φ、伸缩臂与托架的相对夹角ɑ，折臂与伸缩臂夹角θ，伸缩臂长度l ₁，折臂长度l ₂与末端初始状态空间坐标x ₀、y ₀、z ₀之间关系式，将直线离散成多个点，通过相邻点递归差补逼近的方法，控制器根据转台编码器、伸缩臂编码器、伸缩位移传感器、折臂编码器反馈信号，不断控制转台回转、伸缩臂伸缩、折臂摆动，调整臂架姿态，到达预期状态。

6.根据权利要求5所述一种混合式臂架末端姿态直线控制方法，其特征在于：所述数学控制模型如下：

简化模型，建立直角坐标系，则混合式臂架末端空间坐标为(x、y、z)，几何分析获得空间坐标系方程组：

　　　　　　　　　　　 （1）

其中φ为臂架在面-XY的投影与X轴的夹角，ɑ为伸缩臂与面-XY之间的夹角，θ为折臂与伸缩臂之间的夹角，l ₁为伸缩臂的长度，l ₂为折臂的长度；

混合式臂架末端保持直线运动，即保持x、y、z其中两个坐标变，令初始状态点为A₀，坐标为(x ₀、y ₀、z ₀)，通过空间坐标方程得到状态变量φ、ɑ、θ、l ₁、l ₂之间关系式；（1）左右直线运动

左右直线运动时，x、z量坐标保持不变，即(x ₀、z ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

　　　　　　　　　　　（2）

　　　　　　 （3）

（2）前后直线运动

前后直线运动时，量坐标保持y、z不变，即(y ₀、z ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

　　　　　　　　　　　　（4）

　　　　　　　　 （5）

（3）上下直线运动

上下直线运动时，量坐标保持x、y不变，即(x ₀、y ₀)不变，等式变换得到φ、ɑ、θ、l ₁、l ₂数学关系式：

（6）

其中x ₀/cosφ=y ₀/sinφ为常数，公式(6)看作l ₁cosɑ与l ₂cos(ɑ+θ)两部分，l ₁cosɑ与ɑ、l ₁有关，而l ₂cos(ɑ+θ)只与ɑ、θ有关，因此只需要保证l ₁cosɑ与l ₂cos(ɑ+θ)值为定值，便可实现上下直线运动；

令初始状态点俯仰角、伸缩臂长、折臂角分别为ɑ ₀、θ ₀、l ₁₀，则可以得到：

（7）

（8）

以上计算分析可知，在直线运动过程中φ、ɑ、θ、l ₁、l ₂及A₀坐标(x ₀、y ₀、z ₀)存在一定数学关系。

7.根据权利要求5或6所述一种混合式臂架末端姿态直线控制方法，其特征在于：所述相邻点递归差补逼近算法即提取两个相邻点，通过前一点预测后一点的状态，即预测φ、ɑ、θ、l ₁、l ₂之间关系，具体方法如下

假设将运动过程离散成n个状态，可得到一个空间坐标点集(A₀，A₁，A₂，...... ，A_n)，记作A_n，每个点的坐标可表示为(x _n、y _n、z _n)； 时间集合(t₀，t₁，t₂，...... ，t_n)，记作T_n；臂架在面-XY的投影与X轴的夹角φ集合(φ ₀ 、φ ₁ 、φ ₂ 、......、φ _n)，记作φ _n；伸缩臂与面-XY之间的夹角ɑ集合(ɑ ₀ 、ɑ ₁ 、ɑ ₂ 、......、ɑ _n)记作ɑ _n；折臂与伸缩臂之间的夹角θ集合(θ ₀ 、θ ₁ 、 θ ₂ 、......、θ _n)，记作θ _n；伸缩臂的长度l ₁集合(l ₁₀ 、l ₁₁ 、l ₁₂ 、......、l _1n)，记作l _1n；l ₂为定值；

任取一个时刻t_m-1的所对应点A_m-1，其中m属于n，其对应的状态变量为φ _m-1，θ _m-1，l _1m-1，可通过传感器信号获得，作为已知量输入，而下一时刻t_m对应点A_m，基于公式(2)、(3、(4)、(5)、(7)、(8)计算得到φ _m 、ɑ _m 、θ _m 、l _1m值，利用闭环控制原理，臂架末端从t_m-1到t_m运动时，控制器利用传感器实时反馈信号φ、ɑ、θ、l ₁，计算差值Δφ=φ _m-φ、Δθ=θ _m-θ、Δl ₁=l _1m-l ₁、Δɑ=ɑ _m-ɑ，精确控制转台回转、伸缩臂伸缩、折臂摆动，实现在t_m时刻，状态变量达到φ _m 、ɑ _m 、θ _m 、l _1m；

(1)左右直线运动

左右直线运动时，转台回转为主动作，伸缩臂伸缩与折臂摆动为随动动作，通过控制器控制转台以角速度ω匀速回转，把时间集合看作等差数列Δt=t _m-t _m-1，Δt _m为常数，任意相邻两点的角度差Δφ=ω*Δt为定值，以φ _m-1为已知量输入，由公式φ _m=φ _m-1+Δφ预测得到下一时刻回转角度φ _m，再由公式(2)和(3)可得到θ _m 、l _1m；当转台实时回转角φ=φ _m时，同样的方法预测t_m+1时刻φ _m+1、θ _m+1、l _1m+1，以此类推；

(2)前后直线运动

前后直线运动时，伸缩臂伸缩为主动作，转台回转与折臂摆动为随动动作，通过控制器控制伸缩臂以速度v匀速伸缩，把时间集合看作等差数列，则任意相邻两点的长度差Δl ₁=v*Δt为定值，以l _1m-1为已知量输出，由公式l _1m=l _1m-1+Δl ₁预测得到下一时刻伸缩臂长度l _1m，再由公式(4)和(5)可得到θ _m 、φ _m；当伸缩臂实时长度l ₁=l _1m时，同样的方法预测t_m+1时刻φ _m+1、θ _m+1、l _1m+1，以此类推；

(3)上下直线运动

上下直线运动时，转台回转角φ不变，伸缩臂俯仰为主动作，伸缩臂伸与缩折臂摆动为随动动作，通过控制器控制伸缩臂以角速度ω _ɑ匀速俯仰，把时间集合看作等差数列，则任意相邻两点的俯仰角差Δɑ=ω _ɑ*Δt为定值，以ɑ _m-1为已知量输出，由公式ɑ _m=ɑ _m-1+Δɑ预测得到下一时刻俯仰角度ɑ _m，再由公式(6)或(7)可得到θ _m 、l _1m；当伸缩臂实时俯仰角ɑ=ɑ _m时，同样的方法预测t_m+1时刻θ _m+1、l _1m+1，以此类推。

8.根据权利要求1或7所述一种混合式臂架末端姿态直线控制系统及方法，其特征在于：所述控制系统程序架构包括系统自检、参数设置、臂架状态计算、手柄臂架控制、工作平台姿态调整、建立基准坐标系、直线运动控制等子系统程序模块，具体控制流程如下：

S1，系统上电初始化后，开始自检，检查控制器各端口及传感器工作状态，如果控制器或传感器异常，报警并自锁系统，禁止操作；如果系统正常执行S2模块；

S2，显示屏通过CAN总线向控制器发送数据包，设置并修改系统内部参数，控制器实时接收并保存，子程序运行完毕后后执行S3模块；

S3，先判断是否执行直线运动，若是，结束运行S3，执行S4模块，若不是，再检测比例手柄输入控制信号，判断是否控制伸缩臂上行、下行、伸展、回收及转台回转动作，子程序运行完毕后再执行S4模块；

S4，先判断是否执行直线运动，若是，结束运行S4，执行S5模块，若不是，再检测平台姿态调整输入控制信号，判断是否控制平台内外、左右、平台前后翻转动作，子程序运行完毕后执行S5模块；

S5，判断是否建立新的基准坐标系，若是清除上一次记录参数，并建立新的基准坐标系，若不是，建立以运动的最初状态时的伸缩臂方向为X轴的默认坐标系，程序运行完毕后执行S6模块；

S6，判断是否执行特定直线运动，若是，以计算建立基准坐标系下臂架末端姿态空间坐标，若不是，计算默认准坐标系下臂架末端姿态空间坐标，子程序运行完毕后执行S7模块；

S7，判断是否执行直线运动，若是，执行直线运动，若不是，子模块程序运行完毕，系统程序结束。

9.根据权利要求8所述一种混合式臂架末端姿态直线控制方法，其特征在于：

所述S1中，系统不正常，若不开启强制操作，结束程序，若开启强制操作，执行S2；

所述S3中，控制调整臂架末端姿态，当检测到末端位置不在安全范围内，判断是否强制操作，若是，继续执行动作，若不是，禁止操作；

所述S4中，控制调整平台姿态，当检测到平台不在安全范围内，判断是否强制操作，若是，继续执行动作，若不是，禁止操作；

所述S7中，同时执行上下、左右、前后其中两个及以上动作，指令按键互锁，系统禁止执行直线运动；臂架或平台达到其极限运动位置，系统停止执行直线运动；直线运动过程中平台始保持终初始位置状态。

10.根据权利要求9所述一种混合式臂架末端姿态直线控制方法，其特征在于：所述执行直线运动控制程序，首先判断臂架系统是否达到预定状态，若是，停止相关动作，若不是，再对比预定状态差值，实时调节相关动作，使臂架系统达到预定状态。

Images