CN110348140B - 基于拖曳距的双轮机器人建模与静止平衡方法及装置 - Google Patents

Abstract

本发明公开了一种基于拖曳距的双轮机器人建模与静止平衡方法及装置，其中，该方法包括：将双轮机器人系统设置为多连杆多关节系统，在多连杆多关节系统中定义多个坐标系并计算出双轮机器人系统的拖曳距范围；根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程并建立运动学模型；利用第一类拉格朗日方程对运动学模型进行求解得到双轮机器人动力学模型，并对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在拖曳距范围中确定出符合控制需求的拖曳距。该方法能反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并能为拖曳距的选取提供一套分析流程，提高静止平衡的控制效果。

Description

基于拖曳距的双轮机器人建模与静止平衡方法及装置

技术领域

本发明涉及机械系统建模与动力学分析技术领域，特别涉及一种基于拖曳距的双轮机器人建模与静止平衡方法及装置。

背景技术

拖曳距是车把转轴和地面的交点与前轮地面接触点间的距离，对双轮机器人利用车把转向的平衡控制有着重要的影响。

对于拖曳距不为零的双轮机器人，转动车把能微调机器人的质心高度，防止倾倒。当双轮机器人的速度达到一定程度时，地面提供的回复力矩使前轮转轴无需额外控制力也能将机器人自动扶正；但当双轮机器人处于超低速甚至静止时，这种自稳性消失，此时的平衡主要依靠车把转向实现。由此可知，对于更具挑战性的静止平衡，拖曳距成为了关键。现有的车把转向静止平衡研究指出，基于正拖曳距设计的控制器，吸引域较小、鲁棒性较差，因此探究拖曳距对控制性能的影响就很有必要。然而，目前的研究都仅针对固定拖曳距的机器人进行建模，且将质心高度变化简化为线性模型。这样的模型不足以分析拖曳距的影响，为此迫切需要建立适用于任意拖曳距的动力学模型。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的一个目的在于提出一种基于拖曳距的双轮机器人建模与静止平衡方法，该方法可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并且能为拖曳距的选取提供一套分析流程，以提高静止平衡的控制效果。

本发明的另一个目的在于提出一种基于拖曳距的双轮机器人建模与静止平衡装置。

为达到上述目的，本发明一方面实施例提出了一种基于拖曳距的双轮机器人建模与静止平衡方法，包括：

S1，在检测双轮机器人系统满足预设等效设置条件时，将所述双轮机器人系统设置为多连杆多关节系统，并在所述多连杆多关节系统中定义多个坐标系，根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围；

S2，根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，并根据所述两个约束方程建立运动学模型；

S3，利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型，并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距。

本发明实施例的基于拖曳距的双轮机器人建模与静止平衡方法，首先根据运动约束，从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性，建立两个约束方程，得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后，从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并且能为拖曳距的选取提供一套分析流程，以提高静止平衡的控制效果。

另外，根据本发明上述实施例的基于拖曳距的双轮机器人建模与静止平衡方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片。

进一步地，在本发明的一个实施例中，所述多个坐标系为：

(1)惯性参考系{I}，A₀xyz：原点固定于A₀点，x轴由A₀指向E₀，z轴竖直向下，y轴与x轴和z轴形成右手系；其中，A₀为车把转动时的后车轮与地面接触点，E₀为车把转动时前车轮与地面的接触点；

(2)后轮坐标系{B}，Bx_by_bz_b：原点固定于B点，x_b轴与惯性参考系x轴平行，z和y两轴可由惯性参考系绕x轴旋转

角得到，则从{I}到{B}的旋转矩阵为：

其中，B为后车轮圆心，

为后车体的滚转角；

(3)车把坐标系{C}，Cx_cy_cz_c：原点固定于C点，y_c轴与后轮坐标系y_b轴平行，x和z两轴可由{B}绕y_b轴旋转θ+η角得到，则从{B}到{C}的旋转矩阵为：

其中，C为车把旋转副与后车架的连接点，θ为后车体的俯仰角，η是车把倾角；

车把倾角η满足以下几何约束：

其中，θ₀是车把转角为零时的后车架连杆向量的俯仰角，ε是后车架连杆安装角；

(4)前轮坐标系{D}，Dx_dy_dz_d：原点固定于D点，z_d轴与车把坐标系z_c轴平行，x和y两轴可由车把坐标系绕z_c轴旋转δ角得到，则从{C}到{D}的旋转矩阵为：

其中，D为前车轮圆心，δ为车把转角；

(5)后车体坐标系{G₁}，G₁x₁y₁z₁：原点固定于G₁，后车体坐标系由后轮坐标系绕y_b轴旋转θ角得到，令

中η＝0可得从{B}系到{G₁}系的旋转矩阵

其中，G₁为后车体质心；

(6)前车体坐标系{G₂}，G₂x₂y₂z₂：原点固定于G₂，前车体坐标系与前轮坐标系平行，G₂为前车体质心。

进一步地，在本发明的一个实施例中，所述双轮机器人系统的拖曳距范围为：

其中，R为车轮半径，l_r为线段BC的长度，d为线段CC′的长度，l_f为前车架线段C′D的长度，λ是车把前叉角，η是车把倾角。

进一步地，在本发明的一个实施例中，所述根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，包括：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量，上标指明该向量所对应的坐标系，

为从{B}到{I}的旋转矩阵，

为从{C}到{B}的旋转矩阵，

为从{D}到{C}的旋转矩阵；

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量。

进一步地，在本发明的一个实施例中，所述根据所述两个约束方程建立运动学模型，包括：

对约束1和约束2的约束条件进行求导，得：

其中，J为雅克比矩阵；雅克比矩阵又可依据广义坐标

与非独立坐标

分解成两部分：

非独立坐标的速度由广义速度来表示：

可得，所述运动学模型为：

其中，

为2阶单位矩阵。

进一步地，在本发明的一个实施例中，利用第一类拉格朗日方程求解所述双轮机器人动力学模型包括：

其中，L＝T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能，γ为拉格朗日乘子，

为双轮机器人系统受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

进一步地，在本发明的一个实施例中，所述S3具体包括：

在所述拖曳距范围内，对所述可控性矩阵的奇异值、所述闭环控制器的吸引域和所述控制代价进行分析，并做出对应图像，根据对应图像及所述控制需求确定符合所述控制需求的拖曳距。

为达到上述目的，本发明另一方面实施例提出了一种基于拖曳距的双轮机器人建模与静止平衡装置，包括：

等效模块，用于在检测双轮机器人系统满足预设等效设置条件时，将所述双轮机器人系统设置为多连杆多关节系统，并在所述多连杆多关节系统中定义多个坐标系，根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围；

约束模块，用于根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，并根据所述两个约束方程建立运动学模型；

建模分析模块，用于利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型，并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距。

本发明实施例的基于拖曳距的双轮机器人建模与静止平衡装置，首先根据运动约束，从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性，建立两个约束方程，得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后，从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并且能为拖曳距的选取提供一套分析流程，以提高静止平衡的控制效果。

另外，根据本发明上述实施例的基于拖曳距的双轮机器人建模与静止平衡装置还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡方法流程图；

图2为根据本发明一个实施例的多连杆多关节等效车体示意图；

图3为根据本发明一个实施例的拖曳距可控度分析流程图；

图4为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡装置结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法及装置。

首先将参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法。

图1为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡方法流程图。

如图1所示，该基于拖曳距的双轮机器人建模与静止平衡方法包括以下步骤：

步骤S1，在检测双轮机器人系统满足预设等效设置条件时，将双轮机器人系统设置为多连杆多关节系统，并在多连杆多关节系统中定义多个坐标系，根据多个坐标系的几何关系计算出双轮机器人系统的拖曳距范围。

进一步地，预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片。

具体地，在推导模型的过程中，需要进行一些假设，方便建立与推导模型，比如：

(1)假设后车体质心除了滚转和由车把转向引起的俯仰外，无其他运动；

(2)前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

(3)忽略轮胎厚度与形变，将两车轮视为大小相等的刚性薄圆片。

在满足上述可以假设的条件后，可以将双轮机器人系统等效为多连杆多关节系统，如图2所示，为多连杆多关节等效车体示意图。其中，A₀和A分别为车把转动和未转动时的后车轮与地面接触点。B为后车轮圆心，C为车把旋转副与后车架的连接点，D为前车轮圆心，E为前车轮与地面的接触点。

δ、θ分别代表后车体的滚转角、车把转角和后车体的俯仰角，θ₁为E点在前轮上的角位移。

是前轮滚转角速度。Δx_r是后轮由俯仰运动所引起的沿后车体平面Π₁与地面的交线的平移距离。Δx_f和Δy_f是E点由于车把旋转相对于初始点E₀在地面上的偏移坐标。表1是本发明实施例的等效关节总结表，使以上平移与旋转运动由五个关节实现。r_i(i＝1～4)为链接这五个关节的等效连杆向量，其中，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量。此外，θ₀是车把转角为零时的后车架连杆向量的俯仰角；ε是后车架连杆安装角，为r₂和车把转轴间的夹角；η是车把倾角，为车把转轴与后车体平面竖直方向的夹角；λ是车把前叉角，为前车架C′D与车把转轴间的夹角。G₂和G₁分别为前后车体质心。

表1

在将双轮机器人系统设置为多连杆多关节系统后，在多连杆多关节系统中定义以下六个坐标系：

(1)惯性参考系{I}，A₀xyz：原点固定于A₀点，x轴由A₀指向E₀，z轴竖直向下，y轴与x轴和z轴形成右手系；其中，A₀为车把转动时的后车轮与地面接触点，E₀为车把转动时前车轮与地面的接触点；

(2)后轮坐标系{B}，Bx_by_bz_b：原点固定于B点，x_b轴与惯性参考系x轴平行，z和y两轴可由惯性参考系绕x轴旋转

角得到，则从{I}到{B}的旋转矩阵为：

其中，B为后车轮圆心，

为后车体的滚转角；

(3)车把坐标系{C}，Cx_cy_cz_c：原点固定于C点，y_c轴与后轮坐标系y_b轴平行，x和z两轴可由{B}绕y_b轴旋转θ+η角得到，则从{B}到{C}的旋转矩阵为:

其中，C为车把旋转副与后车架的连接点，θ为后车体的俯仰角，η是车把倾角；

车把倾角η满足以下几何约束：

其中，θ₀是车把转角为零时的后车架连杆向量的俯仰角，ε是后车架连杆安装角；

(4)前轮坐标系{D}，Dx_dy_dz_d：原点固定于D点，z_d轴与车把坐标系z_c轴平行，x和y两轴可由车把坐标系绕z_c轴旋转δ角得到，则从{C}到{D}的旋转矩阵为：

其中，D为前车轮圆心，δ为车把转角；

(5)后车体坐标系{G₁}，G₁x₁y₁z₁：原点固定于G₁，后车体坐标系由后轮坐标系绕y_b轴旋转θ角得到，令

中η＝0可得从{B}系到{G₁}系的旋转矩阵

其中，G₁为后车体质心；

(6)前车体坐标系{G₂}，G₂x₂y₂z₂：原点固定于G₂，前车体坐标系与前轮坐标系平行，G₂为前车体质心。

根据上述坐标系，图2中的各矢量可表示为：

式中：上标指明该向量所对应的坐标系，R为车轮半径，l_r为线段BC的长度，d为线段CC′的长度，l_f为线段C′D的长度。

由几何关系可得双轮机器人拖曳距为：

步骤S2，根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程，并根据两个约束方程建立运动学模型。

进一步地，在本发明的一个实施例中，根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程，包括：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量，上标指明该向量所对应的坐标系，

为从{B}到{I}的旋转矩阵，

为从{C}到{B}的旋转矩阵，

为从{D}到{C}的旋转矩阵；

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量。

进一步地，在本发明的一个实施例中，根据两个约束方程建立运动学模型，包括：

对约束1和约束2的约束条件进行求导，得：

其中，J为雅克比矩阵；雅克比矩阵又可依据广义坐标

与非独立坐标

分解成两部分：

非独立坐标的速度由广义速度来表示：

可得，运动学模型为：

其中，

为2阶单位矩阵。

具体地，首先定义系统广义坐标及完整约束，

在上述的运动中，只有后车体的滚转角

与车把的转向角δ是独立的，因此定义它们为系统的广义坐标：

在静止平衡过程中车辆前后两轮均需着地，形成一条闭环运动链。闭环运动链的存在使得从地面出发有两条路径能到达系统上的任意一点。考虑到前轮涉及到的非独立坐标较多，故本发明以后轮与地面的接触点A₀作为路径起始点。这样只需一半的坐标即可表示前轮接触点在{I}系下的位置。因此，令新的系统坐标为：

根据系统所受的闭环运动链和车轮几何特性构建以下两个完整约束：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量。

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量，也是前车体平面Π₂的法向量。

当δ＝θ＝0时，θ₁应满足：

θ₁＝-η|_δ＝θ＝0 (11)

此外，θ₀也应满足的如下约束：

根据约束方程建立机器人运动学模型，以获得非线性质心高度变化模型，

对完整约束(9)和(10)求导可得：

式中：J为雅克比矩阵。该矩阵又可依据q_i与非独立坐标

分解成两部分：

基于上式，非独立坐标的速度就可由广义速度来表示：

因此，可获得系统运动学模型为：

式中：

为2阶单位矩阵。

步骤S3，利用第一类拉格朗日方程对运动学模型进行求解得到双轮机器人动力学模型，并根据双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在拖曳距范围中确定出符合控制需求的拖曳距。

进一步地，在本发明的一个实施例中，利用第一类拉格朗日方程求解双轮机器人动力学模型包括：

其中，L＝T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能，γ为拉格朗日乘子，

为双轮机器人系统受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

进一步地，在本发明的一个实施例中，S3具体包括：

在拖曳距范围内，对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，并做出对应图像，根据对应图像及控制需求确定符合控制需求的拖曳距。

具体地，根据运动学模型建立双轮机器人动力学模型，首先，推导双轮机器人系统的总动能，由前后车体的质心平动动能T_ti和绕质心的转动动能T_ri组成：

动能1、将后车体的质心G₁坐标表达在惯性系{I}下：

式中：

x_G1＝x_g1cosθ₀+z_g1sinθ₀、z_G1＝z_g1cosθ₀-x_g1sinθ₀，x_g1和z_g1为后车质心相对于后车架的坐标。

是俯仰导致的后轮在x轴上的平移，根据假设2可求得

再将后车架角速度投射到后车体坐标系{G₁}中：

因此，可得后车体动能如下：

式中：m₁为后车体质量，令{G₁}为后车体的惯量主轴系，则

为后车体在体坐标系{G₁}中的转动惯量矩阵。

动能2、将前车体的质心G₂坐标表达在惯性系{I}下：

式中：

x_g2和z_g2为前车质心相对于前车架的坐标，x_G2＝d+x_g2cosλ+z_g2sinλ、z_G2＝z_g2cosλ-x_g2sinλ。同样地，再将前车架角速度投射到前车体坐标系{G₂}中：

因此，可得前车体动能如下：

式中：m₂为前车体质量，令{G₂}前车体的惯量主轴系，则

为前车体在体坐标系{G₂}中的转动惯量矩阵。

以地面为零势能面，则取两车体质心在惯性系{I}下的z轴分量，可得车辆总势能为:

