自己定义atan2的backward

这篇博客探讨了在深度学习网络训练过程中遇到的NaN值问题,其原因是函数在某些点不可导。作者提供了一个自定义的`angle`函数,通过引入小值避免在反向传播中出现NaN。该函数使用`torch.atan2`进行正向传播,并在反向传播时确保除数不为零,以防止数值不稳定。博客还提到了`clamp_min_`函数的应用,用于设置梯度计算的下限,以保持数值稳定性。

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

报错:网络训练过程中出现nan值

原因:函数并不是在所有点可导,增加小值避免反向传播出现nan值的情况。

from torch.autograd import Function

from torch import Tensor

class angle(Function):

    """Similar to torch.angle but robustify the gradient for zero magnitude."""

    @staticmethod

    def forward(ctx, x: Tensor):

        ctx.save_for_backward(x)

        return torch.atan2(x.imag, x.real)

    @staticmethod

    def backward(ctx, grad: Tensor):

        (x,) = ctx.saved_tensors

        grad_inv = grad / (x.real.square() + x.imag.square()).clamp_min_(1e-10)

        return torch.view_as_complex(torch.stack((-x.imag * grad_inv, x.real * grad_inv), dim=-1))

 ref:GitHub - Rikorose/DeepFilterNet: Noise supression using deep filtering

