文章目录

前言

一、国内外移动操作机器人现状

二、方案概述

三、主要部件BOM清单

1.差动轮式AGV：

2.UR5系列机械臂

3.Cognex智能相机

4.加工台

5.控制系统

6.电源和电缆

四、技术点及工作流程

五、计算自动化方案与人工方案成本收回时间

1.自动化方案成本分析：

2.人工方案成本分析：

3.两种方案的比较及成本收回时间的计算：

六、主要技术点分析与实现方案及仿真实现（附带源代码在文件包中）

1.视觉SLAM建图

2.AGV路径规划与自主避障的自动导航技术

3.UR5机械臂路径规划

---

前言

目标：某企业为3C部件精密加工企业，其加工的零件为手机玻璃，要求加工精度为±0.01mm，目前为人工运输至加工中心加工，由人工采用千分表在大理石平台上逐个测量实现。企业为减少人工成本，提高生产效率，要求采用自动化生产线方式实现。试调研国内外移动操作机器人现状，并作出自动化解决方案，列出主要部件BOM清单，并列出AGV+机器人+视觉形成的解决方案，列出技术点，并尝试计算采用自动化方案与采用人工方案相比，何时收回自动化生产线改造成本。

备注：AGV采用差动轮式60Kg负载AGV，机械臂采用UR5系列，机器视觉采用cognex智能相机，AGV预计5万，UR5预计10万，cognex智能相机预计1万。

---

一、国内外移动操作机器人现状

在3C部件加工领域，尤其是手机玻璃等高精度零件的处理，对移动操作机器人的需求日益增长。这类机器人需要具备高精度、高稳定性和高效率的特点，以满足±0.01mm的加工精度要求。目前，国内外已有众多企业投入到移动操作机器人的研发和生产中，技术水平不断提升，市场应用前景广阔。

在国外，一些知名的机器人企业如ABB、KUKA、FANUC等已经推出了针对3C部件加工的移动操作机器人，这些机器人采用先进的运动控制技术、传感器技术和机器视觉技术，能够实现高精度、高稳定性的加工。同时，这些机器人还具备自动化、智能化的特点，能够与加工中心、磨床等设备无缝对接，实现整条生产线的自动化和智能化。

在国内，随着制造业转型升级的加速，越来越多的企业开始重视自动化生产线的建设和改造。移动操作机器人在国内的应用也逐渐普及，国内一些知名的机器人企业如新松、埃夫特、汇川等也推出了自己的移动操作机器人产品，这些机器人产品在精度、稳定性、智能化等方面也在逐步提升。

总的来说，移动操作机器人在国内外已经得到了广泛的应用和推广，技术水平不断提升，市场前景广阔。未来，随着技术的不断创新和进步，移动操作机器人将在3C部件加工领域发挥更加重要的作用。

二、方案概述

本方案采用差动轮式AGV、UR5系列机械臂和Cognex智能相机，构建一套完整的自动化生产线。该生产线能够实现手机玻璃的高精度、高效率加工，满足企业的需求。

具体来说，首先使用双目三维重建技术，实时计算相机位姿并建立稀疏的三维点云地图，SLAM模式用于构建环境的地图，并确定小车相对于地图的位置。定位模式帮助小车实时定位自身的位置，并且在地图上进行准确定位。使用视觉传感器（如摄像头）来捕获物体的图像，并利用计算机视觉算法来识别和定位物体。将这些信息与SLAM系统构建的地图进行关联，从而确定物体在地图中的位置。使用三维SLAM系统输出的三维点云数据，可以通过投影转换为二维地图数据，实现在二维地图上的视觉感知和定位任务，例如障碍物检测、路径规划等。然后使用AGV技术，负责运输手机玻璃零件到加工中心，让小车自主规划路径，避开障碍物，并安全地到达目标位置。相机固定在机械臂上，在抓取前需要使用九点标定法来确认相机与机械臂的坐标变换矩阵。通过智能相机和计算机视觉技术获取玻璃相对于相机坐标，坐标变换后UR5机械臂路径规划抓取玻璃并进行加工。

