面向车载立体视觉系统的头部运动姿态检测系统的制作方法

本发明涉及辅助驾驶系统领域，具体地，涉及一种面向车载立体视觉系统的头部运动姿态检测系统。

背景技术：

在现有技术中，惯性传感器检测头部姿态：利用惯性传感器可以检测驾驶员的头部姿态，通过计算得到驾驶员面朝的方向。在驾驶过程中，由于人随车一起运动，人与车之间并未发生相对运动，而此时若使用单一的惯性传感器，由于传感器相对于地球发生了运动，此时检测到的姿态就会发生变化，这与实际需求不符。基于姿态检测的辅助驾驶系统：目前在该方向上并没有考虑到特殊车身，在夜间和可见度条件较差，或者车身封闭或观察窗口较小的情况下，这种情况下驾驶员的观察能力受限，无法进行良好的驾驶操作，因此需要一种能够跟随驾驶员头部方向运动的视觉辅助系统，增强驾驶员正常驾驶视觉和观察外界的能力。

技术实现要素：

本发明提供了一种面向车载立体视觉系统的头部运动姿态检测系统，使用该检测系统后可以准确检测出驾驶员头部相对于驾驶车身的姿态，从而判断出驾驶员头部在车内的运动情况。

进一步的，本系统能够解决在封闭或观察窗口较小的车身中驾驶员的观察能力受限问题，同时能够在夜间(不开车灯、无路灯)能见度条件差的情况下，跟随驾驶员头部方向运动辅助驾驶员驾驶，增强驾驶员或观察员从车内或舱内观察外部的能力。

其中，本发明中的头部姿态检测模组分为两种情况：第一种情况为头部姿态检测模组具体为头部姿态检测模块；第二种情况为头部姿态检测模组包括：头部姿态检测模块和图像捕获模块。

为实现上述发明目的，本申请提供了一种面向车载立体视觉系统的头部运动姿态检测系统。该检测系统包括母级系统与子级系统两级，其中母级系统是主系统，包括母级惯性姿态检测模块以及服务器部分，子级系统是次级系统，物理结构上包含于母级系统，包括子级姿态检测模块。子级系统和母级系统的姿态检测模块分别检测相对地球的绝对姿态，并上传至服务器，服务器对两级系统传入的数据进行分析比较，从而计算出子级系统相对于母级系统的相对姿态。

在车身驾驶的具体问题中,当头部姿态检测模组具体为第一种情况，即为头部姿态检测模块时，子级系统由一个具有惯性传感模块的头戴显示器组成，该显示器能够获取驾驶员头部姿态，同时能够通过显示屏将母级系统中双目摄像机采集到的车身外的景象显示给驾驶员；母级系统由一个惯性传感模块，服务器，一个双目摄像机以及承载双目摄像机的云台系统组成，能够获取车身的姿态及驾驶员需要获取的车身外部场景图像。其中，本申请中的服务器可由上位机或者本地处理模块构成如：fpga器件，mcu器件，亦或者由上位机与本地处理模块如fpga器件，mcu器件协同构成。

工作时，两级系统的惯性传感模块分别获取车身及车身内驾驶员头部的姿态，传至服务器进行分析，服务器比对两组数据，判断此时驾驶员头部与车身的相对姿态，将该姿态值映射为云台系统的旋转角度，于是云台带动双目摄像机转动至某个角度，此时摄像机将拍摄到的画面实时地传回服务器，并在驾驶员的头戴显示器上进行显示，如此驾驶员便看到了与自己头部姿态相同方向的车身外的景象，解决了在夜间(不开车灯、无路灯)能见度条件差的情况下，或者封闭或观察窗口较小的车身中驾驶员及观察员观察外部的问题。

其中，头部姿态检测模块包含：陀螺仪，地磁计，加速度计三种传感器，每种传感器分别采集x轴，y轴，z轴三个方向上的数据，即陀螺仪x轴的数据，y轴的数据，z轴的数据；地磁计x轴的数据，地磁计y轴的数据，地磁计z轴的数据；加速度计x轴的数据，加速度计y轴的数据，加速度计z轴的数据，总共9项数据，利用这9项数据计算出用于计算最终被测物三轴姿态(欧拉角)的“四元数”，在利用“四元数”与欧拉角的转换关系计算出三轴姿态即欧拉角(俯仰角pitch,翻滚角roll,偏航角yaw)。车身姿态检测模块同理。

本发明的两级惯性传感器进行姿态检测系统采用两个惯性传感器，分别采集车身及驾驶员头部的姿态进行比对计算，能够准确的计算出两者的相对姿态，同时通过车身外的随动摄像机和驾驶员头部的显示器能够实时的让驾驶员观察到车身外需要观察的区域，从而达到辅助驾驶的目的。

其中，本申请中两个姿态检测模块上传的分别是：驾驶员头部俯仰角pitch1，驾驶员头部偏航角yaw1，驾驶员头部翻滚角roll1；车身俯仰角pitch2，车身偏航角yaw2，车身翻滚角roll2。分别将两个俯仰角做差，两个偏航角做差，两个翻滚角做差。从而获得驾驶员头部与车身的相对姿态，即是两者间，相对偏航角yaw和相对翻滚角roll。

其中，车身俯仰角为车身相对于地球坐标系的俯仰角；车身偏航角为车身相对于地球坐标系的偏航角；车身翻滚角为车身相对于地球坐标系的翻滚角；驾驶员头部俯仰角，偏航角和翻滚角同理。

其中，检测系统中的头部姿态检测模块与车身姿态检测模块对应为两级两级惯性传感，检测系统可扩展为3级或多级惯性传感。

当头部姿态检测模组具体为第二种情况，头部姿态检测模组包括：头部姿态检测模块和图像捕获模块时，目的是利用视频检测坐标消除累计误差，头部姿态检测在辅助驾驶、无人机、工程机械操作的姿态系统中的核心问题，目前主要采取的技术方案是利用数字磁罗盘进行姿态检测，不过数字磁罗盘易受外界磁干扰，长时间使用会产生较大误差，同时该方法检测精度有限，制约了其应用和发展。第二种情况解决头部姿态检测中误差大，精度小，长时间使用误差累计的问题。

优选的，头部姿态检测模组包括：头部姿态检测模块和图像捕获模块，头部姿态检测模块用于获得驾驶员头部的第一姿态数据，并将获得第一姿态数据传输至服务器；图像捕获模块用于获得驾驶员头部相关图像信息，基于驾驶员头部相关图像信息获得驾驶员头部的第二姿态数据；基于第一姿态数据和第二姿态数据对头部姿态检测模组检测到的驾驶员头部姿态数据进行矫正，获得矫正后的驾驶员头部姿态数据。

优选的，图像捕获模块包括：若干摄像机和一个标识，标识安装在用户头部，摄像机用于采集标识信息，并将采集的标识信息发送至服务器，服务器对标识信息进行处理获得驾驶员头部姿态数据，根据对应摄像机的位置对驾驶员头部姿态数据进行修正，获得驾驶员头部的第一姿态数据。

优选的，图像捕获模块包括：1个摄像机和若干标识，该摄像机正对驾驶员人脸或正对驾驶员后脑，3个标识均安装在用户头部，其中一个标识位于驾驶员头部正前方或正后方，摄像机用于采集标识信息，并将采集的标识信息发送至服务器，服务器对标识信息进行处理获得驾驶员头部姿态数据，根据对应摄像机的位置对驾驶员头部姿态数据进行修正，获得驾驶员头部的第一姿态数据。

优选的，图像捕获模块为一个正对驾驶员人脸或正对驾驶员后脑的摄像机，摄像机用于采集驾驶员头部特征图像，并将采集的驾驶员头部特征图像发送至服务器，服务器对驾驶员头部特征图像进行识别，获得驾驶员头部的第一姿态数据。

优选的，基于第一姿态数据和第二姿态数据对头部姿态检测模组检测到的驾驶员头部姿态数据进行矫正，获得矫正后的驾驶员头部姿态数据，具体包括：获得当前第二姿态数据与第一姿态数据的差值，并存为最新的偏差值；并在之后的数据处理中持续使用该偏差值对头部姿态检测模组检测到的驾驶员头部姿态数据进行矫正，获得矫正后的驾驶员头部姿态数据。

优选的，在对驾驶员头部姿态数据进行矫正前还包括：判断第二姿态数据是否达到设定的校准点。

