CN104699119A - 一种装载平台及装载物的水平调节控制方法 - Google Patents

Abstract

本发明的目的在于提供一种控制精度高，且能够实时监测和自动调节的装载平台及装载物的水平调节控制方法。本发明包括：采集水平仪数据；将水平仪坐标系下的倾角单位化；将水平仪坐标系下的表达式利用转换矩阵转换成为调平目标下的表达式，平台为调平目标，转换后的倾角表达式变为，求出倾角值；用当前倾斜角分别与初始安装倾斜角作差得到调整角；再次置平装载平台的支腿，调整位移。本发明实时监测水平仪倾角，出现偏差时自动实时调节，无需人工操作，效率高，控制精度高，可靠性高，在出现某一支腿卡死时，还可以通过调节其他支腿使调平目标水平。

Description

一种装载平台及装载物的水平调节控制方法

技术领域

本发明涉及地面空间模拟实验领域，具体涉及一种满足仪器设备工作对水平度的高精度要求的装载平台及装载物的水平调节控制方法。

背景技术

卫星在地面进行空间模拟实验时对水平度有很高的要求，然而外部辐射和内部设备的发热引起的装载卫星的平台受热不均匀会引起水平度超差，在真空环境中不可能进行人工调节，自动调平可解决这一问题。一些仪器设备对安放的水平度有较高要求，虽然已有很多水平调节仪器，但它们的调节效果不是很好，在装载平台上安装有设备时，通过传统的观察水平仪，用人工去升降各支点使其水平，难度大，效率低。例如在军工领域，对雷达天线车要求能快速自动地实现较高精度水平基准的调整，对于水平基准的调整，过去往往采用人工手动调平方式，依靠人眼观察基准水泡，经反复调整才能达到水平要求，调平时间较长，而且调平精度受到人为因素干扰较大；在医学领域，放射性治疗技术对CT治疗床的安放要求水平度很高，但目前的技术要达到此精度尚困难。

目前已知的平台水平调节控制方法中，有通过重力作用使摆杆与水平面保持垂直，悬臂保持水平，通过悬臂控制充气阀的开闭调节支腿上的空气弹簧来达到水平的。相关专利文献请参考公开号为CN 102806819B的中国专利申请文件《平台水平自动调整装置及调整方法》。该方法在调平后需要保证其拥有自锁能力，即调平后长时间保持水平状态，且需要系统抗干扰能力强，这就需要平台具有极高的稳定性，传统的气压式伺服系统往往由于气压系统自身缺点无法保证长时间的保持高度水平状态。再者参考公开号为CN 104057612A的中国专利申请文件《3D打印机用自动调平装置及3D打印机及调平方法》给出了一种位置误差调平法，判断各测量点到测距传感器的距离与距离的平均值是否在误差允许范围内，若超差则控制电机调整距离，若不超差结束此次调平。需要频繁移动测距传感器确定电机支撑点坐标和测量点到测距传感器的距离，操作相对复杂。文章《空间模拟器大型卫星装载平台监控系统的实现》所述调平方法未考虑水平仪与平台不在同一平面的情况。

总的来说，现有平台水平调节方法存在如下的一些缺点：

1.自动调平的液压驱动方式可承载较大的负载,但液压系统复杂，反应迟缓，一般会滞后，不易实现实时控制，且液压油易泄漏，维护较难。

2.实时性差，从监测到倾角超差到再次置平需要时间周期长；

3.调节精度低，运用常规的气泡水平仪或重力作用监测水平度；

4.自动化程度低，甚至需要人工调平。

发明内容

本发明的目的在于提供一种控制精度高，且能够实时监测和自动调节的装载平台及装载物的水平调节控制方法。

本发明的目的是这样实现的：

(1)采集水平仪数据：调平目标水平时，装载平台坐标系为O-XYZ、装载物坐标系为O'-X'Y'Z'和水平仪坐标系为O″-X″Y″Z″，水平仪在X轴Y轴上的倾角为θ″_X0,θ″_Y0，采集当前水平仪读数为θ″_X1,θ″_Y1；

(2)将水平仪坐标系下的倾角θ″_X0,θ″_Y0单位化，表达式为(i″₀,j″₀,k″₀)，其中  $\tan {{\mathrm{\θ}}^{\′\′}}_{X0}=\frac{{k^{\′\′}}_0}{{i^{\′\′}}_0},\tan {{\mathrm{\θ}}^{\′\′}}_{Y0}=\frac{{k^{\′\′}}_0}{{j^{\′\′}}_0},{i^{\′\′}}_0^2+{j^{\′\′}}_0^2+{k^{\′\′}}_0^2=1;$

(3)将水平仪坐标系下的表达式(i″₀,j″₀,k″₀)利用转换矩阵转换成为调平目标下的表达式，平台为调平目标，转换后的倾角表达式变为(i₀,j_0,k₀)，求出倾角值

${\mathrm{\θ}}_{X0}=\arctan \frac{k_0}{i_0},{\mathrm{\θ}}_{Y0}=\arctan \frac{k_0}{j_0},$

求出当前装载平台的倾角值θ_X1,θ_Y1；

(4)用当前倾斜角θ_X1,θ_Y1分别与初始安装倾斜角θ_X,θ_Y作差得到调整角：

Δθ_X＝θ_X1-θ_X0，

Δθ_Y＝θ_Y1-θ_Y0，

合成后的倾斜角：

$\mathrm{\θ}=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_X^2+\mathrm{\Δ}{\mathrm{\θ}}_Y^2},$

测量极限倾斜角θ_L，如果θ＞θ_L，继续调整载物平台；如果θ≤θ_L，不再需要调整载物平台；

(5)再次置平装载平台的支腿，调整位移 $\mathrm{\Δx}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_X}{\mathrm{\θ}},\mathrm{\Δy}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_Y}{\mathrm{\θ}}.$

转换矩阵

$\left[\begin{array}{c}i\\ j\\ k\end{array}\right]=T_3^{-1}{T}_2^{-1}{T}_1^{-1}\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]=T\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right],$

$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right],$

$T_2=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right],$

$T_3=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

其中α,β,γ为三维空间直角坐标变换的三个旋转角。

本发明的有益效果在于：

本发明实时监测水平仪倾角，出现偏差时自动实时调节，无需人工操作，效率高，控制精度高，可靠性高，在出现某一支腿卡死时，还可以通过调节其他支腿使调平目标水平。

附图说明

图1是本发明实施例提供的装载平台及其装载物的水平调节控制方法流程图；

图2是实施例一装载平台、装载物和水平仪位置示意图；

图3是水平仪在X和Y轴方向上的倾角读数示意图；

图4是装载平台坐标系O-XYZ、装载物坐标系O'-X'Y'Z'和水平仪坐标系O”-X”Y”Z”转换示意图；

图5是四支腿装载平台坐标系示意图；

图6是三支腿装载平台坐标系示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。图中标号说明：

水平仪1，装载平台2，支腿3，装载物4。

鉴于所述对水平精度的高要求和现有技术的局限，本发明提供了一种装载平台水平调节控制方法，所述装载平台设有三或四条支腿，每条支腿上固定有电机控制支腿的伸缩，水平 仪安放在装载平台或装载物上，根据监测到的水平仪倾角，计算出支腿应伸缩的距离以再次置平调平目标(装载平台或装载物)，控制电机调节支腿。

装载平台的水平控制方法包括：

信号采集系统，由固定在装载平台或装载物上的高精度水平仪、固定在支腿上的位移传感器组成，高精度水平仪实时监测装载平台或装载物在X和Y轴方向的倾角，位移传感器实时监测支腿的位移；

位移控制系统，由支腿组件和固定在支腿上的电机组成，电机通过蜗杆减速器与支腿组件相连，控制支腿的伸缩，并可实现自锁；

电控系统，根据信号采集系统传来的数据计算支腿的位移以控制调平目标的水平。

