本文参照 C. Gosselin 1990年的文章《Determination of the Workspace of 6-DOF Parallel Manipulators》，基于matlab2019b复现了文章内容，实现了6UPS并联机构的定姿态工作空间求解。

1 概述

1.1 圆弧相交法求解工作空间的限制

C. Gosselin 1990年的文章中，主要考虑的是各支链长度限制，而没有虎克铰和求交的工作范围，也没有考虑各支链可能出现的干涉。

1.2 思路

不管并联机构如何运动，动平台的中心点到动平台支链铰点的距离是不会变的。
现在单独考虑一根支链，假设支链在静平台的铰点连接的是球铰，不考虑球铰活动范围限制，让该支链随意运动，那么该支链末端点在空间扫过的区域为一个球体。如果考虑该支链的最大伸长量、最小伸长量，那么支链末端点就会构成一个大球体中间扣除一个小球体，外球的半径是支链的最大长度ρ_max，内球的半径是支链的最小程度ρ_min（图中绿色的是外球，为了简便没画内球）
这时候将这个球体沿 该支链的动平台铰点与动平台中心点构成的向量 移动，得到下图中红色的球体。这个红色球体区域就是在单支链约束下，动平台中心点的运动范围。

对其余5个支链进行相同的操作，就可以得到6个球体，这6个球体的相交部分就是并联机构中心点的运动范围，也就是工作空间。

下面的难点就是如何求这些球体的相交部分，具体有两种方法：

有一些文章是利用CAD软件，比如solidworks之类的，在软件中进行球体的求交运算，这样比较直观，简便，而且很容易得到工作空间的体积。但是这种方法得不到工作空间某一切面边界的函数表达。
C. Gosselin 使用的方法。沿z轴自下而上对这些球体切片，将三维空间球体的求交问题变为二维平面圆环求交问题。这种方法可以得到某一切面的具体函数表达，但是计算工作空间体积有点麻烦。

2 文章解读

首先说明上图中各个字母的含义：

静平台坐标系为 R:O−xyzR:O-xyzR:O−xyz ,OOO 为坐标原点。

动平台坐标系为 R′:O′−x′y′z′R':O'-x'y'z'R′:O′−x′y′z′ ,O′O'O′ 为坐标原点。

向量(ai,i=1,…,6)\left(a_{i}, i=1, \ldots, 6\right)(ai,i=1,…,6)是静平台铰链点AiA_{i}Ai在静平台坐标系下RRR的位置矢量。

向量(bi,i=1,…,6)\left(b_{i}, i=1, \ldots, 6\right)(bi,i=1,…,6)是动平台铰链点BiB_{i}Bi在动平台坐标系下R′R'R′的位置矢量。

QQQ是初始位置时动平台相对静平台的姿态矩阵。

动平台中心点O′O'O′在静平台坐标系RRR下的位置矢量表示为：[r]R=[xr,yr,zr]T[{r}]_{R}=\left[x_{r}, y_{r}, z_{r}\right]^{T}[r]R=[xr,yr,zr]T。

