高压线路抢修机械臂移动路径规划系统及方法

高压线路抢修机械臂移动路径规划系统及方法
【技术领域】
[0001] 本发明设及移动路径规划领域，尤其设及一种高压线路抢修机械臂移动路径规划 系统及方法。
【背景技术】
[0002] 目前，人工智能、机器视觉等技术迅速发展，机器人是一种智能化程度较高的机 器，它们可W利用摄像机获得视觉信息，通过CPU运算数据使其具有逻辑思维能力。利用双 目视觉系统可W模拟出人类双眼观察=维世界的机制，但机器组成的双目视觉系统很难模 拟出人眼对运动信号的敏感特性。
[0003] 一个在S维世界中的目标点，在双目系统的左右视觉中将映射成两个不同位置的 像素点。利用双目系统中左右视觉所获取的两幅二维图像，可W重建出左、右视觉中同时出 现的物体在实际=维世界中的形态，该是机器人可W利用双目视觉系统的基础。我们经常 利用双目视觉系统来引导机器人的移动或进行某项操作，机器人毕竟不具有人类发达的信 息处理机能，它的任何一个操作都是由与之相关的一系列数据运算结果决定的。
[0004] 现有的数据运算方法是计算双目视觉视野内的每一个点相对与规定运动轨迹的 偏移量，但该种方法将带来大量的无用计算，且整体计算量非常庞大。并且，机械臂的运动 在左、右视觉所获取的图像中将呈现出不同的位置，左右视觉图像中的点是依据对极几何 关系一一对应的，因此机械臂的运动轨迹反应在左、右视觉图像中将是一个复杂二维最优 值捜索问题。
[0005]W高压线路抢修为应用背景设计的机器人，在使用机器人进行设备维护作业时， 机器人的前端往往存在一些障碍物。此时，机械臂移动时的避障问题将是路径规划的一个 重要难题。另外机器人的机械臂有多个活动关节，每个关节的角度改变将改变整个机械臂 的位置及移动方式，因此，机械臂各活动关节的参数控制也是路径规划的一个重要难题。

【发明内容】

[0006] 本发明的目的就是为了解决上述问题，提供了一种高压线路抢修机械臂移动路径 规划系统及方法，该系统及方法可有效控制机械臂沿图像中的标定路径移动，解决了机械 臂各关节复杂的控制问题W及机器人避障问题，通过人工路径规划结合遗传算法解决了路 径规划的相关难题。
[0007] 为了实现上述目的，本发明采用如下技术方案：
[000引一种高压线路抢修机械臂的移动路径规划系统，包括：
[0009] 双目视觉系统；用于获取抢修机械臂前端的图像并将图像传送至S维重建系统；
[0010] S维重建系统；用于利用双目视觉系统获取的图像数据，重建目标图像的S维坐 标；
[0011] 路径规划系统；用于设定机械臂移动的路径，并计算机械臂各关节移动角度的最 优控制参数；
[0012] 运动控制系统；用于根据路径规划系统规划的路径控制机器人的机械臂移动；
[0013] 所述双目视觉系统与S维重建系统连接，路径规划系统与S维重建系统和运动控 制系统分别连接。
[0014] 所述双目视觉系统包括：光轴交叉双目摄像机和图像采集卡；
[0015] 光轴交叉双目摄像机用于获取机械臂前端的图像W模拟人眼的双目视觉功能，图 像采集卡用于将双目摄像机输出的模拟信号转换成数字信号，所述图像采集卡至少为两通 道。
[0016] 一种高压线路抢修机械臂的移动路径规划系统的方法，包括如下步骤：
[0017] 步骤1 ;调整双目摄像机的姿态及位置，分别对双目视觉系统的左右摄像机进行 标定；
[0018] 步骤2 ;双目视觉系统获取双目视觉图像，并对双目图像进行校正；
[0019] 步骤3 ;根据获取的双目视觉图像，在路径规划系统里预先设定机械臂应该移动 的路径，所述机械臂的移动路径为一系列由图像坐标值构成的直线段，称之为路径序列；
[0020] 步骤4 ;在双目视觉系统中跟踪机械臂夹取目标的位置，分别根据夹取目标 在双目视觉系统中的坐标（xi，yi)、（X2，y2)，重建目标位置的夹取目标的实际S维坐标 狂八，ZJ;
[0021] 步骤5 ;利用遗传算法计算机械臂各关节移动角度的最优控制参数，并计算机械 臂移动后的夹取目标的理论S维坐标狂。，Y。，Z。)，使机械臂的移动在不断接近路径终点的 同时沿规划路径移动；
[002引步骤6 ;计算夹取目标位置的实际S维坐标0(，，Y，，Z，）与使用遗传算法计算得到 的夹取目标理论S维坐标狂。，￥。，2。）之间的误差；
[0023] 如果所述误差在设定的允许范围内，则机械臂移动有效；如果所述误差超出设 定的允许范围，则根据上述两坐标之间的距离，控制机械臂向接近设定的移动路径方向移 动；
[0024] 步骤7 ;重复步骤5-6,直至机械臂移动至标定的终点位置。
[0025] 所述步骤4中重建夹取目标位置的实际S维坐标狂，，Y，，ZJ的具体方法为：
[0026] 假设双目摄像机的成像平面与地面保持水平，则重建夹取目标位置的S维坐标 为：
[0027]
[0029]
[0030] 其中，1为两个摄像头距离的一半，SA为摄像机水平视角，w为摄像机水平分辨率， Sc为摄像机垂直视角，H为摄像机垂直分辨率，^为两个摄像机光轴线的夹角，5i、^2表 示重建目标位置和光屯、的连线与光轴线的夹角。
[0031] 所述步骤5的具体方法为：
[0032] 1)确定机械臂各活动关节与其=维坐标改变的函数关系，假设机械臂有n个活动 关节，则机械臂夹取目标的S维坐标狂。，￥。，2。）与各关节的角度（0。02,03,…，0。）的关 系可表示为：
[003引 Xc=x(0。02, 0 3，...，0。）
[0034]Yc= 5^0。0 2, 0 3,…，0。）
[003引 Zc= Z(0。0 2, 0 3，...，0。）
[0036] 设置关节角度改变的步长为A0，则机械臂的S维坐标与关节角度的关系可进一 步表示为：
[0037] Xc=X( 0 i+ki A 0，0 2+k2A 0，03+ks A 0，…，0n+knA 0 )
[003引 Yc= y(日i+ki A 0，0 2+k2A 0，03+ks A 0，…，0n+knA 0 )
[0039] Zc=z( 0 i+kiA 0，0 2+k2A 0，03+ksA 0，…，0n+knA 0 )
[0040] 其中kn取-1,0, 1中的任一个值；分别对k进行二进制编码，即；00代表kn= 0， 01 或 10 代表k"= 1，11 代表k"=-l;
[0041] 。机械臂共有n个活动关节，故设定一个染色体的长度为化；随机生成N个该样 的染色体，该样就形成了种群大小为N，长度为化的随机初始值；
[004引 3)假设调整参数后机械臂夹取的目标的S维坐标为狂，Y，幻，将化Y，幻映射到 选取路径的图像坐标系中的（X'i，y' 1)，转换关系为：
[0043]
[0044] 其中，矩阵M为由张正友标定算法对双目视觉系统的左右摄像机进行标定时计算 得出，S为齐次坐标项；
[0045] 4)分别计算（X'i，y' 1)与预先设定的机械臂移动路径的最短距离di，W及 (X' 1)与终点的距离d2,在保持d2不增的条件下，利用遗传算法捜寻使di减小的kn 最优组合值。
[0046] 本发明的有益效果：
[0047] 1、本发明通过计算各关节移动角度的最优控制参数，减小了各关节的移动角度对 机械臂移动路径的影响，使得机械臂能够准确沿规划路径进行移动。
[0048] 2、本发明设定的路径序列能有效解决机械臂在移动过程中的避障问题，使得机械 臂的移动更加准确可靠。
[0049] 3、本发明基于图像信息进行运算，信息量丰富，计算方法简单，成本低，易于推广。
[0050] 4、本发明采用的遗传算法具有快速随机的捜索能力，过程简单，具有可扩展性，容 易与其他算法结合，方便本发明的进一步改进。
【附图说明】
[0化1] 图1为本发明交叉双目视觉模型一；
[0052]图2为本发明交叉双目视觉模型二；
[0化3]图3为本发明路径序列示意图；
[0化4]图4为本发明方法流程图。
【具体实施方式】
[0化5]下面结合附图与实施例对本发明作进一步说明。
[0056]一种利用遗传算法解决高压线路抢修机械臂移动路径规划问题的系统，由W下几 部分组成：
[0057]双目视觉系统：双目视觉系统包括双目摄像机、图像采集卡。双目视觉系统由两台 高清摄像机组成，模拟人眼的双目视觉功能。图像采集卡应具有2通道W上的实时图像采 集功能。双目视觉系统的主要任务是采集双目数据图像，经由图像采集卡转成可利用计算 机处理的数字信号。
[0化引S维重建系统；S维重建系统主要是利用双目视觉系统获取的双目图像数据，根 据双目视觉系统模型，重建双目图像中目标的S维坐标，即根据目标在双目图像中的图像 坐标，重建目标的世界坐标。
[0059]路径规划系统：利用鼠标，在双目视觉的一个视觉图像中画出机械臂移动的路径， 利用遗传算法计算机械臂的最优控制参数。
[0060]W上所述的双目视觉系统采用交叉双目，即两摄像机的光轴相交于两摄像机光屯、 连线的中垂线上，且两相交光轴形成的平面与世界坐标系的参考平面平行，如图1所示。利 用交叉双目可扩大双目视觉系统的有效视野，即同一目标同时出现在双目图像中的移动范 围更大。
[0061] 人工辅助规定机械臂移动路径可解决机械臂的避障问题；利用遗传算法捜索机械 臂的最优控制参数，使机械臂沿人工规定路径移动，解决了机械臂各关节复杂的控制问题。
[0062]一种利用遗传算法解决高压线路抢修机械臂移动路径规划问题的方法如图4所 示，通过W下步骤实现：
[0063] 步骤一：矫正双目视觉系统，人工调整摄像机的姿态及位置，使双目视觉系统满足 图1所示的条件，利用张正友标定算法对两个摄像机进行标定。
[0064]步骤二；获取双目图像，对图像失真进行矫正。任选一个视觉的矫正图像，利用鼠 标画出一条机械臂在该视觉中应移动的路径，为方便描述，该里选用左视觉图像标定移动 路径。在图像中选取的机械臂的移动路径由一系列直线段构成，即由一系列的图像坐标值 组成，该里称之为路径序列，要求直线段数量尽可能的少，该样就可W人工避开障碍物。所 述路径序列如图3所示。
[0065]步骤S;在双目图像中跟踪机械臂夹取的目标，根据双目图像中目标的图像坐标(X r，y,)、（XI，yi)利用S维重建坐标公式计算目标位置的实际^维坐标狂，，Y，，ZJ。
[0066] 步骤四；根据机械臂的移动控制参数，利用遗传算法计算机械臂的最优控制参数， 并计算机械臂移动后的夹取目标的理论S维坐标狂。，Y。，Z。)，使机械臂的移动在不断接近 路径终点的同时沿规划路径移动。
[0067] 步骤五：计算通过双目图像得到的目标位置的实际世界坐标狂,，Y,，W与使用遗 传算法得到的世界坐标之间的误差；根据事先设定的允许的误差范围，如误差在允许范围 内，则机械臂移动有效；若误差超出允许范围，则利用运动控制系统进行纠正，使机械臂的 移动回到正确的路径上来。
[0068] 重建夹取目标位置的实际S维坐标狂，，Y，，ZJ的具体方法为：
[0069] 假设双目摄像机的成像平面与地面保持水平，具体模型如图2所示，则重建夹取 目标位置的S维坐标为：
[0070]
[0073] 其中，1为两个摄像头距离的一半，SA为摄像机水平视角，W为摄像机水平分辨率， Sc为摄像机垂直视角，H为摄像机垂直分辨率，口;为两个摄像机光轴线的夹角，5 1、S2表 示重建目标位置和光屯、的连线与光轴线的夹角。
[0074] 上文所述的步骤四通过W下方式实现：
[0075] 确定机械臂活动关节与目标位置=维坐标改变的函数关系，假设机械臂有n个活 动关节，则机械臂夹取的目标位置的S维坐标化，YwZ。）与各关节的角度（0。0 2, 0 3,… ，9。）的关系可表不为：
[0076] Xc=x(0。02, 0 3，...，0。）
[0077] Yc= 5^0。0 2, 0 3，...，0。）
[0078] Zc=z(01，02, 0 3,…，0。）
[0079] 设置关节角度改变的步长为A0，则机械臂夹取的目标位置的S维坐标与关节角 度的关系可进一步表示为：
[0080] Xc=X( 0 i+kiA 0，02+k2A 0，03+ksA 0，…，0n+knA 0 )
[00川 Yc=y(0 i+kiA 0，02+k2A 0，Bs+kgA 0，...，0n+knA 0)
[008引 Zc=z( 0 i+kiA 0，02+k2A 0，03+ksA 0，…，0n+knA 0 )
[0083] 其中可使dl减小的k。最优组合值可取为-1,0, 1中的任一个值。
[0084] 对k进行二进制编码，每两位一组，代表k。的值十进制值，如；00代表k。= 0,01、 10代表k"= 1，11代表k"=-1。机械臂共有n个活动关节，故一个染色体的长度为化。随 机生成N个该样的染色体，该样就形成了种群大小为N，长度为化的随机初始值。
[0085] 假设调整参数后机械臂夹取的目标的计算世界坐标为狂，Y，Z)，将化Y，幻映射 到选取路径的图像坐标系中的（X'i，y' 1)，转换关系为：
[0086]
[0087] 此处矩阵M可在计算机标定时使用张正友标定法计算得到。
[008引计算（X'i，y' 1)与人工标注路径的最短距离dl，及（X'i，y' 1)与终点的距离d2,在保持d2不增的条件下利用遗传算法捜寻可使dl减小的k。最优组合值。
[0089] 关于遗传算法的一些说明：
[0090] 遗传算法的适应度函数：
[0091]本发明采用随机权重法构造适应度函数，即F=Widi+W2d2。其中，Wi+W2= 1，W1为 随机产生的0到1之间的随机数。
[0092] 选择操作；在上一步旧种群中W-定的概率选择个体组成新的种群。其中，个体的 适应度值越高，被选择的概率就越大，个体被选中的概率表示为：
[0093]
[0094] 其中，。为第i个种群的适应度函数值，N为种群个数。
[0095] 交叉操作；在种群中随机选择两个个体，利用染色体交叉的方式，将父代的优良基 因传给子代，第k个染色体和第i个染色体在第j位的交叉操作。个体采用二进制编码，交 叉时在第j位对两个染色体进行截取，交换两个染色体截取的尾部基因序列。
[0096] 变异操作；变异操作可保持种群的多样性。随机在种群中选择一个个体，选择个体 中的一个点进行变异操作。对第k个体中的第i位基因进行变异操作。由于个体采用二进 制编码方式，采用轮盘赌的方式，W-定的变异概率选择第i位基因进行0/1转换。
[0097] 采用上述遗传算法计算出使dl减小的k。最优组合值。根据计算得到机械臂的最 优控制参数，计算机械臂夹取的目标位置的理论=维坐标。
[009引上述虽然结合附图对本发明的【具体实施方式】进行了描述，但并非对本发明保护范 围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围W内。
【主权项】
1. 一种高压线路抢修机械臂移动路径规划系统，其特征是，包括： 双目视觉系统：用于获取抢修机械臂前端的图像并将图像传送至三维重建系统； 三维重建系统：用于利用双目视觉系统获取的图像数据，重建目标图像的三维坐标； 路径规划系统：用于设定机械臂移动的路径，并计算机械臂各关节移动角度的最优控 制参数； 运动控制系统：用于根据路径规划系统规划的路径控制机器人机械臂的移动； 所述双目视觉系统与三维重建系统连接，路径规划系统与三维重建系统和运动控制系 统分别连接。2. 如权利要求1所述的一种高压线路抢修机械臂移动路径规划系统，其特征是，所述 双目视觉系统包括：光轴交叉双目摄像机和图像采集卡； 光轴交叉双目摄像机用于获取机械臂前端的图像以模拟人眼的双目视觉功能，图像采 集卡用于将双目摄像机输出的模拟信号转换成数字信号，所述图像采集卡至少为两通道。3. -种如权利要求1所述的高压线路抢修机械臂移动路径规划系统的方法，其特征 是，包括如下步骤： 步骤1 :调整双目摄像机的姿态及位置，分别对双目视觉系统的左右摄像机进行标定； 步骤2 :双目视觉系统获取双目视觉图像，并对双目图像进行校正； 步骤3 :根据获取的双目视觉图像，在路径规划系统里预先设定机械臂应该移动的路 径，所述机械臂的移动路径为一系列由图像坐标值构成的直线段，称之为路径序列； 步骤4 :在双目视觉系统中跟踪机械臂夹取目标的位置，分别根据夹取目标在双目视 觉系统中的坐标（X1, Y1)、（X2, y2)，重建目标位置的夹取目标的实际三维坐标（Xw, Yw, Zw); 步骤5 :利用遗传算法计算机械臂各关节移动角度的最优控制参数，并计算机械臂移 动后的夹取目标的理论三维坐标（X。，Y。，Z。)，使机械臂的移动在不断接近路径终点的同时 沿规划路径移动； 步骤6:计算夹取目标位置的实际三维坐标（XW，YW，Zw)与使用遗传算法计算得到的夹 取目标理论三维坐标（Xe，Ye，Z。）之间的误差； 如果所述误差在设定的允许范围内，则机械臂移动有效；如果所述误差超出设定的允 许范围，则根据上述两坐标之间的距离，控制机械臂向接近设定的移动路径方向移动； 步骤7 :重复步骤5-6,直至机械臂移动至标定的终点位置。4. 如权利要求3所述的一种高压线路抢修机械臂移动路径规划系统的方法，其特征 是，所述步骤4中重建夹取目标位置的实际三维坐标（X W，YW，Zw)的具体方法为： 假设双目摄像机的成像平面与地面保持水平，则重建夹取目标位置的三维坐标为：其中，1为两个摄像头距离的一半，Sa为摄像机水平视角，W为摄像机水平分辨率，δ B 为摄像机垂直视角，H为摄像机垂直分辨率，W为两个摄像机光轴线的夹角，δ2表示重 建目标位置和光心的连线与光轴线的夹角。5.如权利要求3所述的一种高压线路抢修机械臂移动路径规划系统的方法，其特征 是，所述步骤5的具体方法为： 1) 确定机械臂各活动关节与其三维坐标改变的函数关系，假设机械臂有η个活动关 节，则机械臂夹取目标的三维坐标（HZ。）与各关节的角度（θ 1; θ2, θ3，…，θη)的关系 可表示为： Xc= Χ( Θ " Θ 2，Θ 3,…，θ η) Yc = y ( θ 1，θ 2, θ 3，…，θ η) Zc= Ζ( θ " θ 2，θ 3,…，θ η) 设置关节角度改变的步长为△ θ，则机械臂的三维坐标与关节角度的关系可进一步表 示为： Xc= X ( Θ Jk1 Δ θ，Θ 2+k2 Δ θ，Θ 3+k 3 Δ Θ，…，Θ n+kn Δ Θ ) Yc= y ( θ i+h Δ θ，Θ 2+k2 Δ θ，Θ 3+k 3 Δ θ，…，Θ n+kn Δ θ ) Zc= ζ ( θ Jk1 Δ θ，θ 2+k2 Δ θ，θ 3+k 3 Δ θ，...，θ n+kn Δ θ ) 其中匕取_1，0,1中的任一个值；分别对kn?行二进制编码，即：00代表kn= 0,01或 10 代表 kn= I，11 代表 k n= -1 ; 2) 机械臂共有η个活动关节，故设定一个染色体的长度为2η ;随机生成N个这样的染 色体，这样就形成了种群大小为Ν，长度为2η的随机初始值； 3) 假设调整参数后机械臂夹取的目标的三维坐标为（Χ，Υ，Ζ)，将（Χ，Υ，Ζ)映射到选取 路径的图像坐标系中的Ge/，y/ )，转换关系为：其中，矩阵M为由张正友标定算法对双目视觉系统的左右摄像机进行标定时计算得 出，s为齐次坐标项； 4) 分别计算Ge/，y/ )与预先设定的机械臂移动路径的最短距离Cl1，以及（Xl'，y/ )与 终点的距离d2，在保持d2不增的条件下，利用遗传算法搜寻使d i减小的kn最优组合值。
【专利摘要】本发明提供了一种高压线路抢修机械臂移动路径规划系统及方法，包括：双目视觉系统、三维重建系统、路径规划系统、运动控制系统；双目视觉系统与三维重建系统连接，路径规划系统与三维重建系统和运动控制系统分别连接。本发明有益效果：通过计算各关节移动角度的最优控制参数，减小了各关节的移动角度对机械臂移动路径的影响，使得机械臂能够准确沿规划路径进行移动；基于图像信息进行运算，信息量丰富，计算方法简单，成本低，易于推广。
【IPC分类】G05D1/02
【公开号】CN104881026
【申请号】CN201510209693
【发明人】李健, 鲁守银, 慕世友, 任杰, 傅孟潮, 韩磊, 王振利, 谭林, 吕曦晨, 张海龙, 李建祥, 赵金龙, 高郎宏, 陈强
【申请人】国家电网公司, 国网山东省电力公司电力科学研究院, 山东鲁能智能技术有限公司
【公开日】2015年9月2日
【申请日】2015年4月28日