CN110348140A - Based on towing away from two-wheel robot modeling and static balance method and device - Google Patents

Based on towing away from two-wheel robot modeling and static balance method and device Download PDF

Info

Publication number
CN110348140A
CN110348140A CN201910637561.5A CN201910637561A CN110348140A CN 110348140 A CN110348140 A CN 110348140A CN 201910637561 A CN201910637561 A CN 201910637561A CN 110348140 A CN110348140 A CN 110348140A
Authority
CN
China
Prior art keywords
wheel
handlebar
axis
angle
wheeled robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201910637561.5A
Other languages
Chinese (zh)
Other versions
CN110348140B (en
Inventor
梁斌
陈章
王秉亨
孙一勇
杨君
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN201910637561.5A priority Critical patent/CN110348140B/en
Publication of CN110348140A publication Critical patent/CN110348140A/en
Application granted granted Critical
Publication of CN110348140B publication Critical patent/CN110348140B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00Computer-aided design [CAD]
    • G06F30/10Geometric CAD
    • G06F30/17Mechanical parametric or variational design
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00Computer-aided design [CAD]
    • G06F30/20Design optimisation, verification or simulation
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00Details relating to CAD techniques
    • G06F2111/04Constraint-based CAD
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/06Power analysis or power optimisation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Geometry (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Motorcycle And Bicycle Frame (AREA)

Abstract

The invention discloses it is a kind of based on towing away from two-wheel robot modeling and static balance method and device, wherein, this method comprises: setting multi link multi-joint system for two-wheel robot system, defining multiple coordinate systems in multi link multi-joint system and calculating the towing of two-wheel robot system away from range;Two constraint equations are established according to the wheel geometrical property of closed loop moving chain suffered by multi link multi-joint system and two-wheel robot system and establish kinematics model;Kinematics model is solved using lagrange equations of the first kind to obtain two-wheel Dynamic Models of Robot Manipulators, and the singular value of controllability matrix, the domain of attraction of closed loop controller and control cost are analyzed, based on the analysis results towing away from determine to meet in range the towing of demand for control away from.This method can be reflected in it is different towing away from lower handlebar corner and height of center of mass variation non-linear relations, and can for towing away from selection a set of analysis process is provided, improve the control effect of static balance.

Description

基于拖曳距的双轮机器人建模与静止平衡方法及装置Modeling and static balancing method and device for a two-wheeled robot based on trailing distance

技术领域technical field

本发明涉及机械系统建模与动力学分析技术领域,特别涉及一种基于拖曳距的双轮机器人建模与静止平衡方法及装置。The invention relates to the technical field of mechanical system modeling and dynamic analysis, in particular to a method and device for modeling and static balancing of a two-wheeled robot based on a drag distance.

背景技术Background technique

拖曳距是车把转轴和地面的交点与前轮地面接触点间的距离,对双轮机器人利用车把转向的平衡控制有着重要的影响。The drag distance is the distance between the intersection point of the handlebar shaft and the ground and the ground contact point of the front wheel, which has an important influence on the balance control of the two-wheeled robot using the handlebar steering.

对于拖曳距不为零的双轮机器人,转动车把能微调机器人的质心高度,防止倾倒。当双轮机器人的速度达到一定程度时,地面提供的回复力矩使前轮转轴无需额外控制力也能将机器人自动扶正;但当双轮机器人处于超低速甚至静止时,这种自稳性消失,此时的平衡主要依靠车把转向实现。由此可知,对于更具挑战性的静止平衡,拖曳距成为了关键。现有的车把转向静止平衡研究指出,基于正拖曳距设计的控制器,吸引域较小、鲁棒性较差,因此探究拖曳距对控制性能的影响就很有必要。然而,目前的研究都仅针对固定拖曳距的机器人进行建模,且将质心高度变化简化为线性模型。这样的模型不足以分析拖曳距的影响,为此迫切需要建立适用于任意拖曳距的动力学模型。For a two-wheeled robot with a non-zero trailing distance, turning the handlebar can fine-tune the height of the center of mass of the robot to prevent it from tipping over. When the speed of the two-wheeled robot reaches a certain level, the restoring torque provided by the ground enables the front wheel shaft to automatically straighten the robot without additional control force; The balance at the time mainly depends on the steering of the handlebar. It can be seen that for the more challenging static balance, the trail becomes the key. The existing research on the static balance of handlebar steering points out that the controller based on the design of positive trailing distance has a small field of attraction and poor robustness, so it is necessary to explore the influence of trailing distance on the control performance. However, current research only models the robot with a fixed drag distance, and simplifies the height change of the center of mass into a linear model. Such a model is not enough to analyze the influence of trailing distance, so it is urgent to establish a dynamic model suitable for any trailing distance.

发明内容Contents of the invention

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。The present invention aims to solve one of the technical problems in the related art at least to a certain extent.

为此,本发明的一个目的在于提出一种基于拖曳距的双轮机器人建模与静止平衡方法,该方法可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系,并且能为拖曳距的选取提供一套分析流程,以提高静止平衡的控制效果。For this reason, an object of the present invention is to propose a kind of two-wheel robot modeling and static balance method based on trailing distance, and this method can reflect the non-linear relation of handlebar rotation angle and barycenter height variation under different trailing distances, and can be The selection of trailing distance provides a set of analysis procedures to improve the control effect of static balance.

本发明的另一个目的在于提出一种基于拖曳距的双轮机器人建模与静止平衡装置。Another object of the present invention is to propose a two-wheeled robot modeling and static balancing device based on trailing distance.

为达到上述目的,本发明一方面实施例提出了一种基于拖曳距的双轮机器人建模与静止平衡方法,包括:In order to achieve the above purpose, an embodiment of the present invention proposes a trail-based two-wheeled robot modeling and static balancing method, including:

S1,在检测双轮机器人系统满足预设等效设置条件时,将所述双轮机器人系统设置为多连杆多关节系统,并在所述多连杆多关节系统中定义多个坐标系,根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围;S1, when it is detected that the two-wheel robot system satisfies the preset equivalent setting conditions, setting the two-wheel robot system as a multi-link multi-joint system, and defining multiple coordinate systems in the multi-link multi-joint system, calculating the drag distance range of the two-wheel robot system according to the geometric relationship of the plurality of coordinate systems;

S2,根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程,并根据所述两个约束方程建立运动学模型;S2, establishing two constraint equations according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematics model according to the two constraint equations;

S3,利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型,并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距。S3, using the first kind of Lagrangian equation to solve the kinematics model to obtain the dynamic model of the two-wheel robot, and according to the dynamic model of the two-wheel robot, the singular value of the controllability matrix and the closed-loop controller The area of attraction and the control cost are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

本发明实施例的基于拖曳距的双轮机器人建模与静止平衡方法,首先根据运动约束,从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性,建立两个约束方程,得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后,从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系,并且能为拖曳距的选取提供一套分析流程,以提高静止平衡的控制效果。In the trail-based two-wheeled robot modeling and static balance method of the embodiment of the present invention, firstly, according to the motion constraints, the two-wheeled robot is equivalent to a multi-link multi-joint system with two front and rear bodies articulated from the perspective of a multi-rigid body system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

另外,根据本发明上述实施例的基于拖曳距的双轮机器人建模与静止平衡方法还可以具有以下附加的技术特征:In addition, the trail-based two-wheeled robot modeling and static balancing method according to the above-mentioned embodiments of the present invention may also have the following additional technical features:

进一步地,在本发明的一个实施例中,所述预设等效设置条件,包括:Further, in an embodiment of the present invention, the preset equivalent setting conditions include:

设定后车体质心仅包括滚转和由车把转向引起的俯仰;Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

设定前后两轮均已被刹住,与车架间无相对运动,且后轮与地面间为纯滚动;The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

设定忽略轮胎厚度与形变,将前后两轮视为大小相等的刚性薄圆片。It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

进一步地,在本发明的一个实施例中,所述多个坐标系为:Further, in one embodiment of the present invention, the multiple coordinate systems are:

(1)惯性参考系{I},A0xyz:原点固定于A0点,x轴由A0指向E0,z轴竖直向下,y轴与x轴和z轴形成右手系;其中,A0为车把转动时的后车轮与地面接触点,E0为车把转动时前车轮与地面的接触点;(1) Inertial reference frame {I}, A 0 xyz: the origin is fixed at A 0 point, the x-axis points from A 0 to E 0 , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A 0 is the contact point between the rear wheel and the ground when the handlebar is turned, and E 0 is the contact point between the front wheel and the ground when the handlebar is turned;

(2)后轮坐标系{B},Bxbybzb:原点固定于B点,xb轴与惯性参考系x轴平行,z和y两轴可由惯性参考系绕x轴旋转角得到,则从{I}到{B}的旋转矩阵为:(2) Rear wheel coordinate system {B}, Bx b y b z b : the origin is fixed at point B, the x and b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

其中,B为后车轮圆心,为后车体的滚转角;Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3)车把坐标系{C},Cxcyczc:原点固定于C点,yc轴与后轮坐标系yb轴平行,x和z两轴可由{B}绕yb轴旋转θ+η角得到,则从{B}到{C}的旋转矩阵为:(3) Handlebar coordinate system {C}, Cx c y c z c : the origin is fixed at point C, the y c axis is parallel to the y b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y b axis It is obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

其中,C为车把旋转副与后车架的连接点,θ为后车体的俯仰角,η是车把倾角;Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

车把倾角η满足以下几何约束:The handlebar inclination η satisfies the following geometric constraints:

其中,θ0是车把转角为零时的后车架连杆向量的俯仰角,ε是后车架连杆安装角;Among them, θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4)前轮坐标系{D},Dxdydzd:原点固定于D点,zd轴与车把坐标系zc轴平行,x和y两轴可由车把坐标系绕zc轴旋转δ角得到,则从{C}到{D}的旋转矩阵为:(4) Front wheel coordinate system {D}, Dx d y d z d : the origin is fixed at point D, the z d axis is parallel to the z c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

其中,D为前车轮圆心,δ为车把转角;Among them, D is the center of the front wheel, δ is the handlebar angle;

(5)后车体坐标系{G1},G1x1y1z1:原点固定于G1,后车体坐标系由后轮坐标系绕yb轴旋转θ角得到,令中η=0可得从{B}系到{G1}系的旋转矩阵 (5) Rear car body coordinate system {G 1 }, G 1 x 1 y 1 z 1 : the origin is fixed at G 1 , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G 1 } system can be obtained

其中,G1为后车体质心;Among them, G 1 is the center of mass of the rear car body;

(6)前车体坐标系{G2},G2x2y2z2:原点固定于G2,前车体坐标系与前轮坐标系平行,G2为前车体质心。(6) Front car body coordinate system {G 2 }, G 2 x 2 y 2 z 2 : the origin is fixed at G 2 , the front car body coordinate system is parallel to the front wheel coordinate system, and G 2 is the center of mass of the front car body.

进一步地,在本发明的一个实施例中,所述双轮机器人系统的拖曳距范围为:Further, in one embodiment of the present invention, the range of the trailing distance of the two-wheeled robot system is:

其中,R为车轮半径,lr为线段BC的长度,d为线段CC′的长度,lf为前车架线段C′D的长度,λ是车把前叉角,η是车把倾角。Among them, R is the radius of the wheel, l r is the length of line segment BC, d is the length of line segment CC′, l f is the length of line segment C′D of the front frame, λ is the front fork angle of the handlebar, and η is the inclination angle of the handlebar.

进一步地,在本发明的一个实施例中,所述根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程,包括:Further, in one embodiment of the present invention, the two constraint equations are established according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

约束1,闭环运动链约束 Constraint 1, closed-loop kinematic chain constraint

其中,ez=[0,0,1]T为惯性参考系{I}的z轴单位方向向量,r1是由A指向B的后车轮连杆向量;r2是由B指向C的后车架连杆向量;r3是由C指向D的前车架连杆向量;r4是由D指向E的前车轮连杆向量,上标指明该向量所对应的坐标系,为从{B}到{I}的旋转矩阵,为从{C}到{B}的旋转矩阵,为从{D}到{C}的旋转矩阵;Among them, e z =[0,0,1] T is the z-axis unit direction vector of the inertial reference frame {I}, r 1 is the rear wheel linkage vector from A to B; r 2 is the rear wheel linkage vector from B to C The frame link vector; r 3 is the front frame link vector from C to D; r 4 is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

约束2,车轮几何约束 Constraint 2, Wheel Geometry Constraints

其中,ny为前轮坐标系{D}的yd轴单位方向向量。Among them, n y is the unit direction vector of the y d axis of the front wheel coordinate system {D}.

进一步地,在本发明的一个实施例中,所述根据所述两个约束方程建立运动学模型,包括:Further, in an embodiment of the present invention, the establishment of a kinematics model according to the two constraint equations includes:

对约束1和约束2的约束条件进行求导,得:Deriving the constraint conditions of constraint 1 and constraint 2, we get:

其中,J为雅克比矩阵;雅克比矩阵又可依据广义坐标与非独立坐标分解成两部分:Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

非独立坐标的速度由广义速度来表示: Velocities of dependent coordinates are represented by generalized velocities:

可得,所述运动学模型为:It can be obtained that the kinematic model is:

其中,为2阶单位矩阵。in, is a second-order identity matrix.

进一步地,在本发明的一个实施例中,利用第一类拉格朗日方程求解所述双轮机器人动力学模型包括:Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

其中,L=T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能,γ为拉格朗日乘子,为双轮机器人系统受到的广义非保守外力,D和d分别为滚转和车把转向通道所受的扰动力矩,τc为车把转轴驱动力矩。Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

进一步地,在本发明的一个实施例中,所述S3具体包括:Further, in an embodiment of the present invention, the S3 specifically includes:

在所述拖曳距范围内,对所述可控性矩阵的奇异值、所述闭环控制器的吸引域和所述控制代价进行分析,并做出对应图像,根据对应图像及所述控制需求确定符合所述控制需求的拖曳距。Within the range of the trailing distance, analyze the singular value of the controllability matrix, the attractive region of the closed-loop controller and the control cost, and make a corresponding image, and determine according to the corresponding image and the control requirement Trailing distance that meets the stated control needs.

为达到上述目的,本发明另一方面实施例提出了一种基于拖曳距的双轮机器人建模与静止平衡装置,包括:In order to achieve the above purpose, another embodiment of the present invention proposes a two-wheeled robot modeling and static balancing device based on drag distance, including:

等效模块,用于在检测双轮机器人系统满足预设等效设置条件时,将所述双轮机器人系统设置为多连杆多关节系统,并在所述多连杆多关节系统中定义多个坐标系,根据所述多个坐标系的几何关系计算出所述双轮机器人系统的拖曳距范围;An equivalent module, configured to set the two-wheel robot system as a multi-link multi-joint system when detecting that the two-wheel robot system satisfies preset equivalent setting conditions, and define multiple a coordinate system, and calculate the drag distance range of the two-wheel robot system according to the geometric relationship of the plurality of coordinate systems;

约束模块,用于根据所述多连杆多关节系统所受的闭环运动链和所述双轮机器人系统的车轮几何特性建立两个约束方程,并根据所述两个约束方程建立运动学模型;A constraint module, configured to establish two constraint equations according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establish a kinematics model according to the two constraint equations;

建模分析模块,用于利用第一类拉格朗日方程对所述运动学模型进行求解得到双轮机器人动力学模型,并根据所述双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,根据分析结果在所述拖曳距范围中确定出符合控制需求的拖曳距。The modeling analysis module is used to solve the kinematics model by using the first kind of Lagrangian equation to obtain the dynamic model of the two-wheel robot, and analyze the singular value of the controllability matrix according to the dynamic model of the two-wheel robot 1. The attraction domain and control cost of the closed-loop controller are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

本发明实施例的基于拖曳距的双轮机器人建模与静止平衡装置,首先根据运动约束,从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性,建立两个约束方程,得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后,从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系,并且能为拖曳距的选取提供一套分析流程,以提高静止平衡的控制效果。The trail-based two-wheel robot modeling and static balancing device of the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheel robot is equivalent to a multi-link multi-joint system with two front and rear bodies articulated. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

另外,根据本发明上述实施例的基于拖曳距的双轮机器人建模与静止平衡装置还可以具有以下附加的技术特征:In addition, the trail-based two-wheeled robot modeling and static balancing device according to the above-mentioned embodiments of the present invention may also have the following additional technical features:

进一步地,在本发明的一个实施例中,所述预设等效设置条件,包括:Further, in an embodiment of the present invention, the preset equivalent setting conditions include:

设定后车体质心仅包括滚转和由车把转向引起的俯仰;Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

设定前后两轮均已被刹住,与车架间无相对运动,且后轮与地面间为纯滚动;The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

设定忽略轮胎厚度与形变,将前后两轮视为大小相等的刚性薄圆片。It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

本发明附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.

附图说明Description of drawings

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:The above and/or additional aspects and advantages of the present invention will become apparent and easy to understand from the following description of the embodiments in conjunction with the accompanying drawings, wherein:

图1为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡方法流程图;Fig. 1 is a flow chart of a two-wheeled robot modeling and static balancing method based on a trailing distance according to an embodiment of the present invention;

图2为根据本发明一个实施例的多连杆多关节等效车体示意图;Fig. 2 is a schematic diagram of a multi-link multi-joint equivalent vehicle body according to an embodiment of the present invention;

图3为根据本发明一个实施例的拖曳距可控度分析流程图;Fig. 3 is a flow chart of drag distance controllability analysis according to an embodiment of the present invention;

图4为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡装置结构示意图。Fig. 4 is a schematic structural diagram of a two-wheeled robot modeling and static balancing device based on trailing distance according to an embodiment of the present invention.

具体实施方式Detailed ways

下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。Embodiments of the present invention are described in detail below, examples of which are shown in the drawings, wherein the same or similar reference numerals designate the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the figures are exemplary and are intended to explain the present invention and should not be construed as limiting the present invention.

下面参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法及装置。The method and device for modeling and static balancing of a two-wheeled robot based on trailing distance proposed according to an embodiment of the present invention will be described below with reference to the accompanying drawings.

首先将参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法。Firstly, a method for modeling and static balancing of a two-wheeled robot based on a trailing distance proposed according to an embodiment of the present invention will be described with reference to the accompanying drawings.

图1为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡方法流程图。Fig. 1 is a flowchart of a method for modeling and static balancing of a two-wheeled robot based on trailing distance according to an embodiment of the present invention.

如图1所示,该基于拖曳距的双轮机器人建模与静止平衡方法包括以下步骤:As shown in Figure 1, the trail-based two-wheel robot modeling and static balancing method includes the following steps:

步骤S1,在检测双轮机器人系统满足预设等效设置条件时,将双轮机器人系统设置为多连杆多关节系统,并在多连杆多关节系统中定义多个坐标系,根据多个坐标系的几何关系计算出双轮机器人系统的拖曳距范围。Step S1, when it is detected that the two-wheel robot system satisfies the preset equivalent setting conditions, set the two-wheel robot system as a multi-link multi-joint system, and define multiple coordinate systems in the multi-link multi-joint system, according to multiple The geometric relationship of the coordinate system is used to calculate the drag distance range of the two-wheel robot system.

进一步地,预设等效设置条件,包括:Further, preset equivalent setting conditions include:

设定后车体质心仅包括滚转和由车把转向引起的俯仰;Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

设定前后两轮均已被刹住,与车架间无相对运动,且后轮与地面间为纯滚动;The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

设定忽略轮胎厚度与形变,将前后两轮视为大小相等的刚性薄圆片。It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

具体地,在推导模型的过程中,需要进行一些假设,方便建立与推导模型,比如:Specifically, in the process of deriving the model, some assumptions need to be made to facilitate the establishment and derivation of the model, such as:

(1)假设后车体质心除了滚转和由车把转向引起的俯仰外,无其他运动;(1) It is assumed that the center of mass of the rear body has no other motion except roll and pitch caused by handlebar steering;

(2)前后两轮均已被刹住,与车架间无相对运动,且后轮与地面间为纯滚动;(2) Both the front and rear wheels have been braked, there is no relative movement with the frame, and the rear wheels are purely rolling with the ground;

(3)忽略轮胎厚度与形变,将两车轮视为大小相等的刚性薄圆片。(3) Neglecting the tire thickness and deformation, the two wheels are regarded as rigid thin discs of equal size.

在满足上述可以假设的条件后,可以将双轮机器人系统等效为多连杆多关节系统,如图2所示,为多连杆多关节等效车体示意图。其中,A0和A分别为车把转动和未转动时的后车轮与地面接触点。B为后车轮圆心,C为车把旋转副与后车架的连接点,D为前车轮圆心,E为前车轮与地面的接触点。δ、θ分别代表后车体的滚转角、车把转角和后车体的俯仰角,θ1为E点在前轮上的角位移。是前轮滚转角速度。Δxr是后轮由俯仰运动所引起的沿后车体平面Π1与地面的交线的平移距离。Δxf和Δyf是E点由于车把旋转相对于初始点E0在地面上的偏移坐标。表1是本发明实施例的等效关节总结表,使以上平移与旋转运动由五个关节实现。ri(i=1~4)为链接这五个关节的等效连杆向量,其中,r1是由A指向B的后车轮连杆向量;r2是由B指向C的后车架连杆向量;r3是由C指向D的前车架连杆向量;r4是由D指向E的前车轮连杆向量。此外,θ0是车把转角为零时的后车架连杆向量的俯仰角;ε是后车架连杆安装角,为r2和车把转轴间的夹角;η是车把倾角,为车把转轴与后车体平面竖直方向的夹角;λ是车把前叉角,为前车架C′D与车把转轴间的夹角。G2和G1分别为前后车体质心。After the above assumptions are satisfied, the two-wheel robot system can be equivalent to a multi-link multi-joint system, as shown in Figure 2, which is a schematic diagram of a multi-link multi-joint equivalent car body. Among them, A 0 and A are the contact points between the rear wheel and the ground when the handlebar is turned and not turned, respectively. B is the center of the rear wheel, C is the connection point between the handlebar swivel and the rear frame, D is the center of the front wheel, and E is the contact point between the front wheel and the ground. δ and θ respectively represent the roll angle of the rear body, the handlebar angle and the pitch angle of the rear body, and θ1 is the angular displacement of point E on the front wheel. is the front wheel roll angular velocity. Δxr is the translational distance along the intersection line between the rear body plane Π1 and the ground caused by the pitching motion of the rear wheel. Δx f and Δy f are the offset coordinates of point E on the ground relative to the initial point E 0 due to handlebar rotation. Table 1 is a summary table of equivalent joints of the embodiment of the present invention, so that the above translation and rotation motions are realized by five joints. r i (i=1~4) is the equivalent link vector linking these five joints, where r 1 is the rear wheel link vector from A to B; r 2 is the rear frame link from B to C Rod vector; r 3 is the front frame linkage vector from C to D; r 4 is the front wheel linkage vector from D to E. In addition, θ 0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero; ε is the installation angle of the rear frame link, which is the angle between r 2 and the handlebar rotation axis; η is the handlebar inclination angle, λ is the angle between the handlebar shaft and the vertical direction of the rear body plane; λ is the front fork angle of the handlebar, which is the angle between the front frame C′D and the handlebar shaft. G 2 and G 1 are the center of mass of the front and rear car bodies respectively.

表1Table 1

在将双轮机器人系统设置为多连杆多关节系统后,在多连杆多关节系统中定义以下六个坐标系:After setting the two-wheel robot system as a multi-link multi-joint system, define the following six coordinate systems in the multi-link multi-joint system:

(1)惯性参考系{I},A0xyz:原点固定于A0点,x轴由A0指向E0,z轴竖直向下,y轴与x轴和z轴形成右手系;其中,A0为车把转动时的后车轮与地面接触点,E0为车把转动时前车轮与地面的接触点;(1) Inertial reference frame {I}, A 0 xyz: the origin is fixed at A 0 point, the x-axis points from A 0 to E 0 , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A 0 is the contact point between the rear wheel and the ground when the handlebar is turned, and E 0 is the contact point between the front wheel and the ground when the handlebar is turned;

(2)后轮坐标系{B},Bxbybzb:原点固定于B点,xb轴与惯性参考系x轴平行,z和y两轴可由惯性参考系绕x轴旋转角得到,则从{I}到{B}的旋转矩阵为:(2) Rear wheel coordinate system {B}, Bx b y b z b : the origin is fixed at point B, the x and b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

其中,B为后车轮圆心,为后车体的滚转角;Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3)车把坐标系{C},Cxcyczc:原点固定于C点,yc轴与后轮坐标系yb轴平行,x和z两轴可由{B}绕yb轴旋转θ+η角得到,则从{B}到{C}的旋转矩阵为:(3) Handlebar coordinate system {C}, Cx c y c z c : the origin is fixed at point C, the y c axis is parallel to the y b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y b axis Obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

其中,C为车把旋转副与后车架的连接点,θ为后车体的俯仰角,η是车把倾角;Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

车把倾角η满足以下几何约束:The handlebar inclination η satisfies the following geometric constraints:

其中,θ0是车把转角为零时的后车架连杆向量的俯仰角,ε是后车架连杆安装角;Among them, θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4)前轮坐标系{D},Dxdydzd:原点固定于D点,zd轴与车把坐标系zc轴平行,x和y两轴可由车把坐标系绕zc轴旋转δ角得到,则从{C}到{D}的旋转矩阵为:(4) Front wheel coordinate system {D}, Dx d y d z d : the origin is fixed at point D, the z d axis is parallel to the z c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

其中,D为前车轮圆心,δ为车把转角;Among them, D is the center of the front wheel, δ is the handlebar angle;

(5)后车体坐标系{G1},G1x1y1z1:原点固定于G1,后车体坐标系由后轮坐标系绕yb轴旋转θ角得到,令中η=0可得从{B}系到{G1}系的旋转矩阵 (5) Rear car body coordinate system {G 1 }, G 1 x 1 y 1 z 1 : the origin is fixed at G 1 , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G 1 } system can be obtained

其中,G1为后车体质心;Among them, G 1 is the center of mass of the rear car body;

(6)前车体坐标系{G2},G2x2y2z2:原点固定于G2,前车体坐标系与前轮坐标系平行,G2为前车体质心。(6) Front car body coordinate system {G 2 }, G 2 x 2 y 2 z 2 : the origin is fixed at G 2 , the front car body coordinate system is parallel to the front wheel coordinate system, and G 2 is the center of mass of the front car body.

根据上述坐标系,图2中的各矢量可表示为:According to the above coordinate system, each vector in Fig. 2 can be expressed as:

式中:上标指明该向量所对应的坐标系,R为车轮半径,lr为线段BC的长度,d为线段CC′的长度,lf为线段C′D的长度。In the formula: the superscript indicates the coordinate system corresponding to the vector, R is the radius of the wheel, l r is the length of the line segment BC, d is the length of the line segment CC′, and l f is the length of the line segment C′D.

由几何关系可得双轮机器人拖曳距为:From the geometric relationship, the drag distance of the two-wheeled robot can be obtained as:

步骤S2,根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程,并根据两个约束方程建立运动学模型。In step S2, two constraint equations are established according to the closed-loop kinematic chain of the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and a kinematic model is established according to the two constraint equations.

进一步地,在本发明的一个实施例中,根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程,包括:Further, in one embodiment of the present invention, two constraint equations are established according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

约束1,闭环运动链约束 Constraint 1, closed-loop kinematic chain constraint

其中,ez=[0,0,1]T为惯性参考系{I}的z轴单位方向向量,r1是由A指向B的后车轮连杆向量;r2是由B指向C的后车架连杆向量;r3是由C指向D的前车架连杆向量;r4是由D指向E的前车轮连杆向量,上标指明该向量所对应的坐标系,为从{B}到{I}的旋转矩阵,为从{C}到{B}的旋转矩阵,为从{D}到{C}的旋转矩阵;Among them, e z =[0,0,1] T is the z-axis unit direction vector of the inertial reference frame {I}, r 1 is the rear wheel linkage vector from A to B; r 2 is the rear wheel linkage vector from B to C The frame link vector; r 3 is the front frame link vector from C to D; r 4 is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

约束2,车轮几何约束 Constraint 2, Wheel Geometry Constraints

其中,ny为前轮坐标系{D}的yd轴单位方向向量。Among them, n y is the unit direction vector of the y d axis of the front wheel coordinate system {D}.

进一步地,在本发明的一个实施例中,根据两个约束方程建立运动学模型,包括:Further, in one embodiment of the present invention, a kinematics model is established according to two constraint equations, including:

对约束1和约束2的约束条件进行求导,得:Deriving the constraint conditions of constraint 1 and constraint 2, we get:

其中,J为雅克比矩阵;雅克比矩阵又可依据广义坐标与非独立坐标分解成两部分:Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

非独立坐标的速度由广义速度来表示: Velocities of dependent coordinates are represented by generalized velocities:

可得,运动学模型为:It can be obtained that the kinematic model is:

其中,为2阶单位矩阵。in, is a second-order identity matrix.

具体地,首先定义系统广义坐标及完整约束,Specifically, first define the generalized coordinates and complete constraints of the system,

在上述的运动中,只有后车体的滚转角与车把的转向角δ是独立的,因此定义它们为系统的广义坐标:In the above motion, only the roll angle of the rear body are independent of the steering angle δ of the handlebar, so define them as generalized coordinates of the system:

在静止平衡过程中车辆前后两轮均需着地,形成一条闭环运动链。闭环运动链的存在使得从地面出发有两条路径能到达系统上的任意一点。考虑到前轮涉及到的非独立坐标较多,故本发明以后轮与地面的接触点A0作为路径起始点。这样只需一半的坐标即可表示前轮接触点在{I}系下的位置。因此,令新的系统坐标为:In the process of static balance, the front and rear wheels of the vehicle need to touch the ground, forming a closed-loop kinematic chain. The existence of the closed-loop kinematic chain makes there are two paths starting from the ground to reach any point on the system. Considering that the front wheels involve more dependent coordinates, the present invention uses the contact point A0 between the rear wheels and the ground as the starting point of the path. In this way, only half of the coordinates are needed to represent the position of the front wheel contact point under the {I} system. Therefore, let the new system coordinates be:

根据系统所受的闭环运动链和车轮几何特性构建以下两个完整约束:Construct the following two complete constraints based on the closed-loop kinematic chain and wheel geometry to which the system is subjected:

约束1,闭环运动链约束 Constraint 1, closed-loop kinematic chain constraint

其中,ez=[0,0,1]T为惯性参考系{I}的z轴单位方向向量。Wherein, e z =[0,0,1] T is the z-axis unit direction vector of the inertial reference frame {I}.

约束2,车轮几何约束 Constraint 2, Wheel Geometry Constraints

其中,ny为前轮坐标系{D}的yd轴单位方向向量,也是前车体平面Π2的法向量。Among them, n y is the unit direction vector of the y d axis of the front wheel coordinate system {D}, which is also the normal vector of the front body plane Π 2 .

当δ=θ=0时,θ1应满足:When δ=θ=0, θ1 should satisfy:

θ1=-η|δ=θ=0 (11)θ 1 =-η | δ = θ = 0 (11)

此外,θ0也应满足的如下约束:In addition, θ 0 should also satisfy the following constraints:

根据约束方程建立机器人运动学模型,以获得非线性质心高度变化模型,Establish the robot kinematics model according to the constraint equation to obtain the nonlinear height change model of the center of mass,

对完整约束(9)和(10)求导可得:Deriving the complete constraints (9) and (10) gives:

式中:J为雅克比矩阵。该矩阵又可依据qi与非独立坐标分解成两部分:In the formula: J is the Jacobian matrix. The matrix can be based on q i and dependent coordinates Break it down into two parts:

基于上式,非独立坐标的速度就可由广义速度来表示:因此,可获得系统运动学模型为:Based on the above formula, the velocity of the dependent coordinates can be expressed by the generalized velocity: Therefore, the system kinematics model can be obtained as:

式中:为2阶单位矩阵。In the formula: is a second-order identity matrix.

步骤S3,利用第一类拉格朗日方程对运动学模型进行求解得到双轮机器人动力学模型,并根据双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,根据分析结果在拖曳距范围中确定出符合控制需求的拖曳距。Step S3, using the first kind of Lagrangian equation to solve the kinematics model to obtain the dynamic model of the two-wheel robot, and according to the dynamic model of the two-wheel robot, the singular value of the controllability matrix, the attraction domain of the closed-loop controller and The control cost is analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

进一步地,在本发明的一个实施例中,利用第一类拉格朗日方程求解双轮机器人动力学模型包括:Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

其中,L=T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能,γ为拉格朗日乘子,为双轮机器人系统受到的广义非保守外力,D和d分别为滚转和车把转向通道所受的扰动力矩,τc为车把转轴驱动力矩。Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

进一步地,在本发明的一个实施例中,S3具体包括:Further, in an embodiment of the present invention, S3 specifically includes:

在拖曳距范围内,对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,并做出对应图像,根据对应图像及控制需求确定符合控制需求的拖曳距。Within the range of the trailing distance, the singular values of the controllability matrix, the domain of attraction and the control cost of the closed-loop controller are analyzed, and the corresponding images are made, and the trailing distance that meets the control requirements is determined according to the corresponding images and control requirements.

具体地,根据运动学模型建立双轮机器人动力学模型,首先,推导双轮机器人系统的总动能,由前后车体的质心平动动能Tti和绕质心的转动动能Tri组成:Specifically, the dynamic model of the two-wheeled robot is established according to the kinematics model. First, the total kinetic energy of the two-wheeled robot system is derived, which is composed of the translational kinetic energy T ti of the center of mass of the front and rear bodies and the rotational kinetic energy T ri around the center of mass:

动能1、将后车体的质心G1坐标表达在惯性系{I}下:Kinetic energy 1. Express the G 1 coordinates of the center of mass of the rear car body under the inertial system {I}:

式中:xG1=xg1cosθ0+zg1sinθ0、zG1=zg1cosθ0-xg1sinθ0,xg1和zg1为后车质心相对于后车架的坐标。是俯仰导致的后轮在x轴上的平移,根据假设2可求得再将后车架角速度投射到后车体坐标系{G1}中:In the formula: x G1 =x g1 cosθ 0 +z g1 sinθ 0 , z G1 =z g1 cosθ 0 -x g1 sinθ 0 , x g1 and z g1 are the coordinates of the center of mass of the rear vehicle relative to the rear frame. is the translation of the rear wheel on the x-axis caused by the pitch, and can be obtained according to assumption 2 Then project the angular velocity of the rear frame into the coordinate system of the rear body {G 1 }:

因此,可得后车体动能如下:Therefore, the kinetic energy of the rear car body can be obtained as follows:

式中:m1为后车体质量,令{G1}为后车体的惯量主轴系,则为后车体在体坐标系{G1}中的转动惯量矩阵。In the formula: m 1 is the mass of the rear car body, let {G 1 } be the main axis of inertia of the rear car body, then is the moment of inertia matrix of the rear body in the body coordinate system {G 1 }.

动能2、将前车体的质心G2坐标表达在惯性系{I}下:Kinetic energy 2. Express the G 2 coordinates of the center of mass of the front car body under the inertial system {I}:

式中:xg2和zg2为前车质心相对于前车架的坐标,xG2=d+xg2cosλ+zg2sinλ、zG2=zg2cosλ-xg2sinλ。同样地,再将前车架角速度投射到前车体坐标系{G2}中:In the formula: x g2 and z g2 are the coordinates of the center of mass of the front vehicle relative to the front frame, x G2 =d+x g2 cosλ+z g2 sinλ, z G2 =z g2 cosλ-x g2 sinλ. Similarly, project the angular velocity of the front frame into the front body coordinate system {G 2 }:

因此,可得前车体动能如下:Therefore, the kinetic energy of the front body can be obtained as follows:

式中:m2为前车体质量,令{G2}前车体的惯量主轴系,则为前车体在体坐标系{G2}中的转动惯量矩阵。In the formula: m 2 is the mass of the front car body, let {G 2 } the main axis of inertia of the front car body, then is the moment of inertia matrix of the front body in the body coordinate system {G 2 }.

以地面为零势能面,则取两车体质心在惯性系{I}下的z轴分量,可得车辆总势能为:Taking the ground as the zero potential energy surface, then taking the z-axis component of the center of mass of the two vehicles under the inertial frame {I}, the total potential energy of the vehicle can be obtained as:

式中:g为地表重力加速度,(·)z为取向量z轴分量运算符。In the formula: g is the surface gravitational acceleration, (·) z is the operator for taking the z-axis component of the vector.

运用第一类拉格朗日方程求解系统的动力学模型:Solve the dynamics model of the system using Lagrangian equations of the first kind:

式中:L=T-V为拉格朗日函数、T为总动能、V为总势能,γ为拉格朗日乘子,为系统所受到的广义非保守外力,D和d分别为滚转和车把转向通道所受的扰动力矩,τc为车把转轴驱动力矩。In the formula: L=TV is the Lagrangian function, T is the total kinetic energy, V is the total potential energy, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the system, D and d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque of the handlebar shaft.

将公式(23)写为如下机器人通用的欧拉-拉格朗日形式:Write formula (23) as the following general Euler-Lagrangian form for robots:

式中:M是广义质量矩阵,V是离心力与科氏力矩阵,E是重力矩阵,其具体表达式在附录中呈现。利用运动学模型(15)可将以上模型转化为只含有2个方程的常微分方程。首先对公式(15)求导可得系统坐标加速度:In the formula: M is the generalized mass matrix, V is the centrifugal force and Coriolis force matrix, E is the gravity matrix, and its specific expressions are presented in the appendix. Using the kinematic model (15), the above model can be transformed into an ordinary differential equation containing only two equations. Firstly, the system coordinate acceleration can be obtained by deriving formula (15):

将上式及运动学方程(15)代入公式(25)中,再两端同乘以GT可得:Substitute the above formula and kinematic equation (15) into formula (25), and then multiply both ends by G T to get:

根据GTJT=0,可消去γ,得到降维动力学模型:According to G T J T = 0, γ can be eliminated to obtain the dimensionality reduction dynamic model:

式中:Mi=GTMG是降维系统质量矩阵,是降维系统阻尼矩阵,Qiext=GTQext为降维系统广义外力矩阵。In the formula: M i = G T MG is the mass matrix of dimensionality reduction system, is the damping matrix of the dimensionality reduction system, and Q iext = G T Q ext is the generalized external force matrix of the dimensionality reduction system.

将降维动力学模型(28)在平衡点处线性化:Put the dimensionality reduction dynamic model (28) at the equilibrium point Linearize at:

式中:是广义坐标相对于平衡位置的微小偏移量;Δτc是控制偏差量;易证忽略可得到如下状态空间形式的线性时不变动力学方程。In the formula: is the small offset of the generalized coordinates relative to the equilibrium position; Δτ c is the control deviation; Easy proof and neglect The linear time-invariant kinetic equation can be obtained in the following state-space form.

根据上述过程分析拖曳距对车把转向静止平衡控制的影响,如图3所示,展示了拖曳距可控度分析流程图,在拖曳距允许的范围内,从可控性矩阵的奇异值、闭环控制器的吸引域和控制代价三方面来分析拖曳距的影响。According to the above process, the influence of the trailing distance on the static balance control of the handlebar steering is analyzed, as shown in Figure 3, which shows the flow chart of the trailing distance controllability analysis. Within the allowable range of the trailing distance, from the singular value of the controllability matrix, The influence of the trailing distance is analyzed from three aspects of the closed-loop controller's domain of attraction and the control cost.

首先,分析可控度,构造线性模型(30)的可控性矩阵QcFirst, analyze the controllability and construct the controllability matrix Q c of the linear model (30):

将系统矩阵A和B代入上式可得:Substituting the system matrices A and B into the above formula can be obtained:

式中:表示的第2列。对Qc进行奇异值分解Qc=U∑V,做出最小奇异值关于拖曳距的图像。In the formula: express column 2 of the . Singular value decomposition Q c = U∑V is performed on Q c to make an image of the minimum singular value with respect to the trailing distance.

然后,分析闭环控制器的吸引域。设计如下线性二次型性能指标:Then, the domain of attraction of the closed-loop controller is analyzed. Design the following linear quadratic performance index:

式中:为待定的权重系数矩阵,可根据具体的控制效果进行调试。通过如下求解黎卡提代数方程:In the formula: and is an undetermined weight coefficient matrix, which can be debugged according to the specific control effect. Solve the Riccati algebraic equation by:

可得矩阵P,进而可以求得车把力矩线性反馈控制律为:The matrix P can be obtained, and then the handlebar torque linear feedback control law can be obtained as:

定义车把转向力矩最大值为τc max,则系统的可达状态空间满足不等式:Define the maximum value of the handlebar steering torque as τ c max , then the reachable state space of the system satisfies the inequality:

|K||x|≤||τc||=||Kx||≤τc max (36)|K| |x| ≤||τ c ||=||Kx||≤τ c max (36)

式中:|·|表示取向量的无穷范数。因此,估计吸引域为:In the formula: |·| means taking the infinite norm of the vector. Therefore, the estimated domain of attraction is:

做出吸引域(37)关于拖曳距的图像。Make a map of the field of attraction (37) with respect to the drag distance.

最后,分析拖曳距对控制代价的影响。定义控制代价为:Finally, the influence of trailing distance on control cost is analyzed. Define the control cost as:

式中:积分上标T表示控制终止时刻。做出控制代价关于拖曳距的图像。In the formula: the integral superscript T represents the control termination time. Make a graph of the control cost with respect to the drag distance.

根据以上三幅图像,可按照具体的控制需求,例如高可控性,大吸引域和低控制代价来选择合适的拖曳距。According to the above three images, the appropriate drag distance can be selected according to the specific control requirements, such as high controllability, large attraction field and low control cost.

下面对上述用到的公式进行补充。The formulas used above are supplemented below.

公式(14)中的雅克比矩阵定义如下:The Jacobian matrix in formula (14) is defined as follows:

式中:In the formula:

式中:In the formula:

动力学方程(25)中的广义质量矩阵定义如下:The generalized mass matrix in the kinetic equation (25) is defined as follows:

式中: In the formula:

M23=m2RxG2sinδcos(θ+η)+m2lrxG2sinδsin(θ0-η)-m2xG2zG2sinδM 23 =m 2 Rx G2 sinδcos(θ+η)+m 2 l r x G2 sinδsin(θ 0 -η)-m 2 x G2 z G2 sinδ

M32=m2RxG2sinδcos(θ+η)+m2lrxG2sinδsin(θ0-η)-m2xG2zG2sinδM 32 =m 2 Rx G2 sinδcos(θ+η)+m 2 l r x G2 sinδsin(θ 0 -η)-m 2 x G2 z G2 sinδ

动力学方程(25)中的阻尼矩阵定义如下:The damping matrix in the dynamic equation (25) is defined as follows:

式中:In the formula:

V22=0V 22 =0

动力学方程(25)中的重力矩阵定义如下:The gravity matrix in the kinetic equation (25) is defined as follows:

式中:In the formula:

根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡方法,首先根据运动约束,从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性,建立两个约束方程,得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后,从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系,并且能为拖曳距的选取提供一套分析流程,以提高静止平衡的控制效果。According to the two-wheeled robot modeling and static balance method based on the trailing distance proposed by the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheeled robot is equivalent to a multi-link multi-joint with two front and rear car bodies articulated system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

其次参照附图描述根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡装置。Next, the two-wheeled robot modeling and static balancing device based on the trailing distance proposed according to the embodiments of the present invention will be described with reference to the accompanying drawings.

图4为根据本发明一个实施例的基于拖曳距的双轮机器人建模与静止平衡装置结构示意图。Fig. 4 is a schematic structural diagram of a two-wheeled robot modeling and static balancing device based on trailing distance according to an embodiment of the present invention.

如图4所示,该基于拖曳距的双轮机器人建模与静止平衡装置包括:等效模块100、约束模块200和建模分析模块300。As shown in FIG. 4 , the trail-based two-wheel robot modeling and static balancing device includes: an equivalent module 100 , a constraint module 200 and a modeling analysis module 300 .

等效模块100,用于在检测双轮机器人系统满足预设等效设置条件时,将双轮机器人系统设置为多连杆多关节系统,并在多连杆多关节系统中定义多个坐标系,根据多个坐标系的几何关系计算出双轮机器人系统的拖曳距范围。Equivalent module 100, configured to set the two-wheel robot system as a multi-link multi-joint system and define multiple coordinate systems in the multi-link multi-joint system when detecting that the two-wheel robot system satisfies preset equivalent setting conditions , calculate the drag range of the two-wheel robot system according to the geometric relationship of multiple coordinate systems.

约束模块200,用于根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程,并根据两个约束方程建立运动学模型。The constraint module 200 is used to establish two constraint equations according to the closed-loop kinematic chain of the multi-link multi-joint system and the geometric characteristics of the wheels of the two-wheel robot system, and establish a kinematics model according to the two constraint equations.

建模分析模块300,用于利用第一类拉格朗日方程对运动学模型进行求解得到双轮机器人动力学模型,并根据双轮机器人动力学模型对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,根据分析结果在拖曳距范围中确定出符合控制需求的拖曳距。The modeling analysis module 300 is used to solve the kinematics model by using the first kind of Lagrangian equation to obtain the dynamic model of the two-wheel robot, and to analyze the singular value of the controllability matrix and the closed-loop control according to the dynamic model of the two-wheel robot. The attraction domain and control cost of the controller are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

进一步地,在本发明的一个实施例中,预设等效设置条件,包括:Further, in one embodiment of the present invention, the preset equivalent setting conditions include:

设定后车体质心仅包括滚转和由车把转向引起的俯仰;Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

设定前后两轮均已被刹住,与车架间无相对运动,且后轮与地面间为纯滚动;The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

设定忽略轮胎厚度与形变,将前后两轮视为大小相等的刚性薄圆片。It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

进一步地,在本发明的一个实施例中,多个坐标系为:Further, in one embodiment of the present invention, the multiple coordinate systems are:

(1)惯性参考系{I},A0xyz:原点固定于A0点,x轴由A0指向E0,z轴竖直向下,y轴与x轴和z轴形成右手系;其中,A0为车把转动时的后车轮与地面接触点,E0为车把转动时前车轮与地面的接触点;(1) Inertial reference frame {I}, A 0 xyz: the origin is fixed at A 0 point, the x-axis points from A 0 to E 0 , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A 0 is the contact point between the rear wheel and the ground when the handlebar is turned, and E 0 is the contact point between the front wheel and the ground when the handlebar is turned;

(2)后轮坐标系{B},Bxbybzb:原点固定于B点,xb轴与惯性参考系x轴平行,z和y两轴可由惯性参考系绕x轴旋转角得到,则从{I}到{B}的旋转矩阵为:(2) Rear wheel coordinate system {B}, Bx b y b z b : the origin is fixed at point B, the x and b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

其中,B为后车轮圆心,为后车体的滚转角;Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3)车把坐标系{C},Cxcyczc:原点固定于C点,yc轴与后轮坐标系yb轴平行,x和z两轴可由{B}绕yb轴旋转θ+η角得到,则从{B}到{C}的旋转矩阵为:(3) Handlebar coordinate system {C}, Cx c y c z c : the origin is fixed at point C, the y c axis is parallel to the y b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y b axis Obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

其中,C为车把旋转副与后车架的连接点,θ为后车体的俯仰角,η是车把倾角;Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

车把倾角η满足以下几何约束:The handlebar inclination η satisfies the following geometric constraints:

其中,θ0是车把转角为零时的后车架连杆向量的俯仰角,ε是后车架连杆安装角;Among them, θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4)前轮坐标系{D},Dxdydzd:原点固定于D点,zd轴与车把坐标系zc轴平行,x和y两轴可由车把坐标系绕zc轴旋转δ角得到,则从{C}到{D}的旋转矩阵为:(4) Front wheel coordinate system {D}, Dx d y d z d : the origin is fixed at point D, the z d axis is parallel to the z c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

其中,D为前车轮圆心,δ为车把转角;Among them, D is the center of the front wheel, δ is the handlebar angle;

(5)后车体坐标系{G1},G1x1y1z1:原点固定于G1,后车体坐标系由后轮坐标系绕yb轴旋转θ角得到,令中η=0可得从{B}系到{G1}系的旋转矩阵 (5) Rear car body coordinate system {G 1 }, G 1 x 1 y 1 z 1 : the origin is fixed at G 1 , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G 1 } system can be obtained

其中,G1为后车体质心;Among them, G 1 is the center of mass of the rear car body;

(6)前车体坐标系{G2},G2x2y2z2:原点固定于G2,前车体坐标系与前轮坐标系平行,G2为前车体质心。(6) Front car body coordinate system {G 2 }, G 2 x 2 y 2 z 2 : the origin is fixed at G 2 , the front car body coordinate system is parallel to the front wheel coordinate system, and G 2 is the center of mass of the front car body.

进一步地,在本发明的一个实施例中,双轮机器人系统的拖曳距范围为:Further, in one embodiment of the present invention, the range of the trailing distance of the two-wheel robot system is:

其中,R为车轮半径,lr为线段BC的长度,d为线段CC′的长度,lf为前车架线段C′D的长度,λ是车把前叉角,η是车把倾角。Among them, R is the radius of the wheel, l r is the length of line segment BC, d is the length of line segment CC′, l f is the length of line segment C′D of the front frame, λ is the front fork angle of the handlebar, and η is the inclination angle of the handlebar.

进一步地,在本发明的一个实施例中,根据多连杆多关节系统所受的闭环运动链和双轮机器人系统的车轮几何特性建立两个约束方程,包括:Further, in one embodiment of the present invention, two constraint equations are established according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

约束1,闭环运动链约束 Constraint 1, closed-loop kinematic chain constraint

其中,ez=[0,0,1]T为惯性参考系{I}的z轴单位方向向量,r1是由A指向B的后车轮连杆向量;r2是由B指向C的后车架连杆向量;r3是由C指向D的前车架连杆向量;r4是由D指向E的前车轮连杆向量,上标指明该向量所对应的坐标系,为从{B}到{I}的旋转矩阵,为从{C}到{B}的旋转矩阵,为从{D}到{C}的旋转矩阵;Among them, e z =[0,0,1] T is the z-axis unit direction vector of the inertial reference frame {I}, r 1 is the rear wheel linkage vector from A to B; r 2 is the rear wheel linkage vector from B to C The frame link vector; r 3 is the front frame link vector from C to D; r 4 is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

