CN102346486B - 一种基于pd控制律的姿态控制ip核的卫星姿态控制力矩计算*** - Google Patents

Abstract

一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，涉及航天电子领域。本发明的目的是提供一种计算速度快、可重用性高的基于PD控制律的姿态控制IP核，以及采用该IP核的卫星姿态控制力矩计算***。所述IP核采用FPGA外部输入时钟作为计算时钟，采用减法器将输入的参考姿态角θ_c与当前姿态角参数做差后输出给乘法器，该乘法器将该数据与比例系数K_P相乘获得一个乘积；采用乘法器将姿态角速度ω与P微分系数K_D相乘，获得的结果与前面的乘积采用减法器做差获得姿态力矩。本发明的控制力矩计算***采用上述IP核实现力矩运算。本发明所述的IP核比现有采用软件方法的计算速度至少提高了5倍，且具有很好的可重复利用性。

Description

一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***

技术领域

本发明涉及航天电子领域，尤其涉及一种卫星应用的、采用硬件描述语言实现的基于PD控制律的卫星姿态控制专用IP核及设计方法。

背景技术

随着技术的发展，国内外将FPGA用于实现各种滤波算法和控制器已经有较多讨论，且FPGA在航天领域的应用也越来越多。大规模的基于SRAM的FPGA，同基于反熔丝的FPGA一样，已经可以被应用于航天领域。建立基于FPGA的姿态控制***，利用了FPGA处理速度快，结构简单、轻便的优点，以硬件处理代替软件计算。既减轻了星载计算机的负担，又减少了姿态控制对星载计算机的依赖，为微小卫星的正常运行提供了保障。这将会是姿态控制技术一个重要的发展方向。

PD控制算法是卫星姿态控制算法中最常用的算法，将其利用IP核实现，通过以FPGA为载体的星载计算机片内总线连接CPU，可以协同星载计算机完成姿态控制算法的计算，从而减小星载计算机的负担，并提高姿态控制***的性能。

坐标系定义到控制算法

设卫星的绝对角速度，即卫星本体相对于惯性空间的姿态角速度为ω；轨道坐标系相对于地心惯性坐标系的角速度为ω₀；卫星本体相对于轨道坐标系的角速度为ω_bo；由角速度矢量的叠加性得：

ω=ω_bo+ω₀    (1)

其中ω₀=[0 -ω₀ 0]^T，ω₀为航天器质心绕地心的轨道角速度。

采用ZYX顺序旋转的欧拉角参数来描述卫星本体坐标相对轨道坐标系的姿态，则姿态角速度ω在星体坐标系下的分量列阵可表示为：

$\left(2\right)$

其中ω_x、ω_y、ω_z分别为卫星相对惯性空间的姿态角速度ω在星体坐标系x、y、z轴的分量，

θ、ψ分别为ZYX旋转顺序下的三个欧拉角，

分别为相应欧拉角的变化率， C _y(θ)分别为旋转过程中的姿态转换矩阵，

为轨道坐标系到星体坐标系的姿态转换矩阵。

在此基础上建立卫星姿态动力学方程,将卫星视为刚体，取***质心C为参考点时，根据动量矩定理：角动量h对时间的导数等于作用在刚体质心的外力矩T。根据矢量在不同坐标系下对时间的导数关系有：

$\frac{d_{I}h}{\mathrm{dt}}=\frac{d_{B}h}{\mathrm{dt}}+\mathrm{\ω}\×h=T---\left(3\right)$

下标_I代表在惯性系下对时间的导数，而_B代表在刚体固联坐标系下对时间的导数。将h=I·ω带入上式得：

$I\·\stackrel{\·}{\mathrm{\ω}}+\mathrm{\ω}\×h=T---\left(4\right)$

其中I为星体转动惯量矩阵，ω为星体相对惯性系的转动角速度，h=I·ω为星体角动量，T为星体所受的外力矩。

控制算法：

在(1)式中ω₀可以表示为：

其中θ为真近点角，p为轨道半正交弦，μ为中心天体引力常数。若卫星轨道为圆形，轨道半径R与半正交弦p相等，则有：

${\mathrm{\ω}}_0=\sqrt{\frac{\mathrm{\μ}}{R^3}}$

在稳定的姿态控制情况下，航天器体坐标系相对于轨道坐标系的偏差很小，使得姿态角和姿态角速度ω均为小量。因此可只保留这些量的一次项，而略去二次以上各项。ω可以简化为：

对上式求导得：

当星体的本体坐标系选择星体的惯性主轴时，动力学方程可以简化为：

$I_x{\stackrel{\·}{\mathrm{\ω}}}_x-(I_y-I_z){\mathrm{\ω}}_y{\mathrm{\ω}}_z=T_x$

$I_y{\stackrel{\·}{\mathrm{\ω}}}_y-(I_z-I_x){\mathrm{\ω}}_x{\mathrm{\ω}}_z=T_y---\left(7\right)$

$I_z{\stackrel{\·}{\mathrm{\ω}}}_z-(I_x-I_y){\mathrm{\ω}}_x{\mathrm{\ω}}_y=T_z$

将式

和ω带入上式得：

$I_y\stackrel{\·\·}{\mathrm{\θ}}=T_y---\left(8\right)$

由于推力器产生的控制力矩通常较大，远大于类似

和

这样的耦合项，因此这些耦合力矩在设计和分析控制***的过程中可以忽略不计。上式可进一步简化为：

$I_y\stackrel{\·\·}{\mathrm{\θ}}=T_{\mathrm{cy}}+T_{\mathrm{dy}}---\left(9\right)$

$I_z\stackrel{\·\·}{\mathrm{\ψ}}=T_{\mathrm{cz}}+T_{\mathrm{dz}}$

其中T_cx、T_cy、T_cz分别为x、y、z三个坐标轴方向的控制力矩，T_dx、T_dy、T_dz分别为x、y、z三个坐标轴方向的干扰力矩。

以俯仰通道为例，采用PD控制律可取控制力矩为：

$T_{\mathrm{cy}}=-K_P\mathrm{\θ}-K_D\stackrel{\·}{\mathrm{\θ}}---\left(10\right)$

式中K_P为比例系数，K_D为微分系数。

将其代入式(9)第二式中得：

$I_y\stackrel{\·\·}{\mathrm{\θ}}+K_D\stackrel{\·}{\mathrm{\θ}}+K_P\mathrm{\θ}=T_{\mathrm{dy}}$

令：

$2\mathrm{\ξ}{\mathrm{\ω}}_n=\frac{K_D}{I_y},{\mathrm{\ω}}_n^2=\frac{K_P}{I_y}$

得：

$\stackrel{\·\·}{\mathrm{\θ}}+{2\mathrm{\ξ\ω}}_n\stackrel{\·}{\mathrm{\θ}}\text{+}{\mathrm{\ω}}_n^2\mathrm{\θ}=\frac{T_{\mathrm{dy}}}{I_y}---\left(11\right)$

其中ω_n为无阻尼自振荡频率，ξ为阻尼比。

式(11)为典型的二阶***环节，ξ和ω_n决定了***的动态过程品质及稳态精度。若给定阻尼比ξ和调整时间t_s，可根据下式计算无阻尼自振频率ω_n，进而计算出系数K_P、K_D。

$t_s\left(0.02\right)=\frac{4\mathrm{\ξ}}{{\mathrm{\ω}}_n}$

采用欧拉角反馈的PD控制律可以表示如下：

T_cy=K_P(θ_c-θ)-K_D(ω_y)。(12)

T_cz=K_P(ψ_c-ψ)-K_D(ω_z)

PD控制算法的IP化设计方法：

目前对于卫星姿态控制多采用FORTRAN、C、C＋＋等语言实现串行计算，这种实现方式运算周期长。以ARM为例，每个乘法需要进行从寄存器中读数等操作，执行7个周期，每个加法需执行5个周期，计算结果输出需要2个周期，则完成乘法与加法一共需要180个计算周期。另外，这种设计方法重用性低，不利于航天电子***快速化设计。

发明内容

