CN116358517B - 一种用于机器人的高度地图构建方法、系统以及存储介质 - Google Patents
一种用于机器人的高度地图构建方法、系统以及存储介质 Download PDFInfo
- Publication number
- CN116358517B CN116358517B CN202310202525.2A CN202310202525A CN116358517B CN 116358517 B CN116358517 B CN 116358517B CN 202310202525 A CN202310202525 A CN 202310202525A CN 116358517 B CN116358517 B CN 116358517B
- Authority
- CN
- China
- Prior art keywords
- point
- coordinate system
- point cloud
- height
- map
- 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
- 238000010276 construction Methods 0.000 title claims abstract description 58
- 238000003860 storage Methods 0.000 title claims abstract description 21
- 238000005259 measurement Methods 0.000 claims abstract description 124
- 238000000034 method Methods 0.000 claims abstract description 54
- 238000001914 filtration Methods 0.000 claims abstract description 19
- 230000009466 transformation Effects 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 33
- 210000002414 leg Anatomy 0.000 claims description 28
- 238000009826 distribution Methods 0.000 claims description 24
- 238000012545 processing Methods 0.000 claims description 24
- 238000013507 mapping Methods 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 10
- 238000007781 pre-processing Methods 0.000 claims description 10
- 230000001131 transforming effect Effects 0.000 claims description 8
- 210000000629 knee joint Anatomy 0.000 claims description 5
- 230000000007 visual effect Effects 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000008901 benefit Effects 0.000 abstract description 11
- 238000010586 diagram Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 6
- 230000007547 defect Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 3
- 125000004122 cyclic group Chemical group 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 241000274965 Cyrestis thyodamas Species 0.000 description 1
- 206010034701 Peroneal nerve palsy Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 244000309466 calf Species 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
Abstract
本发明公开了一种用于机器人的高度地图构建方法、系统以及存储介质，属于高度地图构建技术领域。本发明的一种用于足式机器人运动规划的高度地图构建方法，通过构建运动畸变去除模型、点云滤除模型、高度地图模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航，并且本发明的高度地图可以以足式机器人为中心，实时地跟随足式机器人移动。
Description
技术领域
本发明涉及一种用于机器人的高度地图构建方法、系统以及存储介质，属于高度地图构建技术领域。
背景技术
为了让足式机器人在其运行环境中实现精确的路径规划和运动控制，需要使用传感器反馈回来的数据对环境信息进行建图。在复杂环境中，一个精确的环境地图可以使得足式机器人在环境中能够安全高效地移动并躲开障碍物。通过利用激光雷达和深度相机等传感器，机器人可以对其周围的环境地形进行测量，进而通过相关建图算法来建立完整的环境地图。
当前主流的环境地图建图方法一般是建立世界坐标系下的全局地图，其需要使用一个精确的全局定位系统来进行定位，例如激光SLAM和视觉SLAM等算法。然而，这些定位方法一般都对环境存在特定的要求，否则容易失效。例如，激光SLAM算法需要环境中具有足够的结构特征，使得可以在两帧激光点云之间进行有效的约束匹配；视觉SLAM需要相机图片中可以采集的足够的环境纹理特征，且图片不能过曝，不能由于运动产生模糊等。并且，这些建图方法通常计算开销和内存占用均非常大，其难以在足式机器人中常用的嵌入式计算机中完成实时计算，并且其所建立的地图格式一般也无法直接用于机器人的运动规划。
针对上述全局地图建图方法的缺点，瑞士苏黎世联邦理工大学的研究人员提出了一种以足式机器人为地图中心的局部高度地图建立方法ElevationMapping。该方法仅使用足式机器人自身的运动学里程计来提供定位，并利用传感器测量来更新高度地图，可以实现较好的建图效果。然而，该方法的计算开销和内存占用依然比较大。并且，当足式机器人搭载的传感器所获得的点云较为稀疏时，该方法建立的地图非常稀疏，导致足式机器人难以进行落足点规划。
进一步，中国专利(公布号CN111596665A)公开了一种稠密高度地图构建方法，属于稠密高度地图构建技术领域。该构建方法具体为：使用高斯分布模型估计激光雷达的测量值，并将其映射到局部地图相应的栅格，以进行多帧数据融合，其间根据机器人位姿信息更新地图栅格协方差，并对局部地图进行相应移动，且对动态障碍物进行清除，以构成完整的局部稠密地图。
由于传感器采集的每一个点的时间戳不同，对应该点时间戳处的传感器坐标系在世界坐标系下的位姿也不同。因此，原始采集的一帧点云包含了由于传感器运动所带来的畸变，即运动畸变，但上述方案未涉及对运动畸变处理，如果不去除运动畸变，将影响高度地图的精准度，进而无法对机器人进行准确导航，不利于推广。
更进一步，上述方案，未公开对如何处理与环境信息无关的测量点，如果不去除掉其中与环境信息无关的点，同样会影响高度地图的精准度。
发明内容
针对现有技术的缺陷，本发明的目的一在于提供一种通过构建运动畸变去除模型、点云滤除模型、高度地图模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航的用于机器人的高度地图构建方法。
针对现有技术的缺陷，本发明的目的二在于提供一种利用机身里程计位姿估计，以及标定得到的机身坐标系到传感器坐标系的位姿变换，将该测量点从传感器坐标系变换到世界坐标系下，从而对运动畸变进行去除，进而使用处理后的数据构建以机器人为中心的、用于足式机器人运动规划的稠密高度地图的用于机器人的高度地图构建方法。
针对现有技术的缺陷，本发明的目的三在于提供一种通过设置运动畸变去除模块、点云滤除模块、高度地图模块，对原始测量点数据进行处理，将原始测量点数据，在某时刻的传感器坐标系变换到统一时刻的传感器坐标系，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航的用于机器人的高度地图构建系统。
针对现有技术的缺陷，本发明的目的四在于提供一种计算效率高、建图精度高和建图效果好的用于机器人的高度地图构建方法、系统以及存储介质。
为实现上述目的之一，本发明的第一种技术方案为：
一种用于足式机器人运动规划的高度地图构建方法，包括如下步骤：
第一步，获取地图规划区域的原始测量点数据；
第二步，利用预先构建的运动畸变去除模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云；
第三步，通过预先构建的点云滤除模型，对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；
与环境信息无关的点包括机身点云或/和腿部点云或/和拖尾点云；
第四步，利用预先构建的高度地图模型，并根据机身里程计和/或视觉里程计和/或激光里程计位姿数据，对预处理点云进行处理，得到栅格高度值；
第五步，根据栅格高度值，建立或更新地图规划区域中的栅格，从而实现用于足式机器人运动规划的高度地图构建。
本发明经过不断探索以及试验，通过构建运动畸变去除模型、点云滤除模型、高度地图模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航。
作为优选技术措施：
所述第二步中，运动畸变去除模型对原始测量点数据进行运动畸变去除的方法如下：
步骤1，将带有时间戳的当前帧点云放入点云队列cloudQueue，将带有时间戳的机身里程计位姿放入里程计队列odomQueue；
步骤2，如果当前点云队列不为空，则从前往后遍历里程计队列，维护里程计队列的队首帧时间戳与点云起始点时间戳对齐，即如果里程计队列的队首帧时间戳小于点云起始点时间戳，则将其移出队列；
步骤3，对于当前帧点云，按照时间先后进行排序，第一个点即起始点的时间戳为ts，最后一个点的时间戳为te即结束点；
将结束点时间戳作为当前帧点云的时间戳，记为tc＝te；
步骤4，从前往后遍历里程计队列，从中找出恰好位于点云时间戳前后的相邻两帧里程计位姿，相邻两帧里程计位姿分别为和/>满足tk≤tc<tk+1；
然后，使用线性插值的方法，计算得到点云时间戳处的机身里程计位姿，其表达式如下：
步骤5，按照时间戳从起始点往后遍历点云中的每一个原始测量点；通过线性插值得到该原始测量点时刻的机身里程计位姿，记为
步骤6，对于当前遍历的原始测量点为通过位姿变换将其由tp时刻的传感器坐标系变换到统一的tc时刻的传感器坐标系，从而实现运动畸变地去除；其计算公式如下：
作为优选技术措施：
所述第三步中，点云滤除模型，对当前帧点云进行处理的方法如下：
对于去除运动畸变后的当前帧点云中的一点计算其在传感器坐标系{S}下的极坐标：
其中，sr是向量ps的模长，sp是向量ps与传感器坐标系Z轴正向的夹角，sa是向量ps在传感器坐标系XY平面的投影向量与X轴正向的夹角；
在极坐标系下，将所有的点均投影到sr＝1的球面上，然后选定特定的球面矩形区域以剔除投影到其中的噪声点云，去除掉拖尾点云，其表达式如下：
为了剔除四足机器人腿部的点云，基于机器人正向运动学，由电机关节角度计算得到各关节在机身坐标系下的位置，然后将其变换到传感器坐标系下；
膝关节和足端在传感器坐标系下的位置分别为Ao和Bo，先将其投影到传感器坐标系{S}下半径等于1的球面上；设投影后的两点分别为A和B，则有：
则由向量SA和向量SB构建一组标准正交基：
点P在该标准正交基的XY平面中投影的垂足为H＝[hx,hy,hz]T，则该垂足为：
在该标准正交基下，计算一个激光点P的球面坐标(1,θ,φ)：
小腿连杆投影后在归一化球面上近似为一个矩形，则A和B为该矩形的中轴线；
投影后的连杆最大宽度为b，连杆最大长度为l，则通过如下公式判断点P是否在该矩形范围内；
如果该测量点在该矩形内，则认为该测量点属于腿部点，从而将其从点云中剔除，实现腿部点云地去除。
作为优选技术措施：
所述第四步中，高度地图模型，将预处理点云的每一个测量点均作为一次测量，以建立或更新地图规划区域中的栅格，其对预处理点云进行处理的方法如下：
对于地图规划区域中的每一个栅格，使用高斯分布模型来表征该栅格的高度估计值：
其中，和/>分别为高斯分布的均值和方差；
传感器坐标系下的一个测量点为ps，则利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下：
通过地图索引判断公式组，将该测量点从世界坐标系下映射得到其对应的二维数组索引其对应的高度值为测量点/>的z轴分量，即为当前栅格的一次高度测量值；
建立投影矩阵Mp＝[0,0,1]，得到当前次高度测量值：
传感器坐标系到机器人坐标系的位姿变换是标定得到的固定值；
在传感器坐标系下的测量点ps满足高斯分布，其高度估计的方差传播主要取决于传感器测量ps的方差、机身里程计的旋转量的方差以及机身里程计平移量/>的方差；
推导得到高度测量值关于ps、/>和/>的雅克比矩阵：
然后，由协方差传播计算得到当前高度测量值的方差：
其中，∑p是传感器坐标系下测量点的协方差矩阵，其大小取决于传感器测量噪声模型；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵；
从而得到当前次高度测量的高斯分布：
使用卡尔曼滤波的方法，将当前次高度测量融合到当前栅格的高度估计中，得到更新后的栅格高度值：
最后，根据栅格高度值，完成能用于足式机器人运动规划的高度地图构建。
为实现上述目的之一，本发明的第二种技术方案为：一种用于足式机器人运动规划的高度地图构建方法，包括以下步骤：
S1：获取通过激光雷达采集的一个传感器坐标系下的测量点ps，利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下，其测量点的世界坐标值为pw：
S2：将该测量点从世界坐标系下映射到其在高度地图中的对应栅格，其对应的高度值为测量点的z轴分量，即为当前栅格的一次高度测量值；测量高度值满足高斯分布/>并构建投影矩阵Mp＝[0,0,1]，则得到当前次高度测量均值：
由协方差传播计算得到当前高度测量值的方差：
其中，Jp、JR和Jt是高度测量值关于ps、/>和/>的雅克比矩阵；∑p是传感器坐标系下测量点的协方差矩阵；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵；
S3：根据当前次高度测量均值以及当前高度测量值的方差，构建当前高度测量值的高斯分布；
栅格的高度估计满足高斯分布则通过计算两个高斯分布的归一化积，将当前次高度测量融合到当前栅格的高度估计中，得到更新后的高度估计：
S4：根据更新后的高度估计，构建用于足式机器人运动规划的稠密高度地图。
本发明提供的一种用于足式机器人运动规划的高度地图构建方法，利用机身里程计位姿估计，以及标定得到的机身坐标系到传感器坐标系的位姿变换，将该测量点从传感器坐标系变换到世界坐标系下，从而对运动畸变进行去除，进而使用处理后的数据构建以机器人为中心的、用于足式机器人运动规划的稠密高度地图。相比于传统方法，本发明具有计算效率高、建图精度高和建图效果好的优点，其可以在嵌入式平台上实现实时高效的环境高度地图构建，并且高度地图可以以足式机器人为中心，实时地跟随足式机器人移动。
作为优选技术措施：
将测量点从世界坐标系下映射到其在高度地图中的对应栅格，即将测量点pw从世界坐标系下映射得到其在高度地图中对应栅格所属的二维数组索引其步骤如下：
步骤一，世界坐标系下的测量点为则将其x和y分量分别除以地图分辨率r，再向下取整，得到该测量点在世界坐标系下对应栅格的2D整数索引：
步骤二，当前地图坐标系原点在世界坐标系下的2D整数索引为则计算得到该测量点在地图坐标系下对应栅格的2D整数索引：
步骤三，高度地图为一张N×N大小的栅格地图，则通过判断当前点的索引是否同时满足如下两个条件来判断其是否在高度地图范围内：
步骤四，若该测量点同时满足上述两个条件，即该测量点位于地图范围内，则将世界坐标系下的2D整数索引对地图大小N求模，将该测量点映射到一个在计算机中存储的二维数组索引：
步骤五，通过将高度地图的长宽均设置为相同大小的2的指数，即N＝2n，将索引映射步骤改为计算效率高的二进制形式，即将求模运算“mod”改为二进制的按位与运算“&”：
本发明构建了一种基于循环数组的2.5D高度地图模型，其可以将一个空间点高效的映射到对应的高度地图栅格并进行索引，还可以快速地对地图进行移动。
进而本发明提供的方法，在嵌入式计算机中运行时的CPU占用和内存占用都很低，可以实时地构建足式机器人周围的高精度稠密高度地图，为足式机器人的运动规划提供环境感知信息。
作为优选技术措施：
对传感器坐标系下的测量点ps进行运动畸变去除的步骤包括：
A1，将带有时间戳的当前帧点云放入点云队列，将带有时间戳的机身里程计位姿放入里程计队列；若当前点云队列不为空，则从前往后遍历里程计队列，维护里程计队列的队首帧时间戳与点云起始点时间戳对齐；
A2，对于当前帧点云，设起始点的时间戳为ts，结束点的时间戳为te，将结束点时间戳作为当前帧点云的时间戳，记为tc＝te；
A3，从前往后遍历里程计队列，从中找出恰好位于点云时间戳前后的相邻两帧里程计位姿，相邻两帧里程计位姿分别为和/>并满足tk≤tc<tk+1；则可通过线性插值的方法计算点云时间戳处的机身里程计位姿：
A4，按照时间戳从起始点往后遍历点云队列中的每一个点，当前遍历到的一个原始测量点为其时间戳为tp，则由该时刻相邻两帧里程计位姿线性插值得到该测量点时刻的机身里程计位姿，记为/>
A5，将原始测量点通过位姿变换，将其由tp时刻的传感器坐标系变换到统一的当前帧点云时间戳tc时刻的传感器坐标系：
从而得到去除运动畸变后的当前帧点云
作为优选技术措施：
对当前帧点云进行拖尾点云去除的步骤包括：
B1，计算点云中一点ps在传感器坐标系下的极坐标：
其中，sr是向量ps的模长，sp是向量ps与传感器坐标系Z轴正向的夹角，sa是向量ps在传感器坐标系XY平面的投影向量与X轴正向的夹角；
B2，在极坐标系下，将所有的点均投影到sr＝1的球面上，然后选定特定的球面矩形区域以剔除投影到其中的拖尾点云：
对当前帧点云进行腿部点云去除的步骤包括：
C1，将腿部膝关节在传感器坐标系下的位置Ao和腿部足端在传感器坐标系下的位置Bo，分别投影到传感器坐标系下半径等于1的球面上，投影后的两点分别为A和B，则有：
由向量SA和向量SB构建一组标准正交基：
C2，设点P在该标准正交基的XY平面中投影的垂足为H＝[hx,hy,hz]T，则该垂足为：
在该标准正交基下，计算一个激光点P的球面坐标(1,θ,φ)：
C3，腿部连杆投影在归一化球面上为一个矩形，通过如下公式判断点P是否在该矩形范围内：
其中，b为腿部连杆投影后的最大宽度，l为腿部连杆投影后的最大长度；若点P在该矩形范围内，则去除。
为实现上述目的之一，本发明的第三种技术方案为：
一种用于足式机器人运动规划的高度地图构建系统，其包括激光雷达、运动畸变去除模块、点云滤除模块、高度地图模块、一个或多个处理器、存储装置；
激光雷达，用于采集原始测量点数据；
运动畸变去除模块，用于根据参考坐标系的转换特性，对原始测量点数据进行处理，将原始测量点数据，在某时刻的传感器坐标系变换到统一时刻的传感器坐标系，得到去除运动畸变后的当前帧点云；
点云滤除模块，用于对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；
高度地图模块，用于对预处理点云进行处理，得到栅格高度值；并更新预处理点云在高度地图中的对应栅格；
存储装置，用于存储一个或多个模块的程序；
当所述程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现采用上述的一种用于足式机器人运动规划的高度地图构建方法。
本发明经过不断探索以及试验，通过设置运动畸变去除模块、点云滤除模块、高度地图模块，对原始测量点数据进行处理，将原始测量点数据，在某时刻的传感器坐标系变换到统一时刻的传感器坐标系，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航。
进一步，相比于传统方法，本发明具有计算效率高、建图精度高和建图效果好的优点，其可以在嵌入式平台上实现实时高效的环境高度地图构建，并且高度地图可以以足式机器人为中心，实时地跟随足式机器人移动。
为实现上述目的之一，本发明的第四种技术方案为：
一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现采用上述的一种用于足式机器人运动规划的高度地图构建方法。
与现有技术相比，本发明具有以下有益效果：
本发明经过不断探索以及试验，通过构建运动畸变去除模型、点云滤除模型、高度地图模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航。
进一步，本发明提供的一种用于足式机器人运动规划的高度地图构建方法，利用机身里程计位姿估计，以及标定得到的机身坐标系到传感器坐标系的位姿变换，将该测量点从传感器坐标系变换到世界坐标系下，从而对运动畸变进行去除，进而使用处理后的数据构建以机器人为中心的、用于足式机器人运动规划的稠密高度地图。
更进一步，本发明经过不断探索以及试验，通过设置运动畸变去除模块、点云滤除模块、高度地图模块，对原始测量点数据进行处理，将原始测量点数据，在某时刻的传感器坐标系变换到统一时刻的传感器坐标系，得到去除运动畸变后的当前帧点云，再对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；进而使用处理后的点云和足式机器人的机身里程计位姿数据来构建用于足式机器人运动规划的高度地图，方案科学、合理，切实可行，能够有效提升高度地图的精准度，利于推广，进而可以对机器人进行准确导航。
再进一步，相比于传统方法，本发明具有计算效率高、建图精度高和建图效果好的优点，其可以在嵌入式平台上实现实时高效的环境高度地图构建，并且高度地图可以以足式机器人为中心，实时地跟随足式机器人移动。
附图说明
图1是本发明高度地图构建方法的第一种流程图；
图2是本发明高度地图构建方法的第二种流程图；
图3是本发明的一种坐标系示意图；
图4是本发明的一种球坐标系下腿部点云去除的示意图；
图5是本发明的一种拖尾点云和腿部点云去除前的示意图；
图6是本发明的一种拖尾点云和腿部点云去除后的示意图；
图7是本发明的一种高度地图索引示意图；
图8是本发明的一种高度地图移动示意图；
图9是本发明的一种稠密高度地图构建效果示意图；
图10是本发明的一种稠密高度地图构建效果的另一示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文所使用的术语只是为了描述具体的实施例的目的，不是旨在限制本发明。本文所使用的术语“或/和”包括一个或多个相关的所列项目的任意的和所有的组合。
如图1所示，本发明用于足式机器人运动规划的高度地图构建方法的第一种具体实施例：
一种用于足式机器人运动规划的高度地图构建方法，包括如下步骤：
第一步，获取地图规划区域的原始测量点数据；
第二步，利用预先构建的运动畸变去除模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云；
第三步，通过预先构建的点云滤除模型，对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；
与环境信息无关的点包括机身点云或/和腿部点云或/和拖尾点云；
第四步，利用预先构建的高度地图模型，并根据机身里程计和/或视觉里程计和/或激光里程计位姿数据，对预处理点云进行处理，得到栅格高度值；
第五步，根据栅格高度值，建立或更新地图规划区域中的栅格，从而实现用于足式机器人运动规划的高度地图构建。
如图2所示，本发明用于足式机器人运动规划的高度地图构建方法的第二种具体实施例：
一种用于足式机器人运动规划的高度地图构建方法，包括如下步骤：
S1：使用激光雷达获取一个传感器坐标系下的测量点ps，利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下：
S2：将该测量点从世界坐标系下映射到其在高度地图中的对应栅格，其对应的高度值为测量点的z轴分量，即为当前栅格的一次高度测量值；测量高度值满足高斯分布/>定义投影矩阵Mp＝[0,0,1]，则可以得到当前次高度测量值：
S3：由协方差传播计算得到当前高度测量值的方差：
其中，Jp、JR和Jt是高度测量值关于ps、/>和/>的雅克比矩阵，∑p是传感器坐标系下测量点的协方差矩阵，其大小取决于传感器测量噪声模型；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵；
S4：栅格的高度估计满足高斯分布则通过计算两个高斯分布的归一化积，可以将当前次高度测量融合到当前栅格的高度估计中，得到更新后的高度估计：
本发明提供的一种用于足式机器人运动规划的高度地图构建方法，该方法基于双旋转轴的三维激光雷达，对其获得的稀疏点云进行运动畸变去除、足式机器人的腿部点云去除以及拖尾噪声点云去除等点云预处理步骤，并可以进一步使用处理后的点云和足式机器人的里程计数据来构建以机器人为中心的、用于足式机器人运动规划的稠密高度地图。
本发明提供一种用于足式机器人运动规划的高度地图构建方法，计算开销和内存占用小、计算效率高，可以在嵌入式平台上实现实时高效的环境高度地图构建。
本发明映射得到对应栅格所属的二维数组索引的一种具体实施例：
将pw从世界坐标系下映射得到其在高度地图中对应栅格所属的二维数组索引的步骤如下：
步骤一，设世界坐标系下的一点为则将其x和y分量分别除以地图分辨率r，再向下取整，可以得到该测量点在世界坐标系下对应栅格的2D整数索引：/>
步骤二，设当前地图坐标系原点在世界坐标系下的2D整数索引为则可以计算得到该测量点在地图坐标系下对应栅格的2D整数索引：
步骤三，设高度地图为一张N×N大小的栅格地图，则通过判断是否同时满足如下两个条件来判断该测量点是否在地图范围内：
步骤四，若该测量点同时满足上述两个条件，即该测量点位于地图范围内，则可以将世界坐标系下的2D整数索引对地图大小N求模，这样可以映射得到一个二维数组索引：
步骤五，通过将高度地图的长宽均设置为相同大小的2的指数，即N＝2n，则可以将上述索引映射步骤改为计算效率更高的二进制形式，即将求模运算“mod”改为二进制的按位与运算“&”：
本发明运动畸变去除的一种具体实施例：
对传感器坐标系下的测量点ps进行运动畸变去除的方法，包括以下步骤：
A1，将带有时间戳的当前帧点云放入点云队列，将带有时间戳的机身里程计位姿放入里程计队列；若当前点云队列不为空，则从前往后遍历里程计队列，维护里程计队列的队首帧时间戳与点云起始点时间戳对齐；
A2，对于当前帧点云，设起始点的时间戳为ts，结束点的时间戳为te，将结束点时间戳作为当前帧点云的时间戳，记为tc＝te；
A3，从前往后遍历里程计队列，从中找出恰好位于点云时间戳前后的相邻两帧里程计位姿，设为和/>其满足tk≤tc<tk+1；则可通过线性插值的方法计算点云时间戳处的机身里程计位姿：
A4，按照时间戳从起始点往后遍历点云队列中的每一个点，设当前遍历到的一个原始测量点为其时间戳为tp，则可由该时刻相邻两帧里程计位姿线性插值得到计算该测量点时刻的机身里程计位姿，记为/>
A5，将原始测量点通过位姿变换，将其由tp时刻的传感器坐标系变换到统一的当前帧点云时间戳tc时刻的传感器坐标系：/>
从而得到去除运动畸变后的当前帧点云
本发明拖尾点云去除的一种具体实施例：
对当前帧点云进行拖尾点云去除的步骤包括：
B1，计算点云中一点ps在传感器坐标系下的极坐标：
其中，sr是向量ps的模长，sp是向量ps与传感器坐标系Z轴正向的夹角，sa是向量ps在传感器坐标系XY平面的投影向量与X轴正向的夹角；
B2，在极坐标系下，将所有的点均投影到sr＝1的球面上，然后选定特定的球面矩形区域以剔除投影到其中的拖尾点云：
本发明进行腿部点云去除的一种具体实施例：
对当前帧点云进行腿部点云去除的步骤包括：
C1，将膝关节在传感器坐标系下的位置Ao和足端在传感器坐标系下的位置Bo，分别投影到传感器坐标系下半径等于1的球面上，投影后的两点分别为A和B，则有：
由向量SA和向量SB构建一组标准正交基：
C2，设点P在该标准正交基的XY平面中投影的垂足为H＝[hx,hy,hz]T，则该垂足为：
在该标准正交基下，计算一个激光点P的球面坐标(1,θ,φ)：
C3，腿部连杆投影在归一化球面上为一个矩形，通过如下公式判断点P是否在该矩形范围内：
/>
其中，b为腿部连杆投影后的最大宽度，l为腿部连杆投影后的最大长度；若点P在该矩形范围内，则去除。
如图2所示，本发明方法的一种较佳具体实施例。
为了准确地对本发明进行描述，首先对坐标系及常用符号进行定义。
首先，本发明主要包括四个参考坐标系，均使用右手坐标系，分别为惯性世界坐标系{W}，机器人机身坐标系{B}，传感器坐标系{S}以及地图坐标系{M}，具体如图3所示。其中：
世界坐标系{W}与地形环境保持相对静止，其xy坐标轴平面与水平面保持一致；
机身坐标系{B}为与机器人机体固连的一个固定坐标系，一般为机器人旋转的中心位置；
传感器坐标系{S}为传感器测量值的参考坐标系；
地图坐标系{M}的xyz三个坐标轴朝向与世界坐标系保持一致，其xy平面与世界坐标系重合，其坐标系原点的x和y跟随机身坐标系移动使得地图中心栅格始终与机身坐标系重合。
由于传感器采集的每一个点的时间戳不同，对应该测量点时间戳处的传感器坐标系在世界坐标系下的位姿也不同。因此，原始采集的一帧点云包含了由于传感器运动所带来的畸变，即运动畸变。
运动畸变去除的步骤如下：
首先，将带有时间戳的当前帧点云放入点云队列cloudQueue，将带有时间戳的机身里程计位姿放入里程计队列odomQueue。
第二步，如果当前点云队列不为空，则从前往后遍历里程计队列，维护里程计队列的队首帧时间戳与点云起始点时间戳对齐——即如果里程计队列的队首帧时间戳小于点云起始点时间戳，则将其移出队列。
第三步，对于当前帧点云，设其按照时间先后的第一个点(即起始点)的时间戳为ts，最后一个点的时间戳为te(即结束点)。将结束点时间戳作为当前帧点云的时间戳，记为tc＝te。
第四步，从前往后遍历里程计队列，从中找出恰好位于点云时间戳前后的相邻两帧里程计位姿，设为和/>其满足tk≤tc<tk+1。然后，可以使用线性插值的方法，计算得到点云时间戳处的机身里程计位姿，这里简写为：
第五步，按照时间戳从起始点往后遍历点云中的每一个点。设当前遍历到的一个原始测量点为其时间戳为tp，则与上式类似，可以通过线性插值得到该测量点时刻的机身里程计位姿，记为/>
第六步，对于当前遍历的原始测量点为可以通过位姿变换将其由tp时刻的传感器坐标系变换到统一的tc时刻的传感器坐标系，从而实现运动畸变地去除。计算如下：
球坐标系下的拖尾点云滤除和腿部点云滤除的方法，包括以下内容：
对于去除运动畸变后的当前帧点云还需要进一步地去除掉其中与环境信息无关的点，这里主要包括机身点云、腿部点云和拖尾点云等。
对于去除运动畸变后的当前帧点云中的一点首先可以计算其在传感器坐标系{S}下的极坐标：
其中，sr是向量ps的模长，sp是向量ps与传感器坐标系Z轴正向的夹角，sa是向量ps在传感器坐标系XY平面的投影向量与X轴正向的夹角。
在极坐标系下，可以将所有的点均投影到sr＝1的球面上，然后选定特定的球面矩形区域以剔除投影到其中的拖尾点云，这可以很好地去除掉拖尾点云。示例如下：
为了剔除四足机器人腿部的点云，首先基于机器人正向运动学，由电机关节角度计算得到各关节在机身坐标系下的位置，然后将其变换到传感器坐标系下。
如图4所示，这里以左前方小腿关节为例，设其膝关节和足端在传感器坐标系下的位置分别为Ao和Bo，先将其投影到传感器坐标系{S}下半径等于1的球面上。设投影后的两点分别为A和B，则有：
则可以由向量SA和向量SB构建一组标准正交基：
设点P在该标准正交基的XY平面中投影的垂足为H＝[hx,hy,hz]T，则该垂足为：
在该标准正交基下，可以计算一个激光点P的球面坐标(1,θ,φ)：
假设小腿连杆投影后在归一化球面上近似为一个矩形，则A和B为该矩形的中轴线。设投影后的连杆最大宽度为b，连杆最大长度为l，则可以通过如下公式判断点P是否在该矩形范围内。
如果该测量点在该矩形内，则认为该测量点属于腿部点，从而将其从点云中剔除。
拖尾点云及腿部点云去除前后的效果如图5和图6所示。
基于循环数组的高度地图模型，包括以下内容：
常用的地图模型包括2D栅格地图、3D栅格地图、点云地图、八叉树地图以及拓扑地图等多种形式。其中，八叉树地图内存占用较小，但元素访问效率较低，其是地图节点数目的对数；相反地，栅格地图的内存占用较高，但元素访问时间为常值，访问效率很高。考虑到地图的存储效率和索引效率的平衡，这里选取了2D栅格地图模型来表征环境信息，并构建了一种高效的基于循环数组的高度地图构建方法。
对于计算机来说，不管是什么样的地图模型，其存储方式最后都是线性存储的。为了构建2.5D高度地图，首先要定义在计算机中存储和访问地图元素的数据结构。具体地，该数据结构应该具有如下功能：
给定空间中的一个三维点，可以通过计算得到其在高度地图中的对应栅格索引，并可以进一步得到其在计算机内存中的存储索引；
将整张地图朝某个方向移动一个体素的距离；
判断一个点是否在体素地图内；
如图7所示，这里定义2.5D高度地图的几何结构如下：-地图分辨率为r，即一个正方形栅格的边长为r；-一张高度地图为一个由N×N个栅格构成的正方形区域；-地图坐标系坐标轴方向与世界坐标系保持一致，仅在x和y方向移动；-地图坐标系的原点为当前高度地图的所有栅格中x和y两坐标均最小的那个栅格，记为Om；
判断世界坐标系下一点是否位于地图范围内，并将其映射到地图中对应2D栅格及其一维数组索引的步骤如下：
设世界坐标系下的一点为则将其x和y分量分别除以地图分辨率r，再向下取整，可以得到该测量点在世界坐标系下的2D整数索引：
设当前地图坐标系原点在世界坐标系下的2D整数索引为则可以计算得到该测量点在地图坐标系下的2D整数索引：
则可通过判断是否同时满足如下两个条件来判断该测量点是否在地图范围内：
如果该测量点同时满足上述两个条件，即该测量点位于地图范围内，则可以将世界坐标系下的2D整数索引对地图大小N求模，这样可以映射得到一个二维数组索引：
/>
通过将高度地图的长宽均设置为相同大小的2的指数，即N＝2n，则可以将上述索引映射步骤改为计算效率更高的二进制形式，即将求模运算“mod”改为二进制的按位与运算“&”：
最后，将该二维数组索引唯一地映射到一个一维数组索引Ia，其对应该栅格在计算机中的存储地址：
这里，以上图中的高度地图为例，该地图大小为N＝23＝8。该地图中的深色栅格在世界坐标系下的整数索引为在地图坐标系下的整数索引为/> 二维数组索引为/>一维数组索引为Ia＝5×8+1＝41。
此外，当机器人移动时，需要控制地图坐标系移动使得机器人坐标系原点始终保持在地图中心竖直正上方。如图所示，当地图坐标系由Om1移动到Om2时，移动前后重叠部分的栅格区域(蓝色栅格)在其存储中的二维数组索引和一维数组索引均不变，因此无需在存储数组中对其进行移动，这样可以节省大量的计算资源；移动后新增的栅格(右上角斜线区域)和移动后丢弃的栅格(左下角斜线区域)在二维数组中的索引刚好一一对应，因此只需要将新增栅格的值存储在丢弃栅格的地址上即可。例如，如图8中箭头所示，移动后地图Om2中最右上角的栅格的二维数组索引为(3,1)，其刚好对应移动前地图Om1中的最下一排第2个栅格。
高度地图的构建，包括以下内容：
对于完成预处理步骤后的点云，将其中的每一个点均作为一次测量，来更新其在高度地图中的对应栅格。对于高度地图中的每一个栅格，使用高斯分布模型来表征该栅格的高度估计值：
其中，和/>分别为高斯分布的均值和方差。
设传感器坐标系下的一个测量点为ps，则可以利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下：
由公式(10)-(14)，可以将该测量点从世界坐标系下映射得到其对应的二维数组索引其对应的高度值为测量点/>的z轴分量，即为当前栅格的一次高度测量值。定义投影矩阵Mp＝[0,0,1]，则可以得到当前次高度测量值：
因为传感器坐标系到机器人坐标系的位姿变换是标定得到的固定值，所以无需考虑它的方差。设在传感器坐标系下的测量点ps也满足高斯分布，则高度估计的方差传播主要取决于传感器测量ps的方差、机身里程计的旋转量的方差以及机身里程计平移量/>的方差。可以推导得到高度测量值/>关于ps、/>和/>的雅克比矩阵：
然后，由协方差传播计算得到当前高度测量值的方差：
其中，∑p是传感器坐标系下测量点的协方差矩阵，其大小取决于传感器测量噪声模型；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵。
于是得到当前次高度测量的高斯分布：
最后，可以使用卡尔曼滤波的方法，将当前次高度测量融合到当前栅格的高度估计中，得到更新后的高度估计：
本发明所建立的高度地图效果如图9和图10所示。
应用本发明方法的一种设备实施例：
一种计算机设备，其包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的一种用于足式机器人运动规划的高度地图构建方法。
应用本发明方法的一种计算机介质实施例：
一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的一种用于足式机器人运动规划的高度地图构建方法。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。
Claims (7)
1.一种用于足式机器人运动规划的高度地图构建方法，其特征在于，
包括如下步骤：
第一步，获取地图规划区域的原始测量点数据；
第二步，利用预先构建的运动畸变去除模型，将原始测量点数据进行位姿变换，得到去除运动畸变后的当前帧点云；
第三步，通过预先构建的点云滤除模型，对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；
点云滤除模型，对当前帧点云进行处理的方法如下：
对于去除运动畸变后的当前帧点云中的一点计算其在传感器坐标系{S}下的极坐标：
其中，sr是向量ps的模长，sp是向量ps与传感器坐标系Z轴正向的夹角，sa是向量ps在传感器坐标系XY平面的投影向量与X轴正向的夹角；
在极坐标系下，将所有的点均投影到sr＝1的球面上，然后选定特定的球面矩形区域以剔除投影到其中的拖尾点云，去除掉拖尾点云，其表达式如下：
为了剔除四足机器人腿部的点云，基于机器人正向运动学，由电机关节角度计算得到各关节在机身坐标系下的位置，然后将其变换到传感器坐标系下；
膝关节和足端在传感器坐标系下的位置分别为Ao和Bo，先将其投影到传感器坐标系{S}下半径等于1的球面上；设投影后的两点分别为A和B，则有：
则由向量SA和向量SB构建一组标准正交基：
点P在该标准正交基的XY平面中投影的垂足为H＝[hx,hy,hz]T，则该垂足为：
在该标准正交基下，计算一个激光点P的球面坐标(1,θ,φ)：
小腿连杆投影后在归一化球面上近似为一个矩形，则A和B为该矩形的中轴线；
投影后的连杆最大宽度为b，连杆最大长度为l，则通过如下公式判断点P是否在该矩形范围内；
如果该测量点在该矩形内，则认为该测量点属于腿部点，从而将其从点云中剔除，实现腿部点云地去除；
与环境信息无关的点包括机身点云或/和腿部点云或/和拖尾点云；
第四步，利用预先构建的高度地图模型，并根据机身里程计和/或视觉里程计和/或激光里程计位姿数据，对预处理点云进行处理，得到栅格高度值；
第五步，根据栅格高度值，建立或更新地图规划区域中的栅格，从而实现用于足式机器人运动规划的高度地图构建。
2.如权利要求1所述的一种用于足式机器人运动规划的高度地图构建方法，其特征在于，
所述第二步中，运动畸变去除模型对原始测量点数据进行运动畸变去除的方法如下：
步骤1，将带有时间戳的当前帧点云放入点云队列cloudQueue，将带有时间戳的机身里程计位姿放入里程计队列odomQueue；
步骤2，如果当前点云队列不为空，则从前往后遍历里程计队列，维护里程计队列的队首帧时间戳与点云起始点时间戳对齐，即如果里程计队列的队首帧时间戳小于点云起始点时间戳，则将其移出队列；
步骤3，对于当前帧点云，按照时间先后进行排序，第一个点即起始点的时间戳为ts，最后一个点的时间戳为te即结束点；
将结束点时间戳作为当前帧点云的时间戳，记为tc＝te；
步骤4，从前往后遍历里程计队列，从中找出恰好位于点云时间戳前后的相邻两帧里程计位姿，相邻两帧里程计位姿分别为和/>满足tk≤tc<tk+1；
然后，使用线性插值的方法，计算得到点云时间戳处的机身里程计位姿，其表达式如下：
步骤5，按照时间戳从起始点往后遍历点云中的每一个原始测量点；通过线性插值得到该原始测量点时刻的机身里程计位姿，记为
步骤6，对于当前遍历的原始测量点为通过位姿变换将其由tp时刻的传感器坐标系变换到统一的tc时刻的传感器坐标系，从而实现运动畸变地去除；其计算公式如下：
3.如权利要求1-2任一所述的一种用于足式机器人运动规划的高度地图构建方法，其特征在于，
所述第四步中，高度地图模型，将预处理点云的每一个测量点均作为一次测量，以建立或更新地图规划区域中的栅格，其对预处理点云进行处理的方法如下：
对于地图规划区域中的每一个栅格，使用高斯分布模型来表征该栅格的高度估计值：
其中，和/>分别为高斯分布的均值和方差；
传感器坐标系下的一个测量点为ps，则利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下：
通过地图索引判断公式组，将该测量点从世界坐标系下映射得到其对应的二维数组索引其对应的高度值为测量点/>的z轴分量，即为当前栅格的一次高度测量值；
建立投影矩阵Mp＝[0,0,1]，得到当前次高度测量值：
传感器坐标系到机器人坐标系的位姿变换是标定得到的固定值；
在传感器坐标系下的测量点ps满足高斯分布，其高度估计的方差传播主要取决于传感器测量ps的方差、机身里程计的旋转量的方差以及机身里程计平移量/>的方差；
推导得到高度测量值关于ps、/>和/>的雅克比矩阵：
然后，由协方差传播计算得到当前高度测量值的方差：
其中，∑p是传感器坐标系下测量点的协方差矩阵，其大小取决于传感器测量噪声模型；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵；
从而得到当前次高度测量的高斯分布：
使用卡尔曼滤波的方法，将当前次高度测量融合到当前栅格的高度估计中，得到更新后的栅格高度值：
最后，根据栅格高度值，完成能用于足式机器人运动规划的高度地图构建。
4.一种用于足式机器人运动规划的高度地图构建方法，其特征在于，
应用如权利要求1-3任一所述的一种用于足式机器人运动规划的高度地图构建方法，其包括以下步骤：
S1：获取通过激光雷达采集的一个传感器坐标系下的测量点ps，利用机身里程计位姿估计以及标定得到的机身坐标系到传感器坐标系的位姿变换/>将该测量点从传感器坐标系变换到世界坐标系下，其测量点的世界坐标值为pw：
S2：将该测量点从世界坐标系下映射到其在高度地图中的对应栅格，其对应的高度值为测量点的z轴分量，即为当前栅格的一次高度测量值；测量高度值满足高斯分布/>并构建投影矩阵Mp＝[0,0,1]，则得到当前次高度测量均值：
由协方差传播计算得到当前高度测量值的方差：
其中，Jp、JR和Jt是高度测量值关于ps、/>和/>的雅克比矩阵；∑p是传感器坐标系下测量点的协方差矩阵；∑R是机身里程计旋转量的协方差矩阵；∑t是机身里程计平移量的协方差矩阵；
S3：根据当前次高度测量均值以及当前高度测量值的方差，构建当前高度测量值的高斯分布；
栅格的高度估计满足高斯分布则通过计算两个高斯分布的归一化积，将当前次高度测量融合到当前栅格的高度估计中，得到更新后的高度估计：
S4：根据更新后的高度估计，构建用于足式机器人运动规划的稠密高度地图。
5.如权利要求4所述的一种用于足式机器人运动规划的高度地图构建方法，其特征在于，
将测量点从世界坐标系下映射到其在高度地图中的对应栅格，即将测量点pw从世界坐标系下映射得到其在高度地图中对应栅格所属的二维数组索引其步骤如下：
步骤一，世界坐标系下的测量点为则将其x和y分量分别除以地图分辨率r，再向下取整，得到该测量点在世界坐标系下对应栅格的2D整数索引：
步骤二，当前地图坐标系原点在世界坐标系下的2D整数索引为则计算得到该测量点在地图坐标系下对应栅格的2D整数索引：
步骤三，高度地图为一张N×N大小的栅格地图，则通过判断当前点的索引是否同时满足如下两个条件来判断其是否在高度地图范围内：
步骤四，若该测量点同时满足上述两个条件，即该测量点位于地图范围内，则将世界坐标系下的2D整数索引对地图大小N求模，将该测量点映射到一个在计算机中存储的二维数组索引：
步骤五，通过将高度地图的长宽均设置为相同大小的2的指数，即N＝2n，将索引映射步骤改为计算效率高的二进制形式，即将求模运算“mod”改为二进制的按位与运算“&”：
6.一种用于足式机器人运动规划的高度地图构建系统，其特征在于，
其包括激光雷达、运动畸变去除模块、点云滤除模块、高度地图模块、一个或多个处理器、存储装置；
激光雷达，用于采集原始测量点数据；
运动畸变去除模块，用于根据参考坐标系的转换特性，对原始测量点数据进行处理，将原始测量点数据，在某时刻的传感器坐标系变换到统一时刻的传感器坐标系，得到去除运动畸变后的当前帧点云；
点云滤除模块，用于对当前帧点云进行处理，去除掉其中与环境信息无关的点，得到预处理点云；
高度地图模块，用于对预处理点云进行处理，得到栅格高度值；并更新预处理点云在高度地图中的对应栅格；
存储装置，用于存储一个或多个模块的程序；
当所述程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现采用如权利要求1-5任一所述的一种用于足式机器人运动规划的高度地图构建方法。
7.一种计算机可读存储介质，其特征在于，
其上存储有计算机程序，该程序被处理器执行时实现采用如权利要求1-5任一所述的一种用于足式机器人运动规划的高度地图构建方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310202525.2A CN116358517B (zh) | 2023-02-24 | 2023-02-24 | 一种用于机器人的高度地图构建方法、系统以及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310202525.2A CN116358517B (zh) | 2023-02-24 | 2023-02-24 | 一种用于机器人的高度地图构建方法、系统以及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116358517A CN116358517A (zh) | 2023-06-30 |
| CN116358517B true CN116358517B (zh) | 2024-02-23 |
Family
ID=86926645
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310202525.2A Active CN116358517B (zh) | 2023-02-24 | 2023-02-24 | 一种用于机器人的高度地图构建方法、系统以及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116358517B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116878488B (zh) * | 2023-09-07 | 2023-11-28 | 湘江实验室 | 一种建图方法、装置、存储介质及电子装置 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110223379A (zh) * | 2019-06-10 | 2019-09-10 | 于兴虎 | 基于激光雷达的三维点云重建方法 |
| CN111596665A (zh) * | 2020-05-29 | 2020-08-28 | 浙江大学 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
| CN112015186A (zh) * | 2020-09-09 | 2020-12-01 | 上海有个机器人有限公司 | 一种具有社会属性的机器人路径规划方法、装置和机器人 |
| WO2021237667A1 (zh) * | 2020-05-29 | 2021-12-02 | 浙江大学 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
| CN113960614A (zh) * | 2021-09-22 | 2022-01-21 | 浙江大学 | 一种基于帧-地图匹配的高程图构建方法 |
| JP2022039906A (ja) * | 2020-08-28 | 2022-03-10 | 中国計量大学 | マルチセンサによる複合キャリブレーション装置及び方法 |
| CN114359499A (zh) * | 2022-01-11 | 2022-04-15 | 东北大学 | 基于旋转雷达和imu的巷道三维重建系统及方法 |
| CN115330866A (zh) * | 2022-08-09 | 2022-11-11 | 杭州云深处科技有限公司 | 一种融合激光雷达和深度相机点云的建图定位方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201116959D0 (en) * | 2011-09-30 | 2011-11-16 | Bae Systems Plc | Vehicle localisation with 2d laser scanner and 3d prior scans |
-
2023
- 2023-02-24 CN CN202310202525.2A patent/CN116358517B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110223379A (zh) * | 2019-06-10 | 2019-09-10 | 于兴虎 | 基于激光雷达的三维点云重建方法 |
| CN111596665A (zh) * | 2020-05-29 | 2020-08-28 | 浙江大学 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
| WO2021237667A1 (zh) * | 2020-05-29 | 2021-12-02 | 浙江大学 | 一种适用于腿足机器人规划的稠密高度地图构建方法 |
| JP2022039906A (ja) * | 2020-08-28 | 2022-03-10 | 中国計量大学 | マルチセンサによる複合キャリブレーション装置及び方法 |
| CN112015186A (zh) * | 2020-09-09 | 2020-12-01 | 上海有个机器人有限公司 | 一种具有社会属性的机器人路径规划方法、装置和机器人 |
| CN113960614A (zh) * | 2021-09-22 | 2022-01-21 | 浙江大学 | 一种基于帧-地图匹配的高程图构建方法 |
| CN114359499A (zh) * | 2022-01-11 | 2022-04-15 | 东北大学 | 基于旋转雷达和imu的巷道三维重建系统及方法 |
| CN115330866A (zh) * | 2022-08-09 | 2022-11-11 | 杭州云深处科技有限公司 | 一种融合激光雷达和深度相机点云的建图定位方法 |
Non-Patent Citations (3)
| Title |
|---|
| 基于ORB-SLAM2的四足机器人定位和建图;张政等;《工业控制计算机》;第35卷(第10期);78-84 * |
| 基于局部地图构建的四足机器人自主步态规划技术研究;郭冠洋;《中国优秀硕士学位论文全文数据库 信息科技辑》;20210215;I140-628 * |
| 基于运动灵活性的蟑螂机器人机构参数优化;张建斌;宋荣贵;陈伟海;张广萍;;北京航空航天大学学报(05);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116358517A (zh) | 2023-06-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110176032B (zh) | 一种三维重建方法及装置 | |
| JP7300550B2 (ja) | 視覚標識に基づき標識マップを構築する方法、装置 | |
| CN107588721A (zh) | 一种基于双目视觉的零件多尺寸的测量方法及系统 | |
| CN106997614B (zh) | 一种基于深度相机的大规模场景3d建模方法及其装置 | |
| CN110189400B (zh) | 一种三维重建方法、三维重建系统、移动终端及存储装置 | |
| CN107953329A (zh) | 物体识别和姿态估计方法、装置及机械臂抓取系统 | |
| JP2012026895A (ja) | 位置姿勢計測装置、位置姿勢計測方法、およびプログラム | |
| CN113034571B (zh) | 一种基于视觉—惯性的物体三维尺寸测量方法 | |
| CN116358517B (zh) | 一种用于机器人的高度地图构建方法、系统以及存储介质 | |
| CN113077552A (zh) | 基于无人机影像的dsm生成方法和装置 | |
| CN116030202A (zh) | 一种三维图像重建方法、装置、电子设备及存储介质 | |
| CN112700505B (zh) | 一种基于双目三维跟踪的手眼标定方法、设备及存储介质 | |
| CN111899277A (zh) | 运动目标检测方法及装置、存储介质、电子装置 | |
| CN115631317B (zh) | 隧道衬砌正射影像生成方法及装置、存储介质、终端 | |
| CN110853103A (zh) | 一种用于深度学习姿态估计的数据集制作方法 | |
| CN115560744A (zh) | 机器人以及基于多传感器的三维建图方法、存储介质 | |
| CN115409903A (zh) | 相机标定方法、设备及计算机存储介质 | |
| CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
| JP2019040445A (ja) | 推定装置及びプログラム | |
| CN108151712B (zh) | 一种人体三维建模及测量方法和系统 | |
| CN111815684A (zh) | 一种基于统一残差模型的空间多元特征配准优化方法及装置 | |
| CN116625242B (zh) | 光学三坐标测量机路径规划方法、系统、电子设备及介质 | |
| CN115222799B (zh) | 图像重力方向的获取方法、装置、电子设备及存储介质 | |
| Jimeno-Morenilla et al. | Landmark registration for reverse design: a case study on footwear rapid prototyping | |
| WO2023015915A1 (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 |