1.一种码垛机器人的工作空间确定方法，所述码垛机器人具有五个旋转自由度，第一关节、第二关节、第三关节和第四关节各提供一个旋转自由度，辅助关节提供一个旋转自由度用于保持第四关节轴线始终垂直于地面，第三关节处安装一个三角架连接两个连杆并且与大臂和小臂形成两个局部闭链，其特征在于，所述码垛机器人的工作空间确定方法，包括：建立关节的坐标系以及确定无约束下各关节的最小角度和最大角度；

当运动方式为关节运动方式时，逐一确定在几何关系和机械约束下各关节的运动范围作为码垛机器人的工作空间；

当运动方式为笛卡尔运动方式时，先确定机械臂末端在第一关节坐标系下的运动范围，然后将在第一关节坐标系下的运动范围转换为基座坐标系下的运动范围，根据机械臂末端在基座坐标系下的运动范围生成码垛机器人的工作空间，所述机械臂末端为第四关节轴线上的一点，所述第一关节设置在基座上。

2.根据权利要求1所述的码垛机器人的工作空间确定方法，其特征在于，所述建立关节的坐标系，包括：建立固联在第一关节的第一关节坐标系frame1，第一关节坐标系frame1的原点在第一关节轴线和第二关节轴线的交点处，沿第一关节轴线向上为Z1轴，沿第二关节轴线指向第二关节的方向为Y1轴，以Y1轴叉乘Z1轴的结果确定X1轴；

在第一关节的转角等于0时，基座的基座坐标系frame0和第一关节坐标系frame1重合，基座坐标系frame0的三个轴为X,Y,Z。

3.根据权利要求2所述的码垛机器人的工作空间确定方法，其特征在于，所述确定无约束下各关节的最小角度和最大角度，包括：过机械臂末端向辅助关节轴线作一条垂线并与辅助关节轴线相交，垂线长度为l，定义垂线l和X‑Y平面的夹角为β，通过机械臂末端的位置在Y1轴的投影，确定偏移bias1的值；

取大臂和小臂分别长L1和L2，通过辅助关节到机械臂末端的距离确定bias2和bias3，取bias2＝l*cos(β)和bias3＝l*sin(β)；

计算由三角架确定的角度 atan是反正切函数，取a是连接靠近小臂的连杆和三角架的关节轴线上的一点，过点a作第三关节轴线的垂线u交第三关节轴线于点b，过点a作X1‑Y1平面垂线ac，过点b作Y1‑Z1平面垂线bc，垂线ac和垂线bc交于点c，则距离h＝ac，距离d＝bc；

设定theta1、theta2、theta3和theta4为第一关节、第二关节、第三关节和第四关节的转角，取无约束下各关节转角的取值范围如下：‑180°

定义joint1min和joint1max为无约束下第一关节的最小角度和最大角度，根据无约束下关节转角的取值范围，确定无约束下第一关节的最小角度和最大角度的取值范围如下：‑

180°＜joint1min＜‑90°和90°＜joint1max＜180°；

定义joint2min和joint2max为无约束下第二关节的最小角度和最大角度，根据无约束下关节转角的取值范围，确定无约束下第二关节的最小角度和最大角度的取值范围如下：‑

180°

定义joint3min和joint3max为无约束下第三关节的最小角度和最大角度，根据无约束下关节转角的取值范围，确定无约束下第三关节的最小角度和最大角度的取值范围如下：0°

定义joint3bias1，joint3bias2为第三关节的角度偏置，满足如下约束：joint3bias1＝joint3min和joint3bias2+joint3max＝180°；

定义joint4min和joint4max为无约束下第四关节的最小角度和最大角度，根据无约束下关节转角的取值范围，确定无约束下第四关节的最小角度和最大角度的取值范围如下：‑

270°

