CN114789439A - 斜坡定位校正方法、装置、机器人及可读存储介质 - Google Patents
斜坡定位校正方法、装置、机器人及可读存储介质 Download PDFInfo
- Publication number
- CN114789439A CN114789439A CN202110104694.3A CN202110104694A CN114789439A CN 114789439 A CN114789439 A CN 114789439A CN 202110104694 A CN202110104694 A CN 202110104694A CN 114789439 A CN114789439 A CN 114789439A
- Authority
- CN
- China
- Prior art keywords
- robot
- slope
- pose
- positioning
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000012937 correction Methods 0.000 title claims abstract description 41
- 238000003860 storage Methods 0.000 title claims abstract description 8
- 238000005259 measurement Methods 0.000 claims abstract description 32
- 239000011159 matrix material Substances 0.000 claims description 48
- 230000001133 acceleration Effects 0.000 claims description 36
- 230000007704 transition Effects 0.000 claims description 13
- 238000001514 detection method Methods 0.000 claims description 10
- 230000033001 locomotion Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 abstract description 34
- 235000013305 food Nutrition 0.000 description 13
- 238000004891 communication Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 8
- 230000007613 environmental effect Effects 0.000 description 7
- 235000012054 meals Nutrition 0.000 description 4
- 239000000126 substance Substances 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000003550 marker Substances 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000001174 ascending effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004927 fusion Effects 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
- 230000008447 perception Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
- B25J9/1692—Calibration of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种斜坡定位校正方法，所述方法利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。本发明还提供了一种斜坡定位校正装置、机器人及非易失性计算机可读存储介质。
Description
技术领域
本发明涉及机器人定位领域，尤其涉及一种斜坡定位校正方法、装置、机器人及可读存储介质。
背景技术
随着机器人相关技术的快速发展，人们对机器人的需求也越来越高。轮式移动机器人是依靠滚轮实现移动来自动执行工作，它既可以接受人类指挥，也可以运行预先编排的程序，也可以根据以人工智能技术制定的原则来行动。它的任务是协助或取代人类的工作，例如送餐、收发快递等。
轮式移动机器人的运行依赖于其工作地点的精确地图定位，基于视觉和轮式里程编码器相结合的室内定位技术在机器人领域应用越来越广泛，这种定位方式首先利用相机提取图像特征，进行特征匹配后计算机器人位姿变化，再利用轮式里程计对编码器进行积分得到机器人的位姿变化，最后对两者得到的结果进行融合得到机器人的最终位姿，但是由于相机和轮式里程编码器自身的局限性无法解决大型室内复杂场景下，例如上下斜坡、剧烈颠簸带来的定位跳变引起的定位误差。而这种定位误差在对室内这种定位精度要求较高的场景下，不可避免的会对机器人避障、规划和决策带来影响。
当前技术上无法避免这种室内斜坡环境导致的定位跳变，从而导致定位误差。当前一般有两种解决方案：第一种是在建图时设立斜坡元素，在定位至斜坡位置时直接对定位进行补偿，但是由于机器人在斜坡上也会进行旋转和多方向的运动，就导致这种不足够灵活的补偿方案会使得机器人定位不够精确；第二种方案是在建图时在斜坡处设定障碍物(虚拟墙)，使得机器人在定位和规划时就不会经过斜坡，从而避免由于上下斜坡带来的定位误差，但其限制了机器人的运行边界。
上述现有技术中的两种方案都避免或者减少了由于斜坡带来的定位误差，但是都或多或少有着些许限制和不足，并且无法处理机体剧烈颠簸或者抖动带来的定位误差。
当前的送餐服务机器人的定位方案主要利用Marker(对机器人运行起到引导作用的的标识)与里程计相结合的方案来进行的，由于Marker的定位方案无法判定机器人在运动过程中的位姿变化是由于平面上的运动还是上下斜坡带来的视角变化引起的，因此如果不采用合适的定位校正方案来处理上下斜坡引起的定位跳变，就会导致最终定位结果出现偏差，定位上的偏差会导致机器人的感知、规划和决策等功能出现一系列问题。因此就需要一种鲁棒性高、实时性快、精度高的补偿方案来对斜坡环境导致的定位误差进行补偿和修正。
发明内容
本发明旨在至少在一定程度上解决上述技术问题之一。为此，本发明提出了一种斜坡定位校正方法，所述方法利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。
根据本发明的斜坡定位校正方法，应用于一机器人，所述方法包括以下步骤：
利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿；
通过所述机器人的位姿判断所述预定区域内是否有斜坡；
若判断所述预定区域内有斜坡，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
根据本发明的斜坡定位校正方法，所述方法利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。
另外，根据本发明上述的斜坡定位校正方法，还可以具有如下附加的技术特征：
所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤包括：
利用所述惯性测量元件测量所述机器人的三轴线加速度、三轴角速度，通过所述三轴线加速度构成的第一欧拉角反旋转矩阵以及所述三轴角速度构成的第二欧拉角反旋转矩阵来计算出所述机器人的位姿。
所述机器人的位姿状态方程为：
其中，
所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤还包括：
通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿，其中，利用先验估计的方法计算所述机器人的位姿的公式为：
其中，
所述通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿的步骤之后，还包括以下步骤：
通过测量值来对修正所述机器人的位姿的先验估计值。
所述通过所述机器人的位姿判断所述预定区域内是否有斜坡；若有，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差的具体步骤包括：
通过所述机器人的位姿中的欧拉角来判断所述预定区域内是否有斜坡，若所述欧拉角的俯仰角和翻滚角在机体坐标系下有数值，则判定该预定区域内有斜坡；若所述欧拉角的俯仰角和翻滚角在机体坐标下为0，则判断该预定区域内没有斜坡；
若判定该预定区域内有斜坡，则利用该斜坡处对应的位姿的俯仰角和翻滚角的数值来补偿所述斜坡处的定位误差。
所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据的步骤之后，还包括以下步骤：
对所惯性测量元件测量的定位数据进行窗品滑动滤波处理，其中，所述惯性测量元件包括用于测量角速度的角运动检测装置以及用于测量线加速度的加速度检测装置。
本发明还提供了一种斜坡定位校正装置，应用于一机器人，所述装置包括：位姿计算单元，用于利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿；
斜坡判断单元，用于通过所述机器人的位姿判断所述预定区域内是否有斜坡；以及
定位误差补偿单元，用于在所述斜坡判断单元判断所述预定区域内有斜坡时，利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
根据本发明的斜坡定位校正装置，所述装置利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。
本发明还提供了一种机器人，所述机器人包括一个或多个处理器、存储器；和一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被所述一个或多个处理器执行，所述程序包括用于执行上述任意的斜坡定位校正方法的指令。
本发明还提供了一种包含计算机程序的非易失性计算机可读存储介质，当所述计算机程序被一个或多个处理器执行时，使得所述处理器执行上述任意的斜坡定位校正方法。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1是本发明的一个实施例的斜坡定位校正方法的流程图；
图2是本发明的一个实施例的斜坡定位校正装置的结构框图；
图3是本发明的一个实施例的机器人的模块示意图；以及
图4是本发明的一个实施例的计算机可读存储介质与处理器的连接关系示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
本发明提供了一种斜坡定位校正方法，所述方法相对于传统的人为测量斜坡角度或者人为设置虚拟墙的方法，实时性好、鲁棒性高、校正精度高、方便快捷。
图1是本发明的一个实施例的斜坡定位校正方法的流程图。参考图1，本发明提供了一种斜坡定位校正方法，所述方法应用于一机器人，用于对机器人在运行过程中由于斜坡环境导致的定位误差进行补偿和修正。在本实施例中，所述机器人是以一室内运行的轮式送餐机器人为例，在其他实施例中，所述机器人也可以是其他的机器人，例如从事生产业的工业用机器人等。
所述斜坡定位校正方法包括以下步骤：
S10、利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿。
具体的，本发明的送餐机器人，该送餐机器人内部集成设置有一惯性测量元件，所述惯性测量元件至少包括用于测量角速度的角运动检测装置以及用于测量线加速度的加速度检测装置。在本实施例中，所述角运动检测装置是以一陀螺仪为例，所述加速度检测装置是以一加速度计为例；在其他实施例中，所述角运动检测装置、所述加速度检测装置也可以是其他能分别起到测量机器人角速度、线加速度的其他测量仪器。本实施例的陀螺仪和加速度计均集成设置在所述惯性测量元件中，其中，陀螺仪用于测量机器人运行过程中的每个位置处的角速度及角加速度值，加速度计用于测量机器人运行过程中的每个位置处的线加速度值。
本发明的送餐机器人是在餐厅内进行运行工作，其在餐厅内运行依赖于餐厅内的路径规划地图。送餐机器人在首次工作之前，或者餐厅的摆设出现变化时均需要重新规划地图，优选的，本发明的送餐机器人上设有SLAM(Simultaneous Localization andMapping；即时定位与地图构建)***，该SLAM可以实现机器人在餐厅中一边移动一边逐步描绘出此环境的地图，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，从而不受障碍行进到餐厅中可进入的每个位置，以实现该送餐机器人的自主定位和导航。
本发明的机器人在运行过程中，通过其惯性测量元件可以测量出机器人在每个位置处的定位数据，具体的，本发明的惯性测量元件包含了三个单轴的加速度计和三个单轴的陀螺，加速度计用于检测机器人在机体坐标系下的独立三轴的加速度信号；而陀螺仪用于检测机器人相对于导航坐标系的角速度信号，其测量机器人在三维空间中的角速度和角加速度。
本发明的方法，其在利用惯性测量元件测得机器人在每个位置处的线加速度值、角速度值、角加速度值等定位数据以后，再利用这些定位数据计算出机器人在其相对应位置处的位姿。通常地，机器人的位姿包括其在餐厅中的位置及机器人在餐厅该位置处的姿态，该机器人的姿态包括俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw)等数据。
S20、通过所述机器人的位姿判断所述预定区域内是否有斜坡。
具体的，本发明的送餐机器人，其在通过惯性测量元件测得每个位置处的定位数据，并根据所述定位数据判断该位置处的机器人的位姿之后，即可根据该位置处的机器人的位姿来判断该位置处是否有斜坡。当餐厅内的某位置处有斜坡时，送餐机器人运行到此处时其测得的定位数据中的俯仰角会发生变化，通常翻滚角也会发生变化，因此本发明的方法通过在机器人的位姿数据中的俯仰角发生变化，或者俯仰角和翻滚角均发生变化时，即可判断在该位置处出现了斜坡。
S30、若判断所述预定区域内有斜坡，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
具体的，当判断送餐机器人运行到餐厅内的某个位置处存在斜坡后，即利用该斜坡处的机器人的位姿去补偿该斜坡处的定位误差，即利用机器人测得的该斜坡处的欧拉角去补偿由于环境变化带来的视角跳变，从而避免了机器人因为上下斜坡或者剧烈颠簸带来的视角跳变，而认为其自身状态发生了跳变，继而了定位结果，导致定位跳变的缺陷。
本发明的斜坡定位校正方法，所述方法利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。
在具体实施中，所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤包括：
利用所述惯性测量元件测量所述机器人的三轴线加速度、三轴角速度，通过所述三轴线加速度构成的第一欧拉角反旋转矩阵以及所述三轴角速度构成的第二欧拉角反旋转矩阵来计算出所述机器人的位姿。
具体的，本发明的用于机器人的斜坡定位校正方法，所述方法首先对送餐机器人的状态进行建模，设定状态变量如下：
其中，w代表三轴角速度，其中，wx为空间三维坐标系(包括X轴、Y轴和Z轴)下沿X轴的角速度，wy为空间三维坐标系下沿Y轴的角速度，wz为空间三维坐标系下沿Z轴的角速度；wa代表三维角加速度，其中，wax为空间三维坐标系下沿X轴的角加速度，way为空间三维坐标系下沿Y轴的角加速度，waz为空间三维坐标系下沿Z轴的角加速度；ze代表角加速度在机体坐标系下的三轴分量，其中，zex为角加速度在机体坐标系下X轴的分量，zey为角加速度在机体坐标系下Y轴的分量，zez为角加速度在机体坐标系下Z轴的分量。
本发明的方法在对上述送餐机器人设置好状态变量以后，再对测量矩阵进行设置，其测量矩阵Z的表达式为：
其中，G表示三轴陀螺仪的输出，A表示三轴加速度计的输出，
其中，A为加速度构成的第一欧拉角反旋转矩阵，G为角速度构成的欧拉角反旋转矩阵，该第一欧拉角反旋转矩阵和第二欧拉角反旋转矩阵均由3行3列的元素组成；在公式(3)和(4)中，所述A中的元素ax、ay、az分别表示所述机器人在机体坐标系下的正方向上的三轴线加速度，-ax、-ay、-az分别表示所述机器人在机体坐标系下的反方向上的三轴线加速度，所述G中的元素wx、wy、wz分别表示所述机器人在导航坐标系下的正方向上的三轴角速度，-wx、-wy、-wz分别表示所述机器人在导航坐标系下的反方向上的三轴角速度。
然后即可得到机器人的位姿状态方程：
在公式(5)中，
为所述机器人的状态，所述机器人的状态包括机器人的俯仰角位姿信息(Pitch)和翻滚角位姿信息(Roll)，X为状态变量，
在公式(6)中，字母O为3行3列的零矩阵，即字母O的3行3列中的9个元素均为数字0，字母I为3行3列的单位矩阵，即字母I的3个元素中除了左上角、中间与右下角的3个元素为数字1以外，其余6个元素均为数字0。
在具体实施中，所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤还包括：
通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿，其中，利用先验估计的方法计算所述机器人的位姿的公式为：
其中，
具体的，本发明的方法，其在送餐机器人运行过程中的某一处位置下的位姿状态通过状态转移矩阵来计算该处位置的下一处位置的位姿状态，从而使得计算出的机器人的各个位置处的位姿均只与其前一个位置处的位姿相关联，相对于现有技术中的需要人为设置虚拟墙或者人为测量斜坡角度的方案，简化了操作。
在具体实施中，所述通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿的步骤之后，还包括以下步骤：
通过测量值来对修正所述机器人的位姿的先验估计值。
具体的，测量矩阵与状态矩阵的关系为：
Zk＝ZPk+Rk (7)，
其中，Zk表示测量矩阵，Pk表示位姿状态矩阵，Rk表示噪声，Z即公式(2)，在本公式中，测量矩阵Zk与位姿状态矩阵Pk呈线性关系。在利用测量值修正位姿的先验估计后，计算位姿数据的卡尔曼增益，状态后验估计以及方差后验估计，即可得到机器人的姿态信息，然后再从姿态角中提取欧拉角，包括俯仰角和翻滚角。需要说明的是由于加速度计和陀螺仪较为敏感，因此在接收两者的数据时还做了一次窗口滑动滤波，使得最终解算得到的角度信息更加精确和稳定，减少了噪声的干扰；此时我们就能从这个位姿中获得机器人当前的环境和环境变化，即利用位姿得到欧拉角(Roll和Pitch)获得机器人的状态，然后就能从位姿中区分机器人当前所处的环境(平面还是斜坡)；最后再利用得到的欧拉角去补偿由于环境改变带来的视角跳变，从而避免了机器人因为上下斜坡或者剧烈颠簸带来的视角跳变，而认为自身状态发生了跳变，继而影响了定位结果，导致了定位跳变的缺陷。
在具体实施中，所述通过所述机器人的位姿判断所述预定区域内是否有斜坡；若有，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差的具体步骤包括：
通过所述机器人的位姿中的欧拉角来判断所述预定区域内是否有斜坡，若所述欧拉角的俯仰角和翻滚角在机体坐标系下有数值，则判定该预定区域内有斜坡；若所述欧拉角的俯仰角和翻滚角在机体坐标下为0，则判断该预定区域内没有斜坡；
若判定该预定区域内有斜坡，则利用该斜坡处对应的位姿的俯仰角和翻滚角的数值来补偿所述斜坡处的定位误差。
具体的，本发明的方法，其通过位姿态得到欧拉角来获得机器人的状态，进而从位姿中区分机器人当前所处的环境是平面还是上坡或者下坡等斜坡路面。机器人在运行过程中，如果其欧拉角的俯仰角为0，则说明机器人相对于地面并没有产生前倾或者后仰；如果其欧拉角的翻滚角为0，则说明机器人相对于地面并没有产生左翻或者右翻。通常地，如果运行到某个位置时为上坡或者下坡时，则其欧拉角的俯仰角在机体坐标系下会有数值(正值或者负值)，在机器人上坡或者下坡时，通常会伴随着左翻、右翻的现象；当机器人运行到凹凸不平的地面或者被某个东西绊了一下时，会产生左翻、右翻，此时其欧拉角的翻滚角也会有数值(正值或者负值)。
因此，本发明的方法，其通过判断机器人在运行到某个位置处的欧拉角来判断是否有斜坡，如果判断有斜坡，则利用该斜坡位置处的欧拉角数值来补偿该斜坡处的定位误差。
本发明公开的用于机器人的斜坡定位校正方法，其相对于现有技术中的方案，至少具有以下几点有益效果：
(1)实时性好。能够利用惯性元件实时获得机器人位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿。
(2)鲁棒性高。利用加速度计和陀螺仪数据融合得到的位姿，避免了单使用加速度计时虽然长时间获得的测量值较为精确，但是较短时间内由于噪声的缘故而存在误差，也避免了单独使用陀螺仪时虽然较短时间内获得的测量值较为准确，但是较长时间则会因为陀螺仪本身零漂问题带来误差。继而能在长时间内稳定获得精确的机器人位姿，准确的估计机器人当前所处的环境和环境变化，使得机器人在斜坡或者剧烈颠簸环境下，仍能获得稳定可靠的定位结果，感知、规划和调度模块也能够根据精确的定位结果正常工作。
(3)高精度。有效补偿了斜坡或剧烈颠簸导致的定位误差，从而导致的定位跳变。经测算当斜坡坡度为10度时，就会使得机器人定位跳变0.4m左右。再对于室内定位这类高精度要求的定位场景下，这类定位误差是不能够容忍，该发明校正了该类场景下的定位误差，避免了定位跳变，并最终获得了准确的定位结果。
(4)方便快捷。首先不再需要人为设定虚拟墙，限制机器人运行边界，极大的提高了机器人的环境适应能力；其次不需要人为测量斜坡角度，在地图中设定斜坡元素，当机器人定位置斜坡处时，再利用测量的斜坡角度对定位进行补偿。机器会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正。
参考图2，本发明还提供了一种斜坡定位校正装置，应用于机器人，所述斜坡定位校正装置与上述斜坡定位校正方法相对应，所述斜坡定位校正装置包括位姿计算单元100、斜坡判断单元200以及定位误差补偿单元300。
位姿计算单元100用于利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿。
斜坡判断单元200用于通过所述机器人的位姿判断所述预定区域内是否有斜坡。
定位误差补偿单元300用于在所述斜坡判断单元200判断所述预定区域内有斜坡时，利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
本发明的斜坡定位校正装置，所述装置利用惯性测量元件实时获取机器人的位姿和位姿变化，从而推断出机器人所处环境的变化，能够实时对机器人定位结果做出补偿，实时性好；而且机器人会根据自身位姿的变化测算环境和环境变化，在不需要人为干扰的前提下，机器人本身对自己的定位误差进行校正，不再需要人为设置虚拟墙或者人为测量斜坡角度，方便快捷。
参阅图1、图3，本发明还提供了一种机器人，所述机器人包括：
一个或多个处理器10、存储器20；和
一个或多个程序，其中所述一个或多个程序被存储在所述存储器20中，并且被所述一个或多个处理器10执行，所述程序包括用于执行上述任意的斜坡定位校正方法的指令。
存储器20用于存放可在处理器10上运行的计算机程序，处理器10执行程序时实现上述任一实施方式中的斜坡定位校正方法。
存储器20可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatilememory)，例如至少一个磁盘存储器。进一步地，机器人还可包括通信接口30，通信接口用于存储器20和处理器10之间的通信。
如果存储器20、处理器10和通信接口30独立实现，则通信接口30、存储器20和处理器10可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构(Industry Standard Architecture，简称为ISA)总线、外部设备互连(PeripheralComponent，简称为PCI)总线或扩展工业标准体系结构(Extended IndustryStandardArchitecture，简称为EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图3中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
可选的，在具体实现上，如果存储器20、处理器10及通信接口30，集成在一块芯片上实现，则存储器20、处理器10及通信接口30可以通过内部接口完成相互间的通信。
处理器10可能是一个中央处理器(Central Processing Unit，简称为CPU)，或者是特定集成电路(Application Specific Integrated Circuit，简称为ASIC)，或者是被配置成实施本申请实施例的一个或多个集成电路。
参阅图4，本发明还提供了一种包含计算机程序的非易失性计算机可读存储介质500，当所述计算机程序501被一个或多个处理器10执行时，使得所述处理器10执行上述任意的斜坡定位校正方法。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结果、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结果、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种斜坡定位校正方法，应用于一机器人，其特征在于，包括以下步骤：
利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿；
通过所述机器人的位姿判断所述预定区域内是否有斜坡；
若判断所述预定区域内有斜坡，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
2.根据权利要求1所述的斜坡定位校正方法，其特征在于，所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤包括：
利用所述惯性测量元件测量所述机器人的三轴线加速度、三轴角速度，通过所述三轴线加速度构成的第一欧拉角反旋转矩阵以及所述三轴角速度构成的第二欧拉角反旋转矩阵来计算出所述机器人的位姿。
3.根据权利要求2所述的斜坡定位校正方法，其特征在于，所述机器人的位姿状态方程为：
其中，
4.根据权利要求3所述的斜坡定位校正方法，其特征在于，所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿的具体步骤还包括：
通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿，其中，利用先验估计的方法计算所述机器人的位姿的公式为：
其中，
5.根据权利要求4所述的斜坡定位校正方法，其特征在于，所述通过状态转移矩阵并使用先验估计的方法来计算所述机器人的位姿的步骤之后，还包括以下步骤：
通过测量值来对修正所述机器人的位姿的先验估计值。
6.根据权利要求3所述的斜坡定位校正方法，其特征在于，所述通过所述机器人的位姿判断所述预定区域内是否有斜坡；若有，则利用该斜坡处对应的位姿补偿所述斜坡处的定位误差的具体步骤包括：
通过所述机器人的位姿中的欧拉角来判断所述预定区域内是否有斜坡，若所述欧拉角的俯仰角和翻滚角在机体坐标系下有数值，则判定该预定区域内有斜坡；若所述欧拉角的俯仰角和翻滚角在机体坐标下为0，则判断该预定区域内没有斜坡；
若判定该预定区域内有斜坡，则利用该斜坡处对应的位姿的俯仰角和翻滚角的数值来补偿所述斜坡处的定位误差。
7.根据权利要求1所述的斜坡定位校正方法，其特征在于，所述利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据的步骤之后，还包括以下步骤：
对所述惯性测量元件测量的定位数据进行窗品滑动滤波处理，其中，所述惯性测量元件包括用于测量角速度的角运动检测装置以及用于测量线加速度的加速度检测装置。
8.一种斜坡定位校正装置，应用于一机器人，其特征在于，包括：
位姿计算单元，用于利用所述机器人的惯性测量元件测量所述机器人在预定区域内的定位数据，并根据所述定位数据计算出所述机器人在该预定区域内的位姿；
斜坡判断单元，用于通过所述机器人的位姿判断所述预定区域内是否有斜坡；以及
定位误差补偿单元，用于在所述斜坡判断单元判断所述预定区域内有斜坡时，利用该斜坡处对应的位姿补偿所述斜坡处的定位误差。
9.一种机器人，其特征在于，包括:
一个或多个处理器、存储器；和
一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被所述一个或多个处理器执行，所述程序包括用于执行权利要求1至7中任一项所述的斜坡定位校正方法的指令。
10.一种包含计算机程序的非易失性计算机可读存储介质，当所述计算机程序被一个或多个处理器执行时，使得所述处理器执行权利要求1至7中任一项所述的斜坡定位校正方法。
Priority Applications (4)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110104694.3A CN114789439B (zh) | 2021-01-26 | 2021-01-26 | 斜坡定位校正方法、装置、机器人及可读存储介质 |
| US18/262,455 US20240077880A1 (en) | 2021-01-26 | 2022-01-21 | Slope location correction method and apparatus, robot and readable storage medium |
| EP22745146.5A EP4269044A4 (en) | 2021-01-26 | 2022-01-21 | METHOD AND DEVICE FOR CORRECTING A SLOPE POSITION, ROBOT AND READABLE STORAGE MEDIUM |
| PCT/CN2022/073163 WO2022161271A1 (zh) | 2021-01-26 | 2022-01-21 | 斜坡定位校正方法、装置、机器人及可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110104694.3A CN114789439B (zh) | 2021-01-26 | 2021-01-26 | 斜坡定位校正方法、装置、机器人及可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114789439A true CN114789439A (zh) | 2022-07-26 |
| CN114789439B CN114789439B (zh) | 2024-03-19 |
Family
ID=82460128
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110104694.3A Active CN114789439B (zh) | 2021-01-26 | 2021-01-26 | 斜坡定位校正方法、装置、机器人及可读存储介质 |
Country Status (4)
| Country | Link |
|---|---|
| US (1) | US20240077880A1 (zh) |
| EP (1) | EP4269044A4 (zh) |
| CN (1) | CN114789439B (zh) |
| WO (1) | WO2022161271A1 (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115307544A (zh) * | 2022-08-15 | 2022-11-08 | 淄博市交通建设发展中心 | 一种道路斜坡路面距离测量***及方法 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20160128126A (ko) * | 2015-04-28 | 2016-11-07 | 엘지전자 주식회사 | 이동 로봇 및 그 제어방법 |
| US9561592B1 (en) * | 2015-05-15 | 2017-02-07 | Google Inc. | Ground plane compensation for legged robots |
| CN106406338A (zh) * | 2016-04-14 | 2017-02-15 | 中山大学 | 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 |
| CN109676604A (zh) * | 2018-12-26 | 2019-04-26 | 清华大学 | 机器人曲面运动定位方法及其运动定位*** |
| CN110450159A (zh) * | 2019-08-13 | 2019-11-15 | 之江实验室 | 一种双足机器人的足部状态检测装置以及基于该装置的斜面行走方法 |
| CN110842921A (zh) * | 2019-11-19 | 2020-02-28 | 山东大学 | 四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法 |
| CN210757742U (zh) * | 2019-08-13 | 2020-06-16 | 之江实验室 | 一种双足机器人的足部状态检测装置 |
| CN111730605A (zh) * | 2020-08-04 | 2020-10-02 | 深圳市优必选科技股份有限公司 | 一种机器人姿态控制方法、装置、可读存储介质及机器人 |
| CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100864607B1 (ko) * | 2007-05-31 | 2008-10-22 | 한국과학기술원 | 휴머노이드 이족 보행 로봇을 위한 기울기 센서와 각속도센서를 이용한 관성 측정 장치 및 방법 |
| KR101115012B1 (ko) * | 2009-08-28 | 2012-03-07 | 부산대학교 산학협력단 | 로봇의 각속도 오차 보정 장치 및 방법 |
| CN103412487B (zh) * | 2013-07-29 | 2016-12-07 | 北京邮电大学 | 一种基于模糊滑模控制器的球形机器人坡面运动控制方法 |
| CN106959110B (zh) * | 2017-04-06 | 2020-08-11 | 亿航智能设备（广州）有限公司 | 一种云台姿态检测方法及装置 |
| CN109001787B (zh) * | 2018-05-25 | 2022-10-21 | 北京大学深圳研究生院 | 一种姿态角解算与定位的方法及其融合传感器 |
| CN109634302B (zh) * | 2018-12-06 | 2022-04-08 | 河池学院 | 一种基于光学定位的四旋翼飞行器*** |
| CN211786766U (zh) * | 2020-04-20 | 2020-10-27 | 北京农业信息技术研究中心 | 一种基于激光雷达的畜禽舍巡检机器人行走控制装置 |
-
2021
- 2021-01-26 CN CN202110104694.3A patent/CN114789439B/zh active Active
-
2022
- 2022-01-21 WO PCT/CN2022/073163 patent/WO2022161271A1/zh active Application Filing
- 2022-01-21 US US18/262,455 patent/US20240077880A1/en active Pending
- 2022-01-21 EP EP22745146.5A patent/EP4269044A4/en active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20160128126A (ko) * | 2015-04-28 | 2016-11-07 | 엘지전자 주식회사 | 이동 로봇 및 그 제어방법 |
| US9561592B1 (en) * | 2015-05-15 | 2017-02-07 | Google Inc. | Ground plane compensation for legged robots |
| CN106406338A (zh) * | 2016-04-14 | 2017-02-15 | 中山大学 | 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 |
| CN109676604A (zh) * | 2018-12-26 | 2019-04-26 | 清华大学 | 机器人曲面运动定位方法及其运动定位*** |
| CN110450159A (zh) * | 2019-08-13 | 2019-11-15 | 之江实验室 | 一种双足机器人的足部状态检测装置以及基于该装置的斜面行走方法 |
| CN210757742U (zh) * | 2019-08-13 | 2020-06-16 | 之江实验室 | 一种双足机器人的足部状态检测装置 |
| CN110842921A (zh) * | 2019-11-19 | 2020-02-28 | 山东大学 | 四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法 |
| CN111730605A (zh) * | 2020-08-04 | 2020-10-02 | 深圳市优必选科技股份有限公司 | 一种机器人姿态控制方法、装置、可读存储介质及机器人 |
| CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115307544A (zh) * | 2022-08-15 | 2022-11-08 | 淄博市交通建设发展中心 | 一种道路斜坡路面距离测量***及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| US20240077880A1 (en) | 2024-03-07 |
| EP4269044A1 (en) | 2023-11-01 |
| EP4269044A4 (en) | 2024-07-03 |
| CN114789439B (zh) | 2024-03-19 |
| WO2022161271A1 (zh) | 2022-08-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022105024A1 (zh) | 机器人位姿的确定方法、装置、机器人及存储介质 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及*** | |
| CN111220153B (zh) | 基于视觉拓扑节点和惯性导航的定位方法 | |
| CN110243358A (zh) | 多源融合的无人车室内外定位方法及*** | |
| CN105865461B (zh) | 一种基于多传感器融合算法的汽车定位***及方法 | |
| CN108235735A (zh) | 一种定位方法、装置及电子设备、计算机程序产品 | |
| CN109752725A (zh) | 一种低速商用机器人、定位导航方法及定位导航*** | |
| CN107063305B (zh) | 用惯导、压力传感器修正下坡悬空后轮里程计误差的方法 | |
| CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
| CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| CN111679680A (zh) | 一种无人机自主着舰方法及*** | |
| CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 | |
| CN104546391B (zh) | 一种用于盲杖的陀螺稳定装置及其互补滤波方法 | |
| CN115436955A (zh) | 室内外环境定位方法 | |
| US10466054B2 (en) | Method and system for estimating relative angle between headings | |
| CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
| CN109387198A (zh) | 一种基于序贯检测的惯性/视觉里程计组合导航方法 | |
| CN114789439A (zh) | 斜坡定位校正方法、装置、机器人及可读存储介质 | |
| CN112862818A (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
| CN112697153A (zh) | 自主移动设备的定位方法、电子设备及存储介质 | |
| CN115585805A (zh) | 车辆定位方法、定位装置、定位***以及车辆 | |
| US11992961B2 (en) | Pose determination method, robot using the same, and computer readable storage medium | |
| CN112461237B (zh) | 一种应用于动态变化场景下的多传感器融合定位方法 | |
| CN115290090A (zh) | 基于多传感器信息融合的slam地图构建方法 | |
| CN105874352B (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 |