InvSqrt

本文介绍了一种比传统方法快四倍的InvSqrt计算方法,通过巧妙利用牛顿迭代法并选取合适的初始值,仅需一次迭代即可获得高精度结果。特别适用于计算机图形学中的单位法线计算。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

(转载自飞羽的博客http://blog.youkuaiyun.com/zhengjiaxiang135/article/details/6673639)

先解释下InvSqrt函数吧!InvSqrt(value)函数相当于1.0/sqrt(value),所以你大概应该明白了它是什么意思了吧!由于计算机图形学程序里经常要求一个面或点的单位法线,就不可避免地要用到1.0/sqrt(length)这样的式子。你可能会说:1.0/sqrt(value)不是挺好的么?这还有什么好拿出来的?!

       但是!下面的这个InvSqrt函数比传统的1.0/sqrt(value)平均要快4倍,迭代一次就可以得到相应的结果,而且精度很robust。

下面是源代码:

[cpp]  view plain copy
  1. float invSqrt(float x)  
  2. {  
  3.     float xhalf = 0.5f * x;  
  4.     int i = *(int*)&x;          // get bits for floating value  
  5.     i =  0x5f375a86 - (i>>1);    // gives initial guess  
  6.     x = *(float*)&i;            // convert bits back to float  
  7.     x = x * (1.5f - xhalf*x*x); // Newton step  
  8.     return x;  
  9. }  

         乍看一下上面的代码,你或许还没明白是怎么回事。事实上,说白了,这就是个牛顿迭代法的应用。考虑函数 ,牛顿迭代法求f(y)=0的迭代式是 。所以代入f(y)和f ' (y),就很自然地就明白了
x = x * (1.5f - xhalf*x*x); // Newton step
        这一步的由来。所以这里的关键在于如何选择一个好的初始值来进行迭代才能以尽可能少的次数就达到足够的精度呢?代码中的整形数 i 转换成其机器码对应的浮点数就是一个很好的初始值,事实上它非常地接近x的平方根分之一,因此这也是为什么这里只需迭代一次就可以得到很好的结果的原因。
        这个代码牛逼也正好牛逼在选择了一个好的数据:0x5f375a86-(i>>1),这里的i 是浮点数x对应的机器码转换成的整型数据。然后i>>1就是相当于i/2,统统左移一位,再用一个常量来减i>>2,得到另一个整型数据,这个整形数据对应的浮点数就是一个满足要求的初始值了。

        问题的关键在于,你怎么知道要选择0x5f375a86这么好的一个数据呢?对此,可以后面的参考论文[InvSqrt.pdf],由于里面的数学知识太多,在这个页面里的排版不太方面,所以就不贴出来了~~论文中对于IEEE754浮点数据的格式进行了分析,并将浮点部分与指数部分分开讨论,对指数的奇偶作了一番探讨,最后将R-(i>>1)中的R范围给大致地确定出来,并分析不同取值的结果。大家如果不喜欢看这些头疼的数学,了解一下这个代码到底是怎么回事就行了,没有必

``` import numpy as np from sklearn.neighbors import KNeighborsClassifier from scipy.linalg import sqrtm class JDA: def __init__(self, n_components=3, lambd=1.0): self.n_components = n_components self.lambd = lambd def fit(self, Xs, Xt, ys): ns, _ = Xs.shape nt, _ = Xt.shape Z = np.vstack((Xs, Xt)) Z_mean = np.mean(Z, axis=0) Xs_centered = Xs - np.mean(Xs, axis=0) Xt_centered = Xt - np.mean(Xt, axis=0) C_s = np.cov(Xs_centered.T) / ns C_t = np.cov(Xt_centered.T) / nt Cs_inv_sqrt = invsqrt(C_s + self.lambd * np.eye(len(Z_mean))) Ct_inv_sqrt = invsqrt(C_t + self.lambd * np.eye(len(Z_mean))) M = np.dot(Cs_inv_sqrt, Ct_inv_sqrt).T U, S, V = np.linalg.svd(M[:ns], full_matrices=False) W = np.dot(U[:, :self.n_components], V[:self.n_components]) self.Xs_new = np.dot(Xs_centered, W) self.Xr_new = np.dot(np.concatenate([Xs_centered, Xt_centered]), W) return self def transform(self, X): return np.dot(X - np.mean(X, axis=0), self.W) @staticmethod def invsqrt(matrix): u, s, v = np.linalg.svd(matrix) return np.dot(u, np.dot(np.diag(1.0 / np.sqrt(s)), v)) # 主程序入口 if __name__ == '__main__': dataset = np.load('dataset.npz') X_train_source = dataset['X_train'] X_train_target = dataset['X_val'] # 假设用验证集作为目标域 y_train_source = dataset['y_train'] jda = JDA(n_components=3, lambd=1e-6) jda.fit(X_train_source, X_train_target, y_train_source) X_train_aligned = jda.transform(X_train_source) X_val_aligned = jda.transform(X_train_target) clf = KNeighborsClassifier(n_neighbors=3) clf.fit(X_train_aligned, y_train_source) accuracy = clf.score(jda.transform(dataset['X_test']), dataset['y_test']) print(f"Accuracy on test set after JDA alignment: {accuracy:.4f}") print("Joint Distribution Alignment completed.")```Traceback (most recent call last): File "C:/Users/Lenovo/AppData/Roaming/JetBrains/PyCharmCE2020.2/scratches/scratch_21.py", line 53, in <module> jda.fit(X_train_source, X_train_target, y_train_source) File "C:/Users/Lenovo/AppData/Roaming/JetBrains/PyCharmCE2020.2/scratches/scratch_21.py", line 32, in fit self.Xs_new = np.dot(Xs_centered, W) File "<__array_function__ internals>", line 6, in dot ValueError: shapes (144,3000) and (144,3) not aligned: 3000 (dim 1) != 144 (dim 0)
03-31
``` import numpy as np from sklearn.neighbors import KNeighborsClassifier from sklearn.preprocessing import StandardScaler from scipy.linalg import fractional_matrix_power class JDA: def __init__(self, n_components=3, lambd=1e-4): # 调整默认 lambda 至较安全水平 self.n_components = n_components self.lambd = lambd def fit(self, Xs, Xt, ys): ns, _ = Xs.shape nt, _ = Xt.shape Z = np.vstack((Xs, Xt)) Z_mean = np.mean(Z, axis=0) Xs_centered = Xs - np.mean(Xs, axis=0) Xt_centered = Xt - np.mean(Xt, axis=0) C_s = np.cov(Xs_centered.T) / ns C_t = np.cov(Xt_centered.T) / nt Cs_inv_sqrt = self.invsqrt(C_s + self.lambd * np.eye(len(Z_mean))) Ct_inv_sqrt = self.invsqrt(C_t + self.lambd * np.eye(len(Z_mean))) M = np.dot(Cs_inv_sqrt, Ct_inv_sqrt).T U, S, V = np.linalg.svd(M, full_matrices=False) W = np.dot(U[:, :self.n_components], V[:self.n_components].T) self.W = W self.Xs_new = np.dot(Xs_centered, W) self.Xr_new = np.dot(np.concatenate([Xs_centered, Xt_centered]), W) return self def transform(self, X): return np.dot(X - np.mean(X, axis=0), self.W) @staticmethod def invsqrt(matrix): """ 更加稳定的方式 """ return fractional_matrix_power(matrix, -0.5) if __name__ == '__main__': dataset = np.load('dataset.npz') X_train_source_raw = dataset['X_train'] X_train_target_raw = dataset['X_val'] # 使用验证集作为目标域 y_train_source = dataset['y_train'] # 数据标准化 scaler = StandardScaler() X_train_source = scaler.fit_transform(X_train_source_raw) X_train_target = scaler.transform(X_train_target_raw) jda = JDA(n_components=3, lambd=1e-4) jda.fit(X_train_source, X_train_target, y_train_source) X_train_aligned = jda.transform(X_train_source) X_val_aligned = jda.transform(X_train_target) clf = KNeighborsClassifier(n_neighbors=3) clf.fit(X_train_aligned, y_train_source) accuracy = clf.score(jda.transform(scaler.transform(dataset['X_test'])), dataset['y_test']) print(f"Accuracy on test set after JDA alignment: {accuracy:.4f}")```Traceback (most recent call last): File "C:/Users/Lenovo/AppData/Roaming/JetBrains/PyCharmCE2020.2/scratches/scratch_21.py", line 61, in <module> jda.fit(X_train_source, X_train_target, y_train_source) File "C:/Users/Lenovo/AppData/Roaming/JetBrains/PyCharmCE2020.2/scratches/scratch_21.py", line 31, in fit W = np.dot(U[:, :self.n_components], V[:self.n_components].T) File "<__array_function__ internals>", line 6, in dot ValueError: shapes (3000,3) and (3000,3) not aligned: 3 (dim 1) != 3000 (dim 0)
03-31
# 导入必要的库和模块 from smartcar import ticker, encoder from seekfree import KEY_HANDLER from machine import * import gc import time from display import * from seekfree import TSL1401, MOTOR_CONTROLLER, WIRELESS_UART, IMU963RX import math import ustruct # 初始化硬件设备 imu = IMU963RX() # IMU传感器 ccd = TSL1401(10) # CCD传感器,采集周期10次 key = KEY_HANDLER(5) # 按键处理 motor_l = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C25_DIR_C27, 13000, duty=0, invert=True) # 左电机 motor_r = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C24_DIR_C26, 13000, duty=0, invert=True) # 右电机 encoder_l = encoder("D0", "D1", True) # 左编码器 encoder_r = encoder("D2", "D3") # 右编码器 wireless = WIRELESS_UART(115200) # 无线通信 uart2 = UART(2) # 串口2 uart2.init(115200) # 初始化显示设备 cs = Pin('C5', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 片选引脚 # 拉高拉低一次 CS 片选确保屏幕通信时序正常 cs.high() cs.low() rst = Pin('B9', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 复位引脚 dc = Pin('B8', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 数据/命令选择 blk = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 背光控制 drv = LCD_Drv(SPI_INDEX=1, BAUDRATE=60000000, DC_PIN=dc, RST_PIN=rst, LCD_TYPE=LCD_Drv.LCD200_TYPE) # LCD驱动 lcd = LCD(drv) # LCD实例 lcd.color(0x0000, 0xFFFF) # 设置颜色 lcd.mode(2) # 设置显示模式 lcd.clear(0xDEF7) # 清屏使用淡灰色背景 # 初始化引脚 led = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=True) # LED指示灯 beep = Pin('C9', Pin.OUT, pull=Pin.PULL_UP_47K, value=False) # 蜂鸣器 switch1 = Pin('D8', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关1 switch2 = Pin('D9', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关2 end_switch = Pin('C19', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 终点开关 # 全局变量定义 # PID控制相关变量 err_1 = err_sum_1 = err_x_1 = err_last_1 = 0 # PID1误差项 err_2 = err_sum_2 = err_x_2 = err_last_2 = 0 # PID2误差项 err_3 = err_sum_3 = err_x_3 = err_last_3 = 0 # PID3误差项 errt = errt_sum = errt_x = errt_last = 0 # 转向PID误差项 # 系统状态标志 ticker_flag = ticker_flag2 = False # 定时器标志 ticker_count = ticker_count0 = ticker_count3 = 0 # 定时器计数器 motor_dir = 1 # 电机方向 motor_duty = 0 # 电机占空比 motor_duty_max = 1000 # 最大占空比 turn = 0 # 转向量 state1 = switch1.value() # 开关1状态 state2 = switch2.value() # 开关2状态 end_state = end_switch.value() # 终点开关状态 # CCD相关变量 ccd1_zhongzhi = ccd2_zhongzhi = 64 # CCD中心值 left1 = right1 = left2 = right2 = 0 # CCD边界 flag_stute1 = flag_stute2 = 0 # 状态标志 flag_shizi1 = flag_shizi2 = 0 # 十字标志 shreshlod1 = shreshlod2 = 0 # 阈值 # 路径跟踪相关变量 lline = rline = lline2 = rline2 = 0 # 左右边界线 zThreshold = zThreshold2 = 15 # 阈值 zhong = zhong2 = 64 # 中心位置 banma_flag = stop = ysbzw = 0 # 标志位 track_stop = huang_tag = 0 # 跟踪停止标志 huang_l_flag = huang_r_flag = 0 # 黄线标志 huang_l_zt = huang_r_zt = 0 # 黄线状态 bizhan_l = bizhan_r = txzb = 0 # 避障相关 bizhan_flag = cha2 = 0 # 避障标志和差值 zuocha = zuocha2 = 0 # 左差值 zebra = road2 = jifen = 0 # 斑马线和道路标志 art1 = art2 = art3 = puodao_flag = 0 # 其他标志 road = fix = circle = zebra2 = banma_slow = stop_flag = 0 # 控制参数 delta_T = 0.001 # 采样周期为5ms I_ex, I_ey, I_ez = 0.0, 0.0, 0.0 # 积分误差 """""""""""""""主要调节以下参数""""""""""""""" """根据屏幕显示Pitch确定平衡角度""" med_roll_angle = 34 # 平衡角度 前倾+ 后仰- med_speed1 = -185#-1.9 # 速度(速度为零即静止) med_speed = med_speed1 #0 当前速度 # 角速度PID参数##高频震动:增大kd或减小kp angle_kp = -650#2280.0 调节响应速度 angle_ki = -2.25#-2.25 控制车辆在平衡点附近移动 angle_kd = -320 #-122 # 角度PID参数+- roll_angle_Kp = 0.055 #0.06585 控制车辆平衡角度 roll_angle_Ki = 0.00001 #0.00001 roll_angle_Kd = 0.38 #0.2 低频震动 # 速度PID参数 speed_Kp = -0.070#0.0792 加速/减速过慢,调节绝对值 speed_Ki = 0.00000001 #0.00000001 speed_Kd = 0.014 #0.01055 速度波动过大则增大kd或减小kp """""""""""""""主要调节以上参数""""""""""""""" CCD_dir = [64,64] # 转向PID参数 a = 0.35#0.152 Kpc = 0.45#0.2 turn_ki = 20#25 #中线位置调节 turn_kd = 2.0 #防止左右摆动幅度过大 turn_kd2 = -18 #转向速度 # 按键控制变量 param_index = 0 # 当前选择的参数索引 param_editing = True # 是否处于参数编辑模式 # 参数菜单定义 PARAM_MENU = [ "1.AngVel Kp", "2.AngVel Ki", "3.AngVel Kd", "4.Angle Kp", "5.Angle Ki", "6.Angle Kd", "7.Speed Kp", "8.Speed Ki","9.Speed Kd", "10.med_roll_angle", "11.Launch!" ] pid_params = [ # 可调参数列表 angle_kp, angle_ki, angle_kd, roll_angle_Kp, roll_angle_Ki, roll_angle_Kd, speed_Kp, speed_Ki,speed_Kd,med_roll_angle ] PARAM_STEP = [ # 各参数调节步长 10.0, 0.01, 1.0, # 角速度环 0.001, 0.00001, 0.001,# 角度环 0.0000001, 0.00001, 0.00001, # 速度环 0.5#角度 ] # 编码器卡尔曼滤波参数 KAL_P = KAL_P2 = 0.02 # 估算协方差 KAL_G = KAL_G2 = 0.0 # 卡尔曼增益 KAL_Q = KAL_Q2 = 0.70 # 过程噪声协方差 KAL_R = KAL_R2 = 200 # 测量噪声协方差 KAL_Output = KAL_Output2 = 0.0 # 卡尔曼滤波器输出 # 中间变量 angle_1 = speed_1 = counts = motor1 = motor2 = 0 angle_2 = speed_2 =0 distance = id_number = 0 # 距离和ID号 Filter_data = [0, 0, 0] # 滤波数据 PI = 3.14159265358 # 圆周率 last_yaw = 0 # 上次偏航角 max_gyro_x = 0 # 最大角速度X # 无线数据传输缓冲区 data_flag = wireless.data_analysis() data_wave = [0]*8 # 类定义 class anjian: """按键处理类""" def __init__(self, KEY_1, KEY_2, KEY_3, KEY_4): self.One = KEY_1 self.Two = KEY_2 self.Three = KEY_3 self.Four = KEY_4 class bianmaqi: """编码器类""" def __init__(self, KAL_templ_pluse, KAL_tempr_pluse): self.KAL_templ_pluse = KAL_templ_pluse self.KAL_tempr_pluse = KAL_tempr_pluse class Imu_element: """IMU数据类""" def __init__(self, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, Pitch, Roll, Yaw, X, Y, Z, Total_Yaw): self.acc_x = acc_x self.acc_y = acc_y self.acc_z = acc_z self.gyro_x = gyro_x self.gyro_y = gyro_y self.gyro_z = gyro_z self.Pitch = Pitch self.Roll = Roll self.Yaw = Yaw self.X = X self.Y = Y self.Z = Z self.Total_Yaw = Total_Yaw class param: """PID参数类""" def __init__(self, param_Kp, param_Ki): self.param_Kp = param_Kp self.param_Ki = param_Ki class QInfo: """四元数信息类""" def __init__(self): self.q0 = 1.0 self.q1 = 0.0 self.q2 = 0.0 self.q3 = 0.0 # 实例化类 KEY = anjian(0, 0, 0, 0) Encoders = bianmaqi(0, 0) Imu = Imu_element(0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00) Param = param(15.5, 0.006) Q_info = QInfo() # 函数定义 def limit(value, min_value, max_value): """限制数值范围""" return min(max(value, min_value), max_value) def calculate_pid(err, err_sum, err_last, med, value, kp, ki, kd): """计算PID控制值""" err = med - value err_sum += err err_x = err - err_last pwm = kp * err + ki * err_sum + kd * err_x err_last = err return pwm def pid_position_1(med, value, kp, ki, kd): """PID控制器1""" global err_1, err_sum_1, err_last_1 pwm_1 = calculate_pid(err_1, err_sum_1, err_last_1, med, value, kp, ki, kd) err_last_1 = err_1 return pwm_1 def pid_position_2(med, value, kp, ki, kd): """PID控制器2""" global err_2, err_sum_2, err_last_2 pwm_2 = calculate_pid(err_2, err_sum_2, err_last_2, med, value, kp, ki, kd) err_last_2 = err_2 return pwm_2 def pid_position_3(med, value, kp, ki, kd): """PID控制器3""" global err_3, err_sum_3, err_last_3 pwm_3 = calculate_pid(err_3, err_sum_3, err_last_3, med, value, kp, ki, kd) err_last_3 = err_3 return pwm_3 def pid_turn(med, value, kp, ki, kd): """转向PID控制器""" global errt, errt_sum, errt_last pwmt = calculate_pid(errt, errt_sum, errt_last, med, value, kp, ki, kd) errt_last = errt return pwmt def Key(): global param_index, param_editing, pid_params, angle_kp, angle_ki, angle_kd global roll_angle_Kp, roll_angle_Ki, roll_angle_Kd, speed_Kp, speed_Ki key_data = key.get() beep_triggered = False # 标记是否已经触发蜂鸣器 if param_editing: if key_data[0]: # 参数增加 pid_params[param_index] += PARAM_STEP[param_index] print("Key1") beep_triggered = True if key_data[1]: # 参数减少 pid_params[param_index] -= PARAM_STEP[param_index] print("Key2") beep_triggered = True if key_data[2]: # 切换参数 param_index = (param_index + 1) % len(PARAM_MENU) print("Key3") beep_triggered = True if param_index == 10: # 到达"启动!"选项 param_index = 0 if key_data[3]: # 确认参数并控制电机启动的停止 print("Key4:",param_editing) # 更新实际PID参数 angle_kp, angle_ki, angle_kd = pid_params[0], pid_params[1], pid_params[2] roll_angle_Kp = pid_params[3] roll_angle_Ki = pid_params[4] roll_angle_Kd = pid_params[5] speed_Kp, speed_Ki,speed_Kd= pid_params[6], pid_params[7],pid_params[8] med_roll_angle = pid_params[9] param_editing = not param_editing beep_triggered = True # 如果有按键触发,则蜂鸣器提示 if beep_triggered: beep.on() # 蜂鸣器提示 time.sleep_ms(200) beep.off() def invSqrt(x): """快速平方根倒数""" return 1.0 # 简化实现,实际使用时需要优化 def Imu963(): """IMU数据处理函数""" global imu_data, max_gyro_x, last_yaw alpha = 0.3 # 滤波系数 # 数据有效性检查 for i in range(3, 6): if abs(imu_data[i]) < 30 or abs(imu_data[i]) > 30000: imu_data[i] = 0 # 原始数据处理 Imu.X = int(imu_data[3] / 16.4) Imu.Y = int(imu_data[4] / 16.4) Imu.Z = int(imu_data[5] / 16.4) # 角速度处理 Imu.gyro_x = round((float(imu_data[3]) - Filter_data[0]), 3) * PI / 180 / 16.4 Imu.gyro_y = round((float(imu_data[4]) - Filter_data[1]), 3) * PI / 180 / 16.4 Imu.gyro_z = round((float(imu_data[5]) - Filter_data[2]), 3) * PI / 180 / 14.4 # 加速度滤波处理 Imu.acc_x = round(((float(imu_data[0]) * alpha) / 4096 + Imu.acc_x * (1 - alpha)), 3) Imu.acc_y = round(((float(imu_data[1]) * alpha) / 4096 + Imu.acc_y * (1 - alpha)), 3) Imu.acc_z = round(((float(imu_data[2]) * alpha) / 4096 + Imu.acc_z * (1 - alpha)), 3) # 更新AHRS姿态 IMU_AHRSupdate(Imu.gyro_x, Imu.gyro_y, Imu.gyro_z, Imu.acc_x, Imu.acc_y, Imu.acc_z) # 记录最大角速度 if abs(max_gyro_x) < abs(Imu.Pitch): max_gyro_x = Imu.Pitch def Imu963ra_Init(): """IMU初始化校准""" global Filter_data, imu_data Filter_data = [0, 0, 0] # 采集1000次数据进行平均校准 for i in range(1000): imu_data = imu.get() Filter_data[0] += imu_data[3] Filter_data[1] += imu_data[4] Filter_data[2] += imu_data[5] time.sleep_ms(1) # 计算平均值 Filter_data[0] = float(Filter_data[0] / 1000) Filter_data[1] = float(Filter_data[1] / 1000) Filter_data[2] = float(Filter_data[2] / 1000) def IMU_AHRSupdate(gx, gy, gz, ax, ay, az): """AHRS姿态更新算法""" global I_ex, I_ey, I_ez, last_yaw halfT = 0.5 * delta_T # 计算当前重力单位向量 q0q0 = Q_info.q0 * Q_info.q0 q0q1 = Q_info.q0 * Q_info.q1 q0q2 = Q_info.q0 * Q_info.q2 q1q1 = Q_info.q1 * Q_info.q1 q1q3 = Q_info.q1 * Q_info.q3 q2q2 = Q_info.q2 * Q_info.q2 q2q3 = Q_info.q2 * Q_info.q3 q3q3 = Q_info.q3 * Q_info.q3 # 加速度归一化 norm = invSqrt(ax*ax + ay*ay + az*az) ax *= norm ay *= norm az *= norm # 计算当前重力单位向量 vx = 2 * (q1q3 - q0q2) vy = 2 * (q0q1 + q2q3) vz = q0q0 - q1q1 - q2q2 + q3q3 # 计算误差 ex = ay * vz - az * vy ey = az * vx - ax * vz ez = ax * vy - ay * vx # PI修正 I_ex += delta_T * ex I_ey += delta_T * ey I_ez += delta_T * ez gx += Param.param_Kp * ex + Param.param_Ki * I_ex gy += Param.param_Kp * ey + Param.param_Ki * I_ey gz += Param.param_Kp * ez + Param.param_Ki * I_ez # 四元数微分方程 q0 = Q_info.q0 q1 = Q_info.q1 q2 = Q_info.q2 q3 = Q_info.q3 Q_info.q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT Q_info.q1 = q1 + (q0*gx + q2*gz - q3*gy) * halfT Q_info.q2 = q2 + (q0*gy - q1*gz + q3*gx) * halfT Q_info.q3 = q3 + (q0*gz + q1*gy - q2*gx) * halfT # 四元数归一化 norm = invSqrt(Q_info.q0**2 + Q_info.q1**2 + Q_info.q2**2 + Q_info.q3**2) Q_info.q0 *= norm Q_info.q1 *= norm Q_info.q2 *= norm Q_info.q3 *= norm # 计算欧拉角 value1 = limit(-2 * Q_info.q1 * Q_info.q3 + 2 * Q_info.q0 * Q_info.q2, -1, 1) Imu.Roll = round(math.asin(value1) * 180 / math.pi, 3) # 横滚角 Imu.Pitch = round(math.atan2(2 * Q_info.q2 * Q_info.q3 + 2 * Q_info.q0 * Q_info.q1, -2 * Q_info.q1**2 - 2 * Q_info.q2**2 + 1) * 180 / math.pi, 3) # 俯仰角 Imu.Yaw = round(math.atan2(2 * Q_info.q1 * Q_info.q2 + 2 * Q_info.q0 * Q_info.q3, -2 * Q_info.q2**2 - 2 * Q_info.q3**2 + 1) * 180 / math.pi, 3) # 偏航角 # 计算总偏航角 error_yaw = Imu.Yaw - last_yaw if error_yaw < -360: error_yaw += 360 if error_yaw > 360: error_yaw -= 360 Imu.Total_Yaw += error_yaw last_yaw = Imu.Yaw # 保证总偏航角在0-360度范围内 if Imu.Total_Yaw > 360: Imu.Total_Yaw -= 360 if Imu.Total_Yaw < 0: Imu.Total_Yaw += 360 def Wireless(): """无线数据传输处理""" global data_flag, data_wave data_flag = wireless.data_analysis() # 数据解析 # 更新虚拟示波器数据 data_wave[0] = angle_1 data_wave[1] = speed_1 data_wave[2] = Imu.acc_z data_wave[3] = Imu.gyro_x data_wave[4] = Imu.gyro_y data_wave[5] = Imu.Roll data_wave[6] = Imu.Pitch # 发送数据到上位机 wireless.send_oscilloscope(*data_wave) def KalmanFilter(input): """卡尔曼滤波器""" global KAL_P, KAL_G, KAL_Output KAL_P = KAL_P + KAL_Q # 估算协方差更新 KAL_G = KAL_P / (KAL_P + KAL_R) # 计算卡尔曼增益 KAL_Output = KAL_Output + KAL_G * (input - KAL_Output) # 更新最优估计值 KAL_P = (1 - KAL_G) * KAL_P # 更新协方差 return KAL_Output def KalmanFilter2(input): """第二个卡尔曼滤波器""" global KAL_P2, KAL_G2, KAL_Output2 KAL_P2 = KAL_P2 + KAL_Q2 KAL_G2 = KAL_P2 / (KAL_P2 + KAL_R2) KAL_Output2 = KAL_Output2 + KAL_G2 * (input - KAL_Output2) KAL_P2 = (1 - KAL_G2) * KAL_P2 return KAL_Output2 def draw_rect(x, y, width, height, color, thick=1, fill=False): """用line方法实现矩形绘制""" if fill: # 填充矩形 - 用垂直线条填充 for i in range(height): lcd.line(x, y+i, x+width-1, y+i, color=color, thick=thick) else: # 只画边框 lcd.line(x, y, x+width-1, y, color=color, thick=thick) # 上边 lcd.line(x, y+height-1, x+width-1, y+height-1, color=color, thick=thick) # 下边 lcd.line(x, y, x, y+height-1, color=color, thick=thick) # 左边 lcd.line(x+width-1, y, x+width-1, y+height-1, color=color, thick=thick) # 右边 last_title = None last_param_value = None last_param_index = None def ips200_display(): """美化后的LCD显示函数""" global last_title,last_motors global last_param_value, last_param_index #1. ---- 顶部状态栏 ---- if last_title is None: draw_rect(0, 0, 320, 20, 0x0000, fill=True) # 黑色背景 title = " RT1021 Dashboard " colors = [0xF800, 0xFC00, 0xFFE0, 0x07E0, 0x07FF, 0x001F, 0x801F, 0xF81F] # 彩虹色 # 阴影效果(偏移1像素) for i, char in enumerate(title): x_pos = 40 + i * 8 lcd.str16(x_pos + 1, 2 + 1, char, 0x3186) # 灰色阴影 for i, char in enumerate(title): x_pos = 40 + i * 8 color = colors[i % len(colors)] lcd.str16(x_pos, 2, char, color) last_title = True # 2. IMU数据区域 (带边框) draw_rect(0, 20, 105, 65, 0x001F, thick=1) # 蓝色边框 lcd.str16(15, 20, "Attitude", 0x001F) # 蓝色标题 lcd.str16(5, 40, f"Pitch:{Imu.Pitch:6.2f}", 0x0000) # 黑色数据 lcd.str16(5, 60, f"Yaw:{Imu.Yaw:6.2f}", 0x0000) # 3. 运动控制区域 (带边框) draw_rect(105, 20, 135, 65, 0xF800, thick=1) # 红色边框 lcd.str16(127, 20, "Motion Ctrl", 0xF800) lcd.str16(117, 40, f"Speed: {med_speed:4.1f}", 0x0000) lcd.str16(117, 60, f"Turn: {turn:6.3f}", 0x0000) # 4. 电机信息区域 (带边框) draw_rect(0, 85, 310, 75, 0x0000, thick=1) # 黑色边框 lcd.str16(5, 110, "Motor", 0x0000) lcd.str16(5, 126, "Output", 0x0000) # 5. 陀螺仪数据区域 (带边框) draw_rect(0, 160, 250, 50, 0xFC80, thick=1) # 橙色边框 lcd.str16(5, 165, "Gyroscope", 0xFC80) lcd.str16(5, 185, f"X:{Imu.gyro_x:7.3f} Y:{Imu.gyro_y:7.3f} Z:{Imu.gyro_z:7.3f}", 0x0000) # 6. 编码器数据区域 (带边框) draw_rect(0, 210, 250, 40, 0x79FF, thick=1) # 天蓝色边框 lcd.str16(5, 220, f"Encoder L:{Encoders.KAL_templ_pluse:6.1f} R:{Encoders.KAL_tempr_pluse:6.1f}", 0x0000) # 7. CCD数据区域 (带边框) draw_rect(0, 250, 250, 70, 0xAA55, thick=1) # 紫色边框 lcd.str16(5, 255, "CCD Line Position", 0xAA55) lcd.str16(5, 275, f"L1:{lline:3d} R1:{rline:3d} Mid:{zhong:3d}", 0x0000) lcd.str16(5, 295, f"L2:{lline2:3d} R2:{rline2:3d} Mid2:{zhong2:3d}", 0x0000) # 8. 道路识别信息 lcd.str16(5, 325, f"Road Type: {road:.2f}", 0x0000) # 9. 参数编辑模式特殊显示 lcd.str16(60, 90, PARAM_MENU[param_index], 0xFFFF) # 白色标题 # 操作提示可以只绘制一次 if last_param_index is None: draw_rect(60, 135, 190, 20, 0xFFFF, thick=1, fill=True) lcd.str16(65, 137, "1:+ 2:- 3:Next 4:OK", 0x0000) # 参数值显示 (带背景色) if pid_params[param_index] != last_param_value or param_index != last_param_index: color = 0x07E0 if pid_params[param_index] >=0 else 0xF800 #绿/红区分正负 draw_rect(60, 110, 190, 20, color, thick=1, fill=True) lcd.str16(65, 112, f"Value: {pid_params[param_index]:.4f}", 0x0000) last_param_value = pid_params[param_index] last_param_index = param_index class ALL_CCD: def __init__(self): self.num=0 self.dir = 0 self.dir_0 = 0 self.left = 38 self.right = 82 self.left_0 = 43 self.right_0 = 78 self.lode_wid = 44 self.lode_wid_0 = 35 self.mid = 60 self.mid_line = 60 self.mid_line_0 = 60 self.last_angle = 62 self.last_angle_1 = 62 def CCD1(self,ccd_data): global av_dir0,a8 #计算左右平均像素值 value_av = (max(ccd_data)+min(ccd_data))//2 #二次处理 ccd_data = [250 if num > value_av else 0 for num in ccd_data] #计算左右平均像素值 av_l = int(sum(ccd_data[15: 64])/49) av_r = int(sum(ccd_data[64:113])/49) if av_l>200 and av_r>200: a8=8 else: a8=0 print("ccd1",av_l,av_r) for i in range(127): if ccd_data[i]==0 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]: ccd_data[i] = 250 elif ccd_data[i]==250 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]: ccd_data[i] =0 ccd_dataA = ccd_data #差比和 new_data = [int(abs(ccd_dataA[i]-ccd_dataA[i+1]))for i in range(len(ccd_dataA)-1)]#127 result_l = [self.mid_line - i for i,x in enumerate(new_data[self.mid_line:15+a8:-1]) if x > 0] result_r = [i + self.mid_line for i,x in enumerate(new_data[self.mid_line:len(new_data)-15-a8]) if x >0] #print("1bianjie",result_l,result_r) #判断边界 if len(result_l)>0 and len(result_r)>0: self.dir = 0 av_dir0 = 0 self.left = result_l[0] self.right = result_r[0] if abs(self.left - self.right) > 92: self.mid_line = 62 self.dir = 1 av_dir0 = 404 #边界二次处理 if abs(self.left - self.right) - self.lode_wid > 15: if abs(self.left - self.mid) > abs(self.right- self.mid): self.left = self.right - self.lode_wid elif abs(self.left - self.mid) < abs(self.right- self.mid): self.right = self.left + self.lode_wid elif len(result_l)== 0 and len(result_r)>0 and result_r[0] < 108: if av_r < av_l and av_l>=240 and av_r<180: av_dir0 = 11 else: av_dir0 = 1 if av_r < av_l: self.right = result_r[0] self.left = self.right - self.lode_wid else: self.left = result_r[0] self.right = self.left + self.lode_wid elif len(result_l)>0 and len(result_r)==0 and result_l[0] > 20: if av_r > av_l and av_r>=240 and av_l<180: av_dir0 = 22 else: av_dir0 = 2 if av_r < av_l: self.right = result_l[0] self.left = self.right - self.lode_wid else: self.left = result_l[0] self.right = self.left + self.lode_wid else: self.mid_line = 62 self.dir = 1 av_dir0 = 404 track_info = (self.left + self.right)//2# 计算赛道中心线位置 if abs(track_info - self.mid_line) > 30: track_info = self.mid_line self.dir = 1 self.last_angle = track_info self.mid_line = track_info if self.dir == 1: if abs(track_info-self.last_angle)<4: self.dir =0 else: return self.last_angle else: return track_info def CCD2(self,ccd_data): global av_dir1,a8,speed_l,speed_r,mortor_dir self.num+=1 count=0 value_av = (max(ccd_data)+min(ccd_data))//2 #二次处理 ccd_data = [250 if num > value_av else 0 for num in ccd_data] zebra_crossing = [i for i in range(127) if (ccd_data[i] == 0 and ccd_data[i + 1] == 250) or (ccd_data[i] == 250 and ccd_data[i + 1] == 0)] for i in range(len(zebra_crossing) - 1): if abs(zebra_crossing[i] - zebra_crossing[i + 1]) < 5: count += 1 if count >= 10 and self.num>250 and -25<int(imu_z)<25: speed_l=0 speed_r=0 mortor_dir=2 self.num=0 #计算左右平均像素值 av_l = int(sum(ccd_data[15:64])/49) av_r = int(sum(ccd_data[64:113])/49) if av_l>200 and av_r>200: a8=8 else: a8=0 #print("ccd2",av_l,av_r) for i in range(127): if ccd_data[i]==0 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]: ccd_data[i] = 250 elif ccd_data[i]==250 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]: ccd_data[i] =0 ccd_dataA = ccd_data #差比和 new_data = [int(abs(ccd_dataA[i]-ccd_dataA[i+1]))for i in range(len(ccd_dataA)-1)]#127 result_l = [self.mid_line_0 - i for i,x in enumerate(new_data[self.mid_line_0:15+a8:-1]) if x > 0] result_r = [i + self.mid_line_0 for i,x in enumerate(new_data[self.mid_line_0:len(new_data)-15-a8]) if x >0] #print("2bianjie",result_l,result_r) #判断边界 if len(result_l)>0 and len(result_r)>0: av_dir1 = 0 self.dir_0 = 0 self.left_0 = result_l[0] self.right_0 = result_r[0] if abs(self.left_0 - self.right_0) > 92: self.mid_line_0 = 62 av_dir1 = 404 self.dir_0 = 1 #边界二次处理 if abs(self.left_0 - self.right_0) - self.lode_wid_0 > 15: if abs(self.left_0 - self.mid) > abs(self.right_0- self.mid): self.left_0 = self.right_0 - self.lode_wid_0 elif abs(self.left_0 - self.mid) < abs(self.right_0- self.mid): self.right_0 = self.left_0 + self.lode_wid_0 elif len(result_l)== 0 and len(result_r)>0 and result_r[0] < 108: if av_r < av_l and av_l>=240 and av_r<180: av_dir1 = 11 else: av_dir1 = 1 if av_r < av_l: self.right_0 = result_r[0] self.left_0 = self.right_0 - self.lode_wid_0 else: self.left_0 = result_r[0] self.right_0 = self.left_0 + self.lode_wid_0 elif len(result_l)>0 and len(result_r)==0 and result_l[0] > 20: if av_r > av_l and av_r>=240 and av_l<180: av_dir1 = 22 else: av_dir1 = 2 if av_r < av_l: self.right_0 = result_l[0] self.left_0 = self.right_0 - self.lode_wid_0 else: self.left_0 = result_l[0] self.right_0 = self.left_0 + self.lode_wid_0 else: self.mid_line_0 = 62 av_dir1 = 404 self.dir_0 = 1 track_info = (self.left_0 + self.right_0)//2# 计算赛道中心线位置 if abs(track_info - self.mid_line_0) > 30 : track_info = self.mid_line_0 self.dir_0 = 1 self.last_angle_1 = track_info self.mid_line_0 = track_info if self.dir_0 == 1: if abs(track_info-self.last_angle_1)<4: self.dir_0 =0 else: return self.last_angle_1 else: return track_info def control_turn(zhong1_2): """转向控制函数""" if zhong1_2 is None: # 如果输入无效,使用默认值(居中) zhong1_2 = 64 errt = (zhong1_2 - 63) turn_kp = a * abs(errt) + Kpc turn = pid_turn(63, zhong1_2, turn_kp, turn_ki, turn_kd) + Imu.gyro_z * turn_kd2 return turn def angle_speed1(med_gyro, cur_gyro): """角速度控制函数""" motor = pid_position_1(med_gyro, cur_gyro, angle_kp, angle_ki, angle_kd) return limit(motor, -4000, 4000) def angle(med_roll_angle, cur_roll_angle): """角度控制函数""" global angle_1 angle_1 = pid_position_2(med_roll_angle, cur_roll_angle, roll_angle_Kp, roll_angle_Ki, roll_angle_Kd) return angle_1 def speed(med_speed, cur_speed): """速度控制函数""" global speed_1 speed_1 = pid_position_3(med_speed, cur_speed, speed_Kp, speed_Ki, speed_Kd) return speed_1 def parse_data(): """解析串口数据""" if uart2.any(): line = uart2.readline() # 读取一行数据 if line is None: return None, None try: line_str = line.decode('utf-8').strip() # 转换为字符串 parts = line_str.split(',') # 按逗号分割 if len(parts) >= 2: var1 = float(parts[0]) # 第一个参数为浮点数 var2 = int(parts[1]) # 第二个参数为整数 return var1, var2 except (UnicodeDecodeError, ValueError): pass return None, None # 定时器中断处理函数 def time_pit_handler(time): """定时器1中断处理""" global ticker_flag, ticker_count, speed_1, angle_1, motor1, motor2 ticker_flag = True ticker_count = (ticker_count + 1) if (ticker_count < 10) else 1 # 1ms周期任务 if ticker_count % 1 == 0: Imu963() # 更新IMU数据 # 电机控制 motor1 = angle_speed1(angle_1, Imu.gyro_x) motor2 = angle_speed1(angle_1, Imu.gyro_x) motor1 = limit(motor1 + control_turn(CCD_dir[0]), -4000, 4000) motor2 = limit(motor2 - control_turn(CCD_dir[0]), -4000, 4000) if not param_editing: motor_l.duty(-motor1) motor_r.duty(-motor2) else: motor_l.duty(0) motor_r.duty(0) # 5ms周期任务 if ticker_count % 5 == 0: angle_1 = angle(med_roll_angle - speed_1, Imu.Pitch) # 10ms周期任务 if ticker_count % 10 == 0: speed_1 = speed(med_speed, Encoders.KAL_templ_pluse) speed_2 = speed(med_speed, Encoders.KAL_tempr_pluse) #print("左误差 ={:>6f}, 右误差 ={:>6f}\r\n".format(speed_1,speed_2)) def time_pit2_handler(time): """定时器2中断处理""" global ticker_flag2 ticker_flag2 = True Encoders.KAL_templ_pluse = KalmanFilter(encoder_l.get()) # 左编码器滤波 Encoders.KAL_tempr_pluse = KalmanFilter2(encoder_r.get()) # 右编码器滤波 CCD = ALL_CCD()#实例化 # 初始化定时器 pit1 = ticker(1) pit2 = ticker(2) pit3 = ticker(3) # 关联数据采集 pit1.capture_list(imu) pit2.capture_list(ccd) pit3.capture_list(encoder_l, encoder_r,key) # 设置定时器回调函数 pit1.callback(time_pit_handler) pit2.callback(time_pit2_handler) # 初始化IMU Imu963ra_Init() # 启动定时器 pit1.start(1) # 1ms周期 pit2.start(6) # 6ms周期 pit3.start(10) # 10ms周期 # 主循环 while True: Key() # 处理按键输入 if ticker_flag: ccd_data1 = ccd.get(0) ccd_data2 = ccd.get(1) CCD_dir[0] =CCD.CCD1(ccd_data1) CCD_dir[1] =CCD.CCD2(ccd_data2) # print(ccd_data1) # print(ccd_data2) if CCD_dir[0] is None: zhong2 = 404 else : zhong2=CCD_dir[0] # print(ccd_data1) # print("CCD1:",CCD_dir[0]) # ticker_flag = False #电机启动屏幕关闭刷新 if not param_editing and last_title: lcd.str16(165, 90, "RUN>>>", 0x0000) last_title = None #与上相反 if param_editing: ips200_display() # 检查拨码开关状态 if switch2.value() != state2: print("Test program stop.") break # 垃圾回收 gc.collect() 小车运行代码,不根据轨道走,蛇形走,我应该如何调参
最新发布
06-11
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值