S型加减速程序，C++，PLC，AGV

千次阅读

2020-04-11 14:04:07

做无人驾驶AGV项目，起动和停车冲击非常大，参考牛人的博文https://blog.csdn.net/Septembernine/article/details/53125828，写了一段S型加减速程序，也称抛物线加减速，七段加减速，实际应用效果不错，分享给大家，代码中有比较详细的注释。

图形

MFC++


// DLGDlg.cpp: 实现文件
//

#include "pch.h"
#include "framework.h"
#include "DLG.h"
#include "DLGDlg.h"
#include "afxdialogex.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif


// 对话框数据
#ifdef AFX_DESIGN_TIME
	enum { IDD = IDD_ABOUTBOX };
#endif

	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV 支持

// 实现
protected:
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialogEx(IDD_ABOUTBOX)
{
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()


// CDLGDlg 对话框



CDLGDlg::CDLGDlg(CWnd* pParent /*=nullptr*/)
	: CDialogEx(IDD_DLG_DIALOG, pParent)
	, m_fVelocity(600)
	, m_nTimer(0)
{
	m_mtx = D2D1::Matrix3x2F::Scale(0.2f, -0.2f)	// 把中心移到左下角
		* D2D1::Matrix3x2F::Translation( 50, 380 )	// Y方向反过来
		;

	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CDLGDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
	DDX_Text(pDX, IDC_EDIT1, m_fVelocity);
	DDX_Control(pDX, IDC_EDIT1, m_pEdit1);
}

BEGIN_MESSAGE_MAP(CDLGDlg, CDialogEx)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDOK, &CDLGDlg::OnBnClickedOk)
	ON_BN_CLICKED(IDC_BUTTON1, &CDLGDlg::OnBnClickedButton1)
	ON_BN_CLICKED(IDC_BUTTON2, &CDLGDlg::OnBnClickedButton2)
	ON_WM_TIMER()
END_MESSAGE_MAP()


// CDLGDlg 消息处理程序

BOOL CDLGDlg::OnInitDialog()
{
	CDialogEx::OnInitDialog();

	// 将“关于...”菜单项添加到系统菜单中。

	// IDM_ABOUTBOX 必须在系统命令范围内。
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != nullptr)
	{
		BOOL bNameValid;
		CString strAboutMenu;
		bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
		ASSERT(bNameValid);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// 设置此对话框的图标。  当应用程序主窗口不是对话框时，框架将自动
	//  执行此操作
	SetIcon(m_hIcon, TRUE);			// 设置大图标
	SetIcon(m_hIcon, FALSE);		// 设置小图标

	//ShowWindow(SW_MAXIMIZE);

	//ShowWindow(SW_MINIMIZE);

	// TODO: 在此添加额外的初始化代码

	return TRUE;  // 除非将焦点设置到控件，否则返回 TRUE
}

void CDLGDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialogEx::OnSysCommand(nID, lParam);
	}
}

// 如果向对话框添加最小化按钮，则需要下面的代码
//  来绘制该图标。  对于使用文档/视图模型的 MFC 应用程序，
//  这将由框架自动完成。

void CDLGDlg::OnPaint()
{
	if (IsIconic())
	{
		CPaintDC dc(this); // 用于绘制的设备上下文

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// 使图标在工作区矩形中居中
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// 绘制图标
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CPaintDC dc(this); // 用于绘制的设备上下文

		// X坐标
		dc.MoveTo(transfrom(-50, 0));
		dc.LineTo(transfrom(2600, 0));
		// Y坐标
		dc.MoveTo(transfrom(0, -600));
		dc.LineTo(transfrom(0, 1600));
		// 设写速度
		dc.MoveTo(transfrom(0, m_fVelocity));
		dc.LineTo(transfrom(20, m_fVelocity));

	}
}

//当用户拖动最小化窗口时系统调用此函数取得光标
//显示。
HCURSOR CDLGDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}



int nn = 0;
float b2 = 3;
float b3 = b2 * 1;
float bb = b2;
float b = bb;
float aa = 0;
float xx = 0;
float yy = 0;
float am = 50;

// 设置速度
void CDLGDlg::OnBnClickedOk()
{
	UpdateData();

	m_pEdit1.SetFocus();
	m_pEdit1.SetSel(0xffff0000);

	// 设写速度
	CDC* pdc = GetDC();
	pdc->MoveTo(transfrom(xx, m_fVelocity));
	pdc->LineTo(transfrom(xx + 2000, m_fVelocity));
	pdc->MoveTo(transfrom(xx, m_fVelocity/2));
	pdc->LineTo(transfrom(xx + 2000, m_fVelocity/2));
	ReleaseDC(pdc);

}

// 坐标变换
CPoint CDLGDlg::transfrom(float x, float y)
{
	D2D1_POINT_2F p;
	p.x = x;
	p.y = y;

	D2D1_POINT_2F p1 = m_mtx.TransformPoint(p);

	return CPoint(p1.x, p1.y);
}



// 启动/暂停
void CDLGDlg::OnBnClickedButton1()
{
	m_pEdit1.SetFocus();
	m_pEdit1.SetSel(0xffff0000);

	if(m_nTimer)
	{
		KillTimer(m_nTimer);
		m_nTimer = 0;
	}
	else
		m_nTimer = SetTimer(100, 300, 0);
}

// 初始化
void CDLGDlg::OnBnClickedButton2()
{
	Invalidate();
	if (m_nTimer)
	{
		KillTimer(m_nTimer);
		m_nTimer = 0;
	}
	xx = 0;
	yy = 0;
	aa = 0;
	b = b2;
}

