CN103792843A - 一种惯性平台快速转动控制方法 - Google Patents

Abstract

本发明提供一种惯性平台快速转动控制方法，本方法在惯性平台快速转位过程的每个加矩周期进行如下工作：根据平台的当前框架角和目标框架角（台体处于目标角位置时的框架角）计算台体转位前后两个角位置之间的四元数；根据该四元数计算两个角位置之间的等效旋转矢量（等效旋转角取绝对值小于180°的值）；根据等效旋转矢量和平台各陀螺的加矩角速度饱和值计算加矩角速度；然后，根据加矩角速度计算值对平台各陀螺进行加矩转位。本发明大幅度缩短快速转位所需时间。

Description

一种惯性平台快速转动控制方法

技术领域

本发明提供一种将惯性平台台体转到规定角位置的快速转动控制方法，属于惯性导航领域。

背景技术

惯性平台台体安装在其壳体内的环架系统中。惯性平台系统利用陀螺仪敏感平台台体在空间的角运动，以陀螺敏感到的台体角运动为依据通过稳定回路将平台台体控制在相对惯性空间没有角运动的状态，为安装在台体上的加速度计提供稳定的空间方向基准（导航坐标系）。台体上的加速度计敏感载体在导航坐标系内的线加速度，对其进行积分得到载体在导航坐标系内的速度和位置，从而实现导航功能。

惯性平台通电启动之初，台体的角位置处于随机状态。进入正常工作前，需要将台体转到与导航坐标系重合的角位置。此外，平台多位置自标定过程也需要将台体转到规定的各个角位置。因此，转位是惯性平台必备的功能。转位过程分为快速转位、粗锁定和精锁定三个阶段。常用的快速转位过程如下：首先对输入轴与台体轴平行的陀螺加矩，使台体绕台体轴转动到角度为0°的状态；然后同时对另外两个陀螺加矩使台体转到两外两个框架角为目标框架角的位置；最后对输入轴与台体轴平行的陀螺加矩，使台体轴转动到规定的角位置。以图1所示的三轴平台为例。假设台体初始状态下的框架角：θ_x=150°，θ_y=30°，θ_z=60°。目标框架角为：θ_xd=0°，θ_yd=0°，θ_zd=-160°。则常用快速转位算法流程为：台体绕z_p轴转-60°→台体绕x_p和y_p轴分别同时转动-150°和-30°→台体绕z_p轴转-160°。上述转位过程简单直观，但存在以下两个缺点：台体轴和两外两轴的转动不能同时执行，转动过程分为三个阶段串行执行；台体轴必须首先转到0°，最后再由0°转到目标角位置。上述两个缺点导致转位过程存在时间的浪费。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种惯性平台快速转动控制方法，本发明通过计算当前框架角和目标框架角位置之间的等效旋转矢量，并根据等效旋转矢量控制平台各陀螺进行转动，解决了平台多个轴同时转动的问题。

本发明的技术解决方案是：

一种惯性平台快速转动控制方法，包括步骤如下：

(1)采集当前框架角为θ_x、θ_y和θ_z；

(2)根据如下公式计算惯性平台由当前框架角转到目标框架角的四元数

$Q_{{p2p}_d}=Q_z\left({\mathrm{\θ}}_z\right)\⊗Q_y\left({\mathrm{\θ}}_y\right)\⊗Q_x({\mathrm{\θ}}_x-{\mathrm{\θ}}_{\mathrm{xd}}){\⊗Q}_y(-{\mathrm{\θ}}_{\mathrm{yd}})\⊗Q_z\left({-\mathrm{\θ}}_{\text{zd}}\right)$

其中：

i为沿台体x_p轴的单位虚矢量；

j为沿台体y_p轴的单位虚矢量；

k为沿台体z_p轴的单位虚矢量；

表示四元数乘法，θ_xd、θ_yd和θ_zd为惯性平台的目标框架角；

(3)将四元数

的计算结果记为向量形式

并根据如下公式计算惯性平台当前框架角转相对目标框架角转的等效旋转矢量φ

$\mathrm{\φ}=\frac{{[q_1,q_2,q_3]}^T}{\sqrt{q_1^2+q_2^2+q_3^2}}\mathrm{\φ}$

其中：

q₀、q₁、q₂和q₃分别为

的4个分量；

(4)将步骤（3）中计算得到的等效旋转矢量φ记为

则根据如下公式计算控制台体转动的角速度：

其中，ω_max表示惯性平台绕台体各轴的最大许可旋转角速度，T为惯性平台计算机板对惯性平台台体的控制周期；

(5)根据步骤（4）中得到的ω_cmd控制惯性平台台体转位；

(6)重复执行步骤(1)～(5)，直到

的值减小到预先设定的范围内。

本发明与现有技术相比的有益效果是：

（1）本发明相对于常用快速转位算法，本发明平台多轴同时转动，转动过程时间节省，效率增强。传统方式台体轴需要首先转回0°，然后才能绕另外两轴转动，最后台体轴还要从0°转到规定的角位置；而本发明平台各轴同时转动。例如：假设初始状态下的框架角为θ_x=150°，θ_y=30°，θ_z=60°；目标框架角为θ_x0=0°，θ_y0=0°，θ_z0=-160°；绕每个轴的转位精度要求均为1°，台体绕其自身每个轴允许的最大转动角速度范围是-5～5°/s。采用传统方法时，则整个转位过程按串行转过的角度为370°，最大转动角速度为5°/s，整个转动过程所需时间为74秒。采用本算法时，台体绕等效旋转矢量轴的转动角度为90.59°，绕等效旋转矢量轴的最大转动角速度为5.36°/s，整个转动过程所需时间为16.89秒，说明采用本发明中的算法能大幅度缩短快速转位所需时间。

（2）本发明台体绕等效旋转轴的最大转动角速度大于或等于台体绕单个轴(x_p、y_p或z_p)的最大角速度；台体绕等效旋转轴转到目标状态所需转过的角度小于等于台体分阶段转位时各阶段转过的角度之和。从而缩短台体转动所需的时间。

附图说明

图1为三轴平台结构示意图；

图2为本发明方法流程图。

具体实施方式

下面结合附图对本发明的具体实施方式进行进一步的详细描述。

如图2所示，本发明一种惯性平台快速转动控制方法，包括步骤如下：

(1)采集当前框架角为θ_x，θ_y和θ_z；

(2)根据如下公式计算惯性平台由当前框架角转到目标框架角的四元数

从当前框架角到各框架角均为0°的四元数Q_p2b根据如下公式计算

$Q_{p2b}=Q_z\left({\mathrm{\θ}}_z\right)\⊗Q_y\left({\mathrm{\θ}}_y\right)\⊗Q_x\left({\mathrm{\θ}}_x\right)$

从各框架角均为0°到目标框架角的四元数

根据如下公式计算

$Q_{{b2p}_d}=Q_x\left({-\mathrm{\θ}}_{\mathrm{xd}}\right)\⊗Q_y\left({-\mathrm{\θ}}_{\mathrm{yd}}\right)\⊗Q_z\left({-\mathrm{\θ}}_{\mathrm{zd}}\right)$

所以由当前框架角转到目标框架角的四元数为

$\begin{array}{c}{Q}_{{p2p}_d}=Q_{p2b}\⊗Q_{{b2p}_d}\\ =Q_z\left({\mathrm{\θ}}_z\right)\⊗Q_y\left({\mathrm{\θ}}_y\right)\⊗Q_x\left({\mathrm{\θ}}_x\right)\⊗Q_x\left({-\mathrm{\θ}}_{\mathrm{xd}}\right)\⊗Q_y(-{\mathrm{\θ}}_{\mathrm{yd}})\⊗Q_z\left({-\mathrm{\θ}}_{\mathrm{zd}}\right)\end{array}$

化简得

$Q_{{p2p}_d}=Q_z\left({\mathrm{\θ}}_z\right)\⊗Q_y\left({\mathrm{\θ}}_y\right)\⊗Q_x({\mathrm{\θ}}_x-{\mathrm{\θ}}_{\mathrm{xd}})\⊗Q_y\left({-\mathrm{\θ}}_{\mathrm{yd}}\right)\⊗Q_z\left({-\mathrm{\θ}}_{\mathrm{zd}}\right)$

其中：i为沿台体x_p轴的单位虚矢量；

j为沿台体y_p轴的单位虚矢量；

k为沿台体z_p轴的单位虚矢量；

表示四元数乘法，θ_xd、θ_yd和θ_zd为惯性平台x_p、y_p和z_p轴的目标框架角；

(3)将四元数

的计算结果记为向量形式

并根据如下公式计算惯性平台当前框架角转相对目标框架角转的等效旋转矢量φ

$\mathrm{\φ}=\frac{{[q_1,q_2,q_3]}^T}{\sqrt{q_1^2+q_2^2+q_3^2}}\mathrm{\φ}$

其中：

q₀、q₁、q₂和q₃分别为

的4个分量；

(4)将步骤（3）中计算得到的等效旋转矢量φ记为则根据如下公式计算控制台体转动的角速度ω_cmd：

一个控制周期内转到目标角位置所需的角速度ω_exp为

${\mathrm{\ω}}_{\exp }=\frac{{[{\mathrm{\φ}}_{x_p},{\mathrm{\φ}}_{y_p},{\mathrm{\φ}}_{z_p}]}^T}{T}$

