【双足机器人（2）】倒立摆运动学模型构建（附代码）

最新推荐文章于 2023-10-04 16:36:03 发布

沙漏AI机器人

最新推荐文章于 2023-10-04 16:36:03 发布

阅读量3.1k

点赞数 3

本文链接：https://blog.youkuaiyun.com/shakehands2012/article/details/111413117

版权

1. 简介

在上一期的文章中，我们对线性倒立摆在2D平面内的运动过程进行了分析，并给出了基于轨道能量的线性倒立摆控制过程。

【双足机器人（1）】线性倒立摆及其运动控制（附代码）

在本期文章中，我们将建立线性倒立摆模型的运动学模型，为后面进行倒立摆的仿真控制铺垫基础。最终，我们可以使用所构建的运动学模型控制倒立摆模型的末端机构来绘制心形图案，源代码已开源，详见文末。

在Python中使用绘图工具绘制的结果：

在VREP（CoppeliaSim）中的仿真结果：

2. 倒立摆运动学模型构建

2.1 关节坐标系

在后续的仿真过程中，我们将先使用线性倒立摆模型进行二维矢状面内的仿真控制，然后再扩展到三维。本文使用的线性倒立摆模型如下：

模型一共包含4个关节（Left_hip, Left_knee, Right_hip, Right_knee），当然，在腿的末端还有两个foot，可以用于安装压力传感器。其中：

髋关节为旋转关节，可以控制模型的腿进行前后摆动；
膝关节为伸缩关键（线性运动关节），可以控制腿长。

为了方便后续扩展到三维环境，我们在构建运动学模型的时候保留了髋关节的内收/外展关节，只是在本文中内收/外展关节被锁死，后面在三维仿真的时候再打开。用于构建倒立摆运动学模型的方法来源于书籍《仿人机器人》介绍的方法，我们将倒立摆模型的各个关节的局部坐标系在空间中的相对关系建模如下：

在世界坐标系中，以质心构建质心M所在的坐标系为root坐标系，然后依次构建两条腿的髋关节、膝关节和踝关节坐标系。蓝色的箭头表示该关节的运动方向，例如，每一个髋关节包括两个关节（FE：屈曲/伸展，AD：内收/外展），膝关节为线性关节（平移关节，Prismatic joint），由于本文中，倒立摆模型的踝关节等同于足底，因此踝关节坐标系等同于足底的坐标系。

2.2 正运动学模型

我们将各个关节之间的结构关系构建为树形结构，这样方便我们在编程的时候使用递归方法：

为了方便表示，我们给每个关节取一个名字，质心表示为BODY，其余关节依次命名为J1-J4。

正运动学模型的构建过程就是建立各个关节、连杆的局部坐标系，并求解各个关节之间的旋转、平移关系的过程。本文不打算详细介绍旋转、平移等基本过程的推导过程，如果读者不了解机器人学中基本的坐标变换过程，需要自行补充基础知识。这里我们也没有采用最常见的DH法来构建模型，而是采用了《仿人机器人》中提出的一种用于仿人机器人的运动学模型构建方法（类似于指数积，但作者没有命名该方法），我们这里做一个简单的介绍。

单个连杆的情况：

假设，对于第个连杆的局部坐标系来说，其母连杆的坐标系为。设在母连杆的坐标系中，该连杆的转动轴矢量（即该关节绕哪个坐标轴旋转）为，的原点为，关节角为，当关节角为时，连杆处于初始状态，姿态矩阵为。

相对于其母连杆的齐次变换矩阵为

其中，，表示罗德里格斯式（Rodrigues)，该式子是本方法的核心，详细的推导过程见原书内容。

对于两个连杆的情况：

设母连杆的绝对位置和姿态分别为和，则的齐次变换矩阵为：

其中，。

根据链式法则，我们可以求得：

综合前面的式子，我们可以得到第个连杆的位置和姿态：

这部分的代码在Python中的实现为：

def forwardKinematics(self, j=1):
        if j == 0:
            return

        link = self.link_group.get(j)
        if j != 1: # j=1 means BODY Link
            i = link.mother
            link_mother = self.link_group.get(i)
            link.p = np.dot(link_mother.R,  link.b) + link_mother.p
            link.R = np.dot(link_mother.R, Rodrigues(link.a, link.q))
        self.forwardKinematics(link.sister)
        self.forwardKinematics(link.child

在构建了两个连杆的基本关系解算过程以后，我们就可以根据前面所阐述的倒立摆模型各个关节的局部坐标系以及结构关系来构建倒立摆模型的正运动学模型，这里不再详细阐述，详见源代码。