我无法上传图片,所以我会尽力最好地解释我的问题。我想模拟独轮车型机器人对移动物体的检测。机器人以位置 (x,y) 和方向 theta 作为三个状态进行建模。障碍物表示为半径为 r1 的圆。我想找到从机器人的局部坐标系到圆的角度 alpha_1 和 alpha_2,如下所示:
所以我正在做的是尝试找到从机器人到连接机器人和圆心的线的角度(这个角度在我的代码中称为 aux_t ),然后找到切线和同一条线之间的角度(称为 phi_c)。最后,我通过在 aux_t 中添加和减去 phi_c 来找到我想要的角度。我想到的图如下所示:
问题是,当我尝试查找 alpha 角度时,我的代码遇到了麻烦:它开始正确计算角度(尽管是负值,不确定这是否会造成我的麻烦),但随着汽车和cicle 越来越近,phi_c 变得比 aux_t 大,并且其中一个 alpha 突然改变其符号。例如我得到这个:
aux_t//////phi_c//////alpha_1//////alpha_2
-0.81//////+0.52//////-1.33//////-0.29
-0.74//////+0.61//////-1.35//////-0.12
-0.69//////+0.67//////-1.37//////-0.02
-0.64//////+0.74//////-1.38//////+0.1
所以基本上,alpha_2 在这里得到了错误的形式。我知道我做错了什么,但我不确定是什么,我不知道如何限制从 0 到 pi 的角度。有没有更好的方法来找到 alpha 角? 这是我的代码部分:
最佳答案
就你的数学而言,我要做的唯一改变就是从目标角度减去(pi/2 - theta)
而不是相加。这将为您提供更典型方向的角度(逆时针为正)。
我不完全确定为什么您认为您在答案中发布的数据中的 alpha_2
是错误的。发生的情况是,您的机器人非常接近目标,并且 alpha_2
切线实际上移动到另一侧或指向机器人所看方向的线。我在这里创建了一个类似的情况,其中切线上的标签是相对于机器人的角度(黄线),并且所有角度都强制在 0 到 2*pi 之间。
解决有关将角度强制在特定范围内的问题。为此,您需要使用模数(MATLAB 中的 mod
)。在这些示例中,我使用了 mod(theta, 2*pi)
,因为从技术上讲,如果您的机器人背对目标,则角度可能会大于 pi。
作为测试,我进行了一个简单的模拟,移动机器人并显示切线相对于机器人方向的角度(同样在 0 到 2pi 之间)
如果您确实希望角度介于 0 和 pi 之间,则可以使用 mod(theta, pi)
代替。
alpha_1 = mod(alpha_1, pi);
alpha_2 = mod(alpha_2, pi);
关于matlab - 与圆切线的角度,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/35970213/