轨迹跟踪比例P的实现move2pose

通过对当前航点到目标点的距离比例关系转换成速度。v = kp*s ，距离s越大 v越大，反之越小。随着距离的拉近速度变小为0.导致速度不够平滑。

会有快慢快慢的问题出现。连续点的时候需要进行优化。

目标角度对当前的航向角求比例转换成 角速度。

α = tan(dy,dx)- start_theta;
β = theta_goal - theta - (tan(dy,dx)-theta);
  = theta_goal - tan(dy,dx);
  
dθ = α + β = theta_goal - start_theta ;
上式中 dθ就是为要旋转的角度。dθ > 0 向右转。
dθ <0 向左转。
实际中需要加入 影响比例，该参数需要调试。
dθ = kα + pβ;k的值决定了向右转的幅度，p决定了向左转的幅度。
k越大转向角度越大，p同理.太小的话会导致转弯半径太大无法以最短的距离
行驶 到下一个航点。
 

python 源码

"""
Move to specified pose
Author: Daniel Ingram (daniel-s-ingram)
        Atsushi Sakai(@Atsushi_twi)
P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
"""

import matplotlib.pyplot as plt
import numpy as np
from random import random

# simulation parameters
Kp_rho = 9
Kp_alpha = 35.0#15
Kp_beta = -0.0#-3
dt = 0.01

show_animation = True

def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
    """
    rho is the distance between the robot and the goal position
    alpha is the angle to the goal relative to the heading of the robot
    beta is the angle between the robot's position and the goal position plus the goal angle
    Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal
    Kp_beta*beta rotates the line so that it is parallel to the goal angle
    """
    x = x_start
    y = y_start
    theta = theta_start

    x_diff = x_goal - x
    y_diff = y_goal - y

    x_traj, y_traj = [], []

    rho = np.hypot(x_diff, y_diff)
    dis = rho
    while rho > 0.001:
        x_traj.append(x)
        y_traj.append(y)

        x_diff = x_goal - x
        y_diff = y_goal - y

        # Restrict alpha and beta (angle differences) to the range
        # [-pi, pi] to prevent unstable behavior e.g. difference going
        # from 0 rad to 2*pi rad with slight turn

        rho = np.hypot(x_diff, y_diff)
        alpha = (np.arctan2(y_diff, x_diff)
                 - theta + 0 ) #% (2 * np.pi) - np.pi
        if  (alpha < -np.pi):                    
            alpha = alpha + 2 * np.pi 
        elif(alpha > np.pi):
            alpha = alpha - 2 * np.pi 
        #beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
        beta = theta_goal - theta - alpha
        if  (beta < -np.pi):                     
            beta = beta + 2 * np.pi 
        elif(beta > np.pi):
            beta = beta - 2 * np.pi        
        if rho > dis/2:
            v = Kp_rho * rho
        else :
            v = 0.5
        v = Kp_rho * rho
        w = Kp_alpha * alpha + Kp_beta * beta

        #if alpha > np.pi / 2 or alpha < -np.pi / 2:
            #v = -v
        print(rho,alpha*180/3.14,beta*180/3.14,v,w,x,y,theta*180/3.14)
        theta = theta + w * dt
        x = x + v * np.cos(theta) * dt
        y = y + v * np.sin(theta) * dt

        if show_animation:  # pragma: no cover
            plt.cla()
            plt.arrow(x_start, y_start, np.cos(theta_start),
                      np.sin(theta_start), color='r', width=0.1)
            plt.arrow(x_goal, y_goal, np.cos(theta_goal),
                      np.sin(theta_goal), color='g', width=0.1)
            plot_vehicle(x, y, theta, x_traj, y_traj)

def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
    # Corners of triangular vehicle when pointing to the right (0 radians)
    p1_i = np.array([0.5, 0, 1]).T
    p2_i = np.array([-0.5, 0.25, 1]).T
    p3_i = np.array([-0.5, -0.25, 1]).T

    T = transformation_matrix(x, y, theta)
    p1 = np.matmul(T, p1_i)
    p2 = np.matmul(T, p2_i)
    p3 = np.matmul(T, p3_i)

    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
    plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
    plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')

    plt.plot(x_traj, y_traj, 'b--')

    # for stopping simulation with the esc key.
    plt.gcf().canvas.mpl_connect('key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])

    plt.xlim(0, 20)
    plt.ylim(0, 20)

    plt.pause(dt)

def transformation_matrix(x, y, theta):
    return np.array([
        [np.cos(theta), -np.sin(theta), x],
        [np.sin(theta), np.cos(theta), y],
        [0, 0, 1]
    ])

def main():
    x_goal = 2
    for i in range(1):
        x_start = 1 
        y_start = 10 
        theta_start = 130*3.1415/180
        x_goal  =5 
        y_goal = 12 
        theta_goal = 50*3.1415/180
        print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
              (x_start, y_start, theta_start))
        print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
              (x_goal, y_goal, theta_goal))
        move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)

if __name__ == '__main__':
    main()

可以尝试修改KP ALPHA ,KP-BETA 进行测试。