CN113848780B - 一种多核异构处理器架构下的高机动平台姿态解算装置及方法 - Google Patents

Abstract

本发明提供了一种多核异构处理器架构下的高机动平台姿态解算装置及方法，系统主要包括测量数据采集模块、数据传输模块、姿态融合解算模块和输出模块四部分。测量数据包括角速度、加速度以及磁场强度在内XYZ九轴数据。通过FPGA核采集，测量数据经优化读取方法后通过AXI总线流入ARM核，采用一种基于AHRS的姿态融合算法将数据进行解算得到绝对姿态角。本发明易于实现，有较好的可移植性，能够在采集到数据后快速完成姿态解算，对机动平台的姿态信息进行实时分析，精度高，稳定性强，输出延迟低，可以在各种场景下完成对机动平台的姿态监视和控制。

Description

一种多核异构处理器架构下的高机动平台姿态解算装置及 方法

技术领域

本发明涉及智能控制技术领域，具体涉及的是一种多核异构处理器架构下的高机动平台姿态解算装置及方法。

背景技术

多核处理器是指在统一处理平台中集成两个或多个完整的计算引擎(内核)，此时平台能支持系统总线上的多个处理器，由总线控制器提供所有总线控制信号和命令信号，而ZYNQ作为一种将FPGA核和ARM核通过AXI总线连接的片上处理器架构，在各种场景下都得到了广泛的应用。AHRS指航姿参考系统，包括多个轴向传感器，能够为各种高机动平台提供俯仰、横滚和侧翻信息，这类系统用来为固定平台提供准确可靠的姿态与航行信息，但是对具有快速移动和翻转特点的高机动平台的解算效果却并不理想。航姿参考系统包括基于MEMS的三轴陀螺仪，加速度计和磁强计。另外一种常见的姿态解算算法叫做惯性测量单元IMU，航姿参考系统与IMU的区别在于，航姿参考系统(AHRS)包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元(IMU)仅仅提供传感器数据，并不具有提供准确可靠的姿态数据的功能。本发明所使用的姿态传感器芯片中磁力计由于存在时延，因此传统的数据读取方法也将造成解算不及时、上报有延时的问题。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种多核异构处理器架构下的高机动平台姿态计算装置及方法，基于FPGA核和ARM核的硬件平台实现，具有体积小、功耗低、可移植性强等特点，可直接固定在各类机动平台的中心位置，在平台上直接快速解算姿态信息，通过优化读取方法实现更高效地读取磁力数据，同时通过一种自适应修正的AHRS解算方法得到具有高动态的解算结果，最终传输至上位机以供系统其它部分使用。

本发明所采用的技术解决方法是：一种多核异构处理器架构下的高机动平台姿态解算装置及方法，包括测量数据采集模块、数据传输模块、姿态融合解算模块和输出模块。针对多核异构处理器架构下FPGA核和ARM核可传输密集数据流的特点，对磁力计数据的读取方法进行优化，并通过一种自修正的AHRS解算方法，实现了稳定准确的姿态解算，具有很好的静态稳定性和动态收敛性。本发明的主要创新在于采用一种优化的数据读取方式，并通过在互补滤波中加入P-I控制器来实现对高机动平台姿态的快速解算。

本发明技术解决方案：

本发明的一种多核异构处理器架构下的高机动平台姿态解算装置，包括测量数据采集模块、数据传输模块和姿态融合解算模块；所述姿态融合解算模块通过优化读取方法使得磁力数据读取更加高效；所述多核异构处理器由FPGA核和ARM核构成，所述高机动平台具有快速移动和旋转特性；

所述测量数据采集模块包括姿态传感器芯片：所述姿态传感器芯片由陀螺仪和磁力计构成；陀螺仪和磁力计作为通信从机设备挂载于IIC总线上，以供所述高机动平台读取其测量数据；陀螺仪提供角速度数据和加速度计数据，磁力计提供地磁强度数据；

所述数据传输模块包括：AXI总线；AXI总线完成FPGA核和ARM核间的数据交互，FPGA核将数据采集模块采集到的陀螺仪数据和磁力数据整合为密集数据流后，通过AXI总线发送至ARM核；

所述姿态融合解算模块，由嵌入在ARM端的姿态融合算法构成，所述姿态融合解算模块通过优化读取方法使得磁力数据读取更加高效，在对经过优化读取方法后的陀螺仪数据和磁力数据的基础上，使用一种自适应修正的AHRS方法对陀螺仪数据和磁力数据进融合解算，得到高机动平台的真实姿态。

所述姿态融合解算模块中，优化读取方法实现步骤如下：

(1)采用循环读取的方法读取陀螺仪数据和磁力数据，读取过程中首先读取完整的加速度数据、角速度数据和低8位的X轴磁力数据；

(2)在进入下一次循环后，仍然读取完整的加速度数据和角速度仪数据，同时存储高8位X轴磁力数据；

(3)经步骤(1)、(2)后可拼接得到完整的X轴磁力数据；再次循环执行两次步骤(1)和(2)，执行时改为读取Y轴和Z轴数据，循环结束后即可拼接得到完整的XYZ三轴磁力数据和陀螺仪数据。

所述姿态融合解算模块中，自修正的AHRS方法实现步骤如下：

(1)对陀螺仪和磁力计数据进行单位化处理；

(2)根据余弦矩阵计算加速度数据和磁力数据在平台坐标系下的理论值；

(3)计算加速度数据以及磁力数据理论值和测量值的叉积误差；

(4)将将P-I控制器加入叉积误差对测量值的互补滤波中，通过计算叉积误差的比例值和积分值并乘以相应系数，得到自适应修正偏差量error_value，修正公式如下：

error_value＝K_P·error+K_I·∫error

其中K_P和K_I为P-I控制器参数，用于调节叉积误差比例值和积分值的比例关系，式中error即为叉积误差；

(5)将得到的修正偏差量与测量值相加后转化为四元数形式，并使用一阶龙格库塔方程更新四元数并对四元数进行归一化处理，保留方向信息去除大小信息，得到四元数形式的角度；

(6)将四元数形式的角度换算成欧拉角，得到高机动平台的当前姿态。

本发明的一种多核异构处理器架构下的高机动平台姿态解算方法，实现步骤如下：

(1)采用优化读取方法整合磁力数据，在不同的循环中分别读取磁力数据的低8位和高8位并存储，得到拼接结果，并实时更新拼接结果用于姿态解算，具体优化方法为：循环读取陀螺仪数据和磁力数据，读取过程中读取完整的加速度数据、角速度数据和低8位的磁力数据；在进入下一次循环后，仍然读取完整的加速度数据和角速度仪数据，同时存储高8位磁力数据；在循环执行三次后，即拼接得到完整的优化后的磁力数据和陀螺仪数据；

(2)经过优化后读取方法后将陀螺仪数据和磁力数据实时传入解算算法，经过归一化处理后计算叉积误差，通过加入P-I控制器保证AHRS方法在高机动平台场景下具有高动态特性，实现对AHRS方法的自适应修正，修正公式如下：

error_value＝K_P·error+K_I·∫error

其中K_P和K_I为P-I控制器参数，用于调节叉积误差比例值和积分值的比例关系，式中error即为叉积误差；

(3)将得到的自适应修正数据转化为四元数形式，并使用一阶龙格库塔方程更新四元数并对四元数进行归一化处理，保留方向信息去除大小信息，得到四元数形式的角度；

将四元数形式的角度换算成欧拉角，得到高机动平台的当前姿态。

本发明的目标跟踪系统与现有技术相比的优点有：

(1)本发明采用一种基于AHRS的航姿解算方法，通过计算叉积误差来自适应修测量结果。AHRS的基本思想是通过加速度计和磁力计的叉积误差来修正陀螺仪数据(即角速度数据)，本发明加入P-I控制器来实现对高机动场景下的自适应修正，具有很好的高动态特性，静态稳定性好，动态收敛快。

(2)本发明通过一种优化的数据读取方法，可以更高效的读取磁力数据，为解算实时性提供保障。

