[发明专利]弹丸取放特种机器人的建模与控制方法

摘要

本发明属于程序控制机械手技术，具体涉及一种弹丸取放特种机器人的建模与控制方法。该方法实现了对弹丸取放特种机器人的控制，使得机器人的取放弹丸动作精准可靠，其步骤是：1、坐标系确定；2、获取弹丸取放特种机器人的D‑H参数；3、通过D‑H参数利用齐次变换矩阵的方式进行坐标系变换，得到位置信息0T5；4、得到运动学正解公式和运动学逆解公式并输入至上位机；5、上位机对弹丸取放特种机器人进行控制完成取放弹丸动作。

主权项

1.一种弹丸取放特种机器人的建模与控制方法，其特征在于，包括以下步骤：

1)获取弹丸取放特种机器人的运动学正解和运动学逆解；

1.1)分别建立弹丸取放特种机器人的五个运动轴的坐标系，记为OX_iY_iZ_i；i＝1～5，i代表当前运动轴；其中，Z_i轴为第i个运动轴的方向；X_i轴为沿Z_i‑1轴和Z_i轴的共法线方向；Y_i轴通过右手定则确定；

定义基坐标系为OX₀Y₀Z₀；基坐标系的原点为安装弹丸取放特种机器人可伸缩平台与补给车的连接点在X₁轴的投影；基座标系OX₀Y₀Z₀的方向和弹丸取放特种机器人第1运动轴的坐标系OX₁Y₁Z₁的方向一致；

1.2)获取弹丸取放特种机器人的D‑H参数；所述D‑H参数为弹丸取放特种机器人第1运动轴坐标系与基坐标系之间的相对位置和方向信息以及弹丸取放特种机器人中每两个运动轴坐标系之间的相对位置和方向信息，其包括连杆扭角α_i‑1、连杆长度a_i‑1、连杆偏距d_i以及连杆夹角θ_i；

其中，连杆扭角α_i‑1为使Z_i‑1轴绕X_i‑1轴旋转到与Z_i同一平面内所需要旋转的角度；

连杆长度a_i‑1为使Z_i‑1轴沿X_i‑1轴移动到与Z_i轴同一直线所需要平移的距离；

连杆偏距d_i为使坐标系OX_i‑1Y_i‑1Z_i‑1和坐标系OX_iY_iZ_i的原点重合所需要沿Z_i轴的平移距离；

连杆夹角θ_i为使X_i‑1轴沿Z_i轴旋转到与X_i同一直线上所需要旋转的角度；

1.3)通过D‑H参数利用齐次变换矩阵的方式进行坐标系变换，得到弹丸取放特种机器人末端中心点相对于基坐标系的位置信息⁰T₅；

1.4)通过弹丸取放特种机器人末端中心点相对于基坐标系的位置信息建立弹丸取放特种机器人的运动学正解公式和弹丸取放特种机器人的运动学逆解公式；

2)将步骤1.4)获得的弹丸取放特种机器人的运动学正解公式和运动学逆解公式输入上位机；

3)输入弹丸取放特种机器人末端输出轴相对于基座标系需要到达的目标位置(X,Y,Z,A,B,C)；

4)通过运动学逆解公式获得5个运动轴各自的位置指令信息(θ₁,d₂,θ₃,θ₄,θ₅)；

5)5个运动轴各自的位置指令信息分别乘以比例系数K_i得到每个运动轴其电机对应要转动的脉冲数；然后将计算得到的脉冲数发送给控制各个轴动作的电机驱动器，电机驱动器分别对各自电机进行控制；所述K_i代表第i个轴的位置指令信息和该轴脉冲数的比例关系；

6)获取各个电机编码器反馈的各电机实际走过的位置脉冲数，分别乘以1/K_i，得到5个运动轴各自的实际位置信息(θ₁′,d₂′,θ₃′,θ₄′,θ₅′)，然后根据运动学正解，得到弹丸取放特种机器人末端中心点到达的实际位置(X′,Y′,Z′,A′,B′,C′)，并在上位机上进行显示；

7)判断弹丸取放动作是否成功；若步骤6)实际位置(X′,Y′,Z′,A′,B′,C′)和步骤3)的目标位置(X,Y,Z,A,B,C)相同，则说明弹丸取放成功；

若步骤6)实际位置(X′,Y′,Z′,A′,B′,C′)和步骤3)的目标位置(X,Y,Z,A,B,C)不同，则将目标位置(X,Y,Z,A,B,C)作为输入再次重复步骤3)至步骤7)，直至弹丸取放成功。

2.根据权利要求1所述的弹丸取放特种机器人的建模与控制方法，其特征在于:所述步骤1.3)具体是：

从OX_i‑1Y_i‑1Z_i‑1到坐标系OX_iY_iZ_i的齐次变换矩阵^i‑1T_i依次表示为：

将上述5个齐次变换矩阵相乘，得到弹丸取放特种机器人末端中心点相对于基坐标系的位置信息⁰T₅：

⁰T₅＝⁰T₁¹T₂²T₃³T₄⁴T₅。

3.根据权利要求1所述的弹丸取放特种机器人的建模与控制方法，其特征在于:所述弹丸取放特种机