三、主要部件BOM清单

1.差动轮式AGV：

重量：60Kg

负载：60Kg

移动速度：1800mm/s

控制精度：±0.1mm

功能：自动导航、定位、运输，将手机玻璃运输至机械臂加工区域。

2.UR5系列机械臂

负载：5Kg

重复定位精度：±0.05mm

运动范围：1600mm700mm700mm

功能：接收AGV运送来的手机玻璃，进行加工操作。采用Cognex智能相机进行定位和检测，确保加工精度。

控制器：用于控制机械臂的运动轨迹和姿态。

末端执行器：用于抓取手机玻璃，确保稳定和可靠的夹持。

3.Cognex智能相机

型号：Cognex In-Sight 5300

分辨率：530万像素

检测精度：±0.01mm

功能：对手机玻璃进行高精度检测和定位，确保机械臂加工的准确性。同时，可实时监测生产过程，提高生产效率。

镜头和光源：确保相机拍摄的图像清晰、准确，适用于高精度检测。

图像处理软件：用于处理相机拍摄的图像，进行高精度检测和定位。

4.加工台

台面：提供稳定的加工平台，确保手机玻璃在加工过程中不会移动。

定位系统：用于将手机玻璃准确定位在加工台上。

5.控制系统

主控制器：用于集中控制整个自动化生产线，包括AGV、机械臂、智能相机等设备的协调工作。

传感器和限位开关：用于监测设备的运行状态和位置，确保安全和准确的加工。

6.电源和电缆

电源：为整个自动化生产线提供稳定的电力供应。

电缆：用于传输控制信号、电力等，确保设备之间的通信和供电。

四、技术点及工作流程

1.使用双目三维重建技术，实时计算相机位姿并建立稀疏的三维点云地图，SLAM模式用于构建环境的地图，并确定小车相对于地图的位置。

2.AGV接收手机玻璃，利用SLAM输出的二维地图数据，通过差动轮系统自主规划路径，避开障碍物，自动导航至机械臂加工区域。

3.UR5机械臂路径规划从AGV上抓取手机玻璃，放置在加工台上。

4.Cognex智能相机对手机玻璃进行高精度检测和定位，并将数据反馈给机械臂。

5.机械臂根据相机反馈的数据进行精确加工，完成后将手机玻璃放回AGV。

6.AGV将加工完成后的手机玻璃运输至下一工序或存储区域。

7.重复以上步骤，实现自动化加工生产。

五、计算自动化方案与人工方案成本收回时间

1.自动化方案成本分析：

（1）差动轮式AGV

选择具有良好口碑和市场表现的AGV品牌，如Geek+等，根据实际需求配置适当的差动轮和控制器。成本方面，AGV及其配件的成本将根据型号、性能以及具体需求而有所不同。预计在中等配置下，AGV及其配件的成本可能在5万元左右。

（2）UR5系列机械臂
使用UR5机械臂是一个可靠的选择，它提供了高精度和灵活性。购买全新机械臂及其配件的成本可能在10万元左右。

（3）Cognex智能相机
Cognex是工业视觉领域的知名品牌，其相机能够满足±0.01mm的高精度要求。预计成本在1万元左右。根据工作流程判断，一个自动化生产线需要三个相机，两个用于SLAM建图，一个用于机械臂。

（4）加工台
根据实际需求定制加工台，确保其稳定性和精度。成本将根据材料、尺寸和加工要求而有所不同，预计在1-3万元之间，这里估算为2万。

（5）控制系统
控制系统是整个自动化生产线的核心，需要选择可靠的工业控制器和传感器。成本可能在2-5万元之间。

（6）电源和电缆
电源和电缆的成本相对较低，但它们是保证生产线正常运行的关键因素。预计这部分成本在1-2万元之间。

