基于预测控制的光伏板安装机器人抓取系统的控制方法与流程

本发明涉及太阳能光伏板智能安装，具体涉及室外太阳能光伏板智能安装机器人的抓取系统及控制方法。

背景技术：

1、太阳能板主要是用来吸收太阳能，是一种将太阳能转化为电能存储的装置，其主要由硅片以及光伏玻璃板构成。太阳能板安装支架采用多行多列布置形式，传统安装方式是由多人一组将太阳能板抬运至安装位置，再由人工将太阳能板上抬至安装支架上，在人工搬运安装过程中，太阳能板很容易受到损伤，且存在安装效率低、人工成本高以及安全隐患多等问题。需研究采用自动化光伏板安装机器人替代人工安装解决上述问题。设计一种基于预测控制的光伏板安装机器人抓取系统和控制方法。

技术实现思路

1、针对现有的光伏板安装机器人中基于视觉伺服机械臂抓取光伏板存在抓取精度不低，累计误差大，适应性差的问题，本发明将设计一种基于预测控制的机器人视觉伺服方法抓取系统及控制方法。解决由于数据采集转换，数据传输、数据计算导致时延，并造成机械臂关节累计误差的问题，从而减小系统各部分带来的时延误差，提升系统的控制精度与稳定性。

2、本发明的技术方案如下：

3、一种基于预测控制的光伏板安装机器人抓取系统，包括六轴机械臂、抓取装置；抓取装置上除了用于吸取光伏板的吸盘，还有两个视觉相机和一个伸缩杆，可将视觉相机上下伸缩，防止光伏板遮挡相机的视线。

4、一种基于预测控制的光伏板安装机器人抓取系统的控制方法具体预测控制方法如下：

5、步骤1：状态方程

6、光伏板安装机器人采用六轴机械臂，机械臂的状态方程：

7、；

8、其中，为第个周期的四个特征点像素坐标，为第个周期的四个特征点像素坐标，为输入矩阵，为六轴机械臂的六个关节电机角速度；通过对状态方程的推导建立了机械臂的视觉伺服运动学模型。

9、步骤2：设计视觉伺服预测控制器：

10、在使用模型预测控制的时候，需要根据状态方程与实际要求构建代价函数，在每个周期进行预测时，通过最小化代价函数来求得最优的控制量。本发明将代价函数定义为如下形式

11、；

12、代价函数可分为误差代价、速度代价、终端误差代价三部分，上式中为预测步长，表示第个周期时预测出的未来几步的像素坐标误差，表示第个周期预测出的未来几步的关节角速度，表示第个周期预测出的未来的终端像素坐标误差。、、为权重矩阵，调节权重矩阵中数值的大小便可以调整系统对代价来源方向的注重程度。

13、步骤3：视野约束

14、在控制过程中，为了防止视野丢视，需要为系统设置视野约束条件。在每个周期进行速度预测的时候，要确保根据预测出的速度得到下个周期的像素坐标在图像范围内，从而得出速度的限制范围。本发明采用摄像机的输出图片大小为960×720，故根据式(1)可得

15、；

16、将式(3)重新整理，可得：

17、；

18、其中，，，令,，进一步合并，得视野约束方程为：

19、。

20、步骤4：相机后退约束

21、在控制过程中，为了防止机械臂在运动时出现摄像机后退的问题，需要为系统设置相机后退约束。在初始时刻记录特征点到相机的深度为最大深度（深度就是指摄像机坐标zc值），求出关于深度的状态方程，在每个周期进行速度预测的时候，要确保根据预测出的速度得到下个周期的深度小于最大深度，从而得出速度的限制范围。

22、定义特征点的zc坐标的导数：

23、；

24、其中，表示坐标系相对于坐标系 b的旋转变换矩阵；世界坐标系 w，机械臂末端坐标系 e，相机坐标系 c用符号表示的第 j行，则有，，； pw是指世界坐标系下的特征点坐标；机械臂的关节角度；

25、令，则可得到离散系统下的深度信息的状态方程

26、；

27、对于四个特征点的系统，其相机坐标系下的深度信息为、、、，则有：

28、；

29、令，， t表示时间；

30、则得到四特征点系统的深度信息状态方程为：

31、；

32、构造约束不等式：

33、；

34、令,,，表示zc的最大值；

35、得到相机后退约束方程为：

36、；

37、结合式(5)与 (11)，令 ,得到总约束方程

38、；

39、由于预测步长为，求解时需要将设置同样维度的约束方程，根据式(12)得到最终的约束方程为

40、；

41、至此，已经得出系统的约束方程；

42、根据式(1)得系统 k时刻的状态为：

43、；

44、表示状态量x在时刻对时刻的预测值，x表示像素坐标、表示映射关系；

45、考虑约束条件(13)，系统的控制器为：

46、；

47、；

48、其中，为特征点的像素初始值，为初始像素误差值，为误差下限，为误差上限，为关节速度下限，为关节速度上限，为机械臂关节速度序列，为目标点像素坐标值；

49、每个计算周期内，根据式(15)求得速度序列，并取其中的第一组数据作为机械臂关节速度，并下发给控制器。

50、步骤5：延时

51、设置延时以满足周期时长，当延时结束，表示此周期即将结束，从控制器中读取关节角度进入到下一个周期进行运算，如此往复，直到小于设定的误差值，完成控制任务。

52、一个周期包括 t1时刻-求特征点像素误差、时刻-关节速度下发、时刻-读取关节角度、时刻-获取特征点像素坐标与深度信息、 t5时刻-计算参数矩阵；其中 t1时刻到时刻为时间段， t1时刻到时刻为时间段，时刻到时刻为时间段，时刻到 t5时刻为时间段；

53、时刻获取机械臂关节角度，在时刻采集到特征点的像素坐标和深度信息后，根据当前关节角速度按照时间段计算时刻新的关节角度，然后根据时刻的关节角度及特征点像素坐标与深度信息计算出估计的特征点的世界坐标，再根据时刻的关节角度与当前关节角速度按照时间估计出在预测计算完毕的下周期时刻下的关节角度，根据时刻的估计关节角度与前面计算出的特征点世界坐标计算出在时刻的特征点像素坐标与深度信息，计算出对应的参数矩阵，最后进行代价函数求解，得出最后的最优值。

54、设计思路：

55、本发明算法过程分为为初始化阶段与预测控制阶段共两个阶段，在初始化阶段，先根据机械臂的dh参数进行机械臂建模，通过摄像机获得的特征点信息及深度信息，计算参数矩阵完成程序的初始化；在预测控制阶段，加入时延补偿的mpc算法，求解出最优解，得到预测的关节速度值，再将速度值下发给机械臂控制器，控制机械臂进行运动。本发明解决由于数据采集转换，数据传输、数据计算导致时延，并造成机械臂关节累计误差的问题，从而减小系统各部分带来的时延误差，提升系统的控制精度与稳定性。

56、本发明的有益效果如下：

57、1) 本发明采用基于预测控制的安装机器人的抓取控制方法，可以有效的处理视觉的视野约束和执行器的速度约束，保证机械臂能在摄像机的视野范围内运动，同时保证不超出执行器的速度范围。

58、2) 本发明基于延时补偿的mpc算法，特征点轨迹、摄像头轨迹更平滑，特征点误差变化更平稳，机械臂关节速度变化也更平稳。像素误差曲线与机械臂速度变化曲线波动更小，变化更加平稳；因此时延补偿mpc算法在物体抓取实验中具有更加稳定、精确的控制效果。

59、3）本发明解决由于数据采集转换，数据传输、数据计算导致时延，并造成机械臂关节累计误差的问题，从而减小系统各部分带来的时延误差，提升系统的控制精度与稳定性。