一种面向机器人轨迹规划的圆弧定位方法与流程

1.本发明涉及机器人轨迹规划喷涂领域，具体是一种面向机器人轨迹规划的圆弧定位方法。


背景技术：

2.在机器人轨迹规划领域，对于圆弧半径没有一个有效的方法进行测量，如中国专利号为104075670b公开了红外线半径测量及其半径测量和圆弧定位方法，该专利写到，将红外线数显半径测量仪置于圆形地面的圆心；通过调整仪器，使红外线圆弧半径和地面圆弧半径相近时获取测量信息，根据测量信息计算得到圆弧半径。该发明适用于工程地面圆弧半径的测量。但是在机器人轨迹规划领域，该种方法使用的仪器成本较高，操作困难，费时费力，不满足实时性。


技术实现要素：

3.为了解决上述问题，本发明提出一种面向机器人轨迹规划的圆弧定位方法。
4.一种面向机器人轨迹规划的圆弧定位方法，其具体步骤如下：
5.s1、根据已知的圆心o新建坐标系c：以圆心o为零点，像素坐标系的u+方向为x+方向,像素坐标系的v+方向为y+方向；
6.s2、将图片划分为4个矩形：在坐标系c中，第一象限为矩形r1,第二象限为矩形r2,第三象限为矩形r3，第四象限为矩形r4；
7.s3、遍历已知的圆弧点集p：
8.3.1判断圆弧上的点属于哪一个矩形；
9.3.2如果属于r1，则将该点放入集合vp1中；如果属于r2，则将该点放入集合vp2中；如果属于r3，则将该点放入集合vp3中；如果属于r4，则将该点放入集合vp4中；
10.3.3判断是否遍历结束：
11.3.3.1否；遍历下一个点，回到步骤s3.1；
12.3.3.2是；进入下一步；
13.s4、判断所有点所在的象限；
14.s5、计算圆弧起点，中点，终点：
15.5.1所有点在第1象限：
16.5.1.1将vp1中的点按顺时针方向排序；
17.5.1.2根据排序后的vp1及l1求出起点，中点，终点；
18.5.2所有点在第1，2象限：
19.5.2.1将vp1，vp2中的点分别按照顺时针方向排序；
20.5.2.2根据排序后的vp1,vp2以及l1,l2求出起点，中点，终点；
21.5.3所有点在第1，2,3象限：
22.5.3.1将vp1,vp2,vp3中的点分别按照顺时针方向排序；
23.5.3.2根据排序后的vp1,vp2,vp3以及l1,l2,l3求出起点，中点，终点；5.4所有点在第2象限：
24.5.4.1将vp2中的点按照顺时针方向排序；
25.5.4.2根据排序后的vp2及l1求出起点，中点，终点；
26.5.5所有点在第2,3象限：
27.5.5.1将vp2，vp3中的点分别按照顺时针方向排序；
28.5.5.2根据排序后的vp2,vp3以及l2,l3求出起点，中点，终点；
29.5.6所有点在第2,3,4象限：
30.5.6.1将vp2，vp3,vp4中的点分别按照顺时针方向排序；
31.5.6.2根据排序后的vp2,vp3,vp4以及l2,l3,l4求出起点，中点，终点；5.7所有点在第3象限：
32.5.7.1将vp3中的点按照顺时针方向排序；
33.5.7.2根据排序后的vp3及l3求出起点，中点，终点；
34.5.8所有点在第3,4象限：
35.5.8.1将vp3，vp4中的点分别按照顺时针方向排序；
36.5.8.2根据排序后的vp3,vp4以及l3,l4求出起点，中点，终点；
37.5.9所有点在第1,3,4象限：
38.5.9.1将vp1，vp3,vp4中的点分别按照顺时针方向排序；
39.5.9.2根据排序后的vp1,vp3,vp4以及l1,l3,l4求出起点，中点，终点；5.10所有点在第4象限：
40.5.10.1将vp4中的点按照顺时针方向排序；
41.5.10.2根据排序后的vp4以及l4求出起点，中点，终点；
42.5.11所有点在第1,4象限：
43.5.11.1将vp1,vp4中的点分别按照顺时针方向排序；
44.5.11.2根据排序后的vp1,vp4以及l1,l4求出起点，中点，终点；
45.5.12所有点在第1,2,4象限：
46.5.12.1将vp1,vp2,vp4中的点分别按照顺时针方向排序；
47.5.12.2根据排序后的vp1,vp2,vp4以及l1,l2,l4求出起点，中点，终点；
48.5.13圆弧开口在第1象限；
49.5.14圆弧开口在第1，2象限：
50.5.14.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
51.5.14.2起点为vp1[0],终点为vp2[l2-1]；
[0052]
5.14.3如果(l1+l4)》(l2+l3),中点在第4象限；否则在第3象限；具体根据l1,l2,l3,l4求得；
[0053]
5.15圆弧开口在第2象限；
[0054]
5.16圆弧开口在第2，3象限；
[0055]
5.17则圆弧开口在第3象限：
[0056]
5.18圆弧开口在第3，4象限：
[0057]
5.18.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0058]
5.18.2起点为vp3[0],终点为vp4[l4-1]；
[0059]
5.18.3如果(l1+l4)》(l2+l3),中点在第1象限；否则在第2象限；具体根据l1,l2,l3,l4求得；
[0060]
5.19圆弧开口在第4象限；
[0061]
5.20圆弧开口在第1，4象限。
[0062]
所述的步骤s4的具体步骤如下：
[0063]
4.1如果vp1不为空，vp2,vp3,vp4为空，则所有点在第1象限，进入步骤s5.1；
[0064]
4.2如果vp1,vp2不为空，vp3,vp4为空，则所有点在第1，2象限，进入步骤s5.2；
[0065]
4.3如果vp1,vp2,vp3不为空，vp4为空，则所有点在第1，2，3象限，进入步骤s5.3；
[0066]
4.4如果vp2不为空，vp1,vp3,vp4为空，则所有点在第2象限，进入步骤s5.4；
[0067]
4.5如果vp2,vp3不为空，vp1,vp4为空，则所有点在第2，3象限，进入步骤s5.5；
[0068]
4.6如果vp2,vp3,vp4不为空，vp1为空，则所有点在第2，3，4象限，进入步骤s5.6；
[0069]
4.7如果vp3不为空，vp1,vp2,vp4为空，则所有点在第3象限，进入步骤s5.7；
[0070]
4.8如果vp3,vp4不为空，vp1,vp2为空，则所有点在第3，4象限，进入步骤s5.8；
[0071]
4.9如果vp1,vp3,vp4不为空，vp2为空，则所有点在第1,3,4象限，进入步骤s5.9；
[0072]
4.10如果vp4不为空，vp1,vp2,vp3为空，则所有点在第4象限，进入步骤s5.10；
[0073]
4.11如果vp1,vp4不为空，vp2,vp3为空，则所有点在第1，4象限，进入步骤s5.11；
[0074]
4.12如果vp1,vp2,vp4不为空，vp3为空，则所有点在第1，2，4象限，进入步骤s5.12；
[0075]
4.13如果vp1,vp2,vp3,vp4不为空，则所有点在第1，2，3，4象限。
[0076]
所述的步骤s4.13的具体步骤如下：
[0077]
4.13.1判断圆弧开口的位置；4.13.1.1计算vp1中每一点与x+的夹角并存入集合angle1中；
[0078]
4.13.1.2计算vp2中每一点与x+的夹角并存入集合angle2中；
[0079]
4.13.1.3计算vp3中每一点与x+的夹角并存入集合angle3中；
[0080]
4.13.1.4计算vp4中每一点与x+的夹角并存入集合angle4中；
[0081]
4.13.1.5分别对angle1,angle2,angle3,angle4进行升序排序；
[0082]
4.13.1.6分别计算angle1,angle2,angle3,angle4中相邻角度的差值并分别存入集合angle1_diff,angle2_diff,angle3_diff,angle4_diff；
[0083]
4.13.1.7分别计算angle1_diff,angle2_diff,angle3_diff,angle4_diff的最大值max1_diff,max3_diff,max5_diff,max7_diff，并存入集合max_diff中；
[0084]
4.13.1.8计算angle1_diff中的最大值与angle2_diff中最小值的差值max2_diff,并存入max_diff；
[0085]
4.13.1.9计算angle2_diff中的最大值与angle3_diff中最大值的差值max4_diff,并存入max_diff；
[0086]
4.13.1.10计算angle3_diff中的最小值与angle4_diff中最大值的差值max6_diff,并存入max_diff；
[0087]
4.13.1.11计算angle4_diff中的最小值与angle1_diff中最小值的差值max8_diff,并存入max_diff；
[0088]
4.13.1.12计算max_diff的最大值max_value；
[0089]
4.13.1.13如果max_value＝＝max1_diff，则圆弧开口在第1象限，进入5.13；
[0090]
4.13.1.14如果max_value＝＝max2_diff,则圆弧开口在第1，2象限，进入5.14；
[0091]
4.13.1.15如果max_value＝＝max3_diff,则圆弧开口在第2象限，进入5.15；
[0092]
4.13.1.16如果max_value＝＝max4_diff，则圆弧开口在第2，3象限，进入5.16；
[0093]
4.13.1.17如果max_value＝＝max5_diff，则圆弧开口在第3象限，进入5.17；
[0094]
4.13.1.18如果max_value＝＝max6_diff，则圆弧开口在第3，4象限，进入5.18；
[0095]
4.13.1.19如果max_value＝＝max7_diff，则圆弧开口在第4象限，进入5.19；
[0096]
4.13.1.20如果max_value＝＝max8_diff，则圆弧开口在第1，4象限，进入5.20；
[0097]
1、所述的s5.1.6的具体步骤如下：
[0098]
5.16.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0099]
5.16.2起点为vp2[0],终点为vp3[l3-1]；
[0100]
5.16.3如果(l1+l2)》(l3+l4),中点在第1象限；否则在第4象限；具体根据l1,l2,l3,l4求得。
[0101]
所述的步骤s5.13的具体步骤如下：
[0102]
5.13.1求max1_diff在angle1_diff中的索引；
[0103]
5.13.2根据5.13.1求出圆弧开口的两点在4.13.1.5中的angle1中对应的角度；
[0104]
5.13.3根据5.13.2求出圆弧开口的两点在4.13.1.1中的angle1中对应的索引；
[0105]
5.13.4根据5.13.3求出圆弧的起点和终点；
[0106]
5.13.5中点在第3象限，根据点在各象限的数量，求出中点。
[0107]
所述的步骤s5.15的具体步骤如下：
[0108]
5.15.1求max3_diff在angle2_diff中的索引；
[0109]
5.15.2根据5.15.1求出圆弧开口的两点在4.13.1.5中的angle2中对应的角度；
[0110]
5.15.3根据5.15.2求出圆弧开口的两点在4.13.1.2中的angle2中对应的索引；
[0111]
5.15.4根据5.15.3求出圆弧的起点和终点；
[0112]
5.15.5中点在第4象限，根据点在各象限的数量，求出中点。
[0113]
所述的步骤s5.17的具体步骤如下：
[0114]
5.17.1求max5_diff在angle3_diff中的索引；
[0115]
5.17.2根据5.17.1求出圆弧开口的两点在4.13.1.5中的angle3中对应的角度；
[0116]
5.17.3根据5.17.2求出圆弧开口的两点在4.13.1.3中的angle3中对应的索引；
[0117]
5.17.4根据5.17.3求出圆弧的起点和终点；
[0118]
5.17.5中点在第1象限，根据点在各象限的数量，求出中点。
[0119]
所述的s5.19的具体步骤如下：
[0120]
5.19.1求max7_diff在angle4_diff中的索引；
[0121]
5.19.2根据5.19.1求出圆弧开口的两点在4.13.1.5中的angle4中对应的角度；
[0122]
5.19.3根据5.19.2求出圆弧开口的两点在4.13.1.4中的angle4中对应的索引；
[0123]
5.19.4根据5.19.3求出圆弧的起点和终点；
[0124]
5.19.5中点在第2象限，根据点在各象限的数量，求出中点。
[0125]
所述的s5.20的具体步骤如下：
[0126]
5.20.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0127]
5.20.2起点为vp4[0],终点为vp1[l1-1]；
[0128]
5.20.3如果(l1+l2)》(l3+l4),中点在第2象限；否则在第3象限；具体根据l1,l2,l3,l4求得；
[0129]
其中，l1,l2,l3,l4分别为vp1,vp2,vp3,vp4的长度。
[0130]
本发明的有益效果是：本发明克服现有技术存在的缺陷，提出了一种面向机器人轨迹规划的圆弧定位方法，该方法在已知圆弧上的点和对应的圆心时，给出圆弧的起点，中点，终点，机器人接收到圆弧的起点，中点，终点位置后，就可以根据这些信息对圆弧路径进行轨迹规划，该方法的优点在于不用借助任何外部仪器，操作简单，成本低，不费时，可重复使用。
附图说明
[0131]
下面结合附图和实施例对本发明进一步说明。
[0132]
图1为本发明的流程结构示意图。
具体实施方式
[0133]
为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对本发明进一步阐述。
[0134]
如图1所示，一种面向机器人轨迹规划的圆弧定位方法，其具体步骤如下：
[0135]
s1、根据已知的圆心o新建坐标系c：以圆心o为零点，像素坐标系的u+方向为x+方向,像素坐标系的v+方向为y+方向；
[0136]
s2、将图片划分为4个矩形：在坐标系c中，第一象限为矩形r1,第二象限为矩形r2,第三象限为矩形r3，第四象限为矩形r4；
[0137]
s3、遍历已知的圆弧点集p：
[0138]
3.1判断圆弧上的点属于哪一个矩形；
[0139]
3.2如果属于r1，则将该点放入集合vp1中；如果属于r2，则将该点放入集合vp2中；如果属于r3，则将该点放入集合vp3中；如果属于r4，则将该点放入集合vp4中；
[0140]
3.3判断是否遍历结束：
[0141]
3.3.1否；遍历下一个点，回到步骤s3.1；
[0142]
3.3.2是；进入下一步；
[0143]
s4、判断所有点所在的象限；
[0144]
s5、计算圆弧起点，中点，终点：
[0145]
5.1所有点在第1象限：
[0146]
5.1.1将vp1中的点按顺时针方向排序；
[0147]
5.1.2根据排序后的vp1及l1求出起点，中点，终点；
[0148]
5.2所有点在第1，2象限：
[0149]
5.2.1将vp1，vp2中的点分别按照顺时针方向排序；
[0150]
5.2.2根据排序后的vp1,vp2以及l1,l2求出起点，中点，终点；
[0151]
5.3所有点在第1，2,3象限：
[0152]
5.3.1将vp1,vp2,vp3中的点分别按照顺时针方向排序；
[0153]
5.3.2根据排序后的vp1,vp2,vp3以及l1,l2,l3求出起点，中点，终点；5.4所有点在第2象限：
[0154]
5.4.1将vp2中的点按照顺时针方向排序；
[0155]
5.4.2根据排序后的vp2及l1求出起点，中点，终点；
[0156]
5.5所有点在第2,3象限：
[0157]
5.5.1将vp2，vp3中的点分别按照顺时针方向排序；
[0158]
5.5.2根据排序后的vp2,vp3以及l2,l3求出起点，中点，终点；
[0159]
5.6所有点在第2,3,4象限：
[0160]
5.6.1将vp2，vp3,vp4中的点分别按照顺时针方向排序；
[0161]
5.6.2根据排序后的vp2,vp3,vp4以及l2,l3,l4求出起点，中点，终点；5.7所有点在第3象限：
[0162]
5.7.1将vp3中的点按照顺时针方向排序；
[0163]
5.7.2根据排序后的vp3及l3求出起点，中点，终点；
[0164]
5.8所有点在第3,4象限：
[0165]
5.8.1将vp3，vp4中的点分别按照顺时针方向排序；
[0166]
5.8.2根据排序后的vp3,vp4以及l3,l4求出起点，中点，终点；
[0167]
5.9所有点在第1,3,4象限：
[0168]
5.9.1将vp1，vp3,vp4中的点分别按照顺时针方向排序；
[0169]
5.9.2根据排序后的vp1,vp3,vp4以及l1,l3,l4求出起点，中点，终点；5.10所有点在第4象限：
[0170]
5.10.1将vp4中的点按照顺时针方向排序；
[0171]
5.10.2根据排序后的vp4以及l4求出起点，中点，终点；
[0172]
5.11所有点在第1,4象限：
[0173]
5.11.1将vp1,vp4中的点分别按照顺时针方向排序；
[0174]
5.11.2根据排序后的vp1,vp4以及l1,l4求出起点，中点，终点；
[0175]
5.12所有点在第1,2,4象限：
[0176]
5.12.1将vp1,vp2,vp4中的点分别按照顺时针方向排序；
[0177]
5.12.2根据排序后的vp1,vp2,vp4以及l1,l2,l4求出起点，中点，终点；5.13圆弧开口在第1象限；
[0178]
5.14圆弧开口在第1，2象限：
[0179]
5.14.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0180]
5.14.2起点为vp1[0],终点为vp2[l2-1]；
[0181]
5.14.3如果(l1+l4)》(l2+l3),中点在第4象限；否则在第3象限；具体根据l1,l2,l3,l4求得；
[0182]
5.15圆弧开口在第2象限；
[0183]
5.16圆弧开口在第2，3象限；
[0184]
5.17则圆弧开口在第3象限：
[0185]
5.18圆弧开口在第3，4象限：
[0186]
5.18.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0187]
5.18.2起点为vp3[0],终点为vp4[l4-1]；
[0188]
5.18.3如果(l1+l4)》(l2+l3),中点在第1象限；否则在第2象限；具体根据l1,l2,l3,l4求得；
[0189]
5.19圆弧开口在第4象限；
[0190]
5.20圆弧开口在第1，4象限。
[0191]
所述的步骤s4的具体步骤如下：
[0192]
4.1如果vp1不为空，vp2,vp3,vp4为空，则所有点在第1象限，进入步骤s5.1；
[0193]
4.2如果vp1,vp2不为空，vp3,vp4为空，则所有点在第1，2象限，进入步骤s5.2；
[0194]
4.3如果vp1,vp2,vp3不为空，vp4为空，则所有点在第1，2，3象限，进入步骤s5.3；
[0195]
4.4如果vp2不为空，vp1,vp3,vp4为空，则所有点在第2象限，进入步骤s5.4；
[0196]
4.5如果vp2,vp3不为空，vp1,vp4为空，则所有点在第2，3象限，进入步骤s5.5；
[0197]
4.6如果vp2,vp3,vp4不为空，vp1为空，则所有点在第2，3，4象限，进入步骤s5.6；
[0198]
4.7如果vp3不为空，vp1,vp2,vp4为空，则所有点在第3象限，进入步骤s5.7；
[0199]
4.8如果vp3,vp4不为空，vp1,vp2为空，则所有点在第3，4象限，进入步骤s5.8；
[0200]
4.9如果vp1,vp3,vp4不为空，vp2为空，则所有点在第1,3,4象限，进入步骤s5.9；
[0201]
4.10如果vp4不为空，vp1,vp2,vp3为空，则所有点在第4象限，进入步骤s5.10；
[0202]
4.11如果vp1,vp4不为空，vp2,vp3为空，则所有点在第1，4象限，进入步骤s5.11；
[0203]
4.12如果vp1,vp2,vp4不为空，vp3为空，则所有点在第1，2，4象限，进入步骤s5.12；
[0204]
4.13如果vp1,vp2,vp3,vp4不为空，则所有点在第1，2，3，4象限。
[0205][0206]
其中，如表所示，l1,l2,l3,l4分别表示圆弧上的点在第一，第二，第三，第四象限的数量。
[0207]
所述的步骤s4.13的具体步骤如下：
[0208]
4.13.1判断圆弧开口的位置；4.13.1.1计算vp1中每一点与x+的夹角并存入集合angle1中；
[0209]
4.13.1.2计算vp2中每一点与x+的夹角并存入集合angle2中；
[0210]
4.13.1.3计算vp3中每一点与x+的夹角并存入集合angle3中；
[0211]
4.13.1.4计算vp4中每一点与x+的夹角并存入集合angle4中；
[0212]
4.13.1.5分别对angle1,angle2,angle3,angle4进行升序排序；
[0213]
4.13.1.6分别计算angle1,angle2,angle3,angle4中相邻角度的差值并分别存入集合angle1_diff,angle2_diff,angle3_diff,angle4_diff；
[0214]
4.13.1.7分别计算angle1_diff,angle2_diff,angle3_diff,angle4_diff的最大值max1_diff,max3_diff,max5_diff,max7_diff，并存入集合max_diff中；
[0215]
4.13.1.8计算angle1_diff中的最大值与angle2_diff中最小值的差值max2_diff,并存入max_diff；
[0216]
4.13.1.9计算angle2_diff中的最大值与angle3_diff中最大值的差值max4_diff,并存入max_diff；
[0217]
4.13.1.10计算angle3_diff中的最小值与angle4_diff中最大值的差值max6_diff,并存入max_diff；
[0218]
4.13.1.11计算angle4_diff中的最小值与angle1_diff中最小值的差值max8_diff,并存入max_diff；
[0219]
4.13.1.12计算max_diff的最大值max_value；
[0220]
4.13.1.13如果max_value＝＝max1_diff，则圆弧开口在第1象限，进入5.13；
[0221]
4.13.1.14如果max_value＝＝max2_diff,则圆弧开口在第1，2象限，进入5.14；
[0222]
4.13.1.15如果max_value＝＝max3_diff,则圆弧开口在第2象限，进入5.15；
[0223]
4.13.1.16如果max_value＝＝max4_diff，则圆弧开口在第2，3象限，进入5.16；
[0224]
4.13.1.17如果max_value＝＝max5_diff，则圆弧开口在第3象限，进入5.17；
[0225]
4.13.1.18如果max_value＝＝max6_diff，则圆弧开口在第3，4象限，进入5.18；
[0226]
4.13.1.19如果max_value＝＝max7_diff，则圆弧开口在第4象限，进入5.19；
[0227]
4.13.1.20如果max_value＝＝max8_diff，则圆弧开口在第1，4象限，进入5.20；
[0228]
所述的s5.1.6的具体步骤如下：
[0229]
5.16.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0230]
5.16.2起点为vp2[0],终点为vp3[l3-1]；
[0231]
5.16.3如果(l1+l2)》(l3+l4),中点在第1象限；否则在第4象限；具体根据l1,l2,l3,l4求得。
[0232]
所述的步骤s5.13的具体步骤如下：
[0233]
5.13.1求max1_diff在angle1_diff中的索引；
[0234]
5.13.2根据5.13.1求出圆弧开口的两点在4.13.1.5中的angle1中对应的角度；
[0235]
5.13.3根据5.13.2求出圆弧开口的两点在4.13.1.1中的angle1中对应的索引；
[0236]
5.13.4根据5.13.3求出圆弧的起点和终点；
[0237]
5.13.5中点在第3象限，根据点在各象限的数量，求出中点。
[0238]
所述的步骤s5.15的具体步骤如下：
[0239]
5.15.1求max3_diff在angle2_diff中的索引；
[0240]
5.15.2根据5.15.1求出圆弧开口的两点在4.13.1.5中的angle2中对应的角度；
[0241]
5.15.3根据5.15.2求出圆弧开口的两点在4.13.1.2中的angle2中对应的索引；
[0242]
5.15.4根据5.15.3求出圆弧的起点和终点；
[0243]
5.15.5中点在第4象限，根据点在各象限的数量，求出中点。
[0244]
面向机器人轨迹规划的圆弧定位算法，为机器人对圆弧这种结构的物体的轨迹执行提供了可能，而且任何时候，圆弧的起点，中点，终点都是按照顺时针方向定位的，这对机器人轨迹规划避免碰撞以及机器人末端线束缠绕过多提供了可靠的判断信息，在喷涂领域，对于机器人喷涂圆弧形凹槽，取得了很好的效果，满足客户需求。
[0245]
所述的步骤s5.17的具体步骤如下：
[0246]
5.17.1求max5_diff在angle3_diff中的索引；
[0247]
5.17.2根据5.17.1求出圆弧开口的两点在4.13.1.5中的angle3中对应的角度；
[0248]
5.17.3根据5.17.2求出圆弧开口的两点在4.13.1.3中的angle3中对应的索引；
[0249]
5.17.4根据5.17.3求出圆弧的起点和终点；
[0250]
5.17.5中点在第1象限，根据点在各象限的数量，求出中点。
[0251]
所述的s5.19的具体步骤如下：
[0252]
5.19.1求max7_diff在angle4_diff中的索引；
[0253]
5.19.2根据5.19.1求出圆弧开口的两点在4.13.1.5中的angle4中对应的角度；
[0254]
5.19.3根据5.19.2求出圆弧开口的两点在4.13.1.4中的angle4中对应的索引；
[0255]
5.19.4根据5.19.3求出圆弧的起点和终点；
[0256]
5.19.5中点在第2象限，根据点在各象限的数量，求出中点。
[0257]
所述的s5.20的具体步骤如下：
[0258]
5.20.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；
[0259]
5.20.2起点为vp4[0],终点为vp1[l1-1]；
[0260]
5.20.3如果(l1+l2)》(l3+l4),中点在第2象限；否则在第3象限；具体根据l1,l2,l3,l4求得；
[0261]
其中，l1,l2,l3,l4分别为vp1,vp2,vp3,vp4的长度。
[0262]
本发明克服现有技术存在的缺陷，提出了一种面向机器人轨迹规划的圆弧定位方法，该方法在已知圆弧上的点和对应的圆心时，给出圆弧的起点，中点，终点，机器人接收到圆弧的起点，中点，终点位置后，就可以根据这些信息对圆弧路径进行轨迹规划，该方法的优点在于不用借助任何外部仪器，操作简单，成本低，不费时，可重复使用。
[0263]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

