当前位置: article > 正文

空间圆弧插补算法仿真

作者：我家自动化 | 2024-04-17 08:46:32

踩

空间圆弧插补算法仿真

文章目录

介绍
算法思路
步骤流程
代码块
总结

介绍

大部分空间圆弧插补的给定条件为三点，因为三点确定一平面。我这边因为使用场景为扫描机器，下发的参数有些要求，所以使用参数是圆心，起点，角度以及平面法向量ijk，其实都是一样的，只不过前面需要经过几层的转换罢了。

算法思路

刚开始做的时候，博主还是找了很多资料来看，网上也有各种的算法来支持，做完了之后觉得还是挺有收获的。

控制算法一般和电机相关，所以我们一律采用了s曲线规划来调试电机.,然后该算法只考虑了位置规划，没有姿态规划（我们姿态用的是探针，不用该算法控制）。

思路的话主要就是通过坐标系的转换，圆弧平面为一个坐标系，笛卡尔空间为第二个坐标系，主要就是在平面内得出的各个坐标点通过旋转矩阵，求出在笛卡尔坐标系下的坐标点。

步骤流程

在给定的输入条件后，得出圆弧上三点
根据三点求出旋转矩阵，判断圆弧走向正负（通过改变ijk来实现）
根据弧度和S规划来计算每时刻走过的弧度
进行插补离散点计算

代码块

根据圆心起点角度及ijk计算其他两点，都是按顺序计算

    def circle_pos(self,theta,Center_vector,Start_vector,Normal_vector):
        x0 = Center_vector[0]
        y0 = Center_vector[1]
        z0 = Center_vector[2]
        ax = Normal_vector[0]
        ay = Normal_vector[1]
        az = Normal_vector[2]
        x1 = Start_vector[0]
        y1 = Start_vector[1]
        z1 = Start_vector[2]

        K = 1 - math.cos(theta)
        M = ax * x0 + ay * y0 + az * z0

        x = (pow(ax,2) * K + math.cos(theta)) * x1 + (ax * ay * K - az * math.sin(theta)) * y1 + (
                    ax * az * K + ay * math.sin(theta)) * z1 + (x0 - ax * M) * K + (az * y0 - ay * z0) * math.sin(theta)
        y = (ax * ay * K + az * math.sin(theta)) * x1 + (pow(ay, 2) * K + math.cos(theta)) * y1 + (
                    ay * az * K - ax * math.sin(theta)) * z1 + (y0 - ay * M) * K + (ax * z0 - az * x0) * math.sin(theta)
        z = (ax * az * K - ay * math.sin(theta)) * x1 + (ay * az * K + ax * math.sin(theta)) * y1 + (
                    pow(az, 2) * K + math.cos(theta)) * z1 + (z0 - az * M) * K + (ay * x0 - ax * y0) * math.sin(theta)

        return x ,y, z
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

大家熟悉的s规划，里面稍微该了点输入

    def _compute_maximum_speed_reached_or_not_reached(self,q0,q1, v0, v1, v_max, a_max, j_max):
        v_max = v_max/self.r
        a_max = a_max / self.r
        j_max = j_max / self.r
        if (v_max-v0)*j_max < a_max**2:               # a_max is not reached
            Tj1 = np.sqrt((v_max-v0)/j_max)
            Ta = 2*Tj1
            alima = Tj1 * j_max

        else:
            # a_max is reached
            Tj1 = a_max/j_max
            Ta = Tj1 + (v_max-v0)/a_max
            alima = a_max

        # Deceleration period
        if (v_max-v1)*j_max < a_max**2:
            # a_min is not reached
            Tj2 = np.sqrt((v_max-v1)/j_max)
            Td = 2*Tj2
            alimd = Tj2 * (-j_max)

        else:
            # a_min is reached
            Tj2 = a_max/j_max
            Td = Tj2 + (v_max-v1)/a_max
            alimd = (-a_max)
        # print(Ta,Td)
            # 得出Ta和Td，下面要对这两个时间进行判定

        Tv = (q1-q0)/v_max - (Ta/2)*(1+v0/v_max)-(Td/2)*(1+v1/v_max)
        #计算匀速段时间
        # print (Tv)


        if Tv > 0:
            vlim = v_max

        else:
            Tv = 0
            amax_org = a_max
            delta = (a_max**4)/(j_max**2) + 2*(v0**2+v1**2) + a_max*(4*(q1-q0) - 2*a_max/j_max*(v0 + v1))
            Tj = a_max/j_max
            Tj1 = Tj
            Ta = (a_max**2/j_max - 2*v0 + np.sqrt(delta))/2/a_max
            Tj2 = Tj
            Td = ((a_max**2)/j_max - 2*v1 + np.sqrt(delta))/2/a_max

            if Ta < 0 or Td < 0:
                if Ta < 0:
                    Ta = 0
                    Tj1 = 0
                    Td = 2*(q1-q0)/(v1 + v0)
                    Tj2 = (j_max*(q1-q0) - np.sqrt(j_max*(j_max*((q1-q0)**2) + ((v1 + v0)**2)*(v1 - v0))))/j_max/(v1 + v0)
                    alima = 0
                    alimd = -j_max*Tj2
                    vlim = v0

                elif Td < 0:
                    Td = 0
                    Tj2 = 0
                    Ta = 2 * (q1 - q0) / (v1 + v0)
                    Tj1 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) - ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
                    alima = j_max * Tj1
                    alimd = 0
                    vlim = j_max * Tj1

            elif Ta >= 2*Tj and Td >= 2*Tj:
                alima = a_max
                alimd = -a_max
                vlim = v0 + alima * (Ta - Tj)

            else:
                count = 0
                while Ta < 2*Tj or Td < 2*Tj:
                    count += 1
                    a_max = a_max - amax_org*0.01
                    Tj = a_max / j_max
                    Tj1 = Tj
                    Tj2 = Tj
                    delta = (a_max ** 4) / (j_max ** 2) + 2 * (v0 ** 2 + v1 ** 2) + a_max * (4 * (q1 - q0) - 2 * a_max / j_max * (v0 + v1))
                    Ta = ((a_max**2)/j_max - 2*v0 + np.sqrt(delta))/2/a_max
                    Td = ((a_max ** 2) / j_max - 2 * v1 + np.sqrt(delta)) / 2 / a_max
                    if Ta<0 or Td <0:
                        if Ta <0:
                            Ta = 0
                            Tj1 = 0
                            Td = 2*(q1-q0)/(v0+v1)
                            Tj2 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) + ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
                            alima = 0
                            alimd = -j_max*Tj2
                            vlim = v0

                        elif Td < 0:
                            Td = 0
                            Tj2 = 0
                            Ta = 2*(q1-q0)/(v0+v1)
                            Tj1 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) - ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
                            alima = j_max*Tj1
                            alimd = 0
                            vlim = j_max * Tj1

                    elif Ta >= 2*Tj and Td >= 2*Tj:
                        alima = a_max
                        alimd = -a_max
                        vlim = v0+alima*(Ta -Tj)

        self.T = Tv + Ta + Td
        # print(Tv,"\n",Ta,"\n",Td,"\n",self.T)
        self.Tj1 = Tj1
        self.Tj2 = Tj2
        self.Ta = Ta
        self.Tv = Tv
        self.Td = Td
        self.alima = alima
        self.alimd = alimd
        self.vlim = vlim
        return Ta,Tv,Td

    def S_Pos_Traj(self,t,q0,q1,v0,v1,a_max,j_max):
        j_max = j_max / self.r
        if 0 <= t < self.Tj1:
            q = q0 + v0*t + j_max*(t**3)/6

        elif self.Tj1 <= t < (self.Ta - self.Tj1):
            q = q0 + v0*t + self.alima/6*(3*(t**2) - 3*self.Tj1*t + self.Tj1**2)

        elif (self.Ta-self.Tj1) <= t < self.Ta:
            q = q0 + (self.vlim+v0)*self.Ta/2 - self.vlim*(self.Ta - t) + j_max*(self.Ta - t)**3/6

        # Constant velocity phase
        elif self.Ta <= t < (self.Ta + self.Tv):
            q = q0 + (self.vlim+v0)*self.Ta/2 + self.vlim*(t-self.Ta)

        # Deceleration phase
        elif (self.T - self.Td) <= t < (self.T-self.Td+self.Tj2):
            q = q1 - (self.vlim+v1)*self.Td/2 + self.vlim*(t-self.T+self.Td) -j_max*((t-self.T+self.Td)**3)/6

        elif (self.T-self.Td+self.Tj2) <= t < (self.T-self.Tj2):

            q = q1 - (self.vlim+v1)*(self.Td/2) + self.vlim*(t-self.T+self.Td) +self.alimd/6*(3*((t-self.T+self.Td)**2) - 3*self.Tj2*(t-self.T+self.Td) + self.Tj2**2)

        elif (self.T-self.Tj2) <= t <= self.T:
            q = q1 - v1*(self.T-t) - j_max*(((self.T-t)**3)/6)

        return q
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