#include "control.h" #include "Lidar.h" float Move_X = 0, Move_Z = 0; // Ä¿±êËٶȺÍÄ¿±êתÏòËÙ¶È float PWM_Left, PWM_Right; // ×óÓÒµç»úPWMÖµ float RC_Velocity, RC_Turn_Velocity; // Ò£¿Ø¿ØÖƵÄËÙ¶È u8 Mode = Normal_Mode; // ģʽѡÔñ£¬Ä¬ÈÏÊÇÆÕͨµÄ¿ØÖÆÄ£Ê½ Motor_parameter MotorA, MotorB; // ×óÓÒµç»úÏà¹Ø±äÁ¿ int Servo_PWM = SERVO_INIT; // °¢¿ËÂü¶æ»úÏà¹Ø±äÁ¿ u8 Lidar_Detect = Lidar_Detect_ON; // µç´&Aring;ѲÏßģʽÀ×´ï¼ì²âÕϰ­ÎĬÈÏ¿ªÆô float CCD_Move_X = 0.3; // CCDѲÏßËÙ¶È float ELE_Move_X = 0.3; // µç´&Aring;ѲÏßËÙ¶È u8 Ros_count = 0, Lidar_flag_count = 0; u8 cnt = 0; Encoder OriginalEncoder; // Encoder raw data //±àÂëÆ÷ԭʼÊý¾Ý short Accel_Y, Accel_Z, Accel_X, Accel_Angle_x, Accel_Angle_y, Gyro_X, Gyro_Z, Gyro_Y; int forward_cnt = 800; int left_cnt = 120; int right_cnt = 125; int stop_cnt = 200; int stop_flag = 0; int mode_cnt = 0; int state_cnt = 0; int stop_protect = 0; /************************************************************************** Function: Control Function Input : none Output : none º¯Êý¹¦ÄÜ£º5ms¶¨Ê±ÖжϿØÖƺ¯Êý Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void forward(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 2040; } void left(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 2050; } void right(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 0; } void stop(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 0; } void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim == &htim5) { Get_KeyVal(); if(mode_cnt == 0) stop(); //ģʽһ if(mode_cnt == 1){ if(forward_cnt > 0){ forward(); forward_cnt--; // if(left_cnt > 0){ // left(); // left_cnt--; // if(right_cnt > 0){ // right(); // right_cnt--; } else{ stop(); } } //ģʽ¶þ else if(mode_cnt == 2){ if(forward_cnt > 0){ forward(); forward_cnt--; } else if(stop_cnt > 0 && forward_cnt == 0){ stop(); stop_cnt--; } else if(stop_cnt == 0 && stop_flag == 0){ forward_cnt = 800; stop_flag = 1; } else{ stop(); } } //ģʽÈý else if(mode_cnt == 3){ if(forward_cnt > 0 && state_cnt == 0){//µÚ1״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(right_cnt > 0 && state_cnt == 1){//µÚ2״̬£ºÓÒת right(); right_cnt--; } else if(forward_cnt > 0 && state_cnt == 2){//µÚ3״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 3){//µÚ4״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 4){//µÚ5״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 5){//µÚ6״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 6){//µÚ7״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else{ if(stop_protect == 0 && stop_cnt == 0){ stop_cnt = 100; stop_protect = 1; } else if(stop_cnt > 0){ stop(); stop_cnt--; } else if(stop_protect == 1 && stop_cnt == 0){ stop_protect = 0; if(state_cnt == 0){//״̬1Çл»×´Ì¬2 state_cnt++; right_cnt = 125; } else if(state_cnt == 1){//״̬2Çл»×´Ì¬3 state_cnt++; forward_cnt = 585; } else if(state_cnt == 2){//״̬3Çл»×´Ì¬4 state_cnt++; left_cnt = 120; } else if(state_cnt == 3){//״̬4Çл»×´Ì¬5 state_cnt++; forward_cnt = 585; } else if(state_cnt == 4){//״̬5Çл»×´Ì¬6 state_cnt++; left_cnt = 120; } else if(state_cnt == 5){//״̬6Çл»×´Ì¬7 state_cnt++; forward_cnt = 585; } } } } Set_Pwm(-MotorA.Motor_Pwm, MotorB.Motor_Pwm); // Çý¶¯µç»ú } } /************************************************************************** Function: Bluetooth_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºÊÖ»úÀ¶ÑÀ¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Bluetooth_Control(void) { if (Flag_Direction == 0) Move_X = 0, Move_Z = 0; // Í£Ö¹ else if (Flag_Direction == 1) Move_X = RC_Velocity, Move_Z = 0; // ǰ½ø else if (Flag_Direction == 2) Move_X = RC_Velocity, Move_Z = Pi / 2; // ÓÒǰ else if (Flag_Direction == 3) Move_X = 0, Move_Z = Pi / 2; // ÏòÓÒ else if (Flag_Direction == 4) Move_X = -RC_Velocity, Move_Z = Pi / 2; // ÓÒºó else if (Flag_Direction == 5) Move_X = -RC_Velocity, Move_Z = 0; // ºóÍË else if (Flag_Direction == 6) Move_X = -RC_Velocity, Move_Z = -Pi / 2; // ×óºó else if (Flag_Direction == 7) Move_X = 0, Move_Z = -Pi / 2; // Ïò×ó else if (Flag_Direction == 8) Move_X = RC_Velocity, Move_Z = -Pi / 2; // ×óǰ else Move_X = 0, Move_Z = 0; if (Car_Num == Akm_Car) { // Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed // °¢¿ËÂü½á¹¹Ð¡³µ×ª»»ÎªÇ°ÂÖתÏò½Ç¶È Move_Z = Move_Z * 2 / 10; } Move_X = Move_X / 1000; Move_Z = -Move_Z; // ת»»ÎªËÙ¶ÈתΪm/s } /************************************************************************** Function: PS2_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºPS2ÊÖ±ú¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void PS2_Control(void) { int LY, RX; // ÊÖ±úADCµÄÖµ int Threshold = 20; // ãÐÖµ£¬ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ static u8 Key1_Count = 0, Key2_Count = 0; // ÓÃÓÚ¿ØÖƶÁȡҡ¸ËµÄËÙ¶È // ת»¯Îª128µ½-128µÄÊýÖµ LY = -(PS2_LY - 128); // ×ó±ßYÖá¿ØÖÆÇ°½øºóÍË RX = -(PS2_RX - 128); // ÓÒ±ßXÖá¿ØÖÆ×ªÏò if (LY > -Threshold && LY < Threshold) LY = 0; if (RX > -Threshold && RX < Threshold) RX = 0; // ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ Move_X = (RC_Velocity / 128) * LY; // ËÙ¶È¿ØÖÆ£¬Á¦¶È±íʾËÙ¶È´óС if (Car_Num == Akm_Car) // °¢¿ËÂü³µ×ªÏò¿ØÖÆ£¬Á¦¶È±íʾתÏò½Ç¶È Move_Z = -(RC_Turn_Velocity / 128) * RX; else // ÆäËû³µÐÍתÏò¿ØÖÆ { if (Move_X >= 0) Move_Z = -(RC_Turn_Velocity / 128) * RX; // תÏò¿ØÖÆ£¬Á¦¶È±íʾתÏòËÙ¶È else Move_Z = (RC_Turn_Velocity / 128) * RX; } if (PS2_KEY == PSB_L1) // °´ÏÂ×ó1¼ü¼ÓËÙ£¨°´¼üÔÚ¶¥ÉÏ£© { if ((++Key1_Count) == 20) // µ÷½Ú°´¼ü·´Ó¦ËÙ¶È { PS2_KEY = 0; Key1_Count = 0; if ((RC_Velocity += X_Step) > MAX_RC_Velocity) // ǰ½ø×î´óËÙ¶È800mm/s RC_Velocity = MAX_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity += Z_Step) > MAX_RC_Turn_Bias) // תÏò×î´óËÙ¶È325 RC_Turn_Velocity = MAX_RC_Turn_Bias; } } } else if (PS2_KEY == PSB_R1) // °´ÏÂÓÒ1¼ü¼õËÙ { if ((++Key2_Count) == 20) { PS2_KEY = 0; Key2_Count = 0; if ((RC_Velocity -= X_Step) < MINI_RC_Velocity) // ǰºó×îСËÙ¶È210mm/s RC_Velocity = MINI_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity -= Z_Step) < MINI_RC_Turn_Velocity) // תÏò×îСËÙ¶È45 RC_Turn_Velocity = MINI_RC_Turn_Velocity; } } } else Key2_Count = 0, Key2_Count = 0; // ¶ÁÈ¡µ½ÆäËû°´¼üÖØÐ¼ÆÊý Move_X = Move_X / 1000; Move_Z = -Move_Z; // ËÙ¶ÈMove_XתΪm/s } /************************************************************************** Function: Get_Velocity_From_Encoder Input : none Output : none º¯Êý¹¦ÄÜ£º¶ÁÈ¡±àÂëÆ÷ºÍת»»³ÉËÙ¶È Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Velocity_From_Encoder(void) { // Retrieves the original data of the encoder // »ñÈ¡±àÂëÆ÷µÄԭʼÊý¾Ý float Encoder_A_pr, Encoder_B_pr; OriginalEncoder.A = Read_Encoder(Encoder1); OriginalEncoder.B = Read_Encoder(Encoder2); // Decide the encoder numerical polarity according to different car models // ¸ù¾Ý²»Í¬Ð¡³µÐͺ&Aring;¾ö¶¨±àÂëÆ÷ÊýÖµ¼«ÐÔ switch (Car_Num) { case Akm_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Diff_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Small_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Big_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; } // The encoder converts the raw data to wheel speed in m/s // ±àÂëÆ÷ԭʼÊý¾Ýת»»Îª³µÂÖËÙ¶È£¬µ¥Î»m/s MotorA.Current_Encoder = Encoder_A_pr * Frequency * Perimeter / 60000.0f; // 60000 = 4*500*30 MotorB.Current_Encoder = Encoder_B_pr * Frequency * Perimeter / 60000.0f; // 1560=4*13*30=2£¨Á½Â·Âö³&aring;£©*2£¨ÉÏÏÂÑØ¼ÆÊý£©*»ô¶û±àÂëÆ÷13Ïß*µç»úµÄ¼õËÙ±È // MotorA.Current_Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Akm_wheelspacing//(4*13*30); // MotorB.Current_Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Akm_wheelspacing/Encoder_precision; } /************************************************************************** Function: Drive_Motor Input : none Output : none º¯Êý¹¦ÄÜ£ºÔ˶¯Ñ§Äæ½â Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ // Ô˶¯Ñ§Äæ½â£¬ÓÉxºÍyµÄËٶȵõ½±àÂëÆ÷µÄËÙ¶È,VxÊÇm/s,Vzµ¥Î»ÊǶÈ/s(½Ç¶ÈÖÆ) // °¢¿ËÂü³µVzÊǶæ»úתÏòµÄ½Ç¶È(»¡¶ÈÖÆ) void Get_Target_Encoder(float Vx, float Vz) { float MotorA_Velocity, MotorB_Velocity; float amplitude = 3.5f; // Wheel target speed limit //³µÂÖÄ¿±êËÙ¶ÈÏÞ·ù Move_X = target_limit_float(Move_X, -1.2, 1.2); Move_Z = target_limit_float(Move_Z, -Pi / 3, Pi / 3); if (Car_Num == Akm_Car) // °¢¿ËÂü³µ { // Ackerman car specific related variables //°¢¿ËÂüС³µ×¨ÓÃÏà¹Ø±äÁ¿ float R, ratio = 636.56, AngleR, Angle_Servo; // For Ackerman small car, Vz represents the front wheel steering Angle // ¶ÔÓÚ°¢¿ËÂüС³µVz´ú±íÓÒǰÂÖתÏò½Ç¶È AngleR = Vz; R = Akm_axlespacing / tan(AngleR) - 0.5f * Wheelspacing; // Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad // ǰÂÖתÏò½Ç¶ÈÏÞ·ù(¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È)£¬µ¥Î»£ºrad AngleR = target_limit_float(AngleR, -0.49f, 0.32f); // Inverse kinematics //Ô˶¯Ñ§Äæ½â if (AngleR != 0) { MotorA.Target_Encoder = Vx * (R - 0.081f) / R; MotorB.Target_Encoder = Vx * (R + 0.081f) / R; } else { MotorA.Target_Encoder = Vx; MotorB.Target_Encoder = Vx; } // The PWM value of the servo controls the steering Angle of the front wheel // ¶æ»úPWMÖµ£¬¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È Angle_Servo = -0.628f * pow(AngleR, 3) + 1.269f * pow(AngleR, 2) - 1.772f * AngleR + 1.573f; Servo_PWM = SERVO_INIT + (Angle_Servo - 1.572f) * ratio; // printf("%d\r\n",Servo_PWM); // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); Servo_PWM = target_limit_int(Servo_PWM, 800, 2200); // Servo PWM value limit //¶æ»úPWMÖµÏÞ·ù } else if (Car_Num == Diff_Car) // ²îËÙС³µ { if (Vx < 0) Vz = -Vz; else Vz = Vz; // Inverse kinematics //Ô˶¯Ñ§Äæ½â MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Small_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Big_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } } /************************************************************************** Function: Get_Motor_PWM Input : none Output : none º¯Êý¹¦ÄÜ£º×ª»»³ÉÇý¶¯µç»úµÄPWM Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Motor_PWM(void) { // ¼ÆËã×óÓÒµç»ú¶ÔÓ¦µÄPWM MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder); MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder); if (Mode == Normal_Mode || Mode == Measure_Distance_Mode) { // Â˲¨£¬Ê¹Æð²½ºÍÍ£Ö¹ÉÔ΢ƽ»¬Ò»Ð© MotorA.Motor_Pwm = Mean_Filter_Left(MotorA.Motor_Pwm); MotorB.Motor_Pwm = Mean_Filter_Right(MotorB.Motor_Pwm); } // ÏÞ·ù MotorA.Motor_Pwm = PWM_Limit(MotorA.Motor_Pwm, PWM_MAX, PWM_MIN); MotorB.Motor_Pwm = PWM_Limit(MotorB.Motor_Pwm, PWM_MAX, PWM_MIN); } /************************************************************************** Function: PWM_Limit Input : IN;max;min Output : OUT º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ Èë¿Ú²ÎÊý: IN£ºÊäÈë²ÎÊý max£ºÏÞ·ù×î´óÖµ min£ºÏÞ·ù×îСֵ ·µ»Ø Öµ£ºÏÞ·ùºóµÄÖµ **************************************************************************/ float PWM_Limit(float IN, int max, int min) { float OUT = IN; if (OUT > max) OUT = max; if (OUT < min) OUT = min; return OUT; } /************************************************************************** Function: Limiting function Input : Value Output : none º¯Êý¹¦ÄÜ£ºÏÞ·ùº¯Êý Èë¿Ú²ÎÊý£º·ùÖµ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float target_limit_float(float insert, float low, float high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } int target_limit_int(int insert, int low, int high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } /************************************************************************** Function: Check whether it is abnormal Input : none Output : 1:Abnormal;0:Normal º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£ **************************************************************************/ u8 Turn_Off(void) { u8 temp = Normal; Flag_Stop = KEY2_STATE; // ¶ÁÈ¡°´¼ü2״̬£¬°´¼ü2¿ØÖƵç»úµÄ¿ª¹Ø if (Voltage < 1000) // µç³ØµçѹµÍÓÚ10V¹Ø±Õµç»ú,LEDµÆ¿ìËÙÉÁ˸ LED_Flash(50), temp = Abnormal; else LED_Flash(200); // ÿһÃëÉÁÒ»´Î£¬Õý³£ÔËÐÐ if (Flag_Stop) temp = Abnormal; return temp; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Left(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Right(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Lidar_Avoid Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï±ÜÕÏģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_Avoid(void) { int i = 0; u8 calculation_angle_cnt = 0; // ÓÃÓÚ&Aring;жÏ100¸öµãÖÐÐèÒª×ö±ÜÕϵĵã float angle_sum = 0; // ´ÖÂÔ¼ÆËãÕϰ­ÎïλÓÚ×ó»òÕßÓÒ u8 distance_count = 0; // ¾àÀëСÓÚijֵµÄ¼ÆÊý int distance = 350; // É趨±ÜÕϾàÀë,ĬÈÏÊÇ300 if (Car_Num == Akm_Car) distance = 400; // °¢¿ËÂü³µÉ趨ÊÇ400mm else if (Car_Num == Big_Tank_Car) distance = 500; // ´óÂÄ´ø³µÉ趨ÊÇ500mm for (i = 0; i < lap_count; i++) { if ((Dataprocess[i].angle > 310) || (Dataprocess[i].angle < 50)) { if ((0 < Dataprocess[i].distance) && (Dataprocess[i].distance < distance)) // ¾àÀëСÓÚ350mmÐèÒª±ÜÕÏ,Ö»ÐèÒª100¶È·¶Î§ÄÚµã { calculation_angle_cnt++; // ¼ÆËã¾àÀëСÓÚ±ÜÕϾàÀëµÄµã¸öÊý if (Dataprocess[i].angle < 50) angle_sum += Dataprocess[i].angle; else if (Dataprocess[i].angle > 310) angle_sum += (Dataprocess[i].angle - 360); // 310¶Èµ½50¶Èת»¯Îª-50¶Èµ½50¶È if (Dataprocess[i].distance < 200) // ¼Ç¼СÓÚ200mmµÄµãµÄ¼ÆÊý distance_count++; } } } if (calculation_angle_cnt < 8) // СÓÚ8µã²»ÐèÒª±ÜÕÏ£¬È¥³ýһЩÔëµã { if ((Move_X += 0.1) >= Aovid_Speed) // ±ÜÕϵÄËÙ¶ÈÉ趨Ϊ260£¬Öð½¥Ôö¼Óµ½260¿ÉÉÔ΢ƽ»¬Ò»Ð© Move_X = Aovid_Speed; Move_Z = 0; // ²»±ÜÕÏʱ²»ÐèҪתÍä } else // ÐèÒª±ÜÕÏ£¬¼òµ¥µØ&Aring;жÏÕϰ­Î﷽λ { if (Car_Num == Akm_Car) // °¢¿ËÂü³µÐÍÓжæ»ú£¬ÐèÒªÌØÊâ´¦Àí { if (distance_count > 8) // ¾àÀëСÓÚ±ÜÕ½¾àÀë Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ0.25 Move_X = Aovid_Speed * 0.5; if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ Move_Z = -Pi / 5; // ÿ´ÎתÍä½Ç¶ÈΪPI/5£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ else // Æ«×ó Move_Z = Pi / 5; } } else { if (distance_count > 8) // СÓÚ±ÜÕ½¾àÀëµÄʱºò Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ¶È0.15 Move_X = (Aovid_Speed * 0.5); if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = -1; else if (Car_Num == Small_Tank_Car) Move_Z = -1; else Move_Z = -1; } else // Æ«×ó { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = 1; else if (Car_Num == Small_Tank_Car) Move_Z = 1; else Move_Z = 1; } } } } Move_Z = -Move_Z; } /************************************************************************** Function: Lidar_Follow Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float angle1 = 0; // ¸úËæµÄ½Ç¶È u16 mini_distance1; void Lidar_Follow(void) { static u16 cnt = 0; int i; int calculation_angle_cnt = 0; static float angle = 0; // ¸úËæµÄ½Ç¶È static float last_angle = 0; // u16 mini_distance = 65535; static u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ // ÐèÒªÕÒ³ö¸úËæµÄÄǸöµãµÄ½Ç¶È for (i = 0; i < lap_count; i++) { if (100 < Dataprocess[i].distance && Dataprocess[i].distance < Follow_Distance) // 1200·¶Î§ÄÚ¾ÍÐèÒª¸úËæ { calculation_angle_cnt++; if (Dataprocess[i].distance < mini_distance) // ÕÒ³ö¾àÀë×îСµÄµã { mini_distance = Dataprocess[i].distance; angle = Dataprocess[i].angle; } } } if (angle > 180) angle -= 360; // 0--360¶Èת»»³É0--180£»-180--0£¨Ë³Ê±Õ룩 if (angle - last_angle > 10 || angle - last_angle < -10) // ×öÒ»¶¨Ïû¶¶£¬²¨¶¯´óÓÚ10¶ÈµÄÐèÒª×ö&Aring;Ð¶Ï { if (++data_count == 60) // Á¬Ðø60´Î²É¼¯µ½µÄÖµ(300msºó)ºÍÉϴεıȴóÓÚ10¶È£¬´Ëʱ²&Aring;ÊÇÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } } else // ²¨¶¯Ð¡ÓÚ10¶ÈµÄ¿ÉÒÔÖ±½ÓÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } if (calculation_angle_cnt < 6) // ÔÚ¸úËæ·¶Î§ÄڵĵãÉÙÓÚ6¸ö { if (cnt < 40) // Á¬Ðø¼ÆÊý³¬40´ÎûÓÐÒª¸úËæµÄµã£¬´Ëʱ²&Aring;ÊDz»ÓøúËæ cnt++; if (cnt >= 40) { Move_X = 0; // ËÙ¶ÈΪ0 Move_Z = 0; } } else { cnt = 0; if (Car_Num == Akm_Car) { if ((((angle > 15) && (angle < 180)) || ((angle > -180) && angle < -15)) && (mini_distance < 500)) // °¢¿¨Âü³µÐÍ´¦Àí³µÍ·²»¶Ô×&Aring;¸úËæÎÏ൱ÓÚºó³µÒ»Ñù£¬Ò»´Î²»¶Ô×¼£¬ÄǺóÍËÔÙÀ´¶Ô×¼ { Move_X = -0.20; Move_Z = -Follow_Turn_PID(last_angle, 0); } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); } } else // ÆäÓà³µÐÍ { if ((angle > 50 || angle < -50) && (mini_distance > 400)) { Move_Z = -0.0298f * last_angle; // ½Ç¶È²î¾à¹ý´óÖ±½Ó¿ìËÙתÏò Move_X = 0; // ²îËÙС³µºÍÂÄ´øÐ¡³µ¿ÉÒÔʵÏÖÔ­µØ×ª¶¯ } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); // תÏòPID£¬³µÍ·ÓÀÔ¶¶Ô×&Aring;¸úËæÎïÆ· } } } Move_Z = target_limit_float(Move_Z, -Pi / 6, Pi / 6); // ÏÞ·ù Move_X = target_limit_float(Move_X, -0.6, 0.6); } /************************************************************************** º¯Êý¹¦ÄÜ£ºÐ¡³µ×ßÖ±Ïßģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_along_wall(void) { static u32 target_distance = 0; static int n = 0; u32 distance; u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ for (int i = 0; i < lap_count; i++) { if (Dataprocess[i].angle > 75 && Dataprocess[i].angle < 77) { if (n == 0) { target_distance = Dataprocess[i].distance; // »ñÈ¡µÄµÚÒ»¸öµã×÷ΪĿ±ê¾àÀë n++; } if (Dataprocess[i].distance < target_distance + 100) //+100ÏÞÖÆ»ñÈ¡¾àÀëµÄ·¶Î§Öµ { distance = Dataprocess[i].distance; // »ñȡʵʱ¾àÀë data_count++; } } } // if(data_count <= 0) // Move_X = 0; // Move_X = forward_velocity; // ³õʼËÙ¶È Move_Z = -Along_Adjust_PID(distance, target_distance); if (Car_Num == Akm_Car) { Move_Z = target_limit_float(Move_Z, -Pi / 4, Pi / 4); // ÏÞ·ù } else if (Car_Num == Diff_Car) Move_Z = target_limit_float(Move_Z, -Pi / 5, Pi / 5); // ÏÞ·ù } /************************************************************************** Function: Car_Perimeter_Init Input : none Output : none º¯Êý¹¦ÄÜ£º¼ÆËãС³µ¸÷ÂÖ×ÓµÄÖܳ¤ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Car_Perimeter_Init(void) { if (Car_Num == Diff_Car || Car_Num == Akm_Car) { Perimeter = Diff_Car_Wheel_diameter * Pi; Wheelspacing = Diff_wheelspacing; } else if (Car_Num == Small_Tank_Car) { Perimeter = Small_Tank_WheelDiameter * Pi; Wheelspacing = Small_Tank_wheelspacing; } else { Perimeter = Big_Tank_WheelDiameter * Pi; Wheelspacing = Big_Tank_wheelspacing; } } /************************************************************************** Function: Ultrasonic_Follow Input : none Output : none º¯Êý¹¦ÄÜ£º³¬Éù²¨¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Ultrasonic_Follow(void) // ³¬Éù²¨¸úËæ£¬Ö»Äܵ¥·½Ïò¸úËæ { Move_Z = 0; Read_Distane(); // ¶ÁÈ¡³¬Éù²¨µÄ¾àÀë if (Distance1 < 200) // ¾àÀëСÓÚ200mm£¬Í˺ó { if ((Move_X -= 3) < -210) Move_X = -210; // ¸øÒ»210ºóÍËËÙ¶È } else if (Distance1 > 270 && Distance1 < 750) // ¾àÀëÔÚ270µ½750Ö®¼äÊÇÐèÒª¸úËæÇ°½ø { if ((Move_X += 3) > 210) // ËÙ¶ÈÖð½¥Ôö¼Ó£¬¸øÇ°½øËÙ¶È Move_X = 210; } else { if (Move_X > 0) { if ((Move_X -= 20) < 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } else { if ((Move_X += 20) > 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } } } /************************************************************************** Function: Get angle Input : way£ºThe algorithm of getting angle 1£ºDMP 2£ºkalman 3£ºComplementary filtering Output : none º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È Èë¿Ú²ÎÊý£ºway£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Angle(u8 way) { if (way == 1) // DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯Öж϶ÁÈ¡£¬Ñϸñ×ñѭʱÐòÒªÇó { Read_DMP(); // ¶ÁÈ¡¼ÓËÙ¶È¡¢½ÇËÙ¶È¡¢Çã½Ç } else { Gyro_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_L); // ¶ÁÈ¡XÖáÍÓÂÝÒÇ Gyro_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_L); // ¶ÁÈ¡YÖáÍÓÂÝÒÇ Gyro_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_L); // ¶ÁÈ¡ZÖáÍÓÂÝÒÇ Accel_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_L); // ¶ÁÈ¡ZÖá¼ÓËÙ¶È¼Æ // if(Gyro_X>32768) Gyro_X-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Z>32768) Gyro_Z-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_X>32768) Accel_X-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Y>32768) Accel_Y-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Z>32768) Accel_Z-=65536; //Êý¾ÝÀàÐÍת»» Accel_Angle_x = atan2(Accel_Y, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Accel_Angle_y = atan2(Accel_X, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Gyro_X = Gyro_X / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»£¬Á¿³Ì¡À500¡ã/s¶ÔÓ¦ÁéÃô¶È65.5£¬¿É²éÊÖ²á Gyro_Y = Gyro_Y / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»» if (way == 2) { Roll = -Kalman_Filter_x(Accel_Angle_x, Gyro_X); // ¿¨¶ûÂüÂ˲¨ Pitch = -Kalman_Filter_y(Accel_Angle_y, Gyro_Y); } else if (way == 3) { Roll = -Complementary_Filter_x(Accel_Angle_x, Gyro_X); // »¥²¹Â˲¨ Pitch = -Complementary_Filter_y(Accel_Angle_y, Gyro_Y); } } } /************************************************************************** Function: The remote control command of model aircraft is processed Input : none Output : none º¯Êý¹¦ÄÜ£º¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Remote_Control(void) { // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí static u8 thrice = 200; int Threshold = 100; // limiter //ÏÞ·ù int LX, RY; // static float Target_LX,Target_LY,Target_RY,Target_RX; Remoter_Ch1 = target_limit_int(Remoter_Ch1, 1000, 2000); Remoter_Ch2 = target_limit_int(Remoter_Ch2, 1000, 2000); // Front and back direction of left rocker. Control forward and backward. // ×óÒ¡¸Ëǰºó·½Ïò¡£¿ØÖÆÇ°½øºóÍË¡£ LX = Remoter_Ch2 - 1500; // //Left joystick left and right. Control left and right movement. // //×óÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×óÓÒÒÆ¶¯¡£¡£ // LY=Remoter_Ch2-1500; // Right stick left and right. To control the rotation. // ÓÒÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×Ôת¡£ RY = Remoter_Ch1 - 1500; // if (LX > -Threshold && LX < Threshold) LX = 0; if (RY > -Threshold && RY < Threshold) RY = 0; // if(LX==0) Target_LX=Target_LX/1.2f; // if(LY==0) Target_LY=Target_LY/1.2f; // if(RY==0) Target_RY=Target_RY/1.2f; // //Throttle related //ÓÍÃ&Aring;Ïà¹Ø // Remote_RCvelocity=RC_Velocity+RX; // if(Remote_RCvelocity<0)Remote_RCvelocity=0; // The remote control command of model aircraft is processed // ¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Move_X = LX; Move_Z = -RY; Move_X = Move_X * 1.3; //*1.3ÊÇΪÁËÀ«´óËÙ¶È if (Car_Num == Akm_Car) Move_Z = Move_Z * (Pi / 8) / 350.0; else Move_Z = Move_Z * 2 * (Pi / 4) / 350.0; // Unit conversion, mm/s -> m/s // µ¥Î»×ª»»£¬mm/s -> m/s Move_X = Move_X / 1000; // ZÖáÊý¾Ýת»¯ #if _4WD if (Move_X < 0) Move_Z = -Move_Z; #endif // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí if (thrice > 0) Move_X = 0, Move_Z = 0, thrice--; // Control target value is obtained and kinematics analysis is performed // µÃµ½¿ØÖÆÄ¿±êÖµ£¬½øÐÐÔ˶¯Ñ§·ÖÎö // Get_Target_Encoder(Move_X,Move_Z); } 怎么把OpenMV与STM32串口通信实现二维码/APRILTag码识别及控制的代码加到上述代码中,现在openmv的二维码识别已做好,二维码采用的是Apriltag的36h11类型,id:0表示停止,id:1表示直行,id:2表示左转,id:3表示右转,现在需要再在STM32中修改或添加代码,使其能在接收到openmv识别到的指令后作出相应的动作
05-16
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值