（7）其他成本
此外，还需要考虑安装、调试、维护以及可能的培训成本。这些成本可能会根据实际情况有所波动。初步估计在5-10万元之间。

（8）总成本概算
综上所述，构建这套完整的自动化生产线的大致成本可能在30万元左右。

2.人工方案成本分析：

（1）人力成本

假设每个工人每小时的工资为30元，每人每天工作8小时，每天可以加工100片手机玻璃（基于±0.01mm的精度要求，需要逐个测量）。则每人每天的人工成本为30 * 8 = 240元。而一个工人一天可以加工100片手机玻璃，所以加工一片手机玻璃的人工成本为2.4元。

（2）设备与材料成本

假设人工方案所需的设备（如大理石平台、千分表等）和材料成本为每年5万元，这包括了设备的折旧、维护和替换成本。

（3）其他成本

其他成本可能包括培训、管理、安全等方面的费用。假设这部分费用为每年10万元。

（4）总成本概算

单片成本：每片手机玻璃的人工成本为2.4元。

年固定成本：设备与材料成本为5万元，其他成本为10万元。

年总成本：年总成本 = 年固定成本+年加工量*单片人工成本。即年总成本 = 5万 + 365 * 100 * 2.4元 = 94.9万元。

3.两种方案的比较及成本收回时间的计算：

自动化方案：初步估计总成本在30万元左右。

人工方案：年总成本为94.9万元。

为了计算自动化方案成本的回收时间，我们假设自动化方案的成本为C（30万元），人工方案的年总成本为A（94.9万元），则回收时间T可以用以下公式计算：

T = C / (A - C)

即T = (30万) / (94.9万 - (30万))。根据这个公式，我们可以大致计算出自动化方案成本的回收时间，可能在0.47年左右，即170天。

因此，采用自动化方案与采用人工方案相比，预计在170天左右可以收回自动化生产线改造的成本。

六、主要技术点分析与实现方案及仿真实现（附带源代码在文件包中）

1.视觉SLAM建图

使用双目三维重建技术，实时计算相机位姿并建立稀疏的三维点云地图，SLAM模式用于构建环境的地图，并确定小车相对于地图的位置。定位模式帮助小车实时定位自身的位置，并且在地图上进行准确定位。使用视觉传感器（如摄像头）来捕获物体的图像，并利用计算机视觉算法来识别和定位物体。将这些信息与SLAM系统构建的地图进行关联，从而确定物体在地图中的位置。使用三维SLAM系统输出的三维点云数据，可以通过投影转换为二维地图数据，实现在二维地图上的视觉感知和定位任务，例如障碍物检测、路径规划等。

双目三维SLAM的部分python代码：

"""
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
"""
import argparse
import sys
import os
import numpy
def read_file_list(filename):
    """
    Reads a trajectory from a text file. 
    File format:
    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
    Input:
    filename -- File name
    Output:
    dict -- dictionary of (stamp,data) tuples
    """
    file = open(filename)
    data = file.read()
    lines = data.replace(","," ").replace("\t"," ").split("\n") 
    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
    return dict(list)
