[发明专利]移动机器人动力学建模和轨迹跟踪控制方法及装置

权利要求书

1.一种基于层级式约束的移动机器人动力学建模和轨迹跟踪控制方法，其特征在于，包括：

步骤1，建立无约束下移动机器人的第一动力学方程；

步骤2，基于所述第一动力学方程，根据移动机器人的结构特征建立二阶结构约束方程，求取结构约束力，获得移动机器人具有结构约束的第二动力学方程；

步骤3，基于所述第二动力学方程，根据移动机器人的跟踪轨迹建立二阶运动约束方程，并根据渐进收敛准则ACC处理初始条件不相容问题，求取运动约束力，获得移动机器人具有结构约束和运动约束的第三动力学方程；

步骤4，根据所述第三动力学方程进行轨迹跟踪控制；

所述步骤1具体包括：

根据牛顿力学，基于公式1-4建立无约束下移动机器人系统的第一动力学模型：

其中，移动机器人包括一个具有驱动和转向功能的前轮和两个支撑后轮，m为移动机器人的质量，I₁为移动机器人的转动惯量，I₂为移动机器人前轮的转动惯量，θ为移动机器人的方向角，ψ为移动机器人前轮转向角，(x₁，y₁)为移动机器人前轮质心C₁在坐标系的位置，Q₁₁、Q₁₂、Q₁₃、Q₁₄为给定力；

所述步骤2具体包括：

根据移动机器人的前轮受力平衡得到第一个结构约束：

对时间t进行微分得到第一个二阶形式的方程：

根据移动机器人方向角和方向角速度的关系得到第二个结构约束：

其中，移动机器人速度v为：

将公式7对时间t进行微分得到公式9：

其中，ω为移动机器人的方向角速度，d为移动机器人前轮质心C₁到两后轮连线中点C的距离，v为移动机器人前轮的速度；

根据公式10-12所述的矩阵方程确定移动机器人所受的结构约束：

根据Udwadia-Kalaba理论，根据公式13-15确定结构约束力：

B_s＝A_sM^-1/2          公式14；

其中，

根据公式16确定具有结构约束的移动机器人的第二动力学方程为：

所述步骤3具体包括：

根据移动机器人系统追踪轨迹的变化确定移动机器人系统的第二层约束即第一运动约束方程；

基于移动机器人系统的前轮质心C₁和两后轮连线中点C在轨迹上移动，采用ACC处理，得出移动机器人系统的第二运动约束方程；

基于移动机器人C₁点和C点沿着轨迹顺次推移，得到移动机器人系统的第三个运动约束方程；

将三个运动约束方程放入一个矩阵，矩阵表达式为：

其中，

A₁₁＝-0.1sin(0.1x₁)-0.01x₁cos(0.1x₁)

A₂₂＝-0.1sin(0.1x₁-0.1dcosθ)-0.01x₁cos(0.1x₁-0.1dcosθ)+0.01dcosθcos(0.1x₁-0.01dcosθ)

A₂₃＝-dcosθ-0.01dx₁sinθcos(0.1x₁-0.1dcosθ)-0.1dsinθsin(0.1x₁-0.1dcosθ)+0.01d²sinθcosθcos(0.1x₁-0.1dcosθ)

基于移动机器人由三个电机分别提供其驱动、转向、前轮转向，根据Udwadia-Kalaba理论得到运动约束力：

将写为公式20：

其中C为常数矩阵，u为控制输入力；在控制输入力u中，F_x为作用力在x轴的分量，F_y为作用力在y轴的分量，U_θ为移动机器人的转矩，U_ψ为移动机器人的前轮转矩；

确定移动机器人的驱动力矩U_d为:

根据公式24确定控制约束力：

确定无约束移动机器人系统受结构约束和运动约束下的第三动力学方程为：