CSDN话题挑战赛第2期 参赛话题：学习笔记

1、前记

C站第5年，我还在分享机器人仿真和控制的基础内容，而且大多以MATLAB仿真为主要内容。从去年到现在为止在C站坚持学习记录的次数有所下降，现在慢慢回归到C站来，当然不排除有水的部分，不过关于机器人系统工具箱Robotics System Toolbox的学习记录在我的专栏MATLAB和机器人还是有很多介绍了。

机器人系统工具箱包括碰撞检查、路径规划、轨迹生成、正运动学和逆运动学以及使用刚体树表示的运动学和动力学算法也越来越成熟，反观之对学习机器人来说ROS及其生态圈好像要热闹很多【可能我对其不熟，所以不太喜欢ROS，每次配环境到处都是坑很容易把人搞懵】，但这不影响对MATLAB这个工具的喜爱。我也仍然在关注每一版MATLAB更新后对机器人部分的改善。

2.内容

下面的学习例子我是参考官网代码修改的，关于如何使用系统工具箱进行环境障碍物建模，机器人规划和碰撞检测。

主要参考链接也放在这里，感兴趣的还是建议看官网说明要通透些。Check for Environmental Collisions with Manipulators

（1） 构建环境和机器人

%% 构建环境% Create two platformsclcclearplatform1 = collisionBox(0.5,0.5,0.25);platform1.Pose = trvec2tform([-0.5 0.4 0.2]);platform2 = collisionBox(0.5,0.5,0.25);platform2.Pose = trvec2tform([0.5 0.2 0.2]);% Add a light fixture, modeled as a spherelightFixture = collisionSphere(0.1);lightFixture.Pose = trvec2tform([.2 0 1]);% Store in a cell array for collision-checkingworldCollisionArray = {platform1 platform2 lightFixture};%1，2，3；worldCollisionArray{1}%% 创建figure对象显示环境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% 为了演示修改颜色做下figure;ax=gca;% 显示桌子1并上色[~, patchObj]=show(platform1,'Parent', ax);patchObj.FaceColor = [1 0.6 0.9];axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围view(141,22)%调节视角hold on%显示桌子2并上色[~, patchObj]=show(platform2,'Parent', ax);patchObj.FaceColor = [1 0.6 0.9];% 显示灯并上色[~, patchObj]=show(lightFixture,'Parent', ax);patchObj.FaceColor = [0 1 0];%% 打光l = light;l.Color = [1 1 1];l.Position = [1 1 2];%% 添加机器人模型：可更改为DH方向构建。robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);show(robot,homeConfiguration(robot),"Parent",ax, 'Frames','off');

（2）设置起始位置和目标位置，逆解出机器人两个位置的关节配置并显示

%% 给机器人添加任务起点和终点startPose = trvec2tform([-0.5,0.5,0.6])*axang2tform([1 0 0 pi]);endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);%% 使用逆运动学求解起点个终点位姿的关节构型并显示rng(0);% Use a fixed random seed to ensure repeatable resultsik = inverseKinematics("RigidBodyTree",robot);weights = ones(1,6);startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);endEffector="EndEffector_Link";% Show initial and final positionsshow(robot,startConfig);show(robot,endConfig);collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

（3）梯形速度规划从起点到终点的轨迹

%% 使用梯形速度规划从起点到终点的轨迹【关节空间】[q,qd,qdd,t]= trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);hold onshow(robot,startConfig);axis([-0.8,1,-0.8,1.2,0,1.2])%调整图框范围view(141,22)%调节视角axis onhold ontitle('机器人根据Home位姿、起点和终点位姿直接梯形速度的运动')for i = 1:2:length(q)show(robot,q(:,i),"PreservePlot",false);%false 不留下重影poseNow = getTransform(robot, q(:,i), endEffector);%正运动学plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)drawnowend

（4）显示机器人运动的关节位置和速度等变化

