unity六轴机器⼈⾃动移动路径
unity六轴机器⼈⾃动移动路径
六轴机器⼈如下图:
⾸先我们要明⽩在unity⾥⾯机器⼈的移动是在⼀个⾯上(取巧).只需要让他的⼀轴转90度可以⾃由的左右移动就可以了.这时候机器⼈的六轴想移动到哪⼀个点只需要移动对应的⼆三五轴就可以.
为了确定六轴应该移动到空间的哪⼀个点，我们设置⼀个cube 让他和机器⼈在⼀个平⾯上即z轴保持⼀致.然后将cube为⼿柄射线的⼦物体跟随射线移动.
六轴只需要不停的去追逐这个cube的位置就可以了.直到六轴的位置和cube的位置⼩到⼀定程度才会停⽌⾃动移动路径.
机器⼈搜索路径的时候分为多种情况:
北京考试教育院
1第⼀种情况机器⼈只需要移动五轴就可以碰触到cube.这种情况就⽤向量判断⼀下五轴的.up和五轴向cube的向量的⾓度，移动对应⾓度就可以.
不过这种情况需要注意五轴的⾓度会有限制(ps：轴限制在-135到135度).如果五轴的⾓度超出了限制就让他只移动极限⾓度并且移动下⽅的⼆轴.⼆轴向cube的反⽅向移动20度.然后重新判断情况移动距离.(通过向量叉乘判断⽅向正反)
2第⼆种情况机器⼈五轴的距离不够长够不着，这个时候再判断⼀次cube和三轴的距离是否⼤于三轴⼩于三轴加五轴的距离，如果是，这个时候只需要先移动三轴和cube之间的⾓度(ps；和五轴判断⾓度⼀样，使⽤向量vector3)，在移动五轴和cube之间的⾓度就可以碰触到cube 然后停
⽌⾃动搜索路径.(ps限制⾓度都为-135到135)
3.第三种情况是机器⼈五轴的距离不够长够不着，但是cube和三轴的距离⼩于三轴的长度.这种情况就相当于⼀个东西在你的⼩臂附近，这种情况下单单移动三轴和五轴是⽆法碰触到cube的(想象⼀下⼿臂上有个苹果，只⽤⼿和⼩臂去拿，肯定拿不到).这种情况需要移动⼆轴,⼆轴向cube的反⽅向移动20度，让cube不在紧贴在⼩臂附近.return退出⽅法重新检测直到不是这种情况为⽌.
4第四种情况 ⼆轴的情况和三轴类似，判断cube到⼆轴的距离⼩于尔州的长度说明在⼆轴附近，需要向外移动⼆轴 ,⼆轴向cube的反⽅向移动20度,return退出重新判断
5第五种情况是cube到⼆轴的距离⼤于三轴加五轴的长度⼩于⼆轴加三轴加五轴的长度.这种情况 先移动⼆轴到cube的⾓度，在移动三轴到cube的⾓度在移动五轴到cube的⾓度即可
6第六种情况是cube到⼆轴的距离⼤于⼆轴加三轴加五轴的长度 这种情况 先移动⼆轴到cube的⾓度，在移动三轴到cube的⾓度在移动五轴到cube的⾓度即可
ps: 所有的轴⾓度限制-135到135 计算如果超出⾓度就为极限⾓度.⼆三轴到极限⾓度之后就需要反⽅向移动⼆轴.⼆轴到极限⾓度不需要移动了.说明整个机器⼈都到了极限⾓度，到达了碰触不到的地⽅.
以下为⽅法代码:
public void RobotMoveAI()
{//⼀轴处在90度位置
transform.GetChild(0).localRotation = Quaternion.Euler(0,90,0);
// 每次移动的⾓度除以这个数
float wuscale = 1;
float sanscale = 1;
float erscale = 1;
Vector3 toErzhou;
Vector3 toSanzhou;
Vector3 toWuzhou;
Vector3 isZuoYou;
float isZF;
float angle;
//判断位置是否超出
if (JudgeDistance(ansform.ansform.h
ouLength+modle.sanzhouLength+modle.wuzhou Length))
{
//判断位置在五轴长度能否够得着
if (JudgeDistance(ansform.position, ansform.position, modle.wuzhouLength))
{浔埔女
//到wu轴的向量
toWuzhou = ansform.position - ansform.position;答辩
//判断在左右
isZuoYou = Vector3.Cross(toWuzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
angle = Vector3.Angle(ansform.up, toWuzhou);
modle.wuzhouAngle = Rota(ansform);
modle.wuzhouAngle += angle / wuscale * isZF;
if (modle.wuzhouAngle <= 135f && modle.wuzhouAngle >= -135)
{
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3(modle .wuzhouAngle, 0, 0)));
}
el
{
Debug.LogError("五轴距离越过极限，移动⼆轴");
toErzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toErzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
// angle = Vector3.ansform.up, toErzhou);
if (houAngle <= 135f && houAngle >= -135)
{
}
el
{
Debug.LogError("五轴距离越过极限，移动⼆轴，⼆轴⾓度为" + houAngle);
// RobotTCP();
}
//ansform.localRotation = Quaternion.Euler(new houAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3(modle .erzhouAngle, 0, 0))); //Quaternion.Euler(new houAngle, 0, 0));
/
/⼆轴向外移动重新判断是否还在⾥⾯
return;
RobotTCP();
}
// ansform.localRotation = Quaternion.Euler(new Vector3(modle.wuzhouAngle, 0, 0));
}
//五轴够不着
el
{
//判断三轴距离三周够的着说明是在⾥⾯需要向外移动⼆轴
if (JudgeDistance(ansform.position, ansform.position, modle.sanzhouLength))
{
toErzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toErzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
// angle = Vector3.ansform.up, toErzhou);
if (houAngle <= 135f && houAngle >= -135)
{
}
el
{葱葱绿绿
Debug.LogError("三轴⾥⾯移动⼆轴⼆轴⾓度为" + houAngle);
// RobotTCP();
}
//ansform.localRotation = Quaternion.Euler(new houAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3(modle .er
zhouAngle, 0, 0))); //Quaternion.Euler(new houAngle, 0, 0));
//⼆轴向外移动重新判断是否还在⾥⾯
return;
}
//sanzhou够不着
el
{//判断三轴加五轴距离是否够的着
if (JudgeDistance(ansform.position, ansform.position, modle.sanzhouLength + modle.wuzhouLength)) {
//到san轴的向量
toSanzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toSanzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
angle = Vector3.Angle(ansform.up, toSanzhou);
modle.sanzhouAngle = Rota(ansform);
modle.sanzhouAngle += angle / sanscale * isZF;
if (modle.sanzhouAngle <= 135f && modle.sanzhouAngle >= -135)
{
云的英语怎么读}
el
{
modle.sanzhouAngle = modle.sanzhouAngle > 0 ? 135 : -135;
Debug.LogError("san轴⾓度到达极限，接着移动五轴，三轴⾓度为:"+ modle.sanzhouAngle);
// RobotTCP();
}
// ansform.localRotation = Quaternion.Euler(new Vector3(modle.sanzhouAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( modle.sanzhouAngle, 0, 0)));
//到wu轴的向量
toWuzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toWuzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
angle = Vector3.Angle(ansform.up, toWuzhou);
modle.wuzhouAngle = Rota(ansform);
modle.wuzhouAngle += angle / wuscale * isZF;
if (modle.wuzhouAngle <= 135f && modle.wuzhouAngle >= -135)
{
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( modle.wuzhouAngle, 0, 0)));
}
el
{
modle.wuzhouAngle = modle.wuzhouAngle > 0 ? 135 : -135;
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( modle.wuzhouAngle, 0, 0)));
Debug.LogError("三轴移动之后，五轴⾓度越过极限，接着移动⼆轴，五轴⾓度为" + modle.wuzhouAngle);
//接下来只能移动⼆轴了
toErzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toErzhou, ansform.up);
/
/是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
// angle = Vector3.ansform.up, toErzhou);
我想握住你的手if (houAngle <= 135f && houAngle >= -135)
{
}
el
{
Debug.LogError("三轴移动，五轴移动，移动⼆轴，⼆轴⾓度为" + houAngle);
// RobotTCP();
}
//ansform.localRotation = Quaternion.Euler(new houAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( houAngle, 0, 0)));
// RobotTCP();
}
// ansform.localRotation = Quaternion.Euler(new Vector3(modle.wuzhouAngle, 0, 0));
}//三轴加五轴够不着加⼆轴肯定够的着
el
{//判断是否在⼆轴附近需要向外移动⼆轴
if (JudgeDistance(ansform.position, ansform.position, houLength))
{
toErzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toErzhou, ansform.up);
用美不胜收造句//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
// angle = Vector3.ansform.up, toErzhou);
胃疼的症状if (houAngle <= 135f && houAngle >= -135)
{
}
el
{
Debug.LogError("er轴⾓度达到极限");
// RobotTCP();
}
// ansform.localRotation = Quaternion.Euler(new houAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( houAngle, 0, 0)));
//⼆轴向外移动重新判断是否还在⾥⾯
return;
}
// 加⼆轴肯定够的着
el
{
//到⼆轴的向量
toErzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toErzhou, ansform.up);
//是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
angle = Vector3.ansform.up, toErzhou);
if (houAngle <= 135f && houAngle >= -135)
{
}
el
{
Debug.LogError("er轴⾓度达到极限");
}
// ansform.localRotation = Quaternion.Euler(new houAngle, 0, 0));
ansform.localRotation = ZhouMoveLerp(ansform.localRotation, Quaternion.Euler(new Vector3( houAngle, 0, 0)));
//到san轴的向量
toSanzhou = ansform.position - ansform.position;
//判断在左右
isZuoYou = Vector3.Cross(toSanzhou, ansform.up);
/
/是否正负
isZF = isZuoYou.z / Mathf.Abs(isZuoYou.z);
angle = Vector3.Angle(ansform.up, toSanzhou);
modle.sanzhouAngle = Rota(ansform);
modle.sanzhouAngle += angle / sanscale * isZF;
if (modle.sanzhouAngle <= 135f && modle.sanzhouAngle >= -135)
{
}
el
{