式中：g为地表重力加速度，(·)_z为取向量z轴分量运算符。

运用第一类拉格朗日方程求解系统的动力学模型：

式中：L＝T-V为拉格朗日函数、T为总动能、V为总势能，γ为拉格朗日乘子，

为系统所受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

将公式(23)写为如下机器人通用的欧拉-拉格朗日形式：

式中：M是广义质量矩阵，V是离心力与科氏力矩阵，E是重力矩阵，其具体表达式在附录中呈现。利用运动学模型(15)可将以上模型转化为只含有2个方程的常微分方程。首先对公式(15)求导可得系统坐标加速度：

将上式及运动学方程(15)代入公式(25)中，再两端同乘以G^T可得：

根据G^TJ^T＝0，可消去γ，得到降维动力学模型：

式中：M_i＝G^TMG是降维系统质量矩阵，

是降维系统阻尼矩阵，Q_iext＝G^TQ_ext为降维系统广义外力矩阵。

将降维动力学模型(28)在平衡点

处线性化：

式中：

是广义坐标相对于平衡位置的微小偏移量；Δτ_c是控制偏差量；

易证

且

忽略

可得到如下状态空间形式的线性时不变动力学方程。

根据上述过程分析拖曳距对车把转向静止平衡控制的影响，如图3所示，展示了拖曳距可控度分析流程图，在拖曳距允许的范围内，从可控性矩阵的奇异值、闭环控制器的吸引域和控制代价三方面来分析拖曳距的影响。

首先，分析可控度，构造线性模型(30)的可控性矩阵Q_c：

将系统矩阵A和B代入上式可得：

式中：

表示

的第2列。对Q_c进行奇异值分解Q_c＝U∑V，做出最小奇异值关于拖曳距的图像。

然后，分析闭环控制器的吸引域。设计如下线性二次型性能指标：

式中：

和

为待定的权重系数矩阵，可根据具体的控制效果进行调试。通过如下求解黎卡提代数方程：

可得矩阵P，进而可以求得车把力矩线性反馈控制律为：

定义车把转向力矩最大值为τ_{c max}，则系统的可达状态空间满足不等式：

|K|_∞|x|_∞≤||τ_c||＝||Kx||≤τ_{c max} (36)

式中：|·|_∞表示取向量的无穷范数。因此，估计吸引域为：

做出吸引域(37)关于拖曳距的图像。

最后，分析拖曳距对控制代价的影响。定义控制代价为：

式中：积分上标T表示控制终止时刻。做出控制代价关于拖曳距的图像。

根据以上三幅图像，可按照具体的控制需求，例如高可控性，大吸引域和低控制代价来选择合适的拖曳距。

下面对上述用到的公式进行补充。

公式(14)中的雅克比矩阵定义如下：

式中：

式中：

动力学方程(25)中的广义质量矩阵定义如下：

式中：

M₂₃＝m₂Rx_G2sinδcos(θ+η)+m₂l_rx_G2sinδsin(θ₀-η)-m₂x_G2z_G2sinδ

M₃₂＝m₂Rx_G2sinδcos(θ+η)+m₂l_rx_G2sinδsin(θ₀-η)-m₂x_G2z_G2sinδ

动力学方程(25)中的阻尼矩阵定义如下：

式中：

V₂₂＝0

动力学方程(25)中的重力矩阵定义如下：

式中：

根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法，首先根据运动约束，从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性，建立两个约束方程，得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后，从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并且能为拖曳距的选取提供一套分析流程，以提高静止平衡的控制效果。

其次参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡装置。

图4为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡装置结构示意图。

如图4所示，该基于拖曳距的双轮机器人建模与静止平衡装置包括：等效模块100、约束模块200和建模分析模块300。

等效模块100，用于在检测双轮机器人系统满足预设等效设置条件时，将双轮机器人系统设置为多连杆多关节系统，并在多连杆多关节系统中定义多个坐标系，根据多个坐标系的几何关系计算出双轮机器人系统的拖曳距范围。

