原理讲解

源代码

#!/usr/bin/env python
#-*-coding:utf-8-*-
#本文档用于三次样条曲线规划
#程序员：陈永*
#版权：哈尔滨工业大学
#日期：2019.11.26
import numpy as np
#***三次样条曲线***#
#三弯构造法求取三次样条曲线参数M
def spline_param(t,f,v0,vn):'''本函数用于求取三次样条曲线参数Minput:t,f,时间系列、数值点系列v0,vn，起点、结束点速度output:m,h对应点的加速度,时间间隔系列'''n = len(t)#求取时间间隔及平均值h = np.zeros(n-1)  #时间间隔c = np.zeros(n-1)  #平均速度for i in range(n-1):h[i] = t[i+1] - t[i]c[i] = (f[i+1] - f[i])/h[i]#平均加速度求取d = np.zeros(n)d[0] = 6.0*(c[0] - v0)/h[0]a = np.zeros(n-2)b = np.zeros(n-2)for i in range(n-2):a[i] = h[i]/(h[i] + h[i+1])  #对应λb[i] = 1-a[i]                #对应d[i+1] = 6*(c[i+1] - c[i])/(h[i+1] + h[i])d[n-1] = 6.0*(vn - c[n-2])/h[n-2]#时间矩阵计算P = 2*np.eye(n)P[0,1] = 1P[n-1,n-2] = 1for i in range(n-2):P[i+1,i+2] = b[i]            #对应ab问题P[i+1,i] = a[i]#求取系数Mm = np.dot(np.linalg.inv(P),d)#print hreturn [m,h]#三弯构造法求取三次单变量样条曲线
def spline1(t,f,interval,v0,vn):'''三次样条曲线，n个采样点，第一边界条件的三弯距插值input:时间，采样值序列，插值间隔，初速度，末速度(单变量）output:[s,vel,acc] 返回插值后的位置，速度，加速度'''  n = len(t)#求取插样系数[m,h] = spline_param(t,f,v0,vn)#求取插值函数k = np.floor(t[n-1]/interval).astype(int) + 1s = np.zeros(k)vel = np.zeros(k)acc = np.zeros(k)tt = np.linspace(0,t[n-1],k)for i in range(k-1):for j in range(n-1):if (tt[i] >= t[j]) & (tt[i] < t[j+1]): #判断所属区间#位移s[i] = m[j]*math.pow(t[j+1] - tt[i],3)/(6*h[j]) +m[j+1]*math.pow( tt[i] - t[j],3)/(6*h[j]) + (f[j] - m[j]*math.pow(h[j],2)/6.0)*(t[j+1]-tt[i])/h[j]+ (f[j+1] - m[j+1]*math.pow(h[j],2)/6.0)*(tt[i]-t[j])/h[j]#速度vel[i] = -m[j]*math.pow(t[j+1] - tt[i],2)/(2*h[j]) +m[j+1]*math.pow( tt[i] - t[j],2)/(2*h[j]) + (f[j+1] - f[j])/h[j] - (m[j+1] - m[j])*h[j]/6#加速度acc[i] = m[j]*(t[j+1] - tt[i])/h[j] + m[j+1]*( tt[i] - t[j])/h[j]s[k-1] = f[n-1]vel[k-1] = vnacc[k-1] = m[n-1]return [s,vel,acc]#三弯构造法求取三次样条曲线特定时刻的值
def spline_tt(t,f,current_time,m,h):'''三次样条曲线，n个采样点，第一边界条件的三弯距插值input:时间，采样值序列，当前时刻，采样点加速度，时间间隔系列output:[s,vel,acc] 单关键时刻系列点''' n = len(t)tt = current_time#求取插值函数for j in range(n-1):if (tt >= t[j]) & (tt < t[j+1]): #判断所属区间#位移s = m[j]*math.pow(t[j+1] - tt,3)/(6*h[j]) + m[j+1]*math.pow( tt - t[j],3)/(6*h[j]) + (f[j] - m[j]*math.pow(h[j],2)/6.0)*(t[j+1]-tt)/h[j]+ (f[j+1] - m[j+1]*math.pow(h[j],2)/6.0)*(tt-t[j])/h[j]#速度vel = -m[j]*math.pow(t[j+1] - tt,2)/(2*h[j]) +m[j+1]*math.pow(tt - t[j],2)/(2*h[j]) + (f[j+1] - f[j])/h[j] - (m[j+1] - m[j])*h[j]/6#加速度acc = m[j]*(t[j+1] - tt)/h[j] + m[j+1]*(tt - t[j])/h[j]else:#位移s = f[n-1]#速度vel = 0#加速度acc = m[n-1]return [s,vel,acc]#三弯构造法求取三次样条曲线特定时刻的值,有且仅有两个点
def spline_tt1(t,f,current_time,m,h):'''三次样条曲线，2个采样点，第一边界条件的三弯距插值input:时间，采样值序列，当前时刻，采样点加速度，时间间隔系列output:[s,vel,acc] 单关键时刻系列点'''    n = len(t)if n != 2:print "输入数据有误，本函数仅做两点间的插值!!!!!"tt = current_time#求取插值函数#位移s = m[0]*math.pow(t[1] - tt,3)/(6.0*h) + m[1]*math.pow( tt - t[0],3)/(6.0*h) + (f[0] - m[0]*math.pow(h,2)/6.0)*(t[1]-tt)/h + (f[1] - m[1]*math.pow(h,2)/6.0)*(tt-t[0])/h#速度vel = -m[0]*math.pow(t[1] - tt,2)/(2.0*h) +m[1]*math.pow(tt - t[0],2)/(2.0*h) + (f[1] - f[0])/(1.0*h) - (m[1] - m[0])*h/6.0#加速度??????acc = m[0]*(t[1] - tt)/h + m[1]*(tt - t[0])/hreturn [s,vel,acc]

