Parallel Platform (Stewart Platform) 类型机械臂的正逆解 01

并联机器人相对于串联机器人有着不同的应用场景。本文主要是基于并联机械臂的基础控制算法做一些阐述。对于机械臂的架构通讯方式（RS232 RS485 Canbus Modbus），结构设计（球关节，universal joint），传感器选型，运动规划（奇异解规避）希望在后面的文章中做进一步研究。

并联机械臂 vs 串联机械臂

 在之前的文章中已经写过了串联机械臂的IK&FK，总体上来说，并联型机械臂相对于串联机械臂来说，系统的刚度更好，承载能力（payload）更强（比如很多AR控制器游戏平台），但是运动空间比较有限。从精度上说，串联型机械臂的误差存在累加，而并联型机械臂的误差相对解耦，所以在整体平台校准调平后可以达到比较高的精度。组成的元器件上，串联机械臂使用servo motor 和 对应的光栅或霍尔原理的传感器，而并联使用的是线性电机，在传感器的选型上可以使用线性传感器。算法控制上，并联型运动控制 IK比较简单（下文），而FK比较复杂。但是串联机械臂的FK算法非常多，有numerical method， iterative method (Newton-Raphson), Geometric method.

术语定义：

6-6 6DOF：把并联机器人分为基座和平台，6-6表示基座-平台分别有六个控制点的6自由度并联机器人

奇异解：在给定的Leg length情况下，机器人的位姿解不唯一。也表现为独立表达式出现了关联性，恒等行变换后，矩阵的秩减小。

B frame：基座坐标系

P frame：平台坐标系

串联机器人逆解其实相当简单：

图中有两个坐标系，分别为O（B frame） O‘ （P frame).

机械设计上，固定端点的坐标 Bi (i = 1~6), Ti (i = 1~6)

 输入平台的旋转和平移量：旋转角度（roll yaw pitch），平移量（p (px py pz) 假设初始位置的时候px py pz == 0）

Matlab 代码如下：

B1 = [B1x B1y 0 1]
B2 = [B2x B2y 0 1]
B3 = [B3x B3y 0 1]
B4 = [B4x B4y 0 1]
B5 = [B5x B5y 0 1]
B5 = [B6x B6y 0 1]

T1 = [T1x T2y 0 1]
T2 = [T2x T2y 0 1]
T3 = [T3x T3y 0 1]
T4 = [T4x T4y 0 1]
T5 = [T5x T5y 0 1]
T6 = [T6x T6y 0 1]

TXrad=roll*pi/180;                 
TYrad=pitch*pi/180;                 
TZrad=yaw*pi/180;                   

%T (4X4) is the transformation (translation + rotation) matrix from Frame P to Frame B.
T = [cos(TYrad)*cos(TZrad), -cos(TYrad)*sin(TZrad), sin(TYrad), px;
sin(TXrad)*sin(TYrad)*cos(TZrad)+cos(TXrad)*sin(TZrad),-sin(TXrad)*sin(TYrad)*sin(TZrad)+cos(TXrad)*cos(TZrad),-sin(TXrad)*cos(TYrad),py;
-cos(TXrad)*sin(TYrad)*cos(TZrad)+sin(TXrad)*sin(TZrad), cos(TXrad)*sin(TYrad)*sin(TZrad)+sin(TXrad)*cos(TZrad), cos(TXrad)*cos(TYrad),pz;
0,0,0,1];


T1=T*T1;  
T2=T*T2;
T3=T*T3;
T4=T*T4;
T5=T*T5;
T6=T*T6;

L1=sqrt((abs(T1(1)-B1(1)))^2+(abs(T1(2)-B1(2)))^2+(abs(T1(3)-B1(3)))^2);
L2=sqrt((abs(T2(1)-B2(1)))^2+(abs(T2(2)-B2(2)))^2+(abs(T2(3)-B2(3)))^2);
L3=sqrt((abs(T3(1)-B3(1)))^2+(abs(T3(2)-B3(2)))^2+(abs(T3(3)-B3(3)))^2);
L4=sqrt((abs(T4(1)-B4(1)))^2+(abs(T4(2)-B4(2)))^2+(abs(T4(3)-B4(3)))^2);
L5=sqrt((abs(T5(1)-B5(1)))^2+(abs(T5(2)-B5(2)))^2+(abs(T5(3)-B5(3)))^2);
L6=sqrt((abs(T6(1)-B6(1)))^2+(abs(T6(2)-B6(2)))^2+(abs(T6(3)-B6(3)))^2);

answer = [L1 L2 L3 L4 L5 L6]