CN107457783B - 基于pd控制器的六自由度机械臂自适应智能检测方法 - Google Patents

Abstract

本发明公开了一种基于PD控制器的六自由度机械臂自适应智能检测方法，通过高精度激光跟踪仪对待测试件进行空间位置坐标静态标定或者动态跟踪，获得高精度的空间位姿点，再通过将运动学和动力学结合的建模、仿真方法，在运动学过程中，通过给出的位姿点巧妙的逆解出关节角，在动力学中，通过设计PD控制器，精确的进行路径跟踪闭环控制，使整个系统结合高精度激光跟踪仪和六自由度机械臂,实现对物体的精确定位和智能化自适应检测,更符合实际工业的需求。

Description

基于PD控制器的六自由度机械臂自适应智能检测方法

技术领域

本发明属于智能化自适应检测技术领域，更为具体地讲，涉及一种基于PD控制器的六自由度机械臂自适应智能检测方法。

背景技术

随着机器人工业水平包括控制技术、制造技术、材料技术等的不断提高，机械臂已被广泛应用于各自动化领域，如航空航天等大型设备制造检测，食品加工，医疗手术等。虽然目前工业级机械臂的精度已经很高，但是为了适应更高准确度且更加智能化的去生产工作，就必须设计新的系统实现自适应智能化检测，并提高建模控制等各方面的精度。

目前，大部分对机械臂运动学与动力学的研究都是分开进行建模和仿真。在运动学中，都是通过繁琐的逆解方式求出相应关节角，生成期望轨迹，而在动力学中，给定期望角和初始角，通过控制算法进行轨迹跟踪。但实际过程中，给予机械臂的是点的位姿信息，分开建模进行实验不符合实际对于机械臂的需求，而更合理的方式是运动学结合动力学一起建立模型进行仿真。

激光跟踪仪在航空航天、汽车制造、电子工业、高能粒子加速器工程以及大尺寸计量等行业中，已有广泛应用，如检测试件的表面设计误差，跟踪试件的动态位置等。为了提高系统坐标的精确度以及实现自适应智能化，此发明中将其作为使用机械臂检测试件的第一步，即使用高精度的激光跟踪仪来标定物件的空间位置坐标，在避免人为测量引起误差的同时，实现智能化识别目标物件。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于PD控制器的六自由度机械臂自适应智能检测方法，将运动学和动力学进行结合建模、仿真，通过减少计算复杂度来实现六自由度机械臂的自适应智能检测。

为实现上述发明目的，本发明为一种基于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点时结束，完成被测试样件的检测。

本发明的发明目的是这样实现的：

本发明一种基于PD控制器的六自由度机械臂自适应智能检测方法，通过高精度激光跟踪仪对待测试件进行空间位置坐标静态标定或者动态跟踪，获得高精度的空间位姿点，再通过将运动学和动力学结合的建模、仿真方法，在运动学过程中，通过给出的位姿点巧妙的逆解出关节角，在动力学中，通过设计PD控制器，精确的进行路径跟踪闭环控制，使整个系统结合高精度激光跟踪仪和六自由度机械臂,实现对物体的精确定位和智能化自适应检测,更符合实际工业的需求。

同时，本发明基于PD控制器的六自由度机械臂自适应智能检测方法还具有以下有益效果：

(1)、通过设计的PD控制器直接对六自由度机械臂系统进行跟踪，使其能够从初始点到达期望点；

(2)、通过在运动学中使用了新的且更加巧妙的逆解方法，逆解出关节角，减轻了计算复杂度；

(3)、将运动学和动力学进行结合建模和仿真，能够满足六自由度机械臂的需求。

附图说明

图1是本发明一种基于PD控制器的六自由度机械臂自适应智能检测方法流程图；

图2是机械臂的D-H坐标模型图及对应的实物图；；

图3是第一个位姿点的关节角跟踪曲线图；

图4是第一个位姿点的关节末端点的三维空间轨迹图；

图5是第二个位姿点的关节角跟踪曲线图；

图6是第二个位姿点的关节末端点的三维空间轨迹图。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

实施例

图1是本发明基于PD控制器的六自由度机械臂自适应智能检测方法流程图。

在本实施例中，如图1所示，本发明一种基于PD控制器的六自由度机械臂自适应智能检测方法，包括以下步骤：

S1、激光跟踪仪返回被测试样件的空间位姿初始点p_i和下一个位姿点p_w，其中，i＝1,N,2N,…,KN，K、N均为常数，w＝N，2N，…，(K+1)N；在本实施例中，每返回的一个空间位姿点其实为一个变化矩阵，变化矩阵中包括了该点的位置信息和姿态信息。

S2、在本实施例中，可以利用四元数插值法或三角度差值法在初始点p_i和下一个位姿点p_w之间插入N-2个过渡位姿点p_j使运动轨迹平滑，其中，j＝CN+1,CN+2,…,CN+N-2，C为常数；这样初始点、期望到达的下一个位姿点和中间过渡插值点共N个点，这插值出的N个点的位置信息(x,y,z)可构成末端位置在三维空间中的轨迹，对应于N个变化矩阵，相应的角度值为

m＝1,2,3,4,5,6；n＝1,2,3,…h,h+1,…N，h为常数，总时间为X秒，间隔时间为(X/N)秒,每隔(X/N)秒系统会输出给控制器一组角度值

即可描绘出初始点、期望到达点和中间过渡插值点角度值随时间变化曲线。

S3、根据逆运动学代数法将初始点p_i和下一个位姿点p_j的位姿信息按照一定的顺序反解成各个关节的期望关节角

i'＝1,2,3,4,5,6，表示六自由度，并将期望关节角

传输给PD控制器；

下面是反解的具体过程进行详细描述：

使用D-H法建立机械臂的运动学D-H坐标模型，如图2(b)图所示，对应的实物机械结构，如图2(a)图所示；在每个关节角建立如图2(b)所示的(x_i',y_i',z_i')坐标系，i'＝1,2,3,4,5,6，图中

i₁＝1,2，表示每一条公垂线的长度，即连杆长度，

i₂＝1,4,6，表示在z轴上两条相邻的公垂线之间的距离或称关节偏移。

根据机械臂相关参数构建机械臂D-H参数，对应于六个关节角，D-H参数包括绕z轴的实际转动关节角θ_i'，z轴上两条相邻的公垂线之间的距离或称关节偏移量用

表示，d₁＝0.352,d₄＝0.38,d₆＝0.065，每一条公垂线的长度，即连杆长度用

i₃＝1,2a₁＝0.07,a₂＝0.36，相邻z轴之间的称关节扭角用α_j，j＝0,1,…,5，α₀＝0,α₁＝90°,α₂＝0,α₃＝90°,α₄＝-90°,α₅＝90°；其中，

α_j和

是机械臂固定结构，θ_i'为关节变量。

结合图2，反解出各个关节的期望关节角

的步骤为：

S3.1、根据机械臂的运动学D-H坐标模型及相关参数计算出各关节的变换矩阵T_i'，

r_3×3是反应位姿点的姿态信息，b_3×1是反应位姿点的位置信息；

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

令矩阵L_i的第一行第四列与矩阵R的第一行第四列相等，矩阵L_i的第二行第四列与矩阵R的第二行第四列相等，即有：

其中，p_xw是机械臂末端位置沿x轴的平移量，p_yw是机械臂末端位置沿y轴的平移量，通过该方程组求解出关节角

S3.3、用第一个关节的变换矩阵T₁的逆矩阵⁰T₁ ^-1分别左乘矩阵L_i和R，得到矩阵L₁和R₁，

其中，省略矩阵R₁前三列；

再令矩阵L₁的第一行第四列与矩阵R₁的第一行第四列相等，矩阵L₁的第三行第四列与矩阵R₁的第三行第四列相等，即有：

p_zw是机械臂末端位置沿z轴的平移量，d₁是关节1的偏移量，d₄是关节4的偏移量，a₁是机械臂连杆1的长度，a₂是机械臂连杆2的长度，通过该方程组求解出关节角

S3.4、用第二个关节的变换矩阵T₂的逆矩阵¹T₂ ^-1分别左乘矩阵L₁和R₁，得到矩阵L₂和R₂，

省略矩阵R₂前三列；

再令矩阵L₂的第一行第四列与矩阵R₂的第一行第四列相等，矩阵L₂的第二行第四列与矩阵R₂的第二行第四列相等，即有：

通过该方程组求解出关节角

S3.5、用第三个关节的变换矩阵T₃的逆矩阵²T₃ ^-1分别左乘矩阵L₂和R₂，得到矩阵L₃和R₃，

再令矩阵L₃的第一行第三列与矩阵R₃的第一行第三列相等，矩阵L₃的第三行第三列与矩阵R₃的第三行第三列相等，即有：

其中，L₃(3,3)表示L₃的第三行第三列，通过该方程组求解出关节角

S3.6、令矩阵L₃的第二行第一列与矩阵R₃的第二行第一列相等，矩阵L₃的第二行第二列与矩阵R₃的第二行第二列相等，即有：

通过该方程组求解出关节角

S3.7、令矩阵L₃的第二行第三列与矩阵R₃的第二行第三列相等，矩阵L₃的第三行第三列与矩阵R₃的第三行第三列相等，即有：

通过该方程组求解出关节角

同理，可以反解出下一个位姿点p_j的各个关节的期望关节角。

S4、利用PD控制器直接对六自由度机械臂系统进行跟踪，也就是直接对期望关节角

进行跟踪控制，使其能够从初始点到达期望点，并将控制力矩传输给六自由度机械臂动力学模型，由动力学模型输出机械臂实际转动关节角θ_i'；

在本实施例中，采用的PD控制器的模型为：

e＝θ^*-θ

其中，θ^*为期望关节角，θ为关节角，

为关节角速度，K_p、K_d为PD控制器的比例系数，e为实际关节角和期望关节角之间的误差。

采用的六自由度机械臂动力学模型为：

其中，θ为关节角，

为关节角速度，

为关节角加速度，u为控制力矩，M(θ)为惯性矩阵，

为离心力和科氏力矩阵，g(θ)为重力矩阵。

S5、判断期望关节角

和实际转动关节角θ_i'之差的范数是否不大于阈值ε，如果||θ-θ^*||≤ε，则进入步骤S6；否则将期望关节角

和实际转动关节角θ_i'之差输入给PD控制器，作为闭环反馈控制，直至满足||θ-θ^*||≤ε时，再进入步骤S6；

S6、判断当前过渡位姿点p_j所处位置，如果j＜w，则将p_j点作为空间位姿初始点，p_j+1点作为下一个位姿点，再返回步骤S3，直到j＝w-1；如果j＝w，则将期望关节角

通过正运动学计算出新的空间位姿点，并反馈给激光跟踪仪作为新的空间位姿初始点，同时由激光追踪仪返回出检测的下一个位姿点p_w，w＝2N,3N,…,(K+1)N，再返回步骤S2，直至检测到第p_(K+1)N点时结束，完成被测试样件的检测。

实验仿真

根据整体技术路线图实现实验仿真

1.由激光跟踪仪返回初始点A₁(initial)和期望到达点B₁(desired)的位姿信息，即

并在初始点和期望到达点中插值18个点，初始点、期望到达点和中间过渡点共20个点，这20个点的位置信息(x,y,z)可构成末端位置在三维空间中的轨迹，对应于20个变换矩阵，相应的角度值为

m＝1,2,3,4,5,6；n＝1,2,3,…h,h+1,…20，总时间为10秒，间隔时间为0.5秒,每隔0.5秒系统会输出给控制器一组角度值

控制器模块通过计算，将力矩输出给动力学模块，通过闭环反馈实现由初始点至期望到达点。表1中列出了初始点、期望到达点以及中间插值点所对应的角度值和末端位置在三维空间中的坐标；

表1是初始点A₁、期望到达点B₁以及中间插值点所对应的角度值和末端位置坐标；

表1

随时间变化，相应的六个关节的角度值变化，以及末端点三维空间轨迹如下图3，图4所示。如图3所示，图的横轴表示时间(time)，图的纵轴表示角度值(Radians)，六个关节角的实际转动关节角θ_i'的变化趋势分别用6种不同的曲线来描绘，其中，i'＝1,2,3,4,5,6，而每个θ_i'的期望轨迹则用虚点曲线来勾画，由图3可以看出，通过PID控制器，六个关节角的θ_i'从初始点对应角度的弧度值

平滑的运动到期望点对应角度的弧度值

而在图4中，描绘的是在三维空间中机械臂末端点轨迹与期望轨迹，图4中实线表示的是机械臂末端点实际的轨迹曲线，星点虚线表示的是机械臂末端点的期望轨迹，由图看出，实际轨迹根据每个插值点路径与期望轨迹趋于重合，达到仿真目的。

2.由激光跟踪仪返回初始点A₂(initial)和期望到达点B₂(desired)的位姿信息，即

同样在初始点和期望到达点中插值18个点，初始点、期望到达点和中间过渡点共20个点，这20个点的位置信息(x,y,z)可构成末端位置在三维空间中的轨迹，对应于20个变换矩阵，相应的角度值为

m＝1,2,3,4,5,6；n＝1,2,3,…h,h+1,…20，总时间为10秒，间隔时间为0.5秒,每隔0.5秒系统会输出给控制器一组角度值

控制器模块通过计算，将力矩输出给动力学模块，通过闭环反馈实现由初始点至期望到达点。表2中列出了初始点、期望到达点以及中间插值点所对应的角度值和末端位置在三维空间中的坐标；

表2是初始点A₂、期望到达点B₂以及中间插值点所对应的角度值和末端位置坐标；

表2

随时间变化，相应的六个关节的角度值变化，以及末端点三维空间轨迹如图5，图6所示。在图5中，六个关节角的角度值θ_i'从初始点对应角度的弧度值

平滑的运动到期望点对应角度的弧度值

在图6中机械臂末端点实际轨迹根据每个插值点路径与期望轨迹趋于重合。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (5)

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₃的第三行第三列相等，即有：

通过该方程组求解出关节角

3.根据权利要求1所述的基于PD控制器的六自由度机械臂自适应智能检测方法，其特征在于，所述的六自由度机械臂动力学模型为：

其中，θ为关节角，

为关节角速度，

为关节角加速度，u为控制力矩，M(θ)为惯性矩阵，

为离心力和科氏力矩阵，g(θ)为重力矩阵。

4.根据权利要求1所述的基于PD控制器的六自由度机械臂自适应智能检测方法，其特征在于，所述的PD控制器的模型为：

e＝θ^*-θ

其中，θ^*为期望关节角，θ为实际关节角，

为关节角速度，K_p、K_d为PD控制器的比例系数，e为实际关节角和期望关节角之间的误差。

5.根据权利要求1所述的基于PD控制器的六自由度机械臂自适应智能检测方法，其特征在于，所述的插值法采用四元数或三角度差值法。

Images