Nghiên cứu thiết kế bộ điều khiển trượt thích nghi mờ cho robot OMNI

Nghiên cứu thiết kế bộ điều khiển trượt thích nghi mờ cho robot OMNI

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BỘ MÔN ĐIỀU KHỂN TỰ ĐỘNG ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: NGHIÊN CỨU THIẾT KẾ BỘ ĐIỀU KHIỂN TRƯỢT THÍCH NGHI MỜ CHO ROBOT TỰ HÀNH ĐA HƯỚNG OMNI Giáo viên hướng dẫn : TS Đỗ Thị Tú Anh Sinh viên thực : HÀ NỘI, 8/2017 LỜI MỞ ĐẦU Cùng với phát triển xã hội loài người, ngành khoa học kỹ thuật không ngừng đến thành công mới, đặc biệt phần cứng mà não thiết bị điều khiển Sự có mặt khái niệm hàm điều khiển Lyapunov giúp cho việc giải tính ổn định hệ thống sử dụng điều khiển phi tuyến đơn giản hơn, có ảnh hưởng nhiễu bất định, sai lệch hệ thống Bên cạnh đó, hầu hết mơ hình động lực học đối tượng rõ ràng ngày mở rộng sát với thực tế, khiên điều khiển ngày đáp ứng nhiều yêu cầu điều khiển phức tạp Vì thế, điều khiển thông minh Robot ngày trọng ứng dụng rộng rãi ngành công nghiệp, ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI thay cho hoạt động người môi trường độc hại, nguy hiểm Nổi bật số có Robot tự hành robot ứng dụng rộng rãi Robot Omni robot tự hành đa hướng kiểu four-wheeled omnidirectional mobile robot (FWOMR) có khả di chuyển theo hướng mà không cần phải thay đổi vị trí tư Với cấu trúc bánh khác biệt khả di chuyển vượt trội, robot Omni ngày ứng dụng rộng rãi Các vấn đề kiểm soát quỹ đạo, xử lý tác động nhiễu ngoại sinh, thay đổi thành phần bất định khối lượng, momen, ma sát…đang nội dung quan tâm lĩnh vực điều khiển robot Omni Đã có cơng trình cơng bố thuật toán điều khiển áp dụng cho FWOMR điều khiển tuyến tính hóa quỹ đạo, backstepping, điều khiển thơng minh, điều khiển trượt… phương pháp điều khiển trượt có ưu điểm tính ổn định, bền vững, đáp ứng tốt đối tượng có nhiễu Tuy nhiên, biên độ tín hiệu điều khiển khơng phù hợp gây tượng rung, dao động quanh mặt trượt (chattering) Vì vây, đề tài “NGHIÊN CỨU THIẾT KẾ BỘ ĐIỀU KHIỂN TRƯỢT THÍCH NGHI MỜ CHO ROBOT TỰ HÀNH ĐA HƯỚNG OMNI” đề xuất điều khiển trượt thích nghi sử dụng logic mờ bám quỹ đạo cho đối tượng robot Omni Tính ổn định hệ thống chứng minh dựa tiêu chuẩn Lyapunov Bộ điều khiển trượt thích nghi mờ làm giảm tượng rung (chattering) đảm bảo chất lượng bám robot robot có tham số thay đổi Các kết mô phần mềm MATLAB kết thực nghiệm cho thấy thuật tốn có hiệu cao robot đạt tới quỹ đạo mong muốn thời gian ngắn, hạn chế tượng chattering điều khiển trượt đáp ứng tốt thông số robot Omni thay đổi Điều mở khả ứng dụng điều khiển thực tế Để hoàn thành đồ án em xin gửi lời cảm ơn tới thầy cô viện Điện tạo điều kiện cho em học tập năm qua Đặc biệt em xin gửi lời cảm ơn sâu sắc tới TS Đỗ Thị Tú Anh giúp đỡ em, thời gian thực tập, làm đồ án tốt nghiệp để em hồn thành đồ án ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI Em xin chân thành cảm ơn! ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI MỤC LỤC ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI DANH MỤC HÌNH VẼ ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI DANH MỤC BẢNG BIỂU ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI Chương 1: TỔNG QUAN VỀ ROBOT TỰ HÀNH ĐA HƯỚNG OMNI 1.1 Giới thiệu robot đa hướng tự hành OMNI Robot đa hướng tự hành (Directional robot) loại mobile robot , di động tự hành, tự định hướng, di chuyển đến vị trí mặt phẳng cách kết hợp hướng chuyển động xoay tịnh tiến theo quỹ đạo định trước thời gian ngắn.Có nhiều lựa chọn cho việc thiết kế robot đa hướng tự hành có khả di chuyển mặt cứng, số có dạng chủ yếu dùng bánh xe dùng xích dùng chân Việc dùng bánh xe sử dụng phổ biến kết cấu khí đơn giản việc thực thi dễ dàng Dùng chân xích đòi hỏi kết cấu phức tạp phần cứng nặng so với mức tải yêu cầu, lợi di chuyển địa hình xấu nhấp nhơ trơn trượt Robot đa hướng tự hành OMNI loại robot tự hành sử dụng bánh xe OMNI với thiết kế đặc biệt giúp robot di chuyển đa hướng 1.1.1 Cấu trúc nguyên lý hoạt động robot OMNI Hình 1.1 Cấu trúc robot OMNI ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI Khung robot thiết kế có khớp động Khớp động di chuyển lên xuống để thích hợp với đìa hình gồ ghề Do khớp động giúp cho omni robot di chuyển mặt không phẳng Bánh xe Omni loại bánh có nhiều bánh vệ tinh nhỏ xung quanh chu vi bánh, vòng bánh sole với vng góc với trục bánh xe Do robot di chuyển theo hướng 1.1.2 Nguyên lý di chuyển đa hướng robot OMNI Dựa vào đặc điểm cách thiết kế khung xe bánh xe đưa hướng di chuyển omni robot Ta quy định động bánh xe ( kĩ thuật động trục, động bánh xe quay trục) quay theo chiều mũi tên thuận chiều kim đồng hồ  Di chuyển phía trước (hướng động 2) : động quay chiều mũi tên, động quay ngược chiều mũi tên  Di chuyển phía sau (hướng động 4): động quay chiều mũi tên, động quay ngược chiều mũi tên  Di chuyển sang phải (hướng động 4): động quay chiều mũi tên, động quay ngược chiều mũi tên  Di chuyển sang trái (hướng động 3): động quay chiều mũi tên, động quay ngược chiều mũi tên  Di chuyển theo hướng trục động 1: động quay chiều mũi tên, động quay ngược chiều mũi tên, động dừng  Di chuyển theo hướng trục động 2: động quay chiều mũi tên, động quay ngược chiều mũi tên, động dừng  Di chuyển theo hướng trục động 3: động quay chiều mũi tên, động quay ngược chiều mũi tên, động dừng  Di chuyển theo hướng trục động 4: động quay chiều mũi tên, động quay ngược chiều mũi tên, động dừng  Xoay tròn chỗ: tất động quay chiều mũi tên ngược chiều mũi tên ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI Hình 1.2 Sơ đồ nguyên lý hoạt động chung cho khối robot OMNI 1.2 Tổng quan cấu chấp hành mạch điện tử robot OMNI 1.2.1 Khối cấu chấp hành Khối cấu chấp hành bao gồm động planet có tích hợp encoder cho bánh OMNI ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI Hình 1.3 Động planet robot OMNI Encoder động cơ: sử dụng encoder 255 xung encoder sử dụng kênh A B cặp led thu phát để xác định vị trí tốc độ động Hình 1.4 Encoder động thực tế 1.2.2 Khối driver điều khiển động DC mạch cầu H Mạch cầu H dùng IC kích FET chuyên dụng IR2184 cho phép điều khiển động với dòng liên tục 10A dòng tức thời lên đến 30A Mosfet sử dụng mạch IRF840 cho dòng đỉnh tối đa 22A Điện áp hoạt động lên tới 500VDC Dòng liên tục 8A chế độ cầu kép Điện áp cấp cho FET cao dòng nhỏ Với ứng dụng cầu FET cho robot điện áp cấp thường 24V~25,6V Chu kỳ xung nhỏ cho dòng qua FET lớn, nên băm xung tần số cao có lợi cơng suất hơn, robot chạy nhanh khỏe Sử dụng IC kich FET chuyên dụng IR2184 bảo đảm FET dẫn tốt, chống tượng trùng dẫn có hãm động tốt Diode sử dụng cho IR2184 phải có tần số hoạt động cao Driver có biến trở dùng chỉnh dòng giới hạn qua động giúp bảo vệ thiết bị Khối mạch vi điều khiển trung tâm 10 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI MX_USART1_UART_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM5_Init(); /* USER CODE BEGIN */ //HAL_TIM_Base_Stop_IT(&htim3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_4); HAL_UART_Receive_IT(&huart1, &rx_data,1); esp_init(); HAL_TIM_Base_Start_IT(&htim3); /* USER CODE END */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN */ if (tx_stt>=1) { char temp1[100]; char temp[100]; sprintf(temp1,"set_a=%4.2f,set_b=%4.2f,real_a=%4.2f,real_b= %4.2f\n",set_x,set_y,real_position_x,real_position_y); sprintf(temp,"AT+CIPSEND=0,%d\r\n",strlen(temp1)); HAL_UART_Transmit(&huart1,temp,strlen(temp),10); HAL_Delay(2000); HAL_UART_Transmit(&huart1,temp1,strlen(temp1),10); HAL_Delay(2000); tx_stt=0; } if(tx_stt1>=10) { sprintf(temp,"R%4.2fr\n",real_vellocity); HAL_UART_Transmit(&huart1,temp,strlen(temp),10); tx_stt1=0; } // // // // // // } /* USER CODE END */ } 50 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI /** System Clock Configuration*/ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct; RCC_ClkInitTypeDef RCC_ClkInitStruct; /**Initializes the CPU, AHB and APB busses clocks*/ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL4; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /**Initializes the CPU, AHB and APB busses clocks*/ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK| RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_PCLK1| RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK) { Error_Handler(); } /**Configure the Systick interrupt time*/ HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); /**Configure the Systick*/ HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); /* SysTick_IRQn interrupt configuration */ HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); } /* TIM2 init function */ static void MX_TIM2_Init(void) { TIM_ClockConfigTypeDef sClockSourceConfig; TIM_MasterConfigTypeDef sMasterConfig; TIM_OC_InitTypeDef sConfigOC; 51 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI htim2.Instance = TIM2; htim2.Init.Prescaler = 1; htim2.Init.CounterMode = TIM_COUNTERMODE_UP; htim2.Init.Period = 1000; htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; if (HAL_TIM_Base_Init(&htim2) != HAL_OK) { Error_Handler(); } sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_Init(&htim2) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); if (HAL_TIM_PWM_ConfigChannel(&htim2,&sConfigOC,TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); 52 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI } HAL_TIM_MspPostInit(&htim2); } /* TIM3 init function */ static void MX_TIM3_Init(void) { TIM_ClockConfigTypeDef sClockSourceConfig; TIM_MasterConfigTypeDef sMasterConfig; htim3.Instance = TIM3; htim3.Init.Prescaler = 32; htim3.Init.CounterMode = TIM_COUNTERMODE_UP; htim3.Init.Period = 49000; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; if (HAL_TIM_Base_Init(&htim3) != HAL_OK) { Error_Handler(); } sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) { Error_Handler(); } } /* TIM5 init function */ static void MX_TIM5_Init(void) { TIM_ClockConfigTypeDef sClockSourceConfig; TIM_MasterConfigTypeDef sMasterConfig; TIM_OC_InitTypeDef sConfigOC; htim5.Instance = TIM5; htim5.Init.Prescaler = 0; htim5.Init.CounterMode = TIM_COUNTERMODE_UP; htim5.Init.Period = 1000; htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; if (HAL_TIM_Base_Init(&htim5) != HAL_OK) { Error_Handler(); 53 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI } sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_Init(&htim5) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); } HAL_TIM_MspPostInit(&htim5); } /* USART1 init function */ static void MX_USART1_UART_Init(void) { huart1.Instance = USART1; huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart1.Init.OverSampling = UART_OVERSAMPLING_16; if (HAL_UART_Init(&huart1) != HAL_OK) { Error_Handler(); } 54 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI } /** Configure pins as * Analog * Input * Output * EVENT_OUT * EXTI */ static void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; /* GPIO Ports Clock Enable */ HAL_RCC_GPIOD_CLK_ENABLE(); HAL_RCC_GPIOA_CLK_ENABLE(); HAL_RCC_GPIOB_CLK_ENABLE(); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOA, W1_Pin|W3_Pin|W_2_Pin|W4_Pin, GPIO_PIN_RESET); /*Configure GPIO pin : PA1 */ GPIO_InitStruct.Pin = GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /*Configure GPIO pin : PA2 */ GPIO_InitStruct.Pin = GPIO_PIN_2; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /*Configure GPIO pins : W1_Pin W3_Pin W_2_Pin W4_Pin */ GPIO_InitStruct.Pin = W1_Pin|W3_Pin|W_2_Pin|W4_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /*Configure GPIO pin : PB1 */ GPIO_InitStruct.Pin = GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pin : PB2 */ GPIO_InitStruct.Pin = GPIO_PIN_2; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* EXTI interrupt init*/ 55 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI HAL_NVIC_SetPriority(EXTI1_IRQn, 1, 0); HAL_NVIC_EnableIRQ(EXTI1_IRQn); HAL_NVIC_SetPriority(EXTI2_IRQn, 1, 0); HAL_NVIC_EnableIRQ(EXTI2_IRQn); } /* USER CODE BEGIN */ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if(GPIO_Pin == GPIO_PIN_1) { if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_2)) pulse_Oy++; else pulse_Oy ; } if(GPIO_Pin == GPIO_PIN_2) { if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_1)) pulse_Ox++; else pulse_Ox ; } } // tinh lai phan giai encoder sampling time void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim->Instance == htim3.Instance) { // Solve data from encoder revolution_y = pulse_Oy/resolution_enc; //xung doc ve 50ms velocity_y = revolution_y*2*PI*radius; //quang duong di 50ms real_position_y += velocity_y; revolution_x = pulse_Ox/resolution_enc; //1000 xung velocity_x = revolution_x*2*PI*radius; //quang duong di 50ms real_position_x += velocity_x; x_dot = (real_position_x - pre_real_position_x)*inv_samp_time; pre_real_position_x = real_position_x; x_2_dot = (x_dot - pre_x_dot)*inv_samp_time; pre_x_dot = x_dot; y_dot = (real_position_y - pre_real_position_y)*inv_samp_time; pre_real_position_y = real_position_y; y_2_dot = (y_dot - pre_y_dot)*inv_samp_time; pre_y_dot = y_dot; 56 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI //3 mat truot error_x = set_x - real_position_x; error_y = set_y - real_position_y; s1 = x_2_dot + 80*x_dot+25*error_x; s2 = y_2_dot + 80*y_dot+25*error_y; if(s1>=5) { muy1 = kP*fabs(s1); } if(s1=5) { muy2 = kP*fabs(s2); } if(s20) {sign1=1;} if(s1==0) {sign1=0;} if(s10) {sign2=1;} if(s2==0) {sign2=0;} if(s2=0) { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, PIO_PIN_RESET); } if(u1=0) 57 ĐATN: Nghiên cứu thiết kế điều khiển trượt thích nghi mờ cho robot OMNI { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET); } if(u3=0) { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET); } if(u2=0) { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, PIO_PIN_RESET); } if(u4Instance == huart1.Instance) { if(rx_index==0) { for(int i=0;i