一种基于FOC和MPC算法的触控手套力传感方法

一种基于foc和mpc算法的触控手套力传感方法
技术领域
1.本发明涉及虚拟现实技术应用技术领域，具体地的说是一种基于foc和mpc算法的触控手套力传感方法。


背景技术：

2.vr技术的核心用户体验由三部分组成：沉浸感、流畅度、可交互度，为了提高vr技术的用户体验，诞生了许多交互设备，其中触控手套就是一种使人类能够连接虚拟世界与现实世界的设备，使人类在现实世界中感受虚拟世界中物体的握感。当用户在vr中触摸虚拟物体时，触控手套会追踪用户的移动并以相反的方向反馈用户，仿佛触摸真实物理对象被弹回来一样，这时用于触控手套的力传感方法是否能够快速精准的实现力传感反馈就显得尤为重要。现有的用于触控手套的力传感方法大多采用的是传统foc算法，大多在传统foc控制算法中都是采用pid感应电机电流调节器的方法实现电流环控制，采用扩展卡尔曼滤波模块进行角度环和速度环控制，并且在算法的verilog实现部分采用的是查表法或调用rom ip核来实现正弦余弦的计算。
3.现有技术的力传感方法在控制精准、快速方面皆有欠缺，不仅稳定性不高，易受环境影响，制作成本较高并且不利于集成为foc算法芯片。


技术实现要素：