约束2,车轮几何约束 Constraint 2, Wheel Geometry Constraints

其中,ny为前轮坐标系{D}的yd轴单位方向向量。Among them, n y is the unit direction vector of the y d axis of the front wheel coordinate system {D}.

进一步地,在本发明的一个实施例中,根据两个约束方程建立运动学模型,包括:Further, in one embodiment of the present invention, a kinematics model is established according to two constraint equations, including:

对约束1和约束2的约束条件进行求导,得:Deriving the constraint conditions of constraint 1 and constraint 2, we get:

其中,J为雅克比矩阵;雅克比矩阵又可依据广义坐标与非独立坐标分解成两部分:Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

非独立坐标的速度由广义速度来表示: Velocities of dependent coordinates are represented by generalized velocities:

可得,运动学模型为:It can be obtained that the kinematic model is:

其中,为2阶单位矩阵。in, is a second-order identity matrix.

进一步地,在本发明的一个实施例中,利用第一类拉格朗日方程求解双轮机器人动力学模型包括:Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

其中,L=T-V为拉格朗日函数、T为双轮机器人系统的总动能、V为双轮机器人系统的总势能,γ为拉格朗日乘子,为双轮机器人系统受到的广义非保守外力,D和d分别为滚转和车把转向通道所受的扰动力矩,τc为车把转轴驱动力矩。Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

进一步地,在本发明的一个实施例中,建模分析模块具体用于,在拖曳距范围内,对可控性矩阵的奇异值、闭环控制器的吸引域和控制代价进行分析,并做出对应图像,根据对应图像及控制需求确定符合控制需求的拖曳距。Furthermore, in one embodiment of the present invention, the modeling and analysis module is specifically used to analyze the singular values of the controllability matrix, the attractive domain of the closed-loop controller and the control cost within the range of the trailing distance, and make Corresponding to the image, according to the corresponding image and the control requirement, determine the drag distance that meets the control requirement.

需要说明的是,前述对基于拖曳距的双轮机器人建模与静止平衡方法实施例的解释说明也适用于该实施例的装置,此处不再赘述。It should be noted that the foregoing explanations for the embodiment of the trail-based two-wheeled robot modeling and static balancing method are also applicable to the device of this embodiment, and will not be repeated here.

根据本发明实施例提出的基于拖曳距的双轮机器人建模与静止平衡装置,首先根据运动约束,从多刚体系统的角度将双轮机器人等效为前后两车体铰接的多连杆多关节系统。其次基于系统的闭环运动链和车轮与地面的接触特性,建立两个约束方程,得出系统的运动学模型。随后利用第一类拉格朗日方程推导系统的动力学模型。最后,从系统可控性矩阵的奇异值、闭环控制器吸引域和控制能耗三个方面来分析拖曳距对静止平衡的影响。可以反映在不同拖曳距下车把转角与质心高度变化的非线性关系,并且能为拖曳距的选取提供一套分析流程,以提高静止平衡的控制效果。According to the two-wheel robot modeling and static balance device based on the trailing distance proposed by the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheel robot is equivalent to a multi-link multi-joint hinged front and rear two car bodies system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。In addition, the terms "first" and "second" are used for descriptive purposes only, and cannot be interpreted as indicating or implying relative importance or implicitly specifying the quantity of indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include at least one of these features. In the description of the present invention, "plurality" means at least two, such as two, three, etc., unless otherwise specifically defined.

在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。In the description of this specification, descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific examples", or "some examples" mean that specific features described in connection with the embodiment or example , structure, material or feature is included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms are not necessarily directed to the same embodiment or example. Furthermore, the described specific features, structures, materials or characteristics may be combined in any suitable manner in any one or more embodiments or examples. In addition, those skilled in the art can combine and combine different embodiments or examples and features of different embodiments or examples described in this specification without conflicting with each other.

尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and those skilled in the art can make the above-mentioned The embodiments are subject to changes, modifications, substitutions and variations.

Claims (10)

