【待学记录】人工势场 | 正在更新ing_r = np.sqrt(deltax**2 + deltay**2 + deltaz**2)-优快云博客

本文链接：https://blog.youkuaiyun.com/m0_47171044/article/details/146001883

对网络上相关代码进行学习（批注版）

1.filed.py【代码是错的，别看了活爹】

感觉计算角度这里很难理解啊，相当于是把所有角度都变到第一象限里面？

# 辅助函数：计算角度
def compute_angle(X, Xsum, n):
    angles = []  # 初始化角度列表
    for i in range(n + 1):  # 遍历目标和所有障碍点
        deltaX = Xsum[i, 0] - X[0]  # 计算x方向的差值
        deltaY = Xsum[i, 1] - X[1]  # 计算y方向的差值
        r = np.sqrt(deltaX**2 + deltaY**2)  # 计算两点之间的距离
        if deltaX > 0:  # 如果x方向差值为正
            theta = np.arccos(deltaX / r)  # 计算夹角
        else:  # 如果x方向差值为负
            theta = np.pi - np.arccos(deltaX / r)  # 计算夹角
        angles.append(theta)  # 将计算的角度添加到列表
    return np.array(angles)  # 返回角度数组

改了一下起点和终点的位置，果然算不出来

原结果和代码

# https://blog.youkuaiyun.com/weixin_44141594/article/details/117288990
import numpy as np  # 导入numpy库，用于数学运算
import matplotlib.pyplot as plt  # 导入matplotlib库，用于绘图

# 辅助函数：计算角度
def compute_angle(X, Xsum, n):
    angles = []  # 初始化角度列表
    for i in range(n + 1):  # 遍历目标和所有障碍点
        deltaX = Xsum[i, 0] - X[0]  # 计算x方向的差值
        deltaY = Xsum[i, 1] - X[1]  # 计算y方向的差值
        r = np.sqrt(deltaX**2 + deltaY**2)  # 计算两点之间的距离
        if deltaX > 0:  # 如果x方向差值为正
            theta = np.arccos(deltaX / r)  # 计算夹角
        else:  # 如果x方向差值为负
            theta = np.pi - np.arccos(deltaX / r)  # 计算夹角
        angles.append(theta)  # 将计算的角度添加到列表
    return np.array(angles)  # 返回角度数组

# 辅助函数：计算引力
def compute_attract(X, Xsum, k, angle, Po, n):
    Rat = (X[0] - Xsum[0, 0])**2 + (X[1] - Xsum[0, 1])**2  # 计算目标点与当前位置的距离平方
    rat = np.sqrt(Rat)  # 计算距离
    Fatx = k * rat * np.cos(angle)  # 计算引力在x方向的分量
    Faty = k * rat * np.sin(angle)  # 计算引力在y方向的分量
    return Fatx, Faty  # 返回引力分量

# 辅助函数：计算斥力
def compute_repulsion(X, Xsum, m, angle_at, angle_re, n, Po, a):
    Rat = (X[0] - Xsum[0, 0])**2 + (X[1] - Xsum[0, 1])**2  # 计算目标点与当前位置的距离平方
    rat = np.sqrt(Rat)  # 计算距离
    Yrerxx = 0  # 初始化斥力在x方向的总分量
    Yreryy = 0  # 初始化斥力在y方向的总分量
    Yataxx = 0  # 初始化引力方向上的斥力分量（x方向）
    Yatayy = 0  # 初始化引力方向上的斥力分量（y方向）

    for i in range(n):  # 遍历所有障碍点
        Rrei = (X[0] - Xsum[i + 1, 0])**2 + (X[1] - Xsum[i + 1, 1])**2  # 计算障碍点与当前位置的距离平方
        rre = np.sqrt(Rrei)  # 计算距离
        if rre > Po:  # 如果距离大于障碍影响范围
            Yrerx = 0  # 斥力分量为0
            Yrery = 0
            Yatax = 0
            Yatay = 0
        else:  # 如果在影响范围内
            if rre < Po / 2:  # 如果距离小于影响范围的一半
                Yrer = m * (1 / rre - 1 / Po) * (1 / Rrei) * (rat**a)  # 计算斥力大小
                Yata = a * m * ((1 / rre - 1 / Po)**2) * (rat**(1 - a)) / 2  # 计算引力方向上的斥力分量
            else:  # 如果距离大于等于影响范围的一半
                Yrer = m * (1 / rre - 1 / Po) * (1 / Rrei) * Rat  # 计算斥力大小
                Yata = m * ((1 / rre - 1 / Po)**2) * rat  # 计算引力方向上的斥力分量

            Yrerx = (1 + 0.1) * Yrer * np.cos(angle_re[i])  # 计算斥力在x方向的分量
            Yrery = -(1 - 0.1) * Yrer * np.sin(angle_re[i])  # 计算斥力在y方向的分量
            Yatax = Yata * np.cos(angle_at)  # 计算引力方向上的斥力分量（x方向）
            Yatay = Yata * np.sin(angle_at)  # 计算引力方向上的斥力分量（y方向）

        Yrerxx += Yrerx  # 累加斥力在x方向的分量
        Yreryy += Yrery  # 累加斥力在y方向的分量
        Yataxx += Yatax  # 累加引力方向上的斥力分量（x方向）
        Yatayy += Yatay  # 累加引力方向上的斥力分量（y方向）

    return Yrerxx, Yreryy, Yataxx, Yatayy  # 返回斥力分量

# 初始化参数
Xo = np.array([0, 0])  # 起点位置
k = 50  # 引力增益系数
K = 0  # 初始化变量K（后续用于记录到达目标的步数）
m = 15  # 斥力增益系数
Po = 0.5  # 障碍影响距离
n = 6  # 障碍个数
a = 0.5  # 调整斥力公式的参数
l = 0.2  # 每一步的移动步长
J = 200  # 最大迭代次数
Xsum = np.array([[6, 10], [1, 1.2], [3, 2.5], [4, 4.5], [3, 6], [6, 2], [5.5, 5.5]])  # 目标和障碍点位置
Xj = Xo  # 初始化当前位置为起点
Goal = np.zeros((J, 2))  # 初始化路径数组，用于保存每一步的位置

# 主体循环
for j in range(J):  # 遍历最大迭代次数
    Goal[j] = Xj  # 保存当前位置
    Theta = compute_angle(Xj, Xsum, n)  # 计算当前位置与目标和障碍点的角度
    Angle = Theta[0]  # 获取与目标点的角度
    angle_at = Theta[0]  # 引力方向角度
    Fatx, Faty = compute_attract(Xj, Xsum, k, Angle, Po, n)  # 计算引力分量
    angle_re = Theta[1:]  # 获取与障碍点的角度

    # 计算斥力分量
    Frerxx, Freryy, Fataxx, Fatayy = compute_repulsion(Xj, Xsum, m, angle_at, angle_re, n, Po, a)

    # 计算合力
    Fsumxj = Fatx + Frerxx + Fataxx  # 合力在x方向的分量
    Fsumyj = Faty + Freryy + Fatayy  # 合力在y方向的分量

    # 计算下一步位置
    Position_angle = np.arctan2(Fsumyj, Fsumxj)  # 计算合力方向的角度
    Xnext = Xj + l * np.array([np.cos(Position_angle), np.sin(Position_angle)])  # 根据合力方向和步长计算下一步的位置
    Xj = Xnext  # 更新当前位置

    # 判断是否到达目标
    if (Xj[0] >= Xsum[0, 0]) and (Xj[1] >= Xsum[0, 1]):  # 如果当前位置大于等于目标位置
        K = j  # 记录到达目标的步数
        break  # 跳出循环

# 保存最终路径
Goal = Goal[:K + 1]  # 截取路径数组至到达目标的步数
Goal[-1] = Xsum[0]  # 将目标位置作为路径的最后一个点

# 绘图
plt.figure(figsize=(8, 8))
plt.axis([0, 10, 0, 10])
plt.plot(Xsum[1:, 0], Xsum[1:, 1], 'ok', label='Obstacles')  # 障碍
plt.plot(Xsum[0, 0], Xsum[0, 1], 'vr', label='Goal')  # 目标
plt.plot(Xo[0], Xo[1], 'ms', label='Start')  # 起点
plt.plot(Goal[:, 0], Goal[:, 1], '.-r', label='Path')  # 路径
plt.xlabel('X coordinate')
plt.ylabel('Y coordinate')
plt.title('Artificial Potential Field Path Planning')
plt.legend()
plt.grid(True)
plt.show()

2.afp.py

python_motion_planning/python_motion_planning/local_planner/apf.py at master · ai-winter/python_motion_planning · GitHub

二维的robot

2.1.笔记

2.1.1.cdist计算点之间的距离

from scipy.spatial.distance import cdist # 导入scipy库中的cdist函数，用于计算点之间的距离

我一直都是直接np.linalg.norm(p1-p2)

XA = np.asarray(XA)将某个对象（例如列表、元组或其他数组）转换为 NumPy 数组（numpy.ndarray）的常见方式。

源代码：

先查维度和列数，用预定义函数解距离（可以求字符串？）

# 将输入 XA 转换为 NumPy 数组，确保后续操作的兼容性
XA = np.asarray(XA)
# 将输入 XB 转换为 NumPy 数组，确保后续操作的兼容性
XB = np.asarray(XB)

# 获取 XA 的形状（维度信息），例如 (m, n)，表示 m 行 n 列
s = XA.shape
# 获取 XB 的形状（维度信息），例如 (m, n)，表示 m 行 n 列
sB = XB.shape

# 检查 XA 是否为二维数组，如果不是，抛出 ValueError 异常
if len(s) != 2:
    raise ValueError('XA must be a 2-dimensional array.')
# 检查 XB 是否为二维数组，如果不是，抛出 ValueError 异常
if len(sB) != 2:
    raise ValueError('XB must be a 2-dimensional array.')
# 检查 XA 和 XB 的列数是否相同（即特征维度是否一致），如果不一致，抛出 ValueError 异常
if s[1] != sB[1]:
    raise ValueError('XA and XB must have the same number of columns '
                     '(i.e. feature dimension.)')

# 获取 XA 的行数（样本数量）
mA = s[0]
# 获取 XB 的行数（样本数量）
mB = sB[0]
# 获取特征维度（列数）
n = s[1]

# 如果 metric 是一个可调用函数（用户自定义的距离函数）
if callable(metric):
    # 获取该函数的名称（如果无法获取，则默认为 'Unknown'）
    mstr = getattr(metric, '__name__', 'Unknown')
    # 在预定义的度量别名字典 _METRIC_ALIAS 中查找该函数名称对应的度量信息
    metric_info = _METRIC_ALIAS.get(mstr, None)
    # 如果找到匹配的度量信息
    if metric_info is not None:
        # 调用 _validate_cdist_input 函数对输入数据进行验证和预处理
        XA, XB, typ, kwargs = _validate_cdist_input(
            XA, XB, mA, mB, n, metric_info, **kwargs)
        # 调用 _cdist_callable 函数计算距离矩阵，并返回结果
        return _cdist_callable(XA, XB, metric=metric, out=out, **kwargs)
# 如果 metric 是一个字符串（表示预定义的距离度量名称）
elif isinstance(metric, str):
    # 将字符串转换为小写，以确保不区分大小写
    mstr = metric.lower()
    # 在预定义的度量别名字典 _METRIC_ALIAS 中查找该字符串对应的度量信息
    metric_info = _METRIC_ALIAS.get(mstr, None)
    # 如果找到匹配的度量信息
    if metric_info is not None:
        # 获取对应的 cdist 函数
        cdist_fn = metric_info.cdist_func
        # 调用该函数计算距离矩阵，并返回结果
        return cdist_fn(XA, XB, out=out, **kwargs)
    # 如果 metric 是以 "test_" 开头的字符串，表示这是一个测试用的距离度量
    elif mstr.startswith("test_"):
        # 在测试度量字典 _TEST_METRICS 中查找该字符串对应的度量信息
        metric_info = _TEST_METRICS.get(mstr, None)
        # 如果未找到匹配的度量信息，抛出 ValueError 异常
        if metric_info is None:
            raise ValueError(f'Unknown "Test" Distance Metric: {mstr[5:]}')
        # 调用 _validate_cdist_input 函数对输入数据进行验证和预处理
        XA, XB, typ, kwargs = _validate_cdist_input(
            XA, XB, mA, mB, n, metric_info, **kwargs)
        # 调用 _cdist_callable 函数计算距离矩阵，并返回结果
        return _cdist_callable(
            XA, XB, metric=metric_info.dist_func, out=out, **kwargs)
    # 如果 metric 是字符串，但未在 _METRIC_ALIAS 或 _TEST_METRICS 中找到匹配项
    else:
        raise ValueError('Unknown Distance Metric: %s' % mstr)
# 如果 metric 既不是字符串也不是可调用函数，抛出 TypeError 异常
else:
    raise TypeError('2nd argument metric must be a string identifier '
                    'or a function.')

2.1.2.if else的简写

if else 的简写形式（显得很整齐）——如果有改变（局部变量）则赋值

# 简单显式
if boundary:
    self.boundary = boundary

# or在有值的时候返回前面
self.boundary = boundary or self.boundary

# github的代码【避免self.boundary为None】
self.boundary = boundary if boundary else self.boundary

2.1.3.异常断言assert 或异常raise

这行代码是一个断言语句（assert），用于在程序运行时检查某个条件是否为真。如果条件不满足，程序会抛出一个异常，并显示指定的错误信息

assert len(start) == 3 and len(goal) == 3, \
    "Start and goal parameters must be (x, y, theta)"

assert：

用于调试阶段，确保某些条件始终为真。
如果条件为假，抛出 AssertionError。
可以通过优化模式禁用，不影响运行性能。
适合用于开发和测试阶段。

raise：

用于运行时错误处理，显式抛出异常。
抛出指定类型的异常，始终有效。
适合用于运行时错误处理，确保程序在遇到问题时能够正确中断并提示错误。

python -O script.py

raise AttributeError("Global path searcher is None, please set it first!")

if self.path is None:
    assert RuntimeError("Please plan the path using g_planner!")

2.1.4.params的默认值定义【具有全局性】

在父类定义了就不用写奇怪的命令行了

而且是字典！！！显得很有规律

# parameters and default value
        self.params = {}
        self.params["TIME_STEP"] = params["TIME_STEP"] if "TIME_STEP" in params.keys() else 0.1

之前自己写的命令行来一个

def parser():
    # 命令行名称--config可用短名称-c代替
    parser = argparse.ArgumentParser(description='CT formation')

    # 程序基本参数
    parser.add_argument('--config','-c',default='./config.py',help='config file path')

    # planner的参数
    parser.add_argument('-ut', '--update_time', type=float, default=0.1,
                        help='Time interval to update_v status (default: 0.1)')

2.1.4.将方法转化为属性【只读装饰器】

@property 是一个内置装饰器，用于将类中的方法（函数）定义为只读属性（变量）。
使用 @property 装饰的方法可以像访问普通属性一样被调用，而不需要像调用方法那样加括号。

class LocalPlanner:
    def __init__(self):
        self.g_planner_ = "GlobalPlannerInstance"

    @property
    def g_planner(self):
        return str(self.g_planner_)

planner = LocalPlanner()
print(planner.g_planner)  # 输出: "GlobalPlannerInstance"

前瞻点

自动驾驶控制算法——纯跟踪算法（Pure Pursuit）-优快云博客

2.2.过程

2.2.1.类apf：

继承：LocalPlanner
参数：self.g_planner 全局global，字典
父类自带参数self.params["TIME_STEP"] 【好东西！学！！】

2.2.2.类LocalPlanner：

params的默认值定义【见2.1.笔记】
【重要】函数getLookaheadPoint：在路径上找到距离机器人正好等于前瞻距离的点

2.3.原代码

超级多文件和类的套用，只能说，他并不想让人用他的环境

# https://github.com/ai-winter/python_motion_planning/blob/master/python_motion_planning/local_planner/apf.py
"""
@file: apf.py
@breif: Artificial Potential Field(APF) motion planning
@author: Yang Haodong, Wu Maojia
@update: 2024.5.21
"""
import math  # 导入数学库，用于三角函数计算
import numpy as np  # 导入numpy库，用于矩阵和数组操作
from scipy.spatial.distance import cdist  # 导入scipy库中的cdist函数，用于计算点之间的距离

from .local_planner import LocalPlanner  # 从本地规划模块导入LocalPlanner类
from python_motion_planning.utils import Env  # 从工具模块导入Env类，用于环境建模


class APF(LocalPlanner):
    """
    Class for Artificial Potential Field(APF) motion planning.

    Parameters:
        start (tuple): start point coordinate
        goal (tuple): goal point coordinate
        env (Env): environment
        heuristic_type (str): heuristic function type
        **params: other parameters can be found in the parent class LocalPlanner

    Examples:
        # >>> from python_motion_planning.utils import Grid
        # >>> from python_motion_planning.local_planner import APF
        # >>> start = (5, 5, 0)
        # >>> goal = (45, 25, 0)
        # >>> env = Grid(51, 31)
        # >>> planner = APF(start, goal, env)
        # >>> planner.run()
    """

    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean", **params) -> None:
        super().__init__(start, goal, env, heuristic_type, **params)  # 调用父类初始化方法
        # 初始化APF参数
        self.zeta = 1.0  # 引力系数
        self.eta = 1.0  # 斥力系数
        self.d_0 = 1.0  # 障碍物影响范围

        # 初始化全局规划器
        g_start = (start[0], start[1])  # 全局规划的起点
        g_goal = (goal[0], goal[1])  # 全局规划的目标点
        self.g_planner = {"planner_name": "a_star", "start": g_start, "goal": g_goal, "env": env}  # 全局规划器配置
        self.path = self.g_path[::-1]  # 反转全局路径，用于后续处理

    def __str__(self) -> str:
        return "Artificial Potential Field(APF)"  # 返回类的名称，用于打印或日志记录

    def plan(self):
        """
        APF motion plan function.

        Returns:
            flag (bool): planning successful if true else failed
            pose_list (list): history poses of robot
        """
        dt = self.params["TIME_STEP"]  # 获取时间步长
        for _ in range(self.params["MAX_ITERATION"]):  # 迭代最大次数
            # 如果到达目标点，返回成功标志和历史姿态
            if self.reachGoal(tuple(self.robot.state.squeeze(axis=1)[0:3]), self.goal):
                return True, self.robot.history_pose

            # 计算当前步的目标点和力
            lookahead_pt, _, _ = self.getLookaheadPoint()  # 获取前瞻点
            rep_force = self.getRepulsiveForce()  # 计算斥力
            attr_force = self.getAttractiveForce(np.array(self.robot.position), np.array(lookahead_pt))  # 计算引力
            net_force = self.zeta * attr_force + self.eta * rep_force  # 计算合力

            # 计算期望速度
            v, theta = self.robot.v, self.robot.theta  # 当前速度和角度
            new_v = np.array([v * math.cos(theta), v * math.sin(theta)]) + net_force  # 根据合力更新速度
            new_v /= np.linalg.norm(new_v)  # 归一化速度
            new_v *= self.params["MAX_V"]  # 限制最大速度
            theta_d = math.atan2(new_v[1], new_v[0])  # 计算期望角度

            # 计算速度指令
            e_theta = self.regularizeAngle(self.robot.theta - self.goal[2])  # 计算角度误差
            if not self.shouldMoveToGoal(self.robot.position, self.goal):  # 如果不需要移动到目标
                if not self.shouldRotateToPath(abs(e_theta)):  # 如果不需要旋转到路径方向
                    u = np.array([[0], [0]])  # 速度指令为零
                else:
                    u = np.array([[0], [self.angularRegularization(e_theta / dt)]])  # 只调整角度
            else:
                e_theta = self.regularizeAngle(theta_d - self.robot.theta)  # 计算角度误差
                if self.shouldRotateToPath(abs(e_theta)):  # 如果需要旋转到路径方向
                    u = np.array([[0], [self.angularRegularization(e_theta / dt)]])  # 只调整角度
                else:
                    v_d = np.linalg.norm(new_v)  # 计算期望速度大小
                    u = np.array([[self.linearRegularization(v_d)], [self.angularRegularization(e_theta / dt)]])  # 更新线速度和角速度

            # 将速度指令输入到机器人运动学模型
            self.robot.kinematic(u, dt)

        return False, None  # 如果未成功到达目标，返回失败标志

    def run(self):
        """
        Running both planning and animation.
        """
        _, history_pose = self.plan()  # 执行规划
        if not history_pose:  # 如果路径为空，抛出错误
            raise ValueError("Path not found and planning failed!")

        path = np.array(history_pose)[:, 0:2]  # 提取路径的x和y坐标
        cost = np.sum(np.sqrt(np.sum(np.diff(path, axis=0) ** 2, axis=1, keepdims=True)))  # 计算路径总长度
        self.plot.plotPath(self.path, path_color="r", path_style="--")  # 绘制路径
        self.plot.animation(path, str(self), cost, history_pose=history_pose)  # 绘制动画

    def getRepulsiveForce(self) -> np.ndarray:
        """
        Get the repulsive force of APF.

        Returns:
            rep_force (np.ndarray): repulsive force of APF
        """
        obstacles = np.array(list(self.obstacles))  # 获取障碍物位置
        cur_pos = np.array([[self.robot.px, self.robot.py]])  # 当前机器人位置
        D = cdist(obstacles, cur_pos)  # 计算障碍物与机器人的距离
        rep_force = (1 / D - 1 / self.d_0) * (1 / D) ** 2 * (cur_pos - obstacles)  # 计算斥力
        valid_mask = np.argwhere((1 / D - 1 / self.d_0) > 0)[:, 0]  # 筛选出有效的斥力
        rep_force = np.sum(rep_force[valid_mask, :], axis=0)  # 累加所有有效的斥力

        if not np.all(rep_force == 0):  # 如果斥力不为零
            rep_force = rep_force / np.linalg.norm(rep_force)  # 归一化斥力

        return rep_force

    def getAttractiveForce(self, cur_pos: np.ndarray, tgt_pos: np.ndarray) -> np.ndarray:
        """
        Get the attractive force of APF.

        Parameters:
            cur_pos (np.ndarray): current position of robot
            tgt_pos (np.ndarray): target position of robot

        Returns
            attr_force (np.ndarray): attractive force
        """
        attr_force = tgt_pos - cur_pos  # 计算引力方向
        if not np.all(attr_force == 0):  # 如果引力不为零
            attr_force = attr_force / np.linalg.norm(attr_force)  # 归一化引力

        return attr_force