figuresubplot(3,1,1)plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Position")grid onsubplot(3,1,2)plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint velocity")grid onsubplot(3,1,3)plot(t,qdd(1,:),'-r',t,qdd(2,:),'-b',t,qdd(3,:),'-c',t,qdd(4,:),'-.m',t,qdd(5,:),'.-r',t,qdd(6,:),'-.b',t,qdd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Acceleration")grid on

（5）查看机器人运动过程中是否与环境发生碰撞并显示碰撞部位

%%% 初始化输出inCollision = false(length(q),1); %===> zeros(length(q),1)worldCollisionPairIdx = cell(length(q),1); % 元胞数组，保存与环境碰撞的部件和关节配置的索引for i = 1:length(q)[inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");% 返回发生碰撞的构型索引（200个构型中的那个19到37）和距离。% sepDist为输出是机器人身体与世界碰撞对象之间的距离--->存储为矩阵%IgnoreSelfCollision and Exhaustivename-value on, Self collisions are% ignored .because the robot model joint limits prevent most self% collisions.免除自身碰撞的检查，因为关节限制保证了大多数自碰撞情况。只寻找构型与环境的碰撞情况[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距离是空NaN，则发生碰撞worldCollidingPairs = [bodyIdx,worldCollisionObjIdx];worldCollisionPairIdx{i} = worldCollidingPairs; %机器人部件索引第一列，环境物体索引第二列endisTrajectoryInCollision = any(inCollision)%% 检测到碰撞并可视化构型，第一个和最后一个检测到的碰撞collidingIdx1 = find(inCollision,1);%第一个：构型索引19collidingIdx2 = find(inCollision,1,"last");%最后一个：构型索引172% Identify the colliding rigid bodies.找到机器人中的碰撞部分collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';% Identify the colliding world bodies.找到环境中的碰撞部分collidingworld1 = worldCollisionPairIdx{collidingIdx1}*[0 1]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度collidingworld2 = worldCollisionPairIdx{collidingIdx2}*[0 1]';%% 显示环境，并将碰撞进行可视化ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Add the robotconfigurations & highlight the colliding bodies.show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);show(robot,q(:,collidingIdx2),"Parent"',ax);exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);% hightlight world objecthold on[~, patchObj]=show(worldCollisionArray{collidingworld1(1)},"Parent"',ax);patchObj.FaceColor = [1 0.8 0];[~, patchObj]=show(worldCollisionArray{collidingworld2},"Parent"',ax);patchObj.FaceColor = [1 0.8 0];title('发生碰撞的部分构型显示')

（6）在碰撞点附近添加中间过渡点，看是否能避障【MATLAB自带的RRT可以直接实现，以后介绍】

%% 创建新的轨迹--collision free。添加中间点intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % 球的周围intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % 桌子2上面intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));% Show the new intermediate posesax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',15)%%hold onshow(robot,intermediateConfig2,"Parent",ax);plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',15)

（7）插入中间点的梯形速度规划角度和速度变化

%% 根据新的中间点完成梯形速度规划[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],300,"EndTime",3);%显示规划的关节角度、速度变化曲线figuresubplot(2,1,1)plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Position")grid onsubplot(2,1,2)plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint velocity")grid on

（8）再次检测是否碰撞checkCollision函数，并显示运动

%% 再次检查是否有碰撞%Initialize outputsisCollision = false(length(q),1); %初始化碰撞构型的存储空间for i = 1:length(q)inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on");endisTrajectoryInCollision = any(inCollision)%%% Plot the environmentax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Visualize the robot in its home configurationshow(robot,startConfig,"Parent",ax2);axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围view(146,15)%调节视角hold onplot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',10)plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',10)title('添加中间点的无碰撞路径')% Loop through the other positionsfor i = 1:length(q)show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);poseNow = getTransform(robot, q(:,i), endEffector);%正运动学plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)drawnowend

完整代码如下：上述基本按小结运行可得到每部分结果显示