1. A method for modeling and static balancing of a two-wheeled robot based on a towing distance is characterized by comprising the following steps:
s1, when detecting that the two-wheeled robot system meets the preset equivalent setting condition, setting the two-wheeled robot system as a multi-link multi-joint system, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating the towing distance range of the two-wheeled robot system according to the geometrical relationship of the coordinate systems;
s2, establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematic model according to the two constraint equations;
s3, solving the kinematics model by using a first Lagrange equation to obtain a two-wheeled robot dynamics model, analyzing a singular value of a controllability matrix, an attraction domain of a closed-loop controller and a control cost according to the two-wheeled robot dynamics model, and determining a towing distance meeting a control requirement in the towing distance range according to an analysis result.
2. The method according to claim 1, wherein the preset equivalent setting condition comprises:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
3. The method of claim 1, wherein the plurality of coordinate systems are:
(1) inertial reference system { I }, A0xyz: the origin is fixed at A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axes being rotatable about the x-axis by the inertial reference systemThe angle is found, then the rotation matrix from { I } to { B } is:
wherein B is the circle center of the rear wheel,the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
wherein, theta0Is the pitch angle of the rear frame connecting rod vector when the handlebar turning angle is zero, and epsilon is the rear frame connecting rod mounting angle;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1},G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained byWhere η is 0, from { B } system to { G-1Rotation matrix of the system
Wherein G is1Is the center of mass of the rear vehicle body;
(6) front vehicle body coordinate system { G2},G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid.
4. The method of claim 3, wherein the tow-throw range of the two-wheeled robotic system is:
wherein R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfThe length of the front frame line segment C' D, λ is the handlebar fork angle, and η is the handlebar inclination angle.
5. The method of claim 4, wherein establishing two constraint equations based on the closed-loop kinematic chain experienced by the multi-link multi-joint system and the wheel geometry of the two-wheeled robotic system comprises:
constraint 1, closed-loop kinematic chain constraint
Wherein e isz=[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is a rear wheel link pointing from A to BVector quantity; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,for the rotation matrix from { B } to { I },is a rotation matrix from C to B,is a rotation matrix from { D } to { C };
restraint 2, wheel geometry restraint
Wherein n isyY being the front wheel coordinate system { D }dAxial unit direction vector.
6. The method of claim 5, wherein said building a kinematic model according to said two constraint equations comprises:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinatesAnd non-independent coordinatesThe decomposition is carried out in two parts:
the velocity of the non-independent coordinates is represented by the generalized velocity:
the kinematic model can be found as follows:
wherein,is an identity matrix of order 2.
7. The method of claim 5, wherein solving the two-wheeled robot dynamics model using a first class of lagrangian equations comprises:
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance torque on the rolling and steering channels of the handlebar, taucIs the driving torque of the rotating shaft of the handlebar.
8. The method according to claim 1, wherein the S3 specifically includes:
and in the dragging distance range, analyzing singular values of the controllability matrix, the attraction domain of the closed-loop controller and the control cost, making a corresponding image, and determining the dragging distance meeting the control requirement according to the corresponding image and the control requirement.
9. A tow-distance-based two-wheeled robot modeling and static balancing device is characterized by comprising:
the system comprises an equivalence module, a control module and a control module, wherein the equivalence module is used for setting a two-wheeled robot system into a multi-connecting-rod multi-joint system when detecting that the two-wheeled robot system meets preset equivalence setting conditions, defining a plurality of coordinate systems in the multi-connecting-rod multi-joint system, and calculating the dragging distance range of the two-wheeled robot system according to the geometric relations of the coordinate systems;
the constraint module is used for establishing two constraint equations according to a closed-loop kinematic chain borne by the multi-connecting-rod multi-joint system and the wheel geometric characteristics of the two-wheeled robot system, and establishing a kinematic model according to the two constraint equations;
and the modeling analysis module is used for solving the kinematic model by utilizing a first class of Lagrange equations to obtain a two-wheeled robot dynamic model, analyzing the singular value of a controllability matrix, the attraction domain of the closed-loop controller and the control cost according to the two-wheeled robot dynamic model, and determining the towing distance meeting the control requirement in the towing distance range according to the analysis result.
10. The apparatus of claim 9, wherein the preset equivalent setting condition comprises:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
CN201910637561.5A 2019-07-15 2019-07-15 Modeling and static balance method and device of two-wheeled robot based on drag distance Active CN110348140B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910637561.5A CN110348140B (en) 2019-07-15 2019-07-15 Modeling and static balance method and device of two-wheeled robot based on drag distance

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910637561.5A CN110348140B (en) 2019-07-15 2019-07-15 Modeling and static balance method and device of two-wheeled robot based on drag distance

Publications (2)

Publication Number Publication Date
CN110348140A true CN110348140A (en) 2019-10-18
CN110348140B CN110348140B (en) 2021-06-25

Family

ID=68176323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910637561.5A Active CN110348140B (en) 2019-07-15 2019-07-15 Modeling and static balance method and device of two-wheeled robot based on drag distance

Country Status (1)

Country Link
CN (1) CN110348140B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114146363A (en) * 2021-12-14 2022-03-08 国家康复辅具研究中心 A walking aid training system and its integrated control method
CN116184860A (en) * 2023-04-27 2023-05-30 北京国领智能科技有限公司 Magnetic attraction crawler type wall climbing robot dynamics modeling and control method

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1518488A (en) * 2002-03-15 2004-08-04 ���ṫ˾ Motion control device and motion control method for legged mobile robot, and robot device
CN101791800A (en) * 2010-01-21 2010-08-04 西北工业大学 Motion control method of double-wheel differential type robot
CN102780542A (en) * 2012-07-19 2012-11-14 南京邮电大学 Gain factor adjustment method for Hopfield neural network signal blind detection
US20140130700A1 (en) * 2012-11-14 2014-05-15 Swift Tram, Inc. Suspended coach drive bogie
CN105080013A (en) * 2015-07-13 2015-11-25 李占平 Multi-connecting-rod stable supporting structure of special robot and control method for multi-connecting-rod stable supporting structure
CN105320129A (en) * 2015-10-22 2016-02-10 清华大学 Method for tracking and controlling locus of unmanned bicycle
US20170050832A1 (en) * 2015-08-18 2017-02-23 Powerhandling Inc. Dual Motor Drive System for a Material Handling Device
CN106774360A (en) * 2016-11-24 2017-05-31 西北工业大学 Using the target satellite attitude stabilization method of tether/connecting rod in a kind of towing change rail
CN108839015A (en) * 2018-06-01 2018-11-20 重庆锐纳达自动化技术有限公司 A kind of robot two-wheel differential obstacle detouring bobbin movement track acquisition methods
CN208181323U (en) * 2018-04-19 2018-12-04 北京特睿夫科技有限公司 A kind of motor cycle multi link steering structure
CN109070857A (en) * 2016-03-31 2018-12-21 本田技研工业株式会社 The arrester control device of automatic two-wheeled cycle
CN109202889A (en) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 A kind of Flexible Multi-joint robot electric current Force control system and method
CN109634111A (en) * 2017-10-09 2019-04-16 中国科学院沈阳自动化研究所 A kind of high-speed overload Robotic Dynamic Method for Calculating Deformation
CN109885052A (en) * 2019-02-26 2019-06-14 华南理工大学 Error model predictive control method based on kinematic modeling of omnidirectional mobile robot
CN109982859A (en) * 2016-12-09 2019-07-05 佩尔莫比尔公司 Castor assembly and wheelchair including the castor assembly

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1518488A (en) * 2002-03-15 2004-08-04 ���ṫ˾ Motion control device and motion control method for legged mobile robot, and robot device
CN101791800A (en) * 2010-01-21 2010-08-04 西北工业大学 Motion control method of double-wheel differential type robot
CN102780542A (en) * 2012-07-19 2012-11-14 南京邮电大学 Gain factor adjustment method for Hopfield neural network signal blind detection
US20140130700A1 (en) * 2012-11-14 2014-05-15 Swift Tram, Inc. Suspended coach drive bogie
CN105080013A (en) * 2015-07-13 2015-11-25 李占平 Multi-connecting-rod stable supporting structure of special robot and control method for multi-connecting-rod stable supporting structure
US20170050832A1 (en) * 2015-08-18 2017-02-23 Powerhandling Inc. Dual Motor Drive System for a Material Handling Device
CN105320129A (en) * 2015-10-22 2016-02-10 清华大学 Method for tracking and controlling locus of unmanned bicycle
CN109070857A (en) * 2016-03-31 2018-12-21 本田技研工业株式会社 The arrester control device of automatic two-wheeled cycle
CN106774360A (en) * 2016-11-24 2017-05-31 西北工业大学 Using the target satellite attitude stabilization method of tether/connecting rod in a kind of towing change rail
CN109982859A (en) * 2016-12-09 2019-07-05 佩尔莫比尔公司 Castor assembly and wheelchair including the castor assembly
CN109202889A (en) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 A kind of Flexible Multi-joint robot electric current Force control system and method
CN109634111A (en) * 2017-10-09 2019-04-16 中国科学院沈阳自动化研究所 A kind of high-speed overload Robotic Dynamic Method for Calculating Deformation
CN208181323U (en) * 2018-04-19 2018-12-04 北京特睿夫科技有限公司 A kind of motor cycle multi link steering structure
CN108839015A (en) * 2018-06-01 2018-11-20 重庆锐纳达自动化技术有限公司 A kind of robot two-wheel differential obstacle detouring bobbin movement track acquisition methods
CN109885052A (en) * 2019-02-26 2019-06-14 华南理工大学 Error model predictive control method based on kinematic modeling of omnidirectional mobile robot

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BO XIA 等: "Research on virtual decomposition control of free-flying space robot with an object under nonholonomic constraints", 《2017 13TH IEEE CONFERENCE ON AUTOMATION SCIENCE AND ENGINEERING (CASE)》 *
孙亚军: "基于空间RSSR机构FSAE赛车后轮悬架与转向机构的优化设计", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
张志亮: "FSAE赛车操纵稳定性优化与仿真分析", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
王乐: "大学生方程式赛车多连杆悬架设计优化及性能分析", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
邢斌: "一种变结构双轮机器人的动力学建模与智能控制", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114146363A (en) * 2021-12-14 2022-03-08 国家康复辅具研究中心 A walking aid training system and its integrated control method
CN116184860A (en) * 2023-04-27 2023-05-30 北京国领智能科技有限公司 Magnetic attraction crawler type wall climbing robot dynamics modeling and control method

Also Published As

Publication number Publication date
CN110348140B (en) 2021-06-25

Similar Documents

Publication Publication Date Title
JP2024512235A (en) Control method and device for legged and wheeled robot, legged and wheeled robot and equipment
US7251638B2 (en) Intelligent robust control system for motorcycle using soft computing optimizer
CN113064344B (en) A trajectory tracking control method for a multi-axle unmanned heavy-duty vehicle
CN109421042B (en) Robust self-adaptive sliding mode control method for flexible hinge space station mechanical arm
WO2008041732A1 (en) Moving object with tilt angle estimating mechanism
CN110109354A (en) A kind of reaction wheel balance-bicycle robot adaptive sliding-mode observer method
CN111290272A (en) Attitude stability adjustment method based on multi-legged robot
WO2022227460A1 (en) Pose prediction method and apparatus, electronic device, and storage medium
CN110348140A (en) Based on towing away from two-wheel robot modeling and static balance method and device
Krid et al. Design and control of an active anti-roll system for a fast rover
CN113467478A (en) Track tracking control method for metamorphic heart unmanned carrying vehicle
CN111158376A (en) Coordinated control method for trajectory tracking of rocker rocker-type planetary vehicle in soft and rough terrain
CN111290419A (en) Two-wheel self-balancing vehicle self-adaptive sliding mode control method with time-varying time-lag input
EP4371793A1 (en) Multi-degree-of-freedom active damping mechanism control method, system and damping mechanism
Gans et al. Visual servo velocity and pose control of a wheeled inverted pendulum through partial-feedback linearization
Limebeer et al. Optimal control of a NASCAR–specification race car
CN115431790B (en) AFS and DYC collaborative control method for 8-wheel distributed electric drive vehicle
CN114510066B (en) Tilt rotor heavy-load logistics unmanned aerial vehicle control system
CN118915514B (en) Multi-vehicle coordinated transportation method, device and system
CN111523207B (en) Method, device, equipment and medium for modeling whole vehicle platform and detecting vehicle performance
CN110109353B (en) Fuzzy self-adaptive sliding-mode control system of counteractive wheel balance bicycle robot
CN114407008B (en) Robot motion control method, device, equipment and storage medium
CN110275439B (en) Control method of self-balancing trolley and design method and device of controller
CN114859733A (en) Differential steering unmanned vehicle trajectory tracking and attitude control method
CN114488790A (en) Omnidirectional mobile robot self-adaptive sliding film control method based on nominal model

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant