[发明专利]基于PD控制器的六自由度机械臂自适应智能检测方法

权利要求书

1.一种基于PD控制器的六自由度机械臂自适应智能检测方法，其特征在于，包括以下步骤：

(1)、激光跟踪仪返回被测试样件的空间位姿初始点p_i和下一个位姿点p_w，其中，i＝1,N,2N,…,KN，K、N均为常数，w＝N，2N，…，(K+1)N；

(2)、利用插值法在初始点p_i和下一个位姿点p_w之间插入N-2个过渡位姿点p_j使运动轨迹平滑，其中，j＝CN+1,CN+2,…,CN+N-2，C为常数；

(3)、根据逆运动学代数法将初始点p_i和下一个位姿点p_j的位姿信息按照一定的顺序反解成各个关节的期望关节角i'＝1,2,3,4,5,6，表示六自由度，并将期望关节角传输给控制器；

(4)、利用PD控制器对期望关节角进行跟踪控制，并将控制力矩传输给六自由度机械臂动力学模型，由动力学模型输出机械臂实际转动关节角θ_i'；

(5)、判断期望关节角和实际转动关节角θ_i'之差的范数是否不大于阈值ε，如果则进入步骤(6)；否则将期望关节角和实际转动关节角θ_i'之差输入给PD控制器，作为闭环反馈控制，直至满足时，再进入步骤(6)；

(6)、判断当前过渡位姿点p_j所处位置，如果j＜w，则将p_j点作为空间位姿初始点，p_j+1点作为下一个位姿点，再返回步骤(3)，直到j＝w-1；如果j＝w，则将期望关节角通过正运动学计算出新的空间位姿点，并反馈给激光跟踪仪作为新的空间位姿初始点，同时由激光跟踪仪返回出检测的下一个位姿点p_w，w＝2N,3N,…,(K+1)N，再返回步骤(2)，直至检测到第p_(K+1)N点时结束，完成被测试样件的检测。

2.根据权利要求1所述的基于PD控制器的六自由度机械臂自适应智能检测方法，其特征在于，所述步骤(3)中，按照一定的顺序反解各个关节的期望关节角的方法为：

(2.1)、根据机械臂的运动学D-H坐标模型及相关参数计算出各关节的变换矩阵T_i'，r_3×3是反应位姿点的姿态信息，b_3×1是反应位姿点的位置信息；

(2.2)、将激光跟踪仪返回的位姿点p_i用矩阵表示为L_i；将步骤(2.1)中计算得到的各关节的变换矩阵T_i'相乘，得到总变换矩阵R，L_i和R均为4×4的矩阵；

令矩阵L_i的第一行第四列与矩阵R的第一行第四列相等，矩阵L_i的第二行第四列与矩阵R的第二行第四列相等，即有：其中，p_xw是机械臂末端位置沿x轴的平移量，p_yw是机械臂末端位置沿y轴的平移量，通过该方程组求解出关节角

(2.3)、用第一个关节的变换矩阵T_1'的逆矩阵⁰T_1'^-1分别左乘矩阵L_i和R，得到矩阵L₁和R₁，再令矩阵L₁的第一行第四列与矩阵R₁的第一行第四列相等，矩阵L₁的第三行第四列与矩阵R₁的第三行第四列相等，即有：p_zw是机械臂末端位置沿z轴的平移量，d₁是关节1的偏移量，d₄是关节4的偏移量，a₁是机械臂连杆1的长度，a₂是机械臂连杆2的长度，通过该方程组求解出关节角

(2.4)、用第二个关节的变换矩阵T_2'的逆矩阵¹T_2'^-1分别左乘矩阵L₁和R₁，得到矩阵L₂和R₂，再令矩阵L₂的第一行第四列与矩阵R₂的第一行第四列相等，矩阵L₂的第二行第四列与矩阵R₂的第二行第四列相等，即有：通过该方程组求解出关节角

(2.5)、用第三个关节的变换矩阵T_3'的逆矩阵²T_3'^-1分别左乘矩阵L₂和R₂，得到矩阵L₃和R₃，再令矩阵L₃的第一行第三列与矩阵R₃的第一行第三列相等，矩阵L₃的第三行第三列与矩阵R₃的第三行第三列相等，即有：其中，L₃(3,3)表示L₃的第三行第三列，通过该方程组求解出关节角

(2.6)、令矩阵L₃的第二行第一列与矩阵R₃的第二行第一列相等，矩阵L₃的第二行第二列与矩阵R₃的第二行第二列相等，即有：通过该方程组求解出关节角

(2.7)、令矩阵L₃的第二行第三列与矩阵R₃的第二行第三列相等，矩阵L₃的第三行第三列与矩阵R₃的第三行第三列相等，即有：通过该方程组求解出关节角