%% 检测在结构环境中机器人是否与物体之间发生碰撞情况，如何避免？% https://www.mathworks.com/help/robotics/ug/check-for-environmental-collisions-with-manipulators.html%% 构建环境% Create two platformsclcclearplatform1 = collisionBox(0.5,0.5,0.25);platform1.Pose = trvec2tform([-0.5 0.4 0.2]);platform2 = collisionBox(0.5,0.5,0.25);platform2.Pose = trvec2tform([0.5 0.2 0.2]);% Add a light fixture, modeled as a spherelightFixture = collisionSphere(0.1);lightFixture.Pose = trvec2tform([.2 0 1]);% Store in a cell array for collision-checkingworldCollisionArray = {platform1 platform2 lightFixture};%1，2，3；worldCollisionArray{1}%% 创建figure对象显示环境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% 为了演示修改颜色做下figure;ax=gca;% 显示桌子1并上色[~, patchObj]=show(platform1,'Parent', ax);patchObj.FaceColor = [1 0.6 0.9];axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围view(141,22)%调节视角hold on%显示桌子2并上色[~, patchObj]=show(platform2,'Parent', ax);patchObj.FaceColor = [1 0.6 0.9];% 显示灯并上色[~, patchObj]=show(lightFixture,'Parent', ax);patchObj.FaceColor = [0 1 0];%% 打光l = light;l.Color = [1 1 1];l.Position = [1 1 2];%% 添加机器人模型：可更改为DH方向构建。robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);show(robot,homeConfiguration(robot),"Parent",ax, 'Frames','off');%% 给机器人添加任务起点和终点startPose = trvec2tform([-0.5,0.5,0.6])*axang2tform([1 0 0 pi]);endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);%% 使用逆运动学求解起点个终点位姿的关节构型并显示rng(0);% Use a fixed random seed to ensure repeatable resultsik = inverseKinematics("RigidBodyTree",robot);weights = ones(1,6);startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);endEffector="EndEffector_Link";% Show initial and final positionsshow(robot,startConfig);show(robot,endConfig);collisionArray = exampleHelperManipCollisionsFromVisuals(robot);%% 使用梯形速度规划从起点到终点的轨迹【关节空间】[q,qd,qdd,t]= trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);hold onshow(robot,startConfig);axis([-0.8,1,-0.8,1.2,0,1.2])%调整图框范围view(141,22)%调节视角axis onhold ontitle('机器人根据Home位姿、起点和终点位姿直接梯形速度的运动')for i = 1:2:length(q)show(robot,q(:,i),"PreservePlot",false);%false 不留下重影poseNow = getTransform(robot, q(:,i), endEffector);%正运动学plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)drawnowend%% 显示规划的关节角度、速度变化曲线figuresubplot(3,1,1)plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Position")grid onsubplot(3,1,2)plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint velocity")grid onsubplot(3,1,3)plot(t,qdd(1,:),'-r',t,qdd(2,:),'-b',t,qdd(3,:),'-c',t,qdd(4,:),'-.m',t,qdd(5,:),'.-r',t,qdd(6,:),'-.b',t,qdd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Acceleration")grid on%%% 初始化输出inCollision = false(length(q),1); %===> zeros(length(q),1)worldCollisionPairIdx = cell(length(q),1); % 元胞数组，保存与环境碰撞的部件和关节配置的索引for i = 1:length(q)[inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");% 返回发生碰撞的构型索引（200个构型中的那个19到37）和距离。% sepDist为输出是机器人身体与世界碰撞对象之间的距离--->存储为矩阵%IgnoreSelfCollision and Exhaustivename-value on, Self collisions are% ignored .because the robot model joint limits prevent most self% collisions.免除自身碰撞的检查，因为关节限制保证了大多数自碰撞情况。只寻找构型与环境的碰撞情况[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距离是空NaN，则发生碰撞worldCollidingPairs = [bodyIdx,worldCollisionObjIdx];worldCollisionPairIdx{i} = worldCollidingPairs; %机器人部件索引第一列，环境物体索引第二列endisTrajectoryInCollision = any(inCollision)%% 检测到碰撞并可视化构型，第一个和最后一个检测到的碰撞collidingIdx1 = find(inCollision,1);%第一个：构型索引19collidingIdx2 = find(inCollision,1,"last");%最后一个：构型索引172% Identify the colliding rigid bodies.找到机器人中的碰撞部分collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';% Identify the colliding world bodies.找到环境中的碰撞部分collidingworld1 = worldCollisionPairIdx{collidingIdx1}*[0 1]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度collidingworld2 = worldCollisionPairIdx{collidingIdx2}*[0 1]';%% 显示环境，并将碰撞进行可视化ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Add the robotconfigurations & highlight the colliding bodies.show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);show(robot,q(:,collidingIdx2),"Parent"',ax);exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);% hightlight world objecthold on[~, patchObj]=show(worldCollisionArray{collidingworld1(1)},"Parent"',ax);patchObj.FaceColor = [1 0.8 0];[~, patchObj]=show(worldCollisionArray{collidingworld2},"Parent"',ax);patchObj.FaceColor = [1 0.8 0];title('发生碰撞的部分构型显示')%% 创建新的轨迹--collision free。添加中间点intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % 球的周围intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % 桌子2上面intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));% Show the new intermediate posesax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',15)%%hold onshow(robot,intermediateConfig2,"Parent",ax);plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',15)%% 根据新的中间点完成梯形速度规划[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],300,"EndTime",3);%显示规划的关节角度、速度变化曲线figuresubplot(2,1,1)plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint Position")grid onsubplot(2,1,2)plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')xlabel("Time")ylabel("Joint velocity")grid on%% 再次检查是否有碰撞%Initialize outputsisCollision = false(length(q),1); %初始化碰撞构型的存储空间for i = 1:length(q)inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on");endisTrajectoryInCollision = any(inCollision)%%% Plot the environmentax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Visualize the robot in its home configurationshow(robot,startConfig,"Parent",ax2);axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围view(146,15)%调节视角hold onplot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',10)plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',10)title('添加中间点的无碰撞路径')% Loop through the other positionsfor i = 1:length(q)show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);poseNow = getTransform(robot, q(:,i), endEffector);%正运动学plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)drawnowend

3、小结

这篇记录主要是使用机器人系统工具箱的函数完成环境设置和机器人运动碰撞检测，运动规划主要采用的是梯形速度规划，碰撞避免还是采用的最传统的方式—-在路径中添加中间过渡点实现。后续内容待续……..