CN113432603A - 一种复杂环境下母平台与子平台相对位姿测量方法 - Google Patents

Abstract

本发明涉及一种复杂环境下母平台与子平台相对位姿测量方法，步骤为：1、构建惯性/视觉相对位姿测量系统：在母平台顶部布设视觉测量单元，每组视觉测量单元不少于2台视觉测量相机；建立以母平台为中心为基准的测量坐标系统，为母、子平台配置光学合作标识；每组视觉测量相机工作前，对母平台光学合作标识成像；最后进行“相机‑母平台”相对位置解算，获得相机同母平台相对关系；2、获得子平台相对于母平台的位姿态，并进行位姿态补偿：利用两部视觉测量相机对子平台进行定位，利用配置在子平台首端、尾端光学合作标识，获得子平台中轴线三维姿态；对测量延时进行补偿，将补偿后的运动状态信息发送至子平台，辅助子平台惯导进行校正。本发明实时获得了母平台、子平台相对位姿关系。

Description

一种复杂环境下母平台与子平台相对位姿测量方法

技术领域

本发明属于母/子平台相对位姿测量领域，适用于通视条件下，水面大型母平台同作业子平台进行动态相对定位，支撑母平台(如大型水面勘探平台、作业平台)对子平台(由母平台搭载、控制的作业平台，部署于母平台上或工作于水面)进行定位校准信息传递、任务路径规划等方面，尤其涉及一种复杂环境下母平台与子平台相对位姿测量方法。

背景技术

为进一步拓展海洋空间、利用海洋资源、维护海洋权益，我国海洋工程、海防工程将由近岸区域向远海海洋拓展，工程作业范围不断拓展、精度要求不断提升。在需求牵引下，伴随着科学技术与控制理论的发展，“有人-无人协作”、“无人集群”等形式的海洋母/子平台协同作业，已成为促进生产效率提升的有效措施，母平台携带传感器、通信发挥环境感知、任务规划、任务评估等任务，子平台在母平台指挥下，完成区域化作业任务。

在海洋工程作业中，定位导航信息对高质量完成作业任务至关重要，精确的母/子平台相对位置、姿态信息，可有效支持母平台对子平台实现精确调度，连续、准确的定位校准信息，可辅助子平台惯导校正，以消除随时间积累的误差，支持作业有效进行。

发明内容

本发明的目的在于克服现有技术的不足之处，提供一种基于多手段光学测量方案、相对定位精度高，且母平台可同时获取多个子平台位置信息的复杂环境下母平台与子平台相对位姿测量方法。

本发明的上述目的通过如下技术方案来实现:

一种复杂环境下母平台与子平台相对位姿测量方法，其特征在于，包括如下步骤：

步骤1、构建惯性/视觉相对位姿测量系统；

1.1在母平台顶部布设视觉测量单元，一组视觉测量单元同时对多个子平台进行图像采集，每组视觉测量单元包含不少于2台视觉测量相机；

1.2建立以母平台为中心为基准的测量坐标系统，为母平台、子平台配置光学合作标识；

1.3每组视觉测量相机工作前，需对母平台光学合作标识成像；

1.4进行“相机-母平台”相对位置解算，利用光学合作标识同母平台中心相对关系，获得相机同母平台相对关系；

步骤2、利用步骤1构建的惯性/视觉相对位姿测量系统获得子平台相对于母平台的位姿态，并进行位姿态补偿；

2.1、利用两部视觉测量相机对子平台进行定位，利用配置在子平台首端、尾端光学合作标识，获得子平台中轴线三维姿态；

2.2对测量延时进行补偿，利用光学合作标识前N帧空间位置，获得子平台的连续的运动模型，推断导航信息补偿至子平台时刻，母/子光学合作标识相对空间位置，使母平台、子平台同步获得各平台间相对空间位置；基于母平台导航定位信息计算子平台位姿补偿，将补偿后的运动状态信息发送至子平台，辅助子平台惯导进行校正。

进一步的：步骤1.1中视觉测量相机的布设参数如下：

每组视觉测量单元的两部相机光心距离B不少于最远监控距离d的1/20；被监控子平台尺寸为L₀，并排作业条件下最小监控数量要求为N，要求相机视场角α满足

进一步的：步骤1.2中，建立坐标系统如下：

导航坐标系N-XYZ取当地地理坐标系；母平台坐标系M-XYZ与母平台惯导坐标系MINS-XYZ中心重合，原点位于母平台中心，X、Y轴指向母平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；子平台坐标系S-XYZ与子平台惯导坐标系SINS-XYZ中心重合，原点位子平台中心，X、Y轴指向子平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；建立视觉测量坐标系W-XYZ，原点同每组测量相机的某一(默认为自左计数，第1部相机)测量相机光心重合，坐标轴方向同母平台坐标系保持一致；建立相机坐标系C_i-XYZ和I_i-uv图像坐标系，C_iI和I_iI分别为相机、图像空间坐标系原点。

进一步的：步骤1.4中，按照视觉测量原理，位于母平台的标定专用合作标识P_iI在测量相机C_iI坐标系、母平台坐标系位置(x_i1,y_i1,-f₁)、(X_i,Y_i,Z_i)满足共线条件方程，如式(1)所示；将不少于3组共线条件方程联立，可以计算出母平台坐标系、相机坐标系相对位姿关系^MINST_C、^MINSR_C，如式(2)所示。

其中，其中，^MINST_C、^MINSR_C为母平台坐标系、相机坐标系相对位姿关系；

为测量相机的方位余弦；

b₁＝cosωsinκ，

b₂＝cosκcosω，

b₃＝-sinω，

κ、ω为测量相机的方位余弦角

进一步的：步骤2.1包括如下步骤：

2.1.1由母平台对子平台合作标识进行定位，具体为：对测量相机采集的图像序列进行图像预处理、二值化、特征提取以及目标中心定位处理，提取子平台合作标识在图像中的坐标(x_ij,y_ij)；通过在至少2部相机采集的图像中提取标识点并进行匹配，结合各相对姿态关系，基于共线方程组，计算出子平台合作标识空间位置(X,Y,Z)，如式(3)所示，i为相机编号，j为标识点编号。

其中，其中，(x_ij,y_ij)为像点坐标；

i为第i部相机获取的图像；

j为每幅图像中为各标识点编号；

(X_i,Y_i,Z_i)、(a_ij,b_ij,c_ij)分别为各测量相机安装位置与方向余弦；

f_i——相机焦距；

2.2.2建立子平台合作标识同母平台间相对位姿关系，具体为：设定子平台载体坐标系同子平台惯导坐标系基本重合，相对位姿关系^ST_SINS≈[0，0,0]^T,^SR_SINS≈I_3×3；母平台载体坐标系同母平台惯导坐标系基本重合，相对位姿关系^MINST_\M≈[0，0,0]^T,^MINSR_M≈I_3×3；在投入使用前，通过专用工装，获得子平台合作标识在子平台坐标系下坐标^SP₁、^SP₂、^SP₃；视觉测量系统启动后，获得子平台合作标识在相机坐标系下的坐标为^CP₁、^CP₂、^CP₃，带入(4)式，获得子平台坐标系同相机坐标系间位姿关系^CR_S、^CT_S。

^CP_i＝^CR_S ^SP_i+^CT_S，i＝1,2,3 (4)

其中，^CP_i为子平台合作标识在相机坐标系下的坐标；

^SP_i为子平台合作标识在子平台坐标系下坐标；

^CR_S、^CT_S为子平台坐标系同相机坐标系间位姿关系；

通过标定，母平台坐标系、相机坐标系相对位姿关系^MT_C、^MR_C，建立子平台、母平台相对位姿关系^MT_S、^MR_S，根据传递测量理论，可获知母子平台相对位置、姿态关系，如式(5)

其中，^MT_C、^MR_C为母平台坐标系、相机坐标系相对位姿关系；

^MT_S、^MR_S为子平台、母平台相对位姿关系；

^MP₀、^SP₀为子平台合作标识在母平台、子平台坐标系下的坐标。

进一步的：步骤2.2具体为：考虑信息采集、处理、传输延时、子平台运动等因素，在子惯导校正前采用配准技术，将母平台发送时刻的子平台导航信息推断到子平台信息接收时刻；子平台位姿推断以母平台测量信息为基准进行；通过统计分析，母平台信息采集、处理、传输等时间延时总计为Δt，记母平台测量的第i个子平台在导航坐标系的位置^SP_i、速度^SV_i、姿态矩阵^SR_i，母平台在导航坐标系的姿态矩阵ⁿR_M，速度ⁿV，位置ⁿP，母平台相对导航系的旋转角速度ⁿω，则：

^SP_i＝ⁿR_M ^MT_S+ⁿP (6)

^SV_i＝ⁿω^MT_S+ⁿV (7)

^SR_i＝ⁿR_M ^MR_S (8)

^SP_i为母平台测量的第i个子平台在导航坐标系的位置；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

^SV_i为第i个子平台在导航坐标系的速度；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

ⁿV为母平台在导航坐标系的速度；

ⁿP为母平台在导航坐标系的位置；

ⁿω为母平台相对导航系的旋转角速度；

记在Δt时间段内第i个子平台惯导测量角速度集合为

线加速度集合为

n表示Δt时间段内测量的角速度和线加速度个数，记子平台惯导采用间隔为T，采用多项式对角速度和线加速度函数进行拟合：

其中b_k、c_k满足如下关系：

其中，^Sω_k为子平台惯导测量的角速度；

^Sa_k为子平台惯导测量的加速度；

根据第i个子平台运动计算第i个子平台相对惯性系的旋转角速度为ω_in，推断当前时刻子平台导航信息如下：

将子平台在母平台坐标系下位置、速度信息转换至导航坐标系，则可获得子平台惯导的校准信息，用于支持子平台惯导校正。

其中，

分别为补偿后的子平台在导航坐标系下的姿态矩阵；

分别为补偿后的子平台在导航坐标系下的速度；

分别为补偿后的子平台在导航坐标系下的位置。

本发明具有的优点和积极效果：

1、本发明利用了光学测量手段精确性、实时性、非接触性特点，实现了惯性基准信息在动态环境下由单平台向多平台映射，实时获得母平台、子平台相对位姿关系，支持母平台对多个子平台完成任务规划；考虑观测时延、相对运动等因素，为子平台惯导系统提供所需的绝对空间位置、运动速度、三维姿态等信息，可支持子平台惯导误差校正。

2、本发明将原用于陆地静止环境的相对位姿测量方案应用于水面动/静态协同作业领域，无需无线电定位、声学定位信号，为母/子平台协同定位、子平台惯导校准提供了定位信息，有效地支撑了海洋科考、勘探、开采工程的作业精度、范围、效率提升。

附图说明

图1是本发明视觉照相机布设参考图；

图2是本发明建立的主坐标系关系示意图；

图3是本发明主坐标系转换关系示意图。

具体实施方式

以下结合附图并通过实施例对本发明的结构作进一步说明。需要说明的是本实施例是叙述性的，而不是限定性的。

一种复杂环境下母平台和子平台相对位姿测量方法，包括以下步骤：

步骤1：构建惯性/视觉相对位姿测量系统。首先，在母平台顶部布设视觉测量单元，一组视觉测量单元对应一个子平台，每组视觉测量单元不少于2台视觉测量相机；然后，建立以母平台为中心为基准的测量坐标系统，为母平台、子平台配置光学合作标识，然后，在每组视觉测量相机工作前，需对母平台光学合作标识成像；最后，进行“相机-母平台”相对位置解算，利用光学合作标识同母平台中心相对关系，获得相机同母平台相对关系。

