CN117288187B - 机器人位姿确定方法、装置、电子设备及存储介质 - Google Patents
机器人位姿确定方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN117288187B CN117288187B CN202311576579.1A CN202311576579A CN117288187B CN 117288187 B CN117288187 B CN 117288187B CN 202311576579 A CN202311576579 A CN 202311576579A CN 117288187 B CN117288187 B CN 117288187B
- Authority
- CN
- China
- Prior art keywords
- angle
- coordinate system
- measurement unit
- robot
- rotation matrix
- 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
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000003860 storage Methods 0.000 title claims abstract description 11
- 230000001133 acceleration Effects 0.000 claims abstract description 170
- 238000005259 measurement Methods 0.000 claims abstract description 149
- 230000005484 gravity Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 254
- 238000012545 processing Methods 0.000 claims description 15
- 238000006243 chemical reaction Methods 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 9
- 238000012360 testing method Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 3
- 238000004891 communication Methods 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 6
- 238000012937 correction Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000005236 sound signal Effects 0.000 description 4
- 230000003068 static effect Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000011065 in-situ storage Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013475 authorization Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Manipulator (AREA)
Abstract
本公开涉及机器人控制技术领域中的一种机器人位姿确定方法、装置、电子设备及存储介质，包括：获取机器人的惯性测量单元输出的原始数据；根据原始数据、机器人在世界坐标系下的重力加速度以及惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式；对关系式求最优解，得到第一角度和第二角度；根据第一角度和第二角度，对惯性测量单元进行标定，并根据标定后的惯性测量单元，确定机器人在世界坐标系下的位姿。这样，可以通过第一角度与第二角度两个关键参数进行标定，可以提高标定惯性测量单元的准确性，进而提高对机器人位姿确定的准确性，且无需额外工装或者传感器标定惯性测量单元，降低了标定成本。
Description
技术领域
本公开涉及机器人控制技术领域，尤其涉及一种机器人位姿确定方法、装置、电子设备及存储介质。
背景技术
惯性测量单元（Inertia Measurement Unit，缩写IMU）是安装在机器人机身的重要传感器，主要用于测量机器人相对于世界坐标系的位置和姿态。因此，IMU数据是否准确直接影响到机器人位姿确定的准确性，进而影响到机器人的控制效果。
IMU出厂前一般会通过转台进行标定，但运输、安装、使用的过程中，IMU难免会出现测量误差。误差如果不及时修正补偿，会导致机器人位姿确定的准确性降低。
发明内容
为克服相关技术中存在的问题，本公开提供一种机器人位姿确定方法、装置、电子设备及存储介质。
根据本公开实施例的第一方面，提供一种机器人位姿确定方法，包括：
获取所述机器人的惯性测量单元输出的原始数据；
根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度；
对所述关系式求最优解，得到所述第一角度和所述第二角度；
根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
可选地，所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系，包括：
根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵；
根据所述惯性测量单元的本体坐标系与零系之间的偏转角，确定偏转角矩阵；
根据所述第一旋转矩阵以及所述偏转角矩阵，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式，其中，所述第一偏转矩阵为所述世界坐标系相对所述零系绕x轴的偏转矩阵，所述第二偏转矩阵为所述世界坐标系相对所述零系绕y轴的偏转矩阵；
根据所述矩阵关系式，确定所述第一角度对应的与所述第二角度之间的关系式。
可选地，所述原始数据包括原始加速度和原始旋转矩阵，所述原始旋转矩阵为惯性测量单元的本体坐标系与世界坐标系之间的旋转位置关系；
所述根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵，包括：
根据所述原始加速度以及所述机器人在世界坐标系下的重力加速度，确定第二旋转矩阵；
根据所述原始旋转矩阵和所述第二旋转矩阵，确定所述世界坐标系相对所述零系的第一旋转矩阵。
可选地，所述根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，包括：
根据所述第一角度和所述第二角度，对所述第一旋转矩阵进行修正，得到第三旋转矩阵；
根据所述第三旋转矩阵，对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵；
至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，所述原始数据还包括角速度，所述机器人位姿确定方法还包括：
根据所述目标旋转矩阵、所述第三旋转矩阵以及所述角速度，得到所述惯性测量单元的角速度在所述世界坐标系下的目标角速度；
所述至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定，包括：
根据所述目标角速度、所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，在所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式之前，包括：
根据所述世界坐标系相对所述零系在z轴上的偏差角度，确定所述世界坐标系相对所述零系在z轴上的第四旋转矩阵；
获取所述惯性测量单元在测试运行周期内，输出的所述本体坐标系相对述世界坐标系的第五旋转矩阵；
根据所述第四旋转矩阵的反函数以及所述第五旋转矩阵，确定所述惯性测量单元的本体坐标系与零系之间的偏转角。
可选地，所述获取所述机器人的惯性测量单元输出的原始数据，包括：
获取所述惯性测量单元多次输出的加速度；
计算多次所述加速度的均方差；
在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据。
可选地，所述在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据，包括：
在所述均方差小于所述预设均方差阈值的情况下，计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵；以及
计算多次所述加速度的均值，并根据所述均值的模，对所述加速度的均值进行单位化处理，得到所述原始数据中的原始加速度。
可选地，所述计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵，包括：
将所述多次所述加速度对应的旋转矩阵进行欧拉角转换，得到对应的欧拉角的角参量；
计算对应的所述欧拉角的角参量的均值，得到对应所述欧拉角的平均角参量；
根据各所述欧拉角的平均角参量进行矩阵转换，得到所述原始旋转矩阵。
根据本公开实施例的第二方面，提供一种机器人位姿确定装置，包括：
获取模块，被配置为获取所述机器人的惯性测量单元输出的原始数据；
第一确定模块，被配置为根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度；
求解模块，被配置为对所述关系式求最优解，得到所述第一角度和所述第二角度；
第二确定模块，被配置为根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
可选地，所述第一确定模块，包括：
第一确定子模块，被配置为根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵；
第二确定子模块，被配置为根据所述惯性测量单元的本体坐标系与零系之间的偏转角，确定偏转角矩阵；
第三确定子模块，被配置为根据所述第一旋转矩阵以及所述偏转角矩阵，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式，其中，所述第一偏转矩阵为所述世界坐标系相对所述零系绕x轴的偏转矩阵，所述第二偏转矩阵为所述世界坐标系相对所述零系绕y轴的偏转矩阵；
第四确定子模块，被配置为根据所述矩阵关系式，确定所述第一角度对应的与所述第二角度之间的关系式。
可选地，所述原始数据包括原始加速度和原始旋转矩阵，所述原始旋转矩阵为惯性测量单元的本体坐标系与世界坐标系之间的旋转位置关系；
所述第一确定子模块，被配置为：
根据所述原始加速度以及所述机器人在世界坐标系下的重力加速度，确定第二旋转矩阵；
根据所述原始旋转矩阵和所述第二旋转矩阵，确定所述世界坐标系相对所述零系的第一旋转矩阵。
可选地，所述第二确定模块，包括：
第一修正子模块，被配置为根据所述第一角度和所述第二角度，对所述第一旋转矩阵进行修正，得到第三旋转矩阵；
第二修正子模块，被配置为根据所述第三旋转矩阵，对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵；
标定子模块，被配置为至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，所述原始数据还包括角速度，所述机器人位姿确定装置还包括：第三确定模块，被配置为：
根据所述目标旋转矩阵、所述第三旋转矩阵以及所述角速度，得到所述惯性测量单元的角速度在所述世界坐标系下的目标角速度；
所述标定子模块，被配置为根据所述目标角速度、所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，所述机器人位姿确定装置还包括：第四确定模块，被配置为在所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式之前，根据所述世界坐标系相对所述零系在z轴上的偏差角度，确定所述世界坐标系相对所述零系在z轴上的第四旋转矩阵；
获取所述惯性测量单元在测试运行周期内，输出的所述本体坐标系相对述世界坐标系的第五旋转矩阵；
根据所述第四旋转矩阵的反函数以及所述第五旋转矩阵，确定所述惯性测量单元的本体坐标系与零系之间的偏转角。
可选地，所述获取模块，被配置为：
获取所述惯性测量单元多次输出的加速度；
计算多次所述加速度的均方差；
在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据。
可选地，所述获取模块，被配置为：
在所述均方差小于所述预设均方差阈值的情况下，计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵；以及
计算多次所述加速度的均值，并根据所述均值的模，对所述加速度的均值进行单位化处理，得到所述原始数据中的原始加速度。
可选地，所述获取模块，被配置为：
将所述多次所述加速度对应的旋转矩阵进行欧拉角转换，得到对应的欧拉角的角参量；
计算对应的所述欧拉角的角参量的均值，得到对应所述欧拉角的平均角参量；
根据各所述欧拉角的平均角参量进行矩阵转换，得到所述原始旋转矩阵。
根据本公开实施例的第三方面，提供一种电子设备，包括：
第一处理器；
用于存储第一处理器可执行指令的第一存储器；
其中，所述第一处理器被配置为执行所述第一存储器存储的上述可执行指令，以实现第一方面中任一项所述机器人位姿确定方法。
根据本公开实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机程序指令，该程序指令被第二处理器执行时实现第一方面中任一项所述机器人位姿确定方法的步骤。
本公开的实施例提供的技术方案可以包括以下有益效果：
通过获取机器人的惯性测量单元输出的原始数据；根据原始数据、机器人在世界坐标系下的重力加速度以及惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式；对关系式求最优解，得到第一角度和第二角度；根据第一角度和第二角度，对惯性测量单元进行标定，并根据标定后的惯性测量单元，确定机器人在世界坐标系下的位姿。这样，可以通过第一角度与第二角度两个关键参数进行标定，可以提高标定惯性测量单元的准确性，进而提高对机器人位姿确定的准确性，且无需额外工装或者传感器标定惯性测量单元，降低了标定成本。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。
图1是根据一示例性实施例示出的一种机器人位姿确定方法的流程图。
图2是根据一示例性实施例示出的一种机器人各坐标系的关系示意图。
图3是根据一示例性实施例示出的一种实现图1中步骤S12的流程图。
图4是根据一示例性实施例示出的一种实现图3中步骤S121的流程图。
图5是根据一示例性实施例示出的一种实现图1中步骤S14的流程图。
图6是根据一示例性实施例示出的一种实现图1中步骤S11的流程图。
图7是根据一示例性实施例示出的一种实现图6中步骤S113的流程图。
图8是根据一示例性实施例示出的一种机器人位姿确定装置的框图。
图9是根据一示例性实施例示出的一种用于机器人位姿确定的装置的框图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。
需要说明的是，本公开中所有获取信号、信息或数据的动作都是在遵照所在地国家相应的数据保护法规政策的前提下，并获得由相应装置所有者给予授权的情况下进行的。
在介绍本公开的一种机器人位姿确定方法、装置、电子设备及存储介质前，先对相关场景中技术手段存在的技术问题进行说明，通常，在水平状态下机器人上的IMU测量的加速度在竖直方向上的分量应等于重力加速度，而在水平方向上的分量应该为零。如果IMU在水平方向上的分量不为零，则会因为这一微小的加速度，积分出较大的线速度，从而导致机身速度出现偏差，从而导致机器人位姿确定出现偏差。因此，安装在例如足式机器人上的IMU需要进行在线标定，以确保IMU测量的准确性，从而提高机器人位姿确定的准确性。
相关场景中，传统标定方法是将机器人的机身安装在三轴转台上进行标定，将IMU的3个轴分别朝上、朝下放置一段时间，采集六个面的数据作为测量值，然后使用最小二乘方法求解加速度误差模型中的12个参数，根据12个参数对IMU进行标定。但是有些足式机器人的整体尺寸较大（例如人形机器人）或者机器人机身缺少定位平面，难以使用转台进行标定，而且使用转台标定耗时耗力，转台购置价格昂贵，导致成本较高，无法适用于大规模生产。
此外还可以通过以下方法进行标定，将机器人放置在水平面上，获取静置状态下，IMU采集的X、Y、Z三个方向上的加速度和角速度，以及获取机器人在原地旋转过程中，IMU采集的X、Y、Z三个方向上的角速度；进而根据静置状态下对应的加速度和角速度，以及原地旋转过程对应的角速度，建立加速度计与陀螺仪在线标定误差模型，根据该误差模型计算标定参数，通过标定参数，对IMU的输出进行修正标定。然而，该误差模型为线性模型，描述IMU误差没有考虑非线性因素，因此，精度较差。并且，仅在水平面上根据静置状态和旋转状态IMU采集的加速度和角速度进行修正标定，标定结果无法适用于其他工况，例如不适用机器人的IMU在Roll、Pitch、Yaw方向上有较大运动范围的情况。
有鉴于此，本公开提供一种机器人位姿确定方法，旨在提高机器人的惯性测量单元的标定准确性，从提高机器人位姿确定准确性的同时，降低惯性测量单元标定的成本以及适应任何机器人工况下的惯性测量单元标定。
图1是根据一示例性实施例示出的一种机器人位姿确定方法的流程图，如图1所示，该机器人位姿确定方法可以应用于机器人控制器中，包括以下步骤。
在步骤S11中，获取所述机器人的惯性测量单元输出的原始数据。
本公开实施例中，惯性测量单元可以安装在例如双足机器人的躯干位置，用来测量机器人的机身在世界坐标系下的位姿，其中，位姿可以包括位置、加速度、线速度以及角速度中的至少一者。因此，原始数据可以包括旋转矩阵、角速度以及加速度中的至少一者。其中，加速度可以包括线加速度。可以理解的是，原始数据是在惯性测量单元的坐标系下测量得到的，而不是在世界坐标系下测量得到的。
在步骤S12中，根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式。
其中，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度。
本公开实施例中，惯性测量单元可以定义两个坐标系，本体坐标系Bimu和零系Bimu0，其中，本体坐标系Bimu与惯性测量单元刚性连接，通常可以在惯性测量单元上标出，因此，本体坐标系Bimu与机器人机身的本体系B之间的旋转矩阵可以用imuRB表示，机器人机身的本体系B与机器人的机身刚性连接。零系Bimu0与世界坐标系刚性连接。世界坐标系O与惯性测量单元的零系之间的旋转矩阵可以用ORO表示。
参见图2所示，本体坐标系Bimu为z轴正方向向下、x轴正方向向右、y轴正方向向前的空间坐标系。机器人机身的本体系B为z轴正方向向上、x轴正方向向右、y轴正方向向后的空间坐标系。世界坐标系O为z轴正方向向上、x轴正方向向右、y轴正方向向后的空间坐标系。零系Bimu0为z轴正方向向下、x轴正方向和y轴正方向与世界坐标系O的x轴正方向和y轴正方向存在一定偏转角度的空间坐标系。即世界坐标系O在x轴上与零系Bimu0之间的偏差角度即为第一角度，世界坐标系O在y轴上与零系Bimu0之间的偏差角度即为第二角度。
在步骤S13中，对所述关系式求最优解，得到所述第一角度和所述第二角度。
本公开实施例中，第一角度与第二角度之间的关系式为非线性方程，因此将对惯性测量单元在世界坐标系下存在的偏转转换为世界坐标系相对惯性测量单元的零系的偏差角度之间的非线性问题。进而使用非线性求解器，求解非线性方程得到第一角度和第二角度的最优解。
在一种实施方式中，参见图2所示，惯性测量单元的零系是z轴正方向朝下，而世界坐标系的z轴正方向朝上，因此可知第一角度和第二角度的初值为[π 0]，在求解非线性问题时指定初值，不仅可以加快求解速度，而且可以避免求解器在求解过程中陷入局部最优解。
在步骤S14中，根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
本公开实施例中，机器人的状态估计模块根据标定后的惯性测量单元输出的原始数据对机器人的位姿进行估计。可以基于准确的原始数据，得到准确的机器人位姿。
上述技术方案通过获取机器人的惯性测量单元输出的原始数据；根据原始数据、机器人在世界坐标系下的重力加速度以及惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式；对关系式求最优解，得到第一角度和第二角度；根据第一角度和第二角度，对惯性测量单元进行标定，并根据标定后的惯性测量单元，确定机器人在世界坐标系下的位姿。这样，可以通过第一角度与第二角度两个关键参数进行标定，可以提高标定惯性测量单元的准确性，进而提高对机器人位姿确定的准确性，且无需额外工装或者传感器标定惯性测量单元，降低了标定成本。
可选地，参见图3所示，在步骤S12中，所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系，包括：
在步骤S121中，根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵。
本公开实施例中，可以通过如下第一计算式确定第一旋转矩阵：
其中，0Rimu为第一旋转矩阵，α为原始数据的表达，αO为机器人的位姿估计中的额线加速度在世界坐标系下的表达。
在步骤S122中，根据所述惯性测量单元的本体坐标系与零系之间的偏转角，确定偏转角矩阵。
本公开实施例中，在后文具体介绍得到惯性测量单元的本体坐标系与零系之间的偏转角的实施方式，进而可以通过/>表示偏转角矩阵。
在步骤S123中，根据所述第一旋转矩阵以及所述偏转角矩阵，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式。
其中，所述第一偏转矩阵为所述世界坐标系相对所述零系绕x轴的偏转矩阵，所述第二偏转矩阵为所述世界坐标系相对所述零系绕y轴的偏转矩阵。
本公开实施例中，计算所述第一旋转矩阵与所述偏转角矩阵的商，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式，因此，可以参见以下第一辨析式进行计算：
其中，0Rimu0为第一旋转矩阵，为世界坐标系相对所述零系绕x轴的第一偏转矩阵，/>为世界坐标系相对零系绕y轴的第二偏转矩阵。
在步骤S124中，根据所述矩阵关系式，确定所述第一角度对应的与所述第二角度之间的关系式。
本公开实施例中，对矩阵关系式进行求解，可以得出第一角度对应的与所述第二角度之间的关系式如下：
其中，func（ ）表示求解两个角度的关系式。
可选地，所述原始数据包括原始加速度和原始旋转矩阵，所述原始旋转矩阵为惯性测量单元的本体坐标系与世界坐标系之间的旋转位置关系。
本公开实施例中，将第一计算式拆开来可以得到第二计算式：
其中，为原始旋转矩阵。
参见图4所示，在步骤S121中，所述根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵，包括：
在步骤S1211中，根据所述原始加速度以及所述机器人在世界坐标系下的重力加速度，确定第二旋转矩阵。
本公开实施例中，可以计算原始加速度与所述机器人在世界坐标系下的重力加速度的单位重力矢量之间的商，得到第二旋转矩阵。
本公开实施例中，可以将机器人在世界坐标系下的重力加速度表示为：，原始加速度是在机器人处于静止状态下测量得到的，因此，原始加速度的物理意义等同于单位重力矢量。本公开实施例中通过如下辨析式表示原始加速度、机器人在世界坐标系下的重力加速度与第二旋转矩阵的关系：
其中，Rx为第二旋转矩阵，为原始加速度，/>为重力加速度的单位重力矢量表示。
在步骤S1212中，根据所述原始旋转矩阵和所述第二旋转矩阵，确定所述世界坐标系相对所述零系的第一旋转矩阵。
本公开实施例中，可以计算原始旋转矩阵与所述第二旋转矩阵的商，得到世界坐标系相对所述零系的第一旋转矩阵。因此可以通过以下第二辨析式确定第一旋转矩阵：
其中，为原始旋转矩阵，0Rimu0为第一旋转矩阵。
可选地，参见图5所示，在步骤S14中，所述根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，包括：
在步骤S141中，根据所述第一角度和所述第二角度，对所述第一旋转矩阵进行修正，得到第三旋转矩阵。
本公开实施例中，可以将第一角度和第二角度代回上述第一辨析式，对第一旋转矩阵0Rimu0进行修正，确定第三旋转矩阵。
在步骤S142中，根据所述第三旋转矩阵，对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵。
本公开实施例中，可以将第三旋转矩阵代回上述第二辨析式，可以对对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵。
在步骤S143中，至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
本公开实施例中，可以将目标加速度与原始的加速度进行比较，可以得到加速度误差，以及将原始旋转矩阵与目标旋转矩阵进行比较，可以得到旋转矩阵误差，进而根据加速度误差和旋转矩阵误差对惯性测量单元进行标定。进一步地，标定加速度和旋转矩阵的惯性测量单元用于实时测量机器人的位姿。
可选地，所述原始数据还包括角速度，其中，原始数据中的角速度为惯性测量单元的本体坐标系中测量得到的角速度，所述机器人位姿确定方法还包括：
根据所述目标旋转矩阵、所述第三旋转矩阵以及所述角速度，得到所述惯性测量单元的角速度在所述世界坐标系下的目标角速度。
可以理解的是，目标角速度是惯性测量单元在本体坐标系中的角速度转换到世界坐标系下的角速度。
本公开实施例中，可以计算目标旋转矩阵、第三旋转矩阵以及角速度的乘积，得到惯性测量单元的角速度在所述世界坐标系下的目标角速度。可以参见如下计算公式确定惯性测量单元的角速度在所述世界坐标系下的目标角速度：
其中，为惯性测量单元的角速度在所述世界坐标系下的目标角速度，/>为原始数据中的角速度，imu0Rimu为目标旋转矩阵，/>为第三旋转矩阵。
在步骤S143中，所述至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定，包括：
根据所述目标角速度、所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可以理解的是，惯性测量单元输出的原始数据中包括原始的角速度、原始的加速度和原始旋转矩阵，可以将目标角速度与原始的角速度进行比较，可以得到角速度误差。将目标加速度与原始的加速度进行比较，可以得到加速度误差。以及将原始旋转矩阵与目标旋转矩阵进行比较，可以得到旋转矩阵误差，进而根据角速度误差、加速度误差和旋转矩阵误差对惯性测量单元进行标定。进一步地，标定角速度、加速度和旋转矩阵的惯性测量单元用于实时测量机器人的位姿。
可选地，在所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式之前，包括：
根据所述世界坐标系相对所述零系在z轴上的偏差角度，确定所述世界坐标系相对所述零系在z轴上的第四旋转矩阵。
本公开的一种实施例中，本体坐标系Bimu与零系Bimu0之间的相对关系为惯性测量单元测量的原始数据中的原始旋转矩阵，由于惯性测量单元是安装在机器人的机身上，机身与地面可能存在一定的角度，因此，世界坐标系相对零系在Z轴上可能存在偏差角度，因此根据该偏差角度可以根据三角函数关系，得到世界坐标系相对所述零系在z轴上的第四旋转矩阵。
获取所述惯性测量单元在测试运行周期内，输出的所述本体坐标系相对述世界坐标系的第五旋转矩阵。
本公开的一种实施例中，由于在启动时不确定机器人的朝向，因此需要将机器人在测试运行周期内进行运行，从而通过惯性测量单元测量从测试运行周期开始到测试运行周期结束时，机器人本体坐标系相对述世界坐标系的第五旋转矩阵。
根据所述第四旋转矩阵的反函数以及所述第五旋转矩阵，确定所述惯性测量单元的本体坐标系与零系之间的偏转角。
本公开实施例中，通过以下公式确定惯性测量单元的本体坐标系与零系之间的偏转角：
其中，为惯性测量单元的本体坐标系与零系之间的偏转角，Rz为第四旋转矩阵，R-1 z为第四旋转矩阵的反函数，/>为第五旋转矩阵。
可选地，参见图6所示，在步骤S11中，所述获取所述机器人的惯性测量单元输出的原始数据，包括：
在步骤S111中，获取所述惯性测量单元多次输出的加速度。
本公开实施例中，可以在机器人处于静止状态下，获取所述惯性测量单元多次输出的加速度，由于，无法确定机器人是否绝对静止状态，因此需要获取惯性测量单元多次输出的加速度。
本公开实施例中，可以通过加速度次数，获取所述惯性测量单元多次输出的加速度。示例地，多次加速度的数量可以是预设的，即获取当前最后一次测量到的加速度，以及最后一次加速度之前的N次加速度，其中N为预设加速度次数。
本公开实施例中，可以通过时间段，获取所述惯性测量单元多次输出的加速度。示例地，获取加速度的时间段可以是预设的，即获取当前最后一次测量到的加速度，以及最后一次加速度之前的M时间段内的多次加速度，其中M为预设时间段。
在步骤S112中，计算多次所述加速度的均方差。
本公开实施例中，先计算多次加速度的均值，再根据加速度的均值和每一次测量的加速度的取值，计算加速度的均方差。
在步骤S113中，在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据。
本公开实施例中，如果惯性测量单元测量的加速度在一段时间内的均方差小于预设均方差阈值，则可以确定机器人处于静止状态。避免用一次或者几次的加速度求均值确定机器人是否处于静止状态引起的误差和不准确。
可选地，参见图7所示，在步骤S113中，所述在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据，包括：
在步骤S1131中，在所述均方差小于所述预设均方差阈值的情况下，计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵。
可以理解的是，若参与计算的加速度对应的均方差小于预设均方差阈值，则计算对应该加速度的旋转矩阵的均值，得到原始数据中的原始旋转矩阵，其中，该原始旋转矩阵可以用表示。
在步骤S1132中，计算多次所述加速度的均值，并根据所述均值的模，对所述加速度的均值进行单位化处理，得到所述原始数据中的原始加速度。
本公开实施中，计算多次加速度的均值，能够准确获取到机器人在静止状态下惯性测量单元获取到的加速度。
在一种实施方式中，根据如下公式计算原始数据中的原始加速度：
其中，为原始数据中的原始加速度，/>为多次加速度的均值，|| ||为求模符号。
通过上述单位化处理可以使得不同经纬度下的机器人能够统一加速度。进而提高惯性测量单元标定的准确性。
进一步地，可以得到第一角度与第二角度的非线性方程为：
上述实施例中各参数均可以与前述实施例中确定物理含义，此处不做赘述。并且，还可以根据前述实施例中使用非线性求解器，通过如下公式在指定初值的条件下求解上述非线性方程，得到第一角度和第二角度的最优解。
其中，为在执行初值的条件下，最优解的第一角度，/>为在执行初值的条件下，最优解的第二角度，fsolve( )为求最优解函数方程。
可选地，在步骤S1132中，所述计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵，包括：
将所述多次所述加速度对应的旋转矩阵进行欧拉角转换，得到对应的欧拉角的角参量。
本公开实施例中，将每一次加速度对应的旋转矩阵分别进行欧拉角转换，得到每一次加速度对应的章动角、旋进角和自转角的角参量，其中，角参量可以理解为具体角度大小。
计算对应的所述欧拉角的角参量的均值，得到对应所述欧拉角的平均角参量。
本公开实施例中，计算多次加速度对应的章动角的均值，得到章动角平均角参量，计算多次加速度对应的旋进角的均值，得到旋进角平均角参量，以及计算多次加速度对应的自转角的均值，得到自转角平均角参量。
根据各所述欧拉角的平均角参量进行矩阵转换，得到所述原始旋转矩阵。
本公开实施例中，根据章动角平均角参量、旋进角平均角参量和自转角平均角参量进行矩阵转换，得到所述原始旋转矩阵。
上述技术方案，通过将旋转矩阵进行欧拉角转换得到对应的欧拉角的角参量，再求取各欧拉角的角参量的均值，最后将平均角参量进行矩阵转换得到原始旋转矩阵，旋转矩阵内角度对应的三角函数的平方和为1，提高计算原始旋转矩阵的准确性。
本公开实施例还提供一种机器人位姿确定装置，参见图8所示，该机器人位姿确定装置包括：获取模块810、第一确定模块820、求解模块830和第二确定模块840。
其中，该获取模块810，被配置为获取所述机器人的惯性测量单元输出的原始数据；
该第一确定模块820，被配置为根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度；
该求解模块830，被配置为对所述关系式求最优解，得到所述第一角度和所述第二角度；
该第二确定模块840，被配置为根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
可选地，所述第一确定模块820，包括：
第一确定子模块，被配置为根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵；
第二确定子模块，被配置为根据所述惯性测量单元的本体坐标系与零系之间的偏转角，确定偏转角矩阵；
第三确定子模块，被配置为根据所述第一旋转矩阵以及所述偏转角矩阵，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式，其中，所述第一偏转矩阵为所述世界坐标系相对所述零系绕x轴的偏转矩阵，所述第二偏转矩阵为所述世界坐标系相对所述零系绕y轴的偏转矩阵；
第四确定子模块，被配置为根据所述矩阵关系式，确定所述第一角度对应的与所述第二角度之间的关系式。
可选地，所述原始数据包括原始加速度和原始旋转矩阵，所述原始旋转矩阵为惯性测量单元的本体坐标系与世界坐标系之间的旋转位置关系；
所述第一确定子模块，被配置为：
根据所述原始加速度以及所述机器人在世界坐标系下的重力加速度，确定第二旋转矩阵；
根据所述原始旋转矩阵和所述第二旋转矩阵，确定所述世界坐标系相对所述零系的第一旋转矩阵。
可选地，所述第二确定模块840，包括：
第一修正子模块，被配置为根据所述第一角度和所述第二角度，对所述第一旋转矩阵进行修正，得到第三旋转矩阵；
第二修正子模块，被配置为根据所述第三旋转矩阵，对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵；
标定子模块，被配置为至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，所述原始数据还包括角速度，所述机器人位姿确定装置还包括：第三确定模块，被配置为：
根据所述目标旋转矩阵、所述第三旋转矩阵以及所述角速度，得到所述惯性测量单元的角速度在所述世界坐标系下的目标角速度；
所述标定子模块，被配置为根据所述目标角速度、所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
可选地，所述机器人位姿确定装置还包括：第四确定模块，被配置为在所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式之前，根据所述世界坐标系相对所述零系在z轴上的偏差角度，确定所述世界坐标系相对所述零系在z轴上的第四旋转矩阵；
获取所述惯性测量单元在测试运行周期内，输出的所述本体坐标系相对述世界坐标系的第五旋转矩阵；
根据所述第四旋转矩阵的反函数以及所述第五旋转矩阵，确定所述惯性测量单元的本体坐标系与零系之间的偏转角。
可选地，所述获取模块810，被配置为：
获取所述惯性测量单元多次输出的加速度；
计算多次所述加速度的均方差；
在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据。
可选地，所述获取模块810，被配置为：
在所述均方差小于所述预设均方差阈值的情况下，计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵；
计算多次所述加速度的均值，并根据所述均值的模，对所述加速度的均值进行单位化处理，得到所述原始数据中的原始加速度。
可选地，所述获取模块810，被配置为：
将所述多次所述加速度对应的旋转矩阵进行欧拉角转换，得到对应的欧拉角的角参量；
计算对应的所述欧拉角的角参量的均值，得到对应所述欧拉角的平均角参量；
根据各所述欧拉角的平均角参量进行矩阵转换，得到所述原始旋转矩阵。
关于上述实施例中的机器人位姿确定装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
本公开实施例还提供一种电子设备，包括：
第一处理器；
用于存储第一处理器可执行指令的第一存储器；
其中，所述第一处理器被配置为执行所述第一存储器存储的上述可执行指令，以实现前述实施例中任一项所述机器人位姿确定方法。
本公开实施例还提供一种计算机可读存储介质，其上存储有计算机程序指令，该程序指令被第二处理器执行时实现前述实施例中任一项所述机器人位姿确定方法的步骤。
图9是根据一示例性实施例示出的一种用于确定机器人位姿的装置900的框图。例如，装置900可以被配置为一机器人控制器。
参照图9，装置900可以包括以下一个或多个组件：处理组件902，第二存储器904，电源组件906，多媒体组件908，音频组件910，输入/输出接口912，传感器组件914，以及通信组件916。
处理组件902通常控制装置900的整体操作，诸如与显示，电话呼叫，数据通信，相机操作和记录操作相关联的操作。处理组件902可以包括一个或多个第三处理器920来执行指令，以完成上述的方法的全部或部分步骤。此外，处理组件902可以包括一个或多个模块，便于处理组件902和其他组件之间的交互。例如，处理组件902可以包括多媒体模块，以方便多媒体组件908和处理组件902之间的交互。
第二存储器904被配置为存储各种类型的数据以支持在装置900的操作。这些数据的示例包括用于在装置900上操作的任何应用程序或方法的指令，联系人数据，电话簿数据，消息，图片，视频等。第二存储器904可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（SRAM），电可擦除可编程只读存储器（EEPROM），可擦除可编程只读存储器（EPROM），可编程只读存储器（PROM），只读存储器（ROM），磁存储器，快闪存储器，磁盘或光盘。
电源组件906为装置900的各种组件提供电力。电源组件906可以包括电源管理系统，一个或多个电源，及其他与为装置900生成、管理和分配电力相关联的组件。
多媒体组件908包括在所述装置900和用户之间的提供一个输出接口的屏幕。在一些实施例中，屏幕可以包括液晶显示器（LCD）和触摸面板（TP）。如果屏幕包括触摸面板，屏幕可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。在一些实施例中，多媒体组件908包括一个前置摄像头和/或后置摄像头。当装置900处于操作模式，如拍摄模式或视频模式时，前置摄像头和/或后置摄像头可以接收外部的多媒体数据。每个前置摄像头和后置摄像头可以是一个固定的光学透镜系统或具有焦距和光学变焦能力。
音频组件910被配置为输出和/或输入音频信号。例如，音频组件910包括一个麦克风（MIC），当装置900处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在第二存储器904或经由通信组件916发送。在一些实施例中，音频组件910还包括一个扬声器，用于输出音频信号。
输入/输出接口912为处理组件902和外围接口模块之间提供接口，上述外围接口模块可以是键盘，点击轮，按钮等。这些按钮可包括但不限于：主页按钮、音量按钮、启动按钮和锁定按钮。
传感器组件914包括一个或多个传感器，用于为装置900提供各个方面的状态评估。例如，传感器组件914可以检测到装置900的打开/关闭状态，组件的相对定位，例如所述组件为装置900的显示器和小键盘，传感器组件914还可以检测装置900或装置900一个组件的位置改变，用户与装置900接触的存在或不存在，装置900方位或加速/减速和装置900的温度变化。传感器组件914可以包括接近传感器，被配置用来在没有任何的物理接触时检测附近物体的存在。传感器组件914还可以包括光传感器，如CMOS或CCD图像传感器，用于在成像应用中使用。在一些实施例中，该传感器组件914还可以包括加速度传感器，陀螺仪传感器，磁传感器，压力传感器或温度传感器。
通信组件916被配置为便于装置900和其他设备之间有线或无线方式的通信。装置900可以接入基于通信标准的无线网络，如WiFi，2G或3G，或它们的组合。在一个示例性实施例中，通信组件916经由广播信道接收来自外部广播管理系统的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件916还包括近场通信（NFC）模块，以促进短程通信。例如，在NFC模块可基于射频识别（RFID）技术，红外数据协会（IrDA）技术，超宽带（UWB）技术，蓝牙（BT）技术和其他技术来实现。
在示例性实施例中，装置900可以被一个或多个应用专用集成电路（ASIC）、数字信号处理器（DSP）、数字信号处理设备（DSPD）、可编程逻辑器件（PLD）、现场可编程门阵列（FPGA）、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述机器人位姿确定方法。
在示例性实施例中，还提供了一种包括指令的非临时性计算机可读存储介质，例如包括指令的第二存储器904，上述指令可由装置900的第三处理器920执行以完成上述机器人位姿确定方法。例如，所述非临时性计算机可读存储介质可以是ROM、随机存取存储器（RAM）、CD-ROM、磁带、软盘和光数据存储设备等。
本领域技术人员在考虑说明书及实践本公开后，将容易想到本公开的其它实施方案。本公开旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由下面的权利要求指出。
应当理解的是，本公开并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本公开的范围仅由所附的权利要求来限制。
Claims (12)
1.一种机器人位姿确定方法，其特征在于，包括：
获取所述机器人的惯性测量单元输出的原始数据；
根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度，其中，所述本体坐标系与所述惯性测量单元刚性连接，所述零系与所述世界坐标系刚性连接，所述本体坐标系为z轴正方向向下、x轴正方向向右、y轴正方向向前的空间坐标系，所述世界坐标系为z轴正方向向上、x轴正方向向右、y轴正方向向后的空间坐标系，所述零系为z轴正方向向下、x轴正方向和y轴正方向与所述世界坐标系的x轴正方向和y轴正方向存在偏转角度的空间坐标系；
对所述关系式求最优解，得到所述第一角度和所述第二角度；
根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
2.根据权利要求1所述的机器人位姿确定方法，其特征在于，所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系，包括：
根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵；
根据所述惯性测量单元的本体坐标系与零系之间的偏转角，确定偏转角矩阵；
根据所述第一旋转矩阵以及所述偏转角矩阵，确定第一偏转矩阵与第二偏转矩阵的矩阵关系式，其中，所述第一偏转矩阵为所述世界坐标系相对所述零系绕x轴的偏转矩阵，所述第二偏转矩阵为所述世界坐标系相对所述零系绕y轴的偏转矩阵；
根据所述矩阵关系式，确定所述第一角度对应的与所述第二角度之间的关系式。
3.根据权利要求2所述的机器人位姿确定方法，其特征在于，所述原始数据包括原始加速度和原始旋转矩阵，所述原始旋转矩阵为惯性测量单元的本体坐标系与世界坐标系之间的旋转位置关系；
所述根据所述原始数据以及所述机器人在世界坐标系下的重力加速度，确定所述世界坐标系相对所述零系的第一旋转矩阵，包括：
根据所述原始加速度以及所述机器人在世界坐标系下的重力加速度，确定第二旋转矩阵；
根据所述原始旋转矩阵和所述第二旋转矩阵，确定所述世界坐标系相对所述零系的第一旋转矩阵。
4.根据权利要求3所述的机器人位姿确定方法，其特征在于，所述根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，包括：
根据所述第一角度和所述第二角度，对所述第一旋转矩阵进行修正，得到第三旋转矩阵；
根据所述第三旋转矩阵，对所述原始加速度和原始旋转矩阵进行修正，得到目标加速度和目标旋转矩阵；
至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
5.根据权利要求4所述的机器人位姿确定方法，其特征在于，所述原始数据还包括角速度，所述机器人位姿确定方法还包括：
根据所述目标旋转矩阵、所述第三旋转矩阵以及所述角速度，得到所述惯性测量单元的角速度在所述世界坐标系下的目标角速度；
所述至少根据所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定，包括：
根据所述目标角速度、所述目标加速度和所述目标旋转矩阵，对所述惯性测量单元进行标定。
6.根据权利要求1所述的机器人位姿确定方法，其特征在于，在所述根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式之前，包括：
根据所述世界坐标系相对所述零系在z轴上的偏差角度，确定所述世界坐标系相对所述零系在z轴上的第四旋转矩阵；
获取所述惯性测量单元在测试运行周期内，输出的所述本体坐标系相对述世界坐标系的第五旋转矩阵；
根据所述第四旋转矩阵的反函数以及所述第五旋转矩阵，确定所述惯性测量单元的本体坐标系与零系之间的偏转角。
7.根据权利要求1-6中任一项所述的机器人位姿确定方法，其特征在于，所述获取所述机器人的惯性测量单元输出的原始数据，包括：
获取所述惯性测量单元多次输出的加速度；
计算多次所述加速度的均方差；
在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据。
8.根据权利要求7所述的机器人位姿确定方法，其特征在于，所述在所述均方差小于预设均方差阈值的情况下，获取所述机器人的惯性测量单元输出的原始数据，包括：
在所述均方差小于所述预设均方差阈值的情况下，计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵；以及
计算多次所述加速度的均值，并根据所述均值的模，对所述加速度的均值进行单位化处理，得到所述原始数据中的原始加速度。
9.根据权利要求8所述的机器人位姿确定方法，其特征在于，所述计算多次所述加速度对应的旋转矩阵均值，得到所述原始数据中的原始旋转矩阵，包括：
将所述多次所述加速度对应的旋转矩阵进行欧拉角转换，得到对应的欧拉角的角参量；
计算对应的所述欧拉角的角参量的均值，得到对应所述欧拉角的平均角参量；
根据各所述欧拉角的平均角参量进行矩阵转换，得到所述原始旋转矩阵。
10.一种机器人位姿确定装置，其特征在于，包括：
获取模块，被配置为获取所述机器人的惯性测量单元输出的原始数据；
第一确定模块，被配置为根据所述原始数据、所述机器人在世界坐标系下的重力加速度以及所述惯性测量单元的本体坐标系与零系之间的偏转角，确定第一角度与第二角度之间的关系式，所述第一角度为所述世界坐标系相对所述零系绕x轴的偏差角度，所述第二角度为所述世界坐标系相对所述零系绕y轴的偏差角度，其中，所述本体坐标系与所述惯性测量单元刚性连接，所述零系与所述世界坐标系刚性连接，所述本体坐标系为z轴正方向向下、x轴正方向向右、y轴正方向向前的空间坐标系，所述世界坐标系为z轴正方向向上、x轴正方向向右、y轴正方向向后的空间坐标系，所述零系为z轴正方向向下、x轴正方向和y轴正方向与所述世界坐标系的x轴正方向和y轴正方向存在偏转角度的空间坐标系；
求解模块，被配置为对所述关系式求最优解，得到所述第一角度和所述第二角度；
第二确定模块，被配置为根据所述第一角度和所述第二角度，对所述惯性测量单元进行标定，并根据标定后的惯性测量单元，确定所述机器人在所述世界坐标系下的位姿。
11.一种电子设备，其特征在于，包括：
第一处理器；
用于存储第一处理器可执行指令的第一存储器；
其中，所述第一处理器被配置为执行所述第一存储器存储的上述可执行指令，以实现权利要求1-9中任一项所述机器人位姿确定方法。
12.一种计算机可读存储介质，其上存储有计算机程序指令，其特征在于，该程序指令被第二处理器执行时实现权利要求1-9中任一项所述机器人位姿确定方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311576579.1A CN117288187B (zh) | 2023-11-23 | 2023-11-23 | 机器人位姿确定方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311576579.1A CN117288187B (zh) | 2023-11-23 | 2023-11-23 | 机器人位姿确定方法、装置、电子设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117288187A CN117288187A (zh) | 2023-12-26 |
| CN117288187B true CN117288187B (zh) | 2024-02-23 |
Family
ID=89253868
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311576579.1A Active CN117288187B (zh) | 2023-11-23 | 2023-11-23 | 机器人位姿确定方法、装置、电子设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117288187B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117901100B (zh) * | 2024-01-18 | 2024-08-30 | 广州深度医疗器械科技有限公司 | 一种基于操作器的机器人控制方法及其系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225328A (zh) * | 2017-12-29 | 2018-06-29 | 北京领航视觉科技有限公司 | 一种室内三维数据采集方法 |
| WO2020123988A1 (en) * | 2018-12-13 | 2020-06-18 | Solitonreach, Inc. | System and method for motion based alignment of body parts |
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN114719843A (zh) * | 2022-06-09 | 2022-07-08 | 长沙金维信息技术有限公司 | 复杂环境下的高精度定位方法 |
| CN115063480A (zh) * | 2022-06-24 | 2022-09-16 | 咪咕动漫有限公司 | 位姿确定方法、装置、电子设备和可读存储介质 |
| CN115388884A (zh) * | 2022-08-17 | 2022-11-25 | 南京航空航天大学 | 一种智能体位姿估计器联合初始化方法 |
| CN116721166A (zh) * | 2023-06-09 | 2023-09-08 | 江苏集萃清联智控科技有限公司 | 双目相机和imu旋转外参在线标定方法、装置及存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112188037B (zh) * | 2020-09-24 | 2023-03-24 | 影石创新科技股份有限公司 | 生成陀螺仪旋转方向的方法及计算机设备 |
-
2023
- 2023-11-23 CN CN202311576579.1A patent/CN117288187B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225328A (zh) * | 2017-12-29 | 2018-06-29 | 北京领航视觉科技有限公司 | 一种室内三维数据采集方法 |
| WO2020123988A1 (en) * | 2018-12-13 | 2020-06-18 | Solitonreach, Inc. | System and method for motion based alignment of body parts |
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN114719843A (zh) * | 2022-06-09 | 2022-07-08 | 长沙金维信息技术有限公司 | 复杂环境下的高精度定位方法 |
| CN115063480A (zh) * | 2022-06-24 | 2022-09-16 | 咪咕动漫有限公司 | 位姿确定方法、装置、电子设备和可读存储介质 |
| CN115388884A (zh) * | 2022-08-17 | 2022-11-25 | 南京航空航天大学 | 一种智能体位姿估计器联合初始化方法 |
| CN116721166A (zh) * | 2023-06-09 | 2023-09-08 | 江苏集萃清联智控科技有限公司 | 双目相机和imu旋转外参在线标定方法、装置及存储介质 |
Non-Patent Citations (2)
| Title |
|---|
| 基于视觉和惯性器件的相机姿态估计与目标定位研究;刘泽伟;中国博士学位论文全文数据库 信息科技辑(第1期);I138-123 * |
| 融合语义实例重建的抓取位姿估计方法;韩慧妍;计算机工程与科学;第45卷(第10期);1822-1828 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117288187A (zh) | 2023-12-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN117288187B (zh) | 机器人位姿确定方法、装置、电子设备及存储介质 | |
| CN116079697B (zh) | 一种基于图像的单目视觉伺服方法、装置、设备及介质 | |
| CN112414400B (zh) | 一种信息处理方法、装置、电子设备和存储介质 | |
| JP2017056546A (ja) | ロボットの機構パラメータを校正するために使用される計測装置 | |
| CN113474819A (zh) | 信息处理装置、信息处理方法和程序 | |
| CN111443337A (zh) | 一种基于手眼标定的雷达-imu标定方法 | |
| CN114578329A (zh) | 多传感器联合标定方法、设备、存储介质及程序产品 | |
| TWI387775B (zh) | 定位系統與定位方法 | |
| CN112818898B (zh) | 模型训练方法、装置和电子设备 | |
| CN114290338B (zh) | 二维手眼标定方法、设备、存储介质及程序产品 | |
| CN115655249A (zh) | 指南针校准方法及装置、电子设备和可读存储介质 | |
| CN112629565A (zh) | 像机与惯性测量单元旋转关系校准方法、装置和设备 | |
| CN111275769B (zh) | 一种单目视觉参数的校正方法及装置 | |
| CN115550619A (zh) | 投影校正方法、装置、电子设备及可读存储介质 | |
| CN112148139B (zh) | 一种姿态识别方法和计算机可读存储介质 | |
| CN114765663A (zh) | 防抖处理方法、装置、移动设备及存储介质 | |
| CN110806203A (zh) | 车辆的位置决定方法及系统 | |
| CN118408572A (zh) | 惯性测量单元的标定方法及装置、电子设备、存储介质 | |
| KR101637241B1 (ko) | 포인팅 디바이스 및 포인터 제어 방법 | |
| CN112659115B (zh) | 工业机器人控制方法、装置、设备及存储介质 | |
| CN111625755B (zh) | 数据处理方法、装置、服务器、终端和可读存储介质 | |
| US20240085453A1 (en) | Method for evaluating sensor data, processing unit for evaluating sensor data, and sensor system | |
| CN118583131A (zh) | 基于分布式云台相机的位姿估计系统、方法、设备及介质 | |
| CN114460977A (zh) | 舵机角度校正方法及装置、电子设备和存储介质 | |
| CN116026320A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |