Если вы пытаетесь свести к минимуму углы изгиба в суставах вашего робота, то наилучшей кривой будет дуга окружности.
Пусть L - длина дуги, d - расстояние между конечными точками, тета - половина угла дуги, а r - радиус дуги. Тогда:
d / L = sin (theta) / theta ==> решить это численно для theta, используя один из решателей Matlab
Как только у вас есть тета, радиус дуги будет: r = L / (2 * тета)
Есть еще бесконечное количество дуг между двумя конечными точками с этими r и theta. Вам нужно будет использовать некоторые другие критерии, чтобы выбрать угол поворота точек, которые нужно использовать.