具有方向约束尤其是在起始和目标位置的2D逆运动学，用于路径规划

我正在尝试使用一些逆向运动学算法来执行路径规划任务。我将自己的路径看作是具有连接段的运动链，并定义起点和目标。由于必须限制路径的曲率，因此必须限制每个接头处的最大弯曲角度。

此外，起始段的方向和最终段的方向应在一定的公差范围内明确定义。

我开始实施非常简单的FABRIK算法（youtube或paper）。我想，我还正确地添加了关于内部关节角度的角度约束，但是我无法正确地确定起始段和终止段的固定角度。

我开始时，运动链以曲线的方式展开，将起始角度与目标角度之间的角度差桥接起来。从那以后，我使用FABRIK算法并限制其关节弯曲以维持给定的限制。
这样，角度约束是算法的不变性，并且在运行期间不会被违反。在相当多的设置下，这可以正常进行。

但是，不幸的是，此算法在死锁的情况下一直运行，无法摆脱如下图所示的情况：
上面的面板显示了启动配置，下面的面板显示了死锁情况下18次迭代后的情况，没有更多进展。

紫色路径应从左气泡向右气泡转一圈，再从左向右进入右气泡（如红色草图所示）。

此行为必须归因于以下事实：头部段（目标之前的最后一个）不得转弯，因为它必须始终保持目标角度约束不变。

任何想法，如何克服这个问题，以及如何不仅实现位置目标值，还实现方向运动学的逆运动学。

干杯

费利克斯