优选的，当图像捕获模块包括若干摄像机和一个标识时，此时设定的校准点为：服务器对摄像机采集到的图像进行检测，当检测到摄像机拍摄到的标识在图像中的面积符合预设要求时则判断达到设定的校准点，预设要求为：||smaker1-smaker2||<int1时；maker1、maker2…为标识，smaker1,smaker2…为标识在图像中的面积。

优选的，当图像捕获模块包括1个摄像机和若干标识，此时设定的校准点为：服务器对摄像机采集到的图像进行检测，当检测到位于驾驶员头部正前方的特定标识makerf或正后方的特定标识makerb在拍摄图像中所占面积最大时，则判断达到设定的校准点，即(maxsmaker＝＝smakerf)|(maxsmaker＝＝smakerb)时，maxsmaker为特定标识在拍摄图像中所占最大面积。

优选的，当图像捕获模块为一个正对驾驶员人脸或正对驾驶员后脑的摄像机时，此时设定的校准点为：服务器检测到驾驶员头部姿态偏转角正对摄像机时，即(||yaw_pic||<5)&(||pitch_pic||<5)，yaw_pic为驾驶员头部姿态中偏航角，pitch_pic为驾驶员头部俯仰角。

本发明提供的一个或多个技术方案，至少具有如下技术效果或优点：

本发明使用两级惯性传感器进行姿态检测，在驾驶员和车身共同运动的过程中，驾驶员头部和车身本身相对地球坐标系的姿态都在不断地变化，使用该检测系统后可以准确检测出驾驶员头部相对于驾驶车身的姿态，从而判断出驾驶员头部在车内的运动情况，为下一步的应用提供基础。

本发明利用服务器内两个俯仰角与两个偏航角之间的减法运算，计算两级惯性传感器上传的数据，计算出驾驶员头部与车身的相对姿态。

本发明利用云台系统搭载双目摄像机捕获车身外图像，使得摄像机的朝向可以任意控制。

本发明将头部姿态检测的惯性传感器与头戴显示器结合在一起，同时佩戴在头部，在保证准确采集到驾驶员头部姿态数据的同时提高系统的集成度，减轻驾驶员头部设备重量，方便使用。

通过头戴显示器实时的显示车身外摄像机采集到的图像，从而达到辅助驾驶员驾驶的作用。

当头部姿态检测模组具体为第二种情况，本发明采用了两部分姿态检测方法，即惯性姿态检测和图像姿态检测，其中惯性姿态检测具有体积小，环境适应性强，检测范围大等特点，但存在误差大，精度小，长时间使用存在较大误差累计的问题；图像姿态检测具有精度高，不存在累计误差的特点，但存在抗抖动性差，设备安装有一定环境要求，检测角度较小的问题；本发明将两者结合使用，利用图像检测的高精度，不存在累计误差特性去矫正惯性姿态检测使用一段时间后出现的累计误差，从而解决了仅使用惯性传感模块的传统姿态检测系统中存在的误差大，精度小，长时间使用存在较大误差累计的问题；同时，图像姿态检测部分采用的的三种方案又有着不同的优点：方案一采用两个或多个相机，具有较高的稳定性和较大的检测范围；方案二采用一个相机和多个标识，具有较大的检测范围同时能够有效降低系统成本；方案三采用一个相机和特征物姿态识别的方法，不需要在头部安装标识，对环境适应能力强，并且能够有效地降低成本。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定；

图1是本申请中面向车载立体视觉系统的头部运动姿态检测系统组成示意图；

图2是本申请中面向车载立体视觉系统的头部运动姿态检测系统执行流程示意图；

图3为本申请中摄像机对于轿车的一般模型的安装位置示意图；

图4为本申请中摄像机对于工程车的一般模型的安装位置示意图；

图5为本申请中摄像机对于装甲车的一般模型的安装位置示意图；

图6为本申请中驾驶员头部姿态数据矫正流程示意图；

图7为本申请中图像捕获模块为第一种方案时的结构示意图；

图8为本申请中图像捕获模块为第二种方案时的结构示意图；

图9为本申请中图像捕获模块为第三种方案时的结构示意图；

图10为当头部姿态检测模组具体为第二种情况时的系统组成示意图；

图11为当头部姿态检测模组具体为第二种情况时的驾驶员头部姿态数据矫正过程示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在相互不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述范围内的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

