【技术实现步骤摘要】
本专利技术属于足式机器人，尤其是涉及一种足式机器人局部地形感知方法。
技术介绍
1、足式机器人需要感知身体下方以及周围的地形信息，从而在楼梯等非平地场景进行落脚点规划和避障。由于点云传感器的视场角限制，以及身体下方腿部的干扰，直接观测身体下方的地形并不容易。目前全世界比较有名的足式机器人厂商，例如波士顿动力、anybotics采用的是依赖里程计进行地形累积拼接的方法，即机器人的点云传感器不直接观测身体下方地形，只观测前方或周围地形。机器人依赖高精度的激光/视觉里程计实时估计自身位移和姿态角，在运动过程中用多帧历史点云拼接出完整的地形，即目前身体下方的地形是之前还没到这个位置时扫描过的。这种方案存在以下缺点：1)计算量大；2)对里程计具有较高的精度要求；3)在特定场景下，里程计会失效，例如激光里程计会在过于开阔、狭窄，结构特征较少的场景失效，视觉里程计会在黑暗场景或无纹理场景失效。
技术实现思路
1、为了克服现有技术的不足，本专利技术提供一种足式机器人局部地形感知方法，其不依赖高精度里程计，直接观测身体下方、周围地形，感知更准确。
2、本专利技术解决其技术问题所采用的技术方案是：一种足式机器人局部地形感知方法，包括以下步骤：
3、s1，在足式机器人上设置至少一个激光雷达，并获取激光雷达相对足式机器人本体坐标系的外参，经激光雷达扫描得到的点云转换至足式机器人本体坐标系，定期更新扫描得到的点云；
4、s2，对步骤s1得到的点云去除运动畸变，并转换至足式
5、s3，将步骤s2得到的点云降维成高度信息矩阵；
6、s4，基于步骤s3得到的高度信息矩阵进行空洞补全；
7、s5，基于步骤s4得到的高度信息矩阵去除足式机器人腿部点云对高度矩阵造成的影响；
8、s6，对步骤s5处理后的高度信息矩阵进行分析，计算坡度信息矩阵和障碍物信息矩阵，用于足式机器人的落脚点规划以及避障。
9、进一步的，所述步骤s1包括如下子步骤：
10、s11，在足式机器人上设置至少一个激光雷达，以使得足式机器人的周围及躯干下方均处于扫射范围；
11、s12，定义足式机器人本体坐标系，坐标系原点为足式机器人的任意位置，其中x轴朝足式机器人正前方，y轴朝足式机器人正左方，z轴朝足式机器人垂直上方，外参包括旋转矩阵和位移向量b表示足式机器人本体坐标系，li表示第i个激光雷达；
12、s13，接收到点云数据后，通过以下公式将点云从激光雷达坐标系转换到足式机器人本体坐标系：
13、
14、其中为第i个激光雷达的测量点在该雷达坐标系下的坐标，xb、yb、zb为测量点转换到足式机器人本体坐标系后的坐标；
15、s14，设定收集点云的周期t，每隔t时间，把激光雷达扫描的点云在经过步骤s13处理后，合并到同一点云中。
16、进一步的，
17、所述步骤s2包括如下子步骤：
18、s21，足式机器人上安装imu，激光雷达与可获取imu姿态角的处理器连接，且imu的安装姿态与足式机器人标本系一致，以用imu代表四足机器人的姿态；
19、s22，结合关节电机编码器和imu估计足式机器人的位移；
20、s23，将不低于100hz频率的imu姿态数据和足式机器人位移里程计数据输入点云处理模块，结合点云每个点的时间戳，对点云去除运动畸变，使处理后每个点的时间戳均为点云末尾点的时间：
21、
22、其中xb_trans、yb_trans、zb_trans为去除运动畸变后的点在足式机器人本体坐标系下的坐标,分别表示第j个点时间戳时足式机器人本体坐标系相对到点云末尾时间戳时足式机器人本体坐标系的旋转矩阵和位移向量；
23、s24，定义足式机器人重力坐标系，它的原点与足式机器人本体坐标系原点重合，z轴与重力方向相反，x轴与重力方向垂直，且与足式机器人本体坐标系的x轴处于同一竖直平面，y轴与x轴、z轴分别垂直，方向满足右手定则；
24、s25，根据点云末尾时间戳对应的imu姿态，将步骤s23中去除运动畸变后的点云从足式机器人本体坐标系转换到足式机器人重力坐标系：
25、
26、其中xg、yg、zg为转换到四足机器人重力坐标系下的坐标，为旋转矩阵，它由以下方式得到：
27、
28、将点云末尾时间戳对应的imu姿态欧拉角转换为旋转矩阵，即为将欧拉角中的航向角置零，俯仰角、横滚角不变，再转换为旋转矩阵，即为
29、s26，对步骤s25得到的点云进行直通滤波，在四足机器人重力坐标系的x、y、z方向截取目标范围内的点云保留，舍弃范围之外的点云。
30、由于同一帧激光雷达的点云不是在同一时刻采样的，而激光雷达伴随机器人运动，因此原始点云数据是有畸变的，需要用算法去除运动畸变；点云转换的机器人重力坐标系，可以保证机器人倾斜时，地面和斜坡等地形的倾角与实际相同；只保留设定范围内的点云可以减小计算量。
31、进一步的，所述步骤s3包括如下子步骤：
32、s31，定义用于储存地形高度信息的矩阵，定义一个m行m列的空矩阵，保存四足机器人周围的地形高度信息，矩阵的中心对应四足机器人重力坐标系原点，矩阵的每个元素代表一块n厘米×n厘米的方格，其中行表示x坐标轴方向，列表示y坐标轴方向；
33、s32，将三维点云降维成高度信息矩阵，遍历步骤s2得到的点云，根据每个点的x、y坐标确定该点在矩阵中所属的元素，如果该元素为空，则将当前点的z坐标作为高度赋值给该元素，如果该元素已经被赋值，那么比较元素值和当前点z坐标的大小，若元素值更小，则将元素值更新为当前点z坐标。
34、点云降维成高度信息矩阵，将三维信息降至二维，可以减少数据量，并使得无顺序的数据变成有顺序的数据，方便后期处理。
35、进一步的，所述步骤s4包括如下子步骤：
36、s41，寻找高度矩阵的空元素，用空洞边界的元素高度作为空洞填补的备选高度，遍历步骤s3得到的高度信息矩阵，对于自身有高度值的元素，如果其前、后、左、右的邻居存在高度值为空的元素，则认为当前元素为空洞边缘元素，从空洞边缘元素向空邻居的方向逐一访问各个元素，直至访问到高度值非空的元素，或访问次数达到预设的上限值，或已访问到矩阵边缘，如果访问到了高度值非空的元素，则与当前空洞边缘元素比较高度差，若高度差绝对值小于预设的阈值，则在一个空洞记录容器中记录较小的高度值作为空元素候选高度，并记录逐一访问过程中的所有空元素行、列坐标；
37、s42，在多个备选高度中选择合适高度，空洞记录容器中的空元素坐标可能存在重复，遍历所有容器中的空元素坐标，在高度信息矩阵中访问该元素，若目前仍为空，则将对于的候选高度赋值给该元素，若已被之前的记录赋过高度值，则将目前的高度值与本文档来自技高网...
【技术保护点】
1.一种足式机器人局部地形感知方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S1包括如下子步骤：
3.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S2包括如下子步骤：
4.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S3包括如下子步骤：
5.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S4包括如下子步骤：
6.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S5包括如下子步骤：
7.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤S6包括如下子步骤：
8.根据权利要求7所述的足式机器人局部地形感知方法，其特征在于：所述步骤S65中，从矩阵中心为起点，预设多个搜索方向，多个搜索方向覆盖360°范围，根据矩阵尺寸，预设每个搜索方向的搜索宽度为w，搜索步长为n，与一个元素对应的格子宽度一致，每搜索一个方向时，先得到一条中点在矩阵中心，
9.根据权利要求7所述的足式机器人局部地形感知方法，其特征在于：所述步骤S66中，在每个搜索方向开始搜索时，先设立一个历史搜索队列，队列最大长度为L_queue＝l_Max/n，并向下取整，代表足式机器人最大行走步长对应的搜索步数，队列末端有新的值进入，且队列超过最大长度时，会把队首的值删除。
10.根据权利要求9所述的足式机器人局部地形感知方法，其特征在于：所述步骤S66中，设立一个起始高度，作为队列第0个值，起始高度为足式机器人站立时距离地面高度的相反数；对于每一步搜索的若干格子，如果高度>历史搜索队列中的最大值+h_Max，则认为该格子是障碍物，将障碍物信息矩阵对应元素记为1；如果高度<＝历史搜索队列最大值+h_Max，则认为该格子可通行，不是障碍物，此时，若对应坡度小于s_Max，认为该格子可落脚，将障碍物信息矩阵对应元素记为0，若对应坡度大于s_Max，认为该格子可通过但不可落脚，将障碍物信息矩阵对应元素记为0.1；每完成一个搜索步长，在历史搜索队列末端加入本步搜索到的所有可落脚格子的高度最大值。
...【技术特征摘要】
1.一种足式机器人局部地形感知方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s1包括如下子步骤：
3.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s2包括如下子步骤：
4.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s3包括如下子步骤：
5.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s4包括如下子步骤：
6.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s5包括如下子步骤：
7.根据权利要求1所述的足式机器人局部地形感知方法，其特征在于，所述步骤s6包括如下子步骤：
8.根据权利要求7所述的足式机器人局部地形感知方法，其特征在于：所述步骤s65中，从矩阵中心为起点，预设多个搜索方向，多个搜索方向覆盖360°范围，根据矩阵尺寸，预设每个搜索方向的搜索宽度为w，搜索步长为n，与一个元素对应的格子宽度一致，每搜索一个方向时，先得到一条中点在矩阵中心，宽度为w，且与搜索方向垂直的线段，该线段经过的所有格子对应的矩阵元素，即为第0步的搜索元素，每搜索一...
【专利技术属性】
技术研发人员：黄俊杰，李超，唐威，胡雪亮，储振，陈申红，朱秋国，
申请(专利权)人：杭州云深处科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。