CN108927619B - 对机器人的运动状态控制方法及装置 - Google Patents

Abstract

本发明涉及焊接技术领域，具体涉及一种对机器人的运动状态控制方法及装置，该机器人用于对焊缝进行检测，该机器人上安装有摄像头和两个结构光投射器，两个结构光投射器相对于摄像头呈对称设置。当两个结构光投射器将激光投射至焊缝表面时，控制摄像头采集焊缝图像，进而对该焊缝图像进行分析得到机器人前端距离该焊缝的距离值和角度信息。进一步地，对角度信息进行滤波，获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息，并根据目标角度信息和距离值计算得到设置于机器人的第一电机的第一电机控制量和第二电机的第二电机控制量，使得机器人根据第一电机控制量和第二电机控制量调节运动姿态以对焊缝进行检测。

Description

对机器人的运动状态控制方法及装置

技术领域

本发明涉及焊接技术领域，具体而言，涉及一种对机器人的运动状态控制方法及装置。

背景技术

在无损检测机器人对焊缝检测过程中，有利用磁性贴条与摄像头结合进行焊缝跟踪检测，此种方法的优点在于摄像头获取的前端信息稳定，控制系统采用传统PID即可满足实际工程需求。为了提高焊缝检测机器人的智能化程度，采用结构光投射器与摄像头组合方式识别跟踪焊缝，此种方案前端信息存在诸多不稳定因素，如焊缝表面反光、凹凸不平、生锈等均会影响检测结果，因此需要对控制系统提出更高的要求。

发明内容

本发明的目的在于提供一种对机器人的运动状态控制方法，以实现根据分析得到的机器人相对于焊缝的角度和距离实时调整机器人的转动角度和距离，以使得机器人能稳定跟踪焊缝，实现焊缝检测。

本发明的另一目的在于提供一种对机器人的运动状态控制装置，以实现根据分析得到的机器人相对于焊缝的角度和距离实时调整机器人的转动角度和距离，以使得机器人能稳定跟踪焊缝，实现焊缝检测。

为了实现上述目的，本发明实施例采用的技术方案如下：

第一方面，本发明实施例提供了一种对机器人的运动状态控制方法，所述机器人用于对焊缝进行检测，所述机器人上安装有摄像头以及两个结构光投射器，所述两个结构光投射器相对于所述摄像头呈对称设置，所述方法包括：控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像；对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息；对所述角度信息进行滤波；获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息；根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量；根据所述第一电机控制量第二电机控制量值控制所述机器人对焊缝进行检测。

第二方面，本发明实施例还提供了一种对机器人的运动状态控制装置，所述机器人用于对焊缝进行检测，所述机器人上安装有摄像头以及两个结构光投射器，所述两个结构光投射器相对于所述摄像头呈对称设置，所述装置包括：采集模块，用于控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像；分析模块，用于对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息；滤波模块，用于对所述角度信息进行滤波；获取模块，用于获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息；计算模块，用于根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量；控制模块，用于根据所述第一电机控制量第二电机控制量控制所述机器人对焊缝进行检测。

本发明实施例提供的一种对机器人的运动状态控制方法及装置，该机器人用于对焊缝进行检测，该机器人上安装有摄像头和两个结构光投射器，两个结构光投射器相对于摄像头呈对称设置。当两个结构光投射器将激光投射至焊缝表面时，控制摄像头采集焊缝图像，进而对该焊缝图像进行分析得到机器人前端距离该焊缝的距离值和角度信息。进一步地，对角度信息进行滤波，获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息，并根据目标角度信息和距离值计算得到设置于机器人的第一电机的第一电机控制量和第二电机的第二电机控制量，使得机器人根据第一电机控制量第二电机控制量调节运动姿态以对焊缝进行检测。由此可见，本方案中，通过对分析得到的角度信息进行进一步处理，以调节机器人的运动姿态，使得机器人能更稳定地跟踪焊缝，保证了对焊缝检测的准确性。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本发明实施例提供的一种对机器人的运动状态控制方法的流程示意图。

