python 并联机器人正逆解算法 并联机器人反解

有关并联绳驱机器人运动学正解反解的学习

• Preface（complain）
• Perface（start）

• Advantages of parallel robot
• Disdvantages of parallel robot

• Structure establishment
• Inverse kinematics of parallel robot

• follow path
• MATLAB for Inverse kinematics of parallel robot

• Forward kinematics of parallel robot

Preface（complain）

个人学习总结，不喜勿喷（网图权侵删）

活跃气氛
这几周太忙太忙，上几周金工实习，每天没有时间搞事情，每周六日还要去学车占用了六日的学习时间（还好考完了（QAQ））。还在想竞赛如何进行主动示教（或许会更 ）这周还来了一个电工电子认知实习（哭）。
在没有时间的时间里，我试图通过熬夜（早上起不来

以及掉了的头发！

终于把并联机器人运动学正反解搞出来了，为什么网上关于绳驱的资料很少啊，我找了好久都没抄到现成的源码，还得自己编…
(回归正题)
试一试用英文写博客（感觉一下不一样的快感，折磨自己也折磨别人

大哥大姐求轻喷~~（不喷）~~ 写blog本就是一个在满足自己成就感的同时造福他人的过程，我明白写英文(绝对有语法和拼写错误！)肯定会让人感到有一点点的”不舒服“，但是总之凑合看吧，毕竟是一篇学习性质的blog，本人也是初学，还想练练英语（六级还没给过，哭了）。

Perface（start）

Parallel cable robot is a structure of closed loop kinematic chain,its moving platform has some independent kinematic chains attached to the base.
It always is compiled with base frame,moving platform and some lines between us.

并联机器人相对于串联机器人有很大的区别，它的区别在于它的关节居然并联在了一起，而不是像串糖葫芦一样串在一起的。
由于这样的构型，她便拥有了与串联机器人不一样的优点和缺点：

Advantages of parallel robot

绳索牵引并联机器人采用绳索代替刚性连杆将末端执行器与静平台连接起来，使其具有工作空间大、负载质量比高、造价成本低和易于模块 化和维修等优点。他运动分析较容易、且可避免驱动轴之间的耦合（coupling） 效应。

Disdvantages of parallel robot

1.不易有动态误差，精度较高。

2.运动惯性小。

3.输出轴大部份承受轴向应力，机器刚性高，结构稳定。

4.为热对称性结构设计，热变形量较小。

5.在位置求解上，串联机构正解容易，反解困难，而并联机构正解困难，反解容易。

6.工作空间较小。

source：关注 | 并联式机构的优缺点与并联机构原理在机器人领域的应用 　　
由于上述种种原因，导致我们不能生搬硬拉，而是要在此系统基础上从新搞起来。

Forward kinematics solution：在并联机器人中，运动学正解是事先告诉你绳长数据（根数的绳长），让你通过这几根绳长的数据来计算出末端点为位姿，这不是一件简单的事情。

Inverse kinematics solution：在并联机器人中，运动学逆解是事先告诉你动平台所在的位置和姿态后，让你求出四根绳长，这虽然不是啥容易事情但是却比运动学正解简单多了。（哭）

Next we will use a simple but useful example to demostrate my Learning outcomes of parallel cable robot

Structure establishment

In this section,we will bulid a parallel robot by simple lines in a blank.

In this picture,you can see a black frame( robot body),four orange lines are stand for our cable,and the red square are moving platform,which make a ugly robot…

In order to simplify the calculation，we set the body frame and moving frame’s base point is located at the central of body.(you can see that…)

In fact,it is my personal point and I also don’t want to prove it.

The x-axis, Y-axis and rotation angle have been marked on the drawing

The parallel cable robot use its electric machineries to control its length of cables(orange line),which can control moving plateform to move.

Setting
Beacuse our robot’s body is changeless and our moving plate is a two dimensions.so we set the robot has 3 dofs ：(X,Y,θ)，It can move and rotate.

Inverse kinematics of parallel robot

while (1) wo shi cai ji wo shi cai ji end

In my opinion,parallel robot’s Inverse kinematics is based on our moving platform’s position.our computer use our position to to calculate the length of four ropes.

SO WE MUST FIND OUT THE POSITION OF END EFFECTOR.

follow path

1. In order to control the robot to move a correct line we expect to and it also should meet our expectations——the position.
2. hit him !Scatter all the positions until it turns to some correct points in every tiny time.
3. put this data into the controller and we can control the electric machinery to rotate.
5. complete.
   like this:



6. details：
   1.Determine the trajectory of the hinge point of the moving platform
   As we all know, the reason why the motion platform can move is not that it can move itself, but that the controller can adjust the four ropes Length to force it to move. And the hinge point of our rope is not on the real trajectory, but has a certain distance, so If we calculate the position of the hinge point, we can get the rope length at this time, and the motor can be controlled.

2.These rope length data are arranged to obtain the trajectory of the overall motion platform.

So we use this picture to show the process.

In this picture we depict a nomal position.our moving platform rotates a angle and move some…To express this positon we can use this equation.

we can use a point(B1) to explain it:

The green square are our initial position,first,it will rotate a angle,then it turns the yellow square.It has completed the part of rotate(TransM （旋转矩阵）* load_1 ).Then we will move it(+ P（位移向量）😉

The equation is completed.

PS: we can imagine it as the position attitude of the platform by rotating and then offsetting, but it happens instantaneously.

MATLAB for Inverse kinematics of parallel robot

Its flow chart is：

The main codes are here

while progress < target
    [progress, currentvel] = tcRunCycle(progress, reqvel, currentvel, cycle_time  , target  , maxaccel, feed_override, maxvel);         %调用速度规划函数
    time = time + cycle_time;                                                       %进行时间迭代
    
    x_now = progress / target * (x_1 - x_0) + x_0;                                 %计算当前位置
    y_now = progress / target * (y_1 - y_0) + y_0;

    Yaw_now_degree = (progress / target) * (Yaw_start - Yaw) + Yaw;              %计算当前角度
    
    P = [x_now;y_now;0];                                                       %动平台相对于静平台的实时位置
    
    TransM =[cosd(Yaw_now_degree),-sind(Yaw_now_degree),0;sind(Yaw_now_degree),cosd(Yaw_now_degree),0;0,0,1]
    %Z轴旋转矩阵

load_1 = [ - 1; - 1;0];
    now_1 = TransM * load_1 + P;
    x_1_1 = now_1(1, 1);
    y_1_1 = now_1(2, 1);

`` x1_run = linspace(x_1_1, L1_point_x, 1000);
    y1_run = linspace(y_1_1, L1_point_y, 1000);
    plot(x1_run, y1_run);
    hold on

Then we can move it.

Forward kinematics of parallel robot

Forward kinematics of parallel robot is a difficult thing.As we all know,the fk has many solutions and needs to be solved iteratively. So we choose to use Newton-Raphson to solve it.
（改成中文了（皮））
首先我们需要定义我们的位置矢量P，也就是上面的now_1这样我们可以获得当前末端平台的位置。
由于正运动学是已知绳索长度求解运动平台的位姿，所以我们在使用迭代时，四根绳长也会进行迭代，所以我们要进行绳长的计算：L=A-P（P：铰接点相对于坐标的向量，A：动平台中心点的位置。）叠一叠就出来了不行就继续叠。
对于正运动学，自然要比逆运动学难，所以也不好搞，这里只说一下一种简单的正运动学求解方法，别的我还不会，没学

1. 我们首先要把机器人建起来，包括动平台的大小，出绳点在何处。构建起来后，我们就可以进行下一步的操作了。

load_1 = [-1;-1;0];                                         
load_2 = [-1;1; 0];
load_3 = [1;1;  0];
load_4 = [1;-1; 0];

L1_point_x = - 50;                                              
L1_point_y = - 50;

L2_point_x = - 50;                                              
L2_point_y = 50;

L3_point_x = 50;                                                
L3_point_y = 50;

L4_point_x = 50;                                                L4_point_y = - 50;

可以自己设定哦…………
3. 按照Newton-Raphson的方法，它需要一个初值要去逼近，这个值选的越好迭代越少越完美，但是靠人来估总觉不靠谱，这需要我后续学习如何机器给估值让他更加精确。

x_now=1;                                                         
y_now=1;
z_now=0;
Yaw_now_degree=0;

4. 在这之后，我们还要来个绳长，因为正运动学是知道绳长求位姿，要是没绳长岂不是要坏菜。。。

ps：当然，由于它是一个正运动学演示代码，要是可以你也可以把逆运动学的结果直接写个代码带到这里来，免除cv之苦………………

l1_fk_find=91.107035936923570;                                  
l2_fk_find=66.249280888240930;
l3_fk_find=48.742662884505194;
l4_fk_find=79.292950279803290;

以上就是初始准备来，准备好我们就要开始迭代了！

按照公式，我们一步一步做就完了，其实无脑写也是可以的，想要理解确实实属不易。
它的原理其是就是一步一步逼近我们想要的结果，再把得到的结果与原来的对比，当小于我们设定的误差时就认为达到了我们所要完成的目标。
代码放在了资源里，要的自取，等过一段时间我再理解理解再写，写到这里突然不会了，啊这。