如果ω_exp沿台体每个轴的分量均在最大许可旋转角速度范围内即

$\max (\frac{\left|{\mathrm{\&phi;}}_{x_p}\right|}{T},\frac{\left|{\mathrm{\&phi;}}_{y_p}\right|}{T},\frac{\left|{\mathrm{\&phi;}}_{z_p}\right|}{T})<={\mathrm{\&omega;}}_{\max }$

则控制台体转动的角速度ω_cmd为

${\mathrm{\&omega;}}_{\mathrm{cmd}}=\frac{{[{\mathrm{\&phi;}}_{x_p},{\mathrm{\&phi;}}_{y_p},{\mathrm{\&phi;}}_{z_p}]}^T}{T}$

如果ω_exp沿台体某个轴的分量超过最大许可旋转角速度，则ω_exp最大分量方向按照最大许可角速度加矩，加矩方向沿等效旋转矢量轴向，即控制台体转动的角速度ω_cmd为

${\mathrm{\&omega;}}_{\mathrm{cmd}}={\mathrm{\&omega;}}_{\max }\frac{{[{\mathrm{\&phi;}}_{x_p},{\mathrm{\&phi;}}_{y_p},{\mathrm{\&phi;}}_{z_p}]}^T}{\max \left(\right|{\mathrm{\&phi;}}_{x_p}|,|{\mathrm{\&phi;}}_{y_p}|,|{\mathrm{\&phi;}}_{z_p}\left|\right)}$

所以控制台体转动的角速度为

其中，ω_max表示惯性平台绕台体各轴的最大许可旋转角速度，T为惯性平台计算机板对惯性平台台体的控制周期；

(5)根据步骤（4）中得到的ω_cmd控制惯性平台台体转位；

(6)重复执行步骤(1)～(5)，直到

的值减小到预先设定的范围内。

通过仿真实例可以验证本发明的效果。假设台体初始状态下的框架角为θ_x=150°，θ_y=30°，θ_z=60°；目标框架角为θ_x0=0°，θ_y0=0°，θ_z0=-160°；绕每个轴的转位精度要求均为1°，台体绕其自身每个轴允许的最大转动角速度范围是-5～5°/s。按照传统快速转位算法台体绕z_p轴转-60°→台体绕x_p和y_p轴分别同时转动-150°和-30°→台体绕z_p轴转-160°，转位过程中，控制台体均按最大角速度绕各轴转动则整个转动过程所需时间为74秒。如果采用本发明中方法整个转动过程所需时间仅为16.89秒。说明采用本发明中的算法能大幅度缩短快速转位所需时间。

本发明说明书中未作详细描述的内容属于本领域技术人员的公知技术。

Claims (1)

1.一种惯性平台快速转动控制方法，其特征在于步骤如下：

(1)采集当前框架角为θ_x、θ_y和θ_z；

(2)根据如下公式计算惯性平台由当前框架角转到目标框架角的四元数

$Q_{{p2p}_d}=Q_z\left({\mathrm{\&theta;}}_z\right)\&CircleTimes;Q_y\left({\mathrm{\&theta;}}_y\right)\&CircleTimes;Q_x({\mathrm{\&theta;}}_x-{\mathrm{\&theta;}}_{\mathrm{xd}}){\&CircleTimes;Q}_y(-{\mathrm{\&theta;}}_{\mathrm{yd}})\&CircleTimes;Q_z\left({-\mathrm{\&theta;}}_{\text{zd}}\right)$

其中：

i为沿台体x_p轴的单位虚矢量；

j为沿台体y_p轴的单位虚矢量；

k为沿台体z_p轴的单位虚矢量；

表示四元数乘法，θ_xd、θ_yd和θ_zd为惯性平台的目标框架角；

(3)将四元数

的计算结果记为向量形式

并根据如下公式计算惯性平台当前框架角转相对目标框架角转的等效旋转矢量φ

$\mathrm{\&phi;}=\frac{{[q_1,q_2,q_3]}^T}{\sqrt{q_1^2+q_2^2+q_3^2}}\mathrm{\&phi;}$

其中：q₀、q₁、q₂和q₃分别为的4个分量；

(4)将步骤（3）中计算得到的等效旋转矢量φ记为

则根据如下公式计算控制台体转动的角速度：

其中，ω_max表示惯性平台绕台体各轴的最大许可旋转角速度，T为惯性平台计算机板对惯性平台台体的控制周期；

(5)根据步骤（4）中得到的ω_cmd控制惯性平台台体转位；

(6)重复执行步骤(1)～(5)，直到

的值减小到预先设定的范围内。

Images