CN109176515B - 一种用于机械臂的动态动感模拟算法 - Google Patents
一种用于机械臂的动态动感模拟算法 Download PDFInfo
- Publication number
- CN109176515B CN109176515B CN201811042733.6A CN201811042733A CN109176515B CN 109176515 B CN109176515 B CN 109176515B CN 201811042733 A CN201811042733 A CN 201811042733A CN 109176515 B CN109176515 B CN 109176515B
- Authority
- CN
- China
- Prior art keywords
- motion
- mechanical arm
- dynamic
- algorithm
- limit
- 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.)
- Active
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 35
- 238000005094 computer simulation Methods 0.000 title claims abstract description 15
- 238000004458 analytical method Methods 0.000 claims abstract description 22
- 230000007935 neutral effect Effects 0.000 claims abstract description 15
- 210000000056 organ Anatomy 0.000 claims abstract description 5
- 230000001720 vestibular Effects 0.000 claims abstract description 5
- 230000001133 acceleration Effects 0.000 claims description 24
- 230000006870 function Effects 0.000 claims description 22
- 238000006073 displacement reaction Methods 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 8
- 238000005406 washing Methods 0.000 claims description 7
- 230000035807 sensation Effects 0.000 claims description 3
- 230000003247 decreasing effect Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 6
- 238000000034 method Methods 0.000 description 10
- 230000000875 corresponding effect Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Feedback Control In General (AREA)
Abstract
串联机械臂运动速度快，运动灵活，且运动空间大，因此可以设计出更复杂的轨迹来，是最佳的运动体感的提供装备。本发明提出一种用于机械臂的动态动感模拟算法，所述动态动感模拟算法中包括洗出算法，洗出算法中的参数由模糊控制器和运动空间分析单元计算得出，其中确定的参数包括滤波器的截止频率和比例环节的增益。模糊控制器接收的输入包括动态指令、运动感觉和空间状态，其中运动感觉由人体前庭器官模型结合机械臂的运动状态计算得到，空间状态由运动空间分析单元结合机械臂的运动状态计算得到。运动空间分析单元中使用运动空间分析方法，运动空间分析方法的功能包括确定机械臂运动过程中的中立位置，中立位置包括垂直方向上的坐标。
Description
技术领域
本发明属于机械臂控制领域，尤其是一种用于机械臂的动态动感模拟算法。
背景技术
串联机械臂运动速度快，运动灵活，且运动空间大，因此可以设计出更复杂的轨迹来，是最佳的运动体感的提供装备。串联机械臂与虚拟现实技术相结合，与之前的传统娱乐装备相比，给人更高的沉浸感，更刺激的运动感觉，更真实的体验感受。基于串联机构的运动模拟技术，无论是在航天军用，还是蓬勃发展的文娱产业都有很大应用领域。
洗出算法的设计是串联机械臂控制中一个重要的环节，洗出算法是为了在有限的空间内模拟一次突变运动后，能够迅速回到平衡位置，以便有足够的空间进行下一次运动，经典洗出算法的空间利用率低，基于经典洗出算法控制串联机械臂时，动态复现的逼真度还有待提高。
发明内容
基于此，提出一种用于机械臂的动态动感模拟算法，采用的技术方案如下：
一种用于机械臂的动态动感模拟算法，所述动态动感模拟算法中包括改进洗出算法，改进洗出算法中的参数由模糊控制器和运动空间分析单元计算得出，其中确定的参数包括滤波器的截止频率和比例环节的增益。
进一步的，所述改进洗出算法的输入包括x轴向加速度ax和z轴向加速度az，ax的前向通道为ax依次经过第一比例环节函数、第一比较点、第一高通滤波函数、经两次积分运算后得到输出量，az的前向通道为az依次经过第二比例环节函数、第二比较点、第二高通滤波函数、经两次积分运算后得到输出量，ax经过第一比例环节函数后的引出信号经过第一门限限幅函数后指向第二比较点，az经过第二比例环节函数后的引出信号经过第二门限限幅函数后指向第一比较点。
进一步的，模糊控制器接收的输入包括动态指令、运动感觉和空间状态，其中运动感觉由人体前庭器官模型结合机械臂的运动状态计算得到，空间状态由运动空间分析单元结合机械臂的运动状态计算得到。
进一步的，所述运动状态包括机械臂运动位置极限程度和机械臂运动位置极限程度变化率。
进一步的，所述机械臂运动位置极限程度包括关节角距极限角度的程度和机械臂距极限位移位置的程度。
进一步的，运动空间分析单元中使用运动空间分析方法，运动空间分析方法的功能包括确定机械臂运动过程中的中立位置，中立位置包括垂直方向上的坐标。
进一步的，所述模糊控制器中的模糊规则的制定方法为，当机械臂运动位置远离极限位置时，以减小误差信号为优先选择截止频率，当机械臂运动位置接近极限位置时，以不超过运动极限为优先选择截止频率。
与现有技术相比，本发明的有益效果在于：
1.利用机械臂运动位置极限程度计算出合适的截止频率，当距离极限位置较近时，增大截止频率，减小机械臂的运动幅度，避免超过机械臂的运动范围，当距离极限位置较远时，截止频率，提高机械臂的运动范围，同时利用轨迹规划的思想，对运动空间进行分析，选择合适中立位置，进而更加有效的利用运动空间。
2.在机械臂控制方法中引入人体前庭器官模型，将机械臂的运动状态转化为人体感受到的运动感觉，以此作为与待复现的动感信号相比较，达到在机械臂有较大运动空间的条件下更加逼真的复现运动感觉。
附图说明
图1是动态动感模拟算法结构图；
图2是模糊控制方框图；
图3是改进的洗出算法原理图；
图4是l的梯形隶属函数；
图5是dl的梯形隶属函数；
图6是高通滤波器的截止频率的隶属函数；
图7是xz面内的运动空间示意图；
图8是传统中立位置；
图9是传统中立位置的选择线；
图10是改进后洗出算法中中立位置的选择线。
具体实施方式
如图1至图3所示，本发明提出的动态动感模拟算法包括洗出算法，洗出算法中的参数由模糊控制器和运动空间分析单元计算得出，其中确定的参数包括滤波器的截止频率和比例环节的增益。模糊控制器接收的输入包括动态指令、运动感觉和空间状态，其中运动感觉由人体前庭器官模型结合机械臂的运动状态计算得到，空间状态由运动空间分析单元结合机械臂的运动状态计算得到。
在本实施例中基于5轴机械臂进行公式推导，其中模糊逻辑的输入变量的语言值的模糊子集如下：
输入信号l，运动模拟平台的位置极限程度l，对于串联机械臂指各关节转动角接近极限值的程度，将各关节转动角接近极限值程度最大值作为模糊控制器的输入。由于机械臂各关节运动范围不一致，需要做归一量化处理。如关节角i的运动范围为[θli，θhi]，进行如下处理：
其中θli为关节角下极限，θhi为关节角上极限。将各关节角接近极限程度的描述值l的最大值作为二维模糊逻辑控制器的一个输入信号，l的论域为[0,1]，划分为5个模糊集，分别为A、B、C、D、E。其对应梯度隶属函数如图4所示。
将各关节接近极限程度l的变化率dl作为二维模糊逻辑控制器的另一个输入信号，论域为[-8,8]，划分为5个模糊集，分别为NB，NS，NO，PS，PB。其对应梯度隶属函数如图5所示。
将模糊控制器的输出设为高通滤波器截止频率，它很大程度上影响着洗出信号的逼真度。将模糊控制器的输出划分为5个模糊集，高通滤波器的截止频率论域设为[2,8]，在本实施例中对低通倾斜协调通道进行了角速度的限幅，所以低通滤波器的截止频率设为定制8rad/s。高通滤波器的截止频率的隶属函数如图6所示。
模糊控制规则表如表1所示：
表1
制定模糊规则的基本思想是，当各关节运动角较小时即远离运动空间极限时，有较大的运动空间范围，若误差信号较大时，选择较低的高通滤波器截止频率，较高的低通滤波器截止频率，让更多频率成分的信号通过，若信号误差较小时，可以选择较高一点高通截止频率和较低的低通滤波器截止频率。而当关节转角过大，接近运动范围极限时，选择较高的高通截止频率，较低的低通滤波器截止频率，以防止运动超限。总体来说在不超各关节运动范围极限情况下，以尽可能减小误差信号为优先；而接近运动范围极限时，要以不超过运动范围极限为优先。
模糊控制器确定滤波器的截止频率后，改进洗出算法中的高通滤波器截止频率会随之改变。
本发明提出的动态动感模拟算法包括运动空间分析单元，运动空间分析单元中使用了运动空间分析方法，具体如下：
x向的位移公式如下：
x＝b2c1+l1c1c12-l2c1c23+b3c1s234+b4c1c234 (2)
其中c1表示sinθ1，c2表示cosθ2，c12表示cos(θ1+θ2)，c23表示cos(θ2+θ3)，s234表示sin(θ2+θ3+θ4)，c234表示cos(θ2+θ3+θ4)，θ为关节角角度。
由此x向的位移公式结合机械臂运动时各关节的角度，还可以计算出运动模拟平台x向的位置极限程度l，同理可求得其他运动方向的l值。
若要在运动过程中始终保持座椅的纵向水平，则要使关节角θ2，θ3，θ4的和为常数，即-90°。视关节角θ1＝0，于是x向的位移公式变为：
x＝b2+l1c2+l2c23+b4 (3)
同理可得z向的位移公式为：
z＝b1-l1s2-l2s23+b4 (4)
由以上x、z向的位移公可得纵向空间的位移公式为：
对上式求导可得机械臂末端速度与各关节角度之间的关系：
对上式求导可得机械臂末端的加速度与各关节角加速度之间关系。
在本实施例中，取b1为575mm，b2为300mm，l1为985mm，l2为1025mm，b3为265mm，b4为40mm，θ2的运动范围为[-98.6°,-81.6°]，θ4的运动范围为[-118°,-63°]，θ3＝-90°-(θ2+θ4)，利用MATLAB可得xz面内的运动空间，如图7所示，进行纵向运动空间的数据分析。x的最小值为1223mm，最大值为1734mm；z的最小值为1108mm，最大值为2055mm，若纵向运动时，保持垂向位移为0，则x向的单自由度位移为290mm。约束求解为公式如下：
同理可得，进行垂向运动时，保持纵向位移为0，则垂向的单自由度位移约为940mm，约束求解公式如下：
由上述过程可得如果在模拟纵向运动时，在垂向同时产生相应的运动，此时纵向最大位移约为400mm，此时垂向产生-465mm位移。纵向位移比之前单自由度纵向运动多了37％的空间。但考虑到要利用纵向运动空间模拟的是纵向高频加速度，所以此时人体不应感受到垂向的运动，即z向的加速度不能超过人体的感觉门限参数0.28m/s2。但可以在垂向人感觉门限加速度范围内，来扩大空间的利用。约束求解公式如下：
若给定纵向加速度信号持续1s，各关节在运动范围内运动，由上式可得：
由此可见经过以上的优化求解使得纵向位移可增加至350mm。相对290mm增加了20％的纵向运动空间。
基于此得出了如图3所示的改进洗出算法，即模拟纵向加速度时，在纵向进行纵向加速度相应动作同时，垂向以低于人感觉门限的加速度进行动作，实现方法将给定的纵向加速度信号进行限幅在人体加速度感觉门限内，加到垂向加速度通道上，实现垂向的运动。
同理模拟垂向加速度时，在垂向进行垂向加速度相应动作同时，纵向以低于人感觉门限的加速度(0.17m/s2)进行动作，实现方法将给定的垂向加速度信号进行限幅在人体加速度感觉门限内，加到纵向加速度通道上，实现纵向的运动。
同时基于上述运动空间分析方法还可以得出优化的中立位置选择方法，如图10所示，首先通过运动分析方法选定纵向运动空间的中心(xmid，zmid)，连接中心点和点(xmin，zmax)并延伸至点(xmax，z)，中立位置可在此直线上上进行选取。在机械臂运动过程中中立位置会发生变化，运动空间分析单元计算完新的中立位置后，机械臂x和z通道的增益会随之改变，即改进洗出算法中相应比例环节的参数会随中立位置的变化而变化。
在对机械臂进行空间轨迹规划时，利用了笛卡尔空间直线插补法和多点样条插值运动规划方法。
以上所述仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明创造的保护范围之内。
Claims (6)
1.一种用于机械臂的动态动感模拟算法，其特征在于，所述动态动感模拟算法中包括改进洗出算法，改进洗出算法中的参数由模糊控制器和运动空间分析单元计算得出，其中确定的参数包括滤波器的截止频率和比例环节的增益；
所述改进洗出算法的输入包括x轴向加速度ax和z轴向加速度az，ax的前向通道为ax依次经过第一比例环节函数、第一比较点、第一高通滤波函数、经两次积分运算后得到输出量，az的前向通道为az依次经过第二比例环节函数、第二比较点、第二高通滤波函数、经两次积分运算后得到输出量，ax经过第一比例环节函数后的引出信号经过第一门限限幅函数后指向第二比较点，az经过第二比例环节函数后的引出信号经过第二门限限幅函数后指向第一比较点。
2.如权利要求1所述一种用于机械臂的动态动感模拟算法，其特征在于，模糊控制器接收的输入包括动态指令、运动感觉和空间状态，其中运动感觉由人体前庭器官模型结合机械臂的运动状态计算得到，空间状态由运动空间分析单元结合机械臂的运动状态计算得到。
3.如权利要求2所述一种用于机械臂的动态动感模拟算法，其特征在于，所述运动状态包括机械臂运动位置极限程度和机械臂运动位置极限程度变化率。
4.如权利要求3所述一种用于机械臂的动态动感模拟算法，其特征在于，所述机械臂运动位置极限程度包括关节角距极限角度的程度和机械臂距极限位移位置的程度。
5.如权利要求1所述一种用于机械臂的动态动感模拟算法，其特征在于，运动空间分析单元中使用运动空间分析方法，运动空间分析方法的功能包括确定机械臂运动过程中的中立位置，中立位置包括垂直方向上的坐标。
6.如权利要求1所述一种用于机械臂的动态动感模拟算法，其特征在于，所述模糊控制器中的模糊规则的制定方法为，当机械臂运动位置远离极限位置时，以减小误差信号为优先选择截止频率，当机械臂运动位置接近极限位置时，以不超过运动极限为优先选择截止频率。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811042733.6A CN109176515B (zh) | 2018-09-07 | 2018-09-07 | 一种用于机械臂的动态动感模拟算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811042733.6A CN109176515B (zh) | 2018-09-07 | 2018-09-07 | 一种用于机械臂的动态动感模拟算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109176515A CN109176515A (zh) | 2019-01-11 |
| CN109176515B true CN109176515B (zh) | 2020-10-27 |
Family
ID=64915320
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811042733.6A Active CN109176515B (zh) | 2018-09-07 | 2018-09-07 | 一种用于机械臂的动态动感模拟算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109176515B (zh) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0997176A2 (en) * | 1998-10-28 | 2000-05-03 | Peripherals N.V. | Robotised virtual reality motion simulator-II |
| CN107609263A (zh) * | 2017-09-11 | 2018-01-19 | 了了网络科技（苏州）有限公司 | 一种基于洗出算法的Stewart平台控制方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| AU6406301A (en) * | 2000-06-16 | 2001-12-24 | Kuka Roboter Gmbh | Ride apparatus |
-
2018
- 2018-09-07 CN CN201811042733.6A patent/CN109176515B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0997176A2 (en) * | 1998-10-28 | 2000-05-03 | Peripherals N.V. | Robotised virtual reality motion simulator-II |
| CN107609263A (zh) * | 2017-09-11 | 2018-01-19 | 了了网络科技（苏州）有限公司 | 一种基于洗出算法的Stewart平台控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于并联机构运动范围的自适应洗出算法;杨宇等;《系统工程与电子技术》;20101231;第32卷(第12期);第2716-2720页 * |
| 基于模糊逻辑的体感算法东安逼真度分析;王辉等;《科学技术与工程》;20180430;第18卷(第10期);第250-254页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109176515A (zh) | 2019-01-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Qazani et al. | Adaptive washout filter based on fuzzy logic for a motion simulation platform with consideration of joints’ limitations | |
| Asadi et al. | Robust optimal motion cueing algorithm based on the linear quadratic regulator method and a genetic algorithm | |
| CN108008720B (zh) | 一种轮式移动机器人的模糊滑模轨迹跟踪控制及方法 | |
| Qazani et al. | Adaptive motion cueing algorithm based on fuzzy logic using online dexterity and direction monitoring | |
| Qazani et al. | High-fidelity hexarot simulation-based motion platform using fuzzy incremental controller and model predictive control-based motion cueing algorithm | |
| Qazani et al. | An optimal motion cueing algorithm using the inverse kinematic solution of the hexapod simulation platform | |
| US20110128292A1 (en) | Dynamics-based motion generation apparatus and method | |
| Asadi et al. | An optimal washout filter based on genetic algorithm compensators for improving simulator driver perception | |
| Qazani et al. | A time-varying weight MPC-based motion cueing algorithm for motion simulation platform | |
| CN110377034B (zh) | 一种基于蜻蜓算法优化的水面船轨迹跟踪全局鲁棒滑模控制方法 | |
| Rezende et al. | Constructive time-varying vector fields for robot navigation | |
| Chen et al. | An optimal washout filter design for a motion platform with senseless and angular scaling maneuvers | |
| Salisbury et al. | Optimal motion cueing for race cars | |
| CN112631173A (zh) | 脑控无人平台协同控制系统 | |
| Mauro et al. | Dynamic flight simulation with a 3 dof parallel platform | |
| Qazani et al. | An inverse kinematic-based model predictive motion cueing algorithm for a 6-DoF Gantry-Tau mechanism | |
| CN109176515B (zh) | 一种用于机械臂的动态动感模拟算法 | |
| Asadi et al. | A novel decoupled model predictive control-based motion cueing algorithm for driving simulators | |
| CN114771551A (zh) | 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆 | |
| CN108776432A (zh) | 基于网络的机场跑道检测机器人预测控制方法 | |
| Weiß | Control of a dynamic driving simulator: Time-variant motion cueing algorithms and prepositioning | |
| Rengifo et al. | Impact of human-centered vestibular system model for motion control in a driving simulator | |
| Capustiac et al. | Motion cueing: From design until implementation | |
| CN113419433A (zh) | 一种自平衡电动轮椅欠驱动系统跟踪控制器的设计方法 | |
| Claasen et al. | Tracking and control for handheld surgery tools |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A dynamic motion simulation algorithm for manipulator Effective date of registration: 20211129 Granted publication date: 20201027 Pledgee: Tianjin SME Credit Financing Guarantee Center Pledgor: TIANJIN FUYUN TIANYI TECHNOLOGY Co.,Ltd. Registration number: Y2021120000063 |
|
| PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
| PC01 | Cancellation of the registration of the contract for pledge of patent right | ||
| PC01 | Cancellation of the registration of the contract for pledge of patent right |
Date of cancellation: 20221125 Granted publication date: 20201027 Pledgee: Tianjin SME Credit Financing Guarantee Center Pledgor: TIANJIN FUYUN TIANYI TECHNOLOGY Co.,Ltd. Registration number: Y2021120000063 |
|
| PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A Dynamic Simulation Algorithm for Manipulators Effective date of registration: 20221205 Granted publication date: 20201027 Pledgee: Tianjin SME Credit Financing Guarantee Center Pledgor: TIANJIN FUYUN TIANYI TECHNOLOGY Co.,Ltd. Registration number: Y2022120000070 |
|
| PC01 | Cancellation of the registration of the contract for pledge of patent right | ||
| PC01 | Cancellation of the registration of the contract for pledge of patent right |
Granted publication date: 20201027 Pledgee: Tianjin SME Credit Financing Guarantee Center Pledgor: TIANJIN FUYUN TIANYI TECHNOLOGY Co.,Ltd. Registration number: Y2022120000070 |