基于蚁群算法的六轴机械臂路径规划（运动学模型建立）
机器人运动学模型的建立
- 1 D-H参数法建立坐标系
- 2 机器人运动学分析
- 2.1 运动学正解
- 2.2 运动学逆解
- 3 机器人的轨迹仿真
1 D-H参数法建立坐标系
代码：
clear;
clc;
%建立机器人模型
% theta d a alpha offset
L1=Link([0 0.3 0.025 pi/2 0 ]); %定义连杆的D-H参数
L2=Link([pi/2 0 0.512 0 0 ]);
L3=Link([0 0 0.045 pi/2 0 ]);
L4=Link([0 0.504 0 pi/2 0 ]);
L5=Link([pi 0 0 pi/2 0 ]);
L6=Link([0 0.082 0 0 0 ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','Mechanical arm'); %连接连杆，机器人取名manman
robot.plot([0,pi/2,0,0,pi,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();
teach(robot);
机械臂连杆之间的相对关系是通过6个4×4的变换矩阵描述,由这6个依次变换的矩阵，可以得到执行机构的坐标系和固定参考坐标系之间的变换矩阵,最后推得机器人的运动方程相邻连杆之间的D-H坐标系变换矩阵由
ii−1T=Rot(Zi−1,θi)Trans(0,0,di)Transai(ai,0,0)Rot(Xi,ai)\begin{aligned} {}_{i}^{i-1}T=Rot\left( {{Z}_{i-1}},{{\theta }_{i}} \right)Trans\left( 0,0,{{d}_{i}} \right)Trans{{a}_{i}}\left( {{a}_{i}},0,0 \right)\text{Rot}\left( {{X}_{i}},{{a}_{i}} \right) \end{aligned} ii−1T=Rot(Zi−1,θi)Trans(0,0,di)Transai(ai,0,0)Rot(Xi,ai)
可以得到
ii−1T=[cosθi−sinθicosaisinθisinaiaicosθisinθicosθicosai−cosθisinaiaisinθi0sinaicosaidi0001]\begin{aligned} {}_{i}^{i-1}T=\left[ \begin{matrix} cos{{\theta }_{i}} & -sin{{\theta }_{i}}cos{{a}_{i}} & sin{{\theta }_{i}}sin{{a}_{i}} & {{a}_{i}}cos{{\theta }_{i}} \\ sin{{\theta }_{i}} & cos{{\theta }_{i}}cos{{a}_{i}} & -cos{{\theta }_{i}}sin{{a}_{i}} & {{a}_{i}}sin{{\theta }_{i}} \\ 0 & sin{{a}_{i}} & cos{{a}_{i}} & {{d}_{i}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned} ii−1T=⎣⎢⎢⎡cosθisinθi00−sinθicosaicosθicosaisinai0sinθisinai−cosθisinaicosai0aicosθiaisinθidi1⎦⎥⎥⎤
2 机器人运动学分析
2.1 运动学正解
10T=[cosθ10sinθ10sinθ10−cosθ10010d10001]\begin{aligned} _{1}^{0}T=\left[ \begin{matrix} cos{{\theta }_{1}} & 0 & sin{{\theta }_{1}} & 0 \\ sin{{\theta }_{1}} & 0 & -cos{{\theta }_{1}} & 0 \\ 0 & 1 & 0 & {{d}_{1}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\text{ } \end{aligned} 10T=⎣⎢⎢⎡cosθ1sinθ1000010sinθ1−cosθ10000d11⎦⎥⎥⎤ 21T=[cosθ2−sinθ20a2cosθ2sinθ2cosθ20a2sinθ200100001]\begin{aligned} {}_{2}^{1}T=\left[ \begin{matrix} cos{{\theta }_{2}} & -sin{{\theta }_{2}} & 0 & {{a}_{2}}cos{{\theta }_{2}} \\ sin{{\theta }_{2}} & cos{{\theta }_{2}} & 0 & {{a}_{2}}sin{{\theta }_{2}} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned} 21T=⎣⎢⎢⎡cosθ2sinθ200−sinθ2cosθ2000010a2cosθ2a2sinθ201⎦⎥⎥⎤32T=[cosθ30sinθ3a3cosθ3sinθ30−cosθ3a3sinθ301000001]\begin{aligned} {}_{3}^{2}T=\left[ \begin{matrix} cos{{\theta }_{3}} & 0 & sin{{\theta }_{3}} & {{a}_{3}}cos{{\theta }_{3}} \\ sin{{\theta }_{3}} & 0 & -cos{{\theta }_{3}} & {{a}_{3}}sin{{\theta }_{3}} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]~\text{ } \end{aligned} 32T=⎣⎢⎢⎡cosθ3sinθ3000010sinθ3−cosθ300a3cosθ3a3sinθ301⎦⎥⎥⎤ 43T=[cosθ40−sinθ40sinθ40cosθ400−10d40001]\begin{aligned} \text{ }{}_{4}^{3}T=\left[ \begin{matrix} cos{{\theta }_{4}} & 0 & -sin{{\theta }_{4}} & 0 \\ sin{{\theta }_{4}} & 0 & cos{{\theta }_{4}} & 0 \\ 0 & -1 & 0 & {{d}_{4}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned} 43T=⎣⎢⎢⎡cosθ4sinθ40000−10−sinθ4cosθ40000d41⎦⎥⎥⎤54T=[cosθ50−sinθ50sinθ50cosθ500−1000001]\begin{aligned} {}_{5}^{4}T=\left[ \begin{matrix} cos{{\theta }_{5}} & 0 & -sin{{\theta }_{5}} & 0 \\ sin{{\theta }_{5}} & 0 & cos{{\theta }_{5}} & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\text{ } \end{aligned} 54T=⎣⎢⎢⎡cosθ5sinθ50000−10−sinθ5cosθ5000001⎦⎥⎥⎤ 65T=[cosθ6−sinθ600sinθ6cosθ60000100001]\begin{aligned} \text{ }{}_{6}^{5}T=\left[ \begin{matrix} cos{{\theta }_{6}} & -sin{{\theta }_{6}} & 0 & 0 \\ sin{{\theta }_{6}} & cos{{\theta }_{6}} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned} 65T=⎣⎢⎢⎡cosθ6sinθ600−sinθ6cosθ60000100001⎦⎥⎥⎤
对于六自由度串联机器人, ai−1, α i−1,di, θ i{{\text{a}}_{\text{i}-1}},{{\text{ }\!\!\alpha\!\!\text{ }}_{\text{i}-1}},{{\text{d}}_{\text{i}}},{{\text{ }\!\!\theta\!\!\text{ }}_{\text{i}}}ai−1, α i−1,di, θ i四个参数的取值可根据其下标 而得到六个不同的齐次变换矩阵60T,…,65T{}_{6}^{0}\text{T},\ldots ,{}_{6}^{5}\text{T}60T,…,65T且有以下计算关系：
60T=60T−165T\begin{aligned} {}_{6}^{0}T={}_{6}^{0}{{T}^{-1}}{}_{6}^{5}T \end{aligned} 60T=60T−165T
连续右乘后得到的矩阵就是机器人的正运动计算方程，将关节的变量代入方程后即可求出夹爪的空间位姿。机械臂执行机构为
60T=[nxoxaxpxnyoyaypynzozazpz0001]\begin{aligned} {}_{6}^{0}T=\left[ \begin{matrix} {{n}_{x}} & {{o}_{x}} & {{a}_{x}} & {{p}_{x}} \\ {{n}_{y}} & {{o}_{y}} & {{a}_{y}} & {{p}_{y}} \\ {{n}_{z}} & {{o}_{z}} & {{a}_{z}} & {{p}_{z}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned} 60T=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
其中
nx=S6(C4S1−S4(C1C2C3−C1S2S3))+C6(C5(S1S4+C4(C1C2C3−C1S2S3))−S5(C1C2S3+C1C3S2)){{n}_{x}}={{S}_{6}}\left( {{C}_{4}}{{S}_{1}}-{{S}_{4}}\left( {{C}_{1}}{{C}_{2}}{{C}_{3}}-{{C}_{1}}{{S}_{2}}{{S}_{3}} \right) \right)+{{C}_{6}}\left( {{C}_{5}}\left( {{S}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{C}_{1}}{{C}_{2}}{{C}_{3}}-{{C}_{1}}{{S}_{2}}{{S}_{3}} \right) \right)-{{S}_{5}}\left( {{C}_{1}}{{C}_{2}}{{S}_{3}}+{{C}_{1}}{{C}_{3}}{{S}_{2}} \right) \right)nx=S6(C4S1−S4(C1C2C3−C1S2S3))+C6(C5(S1S4+C4(C1C2C3−C1S2S3))−S5(C1C2S3+C1C3S2))
ny=−S6(C1C4−S4(S1S2S3−C2C3S1))−C6(C5(C1S4+C4(S1S2S3−C2C3S1))+S5(C2S1S3+C3S1S2)){{n}_{y}}=-{{S}_{6}}\left( {{C}_{1}}{{C}_{4}}-{{S}_{4}}\left( {{S}_{1}}{{S}_{2}}{{S}_{3}}-{{C}_{2}}{{C}_{3}}{{S}_{1}} \right) \right)-{{C}_{6}}\left( {{C}_{5}}\left( {{C}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{S}_{1}}{{S}_{2}}{{S}_{3}}-{{C}_{2}}{{C}_{3}}{{S}_{1}} \right) \right)+{{S}_{5}}\left( {{C}_{2}}{{S}_{1}}{{S}_{3}}+{{C}_{3}}{{S}_{1}}{{S}_{2}} \right) \right)ny=−S6(C1C4−S4(S1S2S3−C2C3S1))−C6(C5(C1S4+C4(S1S2S3−C2C3S1))+S5(C2S1S3+C3S1S2))
nz=C6(S5(C2C3−S2S3)+C4C5(C2S3+C3S2)−S4S6(C2S3+C3S2)){{n}_{z}}={{C}_{6}}\left( {{S}_{5}}\left( {{C}_{2}}{{C}_{3}}-{{S}_{2}}{{S}_{3}} \right)+{{C}_{4}}{{C}_{5}}\left( {{C}_{2}}{{S}_{3}}+{{C}_{3}}{{S}_{2}} \right)-{{S}_{4}}{{S}_{6}}\left( {{C}_{2}}{{S}_{3}}+{{C}_{3}}{{S}_{2}} \right) \right)nz=C6(S5(C2C3−S2S3)+C4C5(C2S3+C3S2)−S4S6(C2S3+C3S2))
ox=C6(C4S2−S4(C1C2C3−C1S2S3))−S6(C5(S1S4+C4(C1C2C3−C1S2S3))−S5(C1C2S3+C1C3S2)){{o}_{x}}={{C}_{6}}\left( {{C}_{4}}{{S}_{2}}-{{S}_{4}}\left( {{C}_{1}}{{C}_{2}}{{C}_{3}}-{{C}_{1}}{{S}_{2}}{{S}_{3}} \right) \right)-{{S}_{6}}\left( {{C}_{5}}\left( {{S}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{C}_{1}}{{C}_{2}}{{C}_{3}}-{{C}_{1}}{{S}_{2}}{{S}_{3}} \right) \right)-{{S}_{5}}\left( {{C}_{1}}{{C}_{2}}{{S}_{3}}+{{C}_{1}}{{C}_{3}}{{S}_{2}} \right) \right)ox=C6(C4S2−S4(C1C2C3−C1S2S3))−S6(C5(S1S4+C4(C1C2C3−C1S2S3))−S5(C1C2S3+C1C3S2))
oy=S6(C5(C1S4+C4(S1S2S3−C2C3S1))+S5(C2S1S3+C3S1S2))−C6(C1C4−S4(S1S2S3−C2C3S1)){{o}_{y}}={{S}_{6}}\left( {{C}_{5}}\left( {{C}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{S}_{1}}{{S}_{2}}{{S}_{3}}-{{C}_{2}}{{C}_{3}}{{S}_{1}} \right) \right)+{{S}_{5}}\left( {{C}_{2}}{{S}_{1}}{{S}_{3}}+{{C}_{3}}{{S}_{1}}{{S}_{2}} \right) \right)-{{C}_{6}}\left( {{C}_{1}}{{C}_{4}}-{{S}_{4}}\left( {{S}_{1}}{{S}_{2}}{{S}_{3}}-{{C}_{2}}{{C}_{3}}{{S}_{1}} \right) \right)oy=S6(C5(C1S4+C4(S1S2S3−C2C3S1))+S5(C2S1S3+C3S1S2))−C6(C1C4−S4(S1S2S3−C2C3S1))
oz=−S6(S5(C2C3−S2S3)+C4C5(C2S3+C3S2))−C6S4(C2S3+C3S2){{o}_{z}}=-{{S}_{6}}\left( {{S}_{5}}\left( {{C}_{2}}{{C}_{3}}-{{S}_{2}}{{S}_{3}} \right)+{{C}_{4}}{{C}_{5}}\left( {{C}_{2}}{{S}_{3}}+{{C}_{3}}{{S}_{2}} \right) \right)-{{C}_{6}}{{S}_{4}}\left( {{C}_{2}}{{S}_{3}}+{{C}_{3}}{{S}_{2}} \right)oz=−S6(S5(C2C3−S2S3)+C4C5(C2S3+C3S2))−C6S4(C2S3+C3S2)
ax=S5(S1S4+C4(C1C2C3−C1S2S3)+C5(C1C2S3+C1C3S2)){{a}_{x}}={{S}_{5}}\left( {{S}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{C}_{1}}{{C}_{2}}{{C}_{3}}-{{C}_{1}}{{S}_{2}}{{S}_{3}} \right)+{{C}_{5}}\left( {{C}_{1}}{{C}_{2}}{{S}_{3}}+{{C}_{1}}{{C}_{3}}{{S}_{2}} \right) \right)ax=S5(S1S4+C4(C1C2C3−C1S2S3)+C5(C1C2S3+C1C3S2))
ay=C5(C2S1S3+C3S1S2−S5(C1S4+C4(S1S2S3−C2C3S1)){{a}_{y}}={{C}_{5}}\left( {{C}_{2}}{{S}_{1}}{{S}_{3}}+{{C}_{3}}{{S}_{1}}{{S}_{2}}-{{S}_{5}}\left( {{C}_{1}}{{S}_{4}}+{{C}_{4}}\left( {{S}_{1}}{{S}_{2}}{{S}_{3}}-{{C}_{2}}{{C}_{3}}{{S}_{1}} \right) \right) \right.ay=C5(C2S1S3+C3S1S2−S5(C1S4+C4(S1S2S3−C2C3S1))
az=C4S5(C2S3+C3S2)−C5(C2C3−S2S3){{a}_{z}}={{C}_{4}}{{S}_{5}}\left( {{C}_{2}}{{S}_{3}}+{{C}_{3}}{{S}_{2}} \right)-{{C}_{5}}\left( {{C}_{2}}{{C}_{3}}-{{S}_{2}}{{S}_{3}} \right)az=C4S5(C2S3+C3S2)−C5(C2C3−S2S3)
px=d4(C1C2S3+C1C3S2)+a2C1C2+a3C1C2C3−a3C1S2S3{{p}_{x}}={{d}_{4}}\left( {{C}_{1}}{{C}_{2}}{{S}_{3}}+{{C}_{1}}{{C}_{3}}{{S}_{2}} \right)+{{a}_{2}}{{C}_{1}}{{C}_{2}}+{{a}_{3}}{{C}_{1}}{{C}_{2}}{{C}_{3}}-{{a}_{3}}{{C}_{1}}{{S}_{2}}{{S}_{3}}px=d4(C1C2S3+C1C3S2)+a2C1C2+a3C1C2C3−a3C1S2S3
py=d4(C2S1S3+C3S1S2)+a2C2S1+a3C2C3S1−a3S1S2S3{{p}_{y}}={{d}_{4}}\left( {{C}_{2}}{{S}_{1}}{{S}_{3}}+{{C}_{3}}{{S}_{1}}{{S}_{2}} \right)+{{a}_{2}}{{C}_{2}}{{S}_{1}}+{{a}_{3}}{{C}_{2}}{{C}_{3}}{{S}_{1}}-{{a}_{3}}{{S}_{1}}{{S}_{2}}{{S}_{3}}py=d4(C2S1S3+C3S1S2)+a2C2S1+a3C2C3S1−a3S1S2S3
pz=a2S2−d4(C2C3−S2S3)+a3C2S3+a3C3S2{{p}_{z}}={{a}_{2}}{{S}_{2}}-{{d}_{4}}\left( {{C}_{2}}{{C}_{3}}-{{S}_{2}}{{S}_{3}} \right)+{{a}_{3}}{{C}_{2}}{{S}_{3}}+{{a}_{3}}{{C}_{3}}{{S}_{2}}pz=a2S2−d4(C2C3−S2S3)+a3C2S3+a3C3S2
代码如下：
theta=[0.0351121605635749 -0.729064371146205 -0.531988015514109 0 -1.88054026692948 3.10648049302622];%指定的关节角
p=robot.fkine(theta);%fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p
2.2 运动学逆解
在2.1节建立了六轴机器人的正运动学方程，给定各个关节的旋转角度就可以求出末端的位姿。然而在实际操作的过程中，情况恰恰相反。我们通常需要在知道末端位姿的情况下求出各个关节旋转角度,这就是机器人反向运动学的问题，也称为求运动学逆解，即由笛卡尔空间到关节空间的变换。
针对机器人求逆解的问题，求解方法多种多样，其中应用最广的是封闭解法，因为求封闭解计算速度快、效率高、便于实时控制。封闭解法包括两种方法：一种是代数解法，另一种是几何解法。目前已建立的一种系统化的代数解法为：运用变换矩阵就可得出一个可求解的三角函数方程式。重复上述过程，直到求解出所有的未知数。
六轴机器人运动学方程可以写为:
60T=10T21T32T43T54T65T=[nxoxaxpxnyoyaypynzozazpz0001]\begin{aligned} {}_{6}^{0}T={}_{1}^{0}T{}_{2}^{1}T{}_{3}^{2}T{}_{4}^{3}T{}_{5}^{4}T{}_{6}^{5}T=\left[ \begin{matrix} {{n}_{x}} & {{o}_{x}} & {{a}_{x}} & {{p}_{x}} \\ {{n}_{y}} & {{o}_{y}} & {{a}_{y}} & {{p}_{y}} \\ {{n}_{z}} & {{o}_{z}} & {{a}_{z}} & {{p}_{z}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \end{aligned}60T=10T21T32T43T54T65T=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
在该运动学方程中，等式左边的矩阵中的元素nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz{{\text{n}}_{\text{x}}},{{\text{n}}_{\text{y}}},{{\text{n}}_{\text{z}}},{{\text{o}}_{\text{x}}},{{\text{o}}_{\text{y}}},{{\text{o}}_{\text{z}}},{{\text{a}}_{\text{x}}},{{\text{a}}_{\text{y}}},{{\text{a}}_{\text{z}}},{{\text{p}}_{\text{x}}},{{\text{p}}_{\text{y}}},{{\text{p}}_{\text{z}}}nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz均为已知的，而等式右边的6个矩阵是未知的，它们的值取决于关节变量 θ 1,…, θ 6{{\text{ }\!\!\theta\!\!\text{ }}_{1}},\ldots ,{{\text{ }\!\!\theta\!\!\text{ }}_{6}} θ 1,…, θ 6的大小。用未知的连杆的逆变换左乘方程的两端，把关节变量分离出来，从而求出解，按照这种方法让矩阵左右两边的元素相等便可求出各个关节变量的值。
65T−1T=10T21T32T43T54T\begin{aligned} {}_{6}^{5}{{T}^{-1}}T={}_{1}^{0}T{}_{2}^{1}T{}_{3}^{2}T{}_{4}^{3}T{}_{5}^{4}T \end{aligned} 65T−1T=10T21T32T43T54T54T−165T−1T=10T21T32T43T\begin{aligned} {}_{5}^{4}{{T}^{-1}}{}_{6}^{5}{{T}^{-1}}T={}_{1}^{0}T{}_{2}^{1}T{}_{3}^{2}T{}_{4}^{3}T \end{aligned} 54T−165T−1T=10T21T32T43T43T−154T−165T−1T=10T21T32T\begin{aligned} {}_{4}^{3}{{T}^{-1}}{}_{5}^{4}{{T}^{-1}}{}_{6}^{5}{{T}^{-1}}T={}_{1}^{0}T{}_{2}^{1}T{}_{3}^{2}T \end{aligned} 43T−154T−165T−1T=10T21T32T32T−143T−154T−165T−1T=10T\begin{aligned} {}_{3}^{2}{{T}^{-1}}{}_{4}^{3}{{T}^{-1}}{}_{5}^{4}{{T}^{-1}}{}_{6}^{5}{{T}^{-1}}T={}_{1}^{0}T \end{aligned}32T−143T−154T−165T−1T=10T
代码：
T1=transl(452.120,149.090,-432.450);%根据给定起始点，得到起始点位姿
T2=transl(514.1,-155.5,-136);%根据给定终止点，得到终止点位姿
q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
q2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角
3 机器人的轨迹仿真
为了更好地验证机器人数学模型的正确性以及方便地进行带控制参数的运动仿真，本文基于Robotics Toolbox for MATLAB工具箱，对所建立的六自由度机器人数学模型进行验证。主要代码如下：
利用Link指令，可以将机器人的六个轴的参数一一指定。利用display指令，可以在command window处检查输入的D-H模型是否正确。利用teach指令，可以得到机器人的棍棒模型，如图2所示。
图2 六轴机器人的棍棒模型
对于机器人的运动控制来说，常用的轨迹规划方式是点到点模式，本文选取了一个抬手臂的动作，对夹爪的位姿进行轨迹规划。
利用jtraj指令，可以将两点间的轨迹用一条五阶多项式曲线逼近，以确保机器人在运动过程中的平滑性，图3为机械臂的运动轨迹。利用plot指令，可以将规划的轨迹用动画仿真的形式展现，图4为机械臂的棍棒运动模型。
代码：
T1=transl(0.5,0.2,0);%根据给定起始点，得到起始点位姿
T2=transl(0.5,-0.2,0.9);%根据给定终止点，得到终止点位姿
q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
q2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T = robot.fkine(q);%根据插值，得到末端执行器位姿 (正向运动学，为了画出末端轨迹)
t = [];
for i=1:50[R_temp, t_temp] = tr2rt(T(i));t = [t t_temp];
end
plot3(t(1, :), t(2, :), t(3, :), 'linewidth', 3);%输出末端轨迹
hold on
% while(1)
% robot.plot(q);%动画演示
% end
robot.plot(q);%动画演示figure(2)
i=1:4;
subplot(2,2,1);
plot(q(:,i));
title('位置');
grid on;
subplot(2,2,2);
i=1:4;
plot(qd(:,i));
title('速度');
grid on;
subplot(2,2,3);
i=1:4;
plot(qdd(:,i));
title('加速度');
grid on;Tc=ctraj(T1,T2,50);%利用匀加速匀减速规划轨迹
Tjtraj=transl(Tc);
subplot(2,2,4);
plot2(Tjtraj,'r');
title('p1到p2直线轨迹');
图3 机械臂的运动轨迹
图4 机械臂的棍棒运动模型
基于蚁群算法的六轴机械臂路径规划（运动学模型建立）相关推荐
- 【机械】基于简化几何解法的六轴机械臂位置规划附matlab代码
1 内容介绍 基于简化几何解法的六轴机械臂位置规划附matlab代码 2 部分代码 clc; clear; %载入数据 importfile('shuiping.mat'); theta_shuipi ...
- 【机械仿真】基于matlab简化几何解法六轴机械臂位置规划【含Matlab源码 2128期】
⛄一.获取代码方式 获取代码方式1: 完整代码已上传我的资源:[机械仿真]基于matlab简化几何解法六轴机械臂位置规划[含Matlab源码 2128期] 点击上面蓝色字体,直接付费下载,即可. 获取 ...
- 基于MATLAB的关节型六轴机械臂轨迹规划仿真（2021实测完整代码）
我是一个目录 基于MATLAB的关节型六轴机械臂轨迹规划仿真 1 实验目的 2 实验内容 2.1标准D-H参数法 2.2实验中使用的Matlab函数 3实验结果 4 全部代码 基于MATLAB的关节型 ...
- 基于粒子群算法的智能车辆避障路径规划方法研究
基于粒子群算法的智能车辆避障路径规划方法研究 1.环境生成 1.1 环境生成方法的选择 1.2 坐标法生成环境 1.3 车辆简化 1.4 障碍物数据 2.粒子初始化 2.1 速度迭代设置 2.2 避障 ...
- 关键路径-stl版_A*算法与蚁群算法相结合的无人艇巡逻路径规划
作者:张丹红,陈文文,张华军,苏义鑫 摘要:针对无人艇海上巡逻路径规划问题,提出了一种A*算法与蚁群算法相结合进行最短巡逻路径优化的方法．在传统A*算法的八角度搜索基础上,设计了一种多角度A*算法以获 ...
- 【Matlab DVRP】蚁群算法求解带距离的车辆路径规划问题【含源码 1040期】
一.代码运行视频(哔哩哔哩) [Matlab DVRP]蚁群算法求解带距离的车辆路径规划问题[含源码 1040期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1 ...
- 【TWVRP】基于matlab蚁群算法求解带时间窗车辆路径规划问题【含Matlab源码 1930期】
⛄一.VRP简介 1 VRP基本原理 车辆路径规划问题(Vehicle Routing Problem,VRP)是运筹学里重要的研究问题之一.VRP关注有一个供货商与K个销售点的路径规划的情况,可以简 ...
- 【无人机】基于matlab蚁群算法求解含危险源的无人机路径规划【含Matlab源码 2059期】
一.无人机简介 0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下 ...
- 【TWVRP】蚁群算法求解带时间窗车辆路径规划问题【含Matlab源码 1930期】
⛄一.VRP简介 1 VRP基本原理 车辆路径规划问题(Vehicle Routing Problem,VRP)是运筹学里重要的研究问题之一.VRP关注有一个供货商与K个销售点的路径规划的情况,可以简 ...
最新文章
- python使用numpy的np.power函数计算numpy数组中每个数值的指定幂次（例如平方、立方）、np.power函数默认返回整数格式、np.float_power函数默认返回浮点数
- ZStack中的编程技巧
- git提交代码之git-GUI-Here提交
- 隐马尔科夫模型原理解析
- 人生苦短，我用Python（Python快速教程 - 基础篇）
- Python学习笔记——GIF倒放处理
- java中字符串（1）
- NLP《词汇表示方法（七）BERT》
- Java基础通信_Java网络通信基础编程(必看篇)
- android 导航自动切换,Android导航抽屉切换图标向右
- 使用samba实现linux与windows共享（测试成功）
- 关闭Windows自动更新—大法
- BIM族库下载——Revit配景族
- 华为鸿蒙系统自带什么输入法,搜狗输入法华为鸿蒙系统OS2.0版下载-搜狗输入法华为鸿蒙版v10.15.2 手机版-腾飞网...
- python批量保存网页为pdf_在chrome Python3.6中自动打印/将网页保存为pdf
- 查找图片中隐藏的flag
- 用ViewPager实现微信tab切换
- java连连看怎么加音效_怎么给视频加音效
- 一键打开 macOS Mojave 的黑暗模式
- docker配置centos7（二），dnf，sshd配置及部分其它常用软件