步骤2：利用步骤1构建的惯性/视觉相对位姿测量系统获得子平台相对于母平台的位姿态，并进行位姿态补偿。首先，利用两部视觉测量相机对子平台进行定位，利用其首端、尾端光学合作标识，获得子平台中轴线三维姿态；然后，对测量延时进行补偿，利用光学合作标识前N帧空间位置，获得其运动模型，推断补偿至子平台时刻母/子光学合作标识相对空间位置，使母平台、子平台同步获得各平台间相对空间位置；最后，基于母平台导航定位信息计算子平台位姿补偿，将补偿后的运动状态信息发送至子平台，辅助子平台惯导进行校正。

所述步骤1具体方法如下：

(1)布设视觉测量相机，在母平台顶部布设视觉测量相机，每组视觉测量单元不少于2台，为确保定位精度、视场范围，对测量系统关键参数规定如下：每组视觉测量单元的两相机光心距离B不少于最远监控距离d的1/20；被监控载体尺寸为L₀，并排作业条件下最小监控数量要求为N，要求相机视场角α满足

(2)建立以母平台为中心的测量坐标系统，为母平台、子平台配置光学合作标识，每组视觉测量相机工作前，需对母平台光学合作标识(半径不少于15cm)成像。为进行绝对、相对空间位置解算，建立坐标系统如下：导航坐标系N-XYZ取当地地理坐标系；母平台(载体)坐标系M-XYZ与母平台惯导坐标系MINS-XYZ中心重合，原点位于母平台中心，X、Y轴指向母平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；子平台(载体)坐标系S-XYZ与子平台惯导坐标系SINS-XYZ中心重合，原点位子平台中心，X、Y轴指向子平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；建立视觉测量坐标系W-XYZ，原点同测量相机1光心重合，坐标轴方向同母平台(载体)坐标系保持一致。建立相机坐标系、图像坐标系C_i-XYZ、I_i-uv，C_iI、I_iI分别为相机、图像空间坐标系原点。

(3)按照视觉测量原理，合作标识P_iI在测量相机C_iI坐标系、母平台坐标系位置(x_i1,y_i1,-f₁)、(X_i,Y_i,Z_i)满足共线条件方程，如式(1)所示。将不少于3组共线条件方程联立，可以计算出母平台坐标系、相机坐标系相对位姿关系^MINST_C、^MINSR_C，如式(2)所示。

其中，其中，^MINST_C、^MINSR_C为母平台坐标系、相机坐标系相对位姿关系；

为测量相机的方位余弦；

b₁＝cosωsinκ，

b₂＝cosκcosω，

b₃＝-sinω，

κ、ω为测量相机的方位余弦角

所述步骤2具体方法如下：

(1)通过前方交会方法，可对子平台合作标识进行定位。对测量相机采集的图像序列进行图像预处理、二值化、特征提取以及目标中心定位等处理，提取合作标识在图像中的坐标(x_ij,y_ij)；通过在至少2部相机采集的图像中提取标识点并进行匹配，结合各相对姿态关系，基于共线方程组，计算出合作标识空间位置(X,Y,Z)，如式(3)所示，i为相机编号，j为标识点编号。

其中，其中，(x_ij,y_ij)为像点坐标；

i为第i部相机获取的图像；

j为每幅图像中为各标识点编号；

(X_i,Y_i,Z_i)、(a_ij,b_ij,c_ij)分别为各测量相机安装位置与方向余弦；

f_i——相机焦距；

(2)设定子平台载体坐标系同子平台惯导坐标系基本重合，相对位姿关系^ST_SINS≈[0，0,0]^T,^SR_SINS≈I_3×3，母平台载体坐标系同母平台惯导坐标系基本重合，相对位姿关系^MINST_\M≈[0，0,0]^T,^MINSR_M≈I_3×3，在投入使用前，通过专用工装，获得合作标识在子平台坐标系下坐标^SP₁、^SP₂、^SP₃。视觉测量系统启动后，获得合作标识在相机坐标系下的坐标为^CP₁、^CP₂、^CP₃，带入(4)式，获得子平台坐标系同相机坐标系间位姿关系^CR_S、^CT_S。

^CP_i＝^CR_S ^SP_i+^CT_S，i＝1,2,3 (4)

其中，^CP_i为子平台合作标识在相机坐标系下的坐标；

^SP_i为子平台合作标识在子平台坐标系下坐标；

^CR_S、^CT_S为子平台坐标系同相机坐标系间位姿关系；

通过标定，母平台坐标系、相机坐标系相对位姿关系^MT_C、^MR_C，因此，可建立子平台、母平台相对位姿关系^MT_S、^MR_S，根据传递测量理论，可获知母子平台相对位置、姿态关系，如式(5)所示，由此建立了子平台合作标识同母平台间相对位姿关系，可支持母平台向子平台发送空间位置引导指令，支持其循迹航行。

其中，^MT_C、^MR_C为母平台坐标系、相机坐标系相对位姿关系；

^MT_S、^MR_S为子平台、母平台相对位姿关系；

^MP₀、^SP₀为子平台合作标识在母平台、子平台坐标系下的坐标。

(3)为进一步辅助子平台惯导进行校准，以母平台定位导航信息为基准，通过光学测量手段，获取子平台在母平台坐标系下位置、速度、姿态信息，并传送至子平台进行子惯导校正；考虑信息采集、处理、传输延时、子平台运动等因素，在子惯导校正前采用配准技术，将母平台发送时刻的子平台导航信息推断到子平台信息接收时刻。由于母平台导航信息精度优于子平台，故位姿推断以母平台测量信息为基准进行。

通过统计分析，母平台信息采集、处理、传输等时间延时总计为Δt。记母平台测量的第i个子平台在导航坐标系的位置^SP_i、速度^SV_i、姿态矩阵^SR_i，母平台在导航坐标系的姿态矩阵ⁿR_M，速度ⁿV，位置ⁿP，母平台相对导航系的旋转角速度ⁿω，则：

^SP_i＝ⁿR_M ^MT_S+ⁿP (6)

^SV_i＝ⁿω^MT_S+ⁿV (7)

^SR_i＝ⁿR_M ^MR_S (8)

^SP_i为母平台测量的第i个子平台在导航坐标系的位置；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

^SV_i为第i个子平台在导航坐标系的速度；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

ⁿV为母平台在导航坐标系的速度；

ⁿP为母平台在导航坐标系的位置；

ⁿω为母平台相对导航系的旋转角速度；

记在Δt时间段内第i个子平台惯导测量角速度集合为

线加速度集合为

n表示Δt时间段内测量的角速度和线加速度个数，记子平台惯导采用间隔为T，采用多项式对角速度和线加速度函数进行拟合：

其中b_k、c_k满足如下关系：

其中，^Sω_k为子平台惯导测量的角速度；

^Sa_k为子平台惯导测量的加速度。

根据第i个子平台运动计算第i个子平台相对惯性系的旋转角速度为ω_in，推断当前时刻子平台导航信息如下：

其中，

分别为补偿后的子平台在导航坐标系下的姿态矩阵；

分别为补偿后的子平台在导航坐标系下的速度；

分别为补偿后的子平台在导航坐标系下的位置。

将子平台在母平台坐标系下位置、速度信息转换至导航坐标系，则可获得子平台惯导的校准信息，用于支持子平台惯导校正。

尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神范围内，各种替换、变化和修改都是可以的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (6)

1.一种复杂环境下母平台与子平台相对位姿测量方法，其特征在于，包括如下步骤：

步骤1、构建惯性/视觉相对位姿测量系统；

1.1在母平台顶部布设视觉测量单元，一组视觉测量单元对应一个子平台，每组视觉测量单元不少于2台视觉测量相机；

1.2建立以母平台为中心为基准的测量坐标系统，为母平台、子平台配置光学合作标识；

1.3每组视觉测量相机工作前，需对母平台光学合作标识成像；

1.4进行“相机-母平台”相对位置解算，利用光学合作标识同母平台中心相对关系，获得相机同母平台相对关系；

步骤2、利用步骤1构建的惯性/视觉相对位姿测量系统获得子平台相对于母平台的位姿态，并进行位姿态补偿；

2.1、利用两部视觉测量相机对子平台进行定位，利用配置在子平台首端、尾端光学合作标识，获得子平台中轴线三维姿态；

2.2对测量延时进行补偿，利用光学合作标识前N帧空间位置，获得子平台运动模型，推断补偿至子平台时刻母/子光学合作标识相对空间位置，使母平台、子平台同步获得各平台间相对空间位置；基于母平台导航定位信息计算子平台位姿补偿，将补偿后的运动状态信息发送至子平台，辅助子平台惯导进行校正。

2.根据权利要求1所述的复杂环境下母平台与子平台相对位姿测量方法，其特征在于：步骤1.1中视觉测量相机的布设参数如下：每组视觉测量单元的两相机光心距离B不少于最远监控距离d的1/20；被监控子平台尺寸为L₀，并排作业条件下最小监控数量要求为N，要求相机视场角α满足

3.根据权利要求1所述的复杂环境下母平台与子平台相对位姿测量方法，其特征在于：步骤1.2中，建立坐标系统如下：导航坐标系N-XYZ取当地地理坐标系；母平台坐标系M-XYZ与母平台惯导坐标系MINS-XYZ中心重合，原点位于母平台中心，X、Y轴指向母平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；子平台坐标系S-XYZ与子平台惯导坐标系SINS-XYZ中心重合，原点位子平台中心，X、Y轴指向子平台右舷和艏向，X、Y、Z轴相互垂直且符合右手定则；建立视觉测量坐标系W-XYZ，原点同测量相机1光心重合，坐标轴方向同母平台坐标系保持一致；建立相机坐标系C_i-XYZ和I_i-uv图像坐标系，C_iI和I_iI分别为相机、图像空间坐标系原点。

4.根据权利要求1所述的复杂环境下母平台与子平台相对位姿测量方法，其特征在于：步骤1.4中，按照视觉测量原理，母平台合作标识P_iI在测量相机C_iI坐标系、母平台坐标系位置(x_i1,y_i1,-f₁)、(X_i,Y_i,Z_i)满足共线条件方程，如式(1)所示；将不少于3组共线条件方程联立，可以计算出母平台坐标系、相机坐标系相对位姿关系^MINST_C、^MINSR_C，如式(2)所示。

其中，其中，^MINST_C、^MINSR_C为母平台坐标系、相机坐标系相对位姿关系；

b₁＝cosω sinκ，

b₂＝cosκ cosω，

b₃＝-sinω，

为测量相机的方位余弦；

κ、ω为测量相机的方位余弦角。

5.根据权利要求1所述的复杂环境下母平台与子平台相对位姿测量方法，其特征在于：步骤2.1包括如下步骤：

2.1.1、对子平台合作标识进行定位，具体为：对测量相机采集的图像序列进行图像预处理、二值化、特征提取以及目标中心定位处理，提取子平台合作标识在图像中的坐标(x_ij,y_ij)；通过在至少2部相机采集的图像中提取标识点并进行匹配，结合各相对姿态关系，基于共线方程组，计算出子平台合作标识空间位置(X,Y,Z)，如式(3)所示，i为相机编号，j为标识点编号。

其中，(x_ij,y_ij)为像点坐标，i为第i部相机获取的图像，j为每幅图像中为各标识点编号；(X_i,Y_i,Z_i)、(a_ij,b_ij,c_ij)分别为各测量相机安装位置与方向余弦；f_i——相机焦距；

2.2.2、建立子平台合作标识同母平台间相对位姿关系，具体为：设定子平台载体坐标系同子平台惯导坐标系基本重合，相对位姿关系^ST_SINS≈[0，0,0]^T,^SR_SINS≈I_3×3；母平台载体坐标系同母平台惯导坐标系基本重合，相对位姿关系^MINST_\M≈[0，0,0]^T,^MINSR_M≈I_3×3；在投入使用前，通过专用工装，获得子平台合作标识在子平台坐标系下坐标^SP₁、^SP₂、^SP₃；视觉测量系统启动后，获得子平台合作标识在相机坐标系下的坐标为^CP₁、^CP₂、^CP₃，带入(4)式，获得子平台坐标系同相机坐标系间位姿关系^CR_S、^CT_S。

^CP_i＝^CR_S ^SP_i+^CT_S，i＝1,2,3 (4)

其中，^CP_i为子平台合作标识在相机坐标系下的坐标；

^SP_i为子平台合作标识在子平台坐标系下坐标；

^CR_S、^CT_S为子平台坐标系同相机坐标系间位姿关系；

通过标定，母平台坐标系、相机坐标系相对位姿关系^MT_C、^MR_C，建立子平台、母平台相对位姿关系^MT_S、^MR_S，根据传递测量理论，可获知母子平台相对位置、姿态关系，如式(5)

其中，^MT_C、^MR_C为母平台坐标系、相机坐标系相对位姿关系；

^MT_S、^MR_S为子平台、母平台相对位姿关系；

^MP₀、^SP₀为子平台合作标识在母平台、子平台坐标系下的坐标。

6.根据权利要求1所述的复杂环境下母平台与子平台相对位姿测量方法，其特征在于：步骤2.2具体为：考虑信息采集、处理、传输延时、子平台运动等因素，在子惯导校正前采用配准技术，将母平台发送时刻的子平台导航信息推断到子平台信息接收时刻；以母平台测量信息为基准，进行子平台位置、姿态推算；通过统计分析，母平台信息采集、处理、传输等时间延时总计为Δt，记母平台测量的第i个子平台在导航坐标系的位置^SP_i、速度^SV_i、姿态矩阵^SR_i，母平台在导航坐标系的姿态矩阵ⁿR_M，速度ⁿV，位置ⁿP，母平台相对导航系的旋转角速度ⁿω，则：

^SP_i＝ⁿR_M ^MT_S+ⁿP (6)

^SV_i＝ⁿω^MT_S+ⁿV (7)

^SR_i＝ⁿR_M ^MR_S (8)

其中，^SP_i为母平台测量的第i个子平台在导航坐标系的位置；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

^SV_i为第i个子平台在导航坐标系的速度；

ⁿR_M为母平台在导航坐标系的姿态矩阵；

ⁿV为母平台在导航坐标系的速度；

ⁿP为母平台在导航坐标系的位置；

ⁿω为母平台相对导航系的旋转角速度；

记在Δt时间段内第i个子平台惯导测量角速度集合为

线加速度集合为

n表示Δt时间段内测量的角速度和线加速度个数，记子平台惯导采用间隔为T，采用多项式对角速度和线加速度函数进行拟合：

其中b_k、c_k满足如下关系：

其中，^Sω_k为子平台惯导测量的角速度；

^Sa_k为子平台惯导测量的加速度；

根据第i个子平台运动，计算第i个子平台相对惯性系的旋转角速度为ω_in，推断当前时刻子平台导航信息如下：

将子平台在母平台坐标系下位置、速度信息转换至导航坐标系，则可获得子平台惯导的校准信息，用于支持子平台惯导校正。

其中，

分别为补偿后的子平台在导航坐标系下的姿态矩阵；

^SV_i ⁿ分别为补偿后的子平台在导航坐标系下的速度；

^SP_i ⁿ分别为补偿后的子平台在导航坐标系下的位置。

Images