技术特征：
1.一种面向机器人轨迹规划的圆弧定位方法，其特征在于：其具体步骤如下：s1、根据已知的圆心o新建坐标系c：以圆心o为零点，像素坐标系的u+方向为x+方向,像素坐标系的v+方向为y+方向；s2、将图片划分为4个矩形：在坐标系c中，第一象限为矩形r1,第二象限为矩形r2,第三象限为矩形r3，第四象限为矩形r4；s3、遍历已知的圆弧点集p：3.1判断圆弧上的点属于哪一个矩形；3.2如果属于r1，则将该点放入集合vp1中；如果属于r2，则将该点放入集合vp2中；如果属于r3，则将该点放入集合vp3中；如果属于r4，则将该点放入集合vp4中；3.3判断是否遍历结束：3.3.1否；遍历下一个点，回到步骤s3.1；3.3.2是；进入下一步；s4、判断所有点所在的象限；s5、计算圆弧起点，中点，终点。2.根据权利要求1所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述步骤s5的具体方法为：5.1所有点在第1象限：5.1.1将vp1中的点按顺时针方向排序；5.1.2根据排序后的vp1及l1求出起点，中点，终点；5.2所有点在第1，2象限：5.2.1将vp1，vp2中的点分别按照顺时针方向排序；5.2.2根据排序后的vp1,vp2以及l1,l2求出起点，中点，终点；5.3所有点在第1，2,3象限：5.3.1将vp1,vp2,vp3中的点分别按照顺时针方向排序；5.3.2根据排序后的vp1,vp2,vp3以及l1,l2,l3求出起点，中点，终点；5.4所有点在第2象限：5.4.1将vp2中的点按照顺时针方向排序；5.4.2根据排序后的vp2及l1求出起点，中点，终点；5.5所有点在第2,3象限：5.5.1将vp2，vp3中的点分别按照顺时针方向排序；5.5.2根据排序后的vp2,vp3以及l2,l3求出起点，中点，终点；5.6所有点在第2,3,4象限：5.6.1将vp2，vp3,vp4中的点分别按照顺时针方向排序；5.6.2根据排序后的vp2,vp3,vp4以及l2,l3,l4求出起点，中点，终点；5.7所有点在第3象限：5.7.1将vp3中的点按照顺时针方向排序；5.7.2根据排序后的vp3及l3求出起点，中点，终点；5.8所有点在第3,4象限：5.8.1将vp3，vp4中的点分别按照顺时针方向排序；
5.8.2根据排序后的vp3,vp4以及l3,l4求出起点，中点，终点；5.9所有点在第1,3,4象限：5.9.1将vp1，vp3,vp4中的点分别按照顺时针方向排序；5.9.2根据排序后的vp1,vp3,vp4以及l1,l3,l4求出起点，中点，终点；5.10所有点在第4象限：5.10.1将vp4中的点按照顺时针方向排序；5.10.2根据排序后的vp4以及l4求出起点，中点，终点；5.11所有点在第1,4象限：5.11.1将vp1,vp4中的点分别按照顺时针方向排序；5.11.2根据排序后的vp1,vp4以及l1,l4求出起点，中点，终点；5.12所有点在第1,2,4象限：5.12.1将vp1,vp2,vp4中的点分别按照顺时针方向排序；5.12.2根据排序后的vp1,vp2,vp4以及l1,l2,l4求出起点，中点，终点；5.13圆弧开口在第1象限；5.14圆弧开口在第1，2象限：5.14.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；5.14.2起点为vp1[0],终点为vp2[l2-1]；5.14.3如果(l1+l4)>(l2+l3),中点在第4象限；否则在第3象限；具体根据l1,l2,l3,l4求得；5.15圆弧开口在第2象限；5.16圆弧开口在第2，3象限；5.17则圆弧开口在第3象限：5.18圆弧开口在第3，4象限：5.18.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；5.18.2起点为vp3[0],终点为vp4[l4-1]；5.18.3如果(l1+l4)>(l2+l3),中点在第1象限；否则在第2象限；具体根据l1,l2,l3,l4求得；5.19圆弧开口在第4象限；5.20圆弧开口在第1，4象限。3.根据权利要求1所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的步骤s4的具体步骤如下：4.1如果vp1不为空，vp2,vp3,vp4为空，则所有点在第1象限，进入步骤s5.1；4.2如果vp1,vp2不为空，vp3,vp4为空，则所有点在第1，2象限，进入步骤s5.2；4.3如果vp1,vp2,vp3不为空，vp4为空，则所有点在第1，2，3象限，进入步骤s5.3；4.4如果vp2不为空，vp1,vp3,vp4为空，则所有点在第2象限，进入步骤s5.4；4.5如果vp2,vp3不为空，vp1,vp4为空，则所有点在第2，3象限，进入步骤s5.5；4.6如果vp2,vp3,vp4不为空，vp1为空，则所有点在第2，3，4象限，进入步骤s5.6；4.7如果vp3不为空，vp1,vp2,vp4为空，则所有点在第3象限，进入步骤s5.7；4.8如果vp3,vp4不为空，vp1,vp2为空，则所有点在第3，4象限，进入步骤s5.8；
4.9如果vp1,vp3,vp4不为空，vp2为空，则所有点在第1,3,4象限，进入步骤s5.9；4.10如果vp4不为空，vp1,vp2,vp3为空，则所有点在第4象限，进入步骤s5.10；4.11如果vp1,vp4不为空，vp2,vp3为空，则所有点在第1，4象限，进入步骤s5.11；4.12如果vp1,vp2,vp4不为空，vp3为空，则所有点在第1，2，4象限，进入步骤s5.12；4.13如果vp1,vp2,vp3,vp4不为空，则所有点在第1，2，3，4象限。4.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的步骤s4.13的具体步骤如下：4.13.1判断圆弧开口的位置；4.13.1.1计算vp1中每一点与x+的夹角并存入集合angle1中；4.13.1.2计算vp2中每一点与x+的夹角并存入集合angle2中；4.13.1.3计算vp3中每一点与x+的夹角并存入集合angle3中；4.13.1.4计算vp4中每一点与x+的夹角并存入集合angle4中；4.13.1.5分别对angle1,angle2,angle3,angle4进行升序排序；4.13.1.6分别计算angle1,angle2,angle3,angle4中相邻角度的差值并分别存入集合angle1_diff,angle2_diff,angle3_diff,angle4_diff；4.13.1.7分别计算angle1_diff,angle2_diff,angle3_diff,angle4_diff的最大值max1_diff,max3_diff,max5_diff,max7_diff，并存入集合max_diff中；4.13.1.8计算angle1_diff中的最大值与angle2_diff中最小值的差值max2_diff,并存入max_diff；4.13.1.9计算angle2_diff中的最大值与angle3_diff中最大值的差值max4_diff,并存入max_diff；4.13.1.10计算angle3_diff中的最小值与angle4_diff中最大值的差值max6_diff,并存入max_diff；4.13.1.11计算angle4_diff中的最小值与angle1_diff中最小值的差值max8_diff,并存入max_diff；4.13.1.12计算max_diff的最大值max_value；4.13.1.13如果max_value＝＝max1_diff，则圆弧开口在第1象限，进入5.13；4.13.1.14如果max_value＝＝max2_diff,则圆弧开口在第1，2象限，进入5.14；4.13.1.15如果max_value＝＝max3_diff,则圆弧开口在第2象限，进入5.15；4.13.1.16如果max_value＝＝max4_diff，则圆弧开口在第2，3象限，进入5.16；4.13.1.17如果max_value＝＝max5_diff，则圆弧开口在第3象限，进入5.17；4.13.1.18如果max_value＝＝max6_diff，则圆弧开口在第3，4象限，进入5.18；4.13.1.19如果max_value＝＝max7_diff，则圆弧开口在第4象限，进入5.19；4.13.1.20如果max_value＝＝max8_diff，则圆弧开口在第1，4象限，进入5.20。5.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的s5.1.6的具体步骤如下：5.16.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；5.16.2起点为vp2[0],终点为vp3[l3-1]；5.16.3如果(l1+l2)>(l3+l4),中点在第1象限；否则在第4象限；具体根据l1,l2,l3,l4
求得。6.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的步骤s5.13的具体步骤如下：5.13.1求max1_diff在angle1_diff中的索引；5.13.2根据5.13.1求出圆弧开口的两点在4.13.1.5中的angle1中对应的角度；5.13.3根据5.13.2求出圆弧开口的两点在4.13.1.1中的angle1中对应的索引；5.13.4根据5.13.3求出圆弧的起点和终点；5.13.5中点在第3象限，根据点在各象限的数量，求出中点。7.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的步骤s5.15的具体步骤如下：5.15.1求max3_diff在angle2_diff中的索引；5.15.2根据5.15.1求出圆弧开口的两点在4.13.1.5中的angle2中对应的角度；5.15.3根据5.15.2求出圆弧开口的两点在4.13.1.2中的angle2中对应的索引；5.15.4根据5.15.3求出圆弧的起点和终点；5.15.5中点在第4象限，根据点在各象限的数量，求出中点。8.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的步骤s5.17的具体步骤如下：5.17.1求max5_diff在angle3_diff中的索引；5.17.2根据5.17.1求出圆弧开口的两点在4.13.1.5中的angle3中对应的角度；5.17.3根据5.17.2求出圆弧开口的两点在4.13.1.3中的angle3中对应的索引；5.17.4根据5.17.3求出圆弧的起点和终点；5.17.5中点在第1象限，根据点在各象限的数量，求出中点。9.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的s5.19的具体步骤如下：5.19.1求max7_diff在angle4_diff中的索引；5.19.2根据5.19.1求出圆弧开口的两点在4.13.1.5中的angle4中对应的角度；5.19.3根据5.19.2求出圆弧开口的两点在4.13.1.4中的angle4中对应的索引；5.19.4根据5.19.3求出圆弧的起点和终点；5.19.5中点在第2象限，根据点在各象限的数量，求出中点。10.根据权利要求2所述的一种面向机器人轨迹规划的圆弧定位方法，其特征在于：所述的s5.20的具体步骤如下：5.20.1vp1,vp2,vp3,vp4分别在各自象限内按顺时针排序；5.20.2起点为vp4[0],终点为vp1[l1-1]；5.20.3如果(l1+l2)>(l3+l4),中点在第2象限；否则在第3象限；具体根据l1,l2,l3,l4求得；其中，l1,l2,l3,l4分别为vp1,vp2,vp3,vp4的长度。

技术总结
本发明涉及机器人轨迹规划喷涂领域，具体是一种面向机器人轨迹规划的圆弧定位方法，其具体步骤如下：S1、根据已知的圆心O新建坐标系C：S2、将图片划分为4个矩形：S3、遍历已知的圆弧点集P：S4、判断所有点所在的象限；S5、计算圆弧起点，中点，终点；本发明克服现有技术存在的缺陷，提出了一种面向机器人轨迹规划的圆弧定位方法，该方法在已知圆弧上的点和对应的圆心时，给出圆弧的起点，中点，终点，机器人接收到圆弧的起点，中点，终点位置后，就可以根据这些信息对圆弧路径进行轨迹规划，该方法的优点在于不用借助任何外部仪器，操作简单，成本低，不费时，可重复使用。可重复使用。可重复使用。


技术研发人员：马英 贾时成 李浩来 王慧星 李顺治 王玉成
受保护的技术使用者：埃夫特智能装备股份有限公司
技术研发日：2023.03.21
技术公布日：2023/7/18