约束模块200，用于根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程，并根据两个约束方程建立运动学模型。

建模分析模块300，用于利用第一类拉格朗日方程对运动学模型进行求解得到双轮机器人动力学模型，并根据双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在拖曳距范围中确定出符合控制需求的拖曳距。

进一步地，在本发明的一个实施例中，预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片。

进一步地，在本发明的一个实施例中，多个坐标系为：

(1)惯性参考系{I}，A₀xyz：原点固定于A₀点，x轴由A₀指向E₀，z轴竖直向下，y轴与x轴和z轴形成右手系；其中，A₀为车把转动时的后车轮与地面接触点，E₀为车把转动时前车轮与地面的接触点；

(2)后轮坐标系{B}，Bx_by_bz_b：原点固定于B点，x_b轴与惯性参考系x轴平行，z和y两轴可由惯性参考系绕x轴旋转

角得到，则从{I}到{B}的旋转矩阵为：

其中，B为后车轮圆心，

为后车体的滚转角；

(3)车把坐标系{C}，Cx_cy_cz_c：原点固定于C点，y_c轴与后轮坐标系y_b轴平行，x和z两轴可由{B}绕y_b轴旋转θ+η角得到，则从{B}到{C}的旋转矩阵为:

其中，C为车把旋转副与后车架的连接点，θ为后车体的俯仰角，η是车把倾角；

车把倾角η满足以下几何约束：

其中，θ₀是车把转角为零时的后车架连杆向量的俯仰角，ε是后车架连杆安装角；

(4)前轮坐标系{D}，Dx_dy_dz_d：原点固定于D点，z_d轴与车把坐标系z_c轴平行，x和y两轴可由车把坐标系绕z_c轴旋转δ角得到，则从{C}到{D}的旋转矩阵为：

其中，D为前车轮圆心，δ为车把转角；

(5)后车体坐标系{G₁}，G₁x₁y₁z₁：原点固定于G₁，后车体坐标系由后轮坐标系绕y_b轴旋转θ角得到，令

中η＝0可得从{B}系到{G₁}系的旋转矩阵

其中，G₁为后车体质心；

(6)前车体坐标系{G₂}，G₂x₂y₂z₂：原点固定于G₂，前车体坐标系与前轮坐标系平行，G₂为前车体质心。

进一步地，在本发明的一个实施例中，双轮机器人系统的拖曳距范围为：

其中，R为车轮半径，l_r为线段BC的长度，d为线段CC′的长度，l_f为前车架线段C′D的长度，λ是车把前叉角，η是车把倾角。

进一步地，在本发明的一个实施例中，根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程，包括：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量，上标指明该向量所对应的坐标系，

为从{B}到{I}的旋转矩阵，

为从{C}到{B}的旋转矩阵，

为从{D}到{C}的旋转矩阵；

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量。

进一步地，在本发明的一个实施例中，根据两个约束方程建立运动学模型，包括：

对约束1和约束2的约束条件进行求导，得：

其中，J为雅克比矩阵；雅克比矩阵又可依据广义坐标

与非独立坐标

分解成两部分：

非独立坐标的速度由广义速度来表示：

可得，运动学模型为：

其中，

为2阶单位矩阵。

进一步地，在本发明的一个实施例中，利用第一类拉格朗日方程求解双轮机器人动力学模型包括：

其中，L＝T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能，γ为拉格朗日乘子，

为双轮机器人系统受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

进一步地，在本发明的一个实施例中，建模分析模块具体用于，在拖曳距范围内，对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，并做出对应图像，根据对应图像及控制需求确定符合控制需求的拖曳距。

需要说明的是，前述对基于拖曳距的双轮机器人建模与静止平衡方法实施例的解释说明也适用于该实施例的装置，此处不再赘述。

根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡装置，首先根据运动约束，从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性，建立两个约束方程，得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后，从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系，并且能为拖曳距的选取提供一套分析流程，以提高静止平衡的控制效果。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (3)

