基于Simscape的机器人速度控制模式下的轨迹控制

          速度控制是运动控制中的其中一种，因此，首先讲一下运动控制：

运动控制就是对机械运动部件的位置、速度等进行实时的控制管理，使其按照预期的运动轨迹和规定的运动参数进行运动。

运动控制器：指以中央逻辑控制单元为核心、以传感器为信号敏感元件、以电机或动力装置和执行单元为控制对象的一种控制装置。

那么，控制其实就是对机器人关节电机的控制。

以UR机器人为例：

连续轨迹控制 (CP)
该控制模式实现了对机器人终端执行机构在工作空间的连续控制。它要求在一定精度范围内严格按照预定的轨迹和速度运动。并且速度可控，运动轨迹流畅，完成任务。在机器人中，关节的连续同步运动可以由末端执行机构构成连续轨迹。

Simscape中的机器人旋转关节控制输入设为motion，如下图所示：

motion是由输入信号进行提供的，我们需要Simulink-PS转换器将Simulink输入信号转换为物理信号

Simulink-PS转换器将Simulink输入信号转换为物理信号。输入信号单元参数中的单元表达式与Simulink输入信号相关联，并确定分配给物理信号的单元。“应用仿射转换”复选框只适用于有偏移量的单位(例如温度单位)。如果选择的求解器需要输入导数，您可以通过额外的信号端口显式地提供它们，或者打开输入滤波来计算时间导数。一阶滤波器提供一个导数，而二阶滤波器提供一阶和二阶导数。对于分段常数信号，还可以显式地将输入导数设为零。

————————————————————————

速度控制，简言之，就是机器人运动控制，给定机器人关节运动参考值，例如位置，速度等。

以机器人参考速度作为关节参考运动输入，那么要获得机器人想要运动到笛卡尔空间任务轨迹所需的关节运动输入值就显得尤为重要。

数值模拟关键步骤：

末端速度乘Jacobian的逆就是关节速度，根据V=J*dq，可以得到笛卡尔末端速度和各个关节输入期望速度之间的关系。因为控制量是关节空间的，所以，我们需要把末端任务空间轨迹速度转换为关节空间所需速度信号。

例如：机器人末端画圆，那么首先就是要规划好一个末端的圆形轨迹，然后需要得到运动圆轨迹的速度信息，即需要得到画圆轨迹所需的线速度和角速度信息。然后末端速度乘Jacobian的逆就是关节速度

计算空间圆的速度（线速度和角速度），几何Jacobian矩阵对应刚体固连速度。雅克比矩阵可以联系机器人末端操作空间速度和关节空间速度，已知关节空间速度，利用雅克比矩阵，可以得到操作空间的速度，再结合逆过程，就可以实现速度模式控制机器人。因此，最重要的就是得到空间圆轨迹的线速度和角速度。

采用Matlab 机器人工具箱中的函数可以得到雅克比矩阵：

jacob0()求解的是将关节速度映射到世界坐标系中的末端执行器空间速度；

jaconb()求解的是将关节速度映射到工具坐标系中的末端执行器空间速度。

给他们输入的就是关节期望运动信息。

若机器人在笛卡尔空间的速度已知则可以计算出沿着这个路径所需要的关节速度。

xd和yd画出圆曲线，z=0.1指出圆的z轴位置。然后xyz通过齐次矩阵进行描述，包含姿态变化。

注意：线速度是相对于一点的，角速度是相对于物体的！

匀速圆周运动 1.线速度V=s/t=2πR/T 2.角速度ω=Φ/t=2π/T=2πf 

1、v（线速度）=ΔS/Δt=2πr/T=ωr=2πrf(S代表弧长，t代表时间，r代表半径,f代表频率)

2、ω（角速度）=Δθ/Δt=2π/T=2πn（θ表示角度或者弧度）

3、T（周期）=2πr/v=2π/ω

物体上任一点对定轴作圆周运动时的速度称为“线速度”(linear velocity)。它的一般定义是质点（或物体上各点）作曲线运动（包括圆周运动）时所具有的即时速度。

在Δt时间内转过的角为Δθ. Δθ与Δt的比值，描述了物体绕圆心运动的快慢，这个比值叫做角速度，用符号ω表示：

v=ω×r

三维空间角速度的定义：三维空间的角速度是一个向量，向量的方向与旋转的转轴方向重合，向量的模长代表着角速度的大小。

模型试运行：

如有错误或更好的方案，欢迎大家指出问题集思广益。

数据结果可视化页面如下：

问题：上述的的期望轨迹为平面内的圆，意思是认为机器人末端画圆过程中的姿态保持不变，仅xy平面内的轨迹按照圆轨迹运行。

考虑姿态的空间圆轨迹描述，xyz以及绕三个轴的转动姿态为空间圆的一般情况。

角速度矩阵如下：

其中，S矩阵为：

末端空间圆轨迹的描述：

T为齐次变换矩阵，该矩阵包含位置信息和姿态信息。

T=[1 0 0 r*cos(s(i));0 1 0 r*sin(s(i));0 0 1 0;0 0 0 1]

方法：直接给期望速度加上一个随机扰动（模拟速度控制误差）

目的：速度误差对轨迹控制精度的影响，例如，是否存在累积。或速度误差对轨迹跟踪精度的影响（定量）

常见表述方法：

采用S = skew(V)命令可以获得求斜对称矩阵。

if V (1x3) then S =
 
            |  0  -vz   vy |
            | vz    0  -vx |
            |-vy   vx    0 |

vex具有求逆解的功能，转化为向量。

计算空间圆的速度（线速度和角速度），几何Jacobian矩阵对应刚体固连速度

由于几何空间速度和刚体速度的区别，Jacobian不同，因此，需要进行不同雅克比矩阵的求解。即利用雅克比求解几何空间速度（线速度，角速度），利用机器人中的雅克比矩阵求解关节空间速度！！！

参考文献：

【1】https://baijiahao.baidu.com/s?id=1721528072411624522&wfr=spider&for=pc

【2】http://www.mat-test.com/Post/Details/PT220823000005hOkQn

【3】设计机器人机械手算法 - 文件交换 - MATLAB 中心 (mathworks.cn)

【4】雅克比参考：https://www.renrendoc.com/paper/109961768.html

【5】https://www.guyuehome.com/41600

【6】https://blog.csdn.net/huangjunsheng123/article/details/110118813?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167184948116800182159655%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=167184948116800182159655&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~pc_rank_34-24-110118813-null-null.142^v68^pc_new_rank,201^v4^add_ask,213^v2^t3_control1&utm_term=%E9%9B%85%E5%85%8B%E6%AF%94&spm=1018.2226.3001.4187

【7】https://www.renrendoc.com/paper/109961768.html