CN114625155B

CN114625155B - 自动驾驶软件三重冗余管理系统及方法

Info

Publication number: CN114625155B
Application number: CN202210518413.3A
Authority: CN
Inventors: 徐世平
Original assignee: Vector Sensing Technology Ningbo Co ltd
Current assignee: Vector Sensing Technology Ningbo Co ltd
Priority date: 2022-05-13
Filing date: 2022-05-13
Publication date: 2022-08-19
Anticipated expiration: 2042-05-13
Also published as: CN114625155A

Abstract

本公开提供了一种自动驾驶软件三重冗余管理系统及方法，其中系统包括处理器及下述子系统：惯性导航系统，卫星定位系统，空速/轮速测量系统，罗盘；处理器被配置为接收来自上述子系统的信号，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被三重冗余管理软件集成、比较和监控，以确认和验证基于传感器得到的航向角和/或位置是否以超过预定阈值的速率变化，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三重冗余传感器信号投票，使正确的航向和位置投入使用。本公开降低了载具控制系统失效风险。

Description

自动驾驶软件三重冗余管理系统及方法

技术领域

本公开涉及自动驾驶导航技术领域，尤其涉及一种自动驾驶软件三重冗余管理系统及方法。

背景技术

航姿参考系统（Attitude and Heading Reference System，以下简称AHRS）/惯性测量单元（Inertial Measurement Unit，以下简称IMU）通常用于航空和自动驾驶的应用中，如可以基于载具姿态和航向提供相应的闭环控制。由于低成本的微机电系统（Micro-Electro-Mechanical System，以下简称MEMS）被用于AHRS中，这种类型的AHRS已经被证明是当前通用航空和自动驾驶汽车等自动驾驶载具的导航领域最感兴趣的设备。

AHRS的MEMS陀螺仪和加速度用于测量俯仰、滚转和偏航率。通过陀螺仪角速率和加速度计，结合磁力计传感器的数据后，利用卡尔曼滤波估计方法可以得到三个轴向姿态。基于MEMS的惯性导航系统（Inertial Navigation System，以下简称INS）已被证明是一种低成本的解决方案，其可以提供非常好的惯性导航数据，同时可与全球定位系统(GlobalPositioning System，GPS)数据配合使用。由于其低成本特性和轻量级解决方案因素，这种类型的传感器已被用于自动驾驶载具（Autonomous Vehicle，AV）开发。

在微型和小型无人机中，传感器和执行器故障情况会在飞行过程中引发灾难性事件。多旋翼、直升机、汽车等自动驾驶载具在大磁场环境下运行时，磁力传感器是无法使用的。过去，对于在该环境下运行的有人驾驶飞机，当航向和自动飞行传感器由于电磁干扰和磁效应失效时，驾驶员可以随时进行控制。而自主导航机器在这种复杂的磁场环境下飞行，如果传感器失效，会导致飞行器坠毁。

为实现载具的自主控制功能，具有自动驾驶模式的载人载具也可以使用手动模式、自动驾驶模式或任务计划器模式来进行操作，但当发生功能性故障时，驾驶员是无法发现和操作载具的。在传感器导致的任何功能故障期间，自动驾驶模式将可能执行下列操作：1.立即通知控制单元；2.切换到冗余传感器；3.切换到降档模式；4.切换到手动模式；5.停止运行等待维护。但这些自动切换模式仍存在极大的隐患，即可靠性较低，很可能仍导致载具的不可控而毁坏。如果自动驾驶载具处于磁效应期间，两个或全部罗盘传感器可能同时出现故障，传感器冗余管理设计仍无助于使自动驾驶载具免于灾难。

而目前并无针对传感器或执行控制器在相关测量参数失效时对载具的保护措施。

发明内容

本公开实施例提供一种自动驾驶软件三重冗余管理系统及方法，以至少解决现有技术中存在的以上技术问题。

本公开实施例提供一种自动驾驶软件三重冗余管理系统，所述系统包括处理器及下述子系统：至少三个惯性导航系统，至少两个卫星定位系统，至少两个空速/轮速测量系统，至少两个罗盘；

所述处理器被配置为接收来自所述述子系统的信号，并且至少部分所述信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被所述三重冗余管理软件集成、比较和监控，以确认和验证基于所述子系统的传感器得到的航向角和/或位置是否以超过预定阈值的速率变化，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，所述三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种传感器信号源投票，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。

可选实施例中，任何航向角超过极限值都将被视为航向跳变或可能存在传感器故障，因为所有自主功能都在慢速或中速模式操作，在正常自主工作过程中，传感器响应系统不会超过极限范围工作，除非传感器失效、传感器跳变或传感器故障，或受到外部环境的影响/引起的传感器响应失败。

可选实施例中，通过与来自惯性导航系统的数据进行比较，从特定的传感器得到的自动驾驶载具的航向角度和/或位置超过预定阈值，表明来自自动驾驶载具的卫星定位系统或空速/轮速测量系统或罗盘的航向角/位置信号故障。

可选实施例中，如果传感器的信号值超过预期值，卫星定位系统或罗盘自动报告自动驾驶载具进入传感器信号受影响/屏蔽区域，以警告自动驾驶载具行驶信息。

可选实施例中，三重冗余管理软件通过三种数据源计算的姿态角和速度将以两对一的投票方式进行表决，以确认正确的姿态角和速度，以及卫星定位系统或罗盘的传感器是否测量错误或故障，其中惯性导航系统的数据源得到的伪姿态角和速度将加入投票过程。

可选实施例中，无论自动驾驶载具采用何种姿态角、航向角或速度，三重冗余管理软件同时使用三种数据源分别计算姿态角和位置，当三重冗余管理软件检测到一个或两个其他传感器系统故障时，三重冗余管理软件将自动切换到下一个正确的信号源，以便能够无缝且及时地继续执行自动驾驶载具任务。

可选实施例中，如果所有卫星定位系统、罗盘以及空速/轮速测量系统都出现故障，来自惯性导航系统的伪航向和伪位置将被使用，来避免由于上述传感器故障而导致的自动驾驶载具撞击坠毁。

可选实施例中，由于任何原因，如果任何故障传感器恢复正常运行，则使用所述三重冗余管理软件的重新结合逻辑算法，以确保来自罗盘航向或GPS的传感器数据包是真正健康，并且与自动驾驶载具的当前信号相匹配，为重新结合提供无缝连接的方法，其中所述重新结合逻辑算法包括下述条件：相关子系统的内置自检测系统必须有效且可用；返回的航向角必须在结合前的一定时间内与当前航向角匹配对齐；航向角增量也必须与INS偏航率匹配，以保证相关航向系统正确响应；在单位时间内，卫星定位系统或者罗盘航向角的变化率不能超过该单位时间内INS的最大偏航率。

