| 主权项 |
一种基于关节位置控制器的机器人末端力控制方法，其特征在于：包括以下步骤：1)将期望的机器人末端运动的起点位置矢量p<sub>o</sub>和终点位置矢量p<sub>t</sub>，代入机器人的逆运动学方程，分别计算得出机器人每个关节运动的起始角度矢量θ<sub>o</sub>和终止角度矢量θ<sub>t</sub>；2)根据步骤1)得到的每个关节运动的起始角度矢量θ<sub>o</sub>和终止角度矢量θ<sub>t</sub>，通过多项式插值计算得出连续的关节运动角度函数，函数输入为时间计数t，函数输出为关节角度θ(t)；3)设定期望接触力f<sub>d</sub>，其中期望接触力f<sub>d</sub>包括最大期望接触力f<sub>dmax</sub>和最小期望接触力f<sub>dmin</sub>；4)接触力控制环节将机器人末端与目标的实际接触力f与步骤3设定的期望接触力f<sub>d</sub>比较，如果f>f<sub>dmax</sub>，则时间计数t增加Δt，Δt为机器人控制指令的执行周期；如果f<f<sub>dmin</sub>，则时间计数t减小Δt；如果f<sub>dmin</sub>≤f≤f<sub>dmax</sub>，则时间计数t不变，即<maths num="0001" id="cmaths0001"><math><![CDATA[<mrow><mi>t</mi><mo>=</mo><mfenced open = '{' close = ''><mtable><mtr><mtd><mrow><mi>t</mi><mo>+</mo><mi>Δ</mi><mi>t</mi></mrow></mtd><mtd><mrow><mo>(</mo><mi>f</mi><mo>></mo><msub><mi>f</mi><mrow><mi>d</mi><mi>m</mi><mi>a</mi><mi>x</mi></mrow></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>t</mi></mtd><mtd><mrow><mo>(</mo><msub><mi>f</mi><mrow><mi>d</mi><mi>m</mi><mi>i</mi><mi>n</mi></mrow></msub><mo>≤</mo><mi>f</mi><mo>≤</mo><msub><mi>f</mi><mrow><mi>d</mi><mi>m</mi><mi>a</mi><mi>x</mi></mrow></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mrow><mi>t</mi><mo>-</mo><mi>Δ</mi><mi>t</mi></mrow></mtd><mtd><mrow><mo>(</mo><mi>f</mi><mo><</mo><msub><mi>f</mi><mrow><mi>d</mi><mi>min</mi></mrow></msub><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>;</mo></mrow>]]></math><img file="FDA0000945569140000011.GIF" wi="649" he="223" /></maths>5)将步骤4)得到的时间计数t，代入到步骤2)得到的关节运动角度函数，计算得出期望关节角度θ；6)将步骤5)得到期望关节角度θ作为控制指令发送给机器人的关节位置控制器，关节位置控制器根据控制指令控制机器人各关节运动。 |