本发明的目的是提供一种计算速度快、可重用性高的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***。

本发明所述的一种基于PD控制律的姿态控制IP核包括两个减法器和两个乘法器，该姿态控制IP核采用FPGA外部输入时钟作为计算时钟；

其中，第一减法器11的正输入端输入的参考姿态角θ_c，该第一减法器11的负输入端输入当前姿态角参数，该第一减法器11的输出端输出数据给第一乘法器12，该第一乘法器12将该数据与PD控制算法中比例系数K_P相乘之后输出乘积结果数据给第二减法器13的正输入端；

第二乘法器14将输入的姿态角速度ω参数与PD控制算法中的微分系数K_D相乘之后输出乘积结果给第二减法器13的负输入端，该第二减法器13输出结果即为控制力矩参数。

所述一种基于PD控制律的姿态控制IP核的每个计算周期为5个时钟周期，具体过程为：

第一个时钟周期，将输入的参考姿态角θ_c传给第一减法器11的正输入端，当前姿态角传给第一减法器11的负输入端，姿态角速度ω传给第二乘法器14的一个输入端；

第二个时钟周期，第一减法器11和第二乘法器14分别完成减法和乘法运算；

第三个时钟周期，第一乘法器12完成乘法运算，对第二乘法器14的结果进行一个周期的延时；

第四个时钟周期，第二减法器13对第一乘法器12和第二乘法器14的结果进行减法运算；

第五个时钟周期，第二减法器13输出运算结果，完成计算。

采用上述基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***包括：

处理器核21，用于通过外部总线读取用于计算姿态控制力矩所需的数据，所述数据包括参考姿态角θ_c、卫星当前姿态角θ和角速度，并将所述数据写入数据获取寄存器；还用于通过片内总线从数据输出寄存器中读取姿态控制力矩数据并输出给外部数据总线；

数据获取寄存器单元22，用于存储用于计算姿态控制力矩的参数信息，所属参数信息为32位浮点数格式；

数据类型转换单元23，用于读取数据获取寄存器中的32位浮点数格式的参数信息，并将所述32位浮点数格式的参数转换成定点数格式的数据；还用于将姿态控制计算单元24发送的定点数格式的姿态控制力矩参数转换为32位浮点数格式，并将转换后的数据存储至数据输出寄存器；

数据输出寄存器单元25，用于存储数据类型转换单元23发送的32位浮点数格式的姿态控制力矩参数；

姿态控制计算单元24包括基于PD控制律的姿态控制IP核，该姿态控制计算单元24用于从数据类型转换单元23中读取计算需要的定点数格式的数据，并将所述数据发送给基于PD控制律的姿态控制IP核，还用于将所述基于PD控制律的姿态控制IP核输出的姿态控制力矩参数发送给数据类型转换单元23。

本发明将姿态控制算法做成IP核，可实现姿态控制算法的硬件化。与软件设计方法相比，计算速度至少提高了5倍，且具有很好的可重复利用性，为卫星的姿态控制***的设计提供了良好的设计基础。

本发明所述的基于PD控制律的卫星姿态控制IP核以及采用该IP核的卫星姿态控制力矩计算***具有以下有益效果：

1）采用硬件编程方式实现基于PD姿态控制律的IP核，可使姿态控制力矩的计算采用硬件电路的方式实现，与现有采用软件实现该计算方法的速度相比，计算速度至少提高5倍；

2）采用模块化设计，具有很好的可重复利用性，为卫星姿态控制***的设计提供了良好的设计基础。

附图说明

图1为本发明所述的一种基于PD控制律的姿态控制IP核的电路原理示意图；图2是图1所示的基于PD控制律的姿态控制IP核的工作过程示意图；图3是本发明所述的卫星姿态控制力矩计算***的原理框图；图4是具体实施方式五所述的将32位单精度浮点数格式的数据转换到18_16位定点数格式的数据的流程图；图5是具体实施方式六所述的将20_16位定点数格式的数据转换成32位浮点数格式的数据的流程图。

具体实施方式