装载平台的水平控制方法可实现平台的自动调平功能，水平仪数据的采集和调平算法完全由控制程序来实现。

采集水平仪数据用到中断循环程序，每隔一个中断周期向水平仪发送一次采集请求，水平仪接受请求后向控制器返回当前倾角值

装载平台的调平算法：

装载平台为正方形，四支腿装载平台四个角上分别设置支腿，两对角线分别为X轴Y轴，交点为原点；三支腿装载平台支腿位置呈正三角形分布，两相邻角上设置支腿，设为X轴，垂直平分线为Y轴，另一支腿位置也就确定了。

当水平仪坐标系与装载平台或装载物坐标系不重合时，能够将水平仪的读数转换成装载平台坐标系下的倾斜角度，具体包括：

设装载平台坐标系为O-XYZ，装载物坐标系为O'-X'Y'Z',水平仪坐标系为O″-X″Y″Z″。

①以装载物为调平目标，水平仪安放在装载物上，把水平仪X和Y轴方向的倾角θ_x″和θ_Y″由水平仪坐标系O″-X″Y″Z″转换成装载物坐标系O'-X'Y'Z'下的倾角；

②以装载物为调平目标，水平仪安放在装载平台上，把水平仪X和Y轴方向的倾角θ_x″和θ_Y″由水平仪坐标系O″-X″Y″Z″转换成装载物坐标系O'-X'Y'Z'下的倾角；

③以装载平台为调平目标，水平仪安放在装载物上，把水平仪X和Y轴方向的倾角θ_x″和θ_Y″由水平仪坐标系O″-X″Y″Z″转换成装载平台坐标系O-XYZ下的倾角；

④以装载平台为调平目标，水平仪安放在装载物上，把水平仪X和Y轴方向的倾角θ_x″和 θ_Y″由水平仪坐标系O″-X″Y″Z″转换成装载平台坐标系O-XYZ下的倾角；

装载平台的调平算法：

a采集水平仪数据。受装载物外形的影响，水平仪一般不会无缝贴合安放在装载平台或装载物上，这必将会造成装载平台坐标系O-XYZ、装载物坐标系O'-X'Y'Z'和水平仪坐标系O″-X″Y″Z″不能重合的现象，所以调平目标水平时水平仪可能会倾斜。设调平目标水平时，水平仪在X轴Y轴上的倾角为θ″_X0,θ″_Y0，当前水平仪读数为θ″_X1,θ″_Y1。

b将水平仪坐标系下的倾角θ″_X0,θ″_Y0单位化，表达式为(i″₀,j″₀,k″₀)，其中  $\tan {{\mathrm{\θ}}^{\′\′}}_{X0}=\frac{{k^{\′\′}}_0}{{i^{\′\′}}_0},\tan {{\mathrm{\θ}}^{\′\′}}_{Y0}=\frac{{k^{\′\′}}_0}{{j^{\′\′}}_0},{i^{\′\′}}_0^2+{j^{\′\′}}_0^2+{k^{\′\′}}_0^2=1.$

c将水平仪坐标下的表达式(i″₀,j″₀,k″₀)利用转换矩阵转换成为调平目标下的表达式，不妨设平台为调平目标，转换后的倾角表达式变为(i₀,j_0,k₀)，求出倾角值 用同样方法求出当前装载平台的倾角值θ_X1,θ_Y1，

d用当前倾斜角θ_X1,θ_Y1分别与初始安装倾斜角θ_X,θ_Y作差即可得调整角

Δθ_X＝θ_X1-θ_X0，

Δθ_Y＝θ_Y1-θ_Y0

合成后的倾斜角为

$\mathrm{\θ}=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_X^2+\mathrm{\Δ}{\mathrm{\θ}}_Y^2}$

设极限倾斜角θ_L，如果θ＞θ_L，需要调整载物平台；如果θ≤θ_L，则不需要调整，进入下一次循环。为了保证调节过程的安全性，在自动调平过程中必须防止出现超调，更不允许出现振荡情况。为此将采用保守的调平策略，设置一个最大调整量θ_L，这样就将调整过程分多次进行，调整过程更为安全。

