CN105068536A - 基于非线性优化方法实现的移动基座轨迹规划器 - Google Patents
基于非线性优化方法实现的移动基座轨迹规划器 Download PDFInfo
- Publication number
- CN105068536A CN105068536A CN201510495076.0A CN201510495076A CN105068536A CN 105068536 A CN105068536 A CN 105068536A CN 201510495076 A CN201510495076 A CN 201510495076A CN 105068536 A CN105068536 A CN 105068536A
- Authority
- CN
- China
- Prior art keywords
- speed
- track
- path point
- nonlinear optimization
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)
Abstract
一种基于非线性优化方法实现的移动基座轨迹规划器，属于通信技术领域。本发明的目的是针对移动基座在任务规划过程中对速度提出的需求，采用非线性优化的方法设计了满足需求的基于非线性优化方法实现的移动基座轨迹规划器。本发明采用三次b样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹。本发明讲整个轨迹规划算法封装成标准的输入输出函数，下载到FPGA中运行，构成轨迹规划器，方便了用户的使用。
Description
技术领域
本发明属于通信技术领域。
背景技术
通常，移动基座在工业、港口、行星表面探测、核废料区清理、采矿等方面的应用，其任务规划为期望的路径点。随着科学技术的发展,移动基座的应用越来越广泛,对移动基座完成任务的复杂程度要求也越来越高,比如在航天器轨道模拟中的应用，对其任务规划为速度依赖的路径点。由于传统的应用中，对速度没有需求，速度由规划算法间接决定。但对于新的应用，由于速度由任务规划给定，必须直接对速度进行规划。由于传统的轨迹规划算法没有对速度进行直接规划，已无法满足需求。主要存在以下问题：
1，直线段加抛物线轨迹规划方法：在路径点之间通过直线相连,并在连接点处通过曲线过渡。生成的轨迹无法满足曲率连续，但由于转向机构无法瞬间改变曲率，这样会给轨迹跟踪带来很大的误差。
2，采用多项式进行轨迹规划的方法，由于其多项式阶次会随路径点个数的增加而增加，无法满足需求。
3，采用样条进行轨迹规划的方法，由于无法直接对速度进行规划或者规划过程中优化变量过多，无法满足复杂任务对实时性的需求。
目前为了解决移动基座轨迹规划所遇到的困难，许多专家已经提出了许多成型的方法：
中国专利公开号CNIO1508113A，公开日为2014年3月19日，专利申请号为200910071524.9。专利申请名称为“一种基于余弦二阶的机器人轨迹规划方法“。该专利首先给定基于余弦的二阶轨迹曲线的位置、速度、加速度以及加速度的导数的数学模型,给定两个期望点的位置和速度。然后,将边界条件的值代入所述数学模型中,列出方程组并求解出模型的参数。最后,根据两个期望点间的位置和速度的关系及加速度和加速度的导数的幅值来限制加速度的导数的阀值。确定最终的规划轨迹。但该方法仅针对起点和终点进行轨迹规划，无法满足多路径点的需求。
中国专利公开号CNIO3645725A，公开日为2009年8月19日，专利申请号为201310745783.1，专利申请名称为“一种机器人示教轨迹规划方法和***”。该专利描述了一种机器人示教轨迹规划方法和***,涉及工业过程中的机器人示教领域,其包括:在对机器人进行示教过程中,采集示教轨迹的空间关键点;根据示教轨迹的空间关键点,用多结点样条插值函数以及最小二乘拟合方法,得到示教轨迹曲线。但该方法未考虑执行机构存在的约束，对复杂任务，无法满足需求。
中国专利公开号申请公布号CNIO2794767A，公开日为2012年11月28日，专利申请号为201210319744.0，专利申请名称为“视觉引导的机器人关节空间B样条轨迹规划方法”。本专利涉及一种视觉引导的机器人关节空间B样条轨迹规划方法,其包括如下步骤:第一步、在立体支架内安装二自由度机器人,在前端安装工业相机,传送带的运动方向垂直于二自由度机器人的运动平面;第二步、在工业相机获取到传送带上第一个轨迹点后,并在二自由度机器人运动到第一个轨迹点的时间内,根据得到若干关节时间节点序列,以构造B样条曲线;第三步、采用增加节点向量和控制顶点的方式来延长上述构造的B样条曲线,以使得B样条曲线经过增加的关节位置点;第四步、采用德布尔递推算法计算B样条曲线上的位置点,以驱动二自由度机器人运动。该专利无法对速度进行规划，无法满足特定任务对速度的需求。
发明内容
本发明的目的是针对移动基座在任务规划过程中对速度提出的需求，采用非线性优化的方法设计了满足需求的基于非线性优化方法实现的移动基座轨迹规划器。
本发明采用三次b样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹；
设任务输出为路径点：
①确定速度的方向：将所有路径点用线段连接起来，各点速度分别在x轴和y轴方向投影分解为，这样就将路径点变换为；
②利用B样条对轨迹进行参数化：将路径点由表示,对应的速度由表示；其中.设待求的控制点由表示
a、确定各路径点对应时间参数值：首先将全部运动时间归一化到[0,1].即
（1）
（2）
b、确定节点向量
c、对进行参数化:利用节点向量,和控制点生成参数化表达式
（3）
u为运动时间t归一化后的参数值；
是次B样条基函数,其具体表达式由下式给出：
（4）
（5）
（6）
其中；
d、根据给定的路径点可列以下方程组：
（7）
其中，即：
（8）
令：
（9）
则：（10）
由于对参数值进行了归一化，对应其参数点实际速度为
（11）
则：（12）
即：（13）
由此可得控制点可以由参数表示，即：
（14）
将该控制点代入公式(3)，则x方向和y方向的轨迹均可由参数和归一化后的时间参数值u表示，即：
（15）；
③约束
运动基座存在以下物理约束：
速度约束：（16）
其中：（17）
加速度约束：（18）
其中（19）；
④非线性优化求解满足约束的轨迹：
将轨迹规划问题转化为以下非线性优化问题，选择最小能量优化问题:、（20）
subjectto
（21）
将做为优化变量求解该非线性优化问题。
本发明在FPGA中的应用，将整个算法封装成标准的输入输出函数，下载到FPGA中运行，形成轨迹规划器。
本发明有益效果是：
本发明仅采用三次分段多项式对轨迹进行参数化，能够直接对速度进行规划，且满足曲率连续，大大降低了处理器的计算负担。且采用非线性优化方法对轨迹进行优化，能够处理速度和加速度约束。
本发明在进行非线性优化的过程中，仅采用一个优化变量，大大降低了优化的时间，满足实时性需求。
本发明讲整个轨迹规划算法封装成标准的输入输出函数，下载到FPGA中运行，构成轨迹规划器，方便了用户的使用。
附图说明
图1.为速度方向求解图；
图2.为规划的x和y关系图；
图3.为规划的速度曲线；
图4.为规划的x随时间变化曲线；
图5.规划的y随时间变化曲线；
图6.规划的加速度曲线。
具体实施方式
采用三次b样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹；
设任务输出为路径点：
①首先确定速度的方向：将所有路径点用线段连接起来，第一个和最后一个路径点的速度方向分别由第一条线段和最后一条线段的方向决定。其他各点的速度方向由其对应的角平分线的垂线方向决定。最后将各点速度分别在x轴和y轴方向投影分解为，这样就将路径点变换为；
②利用B样条对轨迹进行参数化：将路径点由表示,对应的速度由表示；其中.设待求的控制点由表示
a、确定各路径点对应时间参数值：首先将全部运动时间归一化到[0,1].即
（1）
（2）
b、确定节点向量
:
c、对进行参数化:利用节点向量,和控制点生成参数化表达式
（3）
u为运动时间t归一化后的参数值；
是次B样条基函数,其具体表达式由下式给出：
（4）
（5）
（6）
其中；
d、根据给定的路径点可列以下方程组：
（7）
其中，即：
（8）
令：
（9）
则：（10）
由于对参数值进行了归一化，对应其参数点实际速度为
（11）
则：（12）
即：（13）
对于公式(13)中的矩阵已根据公式（９）求出，矩阵为已知量，由此可得控制点可以由参数表示，即：
（14）
由于均包含x方向和y方向的位置和速度，控制点包含x方向和y方向的控制点。将该控制点代入公式(3)，则x方向和y方向的轨迹均可由参数和归一化后的时间参数值u表示，即：
（15）；
③约束
运动基座存在以下物理约束：
速度约束：（16）
其中：（17）
加速度约束：（18）
其中（19）；
④非线性优化求解满足约束的轨迹：
将轨迹规划问题转化为以下非线性优化问题，选择最小能量优化问题:、（20）
subjectto
（21）
将做为优化变量求解该非线性优化问题。
根据求解出的即可以根据求解出完成该运动对应的时间节点向量。根据和可以求解出每段时间节点对应的三次多项式系数。
本发明在FPGA中的应用，从工程实现的角度，将整个算法封装成标准的输入输出函数，下载到FPGA中运行，形成轨迹规划器。规划器的输入为速度依赖的路径点，具体的输入数据格式如图1.输出为完成整个运动所需的时间节点，以及每段时间内的三次多项式系数，具体的输出数据格式如图2.所有数据通过串口进行传输。
串口通信的数据格式为：
表1输入数据格式
表2输出数据格式
表1代表输入数据格式，帧头为1,9,35,66,96.当接收到的数据帧前五个数据为1,9,35,66,96，则认为该帧数据有效，否则，认为该帧数据无效。路径点个数代表给定的路径点数目。数据代表具体的路径点。校验位采用奇校验，保证数据的有效性。表2代表输出数据格式，帧头为1,9,35,96,66，以供接收设备识别该数据。时间节点值代表完成整个运动对应的时间节点。每段时间节点内多项式系数代表规划的轨迹在每段时间节点内的多项式系数。校验位采用奇校验，保证数据的有效性。
本方面以下5点为例,对该方法进行仿真验证。
路径点：(0,0,1）,(5,3,1）,(8,-1,1.35),(11.5,-3,0.5),(15,1.5,0)
约束
以上各路径点前两项代表ｘ，ｙ坐标，最后一项代表速度，均为随机选取。对应的约束分别由实验室加工的移动基座实际最大速度和加速度获得,可以根据具体基座的约束进行修改。
根据以上算法进行轨迹规划，规划结果如图2-图6。
图2和图3分别代表根据以上算法规划得到的x,y坐标随时间变化的曲线。图4中的*点代表给定的路径点包含的位置，该图中的曲线根据规划的x,y曲线画出，由该图可以看出规划的轨迹能够精确通过给定的位置。图5中的*点代表给定的路径点包含的速度，该图中的曲线为规划的速度曲线随时间变化曲线，由该图可以看出规划的轨迹能够满足给定的速度需求。图5和图6分别代表速度和加速度随时间变化曲线，满足速度和加速度约束。由以上的规划结果可以看出，规划的曲线可以精确的的经过路径点，且该轨迹规划算法能够直接对速度进行规划，生成的轨迹曲率连续，满足速度和加速度约束。
Claims (2)
1.一种基于非线性优化方法实现的移动基座轨迹规划器，其特征在于：采用三次b样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹；
设任务输出为路径点：
①确定速度的方向：将所有路径点用线段连接起来，各点速度分别在x轴和y轴方向投影分解为，这样就将路径点变换为；
②利用B样条对轨迹进行参数化：将路径点由表示,对应的速度由表示；其中.设待求的控制点由表示
a、确定各路径点对应时间参数值：首先将全部运动时间归一化到[0,1].即
（1）
（2）
b、确定节点向量
:
c、对进行参数化:利用节点向量,和控制点生成参数化表达式
（3）
u为运动时间t归一化后的参数值；
是次B样条基函数,其具体表达式由下式给出：
（4）
（5）
（6）
其中；
d、根据给定的路径点可列以下方程组：
（7）
其中，即：
（8）
令：
（9）
则：（10）
由于对参数值进行了归一化，对应其参数点实际速度为
（11）
则：（12）
即：（13）
由此可得控制点可以由参数表示，即：
（14）
将该控制点代入公式(3)，则x方向和y方向的轨迹均可由参数和归一化后的时间参数值u表示，即：
（15）；
③约束
运动基座存在以下物理约束：
速度约束：（16）
其中：（17）
加速度约束：（18）
其中（19）；
④非线性优化求解满足约束的轨迹：
将轨迹规划问题转化为以下非线性优化问题，选择最小能量优化问题:、（20）
subjectto
（21）
将做为优化变量求解该非线性优化问题。
2.权利要求1所述的基于非线性优化方法实现的移动基座轨迹规划器在FPGA中的应用，其特征在于：将整个算法封装成标准的输入输出函数，下载到FPGA中运行，形成轨迹规划器。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510495076.0A CN105068536B (zh) | 2015-08-13 | 2015-08-13 | 基于非线性优化方法实现的移动基座轨迹规划器 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510495076.0A CN105068536B (zh) | 2015-08-13 | 2015-08-13 | 基于非线性优化方法实现的移动基座轨迹规划器 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105068536A true CN105068536A (zh) | 2015-11-18 |
| CN105068536B CN105068536B (zh) | 2017-12-15 |
Family
ID=54497926
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510495076.0A Active CN105068536B (zh) | 2015-08-13 | 2015-08-13 | 基于非线性优化方法实现的移动基座轨迹规划器 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105068536B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106802625A (zh) * | 2017-03-14 | 2017-06-06 | 成都工业学院 | 一种导数多维空间机床轨迹运动再现方法 |
| CN108717265A (zh) * | 2018-05-30 | 2018-10-30 | 重庆邮电大学 | 一种基于控制变量参数化的无人飞行器巡航跟踪控制***及控制方法 |
| CN109795043A (zh) * | 2019-03-11 | 2019-05-24 | 江阴久盛科技有限公司 | 多槽一次成型金刚线导轮开槽方法 |
| CN110865657A (zh) * | 2019-12-03 | 2020-03-06 | 泉州华中科技大学智能制造研究院 | 一种传送带上轮廓轨迹跟踪控制***及方法 |
| CN112356036A (zh) * | 2020-11-23 | 2021-02-12 | 北京配天技术有限公司 | 一种scara机器人轨迹规划方法、装置、设备及存储介质 |
| CN112405537A (zh) * | 2020-11-11 | 2021-02-26 | 配天机器人技术有限公司 | 一种机器人空间轨迹插补方法及机器人 |
| CN116141341A (zh) * | 2023-04-21 | 2023-05-23 | 之江实验室 | 满足笛卡尔空间约束的五自由度机械臂指向动作实现方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101508113A (zh) * | 2009-03-11 | 2009-08-19 | 哈尔滨工业大学 | 一种基于余弦二阶的机器人轨迹规划方法 |
| CN102794767A (zh) * | 2012-08-31 | 2012-11-28 | 江南大学 | 视觉引导的机器人关节空间b样条轨迹规划方法 |
| CN104020665A (zh) * | 2014-06-25 | 2014-09-03 | 北京邮电大学 | 基于多目标粒子群算法的机械臂最小跃度轨迹优化方法 |
| CN104062902A (zh) * | 2014-05-15 | 2014-09-24 | 江南大学 | Delta机器人时间最优轨迹规划方法 |
-
2015
- 2015-08-13 CN CN201510495076.0A patent/CN105068536B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101508113A (zh) * | 2009-03-11 | 2009-08-19 | 哈尔滨工业大学 | 一种基于余弦二阶的机器人轨迹规划方法 |
| CN102794767A (zh) * | 2012-08-31 | 2012-11-28 | 江南大学 | 视觉引导的机器人关节空间b样条轨迹规划方法 |
| CN104062902A (zh) * | 2014-05-15 | 2014-09-24 | 江南大学 | Delta机器人时间最优轨迹规划方法 |
| CN104020665A (zh) * | 2014-06-25 | 2014-09-03 | 北京邮电大学 | 基于多目标粒子群算法的机械臂最小跃度轨迹优化方法 |
Non-Patent Citations (4)
| Title |
|---|
| FIELD G.，等: "Iterative dynamic programming: an approach to minimum energy trajectory planning for robotic manipulators", 《PROCEEDINGS OF THE IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
| HUANG M S，等: "Minimum-energy point-to-point trajectory planning for a motor-toggle servomechanism", 《MECHATRONICS, IEEE/ASME TRANSACTIONS ON》 * |
| 张红强，等: "机器人关节空间B样条轨迹设计的混沌优化", 《电机与控制学报》 * |
| 钱东海，等: "基于B样条路径的机器人时间最优轨迹规划", 《上海交通大学学报》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106802625A (zh) * | 2017-03-14 | 2017-06-06 | 成都工业学院 | 一种导数多维空间机床轨迹运动再现方法 |
| CN108717265A (zh) * | 2018-05-30 | 2018-10-30 | 重庆邮电大学 | 一种基于控制变量参数化的无人飞行器巡航跟踪控制***及控制方法 |
| CN108717265B (zh) * | 2018-05-30 | 2021-05-18 | 重庆邮电大学 | 一种基于控制变量参数化的无人飞行器巡航跟踪控制***及控制方法 |
| CN109795043A (zh) * | 2019-03-11 | 2019-05-24 | 江阴久盛科技有限公司 | 多槽一次成型金刚线导轮开槽方法 |
| CN110865657A (zh) * | 2019-12-03 | 2020-03-06 | 泉州华中科技大学智能制造研究院 | 一种传送带上轮廓轨迹跟踪控制***及方法 |
| CN112405537A (zh) * | 2020-11-11 | 2021-02-26 | 配天机器人技术有限公司 | 一种机器人空间轨迹插补方法及机器人 |
| CN112356036A (zh) * | 2020-11-23 | 2021-02-12 | 北京配天技术有限公司 | 一种scara机器人轨迹规划方法、装置、设备及存储介质 |
| CN116141341A (zh) * | 2023-04-21 | 2023-05-23 | 之江实验室 | 满足笛卡尔空间约束的五自由度机械臂指向动作实现方法 |
| CN116141341B (zh) * | 2023-04-21 | 2023-08-08 | 之江实验室 | 满足笛卡尔空间约束的五自由度机械臂指向动作实现方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105068536B (zh) | 2017-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105068536A (zh) | 基于非线性优化方法实现的移动基座轨迹规划器 | |
| CN106945041B (zh) | 一种冗余度机械臂重复运动规划方法 | |
| CN106826829A (zh) | 一种可控误差的工业机器人光顺运动轨迹生成方法 | |
| Rutkowski et al. | Novel online speed profile generation for industrial machine tool based on flexible neuro-fuzzy approximation | |
| CN104942808A (zh) | 机器人运动路径离线编程方法及*** | |
| CN107127755A (zh) | 一种三维点云的实时采集装置及机器人打磨轨迹规划方法 | |
| CN103085069B (zh) | 一种新型机器人运动学建模方法 | |
| CN103386683A (zh) | 一种基于Kinect的体感控制机械臂方法 | |
| Rezende et al. | Constructive time-varying vector fields for robot navigation | |
| CN106346480B (zh) | 一种基于ug和matlab的多自由度注塑机械臂建模方法 | |
| CN109683552B (zh) | 一种基面曲线导向的复杂点云模型上的数控加工路径生成方法 | |
| CN110744552A (zh) | 一种基于奇异摄动理论的柔性机械臂运动控制方法 | |
| Ruchanurucks et al. | Humanoid robot motion generation with sequential physical constraints | |
| CN115179306A (zh) | 一种复杂木模工业机器人铣削及控制方法 | |
| CN206825428U (zh) | 一种三维点云的实时采集装置 | |
| CN106338966B (zh) | 一种工业机器人轨迹规划编程方法 | |
| Ramer et al. | A robot motion planner for 6-DOF industrial robots based on the cell decomposition of the workspace | |
| CN109278041B (zh) | 一种过渡误差可控的机械臂直线轨迹自适应衔接规划方法 | |
| CN102393680B (zh) | 一种基于圆锥面刀具矢量插值的参数曲线插补方法 | |
| Zou et al. | A novel robot trajectory planning algorithm based on NURBS velocity adaptive interpolation | |
| CN110435813B (zh) | 一种基于动力定位***的手操杆调校方法 | |
| CN110370276B (zh) | 基于三维模型Morse分解的工业机器人加工轨迹自动规划方法 | |
| Lee et al. | Faster and smoother trajectory generation considering physical system limits under discontinuously assigned target angles | |
| Al-Shuka et al. | Adaptive hybrid regressor and approximation control of robotic manipulators in constrained space | |
| CN104536381A (zh) | 一种用于对多轴运动控制***测量轮廓误差的***及方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |