空间直线、圆弧插补算法仿真

概要

经常在测量机上面会有扫描曲线等轨迹需求，空间的大概就分为直线，圆弧以及不规则的曲线。前面的比较简单，网上也有很多介绍源。不规则的曲线涉及到的一些条件的判断压缩，线性规划的问题，这期先附上直线圆弧插补介绍。

空间直线插补

空间直线相对来说比较的简单，需要求的东西也很少。
例如：
在空间上有2个点，你只要求出他们的距离，在通过s规划，就可以得到每个时刻这个距离上的点，就可以求出这个点在整个距离的占比是多少，然后把这个占比分别分到三个坐标轴上来计算当前位置。
说的应该不是很抽象，看代码就很容易理解出来。直接上代码

仿真代码

#!/usr/bin/python3

import  math

import matplotlib.pyplot as plt
import numpy as np
import turtle
from mpl_toolkits.mplot3d import Axes3D

class line_space():

    def __init__(self):
        pass

    def SpaceLine(self,S,D,V_max,a_max,J):
        self.x1 = S[0]
        self.y1 = S[1]
        self.z1 = S[2]
        self.x2 = D[0]
        self.y2 = D[1]
        self.z2 = D[2]
        length = math.sqrt(pow((self.x2 - self.x1),2) + pow((self.y2 - self.y1),2) + pow((self.z2 - self.z1),2))
        print ("length",length)
        q0 = 0
        q1 = length
        v0 = 0
        v1 = 0
        v_max = V_max
        j_max = J
        self.d_x = self.x2 - self.x1
        self.d_y = self.y2 - self.y1
        self.d_z = self.z2 - self.z1
        x = {}
        y = {}
        z = {}
        self._compute_maximum_speed_reached_or_not_reached(q0,q1, v0, v1, v_max, a_max, J)
        B = self.Normalization(q0,q1,v0,v1,V_max,a_max,j_max)


        return B[0],B[1],B[2]

    def _compute_maximum_speed_reached_or_not_reached(self,q0,q1, v0, v1, v_max, a_max, j_max):

        if (v_max - v0) * j_max < a_max ** 2:

            # # a_max is not reached
            Tj1 = np.sqrt(abs((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(abs((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)

        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
        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 self.T

    def Normalization(self,q0,q1,v0,v1,V_max,a_max,j_max):
        xx = []
        yy = []
        zz = []
        x = {}
        y = {}
        z = {}
        _lambda = {}
        qd = {}
        for i in np.arange(0,self.T,0.0001):
            t = i
            if 0 <= t < self.Tj1:  # 0<= t <0.06
                _lambda[i] = q0 + v0 * t + j_max * (t ** 3) / 6
                qd[i] = v0 + j_max * (t ** 2) / 2

            elif self.Tj1 <= t < (self.Ta - self.Tj1):  # 0.06<= t <0.066
                _lambda[i] = q0 + v0 * t + self.alima / 6 * (3 * (t ** 2) - 3 * self.Tj1 * t + self.Tj1 ** 2)
                qd[i] = v0 + self.alima * (t - self.Tj1 / 2)

            elif (self.Ta - self.Tj1) <= t < self.Ta:  # 0.066<= t <0.126
                _lambda[i] = q0 + (self.vlim + v0) * self.Ta / 2 - self.vlim * (self.Ta - t) + j_max * (self.Ta - t) ** 3 / 6
                qd[i] = self.vlim - j_max * ((self.Ta - t) ** 2) / 2

            # Constant velocity phase
            elif self.Ta <= t < (self.Ta + self.Tv):
                _lambda[i] = q0 + (self.vlim + v0) * self.Ta / 2 + self.vlim * (t - self.Ta)
                qd[i] = self.vlim
            # Deceleration phase

            elif (self.T - self.Td) <= t < (self.T - self.Td + self.Tj2):
                _lambda[i] = q1 - (self.vlim + v1) * self.Td / 2 + self.vlim * (t - self.T + self.Td) - j_max * (
                        (t - self.T + self.Td) ** 3) / 6
                qd[i] = self.vlim - j_max * ((t - self.T + self.Td) ** 2) / 2

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

                _lambda[i] = 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)
                qd[i] = self.vlim + self.alimd * ((t - self.T + self.Td) - self.Tj2 / 2)

            elif (self.T - self.Tj2) <= t <= self.T:
                _lambda[i] = q1 - v1 * (self.T - t) - j_max * (((self.T - t) ** 3) / 6)
                qd[i] = v1 + j_max * ((self.T - t) ** 2) / 2

            x[i] = self.x1 + self.d_x * _lambda[i] / q1
            y[i] = self.y1 + self.d_y * _lambda[i] / q1
            z[i] = self.z1 + self.d_z * _lambda[i] / q1
            xx.append(x[i])
            yy.append(y[i])
            zz.append(z[i])

        return xx,yy,zz

if __name__ == "__main__":
    D = [-1, 45, 4]
    S = [-3, 4, -9]
    V_max = 600
    a_max = 9000
    J = 90000
    A = line_space()
    B = A.SpaceLine(S,D,V_max,a_max,J)

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(B[0],B[1],B[2], label='parametric curve')
    ax.legend()
    plt.show()

博主比较懒，代码分段解释太累
大家将就看看，去仿真单步走一下应该就能理解啦！！！

接下篇空间圆弧插补,明天写！！