1.一种基于拖曳距的双轮机器人建模与静止平衡方法，其特征在于，包括以下步骤：

S1，在检测双轮机器人系统满足预设等效设置条件时，将所述双轮机器人系统设置为多连杆多关节系统，并在所述多连杆多关节系统中定义多个坐标系，根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围，其中，所述预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片，所述多个坐标系为：

(1)惯性参考系{I}，A₀xyz：原点固定于A₀点，x轴由A₀指向E₀，z轴竖直向下，y轴与x轴和z轴形成右手系；其中，A₀为车把转动时的后车轮与地面接触点，E₀为车把转动时前车轮与地面的接触点；

(2)后轮坐标系{B}，Bx_by_bz_b：原点固定于B点，x_b轴与惯性参考系x轴平行，z和y两轴可由惯性参考系绕x轴旋转

角得到，则从{I}到{B}的旋转矩阵为：

其中，B为后车轮圆心，

为后车体的滚转角；

(3)车把坐标系{C}，Cx_cy_cz_c：原点固定于C点，y_c轴与后轮坐标系y_b轴平行，x和z两轴可由{B}绕y_b轴旋转θ+η角得到，则从{B}到{C}的旋转矩阵为:

其中，C为车把旋转副与后车架的连接点，θ为后车体的俯仰角，η是车把倾角；

车把倾角η满足以下几何约束：

其中，θ₀是车把转角为零时的后车架连杆向量的俯仰角，ε是后车架连杆安装角；

(4)前轮坐标系{D}，Dx_dy_dz_d：原点固定于D点，z_d轴与车把坐标系z_c轴平行，x和y两轴可由车把坐标系绕z_c轴旋转δ角得到，则从{C}到{D}的旋转矩阵为：

其中，D为前车轮圆心，δ为车把转角；

(5)后车体坐标系{G₁}，G₁x₁y₁z₁：原点固定于G₁，后车体坐标系由后轮坐标系绕y_b轴旋转θ角得到，令

中η＝0可得从{B}系到{G₁}系的旋转矩阵

其中，G₁为后车体质心；

(6)前车体坐标系{G₂}，G₂x₂y₂z₂：原点固定于G₂，前车体坐标系与前轮坐标系平行，G₂为前车体质心；

所述双轮机器人系统的拖曳距范围为：

其中，R为车轮半径，l_r为线段BC的长度，d为线段CC′的长度，l_f为前车架线段C′D的长度，λ是车把前叉角，η是车把倾角；

S2，根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，并根据所述两个约束方程建立运动学模型，其中，

所述根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，包括：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量，上标指明该向量所对应的坐标系，

为从{B}到{I}的旋转矩阵，

为从{C}到{B}的旋转矩阵，

为从{D}到{C}的旋转矩阵；

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量，

所述根据所述两个约束方程建立运动学模型，包括：

对约束1和约束2的约束条件进行求导，得：

其中，J为雅克比矩阵；雅克比矩阵又可依据广义坐标

与非独立坐标

分解成两部分：

非独立坐标的速度由广义速度来表示：

可得，所述运动学模型为：

其中，

为2阶单位矩阵；

S3，利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型，并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距，其中，利用第一类拉格朗日方程求解所述双轮机器人动力学模型包括：

其中，L＝T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能，γ为拉格朗日乘子，

为双轮机器人系统受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

2.根据权利要求1所述的方法，其特征在于，所述S3具体包括：

在所述拖曳距范围内，对所述可控性矩阵的奇异值、所述闭环控制器的吸引域和所述控制代价进行分析，并做出对应图像，根据对应图像及所述控制需求确定符合所述控制需求的拖曳距。

3.一种基于拖曳距的双轮机器人建模与静止平衡装置，其特征在于，包括：

等效模块，用于在检测双轮机器人系统满足预设等效设置条件时，将所述双轮机器人系统设置为多连杆多关节系统，并在所述多连杆多关节系统中定义多个坐标系，根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围，其中，

