[发明专利]用于六自由度机械臂示教控制的PC端APP系统

权利要求书

1.用于六自由度机械臂示教控制的PC端APP系统，其特征在于：PC端APP安装在用户的Windows电脑上；所述的PC端APP包括依次连接的交互界面模块、APP主控模块、轨迹优化模块、控制文件生成模块，控制文件生成模块通过文件传输协议FTP将控制代码文件下载到一个或多个示教机械臂中，示教机械臂完成优化后的示教工作；

APP主控模块与检测示教机械臂的运动参数信息的数据采集器以WiFi通信连接；

各模块的具体构成是：

交互界面模块，用于用户对整个示教系统进行设置和操作，以及显示示教系统的各种相关运动参数信息和数据；通过PC端APP的交互界面，用户可以设置要连接的WiFi热点的IP地址和网络端口号、示教机械臂各个关节内的编码器的正反向标定、实时角度数据的采集频率、示教机械臂各个轴的轴地址、所使用的示教机械臂型号以及控制代码文件的下载路径，并将这些设置运动参数信息传给APP主控模块处理；用户还能在PC端APP的交互界面进行WiFi通信的连接和断开、实时角度数据采集的开始和停止、生成控制代码文件、查询数据采集器就绪状态、查询实时角度数据采集频率和周期以及查询示教机械臂各轴的轴地址操作，这些操作指令会传给APP主控模块进行处理；交互界面还会对所有能设置的运动参数信息和采集得到的实时角度数据进行显示，这些运动参数信息和数据从APP主控模块得到；

APP主控模块，是PC端APP的通信中枢和控制中枢，对各种数据、运动参数信息和指令进行收发和处理；APP主控模块可以控制PC端APP通过WiFi通信与数据采集器建立通信连接；可以从交互界面模块接收到运动参数信息设置指令和操作指令，进行处理后生成相应通信协议的控制指令，并通过WiFi通信发送给数据采集器和通过网络通信控制仿真模块；能够通过WiFi通信接收来自数据采集器的包括实时角度数据的运动参数信息，将包括实时角度数据的运动参数信息传给交互界面显示，并在示教结束后即停止数据采集后生成示教机械臂的各个关节的运动角度数据包，将运动角度数据包传给轨迹优化模块进行优化和通过网络通信发给仿真模块进行仿真；

轨迹优化模块，接收来自APP主控模块的示教机械臂的各个关节的运动角度数据包，结合当前所使用的示教机械臂型号的结构参数，根据D-H参数建模方法建立示教机械臂每个杆件在各个关节处的笛卡尔坐标系，利用正运动学公式求解末端执行器相对固定参考坐标系的位置和姿态，求出多组解后即得到了示教轨迹；再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化；最后利用逆运动学公式结合当前所使用的示教机械臂型号的结构参数对优化后的轨迹求其关节空间中各关节的关节变量，即可获得到每个采集时刻优化后的运动角度数据包；可将运动角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成；

仿真模块，使用RoboDK仿真软件，模拟示教机械臂的工作轨迹执行情况；PC端APP可以将采集得到的示教过程中示教机械臂的各个关节的运动角度数据包直接通过网络通信传输给RoboDK，RoboDK软件中的虚拟示教机械臂会执行相应的示教运动轨迹；能够对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包在轨迹优化模块中进行优化后，再通过网络通信将优化后的运动角度数据包传输给RoboDK，RoboDK软件中的虚拟示教机械臂就会执行相应的优化后的示教运动轨迹；当确定RoboDK软件中执行的优化后的示教运动轨迹是满足工作需求的后，再让控制文件生成模块生成控制实际示教机械臂完成工作所需的控制代码文件；

控制文件生成模块，生成控制实际示教机械臂完成工作所需的控制代码文件；其输入是轨迹优化模块对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包进行优化后的运动角度数据包；若仿真模块肯定了优化方案的可行性，则控制文件生成模块在收到优化后的运动角度数据包后，就将其转化为可以控制实际示教机械臂执行工作的控制代码文件；再通过FTP(文件传输协议)将控制代码文件下载到一个或多个示教机械臂中，由示教机械臂完成优化后的示教工作。

2.根据权利要求1所述的用于六自由度机械臂示教控制的PC端APP系统，其特征在于：所述的轨迹优化模块从PC端APP主控模块接收到运动角度数据包后，结合了正逆运动学求解和笛卡尔轨迹规划算法来优化示教轨迹，再将优化后的运动角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成，所述的正逆运动学求解和笛卡尔轨迹规划算法如下：

2.1)选择示教机械臂六个关节的D-H运动参数信息即关节1≤i≤6的连杆长度a_i、连杆转角α_i、连杆偏距d_i和关节角θ_i作为输入量，选择轨迹优化后的示教机械臂各个关节的关节角θ_i作为输出量；

2.2)首先对示教机械臂构建其D-H参数模型，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，用以下四个运动参数信息来描述杆件：

(1)连杆长度a_i：关节i的轴和关节i+1的轴之间公垂线n_i的长度；

(2)连杆转角α_i：作一个与两关节轴之间的公垂线垂直的平面，将关节i的轴和关节i+1的轴投影到该平面，在平面内关节i的轴按右手法则绕连杆转向关节i+1的轴，其转角即为连杆转角；

(3)连杆偏距d_i：公垂线n_i-1与关节i的轴的交点到公垂线n_i与关节i+1的轴的交点的有向距离长度；

(4)关节角θ_i：a_i-1的延长线与a_i之间绕关节i的轴旋转所形成的夹角；

2.3)以基座坐标系作为T₀坐标系，设表示i关节坐标系相对i-1关节坐标系的位置和姿态，则：

其中，sθ_i＝sinθ_i，cθ_i＝cosθ_i，sα_i＝sinα_i，cα_i＝cosα_i；

2.4)对于六自由度机械臂，获得示教机械臂末端执行器位姿相对固定参考坐标系的变换矩阵，即示教机械臂末端位置点坐标的变换矩阵：

2.5)求得每个采集时刻的示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹；

2.6)使用笛卡尔圆弧插补法，先取示教轨迹中的p₁(x₁,y₁,z₁)、p₂(x₂,y₂,z₂)和p₃(x₃,y₃,z₃)三点，这三点组成的平面方程为Ax+By+Cz＝1，则可由下式解得平面方程参数A、B和C的值：

2.7)求出p₁p₂和p₂p₃两条线段的中点p₁₂(x₁₂,y₁₂,z₁₂)，p₂₃(x₂₃,y₂₃,z₂₃)，设圆心坐标为p_o(x_o,y_o,z_o)，求解获得圆心坐标，以圆心为球心，r为半径的球参数方程为：

2.8)将上式代入平面方程式Ax+By+Cz＝1中，得到：

A(rcosφcosα+x_o)+B(rcosφsinα+y_o)+C(rsinα+z_o)＝1

从而获得空间圆的参数方程为：

2.9)取矢量为X₁轴向量，平面法向量为Z₁轴向量，建立T₁坐标系，由于姿态变换矩阵为单位正交矩阵，则有Y₁＝X₁×Z₁，进而可以求得Y₁轴向量，其姿态变换矩阵R和平移向量T为：

设则有：

姿态变换矩阵R和平移向量T可以得到坐标变换矩阵为：

2.10)取末端轨迹在T₁的位置坐标为p'(x',y',z')，通过姿态变换矩阵和平移向量可以求得末端轨迹在T₀中的位置p(x,y,z)，即为圆弧插补的规划轨迹；

2.11)在优化后的轨迹上选择需要的插补点，并列出每个点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵：

2.12)对每个优化后的插补点p(x,y,z)进行逆运动学求解，其中r_ij取与插补点最近的末端坐标点的变换矩阵中的值，则可按顺序求解得到：

θ₂₃＝A tan 2[(-a₃-a₂c₃)p_z-(c₁p_x+s₁p_y)(d₄-a₂s₃)

(a₂s₃-d₄)p_z-(a₃+a₂c₃)(c₁p_x+s₁p_y)]

θ₂＝θ₂₃-θ₃

θ₄＝A tan 2(-r₁₃s₁+r₂₃c₁,-r₁₃c₁c₂₃-r₂₃s₁c₂₃+r₃₃s₂₃)

s₅＝-[r₁₃(c₁c₂₃c₄+s₁s₄)+r₂₃(s₁c₂₃c₄-c₁s₄)-r₃₃(s₂₃c₄)]

c₅＝r₁₃(-c₁s₂₃)+r₂₃(-s₁s₂₃)+r₃₃(-c₂₃)

θ₅＝A tan 2(s₅,c₅)

s₆＝-r₁₁(c₁c₂₃s₄-s₁c₄)-r₂₁(s₁c₂₃s₄+c₁c₄)+r₃₁(s₂₃s₄)

c₆＝r₁₁[(c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅]+r₂₁[(s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅]-r₃₁(s₂₃c₄c₅+c₂₃s₅)

θ₆＝A tan 2(s₆,c₆)

其中，A tan 2为反正切函数，s_i＝sinθ_i，c_i＝cosθ_i；

2.13)在每个关节角度求出后，接着在每个关节可活动的范围内通过对关节角度加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值；求出每个优化后的末端位姿对应的各个关节角度，整理成运动角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。