e计算再次置平支腿的调整位移 $\mathrm{\Δx}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_X}{\mathrm{\θ}},\mathrm{\Δy}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_Y}{\mathrm{\θ}},$ 当θ较小时，tanθ≈θ，可取其中l为装载平台边长。为保证支腿的位移不会超限，可控制装载平台中心固定，则四条支腿的调整量分别为

Δl₁₀＝-Δx/2 Δl₂₀＝-Δy/2 Δl₃₀＝Δx/2 Δl₄₀＝Δy/2

若要求平台中心移动距离Δl，则可在原调整量上加上Δl，即

Δl₁₀＝-Δx/2+Δl，Δl₂₀＝-Δy/2+Δl

Δl₃₀＝Δx/2+Δl，Δl₄₀＝Δy/2-Δl₁₀；

若为三支腿支撑平台，取平台边长为m，则调整位移Δx₀＝mtanθ_X，调整量为Δl₁₀＝-Δx₀/2，Δl₂₀＝Δx₀/2，Δl₃₀＝-Δy₀；

水平仪的读数转换装载平台坐标系下的倾斜角度方法，运用转换矩阵：

$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$

$T_2=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$

$T_3=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

可求

$\left[\begin{array}{c}i\\ j\\ k\end{array}\right]=T_3^{-1}{T}_2^{-1}{T}_1^{-1}\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]=T\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]$

其中α,β,γ为三维空间直角坐标变换的三个旋转角。运用以上转换方法分别将平台水平时水平仪的读数和当前监测到的水平仪读数转换成平台坐标系下的倾角值；

本发明可实现平台的自动调平功能，可实现调节范围外自动限位。支腿具有的移动部分具有上下限位开关和位移传感器，当支腿行程超限时触发限位开关闭合，实现硬限位。同时编有限位控制程序，接收来自位移传感器的数据，当支腿位移超过上限值或下限值时，触发报警开关。

若装载平台上安放有两个装载物，在不考虑另一装载物的水平情况下，可控制任一装载 物为调平目标。假设装载平台上有装载物A和B，A和B上分别安放有水平仪，现只调节装载物A的水平，分别将装载物A初始水平时和倾斜后的水平仪坐标系下的倾角值转换成为装载平台坐标系下的倾角值，利用权力4所述方法调平。同样，也可只调节装载物B的水平。

装载平台的水平调节方法，具有支腿“卡死”检测功能。为检测是否有支腿卡死的情况出现，编写一中断循环程序，程序中对每个支腿设置3个计数器，一个是支腿驱动数据的绝对值累加计数器，一个用于存储支腿驱动数据增量值，一个用于存储反馈数据的增量值。当支腿的驱动数据绝对值到某一数值时，检验支腿驱动数据增量值和反馈数据增量值的差值，若在要求的范围以内，说明支腿运动正常，对以上3个计数器清零继续工作；若差值超差，说明支腿运动异常，系统将作出卡死报警。

当装载平台某一支腿出现卡死或其他机械故障不能伸缩时，能够通过调节其他支腿以达到调平目标(装载平台或装载物)的水平。当某一支腿卡死时，调平策略，方法如下：

保持卡死的支腿不动，其他的支腿调整值减去卡死支腿的调整值，假设支腿一卡死，则其他三条支腿的调整量为：

Δl₂₀＝-Δy/2-Δl₁₀；Δl₃₀＝Δx/2-Δl₁₀；Δl₄₀＝Δy/2-Δl₁₀

若为三支腿支撑平台，假设支腿一卡死，则其他两条支腿的调整量为Δl₂₀＝Δx₀/2-Δl₁₀，Δl₃₀＝-Δy₀/2-Δl₁₀。

为了保证调节过程的安全性，在自动调平过程中必须防止出现超调，更不允许出现振荡情况。为此将采用保守的调平策略，设置一个最大调整量θ_L，这样就将调整过程分多次进行，调整过程更为安全。