// 画图
void CDLGDlg::OnTimer(UINT_PTR nIDEvent)
{
	CDC* pdc = GetDC();

	// 上一点坐标
	float xx0 = xx, yy0 = yy, aa0 = aa;

	// 按当前加速度计算到加速度为0时的速度变化，加一个当前加速度，想当于4舍5入
	float sdv = m_fVelocity - yy;			// 设定速度变化量

	//if (sdv < 0)	// 快速减速
	//{
	//	bb = b3;
	//	b = bb;
	//}

	float aa1 = aa < 0 ? -aa : aa;			// 绝对值
	float aa2 = aa1 / 2;
	float cdv = aa * aa1 / bb / 2 + aa;		// 到抛物线顶点的距离，加一个当前加速度，想当于4舍5入

	if (aa != 0 || yy != m_fVelocity)	// 调整中
	{
		if (sdv >= -b3 && sdv <= b3 && aa >= -b3 && aa <= b3)	// 加速度和速度都小于5时，结束
		{
			aa = 0;
			yy = m_fVelocity;
		}
		else
		{
			if (cdv + aa2 < sdv)		// 计算顶点在设定值的上面，加速度加大
				b = bb;
			else if (cdv - aa2 > sdv)	// 计算顶点在设定值的下面，加速度减小
				b = -bb;
			else
				b = 0;					// 计算顶点和设定值接近，加速度不变

			if (sdv > 0 && yy < 100)
				aa = b;
			else if ((b > 0 && aa < am) || (b < 0 && aa > -am))	// 加速度在正负50范围内
				aa += b;

			yy += aa;	// 速度变化
		}
	}

	xx += 20;

	// 加速度曲线
	pdc->MoveTo(transfrom(xx0, aa0 * 5));
	pdc->LineTo(transfrom(xx, aa*5));

	// 速度曲线
	pdc->MoveTo(transfrom(xx0, yy0));
	pdc->LineTo(transfrom(xx, yy));

	ReleaseDC(pdc);

	CDialogEx::OnTimer(nIDEvent);
}

PLC功能块

(* 抛物线加减速 *)

if a = 0.0 and v = sv THEN	(* 调整结束 *)
	RETURN;
END_if;

(* 设定速度与输出速度差值 *)
sdv := sv - v;	

(* 如果是减速，参数加大3倍，快速减 *)
IF sdv < 0.0 THEN
	da2 := da * 2.0;
	ma2 := ma * 2.0;
ELSE
	da2 := da;
	ma2 := ma;
END_IF;

(* 加速度的绝对值 *)
IF a < 0.0 THEN
	ab := -a;
ELSE
	ab := a;
END_IF;

(* 到抛物线顶点的距离，加一个当前加速度，想当于4舍5入*)
cdv := a * ab / da2 / 2.0 + a;

 (*	 加速度和速度都小于5时，结束 *)
if sdv >= -da2 and sdv <= da2 and a >= -da2 and a <= da2 THEN
		a := 0.0;
		v := sv;
		RETURN;
END_IF;

(* 加加速或减加速 *)
if cdv + ab  /  2.0 < sdv then		(* 计算顶点在设定值的上面，加速度加大 *)
	sda := da2;
elsif  cdv - ab / 2.0 > sdv then			(*  计算顶点在设定值的下面，加速度减小 *)
	sda := -da2;
else
	sda := 0.0;					(* 计算顶点和设定值接近，加速度不变 *)
end_if;

(* 新加速度 *)
IF sdv > 0.0 AND v < 30.0 THEN		(* 开始用均加速，减少冲击 *)
	a := da2;
elsif (sda > 0.0 and a < ma2) OR  (sda < 0.0 and a > -ma2) THEN		(* 加速度在正负50范围内 *)
	a := a + sda;
END_IF;

(* 新速度 *)
v := v + a;		(* 速度变化 *) 
		
(* 速度范围 *)
IF v > mv THEN
	v := mv;
ELSIF v < 0.0 THEN
	v := 0.0;
END_IF;

(* 急停后输出为0 *)
IF 前驱mm／s = 0.0 and sv = 0.0 THEN
	v := 0.0;
END_IF;

自动驾驶

更多相关内容

电机S型加减速代码Acc_Dec.c
2019-07-30 14:30:36

本例程为电机S型加减速例程，附上了源代码，代码中有很多注释，通俗易懂

收起
S型加减速曲线算法----matlab
2018-12-03 16:28:18

S型加减速算法，可实现由脉冲数反算速度曲线、由加减速率计算速度曲线和加减速时间计算速度曲线。

收起
NURBS曲线实时插补中S型加减速算法的研究 (2010年)
2021-05-08 23:19:39

针对在高速高精度加工过程中非均匀B样条曲线(non-uniform rational B-spfine，NURBS)的实时插补时，机床加减速容易出现冲击或者振荡现象，提出了S型加减速(S-shape ADC/DEC)前瞻方法。建立了曲线的S型加减速离散采样...

收起
步进电机s型加减速算法、精准定位脉冲程序
2019-10-18 19:59:17

非常流行的步进电机STM32控制代码，S型加减速，代码中可以随时获取电机已走脉冲（实际就是当前位置），可以通过给定步数走指定距离，里面有相关程序说明。

收起
步进电机_S型加减速.rar
2020-04-24 22:21:37

此代码是基于24BYJ-48步进电机实现S型加减速，可以正反转。代码环境为STM32F103RC，HAL库，RTX操作系统。代码备注详细。

收起
STM32F407驱动步进电机S型加减速.7z
2019-12-23 16:34:04

STM32F407驱动步进电机S型加减速,对步进电机驱动器进行驱动。使用STCubeMX和keil5进行编程。

收起
步进电机S型曲线加减速算法.rar
2021-07-07 16:36:12

步进电机的s型曲线的加减速算法，对整个s型曲线有ppt的形式进行详细的分析，希望能帮助大家更好的理解并更好的运用起来。

