극한의 아두이노 DIY생활 - 곡률측정기24

jimae (70)in kr-dev 커뮤니티 • last year (edited)

안녕하세요 Jimae입니다.

오늘은 곡률 알고리즘에 관련하여 글을 써나가볼까 합니다.


 case DATACALCULATION:

                    leftData = (double) (leftLaserData[1] + (leftLaserData[3] * 0.1) + (leftLaserData[4] * 0.01) + (leftLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'

                    centerData = (double) (centerLaserData[1] + (centerLaserData[3] * 0.1) + (centerLaserData[4] * 0.01) + (centerLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'

                    rightData = (double) (rightLaserData[1] + (rightLaserData[3] * 0.1) + (rightLaserData[4] * 0.01) + (rightLaserData[5] * 0.001)); // 배열값 계산 배열의 0번째 ' ' 2번째 '.'        

                    //현재 cm단위
                    leftData = ((leftData * 100) - LEFTDISTANCECORRECTION) + CALCULATIONCORRENTION;
                    centerData = ((centerData * 100) - CENTERDISTANCECORRECTION) + CALCULATIONCORRENTION;
                    rightData = ((rightData * 100) - RIGHTDISTANCECORRECTION) + CALCULATIONCORRENTION;

                    if(leftData <= 0) leftData = 0;
                    else if(centerData <= 0) centerData = 0;
                    else if(rightData <= 0) rightData = 0;
/*
                    sumData = leftData + centerData + rightData;

                    if(sumData - previousSumData < LASERFILITER && previousSumData - sumData < LASERFILITER)
                    {
                      leftData = previousLeftData;
                      centerData = previousCenterData;
                      rightData = previousRightData;
                      sumData = previousSumData;
                    }
                    
                    previousLeftData = leftData;
                    previousCenterData = centerData;
                    previousRightData = rightData;

                    previousSumData = sumData;
*/
                    leftX = leftData * cos(radians(LEFTANGLE));
                    leftY = leftData * sin(radians(LEFTANGLE));
                    
                    centerX = 0;
                    centerY = centerData * sin(radians(CENTERANGLE));
                    
                    rightX = rightData * cos(radians(RIGHTANGLE));
                    rightY = rightData * sin(radians(RIGHTANGLE));

                    d1 = (centerX - leftX)/(centerY - leftY);
                    d2 = (rightX - centerX)/(rightY - centerY);

                    cX = ((rightY - leftY) + (centerX + rightX) * d2 - (leftX + centerX) * d1) / (2 * (d2 - d1));
                    cY = -d1 * (cX - (leftX + centerX) / 2) + (leftY + centerY) / 2;

                    curvature = sqrt(pow((leftX - cX),2) + pow((leftY - cY),2));

                    plusdata = curvature / 100;

                    plusdata = plusdata * 1.5; // 1.5 % 데이터 계산

                    curvature = curvature + plusdata;
                    
                    /*
                    Serial.print("left : ");
                    Serial.println(leftData);
                    Serial.print("center : ");
                    Serial.println(centerData);
                    Serial.print("right : ");
                    Serial.println(rightData);
                    Serial.print("curva : ");
                    Serial.println(curvature);
                    Serial.println("");
              */
                    
                    detectionOrderCurvature = LASERSHOOT;

                    break;

이곳에서 실제 곡률을 계산하는 알고리즘을 따로 떼어서 보게 된다면


                    leftX = leftData * cos(radians(LEFTANGLE));
                    leftY = leftData * sin(radians(LEFTANGLE));
                    
                    centerX = 0;
                    centerY = centerData * sin(radians(CENTERANGLE));
                    
                    rightX = rightData * cos(radians(RIGHTANGLE));
                    rightY = rightData * sin(radians(RIGHTANGLE));

                    d1 = (centerX - leftX)/(centerY - leftY);
                    d2 = (rightX - centerX)/(rightY - centerY);

                    cX = ((rightY - leftY) + (centerX + rightX) * d2 - (leftX + centerX) * d1) / (2 * (d2 - d1));
                    cY = -d1 * (cX - (leftX + centerX) / 2) + (leftY + centerY) / 2;

                    curvature = sqrt(pow((leftX - cX),2) + pow((leftY - cY),2));

                    plusdata = curvature / 100;

                    plusdata = plusdata * 1.5; // 1.5 % 데이터 계산

                    curvature = curvature + plusdata;

이부분들 입니다.

저번에 보여드린 링크에 나온대로 구현을 한거라고 보시면 됩니다.

그런대 실제 적용한 알고리즘과 링크쪽에서 설명한 부분에 약간 괴리감이 있을것입니다.

이부분은 실제 좌표를 구하는 과정에서 이루어진 차이인대요.


                    leftX = leftData * cos(radians(LEFTANGLE));
                    leftY = leftData * sin(radians(LEFTANGLE));
                    
                    centerX = 0;
                    centerY = centerData * sin(radians(CENTERANGLE));
                    
                    rightX = rightData * cos(radians(RIGHTANGLE));
                    rightY = rightData * sin(radians(RIGHTANGLE));

3점의 좌표를 구하려고 하는이유가 실질적으로 우리가 아는것은 거리지 그것에 관한 X,Y 좌표가 아닙니다.

그래서 거리를 측정한 거리값이 빗변으로 보고