def associate(first_list, second_list,offset,max_difference):
    """
    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
    to find the closest match for every input tuple.
    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
    max_difference -- search radius for candidate generation
    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
    
    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b) 
                         for a in first_keys 
                         for b in second_keys 
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
    matches.sort()
    return matches

if __name__ == '__main__':
    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script takes two data files with timestamps and associates them   
    ''')
    parser.add_argument('first_file', help='first text file (format: timestamp data)')
    parser.add_argument('second_file', help='second text file (format: timestamp data)')
    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
    args = parser.parse_args()
    first_list = read_file_list(args.first_file)
    second_list = read_file_list(args.second_file)

    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    

    if args.first_only:
        for a,b in matches:
            print("%f %s"%(a," ".join(first_list[a])))
    else:
        for a,b in matches:
            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))

2.AGV路径规划与自主避障的自动导航技术

AGV全称是Automated Guided Vehicle，即自动引导车，它是一种可以自主运行的无人驾驶车辆，广泛应用于仓库、工厂等场合的物流运输。在AGV的运输路径规划中，Matlab是一个常用的工具。

使用Matlab进行AGV路径规划，通常需要先定义AGV的地图和障碍物信息，然后选择路径规划算法进行规划。常用的路径规划算法包括A*算法、Dijkstra算法、深度优先搜索算法等。

本方案采用A*路径规划算法，输入二维地图点阵，设定AGV起始点与目标点，设定移动障碍物路线与固定障碍物位置，实现自动避障和路径规划的功能。

这里我们模拟了一个加工车间的二维地图，设置了AGV的起始点和目标点，实现了它的自动导航，如图所示，其中蓝色路径为AGV移动路线，红色为移动障碍物路径。同时也得出了姿态角度的变化以及线速度和角速度的变化图。

MATLAB的AGV路径规划部分代码：

clear
close all
figure 
%% 地图建模
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
MAX0 = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 1 1 1 1 0 0 1 1 1 1 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 1 1 1 1 0 0 1 1 1 1 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 1 1 1 1 0 0 1 1 1 1 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 1 1 1 1 0 0 1 1 1 1 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;];
%%% 通道设置为 0 ；障碍点设置为 1 ；起始点设置为 2 ；目标点设置为 -1 。
MAX=rot90(MAX0,3); %%%设置0,1摆放的图像与存入的数组不一样，需要先逆时针旋转90*3=270度给数组，最后输出来的图像就是自己编排的图像
MAX_X=size(MAX,2); %%% 获取列数，即x轴长度
MAX_Y=size(MAX,1); %%% 获取行数，即y轴长度
MAX_VAL=10; %%% 返回由数字组成的字符表达式的数字值，就是函数用于将数值字符串转换为数值

x_val = 1;
y_val = 1;
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',... 
'xGrid','on','yGrid','on')
grid on; %%% 在画图的时候添加网格线
hold on; %%% 当前轴及图像保持而不被刷新，准备接受此后将绘制的图形，多图共存
n=0;%Number of Obstacles %%% 障碍的数量


k=1; %%%% 将所有障碍物放在关闭列表中；障碍点的值为1;并且显示障碍点
CLOSED=[];
for j=1:MAX_X
for i=1:MAX_Y
if (MAX(i,j)==1)
%%plot(i+.5,j+.5,'ks','MarkerFaceColor','b'); 原来是红点圆表示
fill([i,i+1,i+1,i],[j,j,j+1,j+1],'k'); %%%改成 用黑方块来表示障碍物
CLOSED(k,1)=i; %%% 将障碍点保存到CLOSE数组中
CLOSED(k,2)=j; 
k=k+1;
end
end
end
Area_MAX(1,1)=MAX_X;
Area_MAX(1,2)=MAX_Y;
Obs_Closed=CLOSED;
num_Closed=size(CLOSED,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 设置起始点和多个目标点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 设置起始点、目标点
%%% 选择目标位置
pause(0.5); %%% 程序暂停1秒
h=msgbox('请使用鼠标左键选择目标'); %%% 显示提示语 原句是：Please Select the Target using the Left Mouse button
uiwait(h,5); %%% 程序暂停
if ishandle(h) == 1 %%% ishandle(H) 将返回一个元素为 1 的数组；否则，将返回 0。
delete(h);
end
xlabel('请使用鼠标左键选择目标','Color','black'); %%% 显示图x坐标下面的提示语 原句是：Please Select the Target using the Left Mouse button
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%% 重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1); %%% ginput提供了一个十字光标使我们能更精确的选择我们所需要的位置，并返回坐标值。
end
xval=floor(xval); %%% floor（）取不大于传入值的最大整数，向下取整
yval=floor(yval);
xTarget=xval;%X Coordinate of the Target %%% 目标的坐标
yTarget=yval;%Y Coordinate of the Target
plot(xval+.5,yval+.5,'go'); %%% 目标点颜色b 蓝色 g 绿色 k 黑色 w白色 r 红色 y黄色 m紫红色 c蓝绿色
text(xval+1,yval+1,'Target') %%% text(x,y,'string')在二维图形中指定的位置(x,y)上显示字符串string

%%% 选择起始位置
h=msgbox('请使用鼠标左键选择AGV初始位置'); %%%原文 Please Select the Vehicle initial position using the Left Mouse button
uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel('请选择AGV初始位置 ','Color','black'); %%% 原文 Please Select the Vehicle initial position
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%%重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1);
xval=floor(xval);
yval=floor(yval);
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
plot(xval+.5,yval+.5,'b^');
text(xval+1,yval+1,'Start') 
xlabel('起始点位置标记为 △ ，目标点位置标记为 o ','Color','black'); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Start=[xStart yStart];
Goal=[xTarget yTarget];

[Line_path,distance_x,OPEN_num]=Astar_G_du(Obs_Closed,Start,Goal,MAX_X,MAX_Y);

% 局部避障
figure 
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',...
'xGrid','on','yGrid','on'); 
grid on; 
hold on;
num_obc=size(Obs_Closed,1);
for i_obs=1:1:num_obc
x_obs=Obs_Closed(i_obs,1);
y_obs=Obs_Closed(i_obs,2);
fill([x_obs,x_obs+1,x_obs+1,x_obs],[y_obs,y_obs,y_obs+1,y_obs+1],'k');hold on;
end
plot( Line_path(:,1)+.5, Line_path(:,2)+.5,'b:','linewidth',2); 
plot(xStart+.5,yStart+.5,'b^');
plot(Goal(1,1)+.5,Goal(1,2)+.5,'bo'); 
pause(1);
h=msgbox('设置移动障碍物的 起点');
uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel('设置移动障碍物的 起点','Color','black');
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked
[xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
Obst_xS=xval;%X Coordinate of the Target
Obst_yS=yval;%Y Coordinate of the Target

plot(xval+.5,yval+.5,'k^');
pause(1);

h=msgbox('设置移动障碍物的 终点');
uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel('设置移动障碍物的 终点 ','Color','black');
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked
[xval,yval,but]=ginput(1);
xval=floor(xval);
yval=floor(yval);
end
Obst_xT=xval;%Starting Position
Obst_yT=yval;%Starting Position
plot(xval+.5,yval+.5,'ko');
Obst_d_d_St=[Obst_xS Obst_yS];
Obst_d_d_Ta=[Obst_xT Obst_yT];
[Obst_d_path,Obst_d_distance_x,Obst_d_OPEN_num]=Astar_G(Obs_Closed,Obst_d_d_St,Obst_d_d_Ta,MAX_X,MAX_Y);
Obst_d_path_X=[Obst_d_path;Obst_d_d_Ta];
L_obst=0.01;% 设置移动障碍物的速度 0.1s内运动 L_obst m 速度为10*L_obst m/s
Obst_d_d_line=Line_obst(Obst_d_path_X,L_obst);
plot( Obst_d_d_line(:,1)+.5, Obst_d_d_line(:,2)+.5,'r','linewidth',1); 


pause(1);
h=msgbox('设置未知静止障碍物 左键确定后继续设置，右键确定后结束');
xlabel('设置未知静止障碍物 左键确定后继续设置，右键确定后结束','Color','blue');
uiwait(h,10);
if ishandle(h) == 1
delete(h);
end
but=1;
while but == 1
[xval,yval,but] = ginput(1);
xval=floor(xval);
yval=floor(yval);
MAX(xval,yval)=-2;%Put on the closed list as well
%plot(xval+.5,yval+.5,'rp');
fill([xval,xval+1,xval+1,xval],[yval,yval,yval+1,yval+1],[0.8 0.8 0.8]); 
end%End of While loop

dg=0;%Dummy counter
Obs_d_j=[0 0];
for i=1:MAX_X
for j=1:MAX_Y
if(MAX(i,j) == -2)
dg=dg+1;
Obs_d_j(dg,1)=i; 
Obs_d_j(dg,2)=j; 
end
end
end
path_node=Line_path;


%% 机器人运动学模型

%机器人初始方向角度 angle_node
angle_node=-pi/2; 

% 机器人速度参数
% Kinematic = [ 最高速度[m/s], 最高旋转速度[rad/s], 加速度[m/ss], 旋转加速度[rad/ss], 速度分辨率[m/s], 转速分辨率[rad/s] ]
Kinematic=[2,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];

% 评价函数系数设置 [heading,dist,velocity,predictDT]
% [方位角评价函数系数， 障碍物距离评价函数系数， 当前速度大小评价函数系数, 预测是时间 （不变）]
evalParam=[0.05, 0.2, 0.2, 3.0];

Result_x=DWA_ct_dong(Obs_Closed,Obst_d_d_line,Obs_d_j,Area_MAX,Goal,Line_path,path_node,Start,angle_node,Kinematic,evalParam);
%%%%%%%%%%% 画图
figure 
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',...
'xGrid','on','yGrid','on'); 
grid on; 
hold on;

for i_obs=1:1:num_obc
x_obs=Obs_Closed(i_obs,1);
y_obs=Obs_Closed(i_obs,2);
fill([x_obs,x_obs+1,x_obs+1,x_obs],[y_obs,y_obs,y_obs+1,y_obs+1],'k');hold on;
end
plot( Line_path(:,1)+.5, Line_path(:,2)+.5,'b:','linewidth',1.5); 
plot(xStart+.5,yStart+.5,'b^');
plot(Goal(1,1)+.5,Goal(1,2)+.5,'bo');
plot( Obst_d_d_line(:,1)+.5, Obst_d_d_line(:,2)+.5,'r','linewidth',1); 
num_o=size(Obst_d_d_line,1);
x_do=Obst_d_d_line(num_o,1);
y_do=Obst_d_d_line(num_o,2);
fill([x_do+0.15,x_do+0.85,x_do+0.85,x_do+0.15],[y_do+0.15,y_do+0.15,y_do+0.85,y_do+0.85],'y');
num_lin=size(Line_path,1);
for i_lin=2:1:num_lin-1
plot(Line_path(i_lin,1)+.5,Line_path(i_lin,2)+.5,'r*');
end

% dong_num=size(Obs_d_j,1);
% for i_d=1:1:dong_num
% x_do=Obs_d_j(i_d,1);
% y_do=Obs_d_j(i_d,2);
% fill([x_do,x_do+1,x_do+1,x_do],[y_do,y_do,y_do+1,y_do+1],[0.8 0.8 0.8]);
% end
num_x=size(Result_x,1);
Result_plot=[Result_x;Goal(1,1) Goal(1,2) Result_x(num_x,3) 0 0];


plot(Result_x(:,1)+0.5, Result_x(:,2)+0.5,'b','linewidth',2);hold on;
num_p=num_x+1;
ti=1:1:num_p;
figure
plot(ti,Result_plot(:,3),'-b');hold on;
legend('姿态角度')
figure
plot(ti,Result_plot(:,4),'-b');hold on;
plot(ti,Result_plot(:,5),'-r');hold on;
legend('线速度','角速度')
S=0;
for i=1:1:num_x %%%% 求路径所用的实际长度
Dist=sqrt( ( Result_plot(i,1) - Result_plot(i+1,1) )^2 + ( Result_plot(i,2) - Result_plot(i+1,2))^2);
S=S+Dist;
end
disp('路径长度')
disp(S);
S ;
% 
% % 机器人的状态Result_x=[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
% i=1
% figure
% axis([0 2000, -0.4 0.8]) %%% 设置x，y轴上下限
% set(gca,'xtick',0:100:2100,'ytick',-0.4:0.2:0.8,'GridLineStyle','-',... 
% 'xGrid','on','yGrid','on')
% grid off;
% xlabel('控制节点个数');hold on
% ylabel('线速度(m/s) 角速度(rad/s)');hold on
% 
% plot(ti,Result_plot(:,4),'-b','linewidth',1.5);hold on;
% plot(ti,Result_plot(:,5),':r','linewidth',1.5);hold on;

3.UR5机械臂路径规划

下面是九点标记获取坐标变换矩阵（图中为举例结果）：

UR5机械臂路径规划：

九点标记获取坐标变换矩阵HomMat2D的halcon代码：

read_image (ProfileImage, '12.bmp')
**圈定9点区域
* draw_rectangle1(3600, Row11, Column1, Row2, Column2)
* gen_rectangle1(Rectangle, Row11, Column1, Row2, Column2)
gen_rectangle1 (ROI_0, 330.384, 356.221, 798.412, 825.73)
Rectangle:=ROI_0
reduce_domain(ProfileImage, Rectangle, ImageReduced)
threshold(ImageReduced, Region, 0, 50)
connection(Region, ConnectedRegions)
select_shape (ConnectedRegions, SelectedRegions, 'area', 'and', 0, 500)
sort_region(SelectedRegions, SortedRegions, 'character', 'true', 'column')
**求取9点中心坐标
area_center(SortedRegions, Area, Row, Col)
**手动让机械臂依次走过标定板9个点，记录对应点XY坐标
Col1:=[0.46,0.502,0.54,0.462,0.502,0.54,0.462,0.502,0.542]
Row1:=[-0.109,-0.11,-0.11,-0.069,-0.070,-0.071,-0.029,-0.030,-0.031]
**求取变换矩阵
vector_to_hom_mat2d(Row, Col, Row1, Col1, HomMat2D)
**存储
serialize_hom_mat2d (HomMat2D, SerializedItemHandle)
file:='121.txt'
open_file (file, 'output_binary', FileHandle) 
fwrite_serialized_item (FileHandle, SerializedItemHandle) 
close_file (FileHandle)
***********************************************************************
**读取
open_file (file, 'input_binary', FileHandle) 
fread_serialized_item (FileHandle, SerializedItemHandle2) 
deserialize_hom_mat2d (SerializedItemHandle2, HomMat2D_2) 
close_file (FileHandle)
Col3:=Col[0]
Row3:=Row[0]
**应用测试
affine_trans_point_2d(HomMat2D_2, Row3,Col3,  Qy,Qx)

机械手臂轨迹规划方法：

figure(f)
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T=robot2.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
title('输出末端轨迹');
robot2.plot(q);                         %动画演示 
 
%% 求解位置、速度、加速度变化曲线
f = 4
figure(f)
subplot(3,2,[1,3]);                     %subplot 对画面分区 三行两列 占用1到3的位置
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);                         %动画演示
 
figure(f)
subplot(3, 2, 2);
i = 1:6;
plot(q(:,1));
title('位置');
grid on;
 
figure(f)
subplot(3, 2, 4);
i = 1:6;
plot(qd(:,1));
title('速度');
grid on;
 
figure(f)
subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,1));
title('加速度');
grid on;

t = robot2.fkine(q);                    %运动学正解
rpy=tr2rpy(t);                          %t中提取位置（xyz）
figure(f)
subplot(3,2,5);
plot2(rpy);
 
%% ctraj规划轨迹 考虑末端执行器在两个笛卡尔位姿之间移动  
f = 5
T0 = robot2.fkine(init_ang);            %运动学正解
T1 = robot2.fkine(targ_ang);            %运动学正解
 
Tc = ctraj(T0,T1,step);                 %得到每一步的T阵
 
tt = transl(Tc);
figure(f)
plot2(tt,'r');
title('直线轨迹');