所述预设等效设置条件，包括：

设定后车体质心仅包括滚转和由车把转向引起的俯仰；

设定前后两轮均已被刹住，与车架间无相对运动，且后轮与地面间为纯滚动；

设定忽略轮胎厚度与形变，将前后两轮视为大小相等的刚性薄圆片，所述多个坐标系为：

(1)惯性参考系{I}，A₀xyz：原点固定于A₀点，x轴由A₀指向E₀，z轴竖直向下，y轴与x轴和z轴形成右手系；其中，A₀为车把转动时的后车轮与地面接触点，E₀为车把转动时前车轮与地面的接触点；

(2)后轮坐标系{B}，Bx_by_bz_b：原点固定于B点，x_b轴与惯性参考系x轴平行，z和y两轴可由惯性参考系绕x轴旋转

角得到，则从{I}到{B}的旋转矩阵为：

其中，B为后车轮圆心，

为后车体的滚转角；

(3)车把坐标系{C}，Cx_cy_cz_c：原点固定于C点，y_c轴与后轮坐标系y_b轴平行，x和z两轴可由{B}绕y_b轴旋转θ+η角得到，则从{B}到{C}的旋转矩阵为:

其中，C为车把旋转副与后车架的连接点，θ为后车体的俯仰角，η是车把倾角；

车把倾角η满足以下几何约束：

其中，θ₀是车把转角为零时的后车架连杆向量的俯仰角，ε是后车架连杆安装角；

(4)前轮坐标系{D}，Dx_dy_dz_d：原点固定于D点，z_d轴与车把坐标系z_c轴平行，x和y两轴可由车把坐标系绕z_c轴旋转δ角得到，则从{C}到{D}的旋转矩阵为：

其中，D为前车轮圆心，δ为车把转角；

(5)后车体坐标系{G₁}，G₁x₁y₁z₁：原点固定于G₁，后车体坐标系由后轮坐标系绕y_b轴旋转θ角得到，令

中η＝0可得从{B}系到{G₁}系的旋转矩阵

其中，G₁为后车体质心；

(7)前车体坐标系{G₂}，G₂x₂y₂z₂：原点固定于G₂，前车体坐标系与前轮坐标系平行，G₂为前车体质心；

所述双轮机器人系统的拖曳距范围为：

其中，R为车轮半径，l_r为线段BC的长度，d为线段CC′的长度，l_f为前车架线段C′D的长度，λ是车把前叉角，η是车把倾角；

约束模块，用于根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，并根据所述两个约束方程建立运动学模型，其中，

所述根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程，包括：

约束1，闭环运动链约束

其中，e_z＝[0,0,1]^T为惯性参考系{I}的z轴单位方向向量，r₁是由A指向B的后车轮连杆向量；r₂是由B指向C的后车架连杆向量；r₃是由C指向D的前车架连杆向量；r₄是由D指向E的前车轮连杆向量，上标指明该向量所对应的坐标系，

为从{B}到{I}的旋转矩阵，

为从{C}到{B}的旋转矩阵，

为从{D}到{C}的旋转矩阵；

约束2，车轮几何约束

其中，n_y为前轮坐标系{D}的y_d轴单位方向向量，

所述根据所述两个约束方程建立运动学模型，包括：

对约束1和约束2的约束条件进行求导，得：

其中，J为雅克比矩阵；雅克比矩阵又可依据广义坐标

与非独立坐标

分解成两部分：

非独立坐标的速度由广义速度来表示：

可得，所述运动学模型为：

其中，

为2阶单位矩阵；

建模分析模块，用于利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型，并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析，根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距，其中，利用第一类拉格朗日方程求解所述双轮机器人动力学模型包括：

其中，L＝T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能，γ为拉格朗日乘子，

为双轮机器人系统受到的广义非保守外力，D和d分别为滚转和车把转向通道所受的扰动力矩，τ_c为车把转轴驱动力矩。

Images