收起
S型加减速分析附参考源码和文档
2021-09-11 18:57:29

S型加减速分析附参考源码和文档

收起
步进电机加减速S型曲线控制
2020-10-16 10:02:10

网上的步进电机加减速控制，无论程序还是文档，都很难理解，苦心琢磨一周时间，终于搞定，用自己的stm32板子，成功编写出S型曲线控制不进电机的加减速，想完美控制步进电机的小伙伴们，有福啦，资料还有几个，想要的...

收起

步进电机的S型加减速算法

2021-07-19 09:59:53

1.1、S加减速的计算 static float Freq[10][1000]; static unsigned int Period[10][1000]; /****************************************************************************** * * Calculate the Period and ...

电机能运行的速度远远大于启动速度(即最大匀速速度)，那么怎么平稳的运行到最大速度就是S型加减速曲线的作用

1、Qt

1.1、S加减速的计算


static float Freq[10][1000];
static unsigned int Period[10][1000];
/******************************************************************************
*
*  Calculate the Period and Frequency array value, fill the Period value
*	into the Period register during the timer interrupt.
*  Calculate the acceleration procedure, a totally 1000 elements array.
*  parameter : *fre - point to the array that keeps the frequency value.
*  parameter : *period - point to the array that keeps the period value.
*  len : the procedure of accceleration length. it is best thing to set
*			the float number, some compile software maybe transfer error if
*			set it as a int.
*  fre_max : maximum speed, frequency value.
*  fre_min : minimum speed, frequency value.
*  mind : 72 * 1000000 / 2^32 = 0.1, so fre_min can't less than 0.1.
*  flexible : flexible value. adjust the S curves.
*  Y = A + B / (1 + math.pow(exp, (-ax+b)))
*    其中的A分量在y方向进行平移， B分量在y方向进行拉伸，(ax+b)在x方向进行平移和拉伸.
*   加速过程 ： 从fre_min Hz加速到fre_max Hz
*	采用1000个点进行加速处理。最终在加速过程中采用的曲线方程为：
*	Fcurrent = Fmin + (Fmax - Fmin)/(1 + math.pow(exp, -Flexible * (i - num)/num
*   其中1000000为1MHz频率，此为定时器频率，由自己的设置和改变  
*   最大的频率和最小的频率，最好是通过自己调试无杂音的频率
*******************************************************************************/
//S型加减速曲线的参数点的计算公式，
//0-999为加速的，999-0为减速的
void CalculateSModelLine(float *fre, unsigned int *period, float len,
                float fre_max, float fre_min, float flexible)
{
    unsigned int i = 0;
    float deno;
    float melo;
    float delt = fre_max - fre_min;
    for(i = 0; i<len; i++)
    {
       melo = flexible * (i-len/2)/(len/2);
        deno = 1.0/(1 + expf(-melo)); // expf is a library function of exponential(e)
        fre[i] = delt * deno + fre_min;
        period[i] = (unsigned int)(1000000.0 / fre[i]);  //1000000 is the timer driver frequeny : 72000000 / 72
    }
}

1.2、S型加减速的Qt代码

在项目的.pro文件添加

QT += charts

mainwindow.h

#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
#include "QtCharts/QChart"
#include <QLineSeries>
#include <QValueAxis>
#include <QTimer>
#include <QDebug>
#include <QChartView>
#include <QList>
#include <QPointF>
#include <qmath.h>

QT_CHARTS_USE_NAMESPACE

namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
    explicit MainWindow(QWidget *parent = nullptr);
    ~MainWindow();

private:
    Ui::MainWindow *ui;
    QChart *m_chart;
    QLineSeries *m_series[10];
};

#endif // MAINWINDOW_H

mainwindow.c

#include "mainwindow.h"
#include "ui_mainwindow.h"

static float Freq[10][1000];
static unsigned int Period[10][1000];
void CalculateSModelLine(float *fre, unsigned int *period, float len,
                float fre_max, float fre_min, float flexible);

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    for(int i = 0;i < 10;i++)
        CalculateSModelLine(&Freq[i][0], &Period[i][0],1000, 15000,500, i);

    m_chart = new QChart;
    QChartView *chartView = new QChartView(m_chart);
    chartView->setRenderHint(QPainter::Antialiasing);
    chartView->setRubberBand(QChartView::RectangleRubberBand);

    QValueAxis *axisX = new QValueAxis;
    axisX->setRange(1,1000);
    axisX->setLabelFormat("%g");
    axisX->setTitleText("axisX");

    QValueAxis *axisY = new QValueAxis;
    axisY->setRange(0,50);
    axisY->setTitleText("axisY");

    for(int i = 0;i < 10;i++){
        m_series[i] = new QLineSeries;
        m_chart->addSeries(m_series[i]);
        switch(i){
            case 0:m_series[0]->setPen(QPen(QColor(255,0,0),1,Qt::SolidLine));break;
            case 1:m_series[1]->setPen(QPen(QColor(203,158,252),1,Qt::SolidLine));break;
            case 2:m_series[2]->setPen(QPen(QColor(14,189,0),1,Qt::SolidLine));break;
            case 3:m_series[3]->setPen(QPen(QColor(181,145,113),1,Qt::SolidLine));break;
            case 4:m_series[4]->setPen(QPen(QColor(56,68,103),1,Qt::SolidLine));break;
            case 5:m_series[5]->setPen(QPen(QColor(252,158,226),1,Qt::SolidLine));break;
            case 6:m_series[6]->setPen(QPen(QColor(158,163,252),1,Qt::SolidLine));break;
            case 7:m_series[7]->setPen(QPen(QColor(118,41,118),1,Qt::SolidLine));break;
            case 8:m_series[8]->setPen(QPen(QColor(139,0,139),1,Qt::SolidLine));break;
            case 9:m_series[9]->setPen(QPen(QColor(72,61,139),1,Qt::SolidLine));break;
        }
        m_chart->setAxisX(axisX,m_series[i]);
        m_chart->setAxisY(axisY,m_series[i]);
    }

    m_chart->legend()->hide();
    m_chart->setTitle("demo");

    QVBoxLayout *layout = ui->verticalLayout;
    layout->addWidget(chartView);
    QVector<QPointF> points;
    float max = 0;
    for(int i = 0;i < 10;i++){
        points.clear();
        for(int cnt = 0;cnt < 1000;cnt++){
            if(Freq[i][cnt] > max)
                max = Freq[i][cnt];
            points.append(QPointF(cnt+1,Freq[i][cnt]));
        }
        m_series[i]->replace(points);
    }
    axisY->setRange(1,(max+1000));
}

MainWindow::~MainWindow()
{
    delete ui;
}

void CalculateSModelLine(float *fre, unsigned int *period, float len,
                float fre_max, float fre_min, float flexible)
{
    unsigned int i = 0;
    float deno;
    float melo;
    float delt = fre_max - fre_min;
    for(i = 0; i<len; i++)
    {
       melo = flexible * (i-len/2)/(len/2);
        deno = 1.0/(1 + expf(-melo)); // expf is a library function of exponential(e)
        fre[i] = delt * deno + fre_min;
        period[i] = (unsigned int)(1000000.0 / fre[i]);  //1000000 is the timer driver frequeny : 72000000 / 72
    }
}

1.3、显示曲线

从红线开始依次往下，flexible依次为0-9；根据自己调试情况使用

从这里可以知道：

flexible越大，曲线起步的速度越慢，电机加速时间越短，这一种可能需要将最大速度频率调小，否则可能会引起堵转
flexible越小，曲线起步的速度越快，电机加速时间越长，曲线速度会平缓一点，但是需要将最小速度调小，否则可能会引起堵转

2、单片机调用

计算S型加减速曲线的所有频率与Qt的上位机方法一致，单片机中主要说明调用

伪代码

//以STM32F407为例
int actual = 0;
int target = 5000;
int max_point = 950;
int index = 0;
/* 注意这里调用的是Period，Qt显示的Freq，应该频率是周期的倒数 */
void Acc_Speed(void)
{	
	if(index<= max_point ){
        TIM->ARR = Period[index++];
	    TIM->CCR1 = TIM->ARR / 2;
    }
}

void Reduce_Speed(void)
{		
    if(index> 0){
        TIM->ARR = Period[index--];
	    TIM->CCR1 = TIM->ARR / 2;
    }	
}

void TIM_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM, TIM_IT_Update) == RESET){
        TIM_ClearFlag(TIM, TIM_FLAG_Update);
        actual++;//脉冲数加一，代表电机行走了一步
        if(actual >= target){	
            actual  = 0;
            /* 电机停止 */
        }else if(actual < max_point){		
            Acc_Speed();/* 加速 */
        }else if(actual > target  - max_point){
            Reduce_Speed();/* 减速 */
        }
    }
}

收起

展开全文

S型加减速分析-nxc.zip
2021-06-03 09:20:19

步进电机S型加减速资料

收起

加速度连续的s型加减速算法程序

千次阅读 2021-03-28 15:47:59

此算法为加速度连续的7段s型加减速规划实现，理论参考：https://blog.csdn.net/Septembernine/article/details/53125828 （1）加加速段加加速度为 jmaxjmax，加速度线性增加至设定值或最大值amaxamax。（2）匀加速...

在这里插入图片描述
此算法为加速度连续的7段s型加减速规划实现，理论参考：https://blog.csdn.net/Septembernine/article/details/53125828

（1）加加速段
加加速度为 jmaxjmax，加速度线性增加至设定值或最大值amaxamax。
（2）匀加速段
加加速度为0，加速度恒定。
（3）减加速段
当速度接近设定的值或最大值vmaxvmax时，加加速度突变为反向的jmaxjmax，进入加速度线性减小的变减速运动阶段。
（4）匀速段
当速度增至vmaxvmax后，加加速和加速度均变为0，进入匀速运动阶段。
（5）加减速段
加加速度突变为反向的jmaxjmax，加速度反向线性增加至−amax−amax。
（6）匀减速段
加加速度和为0，减加速度恒定。
（7）减减速段
加速度突变为jmaxjmax，加速度由负向的amaxamax线性减小至0。

程序实现如下：

int sign(float q0, float q1);
// S曲线参数计算
void STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
// 计算位置
float S_position(float);
// 计算速度
float S_velocity(float t);
// 计算加速度
float S_acceleration(float t);
// 计算加加速度
float S_jerk(float t);

int sSpeed::sign(float q0, float q1)
{
    int a = 0;
    if (q1 - q0 > 0)
        a = 1;
    else
        a = -1;
    return a;
}

void sSpeed::STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax)
{
    float vmin, amin, jmin;
    int sigma = 0;
    float Tj = 0, delta = 0;
    vmin = -vmax;
    amin = -amax;
    jmin = -jmax;

    // 計算实际的q_0、q_1、v_max、a_max
    sigma = sign(q0, q1);
    Input_data.q0 = sigma * q0;
    Input_data.q1 = sigma * q1;
    Input_data.v0 = sigma * v0;
    vmax = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
    vmin = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
    Input_data.amax = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
    Input_data.amin = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
    Input_data.jmax = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
    Input_data.jmin = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;

    // 判断是否达到最大速度
    if ((vmax - Input_data.v0)*Input_data.jmax < pow(Input_data.amax, 2)){
        // 达不到amax
        Input_data.Tj1 = sqrt((vmax - Input_data.v0)/Input_data.jmax);
        Input_data.Ta = 2*Input_data.Tj1;
        Input_data.alima = Input_data.jmax * Input_data.Tj1;
    }else{
        // 能够达到amax
        Input_data.Tj1 = Input_data.amax/Input_data.jmax;
        Input_data.Ta = Input_data.Tj1 + (vmax - Input_data.v0 )/Input_data.amax;
        Input_data.alima = Input_data.amax;
    }

    if ((vmax - Input_data.v1)*Input_data.jmax < pow(Input_data.amax, 2)){
        // 达不到amin
        Input_data.Tj2 = sqrt((vmax - Input_data.v1)/Input_data.jmax);
        Input_data.Td = 2 * Input_data.Tj2;
        Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
    }else{
        // 能够达到amin
        Input_data.Tj2 = Input_data.amax/Input_data.jmax;
        Input_data.Td = Input_data.Tj2 + (vmax - Input_data.v1)/Input_data.amax;
        Input_data.alimd = -Input_data.amax;
    }
    // 计算匀速段时间
    Input_data.Tv = (Input_data.q1 - Input_data.q0)/vmax - (Input_data.Ta/2)*(1 + Input_data.v0/vmax)
            - (Input_data.Td/2)*(1 + Input_data.v1/vmax);

    // 对Tv进行讨论
    if (Input_data.Tv > 0)
        // 能够达到给定的最大速度vmax， 即存在匀速阶段
        Input_data.vlim = vmax;
    else{
        // 达不到最大速度，即匀速阶段Tv=0
        // 假设最大加速度和最小加速度均能达到
        Input_data.Tv = 0;
        Tj = Input_data.amax / Input_data.jmax;
        Input_data.Tj1 = Tj;
        Input_data.Tj2 = Tj;
        delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
                + Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
        Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
        Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
        // 对Ta和Td进行讨论
        if (Input_data.Ta < 0 || Input_data.Td < 0){
            if (Input_data.Ta < 0){
                // 没有加速段，只有减速段
                Input_data.Ta = 0;
                Input_data.Tj1 = 0;
                Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                                                                                               + pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                Input_data.alima = 0;
                Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
                Input_data.vlim = v0;
            }else if(Input_data.Td < 0){
                // 没有减速段，只有加速段
                Input_data.Td = 0;
                Input_data.Tj2 = 0;
                Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                                                                                               - pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                Input_data.alima = Input_data.jmax * Input_data.Tj1;
                Input_data.alimd = 0;
                Input_data.vlim = Input_data.jmax * Input_data.Tj1;
            }
        }else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
            // 加速段和减速段都能达到最大速度
            Input_data.alima = Input_data.amax;
            Input_data.alimd = -Input_data.amax;
            Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
        }else{
            // 加速段和减速阶段至少有一段不能达到最大加速度
            float lambda = 0.99; // 系统取0<lambda<1
            while (Input_data.Ta < 2*Tj || Input_data.Td < 2*Tj){
                Input_data.amax = lambda * Input_data.amax;
                Input_data.Tv = 0;
                Tj = Input_data.amax / Input_data.jmax;
                Input_data.Tj1 = Tj;
                Input_data.Tj2 = Tj;
                delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
                        + Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
                Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
                Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
                if (Input_data.Ta < 0 || Input_data.Td < 0){
                    if (Input_data.Ta < 0){
                        // 没有加速段，只有减速段
                        Input_data.Ta = 0;
                        Input_data.Tj1 = 0;
                        Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                        Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                            + pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                        Input_data.alima = 0;
                        Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
                        Input_data.vlim = v0;
                    }else if(Input_data.Td < 0){
                        // 没有减速段，只有加速段
                        Input_data.Td = 0;
                        Input_data.Tj2 = 0;
                        Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                        Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                           - pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                        Input_data.alima = Input_data.jmax * Input_data.Tj1;
                        Input_data.alimd = 0;
                        Input_data.vlim = Input_data.jmax * Input_data.Tj1;
                    }
                }else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
                    // 加速段和减速段都能达到最大速度
                    Input_data.alima = Input_data.amax;
                    Input_data.alimd = -Input_data.amax;
                    Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
                }
            }
        }
    }

    qDebug()<<Input_data.Ta;
}

float sSpeed::S_position(float t)
{
    float T = 0, q = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float q0 = Input_data.q0;
    float q1 = Input_data.q1;
    float v0 = Input_data.v0;
    float v1 = Input_data.v1;
    float vlim = Input_data.vlim;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        q = q0 + v0*t + jmax*pow(t, 3)/6;
    else if (t >= Tj1 && t < Ta - Tj1)
        q = q0 + v0*t +(alima/6)*(3*pow(t, 2) - 3*Tj1*t + pow(Tj1, 2));
    else if (t >= Ta - Tj1 && t < Ta)
        q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*(pow(Ta - t, 3)/6);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(pow(t - T + Td, 3)/6);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*pow(t - T + Td, 2) - 3*Tj2*(t - T + Td) + pow(Tj2, 2));
    else if (t >= T - Tj2 && t <= T)
        q = q1 - v1*(T - t) - jmax*(pow(T - t, 3)/6);

    return q;
}

float sSpeed::S_velocity(float t )
{
    float T = 0, qd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float v0 = Input_data.v0;
    float v1 = Input_data.v1;
    float vlim = Input_data.vlim;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qd = v0 + jmax*(pow(t, 2)/2);
    else if (t >= Tj1 && t < Ta - Tj1)
        qd = v0 + alima*(t - Tj1/2);
    else if (t >= Ta - Tj1 && t < Ta)
        qd = vlim + jmin*(pow(Ta - t, 2)/2);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qd = vlim;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qd = vlim - jmax*(pow(t - T + Td, 2)/2);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qd = vlim + alimd*(t - T + Td - Tj2/2);
    else if (t >= T - Tj2 && t <= T)
        qd = v1 + jmax*(pow(t - T, 2)/2);

    return qd;
}

