CN108592902B - 一种基于多传感器的定位设备及定位方法、***和机械臂 - Google Patents
一种基于多传感器的定位设备及定位方法、***和机械臂 Download PDFInfo
- Publication number
- CN108592902B CN108592902B CN201810433771.8A CN201810433771A CN108592902B CN 108592902 B CN108592902 B CN 108592902B CN 201810433771 A CN201810433771 A CN 201810433771A CN 108592902 B CN108592902 B CN 108592902B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- displacement
- data
- rotation angle
- triaxial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Manipulator (AREA)
Abstract
本发明实施例提供了一种基于多传感器的定位设备及其定位方法、***和机械臂,该定位设备应用于机械臂,包括多个传感器和处理器。多个传感器用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到处理器;处理器用于根据三轴加速度值计算第一位移,根据三轴角速度值计算第一旋转角度,根据三轴磁场强度值和三轴加速度值计算第二旋转角度,根据超声数据计算第二位移,并将第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,并利用任务轨迹对机械臂进行训练,省去了重新编程的麻烦。
Description
技术领域
本发明涉及定位技术领域,特别是涉及一种基于多传感器的定位设备及定位方法、***和机械臂。
背景技术
在机器人、人体姿态测量、航空航天、娱乐以及消费电子领域,定位设备有着非常广泛的应用。例如,机器人在移动是需要定位导航***;航空航天领域,需要有定位导航***为飞机或火箭提供精确的经纬度和高度信息;在娱乐和消费电子领域,需要定位设备增强游戏的可玩性以及为移动终端提供定位服务。
特别的,在工业用机械臂的使用过程中,需要通过编写程序对机械臂的运动进行描述,当需要更换机械臂时都需要重新编程,这个过程极为繁琐。而如果能在机械臂的执行任务时对其进行定位,根据定位信息取得其任务轨迹,就能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,就能够省去每次更换机械臂时重新编程的麻烦。
发明内容
有鉴于此,本发明提供了一种基于多传感器的定位设备及定位方法、***和机械臂,以解决目前机械臂因无法定位而导致每次更换机械臂都需重新编程的问题。
为了解决上述问题,本发明公开了一种基于多传感器的定位设备,应用于机械臂,所述定位设备包括多个传感器和处理器,所述处理器分别与每个所述传感器相连接,其中:
所述多个传感器用于检测所述机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到所述处理器;
所述处理器用于根据所述三轴加速度值计算第一位移,根据所述三轴角速度值计算第一旋转角度,根据所述三轴磁场强度值和所述三轴加速度值计算第二旋转角度,根据所述超声数据计算第二位移,并将所述第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到所述机械臂的位置数据和方向数据。
可选的,所述多个传感器包括三轴加速度传感器、三轴陀螺仪、三轴地磁传感器和单发三收超声波传感器,其中:
所述三轴加速度传感器用于检测所述三轴加速度值;
所述三轴陀螺仪用于检测所述三轴角速度值;
所述三轴地磁传感器用于检测所述三轴磁场强度值;
所述单发三收超声波传感器用于检测所述超声数据。
可选的,所述处理器还用于将所述位置数据和所述方向数据输出到上位计算机,以使所述上位计算机根据所述位置数据和所述方向数据精确定位所述机械臂的工作轨迹。
另外,还提供了一种定位方法,应用于如上所述的定位设备,其特征在于,所述定位方法包括步骤:
获取多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
将所述三轴加速度值对时间做二重积分计算,得到第一位移;
将所述三轴角速度值对时间做积分计算,得到第一旋转角度;
将所述三轴磁场强度值和所述三轴加速度值做坐标变换计算,得到第二旋转角度;
根据所述超声数据的数据变化进行计算,得到第二位移;
将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。
可选的,所述将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,包括:
利用所述第二位移对所述第一位移进行位置校正计算,得到所述位置数据;
利用所述第二旋转角度对所述第一旋转角度进行方向校正计算,得到所述方向数据。
可选的,所述卡尔曼滤波算法为扩展卡尔曼滤波算法或者无损卡尔曼滤波算法。
还提供了一种定位***,应用于如上所述的定位设备,其特征在于,所述定位***包括:
数据获取模块,用于获取多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
第一计算模块,用于将所述三轴加速度值对时间做二重积分计算,得到第一位移;
第二计算模块,用于将所述三轴角速度值对时间做积分计算,得到第一旋转角度;
第三计算模块,用于将所述三轴磁场强度值和所述三轴加速度值做坐标变换计算,得到第二旋转角度;
第四计算模块,用于根据所述超声数据的数据变化进行计算,得到第二位移;
融合计算模块,用于将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。
可选的,所述融合计算模块包括:
第一校正单元,用于利用所述第二位移对所述第一位移进行位置校正计算,得到所述位置数据;
第二校正单元,用于利用所述第二旋转角度对所述第一旋转角度进行方向校正计算,得到所述方向数据。
可选的,所述卡尔曼滤波算法为扩展卡尔曼滤波算法或者无损卡尔曼滤波算法。
还提供了一种机械臂,设置有如上所述的定位设备。
还提供了一种机械臂,包括控制器,所述控制器包括如上所述的定位***。
还提供了一种机械臂,包括多个传感器和控制器,所述控制器包括至少一个处理器和存储器,所述存储器用于存储计算机程序或指令,所述处理器用于执行所述计算机程序或指令,以使所述控制器能够对所述机械臂执行如下操作:
获取所述多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
将所述三轴加速度值对时间做二重积分计算,得到第一位移;
将所述三轴角速度值对时间做积分计算,得到第一旋转角度;
将所述三轴磁场强度值和所述三轴加速度值做坐标变换计算,得到第二旋转角度;
根据所述超声数据的数据变化进行计算,得到第二位移;
将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。
可选的,所述处理器还用于将所述位置数据和所述方向数据输出到上位计算机,以使所述上位计算机根据所述位置数据和所述方向数据精确定位所述机械臂的工作轨迹。
从上述技术方案可以看出,本发明提供了一种基于多传感器的定位设备及其定位方法、***和机械臂,该定位设备应用于机械臂,包括多个传感器和处理器,处理器分别与每个传感器相连接。多个传感器用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到处理器;处理器用于根据三轴加速度值计算第一位移,根据三轴角速度值计算第一旋转角度,根据三轴磁场强度值和三轴加速度值计算第二旋转角度,根据超声数据计算第二位移,并将第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种基于多传感器的定位设备的结构框图;
图2为本发明实施例提供的一种超声波测距原理图;
图3为本发明实施例提供的一种基于多传感器的定位方法的步骤流程图;
图4为本发明实施例提供的一种基于多传感器的定位***的结构框图;
图5为本发明实施例提供的一种机械臂的控制器的结构框图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例一
图1为本发明实施例提供的一种基于多传感器的定位设备的结构框图。
参照图1所示,本实施例提供的定位设备应用于机械臂,其具体包括设置在机械臂上相应位置的多个传感器10和分别与每个传感器相连接的处理器20。
这里多个传感器分别为三轴加速度传感器11、三轴陀螺仪12、三轴磁场强度传感器13和单发三收超声波传感器14。上述三轴加速度传感器用于检测机械臂的三轴加速度值,三轴陀螺仪用于检测机械臂的三轴角速度值。
三轴磁场强度传感器主要是用于由于方向不同而导致的磁强计三个轴的输出会发生变化,并且磁强计具有指北功能,可以根据地磁传感器的输出分辨出地磁的北极,并且这个过程的误差很小,而且在室内环境中,可以认为地磁场是不会由于位置变化而改变的,因为这个变化量是非常小的,可以忽略,因此磁强计可以把地磁场作为参考量,因为地磁场具有固定的方向和大小,根据旋转后,磁强计三个轴的输出的变化可以计算出旋转的角度,因此我们可以借助于磁强计对陀螺仪进行方向校正。
单发三收超声波传感器包括一个发射端和三个接收端,具体的测量原理如图2所示,超声波的发射端安装在C点,超声波的三个接收端分别安装在O,A,B点,OA与OB相互垂直,当发射端C在时刻t1发出超声波时,三个接收端能在时刻t2、t3、t4接收到超声信号,那么发射端与接收端之间的距离AC,OC,BC即l,m,n可以通过超声波在空气中的传播速度计算出来,从而C点坐标可以根据简单的数学公式计算出来,当物***移改变后,可以用同样方法计算出物体的绝对位移。因为超声波测距有很高精度,并且很稳定,不会像加速度计那样有积累误差,所以可以使用超声波测距***来补偿加速度计产生的积累误差。
在多个传感器分别检测到相应的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据后,将这些数据输出到处理器。
处理器在得到这些数据后,根据三轴加速度值计算第一位移,再根据三轴角速度值计算第一旋转角度,再根据三轴磁场强度值和三轴加速度值计算第二旋转角度,再根据超声数据计算第二位移,最后,将第一位移、第二位移、第一旋转角度和第二旋转角度通过卡尔曼滤波算法进行融合计算,从而得到该机械臂的位置数据和方向数据。
该处理器在得到位置数据和方向数据后还用于将其上传上位计算机,上位计算机在得到这些位置数据和方向数据后即可计算出机械臂在执行任务时的任务轨迹。
从上述技术方案可以看出,本实施例提供了一种基于多传感器的定位设备,该定位设备应用于机械臂,包括多个传感器和处理器,处理器分别与每个传感器相连接。多个传感器用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到处理器;处理器用于根据三轴加速度值计算第一位移,根据三轴角速度值计算第一旋转角度,根据三轴磁场强度值和三轴加速度值计算第二旋转角度,根据超声数据计算第二位移,并将第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
实施例二
图3为本发明实施例提供的一种基于多传感器的定位方法的步骤流程图。
参照图3所示,本实施例提供的定位方法应用于上一实施例所提供的定位设备,该定位设备设置在机械臂上,包括有多个传感器,分别用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,该定位方法具体包括如下步骤:
S1、获取多个传感器所检测到的数据。
即获取多个传感器分别检测到的机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据。
S2、计算第一位移。
将三轴加速度值对时间做二重积分计算,从而得到机械臂的第一位移。
S3、计算第一旋转角度。
将三轴角速度值对时间做积分计算,从而得到机械臂的第二旋转角度。
S4、计算第二旋转角度。
将三轴磁场强度值和三轴加速度值做坐标变换计算,从而得到第二旋转角度。
S5、计算第二位移。
根据上一实施例中所介绍的原理,根单发三收超声波传感器检测到的超声数据的变化计算该机械臂的第二位移。
S6、将第一位移、第二位移、第一旋转角度和第二旋转角度融合计算。
在得到第一位移、第二位移、第一旋转角度和第二旋转角度后,将这多个数据利用卡尔曼滤波算法进行融合计算,从而得到该机械臂的位置数据和方向数据。
利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
上述的卡尔曼滤波算法可以是无损卡尔曼滤波算法,也可以是扩展卡尔曼滤波算法。
因为无损卡尔曼滤波算法比扩展卡尔曼滤波算法有着更高的精度,并且无损卡尔曼滤波算法可以成功融合加速度计、陀螺仪和磁强计数据计算出方向,所以首先考虑无损卡尔曼滤波算法来计算位置和方向。无损卡尔曼滤波算法算法可以有两种结构来计算,这两种结构在文献中都能找到,从文献中来看使用第一种结构的文献较多。第一种结构是首先使用加速度计和磁强计计算出方向,使用超声波***计算出位置,并把方向和位置作为无损卡尔曼滤波算法的观测值来进行计算;第二种结构是将加速度计和磁强计计算方向的坐标系变换的式子融入到观测方程中,而加速度计、陀螺仪和磁强计的测量值作为无损卡尔曼滤波算法的观测值来计算。
无损卡尔曼滤波算法的第一种结构,利用加速度计和磁强计计算出方向,超声波传感器计算出位置,用这两个观测值来校正和补偿陀螺仪和加速度计产生的积累误差。无损卡尔曼滤波算法的第二种结构,利用陀螺仪、加速度计、磁强计和超声波传感器的测量作为无损卡尔曼滤波算法的观测值,输入到滤波器中,加速度计和磁强计计算方向的方法被加入到观测方程中。
虽然无损卡尔曼滤波算法算法比扩展卡尔曼滤波算法更加精确,但是由于乔里斯基分解无法进行,所以不能再用无损卡尔曼滤波算法算法来计算。通过扩展卡尔曼滤波算法算法融合传感器数据,得到了正确的方向和位置结果。扩展卡尔曼滤波算法算法是通过泰勒级数来对非线性***进行线性化的,状态转移矩阵和观测矩阵分别用雅克比矩阵来代替。扩展卡尔曼滤波算法在对非线性***进行线性化只保留了一阶精度,其实用泰勒级数是可以计算二阶、三阶等高阶项的,但是如果计算高阶项会导致计算量急剧增大,因为高阶项会导致雅克比矩阵的计算量大大增加,从而导致扩展卡尔曼滤波算法不能实时处理数据,所以在用扩展卡尔曼滤波算法算法进行实时计算时,通常只是保留一阶精度。
本实施例中的融合计算具体包括如下步骤:
其一,因为超声波的发射端安装在运动物体上,为三个超声波接收端被固定在地面或者其他固定物体上,这样就可以为位置提供准确的参考标记。从而可以利用第二位移对第一位移进行校正计算,从而得到机械臂的位置数据。
其二,磁强计的原理是测量地球磁场,在一定空间内可以认为地球磁场的大小和方向是不变的,通过磁强计可以以地球磁场向量为参考向量分辨出地磁的南北方向,那么通过磁强计测量地球磁场就保证了方向的准确性。这样可以利用第二旋转角度对第一旋转角度进行校正计算,从而得到机械臂的角度数据。
对于方向校正的原理是:加速度计在静止状态下可以测量到重力加速度,在除重力外的外力加速度很小时,我们可以认为加速度传感器只测量重力加速度,而重力加速度是一个常量,当我们旋转加速度传感器时,那么加速度传感器三个轴的分量会发生变化,在这种情况下,可以认为加速度传感器的任何旋转都是从加速度传感器是水平状态下经过旋转而得到的,那么我们就可以根据加速度传感器最终状态计算它的旋转角度;而地磁在一定范围内,也可以认为它也是一个常量,当磁强计发生旋转时,它的三个坐标轴方向的输出也会发生变化,同样可以认为,磁强计的状态是从它指向地磁北极并且平行于水平面的状态旋转而来的。基于上面的论述,可以使用加速度传感器和地磁传感器实现对方向的校正。
实施例三
图4为本发明实施例提供的一种基于多传感器的定位***的结构框图。
参照图4所示,本实施例提供的定位装置应用于实施例一所提供的定位设备,该定位设备设置在机械臂上,包括有多个传感器,分别用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,该定位***具体包括数据获取模块30、第一计算模块40、第二计算模块50、第三计算模块60、第四计算模块70和融合计算模块80。
数据获取模块用于获取多个传感器所检测到的数据。
即获取多个传感器分别检测到的机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据。
第一计算模块用于将三轴加速度值对时间做二重积分计算,从而得到机械臂的第一位移。
第二计算模块用于将三轴角速度值对时间做积分计算,从而得到机械臂的第二旋转角度。
第三计算模块用于将三轴磁场强度值和三轴加速度值做坐标变换计算,从而得到第二旋转角度。
第四计算模块用于根据上一实施例中所介绍的原理,根单发三收超声波传感器检测到的超声数据的变化计算该机械臂的第二位移。
融合计算模块用于在得到第一位移、第二位移、第一旋转角度和第二旋转角度后,将这多个数据利用卡尔曼滤波算法进行融合计算,从而得到该机械臂的位置数据和方向数据。
利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
上述的卡尔曼滤波算法可以是无损卡尔曼滤波算法,也可以是扩展卡尔曼滤波算法。
本实施例中的融合计算模块具体包括第一校正单元和第二校正单元。
因为超声波的发射端安装在运动物体上,为三个超声波接收端被固定在地面或者其他固定物体上,这样就可以为位置提供准确的参考标记。基于以上分析第一校正单元用于利用第二位移对第一位移进行校正计算,从而得到机械臂的位置数据。
磁强计的原理是测量地球磁场,在一定空间内可以认为地球磁场的大小和方向是不变的,通过磁强计可以以地球磁场向量为参考向量分辨出地磁的南北方向,那么通过磁强计测量地球磁场就保证了方向的准确性。基于以上分析第二校正单元用于利用第二旋转角度对第一旋转角度进行校正计算,从而得到机械臂的角度数据。
实施例四
本实施例提供了一种机械臂,该机械臂设置有实施例一所提供的定位设备,包括多个传感器和处理器,处理器分别与每个传感器相连接。多个传感器用于检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到处理器;处理器用于根据三轴加速度值计算第一位移,根据三轴角速度值计算第一旋转角度,根据三轴磁场强度值和三轴加速度值计算第二旋转角度,根据超声数据计算第二位移,并将第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
实施例五
本实施例提供了另一种机械臂,该机械臂包括控制器,该控制器设置有实施例四所提供的定位***。该定位***可以得到机械臂的位置数据和方向数据,利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
实施例六
本实施例提供了又一种机械臂,该机械臂设置有控制器和多个传感器,多个传感器分别用检测机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据。
该控制器100与多个传感器相连接,包括至少一个处理器101和存储器102,两者通过数据总线103相连接,具体如图5所示。存储器用于存储相应的计算机程序或指令,处理器则用于通过数据总线取得该计算机程序或指令,以使控制器对机械臂实现如下面的操作:
获取多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
将三轴加速度值对时间做二重积分计算,得到第一位移;
将三轴角速度值对时间做积分计算,得到第一旋转角度;
将三轴磁场强度值和三轴加速度值做坐标变换计算,得到第二旋转角度;
根据超声数据的数据变化进行计算,得到第二位移;
将第一位移、第一旋转角度、第二位移和第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据。
利用位置数据和方向数据就可确定机械臂在执行任务时的任务轨迹,从而能够利用任务轨迹对机械臂的控制***进行训练,“教会”其如何动作,省去了每次更换机械臂时重新编程的麻烦。
另外,处理器还用于将位置数据和方向数据输出到上位计算机,以使上位计算机根据位置数据和方向数据精确定位机械臂的工作轨迹。
本说明书中的各个实施例均采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似的部分互相参见即可。
本领域内的技术人员应明白,本发明实施例的实施例可提供为方法、***、或计算机程序产品。因此,本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明实施例是参照根据本发明实施例的方法、终端设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的***。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令***的制造品,该指令***实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上,使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明实施例的优选实施例,但本领域内的技术人员一旦得知了基本创造性概念,则可对这些实施例做出另外的变更和修改。所以,所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。
最后,还需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
以上对本发明所提供的技术方案进行了详细介绍,本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (10)
1.一种基于多传感器的定位设备,应用于机械臂,其特征在于,所述定位设备包括多个传感器和处理器,所述处理器分别与每个所述传感器相连接,其中:
所述多个传感器用于检测所述机械臂的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据,并输出到所述处理器;
所述处理器用于根据所述三轴加速度值计算第一位移,根据所述三轴角速度值计算第一旋转角度,根据所述三轴磁场强度值和所述三轴加速度值计算第二旋转角度,根据所述超声数据计算第二位移;
将所述第一位移、第一旋转角度、第二位移和第二旋转角度通过卡尔曼滤波算法进行融合计算,得到所述机械臂的位置数据和方向数据,具体包括:
将所述第一位移和第二位移通过卡尔曼滤波算法进行位置融合计算,得到所述机械臂的位置数据;
将所述第一旋转角度和第二旋转角度通过卡尔曼滤波算法进行方向融合计算,得到所述机械臂的方向数据;
所述位置数据和方向数据用于确定机械臂在执行任务时的任务轨迹,利用所述任务轨迹训练机械臂的控制***,所述控制***用于对机械臂或更换后的机械臂进行控制。
2.如权利要求1所述的定位设备,其特征在于,所述多个传感器包括三轴加速度传感器、三轴陀螺仪、三轴地磁传感器和单发三收超声波传感器,其中:
所述三轴加速度传感器用于检测所述三轴加速度值;
所述三轴陀螺仪用于检测所述三轴角速度值;
所述三轴地磁传感器用于检测所述三轴磁场强度值;
所述单发三收超声波传感器用于检测所述超声数据。
3.如权利要求1所述的定位设备,其特征在于,所述处理器还用于将所述位置数据和所述方向数据输出到上位计算机,以使所述上位计算机根据所述位置数据和所述方向数据精确定位所述机械臂的工作轨迹。
4.一种定位方法,应用于如权利要求1~3任一项所述的定位设备,其特征在于,所述定位方法包括步骤:
获取多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
将所述三轴加速度值对时间做二重积分计算,得到第一位移;
将所述三轴角速度值对时间做积分计算,得到第一旋转角度;
将所述三轴磁场强度值和所述三轴加速度值做坐标变换计算,得到第二旋转角度;
根据所述超声数据的数据变化进行计算,得到第二位移;
将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据,具体包括:
将所述第一位移和第二位移通过卡尔曼滤波算法进行位置融合计算,得到所述机械臂的位置数据;
将所述第一旋转角度和第二旋转角度通过卡尔曼滤波算法进行方向融合计算,得到所述机械臂的方向数据;
所述位置数据和方向数据用于确定机械臂在执行任务时的任务轨迹,利用所述任务轨迹训练机械臂的控制***,所述控制***用于对机械臂或更换后的机械臂进行控制。
5.如权利要求4所述的定位方法,其特征在于,所述将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,包括:
利用所述第二位移对所述第一位移进行位置校正计算,得到所述位置数据;
利用所述第二旋转角度对所述第一旋转角度进行方向校正计算,得到所述方向数据。
6.如权利要求4所述的定位方法,其特征在于,所述卡尔曼滤波算法为扩展卡尔曼滤波算法或者无损卡尔曼滤波算法。
7.一种定位***,应用于如权利要求1~3任一项所述的定位设备,其特征在于,所述定位***包括:
数据获取模块,用于获取多个传感器检测的三轴加速度值、三轴角速度值、三轴磁场强度值和超声数据;
第一计算模块,用于将所述三轴加速度值对时间做二重积分计算,得到第一位移;
第二计算模块,用于将所述三轴角速度值对时间做积分计算,得到第一旋转角度;
第三计算模块,用于将所述三轴磁场强度值和所述三轴加速度值做坐标变换计算,得到第二旋转角度;
第四计算模块,用于根据所述超声数据的数据变化进行计算,得到第二位移;
融合计算模块,用于将所述第一位移、所述第一旋转角度、所述第二位移和所述第二旋转角度利用卡尔曼滤波算法进行融合计算,得到机械臂的位置数据和方向数据,具体包括:
将所述第一位移和第二位移通过卡尔曼滤波算法进行位置融合计算,得到所述机械臂的位置数据;
将所述第一旋转角度和第二旋转角度通过卡尔曼滤波算法进行方向融合计算,得到所述机械臂的方向数据;
所述位置数据和方向数据用于确定机械臂在执行任务时的任务轨迹,利用所述任务轨迹训练机械臂的控制***,所述控制***用于对机械臂或更换后的机械臂进行控制。
8.如权利要求7所述的定位***,其特征在于,所述融合计算模块包括:
第一校正单元,用于利用所述第二位移对所述第一位移进行位置校正计算,得到所述位置数据;
第二校正单元,用于利用所述第二旋转角度对所述第一旋转角度进行方向校正计算,得到所述方向数据。
9.一种机械臂,其特征在于,设置有如权利要求1~3所述的定位设备。
10.一种机械臂,包括控制器,其特征在于,所述控制器包括如权利要求7或8所述的定位***。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810433771.8A CN108592902B (zh) | 2018-05-08 | 2018-05-08 | 一种基于多传感器的定位设备及定位方法、***和机械臂 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810433771.8A CN108592902B (zh) | 2018-05-08 | 2018-05-08 | 一种基于多传感器的定位设备及定位方法、***和机械臂 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108592902A CN108592902A (zh) | 2018-09-28 |
CN108592902B true CN108592902B (zh) | 2020-07-31 |
Family
ID=63636339
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810433771.8A Active CN108592902B (zh) | 2018-05-08 | 2018-05-08 | 一种基于多传感器的定位设备及定位方法、***和机械臂 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108592902B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109343454A (zh) * | 2018-11-05 | 2019-02-15 | 徐州路马力电控科技有限公司 | 一种基于行为检测的可编程控制器 |
CN110672087B (zh) * | 2019-09-02 | 2023-08-11 | 湖南凌傲科技有限公司 | 一种人体跟踪方法及*** |
CN113799127B (zh) * | 2021-09-15 | 2023-05-23 | 华南理工大学 | 光学双目定位***下六自由度机械臂无标定位姿定位方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6918186B2 (en) * | 2003-08-01 | 2005-07-19 | The Charles Stark Draper Laboratory, Inc. | Compact navigation system and method |
CN102023700A (zh) * | 2009-09-23 | 2011-04-20 | 吴健康 | 一种三维人机交互*** |
US9116002B2 (en) * | 2009-08-27 | 2015-08-25 | Apple Inc. | Context determination to assist location determination accuracy |
CN106679649A (zh) * | 2016-12-12 | 2017-05-17 | 浙江大学 | 一种手部运动追踪***及追踪方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106737673B (zh) * | 2016-12-23 | 2019-06-18 | 浙江大学 | 一种基于深度学习的端到端的机械臂控制的方法 |
CN107457780B (zh) * | 2017-06-13 | 2020-03-17 | 广州视源电子科技股份有限公司 | 控制机械臂运动的方法及装置、存储介质和终端设备 |
-
2018
- 2018-05-08 CN CN201810433771.8A patent/CN108592902B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6918186B2 (en) * | 2003-08-01 | 2005-07-19 | The Charles Stark Draper Laboratory, Inc. | Compact navigation system and method |
US9116002B2 (en) * | 2009-08-27 | 2015-08-25 | Apple Inc. | Context determination to assist location determination accuracy |
CN102023700A (zh) * | 2009-09-23 | 2011-04-20 | 吴健康 | 一种三维人机交互*** |
CN106679649A (zh) * | 2016-12-12 | 2017-05-17 | 浙江大学 | 一种手部运动追踪***及追踪方法 |
Non-Patent Citations (2)
Title |
---|
基于UKF的MEMS传感器姿态测量***;赵鹤等;《传感技术学报》;20110531;第24卷(第5期);全文 * |
基于互补滤波器的MEMS_GPS_地磁组合导航***;张勇刚等;《***工程与电子技术》;20141130;第36卷(第11期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN108592902A (zh) | 2018-09-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Cheviron et al. | Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV | |
CN105203098A (zh) | 基于九轴mems传感器的农业机械全姿态角更新方法 | |
CN108592902B (zh) | 一种基于多传感器的定位设备及定位方法、***和机械臂 | |
CN102937450B (zh) | 一种基于陀螺测量信息的相对姿态确定方法 | |
CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN111895988A (zh) | 无人机导航信息更新方法及装置 | |
Cirillo et al. | A comparison of multisensor attitude estimation algorithms | |
Ludwig | Optimization of control parameter for filter algorithms for attitude and heading reference systems | |
Hoang et al. | Pre-processing technique for compass-less madgwick in heading estimation for industry 4.0 | |
Bazhenov et al. | Software for the mobile robot spatial orientation system | |
Blachuta et al. | Attitude and heading reference system based on 3D complementary filter | |
Panahandeh et al. | Observability analysis of a vision-aided inertial navigation system using planar features on the ground | |
CN110940336A (zh) | 捷联惯导仿真定位解算方法、装置及终端设备 | |
JP2011185868A (ja) | 方位検知装置 | |
Haotian et al. | Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test | |
Zhang et al. | Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors | |
Xuan | Application of real-time Kalman filter with magnetic calibration for MEMS sensor in attitude estimation | |
Pirník et al. | Navigation of the autonomous ground vehicle utilizing low-cost inertial navigation | |
Tran et al. | Dynamical analysis and validation of motion control by filtering performance for aerial robotic system | |
Menna et al. | GPS aided strapdown inertial navigation system for autonomous robotics applications | |
Zhang et al. | An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors | |
CN115839726B (zh) | 磁传感器和角速度传感器联合标定的方法、***及介质 | |
Mizutani et al. | Semiautomatic calibration and alignment of a low cost, 9 sensor inertial magnetic measurement sensor | |
Liu et al. | Study on the inertial navigation system for autonomous driving |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |