基于大研机械臂的离线编程路径规划

学术屎壳郎开始推粪球了

基于学校的大研机械臂-->因为只有示教盒，所以我写一个关于大研机械臂焊接的路径规划课题。

首先基于APF人工势场的算法，在待熔覆物体的质心处，输入目标点以及起始点，可以得到的路径为->

黑点为待熔覆物体的质心，所以红颜色的就是我们的路径点。现在处理Q的数据集

1    1    1
1.57735026918963    1.57735026918963    1.57735026918963
2.15470053837925    2.15470053837925    2.15470053837925
2.73205080756888    2.73205080756888    2.73205080756888
3.30940107675850    3.30940107675850    3.30940107675850
3.88675134594813    3.88675134594813    3.88675134594813
4.46410161513775    4.46410161513775    4.46410161513775
5.04145188432738    5.04145188432738    5.04145188432738
5.61880215351701    5.61880215351701    5.61880215351701
6.19615242270663    6.19615242270663    6.19615242270663
6.77350269189626    6.77350269189626    6.77350269189626
7.35085296108588    7.35085296108588    7.35085296108588
7.92820323027551    7.92820323027551    7.92820323027551
8.50555349946514    8.50555349946514    8.50555349946514
9.08290376865476    9.08290376865476    9.08290376865476
9.66025403784439    9.66025403784439    9.66025403784439
10.2376043070340    10.2376043070340    10.2376043070340
10.8149545762236    10.8149545762236    10.8149545762236
11.3923048454133    11.3923048454133    11.3923048454133
11.9696551146029    11.9696551146029    11.9696551146029
12.5470053837925    12.5470053837925    12.5470053837925
13.1243556529821    13.1243556529821    13.1243556529821
13.7017059221718    13.7017059221718    13.7017059221718
14.2790561913614    14.2790561913614    14.2790561913614
14.8564064605510    14.8564064605510    14.8564064605510
15.4337567297407    15.4337567297407    15.4337567297407
16.0111069989303    16.0111069989303    16.0111069989303
16.5884572681199    16.5884572681199    16.5884572681199
17.1658075373095    17.1658075373095    17.1658075373095
17.7431578064992    17.7431578064992    17.7431578064992
18.3205080756888    18.3205080756888    18.3205080756888
18.8978583448784    18.8978583448784    18.8978583448784
19.4752086140680    19.4752086140680    19.4752086140680
20.0525588832577    20.0525588832577    20.0525588832577
20.6299091524473    20.6299091524473    20.6299091524473
21.2072594216369    21.2072594216369    21.2072594216369
21.7846096908265    21.7846096908265    21.7846096908265
22.3619599600162    22.3619599600162    22.3619599600162
22.9393102292058    22.9393102292058    22.9393102292058
23.5166604983954    23.5166604983954    23.5166604983954
24.0940107675850    24.0940107675850    24.0940107675850
24.6742982319876    24.6654413559461    24.6742982319876
25.2645750557916    25.2160288081820    25.2645750557916
25.9697019527534    25.2908092759656    25.9697019527534
26.4173551047132    24.5167205787990    26.4173551047132
26.8740179728944    23.7532298783651    26.8740179728944
27.3486041506871    23.0119229785617    27.3486041506871
27.8619536440440    22.3242133048327    27.8619536440440
28.4684068435326    21.8099866683579    28.4684068435326
29.1704190677703    21.9298101804143    29.1704190677703
29.7299012447823    22.5413323690647    29.7299012447823
30.2372314629403    23.2379184007131    30.2372314629403
30.7445616810982    23.9345044323615    30.7445616810982
31.2518918992561    24.6310904640099    31.2518918992561
31.7592221174140    25.3276764956584    31.7592221174140
32.2665523355719    26.0242625273068    32.2665523355719
32.7738825537298    26.7208485589552    32.7738825537298
33.2812127718877    27.4174345906036    33.2812127718877
33.7885429900457    28.1140206222520    33.7885429900457
34.2958732082036    28.8106066539004    34.2958732082036
34.8032034263615    29.5071926855488    34.8032034263615
35.3105336445194    30.2037787171973    35.3105336445194
35.8178638626773    30.9003647488457    35.8178638626773
36.3251940808352    31.5969507804941    36.3251940808352
36.8325242989931    32.2935368121425    36.8325242989931
37.3398545171510    32.9901228437909    37.3398545171510
37.8471847353089    33.6867088754393    37.8471847353089
38.3545149534669    34.3832949070878    38.3545149534669
38.8618451716248    35.0798809387362    38.8618451716248
39.3691753897827    35.7764669703846    39.3691753897827
39.8765056079406    36.4730530020330    39.8765056079406
40.3838358260985    37.1696390336814    40.3838358260985
40.8911660442564    37.8662250653298    40.8911660442564
41.3984962624143    38.5628110969782    41.3984962624143
41.9058264805722    39.2593971286267    41.9058264805722
42.4131566987302    39.9559831602751    42.4131566987302
42.9204869168881    40.6525691919235    42.9204869168881
43.4278171350460    41.3491552235719    43.4278171350460
43.9351473532039    42.0457412552203    43.9351473532039
44.4424775713618    42.7423272868687    44.4424775713618
44.9498077895197    43.4389133185172    44.9498077895197
45.4571380076777    44.1354993501656    45.4571380076777
45.9644682258356    44.8320853818140    45.9644682258356
46.4717984439935    45.5286714134624    46.4717984439935
46.9791286621514    46.2252574451108    46.9791286621514
47.4864588803093    46.9218434767592    47.4864588803093
47.9937890984672    47.6184295084076    47.9937890984672
48.5011193166251    48.3150155400561    48.5011193166251
49    49    49

线性拟合 计算每个向量与第一个向量之间的距离

0
0
0
0
0
0
0
0
0
0
0
25.9757810695542
80.9868733040677
80.0372939882006
78.1072523726367
73.7134717112093
61.2101659744529
23.3824726107073
0
3.88934887392322
3.88934887392322
3.88934887392322
3.88934887392322
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392338
3.88934887392301
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392338
3.88934887392301
3.88934887392322
0
0

在第四十点的时候可以发现比阈值要大所以取第41点做线性拟合

其中第一个用到的scripts是APF.m

其中第二个脚本函数是 linearsegement

其中第三个脚本函数是linearfit

可以直接找到非线性段出来

其中的第二段我可以直接用圆的表面去取 实验先这样。

然后我们假设有点[1,1,1,2,3,4] [1,2,3,4,5,6] 

用函数g_test 与last trajectory 作为结尾。