4.本发明的目的是针对现有技术的不足而提供的一种基于foc和mpc算法的触控手套力传感方法，采用烧录foc和mpc控制算法的fpga芯片，将其安装在触控手套上与mcu单片机、电机，以及虚拟世界中的上位机，共同构成基于foc和mpc算法的触控手套系统，测试算法时候，将电机放置在手背的手指处，通过传动杆与链接线，对手指进行力传感的反馈，算法模块通过spi接口接收mcu从上位机获得的拟世界中不同物体的不同的力传感反馈数据并进行参数配置，foc和mpc算法通过输出三相pwm波来控制电机，实现对手指处力传感反馈的精确控制，算法将接收mcu发来的力传感反馈数据经过mpc感应电机电流调节器、反park变换、svpwm、clark变换和park变换，最后将生成的电流向量用作foc和mpc算法的电流环反馈输入mpc模块，完成电流环的闭环计算，扩展卡尔曼滤波模块用于接收电机当前的角度和速度，计算后将输出的结果输入mpc算法模块完成角度环和速度环的闭环计算，该方法控制准确、快速、稳定，提高了foc算法的动态响应和参数鲁棒性，易于烧录在在fpga芯片或集成为foc和mpc控制芯片，安装到触控手套上使之重量轻、制作成本低、结构简单，使用者在现实世界中能够更加快速、准确、真实的感受到虚拟世界中物体的触感，算法能够更及时的提供反馈信息，用户则可以在虚拟世界中收获沉浸式体验，有效减小虚拟世界与现实世界中的感知差距，是一种理想的用于vr虚拟设备的力传感方法，具有很好的应用前景和开发市场。
5.实现本发明目的的具体技术方案是：一种基于foc和mpc算法的触控手套力传感方法，其特点是首先构建触控单指套模型；然后构建foc和mpc控制算法模型，包括：电流mpc计
算模块、反park变换模块、clark变换模块、park变换模块、svpwm生成模块以及扩展卡尔曼滤波模块；最后将foc和mpc控制算法模型用于单指触控手套的力传感模块验证算法的准确性，确保此算法可以扩展至完整的触控手套上。
6.所述触控单指套模型由传动杆、转轴、链接绳、电机、mcu单片机和烧录foc和mpc算法的fpga芯片(或集成foc和mpc算法的fpga芯片)组成，上位机采集的力反馈数据输入mcu单片机，fpga芯片中的foc和mpc算法模块通过spi接口接收mcu单片机传递的参数并进行参数配置；fpga芯片将输入的电机参数通过foc和mpc算法输出pwm波，通过三相全桥电路和信号线控制电机的运转，通过传动杆与线绳，实现对触控单指套上的手指处力传感反馈的精确控制，使用单指触控手套的用户可以明显感受到虚拟到现实的力的传感；
7.所述上位机装有vr软件，用于将现实中采集的虚拟世界中的物体的力传感数据与虚拟世界中的物体的力传感数据相对应，并将虚拟世界中物体的力传感参数传至mcu单片机。
8.所述烧录或集成foc和mpc算法的fpga芯片中的foc和mpc算法通过spi接口从mcu单片机接收数据，foc和mpc算法需要配置电机角度、电阻和电感、电机的磁通量等参数，在使用的电机不同时传入不同的参数，同时mcu单片机需要把上位机传输的虚拟世界中物体的参数发送给fpga芯片，作为foc和mpc算法所需要的控制量。
9.所述烧录或集成foc和mpc算法的fpga芯片中的foc和mpc算法能够进行更快速准确的运算，相比较于常见的传统foc算法，foc和mpc算法具有动态响应和参数鲁棒性更高的优点，并且相较于传统的foc算法，foc和mpc算法用mpc算法代替传统pid算法实现电流环控制，用扩展卡尔曼滤波模块替代了传统的传感器，用cordic算法替代在算法verilog实现过程中采用的查表法或调用rom ip的方法，在计算完成后输出三相pwm波来控制电机。
10.所述foc和mpc算法具体包含了mpc算法模块、park变换模块、反park变换模块、clark变换模块、svpwm生成模块以及扩展卡尔曼滤波模块，foc和mpc算法主要用于对电机进行像素级的控制，使后期将算法运用于触控手套时能呈现更精确更快速的力反馈。foc和mpc算法对电机的控制能够使电机以更平稳光滑的方式进行旋转，电机运动的改善使手感更加真实，相较于其他传统的电机控制方法或foc和pid算法的控制，具有更快的系统动态响应和参数鲁棒性，改善电机旋转卡顿的现象，当虚拟世界物体变化时，力传感也能精准变化，感受更加真实。
11.所述foc和mpc算法中各个模块用于完成无感foc和mpc算法的闭环计算，其中三个mpc算法模块应用于foc速度环，位置环和电流环的闭环控制，foc和mpc算法模块通过spi接口接收到mcu发来的数据，将电机当前的状态与输入的目标状态通过mpc算法训练迭代修正电机的状态；所述反park变换模块用于将mpc算法模块输出的基于dq坐标系的控制电压向量转化到αβ坐标轴方向；所述svpwm生成模块用于将反park变换模块生成的在αβ坐标轴方向上的电压向量转化成控制电机的svpwm波；所述clark变换模块用于将采样得到的电机运转时的三相电流转化为两相电流；所述park变换模块用于将clark变换模块得到的两相电流从αβ坐标轴方向转换到dq坐标系中，生成的电流向量将用作foc和mpc算法的电流环反馈输入mpc模块，完成电流环的闭环计算；所述扩展卡尔曼滤波模块用于检测电机当前的角度和速度，将clark变换模块输出的电流向量作为测量，将用于计算电流环的mpc算法模块生成的电压控制向量作为控制量输入，输出电机的运行速度和电机当前的角度，将输出的结
果输入mpc算法模块用于进行角度环和速度环的闭环计算。
12.所述mpc算法具体实现步骤如下：
13.步骤一：在转子磁场定向的同步旋转坐标系(d-q坐标系)中，异步电机的定子电压方程可以写成下述(a)式定义的方程组：
[0014][0015]
式中：rs为定子电阻；lm为励磁电感；ls、lr为定子和转子电感；l
σ
＝σls为总漏感；ω
δ
为同步角速度；为转子磁链幅值；i
sq
、i
sd
为d轴和q轴的定子电流；u
sd
、u
sq
为d轴和q轴的定子电压；p为微分因子
[0016]
步骤二：为了减少模型预测控制算法的复杂度，可以将二维的电流方程简化为两个单输入单输出(siso)系统。将d轴和q轴之间的交叉耦合项等看成扰动，得到下述(b)式表示电流控制的对象方程组：
[0017][0018]
步骤三：将交叉耦合项看作由下述(c)式表示电流控制的前馈补偿量：
[0019][0020]
步骤四：以d轴为例，其状态方程可以由下述(d)使表示为：
[0021][0022]
其中，为对i
sd
进行微分运算。
[0023]
步骤五：将上述(d)式按照一阶欧拉法离散化，ts为采样时间，d，q轴同理，得到下述(e)～(i)式：
[0024][0025][0026]isd
(k+1)＝ai
sd
(k)+ bu
sd
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(g)；
[0027][0028]isq
(k+1)＝ai
sq
(k)+bu
sq
(i)。
[0029]
步骤六：分别用上述的mpc算法对d轴和q轴的电流建立控制模型。由于d轴和q轴的电流状态方程类似，因此可以采用模块化的编程方式，采用共用的代码来实现d轴和q轴电流的模型预测控制。采用mpc算法可以得到最优d轴q轴的控制量。为了提高控制性能，可以在mpc得到的控制量上面再进行前馈补偿。
[0030]
所述foc和mpc算法中需使用正弦余弦变换成verilog代码时，将传统的查表法或调用rom ip替换成为cordic算法求sin函数和cos函数。这对于集成为foc和mpc算法芯片是
极为重要的，因为在综合布线时，查表法综合完是相互穿插类似于路由结构走线更难。这对后端工具来说是非常麻烦的。后端工具对于绕线没有那么智能，如果绕不开就给他更大的面积，不断的推开(波纹算法)，直到大到足够绕开位置。若调用rom ip开发成本又会变高，而cordic算法是一个化繁为简的算法，将许多复杂的运算转化为一种仅需要移位和加法的迭代操作。cordic算法有旋转和向量两个模式，分别可以在圆坐标系、线性坐标系和双曲线坐标系使用，cordic算法涉及3种坐标系、2种模式，共计6个组合，其中在圆周坐标系下利用旋转模式实现sin和cos，使综合成集成foc和mpc算法芯片时布线更加合理。
[0031]
所述cordic算法的旋转模式具体步骤如下：
[0032]
步骤一：设置迭代次数为16，则x0＝0.607253，y0＝0，并输入待计算的角度θ，θ在[-99.7
°
，99.7
°
]范围内。
[0033]
步骤二：根据下述(j)～(l)三个迭代公式进行迭代：i从0至15；
[0034]
xi＝(x
i-1-y
i-1
ꢀ×ꢀdi
ꢀꢀ×ꢀ
2-(i-1)
)
ꢀꢀ
(j)；
[0035]
yi＝(y
i-1
+ x
i-1
ꢀ×ꢀdi
ꢀꢀ×ꢀ
2-(i-1)
)
ꢀꢀ
(k)；
[0036]
zi＝(z
i-1-θ
i-1
)(l)。
[0037]
其中，i从0至15进行迭代；z0＝θ，di与zi同符号。
[0038]
步骤三：经过16次迭代计算后得到x
16
和y
16
分别为cosθ和sinθ。可以看出每次迭代，x和y只需要i比特的移位器2个，并且x和y坐标变化量和角度的累计一共只需要3个加法器，其中角度的累计还需要将每次的tan-1
(2-i
)求出。
[0039]
所述foc和mpc算法有两种工作模式，一种为正常工作时的模式，即开启所有模块，完成foc和mpc算法的完整的闭环计算；第二种模式为采样时的工作模式，在力反馈控制手套进行采样时使用，在采样时，三个mpc算法模块、反park变换模块、svpwm生成模块和park变换模块均处于关闭状态，只有clark变换模块和扩展卡尔曼滤波模块处于开启状态，用于采样电机的运动状态。
[0040]
本发明基于foc和mpc算法用于触控手套的力传感具体实施步骤如下：
[0041]
步骤一：mcu单片机将参数输入到foc系统中的mpc算法模块，将电机当前的状态与输入的目标状态通过mpc算法训练迭代修正电机的状态，mpc电流控制模型方程如下述(n)～(p)式所示：
[0042][0043]isd
(k+1)＝ai
sd
(k)+ bu
sd
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(m)；
[0044][0045]isq
(k+1)＝ai
sq
(k)+bu
sq
(p)。
[0046]
步骤二：反park变换模块将mpc算法模块输出的基于dq坐标系的控制电压向量由下述(q)～(r)式转化到αβ坐标轴方向：
[0047]
vα ＝vdcos(θ)
ꢀ‑ꢀ
vqsin(θ)
ꢀꢀꢀꢀ
(q)；
[0048]
vβ＝vqcos(θ)+vdsin(θ)(r)。
[0049]
步骤三：svpwm生成模块将反park变换模块生成的在αβ坐标轴方向上的电压向量控制量转化成控制电机的svpwm波。
[0050]
步骤四：clark变换模块由下述(s)～(t)式将三相电流ia、ib和ic转化为两相电流
iα和iβ：
[0051]
iα＝ia-cos(2π/3)ib-cos(2π/3)ic
ꢀꢀ
(s)；
[0052]
iβ＝sin(2π/3)ib-sin(2π/3)ic(t)。
[0053]
步骤五：park变换模块将clark变换模块得到的两相电流从αβ坐标轴方向转换到dq坐标系中，生成的电流向量将用作foc和mpc算法的电流环反馈输入mpc模块，且由下述(u)～(v)式完成电流环的闭环计算：
[0054]
id＝iαcos(θ)+iβsin(θ)
ꢀꢀꢀ
(u)；
[0055]
iq＝-iαsin(θ)+iβcos(θ)(v)。
[0056]
步骤六：扩展卡尔曼滤波模块用于检测电机当前的角度和速度，将clark变换模块输出的电流向量作为测量；将用于计算电流环的mpc算法模块生成的电压控制向量作为控制量输入，输出电机的运行速度和电机当前的角度，输出的结果输入mpc算法模块用于进行角度环和速度环的闭环计算。
[0057]
步骤七：算法测试，烧录或集成foc和mpc算法的fpga芯片通过spi接口接收mcu收到的力反馈数据，用于配置foc和mpc算法，力反馈数据包括：角度和速度，是角度环和速度环的外部输入量。佩戴触控单指套抓握现实中的物体，通过foc和mpc算法的采样模式采集物体的力传感反馈信息，将其传递给mcu单片机的存储模块，在工作模式下与虚拟世界中的物体匹配数据。烧录或集成在fpga芯片中的foc和mpc算法不断地闭环进行，当使用者的手姿势发生改变，比如松开或者环绕，都会根据力反馈数据的不同改变电机当前的运行状态，最终输出三相pwm波输入三相全桥电路控制电机的运转。
[0058]
所述foc和mpc算法根据采用电机的不同，对于相同的物体仍然需要重新采样，不同的电机其力传感反馈数据不同。
[0059]
所述foc和mpc算法在采样时，foc算法只有扩展卡尔曼滤波模块、clark变换模块在运行，其中扩展卡尔曼滤波模块的控制量输入为0，即输入的vα和vβ为0，扩展卡尔曼滤波模块通过计算将电机的运转速度和角度输出给mcu单片机以完成采样。
[0060]
所述步骤一中的foc和mpc算法模块会根据电机当前的运转的速度和角度，通过spi接口接收到mcu传输下一帧的力反馈数据，则使用者可以随时松开或握紧物体，也可以得到力反馈。
[0061]
本发明为了防止算法转化成verilog后集成为foc和mpc算法芯片时出现布线困难以及难以综合的情况，将foc算法中需要使用正弦余弦变换时使用的传统查表法或调用rom ip的方法优化成为cordic算法求sin函数和cos函数。
[0062]
本发明与现有技术相比具有以下的以下有益的技术效果和显著的技术进步：
[0063]
1)算法执行前对现实世界中的物体的力传感数据进行采样，能够更好的实现虚拟物体的里反馈效果，使用者使用时感受到与现实世界的差异化更小。
[0064]
2)foc和mpc算法模块采用spi接口接收mcu发来的参数，其固定参数可以根据电机型号不同，或实际情况不同随时调整输入的参数，使算法可以适配更多种类的触控手套。
[0065]
3)采用扩展卡尔曼滤波模块相比传统角度环和速度环的闭环计算，能够巧妙的融合了观测数据与估计数据，对误差进行闭环管理，将误差限定在一定范围，由于决策时依赖的信息来源多了一个观察数据，时间久远时仍能保持稳定的误差。
[0066]
4)采用mpc的算法替代pid算法能够降低传感器对精度的限制以及提高系统的动
态响应和参数鲁棒性，mpc算法比pid能够更优的处理延迟问题，通过滚动优化和反馈校正，对某一性能指标的在线反复优化来确定最优的控制动作，被控对象的实际输出用于修正预测结果，防止模型不匹配或环境干扰预测结果。这是模型预测控制区别于其他算法的最大的优势。
[0067]
5)采用cordic算法替代传统查表法或调用rom ip的方法，提高了可综合性并且降低了成本。
附图说明
[0068]
图1为本发明架构的触控手套力传感系统示意图；
[0069]
图2为触控单指套结构示意图；
[0070]
图3为foc和mpc算法中两种模式的模块示意图；
[0071]
图4为foc和mpc算法顶层模块示意图；
[0072]
图5为foc和mpc算法整体结构示意图；
[0073]
图6为本发明流程图；
[0074]
图7为foc和mpc算法最终输出的三相pwm波示意图。
具体实施方式
[0075]
参阅图1，本发明采用foc和mpc算法模块通过spi接口接收外部传来的实时参数，通过三个mpc算法模块、反park变换模块、svpwm生成模块、clark变换模块、park变换模块和扩展卡尔曼滤波模块组成电流环、角度环和速度环的闭环计算，所述foc和mpc算法烧录在fpga芯片中或集成为foc算法芯片，通过外部连接mcu单片机、三相位全桥电路、电机和触控单指套来验证算法的正确性，所述mcu单片机从上位机虚拟世界获取物体的力传感反馈，foc和mpc算法部分通过spi接口根据采样或工作模式进入相应状态，输出pwm波通过三相位全桥电路控制电机转动，从而将力传感反馈通过触控单指套传递到手指。
[0076]
参阅图2，所述触控单指套由mcu单片机1、fpga芯片2、电机3、连接绳4、传动杆5、转轴6组成，用于测试foc和mpc算法，所述传动杆5包括两转轴6和其连接的三段长度不等的空杆组成；所述转轴6为设置在触控单指套关节处的连接轴，使电机3带动传动杆5运动时，手指可以弯曲，感受到力传感反馈；所述电机3通过信号线与foc和mpc算法烧录在fpga芯片2相连，连接绳4一端缠绕在电机3的转轴上，另一端与传动杆5，用于传递电机3的转动带动传动杆5的运动。
[0077]
本发明通过foc的闭环控制对力传感进行精确的控制，首先通过算法的采样模式在现实世界中采集物体的力反馈数据，在触碰物体时，电机3以一定的速度旋转一定的角度，通过烧录在fpga芯片2中foc算法模块的spi接口对这些数据进行采集，并将数据发送给mcu单片机1储存在mcu单片机1的存储模块中。采样模式结束后，进入工作模式，当上位机中虚拟世界中物体的发生变化时，烧录在fpga芯片2中的foc和mpc算法模块通过spi接口从mcu单片机1获取配置参数和控制量，控制量有角度和速度以及电流值，角度控制量的目的是控制电机3的旋转角度，可用来模拟物体的大小，速度控制量的目的是控制电机3旋转的速度，可用来模拟物体的软硬程度。当foc和mpc算法模块通过spi接口从mcu1获取控制信号后，foc和mpc算法模块开始生成控制电机3的svpwm波。首先经过位置环，将位置控制量和电
机3目前的旋转位置通过mpc算法模块完成位置环的计算；然后将输入的速度控制量和电机目前的速度通过mpc算法模块完成速度环的计算；接着将计算得到的电流控制量与点击当前的电流值进行计算完成电流环的计算；最后经反park变换模块和svpwm生成模块得到控制电机3的svpwm波，随后foc和mpc算法模块将生成的svpwm波输出给电机3，电机3连接触控单指套，让使用者能够感受到虚拟世界中物体的力传感即物体的触感。
[0078]
以下结合具体实施例和附图，对本发明作进一步的详细说明。实施本发明的过程、条件、实验方法等，除以下专门提及的内容之外，均为本领域的普遍知识和公知常识，本发明没有特别限制内容。
[0079]
实施例1
[0080]
参阅图3，本发明区别于传统的foc算法模式单一的情况，提出采样模式和工作模式两个状态，当处于采样模式时，只有clark变换模块和扩展卡尔曼滤波模块处于开启状态，foc算法模块通过spi接口接收mcu单片机1发来的实物的力传感反馈数据，通过clark变换模块和扩展卡尔曼滤波模块，扩展卡尔曼滤波模块的控制量输入为0，即输入的vα和vβ为0，通过计算将电机3的运转速度和角度输出给mcu单片机1的存储模块以完成采样。
[0081]
当处于工作模式时，foc和mpc算法模块的所有子模块开启，mcu单片机1将上位机虚拟世界传来的物体力传感信息匹配采样模式存储的信息，进而foc和mpc算法模块通过spi接口接收实时的参数配置信息，经过各个子模块的电流环、速度环和角度环的计算，输出pwm波控制电机3转动。
[0082]
本发明的采样模式输入力传感数据时需要经过多次测量，并确保单个物体的采样模式保持的时间大于等于5s，采样的过程包括触控单指套触摸物体、手指环绕物体、手指拖住物体全过程。记录物体力反传感反馈数据三次后，foc算法模块能通过clark变换模块和扩展卡尔曼滤波模块计算后将力传感反馈传输给mcu单片机1的存储模块，使之可以在工作模式时与上位机虚拟世界中的物体进行匹配。
[0083]
参阅图4，所述foc和mpc算法模块的顶层为控制模块，本发明所设计的foc和mpc算法为了加速计算，采用了四级流水线的方式对输入数据进行计算，第一级流水线为spi接口模块，用于接收mcu1传递的参数；第二级流水线为三个mpc算法模块，用于对速度环、角度环以及电流环的控制计算；第三级流水线为反park变换模块和svpwm生成模块，用于生成svpwm波；第四级流水线为clark变换模块、park变换模块和扩展卡尔曼滤波模块，用于对反馈的信号进行计算，完成foc和mpc算法模块的反馈回路计算，从而形成控制闭环。其中，反park变换模块、clark变换模块和park变换模块中均用到了正弦余弦变换，在foc和mpc算法模块转化成verilog代码时，将传统查表法或调用rom ip的计算方式更新为cordic算法进行正弦余弦的计算，降低成本的同时提高了可综合性。
[0084]
参阅图5，本发明设计的foc和mpc算法模块具体计算过程如下：
[0085]
1)foc和mpc算法模块中的前两级mpc算法模块(speed和angel)通过spi接口接收mcu1发来的速度和角度参数，将电机当前的状态与输入的目标状态通过下一级mpc算法模块(current)比较当前id、iq和期望id_aim、iq_aim训练迭代修正电机的状态，mpc电流控制模型方程如下述(n)～(p)式所示：
[0086]
[0087]isd
(k+1)＝ai
sd
(k)+ bu
sd
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(m)；
[0088][0089]isq
(k+1)＝ai
sq
(k)+bu
sq
(p)。
[0090]
2)反park变换模块将mpc算法模块输出的基于dq坐标系的控制电压向量转化到αβ坐标轴方向，具体由下述(q)～(r)式进行转换：
[0091]
vα ＝vdcos(θ)
ꢀ‑ꢀ
vqsin(θ)
ꢀꢀꢀꢀ
(q)；
[0092]
vβ＝vqcos(θ)+vdsin(θ)(r)。
[0093]
3)svpwm生成模块将反park变换模块生成的在αβ坐标轴方向上的电压向量控制量转化成控制电机的svpwm波。
[0094]
4)clark将三相电流ia、ib和ic由下述(s)～(t)式将三相电流ia、ib和ic转化为两相电流iα和iβ：
[0095]
iα＝ia-cos(2π/3)ib-cos(2π/3)ic
ꢀꢀ
(s)；
[0096]
iβ＝sin(2π/3)ib-sin(2π/3)ic(t)。
[0097]
5)park变换模块将clark变换模块得到的两相电流从αβ坐标轴方向转换到dq坐标系中，生成的电流向量将用作foc和mpc算法的电流环反馈输入mpc模块(current)与iq_aim、id_aim比较，且由下述(u)～(v)式完成电流环的闭环计算，完成电流环的闭环：
[0098]
id＝iαcos(θ)+iβsin(θ)
ꢀꢀꢀ
(u)；
[0099]
iq＝-iαsin(θ)+iβcos(θ)(v)。
[0100]
6)扩展卡尔曼滤波模块用于检测电机当前的角度和速度，将clark变换模块输出的电流向量作为测量，将用于计算电流环的mpc算法模块(speed)生成的电压控制向量作为控制量输入，输出电机的运行速度和电机当前的角度，输出的结果输入mpc算法模块形成角度环和速度环的闭环计算；
[0101]
参阅图6，本发明是这样工作的：判断是采样模式还是工作模式，若处于采样模式，所述foc和mpc算法模块进入采样模式，关闭其他子模块仅留下clark变换和扩展卡尔曼滤波，使用测试用的触控单指套环绕需要采样的物体，手指的链接绳4以5rad/s的速度进行回转，保持环绕物体的姿势，当foc和mpc算法模块检测到电机3不再转动时，电机4不再回转，则foc和mpc算法模块开始读取，并通过clark变换模块和扩展卡尔曼滤波模块计算得到物体的力传感反馈数据，传回mcu单片机1中的存储模块进行存储用于在工作模式与上位机虚拟世界发来的力传感反馈数据对应。
[0102]
若处于工作模式，foc和mpc算法模块则进入工作模式，spi接口接收来自mcu单片机1发送的配置参数，通过三个mpc算法模块与反park变换模块、svpwm生成模块、三项全桥电路、clark变换模块、扩展卡尔曼滤波模块、park变换模块的计算，返回mpc算法模块形成电流环、角度环、速度环的闭环计算，并将svpwm模块产生的pwm波通过三相全桥电路输出，从而带动电机转动，完成虚拟世界到现实世界的力传感反馈。
[0103]
参阅图7，foc和mpc算法模块仿真输出的三相pwm波，svpwm生成模块是用于生成svpwm波的模块，根据反park变换模块生成的定子极坐标系下(即αβ坐标系)的电压控制矢量(vsρ，vsθ)生成三相svpwm波(pwma，pwmb，pwmc)。三相svpwm波控制的是三相全桥电路的mos管的开关，当为高电平时，mos管导通，对应的矢量为正向，当为低电平时，mos管关断，对
应的矢量方向为反向，因此电机3中定子的六个基矢量由正向的a、b、c向量和反向的a、b、c向量构成，pwm波给电机提供一个具有一定频率的脉冲宽度可调的脉冲电。脉冲宽度越大即占空比越大，提供给电机的平均电压越大，电机转速就高；反之脉冲宽度越小，则占空比越小提供给电机的平均电压越小，电机转速就低。
[0104]
本发明的保护内容不局限于以上实施例。在不背离本发明构思的精神和范围下，本领域技术人员能够想到的变化和优点都被包括在本发明中，并且以所附的权利要求书为保护范围。