(3)本发明在FPGA核上进行密集数据的采集和传输任务，在ARM核上进行数据的解算工作。根据多核异构处理器架构的优势，进行了针对性的优化，采用合理的方式将姿态解算过程中的不同任务分别分配到FPGA核和ARM核上，使得两部分任务在多核架构下快速运行，避免造成不必要的时间开销，从而保证高机动平台对姿态数据的实时性要求，以满足在不同场景下的应用需求。

附图说明

图1为本发明的系统结构框图；

图2为本发明的优化方法读取磁力数据的流程示意图；

图3为去零漂后三轴陀螺仪实时角速度，可见仍存在一定较小范围的误差。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

如图1所示，本发明主要由测量数据采集模块、数据传输模块，姿态融合解算模块和输出模块四部分组成。

测量数据采集模块主要集成在FPGA核中，在完成对姿态传感器芯片配置后进入循环读取工作状态，轮询读取测量数据，并通过片上AXI总线将通过FPGA核整合为密集数据流的九轴数据传输至ARM核中，ARM核中主要完成姿态解算，最终通过输出模块传输至上位机，以满足不同场景的应用需求。

其中自修正的AHRS解算方法流程如下：

(1)输入数据为通过优化方法读取的三轴加速度数据a_x,a_y,a_z(单位为g)和三轴陀螺仪数据ω_x,ω_y,ω_z，即三轴的角速度，单位rad/s。由于传感器的角速度数据为°/s，所以需要在计算前转换单位。

(2)对加计数据进行单位化处理，保留加速度数据的方向信息、去除大小信息；

(3)根据初始化四元数(四元数的一般形式为

初始值q0＝1，q1＝0，q2＝0，q3＝0，这表示在i,j,k三轴没有发生旋转，q0取1是因为四元数只有在模长为1时，表示一个不包含大小信息只包括方向信息的向量的姿态值来估算出理论加速度v_x,v_y,v_z，这与用四元数表示的方向余弦矩阵的第三列元素相对应：

(4)使用叉乘计算理论加速度v和实际测量加速度a之间的误差error＝[e_x,e_y,e_z]^T

(5)对error进行积分运算(K_I为P-I控制器中的积分系数，Δt为采样间隔，积分初值为0)，得到error_int；

error_int＝error_int+K_I·∫error＝error_int+K_I·error·Δt

(6)对error进行比例运算(K_P为P-I控制器中的比例系数)，得到叉积误差：

对error进行比例运算(比例系数为K_P，比例运算初值为0)，得到error_pro；

error_pro＝error_pro+K_P·error

error_value＝error_int+error_pro

error_value叫做P-I控制器，也叫做比例-积分控制器；

(7)将P-I控制器加到输入的原始角速度数据ω＝[ω_x,ω_y,ω_z]^T上，得到经过加速度数据修正的角速度：

ω＝ω+error_value

(8)使用一阶龙格库塔法更新四元数(实际上完成的是对修正后的角速度在四元数形式下的积分，龙格库塔法更新四元数的含义见附录二第五部分)；

(9)为保留四元数的方向信息并忽略且数量信息，对四元数进行归一化处理；

(10)用归一化后的四元数求解三个欧拉角并完成弧度到度的转化(

为俯仰角，θ为横滚角，Ψ为偏航角)；

(11)输出数据为通过上一步骤中解算得到的三个欧拉角

θ和Ψ，分别是俯仰角、横滚角和偏航角。

如图2为经过优化后的磁力数据读取流程示意图。由于磁力数据寄存器的数据读取方式导致磁力数据的读取延时较大，因此需要对其读取方法进行优化，下面对优化后的读取方法进行说明：

由于完整的磁力数据需要读取6个寄存器(X、Y和Z三轴，每轴共包括高8位和低8位两部分)。此方案的思路是将6部分的数据分别在6个循环中依次完成(每次循环中都正常读取加计数据和陀螺仪数据)。循环每执行两次，就可以读取磁力计一个轴完整的高、低8位数据，将数据进行拼接并存储。当循环进行到第6次时，平台存储空间中就包含了完整的三轴磁力数据，可以进行姿态解算。此后的每一次循环仍然读取单个磁力计寄存器，并对磁力数据进行更新。采用优化后的方案姿态解算频率可达100Hz，图3为去零漂后陀螺仪角速度数据静止误差在0.05°内。

传感器芯片由陀螺仪和磁力计芯片构成，为所述装置提供测量数据；测量数据经FPGA核整合为密集数据流后，经AXI总线发送至ARM核进行解算；由于磁力计数据读取存在延时，因此采用一种优化读取方法将九轴数据传入解算模块；所述解算方法基于AHRS，同时加入P-I控制器对测量值进行自适应修正，以实现满足高机动平台的实际场景需求的目的；最终的解算结果由数据传输模块发送至上位机。

基于AHRS的自适应修正解算方法对通过优化方法读取的测量数据进解算和分析对将平台的真实姿态(欧拉角数据)传输至上位机。实现步骤如下：

(1)本多核异构处理器所选用的实验处理平台为PYNQ系列平台架构，它以XC7020FPGA为核心，可利用FPGA核和ARM核进行开发。通过在FPGA端构建硬件电路设计，引出IIC数据通信接口与姿态传感器进行通信；

(2)创建相应工程后按原理搭建硬件电路，并对电路中各个IP核进行定制，添加管脚约束文件，完成管脚约束；

(3)综合、实现并生成比特流，将比特流文件同硬件描述文件导出至软件端，至此完成FPGA端的开发和设计；

(4)在ARM端完成对传感器芯片各个寄存器的配置；

(5)将实现方式设置为轮询工作(包括零漂校正)，并在各个不同应用场景下配置中断管理；

(6)利用AHRS方法对传感器数据进行解算，并加入P-I控制器进行自适应修正，最终将修正结果传输至上位机，以完成上位机所要求的功能。

所述步骤(5)进一步实现如下：

姿态传感器芯片将各个轴的测量数据分成高、低8位分别存储在相邻的两个寄存器中，因此在循环读取数据的同时，需要对数据进行转化和整合。

由于传感器芯片的测量数据在平台完全静止的情况下存在不同程度的漂移，因此采用去零漂的方法来对测量数据进行初步的预处理。其具体实现方法为在将姿态数据传输至上位机前，首先对静止平台情况下的数据进行1000次连续采样，并将这1000次数据存储起来计算其均值。在多次读取的前提下，可以认为此均值对应着各个轴测量数据的零偏。在平台开始工作后，通过计算各个轴新测量的数据和零偏值的差值，就可以完成对测量数据的零偏校正。

在FPGA端读取和传输数据的步骤为：

(1)系统上电后，完成姿态传感器芯片的初始化，对芯片寄存器进行配置，准备读取传感器数据；

(2)读取芯片ID并与寄存器手册中标注的寄存器ID进行比对，一致时进入循环开始读取数据，不一致时表明芯片启动失败；

(3)通过优化方法循环读取测量数据(分别包括加速度计XYZ三轴、陀螺仪XYZ三轴及磁力计XYZ三轴数据共九轴)并完成零偏校正，

(4)将经过初步校正的测量数据进行整合和转化：将各轴数据的高低8位数据进行加和、存储，并根据(1)中配置的量程选择合适的灵敏度因子进行单位换算；

(5)将测量数据通过片上AXI总线传输至ARM端，以供进一步解算。

本发明通过在多核异构处理器架构下通过优化方法读取姿态传感器数据，在对测量数据进行基本预处理的前提下，采用AHRS方法对九轴测量数据进行解算，并通过自适应修正的方法应对高机动场景，具有较好的动态收敛性和静态稳定性，经实验测试其误差范围可以在±0.5°内，满足各种高机动平台的应用需求，且具有很好的可移植性。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (2)

1.一种多核异构处理器架构下的高机动平台姿态解算装置，其特征在于：包括测量数据采集模块、数据传输模块和姿态融合解算模块；所述姿态融合解算模块通过优化读取方法使得磁力数据读取更加高效；所述多核异构处理器由FPGA核和ARM核构成，所述高机动平台具有快速移动和旋转特性；

所述测量数据采集模块包括姿态传感器芯片：所述姿态传感器芯片由陀螺仪和磁力计构成；陀螺仪和磁力计作为通信从设备挂载于IIC总线上，以供所述高机动平台读取测量数据；陀螺仪提供角速度数据和加速度数据，磁力计提供地磁强度数据；

所述数据传输模块包括：AXI总线；AXI总线完成FPGA核和ARM核间的数据交互，FPGA核将数据采集模块采集到的陀螺仪数据和磁力数据整合为密集数据流后，通过AXI总线发送至ARM核；

所述姿态融合解算模块，由嵌入在ARM端的姿态融合算法构成，姿态融合解算模块对经过优化读取后的陀螺仪数据和磁力数据使用一种自适应修正的AHRS方法对陀螺仪数据和磁力数据进行融合解算，得到高机动平台的真实姿态；

所述姿态融合解算模块中，优化读取方法实现步骤如下：

(1)采用循环读取的方法读取陀螺仪数据和磁力数据，读取过程中首先读取完整的加速度数据、角速度数据和低8位的X轴磁力数据；

(2)在进入下一次循环后，仍然读取完整的加速度数据和角速度数据，同时存储高8位X轴磁力数据；

(3)经步骤(1)、(2)后可以拼接得到完整的X轴磁力数据；再次循环执行两次步骤(1)和(2)，执行时改为读取Y轴和Z轴数据，循环结束后即可得到完整的XYZ三轴磁力数据和陀螺仪数据；

所述姿态融合解算模块中，自适应修正的AHRS方法实现步骤如下：

(1)对陀螺仪和磁力数据进行归一化处理；

(2)根据余弦矩阵计算加速度数据和磁力数据在平台坐标系下的理论值；

(3)计算加速度数据以及磁力数据理论值和测量值的叉积误差；

(4)将P-I控制器加入叉积误差对测量值的互补滤波中，通过计算叉积误差的比例值和积分值并乘以相应系数，得到自适应修正偏差量error_value，修正公式如下：

error_value＝K_P·error+K_I·∫error

其中K_P和K_I为P-I控制器参数，用于调节叉积误差比例值和积分值的比例关系，式中error即为叉积误差；

(5)将得到的修正偏差量与测量值相加后转化为四元数形式，并使用一阶龙格库塔方程更新四元数并对四元数进行归一化处理，保留方向信息去除大小信息，得到四元数形式的角度；

(6)将四元数形式的角度换算成欧拉角，得到高机动平台的当前姿态。

2.一种多核异构处理器架构下的高机动平台姿态解算方法，其特征在于：实现步骤如下：

(1)采用优化读取方法整合磁力数据，在不同的循环中分别读取磁力数据X、Y和Z轴的低8位和高8位并存储，得到拼接结果，并实时更新拼接结果用于姿态解算，具体优化方法为：循环读取陀螺仪数据和磁力数据，读取过程中读取完整的加速度数据、角速度数据和低8位的磁力数据；在进入下一次循环后，仍然读取完整的加速度数据和角速度仪数据，同时存储高8位磁力数据；在循环执行三次后，即拼接得到完整的优化后的磁力数据和陀螺仪数据；

(2)经过优化读取方法后将陀螺仪数据和磁力数据实时传入解算算法，经过归一化处理后计算叉积误差，通过加入P-I控制器保证AHRS方法在高机动平台场景下具有高动态特性，实现对AHRS方法的自适应修正，修正公式如下：

error_value＝K_P·error+K_I·∫error

其中error_value为自适应修正偏差量；K_P和K_I为P-I控制器参数，用于调节叉积误差比例值和积分值的比例关系，式中error即为叉积误差；

(3)将得到的修正偏差量与测量值相加后转化为四元数形式，并使用一阶龙格库塔方程更新四元数并对四元数进行归一化处理，保留方向信息去除大小信息，得到四元数形式的角度；

将四元数形式的角度换算成欧拉角，得到高机动平台的当前姿态。

Images