轨迹生成器 BezierCreator
未完待续
文档在完善中
项目简介
机器人在路径跟踪时需要提供离散的路径信息，这些路径信息以点的形式存在
本项目的作用是通过贝塞尔曲线生成离散的路径点
通过自定义格式以适配不同的代码
项目代码
import matplotlib.pyplot as plt
import numpy as np
import scipy.special
def calc_bezier_path(control_points, n_points=100):
"""
Compute bezier path (trajectory) given control points.
:param control_points: (numpy array)
:param n_points: (int) number of points in the trajectory
:return: (numpy array)
"""
traj = []
for t in np.linspace(0, 1, n_points):
traj.append(bezier(t, control_points))
return np.array(traj)
def bernstein_poly(n, i, t):
"""
Bernstein polynom.
:param n: (int) polynom degree
:param i: (int)
:param t: (float)
:return: (float)
"""
return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)
def bezier(t, control_points):
"""
Return one point on the bezier curve.
:param t: (float) number in [0, 1]
:param control_points: (numpy array)
:return: (numpy array) Coordinates of the point
"""
n = len(control_points) - 1
return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
def main():
"""贝塞尔曲线生成器"""
"""运行之后，将out.txt文件中的内容拷贝到程序中即可"""
PATH_NAME = 'path_0' # 生成路径的名称
PATH_NUM_NAME = 'PATH_POINT_0' # 生成路径点数量宏的名称
PATH_POINT_NUM = 100 # 路径点的数量
MAX_PLAN_V = 0.6 # 最高直线速度
ACC_POINT_NUM = 20 # 路径点的数量
control_points = np.array([ # 控制点
[0., 0.],
[0., 2000.],
[0., 3000.],
[1000., 4000.],
[2000., 4000.],
[4000., 4000.],
[10000., 4000.]
])
path = calc_bezier_path(control_points, n_points=PATH_POINT_NUM)
f = open('out_contain.txt','w')
f.write( '#define ' + 'MAX_PLAN_V' + ' '+ str(MAX_PLAN_V) + '\n')
f.write( '#define ' + PATH_NUM_NAME + ' '+ str(PATH_POINT_NUM) + '\n')
f.write( 'pose_t ' + PATH_NAME + '[' +PATH_NUM_NAME+ ']' + '=' + '\n')
f.write( '{' + '\n')
for i in range(PATH_POINT_NUM):
point_x = format(path.T[0][i],'.3f')
point_y = format(path.T[1][i],'.3f')
theta = 0.0
if i >=0 and i<ACC_POINT_NUM:
v_scale = format(i*(1/ACC_POINT_NUM),'.2f')
elif (PATH_POINT_NUM-i)>=0 and (PATH_POINT_NUM-i)<=ACC_POINT_NUM:
v_scale = format((PATH_POINT_NUM-i-1)*(1/ACC_POINT_NUM),'.2f')
else:
v_scale = format(1.0,'.2f')
v_plan = 'f* MAX_PLAN_V'
f.write(' ' + '{' + str(point_x)+'f' + ' , ' + str(point_y)+'f' + ' , ' + str(theta)+'f' + ' , ' + str(v_scale) + v_plan + '}' + ','+ '\n')
f.write( '};' + '\n')
f.close()
''' 绘制曲线 '''
fig, ax = plt.subplots()
ax.plot(path.T[0], path.T[1], label="Bezier Path")
ax.plot(control_points.T[0], control_points.T[1],
'--o', label="Control Points")
ax.legend()
ax.axis("equal")
ax.grid(True)
plt.show()
if __name__ == '__main__':
main()
解析代码
在下位机中定义如下文件
path.c
typedef struct
{
float x; // [mm]
float y; // [mm]
float theta; // 方向 [du 0-360]
float v_plan; // 速度 [m/s]
float v_scale; // 速度 [m/s]
} pose_t;
typedef struct
{
uint16_t sn; // 该路径的标识符
uint16_t point_num; // 该路径的点的数量
uint16_t acc_num; // 加减速的点的数量
pose_t *pose; // 路径点的数组
} path_t;
/* 第一条路径 */
#define PATH0_MAX_PLAN_V 3.0
#define PATH0_POINT_NUM 200
#define PATH0_ACC_NUM 10
pose_t path0_point[PATH0_POINT_NUM]=
{
... //从输出的文件中拷贝路径点信息
};
/* 第二条路径 */
#define PATH1_MAX_PLAN_V 3.0
#define PATH1_POINT_NUM 50
#define PATH1_ACC_NUM 10
pose_t path1_point[PATH1_POINT_NUM]=
{
... //从输出的文件中拷贝路径点信息
};
path_t g_path[G_PATH_NUM] =
{
{ .sn = 0 , .point_num = PATH0_POINT_NUM , .acc_num = PATH0_ACC_NUM , .pose = path0_point },
{ .sn = 1 , .point_num = PATH1_POINT_NUM , .acc_num = PATH1_ACC_NUM , .pose = path1_point },
};
对g_path[G_PATH_NUM]结构体中的数据进行读取跟踪即可拿到路径点