机械臂规划----三次样条曲线相关推荐

遨博协作机器人ROS开发 - 机械臂规划场景构建
目录一.简介二.环境版本三.学习目标四.知识储备五.任务实施六.任务拓展七.课堂小结八.课后练习一.简介大家好,欢迎关注遨博学院带来的系列技术分享文章(协作机器人ROS开发),今天 ...
通过ROS控制真实机械臂(7)---三次样条插补
在之前的move_group界面中,当点击plan and execute之后,move_group就会帮我们规划出一条通往指定位姿的轨迹,发布在follow_joint_trajectory上,通过 ...
Franka Emika Panda机械臂规划路径时，rviz中手爪显示碰撞
[Franka Emika Panda连接真实机械臂(二)] 本文是对上文的一些补充. 前文中的franka_ros功能包和panda_moveit_config功能包是分别在不同的工作空间编译的,所 ...
多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试
多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试 ) DH建模法可以参考这个博客: 还有<机器人>这本书,一定要理论实践相结合,理解后可以用几何法建模也可以用 ...
使用 MoveIt 控制自己的真实机械臂【3】——优化轨迹发给真实机械臂执行
上一篇文章中,使用 MoveIt 控制自己的真实机械臂[2]--编写 action server 端代码,已经实现了 action server 端代码,并且拿到了 MoveIt 规划的轨迹数据.当然 ...
Moveit!入门——古月居机械臂开发笔记（一）
Moveit!入门--古月居机械臂开发笔记(一) 引言 Moveit!与机械臂控制 1.创作机械臂模型 2.生成配置文件 3.如何使用Moveit!实现机械臂仿真(gazebo) 完善模型在gaze ...
matlab 角度转四元数_基于Matlab的机械臂路径规划
什么是 trajectory(路径)规划中文路径在英语中可能有两种翻译: 1. path 2. trajectory 首先告诉大家,我们所说的"路径"是后者--trajector ...
(9)机械臂路径规划
参考学习资料: ①:路径规划学习笔记之一-概述 - 古月居 ②:路径规划五种算法简述及对比 - 知乎机器人的运动规划(motion planning)包括路径规划(path planning)和轨迹 ...
机械臂轨迹规划篇（一）MATLAB测试三次多项式样条插值
MATLAB三次多项式样条插值前言机械臂运动空间三次多项式插值原理 MATLAB仿真测试前言最近这一段时间有点忙,所以博客这方面也更的少了,不过我有学到新的知识,还是会尽量更的,由于之前一直 ...

机械臂规划----三次样条曲线

机械臂规划----三次样条曲线

原理讲解

源代码

机械臂规划----三次样条曲线相关推荐

最新文章

热门文章