可选实施例中，由于任何原因，传感器从故障恢复正常，并达到重新结合逻辑要求，所述三重冗余管理软件通过下述步骤提供平滑启用航向角转换和速度转换，以使自动驾驶载具平滑地将所有传感器重新转换回其最佳初始性能表现状态：避免载具返回新的卫星定位系统的位置切换太快导致载具偏离；与自动驾驶载具防撞功能配合使用，避免在重新结合期间撞到物体；避免所有控制轴同时打开，而是逐步打开，使得载具从伪位置平滑过渡到新的卫星定位系统位置；将各载具的各控制轴布置在安全极限内，使载具平稳返回其轨道或任务路径。

本公开实施例另一方面提供一种自动驾驶软件三重冗余管理方法，所述方法包括：

获取子系统测得的信号，所述子系统包括至少三个惯性导航系统、至少两个卫星定位系统、至少两个空速/轮速测量系统和至少两个罗盘；

至少部分所述信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被所述三重冗余管理软件集成、比较和监控，以确认和验证基于所述子系统的传感器得到的航向角和/或位置是否以超过预定阈值的速率变化，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，所述三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种传感器信号源投票，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。

本公开实施例再一方面提供一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现所述的自动驾驶载具的冗余保护方法的步骤。

本公开实施例再一方面提供一种自动驾驶载具，包括：

至少一个处理器；以及

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本公开所述的方法。

本公开实施例通过为无人机或自动驾驶汽车等自动载具设置多种传感器，通过多种传感器对载具的相关运动参数进行采集，至少部分地基于各传感器的信号形成三个数据源的导航参数，例如航向、位置及速度，以建立三重冗余管理软件，三重冗余管理软件通过不同来源数据可以得到包括来自惯性导航系统的伪航向、速度和位置在内的三种导航数据，三重冗余管理软件可以通过三种导航参数进行投票，自主选择正确的导航参数，避免其中传感器故障或测量参数失效。不同种类的传感器之间的测量参数之间也互为冗余数据，以便在某种传感器处于干扰环境下时，通过其他传感器测量的相关参数进行航向或速度的相关控制等。本公开实施例基于各种传感器测量的相关参数，通过相应的逻辑算法来帮助载具确保传感器即使在恶劣工作环境情况下，也能输出正确的传感器数据。工作环境包括磁场环境、金属接地场景、大型电塔建筑和所有EMI限制条件的环境等，均能向载具应提供正确的数据输出，保证载具的平稳运行，使载具的自动行驶更可靠。

FBW(flight-by-wire，电传飞行) 和DBW(drive-by-wire，电传驾驶)常见的导致载具故障的情况，如1)丢失GPS信号；2)强磁场；3）强EMI影响环境；4)电器镇/厂区。所有这些都可能导致FBW和/或 DBW系统在自动驾驶模式下向控制系统提供错误的传感器数据。这可能导致载具偏离，而驶向建筑物或地面或人或物体。

本公开提供的给自动驾驶载具提供传感器故障保护的软件三重冗余管理系统能够区分传感器（GPS/CMPS）上的这些环境变化，并使用非常可靠的惯性导航系统生成的伪导航参数提供解决方案，以便载具可以继续执行并完成其任务。具体说明如下。

附图说明

图1示出了本公开实施例的自动驾驶软件三重冗余管理系统的原理示意图；

图2示出了本公开实施例的自动驾驶软件三重冗余管理系统的传感器故障保护和规则的示意图；

图3示出了本公开实施例的自动驾驶软件三重冗余管理系统中传感器对应的测量参数和功能以及受影响因素的汇总示意图；

图4示出了本公开实施例的自动驾驶软件三重冗余管理系统的罗盘/GPS故障保护状态流程示意图；

图5示出了本公开实施例的自动驾驶软件三重冗余管理系统的位置故障保护状态流程示意图；

图6示出了本公开实施例的触发航向角切换逻辑算法的示意图；

图7示出了本公开实施例的使用伪航向角的对齐流程示意图；

图8示出了本公开实施例的传感器故障检测的逻辑算法示意图；

图9示出了本公开实施例的伪位置对齐流程示意图；

图10示出了本公开实施例的重新结合的相关设备的逻辑示意图；

图11示出了本公开实施例的无人机的GPS重新结合位置的逻辑示意图。

具体实施方式

为使本公开的目的、特征、优点能够更加的明显和易懂，下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本公开一部分实施例，而非全部实施例。基于本公开中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。

这些年来，用于公用事业/桥梁/太阳能电池板/管道检查的无人机（UnmannedAerial Vehicle，UAV）等自动驾驶载具，例如在空中，无人驾驶的多旋翼机，固定翼机，垂直起降机；在地面上，ADAS用于汽车、卡车、自动导引车等；在海洋中，无人驾驶划船系统、远程海洋航行器（ROV）、水下机器人等，引起了很多关注。结果表明，用于检查的公用事业服务将从人工检查转向自主检查，以节省人力成本，或在危险/偏远地点或危及生命的操作中工作。从人工检查转向自主检查的主要优点包括：提高人身安全、降低成本、使整个公用事业系统易于维护和服务。但是，使用自动驾驶载具进行公用事业检查也存在部分缺点，其中包括：电塔和线路与自动驾驶载具碰撞的风险，自动驾驶载具传感器/执行器故障导致自动驾驶载具失控，如自动驾驶载具遭受大电磁场冲击等。

类似地，如果自动驾驶载具用于桥梁检查，当自动驾驶载具飞到桥下时，卫星定位系统可能会失效。如果自动驾驶载具没有适当的控制算法，可能导致自动驾驶载具坠毁。

本公开实施例中，自动驾驶载具不限于无人飞机或自动导引车（AutomatedGuided Vehicle，AGV），也可以是任何自主运行的机器，如用于自主海港作业、自主采矿工业和各种水下作业的自动驾驶载具等。所有这些自主操作自动驾驶载具都需要对其控制逻辑和传感器进行安全和故障保护设计。然而，这些低成本的自主式机器大多停留在单通道设计上，这意味着一旦一组传感器或控制逻辑失效，自动驾驶载具将遭受更高的毁坏概率。即使采用双冗余管理设计，也应根据安全要求快速确定故障来源。

本公开实施例正是针对上述缺陷而提出的，采用额外的等效测量参数控制等，利用冗余管理设计及可重构设计来实现传感器和执行器故障保护，对自动驾驶载具的控制更安全可靠，航向、速度及位置更准确。

如图1和图2所示，本公开实施例的自动驾驶软件三重冗余管理系统（以下简称管理系统）包括处理器和以下子系统：至少三个惯性导航系统，至少两个卫星定位系统，至少两个空速/轮速测量系统，至少两个罗盘（Compass，CMPS）。惯性导航系统（INS）由IMU或AHRS组成，惯性导航系统能够对载具的空间三维度的角速率、姿态和加速度中的至少之一进行测量。如图2中所示的管理系统中，包括三个惯性导航系统（Inertial or IntegratedNavigation System，INS），为保证测量准确性，需要设置多个INS，形成INS组，以保证测量参数的冗余性。

卫星定位系统用于对载具的位置信息进行检测。本公开实施例中，卫星定位系统具体可以为图2中的GPS芯片处理系统，同样地，GPS芯片处理系统也需要设置多个，以保证GPS测量的准确性，确保多个GPS测量结果的一致性。作为一种实现方式，可以将多个GPS设置于载具的不同部位。当然，卫星定位系统也可以是北斗定位系统或其他任何定位系统。以下实施例中以GPS为例进行说明。

空速/轮速测量系统中，空速测量系统用于对飞行载具所处环境的空气密度、高度和空气速度的至少之一进行测量；轮速测量系统用于对汽车等地面载具的车轮速度进行测量。本公开实施例中，图2中的空速/轮速测量系统，空速测量系统可以是包括皮托/静态系统的空中数据系统（Air Data System，ADS），轮速测量系统可以包括轮速传感器。为了保证测量结果的准确性，可以设置两个或更多空速/轮速测量系统，以保证不同空速/轮速测量系统的测量结果之间互相验证或选用，在测量结果一致性达到设定条件时，确认测量结果可以使用。

罗盘设置于载具上，罗盘用于对载具的方位信息进行测量。参见图2，罗盘同样设置为多个，构成CMPS组，以保证测量结果的可靠性。

AHRS传感器可以基于机械陀螺，光纤陀螺，激光陀螺，MEMS陀螺等，以航向角为例，因为AHRS不受外界因素影响，三个由AHRS组成的INS的故障率可以低于10^-9，满足航空设计要求，成为判断和参考航向角是否正确的基准。当罗盘和GPS都正常时，以GPS航向角为默认值。当罗盘有效，而GPS航向故障或不合理时，以罗盘航向角为准。当GPS和罗盘都失效时，以AHRS组成的INS提供的伪航向角为准。

如图2所示，本公开实施例的管理系统中，处理器被配置为接收来自上述子系统的信号，并且至少部分信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被三重冗余管理软件集成、比较和监控，以确认和验证基于子系统的传感器得到的航向角和/或位置是否以超过预定阈值的速率变化，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种传感器信号源投票，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。

处理器接收的上述子系统的传感器的信号包括各子系统测得的测量参数，例如，GPS数据，CMPS数据等，根据不同子系统的传感器测得的测量参数得到不同数据源的姿态角和位置，包括来源于惯性导航系统的伪姿态角和位置，通过对比，确认正确的姿态角和位置，并能够选择正确的姿态角和位置，当多个姿态角和位置均正确时，也可以按照优先级顺序切换至优先级在前的正确的姿态角和位置进行导航。

本公开实施例中，处理器能够根据接收的信号确定上述各子系统是否故障或测量参数失效；或基于上述各子系统的上报的自身工作状态确定子系统是否故障。本公开实施例中，处理器对上述子系统的测得的信号进行逻辑运算，以确定上述子系统测量的结果是否可靠，子系统测量的结果是否出现了异常，并能进行异常原因的初步分析，基于其他正常的子系统的测量结果对异常子系统的测量结果进行运算及补偿，即使部分类型的子系统因当前的测量环境异常或子系统自身故障不能正常工作时，也能保证该故障子系统的相关测量参数的计算或推算，或以其他子系统的测量结果对其进行替代，保证载具的航向及速度控制，使载具的运行平稳安全。本公开实施例的管理系统能够区分传感器 (GPS/CMPS) 上的这些数据变化，并使用非常可靠的惯性导航系统生成的伪导航参数，例如伪航向、伪位置或伪速度，提供解决方案，以便载具可以继续执行并完成其任务。

在用于方位测量的子系统（例如罗盘）和定位测量的子系统（例如GPS等卫星定位系统）中之一无故障时，选择其中无故障的子系统的导航参数对载具进行航向控制；罗盘和GPS均故障时，利用伪航向角对载具进行航向控制。其中，伪航向角可以根据偏航率的航向角增量确定，航向角增量等于单位时间的航向角的变化量。本公开实施例中，在某子系统出现错误时，例如传感器信号受影响/屏蔽，或传感器发生永久或临时故障等，可以通过伪航向参与的投票方式确定出错的子系统，通过多个子系统中的多数子系统测量得到的一致的信号作为可靠的信号，以提升子系统的信号的准确性和可靠性，及时转换为正确的子系统的信号进行航向及速度控制。

为了避免大磁效应导致GPS和/或罗盘故障，或GPS被拒绝情况导致自动驾驶载具发生事故，自动驾驶载具可以采用以下的设计思路：设置冗余的子系统，改进电子硬件金属设计以阻挡电磁干扰和磁效应。在所有工作区域安装子系统的中继器，在紧急情况下提供更好的软件保护算法。

对于中型、小型、微型和微型自动驾驶载具，载具控制系统设计通常是双通道或单通道。一些中型自动驾驶载具在INS中可能具有三个传感器组(ADAHRS，GPAHRS，AHRS)。但是，对于小型和微型自动驾驶载具上使用的罗盘和GPS等传感器，由于重量轻和成本低的要求，通常使用单通道位置传感器封装。无论使用多少组位置传感器，位置传感器都可能因以下原因发生故障：如进入GPS的屏蔽环境，罗盘由于磁效应而数据跳动或输出错误等。

当自动驾驶载具在自动驾驶模式或任务规划模式下工作时，如果没有对软件切换或其他降级模式控制采取适当措施，则可能会突然丢失位置传感器反馈信息，这将会导致灾难性的后果。对于冗余传感器设计的自动驾驶载具，如果类似的一组传感器发生故障，可以使用冗余管理系统进行故障恢复。然而，对于小型和迷你自动驾驶载具，设计冗余传感器的方式并不适用。如果在其电子控制系统中没有设计适当的通用传感器故障保护控制算法，即使是冗余传感器设计的自动驾驶载具，也可能导致撞毁。

本公开实施例的管理系统能够保护自动驾驶载具不因GPS和罗盘传感器发生故障以及开/关跳动（指信号时有时无）情况而失去稳定性和控制。通过使用软件估计的伪位置与速度，与相关控制算法一起实现GPS位置与伪位置的无缝连接，可以用于小型和微型自动驾驶载具的传感器故障恢复。本公开实施例也可应用于通用航空的飞行控制和导航系统。

如图2所示，对于自动驾驶载具进行三重冗余设计，通过软件分别对空间三轴向(x，y，z)中的角速率、姿态角和加速度进行测量，并通过冗余管理软件确定其测量结果的可靠性；设置双GPS来提供自动驾驶载具位置及速度等导航参数信息。设置双空速/轮速测量系统，空速测量系统用来提供自动驾驶载具的周围环境的空气密度、高度和空速。双罗盘设计可以提供准确的航向角。