那么动平台铰链点BiB_{i}Bi在静平台坐标系下RRR的位置矢量为：
[bi]R=[r]R+Q[bi]R′i=1,…,6(1)\left[{b}_{i}\right]_{R}=[{r}]_{R}+{Q}\left[{b}_{i}\right]_{R'} \quad i=1, \ldots, 6 \tag{1}[bi]R=[r]R+Q[bi]R′i=1,…,6(1)
向量AiBiA_{i}B_{i}AiBi在静平台坐标系下RRR就可以表示为
[bi−ai]R=[r]R+Q[bi]R′−[ai]Ri=1,…,6(2)\left[{b}_{i}-{a}_{i}\right]_{R}=[{r}]_{R}+{Q}\left[{b}_{i}\right]_{R'}-\left[{a}_{i}\right]_{R}\quad i=1, \ldots, 6 \tag{2} [bi−ai]R=[r]R+Q[bi]R′−[ai]Ri=1,…,6(2)
向量AiBiA_{i}B_{i}AiBi的模长就是支链的长度：
ρi=∥[bi−ai]R∥=∥[r]R+Q[bi]R′−[ai]R∥,i=1,…,6(3)\rho_{i}=\left\|\left[{b}_{i}-{a}_{i}\right]_{R}\right\|=\left\|[{r}]_{R}+{Q}\left[{b}_{i}\right]_{R'}-\left[{a}_{i}\right]_{R}\right\|, \quad i=1, \ldots, 6 \tag{3} ρi=∥[bi−ai]R∥=∥[r]R+Q[bi]R′−[ai]R∥,i=1,…,6(3)
对（3）式平方，并展开
ρi2=(xr−ui)2+(yr−vi)2+(zr−wi)2,i=1,…,6(4)\rho_{i}^{2}=\left(x_{r}-u_{i}\right)^{2}+\left(y_{r}-v_{i}\right)^{2}+\left(z_{r}-w_{i}\right)^{2}, \quad i=1, \ldots, 6 \tag{4} ρi2=(xr−ui)2+(yr−vi)2+(zr−wi)2,i=1,…,6(4)

ui=xai−q11xbi−q12ybi−q13zbi,i=1,…,6vi=yai−q21xbi−q22ybi−q23zbi,i=1,…,6wi=zai−q31xbi−q32ybi−q33zbi,i=1,…,6\begin{aligned} u_{i} &=x_{a i}-q_{11} x_{b i}-q_{12} y_{b i}-q_{13} z_{b i}, & & i=1, \ldots, 6 \\ v_{i} &=y_{a i}-q_{21} x_{b i}-q_{22} y_{b i}-q_{23} z_{b i}, & & i=1, \ldots, 6 \\ w_{i} &=z_{a i}-q_{31} x_{b i}-q_{32} y_{b i}-q_{33} z_{b i}, & & i=1, \ldots, 6 \end{aligned} uiviwi=xai−q11xbi−q12ybi−q13zbi,=yai−q21xbi−q22ybi−q23zbi,=zai−q31xbi−q32ybi−q33zbi,i=1,…,6i=1,…,6i=1,…,6

[ai]R=[xai,yai,zai]T,i=1,…,6[bi]R′=[xbi,ybi,zbi]T,i=1,…,6\begin{aligned} \left[{a}_{i}\right]_{R}=\left[x_{a i}, y_{a i}, z_{a i}\right]^{T}, & i=1, \ldots, 6 \\ \left[{b}_{i}\right]_{R^{\prime}}=\left[x_{b i}, y_{b i}, z_{b i}\right]^{T}, & i=1, \ldots, 6 \end{aligned} [ai]R=[xai,yai,zai]T,[bi]R′=[xbi,ybi,zbi]T,i=1,…,6i=1,…,6
其中qijq_{ij}qij为姿态矩阵中QQQ中的对应元素

根据实际支链的最大最小长度：
ρi=ρmin⁡or ρi=ρmax⁡\rho_{i}=\rho_{\min } \quad \text { or } \quad \rho_{i}=\rho_{\max } ρi=ρmin or ρi=ρmax
这样我们就能得到每个支链的内外球体的方程
(xr−ui)2+(yr−vi)2+(zr−wi)2=ρmin2(xr−ui)2+(yr−vi)2+(zr−wi)2=ρmax⁡2\begin{array}{l} \left(x_{r}-u_{i}\right)^{2}+\left(y_{r}-v_{i}\right)^{2}+\left(z_{r}-w_{i}\right)^{2}=\rho_{m i n}^{2} \\ \left(x_{r}-u_{i}\right)^{2}+\left(y_{r}-v_{i}\right)^{2}+\left(z_{r}-w_{i}\right)^{2}=\rho_{\max }^{2} \end{array} (xr−ui)2+(yr−vi)2+(zr−wi)2=ρmin2(xr−ui)2+(yr−vi)2+(zr−wi)2=ρmax2
上式中ui，vi，wiu_{i}， v_{i}， w_{i}ui，vi，wi就是第i个支链的末端点构成的球体沿向量BiO′B_{i}O'BiO′平移后的球心坐标。对于一个给定结构参数和初始位姿的并联机构，ui，vi，wiu_{i}， v_{i}， w_{i}ui，vi，wi就是确定的。

现在我们用XOYXOYXOY平面沿ZZZ轴切割这些球体，就可以在XOYXOYXOY平面上得到一些圆、或者圆环，他们的方程如下：
(xr−ui)2+(yr−vi)2=Rmin⁡,i2(xr−ui)2+(yr−vi)2=Rmax⁡,i2\begin{aligned} \left(x_{r}-u_{i}\right)^{2}+\left(y_{r}-v_{i}\right)^{2} &=R_{\min , i}^{2} \\ \left(x_{r}-u_{i}\right)^{2}+\left(y_{r}-v_{i}\right)^{2} &=R_{\max , i}^{2} \end{aligned} (xr−ui)2+(yr−vi)2(xr−ui)2+(yr−vi)2=Rmin,i2=Rmax,i2
其中
Rmin⁡,i2={ρmin⁡2−(zH−wi)2,if ρmin⁡2−(zH−wi)2>00,otherwise R_{\min , i}^{2}=\left\{\begin{array}{ll} \rho_{\min }^{2}-\left(z_{H}-w_{i}\right)^{2}, & \text { if } \rho_{\min }^{2}-\left(z_{H}-w_{i}\right)^{2}>0 \\ 0, & \text { otherwise } \end{array}\right. Rmin,i2={ρmin2−(zH−wi)2,0, if ρmin2−(zH−wi)2>0 otherwise
Rmax⁡,i2={ρmax⁡2−(zH−wi)2,if ρmax⁡2−(zH−wi)2>00,otherwise R_{\max , i}^{2}=\left\{\begin{array}{ll} \rho_{\max }^{2}-\left(z_{H}-w_{i}\right)^{2}, & \text { if } \rho_{\max }^{2}-\left(z_{H}-w_{i}\right)^{2}>0 \\ 0, & \text { otherwise } \end{array}\right. Rmax,i2={ρmax2−(zH−wi)2,0, if ρmax2−(zH−wi)2>0 otherwise

3 具体实施

利用我上一篇文章《多个圆/圆环求交》的知识，可以求出二维平面中圆/圆环的相交区域。将XOYXOYXOY切割平面沿ZZZ轴向上平移一定距离，再求一个相交区域，如此往复就能得到如下图这样的工作空间的边界。

我们也可以利用XOZXOZXOZ平面沿Y轴切割，可以得到下图

同样可以绕Z轴旋转切割，可以得到下图

4 总结及提示

在我实际编程中发现，切割面不好计算。如果只是与XOYXOYXOY平行的面还好，一旦涉及到旋转切割，或者空间任意方向切割，求切割面中的圆/圆环方程就特别麻烦。
为此，我想要了一种方法。我可以不移动切割面，而是移动这个六个球体，利用一个固定的XOYXOYXOY平面对其进行切割。
举个例子，比如我想用XOZXOZXOZ平面在Y=10的位置对球体进行切割，我可以将这六个球体先绕X轴旋转90°，再沿Z轴平移-10（在我的程序中使用的了齐次变换，即用一个4×4的矩阵就能将旋转和平移同时表示了）。然后用XOYXOYXOY平面对其进行切割，并求出其相交部分，最后再把相交部分反变换回去（即先沿Z轴平移10，再绕X轴旋转-90°）。
这样做的好处是我们可以只使用XOYXOYXOY平面进行切割、求交，就能得到空间任意切割面的相交部分。我们只需要知道切割面的法向量与XOYXOYXOY平面的法向量，就能得到变换矩阵。
最后我们就能得到下图这样的工作空间：

5 matlab代码

需要注意的是，本程序中使用了matlab机器人工具箱，利用了其中transl()，rotz()等函数。

这里只有主程序，完整程序请看我的资源！

主程序

clear all%静平台 坐标参数
xa=[92.58 132.58 40 -40 -132.58 -92.58];
ya=[99.64 30.36 -130 -130 30.36 99.64];
za=23.1*ones(1,6);
%动平台 坐标参数
xb=[30 78.22 48.22 -48.22 -78.22 -30];
yb=[73 -10.52 -62.48 -62.48 -10.52 73];
zb=-37.1*ones(1,6);
%支链长度范围
roumin=454.5*ones(1,6);
roumax=504.5*ones(1,6);%初始位置时 动平台相对静平台的位姿
Q=eye(3);
%Q=rotz(0)*roty(0)*rotx(45);%支链球体移动后的球心坐标
sphere=[xa;ya;za]-Q*[xb;yb;zb];
u=sphere(1,:);
v=sphere(2,:);
w=sphere(3,:);zmin=min(w);%Z方向工作空间最小高度
zmax=max(w)+max(roumax);%Z方向工作空间最大高度discreteNum=10;%每条弧的离散点数量
numCir=size(sphere,2);%支链个数% Rot=VectorRotMartix([1,0,0],[0,1,0]) 切割面法向量与XOY平面法向量 的变换矩阵
tic
%XY平面切割遍历
for range=zmin:4:zmax%平移矩阵Tra=transl(0,0,-range);%旋转矩阵Rot=eye(4);%齐次变换矩阵P=Tra*Rot;%构建球的齐次坐标 并变换到XOY平面sphere_P=P*[sphere;ones(1,numCir)];%求平面内的重合弧arcList=TraversePlane(sphere_P,[roumin;roumax]);numArc=size(arcList,2);%这个平面中 重合弧的数量if numArc==0continue;end%离散每条重合弧points=ones(4,numArc*discreteNum);%记录该平面内的离散点 齐次坐标for i = 1:numArcxr = arcList(1,i);yr = arcList(2,i);r = arcList(3,i);startAngle = arcList(4,i);endAngle = arcList(5,i)+startAngle;theta = linspace(startAngle,endAngle,discreteNum);x = xr+r*cosd(theta);y = yr+r*sind(theta);%XY平面内的离散点 齐次坐标pointsXY(1:2,:)=[x;y];pointsXY(3,:)=0;pointsXY(4,:)=1;%XY平面中的点反变换回原切割平面pointsInitial=inv(P)*pointsXY;plot3(pointsInitial(1,:),pointsInitial(2,:),pointsInitial(3,:));hold on;end
end
toctic
%绕Z轴旋转切割遍历
for range=0:10:360%平移矩阵Tra=transl(0,0,0);%旋转矩阵OA=[sind(range),cosd(range),0];OB=[0,0,1];Rot=VectorRotMartix(OA,OB);%OA为切割面法向量 OB为XOY平面法向量%齐次变换矩阵P=Tra*Rot;%构建球的齐次坐标 并变换到XOY平面sphere_P=P*[sphere;ones(1,numCir)];%求平面内的重合弧arcList=TraversePlane(sphere_P,[roumin;roumax]);numArc=size(arcList,2);%这个平面中 重合弧的数量if numArc==0continue;end%离散每条重合弧points=ones(4,numArc*discreteNum);%记录该平面内的离散点 齐次坐标for i = 1:numArcxr = arcList(1,i);yr = arcList(2,i);r = arcList(3,i);startAngle = arcList(4,i);endAngle = arcList(5,i)+startAngle;theta = linspace(startAngle,endAngle,discreteNum);x = xr+r*cosd(theta);y = yr+r*sind(theta);%XY平面内的离散点 齐次坐标pointsXY(1:2,:)=[x;y];pointsXY(3,:)=0;pointsXY(4,:)=1;%XY平面的点反变换回原始平面pointsInitial=inv(P)*pointsXY;%找到工作空间中Z轴于0的列 的索引LessZero=find(pointsInitial(3,:)<0);%删除Z轴小于0的数据 对于工作空间没有意义pointsInitial(:,LessZero)=[];plot3(pointsInitial(1,:),pointsInitial(2,:),pointsInitial(3,:));hold on;end
end
toc
view([6,2,2]);
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on

【并联机构工作空间分析系列2】圆弧相交法论文解读及matlab程序相关推荐

脑电分析系列[MNE-Python-5]| Python机器学习算法随机森林判断睡眠类型
案例介绍本案例通过对多导睡眠图(Polysomnography,PSG)数据进行睡眠阶段的分类来判断睡眠类型. 训练:对Alice的睡眠数据进行训练: 测试:利用训练结果对Bob的睡眠数据进行测试, ...
GDB调试器源代码分析系列--Inferior call的实现与分析(1)
[转] GDB调试器源代码分析系列--Inferior call的实现与分析(1) (2011-10-11 20:41) 标签: 分析分类: 调试器先说说几个概念: (1) 什么是infe ...
Spark Shuffle源码分析系列之PartitionedPairBufferPartitionedAppendOnlyMap
概述 SortShuffleWriter使用ExternalSorter进行ShuffleMapTask数据内存以及落盘操作,ExternalSorter中使用内存进行数据的缓存过程中根据是否需要ma ...
Kylin源码分析系列三—rowKey编码
Kylin源码分析系列三-rowKey编码注:Kylin源码分析系列基于Kylin的2.5.0版本的源码,其他版本可以类比. 1. 相关概念前面介绍了Kylin中Cube构建的流程,但Cube数据 ...
Mysql源代码分析系列
Mysql源代码分析系列(2): 源代码结构 Mysql源代码主要包括客户端程序代码,服务器端代码,测试工具和一些库构成,下面我们对比较重要的目录做些介绍. BUILD 这个目录在本系列的上篇文章中我 ...
BlogEngine.Net架构与源代码分析系列（转载）
01.BlogEngine.Net架构与源代码分析系列part1:开篇介绍 02.BlogEngine.Net架构与源代码分析系列part2:业务对象--共同的父类BusinessBase 03.Bl ...
脑电分析系列 | eeglab汇总
1 脑电分析系列eeglab教程 eeglab教程系列(1)-安装教程 eeglab教程系列(2)-加载.显示数据 eeglab教程系列(3)-绘制脑电头皮图 eeglab教程系列(4)-绘制通道光谱 ...
脑电分析系列 | MNE-Python汇总
1 脑电分析系列MNE-Python教程 [MNE-1]| MNE-Python详细安装与使用(更新) [MNE-2]| MNE中数据结构Raw及其用法简介(更新) [MNE-3]| MNE中数据结构 ...
脑电分析系列[MNE-Python-19]| 可视化Evoked数据
在前面我们介绍过Evoked的数据结构以及如何创建Evoked对象: 脑电分析系列[MNE-Python-4]| MNE中数据结构Evoked及其对象创建 Evoked potential(EP)诱发 ...
脑电分析系列[MNE-Python-2]| MNE中数据结构Raw及其用法简介(更新)
Raw对象主要用来存储连续型数据,核心数据为n_channels和times,也包含Info对象. 下面可以通过几个案例来说明Raw对象和相关用法. Raw结构查看: # 引入python库 impo ...

【并联机构工作空间分析系列2】圆弧相交法论文解读及matlab程序

目录

1 概述

1.1 圆弧相交法求解工作空间的限制

1.2 思路

2 文章解读

3 具体实施

4 总结及提示

5 matlab代码

【并联机构工作空间分析系列2】圆弧相交法论文解读及matlab程序相关推荐

最新文章

热门文章

【并联机构工作空间分析系列2】圆弧相交法 论文解读及matlab程序

目录

1 概述

1.1 圆弧相交法求解工作空间的限制

1.2 思路

2 文章解读

3 具体实施

4 总结及提示

5 matlab代码

【并联机构工作空间分析系列2】圆弧相交法 论文解读及matlab程序相关推荐

最新文章

热门文章

【并联机构工作空间分析系列2】圆弧相交法论文解读及matlab程序

【并联机构工作空间分析系列2】圆弧相交法论文解读及matlab程序相关推荐