我在 python 中针对非完整约束的四阶龙格库塔实现有什么问题？

Question

我正在尝试为类车机器人的非完整运动实现四阶龙格库塔。我不知道我做错了什么，本质上我是通过 +-Pi/4 来计算左右转弯以获得不同的轨迹。但无论我将 +pi/4 还是 -pi/4 传递给它，我都会得到相同的答案。我不知道我做错了什么。我使用的约束方程是：

thetadot = (s/L)*tan(phi)
xdot = s*cos(theta)
ydot = s*sin(theta)

其中 s 是速度，L 是机器人汽车的长度。

#! /usr/bin/env python
import sys, random, math, pygame
from pygame.locals import *
from math import sqrt,cos,sin,atan2,tan

import numpy as np
import matplotlib.pyplot as plt

XDIM = 640
YDIM = 480
WINSIZE = [XDIM, YDIM]
PHI = 45
s = 0.5
white = 255, 240, 200
black = 20, 20, 40
red = 255, 0, 0
green = 0, 255, 0
blue = 0, 0, 255
cyan = 0,255,255

pygame.init()
screen = pygame.display.set_mode(WINSIZE)


X = XDIM/2
Y = YDIM/2
THETA = 45
def main():
    nodes = []
    nodes.append(Node(XDIM/2.0,YDIM/2.0,0.0))
    plt.plot(runge_kutta(nodes[0], (3.14/4)))  #Hard Left turn
    plt.plot(runge_kutta(nodes[0], 0))         #Straight ahead
    plt.plot(runge_kutta(nodes[0], -(3.14/4))) #Hard Right turn
    plt.show()

class Node:
    x = 0
    y = 0
    theta = 0
    distance=0  
    parent=None
    def __init__(self,xcoord, ycoord, theta):
         self.x = xcoord
         self.y = ycoord
         self.theta = theta

def rk4(f, x, y, n):
    x0 = y0 =  0
    vx = [0]*(n + 1)
    vy = [0]*(n + 1)
    h = 0.8
    vx[0] = x = x0
    vy[0] = y = y0
    for i in range(1, n + 1):
        k1 = h*f(x, y)
        k2 = h*f(x + 0.5*h, y + 0.5*k1)
        k3 = h*f(x + 0.5*h, y + 0.5*k2)
        k4 = h*f(x + h, y + k3)
        vx[i] = x = x0 + i*h
        vy[i] = y = y + (k1 + k2 + k2 + k3 + k3 + k4)/6
    print "1"
    print vy
    return vy


def fun1(x,y):
    x = (0.5/2)*tan(y)
    print "2"
    print x
    return x

def fun2(x,y):
    x = 0.5*cos(y)
    print "3"
    print x
    return x

def fun3(x,y):
    x = 0.5*sin(y)
    print "4"
    print x
    return x

def runge_kutta(p, phi):
    x1 = p.x
    y1 = p.y
    theta1 = p.theta
    fi = phi
    for i in range(0,5):
        x2 = rk4(fun2, x1, theta1, 5)
        y2 = rk4(fun3, y1, theta1, 5)
        theta2 = rk4(fun1, theta1 ,fi, 5)
        theta1 = theta2
    print "5"
    print zip(x2,y2)
    return zip(x2,y2)





# if python says run, then we should run
if __name__ == '__main__':
    main()
    running = True
    while running:
       for event in pygame.event.get():
        if event.type == pygame.QUIT:
               running = False

Answer 1

关于算法我真的不能说太多，但是你设置我们的 rk4 函数的方式，x 和 y 参数永远不会有任何效果：

def rk4(f, x, y, n):
    x0 = y0 =  0          # x0 and y0 will both be 0 after this
    vx = [0]*(n + 1)
    vy = [0]*(n + 1)
    h = 0.8
    vx[0] = x = x0        # now x will be 0
    vy[0] = y = y0        # and y will be 0 too
    ...

函数的其余部分将在任何情况下使用 x=0 和 y=0。

此外，我不知道这是不是故意的，但是其他函数 fun1、fun2 和 fun3 从来没有使用作为 [=12= 传递的参数]，他们只使用y。他们在本地更改 x，但这不会反映在函数外部。

我在 python 中针对非完整约束的四阶龙格库塔实现有什么问题？

What is wrong with my Implementation of 4th Order runge kutta in python for nonholonomic constraints?

python

math

robotics

runge-kutta

motion-planning