[发明专利]基于拖曳距的双轮机器人建模与静止平衡方法及装置

权利要求书

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为车把转轴驱动力矩。