一种码垛机器人弧形运动轨迹算法

摘要

一种码垛机器人弧形运动轨迹算法，包括以下步骤：（2）获取码垛机器人当前位置的坐标P1（x1，y1，z1）和目标位置的坐标P3（x3，y3，z3）；（3）码垛机器人读取输入的高度参数H，并将当前位置坐标P1和目标位置坐标P3转化为世界坐标；（4）将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2；（5）将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层；（6）在码垛机器人的用户应用层使用Circle指令运行规划圆弧运动轨迹。将码垛机器人的运动轨迹规划呈抛物线，减少机器人运动路程，使其减少每个周期的运动时间，同时使其运动过程更加柔性灵活。

主权项

一种码垛机器人弧形运动轨迹算法，其特征在于，包括以下步骤：（1）设定码垛机器人当前位置的坐标P1（x1，y1，z1），目标位置的坐标P3（x3，y3，z3），过渡点的坐标P2（x2，y2，z2）；坐标点P1为弧形运动轨迹的起点，坐标点P3为弧形运动轨迹的终点，坐标点P2（x2，y2，z2）为坐标点P1到坐标点P3的过渡点；坐标点P2到达由坐标点P1、P3形成的直线的垂直高度为H；（2）获取码垛机器人当前位置的坐标P1（x1，y1，z1）和目标位置的坐标P3（x3，y3，z3）；（3）码垛机器人读取输入的高度参数H，并将当前位置坐标P1和目标位置坐标P3转化为世界坐标；（4）将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2；（5）将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层；（6）在码垛机器人的应用层使用Circle指令运行规划圆弧运动轨迹；所述步骤（4）中，底层算法函数function P2=FuncSolve（ P1，P3，h ），根据P1，P3点求出中间的坐标：dist=（P1+P3）/2；dist包含三个元素dist（1）X轴坐标，dist（2）Y轴坐标，dist（3）Z轴坐标；码垛机器人第四轴垂直于水平面， TCP运行的抛物线轨迹垂直于XOY平面，假设垂直于此平面的一个方向量是（1，k，0），则k=( P1(1) – P3(1) ) / ( P3(2) ‑ P1(2) )；然后再根据圆弧曲线在XOY平面投影的距离求取P2(2)即过渡点Y轴的坐标；具体公式如下：fun = @(Y)(point3(1)+k*point3(2)‑k*Y‑dist(1))^2+(Y‑dist(2))^2+...(((point3(1)+k*point3(2)‑k*Y‑dist(1))*(point3(1)‑point1(1))+(Y‑dist(2))*(point3(2)‑point1(2)))/(point3(3)‑point1(3)))^2‑high^2P2（2）= FSOLVE(fun，1)其中FSOLVE是求变量为Y，fun=1时Y的值；然后根据已知k，P2(2)的值求取P2(1)的值，公式如下：P2(1) = point3(1)+k*point3(2)‑k*Y最后求出P2(3)：P2(3)=dist(3)‑((point3(1)+k*point3(2)‑k*Y‑dist(1))*(point3(1)‑point1(1))+(Y‑dist(2))*(point3(2)‑point1(2)))/(point3(3)‑point1(3))。