求出每时刻走的弧长占总弧长的占比

    def Normalization_S(self,pos,v0,v1,vmax,amax,jmax,t):
        q0 = 0
        q1 = pos
        # q1 = 10
        T_para = self._compute_maximum_speed_reached_or_not_reached(q0,q1,v0,v1,vmax,amax,jmax)
        T = T_para[0] + T_para[1] + T_para[2]
        print(T)

        self.N = math.ceil(T/t)
        j = 0
        q = []
        lambda_ = []
        for i in np.arange(0,T,t):
            # q[j] = self.S_Pos_Traj(i,q0,q1,v0,v1,amax,jmax)
            tt = self.S_Pos_Traj(i,q0,q1,v0,v1,amax,jmax)
            q.append(tt)
            lambda_.append(q[j] / pos)
            j += 1
        # print (j)
        # print (self.N)
        return lambda_
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

主程序如下：

    def SpaceCircle(self,theta,Center_vector,Start_vector, QS, QD, v0,v1,vmax,amax,jmax,t):
        the = theta / 2
        Mid_vector = self.circle_pos(the, Center_vector, Start_vector, Normal_vector)
        End_vector = self.circle_pos(theta/1.8, Center_vector, Start_vector, Normal_vector)

        x1 = Start_vector[0]
        x2 = Mid_vector[0]
        x3 = End_vector[0]
        y1 = Start_vector[1]
        y2 = Mid_vector[1]
        y3 = End_vector[1]
        z1 = Start_vector[2]
        z2 = Mid_vector[2]
        z3 = End_vector[2]       #圆弧上3个坐标

        A1 = (y1 - y3) * (z2 - z3) - (y2 - y3) * (z1 - z3)
        B1 = (x2 - x3) * (z1 - z3) - (x1 - x3) * (z2 - z3)
        C1 = (x1 - x3) * (y2 - y3) - (x2 - x3) * (y1 - y3)

        x0 = Center_vector[0]
        y0 = Center_vector[1]
        z0 = Center_vector[2]   ##圆心
        self.r = np.sqrt(pow((x1-x0), 2) + pow((y1-y0), 2) + pow((z1-z0) , 2))

        L = np.sqrt(float(A1*A1 + B1*B1 + C1*C1))
        ax = A1 / L
        ay = B1 / L
        az = C1 / L     ##新坐标z0方向余弦
        nx = (x1-x0) / self.r
        ny = (y1-y0) / self.r
        nz = (z1-z0) / self.r    ##新坐标x0方向余弦
        o = np.cross([ax,ay,az] , [nx,ny,nz])
        ox = o[0]
        oy = o[1]
        oz = o[2]           ##新坐标Y0的方向余弦

        T = np.mat([[nx, ox, ax, x0], [ny, oy, ay, y0], [nz, oz, az, z0], [0, 0, 0, 1]])   #变换矩阵

        T_ni = np.linalg.inv(T)
        new_axis_x = np.append(np.array(Start_vector), 1)
        new_axis_y = np.append(np.array(Mid_vector), 1)
        new_axis_z = np.append(np.array(End_vector), 1)
        S_ = T_ni * new_axis_x.reshape(new_axis_x.shape[0], 1)
        sss = new_axis_x.reshape(new_axis_x.shape[0], 1)

        M_ = T_ni * new_axis_y.reshape(new_axis_y.shape[0], 1)
        D_ = T_ni * new_axis_z.reshape(new_axis_z.shape[0], 1)
        x1_ = S_[0]; y1_ = S_[1]; z1_ = S_[2]
        x2_ = M_[0]; y2_ = M_[1]; z2_ = M_[2]
        x3_ = D_[0]; y3_ = D_[1]; z3_ = D_[2]

        ##圆弧顺逆时针判断
        if (math.atan2(y2_, x2_) < 0):
            angle_SOM = math.atan2(y2_, x2_) + 2 * math.pi
        else:
            angle_SOM = math.atan2(y2_, x2_)

        if (math.atan2(y3_, x3_) < 0):
            angle_SOD = math.atan2(y3_, x3_) + 2 * math.pi
        else:
            angle_SOD = math.atan2(y3_, x3_)
        ##逆时针
        if (angle_SOM < angle_SOD):
            flag = 1
        ##顺时针
        if (angle_SOM >= angle_SOD):
            flag = -1

        if theta>0 :
            delta_ang = theta
        else:
            delta_ang = -theta

        x_ = []
        y_ = []
        x = []
        y = []
        z = []
        Planning_factors = self.Normalization_S(delta_ang,v0,v1,vmax,amax,jmax,t)

        for i in range(0,self.N):
            x_.append(flag * self.r * math.cos(Planning_factors[i] * delta_ang))
            y_.append(flag * self.r * math.sin(Planning_factors[i] * delta_ang))
            P = T * [[x_[i]], [y_[i]], [0], [1]]
            pp = P.tolist()
            x.append(pp[0])
            y.append(pp[1])
            z.append(pp[2])

        xx = [float(x) for item in x for x in item]
        yy = [float(x) for item in y for x in item]
        zz = [float(x) for item in z for x in item]

        with open("data.csv", 'w+',newline="") as f:
            write = csv.writer(f)
            write.writerow(xx)
            write.writerow(yy)
            write.writerow(zz)

        return xx,yy,zz,x0,y0,z0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

if __name__ == "__main__":
    ACTION = Arc_space()
    Start_vector = [-45,-777,488]
    Center_vector = [-241, 536, 488]
    Normal_vector = [0,0, 1]
    theta = 2*math.pi
    QS = 0
    QD = 0
    v0 = 0
    v1 = 0
    vmax = 48500
    amax = 485000
    jmax = 48500000
    t = 0.0001
    # B = ACTION.test(Start_vector,Center_vector,Normal_vector)
    A = ACTION.SpaceCircle(theta,Center_vector,Start_vector, QS, QD, v0,v1,vmax,amax,jmax,t)


    fig = plt.figure(1)
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(A[0],A[1],A[2], label='parametric curve')
    ax.plot(A[3], A[4], A[5],c = "r", marker = "x")
    ax.legend()

    plt.show()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

仿真结果如下图
在这里插入图片描述

总结

该算法中没有任何的约束及不可控点的排除，工程需要加上约束，不然可能会有惊喜哦！
改算法只是还并不完善，大概的思路及方式是ok的，可能后面根据一些要求会进行二次的开发。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家自动化/article/detail/439258