数值计算¶
数值计算是将数学理论转化为实际算法的关键。本章将讲解机器人学中常用的数值计算方法。
学习目标¶
完成本章后,你将能够:
- 理解数值线性代数的基本方法
- 掌握数值积分和微分方法
- 理解迭代求解方法
- 应用数值计算解决机器人学问题
1. 数值线性代数¶
1.1 矩阵分解¶
import numpy as np
# LU 分解
def lu_decomposition(A):
"""LU 分解"""
from scipy.linalg import lu
P, L, U = lu(A)
return P, L, U
# QR 分解
def qr_decomposition(A):
"""QR 分解"""
Q, R = np.linalg.qr(A)
return Q, R
# 奇异值分解 (SVD)
def svd_decomposition(A):
"""奇异值分解"""
U, S, Vt = np.linalg.svd(A)
return U, S, Vt
# Cholesky 分解
def cholesky_decomposition(A):
"""Cholesky 分解(要求 A 对称正定)"""
L = np.linalg.cholesky(A)
return L
# 示例
A = np.array([[4, 12, -16],
[12, 37, -43],
[-16, -43, 98]])
# LU 分解
P, L, U = lu_decomposition(A)
print("LU 分解:")
print("L =", L)
print("U =", U)
print("验证 A = LU:", np.allclose(A, L @ U))
# QR 分解
Q, R = qr_decomposition(A)
print("\nQR 分解:")
print("Q =", Q)
print("R =", R)
print("验证 A = QR:", np.allclose(A, Q @ R))
# Cholesky 分解
L = cholesky_decomposition(A)
print("\nCholesky 分解:")
print("L =", L)
print("验证 A = LL^T:", np.allclose(A, L @ L.T))
1.2 线性方程组求解¶
import numpy as np
def solve_linear_system(A, b, method='lu'):
"""
求解线性方程组 Ax = b
参数:
A: 系数矩阵
b: 右端向量
method: 求解方法 ('lu', 'qr', 'svd', 'cholesky')
返回:
x: 解向量
"""
if method == 'lu':
# LU 分解
from scipy.linalg import lu_solve, lu_factor
lu, piv = lu_factor(A)
x = lu_solve((lu, piv), b)
elif method == 'qr':
# QR 分解
Q, R = np.linalg.qr(A)
x = np.linalg.solve(R, Q.T @ b)
elif method == 'svd':
# SVD 分解
U, S, Vt = np.linalg.svd(A)
x = Vt.T @ np.diag(1/S) @ U.T @ b
elif method == 'cholesky':
# Cholesky 分解
L = np.linalg.cholesky(A)
y = np.linalg.solve(L, b)
x = np.linalg.solve(L.T, y)
else:
# 直接求解
x = np.linalg.solve(A, b)
return x
# 示例
A = np.array([[2, 1], [5, 7]])
b = np.array([11, 13])
# 不同方法求解
methods = ['lu', 'qr', 'svd', 'cholesky', 'direct']
for method in methods:
try:
x = solve_linear_system(A, b, method)
print(f"{method}: x = {x}")
except Exception as e:
print(f"{method}: 失败 - {e}")
1.3 最小二乘问题¶
import numpy as np
def least_squares(A, b, method='qr'):
"""
最小二乘问题 min ||Ax - b||²
参数:
A: 系数矩阵
b: 右端向量
method: 求解方法 ('qr', 'svd', 'normal')
返回:
x: 最小二乘解
residual: 残差
"""
if method == 'qr':
# QR 分解
Q, R = np.linalg.qr(A)
x = np.linalg.solve(R, Q.T @ b)
elif method == 'svd':
# SVD 分解
U, S, Vt = np.linalg.svd(A, full_matrices=False)
x = Vt.T @ np.diag(1/S) @ U.T @ b
elif method == 'normal':
# 正规方程
x = np.linalg.solve(A.T @ A, A.T @ b)
else:
# 使用 numpy
x, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)
# 计算残差
residual = np.linalg.norm(A @ x - b)
return x, residual
# 示例:超定系统
A = np.array([[1, 1], [1, 2], [1, 3]])
b = np.array([1, 2, 2])
x, residual = least_squares(A, b, method='qr')
print(f"最小二乘解: {x}")
print(f"残差: {residual:.4f}")
1.4 特征值问题¶
import numpy as np
def eigenvalue_problem(A, method='qr'):
"""
特征值问题
参数:
A: 矩阵
method: 求解方法 ('qr', 'power')
返回:
eigenvalues: 特征值
eigenvectors: 特征向量
"""
if method == 'qr':
# QR 迭代
eigenvalues, eigenvectors = np.linalg.eig(A)
elif method == 'power':
# 幂迭代
n = A.shape[0]
x = np.random.rand(n)
x = x / np.linalg.norm(x)
for _ in range(100):
x_new = A @ x
eigenvalue = np.linalg.norm(x_new)
x = x_new / eigenvalue
eigenvalues = np.array([eigenvalue])
eigenvectors = x.reshape(-1, 1)
return eigenvalues, eigenvectors
# 示例
A = np.array([[2, 1], [1, 2]])
eigenvalues, eigenvectors = eigenvalue_problem(A, method='qr')
print("特征值:", eigenvalues)
print("特征向量:")
print(eigenvectors)
2. 数值积分¶
2.1 基本积分方法¶
import numpy as np
def trapezoidal_rule(f, a, b, n):
"""
梯形法则
参数:
f: 被积函数
a, b: 积分区间
n: 子区间数量
返回:
I: 积分值
"""
x = np.linspace(a, b, n + 1)
y = f(x)
h = (b - a) / n
I = h * (y[0] + 2 * np.sum(y[1:-1]) + y[-1]) / 2
return I
def simpsons_rule(f, a, b, n):
"""
辛普森法则
参数:
f: 被积函数
a, b: 积分区间
n: 子区间数量(必须是偶数)
返回:
I: 积分值
"""
if n % 2 != 0:
n += 1 # 确保 n 是偶数
x = np.linspace(a, b, n + 1)
y = f(x)
h = (b - a) / n
I = h * (y[0] + 4 * np.sum(y[1:-1:2]) + 2 * np.sum(y[2:-1:2]) + y[-1]) / 3
return I
def gaussian_quadrature(f, a, b, n):
"""
高斯求积
参数:
f: 被积函数
a, b: 积分区间
n: 求积点数量
返回:
I: 积分值
"""
# 高斯-勒让德求积
points, weights = np.polynomial.legendre.leggauss(n)
# 变换到 [a, b] 区间
x = 0.5 * (b - a) * points + 0.5 * (b + a)
w = 0.5 * (b - a) * weights
I = np.sum(w * f(x))
return I
# 示例
f = lambda x: np.sin(x)
a, b = 0, np.pi
# 不同方法
I_trap = trapezoidal_rule(f, a, b, 100)
I_simp = simpsons_rule(f, a, b, 100)
I_gauss = gaussian_quadrature(f, a, b, 10)
print(f"梯形法则: {I_trap:.6f}")
print(f"辛普森法则: {I_simp:.6f}")
print(f"高斯求积: {I_gauss:.6f}")
print(f"精确值: {2.0:.6f}")
2.2 自适应积分¶
import numpy as np
def adaptive_quadrature(f, a, b, tol=1e-6, max_iter=100):
"""
自适应求积
参数:
f: 被积函数
a, b: 积分区间
tol: 容差
max_iter: 最大迭代次数
返回:
I: 积分值
"""
def simpsons_rule_single(f, a, b):
"""单步辛普森法则"""
c = (a + b) / 2
h = (b - a) / 6
return h * (f(a) + 4 * f(c) + f(b))
# 递归自适应
def adaptive_step(f, a, b, tol, whole):
c = (a + b) / 2
left = simpsons_rule_single(f, a, c)
right = simpsons_rule_single(f, c, b)
if abs(left + right - whole) < 15 * tol:
return left + right
return (adaptive_step(f, a, c, tol/2, left) +
adaptive_step(f, c, b, tol/2, right))
whole = simpsons_rule_single(f, a, b)
return adaptive_step(f, a, b, tol, whole)
# 示例
f = lambda x: np.sin(x)
a, b = 0, np.pi
I = adaptive_quadrature(f, a, b)
print(f"自适应求积: {I:.6f}")
2.3 常微分方程数值解¶
import numpy as np
def euler_method(f, y0, t_span, dt):
"""
欧拉方法
参数:
f: 导数函数 f(t, y)
y0: 初始值
t_span: 时间范围 (t0, tf)
dt: 时间步长
返回:
t: 时间点
y: 解值
"""
t0, tf = t_span
t = np.arange(t0, tf + dt, dt)
y = np.zeros((len(t), len(y0)))
y[0] = y0
for i in range(len(t) - 1):
y[i + 1] = y[i] + dt * f(t[i], y[i])
return t, y
def runge_kutta_4(f, y0, t_span, dt):
"""
四阶龙格-库塔方法
参数:
f: 导数函数 f(t, y)
y0: 初始值
t_span: 时间范围 (t0, tf)
dt: 时间步长
返回:
t: 时间点
y: 解值
"""
t0, tf = t_span
t = np.arange(t0, dt + dt, dt)
y = np.zeros((len(t), len(y0)))
y[0] = y0
for i in range(len(t) - 1):
k1 = f(t[i], y[i])
k2 = f(t[i] + dt/2, y[i] + dt/2 * k1)
k3 = f(t[i] + dt/2, y[i] + dt/2 * k2)
k4 = f(t[i] + dt, y[i] + dt * k3)
y[i + 1] = y[i] + dt * (k1 + 2*k2 + 2*k3 + k4) / 6
return t, y
# 示例:单摆
def pendulum(t, y):
"""单摆动力学"""
theta, omega = y
g, l = 9.81, 1.0
return np.array([omega, -(g/l) * np.sin(theta)])
# 初始条件
y0 = np.array([np.radians(30), 0]) # 角度=30度,角速度=0
t_span = (0, 10)
dt = 0.01
# 欧拉方法
t_euler, y_euler = euler_method(pendulum, y0, t_span, dt)
# 龙格-库塔方法
t_rk4, y_rk4 = runge_kutta_4(pendulum, y0, t_span, dt)
print(f"欧拉方法 - 最终角度: {np.degrees(y_euler[-1, 0]):.2f} 度")
print(f"龙格-库塔 - 最终角度: {np.degrees(y_rk4[-1, 0]):.2f} 度")
3. 迭代方法¶
3.1 线性方程组迭代求解¶
import numpy as np
def jacobi_iteration(A, b, x0=None, tol=1e-6, max_iter=1000):
"""
雅可比迭代法
参数:
A: 系数矩阵
b: 右端向量
x0: 初始解
tol: 容差
max_iter: 最大迭代次数
返回:
x: 解向量
iterations: 迭代次数
"""
n = len(b)
x = x0.copy() if x0 is not None else np.zeros(n)
x_new = np.zeros(n)
for iteration in range(max_iter):
for i in range(n):
sigma = 0
for j in range(n):
if j != i:
sigma += A[i, j] * x[j]
x_new[i] = (b[i] - sigma) / A[i, i]
# 检查收敛
if np.linalg.norm(x_new - x) < tol:
return x_new, iteration + 1
x = x_new.copy()
return x, max_iter
def gauss_seidel(A, b, x0=None, tol=1e-6, max_iter=1000):
"""
高斯-赛德尔迭代法
参数:
A: 系数矩阵
b: 右端向量
x0: 初始解
tol: 容差
max_iter: 最大迭代次数
返回:
x: 解向量
iterations: 迭代次数
"""
n = len(b)
x = x0.copy() if x0 is not None else np.zeros(n)
for iteration in range(max_iter):
x_old = x.copy()
for i in range(n):
sigma = 0
for j in range(n):
if j != i:
sigma += A[i, j] * x[j]
x[i] = (b[i] - sigma) / A[i, i]
# 检查收敛
if np.linalg.norm(x - x_old) < tol:
return x, iteration + 1
return x, max_iter
# 示例
A = np.array([[4, -1, 0, 0],
[-1, 4, -1, 0],
[0, -1, 4, -1],
[0, 0, -1, 4]])
b = np.array([15, 10, 10, 10])
# 雅可比迭代
x_jacobi, iter_jacobi = jacobi_iteration(A, b)
print(f"雅可比迭代: x = {x_jacobi}, 迭代次数 = {iter_jacobi}")
# 高斯-赛德尔迭代
x_gs, iter_gs = gauss_seidel(A, b)
print(f"高斯-赛德尔: x = {x_gs}, 迭代次数 = {iter_gs}")
# 直接求解验证
x_exact = np.linalg.solve(A, b)
print(f"精确解: {x_exact}")
3.2 非线性方程求解¶
import numpy as np
def bisection_method(f, a, b, tol=1e-6, max_iter=100):
"""
二分法
参数:
f: 函数
a, b: 区间
tol: 容差
max_iter: 最大迭代次数
返回:
x: 根
iterations: 迭代次数
"""
if f(a) * f(b) > 0:
raise ValueError("f(a) 和 f(b) 必须异号")
for iteration in range(max_iter):
c = (a + b) / 2
if abs(f(c)) < tol or (b - a) / 2 < tol:
return c, iteration + 1
if f(c) * f(a) < 0:
b = c
else:
a = c
return (a + b) / 2, max_iter
def newton_method_root(f, df, x0, tol=1e-6, max_iter=100):
"""
牛顿法求根
参数:
f: 函数
df: 导数
x0: 初始点
tol: 容差
max_iter: 最大迭代次数
返回:
x: 根
iterations: 迭代次数
"""
x = x0
for iteration in range(max_iter):
x_new = x - f(x) / df(x)
if abs(x_new - x) < tol:
return x_new, iteration + 1
x = x_new
return x, max_iter
def secant_method(f, x0, x1, tol=1e-6, max_iter=100):
"""
割线法
参数:
f: 函数
x0, x1: 初始点
tol: 容差
max_iter: 最大迭代次数
返回:
x: 根
iterations: 迭代次数
"""
for iteration in range(max_iter):
if abs(f(x1) - f(x0)) < 1e-12:
break
x2 = x1 - f(x1) * (x1 - x0) / (f(x1) - f(x0))
if abs(x2 - x1) < tol:
return x2, iteration + 1
x0, x1 = x1, x2
return x1, max_iter
# 示例:求解 x³ - 2x - 5 = 0
f = lambda x: x**3 - 2*x - 5
df = lambda x: 3*x**2 - 2
# 二分法
x_bisect, iter_bisect = bisection_method(f, 2, 3)
print(f"二分法: x = {x_bisect:.6f}, 迭代次数 = {iter_bisect}")
# 牛顿法
x_newton, iter_newton = newton_method_root(f, df, 2)
print(f"牛顿法: x = {x_newton:.6f}, 迭代次数 = {iter_newton}")
# 割线法
x_secant, iter_secant = secant_method(f, 2, 3)
print(f"割线法: x = {x_secant:.6f}, 迭代次数 = {iter_secant}")
3.3 非线性方程组求解¶
import numpy as np
from scipy.optimize import fsolve
def newton_method_system(F, J, x0, tol=1e-6, max_iter=100):
"""
牛顿法求解非线性方程组
参数:
F: 方程组函数
J: 雅可比矩阵函数
x0: 初始点
tol: 容差
max_iter: 最大迭代次数
返回:
x: 解
iterations: 迭代次数
"""
x = x0.copy()
for iteration in range(max_iter):
Fx = F(x)
Jx = J(x)
# 求解线性方程组
try:
d = np.linalg.solve(Jx, -Fx)
except np.linalg.LinAlgError:
break
x_new = x + d
if np.linalg.norm(x_new - x) < tol:
return x_new, iteration + 1
x = x_new
return x, max_iter
# 示例:求解方程组
# x² + y² = 4
# x² - y = 1
def F(x):
return np.array([
x[0]**2 + x[1]**2 - 4,
x[0]**2 - x[1] - 1
])
def J(x):
return np.array([
[2*x[0], 2*x[1]],
[2*x[0], -1]
])
x0 = np.array([1.0, 1.0])
# 牛顿法
x_newton, iter_newton = newton_method_system(F, J, x0)
print(f"牛顿法: x = {x_newton}, 迭代次数 = {iter_newton}")
# 使用 scipy
x_scipy = fsolve(F, x0)
print(f"scipy: x = {x_scipy}")
4. 插值与拟合¶
4.1 多项式插值¶
import numpy as np
def lagrange_interpolation(x_points, y_points, x):
"""
拉格朗日插值
参数:
x_points, y_points: 插值点
x: 求值点
返回:
y: 插值结果
"""
n = len(x_points)
y = 0
for i in range(n):
# 计算拉格朗日基函数
L = 1
for j in range(n):
if j != i:
L *= (x - x_points[j]) / (x_points[i] - x_points[j])
y += y_points[i] * L
return y
def newton_interpolation(x_points, y_points, x):
"""
牛顿插值
参数:
x_points, y_points: 插值点
x: 求值点
返回:
y: 插值结果
"""
n = len(x_points)
# 计算差商表
divided_diff = np.zeros((n, n))
divided_diff[:, 0] = y_points
for j in range(1, n):
for i in range(n - j):
divided_diff[i, j] = (divided_diff[i+1, j-1] - divided_diff[i, j-1]) / \
(x_points[i+j] - x_points[i])
# 计算插值结果
y = divided_diff[0, 0]
product = 1
for i in range(1, n):
product *= (x - x_points[i-1])
y += divided_diff[0, i] * product
return y
# 示例
x_points = np.array([0, 1, 2, 3])
y_points = np.array([1, 2, 5, 10])
x = 1.5
y_lagrange = lagrange_interpolation(x_points, y_points, x)
y_newton = newton_interpolation(x_points, y_points, x)
print(f"拉格朗日插值: f({x}) = {y_lagrange:.4f}")
print(f"牛顿插值: f({x}) = {y_newton:.4f}")
4.2 样条插值¶
import numpy as np
from scipy.interpolate import CubicSpline
def cubic_spline_interpolation(x_points, y_points, x):
"""
三次样条插值
参数:
x_points, y_points: 插值点
x: 求值点
返回:
y: 插值结果
"""
cs = CubicSpline(x_points, y_points)
return cs(x)
# 示例
x_points = np.array([0, 1, 2, 3, 4])
y_points = np.array([0, 1, 4, 9, 16])
# 创建样条
cs = CubicSpline(x_points, y_points)
# 求值
x = np.linspace(0, 4, 100)
y = cs(x)
print(f"样条插值在 x=1.5 处的值: {cs(1.5):.4f}")
4.3 曲线拟合¶
import numpy as np
from scipy.optimize import curve_fit
def polynomial_fit(x, y, degree):
"""
多项式拟合
参数:
x, y: 数据点
degree: 多项式阶数
返回:
coefficients: 多项式系数
"""
coefficients = np.polyfit(x, y, degree)
return coefficients
def curve_fitting(x, y, func, p0=None):
"""
曲线拟合
参数:
x, y: 数据点
func: 拟合函数
p0: 初始参数
返回:
params: 拟合参数
"""
params, covariance = curve_fit(func, x, y, p0=p0)
return params
# 示例:多项式拟合
x = np.array([0, 1, 2, 3, 4, 5])
y = np.array([0, 2, 6, 12, 20, 30])
# 二次多项式拟合
coeffs = polynomial_fit(x, y, 2)
print(f"二次多项式系数: {coeffs}")
# 拟合函数
def quadratic(x, a, b, c):
return a * x**2 + b * x + c
params = curve_fitting(x, y, quadratic, p0=[1, 1, 0])
print(f"曲线拟合参数: {params}")
5. 应用示例¶
5.1 机器人动力学仿真¶
import numpy as np
def robot_dynamics(state, t, params):
"""
机器人动力学
state: [q1, q2, dq1, dq2] - 关节角度和角速度
t: 时间
params: 机器人参数
"""
q1, q2, dq1, dq2 = state
m1, m2, l1, l2, g = params
# 质量矩阵
M = np.array([
[(m1 + m2) * l1**2 + m2 * l2**2 + 2 * m2 * l1 * l2 * np.cos(q2),
m2 * l2**2 + m2 * l1 * l2 * np.cos(q2)],
[m2 * l2**2 + m2 * l1 * l2 * np.cos(q2),
m2 * l2**2]
])
# 科里奥利和离心力
C = np.array([
[-m2 * l1 * l2 * (2 * dq1 * dq2 + dq2**2) * np.sin(q2)],
[m2 * l1 * l2 * dq1**2 * np.sin(q2)]
])
# 重力
G = np.array([
[(m1 + m2) * g * l1 * np.cos(q1) + m2 * g * l2 * np.cos(q1 + q2)],
[m2 * g * l2 * np.cos(q1 + q2)]
])
# 计算加速度
ddq = np.linalg.solve(M, -C - G)
return np.array([dq1, dq2, ddq[0, 0], ddq[1, 0]])
# 仿真参数
params = [1.0, 1.0, 1.0, 1.0, 9.81] # m1, m2, l1, l2, g
state0 = np.array([np.pi/4, 0, 0, 0]) # 初始状态
t_span = (0, 2)
dt = 0.01
# 使用龙格-库塔方法
t, states = runge_kutta_4(lambda t, y: robot_dynamics(y, t, params),
state0, t_span, dt)
print(f"仿真完成: {len(t)} 个时间步")
print(f"最终状态: q1={np.degrees(states[-1, 0]):.2f}°, q2={np.degrees(states[-1, 1]):.2f}°")
5.2 卡尔曼滤波实现¶
import numpy as np
class KalmanFilter:
"""卡尔曼滤波器"""
def __init__(self, A, B, H, Q, R, x0, P0):
self.A = A
self.B = B
self.H = H
self.Q = Q
self.R = R
self.x = x0
self.P = P0
def predict(self, u=None):
# 状态预测
if u is not None:
self.x = self.A @ self.x + self.B @ u
else:
self.x = self.A @ self.x
# 协方差预测
self.P = self.A @ self.P @ self.A.T + self.Q
def update(self, z):
# 卡尔曼增益
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
# 状态更新
y = z - self.H @ self.x
self.x = self.x + K @ y
# 协方差更新
I = np.eye(len(self.x))
self.P = (I - K @ self.H) @ self.P
def get_state(self):
return self.x
# 示例
dt = 1.0
A = np.array([[1, dt], [0, 1]])
B = np.array([[0.5 * dt**2], [dt]])
H = np.array([[1, 0]])
Q = np.array([[1, 0], [0, 1]])
R = np.array([[10]])
x0 = np.array([0, 0])
P0 = np.array([[100, 0], [0, 100]])
kf = KalmanFilter(A, B, H, Q, R, x0, P0)
# 模拟
np.random.seed(42)
true_positions = [0, 1, 2, 3, 4, 5]
measurements = [p + np.random.normal(0, np.sqrt(10)) for p in true_positions]
print("卡尔曼滤波:")
for i, (true_pos, z) in enumerate(zip(true_positions, measurements)):
kf.predict()
kf.update(np.array([z]))
estimated_pos = kf.get_state()[0]
print(f"步骤 {i}: 真实={true_pos}, 测量={z:.1f}, 估计={estimated_pos:.1f}")
6. 练习题¶
基础练习¶
- 实现梯形法则进行数值积分
- 使用欧拉方法求解常微分方程
- 实现拉格朗日插值
进阶练习¶
- 实现四阶龙格-库塔方法
- 使用牛顿法求解非线性方程组
- 实现三次样条插值
应用练习¶
- 实现机器人动力学仿真
- 实现卡尔曼滤波器
- 实现曲线拟合