计算每个支腿的位移量，自动发出支腿运动指令，电机按给定的位移控制支腿的伸缩，四条支腿同时运动。

平台测控系统配备2套电子水平装置，每套电子水平装置均具备独立调平控制功能，可以在主备水平仪之间实现切换。如果在调平系统运行期间控制系统连续多次接收不到水平仪的反馈数据，则认为该水平仪出现故障，切换到备用水平仪。

1.调平算法的步骤如下：

1)运用控制系统的中断循环程序实时给水平仪发送请求返回倾角数据的指令，并把该数据翻译为角度值或弧度值；

2)运用下式将步骤1得到的水平仪的在X轴和Y轴方向的倾角θ″_X,θ″_Y单位化

$\left\{\begin{array}{c}\tan {{\mathrm{\θ}}_X}^{\′\′}=\frac{k^{\′\′}}{i^{\′\′}}\\ \tan {{\mathrm{\θ}}_Y}^{\′\′}=\frac{k^{\′\′}}{j^{\′\′}}\\ i^{\′\′2}+j^{\′\′2}+k^{\′\′2}=1\\ \end{array}\right.$

受装载物外形的影响，水平仪一般不会无缝贴合安放在装载平台或装载物上，这必将会造成装载平台坐标系O-XYZ、装载物坐标系O'-X'Y'Z'和水平仪坐标系O″-X″Y″Z″不能重合的现象，所以平台水平时水平仪可能会倾斜。因为支腿直接控制的是装载平台，所以必须将将监测到的水平仪坐标系下的倾角表达式，转换成装载平台坐标系下的倾角表达式。运用转换矩阵

$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$

$T_2=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$

$T_3=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

可求

$\left[\begin{array}{c}i\\ j\\ k\end{array}\right]=T_3^{-1}{T}_2^{-1}{T}_1^{-1}\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]=T\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]$

其中α,β,γ为三维空间直角坐标变换的三个旋转角。平台坐标系下的倾角值分别为 运用以上转换方法分别将调平目标水平时水平仪倾角和当前监测到的水平仪倾角转换成平台坐标系下在X轴和Y轴方向的倾角值θ_X0,θ_Y0、θ_X1,θ_Y1；

3)用当前倾角θ_X1,θ_Y1分别与调平目标水平时倾角θ_X0,θ_Y0作差可得X轴和Y轴方向的调整角

Δθ_X＝θ_X1-θ_X0，

Δθ_Y＝θ_Y1-θ_Y0

合成后的倾斜角为

$\mathrm{\θ}=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_X^2+\mathrm{\Δ}{\mathrm{\θ}}_Y^2}$

设极限角为常量θ_L，如果θ＞θ_L，需要调整载物平台，执行步骤4；如果θ≤θ_L，则不需要调整，返回步骤1。

4)为了保证调节过程的安全性，在自动调平过程中必须防止出现超调，更不允许出现振荡情况。为此将采用保守的调平策略，设置一个最大调整量θ_L，这样就将调整过程分多次进行，调整过程更为安全。

计算再次置平时支腿的调整位移

$\mathrm{\Δx}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_X}{\mathrm{\θ}},\mathrm{\Δy}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_Y}{\mathrm{\θ}}$

当θ较小时，tanθ≈θ，可取其中l为装载平台边长。为保证支腿的位移不会超限，可控制装载平台中心固定，则四条支腿的调整量分别为

Δl₁₀＝-Δx/2，Δl₂₀＝-Δy/2，Δl₃₀＝Δx/2，Δl₄₀＝Δy/2

若要求平台中心移动距离Δl，则可在原调整量上加上Δl，即

Δl₁₀＝-Δx/2+Δl，Δl₂₀＝-Δy/2+Δl

Δl₃₀＝Δx/2+Δl，Δl₄₀＝Δy/2-Δl₁₀

若为三支腿支撑平台，取平台边长为m，则调整位移Δx₀＝mtanθ_X，调整量为Δl₁₀＝-Δx₀/2，Δl₂₀＝Δx₀/2，Δl₃₀＝-Δy₀