本公开实施例中，某些自动驾驶载具使用的子系统可以比图示列出的子系统少。电子反馈控制规则将自动抓取其关联的传感器信息，以使自动驾驶载具正确执行任务。本公开实施例中，采用标准的冗余管理系统，以便冗余管理系统可以自动容忍和修复单个传感器的故障。

图3示出了本公开实施例的软件三重冗余管理系统中传感器对应的测量参数和功能以及受影响因素的汇总示意图。如图3所示，自动驾驶载具的整个状态参数变量，如可以使用INS来测量自动驾驶载具的相关参数，称为(P，Q，R，φ ，θ，ψ，Ax，Ay，Az)，其中，P表示滚动角速率，Q表示俯仰角速率，R表示航向角速率，φ表示侧倾姿态也即倾斜角度，θ表示俯仰角，ψ表示航向角，用于描述自动驾驶载具的运行姿态等；A _x表示纵向（x轴方向）加速度、A _y表示横向(y轴方向)加速度、A _z表示垂直(z轴方向)加速度。其中，航向角可以通过罗盘和GPS测量参数及相关运算得出。如飞行器的空速，空气密度和高度可以从空速测量系统得出，例如由空气数据系统（称为ADS）或机载数据计算机（ADC）的皮托管和静力板（皮托/静态系统）提供。最后，从GPS提供载具的位置。GPS还可以提供载具的相关速度参数（U、V、W），将其转换为载具速度；其中，U=Vx表示纵向前进/后退（X轴向）速度，V=Vy表示横向左/右（Y轴向）速度，W=Vz表示垂直上升/下降（Z轴向）速度。由于GPS更新率限制在10赫兹左右，因此GPS 信号太慢，无法单独用于自动驾驶仪/飞行管理，不应将其用作主要控制反馈设计。对于地面载具控制系统而言，可以用轮速（车轮转速）测量系统代替ADS，从而正确获取车身地面速度。使用GPS速度可以降低反馈控制系统的处理频率，以及，忽略了风对空中和地面载具的影响。其中，航向角（Heading，HDG）的测量可以通过罗盘或GPS测量获得。载具的高度（Altitude）即垂直位置Z-Alt可以通过ADS或GPS测量获得。利用GPS还能测量载具的三维空间中的纵向位置X，以及横向位置Y，这里的X、Y、Z-Alt分别表示三维空间中的坐标值，以此来确定载具的空间位置信息。

本公开实施例中的INS，能够提供相应的逻辑算法来帮助载具确保传感器即使在环境变化的情况下也能输出正确的传感器数据。因此，良好的INS应设计为有无磁、有无铁、有无电塔和有无EMI环境，均能输出正确的传感器数据。这意味着在任何磁场影响、金属接地铁场景、大型电塔建筑和所有EMI限制条件下，INS应保持提供正确的数据输出。

本公开实施例中，在自动驾驶载具载具进入强磁场、重型铁结构或大型电子工厂等时，为避免自动驾驶载具受传感器输出开/关跳动影响的问题，可以使用伪航向角进行导航。自动驾驶载具进入这些领域时，内置自检测系统（Built-In Test，BIT）将保持健康运行。但是，由于自动驾驶载具上的有些子系统在特定环境中不能正常测量，会向自动驾驶载具的控制系统提供错误的传感数据，这可能会因传感器输出错误而导致载具被毁。

如果以GPS为主进行航向控制，在GPS故障情况下，控制系统必须进入降级模式，以避免事故。但是，由于很多无人机公司使用开源软件来设计其无人机的传感、控制和导航功能，控制系统是否能够及时切换到自动驾驶载具期望的保护模式仍然是未知数。为避免上述顾虑，各个INS应具有其智能逻辑算法，以避免因环境变化而导致的故障。本公开实施例提出基于罗盘和GPS故障航向保护设计，其流程如图4所示。

航向传感器故障保护设计的主要思想是将所有航向相关参数综合考虑，形成航向逻辑确定控制算法。这种算法称为航向同步控制逻辑算法。罗盘/GPS航向故障保护系统的结构由与下述航向角相关的传感器组成，如1）GPS航向角，2）罗盘航向角，3）由偏航率通过离散积分器产生的INS航向角，这种航向角称为伪航向角。

如图4所示，在所有传感器均可用且测量参数准确的情况下，自动驾驶载具的控制系统（处理器）可以选择罗盘航向或GPS航向为主控制载具的航向。在这种情况下，航向角的初始同步将基于GPS航向。传感器故障的条件可分为以下几个要素：

GPS、罗盘及INS伪航向均可用；或罗盘航向正常但GPS失效；或GPS正常，但罗盘失灵；或GPS和罗盘航向均出现故障。在前三种情况下，可以切换至正常的传感器的航向角，以确保载具继续执行任务而不会中断任务。然而，如果GPS和罗盘航向均出现故障，可能导致自动驾驶载具陷于被毁的危险境地。在某些情况下，例如GPS失效且罗盘陷入大磁场效应，可以使用伪航向来确保自动驾驶载具的安全。如果不执行上述控制算法，有故障的传感器输出可能会导致载具反应过度并可能导致事故。本公开实施例通过使用伪航向的逻辑算法，能够避免这种事故。当GPS和罗盘输出失败或出现故障时，将使用伪航向，以便载具可以继续其任务。伪航向可以通过非线性鲁棒滤波器消除噪声、自动中心偏移（autonomouscentered offset）和动态存储微调功能（dynamic storage trim function）得到，使传感器的YR（偏航率）在长时间的实时运行中没有任何偏差。

为了在伪航向模式下帮助载具自主控制，生成伪位置，为地面控制站提供载具位置。该伪位置是通过使用组成INS的AHRS传感器重构载具位置生成的，也可以采用以下方法消除长期运行的偏差影响：1.用非线性鲁棒滤波器消除噪声；2.自动中心偏移；3.动态存储微调功能，使传感器的偏航率(yaw rate，YR)免受因温度和材料特性变化而产生偏差。

因此，采用本公开实施例的技术方案后，对于自动驾驶载具，可以不用担心会突然失去航向。自动驾驶载具在GPS信号丢失或者进入磁场后，依然可以执行自动控制任务。然而，为了确保载具能够以其预选的位置航路点继续其任务或返回目的地，伪位置作为载具控制计算机的参考。本公开实施例中，使用载具的伪位置不会考虑外部环境，如侧风、尾风和前风对载具的影响等。因此，对于纯依赖惯导系统的载具只能到达目的地所在的一定区域范围内。GPS完全失效期间，所有执行任务时间都必须使用伪位置方法。一旦GPS消除相应的干扰并重新结合，伪位置模式将自动切换回以GPS为主的导航方式。本公开实施例能够避免载具在当前GPS开/关信号跳动时而带来的危险。与伪航向方法一样，伪位置模式也是针对伪航向模式设计的。一旦检测到GPS失效，将开启伪位置模式。伪位置和伪航向模式的区别是，当GPS失效时，无论采用磁力计还是伪航向方法计算航向信息，都会启用伪位置以确保载具任务可以继续。