图2示出了本发明实施例提供的一种应用场景示意图。

图3示出了本发明实施例提供的一种对焊缝图像分析示意图。

图4示出了本发明实施例提供的一种Angle_Kp参数模糊控制规则表。

图5示出了本发明实施例提供的一种Angle_Kd参数模糊控制规则表。

图6示出了本发明实施例提供的一种Dis_Kp参数模糊控制规则表。

图7示出了本发明实施例提供的一种Dis_Kd参数模糊控制规则表。

图8示出了本发明实施例提供的一种对机器人的运动状态控制装置的功能模块示意图。

图示：100-对机器人的运动状态控制装置；110-采集模块；120-分析模块；130-滤波模块；140-获取模块；150-计算模块；160-控制模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

目前可通过结构光投射器和摄像头共同作用对焊缝图像进行检测，该机器人可对焊缝图像进行分析得到距离该焊缝的距离值和角度信息，若直接依据该角度信息调节机器人的运动姿态以对焊缝进行检测，则将导致检测结果不准确，其原因在于，该角度信息未经处理，仍然包含很多不确定因素。因此，本发明实施例提供一种对机器人的运动状态控制方法，其通过对角度信息进行进一步处理，进而该机器人可依据处理后的数据进行控制运动姿态，以对焊缝进行检测，使得检测结果更加准确。

请参照图1，是本发明实施例提供的一种对机器人的运动状态控制方法的流程示意图，该方法包括：

步骤S110，控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像。

具体为，请参照图2，是本发明实施例提供的一种应用场景示意图，在实际使用时，该机器人上设置有摄像头以及两个结构光投射器，该两个结构光投射器相对于摄像头呈对称设置。该摄像头距离该焊缝表面高度45-50mm，其视野范围约为25mm*30mm，该两个结构光投射器的距离焊缝表面的安装高度和入射角度分别为26.5mm和40度(与水平线夹角)，并距离摄像头25mm。

由于该两个结构光投射器分别设置于摄像头的两侧，故在实际应用时，该位于左侧的结构光投射器发出的结构光将会在摄像头右侧平面上平行分布，该位于右侧的结构光投射器发出的结构光将会在摄像头左侧平面上平行分布，进而得到交叉平行结构光。此时，该摄像头将采集该交叉平行结构光的图像得到焊缝图像。一般情况下，一个结构光投射器将会发出5注平行光，因而，该交叉平行结构光为10注平行光的交叉图像。

步骤S120，对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息。

具体为，请参照图3，是本发明实施例提供的一种对焊缝图像分析示意图，即是说，对采集的焊缝图像进行分析得到机器人前端距离焊缝的距离值L和角度信息θ。

步骤S130，对所述角度信息进行滤波。

即是说，该角度信息受诸多因素的影响，其角度信息波动较大，如该焊缝表面存在很多凹凸不平的地方、反光或有生锈的地方等，都会影响角度信息的准确性，故需对角度信息进行滤波。其具体的实现方式为：将角度信息带入预设的角度状态方程，该角度状态方程为：

其中，ω(k)为过程噪声，ν(k)为测量噪声设为均值为0的高斯分布，θ(k)为角度状态方程的输入，Z(k)为角度状态方程的输出。

进而，利用经典的卡尔曼滤波五项公式对已经带入角度信息的预设角度状态方程进行迭代运算，以实现对角度信息进行滤波。经实验证明，若选取650个角度信息进行运算，可以发现角度方差从3.4度降至0.59度，可见通过滤波手段对角度信息进行处理，可以大大提升角度信息的稳定性。

步骤S140，获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息。

即是说，将滤波后的角度信息与动态阈值进行比较，若该滤波后的角度信息小于动态阈值，则表明该角度信息正常，进而选取该角度信息为目标角度信息，若该滤波后的角度信息大于动态阈值，则表明该角度信息异常，进而排除该角度信息。