float sSpeed::S_acceleration(float t)
{
    float T = 0, qdd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qdd = jmax * t;
    else if (t >= Tj1 && t < Ta - Tj1)
        qdd = alima;
    else if (t >= Ta - Tj1 && t < Ta)
        qdd = -jmin * (Ta - t);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qdd = 0;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qdd = -jmax * (t - T - Td);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qdd = alimd;
    else if (t >= T - Tj2 && t <= T)
        qdd = -jmax * (T - t);

    return qdd;
}

float sSpeed::S_jerk(float t)
{
    float T = 0, qddd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qddd = jmax;
    else if (t >= Tj1 && t < Ta - Tj1)
        qddd = 0;
    else if (t >= Ta - Tj1 && t < Ta)
        qddd = -jmin;
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qddd = 0;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qddd = -jmax;
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qddd = 0;
    else if (t >= T - Tj2 && t <= T)
        qddd = jmax;

    return qddd;
}

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

完整工程Git下载链接：https://gitee.com/luck126/sspeed
工具：Qt Creater5.9

收起

展开全文

c++ 算法

S型曲线模拟器_STM32F103_s曲线_S型曲线加减速
2021-09-10 18:12:49

步进电机伺服电机S曲线加减速模拟软件。做运动控制的可以很好用此软件了解S曲线加减速的各参数。

收起
STM32 S型加减速计算程序
2018-04-25 10:43:56

STM32 S型加减速计算程序 STM32 S型加减速计算程序 STM32 S型加减速计算程序

收起
基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）
万次阅读 多人点赞 2019-08-03 21:59:31
写在前面在前面的博文中：基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）...下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中，仿真效果还可以，加速度曲线连续，更柔...
写在前面

学习代码都记录在个人github上，欢迎关注~

Matlab机器人工具箱版本9.10

在前面的博文中：

基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）

基于单位四元数的姿态插补（Matlab）

我使用了抛物线过渡（也就是梯形加减速）作为空间插补算法的速度规划方法，但是梯形曲线的缺点也很明显，即加速度不连续，速度过渡不平滑，容易造成机械系统冲击。下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中，仿真效果还可以，加速度曲线连续，更柔和，适用于精度速度要求更高的场合。

空间直线插补：

空间圆弧插补：

直观插补：

带约束S型速度规划

预备知识：

机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划

参照基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）中归一化参数思想，将归一化参数计算函数中的梯形加减速方法换成S型加减速法。此时，定义归一化时间算子：
$\lambda( t ) = \frac { s ( t ) - s ( 0 ) } { L }$
其中， $\in [ 0 , T ]$ , $s (t)$ 是S型曲线中位移在全局时间坐标下的函数， $L$ 是机器人执行器末端经过的距离。

空间直线插补与空间圆弧插补

位置插补与姿态插补需要分开讨论。

位置插补

假设机器人末端由P1点沿直线运动到P2点， $\left(x_{1}, y_{1}, z_{1}\right)$ 分别为起点P1的位置， $\left(x_{2}, y_{2}, z_{2}\right)$ 为终点P2的位置， $\left(x_{i}, y_{i}, z_{i}\right)$ 为中间插补点Pi的位置。

各个插值点位置坐标向量为：
$\left\{\begin{array}{l}{x=x_{1}+\lambda \Delta x} \\ {y=y_{1}+\lambda \Delta y} \\ {z=z_{1}+\lambda \Delta z}\end{array}\right.$
其中， $\lambda$ 是归一化时间因子， $(\Delta x, \Delta y, \Delta z)$ 表示首末位置间的位置增量，其解为：
$\left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \end{array}\right.$

基于单位四元数的姿态插补

预备知识：

基于单位四元数的姿态插补（Matlab）

已知起点S、终点D的齐次变换矩阵（位姿）分别为 $T_S$ 和 $T_D$ ，则姿态插补步骤如下：
1. 从齐次变换矩阵中提取出各自的旋转矩阵 $R_S$ 和 $R_D$ ，并将其转换为四元数 $Q_s$ 和 $Q_d$ ;
2. 姿态四元数插补公式为：
  $Q_{k}(t)=\operatorname{Slerp}\left(Q_{s}, Q_{d}, t\right)=\frac{\sin [(1-\lambda(t)) \theta]}{\sin \theta} Q_{s}+\frac{\sin [\lambda(t) \theta]}{\sin \theta} Q_{d}$
  式中， $\theta=arccos(Q_{s} \cdot Q_{d})$ ；
3. 将四元数 $Q_{k}(t)$ 转换为旋转矩阵 $R_{k}$ ，将 $R_{k}$ 和位置插补得到的 $P_{k}$ 组合得到各插值点对应的齐次变换矩阵 $T_{k}$ ；
4. 带入逆运动学计算，得到各关节角度变量，进行插补运动。
规划插补流程

基于带约束S型加减速曲线的插补算法流程图如下：

Matlab实现代码

和前面博文的联系非常紧密，部分函数并未展出。
```
% 归一化处理
% S型加减速曲线
% 输入参数：机械臂末端运动总位移（角度）pos 
%          机械臂末端线速度（角速度）初始v0，终止v1，最大速度vmax
%          最大加减速度amax、最大加加速度jmax
%          插补周期t
% 返回值： 插值点数N
function [lambda, N] = Normalization_S(pos, v0, v1, vmax, amax, jmax, t)
% S曲线参数计算（S型速度规划，又称七段式轨迹）
% function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
q0 = 0; q1 = pos;
para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax); % 获取S曲线参数
% 总的规划时间
T = para(1) + para(2) + para(3)
% 等时插补
N = ceil(T / t);
j = 1;
for i = 0 : t: T
   q(j) = S_position(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
%    qd(j) = S_velocity(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   lambda(j) = q(j) / pos;
   j = j + 1;
end
end
```
```
% 空间单一直线位置插补与单元四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数：起点S位置， 终点D位置
%      起点S的单元四元数Qs，终点的单元四元数Qd
%      起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%      插补周期t
% 返回值：插值点位置、单元四元数、插值点数
function [x y z qk N] = SpaceLine_Q(S, D, Qs, Qd, v0, v1, vmax, amax, jmax, t)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = D(3);

P = 1; % 插值参数，增加插值点数，避免过小
% 总位移S
s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
% 插值点数N
% N = ceil(P*s / vs)
% 求归一化参数：梯形加减速曲线
% function lambda = Normalization(pos, vel, accl, N)
% lambda = Normalization(s, vs, a, N);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(s, v0, v1, vmax, amax, jmax, t);
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
% 计算两个四元数之间的夹角
dot_q = Qs.s*Qd.s + Qs.v(1)*Qd.v(1) + Qs.v(2)*Qd.v(2) + Qs.v(3)*Qd.v(3);
if (dot_q < 0)
    dot_q = -dot_q;
end
 
for i = 1: N
    % 位置插补
    x(i) = x1 + delta_x*lambda(i);
    y(i) = y1 + delta_y*lambda(i);
    z(i) = z1 + delta_z*lambda(i);
    % 单位四元数球面线性姿态插补
    % 插值点四元数
    if (dot_q > 0.9995)
        k0 = 1-lambda(i);
        k1 = lambda(i);
    else
        sin_t = sqrt(1 - power(dot_q, 2));
        omega = atan2(sin_t, dot_q);
        k0 = sin((1-lambda(i)*omega)) / sin(omega);
        k1 = sin(lambda(i)*omega) / sin(omega);
    end
    qk(i) = Qs * k0 + Qd * k1;
end

end
```
```
% 空间单一圆弧位置插补与单位四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数： 起点S位置, 中间点M位置, 终点D位置
%       起点S和终点D的单位四元数Qs_,Qd_
%       起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%       插值周期t
% 返回值：插值点位置、单元四元数、插值点数
% 方便起见，角速度和角加速度均为角度制
function [x y z qk N] = SpaceCircle_Q(S, M, D, Qs_, Qd_, v0, v1, vmax, amax, jmax, t)
x1 = S(1); x2 = M(1); x3 = D(1);
y1 = S(2); y2 = M(2); y3 = D(2);
z1 = S(3); z2 = M(3); z3 = D(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);
D1 = -(A1*x3 + B1*y3 + C1*z3);

A2 = x2 - x1;
B2 = y2 - y1;
C2 = z2 - z1;
D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;

A3 = x3 - x2;
B3 = y3 - y2;
C3 = z3 - z2;
D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
b = [-D1, -D2, -D3]'
% 圆心
C = Gauss_lie(3, A, b)
x0 = C(1); y0 = C(2); z0 = C(3);
plot3(x0, y0, z0, 'bo')
hold on
% 外接圆半径
r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
% 新坐标系Z0的方向余弦
L = sqrt(A1^2 + B1^2 + C1^2);
ax = A1 / L; ay = B1 / L; az = C1 / L;
% 新坐标系X0的方向余弦
nx = (x1 - x0) / r;
ny = (y1 - y0) / r;
nz = (z1 - z0) / r;
% 新坐标系Y0的方向余弦
o = cross([ax, ay, az], [nx, ny, nz]);
ox = o(1);
oy = o(2);
oz = o(3);
% 相对于基座标系{O-XYZ}， 新坐标系{C-X0Y0Z0}的坐标变换矩阵
T = [nx ox ax x0;
     ny oy ay y0;
     nz oz az z0;
      0  0  0  1]
T_ni = T^-1
% 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
S_ = (T^-1)*[S'; 1]
M_ = (T^-1)*[M'; 1]
D_ = (T^-1)*[D'; 1]
x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
% 判断圆弧是顺时针还是逆时针，并求解圆心角
if (atan2(y2_, x2_) < 0)
    angle_SOM = atan2(y2_, x2_) + 2*pi;
else
    angle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)
    angle_SOD = atan2(y3_, x3_) + 2*pi;
else
    angle_SOD = atan2(y3_, x3_);
end
% 逆时针
if (angle_SOM < angle_SOD)
    flag = 1;
    theta = angle_SOD % 圆心角
end
% 顺时针
if (angle_SOM >= angle_SOD)
    flag = -1;
    theta = 2*pi - angle_SOD % 圆心角
end
% 插补点数N
% P = 1; %插补参数，增加插值点数，避免过小
% ws = ws*pi / 180; % 角度换成弧度
% a = a*pi / 180;
% N = ceil(P*theta / ws);
% % 求归一化参数：梯形加减速曲线
% lambda = Normalization(theta, ws, a, N);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(theta, v0, v1, vmax, amax, jmax, t);

% 插补原理: 在新平面上进行插补（简化）
% 在新坐标系下z1_,z2_,z3_均为0，即外接圆在新坐标系的XOY平面内
% 此时转化为平面圆弧插补问题
delta_ang = theta;
% 计算两个四元数之间的夹角
dot_q = Qs_.s*Qd_.s + Qs_.v(1)*Qd_.v(1) + Qs_.v(2)*Qd_.v(2) + Qs_.v(3)*Qd_.v(3);
if (dot_q < 0)
    dot_q = -dot_q;
end

for i = 1: N
    % 位置插补
    x_(i) = flag * r * cos(lambda(i)*delta_ang);
    y_(i) = flag * r * sin(lambda(i)*delta_ang);
    P = T*[x_(i); y_(i); 0; 1];
    x(i) = P(1);
    y(i) = P(2);
    z(i) = P(3);
    % 单位四元数球面线性姿态插补
    % 插值点四元数
    if (dot_q > 0.9995)
        k0 = 1-lambda(i);
        k1 = lambda(i);
    else
        sin_t = sqrt(1 - power(dot_q, 2));
        omega = atan2(sin_t, dot_q);
        k0 = sin((1-lambda(i))*omega) / sin(omega);
        k1 = sin(lambda(i)*omega) / sin(omega);
    end
    qk(i) = Qs_ * k0 + Qd_ * k1;
end

end
```
不足之处
1. 圆弧的姿态插补只严格通过了起始姿态和终点姿态，没有经过中间姿态；
2. 无法达到时间最优。
参考文献

[1]刘鹏飞,杨孟兴,宋科,段晓妮.‘S’型加减速曲线在机器人轨迹插补算法中的应用研究[J].制造业自动化,2012,34(20):4-8+11.

[2]李振娜,王涛,王斌锐,郭振武,陈迪剑.基于带约束S型速度曲线的机械手笛卡尔空间轨迹规划[J].智能系统学报,2019,14(04):655-661.

[3]王添. 基于单位四元数机器人多姿态轨迹平滑规划[D].天津工业大学,2018.

[4]季晨. 工业机器人姿态规划及轨迹优化研究[D].哈尔滨工业大学,2013.
收起
展开全文
空间直线插补空间圆弧插补
STM32实现步进电机S型加减速
2022-04-21 14:57:14

第二部分 加减速曲线这里采用的是S型加减速，此处算法参考https://wenku.baidu.com/view/86c9fc305c0e7cd184254b35eefdc8d376ee14ae.html 这里作者采用的是直接在中断里边计算当前速度，强烈建议没有硬件FPU的话...

收起

stm32 单片机
STM32步进电机S型加减速算法
千次阅读 多人点赞 2020-03-24 15:07:38

简单说明一下硬件资源，需要用到STM32两个定时器，TIM1产生PWM脉冲并对脉冲个数计数，TIM2开启定时...采用CubeMX+Hal库配置，这里不做详细介绍，重点介绍S型加减速算法的实现。首先了解一下S曲线函数，见下图： ...

收起

stm32
S型加减速算法，基于matlab仿真（1）
千次阅读 2020-12-17 20:14:50

公式推导加速度推导速度推导位移推导

收起

matlab stateflow
步进电机S型加减速分析
2018-12-14 10:46:26

关于步进电机S形加减速曲线程序，程序注释清楚，通俗易懂。包含公式详细说明。

收起
【运动控制】梯形加减速 三角加减速 柔性S型加减速曲线的认识
千次阅读 2020-05-25 00:23:16

在运动控制中常用的加减速控制算法有指数、直线、S型曲线和三角函数加减速控制算法。 PS：S型曲线加减速关注度指数，近年在上升。冲击类型和加加速度解释刚性冲击：速度发生突变，加速度理论上为无穷大，在起止...

收起

运动控制
STM32-步进电机S型加减速控制
万次阅读 多人点赞 2017-05-16 19:57:16

基于STM32的步进电机S型加减速控制算法STM32简介 STM32代表ARM Cortex-M内核的32位微控制器。专为要求高性能、低成本、低功耗的嵌入式应用专门设计的: STM32系列的内核主要有：Cortex-M0、Cortex-M0+、Cortex-M3、...

收起

半闭环控制
ucosii控制系统程序_STM32f103VCT6SPi_STM32F103VCT6_CAN_S型加减速_uart
2021-09-10 18:36:35

STM32F103VCT6的ucosii工程，包括CAN

收起
STM32控制器步进电机驱动器控制S型加减速控制本例程源码
2022-01-19 18:07:48

STM32控制器步进电机驱动器控制S型加减速控制本例程源码，有引脚接线图，红外遥控器s型曲线控制，精确定位，可以自己修改成wifi蓝牙，任意修改，只要本例程在手id=595533992714&

收起

stm32 单片机
七段S型加减速算法--多轴时间同步方案调研
千次阅读 2019-08-07 10:30:40

七段S型加减速算法–时间同步基本概念 7段S型加减速算法（7 segments S-curve velocity profile），是由T型加速度曲线(Trapezoidal motion profile)演化而来(此外，还有多项式规划（Polynomial Profile），如三次...

收起
CNC控制器S型加减速算法和实例.doc
2019-08-25 16:19:41

真正的落地需要积分方法才能实现，也就是说S曲线实际上是3阶可导的函数，用速度对应每个时间点，积出来对应的速度，加速度，距离等，最后形成真正的S曲线表格，方便微处理器去查表，方便运动控制实现。

收起
步进电机S型T梯形曲线和国外流行的SpTA加减速控制算法
热门讨论 2017-07-13 16:22:29

项目包括步进电机的S型曲线加减速控制算法 T梯形加减速控制算法和国外流行的SpTA加减速控制算法，花钱买的，国外流行的SpTA算法比够高效，很适合单片机，强烈推荐。

收起
Welding20190820—S型加减速.rar
2019-09-23 11:50:12

项目内容包括：s型加减速脉冲输出，数据掉电保存到外部flash，基于freertos系统。

收起

12
STM32简易步进电机S曲线加减速（程序）-电路方案
2021-04-21 23:07:06

分享简易的步进电机S曲线程序，本代码为查表方式 S曲线的算法可以百度查到，在代码中可以以算式的方式算出频率，也...S曲线的加入极大的减少了电机在加减速时的过冲现象，学习下思想还是很有必要的。附件内容截图:

收起