避障规划

import numpy as np
np.random.seed(42)
class Obs:
    def __init__(s,c,r): s.c=np.array(c); s.r=r
    def dist(s,p): return max(np.linalg.norm(p-s.c)-s.r,0)
    def grad(s,p): d=p-s.c; n=np.linalg.norm(d); return d/n if n>1e-10 else np.random.randn(2)
obs=[Obs([1.,.5],.2),Obs([.5,1.],.15),Obs([.8,.8],.1)]
L1,L2=1.,.8; qs=np.array([0.,0.]); qg=np.array([np.pi/3,np.pi/4])
# APF
q=qs.copy(); apf_ok=False
for i in range(500):
    Fa=-1.0*(q-qg); Fr=np.zeros(2)
    for o in obs:
        d=o.dist(q)
        if d<.3:
            p_end=np.array([L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L1*np.sin(q[0])+L2*np.sin(q[0]+q[1])])
            Fr+=.5*(1/d-1/.3)*(1/d**2)*o.grad(p_end)
    F=Fa+Fr; q+=0.02*F
    if np.linalg.norm(q-qg)<.05: apf_ok=True; break
print("避障规划仿真"); print("="*60)
print(f"APF: {'收敛' if apf_ok else '未收敛'}, iter={i+1}")
# RRT
tree=[qs.copy()]; parent={0:-1}
for i in range(2000):
    qr=qg.copy() if np.random.random()<.1 else np.random.uniform(-np.pi,np.pi,2)
    ds=[np.linalg.norm(n-qr) for n in tree]; ni=np.argmin(ds); qn=tree[ni]
    d=qr-qn; dn=np.linalg.norm(d); qnew=qn+.05*d/dn if dn>1e-10 else qn
    x=L1*np.cos(qnew[0])+L2*np.cos(qnew[0]+qnew[1]); y=L1*np.sin(qnew[0])+L2*np.sin(qnew[0]+qnew[1])
    coll=any(o.dist(np.array([x,y]))<.01 for o in obs)
    if not coll:
        tree.append(qnew); parent[len(tree)-1]=ni
        if np.linalg.norm(qnew-qg)<.1:
            path=[]; idx=len(tree)-1
            while idx!=-1: path.append(tree[idx]); idx=parent[idx]
            print(f"RRT: 找到路径! 树={len(tree)} 路径={len(path)}")
            break
else: print("RRT: 未找到路径")

import numpy as np
np.random.seed(42)
print("感知系统噪声鲁棒性")
for sigma in [0.5,1.0,2.0,5.0]:
    K=np.array([[500,0,320],[0,500,240],[0,0,1]]); n=8
    pts3d=np.random.rand(n,3)*0.2; R=np.eye(3); t=np.array([0,0,1.])
    pc=(R@pts3d.T).T+t; p2d=(K@pc.T).T; p2d=p2d[:,:2]/p2d[:,2:3]
    noisy=p2d+np.random.randn(*p2d.shape)*sigma
    proj_err=np.mean(np.linalg.norm(noisy-p2d,axis=1))
    print(f"  σ={sigma:.1f}px: 2D误差={proj_err:.2f}px, 3D误差≈{proj_err/500*1000:.1f}mm")