该动态阈值的确定方式为：Threshold＝K·Angle

其中，Threshold为动态阈值，K为与速度成正比例的一个值，Angle为通过实验数据获得的固定角度阈值。

需要说明的是，当机器人的运动速度加快时，该K的值也会增大，此外，若机器人的运动速度加快，也会同步影响每帧采集时间内机器人与焊缝的角度大小，一般情况下为机器人运动速度越快，每帧采集时间内机器人与焊缝的角度变化范围越大，故需设置动态阈值Threshold与K值成正相关，以体现出机器人运动速度对角度信息的影响。

步骤S150，根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量。

具体为，首先，计算所述目标角度信息与预设角度信息的角度差值。

亦即是说，该机器人内预存储了预设角度信息，进而确定出目标角度信息后，计算该目标角度信息与预设角度信息的角度差值。

其次，计算所述距离值与预设距离值的距离差值。

亦即是说，该机器人内预存储了预设距离值，进而确定出距离值后，计算该距离值与预设距离值的距离差值。

然后，获取第一预设公式和第二预设公式包含的目标参数。

具体为，该第一预设公式用于计算第一电机的第一角速度，该第一预设公式具体为：

LeftOut＝Angle_Kp*AngleErr+Dis_Kp*DisErr+Dis_Kd*(DisErr-Pr eDisErr)+Angle_Kd*(AngleErr-Pr eAngleErr)

该第二预设公式用于计算第二电机的第二角速度，该第二预设公式为：

RightOut＝Dis_Kp*DisErr-Angle_Kp*AngleErr+Dis_Kd*(DisErr-Pr eDisErr)-Angle_Kd*(AngleErr-Pr eAngleErr)

其中，该第一电机和第二电机分别为安装于机器人内的电机，其用于控制机器人的运行姿态，公式中AngleErr为角度差值，DisErr为距离差值，Angle_Kp、Dis_Kp、Dis_Kd和Angle_Kd为第一预设公式和第二预设公式中的未知参数。

进而，对该第一预设公式和第二预设公式中包含的不同未知参数Angle_Kp、Dis_Kp、Dis_Kd和Angle_Kd采用对应的模糊规则表进行模糊化。具体为：请参照图4，是本发明实施例提供的一种Angle_Kp参数模糊控制规则表，进而根据该模糊控制规则表对Angle_Kp进行模糊化；请参照图5，是本发明实施例提供的一种Angle_Kd参数模糊控制规则表，进而根据该模糊控制规则表对Angle_Kd进行模糊化；请参照图6，是本发明实施例提供的一种Dis_Kp参数模糊控制规则表，进而根据该模糊控制规则表对Dis_Kp进行模糊化；请参照图7，是本发明实施例提供的一种Dis_Kd参数模糊控制规则表，进而根据该模糊控制规则表对Dis_Kd进行模糊化。

进一步地，采用加权平均法对模糊化后的未知参数去模糊化得到目标参数。

最后，将目标参数、角度差值以及距离差值带入所述第一预设公式和第二预设公式计算得到第一电机的第一角速度和第二电机的第二角速度。

即使是说，该第一预设公式中LeftOut为第一电机的第一角速度，该第二预设公式中RightOut为第二电机的第二角速度。

进而，根据所述第一角速度和第二角速度计算出所述第一电机控制量和第二电机控制量。

具体为，该第一电机控制量和第二电机控制量的计算公式为：

该LeftAngleOut为第一电机控制量，该RightAngleOut为第二电机控制量，该0.1为采用到的运动控制周期100ms，由公式可知，只需将第一电机的第一角速度LeftOut和第二电机的第二角速度RightOut带入，即可求得第一电机控制量和第二电机控制量。

步骤S160，根据所述第一电机控制量第二电机控制量控制所述机器人对焊接进行检测。

