接触检测与响应

import numpy as np
def dyn(q,qd,tau):
    c2=np.cos(q[1]); M=np.array([[2.45+.64*c2,.36+.32*c2],[.36+.32*c2,.36]])
    h=.32*np.sin(q[1]); C=np.array([[-h*qd[1],-h*(qd[0]+qd[1])],[h*qd[0],0]])
    G=np.array([(5*.5+3)*9.81*np.cos(q[0])+3*.4*9.81*np.cos(q[0]+q[1]),3*.4*9.81*np.cos(q[0]+q[1])])
    return np.linalg.solve(M,tau-C@qd-G)
Kobs=np.diag([50.,50.]); obs_state=np.zeros(2); threshold=5.0
q=np.array([.5,.8]); qd=np.zeros(2); dt=0.001; coll_t=1.5; det_t=None
hist=[]
for i in range(int(4./dt)):
    t=i*dt
    tau=200*(np.array([.5,.8])-q)+40*(0-qd)
    G=np.array([(5*.5+3)*9.81*np.cos(q[0])+3*.4*9.81*np.cos(q[0]+q[1]),3*.4*9.81*np.cos(q[0]+q[1])])
    tau+=G
    if abs(t-coll_t)<.05:
        J=np.array([[-1*np.sin(q[0])-.8*np.sin(q[0]+q[1]),-.8*np.sin(q[0]+q[1])],[1*np.cos(q[0])+.8*np.cos(q[0]+q[1]),.8*np.cos(q[0]+q[1])]])
        tau_ext=J.T@np.array([30.,15.])
    else: tau_ext=np.zeros(2)
    # 动量观测器
    c2=np.cos(q[1]); M=np.array([[2.45+.64*c2,.36+.32*c2],[.36+.32*c2,.36]])
    p=M@qd; p_dot=tau+tau_ext-np.array([(5*.5+3)*9.81*np.cos(q[0])+3*.4*9.81*np.cos(q[0]+q[1]),3*.4*9.81*np.cos(q[0]+q[1])])
    obs_state+=(p_dot+Kobs@(p-obs_state))*dt; res=Kobs@(p-obs_state)
    collision=np.linalg.norm(res)>threshold
    if collision and det_t is None: det_t=t
    if collision: tau=-50*qd  # 紧急停止
    qdd=dyn(q,qd,tau+tau_ext); qd+=qdd*dt; q+=qd*dt
    if i%100==0: hist.append((t,np.linalg.norm(res),collision))
print("碰撞检测与响应"); print("="*60)
if det_t: print(f"碰撞时间:{coll_t}s 检测时间:{det_t:.3f}s 延迟:{(det_t-coll_t)*1000:.1f}ms")
for t,r,c in hist[::5]:
    print(f"t={t:.2f}s: 残差={r:.2f} {'⚠️碰撞' if c else '✓正常'}")

import numpy as np
print("力控增益调优分析")
for Kf in [0.1, 0.3, 0.5, 1.0, 2.0]:
    F_des=10.0; tau=0.05/Kf; settling=4*tau
    print(f"  Kf={Kf:.1f}: 调节时间={settling*1000:.1f}ms, 稳态力≈{Kf*F_des:.1f}N")
print("\n设计原则: 高增益→快响应但可能不稳定; 低增益→稳定但慢")