5)按步骤4得到的调整量控制电机移动支腿。返回步骤1进入下一次调节。

2.如果装载平台某一支腿出现卡死不能移动时，能够通过调节其他支腿以达到调平目标(装载平台或装载物)的水平。

为检测是否有支腿卡死的情况出现，在控制系统中编写一中断循环程序，程序中对每个支腿设置3个计数器，一个是支腿驱动数据的绝对值累加计数器，一个用于存储支腿驱动数据增量值，一个用于存储反馈数据的增量值。当支腿的驱动数据绝对值到某一数值时，检验 支腿驱动数据增量值和反馈数据增量值的差值，若在要求的范围以内，说明支腿运动正常，对以上3个计数器清零继续工作；若差值超差，说明支腿运动异常，系统将作出卡死报警。

当某一支腿卡死时，调平策略，方法如下：

保持卡死的支腿不动，其他的支腿调整值减去卡死支腿的调整值，假设支腿一卡死，则其他三条支腿的调整量为：

Δl₂₀＝-Δy/2-Δl₁₀；Δl₃₀＝Δx/2-Δl₁₀；Δl₄₀＝Δy/2-Δl₁₀

若为三支腿支撑平台，则调整量为Δl₁₀＝-Δx₀/2，Δl₂₀＝Δx₀/2，Δl₃₀＝-Δy₀。

3.可实现调节范围外自动限位，支腿的移动部分具有上下限位开关和位移传感器，当支腿行程超限时触发限位开关闭合，实现硬限位。同时编有限位控制程序，接收来自位移传感器的数据，当支腿位移超过上限值或下限值时，触发报警开关。

4.若装载平台上安放有两个装载物，在不考虑另一装载物的水平情况下，可控制任一装载物为调平目标。假设装载平台上有装载物A和B，A和B上分别安放有水平仪，现只调节装载物A的水平，分别将装载物A初始水平时和倾斜后的水平仪坐标系下的倾角值转换成为装载物A坐标系下的倾角值，判断是否超差，利用权力3所述方法调平。同样，也可只调节装载物B的水平。

5.平台测控系统配备2套电子水平装置，每套电子水平装置均具备独立调平控制功能，可以在主备水平仪之间实现切换。如果在调平系统运行期间控制系统连续多次接收不到水平仪的反馈数据，则认为该水平仪出现故障，切换到备用水平仪。

实施例：四支腿支撑平台以装载平台为调平目标，水平仪安放在装载平台上。

本实施例提供了一种装载平台及其装载物的水平调节控制方法，所述装载平台设有四条支腿(如图2)，每条支腿上配置电机控制支腿的伸缩，该方法包括：

步骤1.运用PLC的中断循环组织块实时向水平仪发送反馈当前倾角数据的请求，监测水平仪的倾角(如图3)，假设初始水平状态下水平仪坐标系X″方向的倾角为θ_X0″＝0.0400rad,Y″方向的倾角为θ_Y0″＝0.0500rad

步骤2.求倾斜矢量R″在水平仪坐标系O″-X″Y″Z″中的表达式i₀″,j₀″,k₀″

$\left\{\begin{array}{c}\tan {{\mathrm{\θ}}_{X0}}^{\′\′}=\frac{{k_0}^{\′\′}}{{i_0}^{\′\′}}\\ \tan {{\mathrm{\θ}}_{Y0}}^{\′\′}=\frac{{k_0}^{\′\′}}{{j_0}^{\′\′}}\\ {i_0}^{\′\′2}+{j_0}^{\′\′2}+{k_0}^{\′\′2}=1\\ \end{array}\right.$

得出i₀″＝0.7800，j₀″＝0.6240，k₀″＝0.0312

由于安装位置的限制，水平仪坐标系一般与装载平台坐标系不重合，由倾斜矢量R″在水平仪坐标系O″-X″Y″Z″中的表达式i₀″,j₀″,k₀″转换成倾斜矢量R在装载平台坐标系O-XYZ的表达式i₀,j₀,k₀，如图4：

①绕OZ″旋转α＝0.1800rad，OX″,OY″旋转至OX⁰,OY⁰；

②绕OY⁰旋转β＝0.3500rad，OX⁰,OZ″旋转至OX,OZ⁰；

③绕OX旋转γ＝0.5000rad，OY⁰,OZ⁰旋转至OY,OZ。

α,β,γ为三维空间直角坐标变换的三个旋转角，与它相对应的旋转矩阵分别为：

$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$

$T_2=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$

$T_3=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

因此，可求 

$\left[\begin{array}{c}{i}_0\\ j_0\\ k_0\end{array}\right]=T_3^{-1}{T}_2^{-1}{T}_1^{-1}\left[\begin{array}{c}{i_0}^{\′\′}\\ {j_0}^{\′\′}\\ {k_0}^{\′\′}\end{array}\right]=T\left[\begin{array}{c}{i_0}^{\′\′}\\ {j_0}^{\′\′}\\ {k_0}^{\′\′}\end{array}\right]$

得出i₀＝0.3942,j₀＝0.9086,k₀＝-0.1337；

步骤3，求调整角度和调整位移，如图5，装载平台的边长是l＝2828mm

初始安装倾斜角 ${\mathrm{\θ}}_{X0}=\arctan \frac{k_0}{i_0}=\arctan \frac{-0.1337}{0.3942}=-0.3270\mathrm{rad},$

${\mathrm{\θ}}_{Y0}=\arctan \frac{k_0}{j_0}=\arctan \frac{-0.1337}{0.9086}=-0.1461\mathrm{rad}$

假设某时刻监测到的在水平仪坐标系X″方向的倾角为θ_X″₁＝0.0500rad,Y″方向的倾角为θ_Y″₁＝0.0700rad，同理重复步骤2、3可得当前状态的倾斜矢量R₁＝(i₁,j₁,k₁)＝(0.4437,0.8859,-0.1437)

倾角θ_X1＝-0.3132rad，θ_Y1＝-0.1608rad

用当前倾角θ_X1,θ_Y1分别与初始水平时的倾角θ_X0,θ_Y0作差即可得调整角

Δθ_X＝θ_X1-θ_X0＝-0.3132-(-0.3270)＝0.0138rad，

Δθ_Y＝θ_Y1-θ_Y0＝-0.1608-(-0.1461)＝-0.0147rad

倾斜角 $\mathrm{\θ}=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_X^2+\mathrm{\Δ}{\mathrm{\θ}}_Y^2}=\sqrt{{0.0138}^2+{(-0.0147)}^2}=0.0202\mathrm{rad}$

设极限角θ_L＝0.0005rad，因θ＞θ_L，所以需要调整载物平台，执行步骤4。

步骤4.计算调整位移 

$\mathrm{\Δx}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_X}{\mathrm{\θ}}=\sqrt{2}\×2828\tan 0.0005\×\frac{0.0189}{0.1386}=0.2727\mathrm{mm},$

$\mathrm{\Δy}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_Y}{\mathrm{\θ}}=-\sqrt{2}\×2828\tan 0.0005\×\frac{0.1373}{0.1386}=-1.9809\mathrm{mm}$

求调整量，设装载平台四条支腿的初始高度分别为l₁₀,l₂₀,l₃₀和l₄₀，为了保证平台的高度不超限，可使装载平台中心固定，则四条支腿的调整量为

Δl₁₀＝-Δx/2＝0.2727/2＝-0.1364mm，

Δl₂₀＝-Δy/2＝1.9809/2＝0.9905mm，

Δl₃₀＝Δx/2＝0.2727/2＝0.1364mm，

Δl₄₀＝Δy/2＝-1.9809/2＝-0.9905mm；

若要求平台中心移动距离Δl，则可在原调整量上加上Δl，即

Δl₁₀＝-Δx/2+Δl，Δl₂₀＝-Δy/2+Δl，Δl₃₀＝Δx/2+Δl，Δl₄₀＝Δy/2-Δl₁₀

步骤5.按步骤4计算出的调整量控制电机移动支腿。