具体为，机器人将根据第一电机控制量第二电机控制量控制机器人发生转动以对焊缝进行追踪，实现对焊缝的精确检测。需要说明的是，在机器人运动的一段时间内，将实时测量机器人与焊缝的角度和距离信息，且对该角度信息将会依次进行滤波处理和动态阈值比较，最后再根据该角度和距离信息得到机器人内安装的两个电机的转动控制量，由此可见，该两个电机的转动角度是实时根据机器人与焊缝的角度和距离信息确定，以使得机器人能够稳定地对焊缝进行追踪，实现焊缝的精确检测。

请参照图8，是本发明实施例提供的一种对机器人的运动状态控制装置100的功能模块示意图，该装置包括采集模块110、分析模块120、滤波模块130、获取模块140、计算模块150以及控制模块160。

采集模块110，用于控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像。

在本发明实施例中，步骤S110可以由采集模块110执行。

分析模块120，用于对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息。

在本发明实施例中，步骤S120可以由分析模块120执行。

滤波模块130，用于对所述角度信息进行滤波。

在本发明实施例中，步骤S130可以由滤波模块130执行。

获取模块140，用于获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息。

在本发明实施例中，步骤S140可以由获取模块140执行。

计算模块150，用于根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量。

在本发明实施例中，步骤S150可以由计算模块150执行。

控制模块160，用于根据所述第一电机控制量第二电机控制量控制所述机器人对焊缝进行检测。

在本发明实施例中，步骤S160可以由控制模块160执行。

由于在对机器人的运动状态控制方法部分已经详细描述，在此不再赘述。

综上所述，本发明实施例提供的一种对机器人的运动状态控制方法及装置，该机器人用于对焊缝进行检测，该机器人上安装有摄像头和两个结构光投射器，两个结构光投射器相对于摄像头呈对称设置。当两个结构光投射器将激光投射至焊缝表面时，控制摄像头采集焊缝图像，进而对该焊缝图像进行分析得到机器人前端距离该焊缝的距离值和角度信息。进一步地，对角度信息进行滤波，获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息，并根据目标角度和距离值计算得到设置于机器人的第一电机的第一电机控制量和第二电机的第二电机控制量，使得机器人根据第一电机控制量第二电机控制量调节运动姿态以对焊缝进行检测。由此可见，本方案中，通过对分析得到的角度信息进行进一步处理，以调节机器人的运动姿态，使得机器人能更稳定地跟踪焊缝，保证了对焊缝检测的准确性。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (6)

1.一种对机器人的运动状态控制方法，所述机器人用于对焊缝进行检测，所述机器人上安装有摄像头以及两个结构光投射器，所述两个结构光投射器相对于所述摄像头呈对称设置，其特征在于，所述方法包括：

控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像；

对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息；

对所述角度信息进行滤波；

获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息；

根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量；

根据所述第一电机控制量和第二电机控制量控制所述机器人对焊缝进行检测；其中，所述根据所述目标角度信息计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量的步骤包括：

计算所述目标角度信息与预设角度信息的角度差值；

计算所述距离值与预设距离值的距离差值；

获取第一预设公式和第二预设公式包含的目标参数；

将所述目标参数、角度差值以及距离差值带入所述第一预设公式和第二预设公式计算得到所述第一电机的第一角速度和第二电机的第二角速度；

根据所述第一角速度和第二角速度计算出所述第一电机控制量和第二电机控制量；其中，

该第一预设公式具体为：

LeftOut＝Angle_Kp*AngleErr+Dis_Kp*DisErr+Dis_Kd*(DisErr-Pr eDisErr)+Angle_Kd*(AngleErr-Pr eAngleErr)

该第二预设公式为：

RightOut＝Dis_Kp*DisErr-Angle_Kp*AngleErr+Dis_Kd*(DisErr-Pr eDisErr)-Angle_Kd*(AngleErr-Pr eAngleErr)