图5展示了GPS位置故障保护状态流程图。如图5所示，载具位置也可以由ADS或GPS或伪速度计算得出。通过基于载具的（X，Y，Z）即纵向、横向和垂直向的姿态变换的变换矩阵，结合加速度计积分算出速度和位置。位置精度将取决于加速计输出的精度。使用INS的伪速度和伪位置并不能准确地将自动驾驶载具带回目的地，而是让载具返回到目的地一定距离的区域范围内，以便地面工程师/工程人员能通过载具本身发射信号，帮助确定自动驾驶载具的位置。该功能适用于自动驾驶辅助系统、无人车、无人艇、无人飞机和水下航行器，如遥控无人潜水器（Remote Operated Vehicle，ROV）。其设计可以使用模拟的运动学和动力学载具运动的伪航向，正确计算整车动态位置。

无论采用何种姿态角、航向角或速度，载具的管理系统都会同时使用三种数据源计算正确的姿态角和位置。本公开实施例中，三种来源的数据的计算可以通过软件实现。当GPS信号被阻止和拒绝时，将自动打开软件通过其他两个来源的数据计算得到的姿态角和速度以及相应的位置。同样，如果罗盘失去功能，也将自动打开软件计算的其他两个姿态角和速度确定位置。软件计算的姿态角和速度将以两对一的投票方式进行表决，来源于INS的伪姿态角和速度参与投票，以确保切换功能得到有效的验证。此外，当降级至最后两个传感器软件包时，INS的原始数据加入投票过程。这将确保整个管理系统可以一直使用两对一投票，并保持验证和确认，以确保传感器和测量数据的准确。本公开实施例中，通过三种传感器的数据源设置来确保其测量的参数的准确性，通过两种以上的测量参数来确定其他的某种子系统的测量参数可能失效。通过INS原始数据来确定哪种子系统的测量参数是准确的，实质上还是通过两对一的投票方式确定测量参数的准确性，从而提升了子系统的测量结果的可靠性和有效性。

图6说明了触发航向角切换逻辑算法的详细过程。在航向角状态（HDG_State）逻辑切换算法中，有3个HDG_State的代码，这里称为0、1、2，分别由软件通过三种数据源计算得出。当HDG_State=0时，表示所有航向传感器或至少GPS都有效且合理地输出数据，

这被定义为默认状态。当管理系统开启且GPS接收到有效且正确的航向数据时，管理系统将处于HDG_State=0。

当GPS处于接收模式时，或GPS处于搜索状态，或GPS输出数据将为空（NAN），这将被视为GPS得到的航向角状态不合理或无效。在这种情况下，如果罗盘数据有效且合理，则会显示HDG_State=1。这表明管理系统正在使用罗盘为主的航向角而不是GPS为主的航向角。但是，如果系统在磁场中时，将导致罗盘航向的不合理输出，则罗盘航向将被视为错误输出，且GPS仍处于失效状态，在这种情况下，航向将切换到伪航向模式，此时，HDG_State=2。本公开实施例中，当GPS或罗盘航向通过BIT测试并输出有效数据时，HDG_State将切换到相应模式。开机阶段的整个过程可能需要30秒左右，具体取决于GPS卫星搜索时间。但是，当所有系统均有效且输出合理时，HDG_State将从2至1至0或从1至0进行切换。

伪航向角是通过航向角增量（△ψ）计算出来的，其中△（增量）定义为单位时间的参数变化量。伪航向模式的初始航向角将重置为零，以启用伪航向的对齐算法，直到启用GPS和/或罗盘。GPS航向角、罗盘航向角和伪航向角的初始航向的校准对齐算法如图7所示。

整个航向角的计算采用航向角增量作为主要的方式。由于，在单位时间（△t）计算期间里，航向角增量等于航向角的速度单位，因此可以应用线性系统中的小扰动方法，用来简化整个计算，使用单位时间的航向角度的变化量而不是整个航向角度的改变量进行计算。这将大大减少在逻辑算法计算和角度调整复杂度。此外，该方法还可以避免在短时间内航向角实时响应过程中的瞬间上下跳跃的问题，可以平滑启用航向角转换和速度转换。

在初始上电期间或航向故障切换之前，只要罗盘航向或GPS航向角有效且输出合理，就会启用图7所示的用于航向对齐的航向同步控制逻辑算法。图7所示的航向对齐方法如果未通过，控制系统将不会为载具控制回路提供系统反馈控制所需的信号。对于系统启动初始化，在对齐算法中还设计了一个（失败/通过）计数器，以确保系统安全性，可靠性和有效性设计。

本公开实施例中，包括采用航向角同步控制逻辑算法的整个软件采用三冗余管理技术。航向角同步控制逻辑算法采用航向角增量（△HDG）进行初始化和参数更新。因为所有三个数据源的航向角在通电时同时计算，期间不会有任何时间延迟和数据丢失。

如图7所示，伪航向角的创新计算不是使用总的航向角（ψ）进行自动驾驶仪航向计算，而是使用△ψ（航向角增量）作为单位更新方法。航向同步控制逻辑算法用于确保三个数据源的所有航向角在载具启动期间对齐和同步。伪航向、GPS航向和罗盘航向，以及确保当前航向角（ψ₁），前1步时间的航向角（ψ₀）和上一时刻三者都对其的航向角（ψ_old）都必须在一个预定的很小的角度差值内，才能被视为航向对齐和同步。这里，ψ₀是指前一个数据的航向角，ψ_old是指上一时刻伪航向、GPS航向和罗盘航向三者都对齐的航向角。通过前述的设置，能够保证伪航向、GPS航向和罗盘航向之间的完全同步及对齐。当△ψ超过某个值时，传感器的输出会被投票认为是一个错误输出。这将有助于实现同步、正确的投票算法和传感器故障保护。下面将详细介绍。

对于所有出现故障的航向角，采用图8所示的逻辑算法来确定哪个传感器是可用的。投票系统采用以下偏航角：原始偏航率（未过滤）；过滤的偏航率；由当前航向和先前航向之间的差值除以时间计算得出的偏航率，该偏航率用于罗盘航向角、GPS航向角和伪航向角（伪航向角使用鲁棒滤波器估算出）。

上述三个偏航率将用于确定来自鲁棒滤波器和估算的航向角是否正确，或者确定来自扩展卡尔曼滤波器(EKF)的罗盘航向角是否正确。