4.根据权利要求3所述的码垛机器人的工作空间确定方法，其特征在于，当运动方式为关节运动方式时，逐一确定在几何关系和机械约束下各关节的运动范围作为码垛机器人的工作空间，包括：结合码垛机器人的几何关系和机械约束，得到约束下第三关节的最小角度为max(joint3bias1,‑theta2‑m+joint3bias1)，约束下第三关节的最大角度为min(joint3max,180°‑m‑joint3bias2‑theta2)，约束下第二关节的最小角度为max(joint2min,‑theta3‑m+joint3bias1)，约束下第二关节的最大角度为min(joint2max,180°‑m‑joint3bias2‑theta3)；

则确定约束下第一关节的运动范围如下：

theta1∈[joint1min,joint1max]确定约束下第二关节的运动范围如下：

theta2∈[theta2min(theta3),theta2max(theta3)]theta2min(theta3)＝max(joint2min,‑theta3‑m+joint3bias1)theta2max(theta3)＝min(joint2max,180°‑m‑joint3bias2‑theta3)式中，theta2min(theta3)为第三关节约束下第二关节的最小角度，theta2max(theta3)为第三关节约束下第二关节的最大角度；

确定约束下第三关节的运动范围如下：

theta3∈[theta3min(theta2),theta3max(theta2)]theta3min(theta2)＝max(joint3bias1,‑theta2‑m+joint3bias1)theta3max(theta2)＝min(joint3max,180°‑m‑joint3bias2‑theta2)式中，theta3min(theta2)为第二关节约束下第三关节的最小角度，theta3max(theta2)为第二关节约束下第三关节的最大角度；

确定约束下第四关节的运动范围如下：

theta4∈[joint4min,joint4max]在关节运动方式下，各关节转角theta1、theta2、theta3和theta4中任意一个超出对应的运动范围，则码垛机器人不在工作空间内。

5.根据权利要求3所述的码垛机器人的工作空间确定方法，其特征在于，所述当运动方式为笛卡尔运动方式时，先确定机械臂末端在第一关节坐标系下的运动范围，包括：通过传感器获得各关节的转角theta1、theta2、theta3和theta4的值，然后根据各关节T转角值，通过以下公式计算机械臂末端在基座坐标系frame0下的位置为(x,y,z) ：则计算在第一关节坐标系frame1下，机械臂末端的位置为：T

机械臂末端在第一关节坐标系frame1下的位置记为p＝(x1 bias1 z1) ，固定第一关节，计算六段圆弧，其中C1,C2,C3,C4,C5,C6为圆心，其坐标分别为：C1:(L2*cos(m‑joint3bias1)+bias2,L2*sin(m‑joint3bias1)‑bias3)C2:(L1*cos(‑joint2min)+bias2,L1*sin(‑joint2min)‑bias3)C3:(bias2,‑bias3)

C4:(bias2,‑bias3)

C5:(L2*cos(180°‑m‑joint3bias2)+bias2,L2*sin(‑180°+m+joint3bias2)‑bias3)C6:(L1*cos(‑joint2max)+bias2,L1*sin(‑joint2max)‑bias3)式中，C1为第一段圆弧的圆心，C2为第二段圆弧的圆心，C3为第三段圆弧的圆心，C4为第四段圆弧的圆心，C5为第五段圆弧的圆心，C6为第六段圆弧的圆心；

六段圆弧的半径分别为：

r1＝L1

r2＝L2

r5＝L1

r6＝L2

式中，r1为第一段圆弧的半径，r2为第二段圆弧的半径，r3为第三段圆弧的半径，r4为第四段圆弧的半径，r5为第五段圆弧的半径，r6为第六段圆弧的半径；取六段圆弧所围成的区域作为机械臂末端在第一关节坐标系下的运动范围；

定义x(theta2,theta3)和z(theta2,theta3)为机械臂末端在第一关节坐标系frame1X1‑Z1平面的坐标：

x(theta2,theta3)＝L2*cos(theta2+theta3)+L1*cos(theta2)+bias2z(theta2,theta3)＝‑L2*sin(theta2+theta3)‑L1*sin(theta2)‑bias3计算第一关节坐标系frame1 X1‑Z1平面内六段圆弧交点p1,p2,p3,p4,p5,p6如下：p1:(x(joint2min,theta3min(joint2min)),z(joint2min,theta3min(joint2min)))p2:(x(‑m,theta3min(‑m)),z(‑m,theta3min(‑m)))p3:(x(joint2max,theta3min(joint2max)),z(joint2max,theta3min(joint2max)))p4:(x(joint2max,theta3max(joint2max)),z(joint2max,theta3max(joint2max))p5:(x(‑m,theta3max(‑m)),z(‑m,theta3max(‑m)))p6:(x(joint2min,theta3max(joint2min)),z(joint2min,theta3max(joint2min)))theta3min(joint2min)＝max(joint3bias1,‑joint2min‑m+joint3bias1)theta3min(‑m)＝max(joint3bias1,joint3bias1)theta3min(joint2max)＝max(joint3bias1,‑joint2max‑m+joint3bias1)theta3max(joint2max)＝min(joint3max,180°‑m‑joint3bias2‑joint2max)theta3max(‑m)＝min(joint3max,180°‑joint3bias2)theta3max(joint2min)＝min(joint3max,180°‑m‑joint3bias2‑joint2min)式中，theta3min(joint2min)为角度joint2min约束下第三关节的最小角度，theta3min(‑m)为角度m约束下第三关节的最小角度，theta3min(joint2max)为角度joint2max约束下第三关节的最小角度，theta3max(joint2max)为角度joint2max约束下第三关节的最大角度，theta3max(‑m)为角度m约束下第三关节的最大角度，theta3max(joint2min)为角度joint2min约束下第三关节的最大角度；

定义pix,i＝1,2,3,4,5,6为交点pi的横坐标，定义piz,i＝1,2,3,4,5,6为交点pi的纵坐标，按照降序将pix排列为pixj,j＝1,2,3,4,5,6,j表示次序，按照降序将piz排列为pizj,j＝

1,2,3,4,5,6，j表示次序；

计算 的

值，若incart(p)为零，则结束；否则继续执行；

其中函数incircle的通用格式为incircle(p′,center,r)，其中center为圆在第一关节坐标系frame1 X1‑Z1平面圆心的坐标(x',z')，r为圆的半径，坐标p′记录的机械臂末端在

2 2 2

第一关节坐标系frame1坐标系下X1‑Z1平面的坐标为(x″,z″)，如果(x'‑x″)+(z'‑z″) ≤r，则函数值为1，否则函数值为0；

其中函数inarea(x,y)的计算如下，定义直线line1的方程为y＝tan(joint1max)*x，定义直线line2的方程为y＝tan(joint1min)*x，直线line1和line2围成了一个锥形区域，当y＜tan(joint1max)*x且y＞tan(joint1min)*x时，函数值为1，否则函数值为0。

6.根据权利要求5所述的码垛机器人的工作空间确定方法，其特征在于，所述将在第一关节坐标系下的运动范围转换为基座坐标系下的运动范围，包括：确定机械臂末端在基座坐标系frame0下沿X轴和Y轴的运动范围，即笛卡尔运动方式下，机械臂末端沿X轴和Y轴的运动范围，步骤如下：取机械臂末端在第一关节坐标系下的运动范围作为工作区域，pi,i＝1,2,3,4,5,6将工作区域分成七个区域，遍历pizj，如果能找到使得z1≥pizj,j＝1,2,3,4,5,6的第一个pizj，那么机械臂末端此时在第j个区域；否则机械臂末端在第七个区域；

然后，根据确定的区域所覆盖的圆弧，计算在X1‑Z1平面内机械臂末端运动的最大和最小的X1坐标：其中，rp和rq为第p个圆和第q个圆的半径，Cpx和Cqx为第p个圆和第q个圆的圆心的x坐标，Cpz和Cqz为第p个圆和第q个圆的圆心的z坐标，xl为X1‑Z1平面内机械臂末端运动的最大X1坐标，xs为X1‑Z1平面机械臂末端运动的最小X1坐标；

计算在基座坐标系frame0下，Z＝z1平面内的两个同心圆，两个同心圆组成的圆环确定了在高度z1下机械臂末端的运动范围，同时也是工作空间的一个截面，两个同心圆的半径分别为：式中，rmax为两个同心圆中较大一个圆的半径，rmin为两个同心圆中较小一个圆的半径；

当joint1min＝‑180°和joint1max＝180°时，确定机械臂末端沿X移动的范围：式中，Xmax为机械臂末端沿X移动的最大值，Xmin为机械臂末端沿X移动的最小值；

确定机械臂末端沿Y移动的范围：

式中，Ymax为机械臂末端沿Y移动的最大值，Ymin为机械臂末端沿Y移动的最小值；

当joint1min≠‑180°或joint1max≠180°时，首先确定直线line1和line2的方程：line1:y＝tan(joint1max)*x

line2:y＝tan(joint1min)*x

此时工作空间变成了由直线line1和line2围成的锥形区域和圆环的交集，确定机械臂末端沿X移动的范围：其中函数 的计算如下，定义直线line3的方程为

定义直线line4的方程为

直线line3和line4围成了一个锥形区域，当 且时，函数值为1，否则函数值为0；

确定机械臂末端沿Y移动的范围：

确定笛卡尔运动方式下，机械臂末端沿Z移动的范围，步骤如下：遍历pixj，找到使得x1≥pixj,j＝1,2,3,4,5,6的第一个pixj，那么末端此时在第j个区域；

然后，根据确定的区域所覆盖的圆弧，计算在第一关节坐标系frame1 X1‑Z1平面末端内运动的最大和最小的Z1坐标：其中，rp和rq为第p个圆和第q个圆的半径，Cpx和Cqx为第p个圆和第q个圆的圆心的x坐标，Cpz和Cqz为第p个圆和第q个圆的圆心的z坐标，zl为在X1‑Z1平面内机械臂末端运动的最大Z1坐标，zs为在X1‑Z1平面内机械臂末端运动的最小的Z1坐标；当有不止一个解时，如果求沿Z轴移动的最大值Zmax，则取使得|zl‑z1|最小的解；如果求沿Z轴移动的最小值Zmin，则取使得|zs‑z1|最小的解；因此得到Zmax＝zl，Zmin＝zs。

7.根据权利要求6所述的码垛机器人的工作空间确定方法，其特征在于，所述根据基座坐标系下的运动范围生成码垛机器人的工作空间，包括：在第一关节坐标系frame1下得到工作空间内Z1坐标最大值对应的点Ztop＝(C1x bias1 T TC1z+r1) 和最小值对应的点Zdown＝(C6x bias1 C6z‑r6)，在线段 上，以间距d均匀的T取N个点，每个点对应的坐标记为Pn＝(xn bias1 zn)，然后对每个坐标Pn计算出对应高度下半径为rmin和rmax的两个同心圆，这两个同心圆平行于基座坐标系frame0的X‑Y平面，且两个同心圆围成的圆环部分和直线line1、line2围成的锥形区域确定了工作空间的一部分：当遍历完所有的Pn时，则得到了码垛机器人的整个工作空间，其中，C1x和C6x为第1个圆和第6个圆的圆心的x坐标，C1z和C6z为第1个圆和第6个圆的圆心的z坐标，n∈[1,N]。

8.根据权利要求4所述的码垛机器人的工作空间确定方法，其特征在于，当机械臂末端夹有货物时，取一个可以容纳货物的直径为w, 的球，其截面为一个圆，在圆上均匀的取M个点，则在第一关节坐标系frame1坐标系X1‑Z1平面下这M个点的坐标为：遍历集合s(x1,z1,w,M)中的每一个点，取使得incart(p2)函数的值为0的点的个数，记为n(s(x1,z1,w,M))，预设一个正整数作为阈值thres,thres＜M，incart(p2)函数的公式为：incart(p2)＝(1‑incircle(p2,C2,r2))*(1‑incircle(p2,C4,r4))*(1‑incircle(p2,C5,r5))*(incircle(p2,C1,r1)+incircle(p2,C3,r3)+incircle(p2,C6,r6)),p2∈s(x1,z1,w,M)当运动方式为关节运动方式时，如果n(s(x1,z1,w,M))＞thres，则此时码垛机器人不在工作空间内，结束；否则，更新约束下各个关节的运动范围如下：theta1∈[joint1min,joint1max]theta2∈[theta2min(theta3)+offsetmin2,theta2max(theta3)‑offsetmax2]其中：theta2min(theta3)＝max(joint2min,‑theta3‑m+joint3bias1)theta2max(theta3)＝min(joint2max,180°‑m‑joint3bias2‑theta3)theta3∈[theta3min(theta2)+offsetmin3,theta3max(theta2)‑offsetmax3]其中：theta3min(theta2)＝max(joint3bias1,‑theta2‑m+joint3bias1)theta3max(theta2)＝min(joint3max,180°‑m‑joint3bias2‑theta2)theta4∈[joint4min,joint4max]上式中offsetmin2、offsetmax2、offsetmin3和offsetmax3为机械臂末端夹持货物引起的角度偏移，按照如下流程进行求解：(1)初始化迭代起点v0，机械臂末端在第一关节坐标系frame1 X1‑Z1平面的坐标(x0,z0)，每次迭代的步长step，阈值thres，最大迭代次数maxn，定义x0坐标更新公式update_x0和z0坐标更新公式update_z0，取参数v1＝0，参数v2＝step；

当待求解参数为offsetmin2时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝theta2min(theta3)

x0＝x(theta2min(theta3),theta3)z0＝z(theta2min(theta3),theta3)update_x0＝x(v0+v1,theta3)

update_z0＝z(v0+v1,theta3)

当待求解参数为offsetmax2时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝theta2max(theta3)

x0＝x(theta2max(theta3),theta3)z0＝z(theta2max(theta3),theta3)update_x0＝x(v0‑v1,theta3)

update_z0＝z(v0‑v1,theta3)

当待求解参数为offsetmin3时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝theta3min(theta2)

x0＝x(theta2,theta3min(theta2))z0＝z(theta2,theta3min(theta2))update_x0＝x(theta2,v0+v1)

update_z0＝z(theta2,v0+v1)

当待求解参数为offsetmax3时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝theta3max(theta2)

x0＝x(theta2,theta3max(theta2))z0＝z(theta2,theta3max(theta2))update_x0＝x(theta2,v0‑v1)

update_z0＝z(theta2,v0‑v1)

(2)取当前迭代次数cnts＝0，参数curn＝n(s(x0,z0,w,M))，遍历集合s(x0,z0,w,M)中的每一个点，取使得incart(p3)函数的值为0的点的个数，记为n(s(x0,z0,w,M))，其中集合s(x0,z0,w,M)和incart(p3)函数表达式如下：incart(p3)＝(1‑incircle(p3,C2,r2))*(1‑incircle(p3,C4,r4))*(1‑incircle(p3,C5,r5))*(incircle(p3,C1,r1)+incircle(p3,C3,r3)+incircle(p3,C6,r6)),p3∈s(x0,z0,w,M)(3)若cnts＜maxn，则执行步骤(4)；否则输出v1的值作为待求解参数的值并结束；

(4)若curn≥thres或curn＝0，则执行步骤(5)；否则输出v1的值作为待求解参数的值并结束；

(5)若curn≥thres，则执行步骤(6)；否则执行步骤(7)；

(6)对参数进行迭代更新如下：

v2＝v1

v1＝v1+step

step＝step*2

x0＝update_x0

z0＝update_z0

curn＝n(s(x0,z0,w,M))

cnts＝cnts+1

更新完成后返回步骤(3)；

(7)对参数进行迭代更新如下：

v1＝(v1+v2)*0.5

x0＝update_x0

z0＝update_z0

curn＝n(s(x0,z0,w,M))

cnts＝cnts+1

更新完成后返回步骤(3)。

9.根据权利要求7所述的码垛机器人的工作空间确定方法，其特征在于，当机械臂末端夹有货物时，取一个可以容纳货物的直径为w, 的球，其截面为一个圆，在圆上均匀的取M个点，则在第一关节坐标系frame1坐标系X1‑Z1平面下这M个点的坐标为：遍历集合s(x1,z1,w,M)中的每一个点，取使得incart(p2)函数的值为0的点的个数，记为n(s(x1,z1,w,M))，预设一个正整数作为阈值thres,thres＜M，incart(p2)函数的公式为：incart(p2)＝(1‑incircle(p2,C2,r2))*(1‑incircle(p2,C4,r4))*(1‑incircle(p2,C5,r5))*(incircle(p2,C1,r1)+incircle(p2,C3,r3)+incircle(p2,C6,r6)),p2∈s(x1,z1,w,M)当运动方式为笛卡尔运动方式时，如果n(s(x1,z1,w,M))＞thres，则此时码垛机器人不在工作空间内，结束；否则更改xl，xs，zl和zs的计算方法如下，其它按照机械臂末端没有夹取货物时的方法计算确定工作空间：其中，rp和rq为第p个圆和第q个圆的半径，Cpx和Cqx为第p个圆和第q个圆的圆心的x坐标，Cpz和Cqz为第p个圆和第q个圆的圆心的z坐标，xl为X1‑Z1平面内机械臂末端运动的最大X1坐标，xs为X1‑Z1平面内机械臂末端运动的最小X1坐标，zl为在X1‑Z1平面内机械臂末端运动的最大的Z1坐标，zs为在X1‑Z1平面内机械臂末端运动的最小的Z1坐标，offsetxl,offsetxs,offsetzl和offsetzs为机械臂末端夹持货物引起的距离偏移，按照如下流程进行求解：(1)初始化迭代起点v0，机械臂末端在第一关节坐标系frame1 X1‑Z1平面的坐标(x0,z0)，每次迭代的步长step，阈值thres，最大迭代次数maxn，定义x0坐标更新公式update_x0和z0坐标更新公式update_z0，取参数v1＝0，参数v2＝step；

当待求解参数为offsetxl时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝xl

x0＝xl

z0＝z1

update_x0＝v0‑v1

update_z0＝z1

当待求解参数为offsetxs时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝xs

x0＝xs

z0＝z1

update_x0＝v0+v1

update_z0＝z1

当待求解参数为offsetzl时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝zl

x0＝x1

z0＝zl

update_x0＝x1

update_z0＝v0‑v1

当待求解参数为offsetzs时，v0、x0、z0、update_x0和update_z0初始化如下：v0＝zs

x0＝x1

z0＝zs

update_x0＝x1

update_z0＝v0+v1

(2)取当前迭代次数cnts＝0，参数curn＝n(s(x0,z0,w,M))，遍历集合s(x0,z0,w,M)中的每一个点，取使得incart(p3)函数的值为0的点的个数，记为n(s(x0,z0,w,M))，其中集合s(x0,z0,w,M)和incart(p3)函数表达式如下：incart(p3)＝(1‑incircle(p3,C2,r2))*(1‑incircle(p3,C4,r4))*(1‑incircle(p3,C5,r5))*(incircle(p3,C1,r1)+incircle(p3,C3,r3)+incircle(p3,C6,r6)),p3∈s(x0,z0,w,M)(3)若cnts＜maxn，则执行步骤(4)；否则输出v1的值作为待求解参数的值并结束；

(4)若curn≥thres或curn＝0，则执行步骤(5)；否则输出v1的值作为待求解参数的值并结束；

(5)若curn≥thres，则执行步骤(6)；否则执行步骤(7)；

(6)对参数进行迭代更新如下：

v2＝v1

v1＝v1+step

step＝step*2

x0＝update_x0

z0＝update_z0

curn＝n(s(x0,z0,w,M))

cnts＝cnts+1

更新完成后返回步骤(3)；

(7)对参数进行迭代更新如下：

v1＝(v1+v2)*0.5

x0＝update_x0

z0＝update_z0

curn＝n(s(x0,z0,w,M))

cnts＝cnts+1

更新完成后返回步骤(3)。