其中，AngleErr为角度差值，DisErr为距离差值，Angle_Kp、Dis_Kp、Dis_Kd和Angle_Kd为第一预设公式和第二预设公式中的未知参数；

其中，所述获取第一预设公式和第二预设公式包含的目标参数的步骤包括：

根据所述第一预设公式和第二预设公式包含的不同未知参数对应的模糊规则表对未知参数进行模糊化；

采用加权平均法对模糊化后的未知参数去模糊化得到目标参数。

2.如权利要求1所述的方法，其特征在于，所述对所述角度信息进行滤波的步骤包括：

将所述角度信息带入预设的角度状态方程；

采用卡尔曼滤波对所述角度状态方程进行迭代运算。

3.如权利要求1所述的方法，其特征在于，所述动态阈值的设置方式为：

Threshold＝K·Angle

其中，Threshold为动态阈值，K为与速度成正比例的一个值，Angle为通过实验数据获得的固定角度阈值；

所述方法还包括：

若滤波后的角度信息大于所述动态阈值，则所述角度信息为异常数据。

4.一种对机器人的运动状态控制装置，所述机器人用于对焊缝进行检测，所述机器人上安装有摄像头以及两个结构光投射器，所述两个结构光投射器相对于所述摄像头呈对称设置，其特征在于，所述装置包括：

采集模块，用于控制所述摄像头在所述两个结构光投射器将激光投射至所述焊缝表面时采集焊缝图像；

分析模块，用于对所述焊缝图像进行分析得到所述机器人前端距离所述焊缝的距离值和角度信息；

滤波模块，用于对所述角度信息进行滤波；

获取模块，用于获取滤波后的角度信息中小于动态阈值的角度信息为目标角度信息；

计算模块，用于根据所述目标角度信息和距离值计算得到设置于所述机器人中的第一电机的第一电机控制量和第二电机的第二电机控制量；

控制模块，用于根据所述第一电机控制量和第二电机控制量控制所述机器人对焊缝进行检测；

其中，所述计算模块具体用于：

计算所述目标角度信息与预设角度信息的角度差值；

计算所述距离值与预设距离值的距离差值；

获取第一预设公式和第二预设公式包含的目标参数；

将所述目标参数、角度差值以及距离差值带入所述第一预设公式和第二预设公式计算得到所述第一电机的第一角速度和第二电机的第二角速度；

根据所述第一角速度和第二角速度计算出所述第一电机控制量和第二电机控制量；

其中，该第一预设公式具体为：

LeftOut＝Angle_Kp*AngleErr+Dis_Kp*DisErr+Dis_Kd*(DisErr-Pr eDisErr)+Angle_Kd*(AngleErr-Pr eAngleErr)

该第二预设公式为：

RightOut＝Dis_Kp*DisErr-Angle_Kp*AngleErr+Dis_Kd*(DisErr-Pr eDisErr)-Angle_Kd*(AngleErr-Pr eAngleErr)

其中，AngleErr为角度差值，DisErr为距离差值，Angle_Kp、Dis_Kp、Dis_Kd和Angle_Kd为第一预设公式和第二预设公式中的未知参数；

其中，所述计算模块具体用于：

根据所述第一预设公式和第二预设公式包含的不同未知参数对应的模糊规则表对未知参数进行模糊化；

采用加权平均法对模糊化后的未知参数去模糊化得到目标参数。

5.如权利要求4所述的装置，其特征在于，所述滤波模块具体用于：

将所述角度信息带入预设的角度状态方程；

采用卡尔曼滤波对所述角度状态方程进行迭代运算。

6.如权利要求4所述的装置，其特征在于，所述动态阈值的设置方式为：

Threshold＝K·Angle

其中，Threshold为动态阈值，K为与速度成正比例的一个值，Angle为通过实验数据获得的固定角度阈值；

所述滤波模块还用于：

若滤波后的角度信息大于所述动态阈值，则所述角度信息为异常数据。

Images