具体实施方式一、参见图1说明本实施方式。本实施方式所述的一种基于PD控制律的姿态控制IP核包括两个减法器和两个乘法器，该姿态控制IP核采用FPGA外部输入时钟作为计算时钟；

其中，第一减法器11的正输入端输入的参考姿态角θ_c，该第一减法器11的负输入端输入当前姿态角参数，该第一减法器11的输出端输出数据给第一乘法器12，该第一乘法器12将该数据与PD控制算法中比例系数K_P相乘之后输出乘积结果数据给第二减法器13的正输入端；

第二乘法器14将输入的姿态角速度ω参数与PD控制算法中的微分系数K_D相乘之后输出乘积结果给第二减法器13的负输入端，该第二减法器13输出结果即为控制力矩参数。

本实施方式是采用硬件编程方式将卫星姿态控制***中的姿态控制算法计算单元做成IP核，实现姿态控制算法的硬件计算，所谓硬件编程方式，是指采用硬件描述语言对需要实现的任务或算法进行编程，通过仿真、验证、逻辑综合等一系列专业设计流程，生成具有相应功能的IP核。处理器可通过对IP核对应地址空间的访问实现对IP核的控制。

在实际应用中，参见图1所示，第一减法器11的两个数据输入端和第二乘法器14的一个数据输入端分别连接外部提供的三个参数输入端既可。如果数据类型不适用于姿态控制IP核计算使用，则在该姿态控制IP核的三个数据输入端增加数据格式转换模块既可。

一般情况，外部采集获得的数据都是浮点数格式，而姿态控制IP核计算使用的数据应当是定点格式数据，因此，在实际应用时，参见图1所示，在本实施方式所述的基于PD控制律的姿态控制IP核1的数据输入端增加浮点数转换成定点数的格式转换模块既可。

具体实施方式二、参见图是2说明本实施方式。本实施方式是对具体实施方式一所述一种基于PD控制律的姿态控制IP核的进一步说明，本实施方式中的基于PD控制律的姿态控制IP核的每个计算周期为5个时钟周期，具体过程为：

第一个时钟周期，将输入的参考姿态角θ_c传给第一减法器11的正输入端，当前姿态角传给第一减法器11的负输入端，姿态角速度ω传给第二乘法器14的一个输入端；

第二个时钟周期，第一减法器11和第二乘法器14分别完成减法和乘法运算；

第三个时钟周期，第一乘法器12完成乘法运算，对第二乘法器14的结果进行一个周期的延时；

第四个时钟周期，第二减法器13对第一乘法器12和第二乘法器14的结果进行减法运算；

第五个时钟周期，第二减法器13输出运算结果，完成计算。

本实施方式中所述的基于PD控制律的姿态控制IP核5个时钟周期就能够完成一个计算周期，计算速度快。

具体实施方式三、参见图3说明本实施方式，图中2为本实施方式所述的卫星姿态控制力矩计算***，该卫星姿态控制力矩计算***是采用具体实施方式一或二所述的一种基于PD控制律的姿态控制IP核实现的，本实施方式所述卫星姿态控制力矩计算***包括：

处理器核21，用于通过外部总线读取用于计算姿态控制力矩所需的数据，所述数据包括参考姿态角θ_c、卫星当前姿态角θ和角速度，并将所述数据写入数据获取寄存器；还用于通过片内总线从数据输出寄存器中读取姿态控制力矩数据并输出给外部数据总线；

数据获取寄存器单元22，用于存储用于计算姿态控制力矩的参数信息，所属参数信息为32位浮点数格式；

数据类型转换单元23，用于读取数据获取寄存器中的32位浮点数格式的参数信息，并将所述32位浮点数格式的参数转换成定点数格式的数据；还用于将姿态控制计算单元24发送的定点数格式的姿态控制力矩参数转换为32位浮点数格式，并将转换后的数据存储至数据输出寄存器；

数据输出寄存器单元25，用于存储数据类型转换单元23发送的32位浮点数格式的姿态控制力矩参数；

姿态控制计算单元24包括基于PD控制律的姿态控制IP核，该姿态控制计算单元24用于从数据类型转换单元23中读取计算需要的定点数格式的数据，并将所述数据发送给基于PD控制律的姿态控制IP核，还用于将所述基于PD控制律的姿态控制IP核输出的姿态控制力矩参数发送给数据类型转换单元23。

本实施方式所述的卫星姿态控制力矩计算***可以采用FPGA实现，所述姿态控制计算单元24采用FPGA内时钟周期控制，该时钟信号用于为姿态控制计算单元24中的姿态控制IP核提供计算过程中的数据流动控制信号，即：提供了产生装载、读取和寄存器移位的控制信号。

具体实施方式四、本实施方式是对具体实施方式三所述的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***中的数据获取寄存器单元22的进一步说明，本实施方式所述的数据获取寄存器单元22由3个输入寄存器组成；输出数据寄存器单元由1个输出寄存器组成。

本实施方式中的数据获取寄存器单元22中有3个输入寄存器组，分别用于接收和存储进行姿态控制所需的参考姿态角θ_c、卫星当前姿态角θ和姿态角速度ω三个姿态参数。三个参数的地址是固定地址，便于数据的读取，加快计算速度。

输出数据寄存器单元仅存储一个数据，即：计算获得的力矩参数，因此，该寄存器单元采用1个输出寄存器实现既可。

具体实施方式五、参见图4说明本实施方式。本实施方式是对具体实施方式三所述的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***中的数据类型转换单元23的进一步说明，本实施方式中，所述数据类型转换单元23将所述32位浮点数格式的参数转换成定点数格式的数据，是指将32位单精度浮点数格式的数据转换到18_16位定点数格式的数据，具体过程为：

分别取32位浮点数的符号位、偏置指数位和尾数位；

将尾数第24位写为1；

如果最高位为0，则按如下方法执行：

首先将偏置指数位的数值减去127作为实际数的指数值；

然后将23位尾数的第24位写为1后得到24_23（24位其中23位小数）的定点数形式；

最后按照实际指数的数值，将小数点左移或右移相应的位数，得到18位定点数的低17位；

最终将第18位写为0得到18_16位定点数；

如果最高位为1，则按如下方法执行：

首先将偏置指数位的数值减去127作为实际数的指数；

然后将23位尾数的第24位写为1后再按位取反加1，得到尾数的补码形式；

最后按照实际指数的数值，将小数点左移或右移相应的位数，得到18位定点数的低17位；

最终将第18位写为1得到18_16位定点数。

具体实施方式六、参见图5说明本实施方式。本实施方式是对具体实施方式三所述的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***中的数据类型转换单元23的进一步说明，本实施方式中，数据类型转换单元23将定点数格式的姿态控制力矩参数转换为32位浮点数格式，是指将20_16位定点数格式的数据转换成32位浮点数格式的数据，具体过程为：

获得定点数的最高位；

如果最高位为1，则进行如下操作：

将低19位减1后按位取反，得到新的小数；

如果最高位为0,则将原数作为新的小数；

除符号位以外，按从高位到低位的顺序找出第一个数据位为1的位，并计算出相应的指数大小；

然后将指数加上128后写入浮点数的23到30位，将移位后的小数从高位到低位写入0到22位；

最后将定点数的最高位写入32位浮点数的最高位，至此得到了32位浮点数的表示形式。

以上实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴。

Claims (5)

1.一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，其特征在于，该***包括：

处理器核（21），用于通过外部数据总线读取用于计算姿态控制力矩所需的数据，所述数据包括参考姿态角θ_c、卫星当前姿态角θ和角速度，并将所述数据写入数据获取寄存器；还用于通过片内总线从数据输出寄存器中读取姿态控制力矩数据并输出给外部数据总线；

数据获取寄存器单元（22），用于存储用于计算姿态控制力矩的参数信息，所属参数信息为32位浮点数格式；

数据类型转换单元（23），用于读取数据获取寄存器中的32位浮点数格式的参数信息，并将所述32位浮点数格式的参数转换成定点数格式的数据；还用于将姿态控制计算单元（24）发送的定点数格式的姿态控制力矩参数转换为32位浮点数格式，并将转换后的数据存储至数据输出寄存器；

