本发明涉及机械臂视觉伺服控制，具体涉及一种带两级扰动补偿的动态视觉伺服控制方法。
背景技术：
1、机器人视觉伺服系统是一个带有图像传感器的复合系统，不仅受到成像系统的影响，还会受到机械臂系统的影响。在成像过程中受分辨率、镜头畸变等标定参数影响，数据传输过程中图像噪声、传输延时等问题，都降低了从图像中提取的有用信息的精度。此外，由于机械臂系统复杂程度高，存在多种多样的不确定性因素影响其控制性能，比如未建模动态、外部干扰等建模不确定性，这些均导致了系统在动态性能和稳定性方面的控制难度。
2、鉴于视觉控制器的输入信号为图像特征误差，以及图像雅可比矩阵的深度估计需要图像信息，不确定性为图像噪声、相机标定误差、视觉延迟等，这会影响图像雅可比矩阵的计算，最终导致视觉伺服性能受到影响。在处理不确定性时，基于图像的视觉伺服由于不需要进行三维重构，对相机标定误差有更高的鲁棒性；对于其余不确定性，可以在硬件方面提高光照条件或使用低噪声传感器，以保证在图像采集阶段尽可能少地引入图像噪声，或在软件方面使用小波降噪、深度学习方法、滤波器等进行处理。卡尔曼滤波作为一种非常经典的滤波算法，能够处理带有噪声的动态系统和提供良好的系统状态预测结果，在降低图像噪声影响的方面具有较好的作用。在视觉伺服系统中使用虚拟视觉伺服和卡尔曼滤波结合的位姿与速度估计器可以降低图像噪声的影响，并且能高效地估计所需信息。
3、视觉伺服技术如果仅仅只是用于分拣和喷涂等普通场合，基于运动学的视觉伺服基本就能满足任务要求，即假设机械臂是一个理想的笛卡尔运动装置，能够根据指令准确地运动到所需位姿。但当机械臂在高速、动态或不确定的环境中操作时，就需要考虑机械臂的动力学特性，研究基于动力学视觉伺服的技术。传统的pid控制已经很难满足基于动力学的视觉伺服系统的控制需求，难以处理模型中的多种不确定性。计算转矩控制作为一种基于动力学模型的控制方法，可以对重力、速度和加速度耦合等因素相关的不确定性进行处理，在机械臂能够精确建模、参数准确的条件下，其控制器结构简单、使用方便。但是由于动力学模型的参数随时间变化或其先验知识不够充分，难以建立精确的动力学模型，使得基于模型的控制难以得到良好的效果。自抗扰控制不需要描述输入输出关系的详细数学模型，可以将外部干扰、未建模动态等不确定性视为总扰动进行估计和补偿，自其提出以来，已经在控制工程的各个领域实现了应用，并展现出极好的效果。在视觉伺服系统中使用自抗扰控制器可以实现高精度动态伺服控制，并处理各种不确定性。
4、综上所述，针对具有图像噪声、未建模动态、外部扰动等不确定性的机械臂视觉伺服系统，设计一种带两级扰动补偿的动态视觉伺服控制方法具有重要的理论和实用价值。
技术实现思路
1、针对视觉伺服系统中存在的不确定性问题，本发明提供一种带两级扰动补偿的动态视觉伺服控制方法，设计合理，解决了现有技术的不足，具有良好的效果。
2、为了实现上述目的，本发明采取如下技术方案：
3、一种带两级扰动补偿的动态视觉伺服控制方法，包括以下步骤：
4、步骤1、设计基于虚拟视觉伺服的位姿与速度估计器；
5、步骤2、利用末端执行器运动模型和位姿与速度估计器结果，分别设计位置卡尔曼滤波器和旋转角卡尔曼滤波器，对估计结果进行滤波处理后重新合成位姿矩阵和速度矩阵；
6、步骤3、利用单关节动力学方程为机械臂各关节独立设计非线性自抗扰控制器；
7、步骤4、利用经验公式法为部分参数确定取值范围，引入沙猫群算法为自抗扰控制器进行参数整定。
8、进一步地，所述步骤1中，被控对象为串联机械臂的视觉伺服系统，该系统为眼在手型，即相机安装在机械臂末端，该相机为真实相机；设定一个虚拟相机，虚拟相机所处位姿和所获取的图像均与真实相机相同，当真实相机到达新的位姿时获取当前真实图像，虚拟相机根据虚拟图像与真实图像中图像特征误差来进行运动，当误差收敛时虚拟相机与虚拟物体的相对位姿即真实相机与真实物体的相对位姿；具体过程如下：
9、设m表示实际对象点pj在图像平面上对应的图像特征，m*表示虚拟对象点p0在图像平面上对应的图像特征；
10、定义虚拟对象点的空间速度cvvir和虚拟图像特征的时间导数的关系为：
11、
12、式中，cp＝[cx,cy,cz]为虚拟对象点p0在虚拟相机坐标系中的坐标，δt为单位时间间隔，cv和cw为虚拟对象点的线速度和角速度，i为单位矩阵，2dj3d为二维点与三维点的变换矩阵，为一阶近似下的三维点交互矩阵，l1为二维点交互矩阵，即图像雅可比矩阵；
13、m*与m之间的图像特征误差evir定义如下：
14、evir＝m*-m； (2)
15、使图像特征误差evir呈指数收敛的形式，则有：
16、
17、式中，λ为虚拟视觉伺服中视觉控制律的增益；
18、将式(1)、式(2)代入式(3)中得到：
19、
20、式中，为虚拟视觉伺服中图像雅可比矩阵的逆矩阵；
21、刚体作一般运动的任意位移都能够分解为平移增量δpi和旋转增量δθi，其表达式为：
22、
23、
24、式中，v(t)为刚体线速度，i表示从t0时刻到ti时刻的i个时间间隔，k表示从t0时刻到tk时刻的k个时间间隔，vk为tk时刻到tk+1时刻间的线速度，w(t)为刚体角速度，wk为tk时刻到tk+1时刻间的角速度；线速度v(t)和角速度w(t)均为的分量；
25、δθi是绕特定旋转轴的旋转增量，其对应的旋转矩阵δri可以通过叉乘矩阵[]×和矩阵指数exp()计算得到：
26、δri＝exp([δθi]×)； (7)
27、在持续时间iδt＝ti-t0内，通过采用速度指令cvvir和加速度指令按照刚体运动的位移增量δpi和δθi去控制虚拟相机实现对应的运动，则虚拟图像特征m*会更新为新的图像特征m'，在多次重复操作之后最终在tj时刻图像特征收敛到m，此时evir最小，这时虚拟相机与虚拟对象点的相对位姿即实际相机与实际对象点的相对位姿；在估计位姿时，初始虚拟相机及虚拟图像的给定是任意的，而在估计速度时，将前一时刻的估计结果作为虚拟视觉伺服的初始给定，按照视觉控制规律得到位姿估计结果后，的累加值即为速度估计结果；
28、在已知实际对象点位姿的前提下，基于虚拟视觉伺服设计位姿与速度估计器，将当前图像特征m和初始虚拟相机位姿t0作为输入，最终得到真实相机位姿矩阵和速度矩阵由于相机安装方式为眼在手型，即可知机械臂末端执行器的位姿与速度。
29、进一步地，所述步骤2中，首先对于位姿与速度估计器输出的位姿矩阵和速度矩阵，将4×4的位姿矩阵，分解为位置向量t3×1与旋转矩阵r3×3，利用式(8)得到对应轴的旋转角：
30、
31、其中，θx、θy、θz分别为绕x、y、z轴的旋转角，r11为旋转矩阵中第1行第1列的元素；r21为旋转矩阵中第2行第1列的元素，r31为旋转矩阵中第3行第1列的元素，r32为旋转矩阵中第3行第2列的元素，r33为旋转矩阵中第3行第3列的元素；
32、然后，根据刚体运动学知识，分别得到末端执行器位置、旋转角与速度的状态空间方程，即末端执行器的运动模型；
33、六自由度机械臂末端执行器的位置的离散时间状态方程和输出方程表示为：
34、
35、式中，x是末端执行器的位置和线速度，u是末端执行器的线加速度，y是估计器提供的位置和线速度估计结果，ξ是过程噪声向量，与系统动态特性和环境条件相关，η是测量噪声向量，与图像噪声、相机标定误差等有关，协方差分别为q和r，即p(ξ)∈n(0,q)，p(η)∈n(0,r)，ad，bd，cd是常数矩阵；
36、将位姿矩阵中的位置估计值和速度矩阵中的线速度估计值作为测量向量，利用状态空间方程设计位置卡尔曼滤波器：
37、
38、式中，是状态预测值，是状态估计值，kk是卡尔曼增益，表示了状态最优估计过程中模型预测误差和测量误差的比重，∑是状态估计值的协方差矩阵，∑-是状态预测值的协方差矩阵；
39、六自由度机械臂末端执行器的旋转角的离散时间状态方程和输出方程表示为：
40、
41、式中，x'是末端执行器的旋转角和角速度，输入变量u'是末端执行器的角加速度，测量向量y'是估计器提供的旋转角和角速度速度估计结果；
42、将位姿矩阵中的旋转角估计值与速度矩阵中的角速度估计值作为测量向量，设计旋转角卡尔曼滤波器：
43、
44、最后根据式(13)和(14)得到旋转矩阵，与滤波后的位置向量合成为位姿矩阵；
45、
46、r＝rzryrx； (14)
47、式中，r为整体的旋转矩阵，rx、ry、rz分别为x、y、z轴旋转角对应的旋转矩阵，i为x轴的单位向量，j为y轴的单位向量，k为z轴的单位向量；
48、将滤波后的线速度向量和角速度向量合成为速度矩阵。
49、进一步地，所述步骤3中，六自由度机械臂动力学方程简化为单关节动力学方程：
50、
51、式中，q为关节角度，为关节角速度，为关节角加速度，为总扰动，ω为外部扰动，τ为输入力矩，b为输入力矩的系数；
52、六自由度机械臂的每个驱动关节都单独设计非线性自抗扰控制器nladrc，包括扩张状态观测器eso、非线性状态误差反馈控制律nlsef和干扰补偿装置dcd；
53、eso用于估计系统的状态变量和总扰动具体为：
54、
55、式中，z1为机械臂关节角度q的跟踪信号，为z1的微分信号，z2是关节角度q的一阶微分信号，为z2的微分信号，z3为系统总扰动的估计值，为z3的微分信号，e为关节角度的跟踪误差；β01、β01、β03为eso的增益系数，α0、α1为用户定义的非线性因子，取值在0与1之间，δ0、δ1为用户定义的滤波因子，b0是补偿系数的估计；
56、eso得到状态变量的观测值z1、z2和z3，视觉伺服系统给出关节角度指令qd和关节速度指令nlsef以非线性的形式将其结合，利用fal函数构造非线性控制律，具体为：
57、
58、式中，e1为角度误差，e2为角速度误差，qd和是期望角度和期望速度指令，τ0为nlsef的原始控制力矩，β1和β2是误差和误差微分的增益系数，α3为用户定义的非线性因子，δ2、δ3为用户定义的滤波因子；
59、在eso估计到系统总扰动后，dcd对扰动进行补偿，具体如下：
60、τ＝τ0-z3/b0； (19)
61、式中，τ是机械臂的实际输入力矩，z3/b0是系统的总扰动补偿量。
62、进一步地，所述步骤4中具体过程如下：
63、步骤4.1、初始化参数；假设沙猫的种群规模为n，所优化的参数的维数为n，搜索空间的上下限分别为ub、lb，迭代最大次数为t；自抗扰控制器中部分参数取值为α0＝0.25，α1＝0.5，δ0＝δ1＝0.01，其余参数用沙猫群算法进行整定，取值范围为β01∈[0,150]，β02∈[0,7500]，β03∈[0,12500]，β1∈[0,500]，β2∈[0,50]，b0∈[0,5]；
64、设置适应度函数：
65、
66、式中，j为适应度值，w1、w2、w3、w4为权重因子；e1为反馈误差；ez1、ez2为eso所观测的角度q和角速度的误差；u为输入力矩；
67、步骤4.2、初始化种群；引入pwlcm混沌映射，进行pwlcm混沌种群初始化，如下所示：
68、
69、pc＝lb+(ub-lb)*xi+1； (22)
70、式中，xi为rand函数产生的随机数，xi+1为pwlcm混沌映射后的随机数，fp为pwlcm混沌映射的整体函数，p为pwlcm混沌映射的控制参数，p∈(0,0.5)，xi∈(0,1)，i为当前迭代个体序号；pc为沙猫个体的初始位置，ub和lb分别为种群位置的上下限；
71、然后根据所设计的适应度函数计算沙猫个体的适应度值，选择最优的沙猫个体作为最佳位置pbc，其余个体在后续迭代中向该个体附近移动；
72、步骤4.3、搜寻最优参数值，进行搜索行为和攻击行为；
73、假设sm模拟了沙猫的听觉特征，其值为2，听觉灵敏度rg表达式为：
74、
75、式中，t'为当前迭代次数，t为最大迭代次数；
76、为避免陷入局部最优，每只沙猫的灵敏度范围是不同的，其表达式为：
77、r＝rg*rand； (24)
78、式中，r表示每只沙猫的灵敏度范围，rand为0到1的随机数；
79、听觉灵敏度rg随着迭代次数的增加由2线性递减为0，因此引入一种非线性调整策略来描述沙猫种群的捕猎过程：
80、
81、式中，η为控制曲线非线性程度的参数；
82、利用公式(25)为rg赋值，根据η值不同，rg的曲线也发生变化；
83、r为控制搜索行为和攻击行为过渡的参数，其表达式为：
84、r＝2*rg*rand-rg； (26)
85、当|r|>1时，沙猫进行搜索行为，每只沙猫根据最佳位置pbc、当前位置pc及灵敏度范围r更新自己的位置，因此沙猫能够找到其他可能的最佳猎物位置；
86、沙猫群搜索行为的数学模型为：
87、pc(t+1)＝r*(pbc(t)-rand*pc(t))； (27)
88、式中，pc(t+1)为沙猫群进行搜索行为后移动到的位置；
89、当|r|≤1，沙猫进行攻击行为，首先按照最佳位置pbc和当前位置pc来生成一个随机位置，其表达式为：
90、pr＝|rand*pbc(t)-pc(t)|； (28)
91、假设沙猫的灵敏度范围是一个圆，利用轮盘赌法给每一只沙猫随机选择一个角度θ，最后通过公式(29)实现攻击行为；
92、沙猫群攻击行为的数学模型为：
93、pc'(t+1)＝pc(t)-r*pr*cos(θ)； (29)
94、式中，pc'(t+1)为沙猫群进行攻击行为后移动到的位置，pr为随机位置；
95、步骤4.4、将当前迭代中的沙猫个体所代表的参数值赋给nladrc控制器，并分别运行视觉伺服系统，按照不同的参数取值，系统会输出不同的性能指标，计算各自的适应度值；
96、步骤4.5、引入淘汰机制，将参数整定效果最差，即适应度最高的沙猫个体进行淘汰，并为了保持种群总数不变，根据当前沙猫群位置的区间随机生成新的个体；
97、步骤4.6、判断是否达到最大迭代次数，若没有达到，则进行下一轮搜索行为和攻击行为，继续寻找最佳猎物位置，即最佳参数值；若达到最大迭代次数，则结束沙猫群参数整定，得到当前迭代次数下的最优nladrc参数值。
98、本发明所带来的有益技术效果：
99、本发明提供一种带两级扰动补偿的动态视觉伺服控制方法，利用虚拟视觉伺服、卡尔曼滤波器和自抗扰控制技术设计了动态视觉伺服框架，实现了机械臂动态视觉伺服系统的控制。其有益效果包括1)设计了一种基于虚拟视觉伺服和卡尔曼滤波位姿与速度估计器，可以仅利用图像信息估计所需信息，且降低了图像噪声的影响。2)针对结构特性不同的各关节分别设计了非线性自抗扰控制器，能够估计和补偿不确定性。3)设计了基于改进沙猫群优化算法的参数整定方法，使得复杂繁琐的参数整定工作变得简单，减少了参数整定耗费的人工时间，提高了视觉伺服的控制性能。
1.一种带两级扰动补偿的动态视觉伺服控制方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种带两级扰动补偿的动态视觉伺服控制方法，其特征在于，所述步骤1中，被控对象为串联机械臂的视觉伺服系统，该系统为眼在手型，即相机安装在机械臂末端，该相机为真实相机；设定一个虚拟相机，虚拟相机所处位姿和所获取的图像均与真实相机相同，当真实相机到达新的位姿时获取当前真实图像，虚拟相机根据虚拟图像与真实图像中图像特征误差来进行运动，当误差收敛时虚拟相机与虚拟物体的相对位姿即真实相机与真实物体的相对位姿；具体过程如下：
3.根据权利要求2所述的一种带两级扰动补偿的动态视觉伺服控制方法，其特征在于，所述步骤2中，首先对于位姿与速度估计器输出的位姿矩阵和速度矩阵，将4×4的位姿矩阵，分解为位置向量t3×1与旋转矩阵r3×3，利用式(8)得到对应轴的旋转角：
4.根据权利要求3所述的一种带两级扰动补偿的动态视觉伺服控制方法，其特征在于，所述步骤3中，六自由度机械臂动力学方程简化为单关节动力学方程：
5.根据权利要求4所述的一种带两级扰动补偿的动态视觉伺服控制方法，其特征在于，所述步骤4中具体过程如下：
