多自由度单腿运动学求解方法、装置和机器人与流程

技术特征：
1.一种多自由度单腿运动学求解方法，其特征在于，所述单腿包括依次设置的三个旋转轴不相交的髋侧关节、髋转关节和髋前关节，及膝前关节，以及两个旋转轴相交的踝前关节和踝侧关节，所述方法包括：从所有关节的旋转轴中选取若干节点，将所有节点依次连接，得到基于脚踝局部坐标系建立的所述单腿的简化运动学模型，其中，所述若干节点包括作为起始节点的所述踝前关节和所述踝侧关节的旋转轴交点、作为第一中间节点的所述膝前关节的旋转轴中点、作为第二中间节点的所述髋前关节与所述髋转关节的旋转轴交点、以及作为末端节点的所述髋侧关节和所述髋转关节的旋转轴交点；根据获取的所述起始节点与所述末端节点在世界坐标系中的位姿，基于所述简化运动学模型中各个节点的位姿与关节角度之间的几何关系，计算所述髋侧关节、所述髋转关节、所述髋前关节、所述膝前关节、所述踝前关节和所述踝侧关节各自的关节角度。2.根据权利要求1所述的多自由度单腿运动学求解方法，其特征在于，所述髋侧关节、所述髋转关节、所述髋前关节、所述膝前关节、所述踝前关节和所述踝侧关节各自的关节角度的计算，包括：根据所述起始节点与所述末端节点在世界坐标系中的位姿，计算所述末端节点与所述起始节点之间的相对位置和相对姿态、以及所述末端节点相对于所述起始节点对应的第一旋转矩阵；根据所述起始节点与所述末端节点之间的相对位置基于第一等式方程，计算所述踝侧关节的关节角度，进而计算所述踝侧关节的关节角度对应的第二旋转矩阵；根据所述第一旋转矩阵和所述第二旋转矩阵，基于所述髋侧关节到所述踝侧关节的姿态转换对应的旋转矩阵关系，计算第三旋转矩阵；根据所述第三旋转矩阵，计算所述髋侧关节的关节角度、所述髋转关节的关节角度、以及由所述髋前关节、所述膝前关节和所述踝前关节的关节角度在同一平面合并后的等效角度；计算所述等效角度对应的等效旋转矩阵，并根据所述等效旋转矩阵基于第二等式方程，计算所述髋前关节、所述膝前关节、所述踝前关节各自的关节角度。3.根据权利要求2所述的多自由度单腿运动学求解方法，其特征在于，所述第一等式方程通过预先构建得到，包括：根据所述末端节点和所述起始节点的几何位置，构建所述踝侧关节的关节角度的正切值等于所述末端节点与所述起始节点之间的相对位置中的z方向分量与y方向分量的比值的方程以作为第一等式方程。4.根据权利要求2所述的多自由度单腿运动学求解方法，其特征在于，所述第二等式方程通过预先构建得到，包括：将所述末端节点、所述第一中间节点和所述第二中间节点投影到同一平面，构建所述髋前关节、所述膝前关节和所述踝前关节各自的关节角度在同一平面合并后的等效角度对应的等效旋转矩阵等于所述髋前关节、所述膝前关节、所述踝前关节各自的关节角度对应的旋转矩阵的乘积的方程以作为第二等式方程。5.根据权利要求2所述的多自由度单腿运动学求解方法，其特征在于，所述根据所述第三旋转矩阵，计算所述髋侧关节的关节角度、所述髋转关节的关节角度、以及所述等效角
度，包括：基于所述第三旋转矩阵等于所述髋侧关节的关节角度、所述髋转关节的关节角度和所述等效角度各自对应的旋转矩阵的乘积关系，从所述第三旋转矩阵中分别确定与所述髋侧关节的关节角度、所述髋转关节的关节角度和所述等效角度相关的向量；根据所述髋侧关节、所述髋转关节和所述等效角度各自对应的所述向量中的相应元素，分别计算所述髋侧关节的关节角度、所述髋转关节的关节角度和所述等效角度。6.根据权利要求2或4所述的多自由度单腿运动学求解方法，其特征在于，所述根据所述等效旋转矩阵基于第二等式方程，计算所述髋前关节、所述膝前关节、所述踝前关节各自的关节角度，包括：根据所述第二等式方程，从所述等效旋转矩阵中确定与所述髋前关节、所述膝前关节、所述踝前关节的关节角度相关的向量；根据所述髋前关节、所述膝前关节和所述踝前关节各自对应的所述向量中的相应元素，分别计算所述髋前关节、所述膝前关节、所述踝前关节的关节角度。7.根据权利要求2所述的多自由度单腿运动学求解方法，其特征在于，所述踝侧关节的关节角度对应的第二旋转矩阵、以及所述等效角度对应的等效旋转矩阵均通过罗德里格斯公式计算得到。8.一种机器人运动控制方法，其特征在于，所述机器人包括至少一条多自由度单腿结构，所述方法包括：获取机器人的躯干与至少一足底末端在世界坐标系下的位姿；根据所述躯干的所述位姿确定机器人各腿的髋侧关节和髋转关节的旋转轴交点在世界坐标系下的位姿，以及根据对应足底末端的所述位姿确定对应腿的踝前关节和踝侧关节的旋转轴交点在世界坐标系下的位姿；通过如权利要求1至7中任一项所述的多自由度单腿运动学求解方法计算所述机器人每条腿的各个关节角度；根据计算的所有关节角度对机器人进行相应控制。9.一种多自由度单腿运动学求解装置，其特征在于，所述单腿包括依次设置的三个旋转轴不相交的髋侧关节、髋转关节和髋前关节，及膝前关节，以及两个旋转轴相交的踝前关节和踝侧关节，所述装置包括：简化模型构建模块，用于从所有关节的旋转轴中选取若干节点，将所有节点依次连接，得到基于脚踝局部坐标系建立的所述单腿的简化运动学模型，其中，所述若干节点包括作为起始节点的所述踝前关节和所述踝侧关节的旋转轴交点、作为第一中间节点的所述膝前关节的旋转轴中点、作为第二中间节点的所述髋前关节与所述髋转关节的旋转轴交点、以及作为末端节点的所述髋侧关节和所述髋转关节的旋转轴交点；关节角度求解模块，用于根据获取的所述起始节点与所述末端节点在世界坐标系中的位姿，基于所述简化运动学模型中各个节点的位姿与关节角度之间的几何关系，计算所述髋侧关节、所述髋转关节、所述髋前关节、所述膝前关节、所述踝前关节和所述踝侧关节各自的关节角度。10.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的多自
由度单腿运动学求解方法或权利要求8中所述的机器人运动控制方法。11.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-7中任一项所述的多自由度单腿运动学求解方法或权利要求8中所述的机器人运动控制方法。

技术总结
本申请实施例提供一种多自由度单腿运动学求解方法、装置和机器人，所述单腿包括依次设置的三个旋转轴不相交的髋侧关节、髋转关节和髋前关节，及膝前关节，以及两个旋转轴相交的踝前关节和踝侧关节，该方法包括：从所有关节的旋转轴中选取若干节点，将所有节点依次连接，得到基于脚踝局部坐标系建立的单腿的简化运动学模型，根据获取的起始节点与末端节点在世界坐标系中的位姿，基于简化运动学模型中各个节点的位姿与关节角度之间的几何关系，计算各个关节的关节角度。该方法可以有效解决具有三旋转轴不相交的X-Z-Y构型的多连杆机构的逆运动学求解问题。运动学求解问题。运动学求解问题。


技术研发人员：周江琛 陈春玉 刘益彰 葛利刚 闫萌 熊友军
受保护的技术使用者：深圳市优必选科技股份有限公司
技术研发日：2021.12.29
技术公布日：2022/4/15