除非INS偏航率组件失败，否则基于INS原始偏航率的所有过滤后的偏航率能够很好地工作，过滤后的偏航率包括：1.偏航角增量之间的单位时间差；2.从各种过滤和估计方法中获得的偏航率集等。

由于自动驾驶载具不会局限于自主操作，它可以是有人操作、可选有人操作和无人操作，因此操作杆信息（Dir_Stick）、自动驾驶模式（AP_Button）和任务规划模式（Mission_On）都可以作为三重冗余管理软件的输入信息，用于估计载具是否处于其最佳工作状态。任何GPS或罗盘偏航率超过INS的最大偏航率（HDG_Lim_Hit）都将被视为航向跳变或可能存在传感器故障。因为所有自主功能都在慢速或中速模式操作，在正常自主工作过程中，传感器响应系统不会超过极限范围工作，除非传感器失效、传感器跳变或传感器故障，或受到外部环境的影响/引起的传感器响应失败。因此，如图8右侧的输出，所有的Over_HDG_YR（航向角偏航率过大）、HDG_Lim_Hit（航向角偏航率超过INS最大偏航率）和Dynamic_HDG_on（航向角正常）信号都用来帮助三重冗余管理软件改进其逻辑规划，以了解系统是处于稳定状态，还是动态状态或进入故障状态。如果确认传感器故障，则三重冗余管理系统的同步切换逻辑算法将实现无缝连接。

上述过滤和估计偏航率的预设值和测试值的任何差异都可以认为估计方法有问题，或者来自第三源传感器（如磁力计）的偏航率的相关计算受到环境影响。由于比较了三个以上的偏航率，可以使用二对一的投票逻辑来让航向系统切换到正确的航向角。

如图9所示，自动驾驶载具的x，y，z三轴的速度测量来源于三种子系统：ADS、GPS和INS估算。其中，INS估算的速度也可以称为（x，y，z）三轴的伪速度。这三个速度源将同时在三重冗余管理系统上进行主动计算，为自动驾驶载具提供正确的速度值。在正常运行期间，所有三个速度集都会进行比较并同时更新。对于未安装子系统的载具，例如室内机器人可能不安装速度测量子系统，如ADS、轮速或GPS等，INS估计的速度将成为速度估计的主要来源。同样，如果自动驾驶载具进入隧道时丢失GPS信号，INS估计的速度将反馈给控制计算机作为速度输出，并使自动驾驶载具的控制系统继续运行，为路线图提供相关位置信息。