数据输出寄存器单元（25），用于存储数据类型转换单元（23）发送的32位浮点数格式的姿态控制力矩参数；

姿态控制计算单元（24）包括基于PD控制律的姿态控制IP核，该姿态控制计算单元（24）用于从数据类型转换单元（23）中读取计算需要的定点数格式的数据，并将所述数据发送给基于PD控制律的姿态控制IP核，还用于将所述基于PD控制律的姿态控制IP核输出的姿态控制力矩参数发送给数据类型转换单元（23）；

所述姿态控制IP核包括两个减法器和两个乘法器，该姿态控制IP核采用FPGA外部输入时钟作为计算时钟；

其中，第一减法器（11）的正输入端输入的参考姿态角θ_c，该第一减法器（11）的负输入端输入当前姿态角参数，该第一减法器（11）的输出端输出数据给第一乘法器（12），该第一乘法器（12）将该数据与PD控制算法中比例系数K_P相乘之后输出乘积结果数据给第二减法器（13）的正输入端；

第二乘法器（14）将输入的姿态角速度ω参数与PD控制算法中的微分系数K_D相乘之后输出乘积结果给第二减法器（13）的负输入端，该第二减法器（13）输出结果即为控制力矩参数。

2.如权利要求1所述的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，其特征在于，所述姿态控制IP核的每个计算周期为5个时钟周期，具体过程为：

第一个时钟周期，将输入的参考姿态角θ_c传给第一减法器（11）的正输入端，当前姿态角传给第一减法器（11）的负输入端，姿态角速度ω传给第二乘法器（14）的一个输入端；

第二个时钟周期，第一减法器（11）和第二乘法器（14）分别完成减法和乘法运算；

第三个时钟周期，第一乘法器（12）完成乘法运算，对第二乘法器（14）的结果进行一个周期的延时；

第四个时钟周期，第二减法器（13）对第一乘法器（12）和第二乘法器（14）的结果进行减法运算；

第五个时钟周期，将第二减法器（13）输出运算结果，完成计算。

3.如权利要求1所述的一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，其特征在于，所述数据获取寄存器单元（22）由3个输入寄存器组成；输出数据寄存器单元由1个输出寄存器组成。

4.如权利要求1一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，其特征在于，所述数据类型转换单元（23）将所述32位浮点数格式的参数转换成定点数格式的数据，是指将32位单精度浮点数格式的数据转换到18_16位定点数格式的数据，具体过程为：

分别取32位浮点数的符号位、偏置指数位和尾数位；

将尾数第24位写为1；

如果最高位为0，则按如下方法执行：

首先将偏置指数位的数值减去127作为实际数的指数值；

然后将23位尾数的第24位写为1后得到24_23（24位其中23位小数）的定点数形式；

最后按照实际指数的数值，将小数点左移或右移相应的位数，得到18位定点数的低17位；

最终将第18位写为0得到18_16位定点数；

如果最高位为1，则按如下方法执行：

首先将偏置指数位的数值减去127作为实际数的指数；

然后将23位尾数的第24位写为1后再按位取反加1，得到尾数的补码形式；

最后按照实际指数的数值，将小数点左移或右移相应的位数，得到18位定点数的低17位；

最终将第18位写为1得到18_16位定点数。

5.如权利要求1一种基于PD控制律的姿态控制IP核的卫星姿态控制力矩计算***，其特征在于，数据类型转换单元（23）将定点数格式的姿态控制力矩参数转换为32位浮点数格式，是指将20_16位定点数格式的数据转换成32位浮点数格式的数据，具体过程为：

获得定点数的最高位；

如果最高位为1，则进行如下操作：

将低19位减1后按位取反，得到新的小数；

如果最高位为0，则将原数作为新的小数；

除符号位以外，按从高位到低位的顺序找出第一个数据位为1的位，并计算出相应的指数大小；

然后将指数加上128后写入浮点数的23到30位，将移位后的小数从高位到低位写入0到22位；

最后将定点数的最高位写入32位浮点数的最高位，至此得到了32位浮点数的表示形式。

Images