请参考图1-图2，本发明的两级惯性姿态传感器包括服务器(服务器可由上位机或者本地处理模块构成如：fpga器件，mcu器件，亦或者由上位机与本地处理模块如fpga器件，mcu器件协同构成)，车身姿态检测模块，头部姿态检测模块，云台，头戴显示器和双目摄像机。服务器接收和发送数据，并进行处理和运算，两级姿态传感器分别检测目标的姿态，并将姿态数据上传至服务器，云台接收服务器发出的数据，完成相应的姿态调整，头戴显示器显示相应画面，为驾驶员提供外部场景图像，双目摄像机采集视频图像；上述各个单元的设置关系如下：头部姿态检测模块安装在头戴显示器上，共同构成子级系统佩戴在驾驶员头部，车身姿态检测模块与云台安装在同一平面，放置在车身前引擎盖或车顶，双目摄像机安装在云台上，服务器放置在车身内部，四者共同构成了母级系统。

如图1所示，首先驾驶员头部的头部姿态检测模块与车身引擎盖上车身姿态检测模块分别检测驾驶员头部和车身的姿态，分别是俯仰角pitch，偏航角yaw以及滚转角roll，在辅助车身驾驶的具体应用背景下，若滚转角影响较小，则可考虑忽略该轴上的姿态。传感器检测到俯仰角及偏航角数据后，利用usb将数据传输到服务器，服务器将两部分数据进行比较，计算出其差值，从而判断出两者的相对姿态，即驾驶员头部相对于车身的左右偏转角度和上下俯仰角度。得到该角度后将数值发送至控制云台随动系统的云台控制模块，单片机将该数值转换成控制云台的信号数据并通过串口发送给云台舵机，利用pwm信号控制舵机转至指定角度，云台为两自由度或者三个自由度(水平，俯仰，翻转)，即拥有三个舵机，分别控制偏转角度，俯仰角度和翻滚角，从而带动安装在云台上的双目摄像机，使安装在车身上的双目摄像机相对车身的姿态与驾驶员头部相对于车身的姿态相同，即朝向同一方向。在这个过程中摄像机持续采集图像，并通过usb回传至服务器，服务器再将采集到的左右眼两幅图像进行适当的裁剪之后输出至头戴显示器内部的左右两个显示屏，从而形成信息闭环，这时驾驶员就在车体遮挡的情况下看到了车体外的图像。

车身姿态检测模块可安装在任意与车身固定连接，可随车身移动的地方，结合本发明中设计的部件，可与云台安装在同一平面，安装在车身前引擎盖或车顶或车内防风玻璃处，若是安装在车顶，需安装在驾驶员头部正上方，保证摄像机与驾驶员双眼在同一垂直线，若是安装在前引擎盖或挡风玻璃内，需安装在驾驶员头部正前方，保证摄像机与驾驶员双眼在同一水平线，最好与驾驶员双目位置接近，与前视平行；也可以同服务器结合在一起，安装或放置在车身内安全稳定的位置，如后备箱，储物箱子或专门设计的安装平台；安装位置如附图3所示。双目摄像机安装在云台上，服务器放置在车身内部。

云台控制电路采用微处理单元(mcu)和单片机，例如stm32f103rbt6，传感器采用九轴姿态传感器，例如mpu-9250，或者采用上述相同功能的器件，本申请不对具体型号进行限定。

当头部姿态检测模组具体为第二种情况时，本发明实施例提供了一种图像与惯性结合的头部姿态检测系统，该系统由图像检测部分、惯性检测部分与服务器(服务器可由上位机或者本地处理模块构成如：fpga器件，mcu器件，亦或者由上位机与本地处理模块如fpga器件，mcu器件协同构成，其中图像检测部分有三种方案：①.由两个或多个呈一定角度摆放的摄像机及安装在头部的一个特定标识组成，该标识可以是任何具有一定特征的图案，如二维码，棋盘格等，通常选用特征明显，便于识别的二维码，棋盘格。

②.由一个正对人脸或正对人后脑的摄像机及安装在头部周围的三个或多个特定标识组成，其中有一块标识位于头部正前方或正后方，上述标识可以是任何具有一定特征的图案，如二维码，棋盘格等，本申请对标识的具体形式不做具体的限制，通常选用特征明显，便于识别的二维码，棋盘格。

③.由一个正对人脸或正对人后脑的摄像机组成。惯性姿态检测部分由一个9轴传感模块组成。该部分数据的解算有两种方案。服务器可由上位机或者本地处理模块构成如：fpga器件，mcu器件，亦或者由上位机与本地处理模块如fpga器件，mcu器件协同构成。

工作时，惯性姿态检测部分方案一：惯性传感模块检测被测头部的9轴数据，发送至服务器，服务器通过现有的姿态解算算法计算出惯性模块的姿态数据；方案二：惯性传感模块检测被测头部的9轴数据，并直接通过现有的姿态解算算法计算出惯性模块的姿态数据，然后发送至服务器。

图像检测部分方案一将两个相机检测到的标识图像发送至服务器，服务器在两张图像中选择效果标识面积较大的作为待处理的图像，通过现有的图像处理算法对待处理图像进行解算，根据所用相机的位置对解算出的姿态数据进行修正，得到头部的姿态数据；

方案二中的相机会将拍摄到的包含三个标识的图像发送至服务器，服务器通过比较各个标识的面积大小选出待处理标识，通过现有的图像处理算法对待处理标识进行解算，根据所选择标识的位置对解算出的姿态数据进行修正，得到头部的姿态数据；

方案三中利用特定的头部特征物姿态识别算法，相机将拍摄到的特征物图像上传至服务器，服务器通过特定的头部特征物姿态识别算法对图像进行处理分析解算出特征物的姿态数据，根据所选用特征物相对于头部的姿态，对上一步解算出的姿态数据进行修正，得到头部的姿态数据；

在获得两部分的数据之后，服务器通过图6算法将两部分数据结合，最终实现使用图像姿态数据矫正惯性姿态数据的作用，从而获得更加准确的头部姿态数据。

其中校准点是指具有良好图像效果，姿态数据解算结果比较准确的特殊位置，在方案一中指：当服务器通过图像检测，检测到两个或多个相机拍摄到的标识(如:maker1,maker2…)在图像中的面积(smaker1,smaker2…)基本相同时，即||smaker1-smaker2||<int1时；在方案二中指：当服务器通过图像检测，检测到位于人头部正前方的特定标识makerf或正后方的特定标识makerb在拍摄图像中所占面积最大时，即“(maxsmaker＝＝smakerf)|(maxsmaker＝＝smakerb)”时；在方案三中指通过头部特征物姿态识别算法检测到头部姿态偏转角正对相机，即头部正对前方时，此时有“(||yaw_pic||<5)&(||pitch_pic||<5)”：。

以上三种方案中设定的校准点均是在头部朝向正前方，即当图像姿态检测出头部姿态中偏航角yaw_pic满足正前方对应的偏航角yaw0,俯仰角pitch_pic满足正前方对应的俯仰角pitch0时，用此时图像姿态检测出的偏航角yaw_pic,俯仰角pitch_pic与翻滚角roll_pic数据更新惯性姿态检测数据偏航角yaw_ahrs,俯仰角pitch_ahrs与翻滚角roll_ahrs。

另外需要注意的是：在为进行矫正时主要使用的是惯性姿态检测模块的数据，但图像姿态检测模块依旧在不停地向服务器发送数据，并不断地做如图6的判断。

当头部姿态检测模组具体为第二种情况时，本发明中的系统包括图像姿态检测，惯性姿态检测，姿态矫正三大部分，如图10。

其中像姿态检测部分包含三种方案，惯性姿态检测包含两种方案，两部分使用哪种方案并不影响其他部分的工作方式与结果。

工作时，【惯性姿态检测部分方案一】被测头部的惯性传感模块检测被测头部的9轴数据姿态，发送至到服务器，服务器通过现有的姿态解算算法计算出惯性模块的姿态数据,分别是偏航角yaw_ahrs,俯仰角pitch_ahrs与翻滚角roll_ahrs

【惯性姿态检测部分方案二】被测头部的惯性传感模块检测被测头部的9轴数据姿态，随后就在模块上直接通过现有的姿态解算算法计算出惯性模块的姿态数据,分别是偏航角yaw_ahrs,俯仰角pitch_ahrs与翻滚角roll_ahrs，之后发送到服务器。

【图像姿态检测部分方案一】两个或多个相机不断地将检测到的图像发送至服务器，服务器对两张或多张图像进行初步的处理找出特定标识maker，比较两张或多张图中特定标识所占图像的面积(smaker1,smaker2…)，选择面积最大的作为待处理图像(maxsmaker)，之后通过现有的图像处理算法解算出特定标识的姿态，同时根据不同的相机安装的位置不同，需要在计算出的结果上加上有相机初始位置带来的偏移(offset_cam1,offset_cam2)，从而得到标识的准确姿态数据，分别是偏航角yaw_pic,俯仰角pitch_pic与翻滚角roll_pic，作为通过图像检测到的头部的姿态数据；

【图像姿态检测部分方案二】相机将拍摄到的包含三个或多个标识(maker1,maker2,maker3…)的图像发送至服务器，服务器通过比较在拍摄图像中检测到的不同标识的面积大小(smaker1,smaker2,smaker3…)，选出面积最大的最为待处理标识(maxsmaker)，通过现有的图像处理算法解算出待处理标识的姿态数据，分别是偏航角yaw_pic,俯仰角pitch_pic与翻滚角roll_pic。根据选取的特定标识(maxsmaker)的不同，需要在计算出的姿态数据基础上加上不同的基础偏移(offset1,offset2,offset3…)，该偏移量由所选取的标识安装在头部的位置决定。得到最终修正过的特定标识的姿态数据作为通过图像检测到的头部的姿态数据；

【图像姿态检测部分方案三】相机将拍摄到的头部图像发送至服务器，服务器首先从图像中选取利于分析的指定特征物，之后利用特定的头部特征物姿态识别算法解算出特征物的姿态数据。根据选取的特征物的不同，需要在计算出的姿态数据基础上加上不同的基础偏移(offset_rela)，该偏移量由所选取的特征物与人头部的相对姿态决定。得到最终修正过的特征物的姿态数据作为通过图像检测到的头部的姿态数据，分别是偏航角yaw_pic,俯仰角pitch_pic与翻滚角roll_pic。

在获得两部分的数据之后，首先进行第一次图像姿态检测数据对惯性姿态检测数据的矫正，此时惯性姿态检测数据将作为头部的姿态数据正常使用，之后服务器开始不断地进行第一步检测。

第一步检测：服务器不断地判断当前时间是否与上一次矫正时间满足一定间隔，即cur_time–last_ad_time>intervaltime？)，若不满足，则继续使用当前惯性姿态检测数据将作为头部的姿态数据；若满足，则开始第二步检测。

第二步检测图像姿态检测数据是否达到设定的校准点。若不满足则继续检测，若满足则开始进行矫正。

【校准点】是指具有良好图像效果，姿态数据解算结果比较准确的特殊位置，在方案一中指：当服务器通过图像检测，检测到两个或多个相机拍摄到的标识(如:maker1,maker2…)在图像中的面积(smaker1,smaker2…)基本相同时，即||smaker1-smaker2||<int1时；在方案二中指：当服务器通过图像检测，检测到位于人头部正前方的特定标识makerf或正后方的特定标识makerb在拍摄图像中所占面积最大时，即“(maxsmaker＝＝smakerf)|(maxsmaker＝＝smakerb)”时；在方案三中指通过头部特征物姿态识别算法检测到头部姿态偏转角正对相机，即头部正对前方时，此时有“(||yaw_pic||<5)&(||pitch_pic||<5)”。

【矫正过程】为：检测当前的图像姿态检测数据与当前的惯性姿态检测数据间的差值(yaw_gap,pitch_gap与roll_gap；yaw_gap＝yaw_pic-yaw_ahrs,pitch_gap＝pitch_pic-pitch_ahrs与roll_gap＝roll_pic-roll_ahrs)存为最新的偏差值，并在之后的数据处理中持续使用该偏差值对惯性姿态检测数据进行修正,即yaw＝yaw_ahrs+yaw_gap,pitch＝pitch_ahrs+pitch_gap,roll＝roll_ahrs+roll_gap，将修正后的惯性姿态检测数据作为最终头部的姿态数据(yaw，pitch，roll)，并一直持续到下一次矫正，更新偏差值，再重复上述过程，过程如图11所示。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。