所有三种子系统得到的速度都使用速度增量作为交换数据更新率。例如，如果当前x轴速度为20KM/H，则在下一时间步中，x轴速度将更改为20.1KM/H，即速度增量(△U=0.1）。所有三个速度源（U0I、U0G、U0A）均为20KM/H（I是指从INS计算得出的，G是指从GPS计算得出的，A是指从空速或轮速计算得出的）。如果ADS或汽车车轮速度由逻辑算法控制，则下一个速度为（U1A=20.1）。三种速度（U_1I、U_1G、U_1A）的差值不应出现跳跃情况，即它们的值应接近20.1km/H。每个时间步上的任何跳跃值，都将使用二对一投票来确定控制子系统采用的速度是否应保持在控制状态。因此，在加速或减速的情况下，所有三个速度同步到相似的值。突然数据丢失，如GPS丢失信号，GPS的速度增量将被检测为失败。同样，如果自动驾驶载具是一辆汽车，当连接到CPU的车轮速度线断开，汽车轮速可以被检测为速度故障，在这种情况下，如果GPS可用，三重冗余管理系统将控制自动驾驶载具的速度切换到GPS速度，否则，将使用伪速度。以速度增量代替全速度作为主要计算方法的方法具有以下优点：伪速度可以使用离散加速度计即时更新，而无需任何额外的参数操作；任何速度增量的跳跃都可以即时被检测到。将滤波后的惯性加速度计数据作为目标参数，作为与其他两种速度增量进行比较的关键元素，设备速度故障状态的检测时间将大大的缩短。基于上一个总速度的切换也将有助于减少下一个速度校正。

自动驾驶载具的位置将使用以下公式计算，该位置作为位置增量（△X、△Y、△Z）：

△X=U_0A△t+△U_0A△t

△Y=V_0A△t+△V_0A△t

△Z=W_0A△t+△W_0A△t

当INS使用伪速度计算载具位置时，公式将变为：

△X=U_0A△t+1/2A_x0△t²

△Y=V_0A△t+1/2A_y0△t²

△Z=W_0A△t+1/2A_z0△t²

△X为X轴方向增量，△Y为Y轴方向增量，△Z为Z轴方向增量，U_0A表示从空速或轮速计算得出的X轴方向速度，V_0A表示从空速或轮速计算得出的Y轴方向速度，W_0A表示从空速或轮速计算得出的Z轴方向速度，A_x0表示INS测得的X轴方向加速度，A_y0表示INS测得的Y轴方向加速度，A_z0表示INS测得的Z轴方向加速度，△t表示时间增量。

GPS和罗盘的航向角信息可以有两种类型的错误。首先，对于GPS信号失效，GPS将处于故障模式，载具全球定位系统的位置和航向角数据都为空（NAN）或99999。在这种情况下，定位软件的内置自检系统(BIT)将显示搜索模式无效，GPS设备收集的卫星总数不足以确定位置和速度。因此，GPS不被视为完全失效。失效的GPS将发送GPS等待恢复模式。等待超过几分钟后如20分钟，如果GPS仍然没有响应，则可以认为GPS失败。一旦确定GPS处于真正失效模式，GPS要么不发送数据，要么给出无效位置，在这种情况下，GPS将被隔离并锁定禁止模式，GPS航向和位置将不被使用。此时，维护和失败报告将被记录。伪位置或伪航向角将处于待机模式，直到其他位置和航向相关设备也出现故障。

同样，对于磁力计罗盘系统，当磁力计罗盘进入强磁场或铁场时，罗盘的输出将不会显示正确的数据。在这种情况下，同步逻辑算法将检测出异常磁罗盘响应并继续监测。磁罗盘将被视为没有故障，但有错误输出。罗盘的BIT将被设为等待恢复模式，以进行临时隔离。

一旦自动驾驶载具离开磁场/铁场，罗盘功能恢复正常。为保证磁罗经返回正常工作模式，设计了重新结合逻辑算法，验证磁罗经参数是否可以重新结合或通过定时器、监控/内置自检测系统和罗经恢复功能的同步逻辑进行匹配。

但是，如果三冗余管理软件检测到罗盘故障，罗盘将被隔离并设为维护模式。相关罗盘的故障报告将记录给地面维护人员。伪航向角是否接手控制将由第三套航向角子系统(例如GPS)是否正常或故障决定。

在图8中，偏航率超过限制表明无论罗盘还是GPS至少一个相关传感器的角度响应不在合理范围内，这意味着关联的航向角要么受限（无输出），要么超出INS陀螺仪传感器的偏航率范围。因此，确定子系统的故障情况，并将切换到正确的航向，以确保载具任务可以继续，称为重新结合。然而，当GPS或罗盘从故障输出恢复正常运行时，冗余管理系统需要另一种逻辑算法来验证反馈的航向角以及相关系统是否健康有效。确定可以重新结合（re-engagement）相关子系统的逻辑算法如图10所示。切换和重新结合逻辑算法必须非常严格，因为它应保护传感系统免受开/关类型的情况的影响。超过四个条件用于判断子系统是否可以重新结合，相关条件如下：

其一是相关子系统的内置自检测系统（BIT）必须有效且可用（参见图中输入部分

）。

其二是返回的航向角必须在结合前的一定时间内与当前航向角匹配对齐（参见图中输入部分

）。

其三是航向角增量也必须与INS偏航率匹配，以保证相关航向系统正确响应（参见图中输入部分

）。

其四是在单位时间内，GPS或者罗盘航向角的变化率不能超过该单位时间内INS的最大偏航率（参见图中输入部分

，HDG_Lim_Hit状态为“Not”）。

在一定时间内满足图10中的上述条件，将确保相关航向角可以被重新结合。重新结合后，仍然采用图9所示的逻辑，以检测和监控相关航向角是否出现故障。如果相关子系统的传感器按照图9的逻辑算法在3个单位的时间增量内出现故障，则相关子系统将永久重置为维护模式，并记录故障报告，用于提示地面人员更换相应的子系统。

本领域技术人员应当理解，更多种重新结合逻辑算法可以确定相关传感设备何时以及是否可以重新结合，图10只是示例之一。

类似情况也适用于GPS拒绝或GPS丢失后的重新结合。GPS必须显示足够的卫星接收号码的可用性，并且航向角、速度和位置与当前的状态接近，以确保重新结合算法良好。重新结合必须包含多个时间步骤，才能视为完成确认和验证。

如图5所示，对于使用INS航向和期望或计算出的载具速度伪位置的GPS故障保护逻辑算法，在GPS故障情况下，系统将提供伪航向和伪位置，使载具可以继续执行其任务。一旦航向返回如图9和图10所示的控制逻辑，GPS航向角将重新结合，作为载具的GPS位置。航向重新结合是基于GPS航向角和当前航向角的差异重新对齐和验证。将当前位置重新定位到新的GPS位置需要几个步骤。

步骤1：避免载具返回新的GPS位置切换太快导致载具偏离。

步骤2：可能需要与自动驾驶载具防撞功能配合使用，避免在重新结合期间撞到物体。

步骤3：避免所有控制轴都打开，而是逐步打开，使得载具从伪位置转换到新的GPS位置。

步骤4：将载具的各控制轴布置在安全极限内，使载具平稳返回其轨道或任务路径。

上述步骤取决于GPS丢失的时间，通常，如果GPS丢失时间不太长，载具速度不太快，伪位置不会离重新结合的新GPS位置太远。但是，将载具从伪位置重新定位到新的重新结合的GPS位置时必须小心谨慎。如图11所示，以无人机为例，位置差(△X,△Y,△Z)可以使用航向角、载具速度和飞机功率来实现所需的GPS位置，其中△定义为伪位置和新的重新结合的GPS位置的差。航向角增量可用于使伪位置(x，y)接近新重新结合的GPS(x，y)。为了避免可能使载具发生快速航向变化，可以设置航向转换限制(HDG_slew_Limit)，转换限制定义为单位时间增量下的单位偏航角增量回转极限，其中，如果操作系统为200赫兹，则增量时间可为1/200赫兹是根据FBW控制律限制或DBW控制限制定义的。通常自动驾驶仪（FBW/DBW）的航向回转极限非常慢。在切换逻辑触发点期间，建议使用每秒1度到5度的速率的航向转换限制切换到新航向，直到两个航向角对齐。使用较大的航向转换限制突然切换偏航角可能会导致载具进入不稳定状态，从而导致灾难性事件。同样，对于伪高度和新结合的GPS高度之间的高度差，可以设置如每秒一英尺爬升或下降速率来限制载具的机动性。对于匹配载具巡航速度的自动驾驶汽车，可以在行驶结束或行驶开始时重新设置此项，以避免三个功能同时触发。然而，对于一些先进的载具传感器系统，例如，如果有空速测量系统，如皮托管和静压板（皮托/静态系统），则可以在GPS拒绝情况下，使用皮托速度和空气密度高度来控制这两个参数始终处于启用状态。对于这种情况，然后可以避免失去高度和速度。

本公开采用软件三重冗余管理设计，对三个数据源处理，由三个来源的数据得到三种航向角。操作系统将接收各种传感器数据，如空速/轮速测量系统和/或GPS、罗盘和惯性导航系统。操作系统将形成三个软件环路，确保建立三重冗余管理系统，以保护整个操作系统。这三个数据集与原始INS数据将形成一个投票系统，以帮助确定哪个系统出现故障或将出现故障。一旦速度和航向传感器发生故障，启用伪航向和伪位置对载具进行控制，以确保只要INS可用，载具不会崩溃和失去其返航位置，这样，载具将能够从危险区域或强磁效应区逃生。

本公开实施例的自动驾驶软件三重冗余保护方法包括以下处理步骤：

获取下述子系统测得的信号：至少三个惯性导航系统，至少两个GPS，至少两个空速/轮速测量系统以及至少两个罗盘；

至少部分信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被三重冗余管理软件集成、比较和监控，以确认和验证基于子系统的传感器得到的航向角和/或位置是否以超过预定阈值的速率变化，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种传感器信号源投票，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。

在前述处理步骤的基础上，本公开实施例的方法中，任何航向角超过极限值都将被视为航向跳变或可能存在传感器故障。因为所有自主功能都在慢速或中速模式操作，在正常自主工作过程中，传感器响应系统不会超过极限范围工作，除非传感器失效、传感器跳变或传感器故障，或受到外部环境的影响/引起的传感器响应失败。

在前述处理步骤的基础上，本公开实施例的方法还包括：通过与来自惯性导航系统的数据进行比较，从特定的传感器得到的自动驾驶载具的航向角度和/或位置超过预定阈值，表明来自自动驾驶载具的卫星定位系统或空速/轮速测量系统或罗盘的航向角/位置信号故障。

在前述处理步骤的基础上，本公开实施例的方法还包括：如果传感器的信号值超过预期值，卫星定位系统或罗盘自动报告自动驾驶载具进入传感器信号受影响/屏蔽区域，以警告自动驾驶载具行驶信息。

在前述处理步骤的基础上，本公开实施例的方法中，三重冗余管理软件通过三种数据源计算的姿态角和速度将以两对一的投票方式进行表决，以确认正确的姿态角和速度，以及卫星定位系统或罗盘的传感器是否测量错误和/或故障，其中惯性导航系统的数据源得到的伪姿态角和速度将加入投票过程。

在前述处理步骤的基础上，本公开实施例的方法还包括：无论自动驾驶载具采用何种姿态角、航向角或速度，三重冗余管理软件同时使用三种数据源分别计算姿态角和位置，当三重冗余管理软件检测到一个或两个其他传感器系统故障时，三重冗余管理软件将自动切换到下一个正确的信号源，以便能够无缝且及时地继续执行自动驾驶载具任务。

在前述处理步骤的基础上，本公开实施例的方法还包括：如果所有卫星定位系统、罗盘以及空速/轮速测量系统都出现故障，来自惯性导航系统的伪航向和伪位置将被使用，来避免由于上述传感器故障而导致的自动驾驶载具撞击坠毁。

在前述处理步骤的基础上，本公开实施例的方法还包括：由于任何原因，如果任何故障传感器恢复正常运行，则使用所述三重冗余管理软件的重新结合逻辑算法，以确保来自罗盘航向或GPS的传感器数据包是真正健康，并且与自动驾驶载具的当前信号相匹配，为重新结合提供无缝连接的方法，其中重新结合逻辑算法包括下述条件：相关子系统的内置自检测系统必须有效且可用；返回的航向角必须在结合前的一定时间内与当前航向角匹配对齐；航向角增量也必须与INS偏航率匹配，以保证相关航向系统正确响应；在单位时间内，卫星定位系统或者罗盘航向角的变化率不能超过该单位时间内INS的最大偏航率。

在前述处理步骤的基础上，本公开实施例的方法还包括：由于任何原因，传感器从故障恢复正常，并达到重新结合逻辑要求，三重冗余管理软件通过下述步骤提供平滑启用航向角转换和速度转换，以使自动驾驶载具平滑地将所有传感器重新转换回其最佳初始性能表现状态：避免载具返回新的GPS位置切换太快导致载具偏离；与自动驾驶载具防撞功能配合使用，避免在重新结合期间撞到物体；避免所有控制轴同时打开，而是逐步打开使得载具从伪位置平滑过渡到新的GPS位置；将各载具的各控制轴布置在安全极限内，使载具平稳返回其轨道或任务路径。

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims

1.一种自动驾驶软件三重冗余管理系统，其特征在于，所述系统包括处理器及下述子系统：至少三个惯性导航系统，至少两个卫星定位系统，至少两个空速/轮速测量系统，至少两个罗盘；

所述处理器被配置为接收来自所述子系统的信号，并且至少部分所述信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被所述三重冗余管理软件集成、比较和监控，通过与来自惯性导航系统的数据进行比较，以确认和验证基于所述子系统的传感器得到的航向角和/或位置在单位时间内变化率是否超过预定阈值，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，所述三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种不同子系统的传感器信号源投票，以多数子系统测量得到的一致的信号作为可靠的信号，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。

2.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，任何航向角超过极限值都将被视为航向跳变或可能存在传感器故障，因为所有自主功能都在慢速或中速模式操作，在正常自主工作过程中，传感器响应系统不会超过极限范围工作，除非传感器失效、传感器跳变或传感器故障，或受到外部环境的影响/引起的传感器响应失败。

3.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，通过与来自惯性导航系统的数据进行比较，从特定的传感器得到的自动驾驶载具的航向角度和/或位置超过预定阈值，表明来自自动驾驶载具的卫星定位系统或空速/轮速测量系统或罗盘的航向角/位置信号故障。

4.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，如果传感器的信号值超过预期值，卫星定位系统或罗盘自动报告自动驾驶载具进入传感器信号受影响区域，以警告自动驾驶载具行驶信息。

5.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，三重冗余管理软件通过三种数据源计算的姿态角和速度将以两对一的投票方式进行表决，以确认正确的姿态角和速度，以及卫星定位系统或罗盘的传感器是否测量错误或故障，其中惯性导航系统的数据源得到的伪姿态角和速度将加入投票过程。

6.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，无论自动驾驶载具采用何种姿态角、航向角或速度，三重冗余管理软件同时使用三种数据源分别计算姿态角和位置，当三重冗余管理软件检测到一个或两个其他传感器系统故障时，三重冗余管理软件将自动切换到下一个正确的信号源，以便能够无缝且及时地继续执行自动驾驶载具任务。

7.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，如果所有卫星定位系统、罗盘以及空速/轮速测量系统都出现故障，来自惯性导航系统的伪航向和伪位置将被使用，来避免由于上述传感器故障而导致的自动驾驶载具撞击坠毁。

8.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，如果故障传感器恢复正常运行，则使用所述三重冗余管理软件的重新结合逻辑算法，以确保来自罗盘航向或卫星定位系统的传感器数据包是真正健康，并且与自动驾驶载具的当前信号相匹配，为重新结合提供无缝连接的方法，其中所述重新结合逻辑算法包括下述条件：相关子系统的内置自检测系统必须有效且可用；返回的航向角必须在结合前的一定时间内与当前航向角匹配对齐；航向角增量也必须与惯性导航系统的偏航率匹配，以保证相关航向系统正确响应；在单位时间内，卫星定位系统或者罗盘航向角的变化率不能超过该单位时间内惯性导航系统的最大偏航率。

9.根据权利要求1所述的自动驾驶软件三重冗余管理系统，其特征在于，由于任何原因，传感器从故障恢复正常，并达到重新结合逻辑要求，所述三重冗余管理软件通过下述步骤提供平滑启用航向角转换和速度转换，以使自动驾驶载具平滑地将所有传感器重新转换回其最佳初始性能表现状态：避免载具返回新的卫星定位系统的位置切换太快导致载具偏离；与自动驾驶载具防撞功能配合使用，避免在重新结合期间撞到物体；避免所有控制轴同时打开，而是逐步打开使得载具从伪位置平滑过渡到新的卫星定位系统位置；将各载具控制轴布置在安全极限内，使载具平稳返回其轨道或任务路径。

10.一种自动驾驶软件三重冗余管理方法，其特征在于，所述方法包括：

至少部分所述信号作为三重冗余管理软件的三种数据源，其中包括来自惯性导航系统在内的所有子系统的传感器的信号都被所述三重冗余管理软件集成、比较和监控，通过与来自惯性导航系统的数据进行比较，以确认和验证基于所述子系统的传感器得到的航向角和/或位置在单位时间内变化率是否超过预定阈值，或当自动驾驶载具接近传感器信号影响区域或传感器信号屏蔽区时，所述三重冗余管理软件以来自惯性导航系统的伪航向、速度和位置作为第三投票源，通过三种不同子系统的传感器信号源投票，以多数子系统测量得到的一致的信号作为可靠的信号，使正确的航向和位置投入使用，来避免自动驾驶载具因为传感器故障撞击坠毁。