[发明专利]一种基于分段控制的2R欠驱动平面机械臂的位置控制方法

权利要求书

1.一种基于分段控制的2R欠驱动平面机械臂的位置控制方法，其特征在于具体实现方法为：

（一）建立动力学模型

针对第一关节主动、第二关节被动的2R欠驱动平面机械臂，考虑各关节的摩擦力，建立系统动力学模型如下

D11·q..1+D12·q..2+C11·q.1·q.2+C12·q.22+B1·q.1+sgn(q.1)·B1j=τ1D21·q..1+D22·q..2+C21·q.12+B2·q.2+sgn(q.2)·B2j=0---(1)]]>式中，D₁₁、D₁₂、D₂₁、D₂₂、C₁₁、C₁₂和C₂₁分别是动力学参数，q₁、和q₂、分别为第一关节、第二关节的位置、速度和加速度变量，τ₁为第一关节的电机驱动力矩，而B₁和B₂分别是第一关节、第二关节的粘性摩擦系数，B_1j和B_2j是各自的库仑摩擦力，sgn(x)为符号函数；

D11=m1r12+J1+m1l12+m2l1l2+13m2l22+m2l1l2cos(q2)+Jm]]>D12=12m2l22+12m2l1l2cos(q2)]]>

D₂₁=D₁₂D22=13m2l22]]>

C₁₁=-m₂l₁l₂sin(q₂)C₂₁=C₁₂

C12=-12m2l1l2sin(q2)]]>C₂₂=0

其中，m₁和m₂分别为两杆的质量，l₁和l₂分别为两杆的长度，J₁为第一关节的转动惯量，J_m为电机转动惯量，r₁为第一杆质心到端点的长度；

（二）分段位置控制，具体包含如下步骤：

首先，这里限定2R机器人两杆成一条直线的状态为初始位形，并设此时关节位置都为0rad；欠驱动机械臂分五个阶段完成操作任务，各阶段的关节运动分别如下：

第一阶段：第一关节顺时针从初始位置q₁₀运动到预定位置q_1d，关节运动规律为：

q1=q10+(q1d-q10)(tT-12πsin2πtT)=q10+(q1d-q10)(kt-12πsin2πkt)---(2)]]>

式中，T是采样时间间隔，k为时间尺度参数；

此阶段，当第一关节运动时，第二关节利用与第一关节之间的动力学耦合间接获得驱动力，产生相应的运动，其中第二关节的预定加速度通过动力学方程(1)的第二式计算得到，即对该式进行两次积分，得到第二关节预定位置q_2d，此时第一关节的电机驱动力矩为

τ1=(D12-D11D21-1D22)q..2+b2-D11D21-1b2---(3)]]>

式中，q₂为第二关节的实际位置，为了使第二关节很好跟踪预定值，对第二关节施加PID位置控制，控制算法为

u=q..2=q..2d+KP2(q2d-q2)+KD2(q.2d-q.2)+KI2∫(q2d-q2)dt---(4)]]>

式中，K_P2、K_I2和K_D2分别为PID控制器的位置、积分和微分增益；

第二阶段：将第一关节的位置作为控制目标，控制第一关节在q_1d保持不动，对第二关节不作控制，使第二关节利用惯性继续运动，并在接近预定位置0rad时刚好进入关节的摩擦区；此阶段对第一关节运用PID控制，控制算法为

q..1=q..1d+KP1(q1d-q1)+KD1(q.1d-q.1)+KI1∫(q1d-q1)dt---(5)]]>

式中，K_P1、K_I1和K_D1分别为PID控制器的位置、积分和微分增益；

第三阶段：在第二阶段结束时刻，第一关节、第二关节的速度都接近0rad/s，此阶段仍然将第一关节的位置作为控制目标，并运用PID控制使第一关节在q_1d附近稳定不动，控制算法如式(5)；由于第一关节运动速度接近0rad/s，第二关节不能从第一关节获得间接驱动力，因而相对第一关节静止；

第四阶段：第一关节按照如(2)式所示运动规律逆时针运动返回到位置q₁₀，并要求第二关节相对第一关节静止不动；此阶段将第二关节的角度位置作为控制目标，并对第二关节施加PID控制方法，控制算法见式(4)；

由动力学模型(1)知，当第二关节通过动力学耦合从第一关节获得的间接驱动力小于关节摩擦力时，第二关节就会相对第一关节位置保持不动，即必须满足

|D21·q..1+C21·q.12|≤B2j---(6)]]>

这时第一关节的输入力矩同时需要满足

τ1=-D11D12(b2j+B2·q.2)+B1j+B1·q.1q.1>ϵD11D12(b2j-B2·q.2)-B1j+B1·q.1q.1<ϵ0q.1∈{-ϵ,ϵ}---(7)]]>

当调节k值并减小，即增大运动时间T时，则减小了第一关节的运动速度和加速度因而两个关节之间的动力学耦合也随之减小；当两个关节之间的动力学耦合值满足式(6)，并同时使第一关节的输入力矩满足式(7)，即能够保证在第一关节运动，第二关节保持相对静止；

第五阶段：在第四阶段结束时刻，两个关节速度都趋于0rad/s，此阶段使两个关节同时保持稳定不动，将第一关节的位置作为控制目标，这时对第一关节运用PID控制，控制算法见式(5)。