[发明专利]一种双足机器人及其导航定位方法和装置

权利要求书

1.一种双足机器人的导航定位方法，其特征在于，所述双足机器人的导航定位方法包括：

根据预先标定的机器人定位点坐标系与机器人中轴线坐标系的位姿关系，确定机器人在机器人定位点坐标系中的定位点投影到机器人中轴线坐标系中的第一坐标，所述机器人定位点坐标系为根据机器人的定位点所在的位置所确定的坐标系，所述机器人中轴线坐标系是以机器人的中轴线所确定的坐标系；

将所述定位点投影在机器人中轴线坐标系中的第一坐标投影到预定的二维平面，得到机器人定位点在二维平面的第二坐标和航向角；

获取所述第二坐标所对应的机器人摆动周期内的平均坐标；

根据所述第二坐标的平均坐标以及所述定位点的航向角，确定所述机器人的导航点，根据所述导航点进行机器人导航规划；

根据所述第二坐标的平均坐标以及所述定位点的航向角，确定所述机器人的导航点，包括：

根据机器人在前一个摆动周期的第一平均坐标和航向角，以及机器人当前定位点的坐标和航向角，确定机器人运动状态为圆弧运动时所对应的圆心；

根据所述圆心位置，结合公式：确定机器人当前的导航点(x，y，θ)，其中，θ_C'为机器人振动到C'点处所对应的朝向，L_NC'为导航点N与振动点C'之间的距离。

2.根据权利要求1所述的双足机器人的导航定位方法，其特征在于，所述根据预先标定的机器人定位点坐标系与机器人中轴线坐标系的位姿关系，确定机器人在机器人定位点坐标系中的定位点投影到机器人中轴线坐标系中的第一坐标的步骤包括：

获取机器人定位点在预设的机器人定位点坐标系中的第三坐标；

根据机器人定位点坐标系与机器人中轴线坐标系的标定关系，确定机器人定位点坐标系与机器人中轴线坐标系之间的齐次变换矩阵；

根据所述齐次变换矩阵，将所述定位点在机器人定位点坐标系中的第三坐标变换为机器人中轴线坐标系中的第一坐标。

3.根据权利要求1所述的双足机器人的导航定位方法，其特征在于，所述机器人中轴线坐标系以机器人中轴线为Z轴，机器人前方为X轴，所构建的右手坐标系，所述将所述定位点投影在机器人中轴线坐标系中的第一坐标投影预定的二维平面，得到机器人定位点在二维平面的第二坐标和航向角的步骤包括：

获取定位点在机器人中轴线坐标系中的坐标(x,y,z)，以及获取机器人当前的朝向；

根据机器人中轴线坐标系中的坐标(x,y,z)确定机器人在二维平面的第二坐标为(x,y)，根据机器人的当前朝向确定为机器人航向角。

4.根据权利要求1所述的双足机器人的导航定位方法，其特征在于，所述获取所述第二坐标所对应的机器人摆动周期内的平均坐标的步骤包括：

获取机器人行走的摆动周期；

根据所述摆动周期，确定位于所述摆动周期内的机器人的定位点的第二坐标的集合；

根据所确定的第二坐标的集合计算平均值，得到平均坐标。

5.根据权利要求4所述的双足机器人的导航定位方法，其特征在于，所述获取所述第二坐标所对应的机器人摆动周期内的平均坐标的步骤包括：

获取机器人的当前定位点所在摆动周期的前一个摆动周期；

获取所述前一个摆动周期内的定位点的第二坐标的集合；

根据所述第二坐标的集合计算均值得到第一平均坐标。

6.根据权利要求4所述的双足机器人的导航定位方法，其特征在于，所述获取所述第二坐标所对应的机器人摆动周期内的平均坐标的步骤包括：

根据机器人当前位置，确定以前位置为周期结束位置的摆动周期；

获取所述摆动周期内的定位点的第二坐标的集合，计算所述第二坐标的集合得到所述平均坐标；

所述根据所述第二坐标的平均坐标以及所述定位点的航向角，确定所述机器人的导航点的步骤包括：

根据所述平均坐标作为所述机器人在当前时刻的导航点。