如果支腿1卡死，则其他三条支腿的调整量为：

Δl₂₀＝-Δy/2-Δl₁₀＝0.9905-(-0.1364)＝1.1269mm，

Δl₃₀＝Δx/2-Δl₁₀＝0.1364-(-0.1364)＝0.2728mm，

Δl₄₀＝Δy/2-Δl₁₀＝-0.9905-(-0.1364)＝-0.8541mm

至于权利2所列出的其他三种情况，与该实例类似。现以情况②说明，假设初始安放时装载物是水平状态，水平仪读数为θ″_X0、θ″_Y0，转换成装载平台坐标系下的读数为θ_X0、θ_Y0。某时刻倾角超限读数变为θ″_X1、θ″_Y1，转换成装载平台坐标系下的读数为θ_X1、θ_Y1，则实际倾角为：θ_X＝θ_X1-θ_X0,θ_Y＝θ_Y1-θ_Y0

接下来算法与实施例步骤4以后相同。因为调平目标为装载物，所以不用考虑平台的水平状况。

Claims (2)

1.一种装载平台及装载物的水平调节控制方法，其特征在于：

(1)采集水平仪数据：调平目标水平时，装载平台坐标系为O-XYZ、装载物坐标系为O′-X′Y′Z′和水平仪坐标系为O″-X″Y″Z″，水平仪在X轴Y轴上的倾角为θ″_X0,θ″_Y0，采集当前水平仪读数为θ″_X1,θ″_Y1；

(2)将水平仪坐标系下的倾角θ″_X0,θ″_Y0单位化，表达式为(i″₀,j″₀,k″₀)，其中 $\tan {{\mathrm{\θ}}^{\′\′}}_{X0}=\frac{{k^{\′\′}}_0}{{i^{\′\′}}_0},\tan {{\mathrm{\θ}}^{\′\′}}_{Y0}=\frac{{k^{\′\′}}_0}{{j^{\′\′}}_0},{i^{\′\′}}_0^2+{j^{\′\′}}_0^2+{k^{\′\′}}_0^2=1;$

(3)将水平仪坐标系下的表达式(i″₀,j″₀,k″₀)利用转换矩阵转换成为调平目标下的表达式，平台为调平目标，转换后的倾角表达式变为(i₀,j₀,k₀)，求出倾角值

${\mathrm{\θ}}_{X0}=\arctan \frac{k_0}{i_0},{\mathrm{\θ}}_{Y0}=\arctan \frac{k_0}{j_0},$

求出当前装载平台的倾角值θ_X1,θ_Y1；

(4)用当前倾斜角θ_X1,θ_Y1分别与初始安装倾斜角θ_X,θ_Y作差得到调整角：

Δθ_X＝θ_X1-θ_X0，

Δθ_Y＝θ_Y1-θ_Y0，

合成后的倾斜角：

$\mathrm{\θ}=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_X^2+\mathrm{\Δ}{\mathrm{\θ}}_Y^2},$

测量极限倾斜角θ_L，如果θ＞θ_L，继续调整载物平台；如果θ≤θ_L，不再需要调整载物平台；

(5)再次置平装载平台的支腿，调整位移 $\mathrm{\Δx}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_X}{\mathrm{\θ}},\mathrm{\Δy}=\sqrt{2}l\tan {\mathrm{\θ}}_L\frac{\mathrm{\Δ}{\mathrm{\θ}}_Y}{\mathrm{\θ}}.$

2.根据权利要求1所述的一种装载平台及装载物的水平调节控制方法，其特征在于：所述的转换矩阵为：

$\left[\begin{array}{c}i\\ j\\ k\end{array}\right]=T_3^{-1}{T}_2^{-1}{T}_1^{-1}\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right]=T\left[\begin{array}{c}{i}^{\′\′}\\ j^{\′\′}\\ k^{\′\′}\end{array}\right],$

$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right],$

$T_2=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right],$

$T_3=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

其中α,β,γ为三维空间直角坐标变换的三个旋转角。