精华内容
下载资源
问答
  • 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型加减速例程,附上了源代码,代码中有很多注释,通俗易懂
  • S型加减速算法,可实现由脉冲数反算速度曲线、由减速率计算速度曲线和加减速时间计算速度曲线。
  • 针对在高速高精度加工过程中非均匀B样条曲线(non-uniform rational B-spfine,NURBS)的实时插补时,机床加减速容易出现冲击或者振荡现象,提出了S型加减速(S-shape ADC/DEC)前瞻方法。建立了曲线的S型加减速离散采样...
  • 非常流行的步进电机STM32控制代码,S型加减速,代码中可以随时获取电机已走脉冲(实际就是当前位置),可以通过给定步数走指定距离,里面有相关程序说明。
  • 此代码是基于24BYJ-48步进电机实现S型加减速,可以正反转。 代码环境为STM32F103RC,HAL库,RTX操作系统。代码备注详细。
  • STM32F407驱动步进电机S型加减速,对步进电机驱动器进行驱动。使用STCubeMX和keil5进行编程。
  • 步进电机的s型曲线的减速算法,对整个s型曲线有ppt的形式进行详细的分析,希望能帮助大家更好的理解并更好的运用起来。
  • S型加减速分析附参考源码和文档
  • 网上的步进电机加减速控制,无论程序还是文档,都很难理解,苦心琢磨一周时间,终于搞定,用自己的stm32板子,成功编写出S型曲线控制不进电机的加减速,想完美控制步进电机的小伙伴们,有福啦,资料还有几个,想要的...
  • 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

    展开全文
  • 步进电机伺服电机S曲线加减速模拟软件。做运动控制的可以很好用此软件了解S曲线加减速的各参数。
  • STM32 S型加减速计算程序 STM32 S型加减速计算程序 STM32 S型加减速计算程序
  • 写在前面 在前面的博文中: 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab)...下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中,仿真效果还可以,速度曲线连续,更柔...

    写在前面

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

    Matlab机器人工具箱版本9.10

    在前面的博文中:

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

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

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

    空间直线插补:

    在这里插入图片描述在这里插入图片描述在这里插入图片描述

    空间圆弧插补:

    在这里插入图片描述在这里插入图片描述在这里插入图片描述

    直观插补:

    在这里插入图片描述在这里插入图片描述

    带约束S型速度规划

    预备知识:

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

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

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

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

    位置插补

    假设机器人末端由P1点沿直线运动到P2点, ( x 1 , y 1 , z 1 ) \left(x_{1}, y_{1}, z_{1}\right) (x1,y1,z1)分别为起点P1的位置, ( x 2 , y 2 , z 2 ) \left(x_{2}, y_{2}, z_{2}\right) (x2,y2,z2)为终点P2的位置, ( x i , y i , z i ) \left(x_{i}, y_{i}, z_{i}\right) (xi,yi,zi)为中间插补点Pi的位置。

    各个插值点位置坐标向量为:
    { x = x 1 + λ Δ x y = y 1 + λ Δ y z = z 1 + λ Δ z \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. x=x1+λΔxy=y1+λΔyz=z1+λΔz
    其中, λ \lambda λ是归一化时间因子, ( Δ x , Δ y , Δ z ) (\Delta x, \Delta y, \Delta z) (Δx,Δy,Δz)表示首末位置间的位置增量,其解为:
    { Δ x = x 2 − x 1 Δ y = y 2 − y 1 Δ z = z 2 − z 1 \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. Δx=x2x1Δy=y2y1Δz=z2z1

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

    预备知识:

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

    已知起点S、终点D的齐次变换矩阵(位姿)分别为 T S T_S TS T D T_D TD,则姿态插补步骤如下:

    1. 从齐次变换矩阵中提取出各自的旋转矩阵 R S R_S RS R D R_D RD,并将其转换为四元数 Q s Q_s Qs Q d Q_d Qd;

    2. 姿态四元数插补公式为:
      Q k ( t ) = Slerp ⁡ ( Q s , Q d , t ) = sin ⁡ [ ( 1 − λ ( t ) ) θ ] sin ⁡ θ Q s + sin ⁡ [ λ ( t ) θ ] sin ⁡ θ Q d 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} Qk(t)=Slerp(Qs,Qd,t)=sinθsin[(1λ(t))θ]Qs+sinθsin[λ(t)θ]Qd
      式中, θ = a r c c o s ( Q s ⋅ Q d ) \theta=arccos(Q_{s} \cdot Q_{d}) θ=arccos(QsQd)

    3. 将四元数 Q k ( t ) Q_{k}(t) Qk(t)转换为旋转矩阵 R k R_{k} Rk,将 R k R_{k} Rk和位置插补得到的 P k P_{k} Pk组合得到各插值点对应的齐次变换矩阵 T k T_{k} Tk

    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.

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

    千次阅读 多人点赞 2020-03-24 15:07:38
    简单说明一下硬件资源,需要用到STM32两个定时器,TIM1产生PWM脉冲并对脉冲个数计数,TIM2开启定时...采用CubeMX+Hal库配置,这里不做详细介绍,重点介绍S型加减速算法的实现。 首先了解一下S曲线函数,见下图: ...
  • S型加减速算法,基于matlab仿真(1)

    千次阅读 2020-12-17 20:14:50
    公式推导 速度推导 速度推导 位移推导
  • 关于步进电机S加减速曲线程序,程序注释清楚,通俗易懂。包含公式详细说明。
  • 在运动控制中常用的减速控制算法有指数、直线、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、...
  • STM32F103VCT6的ucosii工程,包括CAN
  • STM32控制器步进电机驱动器控制S型加减速控制本例程源码,有引脚接线图,红外遥控器s型曲线控制,精确定位,可以自己修改成wifi蓝牙,任意修改,只要本例程在手id=595533992714&
  • 七段S型加减速算法–时间同步 基本概念 7段S型加减速算法(7 segments S-curve velocity profile),是由T型速度曲线(Trapezoidal motion profile)演化而来(此外,还有多项式规划(Polynomial Profile),如三次...
  • 真正的落地需要积分方法才能实现,也就是说S曲线实际上是3阶可导的函数,用速度对应每个时间点,积出来对应的速度,速度,距离等,最后形成真正的S曲线表格,方便微处理器去查表,方便运动控制实现。
  • 项目包括步进电机的S型曲线加减速控制算法 T梯形加减速控制算法和国外流行的SpTA加减速控制算法,花钱买的,国外流行的SpTA算法比够高效,很适合单片机,强烈推荐。
  • 项目内容包括:s型加减速脉冲输出,数据掉电保存到外部flash,基于freertos系统。
  • 分享简易的步进电机S曲线 程序 ,本代码为查表方式 S曲线的算法可以百度查到,在代码中可以以算式的方式算出频率,也...S曲线的加入极大的减少了电机在加减速时的过冲现象,学习下思想还是很有必要的。 附件内容截图:

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 4,444
精华内容 1,777
关键字:

s型加减速

友情链接: bffe3cc7.rar