[发明专利]一种欠驱动机械臂系统的PD平衡控制方法

权利要求书

1.一种欠驱动机械臂系统的PD平衡控制方法，其特征在于：包括以下步骤：

步骤1：欠驱动机械臂系统的控制器发出电压信号，启动直流电机，直流电机输出转矩信号控制主驱动臂和欠驱动臂从垂直向下的初始位置摆起至目标平衡位置；

步骤2：用主驱动臂的编码器和欠驱动臂的编码器分别实时检测主驱动臂的输出角度和欠驱动臂的输出角度，判断主驱动臂和欠驱动臂是否偏离目标平衡位置，是，则执行步骤3；否则保持当前直流电机的输出转矩，使主驱动臂和欠驱动臂保持在目标平衡位置；

步骤3：对欠驱动机械臂系统进行PD平衡控制，控制主驱动臂和欠驱动臂保持在目标平衡位置；

步骤3.1：根据欠驱动机械臂系统的动力学模型，分别针对直流电机的转矩信号和主驱动臂的输出角度、直流电机的转矩信号和欠驱动臂的输出角度，建立并联的主驱动臂线性模型和欠驱动臂线性模型，求得主驱动臂线性模型的输出角度和欠驱动臂线性模型的输出角度：

qi*(k+1)=(1-Ai(z-1))qi(k+1)+Bi(z-1)u(k)]]>

其中：i＝1,2，i＝1表示主驱动臂，i＝2表示欠驱动臂；是线性模型的输出角度；q_i(k+1)是编码器检测到的输出角度；u(k)为直流电机的转矩信号；A_i(z^-1)、B_i(z^-1)为表征欠驱动机械臂系统动力学特性的参数，z^-1是后移算子，A_i(z^-1)＝1+a_i1z^-1+a_i2z^-2，B_i(z^-1)＝b_i0+b_i1z^-1，a_i1,a_i2,b_i0,b_i1是未知参数；

步骤3.2：将主驱动臂的编码器检测到的主驱动臂的输出角度与主驱动臂线性模型的输出角度的差值作为主驱动臂的虚拟未建模动态，欠驱动臂的编码器检测到的欠驱动臂的输出角度与欠驱动臂线性模型的输出角度的差值作为欠驱动臂的虚拟未建模动态：

vi[x(k)]=qi(k+1)-qi*(k+1)]]>

步骤3.3：根据主驱动臂线性模型、欠驱动臂线性模型、直流电机的转矩信号、主驱动臂的虚拟未建模动态和欠驱动臂的虚拟未建模动态建立欠驱动机械臂系统的离散数学模型：

A_i(z^-1)q_i(k+1)＝B_i(z^-1)u(k)+v_i[x(k)]

其中，q_i(k+1)是编码器检测到的输出角度；v_i[x(k)]是虚拟未建模动态；u(k)为直流电机的转矩信号；i＝1,2，i＝1表示主驱动臂，i＝2表示欠驱动臂；A_i(z^-1)、B_i(z^-1)为表征欠驱动机械臂系统动力学特性的参数；

步骤3.4：建立虚拟未建模动态补偿的PD控制模型：

H_i(z^-1)u_i(k)＝R_i(z^-1)w_i(k)-G_i(z^-1)q_i(k)-K_i(z^-1)v_i[x(k-1)]

式中：H_i(z^-1)，R_i(z^-1)，G_i(z^-1)，K_i(z^-1)为虚拟未建模动态补偿的PD控制模型参数，H_i(z^-1)＝(1+h_iz^-1)，h_i是待定系数；R_i(z^-1)＝G_i(z^-1)＝g_i0+g_i1z^-1，g_i0＝K_pi+K_di，g_i1＝-K_di，K_pi和K_di是比例系数和微分系数；u_i(k)为虚拟未建模动态补偿的PD控制模型的输出，即直流电机的转矩信号；w_i(k)是当前k时刻的目标平衡位置；q_i(k)是当前k时刻的输出角度；v_i[x(k-1)]为k-1时刻的虚拟未建模动态；

步骤3.5：根据主驱动臂的编码器检测的主驱动臂的输出角度信号和欠驱动臂的编码器检测的欠驱动臂的输出角度信号、直流电机的转矩信号求解k-1时刻的虚拟未建模动态：

vi[x(k-1)]=qi(k)-qi*(k)]]>

式中：qi*(k)=(1-Ai(z-1))qi(k)+Bi(z-1)u(k-1)]]>是k时刻线性模型的输出角度；

步骤3.6：将虚拟未建模动态补偿的PD控制模型代入欠驱动机械臂系统的离散数学模型，得到欠驱动机械臂系统的闭环方程：

[A_i(z^-1)H_i(z^-1)+z^-1B_i(z^-1)G_i(z^-1)]q_i(k+1)＝B_i(z^-1)G_i(z^-1)w_i(k)+[H_i(z^-1)-B_i(z^-1)K_i(z^-1)]v_i[x(k-1)]+H_i(z^-1)Δv_i[x(k)]

式中：Δv_i[x(k)]＝v_i[x(k)]-v_i[x(k-1)]，v_i[x(k)]为当前k时刻的虚拟未建模动态；

步骤3.7：采用闭环极点配置方法确定虚拟未建模动态补偿的PD控制模型参数H_i(z^-1)、R_i(z^-1)和G_i(z^-1)；

步骤3.8：令H_i(z^-1)-B_i(z^-1)K_i(z^-1)＝0，使得K_i(1)＝H_i(1)/B_i(1)，确定虚拟未建模动态补偿的PD控制模型参数K_i(z^-1)；

步骤3.9：根据建立的虚拟未建模动态补偿的PD控制模型得到虚拟未建模动态补偿的PD控制模型的输出，进而得到直流电机的转矩信号u(k)：

u(k)＝αu₁(k)+βu₂(k)

式中：α和β是线性加权系数；

步骤3.10：根据得到的直流电机的转矩信号调整直流电机的输出转矩，将主驱动臂和欠驱动臂控制在目标平衡位置。

2.根据权利要求1所述的欠驱动机械臂系统的PD平衡控制方法，其特征在于：步骤3.3所述的欠驱动机械臂系统的离散数学模型中的表征欠驱动机械臂系统动力学特性的参数A_i(z^-1)、B_i(z^-1)通过对欠驱动机械臂系统的机理模型进行线性化处理得到或者通过工业过程中PID参数的经验值反向求解得到。