[发明专利]基于视觉感知的四足机器人楼梯攀爬规划方法及其应用

权利要求书

1.基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，包括：

获取空间三维点云中的楼梯几何参数；

根据所述楼梯几何参数和目标机器人尺寸计算出目标机器人的预设参考速度；

根据所述楼梯几何参数和所述预设参考速度计算出所述目标机器人的预设落脚位置；

根据所述楼梯几何参数对所述目标机器人进行位姿矫正，实时计算目标机器人的目标姿态；

根据所述预设参考速度、所述预设落脚位置和所述目标姿态规划出所述目标机器人的楼梯攀爬轨迹。

2.根据权利要求1所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述根据所述预设参考速度、所述预设落脚位置和所述目标姿态规划出所述目标机器人的楼梯攀爬轨迹，包括：

根据所述预设参考速度和所述预设落脚位置计算出目标控制变量；

根据所述目标控制变量计算出所述目标机器人的关节力矩；

根据所述关节力矩和所述目标姿态规划实现所述目标机器人的楼梯攀爬控制。

3.根据权利要求1所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述获取空间三维点云中的楼梯几何参数，包括：

对所述空间三维点云进行去噪处理，得到目标三维点云；

将所述目标三维点云分割成平面点云；

根据所述平面点云和楼梯结构信息提取出所述楼梯几何参数。

4.根据权利要求1所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述根据所述楼梯几何参数和目标机器人尺寸计算出目标机器人的预设参考速度，包括：

根据所述楼梯几何参数定义所述目标机器人的预设足尖长度；

根据预设步态周期、所述目标机器人尺寸和所述预设足尖长度计算出所述预设参考速度。

5.根据权利要求1所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述根据所述楼梯几何参数和所述预设参考速度计算出所述目标机器人的预设落脚位置，包括：

获取所述目标机器人的中心实时位置；

根据所述中心实时位置和所述预设参考速度计算出名义落脚位置；

根据所述楼梯几何参数对所述名义落脚位置进行修正，得到所述预设落脚位置。

6.根据权利要求5所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述根据所述楼梯几何参数对名义落脚位置进行修正，得到所述预设落脚位置，包括：

获取所述目标机器人的实时足尖位置；

从所述楼梯几何参数中提取出楼梯中线位置；

利用二次规划优化方法根据所述楼梯中线位置和所述实时足尖位置对所述名义落脚位置进行修正，得到所述预设落脚位置。

7.根据权利要求1所述的基于视觉感知的四足机器人楼梯攀爬规划方法，其特征在于，所述根据所述楼梯几何参数对所述目标机器人进行位姿矫正，实时计算目标机器人的目标姿态，包括：

根据所述楼梯几何参数建立所述目标机器人的实时位置对应的第一局部坐标系、第二局部坐标系和第三局部坐标系；

根据所述第一局部坐标系计算得到第一位置，根据所述第二局部坐标系计算得到第二位置，根据所述第三局部坐标系计算得到第三位置；

根据所述第一位置、所述第二位置和所述第三位置对所述目标机器人进行位姿矫正，得到所述目标姿态。

8.基于视觉感知的四足机器人楼梯攀爬规划装置，其特征在于，包括：

第一获取模块，用于获取空间三维点云中的楼梯几何参数；

第一计算模块，用于根据所述楼梯几何参数和目标机器人尺寸计算出目标机器人的预设参考速度；

第二计算模块，用于根据所述楼梯几何参数和所述预设参考速度计算出所述目标机器人的预设落脚位置；

矫正模块，用于根据所述楼梯几何参数对所述目标机器人进行位姿矫正，实时计算目标机器人的目标姿态；

规划模块，用于根据所述预设参考速度、所述预设落脚位置和所述目标姿态规划出所述目标机器人的楼梯攀爬轨迹。