CN108956003B - 一种实时标定六维传感器姿态的方法、装置及终端设备 - Google Patents

一种实时标定六维传感器姿态的方法、装置及终端设备 Download PDF

Info

Publication number
CN108956003B
CN108956003B CN201810784129.4A CN201810784129A CN108956003B CN 108956003 B CN108956003 B CN 108956003B CN 201810784129 A CN201810784129 A CN 201810784129A CN 108956003 B CN108956003 B CN 108956003B
Authority
CN
China
Prior art keywords
coordinate system
dimensional sensor
sensor
dimensional
attitude
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
Application number
CN201810784129.4A
Other languages
English (en)
Other versions
CN108956003A (zh
Inventor
黄渭
刘志龙
陈晨
杨薛鹏
王佩闯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Songzhi Intelligent Technology Suzhou Co Ltd
Original Assignee
Songzhi Intelligent Technology Suzhou Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Songzhi Intelligent Technology Suzhou Co Ltd filed Critical Songzhi Intelligent Technology Suzhou Co Ltd
Priority to CN201810784129.4A priority Critical patent/CN108956003B/zh
Publication of CN108956003A publication Critical patent/CN108956003A/zh
Application granted granted Critical
Publication of CN108956003B publication Critical patent/CN108956003B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L25/00—Testing or calibrating of apparatus for measuring force, torque, work, mechanical power, or mechanical efficiency
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
    • G01L5/16—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes for measuring several components of force

Abstract

本发明适用于传感器应用技术领域，提供了一种实时标定六维传感器姿态的方法、装置及终端设备，其方法包括：建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系；控制六维传感器定向运动，获取六维位姿数据，计算六维传感器坐标系相对于被控设备运动坐标系的初始姿态关系；获取陀螺仪传感器相对于六维传感器的姿态关系；检测陀螺仪传感器在旋转坐标系下的角度数据；根据陀螺仪传感器相对于六维传感器的姿态关系、陀螺仪传感器在旋转坐标系下的角度数据以及六维传感器坐标系相对于被控设备运动坐标系的初始姿态关系，计算六维传感器相对于被控设备的姿态关系。通过本发明能够实时的标定六维传感器姿态，实时反映传感器与被控设备的姿态关系。

Description

一种实时标定六维传感器姿态的方法、装置及终端设备
技术领域
本发明涉及传感器应用技术领域，尤其涉及一种实时标定六维传感器姿态的方法、装置及终端设备。
背景技术
传感与测量是控制系统的重要组成部分，多维力传感器中应用最广泛的为六维力传感器。广义的六维力传感器能够检查空间任意力系中的三维正交力及三维正交力矩，由于其测力信息丰富、测量精度高等特点，主要应用在力及力-位控制场合，如机器人末端执行器等被控设备，汽车行驶过程轮力检测、轮廓跟踪、精密装配、双手协调等控制场合。六维传感器作为一种信号输入设备，通过推、拉、压、提、旋转与倾斜控制帽，可产生平移、俯仰、横滚和偏航等描述位姿的信号量，利用六维传感器产生的位姿信号，能够实现对被控设备的位置和姿态的控制。
在利用六维传感器产生的位姿信号，能够实现对被控设备的位置和姿态的控制时，需要标定出传感器与被控设备的姿态关系，但是，初次标定的传感器与被控设备的姿态关系，不再适用于标定姿态变换后的传感器与被控设备的姿态关系。
发明内容
本发明的主要目的在于提出一种实时标定六维传感器姿态的方法、装置及终端设备，以解决现有技术中标定传感器与被控设备的安装关系的方法不能实时反映传感器与被控设备的姿态关系的问题。
为实现上述目的，本发明实施例第一方面提供了一种实时标定六维传感器姿态的方法，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器；
所述实时标定六维传感器姿态的方法包括：
建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系；
控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系；
获取所述陀螺仪传感器相对于所述六维传感器的姿态关系；
检测所述陀螺仪传感器在所述旋转坐标系下的角度数据；
根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系。
结合本发明实施例第一方面，本发明实施例第一方面的第一实施方式中，所述控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系包括：
控制所述六维传感器沿所述被控设备运动坐标系中的坐标轴方向运动，获取所述六维传感器在所述六维传感器坐标系中的初始数据；
根据所述六维传感器在所述六维传感器坐标系中的初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量；
根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的姿态关系；
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
本发明实施例第一方面的第一实施方式中，所述获取所述六维传感器在所述六维传感器坐标系中的初始数据包括：
所述被控设备静止时，获取所述六维传感器的初始值，公式为：
P0＝[x0 y0 z0]T，
其中，x0，y0，z0表示所述六维传感器在所述六维传感器坐标系中的初始位置；
控制所述六维传感器沿所述被控设备运动的X方向运动时，获取第一初始数据，公式为：
Px＝[xx yx zx]T，
其中，xx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量；
控制所述六维传感器沿所述被控设备运动的Y方向运动时，获取第二初始数据，公式为：
Py＝[xy yy zy]T，
其中，xy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量。
本发明实施例第一方面的第一实施方式中，所述根据所述六维传感器初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量包括：
计算得到所述被控设备运动坐标系的X轴在六维传感器坐标系下的单位向量，公式为：
Figure BDA0001733307310000041
计算得到所述被控设备运动坐标系的Y轴在六维传感器坐标系下的单位向量，公式为：
Figure BDA0001733307310000042
计算得到所述被控设备运动坐标系的Z轴在六维传感器坐标系下的单位向量，公式为：
Figure BDA0001733307310000043
根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，公式为：
Figure BDA0001733307310000044
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，公式为：
Figure BDA0001733307310000045
其中A表示被控设备运动坐标系，B表示六维传感器坐标系。
结合本发明实施例第一方面，本发明实施例第一方面的第二实施方式中，所述获取所述陀螺仪传感器相对于所述六维传感器的姿态关系包括：
通过所述陀螺仪传感器与所述六维传感器的安装关系获取所述陀螺仪传感器相对于所述六维传感器的姿态关系。
结合本发明实施例第一方面，本发明实施例第一方面的第三实施方式中，所述检测所述陀螺仪传感器在所述旋转坐标系下的角度数据包括：
检测所述陀螺仪传感器在所述旋转坐标系下俯仰角、横滚角和航向角。
结合本发明实施例第一方面的第一实施方式、第二实施方式和第三实施方式，所述根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系包括：
计算所述陀螺仪传感器相对于所述被控设备的姿态关系，公式为：
Figure BDA0001733307310000051
其中，C表示陀螺仪传感器坐标系，C BR为所述陀螺仪传感器相对于所述六维传感器的姿态关系，
Figure BDA0001733307310000052
为所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，CRzyx(α,β,γ)为所述陀螺仪传感器在所述旋转坐标系下的角度数据；
计算所述六维传感器相对被控设备的姿态关系，公式为：
Figure BDA0001733307310000053
其中
Figure BDA0001733307310000054
为所述陀螺仪传感器相对于所述被控设备的姿态关系，
Figure BDA0001733307310000055
为所述六维传感器相对于所述陀螺仪传感器的姿态关系。
本发明实施例第二方面提供了一种实时标定六维传感器姿态的装置，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器；
所述实时标定六维传感器姿态的装置包括：
坐标系建立模块，用于建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系；
第一计算模块，用于控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系；
获取模块，用于获取所述陀螺仪传感器相对于所述六维传感器的姿态关系；
角度数据检测模块，用于检测所述陀螺仪传感器在所述旋转坐标系下的角度数据；
第二计算模块，用于根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系。
本发明实施例第三方面提供了一种终端设备，其特征在于，包括：存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现上述的实时标定六维传感器姿态的方法中的各个步骤。
本发明实施例第四方面提供了一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现上述的实时标定六维传感器姿态的方法中的各个步骤。
本发明实施例提出的实时标定六维传感器姿态的方法，在被控设备上安装能够产生六维姿态数据的六维传感器，并在六维传感器上安装陀螺仪传感器，首先，通过陀螺仪传感器获得六维传感器相对于陀螺仪传感器的姿态关系，从而实时的计算出陀螺仪传感器相对于被控设备的姿态关系；然后，结合陀螺仪传感器在旋转坐标系下的角度数据，实时的标定六维传感器相对被控设备的姿态关系。
附图说明
图1为本发明实施例一提供的实时标定六维传感器姿态的方法的实现流程示意图；
图2为图1所示实施例中步骤S102的细化步骤流程示意图；
图3为图1所示实施例中步骤S105的细化步骤流程示意图；
图4为本发明实施例二提供的实时标定六维传感器姿态的装置的结构示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
在本文中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身并没有特定的意义。因此，"模块"与"部件"可以混合地使用。
在后续的描述中，发明实施例序号仅仅为了描述，不代表实施例的优劣。
实施例一
如图1所示，本发明实施例提供了一种实时标定六维传感器姿态的方法，其中，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器，所述实时标定六维传感器姿态的方法包括：
S101、建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系。
在上述步骤S101中，被控设备可以是任意的能够根据传感器中的数据执行操作的单位，例如机器人和机械手臂等，因此被控设备通常具有自身的运动坐标系，用以确定其的位置和姿态；六维传感器作为一种信号输入设备，通过推、拉、压、提、旋转与倾斜控制帽，可产生平移、俯仰、横滚和偏航等描述位姿的数据，上述的数据均基于传感器自身的坐标系产生；旋转坐标系是基于陀螺仪传感器的坐标系，陀螺仪传感器同样作为一种信号输入设备，用以产生位姿数据。
在具体应用中，六维传感器可以设置在被控设备中的任意部位，例如，当被控设备为机器人时，将六维传感器设置在机器人执行末端，以使机器人根据六维传感器中的数据控制其执行模块执行操作。
S102、控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
在上述步骤S102中，控制六维传感器定向运动，实际上为控制被控设备定向运动，此时获取六维传感器坐标系中的数据，即六维姿态数据，并将六维姿态数据反馈给被控设备，转换到被控设备运动坐标系下，从而计算六维传感器坐标系相对于被控设备运动坐标系的初始姿态关系。
在具体应用中，将六维传感器绑定在被控设备末端执行器上时，控制六维传感器定向移动，可以理解为控制被控设备定向移动，也可以理解为控制被控设备末端执行器定向移动。
在具体应用中，六维传感器可以为任意的能够产生六维姿态数据的六维传感器，例如六维力六维传感器。
如图2所示，本发明实施例还示出了一种上述步骤S102的具体实现方法，其步骤流程包括：
S1021、控制所述六维传感器沿所述被控设备运动坐标系中的坐标轴方向运动，获取所述六维传感器在所述六维传感器坐标系中的初始数据。
在上述步骤S1021中，控制六维传感器定向运动时，六维传感器将受到这一方向的力，通过分解和分析六维传感器所受到的这一方向力在六维传感器系坐标中的数据，获得六维传感器在六维传感器坐标系中的初始数据。
在具体应用中，所述获取所述六维传感器在所述六维传感器坐标系中的初始数据包括：
所述被控设备静止时，获取所述六维传感器的初始值P0，公式为：
P0＝[x0 y0 z0]T，
其中，x0，y0，z0表示所述六维传感器在所述六维传感器坐标系中的初始位置；
控制所述六维传感器沿所述被控设备运动的X方向运动时，获取第一初始数据Px，公式为：
Px＝[xx yx zx]T，
其中，xx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量；
控制所述六维传感器沿所述被控设备运动的Y方向运动时，获取第二初始数据Py，公式为：
Py＝[xy yy zy]T，
其中，xy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量。
上述被控设备运动的Y方向表示被控设备沿被控设备坐标系的Y轴方向运动，六维传感器在六维传感器坐标系中的初始数据包括P0，Px，Py。
S1022、根据所述六维传感器在所述六维传感器坐标系中的初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量。
在上述步骤S1022中，六维传感器在六维传感器坐标系中的初始数据是通过控制被控设备沿被控设备坐标系轴向方向移动所获得的，此时能够算被控设备运动坐标系的坐标轴在六维传感器坐标系下的单位向量；六维传感器中的数据可以分为力和力矩，力表示作用的方向和大小，力矩表示力作用的距离，根据被控设备运动坐标系的坐标轴在六维传感器坐标系下的单位向量，分析力作用的方向，从而获得被控设备运动坐标系的轴向方向在六维传感器坐标系中的方向。
在具体应用中，所述根据所述六维传感器初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量包括：
计算得到所述被控设备运动坐标系的X轴在六维传感器坐标系下的单位向量
Figure BDA0001733307310000101
公式为：
Figure BDA0001733307310000102
计算得到所述被控设备运动坐标系的Y轴在六维传感器坐标系下的单位向量
Figure BDA0001733307310000103
公式为：
Figure BDA0001733307310000104
计算得到所述被控设备运动坐标系的Z轴在六维传感器坐标系下的单位向量
Figure BDA0001733307310000105
公式为：
Figure BDA0001733307310000106
上述的单位向量
Figure BDA0001733307310000107
中的数据xx、xy、x0、yx、yy、y0、zx、zy、z0等为上述六维传感器在六维传感器坐标系中的初始数据P0，Px，Py的值。
本领域技术人员可以理解的是，本发明实施例中根据被控设备运动坐标系的X轴在六维传感器坐标系下的单位向量和被控设备运动坐标系的Y轴在六维传感器坐标系下的单位向量，计算得出了被控设备运动坐标系的Z轴在六维传感器坐标系下的单位向量，也可以控制六维传感器沿被控设备运动的Z方向运动时，获取第三初始数据Pz，然后根据次六维传感器数据获得被控设备运动坐标系的Z轴在六维传感器坐标系下的单位向量。
S1023、根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系。
S1024、根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
在上述步骤S1023和S1024中，根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系
Figure BDA0001733307310000111
公式为：
Figure BDA0001733307310000112
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，公式为：
Figure BDA0001733307310000113
其值为
Figure BDA0001733307310000114
其中A表示被控设备运动坐标系，B表示六维传感器坐标系。
S103、获取所述陀螺仪传感器相对于所述六维传感器的姿态关系。
在上述步骤S103中，由于陀螺仪传感器安装固定在六维传感器上，因此陀螺仪传感器相对于六维传感器的姿态关系可以认为是不变化的。
在一个实施例中，可以通过陀螺仪传感器与六维传感器的安装关系获取陀螺仪传感器相对于六维传感器的姿态关系；也可以通过对陀螺仪传感器坐标系和六维传感器坐标系进行标定，获得陀螺仪传感器相对于六维传感器的姿态关系。
在本发明实施例中，陀螺仪传感器相对于六维传感器的姿态关系可以用
Figure BDA0001733307310000115
表示，其中B表示六维传感器坐标系，C表示陀螺仪传感器坐标系，则可以想到的是，六维传感器相对于陀螺仪传感器的姿态关系用
Figure BDA0001733307310000116
表示。
S104、检测所述陀螺仪传感器在所述旋转坐标系下的角度数据。
在上述步骤S104中，陀螺仪传感器在所述旋转坐标系下的角度数据能够表现六维传感器坐标系相对于被控设备运动坐标系的姿态变换量。
在一个实施例中，检测陀螺仪传感器在旋转坐标系下的角度数据包括：检测陀螺仪传感器在旋转坐标系下俯仰角、横滚角和航向角。
在具体应用中，俯仰角、横滚角和航向角也称欧拉角。
本发明实施例中，俯仰角用α表示，横滚角用β表示，航向角用γ表示。
S105、根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系。
在上述步骤S105中，陀螺仪传感器在旋转坐标系下的角度数据能够表现六维传感器坐标系相对于被控设备运动坐标系的姿态变换量，再结合陀螺仪传感器相对于六维传感器的姿态关系，则能够得到六维传感器相对于被控设备的姿态变换量，此时若计算得出了六维传感器坐标系相对于被控设备运动坐标系的初始姿态关系，则能够得到六维传感器相对于被控设备的姿态关系。
如图3所示，本发明实施例还提供了上述步骤S105的细化步骤，包括：
S1051、计算所述陀螺仪传感器相对于所述被控设备的姿态关系
Figure BDA0001733307310000121
公式为：
Figure BDA0001733307310000122
其中，C表示陀螺仪传感器坐标系，
Figure BDA0001733307310000123
为所述陀螺仪传感器相对于所述六维传感器的姿态关系，
Figure BDA0001733307310000124
为所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，CRzyx(α,β,γ)为所述陀螺仪传感器在所述旋转坐标系下的角度数据。
S1052、计算所述六维传感器相对被控设备的姿态关系
Figure BDA0001733307310000125
公式为：
Figure BDA0001733307310000126
其中
Figure BDA0001733307310000127
为所述陀螺仪传感器相对于所述被控设备的姿态关系，
Figure BDA0001733307310000128
为所述六维传感器相对于所述陀螺仪传感器的姿态关系。
在上述步骤S1051和S1052中，六维传感器相对被控设备的姿态关系可以表示为
Figure BDA0001733307310000129
本发明实施例提供的实时标定六维传感器姿态的方法，在被控设备上安装能够产生六维姿态数据的六维传感器，并在六维传感器上安装陀螺仪传感器，首先，通过陀螺仪传感器获得六维传感器相对于陀螺仪传感器的姿态关系，从而实时的计算出陀螺仪传感器相对于被控设备的姿态关系；然后，结合陀螺仪传感器在旋转坐标系下的角度数据，实时的标定六维传感器相对被控设备的姿态关系。
实施例二
如图4所示，本发明实施例提供了一种实时标定六维传感器姿态的装置40，其中，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器，实时标定六维传感器姿态的装置40包括：
坐标系建立模块41，用于建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系。
第一计算模块42，用于控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
获取模块43，用于获取所述陀螺仪传感器相对于所述六维传感器的姿态关系。
角度数据检测模块44，用于检测所述陀螺仪传感器在所述旋转坐标系下的角度数据。
第二计算模块45，用于根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系。
在一个实施例中，第一计算模块42控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系可以包括：
控制所述六维传感器沿所述被控设备运动坐标系中的坐标轴方向运动，获取所述六维传感器在所述六维传感器坐标系中的初始数据；
根据所述六维传感器在所述六维传感器坐标系中的初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量；
根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的姿态关系；
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
本发明实施例还提供一种终端设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时，实现如实施例一中所述的实时标定六维传感器姿态的方法中的各个步骤
本发明实施例还提供一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现如实施例一中所述的实时标定六维传感器姿态的方法中的各个步骤。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种实时标定六维传感器姿态的方法，其特征在于，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器；
所述实时标定六维传感器姿态的方法包括：
建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系；
控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系；
获取所述陀螺仪传感器相对于所述六维传感器的姿态关系；
检测所述陀螺仪传感器在所述旋转坐标系下的角度数据；
根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，实时的计算所述六维传感器相对于所述被控设备的姿态关系。
2.如权利要求1所述的实时标定六维传感器姿态的方法，其特征在于，所述控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系包括：
控制所述六维传感器沿所述被控设备运动坐标系中的坐标轴方向运动，获取所述六维传感器在所述六维传感器坐标系中的初始数据；
根据所述六维传感器在所述六维传感器坐标系中的初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量；
根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系；
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系。
3.如权利要求2所述的实时标定六维传感器姿态的方法，其特征在于，所述获取所述六维传感器在所述六维传感器坐标系中的初始数据包括：
所述被控设备静止时，获取所述六维传感器的初始值，公式为：
P0＝[x0 y0 z0]T，
其中，x0，y0，z0表示所述六维传感器在所述六维传感器坐标系中的初始位置；
控制所述六维传感器沿所述被控设备运动的X方向运动时，获取第一初始数据，公式为：
Px＝[xx yx zx]T，
其中，xx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的X方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量；
控制所述六维传感器沿所述被控设备运动的Y方向运动时，获取第二初始数据，公式为：
Py＝[xy yy zy]T，
其中，xy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的X轴上的分量，yy为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Y轴上的分量，zx为沿所述被控设备运动的Y方向推动所述六维传感器的力在所述六维传感器坐标系中的Z轴上的分量。
4.如权利要求2所述的实时标定六维传感器姿态的方法，其特征在于，所述根据所述六维传感器初始数据，计算所述被控设备运动坐标系的坐标轴在所述六维传感器坐标系下的单位向量包括：
计算得到所述被控设备运动坐标系的X轴在六维传感器坐标系下的单位向量，公式为：
Figure FDA0002559769890000031
计算得到所述被控设备运动坐标系的Y轴在六维传感器坐标系下的单位向量，公式为：
Figure FDA0002559769890000032
计算得到所述被控设备运动坐标系的Z轴在六维传感器坐标系下的单位向量，公式为：
Figure FDA0002559769890000033
根据所述单位向量，计算所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，公式为：
Figure FDA0002559769890000034
根据所述被控设备运动坐标系相对于所述六维传感器坐标系的初始姿态关系，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，公式为：
Figure FDA0002559769890000035
其中A表示被控设备运动坐标系，B表示六维传感器坐标系。
5.如权利要求1所述的实时标定六维传感器姿态的方法，其特征在于，所述获取所述陀螺仪传感器相对于所述六维传感器的姿态关系包括：
通过所述陀螺仪传感器与所述六维传感器的安装关系获取所述陀螺仪传感器相对于所述六维传感器的姿态关系。
6.如权利要求1所述的实时标定六维传感器姿态的方法，其特征在于，所述检测所述陀螺仪传感器在所述旋转坐标系下的角度数据包括：
检测所述陀螺仪传感器在所述旋转坐标系下俯仰角、横滚角和航向角。
7.如权利要求1至6任一项所述的实时标定六维传感器姿态的方法，其特征在于，所述根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，计算所述六维传感器相对于所述被控设备的姿态关系包括：
计算所述陀螺仪传感器相对于所述被控设备的姿态关系，公式为：
Figure FDA0002559769890000041
其中，C表示陀螺仪传感器坐标系，
Figure FDA0002559769890000042
为所述陀螺仪传感器相对于所述六维传感器的姿态关系，
Figure FDA0002559769890000043
为所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，CRzyx(α,β,γ)为所述陀螺仪传感器在所述旋转坐标系下的角度数据；
计算所述六维传感器相对被控设备的姿态关系，公式为：
Figure FDA0002559769890000044
其中
Figure FDA0002559769890000045
为所述陀螺仪传感器相对于所述被控设备的姿态关系，
Figure FDA0002559769890000046
为所述六维传感器相对于所述陀螺仪传感器的姿态关系。
8.一种实时标定六维传感器姿态的装置，其特征在于，所述六维传感器设置于被控设备上，所述六维传感器上还安装有陀螺仪传感器；
所述实时标定六维传感器姿态的装置包括：
坐标系建立模块，用于建立被控设备运动坐标系、六维传感器坐标系和旋转坐标系；
第一计算模块，用于控制所述六维传感器定向运动，获取六维位姿数据，计算所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系；
获取模块，用于获取所述陀螺仪传感器相对于所述六维传感器的姿态关系；
角度数据检测模块，用于检测所述陀螺仪传感器在所述旋转坐标系下的角度数据；
第二计算模块，用于根据所述陀螺仪传感器相对于所述六维传感器的姿态关系、所述陀螺仪传感器在所述旋转坐标系下的角度数据以及所述六维传感器坐标系相对于所述被控设备运动坐标系的初始姿态关系，实时的计算所述六维传感器相对于所述被控设备的姿态关系。
9.一种终端设备，其特征在于，包括：存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现如权利要求1至7任一项所述的实时标定六维传感器姿态的方法中的各个步骤。
10.一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1至7任一项所述的实时标定六维传感器姿态的方法中的各个步骤。
CN201810784129.4A 2018-07-17 2018-07-17 一种实时标定六维传感器姿态的方法、装置及终端设备 Active CN108956003B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810784129.4A CN108956003B (zh) 2018-07-17 2018-07-17 一种实时标定六维传感器姿态的方法、装置及终端设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810784129.4A CN108956003B (zh) 2018-07-17 2018-07-17 一种实时标定六维传感器姿态的方法、装置及终端设备

Publications (2)

Publication Number Publication Date
CN108956003A CN108956003A (zh) 2018-12-07
CN108956003B true CN108956003B (zh) 2020-10-20

Family

ID=64481567

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810784129.4A Active CN108956003B (zh) 2018-07-17 2018-07-17 一种实时标定六维传感器姿态的方法、装置及终端设备

Country Status (1)

Country Link
CN (1) CN108956003B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114791333A (zh) * 2022-03-17 2022-07-26 清华大学 力传感器测量坐标系标定方法、装置、设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导系统初始姿态快速测量方法
CN102788596A (zh) * 2012-08-16 2012-11-21 辽宁工程技术大学 一种载体姿态未知的旋转捷联惯导系统现场标定方法
CN103727939A (zh) * 2013-12-31 2014-04-16 清华大学 一种双轴旋转的姿态测量系统及其测量方法
CN104182614A (zh) * 2014-07-25 2014-12-03 山东建筑大学 六自由度机械臂姿态监测系统及方法
CN107270900A (zh) * 2017-07-25 2017-10-20 广州阿路比电子科技有限公司 一种6自由度空间位置和姿态的检测系统和方法
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3748483B2 (ja) * 1997-09-12 2006-02-22 株式会社リコー 姿勢入力装置、姿勢入力機能を備えるペン型入力装置及びそのペン型入力装置を有するペン型入力システム
JP2008052362A (ja) * 2006-08-22 2008-03-06 Toshiba Corp 自律移動装置
CN103424124A (zh) * 2012-05-23 2013-12-04 国家体育总局体育科学研究所 基于图像测量技术的无磁惯导单元标定方法
CN104298243B (zh) * 2014-08-19 2017-02-15 北京理工大学 一种仿人机器人不平整地面行走的稳定控制方法
CN104771175B (zh) * 2015-03-04 2017-08-01 上海交通大学 捕捉人体四肢三维空间姿态的可穿戴智能环
AR104232A1 (es) * 2015-04-13 2017-07-05 Leica Geosystems Pty Ltd Compensación dinámica del movimiento en maquinarias
WO2016208467A1 (ja) * 2015-06-22 2016-12-29 三菱電機株式会社 キャリブレーション装置およびこれを用いたロボットシステム
CN105539890B (zh) * 2015-12-30 2018-01-30 哈尔滨工业大学 一种模拟空间机械臂捕获目标卫星的地面三维空间微重力的装置与方法
CN105818145A (zh) * 2016-03-31 2016-08-03 张锐 仿人机器人的分布式控制系统及方法
CN108127660A (zh) * 2016-12-01 2018-06-08 韩中越 一种液压驱动的四足机器人控制系统
CN106994687B (zh) * 2017-03-30 2019-08-20 北京卫星环境工程研究所 工业机器人末端六维力传感器安装姿态标定方法
CN107953333B (zh) * 2017-12-20 2021-04-20 深圳大学 一种机械手末端工具标定的控制方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导系统初始姿态快速测量方法
CN102788596A (zh) * 2012-08-16 2012-11-21 辽宁工程技术大学 一种载体姿态未知的旋转捷联惯导系统现场标定方法
CN103727939A (zh) * 2013-12-31 2014-04-16 清华大学 一种双轴旋转的姿态测量系统及其测量方法
CN104182614A (zh) * 2014-07-25 2014-12-03 山东建筑大学 六自由度机械臂姿态监测系统及方法
CN107270900A (zh) * 2017-07-25 2017-10-20 广州阿路比电子科技有限公司 一种6自由度空间位置和姿态的检测系统和方法
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法

Also Published As

Publication number Publication date
CN108956003A (zh) 2018-12-07

Similar Documents

Publication Publication Date Title
US11279045B2 (en) Robot pose estimation method and apparatus and robot using the same
US10569419B2 (en) Control device and robot system
CN111415387B (zh) 相机位姿确定方法、装置、电子设备及存储介质
CN109550219B (zh) 一种运动信息的确定方法、系统及移动设备
CN108332758B (zh) 一种移动机器人的走廊识别方法及装置
US8897919B2 (en) Robot controller which conducts a force control by using a three-axial force sensor
CN110702106B (zh) 一种无人机及其航向对准方法、装置和存储介质
JP2010149267A (ja) ロボットのキャリブレーション方法および装置
CN111230879B (zh) 一种基于力传感器的机器人末端接触力补偿方法及系统
US9844872B1 (en) Determining sensor parameters and model parameters of a robot
JP2008510159A (ja) センサシステムの２つの動きセンサの回転関係をキャリブレートする方法及び装置
CN114454174B (zh) 机械臂动作捕捉方法、介质、电子设备及系统
CN108956003B (zh) 一种实时标定六维传感器姿态的方法、装置及终端设备
CN113211445A (zh) 一种机器人参数标定方法、装置、设备及存储介质
CN113188505A (zh) 姿态角度的测量方法、装置、车辆及智能臂架
CN110861081A (zh) 一种欠约束索并联机器人末端执行器的自主定位方法
CN112476435A (zh) 重力加速度方向的标定方法、标定装置及存储介质
CN114174770A (zh) 磁性编码器校准
CN111812668B (zh) 绕机检查装置及其定位方法、存储介质
CN110749327B (zh) 一种合作环境下的车辆导航方法
CN111982155B (zh) 磁传感器的标定方法、装置、电子设备和计算机存储介质
KR100940095B1 (ko) 포인터 이동 값 계산 장치, 포인터 이동 값 보정 방법 및자세 각도 변화량 보정 방법, 이를 사용하는 3차원 포인팅디바이스
KR101878253B1 (ko) 자세 및 방위각 측정장치의 지자기센서 신호처리 방법
CN114770598B (zh) 水下机器人姿态估算方法、装置、电子设备及存储介质
CN114750153B (zh) 机器人机械臂的运动控制系统、协作机器人及存储介质

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 215000 Suzhou (Jiangsu) pilot Free Trade Zone, Suzhou area, Suzhou Industrial Park, No.88 Jinjihu Avenue, artificial intelligence Industrial Park, e3-101

Applicant after: Songzhi Intelligent Technology (Suzhou) Co., Ltd

Address before: 310006 520, room 5, main building, 618 Xing Gang Road, Qingshan Lake Street, Ling'an District, Hangzhou, Zhejiang.

Applicant before: HANGZHOU SONGZHI INTELLIGENT TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant